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

73
kine_ctrl/rm75_kine_qp.py
View File

@ -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"):