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

wrong, not converged

Browse Source
This commit is contained in:
LiuzhengSJ
2026-06-03 21:14:34 +01:00
parent fb64f3c73a
commit 0bed2f87f4
1 changed files with 24 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
kine_ctrl/rm75_kine_qp.py
View File

@ -232,6 +232,7 @@ class KinematicsSolver():
if i < len(q):
q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
self.model.upperPositionLimit[i])
q_ref = q.copy()
# Differential IK with adaptive damping
damping = 0.1
@ -246,7 +247,7 @@ class KinematicsSolver():
self.data,
q,
ee_frame_id,
pin.ReferenceFrame.LOCAL
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
pin.forwardKinematics(self.model, self.data, q)
@ -276,13 +277,9 @@ class KinematicsSolver():
error_norm = np.linalg.norm(error_vec)
if error_norm < tolerance:
joint_angles = q[:7].copy()
fk_result = self.forward_kinematics(joint_angles,tool=tool)
position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
if position_error < best_error:
best_error = position_error
best_solution = joint_angles
if error_norm < best_error:
best_error = error_norm
best_solution = q[:7].copy()
break
# Check if error is increasing (diverging)
@ -296,19 +293,28 @@ class KinematicsSolver():
self.model,
self.data,
ee_frame_id,
pin.ReferenceFrame.LOCAL
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
# =========================
# QP-based IK
# =========================
w_posture = 0.0
H = J.T @ self.W @ J
J_eff = J # -pin.Jlog6(error_SE3) @ J
H = J_eff.T @ self.W @ J_eff
# H = J.T @ self.W @ J
H += damping * damping * np.eye(7)
H += w_posture * np.eye(7)
H_triu = sparse.triu(H).tocsc()
g = - J.T @ self.W @ error_vec
g = -J_eff.T @ self.W @ error_vec
g += w_posture * (q[:7] - q_ref[:7])
# g = - J.T @ self.W @ error_vec
# -------------------------
# Joint velocity constraints
@ -359,13 +365,18 @@ class KinematicsSolver():
dq = result.x
print(f'pred = {J @ dq} and error_vec = {error_vec}')
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
# Apply joint limits with scaling
alpha = 0.5
alpha = 0.2
q = pin.integrate(self.model, q, alpha * dq)
prev_error = error_norm
@ -388,7 +399,6 @@ class KinematicsSolver():
print(
"converged",
error_norm,
position_error
)
return best_solution, True, best_error
else: