test push

add visualization

abc.txt

@@ -0,0 +1 @@
+hhelloworrld

README.md

@@ -1,7 +1,5 @@
 ### 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.
@@ -11,9 +9,9 @@ Key specifications:
 2. Success rate
 3. Minial joint variation.
 
-Next:\
-Comparison with Realman official IK method.
-Embedded with current demo.
+Cost:
+
+<img src="img/cost.jpg" alt="Cost" width="400">
 
 
 ### Comparison (05June2026):
@@ -26,6 +24,9 @@ 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])
@@ -34,15 +35,9 @@ 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.
+Visualization:
 
-<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">
+![demo](kine_ctrl/visual/mjc_ik_test1/mjc_ik.gif)

kine_ctrl/fix_robotics_env.sh

@@ -1,6 +0,0 @@
-#!/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

@@ -12,53 +12,18 @@ import time
 from math import radians, degrees, pi, cos, sin
 import numpy as np
 
-#   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"""
 
     # Create controller
     robot_mjk = MuJoCoPositionController()
 
-
+    tool_name = "scissor"
     #  -----------      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)
+    robot_kine_qp = kine_qp()
 
     #   ----------      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)
-
-    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)
-
-
 
 
 
@@ -67,6 +32,16 @@ def main():
 
     if True:
 
+        # 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])
+
+        ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
+        lb = -ub
+
+        robot_kine_qp.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=True)
+
+        robot_kine_rm.cfg_j_limit(min_j=lb, max_j=ub, rad_flag=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
@@ -95,33 +70,32 @@ def main():
             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)
+                print(f'-- success, in the qp ik, fk_qp_p2 = {fk_qp_p2}, d_p_ik = {d_p_ik}')
                 if d_p_ik < 0.01:
                     result[0][1] += 1
 
-                # robot_mjk.send_command(q)
-                # robot_mjk.wait_until_reached()
-                # robot_mjk.print_state()
+                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}')
+                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}')
+                print(f'== sucess, in the rm ik, fk_rm_p2 = {fk_rm_p2}, d_p_ik = {d_p_ik} ,q = {q}, ret_qp = {ret_qp}')
                 if d_p_ik < 0.01:
                     result[1][1] += 1
             else:
-                print(f'==== fail in the rm ik, ret = {ret_rm}, q = {q}')
+                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'result is {result}')
         print(f'solve_sum is {solve_sum}')
 
 

kine_ctrl/req.txt

@@ -1,3 +0,0 @@
-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

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

kine_ctrl/rm75_kine_qp.py

@@ -8,7 +8,6 @@ import osqp
 from scipy import sparse
 from math import radians, degrees, pi, cos, sin
 import time
-import threading
 
 
 
@@ -20,24 +19,78 @@ class KinematicsSolver():
         unit: m, rad
         """
         print(f' ------------  the qp based kinematic initialising   -----------')
-        self.model, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
+        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
-        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]
+        # -------------------------------------------------
+        # 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.w_q_limit = np.diag(1.0 / (q_range ** 2))
 
-        self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
+        # -------------------------------------------------
+        # 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()
+
+        self.cfg_j_limit()
 
         # ----------  for reused qp_solver  ------------------
         self.nv = 7
@@ -64,36 +117,6 @@ class KinematicsSolver():
         )
 
         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 cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
         if min_j is None:
@@ -109,7 +132,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="omnipic"):
+    def forward_kinematics(self, joint_angles, tool="ee"):
         """
         Compute forward kinematics.
 
@@ -178,7 +201,8 @@ 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],
@@ -274,28 +298,28 @@ class KinematicsSolver():
                 # =========================
                 # QP-based IK
                 # =========================
-                w_ref = 0.0001
-                w_limit_mid = 0.00002
+                w_posture = 0.0001
 
                 J_eff = pin.Jlog6(error_SE3) @ J #J #
 
                 H = J_eff.T @ self.W @ J_eff
 
 
-                H += damping * damping * self.joint_motion_weight
-                H += w_ref * np.eye(7)
-                H += w_limit_mid * self.w_q_limit
+                # H = J.T @ self.W @ J
+                H += damping * damping * np.eye(7)
+                H += w_posture * np.eye(7)
 
                 H_triu = sparse.triu(H).tocsc()
 
                 g = -J_eff.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)
+                g += w_posture * (q[:7] - q_ref[:7])
+                # g = - J.T @ self.W @ error_vec
 
                 # -------------------------
                 # Joint velocity constraints
                 # -------------------------
-                dq_limit =  np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ])  # rad per iteration
+
+                dq_limit = 0.05  # rad per iteration
 
                 lb = -dq_limit * np.ones(7)
                 ub = dq_limit * np.ones(7)
@@ -339,128 +363,11 @@ class KinematicsSolver():
                 iter_count += 1
 
         if best_solution is not None:
-            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()
+            return 0, best_solution
         else:
             # 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]
+            return -1, q[:7].copy()
 
     # def invese_kinematics_velocity(self, target_position, target_rpy=None,
     #                        target_quat=None, initial_guess=None, tool="ee"):

kine_ctrl/rm75_kine_rm.py

@@ -7,18 +7,23 @@ 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_75 Robotic arm
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 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.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.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.work_frames = {
-            '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),
+            '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),
         }
 
-        self.tool_name = "no_tool"
+        self.tool_name = "ee"
         self.work_name = "work"
 
     def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True):
@@ -27,8 +32,6 @@ 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())
@@ -48,46 +51,7 @@ class rm75_kine_api():
     def get_tool_frame(self):
         return self.robot_kine_rm.rm_algo_get_curr_toolframe()
 
-    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"):
+    def forward_kinematics(self, joint_angles, flag  = 1  , tool="ee", 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]
@@ -102,7 +66,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="omnipic", work="work"):
+    def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"):
         '''
         :param target_position:     list of position values, m
         :param target_rpy:          list of rpy values, rad
@@ -120,37 +84,14 @@ class rm75_kine_api():
             self.work_name = work
             self.cfg_work_frame(work)
 
-        target = list(target_position) + list(target_rpy)
+        target = target_position + target_rpy
 
         if initial_guess is not None:
             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)
-        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/workspace_comfortable/Contour_plot.py

@@ -1,80 +0,0 @@
-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

@@ -1,750 +0,0 @@
-"""
-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)