collision detection ik

add

README.md

@@ -1,5 +1,7 @@
 ### This repo is for inverse kinematics and verification
 
+In this branch, the qp-based inverse kinematics method is modified as a python class. The user can call it as in `main.py`
+
 Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
 
 Verification is done with Mujoco simulation.
@@ -12,3 +14,35 @@ Key specifications:
 Next:\
 Comparison with Realman official IK method.
 Embedded with current demo.
+
+
+### Comparison (05June2026):
+
+- With current dual arm joint limit, 
+```
+ub = np.array([150.0, 110.0, 170.0,  130, 175.0, 125.0,  179.0])
+lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0,  -179.0])
+```
+the success rates for **qp-based ik** and **realman Algo ik** are **63%** and **46%**.\
+At least one solver works out the ik, rate = **74%**.
+
+- With realman-75 physical joint limit,
+```
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])
+lb = -ub
+```
+the success rates for **qp-based ik** and **realman Algo ik** are **76%** and **51%**.\
+At least one solver works out the ik, rate = **84%**.
+
+### update(1st July 2026)
+
+In each iteration, update optimization formula:
+
+- new cost item for distance from middle of the joint range. 
+- set up different weight for different joints motion.
+
+<img src="img/optimization.png" alt="Cost" width="400">
+
+<img src="img/cons.png" alt="Cost" width="400">
+
+<img src="img/osqp.png" alt="Cost" width="400">

kine_ctrl/fix_robotics_env.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+echo "Fixing robotics environment..."
+conda activate coppeliasim
+export PYTHONPATH="/home/zl/miniforge3/envs/coppeliasim/lib/python3.10/site-packages"
+pip install osqp==0.6.2.post8 --force-reinstall
+python -c "import osqp; print(f'OSQP version: {osqp.__version__}')"

kine_ctrl/main.py

@@ -1,4 +1,4 @@
-from casadi import print_operator
+
 
 #   conda activate coppeliasim
 #   env fix, in terminal:  fix_robotics_env.sh
@@ -12,107 +12,128 @@ import time
 from math import radians, degrees, pi, cos, sin
 import numpy as np
 
-def demo_position_control():
+#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
+#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
+tools_in_ee = {
+    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
+}
+
+# joint limit
+# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
+# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
+
+
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
+
+tool_name = "scissor"
+
+def main():
     """Demonstrate pure position control"""
 
-    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
-
-
-
     # Create controller
-    robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
-    robot_mjk.start()
-    time.sleep(1)
-
-    print("\n[Test 1] Move joint 1 to 45 degrees")
-    robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0])
-    robot_mjk.wait_until_reached()
-    robot_mjk.print_state()
-    time.sleep(0.5)
-
-    # print("\n[Test 2] Move joint 2 to -30 degrees")
-    # robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
-    # robot_mjk.wait_until_reached()
-    # robot_mjk.print_state()
-    # time.sleep(0.5)
-    #
-    # print("\n[Test 3] Move multiple joints simultaneously")
-    # robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
-    # robot_mjk.wait_until_reached()
-    # robot_mjk.print_state()
-    # time.sleep(0.5)
-
-    print("\n[Test 4] Return home\n")
-    robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
-    robot_mjk.wait_until_reached()
-    robot_mjk.print_state()
-
-
-
-
-#---------------------------------------------------------------------------
-
-
-    joints = [10, 20, -30, -40, 50, 60, 70]
-    joints_rad = [radians(j)  for j in joints] #radians(joints)
-    # target_position = [0.3, 0.2, 0.4]
-    # target_rpy = [0.0, 0.0, 3.14*0.25]
-    target_position = [0.17892041, 0.25274317, 0.83107248]
-    target_rpy = [0.78576018, 0.67554633, 1.86302226]
-    target_p = target_position + target_rpy
-    # target_p_rad = [radians(pos) for pos in target_position] + target_rpy
-    initial_guess = [11, 20, -30, -40, 50, 60, 71] # [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] #
-    initial_guess_rad = [ radians(j) for j in initial_guess ]
-    tool_name = "scissor"
-
-
-    robot_kine_qp = kine_qp()
-    print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}')
-
-    joint_solution, success, error = robot_kine_qp.inverse_kinematics(
-            target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
-            max_iter=500, debug=False, tool=tool_name
-        )
-
-    print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
-    if success:
-        print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
+    robot_mjk = MuJoCoPositionController()
 
 
+    #  -----------      rm75 qp based kine          ------------
+    robot_kine_qp = kine_qp(urdf_path='/home/zl/Downloads/urdf_rm75/RM75-B.urdf', mesh_dir='/home/zl/Downloads/urdf_rm75')
+    robot_kine_qp.add_tool_frames(tools_in_ee)
+    robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
     #   ----------      rm75 official algorithm         -----------
     robot_kine_rm = kine_rm()
-    print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}')
-    ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name)
+    robot_kine_rm.add_tool_frames(tools_in_ee)
+    robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
 
-    print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
-    if ret == 0:
-        print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')
+    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=[-0.6, -0.6 , 0. ], target_rpy=[1.2022060487764064, -1.0097962261845583, -0.6518417572686532],
+                                                 initial_guess=[0.1] * 7, tool="no_tool")
+
+    print(f'ret_rm = {ret_rm}, q = {q}')
+    pose = robot_kine_rm.forward_kinematics(joint_angles=q, tool="no_tool")
+    print(f'pose = {pose}')
+    print('-'*100)
+
+    time.sleep(5)
 
 
 
 
 
+#   --------------  for comparison   ----------------
+    print(f'in the comparison part')
+
+    if True:
+
+        result = np.array([[0,0],[0,0]], dtype=np.int32)        # to collect ik result   qp_fk, qp_ik, rm_fk, rm_ik
+
+        solve_sum = 0
+
+        for i in range(10):
+            print(f'\n-------------- in i = {i} ----------------')
+            joint_rand = np.random.uniform(ub, lb)
+            print(f'the predefined joints are {joint_rand}')
+
+            #   -------------- fk ------------------
+            fk_qp_p1 = robot_kine_qp.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
+
+            fk_rm_p1 = robot_kine_rm.forward_kinematics(joint_angles=joint_rand.tolist(), tool=tool_name)
+
+            d_fk = cal_pose_deviation(pose1=fk_rm_p1, pose2=fk_qp_p1)
+            print(f'fk_qp_p1 = {fk_qp_p1}, fk_rm_p1 = {fk_rm_p1}, d_fk = {d_fk}\n')
 
 
+            #   ----------- ik ----------------
+            t_p = fk_rm_p1
+            joint_rand_init = np.random.uniform(ub, lb)
+            print(f'the guess is {joint_rand_init}')
+
+            ret_qp, q = robot_kine_qp.inverse_kinematics( target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
+
+            if ret_qp == 0:
+                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)
+                if d_p_ik < 0.01:
+                    result[0][1] += 1
+
+                # robot_mjk.send_command(q)
+                # robot_mjk.wait_until_reached()
+                # robot_mjk.print_state()
+            else:
+                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'---- fail, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik},q = {q}, ret_qp = {ret_qp}')
+
+            ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=t_p[0:3], target_rpy=t_p[3:6], initial_guess=joint_rand_init, tool=tool_name)
+            if ret_rm == 0:
+                fk_rm_p2 = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
+                d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_rm_p2)
+                print(f'==== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_rm}')
+                if d_p_ik < 0.01:
+                    result[1][1] += 1
+            else:
+                print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
+
+            if ret_qp == 0 or ret_rm == 0:
+                solve_sum += 1
+
+        print(f'results with qp and rm for ik are {result}')
+        print(f'solve_sum is {solve_sum}')
 
 
-
-    print(f'\nDone\n')
-
-
-    # try:
-    #     while robot_mjk.viewer and robot_mjk.viewer.is_running():
-    #         time.sleep(0.1)
-    # except KeyboardInterrupt:
-    #     pass
-    robot_mjk.stop()
-
-
-
-def main():
-
-    demo_position_control()
+def cal_pose_deviation(pose1, pose2):
+    d_fk_p1 = np.array(pose1) - np.array(pose2)
+    for j in [3, 4, 5]:
+        while d_fk_p1[j] > pi:
+            d_fk_p1[j] -= 2 * pi
+        while d_fk_p1[j] < -pi:
+            d_fk_p1[j] += 2 * pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk
 
 
 

