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,8 +1,10 @@
 
+
 #   conda activate coppeliasim
-#   env fix, in terminal:  ~/fix_robotics_env.sh
+#   env fix, in terminal:  fix_robotics_env.sh
 
-from rm75_kine_qp import KinematicsSolver as kine_ctrl
+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 *
 
@@ -10,184 +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)
-
-    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")
-    robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
-    robot_mjk.wait_until_reached()
-    robot_mjk.print_state()
-
-
-
-
-
-    robot_kine = kine_ctrl()
-
-    # Test 1: Forward Kinematics
-    print("\n1. Forward Kinematics Test")
-    print("-" * 40)
+    robot_mjk = MuJoCoPositionController()
 
     tool_name = "scissor"
-    joint_angles_zero = [0.1] * 7
-    fk_result = robot_kine.forward_kinematics(joint_angles_zero, tool=tool_name)
-
-    # Test 2: Inverse Kinematics with more reachable target
-    print("\n2. Inverse Kinematics Test")
-    print("-" * 40)
-
-    # Try a simpler target first
-    target_pos = [0.3, 0.2, 0.4]  # More reachable position
-    target_rpy = [0.0, 0.0, radians(45)]  # Simpler orientation
-
-    print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
-
-    init_joints = [0.2] * 7
-    time0 = time.time()
-    for ii in range(100):
-        joint_solution, success, error = robot_kine.inverse_kinematics(
-            target_pos, target_rpy=target_rpy, initial_guess=init_joints,
-            max_iter=500, debug=False, tool=tool_name
-        )
-    time1 = time.time()
-    print(f"Time: {time1 - time0}")
-
-    if success:
-        print(f"✓ Solution found! Error: {error:.6f} m")
-        for i, angle in enumerate(joint_solution):
-            print(f"  Joint {i + 1}: {degrees(angle):7.2f}°")
-
-        # Verify
-        fk_verify = robot_kine.forward_kinematics(joint_solution,tool=tool_name)
-        print(
-            f"  Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
-    else:
-        print("✗ IK failed to find a solution!")
-
-    # Test 3: Jacobian
-    print("\n3. Jacobian Matrix")
-    print("-" * 40)
-
-    J = robot_kine.compute_jacobian(joint_angles_zero, tool=tool_name)
-    print(f"Jacobian shape: {J.shape}")
-    for i in range(min(3, J.shape[0])):
-        row_str = "  ".join([f"{J[i, j]:7.3f}" for j in range(7)])
-        print(f"  Row {i + 1}: {row_str}")
-
-    # Test 4: Trajectory Planning with reachable positions
-    print("\n4. Cartesian Trajectory Planning")
-    print("-" * 40)
-
-    start_pos = [0.3, 0.0, 0.4]  # Start position
-    end_pos = [0.3, 0.0, 0.55]  # End position (smaller movement)
-
-    fk0 = robot_kine.forward_kinematics([0.1] * 7, tool=tool_name)
-
-    trajectory = robot_kine.plan_cartesian_trajectory(
-        start_pos,
-        end_pos,
-        start_rpy=fk0['rpy'],
-        end_rpy=[
-            fk0['rpy'][0] + radians(10),
-            fk0['rpy'][1],
-            fk0['rpy'][2]
-        ],
-        num_steps=10,
-        tool=tool_name
-    )
-
-    if trajectory:
-        print(f"\n✓ Generated {len(trajectory)} waypoints")
-
-    if success:
-        print("✓ Inverse kinematics working (with simplified target)")
-    else:
-        print("⚠ Inverse kinematics may need tuning - try different targets")
-
-
-    print("\n" + "=" * 60)
-    print(f'test subchain Jacobian, for future obstacle avoidance')
-    frame_names = [
-        "link_2",
-        "link_4",
-        "link_7"
-    ]
-    Js_sub = robot_kine.get_subchain_jacobian(
-        joint_angles=joint_angles_zero,
-        frame_names=frame_names
-    )
-    print(f'Js_sub: {Js_sub}')
-
+    #  -----------      rm75 qp based kine          ------------
+    robot_kine_qp = kine_qp()
 
     #   ----------      rm75 official algorithm         -----------
-    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
-    robot_kine_rm = Algo(arm_model, force_type)
-    frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0])
-    robot_kine_rm.rm_algo_set_workframe(frame)
-    print(robot_kine_rm.rm_algo_get_curr_workframe())
-    frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0])
-    robot_kine_rm.rm_algo_set_toolframe(frame)
-    print(robot_kine_rm.rm_algo_get_curr_toolframe())
-    joint_max_limit = np.array([
-            3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
-        ])*57
-    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
-    robot_kine_rm.rm_algo_set_joint_min_limit(joint_max_limit.tolist())
-
-    q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
-    ret, phi = robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
-    params = rm_inverse_kinematics_params_t([0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0],
-                                            [0.3, 0.0, 0.3, 3.14, 0.0, 3.14], 1)
-    ret, q_out = robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
-    print(f"rm_algo_inverse_kinematics_rm75_for_arm_angle ret: {ret} q_out: {q_out}")
+    robot_kine_rm = kine_rm()
 
 
 
