#!/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", 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, self.collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir) 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: pass 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 * 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