Update README.md

add visualization

README.md

@@ -9,6 +9,35 @@ 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):
+
+- 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%**.
+
+
+
+Visualization:
+
+
+![demo](kine_ctrl/visual/mjc_ik_test1/mjc_ik.gif)

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,102 @@ import time
 from math import radians, degrees, pi, cos, sin
 import numpy as np
 
-def demo_position_control():
+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)
+    robot_mjk = MuJoCoPositionController()
 
-    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"
-
-
+    #  -----------      rm75 qp based kine          ------------
     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')
-
-
 
     #   ----------      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)
-
-    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)} ')
 
 
 
+#   --------------  for comparison   ----------------
+    print(f'in the comparison part')
+
+    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
+
+        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}')
+                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_qp}')
+                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'result is {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/rm75_kine_qp.py

@@ -12,7 +12,7 @@ import time
 
 
 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).
@@ -90,18 +90,7 @@ class KinematicsSolver():
 
         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.cfg_j_limit()
 
         # ----------  for reused qp_solver  ------------------
         self.nv = 7
@@ -127,8 +116,21 @@ 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])
 
+    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="ee"):
         """
@@ -167,19 +169,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.
 
@@ -255,8 +258,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,7 +298,7 @@ class KinematicsSolver():
                 # =========================
                 # QP-based IK
                 # =========================
-                w_posture = 0.0
+                w_posture = 0.0001
 
                 J_eff = pin.Jlog6(error_SE3) @ J #J #
 
@@ -342,32 +345,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 +362,12 @@ 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
+            # return best_solution, True, best_error, iter_count
+            return 0, best_solution
         else:
-            return None, False, None
+            # return q[:7].copy(), False, error_norm, iter_count
+            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

@@ -1,6 +1,7 @@
 
 from Robotic_Arm.rm_robot_interface import *
 import numpy as np
+import math
 
 class rm75_kine_api():
     def __init__(self):
@@ -11,6 +12,8 @@ class rm75_kine_api():
         # 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),
@@ -23,15 +26,18 @@ class rm75_kine_api():
         self.tool_name = "ee"
         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  ])
+
+        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 +51,11 @@ 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 forward_kinematics(self, joint_angles, flag  = 1  , tool="ee", 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 +64,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"):
+        '''
+        :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)
@@ -70,11 +87,11 @@ class rm75_kine_api():
         target = target_position + 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)
         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
+        return ret, [ q/180*math.pi for q in q_out]

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>