diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index 9e11d37..009d241 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -22,32 +22,8 @@ class KinematicsSolver(): print(f' ------------ the qp based kinematic initialising -----------') self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir) - # ------------------------------------------------- - # ee - # ------------------------------------------------- - self.add_frame(frame_name="ee", position=[0.0, 0.0, 0.0], rotationXYZ=[0.0, 0.0, 0.0]) - # ------------------------------------------------- - # Scissor tool - # ------------------------------------------------- - self.add_frame(frame_name="scissor", position=[0.0, 0.0, 0.19], rotationXYZ=[0.0, 0.0, 0.0]) - # ------------------------------------------------- - # Camera tool - # ------------------------------------------------- - self.add_frame(frame_name="camera",position=[0.05, 0.02, 0.10], rotationXYZ=[-pi*0.5, 0.0, -pi*0.5]) - # ------------------------------------------------- - # Store tool frame IDs - # ------------------------------------------------- - self.tool_frames = { - "scissor": self.model.getFrameId("scissor"), - "camera": self.model.getFrameId("camera"), - "ee": self.model.getFrameId("ee") - } - - - self.data = self.model.createData() - self.cfg_j_limit() # ---------- for reused qp_solver ------------------ @@ -89,6 +65,17 @@ class KinematicsSolver(): ) 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] @@ -103,7 +90,7 @@ class KinematicsSolver(): 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="ee"): + def forward_kinematics(self, joint_angles, tool="omnipic"): """ Compute forward kinematics. @@ -172,8 +159,7 @@ class KinematicsSolver(): """ # 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]) + 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], @@ -335,10 +321,39 @@ class KinematicsSolver(): if best_solution is not None: # return best_solution, True, best_error, iter_count - return 0, best_solution + return 0, best_solution.tolist() else: # return q[:7].copy(), False, error_norm, iter_count - return -1, q[:7].copy() + 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"): diff --git a/kine_ctrl/rm75_kine_rm.py b/kine_ctrl/rm75_kine_rm.py index 9fd3790..34fa4cb 100644 --- a/kine_ctrl/rm75_kine_rm.py +++ b/kine_ctrl/rm75_kine_rm.py @@ -14,16 +14,11 @@ class rm75_kine_api(): self.cfg_j_limit() - 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.19, 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), + '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 = "ee" + self.tool_name = "no_tool" self.work_name = "work" def cfg_j_limit(self, min_j=None, max_j=None, rad_flag = True): @@ -32,6 +27,8 @@ class rm75_kine_api(): 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()) @@ -51,7 +48,46 @@ class rm75_kine_api(): def get_tool_frame(self): return self.robot_kine_rm.rm_algo_get_curr_toolframe() - def forward_kinematics(self, joint_angles, flag = 1 , tool="ee", work="work"): + 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] @@ -66,7 +102,7 @@ class rm75_kine_api(): 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="ee", work="work"): + 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