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