start to adjust to ready-to-use class

kine_ctrl/rm75_kine_qp.py

@@ -22,32 +22,8 @@ class KinematicsSolver():
         print(f' ------------  the qp based kinematic initialising   -----------')
         self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
-        # -------------------------------------------------
-        # ee
-        # -------------------------------------------------
-        self.add_frame(frame_name="ee", position=[0.0, 0.0, 0.0], rotationXYZ=[0.0, 0.0, 0.0])
-        # -------------------------------------------------
-        # Scissor tool
-        # -------------------------------------------------
-        self.add_frame(frame_name="scissor", position=[0.0, 0.0, 0.19], rotationXYZ=[0.0, 0.0, 0.0])
-        # -------------------------------------------------
-        # Camera tool
-        # -------------------------------------------------
-        self.add_frame(frame_name="camera",position=[0.05, 0.02, 0.10], rotationXYZ=[-pi*0.5, 0.0, -pi*0.5])
-        # -------------------------------------------------
-        # Store tool frame IDs
-        # -------------------------------------------------
-        self.tool_frames = {
-            "scissor": self.model.getFrameId("scissor"),
-            "camera": self.model.getFrameId("camera"),
-            "ee":   self.model.getFrameId("ee")
-        }
 
 
-
-
-        self.data = self.model.createData()
-
         self.cfg_j_limit()
 
         # ----------  for reused qp_solver  ------------------
@@ -89,6 +65,17 @@ class KinematicsSolver():
         )
         self.model.addFrame(  pin.Frame(  frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) )
 
+    def add_tool_frames(self,dict_frames):
+        self.tool_frames ={}
+        for tool_name in dict_frames:
+            tool_attr = dict_frames[tool_name]
+            position = tool_attr[0][0:3]
+            rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
+            self.add_frame(tool_name, position, rotationXYZ)
+            self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)})
+        self.data = self.model.createData()
+
+
     def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
         if min_j is None:
             min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
@@ -103,7 +90,7 @@ class KinematicsSolver():
                 self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
                 self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
 
-    def forward_kinematics(self, joint_angles, tool="ee"):
+    def forward_kinematics(self, joint_angles, tool="omnipic"):
         """
         Compute forward kinematics.
 
@@ -172,8 +159,7 @@ class KinematicsSolver():
         """
         # Build target SE3 placement
         if target_quat is not None:
-            quat = pin.Quaternion(target_quat[3], target_quat[0],
+            quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
-                                  target_quat[1], target_quat[2])
             target_rotation = quat.matrix()
         elif target_rpy is not None:
             target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
@@ -335,10 +321,39 @@ class KinematicsSolver():
 
         if best_solution is not None:
             # return best_solution, True, best_error, iter_count
-            return 0, best_solution
+            return 0, best_solution.tolist()
         else:
             # return q[:7].copy(), False, error_norm, iter_count
-            return -1, q[:7].copy()
+            return -1, q[:7].copy().tolist()
+
+    def quaternion_to_euler(self, q):
+        """
+        Convert quaternion to Euler angles (roll, pitch, yaw)
+
+        Args:
+            qx, qy, qz, qw: quaternion components
+
+        Returns:
+            tuple: (roll, pitch, yaw) in radians
+        """
+        # Roll (x-axis rotation)
+        sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
+        cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
+        roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+        # Pitch (y-axis rotation)
+        sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
+        if abs(sinp) >= 1:
+            pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
+        else:
+            pitch = np.arcsin(sinp)
+
+        # Yaw (z-axis rotation)
+        siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
+        cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
+        yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+        return [roll, pitch, yaw]
 
     # def invese_kinematics_velocity(self, target_position, target_rpy=None,
     #                        target_quat=None, initial_guess=None, tool="ee"):

kine_ctrl/rm75_kine_rm.py

@@ -14,16 +14,11 @@ class rm75_kine_api():
 
         self.cfg_j_limit()
 
-        self.tool_frames = {
-            'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
-            'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.19, 0.0, 0, 0.0), payload=1, x=0, y=0, z=72),
-            'camera': rm_frame_t(frame_name="camera", pose=(0.05, 0.02, 0.10, -1.57, 0, -1.57), payload=1, x=0, y=0, z=72)
-        }
         self.work_frames = {
-            'work': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
+            'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
         }
 
-        self.tool_name = "ee"
+        self.tool_name = "no_tool"
         self.work_name = "work"
 
     def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
@@ -32,6 +27,8 @@ class rm75_kine_api():
         if min_j is None:
             min_j = np.array([ -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159  ])
 
+        max_j = np.array(max_j)
+        min_j = np.array(min_j)
         if rad_flag:
             self.robot_kine_rm.rm_algo_set_joint_max_limit((max_j * 180 / math.pi).tolist())
             self.robot_kine_rm.rm_algo_set_joint_min_limit((min_j * 180 / math.pi).tolist())
@@ -51,7 +48,46 @@ class rm75_kine_api():
     def get_tool_frame(self):
         return self.robot_kine_rm.rm_algo_get_curr_toolframe()
 
-    def forward_kinematics(self, joint_angles, flag  = 1  , tool="ee", work="work"):
+    def quaternion_to_euler(self, q):
+        """
+        Convert quaternion to Euler angles (roll, pitch, yaw)
+
+        Args:
+            qx, qy, qz, qw: quaternion components
+
+        Returns:
+            tuple: (roll, pitch, yaw) in radians
+        """
+        # Roll (x-axis rotation)
+        sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
+        cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
+        roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+        # Pitch (y-axis rotation)
+        sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
+        if abs(sinp) >= 1:
+            pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
+        else:
+            pitch = np.arcsin(sinp)
+
+        # Yaw (z-axis rotation)
+        siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
+        cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
+        yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+        return [roll, pitch, yaw]
+
+    def add_tool_frames(self, dict_frames):
+        self.tool_frames = {}
+        for tool_name in dict_frames:
+            tool_attr = dict_frames[tool_name]
+            position = tool_attr[0][0:3]
+            rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
+            f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
+
+            self.tool_frames.update({tool_name:f})
+
+    def forward_kinematics(self, joint_angles, flag  = 1  , tool="omnipic", work="work"):
         '''
         :param joint_angles:   list of joint values, in rad
         :param flag:       0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
@@ -66,7 +102,7 @@ class rm75_kine_api():
 
         return self.robot_kine_rm.rm_algo_forward_kinematics(joint=[q_s*180/math.pi for q_s in joint_angles]  , flag=flag)
 
-    def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"):
+    def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="omnipic", work="work"):
         '''
         :param target_position:     list of position values, m
         :param target_rpy:          list of rpy values, rad