Update README.md

add visualization

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:\
+Cost:
-Comparison with Realman official IK method.
+
-Embedded with current demo.
+<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. 
+Visualization:
-- 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">
+![demo](kine_ctrl/visual/mjc_ik_test1/mjc_ik.gif)
-
-<img src="img/osqp.png" alt="Cost" width="400">

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,41 +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 = kine_qp()
-    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)
 
 
 
@@ -55,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
@@ -108,7 +95,7 @@ def main():
             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/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
 
 
 
@@ -22,18 +21,76 @@ class KinematicsSolver():
         print(f' ------------  the qp based kinematic initialising   -----------')
         self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
-
+        # -------------------------------------------------
-
+        # ee
-        self.cfg_j_limit()
+        # -------------------------------------------------
-
+        ee_offset = pin.SE3(np.eye(3), np.array([0, 0, 0.0]))
-        q_range = (
+        self.model.addFrame(
-                self.model.upperPositionLimit[:7] -
+            pin.Frame(
-                self.model.lowerPositionLimit[:7]
+                "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
@@ -60,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:
@@ -105,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.
 
@@ -174,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],
@@ -270,28 +298,28 @@ class KinematicsSolver():
                 # =========================
                 # QP-based IK
                 # =========================
-                w_ref = 0.0001
+                w_posture = 0.0001
-                w_limit_mid = 0.00002
 
                 J_eff = pin.Jlog6(error_SE3) @ J #J #
 
                 H = J_eff.T @ self.W @ J_eff
 
 
-                H += damping * damping * self.joint_motion_weight
+                # H = J.T @ self.W @ J
-                H += w_ref * np.eye(7)
+                H += damping * damping * np.eye(7)
-                H += w_limit_mid * self.w_q_limit
+                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_posture * (q[:7] - q_ref[:7])
-                g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
+                # 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)
@@ -336,39 +364,10 @@ class KinematicsSolver():
 
         if best_solution is not None:
             # return best_solution, True, best_error, iter_count
-            return 0, best_solution.tolist()
+            return 0, best_solution
         else:
             # return q[:7].copy(), False, error_norm, iter_count
-            return -1, q[:7].copy().tolist()
+            return -1, q[:7].copy()
-
-    def quaternion_to_euler(self, q):
-        """
-        Convert quaternion to Euler angles (roll, pitch, yaw)
-
-        Args:
-            qx, qy, qz, qw: quaternion components
-
-        Returns:
-            tuple: (roll, pitch, yaw) in radians
-        """
-        # Roll (x-axis rotation)
-        sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
-        cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
-        roll = np.arctan2(sinr_cosp, cosr_cosp)
-
-        # Pitch (y-axis rotation)
-        sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
-        if abs(sinp) >= 1:
-            pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
-        else:
-            pitch = np.arcsin(sinp)
-
-        # Yaw (z-axis rotation)
-        siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
-        cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
-        yaw = np.arctan2(siny_cosp, cosy_cosp)
-
-        return [roll, pitch, yaw]
 
     # def invese_kinematics_velocity(self, target_position, target_rpy=None,
     #                        target_quat=None, initial_guess=None, tool="ee"):

kine_ctrl/rm75_kine_rm.py

@@ -14,11 +14,16 @@ class rm75_kine_api():
 
         self.cfg_j_limit()
 
+        self.tool_frames = {
+            'ee': rm_frame_t(frame_name="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
+            'scissor': rm_frame_t(frame_name="scissor", pose=(0.0, 0.0, 0.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):
+    def forward_kinematics(self, joint_angles, flag  = 1  , tool="ee", work="work"):
-        """
-        Convert quaternion to Euler angles (roll, pitch, yaw)
-
-        Args:
-            qx, qy, qz, qw: quaternion components
-
-        Returns:
-            tuple: (roll, pitch, yaw) in radians
-        """
-        # Roll (x-axis rotation)
-        sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2])
-        cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1])
-        roll = np.arctan2(sinr_cosp, cosr_cosp)
-
-        # Pitch (y-axis rotation)
-        sinp = 2.0 * (q[3] * q[1] - q[2] * q[0])
-        if abs(sinp) >= 1:
-            pitch = np.copysign(np.pi / 2, sinp)  # Use 90 degrees if out of range
-        else:
-            pitch = np.arcsin(sinp)
-
-        # Yaw (z-axis rotation)
-        siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1])
-        cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
-        yaw = np.arctan2(siny_cosp, cosy_cosp)
-
-        return [roll, pitch, yaw]
-
-    def add_tool_frames(self, dict_frames):
-        self.tool_frames = {}
-        for tool_name in dict_frames:
-            tool_attr = dict_frames[tool_name]
-            position = tool_attr[0][0:3]
-            rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7])
-            f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0)
-
-            self.tool_frames.update({tool_name:f})
-
-    def forward_kinematics(self, joint_angles, flag  = 1  , tool="omnipic", work="work"):
         '''
         :param joint_angles:   list of joint values, in rad
         :param flag:       0: return list [x,y,z,w,x,y,z]. 1: return list [x,y,z,rx,ry,rz]
@@ -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