Closed RemiFabre closed 1 year ago
I initially fixed this in branch 58 of poc_grasping, with this:
if USE_QP_IK:
is_retry_call = np.allclose(
np.array(self.previous_ik_target, dtype=float),
np.array(M, dtype=float),
atol=1e-8,
rtol=1e-5,
)
self.previous_ik_target = M
if not is_retry_call:
# This will set placo's internal model to the current joint positions of the robot
self.ik_reachy_placo._set_joints(self._current_pos)
else:
# If the user is retrying the same pose, we wont update the internal model so that the solver
# has a better starting position
pass
This fixed all comon known "stuck situations" (still not tested enough).
However, a better fix was done directly in reachy_placo_ik. So I won't merge this.
The fix in reachy_placo is in: 32c3c1b9c9c3c4f1b1d8eae45f36334282f40983
…to the current positions unless the goal pose is identical to the previous call.