kine_ctrl/req.txt

@@ -0,0 +1,3 @@
+conda install -c conda-forge osqp scipy tqdm matplotlib pandas "numpy<1.24" pinocchio -y
+pip install urdfpy mujoco "networkx>=2.8.4"
+pip install Robotic_Arm

kine_ctrl/requirements.txt

@@ -0,0 +1,9 @@
+numpy
+pandas
+matplotlib
+tqdm
+scipy
+urdfpy
+pin
+osqp
+Robotic_Arm

kine_ctrl/rm75_kine_qp.py

@@ -8,100 +8,36 @@ import osqp
 from scipy import sparse
 from math import radians, degrees, pi, cos, sin
 import time
+import threading
 
 
 
 class KinematicsSolver():
-    def __init__(self,urdf_path="/home/zl/Downloads/urdf_rm75/RM75-B.urdf", mesh_dir="/home/zl/Downloads/meshes"):
+    def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
         """
         for realman 75b
         Initialize robotic arm kinematics using Pinocchio (ROS2 version).
         unit: m, rad
         """
         print(f' ------------  the qp based kinematic initialising   -----------')
-        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
+        self.model, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
-        # -------------------------------------------------
-        # ee
-        # -------------------------------------------------
-        ee_offset = pin.SE3(np.eye(3), np.array([0, 0, 0.0]))
-        self.model.addFrame(
-            pin.Frame(
-                "ee",
-                self.model.getJointId("joint_7"),
-                self.model.getFrameId("link_7"),
-                ee_offset,
-                pin.FrameType.OP_FRAME
-            )
+        self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir)
+        self.geom_model.addAllCollisionPairs()
+        self.remove_adjacent_collision_pairs(verbose=True)
+        self.geom_data = pin.GeometryData(self.geom_model)
+
+
+        self.cfg_j_limit()
+
+        q_range = (
+                self.model.upperPositionLimit[:7] -
+                self.model.lowerPositionLimit[:7]
         )
 
+        self.w_q_limit = np.diag(1.0 / (q_range ** 2))
 
-        # -------------------------------------------------
-        # Scissor tool
-        # -------------------------------------------------
-        scissor_offset = pin.SE3(
-            np.eye(3),
-            np.array([0.0, 0.0, 0.144])
-        )
-        self.model.addFrame(
-            pin.Frame(
-                "scissor",
-                self.model.getJointId("joint_7"),
-                self.model.getFrameId("link_7"),
-                scissor_offset,
-                pin.FrameType.OP_FRAME
-            )
-        )
-
-        # -------------------------------------------------
-        # Camera tool
-        # -------------------------------------------------
-        camera_rotation = pin.rpy.rpyToMatrix(
-            radians(-90),
-            0,
-            radians(-90)
-        )
-        camera_offset = pin.SE3(
-            camera_rotation,
-            np.array([0.05, 0.02, 0.10])
-        )
-        self.model.addFrame(
-            pin.Frame(
-                "camera",
-                self.model.getJointId("joint_7"),
-                self.model.getFrameId("link_7"),
-                camera_offset,
-                pin.FrameType.OP_FRAME
-            )
-        )
-
-        # -------------------------------------------------
-        # 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()
-
-        # Joint limits (radians) - expanded for better reachability
-        self.lower_limits = np.array([
-            -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159
-        ])
-        self.upper_limits = np.array([
-            3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
-        ])
-
-        # Set joint limits in the model
-        for i in range(7):
-            self.model.lowerPositionLimit[i] = self.lower_limits[i]
-            self.model.upperPositionLimit[i] = self.upper_limits[i]
+        self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
 
         # ----------  for reused qp_solver  ------------------
         self.nv = 7
@@ -127,10 +63,53 @@ class KinematicsSolver():
             polish=False
         )
 
-        self.W = np.diag([1, 1, 1, 0.2, 0.2, 0.2])
+        self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
+        # Smaller value => joint moves more actively
+        # Larger value  => joint moves less / more lazy
+        self.joint_motion_weight = np.diag([
+            1.0, 1.0, 1.0, 1.0,
+            0.3, 0.3, 0.2
+        ])
+
+    def add_frame(self,frame_name, position, rotationXYZ):
+        '''
+        :param frame_name:  str
+        :param position: [x, y, z] target position (meters)
+        :param rotationXYZ: [x, y, z] target rotation (rad)
+        '''
+        camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0],  rotationXYZ[1],  rotationXYZ[2] )
+        camera_offset = pin.SE3(
+            camera_rotation,
+            np.array(position)
+        )
+        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 forward_kinematics(self, joint_angles, tool="ee"):
+    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]
+        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
+
+    def forward_kinematics(self, joint_angles, tool="omnipic"):
         """
         Compute forward kinematics.
 
@@ -167,19 +146,20 @@ class KinematicsSolver():
         rpy = pin.rpy.matrixToRpy(rotation)
 
         # Compute quaternion
-        quat = pin.Quaternion(rotation)
-
-        return {
-            'position': position,
-            # 'rotation': rotation,
-            'rpy': rpy,
-            'quaternion': [quat.x, quat.y, quat.z, quat.w],
-            # 'transform': frame_transform
-        }
+        # 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,
-                           max_iter=500, tolerance=3e-3, debug=False, tool="ee"):
+                           max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
         """
         Compute inverse kinematics using differential IK with multiple strategies.
 
