From a875ef6280415a0df0f482a3f7ebcf68a004a7a1 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Thu, 2 Jul 2026 15:45:29 +0100 Subject: [PATCH] necessary file for kinematics --- .../harvest_arm_ctrl/rm75_kine_qp.py | 824 ++++++++++++++++++ .../harvest_arm_ctrl/rm75_kine_rm.py | 133 +++ .../harvest_arm_ctrl/run_kinematics.py | 6 + 3 files changed, 963 insertions(+) create mode 100644 harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py create mode 100644 harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py create mode 100644 harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py new file mode 100644 index 0000000..6bbf00d --- /dev/null +++ b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_qp.py @@ -0,0 +1,824 @@ +#!/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 +import threading + + + +class KinematicsSolver(): + 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) + + + + self.cfg_j_limit() + + q_range = ( + self.model.upperPositionLimit[:7] - + self.model.lowerPositionLimit[:7] + ) + + self.w_q_limit = np.diag(1.0 / (q_range ** 2)) + + self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7]) + + # ---------- 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.4, 0.4, 0.4]) + # Smaller value => joint moves more actively + # Larger value => joint moves less / more lazy + self.joint_motion_weight = np.diag([ + 0.3, 0.3, 0.3, 1.0, + 1.0, 1.0, 1.0 + ]) + + def add_frame(self,frame_name, position, rotationXYZ): + ''' + :param frame_name: str + :param position: [x, y, z] target position (meters) + :param rotationXYZ: [x, y, z] target rotation (rad) + ''' + camera_rotation = pin.rpy.rpyToMatrix( rotationXYZ[0], rotationXYZ[1], rotationXYZ[2] ) + camera_offset = pin.SE3( + camera_rotation, + np.array(position) + ) + self.model.addFrame( pin.Frame( frame_name, self.model.getJointId("joint_7"), self.model.getFrameId("link_7"), camera_offset, pin.FrameType.OP_FRAME ) ) + + def add_tool_frames(self,dict_frames): + self.tool_frames ={} + for tool_name in dict_frames: + tool_attr = dict_frames[tool_name] + position = tool_attr[0][0:3] + rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7]) + self.add_frame(tool_name, position, rotationXYZ) + self.tool_frames.update({tool_name: self.model.getFrameId(tool_name)}) + self.data = self.model.createData() + + + 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="omnipic"): + """ + 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) + pose = np.concatenate([position, rpy], axis=0) + return pose + # return { + # 'position': position, + # # 'rotation': rotation, + # 'rpy': rpy, + # 'quaternion': [quat.x, quat.y, quat.z, quat.w], + # # 'transform': frame_transform + # } + + def inverse_kinematics(self, target_position, target_rpy=None, + target_quat=None, initial_guess=None, + max_iter=500, tolerance=5e-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_ref = 0.0001 + w_limit_mid = 0.00005 + + J_eff = pin.Jlog6(error_SE3) @ J #J # + + H = J_eff.T @ self.W @ J_eff + + + H += damping * damping * self.joint_motion_weight + H += w_ref * np.eye(7) + H += w_limit_mid * self.w_q_limit + + H_triu = sparse.triu(H).tocsc() + + g = -J_eff.T @ self.W @ error_vec + g += w_ref * (q[:7] - q_ref[:7]) + g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid) + + # ------------------------- + # Joint velocity constraints + # ------------------------- + dq_limit = np.array([ 0.08, 0.08, 0.08, 0.08, 0.05, 0.05, 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 + ) + + # 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 = 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, iter_count + return 0, best_solution.tolist() + else: + # return q[:7].copy(), False, error_norm, iter_count + return -1, q[:7].copy().tolist() + + def quaternion_to_euler(self, q): + """ + Convert quaternion to Euler angles (roll, pitch, yaw) + + Args: + qx, qy, qz, qw: quaternion components + + Returns: + tuple: (roll, pitch, yaw) in radians + """ + # Roll (x-axis rotation) + sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2]) + cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + # Pitch (y-axis rotation) + sinp = 2.0 * (q[3] * q[1] - q[2] * q[0]) + if abs(sinp) >= 1: + pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range + else: + pitch = np.arcsin(sinp) + + # Yaw (z-axis rotation) + siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1]) + cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return [roll, pitch, yaw] + + # 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) \ No newline at end of file diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py new file mode 100644 index 0000000..34fa4cb --- /dev/null +++ b/harvest_arm_ctrl/harvest_arm_ctrl/rm75_kine_rm.py @@ -0,0 +1,133 @@ + +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.work_frames = { + 'work': rm_frame_t(frame_name="work", pose=(0.0, 0.0, 0.0, 0.0, 0, 0.0), payload=1, x=0, y=0, z=0), + } + + self.tool_name = "no_tool" + 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 ]) + + max_j = np.array(max_j) + min_j = np.array(min_j) + 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 quaternion_to_euler(self, q): + """ + Convert quaternion to Euler angles (roll, pitch, yaw) + + Args: + qx, qy, qz, qw: quaternion components + + Returns: + tuple: (roll, pitch, yaw) in radians + """ + # Roll (x-axis rotation) + sinr_cosp = 2.0 * (q[3] * q[0] + q[1] * q[2]) + cosr_cosp = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + # Pitch (y-axis rotation) + sinp = 2.0 * (q[3] * q[1] - q[2] * q[0]) + if abs(sinp) >= 1: + pitch = np.copysign(np.pi / 2, sinp) # Use 90 degrees if out of range + else: + pitch = np.arcsin(sinp) + + # Yaw (z-axis rotation) + siny_cosp = 2.0 * (q[3] * q[2] + q[0] * q[1]) + cosy_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return [roll, pitch, yaw] + + def add_tool_frames(self, dict_frames): + self.tool_frames = {} + for tool_name in dict_frames: + tool_attr = dict_frames[tool_name] + position = tool_attr[0][0:3] + rotationXYZ = self.quaternion_to_euler(tool_attr[0][3:7]) + f = rm_frame_t(frame_name=tool_name, pose=(position[0], position[1], position[2], rotationXYZ[0], rotationXYZ[1], rotationXYZ[2]), payload=1, x=0, y=0, z=0) + + self.tool_frames.update({tool_name:f}) + + def forward_kinematics(self, joint_angles, flag = 1 , tool="omnipic", 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="omnipic", 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] \ No newline at end of file diff --git a/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py new file mode 100644 index 0000000..931284f --- /dev/null +++ b/harvest_arm_ctrl/harvest_arm_ctrl/run_kinematics.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from harvest_arm_ctrl.') + + +if __name__ == '__main__': + main()