diff --git a/Controller/config/components/devices/COARM/COARM b/Controller/config/components/devices/COARM/COARM index 5428237fd43420bd52dc6b041ee7a729c6509fb2..70872dc95175ad3bd4c9fbb0c39e337c5e86d12f 100644 --- a/Controller/config/components/devices/COARM/COARM +++ b/Controller/config/components/devices/COARM/COARM @@ -12,7 +12,8 @@ motor6 = COARM-M6 [KINEMATICS] model = coarm -type = FREE +dhtype = MDH +handtype = FREE [BASEOFFSET] offset = {"x": 0.0, "y": 0.0, "z": 0.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0} @@ -20,9 +21,17 @@ offset = {"x": 0.0, "y": 0.0, "z": 0.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0} [LINK] # 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} +# link1 = {'alpha': 90.0, 'a': 0.0, 'd': 83.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} + +# modified DH model (MDH), The current posture is the same as that of SDH +link1 = {'alpha': 0.0, 'a': 0.0, 'd': 83.0, 'theta': 180.0} +link2 = {'alpha': -90.0, 'a': 0.0, 'd': 79.0, 'theta': 0.0} +link3 = {'alpha': 0.0, 'a': 264.0, 'd': 0.0, 'theta': 0.0} +link4 = {'alpha': 0.0, 'a': 258.0, 'd': -0.0, 'theta': 180.0} +link5 = {'alpha': 90.0, 'a': 0.0, 'd': 79.0, 'theta': 0.0} +link6 = {'alpha': -90.0, 'a': 0.0, 'd': 47.0, 'theta': 0.0} diff --git a/Controller/driver/driver.py b/Controller/driver/driver.py index 1a17a5b89bdb944c1768ac3e4c7df1d4dca667a4..ad7cfc72ae0e3ff4b668620e6cbab539ab76a330 100644 --- a/Controller/driver/driver.py +++ b/Controller/driver/driver.py @@ -91,6 +91,7 @@ class RobotConfig: self.__base_offset = dict() self.__kinematics = None self.__handtype = None + self.__dhtype = None self.__links = dict() # Dynamics related cache self.__dynamics = None @@ -213,8 +214,9 @@ class RobotConfig: """ if self.__KINEMATICS_SECTION in file.sections(): self.__kinematics = self.read_parameter(self.__KINEMATICS_SECTION, 'model', file) + self.__dhtype = self.read_parameter(self.__KINEMATICS_SECTION, 'dhtype', file) try: - self.__handtype = self.read_parameter(self.__KINEMATICS_SECTION, 'type', file) + self.__handtype = self.read_parameter(self.__KINEMATICS_SECTION, 'handtype', file) except Exception: self.__handtype = None @@ -338,6 +340,14 @@ class RobotConfig: """ return self.__links + def get_dhtype(self): + """ + Get DH parameter type. + + :return: DH parameter type + """ + return self.__dhtype + def get_kinematics(self): """ Get DH model name. @@ -732,6 +742,7 @@ class Driver: self.__kinematics = Kinematics(self.__kinematics_name) # Generate DH table links = self.__config.get_links() + dhtype = self.__config.get_dhtype() handtype = self.__config.get_handtype() self.__dh_table = copy.deepcopy(links) for joint in self.get_joint_ids(): @@ -744,7 +755,7 @@ class Driver: "flip": flip[joint], "offset": offset[joint], "type": joint_type[joint]} # Load kinematics model - self.__kinematics.load(self.__dh_table, handtype) + self.__kinematics.load(self.__dh_table, dhtype, handtype) # Set base offset base_offset = self.__config.get_base_offset() base_offset_point = Point3D(base_offset['x'], base_offset['y'], base_offset['z']) diff --git a/Controller/driver/kinematics/kinematics.py b/Controller/driver/kinematics/kinematics.py index 7232d40c2f127e640019e3c8f007317f6869e41c..a88dbf94bcadce197089e5e0355acc3284fd6020 100644 --- a/Controller/driver/kinematics/kinematics.py +++ b/Controller/driver/kinematics/kinematics.py @@ -72,11 +72,12 @@ class Kinematics: # /////////////////////////////////////////////////////////// # Main function declarations # /////////////////////////////////////////////////////////// - def load(self, dh_table, handtype='free'): + def load(self, dh_table, dhtype='sdh', handtype='free'): """ Load kinematics module :param dh_table: DH params. + :param dhtype: DH params type. :param handtype: Hand-type for kinematics. :return: @@ -105,6 +106,15 @@ class Kinematics: raise ParamError(CONTROLLER_ERROR_KINEMATIC_ILLPARAM, error_message) self.__dh_table = dh_table + # Check DH-type + if not CheckTypes.is_string(dhtype): + dhtype = str(dhtype) + + kinematic_dhtype = KINEMATIC_DHTYPE_SDH + lowercase = dhtype.lower() + if lowercase in ['mdh', 'm']: + kinematic_dhtype = KINEMATIC_DHTYPE_MDH + # Check hand-type if not CheckTypes.is_string(handtype): handtype = str(handtype) @@ -120,7 +130,7 @@ class Kinematics: try: package = 'Controller.driver.kinematics.modules' module = importlib.import_module(".%s" % self.__kinematics_name, package=package) - self.__kinematics = module.load(self.__joint_size, dh_table, kinematic_handtype) + self.__kinematics = module.load(self.__joint_size, dh_table, kinematic_dhtype, kinematic_handtype) except ModuleNotFoundError as err: error_message = "Could not find %s kinematic package" % self.__kinematics_name raise ExecuteError(CONTROLLER_ERROR_KINEMATIC_LOAD, error_message) diff --git a/Controller/driver/kinematics/modules/weeewe/weeewe.py b/Controller/driver/kinematics/modules/AnalyticalKinematicsFramework/AnalyticalKinematicsFramework.py similarity index 92% rename from Controller/driver/kinematics/modules/weeewe/weeewe.py rename to Controller/driver/kinematics/modules/AnalyticalKinematicsFramework/AnalyticalKinematicsFramework.py index 99de6068dc22c8e8014ed5a8de34428458daa8a2..179fafb35a46493f16bba531140afff52c2d52b7 100644 --- a/Controller/driver/kinematics/modules/weeewe/weeewe.py +++ b/Controller/driver/kinematics/modules/AnalyticalKinematicsFramework/AnalyticalKinematicsFramework.py @@ -15,11 +15,11 @@ from Controller.utils.unit import * # /********************************************************************************************************************/ -# /*************************************** WEEEWE module kinematics *****************************************/ +# /*************************************** AnalyticalKinematicsFramework *****************************************/ # /********************************************************************************************************************/ -class WEEEWE(IKinematics): - def __init__(self, joint_size, dh_table, handtype): - IKinematics.__init__(self, joint_size, dh_table, handtype) +class AnalyticalKinematicsFramework(IKinematics): + def __init__(self, joint_size, dh_table, dhtype, handtype): + IKinematics.__init__(self, joint_size, dh_table, dhtype, handtype) # /////////////////////////////////////////////////////////// # Main function declarations diff --git a/Controller/driver/kinematics/modules/weeewe/__init__.py b/Controller/driver/kinematics/modules/AnalyticalKinematicsFramework/__init__.py similarity index 68% rename from Controller/driver/kinematics/modules/weeewe/__init__.py rename to Controller/driver/kinematics/modules/AnalyticalKinematicsFramework/__init__.py index 9f693f58b60543a5b80f62495e2e8f5f1d1fb5cf..8e727e8f5165d4c0305e1631ef7573c780236673 100644 --- a/Controller/driver/kinematics/modules/weeewe/__init__.py +++ b/Controller/driver/kinematics/modules/AnalyticalKinematicsFramework/__init__.py @@ -1,9 +1,9 @@ # -*- coding: utf-8 -*- -from .weeewe import * +from .AnalyticalKinematicsFramework import * # /********************************************************************************************************************/ # /************************************************* Import *************************************************/ # /********************************************************************************************************************/ -def load(joint_size, dh_table, hand_type): - return WEEEWE(joint_size, dh_table, hand_type) +def load(joint_size, dh_table, dhtype, hand_type): + return AnalyticalKinematicsFramework(joint_size, dh_table, dhtype, hand_type) diff --git a/Controller/driver/kinematics/modules/coarm/__init__.py b/Controller/driver/kinematics/modules/coarm/__init__.py index ec46fb749ece9f79f3623025af1155d21f1c5732..f8965b29d9e91dfcbd5992424bb12bfd96bbb823 100644 --- a/Controller/driver/kinematics/modules/coarm/__init__.py +++ b/Controller/driver/kinematics/modules/coarm/__init__.py @@ -5,5 +5,5 @@ from .coarm import * # /********************************************************************************************************************/ # /************************************************* Import *************************************************/ # /********************************************************************************************************************/ -def load(joint_size, dh_table, hand_type): - return COARM(joint_size, dh_table, hand_type) +def load(joint_size, dh_table, dhtype, hand_type): + return COARM(joint_size, dh_table, dhtype, hand_type) diff --git a/Controller/driver/kinematics/modules/coarm/coarm.py b/Controller/driver/kinematics/modules/coarm/coarm.py index deddb64999f47bf47f43f4775294129a1cb1237f..0e9e4abe9e5cd4a81d19c11e2fa4897b813a1b78 100644 --- a/Controller/driver/kinematics/modules/coarm/coarm.py +++ b/Controller/driver/kinematics/modules/coarm/coarm.py @@ -18,8 +18,8 @@ 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) + def __init__(self, joint_size, dh_table, dhtype, handtype): + IKinematics.__init__(self, joint_size, dh_table, dhtype, handtype) # /////////////////////////////////////////////////////////// # Main function declarations @@ -27,6 +27,7 @@ class COARM(IKinematics): def inverse(self, point, euler, previews, handtype_match=True, closer=False): """ Transfrom position and orientation of robot end to joints of robot + 已知在关节角度[0, 0, 0 , 0, 0, 0], 位置姿态下的逆解解算失败。 :param point: Position of robot end. :param euler: Orientation of robot end. @@ -46,14 +47,23 @@ class COARM(IKinematics): # 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"] + if self.dhtype: + 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"] + else: + # 以SDH中的参数对比取值的 + a2 = -self.dh_table["link3"]["a"] + a3 = -self.dh_table["link4"]["a"] + + d1 = self.dh_table["link1"]["d"] + d4 = self.dh_table["link2"]["d"] + self.dh_table["link4"]["d"] + d5 = self.dh_table["link5"]["d"] + d6 = self.dh_table["link6"]["d"] # 预先取值 px = rotation[0, 3] @@ -135,6 +145,9 @@ class COARM(IKinematics): previews = numpy.deg2rad(previews) for solution in solutions: + # Limit the value to [-pi,pi] + solution = [self.adjust(angle) for angle in solution] + # Range range_flag = True for i in range(self.joint_size): diff --git a/Controller/driver/kinematics/modules/defines.py b/Controller/driver/kinematics/modules/defines.py index 610ac1db2dd46ae784bdb07fe14bc9bce38e9568..3361f3d4c2af179633eb824526dfcf0244c84bc1 100644 --- a/Controller/driver/kinematics/modules/defines.py +++ b/Controller/driver/kinematics/modules/defines.py @@ -4,6 +4,9 @@ KINEMATIC_HANDTYPE_FREE = 0 KINEMATIC_HANDTYPE_LEFT = 1 KINEMATIC_HANDTYPE_RIGHT = 2 +KINEMATIC_DHTYPE_MDH = 0 +KINEMATIC_DHTYPE_SDH = 1 + # Joint types REVOLUTE = "REVOLUTE" # 沿轴旋转的铰链关节，即转动关节，具有由上限和下限指定的有限范围。 CONTINUOUS = "CONTINUOUS" # 绕轴旋转的连续铰链关节，即转动关节，没有上限和下限。 diff --git a/Controller/driver/kinematics/modules/ikinematics.py b/Controller/driver/kinematics/modules/ikinematics.py index 96d03111224a6221186ffd5acd697e812d74efeb..1ed26a2dd7a0b28a4a6a2160e8dff7c888cf29ef 100644 --- a/Controller/driver/kinematics/modules/ikinematics.py +++ b/Controller/driver/kinematics/modules/ikinematics.py @@ -15,10 +15,14 @@ from Controller.utils.unit import * # /********************************************** IKinematics ***********************************************/ # /********************************************************************************************************************/ class IKinematics(object): - def __init__(self, joint_size, dh_table, handtype=None): + def __init__(self, joint_size, dh_table, dhtype, handtype=None): # Load kinematics param self.joint_size = joint_size self.dh_table = dh_table + # Set dh-type + self.dhtype = KINEMATIC_DHTYPE_SDH + if dhtype is not None: + self.dhtype = dhtype # Set hand-type self.handtype = KINEMATIC_HANDTYPE_FREE if handtype is not None: @@ -42,14 +46,15 @@ class IKinematics(object): return ar @staticmethod - def dh_trans(alpha, a, d, theta): + def dh_trans(alpha, a, d, theta, dhtype): """ - Return DH matrix base DH params (alpha, a, d, theta) + Return DH matrix base DH params (alpha, a, d, theta, dhtype) :param alpha: Link twist. :param a: Link length(x). :param d: Link offset(z). :param theta: Joint angle. + :param dhtype: DH-type (SDH/MDH). :return: DH matrix """ @@ -57,16 +62,18 @@ 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], - # [0.0, 0.0, 0.0, 1]]) + if dhtype: + # 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]]) + else: + # 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 @@ -105,7 +112,7 @@ class IKinematics(object): a = self.dh_table["link%s" % joint_cnt]["a"] else: raise AssertError(CONTROLLER_ERROR_KINEMATIC_ASSERT, "Illegal joint type %s" % jtype) - matrix = self.dh_trans(alpha, a, d, theta) + matrix = self.dh_trans(alpha, a, d, theta, self.dhtype) rotation = numpy.dot(rotation, matrix) joint_cnt += 1 @@ -114,7 +121,7 @@ class IKinematics(object): alpha = math.radians(self.dh_table["link%s" % joint_cnt]["alpha"]) d = self.dh_table["link%s" % joint_cnt]["d"] a = self.dh_table["link%s" % joint_cnt]["a"] - matrix = self.dh_trans(alpha, a, d, theta) + matrix = self.dh_trans(alpha, a, d, theta, self.dhtype) rotation = numpy.dot(rotation, matrix) # Calc offset rotation matrix diff --git a/Materials/DH-Transformation.xlsx b/Materials/DH-Transformation.xlsx new file mode 100644 index 0000000000000000000000000000000000000000..7e4409069349e3ed1d8acfa70f6b4ff5195ecc1f Binary files /dev/null and b/Materials/DH-Transformation.xlsx differ diff --git a/README.md b/README.md index 4d739f9e2e77d02f5850c98fdc8283f4af281e23..3e4055fba901dbe45543dec75818663497395456 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,11 @@ Coarm机械臂底层运动控制库 ## 更新说明 +### Uptate v1.0.5 +1. 增加运动学解算模块对MDH的支持 +2. 优化配置文件 +3. 新增SDH参数运动学变换计算表-DH-Transformation.xlsx + ### Update v1.0.4 1. 调整COARM机械臂为六轴机械臂 2. 新增WEEEWE模块解算DEMO