refine qp based controller

kine_ctrl/main.py

@@ -1,8 +1,10 @@
+from casadi import print_operator
 
 #   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 *
 
@@ -28,19 +30,19 @@ def demo_position_control():
     robot_mjk.print_state()
     time.sleep(0.5)
 
-    print("\n[Test 2] Move joint 2 to -30 degrees")
+    # print("\n[Test 2] Move joint 2 to -30 degrees")
-    robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
+    # robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
-    robot_mjk.wait_until_reached()
+    # robot_mjk.wait_until_reached()
-    robot_mjk.print_state()
+    # robot_mjk.print_state()
-    time.sleep(0.5)
+    # 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 3] Move multiple joints simultaneously")
+    print("\n[Test 4] Return home\n")
-    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()
@@ -48,139 +50,62 @@ def demo_position_control():
 
 
 
+#---------------------------------------------------------------------------
 
-    robot_kine = kine_ctrl()
-
-    # Test 1: Forward Kinematics
-    print("\n1. Forward Kinematics Test")
-    print("-" * 40)
 
+    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"
-    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
+    robot_kine_qp = kine_qp()
-    target_pos = [0.3, 0.2, 0.4]  # More reachable position
+    print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}')
-    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")
+    joint_solution, success, error = robot_kine_qp.inverse_kinematics(
-
+            target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
-    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}")
 
+    print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
     if success:
-        print(f"✓ Solution found! Error: {error:.6f} m")
+        print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')
-        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 official algorithm         -----------
-    arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E  # RM_65 Robotic arm
+    robot_kine_rm = kine_rm()
-    force_type = rm_force_type_e.RM_MODEL_RM_B_E  # Standard version
+    print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}')
-    # Initialize the robotic arm model and sensor type in the algorithm
+    ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name)
-    robot_kine_rm = 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]
+    print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
-    ret, phi = robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
+    if ret == 0:
-    params = rm_inverse_kinematics_params_t([0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0],
+        print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')
-                                            [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}")
 
 
 
 
-    try:
+
-        while robot_mjk.viewer and robot_mjk.viewer.is_running():
+
-            time.sleep(0.1)
+
-    except KeyboardInterrupt:
+
-        pass
+
+
+    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()
 
 

kine_ctrl/rm75_kine_qp.py

@@ -16,8 +16,9 @@ class KinematicsSolver():
         """
         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"),
+            "scissor": self.model.getFrameId("scissor"),
-            "camera": self.model.getFrameId("camera_frame"),
+            "camera": self.model.getFrameId("camera"),
             "ee":   self.model.getFrameId("ee")
         }
 
@@ -106,9 +107,10 @@ class KinematicsSolver():
         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')
 
@@ -138,6 +140,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)}")
@@ -167,15 +171,15 @@ class KinematicsSolver():
 
         return {
             'position': position,
-            'rotation': rotation,
+            # 'rotation': rotation,
             'rpy': rpy,
             'quaternion': [quat.x, quat.y, quat.z, quat.w],
-            'transform': frame_transform
+            # '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=3e-3, debug=False, tool="ee"):
         """
         Compute inverse kinematics using differential IK with multiple strategies.
 
@@ -230,7 +234,7 @@ class KinematicsSolver():
                                    self.model.upperPositionLimit[i])
 
             # Differential IK with adaptive damping
-            damping = 0.01
+            damping = 0.1
             damping_reduction = 0.95
             iter_count = 0
             prev_error = float('inf')
@@ -242,9 +246,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("initial error =", np.linalg.norm(error_vec))
+            print(error_vec)
+
             while iter_count < max_iter:
                 # Compute forward kinematics
 
@@ -281,7 +296,7 @@ class KinematicsSolver():
                     self.model,
                     self.data,
                     ee_frame_id,
-                    pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+                    pin.ReferenceFrame.LOCAL
                 )
 
                 # =========================
@@ -293,7 +308,7 @@ class KinematicsSolver():
 
                 H_triu = sparse.triu(H).tocsc()
 
-                g = -J.T @ self.W @ error_vec
+                g = - J.T @ self.W @ error_vec
 
                 # -------------------------
                 # Joint velocity constraints
@@ -319,20 +334,32 @@ 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
                 )
 
+
+
+                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
 
+                print(f'pred = {J @ dq} and error_vec = {error_vec}')
 
                 if dq is None:
                     break
@@ -344,181 +371,199 @@ 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:
-            return best_solution, True, best_error
+            print(
-        else:
+                "converged",
-            return None, False, None
+                error_norm,
-
+                position_error
-    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 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)"""
         q = pin.neutral(self.model)
@@ -557,7 +602,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,80 @@
+
+from Robotic_Arm.rm_robot_interface import *
+import numpy as np
+
+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.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_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_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, q, 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]
+        '''
+        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, flag=flag)
+
+    def inverse_kinematics(self, target_position, target_rpy=None, initial_guess=None, tool="ee", work="work"):
+        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 = 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