From fb64f3c73a1f5899bfc2b858527cb8c04e4d1ea7 Mon Sep 17 00:00:00 2001 From: LiuzhengSJ Date: Wed, 3 Jun 2026 20:15:48 +0100 Subject: [PATCH] refine qp based controller --- kine_ctrl/main.py | 181 +++++------------ kine_ctrl/rm75_kine_qp.py | 415 +++++++++++++++++++++----------------- kine_ctrl/rm75_kine_rm.py | 80 ++++++++ 3 files changed, 363 insertions(+), 313 deletions(-) create mode 100644 kine_ctrl/rm75_kine_rm.py diff --git a/kine_ctrl/main.py b/kine_ctrl/main.py index 182ddf2..554700b 100644 --- a/kine_ctrl/main.py +++ b/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") - robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0]) - robot_mjk.wait_until_reached() - robot_mjk.print_state() - time.sleep(0.5) + # print("\n[Test 2] Move joint 2 to -30 degrees") + # robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0]) + # robot_mjk.wait_until_reached() + # robot_mjk.print_state() + # time.sleep(0.5) + # + # print("\n[Test 3] Move multiple joints simultaneously") + # robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0]) + # robot_mjk.wait_until_reached() + # robot_mjk.print_state() + # time.sleep(0.5) - print("\n[Test 3] Move multiple joints simultaneously") - robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0]) - robot_mjk.wait_until_reached() - robot_mjk.print_state() - time.sleep(0.5) - - print("\n[Test 4] Return home") + print("\n[Test 4] Return home\n") 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 - target_pos = [0.3, 0.2, 0.4] # More reachable position - target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation + robot_kine_qp = kine_qp() + print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}') - print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m") - - init_joints = [0.2] * 7 - time0 = time.time() - for ii in range(100): - joint_solution, success, error = robot_kine.inverse_kinematics( - target_pos, target_rpy=target_rpy, initial_guess=init_joints, + joint_solution, success, error = robot_kine_qp.inverse_kinematics( + target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad, 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") - for i, angle in enumerate(joint_solution): - print(f" Joint {i + 1}: {degrees(angle):7.2f}°") + print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n') - # 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 - force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version - # Initialize the robotic arm model and sensor type in the algorithm - robot_kine_rm = Algo(arm_model, force_type) - frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0]) - robot_kine_rm.rm_algo_set_workframe(frame) - print(robot_kine_rm.rm_algo_get_curr_workframe()) - frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0]) - robot_kine_rm.rm_algo_set_toolframe(frame) - print(robot_kine_rm.rm_algo_get_curr_toolframe()) - joint_max_limit = np.array([ - 3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159 - ])*57 - robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist()) - joint_min_limit = np.array([ - -3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159 - ]) * 57 - robot_kine_rm.rm_algo_set_joint_min_limit(joint_max_limit.tolist()) + robot_kine_rm = kine_rm() + print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}') + 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) - q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] - ret, phi = robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) - params = rm_inverse_kinematics_params_t([0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0], - [0.3, 0.0, 0.3, 3.14, 0.0, 3.14], 1) - ret, q_out = robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) - print(f"rm_algo_inverse_kinematics_rm75_for_arm_angle ret: {ret} q_out: {q_out}") + print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}') + if ret == 0: + print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ') - 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() diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index dd5f5fe..e1e921e 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/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"), - "camera": self.model.getFrameId("camera_frame"), + "scissor": self.model.getFrameId("scissor"), + "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 - 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 + print( + "converged", + error_norm, + position_error ) - - 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]) diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py new file mode 100644 index 0000000..47092ae --- /dev/null +++ b/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 \ No newline at end of file