#!/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). unit: m, rad """ print(f' ------------ the qp based kinematic initialising -----------') 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", self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), scissor_offset, pin.FrameType.OP_FRAME ) ) # ------------------------------------------------- # Camera tool # ------------------------------------------------- camera_rotation = pin.rpy.rpyToMatrix( radians(-90), 0, radians(-90) ) camera_offset = pin.SE3( camera_rotation, np.array([0.05, 0.02, 0.10]) ) self.model.addFrame( pin.Frame( "camera", self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) ) # ------------------------------------------------- # Store tool frame IDs # ------------------------------------------------- self.tool_frames = { "scissor": self.model.getFrameId("scissor"), "camera": self.model.getFrameId("camera"), "ee": self.model.getFrameId("ee") } self.data = self.model.createData() # 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))) self.P_pattern = sparse.triu(np.ones((7,7))).tocsc() P_sparse = sparse.csc_matrix(self.P_pattern) 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 unit: position: m rpy: rad """ 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=500, tolerance=3e-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 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]) q_ref = q.copy() # Differential IK with adaptive damping damping = 0.1 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 ) 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 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: if error_norm < best_error: best_error = error_norm best_solution = q[:7].copy() 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 ) # ========================= # QP-based IK # ========================= w_posture = 0.0001 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_eff.T @ self.W @ error_vec g += w_posture * (q[:7] - q_ref[:7]) # 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, #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)) u, s, vh = np.linalg.svd(J_eff) print("sv =", s) print("trans =", error_vec[:3]) print("rot =", error_vec[3:]) # Solve result = self.osqp_solver.solve() print("OSQP status =", result.info.status) print("dq =", result.x) if result.x is not None: print("dq norm:", np.linalg.norm(result.x)) if result.info.status != 'solved': break dq = result.x pred_err = np.linalg.norm(error_vec) pred_next = np.linalg.norm(error_vec - J_eff @ dq) print("predicted error:", pred_next) print(f'pred = {J_eff @ dq} and error_vec = {error_vec}') if dq is None: break # Apply joint limits with scaling alpha = 1.0 q = pin.integrate(self.model, q, alpha * dq) 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(result.info.status) print(np.degrees(q)) print(np.degrees(self.model.upperPositionLimit[:7])) print(np.degrees(self.model.lowerPositionLimit[:7])) if best_solution is not None: print( "converged", error_norm, ) 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 ) 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)