[FEAT]增加正解C_PiperForwardKinematics类的偏移判断,如果is_offset为0则DH 参数是未偏移的版本,如果为1则为j1j2偏移2°的版本,可以在创建interface实例时将dh_is_offset设置为0/1来切换dh参数;在C_PiperForwardKinematics类中增加末端初始位置数值self.init_pos

Author: RosenYin

interface/piper_interface.py

@@ -29,6 +29,9 @@ class C_PiperInterface():
         judge_flag(bool): Determines if the CAN port is functioning correctly.
                         When using a PCIe-to-CAN module, set to false.
         can_auto_init(bool): Determines if the CAN port is automatically initialized.
+        dh_is_offset([0,1] -> default 0): Does the j1-j2 offset by 2° in the DH parameters? 
+                    0 -> No offset
+                    1 -> Offset applied
     '''
     class ArmStatus():
         '''
@@ -352,7 +355,8 @@ def __new__(cls,
     def __init__(self,
                  can_name:str="can0",
                  judge_flag=True,
-                 can_auto_init=True) -> None:
+                 can_auto_init=True,
+                 dh_is_offset: int = 0) -> None:
         if getattr(self, "_initialized", False):  
             return  # 避免重复初始化
         self.__can_channel_name:str
@@ -363,7 +367,8 @@ def __init__(self,
         self.__can_judge_flag = judge_flag
         self.__can_auto_init = can_auto_init
         self.__arm_can=C_STD_CAN(can_name, "socketcan", 1000000, judge_flag, can_auto_init, self.ParseCANFrame)
-        self.__piper_fk = C_PiperForwardKinematics()
+        self.__dh_is_offset = dh_is_offset
+        self.__piper_fk = C_PiperForwardKinematics(self.__dh_is_offset)
         # protocol
         self.__parser: Type[C_PiperParserBase] = C_PiperParserV1()
         # thread
@@ -1114,55 +1119,61 @@ def __UpdateDriverInfoHighSpdFeedback(self, msg:PiperMessage):
         with self.__arm_motor_info_high_spd_mtx:
             if(msg.type_ == ArmMsgType.PiperMsgHighSpdFeed_1):
                 self.__fps_counter.increment("ArmMotorDriverInfoHighSpd_1")
-                self.__arm_time_stamp.time_stamp_motor_low_spd_1 = time.time_ns()
+                self.__arm_time_stamp.time_stamp_motor_high_spd_1 = time.time_ns()
                 self.__arm_motor_info_high_spd.motor_1.can_id = msg.arm_high_spd_feedback_1.can_id
                 self.__arm_motor_info_high_spd.motor_1.motor_speed = msg.arm_high_spd_feedback_1.motor_speed
                 self.__arm_motor_info_high_spd.motor_1.current = msg.arm_high_spd_feedback_1.current
                 self.__arm_motor_info_high_spd.motor_1.pos = msg.arm_high_spd_feedback_1.pos
+                self.__arm_motor_info_high_spd.motor_1.effort = msg.arm_high_spd_feedback_1.cal_effort()
             elif(msg.type_ == ArmMsgType.PiperMsgHighSpdFeed_2):
                 self.__fps_counter.increment("ArmMotorDriverInfoHighSpd_2")
-                self.__arm_time_stamp.time_stamp_motor_low_spd_2 = time.time_ns()
+                self.__arm_time_stamp.time_stamp_motor_high_spd_2 = time.time_ns()
                 self.__arm_motor_info_high_spd.motor_2.can_id = msg.arm_high_spd_feedback_2.can_id
                 self.__arm_motor_info_high_spd.motor_2.motor_speed = msg.arm_high_spd_feedback_2.motor_speed
                 self.__arm_motor_info_high_spd.motor_2.current = msg.arm_high_spd_feedback_2.current
                 self.__arm_motor_info_high_spd.motor_2.pos = msg.arm_high_spd_feedback_2.pos
+                self.__arm_motor_info_high_spd.motor_2.effort = msg.arm_high_spd_feedback_2.cal_effort()
             elif(msg.type_ == ArmMsgType.PiperMsgHighSpdFeed_3):
                 self.__fps_counter.increment("ArmMotorDriverInfoHighSpd_3")
-                self.__arm_time_stamp.time_stamp_motor_low_spd_3 = time.time_ns()
+                self.__arm_time_stamp.time_stamp_motor_high_spd_3 = time.time_ns()
                 self.__arm_motor_info_high_spd.motor_3.can_id = msg.arm_high_spd_feedback_3.can_id
                 self.__arm_motor_info_high_spd.motor_3.motor_speed = msg.arm_high_spd_feedback_3.motor_speed
                 self.__arm_motor_info_high_spd.motor_3.current = msg.arm_high_spd_feedback_3.current
                 self.__arm_motor_info_high_spd.motor_3.pos = msg.arm_high_spd_feedback_3.pos
+                self.__arm_motor_info_high_spd.motor_3.effort = msg.arm_high_spd_feedback_3.cal_effort()
             elif(msg.type_ == ArmMsgType.PiperMsgHighSpdFeed_4):
                 self.__fps_counter.increment("ArmMotorDriverInfoHighSpd_4")
-                self.__arm_time_stamp.time_stamp_motor_low_spd_4 = time.time_ns()
+                self.__arm_time_stamp.time_stamp_motor_high_spd_4 = time.time_ns()
                 self.__arm_motor_info_high_spd.motor_4.can_id = msg.arm_high_spd_feedback_4.can_id
                 self.__arm_motor_info_high_spd.motor_4.motor_speed = msg.arm_high_spd_feedback_4.motor_speed
                 self.__arm_motor_info_high_spd.motor_4.current = msg.arm_high_spd_feedback_4.current
                 self.__arm_motor_info_high_spd.motor_4.pos = msg.arm_high_spd_feedback_4.pos
+                self.__arm_motor_info_high_spd.motor_4.effort = msg.arm_high_spd_feedback_4.cal_effort()
             elif(msg.type_ == ArmMsgType.PiperMsgHighSpdFeed_5):
                 self.__fps_counter.increment("ArmMotorDriverInfoHighSpd_5")
-                self.__arm_time_stamp.time_stamp_motor_low_spd_5 = time.time_ns()
+                self.__arm_time_stamp.time_stamp_motor_high_spd_5 = time.time_ns()
                 self.__arm_motor_info_high_spd.motor_5.can_id = msg.arm_high_spd_feedback_5.can_id
                 self.__arm_motor_info_high_spd.motor_5.motor_speed = msg.arm_high_spd_feedback_5.motor_speed
                 self.__arm_motor_info_high_spd.motor_5.current = msg.arm_high_spd_feedback_5.current
                 self.__arm_motor_info_high_spd.motor_5.pos = msg.arm_high_spd_feedback_5.pos
+                self.__arm_motor_info_high_spd.motor_5.effort = msg.arm_high_spd_feedback_5.cal_effort()
             elif(msg.type_ == ArmMsgType.PiperMsgHighSpdFeed_6):
                 self.__fps_counter.increment("ArmMotorDriverInfoHighSpd_6")
-                self.__arm_time_stamp.time_stamp_motor_low_spd_6 = time.time_ns()
+                self.__arm_time_stamp.time_stamp_motor_high_spd_6 = time.time_ns()
                 self.__arm_motor_info_high_spd.motor_6.can_id = msg.arm_high_spd_feedback_6.can_id
                 self.__arm_motor_info_high_spd.motor_6.motor_speed = msg.arm_high_spd_feedback_6.motor_speed
                 self.__arm_motor_info_high_spd.motor_6.current = msg.arm_high_spd_feedback_6.current
                 self.__arm_motor_info_high_spd.motor_6.pos = msg.arm_high_spd_feedback_6.pos
+                self.__arm_motor_info_high_spd.motor_6.effort = msg.arm_high_spd_feedback_6.cal_effort()
             else:
                 pass
             # 更新时间戳，取筛选ID的最新一个
-            self.__arm_motor_info_high_spd.time_stamp = max(self.__arm_time_stamp.time_stamp_motor_low_spd_1, 
-                                                    self.__arm_time_stamp.time_stamp_motor_low_spd_2, 
-                                                    self.__arm_time_stamp.time_stamp_motor_low_spd_3, 
-                                                    self.__arm_time_stamp.time_stamp_motor_low_spd_4, 
-                                                    self.__arm_time_stamp.time_stamp_motor_low_spd_5, 
-                                                    self.__arm_time_stamp.time_stamp_motor_low_spd_6) / 1_000_000_000
+            self.__arm_motor_info_high_spd.time_stamp = max(self.__arm_time_stamp.time_stamp_motor_high_spd_1, 
+                                                    self.__arm_time_stamp.time_stamp_motor_high_spd_2, 
+                                                    self.__arm_time_stamp.time_stamp_motor_high_spd_3, 
+                                                    self.__arm_time_stamp.time_stamp_motor_high_spd_4, 
+                                                    self.__arm_time_stamp.time_stamp_motor_high_spd_5, 
+                                                    self.__arm_time_stamp.time_stamp_motor_high_spd_6) / 1_000_000_000
             # print(self.__arm_motor_info_high_spd)
             return self.__arm_motor_info_high_spd
 

interface/piper_interface_v1.py

@@ -29,6 +29,9 @@ class C_PiperInterface_V1():
         judge_flag(bool): Determines if the CAN port is functioning correctly.
                         When using a PCIe-to-CAN module, set to false.
         can_auto_init(bool): Determines if the CAN port is automatically initialized.
+        dh_is_offset([0,1] -> default 0): Does the j1-j2 offset by 2° in the DH parameters? 
+                    0 -> No offset
+                    1 -> Offset applied
     '''
     class ArmStatus():
         '''
@@ -352,7 +355,8 @@ def __new__(cls,
     def __init__(self,
                  can_name:str="can0",
                  judge_flag=True,
-                 can_auto_init=True) -> None:
+                 can_auto_init=True,
+                 dh_is_offset: int = 0) -> None:
         if getattr(self, "_initialized", False):  
             return  # 避免重复初始化
         self.__can_channel_name:str
@@ -363,7 +367,8 @@ def __init__(self,
         self.__can_judge_flag = judge_flag
         self.__can_auto_init = can_auto_init
         self.__arm_can=C_STD_CAN(can_name, "socketcan", 1000000, judge_flag, can_auto_init, self.ParseCANFrame)
-        self.__piper_fk = C_PiperForwardKinematics()
+        self.__dh_is_offset = dh_is_offset
+        self.__piper_fk = C_PiperForwardKinematics(self.__dh_is_offset)
         # protocol
         self.__parser: Type[C_PiperParserBase] = C_PiperParserV1()
         # thread

interface/piper_interface_v2.py

@@ -29,6 +29,9 @@ class C_PiperInterface_V2():
         judge_flag(bool): Determines if the CAN port is functioning correctly.
                         When using a PCIe-to-CAN module, set to false.
         can_auto_init(bool): Determines if the CAN port is automatically initialized.
+        dh_is_offset([0,1] -> default 0): Does the j1-j2 offset by 2° in the DH parameters? 
+                    0 -> No offset
+                    1 -> Offset applied
     '''
     class ArmStatus():
         '''
@@ -368,7 +371,8 @@ def __new__(cls,
     def __init__(self,
                  can_name:str="can0",
                  judge_flag=True,
-                 can_auto_init=True) -> None:
+                 can_auto_init=True,
+                 dh_is_offset: int = 0) -> None:
         if getattr(self, "_initialized", False):  
             return  # 避免重复初始化
         self.__can_channel_name:str
@@ -379,7 +383,8 @@ def __init__(self,
         self.__can_judge_flag = judge_flag
         self.__can_auto_init = can_auto_init
         self.__arm_can=C_STD_CAN(can_name, "socketcan", 1000000, judge_flag, can_auto_init, self.ParseCANFrame)
-        self.__piper_fk = C_PiperForwardKinematics()
+        self.__dh_is_offset = dh_is_offset
+        self.__piper_fk = C_PiperForwardKinematics(self.__dh_is_offset)
         # protocol
         self.__parser: Type[C_PiperParserBase] = C_PiperParserV2()
         # thread

kinematics/piper_fk.py

@@ -1,7 +1,10 @@
 import math
+from typing_extensions import (
+    Literal,
+)
 
 class C_PiperForwardKinematics():
-    def __init__(self):
+    def __init__(self, dh_is_offset: Literal[0x00, 0x01] = 0x00):
         self.RADIAN = 180 / math.pi
         self.PI = math.pi
         # Denavit-Hartenberg parameters for each link
@@ -12,9 +15,15 @@ def __init__(self):
         self._a     = [0     , 0                      , 285.03                   , -21.98        , 0             , 0          ]
         self._alpha = [0     , -self.PI / 2           , 0                        , self.PI / 2   , -self.PI / 2  , self.PI / 2]
         self._theta = [0     , -self.PI * 174.22 / 180, -100.78 / 180 * self.PI  , 0             , 0             , 0          ]
-        # self._theta = [0     , -self.PI * 172.22 / 180, -102.78 / 180 * self.PI  , 0             , 0             , 0          ]
         self._d     = [123   , 0                      , 0                        , 250.75        , 0             , 91         ]
-        # self.zero   = [56.128, 0, 213.266, 0.0, 85.0, 0.0]
+        self.init_pos   = [55.0  , 0.0                    , 205.0                    , 0.0           , 85.0          , 0.0] # unit xyz-mm, rpy-degree
+        # if j2, j3 offset 2°
+        if(dh_is_offset == 0x01):
+            self._a     = [0     , 0                      , 285.03                   , -21.98        , 0             , 0          ]
+            self._alpha = [0     , -self.PI / 2           , 0                        , self.PI / 2   , -self.PI / 2  , self.PI / 2]
+            self._theta = [0     , -self.PI * 172.22 / 180, -102.78 / 180 * self.PI  , 0             , 0             , 0          ]
+            self._d     = [123   , 0                      , 0                        , 250.75        , 0             , 91         ]
+            self.init_pos   = [56.128, 0.0                    , 213.266                  , 0.0           , 85.0          , 0.0] # unit xyz-mm, rpy-degree
     def __MatrixToeula(self, T):
         '''
         Convert a transformation matrix to Euler angles (roll, pitch, yaw).