ZhengLiu-cart/IK_qp
2
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

correct the w_posture

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-04 14:31:29 +01:00
parent c0458f0e12
commit f10152fc29
2 changed files with 21 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
kine_ctrl/rm75_kine_qp.py
View File

@ -255,7 +255,7 @@ class KinematicsSolver():
error_SE3 = current_placement.actInv(target_placement)
error_vec = pin.log(error_SE3).vector
print("initial error =", np.linalg.norm(error_vec))
print("\n initial error =", np.linalg.norm(error_vec))
print(error_vec)
while iter_count < max_iter:
@ -295,7 +295,7 @@ class KinematicsSolver():
# =========================
# QP-based IK
# =========================
w_posture = 0.0
w_posture = 0.0001
J_eff = pin.Jlog6(error_SE3) @ J #J #
@ -348,6 +348,14 @@ class KinematicsSolver():
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)
@ -385,11 +393,10 @@ class KinematicsSolver():
fk0 = self.forward_kinematics(initial_guess)
print("fk guess:", fk0)
print("initial error norm:", error_norm)
print(iter_count,
error_norm,
result.info.status)
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(