diff --git a/kine_ctrl/test_pin.py b/kine_ctrl/test_pin.py
new file mode 100644
index 0000000..dd5f5fe
--- /dev/null
+++ b/kine_ctrl/test_pin.py
@@ -0,0 +1,803 @@
+#!/usr/bin/env python3
+import sys
+import os
+
+import pinocchio as pin
+import numpy as np
+import osqp
+from scipy import sparse
+from math import radians, degrees, pi, cos, sin
+import time
+
+
+
+class KinematicsSolver():
+    def __init__(self,urdf_path="/home/zl/Downloads/urdf_rm75/RM75-B.urdf", mesh_dir="/home/zl/Downloads/meshes"):
+        """
+        for realman 75b
+        Initialize robotic arm kinematics using Pinocchio (ROS2 version).
+        """
+
+        self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
+
+        # -------------------------------------------------
+        # ee
+        # -------------------------------------------------
+        ee_offset = pin.SE3(np.eye(3), np.array([0, 0, 0.0]))
+        self.model.addFrame(
+            pin.Frame(
+                "ee",
+                self.model.getJointId("joint_7"),
+                self.model.getFrameId("link_7"),
+                ee_offset,
+                pin.FrameType.OP_FRAME
+            )
+        )
+
+
+        # -------------------------------------------------
+        # Scissor tool
+        # -------------------------------------------------
+        scissor_offset = pin.SE3(
+            np.eye(3),
+            np.array([0.0, 0.0, 0.144])
+        )
+        self.model.addFrame(
+            pin.Frame(
+                "scissor_tcp",
+                self.model.getJointId("joint_7"),
+                self.model.getFrameId("link_7"),
+                scissor_offset,
+                pin.FrameType.OP_FRAME
+            )
+        )
+
+        # -------------------------------------------------
+        # Camera tool
+        # -------------------------------------------------
+        camera_rotation = pin.rpy.rpyToMatrix(
+            radians(-90),
+            0,
+            radians(-90)
+        )
+        camera_offset = pin.SE3(
+            camera_rotation,
+            np.array([0.05, 0.02, 0.10])
+        )
+        self.model.addFrame(
+            pin.Frame(
+                "camera_frame",
+                self.model.getJointId("joint_7"),
+                self.model.getFrameId("link_7"),
+                camera_offset,
+                pin.FrameType.OP_FRAME
+            )
+        )
+
+        # -------------------------------------------------
+        # Store tool frame IDs
+        # -------------------------------------------------
+
+        self.tool_frames = {
+            "scissor": self.model.getFrameId("scissor_tcp"),
+            "camera": self.model.getFrameId("camera_frame"),
+            "ee":   self.model.getFrameId("ee")
+        }
+
+
+
+
+        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]
+
+        # ----------  for reused qp_solver  ------------------
+        self.nv = 7
+
+        # Full dense symmetric matrix structure
+        P_template = np.triu(np.ones((7, 7)))
+
+        P_sparse = sparse.csc_matrix(P_template)
+
+        A_sparse = sparse.eye(7, format='csc')
+
+        self.osqp_solver = osqp.OSQP()
+
+        self.osqp_solver.setup(
+            P=P_sparse,
+            q=np.zeros(7),
+            A=A_sparse,
+            l=-np.ones(7),
+            u=np.ones(7),
+            verbose=False,
+            warm_start=True,
+            polish=False
+        )
+
+        self.W = np.diag([1, 1, 1, 0.2, 0.2, 0.2])
+
+
+    def forward_kinematics(self, joint_angles, tool="ee"):
+        """
+        Compute forward kinematics.
+
+        Args:
+            joint_angles: List or array of 7 joint angles (radians)
+            tool: Name of frame to compute
+
+        Returns:
+            dict: Position, rotation, rpy, quaternion
+        """
+        if len(joint_angles) != 7:
+            raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
+
+        # Create configuration vector
+        q = pin.neutral(self.model)
+        for i, angle in enumerate(joint_angles):
+            q[i] = angle
+
+        # Compute forward kinematics
+        pin.forwardKinematics(self.model, self.data, q)
+        pin.updateFramePlacements(self.model, self.data)
+
+        # Get frame transform
+        frame_id = self.tool_frames[tool]
+        frame_transform = self.data.oMf[frame_id]
+
+        # Extract results
+        position = frame_transform.translation.copy()
+        rotation = frame_transform.rotation.copy()
+
+        # Compute RPY
+        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
+        }
+
+    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"):
+        """
+        Compute inverse kinematics using differential IK with multiple strategies.
+
+        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)
+            max_iter: Maximum iterations
+            tolerance: Error tolerance
+            debug: Print debug information
+            tool: the frame name ('scissor', 'camera', 'ee')
+
+        Returns:
+            tuple: (joint_angles, success, error)
+        """
+        # 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)
+        for i, angle in enumerate(joint_angles):
+            q[i] = angle
+
+        pin.forwardKinematics(self.model, self.data, q)
+        pin.updateFramePlacements(self.model, self.data)
+        ee_frame_id = self.tool_frames[tool]
+        J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id)
+
+        return J
+
+    def get_subchain_jacobian(self,
+                              joint_angles,
+                              frame_names
+                              ):
+
+        q = pin.neutral(self.model)
+
+        all_active_joints = self.get_active_joints_from_frame(frame_names)
+
+        for i in range(7):
+            q[i] = joint_angles[i]
+
+        pin.forwardKinematics(self.model, self.data, q)
+        pin.updateFramePlacements(self.model, self.data)
+        pin.computeJointJacobians(self.model, self.data, q)
+
+        Js = []
+
+        for  frame_name, active_joints in zip(frame_names, all_active_joints):
+            frame_id = self.model.getFrameId(frame_name)
+
+            J = pin.getFrameJacobian(
+                self.model,
+                self.data,
+                frame_id,
+                pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
+            )
+            Js.append(J[:, active_joints])
+
+        return Js
+
+    def get_active_joints_from_frame(self, frame_names):
+        """
+        Return active joint indices affecting a frame.
+
+        Example:
+            frame_name='link_4'
+            -> [0,1,2,3]
+        """
+        all_active_joint_ids = []
+        for frame_name in frame_names:
+            frame_id = self.model.getFrameId(frame_name)
+
+            # Parent joint of this frame
+            joint_id = self.model.frames[frame_id].parentJoint
+
+            print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
+
+            active_joint_ids = []
+
+
+            # Traverse upward to root
+            while joint_id > 0:
+                # Pinocchio joint indexing:
+                # universe joint = 0
+                # robot joints start from 1
+
+                active_joint_ids.append(joint_id - 1)
+
+                # Move to parent joint
+                joint_id = self.model.parents[joint_id]
+
+            # Reverse so order becomes base -> tip
+            active_joint_ids.reverse()
+            all_active_joint_ids.append(active_joint_ids)
+
+        return all_active_joint_ids
+
+    def plan_cartesian_trajectory(self, start_pos, end_pos,
+                                  start_rpy=None, end_rpy=None,
+                                  num_steps=20, tool='ee'):
+        """
+        Plan a Cartesian trajectory with IK for each waypoint.
+        """
+        # Get current end-effector pose if start_rpy not provided
+        if start_rpy is None:
+            # Try to find a valid starting configuration
+            test_angles = [0.1] * 7
+            fk_test = self.forward_kinematics(test_angles,tool=tool)
+            start_rpy = fk_test['rpy']
+
+        if end_rpy is None:
+            end_rpy = start_rpy
+
+        # First, check if target is reachable
+        print(f"\nChecking if target is reachable...")
+        target_pos = end_pos
+        target_rpy = end_rpy
+
+        test_solution, success, error = self.inverse_kinematics(
+            target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
+        )
+
+        if not success:
+            print(f"Warning: Target may be unreachable or difficult to reach")
+            print(f"Trying with relaxed tolerance...")
+
+        # Initial guess for IK (start with zero configuration)
+        current_angles = [0.1] * 7
+        trajectory = []
+
+        print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
+        print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
+        print("-" * 60)
+
+        for i in range(num_steps + 1):
+            t = i / num_steps
+
+            # Interpolate position
+            pos = [
+                start_pos[0] + t * (end_pos[0] - start_pos[0]),
+                start_pos[1] + t * (end_pos[1] - start_pos[1]),
+                start_pos[2] + t * (end_pos[2] - start_pos[2])
+            ]
+
+            # Interpolate orientation
+            rpy = [
+                start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
+                start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
+                start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
+            ]
+
+            # Compute IK
+            joint_angles, success, error = self.inverse_kinematics(
+                pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
+            )
+
+            if not success:
+                print(f"  Waypoint {i}: IK failed!")
+                break
+
+            # Verify
+            fk_verify = self.forward_kinematics(joint_angles, tool=tool)
+
+            trajectory.append({
+                'step': i,
+                't': t,
+                'position': pos,
+                'rpy': rpy,
+                'joint_angles': joint_angles,
+                'actual_position': fk_verify['position'],
+                'error': error
+            })
+
+            # Update current angles for next iteration
+            current_angles = joint_angles
+
+            if i % 5 == 0 or i == num_steps:
+                print(f"  Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
+                      f"error={error:.6f}m")
+
+        return trajectory
+
+
+
+def main():
+    """Main test function"""
+
+
+    rm75 = KinematicsSolver()
+
+    # Test 1: Forward Kinematics
+    print("\n1. Forward Kinematics Test")
+    print("-" * 40)
+
+    tool_name = "scissor"
+    joint_angles_zero = [0.1] * 7
+    fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
+
+    print(f"Init configuration:")
+    print(f"  Position: ({fk_result['position'][0]:.3f}, "
+          f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
+
+    # 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")
+
+    import time
+    init_joints = [0.2] * 7
+    time0 = time.time()
+    for ii in range(100):
+        joint_solution, success, error = rm75.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 = rm75.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 = rm75.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 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
+
+    trajectory = rm75.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 = rm75.get_subchain_jacobian(
+        joint_angles=joint_angles_zero,
+        frame_names=frame_names
+    )
+    print(f'Js_sub: {Js_sub}')
+
+    return rm75, trajectory
+
+
+if __name__ == "__main__":
+    rm75, trajectory = main()
+
+    print("\n" + "=" * 60)
+    print("All tests completed!")
+    print("=" * 60)
\ No newline at end of file