From 9849466430c51112f5a3b7e58754fe96d0c6be96 Mon Sep 17 00:00:00 2001
From: LiuzhengSJ <zhengliu@sjtu.edu.cn>
Date: Tue, 28 Jul 2026 22:41:35 +0100
Subject: [PATCH] add collision detection

---
 kine_ctrl/main.py         |  6 ++--
 kine_ctrl/rm75_kine_qp.py | 64 +++++++++++++++++++++++----------------
 2 files changed, 41 insertions(+), 29 deletions(-)

diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py
index e36dd47..dafea53 100644
--- a/kine_ctrl/main.py
+++ b/kine_ctrl/main.py
@@ -43,11 +43,11 @@ def main():
 
 
     #  -----------      rm75 qp based kine          ------------
-    robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75')
+    robot_kine_qp = kine_qp(urdf_path='./urdf_rm75/RM75-SCI.urdf', mesh_dir='./urdf_rm75', tcps=["scissor_tcp", "camera_tcp"])
     robot_kine_qp.add_tool_frames(tools_in_ee)
     robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
-    fp = robot_kine_qp.forward_kinematics(np.zeros(7))
+    fp = robot_kine_qp.forward_kinematics(np.zeros(7),tool='scissor_tcp')
     print(f'forward kine res = {fp}')
 
     #   ----------      rm75 official algorithm         -----------
@@ -103,7 +103,7 @@ def main():
                 fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
                 d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
                 print(f'---- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
-                # robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
+                robot_kine_qp.collision_detect(q,stop_at_first_collision=True, verbose=True)
                 if d_p_ik < 0.01:
                     result[0][1] += 1
 
diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py
index f0293ac..d42fd0d 100644
--- a/kine_ctrl/rm75_kine_qp.py
+++ b/kine_ctrl/rm75_kine_qp.py
@@ -13,7 +13,7 @@ import threading
 
 
 class KinematicsSolver():
-    def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
+    def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75", tcps=None):
         """
         for realman 75b
         Initialize robotic arm kinematics using Pinocchio (ROS2 version).
@@ -30,10 +30,7 @@ class KinematicsSolver():
 
         self.cfg_j_limit()
 
-        q_range = (
-                self.model.upperPositionLimit[:7] -
-                self.model.lowerPositionLimit[:7]
-        )
+        q_range = (  self.model.upperPositionLimit[:7] - self.model.lowerPositionLimit[:7] )
 
         self.w_q_limit = np.diag(1.0 / (q_range ** 2))
 
@@ -71,8 +68,19 @@ class KinematicsSolver():
             0.3, 0.3, 0.2
         ])
 
-        self.scissor_frame_id = self.model.getFrameId("scissor_tcp")
-        self.camera_frame_id = self.model.getFrameId("camera_tcp")
+
+        if tcps is None:
+            pass
+        else:
+            self.tcps = tcps
+            self.tcp_ids = []
+            try:
+                for tcp in self.tcps:
+                    tcp_id = self.model.getFrameId(tcp)
+                    self.tcp_ids.append(tcp_id)
+            except:
+                print(f'tcp_id of {tcp} not found')
+
 
 
 
@@ -107,14 +115,13 @@ class KinematicsSolver():
             min_j = [-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159]
         if max_j is None:
             max_j = [3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159]
-        if rad_flag:
-            for i in range(7):
-                self.model.lowerPositionLimit[i] = min_j[i]
-                self.model.upperPositionLimit[i] = max_j[i]
-        else:
-            for i in range(7):
-                self.model.lowerPositionLimit[i] = min_j[i] / 180 * pi
-                self.model.upperPositionLimit[i] = max_j[i] / 180 * pi
+
+        k = 1.0 if rad_flag is True else 1.0 / 180 * pi
+
+        for i in range(7):
+            self.model.lowerPositionLimit[i] = min_j[i] * k
+            self.model.upperPositionLimit[i] = max_j[i] * k
+
 
     def forward_kinematics(self, joint_angles, tool="omnipic"):
         """
@@ -142,8 +149,14 @@ class KinematicsSolver():
         pin.updateFramePlacements(self.model, self.data)
 
         # Get frame transform
-        # frame_id = self.tool_frames[tool]
-        frame_id = self.scissor_frame_id
+        if tool in self.tcps:
+            frame_id = self.tcp_ids[self.tcps.index(tool)]
+        else:
+            try:
+                frame_id = self.tool_frames[tool]
+            except:
+                print(f'{tool} definition not found')
+
         frame_transform = self.data.oMf[frame_id]
 
         # Extract results
@@ -154,16 +167,8 @@ class KinematicsSolver():
         rpy = pin.rpy.matrixToRpy(rotation)
 
         # Compute quaternion
-        # quat = pin.Quaternion(rotation)
         pose = np.concatenate([position, rpy], axis=0)
         return pose
-        # return {
-        #     'position': position,
-        #     # 'rotation': rotation,
-        #     'rpy': rpy,
-        #     'quaternion': [quat.x, quat.y, quat.z, quat.w],
-        #     # 'transform': frame_transform
-        # }
 
     def inverse_kinematics(self, target_position, target_rpy=None,
                            target_quat=None, initial_guess=None,
@@ -224,7 +229,14 @@ class KinematicsSolver():
             iter_count = 0
             prev_error = float('inf')
 
-            ee_frame_id = self.tool_frames[tool]
+            # ee_frame_id = self.tool_frames[tool]
+            if tool in self.tcps:
+                ee_frame_id = self.tcp_ids[self.tcps.index(tool)]
+            else:
+                try:
+                    ee_frame_id = self.tool_frames[tool]
+                except:
+                    print(f'{tool} definition not found')
 
             J = pin.computeFrameJacobian(
                 self.model,