Slowdown when the end effector approaches the target

[Ayman Damoun]

Hi,

Thanks for this inspiring library! I've encountered an issue with the velocity QP formulation when the robot is near the target. Here's the problem I'm facing:

I'm using the following formulation:

$$\begin{array}{ccl} \dot{q}^{opt} = & \arg \min & || v^{*} - J(q)\dot{q} ||^2_2 + \omega_{reg}||\dot{q} - k_{p,reg} (q_{ref} - q) ||^2_2\\\ & s.t & A \dot{q} \leq b \\\ \end{array}$$

However, I've noticed that when the robot is near the target, it slows down significantly. I believe this slowdown occurs because the position error decreases, leading to a situation where we primarily minimize the end effector Cartesian velocity. Even when I increase the gain, the robot still slows down near the target, which can result in spending a lot of time before convergence or even failing to converge. This issue is especially noticeable in physical simulations and hardware tests.

Could you provide some advice or hints on how to overcome this problem?

Thank you!

[Stéphane Caron]

Your interpretation is correct and this is the expected behavior when using this fixed-gain formulation (error becomes small ⇒ velocities become small).

A common practice is to set a high gain, or even a deadbeat gain such as $\alpha = 1$ in Pink, and have the target interpolate nicely from A to B (for instance with capped velocities). A deadbeat gain means the target will be reached exactly at the next step, up to linearization error. Then, traveling from A to B becomes a matter of interpolating the target. The tradeoff is that with such a high gain one needs to ensure the target is never too far away from the measurement, especially when running live on a real robot.

Other options include gain scheduling, or using a custom gain formula depending on the current error. I don't have any particular recommendation to make but you can search for finite-time stability in the control-theory literature.