From cfd4243b92a946e92ec39d33740f24fd1d8c3e48 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Mon, 28 Oct 2024 14:48:10 +0800 Subject: [PATCH 1/3] Change the motor mode to MIT --- .../config/components/devices/COARM/COARM-M1 | 10 ++- .../config/components/devices/COARM/COARM-M2 | 10 ++- Controller/driver/motors/dmbots/dmbotscan.py | 3 +- Controller/driver/motors/dmbots/motor.py | 65 ++++++++++++++----- Controller/driver/motors/motor.py | 20 +++++- 5 files changed, 87 insertions(+), 21 deletions(-) diff --git a/Controller/config/components/devices/COARM/COARM-M1 b/Controller/config/components/devices/COARM/COARM-M1 index 68f8c88..0324c5a 100644 --- a/Controller/config/components/devices/COARM/COARM-M1 +++ b/Controller/config/components/devices/COARM/COARM-M1 @@ -13,10 +13,18 @@ kinematics_flip = 0 kinematics_offset = 0.0 # Unit rad, for judge motion complete status -motion_complete_limit = 0.2 +motion_complete_limit = 0.02 motion_complete_ratio = 1.5 control_error_time = 3 +# Motor control factors, rad, rad/s, Nm +motor_angle_limit = 12.5 +motor_velocity_limit = 8 +motor_torque_limit = 28 +motor_kp = 49 +motor_kd = 1.3 +motor_tau = 0 + mounting_direction = 1 mechanical_stop = 0 mechanical_offset = 0 diff --git a/Controller/config/components/devices/COARM/COARM-M2 b/Controller/config/components/devices/COARM/COARM-M2 index 57ae674..a3db31b 100644 --- a/Controller/config/components/devices/COARM/COARM-M2 +++ b/Controller/config/components/devices/COARM/COARM-M2 @@ -13,10 +13,18 @@ kinematics_flip = 0 kinematics_offset = 0.0 # Unit rad, for judge motion complete status -motion_complete_limit = 0.2 +motion_complete_limit = 0.02 motion_complete_ratio = 1.5 control_error_time = 3 +# Motor control factors, rad, rad/s, Nm +motor_angle_limit = 12.5 +motor_velocity_limit = 30 +motor_torque_limit = 10 +motor_kp = 20 +motor_kd = 0.45 +motor_tau = 0 + mounting_direction = 1 mechanical_stop = 0 mechanical_offset = 0 diff --git a/Controller/driver/motors/dmbots/dmbotscan.py b/Controller/driver/motors/dmbots/dmbotscan.py index 9619f01..579c9f7 100644 --- a/Controller/driver/motors/dmbots/dmbotscan.py +++ b/Controller/driver/motors/dmbots/dmbotscan.py @@ -110,7 +110,8 @@ class DMBotsCAN(threading.Thread): # self.setDaemon(True) self.daemon = True # Arbitration Id - self.__send_cob_id = 0x100 + aid + # self.__send_cob_id = 0x100 + aid + self.__send_cob_id = 0x000 + aid self.__recv_cob_id = 0x010 + aid # CAN messgae listener self.__listener = None diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index 23c76f0..4ae7a56 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -4,6 +4,7 @@ import copy import os import time import math +import numpy as np # Location from Controller.driver.trajectory.trajectory import Trajectory @@ -25,7 +26,7 @@ import matplotlib.pyplot as plt # /********************************************* Motor DMBots ***********************************************/ # /********************************************************************************************************************/ class Sync(threading.Thread): - DUTATION = 0.01 + DURATION = 0.01 def __init__(self, parent, bus, config): threading.Thread.__init__(self) @@ -58,6 +59,12 @@ class Sync(threading.Thread): self.__pvts = list() # Config variable self.__motion_complete_limit = self.__config.get('motion_complete_limit') + self.__motor_angle_limit = self.__config.get('motor_angle_limit') + self.__motor_velocity_limit = self.__config.get('motor_velocity_limit') + self.__motor_torque_limit = self.__config.get('motor_torque_limit') + self.__motor_kp = self.__config.get('motor_kp') + self.__motor_kd = self.__config.get('motor_kd') + self.__motor_tau = self.__config.get('motor_tau') @property def bus(self): @@ -108,18 +115,18 @@ class Sync(threading.Thread): :return: """ - # def LIMIT_MIN_MAX(x, min, max): - # if x <= min: - # x = min - # elif x > max: - # x = max + def limit_min_max(x, min, max): + if x <= min: + x = min + elif x > max: + x = max def uint_to_float(x, x_min, x_max, bits): return x * (x_max - x_min) / ((1 << bits) - 1) + x_min - # def float_to_uint(x, x_min, x_max, bits): - # LIMIT_MIN_MAX(x, x_min, x_max) - # return (x - x_min) * (((1 << bits) - 1) / (x_max - x_min)) + def float_to_uint(x, x_min, x_max, bits): + limit_min_max(x, x_min, x_max) + return np.uint16((x - x_min) * (((1 << bits) - 1) / (x_max - x_min))) def decode_response_frame(data): status = (data[0] & 0x10) >> 4 @@ -130,6 +137,25 @@ class Sync(threading.Thread): motor_coil_temperature = data[7] return status, position, velocity, torque, mos_temperature, motor_coil_temperature + def encode_send_frame(q,dq): + kp_uint = float_to_uint(self.__motor_kp, 0, 500, 12) + kd_uint = float_to_uint(self.__motor_kd, 0, 5, 12) + + q_uint = float_to_uint(q, -self.__motor_angle_limit, self.__motor_angle_limit, 16) + dq_uint = float_to_uint(dq, -self.__motor_velocity_limit, self.__motor_velocity_limit, 12) + tau_uint = float_to_uint(self.__motor_tau, -self.__motor_torque_limit, self.__motor_torque_limit, 12) + + data_buf = np.array([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], np.uint8) + data_buf[0] = (q_uint >> 8) & 0xff + data_buf[1] = q_uint & 0xff + data_buf[2] = dq_uint >> 4 + data_buf[3] = ((dq_uint & 0xf) << 4) | ((kp_uint >> 8) & 0xf) + data_buf[4] = kp_uint & 0xff + data_buf[5] = kd_uint >> 4 + data_buf[6] = ((kd_uint & 0xf) << 4) | ((tau_uint >> 8) & 0xf) + data_buf[7] = tau_uint & 0xff + return data_buf + self.__is_active = True self.__is_pause = False power_off = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD] @@ -150,22 +176,27 @@ class Sync(threading.Thread): target_velocity = pvt[1][0] target_position = pvt[0][0] else: - target_velocity = self.__target_velocity + # target_velocity = self.__target_velocity + target_velocity = 0 target_position = self.__target_position # # 发送前数据查看 # print("target_position", target_position) # print("target_velocity", target_velocity) - target_position = write_multi_float(target_position, 4, True) - target_velocity = write_multi_float(target_velocity, 4, True) - target_position.extend(target_velocity) + # target_position = write_multi_float(target_position, 4, True) + # target_velocity = write_multi_float(target_velocity, 4, True) + # target_position.extend(target_velocity) + + data_buf = encode_send_frame(target_position,target_velocity) # # 发送数据查看 # hex_data = [hex(num) for num in target_position] # print(hex_data) - response = self.__bus.transmit(target_position) + # response = self.__bus.transmit(target_position) + response = self.__bus.transmit(data_buf) + self.__status, self.__position, \ self.__velocity, self.__torque, \ self.__mos_temperature, self.__motor_coil_temperature = decode_response_frame(response) @@ -173,7 +204,7 @@ class Sync(threading.Thread): self.__motion_completed = True else: self.__motion_completed = False - time.sleep(self.DUTATION) + time.sleep(self.DURATION) self.__is_sync = False def initialize(self): @@ -200,7 +231,7 @@ class Sync(threading.Thread): jmove = Trajectory("joints") jmove.load({"joint_ids": ids}) points = [[self.__position], [position]] - jmove.set(points, self.__target_velocity, self.__target_acceleration, self.DUTATION, 1, False) + jmove.set(points, self.__target_velocity, self.__target_acceleration, self.DURATION, 1, False) pvts = list() last_flag = list() while True: @@ -211,7 +242,7 @@ class Sync(threading.Thread): for i in range(len(ids)): p.append(pvt[i].position) v.append(pvt[i].velocity) - pvts.append([p, v, self.DUTATION]) + pvts.append([p, v, self.DURATION]) last_flag.append(False) if not info.success: last_flag[-1] = True diff --git a/Controller/driver/motors/motor.py b/Controller/driver/motors/motor.py index e29bbe0..4e0c550 100644 --- a/Controller/driver/motors/motor.py +++ b/Controller/driver/motors/motor.py @@ -327,6 +327,7 @@ class MotorConfig: self.__robot_config_path = os.path.join(self.__resources, 'devices', self.__robot_name) self.__motor_config_path = os.path.join(self.__resources, 'motors') self.__parameters = {} + self.__control_factors = {} self.__setup_file = None @property @@ -497,7 +498,21 @@ class MotorConfig: motion_complete_preview = abs(float(self.read_parameter(self.DETAIL, 'motion_complete_preview', file))) self.__parameters['motion_complete_preview'] = motion_complete_preview # Add by sjhuang 20240705 end - self.__parameters['control_error_time'] = float(self.read_parameter(self.DETAIL, 'control_error_time', file)) + self.__parameters['control_error_time'] = float( + self.read_parameter(self.DETAIL, 'control_error_time', file)) + # Get control factors + self.__control_factors['motor_angle_limit'] = float( + self.read_parameter(self.DETAIL, 'motor_angle_limit', file)) + self.__control_factors['motor_velocity_limit'] = float( + self.read_parameter(self.DETAIL, 'motor_velocity_limit', file)) + self.__control_factors['motor_torque_limit'] = float( + self.read_parameter(self.DETAIL, 'motor_torque_limit', file)) + self.__control_factors['motor_kp'] = float( + self.read_parameter(self.DETAIL, 'motor_kp', file)) + self.__control_factors['motor_kd'] = float( + self.read_parameter(self.DETAIL, 'motor_kd', file)) + self.__control_factors['motor_tau'] = float( + self.read_parameter(self.DETAIL, 'motor_tau', file)) # Get mechanical params mounting_direction = int(self.read_parameter(self.DETAIL, 'mounting_direction', file)) if abs(mounting_direction) != 1: @@ -587,6 +602,9 @@ class MotorConfig: self.__parameters['collision_brake_release'] = collision_brake_release # Add by sjhuang 20240604 end + # Merge two dictionaries + self.__parameters.update(self.__control_factors) + def __read_motor_parameters(self, file): """ Load motor parameters -- Gitee From 0e44ab284d4a9671ab620d78258644e674e49971 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Mon, 28 Oct 2024 16:48:09 +0800 Subject: [PATCH 2/3] Change the motor mode to MIT 2 --- Controller/driver/motors/dmbots/motor.py | 12 ++++++-- Controller/driver/motors/motor.py | 37 ++++++++++++++++-------- 2 files changed, 34 insertions(+), 15 deletions(-) diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index 4ae7a56..e8ac6eb 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -62,9 +62,10 @@ class Sync(threading.Thread): self.__motor_angle_limit = self.__config.get('motor_angle_limit') self.__motor_velocity_limit = self.__config.get('motor_velocity_limit') self.__motor_torque_limit = self.__config.get('motor_torque_limit') - self.__motor_kp = self.__config.get('motor_kp') - self.__motor_kd = self.__config.get('motor_kd') - self.__motor_tau = self.__config.get('motor_tau') + + self.__motor_kp = 0.0 + self.__motor_kd = 0.0 + self.__motor_tau = 0.0 @property def bus(self): @@ -219,6 +220,11 @@ class Sync(threading.Thread): while not self.__is_sync: time.sleep(0.01) + if self.__config.get('version') == 'DMBots': + self.__motor_kp = self.__config.get('motor_kp') + self.__motor_kd = self.__config.get('motor_kd') + self.__motor_tau = self.__config.get('motor_tau') + def set_position(self, position): """ Set target position to motor diff --git a/Controller/driver/motors/motor.py b/Controller/driver/motors/motor.py index 4e0c550..25281fc 100644 --- a/Controller/driver/motors/motor.py +++ b/Controller/driver/motors/motor.py @@ -501,18 +501,30 @@ class MotorConfig: self.__parameters['control_error_time'] = float( self.read_parameter(self.DETAIL, 'control_error_time', file)) # Get control factors - self.__control_factors['motor_angle_limit'] = float( - self.read_parameter(self.DETAIL, 'motor_angle_limit', file)) - self.__control_factors['motor_velocity_limit'] = float( - self.read_parameter(self.DETAIL, 'motor_velocity_limit', file)) - self.__control_factors['motor_torque_limit'] = float( - self.read_parameter(self.DETAIL, 'motor_torque_limit', file)) - self.__control_factors['motor_kp'] = float( - self.read_parameter(self.DETAIL, 'motor_kp', file)) - self.__control_factors['motor_kd'] = float( - self.read_parameter(self.DETAIL, 'motor_kd', file)) - self.__control_factors['motor_tau'] = float( - self.read_parameter(self.DETAIL, 'motor_tau', file)) + motor_angle_limit = 0.0 + if 'motor_angle_limit' in file[self.DETAIL]: + motor_angle_limit = float(self.read_parameter(self.DETAIL, 'motor_angle_limit', file)) + self.__control_factors['motor_angle_limit'] = motor_angle_limit + motor_velocity_limit = 0.0 + if 'motor_velocity_limit' in file[self.DETAIL]: + motor_velocity_limit = float(self.read_parameter(self.DETAIL, 'motor_velocity_limit', file)) + self.__control_factors['motor_velocity_limit'] = motor_velocity_limit + motor_torque_limit = 0.0 + if 'motor_torque_limit' in file[self.DETAIL]: + motor_torque_limit = float(self.read_parameter(self.DETAIL, 'motor_torque_limit', file)) + self.__control_factors['motor_torque_limit'] = motor_torque_limit + motor_kp = 0.0 + if 'motor_kp' in file[self.DETAIL]: + motor_kp = float(self.read_parameter(self.DETAIL, 'motor_kp', file)) + self.__control_factors['motor_kp'] = motor_kp + motor_kd = 0.0 + if 'motor_kd' in file[self.DETAIL]: + motor_kd = float(self.read_parameter(self.DETAIL, 'motor_kd', file)) + self.__control_factors['motor_kd'] = motor_kd + motor_tau = 0.0 + if 'motor_tau' in file[self.DETAIL]: + motor_tau = float(self.read_parameter(self.DETAIL, 'motor_tau', file)) + self.__control_factors['motor_tau'] = motor_tau # Get mechanical params mounting_direction = int(self.read_parameter(self.DETAIL, 'mounting_direction', file)) if abs(mounting_direction) != 1: @@ -818,6 +830,7 @@ class MotorBase(threading.Thread): if self.mode == OFFLINE: self.enabled = True return + # Virtual Function def finalize(self): -- Gitee From cd7aa1f2c2be7aeaf5cd185a5b8dae872927360d Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Mon, 28 Oct 2024 17:20:01 +0800 Subject: [PATCH 3/3] =?UTF-8?q?Update=20v1.0.3=20=E6=9B=B4=E6=94=B9?= =?UTF-8?q?=E4=B8=BAMIT=E6=8E=A7=E5=88=B6=E6=A8=A1=E5=BC=8F=EF=BC=8C?= =?UTF-8?q?=E4=BC=98=E5=8C=96=E9=85=8D=E7=BD=AE=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Controller/driver/motors/dmbots/motor.py | 12 ++++++-- Controller/driver/motors/motor.py | 37 ++++++++++++++++-------- README.md | 4 +++ 3 files changed, 38 insertions(+), 15 deletions(-) diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index 4ae7a56..e8ac6eb 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -62,9 +62,10 @@ class Sync(threading.Thread): self.__motor_angle_limit = self.__config.get('motor_angle_limit') self.__motor_velocity_limit = self.__config.get('motor_velocity_limit') self.__motor_torque_limit = self.__config.get('motor_torque_limit') - self.__motor_kp = self.__config.get('motor_kp') - self.__motor_kd = self.__config.get('motor_kd') - self.__motor_tau = self.__config.get('motor_tau') + + self.__motor_kp = 0.0 + self.__motor_kd = 0.0 + self.__motor_tau = 0.0 @property def bus(self): @@ -219,6 +220,11 @@ class Sync(threading.Thread): while not self.__is_sync: time.sleep(0.01) + if self.__config.get('version') == 'DMBots': + self.__motor_kp = self.__config.get('motor_kp') + self.__motor_kd = self.__config.get('motor_kd') + self.__motor_tau = self.__config.get('motor_tau') + def set_position(self, position): """ Set target position to motor diff --git a/Controller/driver/motors/motor.py b/Controller/driver/motors/motor.py index 4e0c550..25281fc 100644 --- a/Controller/driver/motors/motor.py +++ b/Controller/driver/motors/motor.py @@ -501,18 +501,30 @@ class MotorConfig: self.__parameters['control_error_time'] = float( self.read_parameter(self.DETAIL, 'control_error_time', file)) # Get control factors - self.__control_factors['motor_angle_limit'] = float( - self.read_parameter(self.DETAIL, 'motor_angle_limit', file)) - self.__control_factors['motor_velocity_limit'] = float( - self.read_parameter(self.DETAIL, 'motor_velocity_limit', file)) - self.__control_factors['motor_torque_limit'] = float( - self.read_parameter(self.DETAIL, 'motor_torque_limit', file)) - self.__control_factors['motor_kp'] = float( - self.read_parameter(self.DETAIL, 'motor_kp', file)) - self.__control_factors['motor_kd'] = float( - self.read_parameter(self.DETAIL, 'motor_kd', file)) - self.__control_factors['motor_tau'] = float( - self.read_parameter(self.DETAIL, 'motor_tau', file)) + motor_angle_limit = 0.0 + if 'motor_angle_limit' in file[self.DETAIL]: + motor_angle_limit = float(self.read_parameter(self.DETAIL, 'motor_angle_limit', file)) + self.__control_factors['motor_angle_limit'] = motor_angle_limit + motor_velocity_limit = 0.0 + if 'motor_velocity_limit' in file[self.DETAIL]: + motor_velocity_limit = float(self.read_parameter(self.DETAIL, 'motor_velocity_limit', file)) + self.__control_factors['motor_velocity_limit'] = motor_velocity_limit + motor_torque_limit = 0.0 + if 'motor_torque_limit' in file[self.DETAIL]: + motor_torque_limit = float(self.read_parameter(self.DETAIL, 'motor_torque_limit', file)) + self.__control_factors['motor_torque_limit'] = motor_torque_limit + motor_kp = 0.0 + if 'motor_kp' in file[self.DETAIL]: + motor_kp = float(self.read_parameter(self.DETAIL, 'motor_kp', file)) + self.__control_factors['motor_kp'] = motor_kp + motor_kd = 0.0 + if 'motor_kd' in file[self.DETAIL]: + motor_kd = float(self.read_parameter(self.DETAIL, 'motor_kd', file)) + self.__control_factors['motor_kd'] = motor_kd + motor_tau = 0.0 + if 'motor_tau' in file[self.DETAIL]: + motor_tau = float(self.read_parameter(self.DETAIL, 'motor_tau', file)) + self.__control_factors['motor_tau'] = motor_tau # Get mechanical params mounting_direction = int(self.read_parameter(self.DETAIL, 'mounting_direction', file)) if abs(mounting_direction) != 1: @@ -818,6 +830,7 @@ class MotorBase(threading.Thread): if self.mode == OFFLINE: self.enabled = True return + # Virtual Function def finalize(self): diff --git a/README.md b/README.md index 30fcf42..0e84c2d 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,10 @@ Coarm机械臂底层运动控制库 ## 更新说明 +### Update v1.0.3 +1. 更改为MIT控制模式 +2. 优化配置文件 + ### Update v1.0.2 1. 修复轨迹缺点问题 2. 增加dmbotscan线程锁 -- Gitee