+#   --------------  for comparison   ----------------
+    print(f'in the comparison part')
 
-    try:
-        while robot_mjk.viewer and robot_mjk.viewer.is_running():
-            time.sleep(0.1)
-    except KeyboardInterrupt:
-        pass
-    robot_mjk.stop()
+    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}')
 
-def main():
+            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)
 
-    demo_position_control()
+            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}')
+
+
+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,12 +12,13 @@ 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).
+        unit: m, rad
         """
-
+        print(f' ------------  the qp based kinematic initialising   -----------')
         self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
 
         # -------------------------------------------------
@@ -44,7 +45,7 @@ class KinematicsSolver():
         )
         self.model.addFrame(
             pin.Frame(
-                "scissor_tcp",
+                "scissor",
                 self.model.getJointId("joint_7"),
                 self.model.getFrameId("link_7"),
                 scissor_offset,
@@ -66,7 +67,7 @@ class KinematicsSolver():
         )
         self.model.addFrame(
             pin.Frame(
-                "camera_frame",
+                "camera",
                 self.model.getJointId("joint_7"),
                 self.model.getFrameId("link_7"),
                 camera_offset,
@@ -79,8 +80,8 @@ class KinematicsSolver():
         # -------------------------------------------------
 
         self.tool_frames = {
-            "scissor": self.model.getFrameId("scissor_tcp"),
-            "camera": self.model.getFrameId("camera_frame"),
+            "scissor": self.model.getFrameId("scissor"),
+            "camera": self.model.getFrameId("camera"),
             "ee":   self.model.getFrameId("ee")
         }
 
@@ -89,26 +90,16 @@ 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
 
         # Full dense symmetric matrix structure
-        P_template = np.triu(np.ones((7, 7)))
+        # P_template = np.triu(np.ones((7, 7)))
+        self.P_pattern = sparse.triu(np.ones((7,7))).tocsc()
 
-        P_sparse = sparse.csc_matrix(P_template)
+        P_sparse = sparse.csc_matrix(self.P_pattern)
 
         A_sparse = sparse.eye(7, format='csc')
 
@@ -125,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"):
         """
@@ -138,6 +142,8 @@ class KinematicsSolver():
 
         Returns:
             dict: Position, rotation, rpy, quaternion
+                unit:   position:   m
+                        rpy:        rad
         """
         if len(joint_angles) != 7:
             raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
@@ -163,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=200, tolerance=1e-3, debug=False, tool="ee"):
+                           max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
         """
         Compute inverse kinematics using differential IK with multiple strategies.
 
@@ -214,10 +221,7 @@ class KinematicsSolver():
         else:
             # Try different initial configurations
             initial_guesses.append([0.1] * 7)  # Zero config
