From b924ef47b3fdf5e7f7a5efd16d7d06ddae8fdb24 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Fri, 17 Jan 2025 19:56:51 +0800 Subject: [PATCH 1/4] =?UTF-8?q?Update=20v1.0.13=20DMCAN=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/components/EOAT/DMCAN/parameters | 23 +- Controller/driver/EOAT/EOAT.py | 20 +- Controller/driver/EOAT/modules/DMCAN/DMCAN.py | 327 ++++++++++++++---- .../driver/EOAT/modules/DMCAN/defines.py | 9 + 4 files changed, 299 insertions(+), 80 deletions(-) diff --git a/Controller/config/components/EOAT/DMCAN/parameters b/Controller/config/components/EOAT/DMCAN/parameters index 423f705..096a2f0 100644 --- a/Controller/config/components/EOAT/DMCAN/parameters +++ b/Controller/config/components/EOAT/DMCAN/parameters @@ -1,21 +1,22 @@ [CONNECTION] bus = CAN -connections = {"id": 1, "channel_name": "PCAN_USBBUS1", "interface": "pcan", "baud_rate": 1000000, - "control_mode": "POS_VEL"} +connections = {"id": 7, "channel_name": "PCAN_USBBUS1", "interface": "pcan", "baud_rate": 1000000, + "control_mode": "Torque_Pos"} [PARAMETERS] # Motor parameters -motor_angle_limit: 12.5, -motor_velocity_limit: 30, -motor_torque_limit: 10, -motor_kp: 20, -motor_kd: 0.45, +motor_angle_limit: 70 +motor_velocity_limit: 30 +motor_torque_limit: 10 +motor_kp: 20 +motor_kd: 0.45 motor_tau: 0 +motion_complete_limit = 0.1 # Unit parameters -transmission_ratio = 1 -resolution = 1 -ts_s = 1 -i_max_ps = 0.01 +transmission_ratio = 1 +resolution = 1 +ts_s = 1 +i_max_ps = 0.01 [KINEMATICS] x = 0 diff --git a/Controller/driver/EOAT/EOAT.py b/Controller/driver/EOAT/EOAT.py index 9a62a74..fa638e3 100644 --- a/Controller/driver/EOAT/EOAT.py +++ b/Controller/driver/EOAT/EOAT.py @@ -41,6 +41,23 @@ class EOATConfig: if config_parameter not in config_file[config_type]: return False return True + + @staticmethod + def parse_value(value): + # 尝试将值转换为整数 + try: + return int(value) + except ValueError: + pass # 如果转换失败，继续尝试 + + # 尝试将值转换为浮点数 + try: + return float(value) + except ValueError: + pass # 如果转换失败，继续尝试 + + # 如果以上都失败，则返回原始值作为字符串 + return value def __init__(self, name): # Module name and path @@ -89,7 +106,8 @@ class EOATConfig: def __read_parameters(self, file): if self.check_parameter(self.__PARAMETERS, None, file): for key in file[self.__PARAMETERS]: - self.__parameters[key] = file[self.__PARAMETERS][key] + value = file[self.__PARAMETERS][key] + self.__parameters[key] = self.parse_value(value) def __read_kinematics(self, file): x = float(self.read_parameter(self.__KINEMATICS, 'x', file)) diff --git a/Controller/driver/EOAT/modules/DMCAN/DMCAN.py b/Controller/driver/EOAT/modules/DMCAN/DMCAN.py index 79e0603..887aae0 100644 --- a/Controller/driver/EOAT/modules/DMCAN/DMCAN.py +++ b/Controller/driver/EOAT/modules/DMCAN/DMCAN.py @@ -4,20 +4,24 @@ import time import threading import queue import copy +import numpy +import math # Location from Controller.driver.EOAT.modules.DMCAN.defines import * +from Controller.driver.trajectory.trajectory import Trajectory # Utils from Controller.utils.error import * from Controller import logger +from Controller.utils.tools import write_multi_float # Local Thirdparty from SitePackages import can # /********************************************************************************************************************/ -# /********************************************** DMBots CAN ************************************************/ +# /*********************************************** DMBots CAN ***********************************************/ # /********************************************************************************************************************/ class DriverStatus(object): DISABLE = 0x00 @@ -325,7 +329,7 @@ class Sync(threading.Thread): self.__motor = parent # CAN bus handle self.__bus = bus - self.__config = config + # self.__config = config # Active flag self.__is_active = False # Function Flag @@ -345,11 +349,18 @@ class Sync(threading.Thread): self.__target_position = 0 self.__target_velocity = 0 self.__target_acceleration = 0 + self.__target_tau = config.get('motor_torque_limit') * 0.8 self.__pvts = list() # Config variable - self.__motor_control_mode = self.__config.get('control_mode') - self.__motion_complete_limit = self.__config.get('motion_complete_limit') - self.__control_factors = self.__config.get('control_factors') + # self.__motor_control_mode = self.__config.get('control_mode') + # self.__motion_complete_limit = self.__config.get('motion_complete_limit') + # self.__control_factors = self.__config.get('control_factors') + self.__motor_control_mode = self.__motor.motor_control_mode + if config.get('motion_complete_limit') is None: + self.__motion_complete_limit = 0.1 + else: + self.__motion_complete_limit = config.get('motion_complete_limit') + self.__control_factors = config # Default Control factor self.__motor_angle_limit = 0.0 self.__motor_velocity_limit = 0.0 @@ -357,6 +368,12 @@ class Sync(threading.Thread): self.__motor_kp = 0.0 self.__motor_kd = 0.0 self.__motor_tau = 0.0 + self.__motor_i_des = 0.8 + + self.__gripper_catch_count = 0 + self.__gripper_falling_count = 0 + self.__initialize_gripper_mode = None + self.__initialize_gripper_count = 0 @property def bus(self): @@ -407,7 +424,6 @@ class Sync(threading.Thread): :return: """ - def limit_min_max(value, min_value, max_value): return max(min_value, min(value, max_value)) @@ -420,9 +436,12 @@ class Sync(threading.Thread): def decode_response_frame(data): status = (data[0] & 0xF0) >> 4 - position = uint_to_float((data[1] << 8) + data[2], -12.5, 12.5, 16) - velocity = uint_to_float((data[3] << 4) + (data[4] >> 4), -30, 30, 12) - torque = uint_to_float(((data[4] & 0x0F) << 8) + data[5], -10, 10, 12) + # position = uint_to_float((data[1] << 8) + data[2], -12.5, 12.5, 16) + # velocity = uint_to_float((data[3] << 4) + (data[4] >> 4), -30, 30, 12) + # torque = uint_to_float(((data[4] & 0x0F) << 8) + data[5], -10, 10, 12) + position = uint_to_float((data[1] << 8) + data[2], -self.__motor_angle_limit, self.__motor_angle_limit, 16) + velocity = uint_to_float((data[3] << 4) + (data[4] >> 4), -self.__motor_velocity_limit, self.__motor_velocity_limit, 12) + torque = uint_to_float(((data[4] & 0x0F) << 8) + data[5], -self.__motor_torque_limit, self.__motor_torque_limit, 12) mos_temperature = data[6] motor_coil_temperature = data[7] return status, position, velocity, torque, mos_temperature, motor_coil_temperature @@ -484,7 +503,7 @@ class Sync(threading.Thread): """ return write_multi_float(velocity, 4, True) - def torque_pos_encode_send_frame(position, velocity): + def torque_pos_encode_send_frame(position, velocity, i_des): """ VEL Control Mode Function @@ -493,19 +512,19 @@ class Sync(threading.Thread): :return: data_buf: encode_send_frame """ - # TODO 有个电流标幺值没有填入，现使用固定值 8000 + # i_des 0.8以上过流, 及不能超出8000 data_buf = numpy.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], numpy.uint8) Pos_desired_uint8s = write_multi_float(position, 4, True) data_buf[0:4] = Pos_desired_uint8s - Vel_uint = numpy.uint16(velocity * 100) - ides_uint = numpy.uint16(8000) + Vel_uint = numpy.int16(velocity * 100) + ides_uint = numpy.uint16(i_des * 10000) data_buf[4] = Vel_uint & 0xff data_buf[5] = Vel_uint >> 8 data_buf[6] = ides_uint & 0xff data_buf[7] = ides_uint >> 8 return data_buf - def send_data(position, velocity, kp=0.0, kd=0.0, tau=0.0): + def send_data(position, velocity, kp=0.0, kd=0.0, tau=0.0, i_des=0.8): """ VEL Control Mode Function @@ -527,13 +546,13 @@ class Sync(threading.Thread): self.__motor_torque_limit) elif self.__motor_control_mode == POS_VEL: # 位置速度模式 - data_buf = pos_vel_encode_send_frame(self.__target_position, velocity) + data_buf = pos_vel_encode_send_frame(position, velocity) elif self.__motor_control_mode == VEL: # 速度模式 data_buf = vel_encode_send_frame(velocity) elif self.__motor_control_mode == Torque_Pos: # 力位混合模式 - data_buf = torque_pos_encode_send_frame(self.__target_position, velocity) + data_buf = torque_pos_encode_send_frame(position, velocity, i_des) response = self.__bus.transmit(data_buf) @@ -541,25 +560,92 @@ class Sync(threading.Thread): self.__velocity, self.__torque, \ self.__mos_temperature, self.__motor_coil_temperature = decode_response_frame(response) - def process_sync_mode(last_velocity): + def determine_current_state(): """ - Process Sync Mode + Determine current state :return: """ - if self.__motor_control_mode == POS_VEL or self.__motor_control_mode == Torque_Pos: - target_velocity = copy.deepcopy(last_velocity) - else: - target_velocity = 0.0 - target_position = self.__target_position - - send_data(target_position, target_velocity, self.__motor_kp, self.__motor_kd, self.__motor_tau) - if math.fabs(self.__target_position - self.__position) < self.__motion_complete_limit: self.__motion_completed = True + if self.__initialize_gripper_mode is not None: + if self.__initialize_gripper_mode == ALL_INITIALIZE: + self.__motor.grip_angle_min = self.__position + grip_angle_max = self.__motor.grip_angle_min + self.__motor_angle_limit + if grip_angle_max > self.__motor_angle_limit: + self.__motor.grip_angle_max = self.__motor_angle_limit + else: + self.__motor.grip_angle_max = grip_angle_max + self.__motor.init_status = GRIPPER_INITIALIZED + self.__initialize_gripper_mode = None + self.__target_position = self.__position - self.__target_velocity * self.DURATION + self.__pvts.clear() + elif self.__initialize_gripper_count == 1 and self.__initialize_gripper_mode == REINITIALIZE: + self.__motor.grip_angle_min = self.__position + self.__initialize_gripper_count = 2 + self.__target_position = self.__position + self.__target_velocity * self.DURATION + self.__pvts.clear() + self.set_position(self.__motor_angle_limit) + else: + self.__motor.grip_angle_max = self.__position + self.__motor.init_status = GRIPPER_INITIALIZED + self.__initialize_gripper_mode = None + self.__target_position = self.__position - self.__target_velocity * self.DURATION + self.__pvts.clear() + else: + if self.__motor.grip_status != GRIPPER_FALLING: + self.__motor.grip_status = GRIPPER_NO_CATCH + # if math.fabs(self.__torque) < (self.__target_tau * 0.8) and math.fabs(self.__velocity) < VELOCITY_ZERO_THRESHOLD: + # self.__motor.grip_status = GRIPPER_NO_CATCH + # else: + # self.__motor.grip_status = GRIPPER_CATCH else: self.__motion_completed = False - time.sleep(self.DURATION) + if (math.fabs(self.__torque) > self.__target_tau * 0.8) and (math.fabs(self.__velocity) < VELOCITY_ZERO_THRESHOLD): + self.__gripper_falling_count = 0 + self.__gripper_catch_count = self.__gripper_catch_count % 10 + 1 + if self.__gripper_catch_count == 10: + if self.__initialize_gripper_mode is not None: + if self.__initialize_gripper_mode == ALL_INITIALIZE: + self.__motor.grip_angle_min = self.__position + grip_angle_max = self.__motor.grip_angle_min + self.__motor_angle_limit + if grip_angle_max > self.__motor_angle_limit: + self.__motor.grip_angle_max = self.__motor_angle_limit + else: + self.__motor.grip_angle_max = grip_angle_max + self.__motor.init_status = GRIPPER_INITIALIZED + self.__initialize_gripper_mode = None + self.__target_position = self.__position - self.__target_velocity * self.DURATION + self.__pvts.clear() + elif self.__initialize_gripper_count == 1 and self.__initialize_gripper_mode == REINITIALIZE: + self.__motor.grip_angle_min = self.__position + self.__initialize_gripper_count = 2 + self.__target_position = self.__position + self.__target_velocity * self.DURATION + self.__pvts.clear() + self.set_position(self.__motor_angle_limit) + else: + if grip_angle_max > self.__motor_angle_limit: + self.__motor.grip_angle_max = self.__motor_angle_limit + else: + self.__motor.grip_angle_max = grip_angle_max + self.__motor.grip_angle_max = self.__position + self.__motor.init_status = GRIPPER_INITIALIZED + self.__initialize_gripper_mode = None + self.__target_position = self.__position - self.__target_velocity * self.DURATION + self.__pvts.clear() + else: + self.__motor.grip_status = GRIPPER_CATCH + self.__pvts.clear() + elif self.__motor.grip_status == GRIPPER_CATCH and (math.fabs(self.__velocity) > VELOCITY_ZERO_THRESHOLD): + self.__gripper_catch_count = 0 + self.__gripper_falling_count = self.__gripper_falling_count % 10 + 1 + if self.__gripper_falling_count == 10: + self.__motor.grip_status = GRIPPER_FALLING + target_position = self.__position - self.__target_velocity * self.DURATION + if target_position < self.__motor.grip_angle_min: + self.__target_position = self.__motor.grip_angle_min + else: + self.__target_position = target_position self.__is_active = True self.__is_pause = False @@ -589,10 +675,22 @@ class Sync(threading.Thread): target_velocity = copy.deepcopy(last_velocity) last_position = copy.deepcopy(pvt[0][0]) - send_data(target_position, target_velocity, self.__motor_kp, self.__motor_kd, self.__motor_tau) - time.sleep(self.DURATION) + send_data(target_position, target_velocity, self.__motor_kp, self.__motor_kd, self.__motor_tau, self.__motor_i_des) + + if self.__initialize_gripper_mode is not None: + pass + else: + self.__motor.grip_status = GRIPING else: - process_sync_mode(last_velocity) + if self.__motor_control_mode == POS_VEL or self.__motor_control_mode == Torque_Pos: + target_velocity = copy.deepcopy(last_velocity) + else: + target_velocity = 0.0 + target_position = self.__target_position + send_data(target_position, target_velocity, self.__motor_kp, self.__motor_kd, self.__motor_tau, self.__motor_i_des) + + determine_current_state() + time.sleep(self.DURATION) self.__is_sync = False @@ -620,6 +718,35 @@ class Sync(threading.Thread): self.__is_sync = False while not self.__is_sync: time.sleep(0.01) + + def initialize_gripper(self, mode): + """ + Sync gripper module Initialization + + :param mode: Initialize mode + :param range: Initialize angle range + + :return: + """ + self.set_motorr_i_des(0.05) + self.set_velocity(6.0) + self.set_acceleration(12.0) + time.sleep(0.1) + self.set_position(-self.__motor_angle_limit) + self.__initialize_gripper_mode = mode + self.__initialize_gripper_count = 1 + + def shutdown(self): + """ + Sync module Shutdown + + :return: + """ + self.__is_pause = True + self.__is_active = False + while self.__is_sync: + time.sleep(0.01) + self.join() def set_position(self, position): """ @@ -630,6 +757,7 @@ class Sync(threading.Thread): :return: """ if math.fabs(self.__position - position) < self.__motion_complete_limit: + self.__motor.grip_status = GRIPING return ids = [self.__motor.axis_id] jmove = Trajectory("joints") @@ -675,6 +803,17 @@ class Sync(threading.Thread): :return: """ self.__target_acceleration = acceleration + + def set_motorr_i_des(self, i_des): + """ + Set target acceleration to motor + + :param acceleration: Target acceleration + + :return: + """ + self.__motor_i_des = i_des + self.__target_tau = i_des * self.__motor_torque_limit def switch_control_mode(self, mode): """ @@ -766,12 +905,14 @@ class Sync(threading.Thread): class DMCAN: + TIMEOUT = 2.0 + def __init__(self, mode, bus, config, connections, parameters): - self.__axis_id = config['id'] - self.__motor_control_mode = config['control_mode'] + self.axis_id = config['id'] + self.motor_control_mode = self.config_control_mode(config['control_mode']) self.__mode = mode self.__bus = bus - self.__config = config + # self.__config = config self.__parameters = parameters if self.__mode == ONLINE: if connections is not None: @@ -786,9 +927,9 @@ class DMCAN: self.__notifier = can.Notifier(self.__connection, []) time.sleep(0.5) # CAN网络节点句柄 - self.__bus = DMBotsCAN(self.__axis_id, self.__connection, self.__notifier) + self.__can = DMBotsCAN(self.axis_id, self.__connection, self.__notifier) # Sync Module - self.__sync = Sync(self, self.__bus, self.__parameters) + self.__sync = Sync(self, self.__can, self.__parameters) else: self.__connection = None self.__notifier = None @@ -797,9 +938,24 @@ class DMCAN: self.init_status = GRIPPER_NO_INITIALIZED self.grip_status = GRIPPER_NO_CATCH + self.grip_angle_min = 0.0 + self.grip_angle_max = self.__parameters['motor_angle_limit'] + self.target_force = 0.0 self.target_position = 0.0 self.target_velocity = 0.0 + + def config_control_mode(self, control_mode): + if control_mode == "MIT": + return MIT + elif control_mode == "POS_VEL": + return POS_VEL + elif control_mode == "VEL": + return VEL + elif control_mode == "Torque_Pos": + return Torque_Pos + else: + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "Invalid control mode") # /////////////////////////////////////////////////////////// # driver function declarations @@ -906,7 +1062,7 @@ class DMCAN: return # Initialized CAN Bus custom - self.__bus.initialize() + self.__can.initialize() # Initialized Sync module self.__sync.initialize() # Check error code @@ -919,25 +1075,32 @@ class DMCAN: self.__sync.clear_error() time.sleep(0.1) # Switch motor control mode - if self.__sync.status == DriverStatus.DISABLE: + if self.__sync.status == DriverStatus.ENABLE: # Power off self.power_off() - # Switch control mode - self.sync.switch_control_mode(motor_control_mode) - time.sleep(0.1) - # # Make sure switch control mode - # timeout = time.monotonic() + self.TIMEOUT - # # TODO 控制指令返回的值和运动指令不同，返回判断和解析可能需要更改 - # while True: - # if time.monotonic() > timeout: - # logger.warning(self.message_filter("Switch motor control mode timed out")) - # break - # state = self.sync.status - # if state == DriverStatus.ENABLE: - # break - # time.sleep(0.1) + # Switch control mode + self.__sync.switch_control_mode(self.motor_control_mode) + time.sleep(0.1) + # # Make sure switch control mode + # timeout = time.monotonic() + self.TIMEOUT + # # TODO 控制指令返回的值和运动指令不同，返回判断和解析可能需要更改 + # while True: + # if time.monotonic() > timeout: + # logger.warning(self.message_filter("Switch motor control mode timed out")) + # break + # state = self.sync.status + # if state == DriverStatus.ENABLE: + # break + # time.sleep(0.1) # Power on motor and set motion to PP mode self.power_on() + + self.init_status = GRIPPER_INITIALIZING + # self.__sync.initialize_gripper(mode) + + # 本次的实物不校准 + self.init_status = GRIPPER_INITIALIZED + # TODO 实现抓手校准 # 一般情况下，抓手张开表示 ALL_INITIALIZE 单向回零 # REINITIALIZE 表示重新校准，抓手需要进行一次关闭-张开动作，通过机械限位标定抓手行程 @@ -955,21 +1118,35 @@ class DMCAN: :return: """ - # Power off - self.power_off() - # Shutdown CAN bus - if self.__bus is None: - if self.__notifier is not None: - self.__notifier.stop() - if self.__connection is not None: - self.__connection.shutdown() + try: + self.init_status = GRIPPER_NO_INITIALIZED + if self.__mode == OFFLINE: + return + # Power off + self.power_off() + # Shutdown Sync module + self.__sync.shutdown() + # Shutdown CAN bus + self.__can.shutdown() + if self.__bus is None: + if self.__notifier is not None: + self.__notifier.stop() + if self.__connection is not None: + self.__connection.shutdown() + print("EOAT DMCAN unloaded") + except BaseError as err: + raise err + except Exception as err: + error_message = "DMCAN unload failed with error: %s" % repr(err) + logger.error(error_message) + raise ExecuteError(CONTROLLER_ERROR_EOAT_EXEC, error_message) def set_gripper_force(self, force=GRIPPER_FORCE_MAX): """ Set gripper force, gripper will stop until force match this value in motion. - Default 100%. + Default 80%. - :param force: Force value (20%-100%) + :param force: Force value (20%-80%), When the motor current exceeds 80%, it is considered an overload. :return: """ @@ -983,14 +1160,23 @@ class DMCAN: raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'force' param must be type of integer") # TODO 力参数如何设置到Sync中，影响抓手运行施加的抓力 - self.target_force = force + if self.__mode == OFFLINE: + self.target_force = force + else: + # 限制力参数在20%-80%之间 + # 结构问题，小木块，把力设置在2%才触发到判断，手都可以掰的动5%时候下的电机（5N·m） + # if force > 80: + # force = 80 + # elif force < 20: + # force = 20 + self.__sync.set_motorr_i_des(force / 100) def set_gripper_position(self, position): """ - Set gripper position, position unit is 1/1000(‰), + Set gripper position, position unit is 1/100(%), transform to actual position base actual position range. - :param position: Position value (0‰-1000‰) + :param position: Position value (0%-100%) :return: """ @@ -1007,6 +1193,8 @@ class DMCAN: if self.__mode == OFFLINE: self.grip_status = GRIPPER_NO_CATCH else: + # 电机负向旋转夹爪闭合 + self.__sync.set_position((1- position / 100) * (self.grip_angle_max - self.grip_angle_min) + self.grip_angle_min) # TODO 位置参数如何设置到Sync中，影响抓手运行的目标位置， # 抓手主要通过位置控制来实现，当抓力上升到力限制的时候，停止抓手运动或者保持恒定抓力 # 运动过程受速度控制 @@ -1016,7 +1204,6 @@ class DMCAN: # GRIPPER_CATCH = 0x02 # 夹住物体 # GRIPPER_FALLING = 0x03 # 物体掉落 # 该过程在Sync中实现，不阻塞该指令，通过状态判断实现检测 - pass def set_gripper_velocity(self, velocity): """ @@ -1036,7 +1223,11 @@ class DMCAN: raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'velocity' param is out of range") # TODO 速度参数如何设置到Sync中，影响抓手运行的速度 - self.target_velocity = velocity + if self.__mode == OFFLINE: + self.target_velocity = velocity + else: + self.__sync.set_velocity(velocity / 100 * self.__parameters['motor_velocity_limit']) + self.__sync.set_acceleration(velocity / 100 * self.__parameters['motor_velocity_limit'] * 5) # /////////////////////////////////////////////////////////// # Reading function declarations @@ -1053,12 +1244,12 @@ class DMCAN: """ Get gripper position. - :return: Gripper position (0‰-1000‰) + :return: Gripper position (0%-100%) """ if self.__mode == OFFLINE: return self.target_position else: - return self.sync.position + return (1 - (self.__sync.position - self.grip_angle_min) / (self.grip_angle_max - self.grip_angle_min)) * 100 def get_gripper_status(self): """ diff --git a/Controller/driver/EOAT/modules/DMCAN/defines.py b/Controller/driver/EOAT/modules/DMCAN/defines.py index 34f6681..534cc3d 100644 --- a/Controller/driver/EOAT/modules/DMCAN/defines.py +++ b/Controller/driver/EOAT/modules/DMCAN/defines.py @@ -3,6 +3,12 @@ OFFLINE = 0 ONLINE = 1 +# Motor control_Type +MIT = 1 +POS_VEL = 2 +VEL = 3 +Torque_Pos = 4 + # Initialize function params ALL_INITIALIZE = 0x01 # 回零位（找单向位置） REINITIALIZE = 0xA5 # 重新标定（张开关闭确定行程） @@ -32,3 +38,6 @@ GRIPPER_FALLING = 0x03 # 物体掉落 # Get error status function params STATUS_OK = 0x00 # 无任何问题 + +# ZERO THRESHOLD +VELOCITY_ZERO_THRESHOLD = 1e-2 #速度零阈值 \ No newline at end of file -- Gitee From ed94e20f7dcf9d16c769ecc52c3e0e2d5e4e5cd3 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Fri, 17 Jan 2025 20:04:13 +0800 Subject: [PATCH 2/4] =?UTF-8?q?Update=20v1.0.13=20=E6=B7=BB=E5=8A=A0OmniPi?= =?UTF-8?q?cker=E5=A4=B9=E7=88=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../components/EOAT/OmniPicker/parameters | 32 + .../EOAT/modules/OmniPicker/OmniPicker.py | 909 ++++++++++++++++++ .../EOAT/modules/OmniPicker/__init__.py | 10 + .../driver/EOAT/modules/OmniPicker/defines.py | 37 + README.md | 1 + 5 files changed, 989 insertions(+) create mode 100644 Controller/config/components/EOAT/OmniPicker/parameters create mode 100644 Controller/driver/EOAT/modules/OmniPicker/OmniPicker.py create mode 100644 Controller/driver/EOAT/modules/OmniPicker/__init__.py create mode 100644 Controller/driver/EOAT/modules/OmniPicker/defines.py diff --git a/Controller/config/components/EOAT/OmniPicker/parameters b/Controller/config/components/EOAT/OmniPicker/parameters new file mode 100644 index 0000000..78d41c0 --- /dev/null +++ b/Controller/config/components/EOAT/OmniPicker/parameters @@ -0,0 +1,32 @@ +[CONNECTION] +bus = CAN +connections = {"id": 7, "channel_name": "PCAN_USBBUS1", "interface": "pcan", "baud_rate": 1000000} + +# [PARAMETERS] +# # Motor parameters +# motor_angle_limit: 70 +# motor_velocity_limit: 30 +# motor_torque_limit: 10 +# motor_kp: 20 +# motor_kd: 0.45 +# motor_tau: 0 +# motion_complete_limit = 0.1 +# # Unit parameters +# transmission_ratio = 1 +# resolution = 1 +# ts_s = 1 +# i_max_ps = 0.01 + +[KINEMATICS] +x = 0 +y = 0 +z = 0 +roll = 0 +pitch = 0 +yaw = 0 + +[DYNAMICS] +mass = 0.0 +com_x = 0 +com_y = 0 +com_z = 0 diff --git a/Controller/driver/EOAT/modules/OmniPicker/OmniPicker.py b/Controller/driver/EOAT/modules/OmniPicker/OmniPicker.py new file mode 100644 index 0000000..092952f --- /dev/null +++ b/Controller/driver/EOAT/modules/OmniPicker/OmniPicker.py @@ -0,0 +1,909 @@ +# -*- coding: utf-8 -*- +# Thirdparty +import time +import threading +import queue +import copy +import numpy +import math + +# Location +from Controller.driver.EOAT.modules.OmniPicker.defines import * + +# Utils +from Controller.utils.error import * +from Controller import logger + +# Local Thirdparty +from SitePackages import can + + +# /********************************************************************************************************************/ +# /******************************************* OmniPicker CAN ***********************************************/ +# /********************************************************************************************************************/ +class DriverStatus(object): + STATUS_OK = 0x00 + OVER_TEMPERATURE = 0x01 + OVER_VELOCITY = 0x02 + INITIALIZE_FALL = 0x03 + OVER_LIMIT = 0x04 + + ErrorStatus = [ + OVER_TEMPERATURE, + OVER_VELOCITY, + INITIALIZE_FALL, + OVER_LIMIT, + ] + + +class ICANBusMessage(object): + """ + Base define of Custom CAN Message + """ + def __init__(self, uuid=None, message=None): + # Unique + self.uuid = uuid + # Message Content + self.message = message + # Is empty messgae or not + self.empty = False + if self.uuid is None and self.message is None: + self.empty = True + + +class Listener(can.Listener): + def __init__(self, cob_id, parent): + super(Listener, self).__init__() + self.__cob_id = cob_id + self.__parent = parent + self.__dpool = {} + self.__exception = None + + @property + def dpool(self): + """ + Data pool of CAN received + """ + return self.__dpool + + @property + def exception(self): + """ + Exception of Listener + """ + return self.__exception + + def __call__(self, msg: can.Message): + """ + Call back necessary + + :return: + """ + self.on_message_received(msg) + + def on_message_received(self, msg): + """ + CAN message listener + + :return: + """ + # print(msg, msg.arbitration_id) + if self.__cob_id == msg.arbitration_id: + self.__dpool[msg.arbitration_id] = msg + + def on_error(self, exc): + """ + This method is called to handle any exception in the receive thread. + + :param exc: The exception causing the thread to stop + + :return: + """ + self.__exception = exc + + +class OmniCAN(threading.Thread): + # Transmit timed out + TIMEOUT = 5 + # Max Transmit count + MAX_COUNT = 65536 + + def __init__(self, aid, bus: can.interface.BusABC, notifier: can.Notifier): + threading.Thread.__init__(self) + # self.setDaemon(True) + self.daemon = True + # Arbitration Id + self.__send_cob_id = 0x000 + aid + self.__recv_cob_id = 0x000 + aid + # CAN messgae listener + self.__listener = None + # CAN bus handle + self.__bus = bus + # CAN bus notifier + self.__notifier = notifier + # Transmit queue + self.__send_queue = queue.Queue() + self.__recv_pool = dict() + # Flag + self.__is_active = False + # uuid count + self.__count = 0 + # Thread lock + self.send_lock = threading.Lock() + + @property + def bus(self): + return self.__bus + + # /////////////////////////////////////////////////////////// + # Main function declarations + # /////////////////////////////////////////////////////////// + def run(self) -> None: + """ + Main loop + + :return: + """ + self.__is_active = True + while self.__is_active: + # Get message from queue + msg = self.__send_queue.get() + # assert isinstance(msg, ICANBusMessage) + # Check data valid + if msg.empty: + self.__is_active = False + break + else: + try: + # Send message and check return + with self.send_lock: + self.__bus.send(msg.message, timeout=self.TIMEOUT) + # print(msg.message) + # Wait for receiving message + timeout = time.monotonic() + self.TIMEOUT + while True: + if time.monotonic() > timeout: + raise Exception("timed out") + if self.__recv_cob_id in self.__listener.dpool: + if self.__listener.dpool[self.__recv_cob_id].timestamp >= msg.message.timestamp: + self.__recv_pool[msg.uuid] = self.__listener.dpool[self.__recv_cob_id] + self.__listener.dpool.pop(self.__recv_cob_id) + # print(self.__recv_pool[msg.uuid]) + break + time.sleep(0.005) + except Exception as err: + self.__recv_pool[msg.uuid] = err + + def initialize(self): + """ + Initialize CAN custom. + Register notifier and start thread. + + :return: + """ + # CAN messgae listener + self.__listener = Listener(self.__recv_cob_id, self) + # Add notifier + self.__notifier.add_listener(self.__listener) + # Start thread + self.start() + + def shutdown(self): + """ + Finalize CAN custom. + Clear queue and shutdown communication bus. + + :return: + """ + # Clear transmit queue and pool + self.__send_queue.empty() + self.__recv_pool.clear() + # Stop thread + empty_msg = ICANBusMessage() + self.__send_queue.put(empty_msg) + # # Stop notifier + # if self.__notifier is not None: + # self.__notifier.stop() + # # Shutdown CAN bus + # if self.__bus is not None: + # self.__bus.shutdown() + # Wait for thread stop + self.join() + + def transmit(self, data): + """ + Transmit message. + Write or read value base Object dictionary. + + :param data: Message data + + :return: Value of object dictionary + """ + # Check listener status + if self.__listener.exception is not None: + raise self.__listener.exception + # Generate CAN bus message + transmit_uuid = self.get_uuid() + transmit_msg = can.Message(timestamp=time.monotonic(), + arbitration_id=self.__send_cob_id, + data=data, is_extended_id=False) + message = ICANBusMessage(transmit_uuid, transmit_msg) + # Put into send queue + self.__send_queue.put(message) + # Check return + while self.__is_active: + if transmit_uuid in self.__recv_pool: + # Receive and pop data + recv = copy.deepcopy(self.__recv_pool[transmit_uuid]) + self.__recv_pool.pop(transmit_uuid) + # Receive data is CAN message + if isinstance(recv, can.Message): + recv = recv.data.hex().upper() + raw_recv = list() + for i in range(0, len(recv), 2): + raw_recv.append(int(recv[i: i + 2], base=16)) + return raw_recv + # Receive data is Exception + elif isinstance(recv, Exception): + raise recv + else: + raise Exception("Unexpected return") + time.sleep(0.005) + + def get_uuid(self): + self.__count += 1 + self.__count %= self.MAX_COUNT + return self.__count + + +# /********************************************************************************************************************/ +# /***************************************** OmniPicker module EOAT *****************************************/ +# /********************************************************************************************************************/ +class Sync(threading.Thread): + + DURATION = 0.01 + + def __init__(self, parent, bus, config): + threading.Thread.__init__(self) + self.daemon = True + # Motor handle + self.__motor = parent + # CAN bus handle + self.__bus = bus + self.__config = config + # Active flag + self.__is_active = False + # Function Flag + self.__is_sync = False + self.__is_pause = False + self.__paused = False + # State cache + self.__error_code = DriverStatus.STATUS_OK + self.__status = GRIPPER_NO_CATCH + self.__position = 0 + self.__velocity = 0 + self.__torque = 0 + # Setting variable + self.__target_position = 0 + self.__target_velocity = 0 + self.__target_acceleration = 0 + self.__target_force = 0 + + self.__motion_completed = True + + self.__initialize_gripper_mode = None + self.__gripper_catch_count = 0 + self.__gripper_falling_count = 0 + self.__gripper_signal = False + + @property + def bus(self): + return self.__bus + + @property + def status(self): + return self.__status + + @property + def error_code(self): + return self.__error_code + + @property + def position(self): + return self.__position + + @property + def velocity(self): + return self.__velocity + + @property + def torque(self): + return self.__torque + + @property + def motion_completed(self): + return self.__motion_completed + + @property + def target_position(self): + return self.__target_position + + @property + def target_velocity(self): + return self.__target_velocity + + # /////////////////////////////////////////////////////////// + # Main function declarations + # /////////////////////////////////////////////////////////// + def run(self) -> None: + """ + Main loop + + :return: + """ + def decode_response_frame(data): + error_code = data[0] + status = data[1] + position = data[2] + velocity = data[3] + torque = data[4] + return error_code, status, position, velocity, torque + + def encode_send_frame(Pos, Force, Vel, Acc, Dec): + """ + Control Mode Function + + :param Pos : 目标位置，数值范围 0 - FF，0为夹紧，FF为完全张开 + :param Force : 目标力矩，数值范围 0 - FF，FF为最大力矩 + :param Vel : 目标速度，数值范围 0 - FF，FF为最大速度 + :param Acc : 目标加速度，数值范围 0 - FF，FF为最大加速度 + :param Dec : 目标减速度，数值范围 0 - FF，FF为最大减速度 + + :return: data_buf: encode_send_frame + """ + data_buf = numpy.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], numpy.uint8) + data_buf[1] = Pos + data_buf[2] = Force + data_buf[3] = Vel + data_buf[4] = Acc + data_buf[5] = Dec + return data_buf + + def determine_current_state(): + """ + Determine current state + + :return: + """ + if math.fabs(self.__target_position - self.__position) < COMPLETE_THRESHOLD: + self.__motion_completed = True + # 夹爪自带初始化，所以这个只是打开夹爪 + if self.__initialize_gripper_mode == ALL_INITIALIZE: + self.__motor.init_status = GRIPPER_INITIALIZED + self.__initialize_gripper_mode = None + elif self.__initialize_gripper_mode == REINITIALIZE: + self.__motor.init_status = GRIPPER_INITIALIZED + self.__initialize_gripper_mode = None + else: + if self.__motor.grip_status != GRIPPER_FALLING: + self.__motor.grip_status = GRIPPER_NO_CATCH + else: + self.__motion_completed = False + if self.__status == GRIPPER_CATCH and self.__velocity < VELOCITY_ZERO_THRESHOLD: + self.__gripper_falling_count = 0 + self.__gripper_catch_count = self.__gripper_catch_count % 10 + 1 + if self.__gripper_catch_count == 10: + self.__motor.grip_status = GRIPPER_CATCH + elif self.__motor.grip_status == GRIPPER_CATCH and self.__velocity > VELOCITY_ZERO_THRESHOLD: + self.__gripper_catch_count = 0 + self.__gripper_falling_count = self.__gripper_falling_count % 2 + 1 + if self.__gripper_falling_count == 2: + self.__motor.grip_status = GRIPPER_FALLING + self.__gripper_signal = False + else: + if self.__gripper_signal: + self.__motor.grip_status = GRIPING + + self.__is_active = True + self.__is_pause = False + empty_frame = [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00] + response = self.__bus.transmit(empty_frame) + + self.__error_code, self.__status, self.__position, self.__velocity, self.__torque = decode_response_frame(response) + self.__target_position = self.__position + self.__target_velocity = self.__velocity + self.__target_force = self.__torque + self.__is_sync = True + while self.__is_active: + if self.__is_pause: + self.__paused = True + time.sleep(self.DURATION) + else: + self.__paused = False + + data_buf = encode_send_frame(self.__target_position, self.__target_force, self.__target_velocity, \ + self.__target_acceleration, self.__target_acceleration) + + response = self.__bus.transmit(data_buf) + + self.__error_code, self.__status, self.__position, self.__velocity, self.__torque = decode_response_frame(response) + # print(self.__status) + determine_current_state() + + time.sleep(self.DURATION) + + self.__is_sync = False + + def initialize(self): + """ + Sync module Initialization + + :return: + """ + if not self.__is_active: + self.start() + self.__is_sync = False + while not self.__is_sync: + time.sleep(0.01) + + def initialize_gripper(self, mode): + """ + Sync gripper module Initialization + + :param mode: Initialize mode + :param range: Initialize angle range + + :return: + """ + self.__initialize_gripper_mode = mode + self.set_force(10) + self.set_velocity(60) + self.set_acceleration(100) + self.set_position(200) + + def shutdown(self): + """ + Sync module Shutdown + + :return: + """ + self.__is_pause = True + self.__is_active = False + while self.__is_sync: + time.sleep(0.01) + self.join() + + def set_position(self, position): + """ + Set target position to motor + + :param position: Target position + + :return: + """ + self.__target_position = position + self.__is_pause = False + self.__gripper_signal = True + + def set_velocity(self, velocity): + """ + Set target velocity to motor + + :param velocity: Target velocity + + :return: + """ + self.__is_pause = True + self.__target_velocity = velocity + + def set_acceleration(self, acceleration): + """ + Set target acceleration to motor + + :param acceleration: Target acceleration + + :return: + """ + self.__is_pause = True + self.__target_acceleration = acceleration + + def set_force(self, force): + """ + Set target acceleration to motor + + :param acceleration: Target acceleration + + :return: + """ + self.__is_pause = True + self.__target_force = force + + # def power_on(self): + # """ + # Set motor power on + + # :return: + # """ + # self.__paused = False + # self.__is_pause = True + # while not self.__paused: + # time.sleep(0.01) + # power_on = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC] + # self.__bus.transmit(power_on) + # self.__is_pause = False + + # def power_off(self): + # """ + # Set motor power off + + # :return: + # """ + # self.__paused = False + # self.__is_pause = True + # while not self.__paused: + # time.sleep(0.01) + # power_off = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD] + # self.__bus.transmit(power_off) + # self.__is_pause = False + + # def save_zero_position(self): + # """ + # Set motor power off + + # :return: + # """ + # self.__paused = False + # self.__is_pause = True + # while not self.__paused: + # time.sleep(0.01) + # save_zero_position = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE] + # self.__bus.transmit(save_zero_position) + # self.__is_pause = False + + # def clear_error(self): + # """ + # Clear motor error + + # :return: + # """ + # self.__paused = False + # self.__is_pause = True + # while not self.__paused: + # time.sleep(0.01) + # clear_error = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB] + # self.__bus.transmit(clear_error) + # self.__is_pause = False + + +class OmniPicker: + TIMEOUT = 2.0 + + def __init__(self, mode, bus, config, connections, parameters): + self.axis_id = config['id'] + # # 要利用driver测试时使用 + # bus = None + # connections = None + self.__mode = mode + self.__bus = bus + # self.__config = config + self.__parameters = parameters + if self.__mode == ONLINE: + if connections is not None: + self.__connection = connections[bus]['bus'] + self.__notifier = connections[bus]['notifier'] + else: + channel = config['channel_name'] + interface = config['interface'] + bitrate = config['baud_rate'] + # Create CAN client + self.__connection = can.interface.Bus(channel=channel, interface=interface, bitrate=bitrate) + self.__notifier = can.Notifier(self.__connection, []) + time.sleep(0.5) + # CAN网络节点句柄 + self.__can = OmniCAN(self.axis_id, self.__connection, self.__notifier) + # Sync Module + self.__sync = Sync(self, self.__can, self.__parameters) + else: + self.__connection = None + self.__notifier = None + + # Cache + self.init_status = GRIPPER_NO_INITIALIZED + self.grip_status = GRIPPER_NO_CATCH + + # /////////////////////////////////////////////////////////// + # driver function declarations + # /////////////////////////////////////////////////////////// + # def power_on(self): + # """ + # Set motor power on. + # As the same time, will reset driver fault status + + # :return: + # """ + # try: + # if self.__mode == OFFLINE: + # time.sleep(0.1) + # return + + # if self.__sync.status == DriverStatus.DISABLE: + # # Power on + # self.__sync.power_on() + # time.sleep(0.1) + # # Make sure power on + # timeout = time.monotonic() + self.TIMEOUT + # while True: + # if time.monotonic() > timeout: + # logger.warning("OmniPicker power on timed out") + # break + # state = self.__sync.status + # if state == DriverStatus.ENABLE: + # break + # time.sleep(0.1) + # except BaseError as err: + # raise err + # except Exception as err: + # error_message = "OmniPicker power on failed with error: %s" % repr(err) + # logger.error(error_message) + # raise ExecuteError(CONTROLLER_ERROR_EOAT_EXEC, error_message) + + # def power_off(self): + # """ + # Set motor power off + + # :return: + # """ + # try: + # if self.__mode == OFFLINE: + # time.sleep(0.1) + # return + + # # Power off + # self.__sync.power_off() + # time.sleep(0.1) + # # Make sure power off + # timeout = time.monotonic() + self.TIMEOUT + # while True: + # if time.monotonic() > timeout: + # logger.warning("OmniPicker power off timed out") + # break + # state = self.__sync.status + # if state == DriverStatus.DISABLE: + # break + # time.sleep(0.1) + # except BaseError as err: + # raise err + # except Exception as err: + # error_message = "OmniPicker power off failed with error: %s" % repr(err) + # logger.error(error_message) + # raise ExecuteError(CONTROLLER_ERROR_EOAT_EXEC, error_message) + + # def reset_fault_status(self): + # """ + # Reset fault status. + + # :return: + # """ + # try: + # if self.__mode == OFFLINE: + # return + # else: + # self.__sync.clear_error() + # except BaseError as err: + # raise err + # except Exception as err: + # error_message = "OmniPicker reset fault status failed with error: %s" % repr(err) + # logger.error(error_message) + # raise ExecuteError(CONTROLLER_ERROR_EOAT_EXEC, error_message) + + # /////////////////////////////////////////////////////////// + # Main function declarations + # /////////////////////////////////////////////////////////// + def initialize(self, mode=REINITIALIZE): + """ + Initialize OmniPicker gripper, re-calibrate gripper and rotary zero position. + + :param mode: Initialize mode, match different gripper module + + :return: + """ + if not isinstance(mode, int): + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'mode' param must be type of integer") + + try: + if self.__mode == OFFLINE: + self.init_status = GRIPPER_INITIALIZED + return + + # Initialized CAN Bus custom + self.__can.initialize() + # Initialized Sync module + self.__sync.initialize() + # Check error code + # error_code = self.__sync.status + # if error_code in DriverStatus.ErrorStatus: + # error_message = "OmniPicker initialized failed with error code: %s" % error_code + # logger.warning(error_message) + # time.sleep(0.1) + # # Clear Error + # self.__sync.clear_error() + # time.sleep(0.1) + + # self.power_on() + + self.init_status = GRIPPER_INITIALIZING + self.__sync.initialize_gripper(mode) + + # TODO 实现抓手校准 + # 一般情况下，抓手张开表示 ALL_INITIALIZE 单向回零 + # REINITIALIZE 表示重新校准，抓手需要进行一次关闭-张开动作，通过机械限位标定抓手行程 + # 该过程在Sync中实现，不阻塞该指令，通过状态判断实现检测 + except BaseError as err: + raise err + except Exception as err: + error_message = "OmniPicker initialized failed with error: %s" % repr(err) + logger.error(error_message) + raise ExecuteError(CONTROLLER_ERROR_EOAT_EXEC, error_message) + + def unload(self): + """ + Finalize OmniPicker gripper, close communication connection. + + :return: + """ + try: + self.init_status = GRIPPER_NO_INITIALIZED + if self.__mode == OFFLINE: + return + # Power off + # self.power_off() + # Shutdown Sync module + self.__sync.shutdown() + # Shutdown CAN bus + self.__can.shutdown() + if self.__bus is None: + if self.__notifier is not None: + self.__notifier.stop() + if self.__connection is not None: + self.__connection.shutdown() + print("EOAT OmniPicker unloaded") + except BaseError as err: + raise err + except Exception as err: + error_message = "OmniPicker unload failed with error: %s" % repr(err) + logger.error(error_message) + raise ExecuteError(CONTROLLER_ERROR_EOAT_EXEC, error_message) + + def set_gripper_force(self, force=GRIPPER_FORCE_MAX): + """ + Set gripper force, gripper will stop until force match this value in motion. + Default 100%. + + :param force: Force value (20%-100%), When the motor current exceeds 100%, it is considered an overload. + + :return: + """ + if isinstance(force, float) or isinstance(force, str): + force = int(force) + + if not isinstance(force, int): + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'force' param must be type of integer") + + if force < GRIPPER_FORCE_MIN or force > GRIPPER_FORCE_MAX: + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'force' param must be type of integer") + + # TODO 力参数如何设置到Sync中，影响抓手运行施加的抓力 + if self.__mode == OFFLINE: + self.target_force = force + else: + # 限制力参数在20%-100%之间 + # if force > 100: + # force = 100 + # elif force < 20: + # force = 20 + self.__sync.set_force(int(force / 100 * 255)) + + def set_gripper_position(self, position): + """ + Set gripper position, position unit is 1/100(%), + transform to actual position base actual position range. + + :param position: Position value (0%-100%) + + :return: + """ + if isinstance(position, float) or isinstance(position, str): + position = int(position) + + if not isinstance(position, int): + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'position' param must be type of integer") + + if position < GRIPPER_POSITION_MIN or position > GRIPPER_POSITION_MAX: + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'position' param is out of range") + + self.target_position = position + if self.__mode == OFFLINE: + self.grip_status = GRIPPER_NO_CATCH + else: + self.__sync.set_position(int(position / 100 * 255)) + # TODO 位置参数如何设置到Sync中，影响抓手运行的目标位置， + # 抓手主要通过位置控制来实现，当抓力上升到力限制的时候，停止抓手运动或者保持恒定抓力 + # 运动过程受速度控制 + # 结束运动过程后，需进行状态判断，判定是否为如下四个状态 + # GRIPPER_NO_CATCH = 0x00 # 到达位置 + # GRIPING = 0x01 # 运动中 + # GRIPPER_CATCH = 0x02 # 夹住物体 + # GRIPPER_FALLING = 0x03 # 物体掉落 + # 该过程在Sync中实现，不阻塞该指令，通过状态判断实现检测 + + def set_gripper_velocity(self, velocity): + """ + Set gripper velocity, gripper will move base this velocity. + + :param velocity: Velocity value (1%-100%) + + :return: + """ + if isinstance(velocity, float) or isinstance(velocity, str): + velocity = int(velocity) + + if not isinstance(velocity, int): + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'velocity' param must be type of integer") + + if velocity < GRIPPER_POSITION_MIN or velocity > GRIPPER_POSITION_MAX: + raise ParamError(CONTROLLER_ERROR_EOAT_ILLPARAM, "'velocity' param is out of range") + + # TODO 速度参数如何设置到Sync中，影响抓手运行的速度 + if self.__mode == OFFLINE: + self.target_velocity = velocity + else: + self.__sync.set_velocity(int(velocity / 100 * 255)) + self.__sync.set_acceleration(int(velocity / 100 * 255)) + + # /////////////////////////////////////////////////////////// + # Reading function declarations + # /////////////////////////////////////////////////////////// + def get_gripper_initialize_status(self): + """ + Get gripper initialize status, check gripper is initialized or not. + + :return: Initialize status + """ + return self.init_status + + def get_gripper_position(self): + """ + Get gripper position. + + :return: Gripper position (0%-100%) + """ + if self.__mode == OFFLINE: + return self.target_position + else: + return (self.__sync.position / 255 * 100) + + def get_gripper_status(self): + """ + Get gripper status. + This status could be used to check object is catched or not and falling. + + :return: Gripper status + """ + return self.grip_status + + def get_error_status(self): + """ + Get error status. Get error code message from gripper. + + :return: Error status + """ + status = {"fault": 0, "msg": ""} + if self.__mode == OFFLINE: + return status + else: + state = self.__sync.error_code + if state in DriverStatus.ErrorStatus: + status["msg"] = "DMCAN raise an fault, error code: %s" % state + status["fault"] = state + return status + diff --git a/Controller/driver/EOAT/modules/OmniPicker/__init__.py b/Controller/driver/EOAT/modules/OmniPicker/__init__.py new file mode 100644 index 0000000..4886f2b --- /dev/null +++ b/Controller/driver/EOAT/modules/OmniPicker/__init__.py @@ -0,0 +1,10 @@ +# -*- coding: utf-8 -*- +from .OmniPicker import * + + +# /********************************************************************************************************************/ +# /************************************************* Import *************************************************/ +# /********************************************************************************************************************/ +def load(mode, bus, config, connections, parameters): + module = OmniPicker(mode, bus, config, connections, parameters) + return module diff --git a/Controller/driver/EOAT/modules/OmniPicker/defines.py b/Controller/driver/EOAT/modules/OmniPicker/defines.py new file mode 100644 index 0000000..e00ebcf --- /dev/null +++ b/Controller/driver/EOAT/modules/OmniPicker/defines.py @@ -0,0 +1,37 @@ +# -*- coding: utf-8 -*- +# Constants used for working mode +OFFLINE = 0 +ONLINE = 1 + +# Initialize function params +ALL_INITIALIZE = 0x01 # 回零位（找单向位置） +REINITIALIZE = 0xA5 # 重新标定（张开关闭确定行程） + +# Set gripper force function params（Range 1-100%） +GRIPPER_FORCE_MIN = 1 +GRIPPER_FORCE_MAX = 100 + +# Set gripper position function params（Range 0-100%） +GRIPPER_POSITION_MIN = 0 +GRIPPER_POSITION_MAX = 100 + +# Set gripper velocity function params（Range 0-100%） +GRIPPER_VELOCITY_MIN = 1 +GRIPPER_VELOCITY_MAX = 100 + +# Set gripper initialize status function params +GRIPPER_NO_INITIALIZED = 0x00 # 未初始化 +GRIPPER_INITIALIZED = 0x01 # 初始化成功 +GRIPPER_INITIALIZING = 0x02 # 初始化中 + +# Get gripper status function params +GRIPPER_NO_CATCH = 0x00 # 到达位置、未抓取到 +GRIPING = 0x01 # 运动中 +GRIPPER_CATCH = 0x02 # 夹住物体 +GRIPPER_FALLING = 0x03 # 物体掉落 + +# Get error status function params +STATUS_OK = 0x00 # 无任何问题 + +COMPLETE_THRESHOLD = 0x09 +VELOCITY_ZERO_THRESHOLD = 0x0F #速度零阈值 \ No newline at end of file diff --git a/README.md b/README.md index 8c57b03..c9aa824 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ Coarm机械臂底层运动控制库 ### Update v1.0.13 1. 更新DMCAN抓手框架 2. 添加DMCAN控制 +3. 添加OmniPicker夹爪 ### Update v1.0.12 1. 修复movej可能出现 `PVT motion ran out of points` 问题 -- Gitee From af07157b5b774cbe34348251216305f22b60fc4e Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Fri, 17 Jan 2025 20:05:13 +0800 Subject: [PATCH 3/4] =?UTF-8?q?Update=20v1.0.14=20=E4=BF=AE=E6=94=B9motor?= =?UTF-8?q?=E4=B8=8B=E5=88=A4=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Controller/driver/motors/dmbots/motor.py | 48 ++++++++++++------------ README.md | 3 ++ 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index a79b5d9..71db370 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -144,9 +144,12 @@ class Sync(threading.Thread): def decode_response_frame(data): status = (data[0] & 0xF0) >> 4 - position = uint_to_float((data[1] << 8) + data[2], -12.5, 12.5, 16) - velocity = uint_to_float((data[3] << 4) + (data[4] >> 4), -30, 30, 12) - torque = uint_to_float(((data[4] & 0x0F) << 8) + data[5], -10, 10, 12) + # position = uint_to_float((data[1] << 8) + data[2], -12.5, 12.5, 16) + # velocity = uint_to_float((data[3] << 4) + (data[4] >> 4), -30, 30, 12) + # torque = uint_to_float(((data[4] & 0x0F) << 8) + data[5], -10, 10, 12) + position = uint_to_float((data[1] << 8) + data[2], -self.__motor_angle_limit, self.__motor_angle_limit, 16) + velocity = uint_to_float((data[3] << 4) + (data[4] >> 4), -self.__motor_velocity_limit, self.__motor_velocity_limit, 12) + torque = uint_to_float(((data[4] & 0x0F) << 8) + data[5], -self.__motor_torque_limit, self.__motor_torque_limit, 12) mos_temperature = data[6] motor_coil_temperature = data[7] return status, position, velocity, torque, mos_temperature, motor_coil_temperature @@ -221,7 +224,7 @@ class Sync(threading.Thread): data_buf = numpy.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], numpy.uint8) Pos_desired_uint8s = write_multi_float(position, 4, True) data_buf[0:4] = Pos_desired_uint8s - Vel_uint = numpy.uint16(velocity * 100) + Vel_uint = numpy.int16(velocity * 100) ides_uint = numpy.uint16(8000) data_buf[4] = Vel_uint & 0xff data_buf[5] = Vel_uint >> 8 @@ -283,7 +286,6 @@ class Sync(threading.Thread): self.__motion_completed = True else: self.__motion_completed = False - time.sleep(self.DURATION) self.__is_active = True self.__is_pause = False @@ -343,6 +345,7 @@ class Sync(threading.Thread): time.sleep(self.DURATION) else: process_sync_mode(last_velocity) + time.sleep(self.DURATION) else: if len(self.__pvts) > 0: pvt = copy.deepcopy(self.__pvts[0]) @@ -359,6 +362,7 @@ class Sync(threading.Thread): time.sleep(self.DURATION) else: process_sync_mode(last_velocity) + time.sleep(self.DURATION) self.__is_sync = False @@ -706,26 +710,24 @@ class Motor(MotorBase): time.sleep(0.1) return - if self.sync.status == DriverStatus.DISABLE: - # Stop motion - self.stop(set_mode=True) + if self.sync.status == DriverStatus.ENABLE: # Power off self.power_off() - # Switch control mode - self.sync.switch_control_mode(motor_control_mode) - time.sleep(0.1) - # # Make sure switch control mode - # timeout = time.monotonic() + self.TIMEOUT - - # # TODO 控制指令返回的值和运动指令不同，返回判断和解析可能需要更改 - # while True: - # if time.monotonic() > timeout: - # logger.warning(self.message_filter("Switch motor control mode timed out")) - # break - # state = self.sync.status - # if state == DriverStatus.ENABLE: - # break - # time.sleep(0.1) + # Switch control mode + self.sync.switch_control_mode(motor_control_mode) + time.sleep(0.1) + # # Make sure switch control mode + # timeout = time.monotonic() + self.TIMEOUT + + # # TODO 控制指令返回的值和运动指令不同，返回判断和解析可能需要更改 + # while True: + # if time.monotonic() > timeout: + # logger.warning(self.message_filter("Switch motor control mode timed out")) + # break + # state = self.sync.status + # if state == DriverStatus.ENABLE: + # break + # time.sleep(0.1) except BaseError as err: raise err except Exception as err: diff --git a/README.md b/README.md index c9aa824..a991f58 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,9 @@ Coarm机械臂底层运动控制库 ## 更新说明 +### Update v1.0.14 +1. 修改motor下判断 + ### Update v1.0.13 1. 更新DMCAN抓手框架 2. 添加DMCAN控制 -- Gitee From 2f5559b432155a313a694127f6ec7c1a70238a6b Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Fri, 17 Jan 2025 20:05:42 +0800 Subject: [PATCH 4/4] =?UTF-8?q?Update=20v1.0.14=20=E6=B7=BB=E5=8A=A0joints?= =?UTF-8?q?=E7=9A=84add=E6=93=8D=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../trajectory/modules/joints/joints.py | 227 ++++++++++++++++-- README.md | 1 + 2 files changed, 214 insertions(+), 14 deletions(-) diff --git a/Controller/driver/trajectory/modules/joints/joints.py b/Controller/driver/trajectory/modules/joints/joints.py index 2241260..a15ec8d 100644 --- a/Controller/driver/trajectory/modules/joints/joints.py +++ b/Controller/driver/trajectory/modules/joints/joints.py @@ -2,6 +2,7 @@ # Thirdparty import math import numpy +import copy # Location from ..defines import * @@ -91,6 +92,7 @@ class Joints(ITrajectory): self.poses = list() self.dists = list() self.interval = list() + self.interval_calc = list() self.sfunc = list() self.start_section = list() @@ -163,8 +165,7 @@ class Joints(ITrajectory): joint_ids = self.arguments["joint_ids"] self.dists.clear() self.interval.clear() - - interval_calc = list() + self.interval_calc.clear() # 计算位置距离 for s in range(self.segments): @@ -181,7 +182,7 @@ class Joints(ITrajectory): temp = math.fabs(self.dists[s][index] / self.velocity[s]) if temp > maxt: maxt = temp - interval_calc.append(maxt) + self.interval_calc.append(maxt) self.interval.append(maxt) self.looptime += maxt @@ -191,10 +192,10 @@ class Joints(ITrajectory): # 计算速度期望 # 起点，开始速度为0 for index in range(len(joint_ids)): - v = self.dists[0][index] / interval_calc[0] if interval_calc[0] != 0 else 0 + v = self.dists[0][index] / self.interval_calc[0] if self.interval_calc[0] != 0 else 0 v3 = 0 if self.segments > 1: - vns = (self.dists[1][index] / interval_calc[1]) if interval_calc[1] != 0 else 0 + vns = (self.dists[1][index] / self.interval_calc[1]) if self.interval_calc[1] != 0 else 0 if v * vns > 0: v3 = (v + vns) / 2 self.start_section.append(SecParam(self.dists[0][index], 0, v, v3)) @@ -203,10 +204,10 @@ class Joints(ITrajectory): for index in range(len(joint_ids)): v1 = 0 s = self.segments - 1 - v = self.dists[s][index] / interval_calc[s] if interval_calc[s] != 0 else 0 + v = self.dists[s][index] / self.interval_calc[s] if self.interval_calc[s] != 0 else 0 if self.segments > 1: ls = self.segments - 2 - vls = (self.dists[ls][index] / interval_calc[ls]) if interval_calc[ls] != 0 else 0 + vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 if v * vls > 0: v1 = (v + vls) / 2 self.end_section.append(SecParam(self.dists[s][index], v1, v, 0)) @@ -216,16 +217,16 @@ class Joints(ITrajectory): sec = list() for index in range(len(joint_ids)): v1 = 0 - v = (self.dists[s][index] / interval_calc[s]) if interval_calc[s] != 0 else 0 + v = (self.dists[s][index] / self.interval_calc[s]) if self.interval_calc[s] != 0 else 0 v3 = 0 ls = (s - 1) if s > 0 else self.segments - 1 - vls = (self.dists[ls][index] / interval_calc[ls]) if interval_calc[ls] != 0 else 0 + vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 if v * vls > 0: v1 = (v + vls) / 2 ns = (s + 1) % self.segments - vns = (self.dists[ns][index] / interval_calc[ns]) if interval_calc[ns] != 0 else 0 + vns = (self.dists[ns][index] / self.interval_calc[ns]) if self.interval_calc[ns] != 0 else 0 if v * vns > 0: v3 = (v + vns) / 2 sec.append(SecParam(self.dists[s][index], v1, v, v3)) @@ -251,11 +252,12 @@ class Joints(ITrajectory): if tdec < temp2: tdec = temp2 - if interval_calc[s] < (tacc + tdec): - interval_calc[s] = tacc + tdec + temp = copy.deepcopy(self.interval_calc) + if self.interval_calc[s] < (tacc + tdec): + temp[s] = tacc + tdec for index in range(len(joint_ids)): - interval_ret = sec[index].set_time_scale(tacc, interval_calc[s] - tacc - tdec, tdec) + interval_ret = sec[index].set_time_scale(tacc, temp[s] - tacc - tdec, tdec) if interval_ret > self.interval[s]: self.interval[s] = interval_ret @@ -335,7 +337,203 @@ class Joints(ITrajectory): :return: """ - pass + if not self.is_set: + raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Trajectory generator is not set") + + if self.closed: + raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ASSERT, "Trajectory add does not support loop tracks") + + joint_ids = self.arguments["joint_ids"] + # 确保输入的维度和现有数据匹配 + if not CheckTypes.is_list_of_numbers(poses, len(joint_ids)): + raise ParamError(CONTROLLER_ERROR_TRAJECTORY_ILLPARAM, "Illegal poses parameter") + self.poses.append(poses) + if not CheckTypes.is_number(velocity) or velocity < MIN_VELOCITY: + raise ParamError(CONTROLLER_ERROR_TRAJECTORY_ILLPARAM, "Illegal velocity parameter") + self.velocity.append(velocity) + self.segments = len(self.velocity) + # self.acceleration = self.velocity[-1] * 3 + # if self.velocity[-1] != 0 and self.velocity[-2] != 0: + # self.acceleration = math.fabs(self.velocity[-1] / self.velocity[-2]) * self.acceleration + # self.is_set = True + + # # 重新计算 + # # 清空相关计算缓存 + # self.dists.clear() + # self.interval.clear() + # self.interval_calc.clear() + # self.start_section.clear() + # self.end_section.clear() + # self.sfunc.clear() + + # self.generate() + + # 动态添加 + s = self.segments - 1 + # 计算位置距离 + ns = (s + 1) % len(self.poses) + dist = list() + for index in range(len(joint_ids)): + dist.append(self.poses[ns][index] - self.poses[s][index]) + self.dists.append(dist) + + # 计算时间间隔 + maxt = 0 + for index in range(len(joint_ids)): + temp = math.fabs(self.dists[s][index] / self.velocity[s]) + if temp > maxt: + maxt = temp + self.interval_calc.append(maxt) + self.interval.append(maxt) + self.looptime += maxt + + if self.looptime < EPS: + raise AssertError(CONTROLLER_ERROR_TRAJECTORY_ILLPARAM, "Illegal target pose") + + # 计算速度期望 + # 起点，开始速度为0 + if self.segments == 2: + self.start_section.clear() + for index in range(len(joint_ids)): + v = self.dists[0][index] / self.interval_calc[0] if self.interval_calc[0] != 0 else 0 + v3 = 0 + if self.segments > 1: + vns = (self.dists[1][index] / self.interval_calc[1]) if self.interval_calc[1] != 0 else 0 + if v * vns > 0: + v3 = (v + vns) / 2 + self.start_section.append(SecParam(self.dists[0][index], 0, v, v3)) + + # 终点，结尾速度为0 + self.end_section.clear() + for index in range(len(joint_ids)): + v1 = 0 + s = self.segments - 1 + v = self.dists[s][index] / self.interval_calc[s] if self.interval_calc[s] != 0 else 0 + if self.segments > 1: + ls = self.segments - 2 + vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 + if v * vls > 0: + v1 = (v + vls) / 2 + self.end_section.append(SecParam(self.dists[s][index], v1, v, 0)) + + # 循环部分 + self.sfunc.pop() + for s in range(self.segments - 2, self.segments): + sec = list() + for index in range(len(joint_ids)): + v1 = 0 + v = (self.dists[s][index] / self.interval_calc[s]) if self.interval_calc[s] != 0 else 0 + v3 = 0 + + ls = (s - 1) if s > 0 else self.segments - 1 + vls = (self.dists[ls][index] / self.interval_calc[ls]) if self.interval_calc[ls] != 0 else 0 + if v * vls > 0: + v1 = (v + vls) / 2 + + ns = (s + 1) % self.segments + vns = (self.dists[ns][index] / self.interval_calc[ns]) if self.interval_calc[ns] != 0 else 0 + if v * vns > 0: + v3 = (v + vns) / 2 + sec.append(SecParam(self.dists[s][index], v1, v, v3)) + self.sfunc.append(sec) + + # 计算 + for s in range(self.segments - 2, self.segments): + sec = list() + if s == 0: + sec = self.start_section + elif s == self.segments - 1: + sec = self.end_section + else: + sec = self.sfunc[s] + + tacc = 0.0 + tdec = 0.0 + for index in range(len(joint_ids)): + temp1 = math.fabs(sec[index].vt - sec[index].v1) / self.acceleration + temp2 = math.fabs(sec[index].vt - sec[index].v2) / self.acceleration + if tacc < temp1: + tacc = temp1 + if tdec < temp2: + tdec = temp2 + + temp = copy.deepcopy(self.interval_calc) + if self.interval_calc[s] < (tacc + tdec): + temp[s] = tacc + tdec + + for index in range(len(joint_ids)): + interval_ret = sec[index].set_time_scale(tacc, temp[s] - tacc - tdec, tdec) + if interval_ret > self.interval_calc[s]: + self.interval[s] = interval_ret + + # 起始点处理 + if self.segments == 2: + tacc = 0.0 + tdec = 0.0 + for index in range(len(joint_ids)): + temp1 = math.fabs(self.start_section[index].vt - self.start_section[index].v1) / self.acceleration + temp2 = math.fabs(self.start_section[index].vt - self.start_section[index].v2) / self.acceleration + if tacc < temp1: + tacc = temp1 + if tdec < temp2: + tdec = temp2 + + if tacc == 0 and self.interval[0] < tdec: + tdec = self.interval[0] + elif self.interval[0] < tacc and tdec == 0: + tacc = self.interval[0] + else: + if self.interval[0] < (tacc + tdec): + self.interval[0] = tacc + tdec + + for index in range(len(joint_ids)): + self.start_section[index].set_time_scale(tacc, self.interval[0] - tacc - tdec, tdec) + + # 终止点处理 + tacc = 0.0 + tdec = 0.0 + for index in range(len(joint_ids)): + temp1 = math.fabs(self.end_section[index].vt - self.end_section[index].v1) / self.acceleration + temp2 = math.fabs(self.end_section[index].vt - self.end_section[index].v2) / self.acceleration + if tacc < temp1: + tacc = temp1 + if tdec < temp2: + tdec = temp2 + + if tacc == 0 and self.interval[self.segments - 1] < tdec: + tdec = self.interval[self.segments - 1] + elif self.interval[self.segments - 1] < tacc and tdec == 0: + tacc = self.interval[self.segments - 1] + else: + if self.interval[self.segments - 1] < (tacc + tdec): + self.interval[self.segments - 1] = tacc + tdec + + for index in range(len(joint_ids)): + self.end_section[index].set_time_scale(tacc, self.interval[self.segments - 1] - tacc - tdec, tdec) + + # 计算 + for s in range(self.segments - 2, self.segments): + tacc = 0.0 + tdec = 0.0 + for index in range(len(joint_ids)): + temp1 = math.fabs(self.sfunc[s][index].vt - self.sfunc[s][index].v1) / self.acceleration + temp2 = math.fabs(self.sfunc[s][index].vt - self.sfunc[s][index].v2) / self.acceleration + if tacc < temp1: + tacc = temp1 + if tdec < temp2: + tdec = temp2 + + if tacc == 0 and self.interval[s] < tdec: + tdec = self.interval[s] + elif self.interval[s] < tacc and tdec == 0: + tacc = self.interval[s] + else: + if self.interval[s] < (tacc + tdec): + self.interval[s] = tacc + tdec + + for index in range(len(joint_ids)): + self.sfunc[s][index].set_time_scale(tacc, self.interval[s] - tacc - tdec, tdec) + def get(self): """ @@ -422,6 +620,7 @@ class Joints(ITrajectory): self.poses.clear() self.dists.clear() self.interval.clear() + self.interval_calc.clear() self.sfunc.clear() self.start_section.clear() self.end_section.clear() diff --git a/README.md b/README.md index a991f58..2421a1d 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ Coarm机械臂底层运动控制库 ### Update v1.0.14 1. 修改motor下判断 +2. 添加joints的add操作 ### Update v1.0.13 1. 更新DMCAN抓手框架 -- Gitee