AMC V1.13 with improved wrist Mk2 direct kinematics solver

[ale-git]

Build here: robotology/icub-firmware-build#139

I have improved the gradient descent in the direct kinematics solver (motor angles -> yaw, pitch, roll), so that now it never reaches singularity even if brutalised with a cycle of +/- 75 degrees in pitch and roll targets, at high speed, with large overshootings.

Important

The inverse kinematics algebraic solver prevents generating motor targets mechanically unreachable.

Now the solver tries to solve for the new ypr configuration starting from the configuration of the previous control cycle, but if it leads to invalid solution it performs backtracking and reduces the step of the gradient descent. If it still fails, the initial position is resetted to (0,0,0). In this way, the average number of required steps is 4.

It is likely that all the parking procedures (startup and emergency) will result unneccessary after some time of use of the robot, and we'll be able to remove them.

[ale-git]

The digital twin is stressed with high out-of-range angle values and overshootings, and the solver never hits singularities:

spherical_wrist2.mp4

This is the real wrist during some tests - it never hitted safety parking conditions:

WhatsApp.Video.2024-02-14.at.18.21.19_46baa4b6.mp4

[pattacini]

Nicely done!

The digital twin is stressed with high out-of-range angle values and overshootings, and the solver never hits singularities:

Related PR:

• Improved direct kinematics in wrist mk2.1 model icub-firmware-models#82 (comment)

[marcoaccame]

ok @ale-git, thanks