#!/usr/bin/env python3 # Software License Agreement (BSD License) # # Copyright (c) 2018, UFACTORY, Inc. # All rights reserved. # # Author: Jimy Zhang # Author: Vinman import time import math import threading import functools from ..utils.bytes_data import BytesData from ..config.x_config import XCONF def lock_require(func): @functools.wraps(func) def decorator(*args, **kwargs): with args[0].lock: return func(*args, **kwargs) return decorator class UxbusCmd(object): BAUDRATES = (4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600, 1000000, 1500000, 2000000, 2500000) def __init__(self, set_feedback_key_tranid=None): self._has_error = False self._has_warn = False self._state_is_ready = False self._error_code = 0 self._warn_code = 0 self._cmd_num = 0 self._debug = False self.lock = threading.Lock() self._G_TOUT = XCONF.UxbusConf.GET_TIMEOUT / 1000 self._S_TOUT = XCONF.UxbusConf.SET_TIMEOUT / 1000 self._last_comm_time = time.monotonic() self._last_modbus_comm_time = time.monotonic() self._feedback_type = 0 self._set_feedback_key_tranid = set_feedback_key_tranid self.tgpio_set_modbus_func = self.tgpio_set_modbus @property def last_comm_time(self): return self._last_comm_time @property def state_is_ready(self): return self._state_is_ready def _get_trans_id(self): return 0 def set_timeout(self, timeout): try: if isinstance(timeout, (tuple, list)): if len(timeout) >= 2: self._S_TOUT = timeout[0] if timeout[0] > 0 else self._S_TOUT self._G_TOUT = timeout[1] if timeout[1] > 0 else self._G_TOUT elif len(timeout) == 1: self._S_TOUT = timeout[0] if timeout[0] > 0 else self._S_TOUT self._G_TOUT = timeout[0] if timeout[0] > 0 else self._G_TOUT elif isinstance(timeout, (int, float)): self._S_TOUT = timeout if timeout > 0 else self._S_TOUT self._G_TOUT = timeout if timeout > 0 else self._G_TOUT except: pass return [self._S_TOUT, self._G_TOUT] if self._S_TOUT != self._G_TOUT else self._S_TOUT def set_debug(self, debug): self._debug = debug def send_modbus_request(self, unit_id, pdu_data, pdu_len, prot_id=-1, t_id=None, debug=False): raise NotImplementedError def recv_modbus_response(self, t_unit_id, t_trans_id, num, timeout, t_prot_id=-1, ret_raw=False, debug=False): raise NotImplementedError @lock_require def set_nu8(self, funcode, datas, num, timeout=None, feedback_key=None, feedback_type=XCONF.FeedbackType.MOTION_FINISH): need_set_fb = feedback_type != 0 and (self._feedback_type & feedback_type) != feedback_type if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type | feedback_type) trans_id = self._get_trans_id() if feedback_key and self._set_feedback_key_tranid: self._set_feedback_key_tranid(feedback_key, trans_id, self._feedback_type) ret = self.send_modbus_request(funcode, datas, num) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] ret = self.recv_modbus_response(funcode, ret, 0, self._S_TOUT if timeout is None else timeout) if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type) return ret @lock_require def getset_nu8(self, funcode, datas, num_send, num_get): ret = self.send_modbus_request(funcode, datas, num_send) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(funcode, ret, num_get, self._S_TOUT) @lock_require def get_nu8(self, funcode, num): ret = self.send_modbus_request(funcode, 0, 0) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (num + 1) return self.recv_modbus_response(funcode, ret, num, self._G_TOUT) @lock_require def set_nu16(self, funcode, datas, num, additional_bytes=None): hexdata = BytesData.from_u16_list(datas, num) if additional_bytes is not None: hexdata += additional_bytes ret = self.send_modbus_request(funcode, hexdata, num * 2 + len(additional_bytes)) else: ret = self.send_modbus_request(funcode, hexdata, num * 2) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] ret = self.recv_modbus_response(funcode, ret, 0, self._S_TOUT) return ret @lock_require def get_nu16(self, funcode, num): ret = self.send_modbus_request(funcode, 0, 0) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (num * 2 + 1) ret = self.recv_modbus_response(funcode, ret, num * 2, self._G_TOUT) data = [0] * (1 + num) data[0] = ret[0] data[1:num+1] = BytesData.to_u16_list(ret[1:num * 2 + 1], num) return data @lock_require def set_nfp32(self, funcode, datas, num, feedback_key=None, feedback_type=XCONF.FeedbackType.MOTION_FINISH): need_set_fb = feedback_type != 0 and (self._feedback_type & feedback_type) != feedback_type if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type | feedback_type) trans_id = self._get_trans_id() if feedback_key and self._set_feedback_key_tranid: self._set_feedback_key_tranid(feedback_key, trans_id, self._feedback_type) hexdata = BytesData.from_fp32_list(datas, num) ret = self.send_modbus_request(funcode, hexdata, num * 4) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] ret = self.recv_modbus_response(funcode, ret, 0, self._S_TOUT) if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type) return ret @lock_require def set_nfp32_with_bytes(self, funcode, datas, num, additional_bytes, rx_len=0, timeout=None, feedback_key=None, feedback_type=XCONF.FeedbackType.MOTION_FINISH): need_set_fb = feedback_type != 0 and (self._feedback_type & feedback_type) != feedback_type if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type | feedback_type) trans_id = self._get_trans_id() if feedback_key and self._set_feedback_key_tranid: self._set_feedback_key_tranid(feedback_key, trans_id, self._feedback_type) hexdata = BytesData.from_fp32_list(datas, num) hexdata += additional_bytes ret = self.send_modbus_request(funcode, hexdata, num * 4 + len(additional_bytes)) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] ret = self.recv_modbus_response(funcode, ret, rx_len, self._S_TOUT if timeout is None else timeout) if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type) return ret @lock_require def set_nint32(self, funcode, datas, num, feedback_key=None, feedback_type=XCONF.FeedbackType.MOTION_FINISH): need_set_fb = feedback_type != 0 and (self._feedback_type & feedback_type) != feedback_type if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type | feedback_type) trans_id = self._get_trans_id() if feedback_key and self._set_feedback_key_tranid: self._set_feedback_key_tranid(feedback_key, trans_id, self._feedback_type) hexdata = BytesData.from_s32_list(datas, num, is_big_endian=False) ret = self.send_modbus_request(funcode, hexdata, num * 4) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] ret = self.recv_modbus_response(funcode, ret, 0, self._S_TOUT) if feedback_key and need_set_fb: self._set_feedback_type_no_lock(self._feedback_type) return ret @lock_require def get_nfp32(self, funcode, num, timeout=None): ret = self.send_modbus_request(funcode, 0, 0) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (num * 4 + 1) ret = self.recv_modbus_response(funcode, ret, num * 4, timeout if timeout is not None else self._G_TOUT) data = [0] * (1 + num) data[0] = ret[0] data[1:num+1] = BytesData.to_fp32_list(ret[1:num * 4 + 1], num) return data @lock_require def get_nfp32_with_datas(self, funcode, datas, num_send, num_get, timeout=None): ret = self.send_modbus_request(funcode, datas, num_send) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] ret = self.recv_modbus_response(funcode, ret, num_get * 4, timeout if timeout is not None else self._G_TOUT) data = [0] * (1 + num_get) data[0] = ret[0] data[1:num_get + 1] = BytesData.to_fp32_list(ret[1:num_get * 4 + 1], num_get) return data @lock_require def swop_nfp32(self, funcode, datas, txn, rxn, additional_bytes=None): hexdata = BytesData.from_fp32_list(datas, txn) if additional_bytes is not None: hexdata += additional_bytes ret = self.send_modbus_request(funcode, hexdata, txn * 4 + len(additional_bytes)) else: ret = self.send_modbus_request(funcode, hexdata, txn * 4) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (rxn + 1) ret = self.recv_modbus_response(funcode, ret, rxn * 4, self._G_TOUT) data = [0] * (1 + rxn) data[0] = ret[0] data[1:rxn+1] = BytesData.to_fp32_list(ret[1:rxn * 4 + 1], rxn) return data @lock_require def is_nfp32(self, funcode, datas, txn): hexdata = BytesData.from_fp32_list(datas, txn) ret = self.send_modbus_request(funcode, hexdata, txn * 4) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * 2 return self.recv_modbus_response(funcode, ret, 1, self._G_TOUT) def get_version(self): return self.get_nu8(XCONF.UxbusReg.GET_VERSION, 40) def get_robot_sn(self): return self.get_nu8(XCONF.UxbusReg.GET_ROBOT_SN, 40) def check_verification(self): # txdata = signature, 175: signature length if use 14-character SN for plain text, do not miss '\n's return self.get_nu8(XCONF.UxbusReg.CHECK_VERIFY, 1) def system_control(self, value): txdata = [value] return self.set_nu8(XCONF.UxbusReg.SYSTEM_CONTROL, txdata, 1) def set_record_traj(self, value): txdata = [value] return self.set_nu8(XCONF.UxbusReg.SET_TRAJ_RECORD, txdata, 1) def playback_traj(self, value, spdx=1, feedback_key=None): txdata = [value, spdx] return self.set_nint32(XCONF.UxbusReg.PLAY_TRAJ, txdata, 2, feedback_key=feedback_key, feedback_type=XCONF.FeedbackType.OTHER_FINISH) def playback_traj_old(self, value): txdata = [value] return self.set_nint32(XCONF.UxbusReg.PLAY_TRAJ, txdata, 1) def save_traj(self, filename, wait_time=2, feedback_key=None): char_list = list(filename) txdata = [ord(i) for i in char_list] name_len = len(txdata) if name_len > 80: print("name length should not exceed 80 characters!") return [XCONF.UxbusState.ERR_PARAM] txdata = txdata + [0] * (81 - name_len) ret = self.set_nu8(XCONF.UxbusReg.SAVE_TRAJ, txdata, 81, feedback_key=feedback_key, feedback_type=XCONF.FeedbackType.OTHER_FINISH) time.sleep(wait_time) # Must! or buffer would be flushed if set mode to pos_mode return ret def load_traj(self, filename, wait_time=2, feedback_key=None): char_list = list(filename) txdata = [ord(i) for i in char_list] name_len = len(txdata) if name_len > 80: print("name length should not exceed 80 characters!") return [XCONF.UxbusState.ERR_PARAM] txdata = txdata + [0] * (81 - name_len) ret = self.set_nu8(XCONF.UxbusReg.LOAD_TRAJ, txdata, 81, feedback_key=feedback_key, feedback_type=XCONF.FeedbackType.OTHER_FINISH) if wait_time > 0: time.sleep(wait_time) # Must! or buffer would be flushed if set mode to pos_mode return ret def get_traj_rw_status(self): return self.get_nu8(XCONF.UxbusReg.GET_TRAJ_RW_STATUS, 1) def set_reduced_mode(self, on_off): txdata = [on_off] return self.set_nu8(XCONF.UxbusReg.SET_REDUCED_MODE, txdata, 1) def set_reduced_linespeed(self, lspd_mm): txdata = [lspd_mm] return self.set_nfp32(XCONF.UxbusReg.SET_REDUCED_TRSV, txdata, 1) def set_reduced_jointspeed(self, jspd_rad): txdata = [jspd_rad] return self.set_nfp32(XCONF.UxbusReg.SET_REDUCED_P2PV, txdata, 1) def get_reduced_mode(self): return self.get_nu8(XCONF.UxbusReg.GET_REDUCED_MODE, 1) def get_reduced_states(self, length=21): ret = self.get_nu8(XCONF.UxbusReg.GET_REDUCED_STATE, length) msg = [0] * 8 msg[0] = ret[0] msg[1] = ret[1] # reduced_mode_is_on msg[2] = BytesData.to_s16_list(ret[2:14], 6) # tcp_boundary msg[3:5] = BytesData.to_fp32_list(ret[14:22], 2) # tcp_speed, joint_speed if length == 79: msg[5] = BytesData.to_fp32_list(ret[22:78], 14) # joint range msg[6:8] = ret[78:80] # fence_is_on, collision_rebound return msg def set_xyz_limits(self, xyz_list): return self.set_nint32(XCONF.UxbusReg.SET_LIMIT_XYZ, xyz_list, 6) def set_timer(self, sec_later, timer_id, fun_code, param1=0, param2=0): txdata = [sec_later, timer_id, fun_code, param1, param2] return self.set_nint32(XCONF.UxbusReg.SET_TIMER, txdata, 5) def cancel_timer(self, timer_id): txdata = [timer_id] return self.set_nint32(XCONF.UxbusReg.CANCEL_TIMER, txdata, 1) def set_world_offset(self, pose_offset): return self.set_nfp32(XCONF.UxbusReg.SET_WORLD_OFFSET, pose_offset, 6) def cnter_reset(self): return self.set_nu8(XCONF.UxbusReg.CNTER_RESET, 0, 0) def cnter_plus(self): return self.set_nu8(XCONF.UxbusReg.CNTER_PLUS, 0, 0) def set_reduced_jrange(self, jrange_rad): return self.set_nfp32(XCONF.UxbusReg.SET_REDUCED_JRANGE, jrange_rad, 14) def set_fence_on(self, on_off): txdata = [on_off] return self.set_nu8(XCONF.UxbusReg.SET_FENCE_ON, txdata, 1) def set_collis_reb(self, on_off): txdata = [on_off] return self.set_nu8(XCONF.UxbusReg.SET_COLLIS_REB, txdata, 1) def motion_en(self, axis_id, enable): txdata = [axis_id, int(enable)] return self.set_nu8(XCONF.UxbusReg.MOTION_EN, txdata, 2, timeout=self._S_TOUT if self._S_TOUT >= 5 else 5) def set_state(self, value): txdata = [value] return self.set_nu8(XCONF.UxbusReg.SET_STATE, txdata, 1) def get_state(self): return self.get_nu8(XCONF.UxbusReg.GET_STATE, 1) def get_cmdnum(self): return self.get_nu16(XCONF.UxbusReg.GET_CMDNUM, 1) def get_err_code(self): return self.get_nu8(XCONF.UxbusReg.GET_ERROR, 2) def get_hd_types(self): return self.get_nu8(XCONF.UxbusReg.GET_HD_TYPES, 2) def reload_dynamics(self): return self.set_nu8(XCONF.UxbusReg.RELOAD_DYNAMICS, 0, 0) def clean_err(self): return self.set_nu8(XCONF.UxbusReg.CLEAN_ERR, 0, 0) def clean_war(self): return self.set_nu8(XCONF.UxbusReg.CLEAN_WAR, 0, 0) def set_brake(self, axis_id, enable): txdata = [axis_id, int(enable)] return self.set_nu8(XCONF.UxbusReg.SET_BRAKE, txdata, 2) def set_mode(self, mode, detection_param=-1): if detection_param >= 0: txdata = [mode, detection_param] return self.set_nu8(XCONF.UxbusReg.SET_MODE, txdata, 2) else: txdata = [mode] return self.set_nu8(XCONF.UxbusReg.SET_MODE, txdata, 1) def set_report_tau_or_i(self, tau_or_i): # 0 for tau(default), 1 for i txdata = [tau_or_i] return self.set_nu8(XCONF.UxbusReg.REPORT_TAU_OR_I, txdata, 1) def get_report_tau_or_i(self): return self.get_nu8(XCONF.UxbusReg.GET_REPORT_TAU_OR_I, 1) def set_cartesian_velo_continuous(self, on_off): # False for not continuous, True for continuous txdata = [int(on_off)] return self.set_nu8(XCONF.UxbusReg.SET_CARTV_CONTINUE, txdata, 1) def set_allow_approx_motion(self, on_off): txdata = [int(on_off)] return self.set_nu8(XCONF.UxbusReg.SET_ALLOW_APPROX_MOTION, txdata, 1) def get_allow_approx_motion(self): return self.get_nu8(XCONF.UxbusReg.GET_ALLOW_APPROX_MOTION, 1) def move_line(self, mvpose, mvvelo, mvacc, mvtime, only_check_type=0, motion_type=0): txdata = [mvpose[i] for i in range(6)] txdata += [mvvelo, mvacc, mvtime] if only_check_type <= 0 and motion_type == 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_LINE, txdata, 9) else: byte_data = bytes([only_check_type]) if motion_type == 0 else bytes([only_check_type, int(motion_type)]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE, txdata, 9, byte_data, 3, timeout=10) def move_line_common(self, mvpose, mvvelo, mvacc, mvtime, radius=-1, coord=0, is_axis_angle=False, only_check_type=0, motion_type=0, feedback_key=None): """ 通用指令, 固件1.10.0开始支持 """ txdata = [mvpose[i] for i in range(6)] _radius = -1 if radius is None else radius txdata += [mvvelo, mvacc, mvtime, _radius] if motion_type == 0: byte_data = bytes([coord, int(is_axis_angle), only_check_type]) else: byte_data = bytes([coord, int(is_axis_angle), only_check_type, int(motion_type)]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE, txdata, 10, byte_data, 3, timeout=10, feedback_key=feedback_key) def move_line_aa(self, mvpose, mvvelo, mvacc, mvtime, mvcoord, relative, only_check_type=0, motion_type=0): float_data = [mvpose[i] for i in range(6)] float_data += [mvvelo, mvacc, mvtime] byte_data = bytes([mvcoord, relative]) if only_check_type <= 0 and motion_type == 0: return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE_AA, float_data, 9, byte_data) else: byte_data += bytes([only_check_type]) if motion_type == 0 else bytes([only_check_type, int(motion_type)]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE_AA, float_data, 9, byte_data, 3, timeout=10) def move_servo_cart_aa(self, mvpose, mvvelo, mvacc, tool_coord, relative): float_data = [mvpose[i] for i in range(6)] float_data += [mvvelo, mvacc] byte_data = BytesData.from_s32(tool_coord, is_big_endian=False) byte_data += bytes([relative]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_SERVO_CART_AA, float_data, 8, byte_data) def move_relative(self, pose, mvvelo, mvacc, mvtime, radius, is_joint_motion=False, is_angle_axis=False, only_check_type=0, motion_type=0, feedback_key=None): float_data = [0] * 7 for i in range(min(7, len(pose))): float_data[i] = pose[i] float_data += [mvvelo, mvacc, mvtime, radius] byte_data = bytes([int(is_joint_motion), int(is_angle_axis)]) if only_check_type <= 0 and motion_type == 0: return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_RELATIVE, float_data, 11, byte_data, feedback_key=feedback_key) else: byte_data += bytes([only_check_type]) if motion_type == 0 else bytes([only_check_type, int(motion_type)]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_RELATIVE, float_data, 11, byte_data, 3, timeout=10, feedback_key=feedback_key) def get_position_aa(self): return self.get_nfp32(XCONF.UxbusReg.GET_TCP_POSE_AA, 6) @lock_require def get_pose_offset(self, pose1, pose2, orient_type_in=0, orient_type_out=0): float_data = [pose1[i] for i in range(6)] float_data += [pose2[j] for j in range(6)] byte_data = bytes([orient_type_in, orient_type_out]) ret_fp_num = 6 funcode = XCONF.UxbusReg.CAL_POSE_OFFSET hexdata = BytesData.from_fp32_list(float_data, 12) hexdata += byte_data ret = self.send_modbus_request(funcode, hexdata, len(hexdata)) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (ret_fp_num * 4 + 1) ret = self.recv_modbus_response(funcode, ret, ret_fp_num * 4, self._G_TOUT) data = [0] * (1 + ret_fp_num) data[0] = ret[0] data[1:ret_fp_num+1] = BytesData.to_fp32_list(ret[1:ret_fp_num * 4 + 1], ret_fp_num) return data def move_line_tool(self, mvpose, mvvelo, mvacc, mvtime, only_check_type=0, motion_type=0): txdata = [mvpose[i] for i in range(6)] txdata += [mvvelo, mvacc, mvtime] if only_check_type <= 0 and motion_type == 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_LINE_TOOL, txdata, 9) else: byte_data = bytes([only_check_type]) if motion_type == 0 else bytes([only_check_type, int(motion_type)]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINE_TOOL, txdata, 9, byte_data, 3, timeout=10) def move_lineb(self, mvpose, mvvelo, mvacc, mvtime, mvradii, only_check_type=0, motion_type=0): txdata = [mvpose[i] for i in range(6)] txdata += [mvvelo, mvacc, mvtime, mvradii] if only_check_type <= 0 and motion_type == 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_LINEB, txdata, 10) else: byte_data = bytes([only_check_type]) if motion_type == 0 else bytes([only_check_type, int(motion_type)]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_LINEB, txdata, 10, byte_data, 3, timeout=10) def move_joint(self, mvjoint, mvvelo, mvacc, mvtime, only_check_type=0, feedback_key=None): txdata = [mvjoint[i] for i in range(7)] txdata += [mvvelo, mvacc, mvtime] if only_check_type <= 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_JOINT, txdata, 10, feedback_key=feedback_key) else: byte_data = bytes([only_check_type]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_JOINT, txdata, 10, byte_data, 3, timeout=10, feedback_key=feedback_key) def move_jointb(self, mvjoint, mvvelo, mvacc, mvradii, only_check_type=0, feedback_key=None): txdata = [mvjoint[i] for i in range(7)] txdata += [mvvelo, mvacc, mvradii] if only_check_type <= 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_JOINTB, txdata, 10, feedback_key=feedback_key) else: byte_data = bytes([only_check_type]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_JOINTB, txdata, 10, byte_data, 3, timeout=10, feedback_key=feedback_key) def move_gohome(self, mvvelo, mvacc, mvtime, only_check_type=0, feedback_key=None): txdata = [mvvelo, mvacc, mvtime] if only_check_type <= 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_HOME, txdata, 3, feedback_key=feedback_key) else: byte_data = bytes([only_check_type]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_HOME, txdata, 3, byte_data, 3, timeout=10, feedback_key=feedback_key) def move_servoj(self, mvjoint, mvvelo, mvacc, mvtime): txdata = [mvjoint[i] for i in range(7)] txdata += [mvvelo, mvacc, mvtime] return self.set_nfp32(XCONF.UxbusReg.MOVE_SERVOJ, txdata, 10) def move_servo_cartesian(self, mvpose, mvvelo, mvacc, mvtime): txdata = [mvpose[i] for i in range(6)] txdata += [mvvelo, mvacc, mvtime] return self.set_nfp32(XCONF.UxbusReg.MOVE_SERVO_CART, txdata, 9) # # This interface is no longer supported # def set_servot(self, jnt_taus): # txdata = [jnt_taus[i] for i in range(7)] # return self.set_nfp32(XCONF.UxbusReg.SET_SERVOT, txdata, 7) def get_joint_tau(self): return self.get_nfp32(XCONF.UxbusReg.GET_JOINT_TAU, 7) def set_safe_level(self, level): txdata = [level] return self.set_nu8(XCONF.UxbusReg.SET_SAFE_LEVEL, txdata, 1) def get_safe_level(self): return self.get_nu8(XCONF.UxbusReg.GET_SAFE_LEVEL, 1) def sleep_instruction(self, sltime): txdata = [sltime] return self.set_nfp32(XCONF.UxbusReg.SLEEP_INSTT, txdata, 1) def move_circle(self, pose1, pose2, mvvelo, mvacc, mvtime, percent, only_check_type=0): txdata = [0] * 16 for i in range(6): txdata[i] = pose1[i] txdata[6 + i] = pose2[i] txdata[12] = mvvelo txdata[13] = mvacc txdata[14] = mvtime txdata[15] = percent if only_check_type <= 0: return self.set_nfp32(XCONF.UxbusReg.MOVE_CIRCLE, txdata, 16) else: byte_data = bytes([only_check_type]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_CIRCLE, txdata, 16, byte_data, 3, timeout=10) def move_circle_common(self, pose1, pose2, mvvelo, mvacc, mvtime, percent, coord=0, is_axis_angle=False, only_check_type=0, feedback_key=None): """ 通用指令, 固件1.10.0开始支持 """ txdata = [0] * 16 for i in range(6): txdata[i] = pose1[i] txdata[6 + i] = pose2[i] txdata[12] = mvvelo txdata[13] = mvacc txdata[14] = mvtime txdata[15] = percent byte_data = bytes([coord, int(is_axis_angle), only_check_type]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.MOVE_CIRCLE, txdata, 16, byte_data, 3, timeout=10, feedback_key=feedback_key) def set_tcp_jerk(self, jerk): txdata = [jerk] return self.set_nfp32(XCONF.UxbusReg.SET_TCP_JERK, txdata, 1) def set_tcp_maxacc(self, acc): txdata = [acc] return self.set_nfp32(XCONF.UxbusReg.SET_TCP_MAXACC, txdata, 1) def set_joint_jerk(self, jerk): txdata = [jerk] return self.set_nfp32(XCONF.UxbusReg.SET_JOINT_JERK, txdata, 1) def set_joint_maxacc(self, acc): txdata = [acc] return self.set_nfp32(XCONF.UxbusReg.SET_JOINT_MAXACC, txdata, 1) def set_tcp_offset(self, pose_offset): return self.set_nfp32(XCONF.UxbusReg.SET_TCP_OFFSET, pose_offset, 6) def set_tcp_load(self, load_mass, load_com, feedback_key=None): param_list = [load_mass] param_list.extend(load_com) return self.set_nfp32(XCONF.UxbusReg.SET_LOAD_PARAM, param_list, 4, feedback_key=feedback_key, feedback_type=XCONF.FeedbackType.TRIGGER) def set_collis_sens(self, value): txdata = [value] return self.set_nu8(XCONF.UxbusReg.SET_COLLIS_SENS, txdata, 1) def set_teach_sens(self, value): txdata = [value] return self.set_nu8(XCONF.UxbusReg.SET_TEACH_SENS, txdata, 1) def set_gravity_dir(self, gravity_dir): return self.set_nfp32(XCONF.UxbusReg.SET_GRAVITY_DIR, gravity_dir, 3) def clean_conf(self): return self.set_nu8(XCONF.UxbusReg.CLEAN_CONF, 0, 0) def save_conf(self): return self.set_nu8(XCONF.UxbusReg.SAVE_CONF, 0, 0) def get_joint_pos(self): return self.get_nfp32(XCONF.UxbusReg.GET_JOINT_POS, 7) def get_joint_states(self, num=3): return self.get_nfp32_with_datas(XCONF.UxbusReg.GET_JOINT_POS, [num], 1, 7 * (num & 0x0F)) def get_tcp_pose(self): return self.get_nfp32(XCONF.UxbusReg.GET_TCP_POSE, 6) def get_ik(self, pose, limited=True, ref_angles=None): if not limited or ref_angles is not None: additional_bytes = bytes([1]) if limited else bytes([0]) if ref_angles is not None: additional_bytes += BytesData.from_fp32_list(ref_angles, 7) return self.swop_nfp32(XCONF.UxbusReg.GET_IK, pose, 6, 7, additional_bytes=additional_bytes) else: return self.swop_nfp32(XCONF.UxbusReg.GET_IK, pose, 6, 7) def get_fk(self, angles): return self.swop_nfp32(XCONF.UxbusReg.GET_FK, angles, 7, 6) def is_joint_limit(self, joint): return self.is_nfp32(XCONF.UxbusReg.IS_JOINT_LIMIT, joint, 7) def is_tcp_limit(self, pose): return self.is_nfp32(XCONF.UxbusReg.IS_TCP_LIMIT, pose, 6) @lock_require def gripper_addr_w16(self, addr, value): return self.tgpio_addr_w16(addr, value, bid=XCONF.GRIPPER_ID) @lock_require def gripper_addr_r16(self, addr): return self.tgpio_addr_r16(addr, bid=XCONF.GRIPPER_ID) @lock_require def gripper_addr_w32(self, addr, value): return self.tgpio_addr_w32(addr, value, bid=XCONF.GRIPPER_ID) @lock_require def gripper_addr_r32(self, addr): return self.tgpio_addr_r32(addr, bid=XCONF.GRIPPER_ID) def gripper_set_en(self, value): return self.gripper_addr_w16(XCONF.ServoConf.CON_EN, value) def gripper_set_mode(self, value): return self.gripper_addr_w16(XCONF.ServoConf.CON_MODE, value) def gripper_set_zero(self): return self.gripper_addr_w16(XCONF.ServoConf.MT_ZERO, 1) def gripper_get_pos(self): return self.gripper_addr_r32(XCONF.ServoConf.CURR_POS) def gripper_set_pos(self, pulse): return self.gripper_addr_w32(XCONF.ServoConf.TAGET_POS, pulse) def gripper_set_posspd(self, speed): return self.gripper_addr_w16(XCONF.ServoConf.POS_SPD, speed) def gripper_get_errcode(self): ret = self.get_nu8(XCONF.UxbusReg.TGPIO_ERR, 2) return ret def gripper_clean_err(self): return self.gripper_addr_w16(XCONF.ServoConf.RESET_ERR, 1) @lock_require def tgpio_addr_w16(self, addr, value, bid=XCONF.ROBOT_RS485_HOST_ID, additional_bytes=None): txdata = bytes([bid]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_fp32(value) if additional_bytes is not None: txdata += additional_bytes ret = self.send_modbus_request(XCONF.UxbusReg.TGPIO_W16B, txdata, 7 + len(additional_bytes)) else: ret = self.send_modbus_request(XCONF.UxbusReg.TGPIO_W16B, txdata, 7) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.TGPIO_W16B, ret, 0, self._G_TOUT) return ret @lock_require def tgpio_addr_r16(self, addr, bid=XCONF.ROBOT_RS485_HOST_ID, fmt='>i'): txdata = bytes([bid]) txdata += BytesData.from_u16(addr) ret = self.send_modbus_request(XCONF.UxbusReg.TGPIO_R16B, txdata, 3) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.TGPIO_R16B, ret, 4, self._G_TOUT) return [ret[0], BytesData.to_num(ret[1:5], fmt=fmt)] @lock_require def tgpio_addr_w32(self, addr, value, bid=XCONF.ROBOT_RS485_HOST_ID): txdata = bytes([bid]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_fp32(value) ret = self.send_modbus_request(XCONF.UxbusReg.TGPIO_W32B, txdata, 7) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.TGPIO_W32B, ret, 0, self._G_TOUT) return ret @lock_require def tgpio_addr_r32(self, addr, bid=XCONF.ROBOT_RS485_HOST_ID, fmt='>i'): txdata = bytes([bid]) txdata += BytesData.from_u16(addr) ret = self.send_modbus_request(XCONF.UxbusReg.TGPIO_R32B, txdata, 3) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.TGPIO_R32B, ret, 4, self._G_TOUT) return [ret[0], BytesData.to_num(ret[1:5], fmt=fmt)] def tgpio_get_digital(self): ret = self.tgpio_addr_r16(XCONF.ServoConf.DIGITAL_IN) value = [0] * 5 value[0] = ret[0] value[1] = ret[1] & 0x0001 value[2] = (ret[1] & 0x0002) >> 1 value[3] = (ret[1] & 0x0004) >> 2 value[4] = (ret[1] & 0x0008) >> 3 return value def tgpio_set_digital(self, ionum, value, sync=None): tmp = 0 if ionum == 1: tmp = tmp | 0x0100 if value: tmp = tmp | 0x0001 elif ionum == 2: tmp = tmp | 0x0200 if value: tmp = tmp | 0x0002 elif ionum == 3: tmp = tmp | 0x1000 if value: tmp = tmp | 0x0010 elif ionum == 4: tmp = tmp | 0x0400 if value: tmp = tmp | 0x0004 elif ionum == 5: tmp = tmp | 0x0800 if value: tmp = tmp | 0x0008 else: return [-1, -1] if sync is not None: return self.tgpio_addr_w16(XCONF.ServoConf.DIGITAL_OUT, tmp, additional_bytes=bytes([sync])) else: return self.tgpio_addr_w16(XCONF.ServoConf.DIGITAL_OUT, tmp) def tgpio_get_analog1(self): ret = self.tgpio_addr_r16(XCONF.ServoConf.ANALOG_IO1) value = [0] * 2 value[0] = ret[0] value[1] = ret[1] * 3.3 / 4095.0 return value def tgpio_get_analog2(self): ret = self.tgpio_addr_r16(XCONF.ServoConf.ANALOG_IO2) value = [0] * 2 value[0] = ret[0] value[1] = ret[1] * 3.3 / 4095.0 return value def set_modbus_timeout(self, value, is_transparent_transmission=False): txdata = [int(value)] return self.set_nu16(XCONF.UxbusReg.TGPIO_COM_TIOUT if is_transparent_transmission else XCONF.UxbusReg.TGPIO_MB_TIOUT, txdata, 1) def set_modbus_baudrate(self, baudrate): if baudrate not in self.BAUDRATES: return [-1, -1] ret = self.tgpio_addr_r16(XCONF.ServoConf.MODBUS_BAUDRATE & 0x0FFF) if ret[0] == 0: baud_val = self.BAUDRATES.index(baudrate) if ret[1] != baud_val: # self.tgpio_addr_w16(XCONF.ServoConf.MODBUS_BAUDRATE, baud_val) self.tgpio_addr_w16(0x1A0B, baud_val) time.sleep(0.3) return self.tgpio_addr_w16(XCONF.ServoConf.SOFT_REBOOT, 1) return ret[:2] @lock_require def tgpio_set_modbus(self, modbus_t, len_t, host_id=XCONF.ROBOT_RS485_HOST_ID, limit_sec=0.0, is_transparent_transmission=False, debug=False): txdata = bytes([host_id]) txdata += bytes(modbus_t) if limit_sec > 0: diff_time = time.monotonic() - self._last_modbus_comm_time if diff_time < limit_sec: time.sleep(limit_sec - diff_time) ret = self.send_modbus_request(XCONF.UxbusReg.RS485_AGENT if is_transparent_transmission else XCONF.UxbusReg.RS485_RTU, txdata, len_t + 1, debug=debug) if ret == -1: self._last_modbus_comm_time = time.monotonic() return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.RS485_AGENT if is_transparent_transmission else XCONF.UxbusReg.RS485_RTU, ret, -1, self._G_TOUT, debug=debug) self._last_modbus_comm_time = time.monotonic() return ret @lock_require def tgpio_delay_set_digital(self, ionum, on_off, delay_sec): txdata = bytes([ionum, on_off]) txdata += BytesData.from_fp32(delay_sec) ret = self.send_modbus_request(XCONF.UxbusReg.DELAYED_TGPIO_SET, txdata, 6) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.DELAYED_TGPIO_SET, ret, 0, self._S_TOUT) @lock_require def cgpio_delay_set_digital(self, ionum, on_off, delay_sec): txdata = bytes([ionum, on_off]) txdata += BytesData.from_fp32(delay_sec) ret = self.send_modbus_request(XCONF.UxbusReg.DELAYED_CGPIO_SET, txdata, 6) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.DELAYED_CGPIO_SET, ret, 0, self._S_TOUT) @lock_require def cgpio_position_set_digital(self, ionum, on_off, xyz, tol_r): txdata = bytes([ionum, on_off]) txdata += BytesData.from_fp32_list(xyz, 3) txdata += BytesData.from_fp32(tol_r) ret = self.send_modbus_request(XCONF.UxbusReg.POSITION_CGPIO_SET, txdata, 18) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.POSITION_CGPIO_SET, ret, 0, self._S_TOUT) @lock_require def tgpio_position_set_digital(self, ionum, on_off, xyz, tol_r): txdata = bytes([ionum, on_off]) txdata += BytesData.from_fp32_list(xyz, 3) txdata += BytesData.from_fp32(tol_r) ret = self.send_modbus_request(XCONF.UxbusReg.POSITION_TGPIO_SET, txdata, 18) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.POSITION_TGPIO_SET, ret, 0, self._S_TOUT) @lock_require def cgpio_position_set_analog(self, ionum, value, xyz, tol_r): txdata = bytes([ionum]) txdata += BytesData.from_u16(int(value / 10.0 * 4095.0)) txdata += BytesData.from_fp32_list(xyz, 3) txdata += BytesData.from_fp32(tol_r) ret = self.send_modbus_request(XCONF.UxbusReg.POSITION_CGPIO_SET_ANALOG, txdata, 19) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.POSITION_CGPIO_SET_ANALOG, ret, 0, self._S_TOUT) # io_type: 0 for CGPIO, 1 for TGPIO def config_io_stop_reset(self, io_type, on_off): txdata = [io_type, on_off] return self.set_nu8(XCONF.UxbusReg.SET_IO_STOP_RESET, txdata, 2) def gripper_modbus_w16s(self, addr, value, count): txdata = bytes([XCONF.GRIPPER_ID]) txdata += bytes([0x10]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_u16(count) txdata += bytes([count * 2]) txdata += value ret = self.tgpio_set_modbus_func(txdata, count * 2 + 7) return ret def gripper_modbus_r16s(self, addr, count): txdata = bytes([XCONF.GRIPPER_ID]) txdata += bytes([0x03]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_u16(count) ret = self.tgpio_set_modbus_func(txdata, 6) return ret def gripper_modbus_set_en(self, value): value = BytesData.from_u16(int(value)) return self.gripper_modbus_w16s(XCONF.ServoConf.CON_EN, value, 1) def gripper_modbus_set_mode(self, value): value = BytesData.from_u16(int(value)) return self.gripper_modbus_w16s(XCONF.ServoConf.CON_MODE, value, 1) def gripper_modbus_set_zero(self): value = BytesData.from_u16(int(1)) return self.gripper_modbus_w16s(XCONF.ServoConf.MT_ZERO, value, 1) def gripper_modbus_get_pos(self): ret = self.gripper_modbus_r16s(XCONF.ServoConf.CURR_POS, 2) ret1 = [0] * 2 ret1[0] = ret[0] if ret[0] in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE] and len(ret) == 9: ret1[1] = BytesData.to_s32(ret[5:9]) else: if ret1[0] == 0: ret1[0] = XCONF.UxbusState.ERR_LENG # print('gripper_modbus_get_pos:', len(ret), ret) # print(ret1, ret) return ret1 def gripper_modbus_set_pos(self, pulse): value = bytes([(int(pulse) >> 24) & 0xFF]) value += bytes([(int(pulse) >> 16) & 0xFF]) value += bytes([(int(pulse) >> 8) & 0xFF]) value += bytes([int(pulse) & 0xFF]) return self.gripper_modbus_w16s(XCONF.ServoConf.TAGET_POS, value, 2) def gripper_modbus_set_posspd(self, speed): speed = BytesData.from_u16(int(speed)) return self.gripper_modbus_w16s(XCONF.ServoConf.POS_SPD, speed, 1) def gripper_modbus_get_errcode(self): ret = self.gripper_modbus_r16s(XCONF.ServoConf.ERR_CODE, 1) ret1 = [0] * 2 ret1[0] = ret[0] if ret[0] in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE] and len(ret) == 7: ret1[1] = BytesData.to_u16(ret[5:7]) else: if ret1[0] == 0: ret1[0] = XCONF.UxbusState.ERR_LENG # print('gripper_modbus_get_errcode:', len(ret), ret) # print(ret1, ret) return ret1 def gripper_modbus_clean_err(self): value = BytesData.from_u16(int(1)) return self.gripper_modbus_w16s(XCONF.ServoConf.RESET_ERR, value, 1) def servo_set_zero(self, axis_id): txdata = [int(axis_id)] ret = self.set_nu8(XCONF.UxbusReg.SERVO_ZERO, txdata, 1) return ret def servo_get_dbmsg(self): ret = self.get_nu8(XCONF.UxbusReg.SERVO_DBMSG, 16) return ret @lock_require def servo_addr_w16(self, axis_id, addr, value): txdata = bytes([axis_id]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_fp32(value) ret = self.send_modbus_request(XCONF.UxbusReg.SERVO_W16B, txdata, 7) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.SERVO_W16B, ret, 0, self._G_TOUT) return ret @lock_require def servo_addr_r16(self, axis_id, addr): txdata = bytes([axis_id]) txdata += BytesData.from_u16(addr) ret = self.send_modbus_request(XCONF.UxbusReg.SERVO_R16B, txdata, 3) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.SERVO_R16B, ret, 4, self._G_TOUT) return [ret[0], BytesData.to_s32(ret[1:5])] @lock_require def servo_addr_w32(self, axis_id, addr, value): txdata = bytes([axis_id]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_fp32(value) ret = self.send_modbus_request(XCONF.UxbusReg.SERVO_W32B, txdata, 7) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.SERVO_W32B, ret, 0, self._G_TOUT) return ret @lock_require def servo_addr_r32(self, axis, addr): txdata = bytes([axis]) txdata += BytesData.from_u16(addr) ret = self.send_modbus_request(XCONF.UxbusReg.SERVO_R32B, txdata, 3) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.SERVO_R32B, ret, 4, self._G_TOUT) return [ret[0], BytesData.to_s32(ret[1:5])] # ----------------------------------------------------- # controler gpio # ----------------------------------------------------- def cgpio_get_auxdigit(self): ret = self.get_nu16(XCONF.UxbusReg.CGPIO_GET_DIGIT, 1) value = [0] * 2 value[0] = ret[0] value[1] = ret[1] return value def cgpio_get_analog1(self): ret = self.get_nu16(XCONF.UxbusReg.CGPIO_GET_ANALOG1, 1) value = [0] * 2 value[0] = ret[0] value[1] = ret[1] * 10.0 / 4095.0 return value def cgpio_get_analog2(self): ret = self.get_nu16(XCONF.UxbusReg.CGPIO_GET_ANALOG2, 1) value = [0] * 2 value[0] = ret[0] value[1] = ret[1] * 10.0 / 4095.0 return value def cgpio_set_auxdigit(self, ionum, value, sync=None): tmp = [0] * 2 if ionum > 7: tmp[1] = tmp[1] | (0x0100 << (ionum - 8)) if value: tmp[1] = tmp[1] | (0x0001 << (ionum - 8)) if sync is not None: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_DIGIT, tmp, 2, additional_bytes=bytes([sync])) else: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_DIGIT, tmp, 2) else: tmp[0] = tmp[0] | (0x0100 << ionum) if value: tmp[0] = tmp[0] | (0x0001 << ionum) if sync is not None: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_DIGIT, tmp, 1, additional_bytes=bytes([sync])) else: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_DIGIT, tmp, 1) # return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_DIGIT, tmp, 2 if ionum > 7 else 1) def cgpio_set_analog1(self, value, sync=None): txdata = [int(value / 10.0 * 4095.0)] if sync is not None: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_ANALOG1, txdata, 1, additional_bytes=bytes([sync])) else: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_ANALOG1, txdata, 1) def cgpio_set_analog2(self, value, sync=None): txdata = [int(value / 10.0 * 4095.0)] if sync is not None: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_ANALOG2, txdata, 1, additional_bytes=bytes([sync])) else: return self.set_nu16(XCONF.UxbusReg.CGPIO_SET_ANALOG2, txdata, 1) def cgpio_set_infun(self, num, fun): txdata = [int(num), int(fun)] return self.set_nu8(XCONF.UxbusReg.CGPIO_SET_IN_FUN, txdata, 2) def cgpio_set_outfun(self, num, fun): txdata = [int(num), int(fun)] return self.set_nu8(XCONF.UxbusReg.CGPIO_SET_OUT_FUN, txdata, 2) def cgpio_get_state(self): # ret = self.get_nu8(XCONF.UxbusReg.CGPIO_GET_STATE, 34) # ret = self.get_nu8(XCONF.UxbusReg.CGPIO_GET_STATE, 50) ret = self.get_nu8(XCONF.UxbusReg.CGPIO_GET_STATE, -1) msg = [0] * 13 msg[0] = ret[0] msg[1] = ret[1] msg[2] = ret[2] msg[3:11] = BytesData.to_u16_list(ret[3:19], 8) msg[7] = msg[7] / 4095.0 * 10.0 msg[8] = msg[8] / 4095.0 * 10.0 msg[9] = msg[9] / 4095.0 * 10.0 msg[10] = msg[10] / 4095.0 * 10.0 msg[11] = ret[19:27] msg[12] = ret[27:35] if len(ret) >= 50: msg[11] = ret[19:27] + ret[35:43] msg[12] = ret[27:35] + ret[43:51] return msg def set_self_collision_detection(self, on_off): txdata = [on_off] return self.set_nu8(XCONF.UxbusReg.SET_SELF_COLLIS_CHECK, txdata, 1) def set_collision_tool_model(self, tool_type, params): if len(params) > 0: byte_data = bytes([tool_type]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.SET_COLLIS_TOOL, params, len(params), byte_data) else: txdata = [tool_type] return self.set_nu8(XCONF.UxbusReg.SET_COLLIS_TOOL, txdata, 1) def set_simulation_robot(self, on_off): txdata = [int(on_off)] return self.set_nu8(XCONF.UxbusReg.SET_SIMULATION_ROBOT, txdata, 1) def get_power_board_version(self): return self.get_nu8(XCONF.UxbusReg.GET_PWR_VERSION, 6) def get_movement(self): return self.get_nu8(XCONF.UxbusReg.GET_MOVEMENT, 1) def vc_set_jointv(self, jnt_v, jnt_sync, duration=-1): additional_bytes = bytes([jnt_sync]) if duration >= 0: additional_bytes += BytesData.from_fp32(duration) return self.set_nfp32_with_bytes(XCONF.UxbusReg.VC_SET_JOINTV, jnt_v, 7, additional_bytes) def vc_set_linev(self, line_v, coord, duration=-1): additional_bytes = bytes([coord]) if duration >= 0: additional_bytes += BytesData.from_fp32(duration) return self.set_nfp32_with_bytes(XCONF.UxbusReg.VC_SET_CARTV, line_v, 6, additional_bytes) def iden_load(self, iden_type, num_get, timeout=500, estimated_mass=0): txdata = bytes([iden_type]) if estimated_mass > 0: txdata += BytesData.from_fp32(estimated_mass) return self.get_nfp32_with_datas(XCONF.UxbusReg.IDEN_LOAD, txdata, 5 if estimated_mass > 0 else 1, num_get, timeout=timeout) def iden_joint_friction(self, sn, timeout=500): txdata = [ord(i) for i in list(sn)] return self.get_nfp32_with_datas(XCONF.UxbusReg.IDEN_FRIC, txdata, 14, 1, timeout=timeout) @lock_require def set_admittance(self, coord, c_axis, M, K, B): txdata = bytes([coord]) txdata += bytes(c_axis[:6]) txdata += BytesData.from_fp32_list(M, 6) txdata += BytesData.from_fp32_list(K, 6) txdata += BytesData.from_fp32_list(B, 6) ret = self.send_modbus_request(XCONF.UxbusReg.ADMITTANCE_CONFIG, txdata, 79) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.ADMITTANCE_CONFIG, ret, 0, self._S_TOUT) @lock_require def set_admittance_mbk(self, M, K, B): txdata = BytesData.from_fp32_list(M, 6) txdata += BytesData.from_fp32_list(K, 6) txdata += BytesData.from_fp32_list(B, 6) ret = self.send_modbus_request(XCONF.UxbusReg.ADMITTANCE_CTRL_MKB, txdata, 72) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.ADMITTANCE_CTRL_MKB, ret, 0, self._S_TOUT) @lock_require def set_admittance_config(self, coord, c_axis): txdata = bytes([coord]) txdata += bytes(c_axis[:6]) ret = self.send_modbus_request(XCONF.UxbusReg.ADMITTANCE_CTRL_CONFIG, txdata, 7) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.ADMITTANCE_CTRL_CONFIG, ret, 0, self._S_TOUT) @lock_require def config_force_control(self, coord, c_axis, f_ref, limits): txdata = bytes([coord]) txdata += bytes(c_axis[:6]) txdata += BytesData.from_fp32_list(f_ref, 6) txdata += BytesData.from_fp32_list(limits, 6) ret = self.send_modbus_request(XCONF.UxbusReg.FORCE_CTRL_CONFIG, txdata, 55) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.FORCE_CTRL_CONFIG, ret, 0, self._S_TOUT) @lock_require def set_force_control_pid(self, kp, ki, kd, xe_limit): txdata = BytesData.from_fp32_list(kp, 6) txdata += BytesData.from_fp32_list(ki, 6) txdata += BytesData.from_fp32_list(kd, 6) txdata += BytesData.from_fp32_list(xe_limit, 6) ret = self.send_modbus_request(XCONF.UxbusReg.FORCE_CTRL_PID, txdata, 96) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.FORCE_CTRL_PID, ret, 0, self._S_TOUT) def ft_sensor_set_zero(self): return self.set_nu8(XCONF.UxbusReg.FTSENSOR_SET_ZERO, 0, 0) # return self.getset_nu8(XCONF.UxbusReg.FTSENSOR_SET_ZERO, [], 0, 1) def ft_sensor_iden_load(self): return self.iden_load(0, 10) def ft_sensor_cali_load(self, iden_result_list): return self.set_nfp32(XCONF.UxbusReg.FTSENSOR_SET_LOAD_OFFSET, iden_result_list, 10) def ft_sensor_enable(self, on_off): txdata = [on_off] return self.set_nu8(XCONF.UxbusReg.FTSENSOR_ENABLE, txdata, 1) def ft_sensor_app_set(self, app_code): txdata = [app_code] return self.set_nu8(XCONF.UxbusReg.FTSENSOR_SET_MODE, txdata, 1) def ft_sensor_app_get(self): return self.get_nu8(XCONF.UxbusReg.FTSENSOR_GET_MODE, 1) def ft_sensor_get_data(self, is_new=True, is_raw=False): if is_new and is_raw: return self.get_nfp32_with_datas(XCONF.UxbusReg.FTSENSOR_GET_DATA, [1], 1, 6) else: return self.get_nfp32(XCONF.UxbusReg.FTSENSOR_GET_DATA if is_new else XCONF.UxbusReg.FTSENSOR_GET_DATA_OLD, 6) def ft_sensor_get_config(self): ret = self.get_nu8(XCONF.UxbusReg.FTSENSOR_GET_CONFIG, 280) if ret[0] in [0, 1, 2]: ft_app_status = ret[1] ft_started = ret[2] ft_type = ret[3] ft_id = ret[4] ft_freq = BytesData.to_u16(ret[5:7]) ft_mass = BytesData.to_fp32(ret[7:11]) ft_dir_bias = BytesData.to_fp32(ret[11:15]) ft_centroid = BytesData.to_fp32_list(ret[15:27], 3) ft_zero = BytesData.to_fp32_list(ret[27:51], 6) imp_coord = ret[51] imp_c_axis = ret[52:58] M = BytesData.to_fp32_list(ret[58:82], 6) K = BytesData.to_fp32_list(ret[82:106], 6) B = BytesData.to_fp32_list(ret[106:130], 6) fc_coord = ret[130] fc_c_axis = ret[131:137] force_ref = BytesData.to_fp32_list(ret[137:161], 6) limits = BytesData.to_fp32_list(ret[161:185], 6) kp = BytesData.to_fp32_list(ret[185:209], 6) ki = BytesData.to_fp32_list(ret[209:233], 6) kd = BytesData.to_fp32_list(ret[233:257], 6) xe_limit = BytesData.to_fp32_list(ret[257:281], 6) return [ ret[0], ft_app_status, ft_started, ft_type, ft_id, ft_freq, ft_mass, ft_dir_bias, ft_centroid, ft_zero, imp_coord, imp_c_axis, M, K, B, fc_coord, fc_c_axis, force_ref, limits, kp, ki, kd, xe_limit ] return ret # def ft_sensor_get_error(self): # ret = self.servo_addr_r16(8, 0x0010) # if ret[0] in [0, 1, 2] and len(ret) > 1 and ret[1] == 27: # ret[0] = 0 # return ret @lock_require def ft_sensor_get_error(self, is_new=True): txdata = bytes([8]) txdata += BytesData.from_u16(0x0010) ret = self.send_modbus_request(XCONF.UxbusReg.SERVO_R16B, txdata, 3) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.SERVO_R16B, ret, 4, self._G_TOUT) if ret[0] in [0, 1, 2]: if BytesData.to_s32(ret[1:5]) == 27: return [ret[0], 0] else: if is_new: return [ret[0], BytesData.to_u16(ret[3:5])] else: return [ret[0], ret[3]] return [ret[0], 0] def cali_tcp_pose(self, four_pnts): txdata = [] for k in range(4): txdata += [four_pnts[k][i] for i in range(6)] return self.swop_nfp32(XCONF.UxbusReg.CALI_TCP_POSE, txdata, 24, 3) # default: mode: x+ then y+; trust_ind: trust x+ dir def cali_user_orient(self, three_pnts, mode=0, trust_ind=0): txdata = [] for k in range(3): txdata += [three_pnts[k][i] for i in range(6)] byte_data = bytes([mode, trust_ind]) rxn = 3 ret = self.set_nfp32_with_bytes(XCONF.UxbusReg.CALI_WRLD_ORIENT, txdata, 18, byte_data, rxn * 4) data = [0] * (1 + rxn) data[0] = ret[0] data[1:rxn+1] = BytesData.to_fp32_list(ret[1:rxn * 4 + 1], rxn) return data def cali_tcp_orient(self, rpy_be, rpy_bt): txdata = [rpy_be[i] for i in range(3)] txdata += [rpy_bt[i] for i in range(3)] return self.swop_nfp32(XCONF.UxbusReg.CALI_TCP_ORIENT, txdata, 6, 3) def cali_user_pos(self, rpy_ub, pos_b_uorg): txdata = [rpy_ub[i] for i in range(3)] txdata += [pos_b_uorg[i] for i in range(3)] return self.swop_nfp32(XCONF.UxbusReg.CALI_WRLD_POSE, txdata, 6, 3) def get_tcp_rotation_radius(self, value): txdata = [value] data = [0] * 2 ret = self.getset_nu8(XCONF.UxbusReg.GET_TCP_ROTATION_RADIUS, txdata, 1, 4) data[0] = ret[0] data[1] = BytesData.to_fp32_list(ret[1:], 1) return data def get_max_joint_velocity(self, eveloc, joint_pos): txdata = [eveloc] txdata += [joint_pos[i] for i in range(7)] return self.swop_nfp32(XCONF.UxbusReg.GET_MAX_JOINT_VELOCITY, txdata, 8, 1) def linear_motor_modbus_w16s(self, addr, value, length): txdata = bytes([XCONF.LINEAR_MOTOR_ID]) txdata += bytes([0x10]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_u16(length) txdata += bytes([length * 2]) txdata += value ret = self.tgpio_set_modbus_func(txdata, length * 2 + 7, host_id=XCONF.CONTROL_BOX_RS485_HOST_ID, limit_sec=0.001) return ret def linear_motor_modbus_r16s(self, addr, length, fcode=0x03): txdata = bytes([XCONF.LINEAR_MOTOR_ID]) txdata += bytes([fcode]) txdata += BytesData.from_u16(addr) txdata += BytesData.from_u16(length) ret = self.tgpio_set_modbus_func(txdata, 6, host_id=XCONF.CONTROL_BOX_RS485_HOST_ID, limit_sec=0.001) return ret def iden_tcp_load(self, estimated_mass=0): return self.iden_load(1, 4, timeout=300, estimated_mass=estimated_mass) @lock_require def servo_error_addr_r32(self, axis, addr): txdata = bytes([axis]) txdata += BytesData.from_u16(addr) ret = self.send_modbus_request(XCONF.UxbusReg.SERVO_ERROR, txdata, 3) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] * (7 + 1) ret = self.recv_modbus_response(XCONF.UxbusReg.SERVO_ERROR, ret, 4, self._G_TOUT) return [ret[0], BytesData.to_s32(ret[1:5])] def get_dh_params(self): return self.get_nfp32(XCONF.UxbusReg.GET_DH, 28) def set_dh_params(self, dh_params, flag=0): if len(dh_params) < 28: dh_params.extend([0] * 28 - len(dh_params)) byte_data = bytes([flag]) return self.set_nfp32_with_bytes(XCONF.UxbusReg.SET_DH, dh_params, 28, byte_data, 1, timeout=10) def _set_feedback_type_no_lock(self, feedback_type): ret = self.send_modbus_request(XCONF.UxbusReg.SET_FEEDBACK_TYPE, [feedback_type], 1) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(XCONF.UxbusReg.SET_FEEDBACK_TYPE, ret, 0, self._S_TOUT) @lock_require def set_feedback_type(self, feedback_type): ret = self._set_feedback_type_no_lock(feedback_type) if ret[0] != XCONF.UxbusState.ERR_NOTTCP: self._feedback_type = feedback_type return ret def check_feedback(self, feedback_key=None): ret = self.set_nu8(XCONF.UxbusReg.FEEDBACK_CHECK, [], 0, feedback_key=feedback_key, feedback_type=XCONF.FeedbackType.MOTION_FINISH) return ret @lock_require def set_modbus_tcp_data(self, datas, raw=True, debug=False): trans_id = BytesData.to_u16(datas[0:2]) if raw else None prot_id = BytesData.to_u16(datas[2:4]) if raw else -1 pdu_len = BytesData.to_u16(datas[4:6]) - 1 if raw else len(datas) - 1 unit_id = datas[6] if raw else datas[0] pdu_data = datas[7:] if raw else datas[1:] if raw and prot_id not in [0, 2, 3]: return [XCONF.UxbusState.ERR_PARAM] # param error if len(pdu_data) != pdu_len: return [XCONF.UxbusState.ERR_PARAM] # param error ret = self.send_modbus_request(unit_id, pdu_data, len(pdu_data), prot_id=prot_id, t_id=trans_id, debug=debug) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(unit_id, ret, -1, self._G_TOUT, t_prot_id=prot_id, ret_raw=True, debug=debug) @lock_require def send_hex_cmd(self, datas, timeout=10): if len(datas) < 7: # datas length error return [-2] trans_id = int('{}{}'.format(datas[0], datas[1]), base=16) prot_id = int('{}{}'.format(datas[2], datas[3]), base=16) if prot_id not in [0, 2, 3]: # protocol_identifier error, only support 0/2/3, # 0: standard modbus protocol # 2: private modbus protocol # 3: private modbus protocol (with heart beat) return [-3] length = int('{}{}'.format(datas[4], datas[5]), base=16) if length != len(datas) - 6: # protocol length data error return [-4] unit_id = int('{}'.format(datas[6]), base=16) pdu_data = bytes.fromhex('{}'.format(''.join(map(str, datas[7:])))) ret = self.send_modbus_request(unit_id, pdu_data, len(pdu_data), prot_id=prot_id, t_id=trans_id) if ret == -1: return [XCONF.UxbusState.ERR_NOTTCP] return self.recv_modbus_response(unit_id, ret, -1, timeout, t_prot_id=prot_id, ret_raw=True) def set_common_param(self, param_type, param_val): txdata = bytes([param_type]) if param_type == 1: txdata += BytesData.from_fp32(param_val) elif param_type in [6, 12, 14]: txdata += BytesData.from_fp32_list(param_val, 6) elif param_type in [26]: txdata += bytes(param_val) if isinstance(param_val, list) else bytes([param_val]) elif param_type in [7]: if isinstance(param_val, list): txdata += BytesData.from_fp32_list(param_val, len(param_val)) else: txdata += BytesData.from_fp32(param_val) else: # 2/3/4/5/11/13/15/24/25 if isinstance(param_val, list): txdata += BytesData.from_s32_list(param_val, len(param_val), is_big_endian=False) else: txdata += BytesData.from_s32(param_val, is_big_endian=False) return self.set_nu8(XCONF.UxbusReg.SET_COMMON_PARAM, txdata, len(txdata)) def get_common_param(self, param_type): txdata = bytes([param_type]) ret = self.getset_nu8(XCONF.UxbusReg.GET_COMMON_PARAM, txdata, 1, -1) data = [0] * 2 data[0] = ret[0] if ret[0] != XCONF.UxbusState.ERR_NOTTCP: if param_type == 1: data[1] = BytesData.to_fp32(ret[1:]) elif param_type in [6, 12, 14]: if len(ret[1:]) < 24: return [XCONF.UxbusState.INVALID, 0] data[1] = BytesData.to_fp32_list(ret[1:], 6) elif param_type in [26]: data[1] = ret[1:] elif param_type in [7]: tmp = BytesData.to_fp32_list(ret[1:], len(ret[1:]) // 4) data[1] = tmp if len(tmp) > 1 else tmp[0] else: # 2/3/4/5/11/13/15/24/25 tmp = BytesData.to_u32_list(ret[1:], len(ret[1:]) // 4) data[1] = tmp if len(tmp) > 1 else tmp[0] # data[1] = BytesData.to_u32(ret[1:]) return data def get_common_info(self, param_type): txdata = bytes([param_type]) ret = self.getset_nu8(XCONF.UxbusReg.GET_COMMON_INFO, txdata, 1, -1) data = [0] * 2 data[0] = ret[0] if ret[0] != XCONF.UxbusState.ERR_NOTTCP: if param_type == 1 or param_type == 2: data[1] = ret[1] elif param_type == 50: data[1] = BytesData.to_fp32(ret[1:]) elif param_type in [101, 107]: data[1] = ret[1] data.append(BytesData.to_fp32(ret[2:6])) data.append(BytesData.to_fp32(ret[6:10])) elif param_type in [102, 104]: data[1] = ret[1] data.append(BytesData.to_fp32(ret[2:])) elif param_type == 105: data[1] = BytesData.to_fp32(ret[1:]) data.append(BytesData.to_fp32(ret[5:])) elif param_type in [103, 106]: data[1] = ret[1] data.extend(BytesData.to_fp32_list(ret[2:], 7)) else: data[0] = XCONF.UxbusState.ERR_PARAM return data def get_traj_speeding(self, rate): txdata = bytes([rate]) ret = self.getset_nu8(XCONF.UxbusReg.GET_TRAJ_SPEEDING, txdata, 1, -1) data = [0] * 4 data[0] = ret[0] if ret[0] != XCONF.UxbusState.ERR_NOTTCP: data[1] = BytesData.to_s32(ret[1:5]) data[2] = BytesData.to_s32(ret[5:9]) data[3] = round(math.degrees(BytesData.to_fp32(ret[9:])),3) return data