Gravity target not working properly

[lrapetti]

With @S-Dafarra we have observed that the gravity target introduced in https://github.com/robotology/human-dynamics-estimation/pull/281, in the add-different-target-types branch is not working properly.

As you can see in the following video, the target gravity vector orientation and the current gravity vector orientation (the two orange arrow in correspondence of the links) seems that certain configuration do not get aligned, but rather they diverge (this visualization has been obtained by modifying the current visualizer HumanStateVisualizer). Those two vectors are exactly the vectors for which we compute the error in https://github.com/robotology/human-dynamics-estimation/blob/e19082754529f2355f437c8e71729df5611a8452/HumanDynamicsEstimationLibrary/algorithms/src/DynamicalInverseKinematics.cpp#L569 corresponding to the following formula

$\omega = -k (z^{target} \times z^{current}) + \omega^{target}$

https://user-images.githubusercontent.com/35487806/155574488-b5a8609a-8737-492b-b394-a9301bb46eb9.mp4

[lrapetti]

As noticed by @S-Dafarra, in the formula above the cross product give us the desired angular velocity in the link frame. Indeed, in order to use it in the implemented Inverse Kinematics (see https://www.mdpi.com/1999-4893/13/10/266) we would need that quantity expressed in the world inertial frame, i.e. $^{I}\omega = -k^IR_{B}(z^{current} \times z^{target}) + \omega^{target}$

[lrapetti]

Fix merged with https://github.com/robotology/human-dynamics-estimation/pull/285.

[DanielePucci]

Nice catch @lrapetti @S-Dafarra