From 4027b0926cb72314a517364c571e4f4a4e1c87c7 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Wed, 30 Oct 2024 17:14:31 +0800 Subject: [PATCH 1/2] =?UTF-8?q?update=20v1.0.4.1=20=E4=BF=AE=E5=A4=8Dv1.0.?= =?UTF-8?q?3=20=E4=B8=AD=E4=BB=A3=E7=A0=81=E6=A0=BC=E5=BC=8F=E5=92=8C?= =?UTF-8?q?=E8=BF=90=E8=A1=8C=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Controller/driver/motors/dmbots/motor.py | 72 +++++------------------- Controller/driver/motors/motor.py | 1 - 2 files changed, 13 insertions(+), 60 deletions(-) diff --git a/Controller/driver/motors/dmbots/motor.py b/Controller/driver/motors/dmbots/motor.py index f6e585b..7151064 100644 --- a/Controller/driver/motors/dmbots/motor.py +++ b/Controller/driver/motors/dmbots/motor.py @@ -122,19 +122,15 @@ class Sync(threading.Thread): :return: """ - # TODO 该函数不太符合python的规范 - def limit_min_max(x, min, max): - if x <= min: - x = min - elif x > max: - x = max + def limit_min_max(value, min_value, max_value): + return max(min_value, min(value, max_value)) - def uint_to_float(x, x_min, x_max, bits): - return x * (x_max - x_min) / ((1 << bits) - 1) + x_min + def uint_to_float(value, min_value, max_value, bits): + return value * (max_value - min_value) / ((1 << bits) - 1) + min_value - 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 float_to_uint(value, min_value, max_value, bits): + limit_min_max(value, min_value, max_value) + return np.uint16((value - min_value) * (((1 << bits) - 1) / (max_value - min_value))) def decode_response_frame(data): status = (data[0] & 0x10) >> 4 @@ -225,12 +221,6 @@ class Sync(threading.Thread): :return: """ - if not self.__is_active: - self.start() - self.__is_sync = False - while not self.__is_sync: - time.sleep(0.01) - for item in self.CONTROL_FACTORS_ITEMS: if item not in self.__control_factors.keys(): error_message = self.__motor.message_filter("can not load control factor - %s" % item) @@ -244,6 +234,12 @@ class Sync(threading.Thread): self.__motor_kd = self.__control_factors['motor_kd'] self.__motor_tau = self.__control_factors['motor_tau'] + if not self.__is_active: + self.start() + self.__is_sync = False + while not self.__is_sync: + time.sleep(0.01) + def set_position(self, position): """ Set target position to motor @@ -277,48 +273,6 @@ class Sync(threading.Thread): self.__target_position = position self.__motion_completed = False - # FIXME 测试代码尽量不上传 - def plot_trajectory(): - # 多个电机一起运行,图容易画在一起重叠,出事故 - # for pvt_group in self.__pvts: - # print(pvt_group) - - # 指定保存文件的文件夹 - output_folder = 'OutputPicture' # 替换为你的文件夹路径 - os.makedirs(output_folder, exist_ok=True) # 创建文件夹（如果不存在） - - # 提取位置、速度和持续时间 - positions = [pvt[0][0] for pvt in self.__pvts] - velocities = [pvt[1][0] for pvt in self.__pvts] - durations = [pvt[2] for pvt in self.__pvts] - - # 创建时间轴 - time_steps = [sum(durations[:i]) for i in range(len(durations) + 1)] - - # 绘制位置图 - plt.figure(figsize=(10, 5)) - plt.plot(time_steps[:-1], positions, color='blue', label='Position') - plt.title(f'Position Joint {ids}') - plt.xlabel('Time (s)') - plt.ylabel('Position (rad)') - plt.legend() - plt.grid() - plt.savefig(os.path.join(output_folder, f'position_plot_Joint_{ids}.png')) # 保存为文件 - plt.close() # 关闭当前图 - - # 绘制速度图 - plt.figure(figsize=(10, 5)) - plt.plot(time_steps[:-1], velocities, color='red', label='Velocity') - plt.title(f'Velocity Joint {ids}') - plt.xlabel('Time (s)') - plt.ylabel('Velocity (rad/s)') - plt.legend() - plt.grid() - plt.savefig(os.path.join(output_folder, f'velocity_plot_Joint_{ids}.png')) # 保存为文件 - plt.close() # 关闭当前图 - - # plot_trajectory() - def set_velocity(self, velocity): """ Set target velocity to motor diff --git a/Controller/driver/motors/motor.py b/Controller/driver/motors/motor.py index 9fdfae2..1aa45e0 100644 --- a/Controller/driver/motors/motor.py +++ b/Controller/driver/motors/motor.py @@ -327,7 +327,6 @@ 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 -- Gitee From b8f767cbc1d3a62deda3821afff29f946a0dd213 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Wed, 30 Oct 2024 20:08:36 +0800 Subject: [PATCH 2/2] =?UTF-8?q?update=20v1.0.4.2=20=E6=96=B0=E5=A2=9ECOARM?= =?UTF-8?q?=E8=BF=90=E5=8A=A8=E5=AD=A6=E9=80=86=E8=A7=A3=E8=A7=A3=E7=AE=97?= =?UTF-8?q?=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/components/devices/COARM/COARM | 16 +- .../kinematics/modules/coarm/__init__.py | 9 + .../driver/kinematics/modules/coarm/coarm.py | 166 ++++++++++++++++++ .../driver/kinematics/modules/ikinematics.py | 18 +- .../kinematics/modules/weeewe/weeewe.py | 6 +- README.md | 2 + 6 files changed, 199 insertions(+), 18 deletions(-) create mode 100644 Controller/driver/kinematics/modules/coarm/__init__.py create mode 100644 Controller/driver/kinematics/modules/coarm/coarm.py diff --git a/Controller/config/components/devices/COARM/COARM b/Controller/config/components/devices/COARM/COARM index 9299345..5428237 100644 --- a/Controller/config/components/devices/COARM/COARM +++ b/Controller/config/components/devices/COARM/COARM @@ -11,16 +11,18 @@ motor5 = COARM-M5 motor6 = COARM-M6 [KINEMATICS] -model = weeewe +model = coarm type = FREE [BASEOFFSET] offset = {"x": 0.0, "y": 0.0, "z": 0.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0} [LINK] -# modified DH model (MDH) -link1 = {'alpha': 0.0, 'a': 0.0, 'd': 222.5, 'theta': 0.0} -link2 = {'alpha': 0.0, 'a': 0.0, 'd': 0.0, 'theta': 0.0} -link3 = {'alpha': 0.0, 'a': 165.0, 'd': -74.0, 'theta': 0.0} -link4 = {'alpha': 0.0, 'a': 325.0, 'd': -78.0, 'theta': 180.0} -link5 = {'alpha': 0.0, 'a': 275.0, 'd': 0.0, 'theta': 180.0} +# standard DH model (SDH) +# theta: degree, alpha: degree, d: mm, a: mm +link1 = {'alpha': 90.0, 'a': 0.0, 'd': 53.0, 'theta': 0.0} +link2 = {'alpha': 0.0, 'a': -264.0, 'd': 0.0, 'theta': 0.0} +link3 = {'alpha': 0.0, 'a': -258.0, 'd': 0.0, 'theta': 0.0} +link4 = {'alpha': 90.0, 'a': 0.0, 'd': 79.0, 'theta': 0.0} +link5 = {'alpha': -90.0, 'a': 0.0, 'd': 79.0, 'theta': 0.0} +link6 = {'alpha': 0.0, 'a': 0.0, 'd': 47.0, 'theta': 0.0} diff --git a/Controller/driver/kinematics/modules/coarm/__init__.py b/Controller/driver/kinematics/modules/coarm/__init__.py new file mode 100644 index 0000000..ec46fb7 --- /dev/null +++ b/Controller/driver/kinematics/modules/coarm/__init__.py @@ -0,0 +1,9 @@ +# -*- coding: utf-8 -*- +from .coarm import * + + +# /********************************************************************************************************************/ +# /************************************************* Import *************************************************/ +# /********************************************************************************************************************/ +def load(joint_size, dh_table, hand_type): + return COARM(joint_size, dh_table, hand_type) diff --git a/Controller/driver/kinematics/modules/coarm/coarm.py b/Controller/driver/kinematics/modules/coarm/coarm.py new file mode 100644 index 0000000..deddb64 --- /dev/null +++ b/Controller/driver/kinematics/modules/coarm/coarm.py @@ -0,0 +1,166 @@ +# -*- coding: utf-8 -*- +# Thirdparty +import math +import numpy +import copy +import time + +# Location +from ..defines import * +from ..ikinematics import IKinematics + +# Utils +from Controller.utils.error import * +from Controller.utils.unit import * + + +# /********************************************************************************************************************/ +# /*************************************** COARM module kinematics *****************************************/ +# /********************************************************************************************************************/ +class COARM(IKinematics): + def __init__(self, joint_size, dh_table, handtype): + IKinematics.__init__(self, joint_size, dh_table, handtype) + + # /////////////////////////////////////////////////////////// + # Main function declarations + # /////////////////////////////////////////////////////////// + def inverse(self, point, euler, previews, handtype_match=True, closer=False): + """ + Transfrom position and orientation of robot end to joints of robot + + :param point: Position of robot end. + :param euler: Orientation of robot end. + :param previews: Base joints of robot. + :param handtype_match: Matching hand-type of previews + :param closer: As closer as previews + + :return: Joints of robot + """ + # Solutions + solutions = list() + # Transfrom to offset rotation matrix + rotation = numpy.array(OrientationMethod.euler_matrix(euler.roll, euler.pitch, euler.yaw)) + rotation[0, 3] = point.x + rotation[1, 3] = point.y + rotation[2, 3] = point.z + # Calc base rotation matrix + rotation = numpy.dot(self.base_offset_inv, rotation) + rotation = numpy.dot(rotation, self.end_offset_inv) + + a2 = self.dh_table["link2"]["a"] + a3 = self.dh_table["link3"]["a"] + + d1 = self.dh_table["link1"]["d"] + d4 = self.dh_table["link4"]["d"] + d5 = self.dh_table["link5"]["d"] + d6 = self.dh_table["link6"]["d"] + + # 预先取值 + px = rotation[0, 3] + py = rotation[1, 3] + pz = rotation[2, 3] + + ax = rotation[0, 2] + ay = rotation[1, 2] + az = rotation[2, 2] + + ox = rotation[0, 1] + oy = rotation[1, 1] + oz = rotation[2, 1] + + nx = rotation[0, 0] + ny = rotation[1, 0] + nz = rotation[2, 0] + + solution_signs = numpy.array([[1, 1, 1], + [1, 1, -1], + [1, -1, 1], + [1, -1, -1], + [-1, 1, 1], + [-1, 1, -1], + [-1, -1, 1], + [-1, -1, -1]]) + + for i in range(8): + k1 = solution_signs[i][0] + k2 = solution_signs[i][1] + k3 = solution_signs[i][2] + + A1 = px - ax * d6 + B1 = py - ay * d6 + q1 = math.atan2(d4, k1 * math.sqrt(A1 * A1 + B1 * B1 - d4 * d4)) + math.atan2(B1, A1) + + sq1 = math.sin(q1) + cq1 = math.cos(q1) + + A6 = ny * cq1 - nx * sq1 + B6 = oy * cq1 - ox * sq1 + q6 = math.atan2(0, k2) - math.atan2(B6, A6) + + sq6 = math.sin(q6) + cq6 = math.cos(q6) + + p1 = cq1 * (px - ax * d6 + d5 * ox * cq6 + d5 * nx * sq6) + sq1 * (py - ay * d6 + d5 * oy * cq6 + d5 * ny * sq6) + p2 = pz - d1 - az * d6 + d5 * oz * cq6 + d5 * nz * sq6 + + c3 = (p1 * p1 + p2 * p2 - a2 * a2 - a3 * a3) / (2 * a2 * a3) + s3 = k3 * math.sqrt(1 - c3 * c3) + q3 = math.atan2(s3, c3) + + A23 = 2 * a3 * p1 + B23 = 2 * a3 * p2 + C23 = a3 * a3 + p1 * p1 + p2 * p2 - a2 * a2 + q23 = math.atan2(k3 * math.sqrt(A23 * A23 + B23 * B23 - C23 * C23), C23) + math.atan2(B23, A23) + + q2 = q23 - q3 + + A5 = (- ny * cq1 * cq6 + nx * cq6 * sq1 + oy * cq1 * sq6 - ox * sq1 * sq6) + B5 = ax * sq1 - ay * cq1 + q5 = math.atan2(0, 1) + math.atan2(A5, B5) + + sq5 = math.sin(q5) + cq5 = math.cos(q5) + + A4 = nz * cq5 * cq6 - az * sq5 - oz * cq5 * sq6 + B4 = -cq1 * (ax * sq5 - nx * cq5 * cq6 + ox * cq5 * sq6) - sq1 * (ay * sq5 - ny * cq5 * cq6 + oy * cq5 * sq6) + q234 = math.atan2(A4, B4) + + q4 = q234 - q23 + + solutions.append([q1, q2, q3, q4, q5, q6]) + + # Select best solution + best_solution = None + distance = MAX_DISTANCE + previews = numpy.deg2rad(previews) + + for solution in solutions: + # Range + range_flag = True + for i in range(self.joint_size): + upper = self.dh_table["joint%s" % (i + 1)]["upper"] + lower = self.dh_table["joint%s" % (i + 1)]["lower"] + if solution[i] < lower or solution[i] > upper: + range_flag = False + break + if not range_flag: + continue + + # Calc distance + if previews is not None: + joint_dis = 0.0 + for i in range(self.joint_size): + joint_dis += abs(solution[i] - previews[i]) + if joint_dis < distance: + best_solution = solution + distance = joint_dis + else: + return numpy.degrees(solution).tolist() + # Check solution + if best_solution is None: + raise ExecuteError(CONTROLLER_ERROR_KINEMATIC_NO_SOLUTION, + "Point(%s, %s, %s), Euler(%s, %s, %s) " + "don't have any solutions in %s kinematics module" + % (point.x, point.y, point.z, euler.roll, euler.pitch, euler.yaw, "coarm")) + # print(best_solution) + return numpy.degrees(best_solution).tolist() diff --git a/Controller/driver/kinematics/modules/ikinematics.py b/Controller/driver/kinematics/modules/ikinematics.py index d5b5c31..96d0311 100644 --- a/Controller/driver/kinematics/modules/ikinematics.py +++ b/Controller/driver/kinematics/modules/ikinematics.py @@ -57,16 +57,16 @@ class IKinematics(object): ctheta = math.cos(theta) salpha = math.sin(alpha) calpha = math.cos(alpha) - # # standard DH model - # matrix = numpy.array([[ctheta, -stheta * calpha, stheta * salpha, a * ctheta], - # [stheta, ctheta * calpha, -ctheta * salpha, a * stheta], - # [0.0, salpha, calpha, d], - # [0.0, 0.0, 0.0, 1]]) - # modified DH model - matrix = numpy.array([[ctheta, -stheta, 0.0, a], - [stheta * calpha, ctheta * calpha, -salpha, -salpha * d], - [stheta * salpha, ctheta * salpha, calpha, calpha * d], + # standard DH model + matrix = numpy.array([[ctheta, -stheta * calpha, stheta * salpha, a * ctheta], + [stheta, ctheta * calpha, -ctheta * salpha, a * stheta], + [0.0, salpha, calpha, d], [0.0, 0.0, 0.0, 1]]) + # # modified DH model + # matrix = numpy.array([[ctheta, -stheta, 0.0, a], + # [stheta * calpha, ctheta * calpha, -salpha, -salpha * d], + # [stheta * salpha, ctheta * salpha, calpha, calpha * d], + # [0.0, 0.0, 0.0, 1]]) return matrix # Virtual function diff --git a/Controller/driver/kinematics/modules/weeewe/weeewe.py b/Controller/driver/kinematics/modules/weeewe/weeewe.py index f8ce2ca..99de606 100644 --- a/Controller/driver/kinematics/modules/weeewe/weeewe.py +++ b/Controller/driver/kinematics/modules/weeewe/weeewe.py @@ -66,6 +66,8 @@ class WEEEWE(IKinematics): # Select best solution best_solution = None distance = MAX_DISTANCE + previews = numpy.deg2rad(previews) + for solution in solutions: # Range range_flag = True @@ -87,11 +89,11 @@ class WEEEWE(IKinematics): best_solution = solution distance = joint_dis else: - return solution + return numpy.degrees(solution).tolist() # Check solution if best_solution is None: raise ExecuteError(CONTROLLER_ERROR_KINEMATIC_NO_SOLUTION, "Point(%s, %s, %s), Euler(%s, %s, %s) " "don't have any solutions in %s kinematics module" % (point.x, point.y, point.z, euler.roll, euler.pitch, euler.yaw, "weeewe")) - return best_solution + return numpy.degrees(best_solution).tolist() diff --git a/README.md b/README.md index bb99b35..4d739f9 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,8 @@ Coarm机械臂底层运动控制库 ### Update v1.0.4 1. 调整COARM机械臂为六轴机械臂 2. 新增WEEEWE模块解算DEMO +3. 修复v1.0.3中代码格式和运行问题 +4. 新增COARM运动学逆解解算模块、计算DH为标准DH ### Update v1.0.3 1. 更改为MIT控制模式 -- Gitee