''' Robotic arm kinematics solver for realman-75 Files: rm75_kinematics.py How to use: Example in test1.py: python test1.py ''' import pinocchio as pin from Robotic_Arm.rm_robot_interface import * import numpy as np import osqp from scipy import sparse import math class rm75_kinematics(): def __init__(self,urdf_path="", mesh_dir="", tcps=None,tools_in_ee=None,min_j=0.0, max_j=0.0): self.robot_kine_qp = rm75_kine_qp(urdf_path=urdf_path, mesh_dir=mesh_dir) self.robot_kine_qp.add_tool_frames(tools_in_ee) self.robot_kine_qp.cfg_j_limit(min_j=min_j, max_j=max_j, rad_flag=True) # ---------- rm75 official algorithm ----------- self.robot_kine_rm = rm75_kine_api() self.robot_kine_rm.add_tool_frames(tools_in_ee) self.robot_kine_rm.cfg_j_limit(min_j=min_j, max_j=max_j, rad_flag=True) self.ik_sts = False self.joint_solved = [0.0] * 7 def get_ik_result(self, target_position, target_rpy, initial_guess, tool): ''' Try both the RM official solver and the QP solver; on success, store the result. return: True if either solver succeeded, joint_solved in rad. ''' ret_rm, q_out = self.robot_kine_rm.inverse_kinematics(target_position, target_rpy, initial_guess, tool) if ret_rm != 0: ret_rm, q_out = self.robot_kine_qp.inverse_kinematics(target_position=target_position, target_rpy=target_rpy, initial_guess=initial_guess, tool=tool, max_iter=300) if ret_rm == 0: self.joint_solved = q_out self.ik_sts = True else: self.ik_sts = False return self.ik_sts, self.joint_solved def get_fk_result(self, joint_angles, tool): ''' Get the forward kinematics result for given joint angles and tool. :param joint_angles: list of joint values, in rad :param return: [x,y,z,rx,ry,rz], m & rad return: [x, y, z, rx, ry, rz] in meters and radians. ''' fk_result = self.robot_kine_rm.forward_kinematics(joint_angles, flag=1, tool=tool) return fk_result def get_self_collision(self, joint_angles): ''' Check for self-collision given joint angles. :param joint_angles: list of joint values, in rad :return: True if self-collision is detected, False otherwise. ''' collision_detected = self.robot_kine_qp.collision_detect(joint_angles) return collision_detected class rm75_kine_qp(): def __init__(self, urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75", tcps=None): """ for realman 75b Initialize robotic arm kinematics using Pinocchio (ROS2 version). unit: m, rad """ print(f' ------------ the qp based kinematic initialising -----------') self.model = pin.buildModelFromUrdf(urdf_path) self.geom_model = pin.buildGeomFromUrdf(self.model, urdf_path, pin.GeometryType.COLLISION, mesh_dir) self.geom_model.addAllCollisionPairs() self.remove_adjacent_collision_pairs(verbose=True) self.geom_data = pin.GeometryData(self.geom_model) self.data = self.model.createData() self.cfg_j_limit() self.nv = 7 q_range = ( self.model.upperPositionLimit[:self.nv] - self.model.lowerPositionLimit[:self.nv] ) self.w_q_limit = np.diag(1.0 / (q_range ** 2)) self.q_mid = 0.5 * (self.model.lowerPositionLimit[:self.nv] + self.model.upperPositionLimit[:self.nv]) # --------------------------------------------------------- # Primary IK solver # # Optimization variable: # dq in R^7 # # Constraints: # lb <= dq <= ub # # Therefore: # A = I, shape = 7 x 7 # Full dense symmetric matrix structure # P_template = np.triu(np.ones((7, 7))) self.ik_P_pattern = sparse.triu( np.ones((self.nv, self.nv)) ).tocsc() self.osqp_solver = osqp.OSQP() self.osqp_solver.setup( P=self.ik_P_pattern, q=np.zeros(self.nv), A=sparse.eye(self.nv, format='csc'), l=-np.ones(self.nv), u=np.ones(self.nv), verbose=False, warm_start=True, polish=False ) # End-effector task weight: self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4]) # Smaller value => joint moves more actively # Larger value => joint moves more lazy self.joint_motion_weight = np.diag([ 1.0, 1.0, 1.0, 1.0, 0.3, 0.3, 0.2 ]) # --------------------------------------------------------- # Refinement solver # # Constraints: # # J_eff * dq = error_vec 6 constraints # dq_lower <= dq <= dq_upper 7 constraints # # A_refine = [J_eff] # [ I ] # # Shape: # 13 x 7 # # The upper 6 x 7 block must have a dense sparsity # pattern because every Jacobian entry can change. # --------------------------------------------------------- # The refinement Hessian used below is diagonal: # # H = w_last * W_last # + w_mid * W_mid # + damping * I # # Therefore a diagonal P pattern is sufficient. self.refine_P_pattern = sparse.eye( self.nv, format="csc", ) # Dense structural pattern for the 6x7 Jacobian block. refine_J_pattern = sparse.csc_matrix( np.ones((6, self.nv)) ) # Identity pattern for joint-step bounds. refine_I_pattern = sparse.eye( self.nv, format="csc", ) self.refine_A_pattern = sparse.vstack( [ refine_J_pattern, refine_I_pattern, ], format="csc", ) self.refine_osqp_solver = osqp.OSQP() self.refine_osqp_solver.setup( P=self.refine_P_pattern, q=np.zeros(self.nv), A=self.refine_A_pattern, l=-np.ones(6 + self.nv), u=np.ones(6 + self.nv), verbose=False, warm_start=True, polish=False, ) if tcps is None: self.tcps = [] else: self.tcps = tcps self.tcp_ids = [] try: for tcp in self.tcps: tcp_id = self.model.getFrameId(tcp) self.tcp_ids.append(tcp_id) except: print(f'tcp_id of {tcp} not found') 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] k = 1.0 if rad_flag is True else 1.0 / 180 * math.pi for i in range(7): self.model.lowerPositionLimit[i] = min_j[i] * k self.model.upperPositionLimit[i] = max_j[i] * k 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 """ 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 if tool in self.tcps: frame_id = self.tcp_ids[self.tcps.index(tool)] else: try: frame_id = self.tool_frames[tool] except: print(f'{tool} definition not found') 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 pose = np.concatenate([position, rpy], axis=0) return pose 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: sts, q_solved """ # 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] if tool in self.tcps: ee_frame_id = self.tcp_ids[self.tcps.index(tool)] else: try: ee_frame_id = self.tool_frames[tool] except: print(f'{tool} definition not found') 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 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.00002 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.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ]) # 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: collision = self.collision_detect(q=best_solution, stop_at_first_collision=True) if collision is False: # return best_solution, True, best_error, iter_count return 0, best_solution.tolist() else: return -2, q[:7].copy().tolist() else: # return q[:7].copy(), False, error_norm, iter_count return -1, q[:7].copy().tolist() def refine_ik_solution( self, q_valid, q_last, tool="ee", max_iter=30, pose_tolerance=5e-4, joint_tolerance=1e-5, step_limit=None, w_last=1.0, w_mid=0.0001, damping=1e-5, debug=False, ): """ Refine an already-valid IK solution. The initial configuration q_valid already reaches the desired end-effector pose. The function searches for another configuration that: 1. maintains the end-effector pose generated by q_valid; 2. is closer to q_last; 3. optionally remains away from joint limits. Optimization at each iteration: minimize over dq: 0.5 * w_last * ||q + dq - q_last||^2_{W_last} + 0.5 * w_mid * ||q + dq - q_mid||^2_{W_mid} + 0.5 * damping * ||dq||^2 subject to: J_eff(q) dq = error_vec(q) dq_lower <= dq <= dq_upper The pose error is defined relative to the pose generated by q_valid. Returns: status, q_refined Status: 0: refinement completed successfully -1: invalid input or tool -2: refinement QP failed -3: final pose error exceeds tolerance -4: refined configuration is in collision """ # --------------------------------------------------------- # Validate input # --------------------------------------------------------- q = np.asarray(q_valid, dtype=np.float64).reshape(-1) q_last = np.asarray(q_last, dtype=np.float64).reshape(-1) if q.size != self.nv: if debug: print(f"q_valid must contain {self.nv} values, " f"got {q.size}" ) return -1, q.tolist() if q_last.size != self.nv: if debug: print( f"q_last must contain {self.nv} values, " f"got {q_last.size}" ) return -1, q.tolist() q_lower = self.model.lowerPositionLimit[:self.nv] q_upper = self.model.upperPositionLimit[:self.nv] q = np.clip(q, q_lower, q_upper) q_last = np.clip(q_last, q_lower, q_upper) # --------------------------------------------------------- # Resolve end-effector frame # --------------------------------------------------------- if tool in self.tcps: tcp_index = self.tcps.index(tool) ee_frame_id = self.tcp_ids[tcp_index] else: try: ee_frame_id = self.tool_frames[tool] except: print(f'{tool} definition not found') # --------------------------------------------------------- # Set per-joint refinement step limits # --------------------------------------------------------- if step_limit is None: dq_limit = np.array([ 0.02, 0.02, 0.02, 0.02, 0.03, 0.03, 0.04, ]) else: dq_limit = np.asarray( step_limit, dtype=np.float64, ) if dq_limit.ndim == 0: dq_limit = np.full( self.nv, float(dq_limit),) else: dq_limit = dq_limit.reshape(-1) if dq_limit.size != self.nv: if debug: print( f"step_limit must be scalar or contain " f"{self.nv} values") return -1, q.tolist() if np.any(dq_limit <= 0.0): if debug: print("All step limits must be positive") return -1, q.tolist() # --------------------------------------------------------- # Save the end-effector pose produced by q_valid # --------------------------------------------------------- pin.forwardKinematics( self.model, self.data, q, ) pin.updateFramePlacements( self.model, self.data, ) target_placement = self.data.oMf[ee_frame_id].copy() # --------------------------------------------------------- # Objective weight matrices # --------------------------------------------------------- # Temporal continuity: # Larger weight for a joint means that joint is more strongly # encouraged to remain close to q_last. W_last = np.eye(self.nv) # Joint-limit-centering weight. W_mid = self.w_q_limit # Since all objective matrices are diagonal, H is diagonal. H_diag = ( w_last * np.diag(W_last) + w_mid * np.diag(W_mid) + damping * np.ones(self.nv) ) # OSQP uses: # 0.5 dq.T P dq + g.T dq # P values correspond to the diagonal pattern created in __init__. Px = H_diag.copy() # Reset the previous refinement warm start. self.refine_osqp_solver.warm_start( x=np.zeros(self.nv) ) previous_distance = np.linalg.norm(q - q_last) # --------------------------------------------------------- # Sequential refinement loop # --------------------------------------------------------- for iteration in range(max_iter): # Update kinematics and frame Jacobians. pin.computeJointJacobians( self.model, self.data, q, ) pin.framesForwardKinematics( self.model, self.data, q, ) current_placement = self.data.oMf[ee_frame_id] # Pose error relative to the pose saved from q_valid. error_SE3 = current_placement.actInv( target_placement ) error_vec = pin.log(error_SE3).vector pose_error_norm = np.linalg.norm(error_vec) J = pin.getFrameJacobian( self.model, self.data, ee_frame_id, pin.ReferenceFrame.LOCAL, ) J_eff = pin.Jlog6(error_SE3) @ J # ----------------------------------------------------- # Linear objective vector # ----------------------------------------------------- # 0.5*w_last*||q+dq-q_last||^2_Wlast # the linear term is: # w_last*W_last*(q-q_last) # The same expansion applies to q_mid. # ----------------------------------------------------- g = (w_last * W_last @ (q - q_last) + w_mid * W_mid @ (q - self.q_mid) ) # ----------------------------------------------------- # Pose equality constraint # ----------------------------------------------------- # e(q + dq) approximately equals: # e(q) - J_eff dq # Requiring the next error to be zero gives: # J_eff dq = e(q) # ----------------------------------------------------- pose_lower = error_vec.copy() pose_upper = error_vec.copy() # ----------------------------------------------------- # Joint increment and position constraints # ----------------------------------------------------- joint_lower = np.maximum( -dq_limit, q_lower - q, ) joint_upper = np.minimum( dq_limit, q_upper - q, ) lower = np.concatenate([ pose_lower, joint_lower, ]) upper = np.concatenate([ pose_upper, joint_upper, ]) # ----------------------------------------------------- # Update A numerical values # ----------------------------------------------------- # refine_A_pattern is CSC. For every joint/column j, # its stored entries are: # J_eff[0, j] # J_eff[1, j] # ... # J_eff[5, j] # I[j, j] = 1 # This produces 7 values per column and 49 total. # ----------------------------------------------------- Ax = np.concatenate([ np.concatenate([ J_eff[:, joint_index], np.array([1.0]), ]) for joint_index in range(self.nv) ]) self.refine_osqp_solver.update( Px=Px, q=g, Ax=Ax, l=lower, u=upper, ) result = self.refine_osqp_solver.solve() if result.info.status not in ( "solved", "solved inaccurate", ): if debug: print( "Refinement QP failed at iteration " f"{iteration}: {result.info.status}" ) return -2, q.tolist() dq = result.x if dq is None or not np.all(np.isfinite(dq)): if debug: print( f"Invalid refinement result at iteration " f"{iteration}" ) return -2, q.tolist() dq_norm = np.linalg.norm(dq) if debug: distance_to_last = np.linalg.norm(q - q_last) print( f"refine iteration={iteration:02d}, " f"pose_error={pose_error_norm:.8f}, " f"distance_to_last={distance_to_last:.6f}, " f"dq_norm={dq_norm:.8f}" ) # No useful redundant motion remains. if dq_norm < joint_tolerance and pose_error_norm < pose_tolerance: break q_candidate = pin.integrate( self.model, q, dq, ) q_candidate = np.clip( q_candidate, q_lower, q_upper, ) candidate_distance = np.linalg.norm( q_candidate - q_last ) # The pose-correction component can occasionally make the # distance increase slightly. Permit a tiny numerical margin. if candidate_distance> previous_distance + 1e-8 and pose_error_norm < pose_tolerance: if debug: print( "Refinement stopped because the candidate " "does not improve temporal continuity" ) break q = q_candidate previous_distance = candidate_distance # --------------------------------------------------------- # Final pose verification # --------------------------------------------------------- pin.forwardKinematics( self.model, self.data, q, ) pin.updateFramePlacements( self.model, self.data, ) final_placement = self.data.oMf[ee_frame_id] final_error_SE3 = final_placement.actInv( target_placement ) final_error_vec = pin.log( final_error_SE3 ).vector final_pose_error = np.linalg.norm( final_error_vec ) if debug: print( f"Final refinement pose error: " f"{final_pose_error:.8f}" ) print( f"Original distance to last: " f"{np.linalg.norm(np.asarray(q_valid) - q_last):.6f}" ) print( f"Refined distance to last: " f"{np.linalg.norm(q - q_last):.6f}" ) if final_pose_error > pose_tolerance: if debug: print( "Refined solution rejected because its " "pose error is too large" ) return -3, np.asarray(q_valid).tolist() # --------------------------------------------------------- # Final collision verification # --------------------------------------------------------- collision = self.collision_detect( q=q, stop_at_first_collision=True, ) if collision: if debug: print( "Refined solution rejected because it is " "in collision" ) return -4, np.asarray(q_valid).tolist() return 0, q.tolist() def collision_detect(self, q ,stop_at_first_collision=True, verbose=False ): q = np.asarray(q, dtype=np.float64).reshape(-1) if q.shape[0] != self.model.nq: raise ValueError(f"q size mismatch: expected {self.model.nq}, got {q.shape[0]}") # Update robot kinematics pin.forwardKinematics(self.model, self.data, q) pin.updateGeometryPlacements( self.model, self.data, self.geom_model, self.geom_data, q ) # Now compute collisions on the updated geometry model collision = pin.computeCollisions( self.geom_model, self.geom_data, stop_at_first_collision ) if verbose: print(f"the collision is {collision}\n") for k, cr in enumerate(self.geom_data.collisionResults): if cr.isCollision(): cp = self.geom_model.collisionPairs[k] geom1 = self.geom_model.geometryObjects[cp.first] geom2 = self.geom_model.geometryObjects[cp.second] print( f"collision pair {k}: " f"{geom1.name} <--> {geom2.name}" ) return bool(collision) def remove_adjacent_collision_pairs(self, verbose=True): """ Remove collision pairs between same/adjacent parent joints. This avoids false positives such as: base_link_0 <--> link_1_0 """ pairs_to_remove = [] for pair_id, pair in enumerate(self.geom_model.collisionPairs): geom1 = self.geom_model.geometryObjects[pair.first] geom2 = self.geom_model.geometryObjects[pair.second] j1 = geom1.parentJoint j2 = geom2.parentJoint # Same body or directly connected bodies if j1 == j2 or abs(j1 - j2) <= 1: pairs_to_remove.append(pair_id) if verbose: print( "Removing adjacent pair:", pair_id, geom1.name, "<-->", geom2.name, "parentJoint:", j1, j2, ) for pair_id in reversed(pairs_to_remove): self.geom_model.removeCollisionPair( self.geom_model.collisionPairs[pair_id] ) # Important: recreate geometry data after modifying pairs self.geom_data = pin.GeometryData(self.geom_model) if verbose: print("Remaining collision pairs:", len(self.geom_model.collisionPairs)) 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)) # 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 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_75 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=[float(q_s)*180.0/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", step_arm_angle = 15.0): ''' :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 = list(target_position) + list(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, phi0 = self.robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref) params = rm_inverse_kinematics_params_t(q_ref, target, 1) offsets = [0.0] arm_angle = step_arm_angle while arm_angle <= 180.0: offsets += [arm_angle, -arm_angle] arm_angle += step_arm_angle best_ret, best_q_out, best_dis = -1, None, None for offset in offsets: phi = ((phi0 + offset + 180.0) % 360.0) - 180.0 ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) if int(ret) != 0: if best_q_out is None: best_ret, best_q_out = ret, q_out continue p_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1) pose_dis = cal_pose_deviation(p_fk, target) if pose_dis < 0.01: # success in ik calculation return ret, [q / 180 * math.pi for q in q_out] if best_dis is None or pose_dis < best_dis: best_ret, best_q_out, best_dis = -10, q_out, pose_dis ret, q_out = self.robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi) pose_fk = self.robot_kine_rm.rm_algo_forward_kinematics(joint=q_out, flag=1) pose_dis = cal_pose_deviation(pose_fk, target) # print(f'target pose is {target}, fk pose is {pose_fk}, dis of poses is {pose_dis}') # # print(f'\nin the rm75_kine_rm, l133, inverse_kinematics, q_ref = {q_ref}, target = {target} phi = {phi}, q_out = {q_out}, ret = {ret}\n\n') # print(f'the tool frame is {self.robot_kine_rm.rm_algo_get_curr_toolframe()}') if int(ret) < 0: return ret, [ q/180*math.pi for q in q_out] elif pose_dis < 0.01: return ret, [ q/180*math.pi for q in q_out] else: return -10, [ q/180*math.pi for q in q_out] def cal_pose_deviation(pose1, pose2): d_fk_p1 = np.array(pose1) - np.array(pose2) for j in [3, 4, 5]: while d_fk_p1[j] > math.pi: d_fk_p1[j] -= 2 * math.pi while d_fk_p1[j] < -math.pi: d_fk_p1[j] += 2 * math.pi d_fk = np.linalg.norm(d_fk_p1) return d_fk