@@ -198,8 +178,7 @@ class KinematicsSolver():
         """
         # Build target SE3 placement
         if target_quat is not None:
-            quat = pin.Quaternion(target_quat[3], target_quat[0],
-                                  target_quat[1], target_quat[2])
+            quat = pin.Quaternion(target_quat[3], target_quat[0], target_quat[1], target_quat[2])
             target_rotation = quat.matrix()
         elif target_rpy is not None:
             target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
@@ -255,8 +234,8 @@ class KinematicsSolver():
             error_SE3 = current_placement.actInv(target_placement)
             error_vec = pin.log(error_SE3).vector
 
-            print("initial error =", np.linalg.norm(error_vec))
-            print(error_vec)
+            # print("\n initial error =", np.linalg.norm(error_vec))
+            # print(error_vec)
 
             while iter_count < max_iter:
                 # Compute forward kinematics
@@ -295,28 +274,28 @@ class KinematicsSolver():
                 # =========================
                 # QP-based IK
                 # =========================
-                w_posture = 0.0
+                w_ref = 0.0001
+                w_limit_mid = 0.00002
 
                 J_eff = pin.Jlog6(error_SE3) @ J #J #
 
                 H = J_eff.T @ self.W @ J_eff
 
 
-                # H = J.T @ self.W @ J
-                H += damping * damping * np.eye(7)
-                H += w_posture * np.eye(7)
+                H += damping * damping * self.joint_motion_weight
+                H += w_ref * np.eye(7)
+                H += w_limit_mid * self.w_q_limit
 
                 H_triu = sparse.triu(H).tocsc()
 
                 g = -J_eff.T @ self.W @ error_vec
-                g += w_posture * (q[:7] - q_ref[:7])
-                # g = - J.T @ self.W @ error_vec
+                g += w_ref * (q[:7] - q_ref[:7])
+                g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
 
                 # -------------------------
                 # Joint velocity constraints
                 # -------------------------
-
-                dq_limit = 0.05  # rad per iteration
+                dq_limit =  np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ])  # rad per iteration
 
                 lb = -dq_limit * np.ones(7)
                 ub = dq_limit * np.ones(7)
@@ -342,32 +321,13 @@ class KinematicsSolver():
                     u=ub
                 )
 
-
-
-                print("iter", iter_count)
-                print("error", error_norm)
-                print("cond(H)", np.linalg.cond(H))
-
                 # Solve
                 result = self.osqp_solver.solve()
-                print("OSQP status =", result.info.status)
-                print("dq =", result.x)
-
-                if result.x is not None:
-                    print("dq norm:", np.linalg.norm(result.x))
-
                 if result.info.status != 'solved':
                     break
 
                 dq = result.x
 
-                pred_err = np.linalg.norm(error_vec)
-                pred_next = np.linalg.norm(error_vec - J_eff @ dq)
-
-                print("predicted error:", pred_next)
-
-                print(f'pred = {J_eff @ dq} and error_vec = {error_vec}')
-
                 if dq is None:
                     break
 
@@ -378,27 +338,129 @@ class KinematicsSolver():
                 prev_error = error_norm
                 iter_count += 1
 
-                print("target:", target_position, target_rpy)
-
-                print("initial guess:", np.degrees(initial_guess))
-
-                fk0 = self.forward_kinematics(initial_guess)
-                print("fk guess:", fk0)
-
-                print("initial error norm:", error_norm)
-
-                print(iter_count,
-                      error_norm,
-                      result.info.status)
-
         if best_solution is not None:
-            print(
-                "converged",
-                error_norm,
-            )
-            return best_solution, True, best_error
+            collision = self.collision_detect(q=best_solution, stop_at_first_collision=True)
+
+            if collision is False:
+                # return best_solution, True, best_error, iter_count
+                return 0, best_solution.tolist()
+            else:
+                return -2, q[:7].copy().tolist()
         else:
-            return None, False, None
+            # return q[:7].copy(), False, error_norm, iter_count
+            return -1, q[:7].copy().tolist()
+
+    def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ):
+        q = np.asarray(q, dtype=np.float64).reshape(-1)
+
+        if q.shape[0] != self.model.nq:
+            raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}")
+
+        # Update robot kinematics
+        pin.forwardKinematics(self.model, self.data, q)
+        pin.updateGeometryPlacements(
+            self.model,
+            self.data,
+            self.geom_model,
+            self.geom_data,
+            q
+        )
+
+        # Now compute collisions on the updated geometry model
+        collision = pin.computeCollisions(
+            self.geom_model,
+            self.geom_data,
+            stop_at_first_collision
+        )
+
+        if verbose:
+            print(f"the collision is {collision}\n")
+
+            for k, cr in enumerate(self.geom_data.collisionResults):
+                if cr.isCollision():
+                    cp = self.geom_model.collisionPairs[k]
+                    geom1 = self.geom_model.geometryObjects[cp.first]
+                    geom2 = self.geom_model.geometryObjects[cp.second]
+
+                    print(
+                        f"collision pair {k}: "
+                        f"{geom1.name} <--> {geom2.name}"
+                    )
+
+        return bool(collision)
+
+    def remove_adjacent_collision_pairs(self, verbose=True):
+        """
+        Remove collision pairs between same/adjacent parent joints.
+
+        This avoids false positives such as:
+            base_link_0 <--> link_1_0
+        """
+
+        pairs_to_remove = []
+
+        for pair_id, pair in enumerate(self.geom_model.collisionPairs):
+            geom1 = self.geom_model.geometryObjects[pair.first]
+            geom2 = self.geom_model.geometryObjects[pair.second]
+
+            j1 = geom1.parentJoint
+            j2 = geom2.parentJoint
+
+            # Same body or directly connected bodies
+            if j1 == j2 or abs(j1 - j2) <= 1:
+                pairs_to_remove.append(pair_id)
+
+                if verbose:
+                    print(
+                        "Removing adjacent pair:",
+                        pair_id,
+                        geom1.name,
+                        "<-->",
+                        geom2.name,
+                        "parentJoint:",
+                        j1,
+                        j2,
+                    )
+
+        for pair_id in reversed(pairs_to_remove):
+            self.geom_model.removeCollisionPair(
+                self.geom_model.collisionPairs[pair_id]
+            )
+
+        # Important: recreate geometry data after modifying pairs
+        self.geom_data = pin.GeometryData(self.geom_model)
+
+        if verbose:
+            print("Remaining collision pairs:", len(self.geom_model.collisionPairs))
+
+    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

@@ -1,37 +1,40 @@
 
 from Robotic_Arm.rm_robot_interface import *
 import numpy as np
+import math
 
 class rm75_kine_api():
     def __init__(self):
         #   ----------      rm75 official algorithm         -----------
         print(f'-------  the realman official kinematic initialising -------')
-        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_75 Robotic arm
         force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
         # Initialize the robotic arm model and sensor type in the algorithm
         self.robot_kine_rm = Algo(arm_model, force_type)
 
-        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.144, 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.cfg_j_limit()
+
         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_limit(self):
-        joint_max_limit = np.array([
-            3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
-        ]) * 57
-        self.robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist())
-        joint_min_limit = np.array([
-            -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159
-        ]) * 57
-        self.robot_kine_rm.rm_algo_set_joint_min_limit(joint_min_limit.tolist())
+    def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
+        if max_j is None:
+            max_j = np.array([3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159])
+        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())
+        else:
+            self.robot_kine_rm.rm_algo_set_joint_max_limit(max_j.tolist())
+            self.robot_kine_rm.rm_algo_set_joint_min_limit(min_j.tolist())
 
     def cfg_work_frame(self , frame_name):
         self.robot_kine_rm.rm_algo_set_workframe(self.work_frames[frame_name])
@@ -45,10 +48,50 @@ class rm75_kine_api():
     def get_tool_frame(self):
         return self.robot_kine_rm.rm_algo_get_curr_toolframe()
 
-    def forward_kinematics(self, q, 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 q:   list of joint values, in degree
-        flag:       0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
+        :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]
+        :param return:      [x,y,z,rx,ry,rz], m & rad
         '''
         if tool != self.tool_name:
             self.tool_name = tool
@@ -57,9 +100,19 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        return self.robot_kine_rm.rm_algo_forward_kinematics(joint=q, flag=flag)
+        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
+        :param initial_guess:       initial guess of angles, rad
+        :param tool:                tool name, refer to self.tool_frames
+        :param work:                work name, refer to self.work_frames
+
+        return  ret:                 state of ik calculation, 0:success, -2: out of workspace
+                [q_]:               the ik calculated angles for joints, rad
+        '''
         if tool != self.tool_name:
             self.tool_name = tool
             self.cfg_tool_frame(tool)
@@ -67,14 +120,37 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = target_position + target_rpy
+        target = list(target_position) + list(target_rpy)
 
         if initial_guess is not None:
-            q_ref = initial_guess
+            q_ref = [ 180/math.pi * ig for ig in  initial_guess ]
         else:
             q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
         ret, phi = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
+        # print(f'the arm angle is ret = {ret}, and phi = {phi}')
         params = rm_inverse_kinematics_params_t(q_ref,
                                                 target, 1)
         ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
-        return ret, q_out
+        pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1)
+        pose_dis = cal_pose_deviation(pose_fk, target)
+
+        # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}')
+        #
+        # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target}  phi = {phi}, q_out = {q_out}, ret = {ret}\n\n')
+        # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}')
+        if int(ret) < 0:
+            return ret, [ q/180*math.pi for q in q_out]
+        elif pose_dis < 0.01:
+            return ret, [ q/180*math.pi for q in q_out]
+        else:
+            return -10, [ q/180*math.pi for q in q_out]
+
+def cal_pose_deviation(pose1, pose2):
+    d_fk_p1 = np.array(pose1) - np.array(pose2)
+    for j in [3, 4, 5]:
+        while d_fk_p1[j] > math.pi:
+            d_fk_p1[j] -= 2 * math.pi
+        while d_fk_p1[j] < -math.pi:
+            d_fk_p1[j] += 2 * math.pi
+    d_fk = np.linalg.norm(d_fk_p1)
+    return d_fk

kine_ctrl/rm75_mjc.py

@@ -18,7 +18,7 @@ class MuJoCoPositionController:
     No velocity commands, no forces - completely stable
     """
 