-            initial_guesses.append([radians(30), radians(45), radians(30),
-                                    radians(-45), radians(30), radians(-30), 0])
-            initial_guesses.append([radians(-30), radians(45), radians(-30),
-                                    radians(45), radians(30), radians(30), 0])
+
 
         best_solution = None
         best_error = float('inf')
@@ -228,9 +232,10 @@ class KinematicsSolver():
                 if i < len(q):
                     q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
                                    self.model.upperPositionLimit[i])
+            q_ref = q.copy()
 
             # Differential IK with adaptive damping
-            damping = 0.01
+            damping = 0.1
             damping_reduction = 0.95
             iter_count = 0
             prev_error = float('inf')
@@ -242,9 +247,20 @@ class KinematicsSolver():
                 self.data,
                 q,
                 ee_frame_id,
-                pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+                pin.ReferenceFrame.LOCAL
             )
 
+            pin.forwardKinematics(self.model, self.data, q)
+            pin.updateFramePlacements(self.model, self.data)
+
+            current_placement = self.data.oMf[ee_frame_id]
+
+            error_SE3 = current_placement.actInv(target_placement)
+            error_vec = pin.log(error_SE3).vector
+
+            # print("\n initial error =", np.linalg.norm(error_vec))
+            # print(error_vec)
+
             while iter_count < max_iter:
                 # Compute forward kinematics
 
@@ -252,7 +268,6 @@ class KinematicsSolver():
                 pin.framesForwardKinematics(self.model, self.data, q)
 
                 # Get current end-effector placement
-
                 current_placement = self.data.oMf[ee_frame_id]
 
                 # Compute error
@@ -261,13 +276,9 @@ class KinematicsSolver():
                 error_norm = np.linalg.norm(error_vec)
 
                 if error_norm < tolerance:
-                    joint_angles = q[:7].copy()
-                    fk_result = self.forward_kinematics(joint_angles,tool=tool)
-                    position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
-
-                    if position_error < best_error:
-                        best_error = position_error
-                        best_solution = joint_angles
+                    if error_norm < best_error:
+                        best_error = error_norm
+                        best_solution = q[:7].copy()
                     break
 
                 # Check if error is increasing (diverging)
@@ -281,19 +292,28 @@ class KinematicsSolver():
                     self.model,
                     self.data,
                     ee_frame_id,
-                    pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+                    pin.ReferenceFrame.LOCAL
                 )
 
                 # =========================
                 # QP-based IK
                 # =========================
+                w_posture = 0.0001
 
-                H = J.T @ self.W @ J
+                J_eff = pin.Jlog6(error_SE3) @ J #J #
+
+                H = J_eff.T @ self.W @ J_eff
+
+
+                # H = J.T @ self.W @ J
                 H += damping * damping * np.eye(7)
+                H += w_posture * np.eye(7)
 
                 H_triu = sparse.triu(H).tocsc()
 
-                g = -J.T @ self.W @ error_vec
+                g = -J_eff.T @ self.W @ error_vec
+                g += w_posture * (q[:7] - q_ref[:7])
+                # g = - J.T @ self.W @ error_vec
 
                 # -------------------------
                 # Joint velocity constraints
