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

run correctly

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

14
kine_ctrl/rm75_kine_qp.py
View File

@ -218,10 +218,7 @@ class KinematicsSolver():
else: else:
# Try different initial configurations # Try different initial configurations
initial_guesses.append([0.1] * 7) # Zero config initial_guesses.append([0.1] * 7) # Zero config
initial_guesses.append([radians(30), radians(45), radians(30),
radians(-45), radians(30), radians(-30), 0])
initial_guesses.append([radians(-30), radians(45), radians(-30),
radians(45), radians(30), radians(30), 0])
best_solution = None best_solution = None
best_error = float('inf') best_error = float('inf')
@ -247,7 +244,7 @@ class KinematicsSolver():
self.data, self.data,
q, q,
ee_frame_id, ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED pin.ReferenceFrame.LOCAL
) )
pin.forwardKinematics(self.model, self.data, q) pin.forwardKinematics(self.model, self.data, q)
@ -268,7 +265,6 @@ class KinematicsSolver():
pin.framesForwardKinematics(self.model, self.data, q) pin.framesForwardKinematics(self.model, self.data, q)
# Get current end-effector placement # Get current end-effector placement
current_placement = self.data.oMf[ee_frame_id] current_placement = self.data.oMf[ee_frame_id]
# Compute error # Compute error
@ -293,7 +289,7 @@ class KinematicsSolver():
self.model, self.model,
self.data, self.data,
ee_frame_id, ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED pin.ReferenceFrame.LOCAL
) )
# ========================= # =========================
@ -301,7 +297,7 @@ class KinematicsSolver():
# ========================= # =========================
w_posture = 0.0 w_posture = 0.0
J_eff = J # -pin.Jlog6(error_SE3) @ J J_eff = pin.Jlog6(error_SE3) @ J #J #
H = J_eff.T @ self.W @ J_eff H = J_eff.T @ self.W @ J_eff
@ -376,7 +372,7 @@ class KinematicsSolver():
break break
# Apply joint limits with scaling # Apply joint limits with scaling
alpha = 0.2 alpha = 1.0
q = pin.integrate(self.model, q, alpha * dq) q = pin.integrate(self.model, q, alpha * dq)
prev_error = error_norm prev_error = error_norm