Extend the documentation of wrist math

Nicogene commented 7 months ago

Follow-up of:

https://github.com/robotology/icub-firmware-models/pull/82#pullrequestreview-1882075579

pattacini commented 7 months ago

Hi @ale-git

While extending the documentation, we can also describe the situation when the wrist system gets stuck at startup.

pattacini commented 6 months ago

We should also address the problem of the disambiguation of the startup configuration. See https://github.com/icub-tech-iit/knowledge-base/discussions/17.

cc @ale-git @Nicogene

ale-git commented 6 months ago

There is an important fact to consider: the motors2ypr solver module never fails if the gap between each couple of arms is less than 180 deg. I have checked this fact by spanning all the allowed arm configurations under this condition:

ale-git commented 6 months ago

The same check in the ypr2motors module should prevent us to reach such singularities. We could add some safety threshold in order to prevent unallowed configurations also in case of overshooting.

The novelty introduced in this last versione of the motors2ypr solver works in this way:

the gradient descent step is initialised to 0.01, and the ypr configuration is initialised with that calculated in the previous control cycle
if the algorithm can't converge (singularity) then it performs backtraking to the last good ypr configuration, and divides the step by 2, and continues
if the step runs below 0.001 then we have reinitialization to (0,0,0) know position, and step = 0.01

ale-git commented 6 months ago

I have updated the documentation with the topics described above. https://github.com/icub-tech-iit/documentation/pull/343

icub-tech-iit / documentation

Extend the documentation of wrist math #334