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

add urdf files.

Browse Source 
aligh the function parameter names of qp and rm methods
This commit is contained in:
LiuzhengSJ
2026-06-05 15:25:35 +01:00
parent aefc7bacd5
commit 2ca5033b46
14 changed files with 562 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
kine_ctrl/rm75_kine_qp.py
View File

@ -12,7 +12,7 @@ import time
class KinematicsSolver():
def __init__(self,urdf_path="/home/zl/Downloads/urdf_rm75/RM75-B.urdf", mesh_dir="/home/zl/Downloads/meshes"):
def __init__(self,urdf_path="urdf_rm75/RM75-B.urdf", mesh_dir="urdf_rm75"):
"""
for realman 75b
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
@ -90,18 +90,7 @@ class KinematicsSolver():
self.data = self.model.createData()
# Joint limits (radians) - expanded for better reachability
self.lower_limits = np.array([
-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -6.14159
])
self.upper_limits = np.array([
3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 6.14159
])
# Set joint limits in the model
for i in range(7):
self.model.lowerPositionLimit[i] = self.lower_limits[i]
self.model.upperPositionLimit[i] = self.upper_limits[i]
self.cfg_j_limit()
# ---------- for reused qp_solver ------------------
self.nv = 7
@ -129,6 +118,19 @@ class KinematicsSolver():
self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
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]
if rad_flag:
for i in range(7):
self.model.lowerPositionLimit[i] = min_j[i]
self.model.upperPositionLimit[i] = max_j[i]
else:
for i in range(7):
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"):
"""
@ -167,15 +169,16 @@ class KinematicsSolver():
rpy = pin.rpy.matrixToRpy(rotation)
# Compute quaternion
quat = pin.Quaternion(rotation)
return {
'position': position,
# 'rotation': rotation,
'rpy': rpy,
'quaternion': [quat.x, quat.y, quat.z, quat.w],
# 'transform': frame_transform
}
# quat = pin.Quaternion(rotation)
pose = np.concatenate([position, rpy], axis=0)
return pose
# return {
# 'position': position,
# # 'rotation': rotation,
# 'rpy': rpy,
# 'quaternion': [quat.x, quat.y, quat.z, quat.w],
# # 'transform': frame_transform
# }
def inverse_kinematics(self, target_position, target_rpy=None,
target_quat=None, initial_guess=None,
@ -360,9 +363,11 @@ class KinematicsSolver():
iter_count += 1
if best_solution is not None:
return best_solution, True, best_error, iter_count
# return best_solution, True, best_error, iter_count
return 0, best_solution
else:
return q[:7].copy(), False, error_norm, iter_count
# return q[:7].copy(), False, error_norm, iter_count
return -1, q[:7].copy()
# def invese_kinematics_velocity(self, target_position, target_rpy=None,
# target_quat=None, initial_guess=None, tool="ee"):