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

compare the qp ik and rm ik

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-04 17:11:02 +01:00
parent f10152fc29
commit cee1a191ea
3 changed files with 102 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
kine_ctrl/rm75_kine_qp.py
View File

@ -127,7 +127,7 @@ class KinematicsSolver():
polish=False
)
self.W = np.diag([1, 1, 1, 0.2, 0.2, 0.2])
self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
def forward_kinematics(self, joint_angles, tool="ee"):
@ -179,7 +179,7 @@ class KinematicsSolver():
def inverse_kinematics(self, target_position, target_rpy=None,
target_quat=None, initial_guess=None,
max_iter=500, tolerance=3e-3, debug=False, tool="ee"):
max_iter=500, tolerance=5e-3, debug=False, tool="ee"):
"""
Compute inverse kinematics using differential IK with multiple strategies.
@ -255,8 +255,8 @@ class KinematicsSolver():
error_SE3 = current_placement.actInv(target_placement)
error_vec = pin.log(error_SE3).vector
print("\n initial error =", np.linalg.norm(error_vec))
print(error_vec)
# print("\n initial error =", np.linalg.norm(error_vec))
# print(error_vec)
while iter_count < max_iter:
# Compute forward kinematics
@ -342,40 +342,13 @@ class KinematicsSolver():
u=ub
)
print("iter", iter_count)
print("error", error_norm)
print("cond(H)", np.linalg.cond(H))
u, s, vh = np.linalg.svd(J_eff)
print("sv =", s)
print("trans =", error_vec[:3])
print("rot =", error_vec[3:])
# Solve
result = self.osqp_solver.solve()
print("OSQP status =", result.info.status)
print("dq =", result.x)
if result.x is not None:
print("dq norm:", np.linalg.norm(result.x))
if result.info.status != 'solved':
break
dq = result.x
pred_err = np.linalg.norm(error_vec)
pred_next = np.linalg.norm(error_vec - J_eff @ dq)
print("predicted error:", pred_next)
print(f'pred = {J_eff @ dq} and error_vec = {error_vec}')
if dq is None:
break
@ -386,26 +359,10 @@ class KinematicsSolver():
prev_error = error_norm
iter_count += 1
print("target:", target_position, target_rpy)
print("initial guess:", np.degrees(initial_guess))
fk0 = self.forward_kinematics(initial_guess)
print("fk guess:", fk0)
print(result.info.status)
print(np.degrees(q))
print(np.degrees(self.model.upperPositionLimit[:7]))
print(np.degrees(self.model.lowerPositionLimit[:7]))
if best_solution is not None:
print(
"converged",
error_norm,
)
return best_solution, True, best_error
return best_solution, True, best_error, iter_count
else:
return None, False, None
return q[:7].copy(), False, error_norm, iter_count
# def invese_kinematics_velocity(self, target_position, target_rpy=None,
# target_quat=None, initial_guess=None, tool="ee"):