Fixes: #58. Now IK/reachability calls will reset the joint positions …

[RemiFabre]

…to the current positions unless the goal pose is identical to the previous call.

[RemiFabre]

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