-    def __init__(self, urdf_path, smoothness=0.2, enable_viewer=True):
+    def __init__(self, urdf_path="./urdf_rm75/RM75-B.urdf", smoothness=0.05, enable_viewer=True):
         """
         Args:
             urdf_path: Path to URDF file
@@ -48,8 +48,8 @@ class MuJoCoPositionController:
             print(
                 f"  {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
 
-        # Target positions (in radians)
-        self.target_positions = self.data.qpos[:self.n_joints].copy()
+        # Target joint angles (in radians)
+        self.target_joints = self.data.qpos[:self.n_joints].copy()
 
         # Smoothing factor (0-1, lower = smoother)
         self.smoothness = smoothness
@@ -57,9 +57,9 @@ class MuJoCoPositionController:
         # Thread safety
         self.command_lock = threading.Lock()
         self.feedback_lock = threading.Lock()
-        self.current_feedback = self.data.qpos[:self.n_joints].copy()
+        self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
 
-        self.max_pos_inc = 0.02
+        self.max_ang_inc = 0.02
 
         # Control flags
         self.running = False
@@ -73,8 +73,7 @@ class MuJoCoPositionController:
                 print("Viewer launched")
             except Exception as e:
                 print(f"Viewer warning: {e}")
-
-        print("Robot controller ready - Pure Position Mode")
+        self.start()
 
     def start(self):
         """Start the simulation thread"""
@@ -109,17 +108,17 @@ class MuJoCoPositionController:
             cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
 
         with self.command_lock:
-            self.target_positions = cmd
+            self.target_joints = cmd
 
     def get_feedback(self):
         """Get current joint positions"""
         with self.feedback_lock:
-            return self.current_feedback.copy()
+            return self.current_feedback_joint.copy()
 
     def get_target(self):
         """Get current target positions"""
         with self.command_lock:
-            return self.target_positions.copy()
+            return self.target_joints.copy()
 
     def _simulation_loop(self):
         """
@@ -129,24 +128,24 @@ class MuJoCoPositionController:
         last_time = time.time()
 
         # For smooth interpolation
-        current_positions = self.data.qpos[:self.n_joints].copy()
+        current_joints = self.data.qpos[:self.n_joints].copy()
 
         while self.running:
             # Get target command
             with self.command_lock:
-                target = self.target_positions.copy()
+                target = self.target_joints.copy()
 
             # Get current positions
-            current_positions = self.data.qpos[:self.n_joints].copy()
+            current_joints = self.data.qpos[:self.n_joints].copy()
 
             # Smooth interpolation toward target
             # This creates natural motion without velocity commands
             alpha = self.smoothness
 
-            next_positions = current_positions + np.clip(alpha * (target - current_positions) , -self.max_pos_inc, self.max_pos_inc)
+            next_joints = current_joints + np.clip(alpha * (target - current_joints) , -self.max_ang_inc, self.max_ang_inc)
 
             # DIRECT POSITION CONTROL - Set joint positions
-            self.data.qpos[:self.n_joints] = next_positions
+            self.data.qpos[:self.n_joints] = next_joints
 
             # IMPORTANT: Set velocities to zero to prevent physics from moving joints
             # This ensures pure kinematic control
@@ -157,12 +156,12 @@ class MuJoCoPositionController:
 
             # After step, ensure our joint positions are maintained
             # (Physics might have altered them slightly)
-            self.data.qpos[:self.n_joints] = next_positions
+            self.data.qpos[:self.n_joints] = next_joints
             self.data.qvel[:self.n_joints] = 0
 
             # Update feedback
             with self.feedback_lock:
-                self.current_feedback = self.data.qpos[:self.n_joints].copy()
+                self.current_feedback_joint = self.data.qpos[:self.n_joints].copy()
 
             # Sync viewer
             if self.viewer:
@@ -175,20 +174,20 @@ class MuJoCoPositionController:
                 time.sleep(sleep_time)
             last_time = time.time()
 
-    def move_to_position(self, target, duration=1.0):
+    def move_to_joints(self, target, duration=1.0):
         """
-        Move to target position over specified duration
+        Move to target joints over specified duration
 
         Args:
-            target: Target joint positions
+            target: Target joint joints
             duration: Time to complete movement (seconds)
         """
-        start_pos = self.get_feedback()
-        end_pos = np.array(target[:self.n_joints])
+        start_js = self.get_feedback()
+        end_js = np.array(target[:self.n_joints])
 
         # Apply limits
         for i in range(self.n_joints):