@@ -319,7 +339,7 @@ class KinematicsSolver():
                 # ------------------------
                 # Update solver
                 self.osqp_solver.update(
-                    Px=H_triu.data,
+                    Px= H_triu.data, #H[np.triu_indices(7)], #
                     q=g,
                     l=lb,
                     u=ub
@@ -327,178 +347,6 @@ class KinematicsSolver():
 
                 # Solve
                 result = self.osqp_solver.solve()
-
-                if result.info.status != 'solved':
-                    break
-
-                dq = result.x
-
-
-                if dq is None:
-                    break
-
-                # Apply joint limits with scaling
-                alpha = 0.5
-                q = pin.integrate(self.model, q, alpha * dq)
-
-                prev_error = error_norm
-                iter_count += 1
-
-        if best_solution is not None:
-            return best_solution, True, best_error
-        else:
-            return None, False, None
-
-    def invese_kinematics_velocity(self, target_position, target_rpy=None,
-                           target_quat=None, initial_guess=None, tool="ee"):
-        """
-                Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
-
-                Args:
-                    target_position: [x, y, z] target position (meters)
-                    target_rpy: [roll, pitch, yaw] target orientation (radians)
-                    target_quat: [x, y, z, w] target orientation as quaternion
-                    initial_guess: Initial joint angles (radians)
-                    tool: the frame name ('scissor', 'camera', 'ee')
-
-                Returns:
-                    joint_velocity: np.array()
-                """
-        # 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])
-            target_rotation = quat.matrix()
-        elif target_rpy is not None:
-            target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
-                                                  target_rpy[1],
-                                                  target_rpy[2])
-        else:
-            target_rotation = np.eye(3)
-
-        target_placement = pin.SE3(target_rotation, np.array(target_position))
-
-        # Try multiple initial guesses
-        initial_guesses = []
-
-        if initial_guess is not None:
-            initial_guesses.append(initial_guess)
-        else:
-            # Try different initial configurations
-            initial_guesses.append([0.1] * 7)  # Zero config
-            initial_guesses.append([radians(30), radians(45), radians(30),
-                                    radians(-45), radians(30), radians(-30), 0])
-            initial_guesses.append([radians(-30), radians(45), radians(-30),
-                                    radians(45), radians(30), radians(30), 0])
-
-        best_solution = None
-        best_error = float('inf')
-
-        for guess_idx, guess in enumerate(initial_guesses):
-            q = pin.neutral(self.model)
-            for i, angle in enumerate(guess):
-                if i < len(q):
-                    q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
-                                   self.model.upperPositionLimit[i])
-
-            # Differential IK with adaptive damping
-            damping = 0.01
-            damping_reduction = 0.95
-            iter_count = 0
-            prev_error = float('inf')
-
-            ee_frame_id = self.tool_frames[tool]
-
-            J = pin.computeFrameJacobian(
-                self.model,
-                self.data,
-                q,
-                ee_frame_id,
-                pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
-            )
-
-            while iter_count < max_iter:
-                # Compute forward kinematics
-
-                pin.computeJointJacobians(self.model, self.data, q)
-                pin.framesForwardKinematics(self.model, self.data, q)
-
-                # Get current end-effector placement
-
-                current_placement = self.data.oMf[ee_frame_id]
-
-                # Compute error
-                error_SE3 = current_placement.actInv(target_placement)
-                error_vec = pin.log(error_SE3).vector
-                error_norm = np.linalg.norm(error_vec)
-
-                if error_norm < tolerance:
-                    joint_angles = q[:7].copy()
-                    fk_result = self.forward_kinematics(joint_angles, tool=tool)
-                    position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
-
-                    if position_error < best_error:
-                        best_error = position_error
-                        best_solution = joint_angles
-                    break
-
-                # Check if error is increasing (diverging)
-                if error_norm > prev_error * 1.1 and iter_count > 10:
-                    damping = min(1.0, damping * 1.5)
-                else:
-                    damping = max(0.01, damping * damping_reduction)
-
-                J = pin.getFrameJacobian(
-                    self.model,
-                    self.data,
-                    ee_frame_id,
-                    pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
-                )
-
-                # =========================
-                # QP-based IK
-                # =========================
-
-                H = J.T @ self.W @ J
-                H += damping * damping * np.eye(7)
-
-                H_triu = sparse.triu(H).tocsc()
-
-                g = -J.T @ self.W @ error_vec
-
-                # -------------------------
-                # Joint velocity constraints
-                # -------------------------
-
-                dq_limit = 0.05  # rad per iteration
-
-                lb = -dq_limit * np.ones(7)
-                ub = dq_limit * np.ones(7)
-
-                # -------------------------
-                # Joint position constraints
-                # -------------------------
-
-                q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
-                q_max_step = self.model.upperPositionLimit[:7] - q[:7]
-
-                lb = np.maximum(lb, q_min_step)
-                ub = np.minimum(ub, q_max_step)
-
-                # -------------------------
-                # Solve QP
-                # ------------------------
-                # Update solver
-                self.osqp_solver.update(
-                    Px=H_triu.data,
-                    q=g,
-                    l=lb,
-                    u=ub
-                )
-
-                # Solve
-                result = self.osqp_solver.solve()
-
                 if result.info.status != 'solved':
                     break
 
