ZhongyuWang-cart/IK_qp
1
0
Fork 0
forked from ZhengLiu-cart/IK_qp
Code Pull Requests Activity

start to adjust to ready-to-use class

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-22 13:53:12 +01:00
parent 6c8a335e1d
commit 58452bce90
2 changed files with 89 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
kine_ctrl/rm75_kine_rm.py
View File

@ -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