diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py index ce5e9d0..b1b7796 100644 --- a/kine_ctrl/rm75_kine_qp.py +++ b/kine_ctrl/rm75_kine_qp.py @@ -218,10 +218,7 @@ class KinematicsSolver(): else: # Try different initial configurations 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_error = float('inf') @@ -247,7 +244,7 @@ class KinematicsSolver(): self.data, q, ee_frame_id, - pin.ReferenceFrame.LOCAL_WORLD_ALIGNED + pin.ReferenceFrame.LOCAL ) pin.forwardKinematics(self.model, self.data, q) @@ -268,7 +265,6 @@ class KinematicsSolver(): pin.framesForwardKinematics(self.model, self.data, q) # Get current end-effector placement - current_placement = self.data.oMf[ee_frame_id] # Compute error @@ -293,7 +289,7 @@ class KinematicsSolver(): self.model, self.data, ee_frame_id, - pin.ReferenceFrame.LOCAL_WORLD_ALIGNED + pin.ReferenceFrame.LOCAL ) # ========================= @@ -301,7 +297,7 @@ class KinematicsSolver(): # ========================= 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 @@ -376,7 +372,7 @@ class KinematicsSolver(): break # Apply joint limits with scaling - alpha = 0.2 + alpha = 1.0 q = pin.integrate(self.model, q, alpha * dq) prev_error = error_norm