@@ -508,16 +356,188 @@ class KinematicsSolver():
                     break
 
                 # Apply joint limits with scaling
-                alpha = 0.5
+                alpha = 1.0
                 q = pin.integrate(self.model, q, alpha * dq)
 
                 prev_error = error_norm
                 iter_count += 1
 
         if best_solution is not None:
-            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"):
+    #     """
+    #             Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
+    #
+    #             Args:
+    #                 target_position: [x, y, z] target position (meters)
+    #                 target_rpy: [roll, pitch, yaw] target orientation (radians)
+    #                 target_quat: [x, y, z, w] target orientation as quaternion
+    #                 initial_guess: Initial joint angles (radians)
+    #                 tool: the frame name ('scissor', 'camera', 'ee')
+    #
+    #             Returns:
+    #                 joint_velocity: np.array()
+    #             """
+    #     # 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])
+    #         target_rotation = quat.matrix()
+    #     elif target_rpy is not None:
+    #         target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
+    #                                               target_rpy[1],
+    #                                               target_rpy[2])
+    #     else:
+    #         target_rotation = np.eye(3)
+    #
+    #     target_placement = pin.SE3(target_rotation, np.array(target_position))
+    #
+    #     # Try multiple initial guesses
+    #     initial_guesses = []
+    #
+    #     if initial_guess is not None:
+    #         initial_guesses.append(initial_guess)
+    #     else:
+    #         # Try different initial configurations
+    #         initial_guesses.append([0.1] * 7)  # Zero config
+    #         initial_guesses.append([radians(30), radians(45), radians(30),
+    #                                 radians(-45), radians(30), radians(-30), 0])
+    #         initial_guesses.append([radians(-30), radians(45), radians(-30),
+    #                                 radians(45), radians(30), radians(30), 0])
+    #
+    #     best_solution = None
+    #     best_error = float('inf')
+    #
+    #     for guess_idx, guess in enumerate(initial_guesses):
+    #         q = pin.neutral(self.model)
+    #         for i, angle in enumerate(guess):
+    #             if i < len(q):
+    #                 q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
+    #                                self.model.upperPositionLimit[i])
+    #
+    #         # Differential IK with adaptive damping
+    #         damping = 0.01
+    #         damping_reduction = 0.95
+    #         iter_count = 0
+    #         prev_error = float('inf')
+    #
+    #         ee_frame_id = self.tool_frames[tool]
+    #
+    #         J = pin.computeFrameJacobian(
+    #             self.model,
+    #             self.data,
+    #             q,
+    #             ee_frame_id,
+    #             pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+    #         )
+    #
+    #         while iter_count < max_iter:
+    #             # Compute forward kinematics
+    #
+    #             pin.computeJointJacobians(self.model, self.data, q)
+    #             pin.framesForwardKinematics(self.model, self.data, q)
+    #
+    #             # Get current end-effector placement
+    #
+    #             current_placement = self.data.oMf[ee_frame_id]
+    #
+    #             # Compute error
+    #             error_SE3 = current_placement.actInv(target_placement)
+    #             error_vec = pin.log(error_SE3).vector
+    #             error_norm = np.linalg.norm(error_vec)
+    #
+    #             if error_norm < tolerance:
+    #                 joint_angles = q[:7].copy()
+    #                 fk_result = self.forward_kinematics(joint_angles, tool=tool)
+    #                 position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
+    #
+    #                 if position_error < best_error:
+    #                     best_error = position_error
+    #                     best_solution = joint_angles
+    #                 break
+    #
+    #             # Check if error is increasing (diverging)
+    #             if error_norm > prev_error * 1.1 and iter_count > 10:
+    #                 damping = min(1.0, damping * 1.5)
+    #             else:
+    #                 damping = max(0.01, damping * damping_reduction)
+    #
+    #             J = pin.getFrameJacobian(
+    #                 self.model,
+    #                 self.data,
+    #                 ee_frame_id,
+    #                 pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+    #             )
+    #
+    #             # =========================
+    #             # QP-based IK
+    #             # =========================
+    #
+    #             H = J.T @ self.W @ J
+    #             H += damping * damping * np.eye(7)
+    #
+    #             H_triu = sparse.triu(H).tocsc()
+    #
+    #             g = -J.T @ self.W @ error_vec
+    #
+    #             # -------------------------
+    #             # Joint velocity constraints
+    #             # -------------------------
+    #
+    #             dq_limit = 0.05  # rad per iteration
+    #
+    #             lb = -dq_limit * np.ones(7)
+    #             ub = dq_limit * np.ones(7)
+    #
+    #             # -------------------------
+    #             # Joint position constraints
+    #             # -------------------------
+    #
+    #             q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
+    #             q_max_step = self.model.upperPositionLimit[:7] - q[:7]
+    #
+    #             lb = np.maximum(lb, q_min_step)
+    #             ub = np.minimum(ub, q_max_step)
+    #
+    #             # -------------------------
+    #             # Solve QP
+    #             # ------------------------
+    #             # Update solver
+    #             self.osqp_solver.update(
+    #                 Px=H_triu.data,
+    #                 q=g,
+    #                 l=lb,
+    #                 u=ub
+    #             )
+    #
+    #             # Solve
+    #             result = self.osqp_solver.solve()
+    #
+    #             if result.info.status != 'solved':
+    #                 break
+    #
+    #             dq = result.x
+    #
+    #             if dq is None:
+    #                 break
+    #
+    #             # Apply joint limits with scaling
+    #             alpha = 0.5
+    #             q = pin.integrate(self.model, q, alpha * dq)
+    #
+    #             prev_error = error_norm
+    #             iter_count += 1
+    #
+    #     if best_solution is not None:
+    #         return best_solution, True, best_error
+    #     else:
+    #         return None, False, None
 
     def compute_jacobian(self, joint_angles, tool="ee"):
         """Compute geometric Jacobian (6x7)"""