-            end_pos[i] = np.clip(end_pos[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
+            end_js[i] = np.clip(end_js[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
 
         n_steps = int(duration / self.time_interval)
 
@@ -198,15 +197,15 @@ class MuJoCoPositionController:
             alpha = (step + 1) / n_steps
             # Use easing for smoother motion
             ease_alpha = 1 - (1 - alpha) ** 2  # Quadratic ease-out
-            current_target = start_pos + ease_alpha * (end_pos - start_pos)
+            current_target = start_js + ease_alpha * (end_js - start_js)
             self.send_command(current_target)
             time.sleep(self.time_interval)
 
         # Ensure exact target
-        self.send_command(end_pos)
+        self.send_command(end_js)
         time.sleep(0.1)
 
-    def wait_until_reached(self, tolerance=0.01, timeout=5.0):
+    def wait_until_reached(self, tolerance=0.01, timeout=10.0):
         """
         Wait until robot reaches target position
 
@@ -230,10 +229,10 @@ class MuJoCoPositionController:
 
     def print_state(self):
         """Print current robot state"""
-        positions = self.get_feedback()
+        joints = self.get_feedback()
         target = self.get_target()
-        print("Current positions:", [f"{p:.3f}" for p in positions[:4]], "...")
-        print("Target positions: ", [f"{t:.3f}" for t in target[:4]], "...")
+        print("Current joints (rad):", [f"{p:.3f}" for p in joints], "...")
+        print("Target joints (rad): ", [f"{t:.3f}" for t in target], "...")
 
 
 # Demo

kine_ctrl/urdf_rm75/RM75-B.csv

@@ -0,0 +1,9 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,0.00049987,5.2709E-05,0.060019,0,0,0,0.83887,0.0017232,-3.1058E-06,-3.7924E-05,0.0017051,1.3691E-06,0.00090158,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,,连杆1-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+link_1,1.4803E-07,-0.021108,-0.025186,0,0,0,0.59354,0.0012661,6.0354E-09,-6.3788E-09,0.0011817,-0.00021121,0.00056132,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,,连杆2-1,link_1,joint_1,joint_1,revolute,0,0,0.2405,0,0,0,base_link,0,0,1,60,3.14,-3.106,3.106,,,,,,,,
+link_2,4.2145E-07,-0.076129,0.011078,0,0,0,0.43285,0.0012584,1.4694E-09,-5.7413E-09,0.00031747,0.000279,0.0012225,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,,连杆3-1,link_2,joint_2,joint_2,revolute,0,0,0,-1.5708,0,0,link_1,0,0,1,60,3.14,-2.2689,2.2689,,,,,,,,
+link_3,-3.2093E-07,-0.023545,-0.027347,0,0,0,0.43132,0.00079433,1.02E-09,1.3908E-08,0.00073037,-0.00014262,0.00031507,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,,连杆4-1,link_3,joint_3,joint_3,revolute,0,-0.256,0,1.5708,0,0,link_2,0,0,1,30,3.14,-3.106,3.106,,,,,,,,
+link_4,5.0722E-06,-0.059593,0.010569,0,0,0,0.28963,0.00063737,7.0681E-08,3.8708E-08,0.00015648,0.00014461,0.00061418,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,,连杆5-1,link_4,joint_4,joint_4,revolute,0,0,0,-1.5708,0,0,link_3,0,0,1,30,3.14,-2.356,2.356,,,,,,,,
+link_5,2.7551E-07,-0.018042,-0.02154,0,0,0,0.23942,0.00028595,1.9823E-09,-1.192E-09,0.00026273,-4.424E-05,0.0001199,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,,连杆6-1,link_5,joint_5,joint_5,revolute,0,-0.21,0,1.5708,0,0,link_4,0,0,1,10,3.14,-3.106,3.106,,,,,,,,
+link_6,3.4947E-06,-0.059381,0.0073681,0,0,0,0.2188,0.00035054,3.4456E-08,1.7975E-08,0.00010493,7.8243E-05,0.00033448,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,,连杆7-1,link_6,joint_6,joint_6,revolute,0,0,0,-1.5708,0,0,link_5,0,0,1,10,3.14,-2.234,2.234,,,,,,,,
+link_7,0.00081557,1.3323E-05,-0.012705,0,0,0,0.065037,2.1144E-05,2.2774E-08,2.5471E-08,1.8109E-05,1.019E-08,3.19E-05,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,,末端法兰件 方案一-1,link_7,joint_7,joint_7,revolute,0,-0.144,0,1.5708,0,0,link_6,0,0,1,10,3.14,-6.28,6.28,,,,,,,,

kine_ctrl/urdf_rm75/RM75-B.urdf

@@ -0,0 +1,453 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="RM75-B">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="0.00049987 5.2709E-05 0.060019"
+        rpy="0 0 0" />
+      <mass
+        value="1.862" />
+      <inertia
+        ixx="0.0017232"
+        ixy="-3.1058E-06"
+        ixz="-3.7924E-05"
+        iyy="0.0017051"
+        iyz="1.3691E-06"
+        izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link_1">
+    <inertial>
+      <origin
+        xyz="0.000241 -0.013273 -0.00995"
+        rpy="0 0 0" />
+      <mass
+        value="1.574" />
+      <inertia
+        ixx="0.002487573"
+        ixy="0.000009663"
+        ixz="-0.000007909"
+        iyy="0.002321038"
+        iyz="0.000179393"
+        izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.2405"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_2">
+    <inertial>
+      <origin
+        xyz="-0.000357 -0.106789 0.005329"
+        rpy="0 0 0" />
+      <mass
+        value="1.217" />
+      <inertia
+        ixx="0.003494121"
+        ixy="0.000002921"
+        ixz="-0.000005613"
+        iyy="0.000892721"
+        iyz="-0.000583884"
+        izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_1" />
+    <child
+      link="link_2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.2689"
+      upper="2.2689"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_3">
+    <inertial>
+      <origin
+        xyz="0.000003 -0.01398 -0.011324"
+        rpy="0 0 0" />
+      <mass
+        value="1.11" />
+      <inertia
+        ixx="0.001836663"
+        ixy="0.000002259"
+        ixz="-0.000004216"
+        iyy="0.001498875"
+        iyz="0.000037167"
+        izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="0 -0.256 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_2" />
+    <child
+      link="link_3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_4">
+    <inertial>
+      <origin
+        xyz="-0.000005 -0.084658 0.004747"
+        rpy="0 0 0" />
+      <mass
+        value="0.685" />
+      <inertia
+        ixx="0.001282444"
+        ixy="-0.000000551"
+        ixz="-0.000000630"
+        iyy="0.000373013"
+        iyz="-0.000232084"
+        izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_3" />
+    <child
+      link="link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.356"
+      upper="2.356"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_5">
+    <inertial>
+      <origin
+        xyz="0.000078 -0.012937 -0.008781"
+        rpy="0 0 0" />
+      <mass
+        value="0.619" />
+      <inertia
+        ixx="0.000627336"
+        ixy="0.000001636"
+        ixz="-0.000001345"
+        iyy="0.000542455"
+        iyz="0.000034970"
+        izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0 -0.21 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_4" />
+    <child
+      link="link_5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_6">
+    <inertial>
+      <origin
+        xyz="-0.000014 -0.078524 0.002819"
+        rpy="0 0 0" />
+      <mass
+        value="0.602" />
+      <inertia
+        ixx="0.000780774"
+        ixy="-0.000000121"
+        ixz="-0.000000469"
+        iyy="0.000289973"
+        iyz="-0.000120513"
+        izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_5" />
+    <child
+      link="link_6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.234"
+      upper="2.234"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_7">
+    <inertial>
+      <origin
+        xyz="0.001094 -0.000077 -0.010119"
+        rpy="0 0 0" />
+      <mass
+        value="0.107" />
+      <inertia
+        ixx="0.000044123"
+        ixy="-0.000000064"
+        ixz="0.0000003"
+        iyy="0.000035078"
+        iyz="-0.000000029"
+        izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="revolute">
+    <origin
+      xyz="0 -0.144 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_6" />
+    <child
+      link="link_7" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-6.28"
+      upper="6.28"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+</robot>

kine_ctrl/workspace_comfortable/Contour_plot.py

@@ -0,0 +1,80 @@
+from pathlib import Path
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+
+# --------------------------------------------------
+# 1. Load the data
+# --------------------------------------------------
+csv_path = Path("workspace_nocollisiondetection.csv")
+
+# The file has no column names, so header=None is important.
+df = pd.read_csv(
+    csv_path,
+    header=None,
+    names=["x", "y", "z", "ik_rate"],
+)
+
+print(df.head())
+print("z planes:", np.sort(df["z"].unique()))
+
+
+# --------------------------------------------------
+# 2. Create an output directory
+# --------------------------------------------------
+output_dir = Path("contour_plots")
+output_dir.mkdir(exist_ok=True)
+
+
+# --------------------------------------------------
+# 3. Use the same colour scale for every z-plane
+# --------------------------------------------------
+value_min = df["ik_rate"].min()
+value_max = df["ik_rate"].max()
+
+# More levels give a smoother-looking contour plot.
+levels = np.linspace(value_min, value_max, 51)
+
+
+# --------------------------------------------------
+# 4. Draw one contour plot for each z-plane
+# --------------------------------------------------
+for z_value, plane in df.groupby("z", sort=True):
+
+    # Rows become y-coordinates, columns become x-coordinates.
+    grid = plane.pivot(index="y", columns="x", values="ik_rate")
+
+    x = grid.columns.to_numpy()
+    y = grid.index.to_numpy()
+    ik_grid = grid.to_numpy()
+
+    X, Y = np.meshgrid(x, y)
+
+    fig, ax = plt.subplots(figsize=(7, 6))
+
+    contour = ax.contourf(
+        X,
+        Y,
+        ik_grid,
+        levels=levels,
+        cmap="viridis",
+        extend="both",
+    )
+
+    colorbar = fig.colorbar(contour, ax=ax)
+    colorbar.set_label("IK rate")
+
+    ax.set_title(f"IK rate at z = {z_value:.2f}")
+    ax.set_xlabel("x")
+    ax.set_ylabel("y")
+    ax.set_aspect("equal")
+
+    fig.tight_layout()
+
+    output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
+    fig.savefig(output_path, dpi=200, bbox_inches="tight")
+    plt.close(fig)
+
+print(f"Plots saved to: {output_dir.resolve()}")

kine_ctrl/workspace_comfortable/workspace_cal.py

@@ -0,0 +1,750 @@
+"""
+RM75-B comfortable workspace evaluator.
+
+You provide:
+    - URDF file path '/home/zl/Downloads/urdf_rm75/RM75-B.urdf'
+    - your own IK solver inside solve_ik_user()
+
+This script computes:
+    - IK success rate
+    - joint-limit comfort
+    - manipulability
+    - singularity / condition number score
+    - final comfort score
+
+Recommended install:
+    pip install numpy scipy urdfpy pandas matplotlib tqdm
+
+Optional:
+    pip install plotly
+"""
+
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+
+from tqdm import tqdm
+from scipy.spatial.transform import Rotation as R
+from urdfpy import URDF
+
+import sys
+from pathlib import Path
+
+# 1. Get the absolute path of the directory containing this current script
+current_dir = Path(__file__).resolve().parent
+
+# 2. Get the parent (upper) directory
+parent_dir = current_dir.parent
+
+# 3. Add the parent directory to the system path
+sys.path.insert(0, str(parent_dir))
+
+from rm75_kine_qp import KinematicsSolver as kine_qp
+from rm75_kine_rm import rm75_kine_api as kine_rm
+from rm75_mjc import MuJoCoPositionController
+from Robotic_Arm.rm_robot_interface import *
+
+import time
+from math import radians, degrees, pi, cos, sin
+
+# Cartesian workspace grid, in meters.
+# Adjust according to your robot placement and task.
+X_RANGE = (-0.6, 0.6)
+Y_RANGE = (-0.6, 0.6)
+Z_RANGE = (0.0, 0.8)
+
+GRID_RESOLUTION = 0.05  # 5 cm. Use 0.02 for finer but slower.
+
+num_orientations =  120 
+
+# Comfort thresholds
+MIN_JOINT_MARGIN = 0.05       # 15% away from joint limits
+MAX_CONDITION_NUMBER = 150.0
+MIN_MANIPULABILITY_RATIO = 0.10
+
+# Scoring weights
+WEIGHT_IK_SUCCESS = 0.70
+WEIGHT_JOINT_LIMIT = 0.10
+WEIGHT_MANIPULABILITY = 0.1
+WEIGHT_SINGULARITY = 0.1
+
+
+
+
+#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
+#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
+tools_in_ee = {
+    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
+    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
+}
+
+# joint limit
+# ub = np.array([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0]) / 180 * pi
+# lb = np.array([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0]) / 180 * pi
+#
+#
+ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+lb = -ub
+
+tool_name = "no_tool"
+
+URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
+MESH_DIR = str(Path(URDF_PATH).parent)
+
+#  -----------      rm75 qp based kine          ------------
+robot_kine_qp = kine_qp(urdf_path=URDF_PATH, mesh_dir=MESH_DIR)
+robot_kine_qp.add_tool_frames(tools_in_ee)
+robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+#   ----------      rm75 official algorithm         -----------
+robot_kine_rm = kine_rm()
+robot_kine_rm.add_tool_frames(tools_in_ee)
+robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+
+
+# ============================================================
+# 1. USER SETTINGS
+# ============================================================
+
+
+
+BASE_LINK = "base_link"
+TCP_LINK = "link_7"
+
+JOINT_NAMES = [
+    "joint_1",
+    "joint_2",
+    "joint_3",
+    "joint_4",
+    "joint_5",
+    "joint_6",
+    "joint_7",
+]
+
+
+
+# Numerical Jacobian settings
+JACOBIAN_EPS = 1e-5
+
+
+
+# ============================================================
+# 2. TASK ORIENTATION SAMPLING
+# ============================================================
+
+def make_task_orientations(num_orientations=num_orientations, seed=1):
+    """
+    Random orientation sampling using RM's Euler convention:
+
+        R = Rz @ Ry @ Rx
+
+    Note:
+        This samples Euler angles randomly.
+        It is useful, but not perfectly uniform over SO(3).
+    """
+
+    rng = np.random.default_rng(seed)
+
+    orientations = []
+
+    for _ in range(num_orientations):
+        rx = rng.uniform(-np.pi, np.pi)
+        ry = rng.uniform(-np.pi / 2.0, np.pi / 2.0)
+        rz = rng.uniform(-np.pi, np.pi)
+
+        orientations.append([rx, ry, rz])
+
+    return orientations
+
+#
+# def euler_angles_to_rotation_matrix(rx, ry, rz):
+#     """
+#     Official RM convention:
+#         R = Rz @ Ry @ Rx
+#     This matches scipy:
+#         Rotation.from_euler("xyz", [rx, ry, rz]).as_matrix()
+#     """
+#     Rx = np.array([
+#         [1, 0, 0],
+#         [0, np.cos(rx), -np.sin(rx)],
+#         [0, np.sin(rx),  np.cos(rx)]
+#     ])
+#
+#     Ry = np.array([
+#         [ np.cos(ry), 0, np.sin(ry)],
+#         [0,           1, 0],
+#         [-np.sin(ry), 0, np.cos(ry)]
+#     ])
+#
+#     Rz = np.array([
+#         [np.cos(rz), -np.sin(rz), 0],
+#         [np.sin(rz),  np.cos(rz), 0],
+#         [0,           0,          1]
+#     ])
+#
+#     return Rz @ Ry @ Rx
+
+
+# ============================================================
+# 3. YOUR IK FUNCTION GOES HERE
+# ============================================================
+
+def solve_ik_user(target_position, target_rotation):
+    """
+    Replace this function with your own IK solver.
+
+    Parameters
+    ----------
+    target_position : np.ndarray, shape (3,)
+        Desired TCP position in base_link frame.
+
+    target_rotation : np.ndarray, shape (3, 3)
+        Desired TCP rotation matrix in base_link frame.
+
+    Returns
+    -------
+    None
+        If IK fails.
+
+    or
+
+    np.ndarray, shape (7,)
+        One IK solution.
+
+    or
+
+    list[np.ndarray]
+        Multiple IK solutions.
+
+    Important:
+        Joint order must be:
+        [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7]
+    """
+
+    # ========================================================
+    # INSERT YOUR IK CODE HERE
+    # ========================================================
+    initial_guess = [0.1] * 7
+
+    ret_qp, q = robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name, max_iter=250)
+    # print(f'---- with qp ik, ret_qp: {ret_qp}, q = {q}')
+    if ret_qp == 0:
+        return q
+
+
+    ret_rm, q = robot_kine_rm.inverse_kinematics(target_position=target_position, target_rpy=target_rotation, initial_guess=initial_guess, tool=tool_name)
+    # print(f'==== with rm ik, ret_rm: {ret_rm}, q = {q}')
+    if ret_rm == 0:
+        pose_rm = robot_kine_rm.forward_kinematics(joint_angles=q, tool=tool_name)
+        # print(f'target position = {target_position}\ntarget_rpy = {target_rotation} \npose_rm = {pose_rm}')
+        return q
+
+
+    return None
+
+
+# ============================================================
+# 4. URDF / FK UTILITIES
+# ============================================================
+
+def load_robot_and_limits(urdf_path):
+    robot = URDF.load(urdf_path)
+
+    joints = []
+    lower = []
+    upper = []
+
+    joint_map = {j.name: j for j in robot.joints}
+
+    for name in JOINT_NAMES:
+        joint = joint_map[name]
+        joints.append(joint)
+
+        if joint.limit is None:
+            raise ValueError(f"Joint {name} has no limit in URDF.")
+
+        lower.append(joint.limit.lower)
+        upper.append(joint.limit.upper)
+
+    lower = np.asarray(lower, dtype=float)
+    upper = np.asarray(upper, dtype=float)
+
+    return robot, lower, upper
+
+
+# def q_to_cfg(q):
+#     """
+#     Convert joint vector to urdfpy FK config dictionary.
+#     """
+#     return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
+
+
+# def fk_transform(robot, q):
+#     """
+#     Forward kinematics from base_link to TCP_LINK.
+#
+#     Returns
+#     -------
+#     T : np.ndarray, shape (4, 4)
+#     """
+#     cfg = q_to_cfg(q)
+#     fk = robot.link_fk(cfg=cfg)
+#     tcp_link = robot.link_map[TCP_LINK]
+#     return fk[tcp_link]
+#
+#
+# def fk_position(robot, q):
+#     T = fk_transform(robot, q)
+#     return T[:3, 3]
+
+
+# ============================================================
+# 5. COMFORT METRICS
+# ============================================================
+
+def is_within_joint_limits(q, lower, upper, tol=1e-8):
+    q = np.asarray(q)
+    return np.all(q >= lower - tol) and np.all(q <= upper + tol)
+
+
+def joint_limit_score(q, lower, upper):
+    """
+    Score in [0, 1].
+    1 means every joint is at center of its range.
+    0 means at least one joint is at its limit.
+    """
+
+    q = np.asarray(q)
+    mid = 0.5 * (lower + upper)
+    half_range = 0.5 * (upper - lower)
+
+    per_joint_score = 1.0 - np.abs(q - mid) / half_range
+    per_joint_score = np.clip(per_joint_score, 0.0, 1.0)
+
+    # Conservative: one bad joint makes the whole pose less comfortable.
+    return float(np.min(per_joint_score))
+
+
+def joint_margin(q, lower, upper):
+    """
+    Minimum normalized distance to joint limits.
+
+    0.15 means the closest joint is 15% away from its limit.
+    """
+    q = np.asarray(q)
+    margin_lower = (q - lower) / (upper - lower)
+    margin_upper = (upper - q) / (upper - lower)
+    margin = np.minimum(margin_lower, margin_upper)
+    return float(np.min(margin))
+
+
+def q_to_cfg(q):
+    """
+    Convert joint vector to urdfpy FK config dictionary.
+    """
+    return {name: float(q[i]) for i, name in enumerate(JOINT_NAMES)}
+
+
+def fk_transform(robot, q):
+    """
+    Forward kinematics from base_link to TCP_LINK.
+
+    Returns
+    -------
+    T : np.ndarray, shape (4, 4)
+    """
+    cfg = q_to_cfg(q)
+    fk = robot.link_fk(cfg=cfg)
+    tcp_link = robot.link_map[TCP_LINK]
+    return fk[tcp_link]
+
+
+def numerical_geometric_jacobian(robot, q, eps=1e-5):
+    """
+    Numerical 6D geometric-like Jacobian, shape (6, 7).
+
+    Top 3 rows:
+        linear velocity approximation
+
+    Bottom 3 rows:
+        angular velocity approximation as rotation-vector difference
+
+    This is useful for manipulability and singularity checks.
+    """
+    q = np.asarray(q, dtype=float)
+    n = len(q)
+
+    J = np.zeros((6, n))
+
+    T0 = fk_transform(robot, q)
+    p0 = T0[:3, 3]
+    R0 = T0[:3, :3]
+
+    for i in range(n):
+        q_plus = q.copy()
+        q_minus = q.copy()
+
+        q_plus[i] += eps
+        q_minus[i] -= eps
+
+        T_plus = fk_transform(robot, q_plus)
+        T_minus = fk_transform(robot, q_minus)
+
+        p_plus = T_plus[:3, 3]
+        p_minus = T_minus[:3, 3]
+
+        R_plus = T_plus[:3, :3]
+        R_minus = T_minus[:3, :3]
+
+        # Linear part
+        J[:3, i] = (p_plus - p_minus) / (2.0 * eps)
+
+        # Angular part
+        # Relative rotation from minus to plus.
+        dR = R_plus @ R_minus.T
+        rotvec = R.from_matrix(dR).as_rotvec()
+        J[3:, i] = rotvec / (2.0 * eps)
+
+    return J
+
+
+def manipulability_score_from_jacobian(J):
+    """
+    Yoshikawa-style manipulability.
+
+    For a 6x7 Jacobian:
+        w = sqrt(det(J J.T))
+
+    To improve numerical robustness, compute from singular values.
+    """
+    singular_values = np.linalg.svd(J, compute_uv=False)
+
+    # Product of singular values.
+    # For a 6x7 Jacobian, there are 6 singular values.
+    w = float(np.prod(singular_values))
+
+    return w
+
+
+def condition_number_from_jacobian(J, min_sigma=1e-9):
+    singular_values = np.linalg.svd(J, compute_uv=False)
+    sigma_max = np.max(singular_values)
+    sigma_min = np.min(singular_values)
+
+    if sigma_min < min_sigma:
+        return np.inf
+
+    return float(sigma_max / sigma_min)
+
+
+def singularity_score(condition_number):
+    """
+    Score in [0, 1].
+    Higher is better.
+
+    condition_number = 1 is ideal.
+    Very large means near singularity.
+    """
+    if not np.isfinite(condition_number):
+        return 0.0
+
+    return float(1.0 / condition_number)
+
+
+# ============================================================
+# 6. IK RESULT HANDLING
+# ============================================================
+
+def normalize_ik_solutions(ik_result):
+    """
+    Your IK returns:
+        - None if failed
+        - one list/array of 7 joint values if successful
+    """
+    if ik_result is None:
+        return []
+
+    q = np.asarray(ik_result, dtype=float).reshape(-1)
+
+    if q.shape[0] != 7:
+        return []
+
+    return [q]
+
+
+def evaluate_single_solution(robot, q, lower, upper):
+    """
+    Evaluate one IK solution.
+
+    Returns a dictionary with metrics.
+    """
+
+    if q.shape[0] != 7:
+        return None
+
+    if not is_within_joint_limits(q, lower, upper):
+        return None
+
+    jl_score = joint_limit_score(q, lower, upper)
+    jl_margin = joint_margin(q, lower, upper)
+
+    J = numerical_geometric_jacobian(robot, q, eps=JACOBIAN_EPS)
+
+    manip = manipulability_score_from_jacobian(J)
+    cond = condition_number_from_jacobian(J)
+    sing_score = singularity_score(cond)
+
+    valid_by_thresholds = (
+        jl_margin >= MIN_JOINT_MARGIN
+        and cond <= MAX_CONDITION_NUMBER
+    )
+
+    return {
+        "q": q,
+        "joint_limit_score": jl_score,
+        "joint_margin": jl_margin,
+        "manipulability": manip,
+        "condition_number": cond,
+        "singularity_score": sing_score,
+        "valid_by_thresholds": valid_by_thresholds,
+    }
+
+
+# ============================================================
+# 7. MAIN WORKSPACE EVALUATION
+# ============================================================
+
+def make_grid():
+    xs = np.arange(X_RANGE[0], X_RANGE[1] + 1e-9, GRID_RESOLUTION)
+    ys = np.arange(Y_RANGE[0], Y_RANGE[1] + 1e-9, GRID_RESOLUTION)
+    zs = np.arange(Z_RANGE[0], Z_RANGE[1] + 1e-9, GRID_RESOLUTION)
+
+    points = []
+
+    for x in xs:
+        for y in ys:
+            for z in zs:
+                points.append(np.array([x, y, z], dtype=float))
+
+    return points
+
+
+def evaluate_workspace():
+    robot, lower, upper = load_robot_and_limits(URDF_PATH)
+    orientations = make_task_orientations()
+    grid_points = make_grid()
+
+    rows = []
+
+    # First pass stores raw manipulability.
+    # Later we normalize manipulability by max observed value.
+    all_valid_solution_metrics = []
+
+    print(f"Loaded robot from: {URDF_PATH}")
+    print(f"Grid points: {len(grid_points)}")
+    print(f"Orientations per point: {len(orientations)}")
+    print("Evaluating IK reachability and raw metrics...")
+
+    for point in tqdm(grid_points):
+        point_solution_metrics = []
+
+        attempted = 0
+        ik_success_count = 0
+
+
+        for rpy in orientations:
+            attempted += 1
+
+
+            ik_result = solve_ik_user(point, rpy)
+
+            # print(f'\n point is {point}, rpy is {rpy}, and ik result q: {ik_result}')
+            candidate_solutions = normalize_ik_solutions(ik_result)
+
+            if len(candidate_solutions) == 0:
+                continue
+
+            evaluated_solutions = []
+
+            for q in candidate_solutions:
+                # pose = robot_kine_qp.forward_kinematics(joint_angles=q, tool=tool_name)
+                # print(f'the fk of q is {pose}\n')
+                metrics = evaluate_single_solution(robot, q, lower, upper)
+                # print(f'matrics: {metrics}, q = {q}, lower = {lower}, upper = {upper}')
+                if metrics is not None:
+                    evaluated_solutions.append(metrics)
+
+            if len(evaluated_solutions) == 0:
+                continue
+
+            ik_success_count += 1
+
+            # Use the best solution for this pose.
+            # At this stage, manipulability is not normalized,
+            # so use joint score + singularity score as temporary ranking.
+            best = max(
+                evaluated_solutions,
+                key=lambda m: 0.6 * m["joint_limit_score"] + 0.4 * m["singularity_score"]
+            )
+
+            point_solution_metrics.append(best)
+            all_valid_solution_metrics.append(best)
+        print(f'this position+all orientations, the point_solution_metrics = {point_solution_metrics}')
+        ik_success_rate = ik_success_count / attempted if attempted > 0 else 0.0
+
+        if len(point_solution_metrics) == 0:
+            rows.append({
+                "x": point[0],
+                "y": point[1],
+                "z": point[2],
+                "ik_success_rate": 0.0,
+                "joint_limit_score": 0.0,
+                "joint_margin": 0.0,
+                "manipulability": 0.0,
+                "manipulability_score": 0.0,
+                "condition_number": np.inf,
+                "singularity_score": 0.0,
+                "comfort_score": 0.0,
+                "comfortable": False,
+                "reachable": False,
+            })
+        else:
+            # Average over task orientations.
+            rows.append({
+                "x": point[0],
+                "y": point[1],
+                "z": point[2],
+                "ik_success_rate": ik_success_rate,
+                "joint_limit_score": np.mean([m["joint_limit_score"] for m in point_solution_metrics]),
+                "joint_margin": np.mean([m["joint_margin"] for m in point_solution_metrics]),
+                "manipulability": np.mean([m["manipulability"] for m in point_solution_metrics]),
+                "manipulability_score": 0.0,  # filled later
+                "condition_number": np.mean([m["condition_number"] for m in point_solution_metrics]),
+                "singularity_score": np.mean([m["singularity_score"] for m in point_solution_metrics]),
+                "comfort_score": 0.0,  # filled later
+                "comfortable": False,
+                "reachable": True,
+            })
+
+    df = pd.DataFrame(rows)
+
+    # Normalize manipulability by maximum observed value.
+    max_manip = df["manipulability"].replace([np.inf, -np.inf], np.nan).max()
+
+    if max_manip is None or not np.isfinite(max_manip) or max_manip <= 0:
+        max_manip = 1.0
+
+    df["manipulability_score"] = df["manipulability"] / max_manip
+    df["manipulability_score"] = df["manipulability_score"].clip(0.0, 1.0)
+
+    # Final comfort score.
+    df["comfort_score"] = (
+        WEIGHT_IK_SUCCESS * df["ik_success_rate"]
+        + WEIGHT_JOINT_LIMIT * df["joint_limit_score"]
+        + WEIGHT_MANIPULABILITY * df["manipulability_score"]
+        + WEIGHT_SINGULARITY * df["singularity_score"]
+    )
+
+    # Comfortable binary classification.
+    df["comfortable"] = (
+        (df["reachable"] == True)
+        & (df["ik_success_rate"] >= 0.80)
+        & (df["joint_margin"] >= MIN_JOINT_MARGIN)
+        & (df["condition_number"] <= MAX_CONDITION_NUMBER)
+        & (df["manipulability_score"] >= MIN_MANIPULABILITY_RATIO)
+    )
+
+    return df
+
+
+# ============================================================
+# 8. PLOTTING
+# ============================================================
+
+def plot_workspace(df):
+    """
+    3D scatter plot:
+        gray/low = low comfort
+        brighter = higher comfort
+    """
+
+    reachable = df[df["reachable"] == True]
+
+    if len(reachable) == 0:
+        print("No reachable points found. Check your IK function.")
+        return
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection="3d")
+
+    sc = ax.scatter(
+        reachable["x"],
+        reachable["y"],
+        reachable["z"],
+        c=reachable["comfort_score"],
+        s=12,
+        alpha=0.8,
+    )
+
+    ax.set_title("RM75-B Comfortable Workspace")
+    ax.set_xlabel("X [m]")
+    ax.set_ylabel("Y [m]")
+    ax.set_zlabel("Z [m]")
+
+    fig.colorbar(sc, ax=ax, label="Comfort score")
+    plt.show()
+
+
+def plot_comfortable_only(df):
+    comfortable = df[df["comfortable"] == True]
+
+    if len(comfortable) == 0:
+        print("No comfortable points found under current thresholds.")
+        return
+
+    fig = plt.figure()
+    ax = fig.add_subplot(111, projection="3d")
+
+    ax.scatter(
+        comfortable["x"],
+        comfortable["y"],
+        comfortable["z"],
+        c=comfortable["comfort_score"],
+        s=16,
+        alpha=0.9,
+    )
+
+    ax.set_title("RM75-B Comfortable Region Only")
+    ax.set_xlabel("X [m]")
+    ax.set_ylabel("Y [m]")
+    ax.set_zlabel("Z [m]")
+
+    plt.show()
+
+
+# ============================================================
+# 9. ENTRY POINT
+# ============================================================
+
+if __name__ == "__main__":
+    df = evaluate_workspace()
+
+    output_csv = "rm75b_comfort_workspace.csv"
+    df.to_csv(output_csv, index=False)
+
+    print(f"\nSaved result to: {output_csv}")
+
+    print("\nSummary:")
+    print(f"Total grid points:       {len(df)}")
+    print(f"Reachable points:        {df['reachable'].sum()}")
+    print(f"Comfortable points:      {df['comfortable'].sum()}")
+
+    if df["reachable"].sum() > 0:
+        print(f"Max comfort score:       {df['comfort_score'].max():.3f}")
+        print(f"Mean comfort score:      {df[df['reachable']]['comfort_score'].mean():.3f}")
+
+    plot_workspace(df)
+    plot_comfortable_only(df)