@@ -557,7 +577,7 @@ class KinematicsSolver():
                 self.model,
                 self.data,
                 frame_id,
-                pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+                pin.ReferenceFrame.LOCAL
             )
             Js.append(J[:, active_joints])
 

kine_ctrl/rm75_kine_rm.py

@@ -0,0 +1,97 @@
+
+from Robotic_Arm.rm_robot_interface import *
+import numpy as np
+import math
+
+class rm75_kine_api():
+    def __init__(self):
+        #   ----------      rm75 official algorithm         -----------
+        print(f'-------  the realman official kinematic initialising -------')
+        arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+        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="ee", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0),
+        }
+
+        self.tool_name = "ee"
+        self.work_name = "work"
+
+    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])
+
+    def get_work_frame(self):
+        return self.robot_kine_rm.rm_algo_get_curr_workframe()
+
+    def cfg_tool_frame(self, frame_name ):
+        self.robot_kine_rm.rm_algo_set_toolframe(self.tool_frames[frame_name])
+
+    def get_tool_frame(self):
+        return self.robot_kine_rm.rm_algo_get_curr_toolframe()
+
+    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]
+        :param return:      [x,y,z,rx,ry,rz], m & rad
+        '''
+        if tool != self.tool_name:
+            self.tool_name = tool
+            self.cfg_tool_frame(tool)
+        if work != self.work_name:
+            self.work_name = work
+            self.cfg_work_frame(work)
+
+        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)
+        if work != self.work_name:
+            self.work_name = work
+            self.cfg_work_frame(work)
+
+        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)
+        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/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>