Add the possibility to display the arrows in MeshcatVizualizer

[GiulioRomualdi]

This PR allows you to easily draw arrows in MeshcatVizualizer

The original idea was taken from @rob-mau and generalized to avoid using non-standard datatype.

For instance the following code

from idyntree.visualize import MeshcatVisualizer
viz = MeshcatVisualizer()
viz.load_arrow(radius=0.01, shape_name="arrow", color=[255/255, 157/255, 68/255])
viz.set_arrow_transform(origin=[0, 0.5, 0], vector=[0.5, 0.33, 1], shape_name="arrow")

draws the following arrow

[rob-mau]

Hi Giulio, I was reading your implementation of the rotation matrix R. You can avoid to compute the variable s, since:

$$ \frac{1 - \cos \theta}{\sin^2 \theta} = \frac{1 - \cos \theta}{1 - \cos^2 \theta} = \frac{1}{1 + \cos \theta} = \frac{1}{1 + \hat{\boldsymbol{x}}^\top \hat{\boldsymbol{y}}} $$

In this case the rotation matrix can be computed as:

$$ R(\hat{\boldsymbol{x}}, \, \hat{\boldsymbol{y}}) \triangleq I + S(\hat{\boldsymbol{x}} \wedge \hat{\boldsymbol{y}}) + \frac{1}{1 + \hat{\boldsymbol{x}}^\top \hat{\boldsymbol{y}}} \, S(\hat{\boldsymbol{x}} \wedge \hat{\boldsymbol{y}})^2 $$

With this formulation, there is no singularity in $\theta = 0$, so you can avoid the conditional statement: if s < 1e-6: R = np.eye(3) else: ...

You can also avoid to compute the cross product $\hat{\boldsymbol{x}} \wedge \hat{\boldsymbol{y}}$, since:

$$ S(\hat{\boldsymbol{x}} \wedge \hat{\boldsymbol{y}}) = \hat{\boldsymbol{y}} \, \hat{\boldsymbol{x}}^\top - \hat{\boldsymbol{x}} \, \hat{\boldsymbol{y}}^\top $$

but I don't know which computation is better.

[GiulioRomualdi]

Hi @rob-mau thank you for the hint. However, I think we still need an if indeed in case $\hat{\boldsymbol{x}}^\top \hat{\boldsymbol{y}} = -1$ the following is not defined (at least in the computer)

$$ R(\hat{\boldsymbol{x}}, \, \hat{\boldsymbol{y}}) \triangleq I + S(\hat{\boldsymbol{x}} \wedge \hat{\boldsymbol{y}}) + \frac{1}{1 + \hat{\boldsymbol{x}}^\top \hat{\boldsymbol{y}}} \, S(\hat{\boldsymbol{x}} \wedge \hat{\boldsymbol{y}})^2 $$

[S-Dafarra]

Btw, you can also compute the relative rotation by simply computing the corresponding axis and angle that align the two vectors. The first can be computed via the cross product, the second via the dot product. Since we are already loading iDynTree bindings, it may make sense to use directly the builtin function in iDynTree: https://github.com/robotology/idyntree/blob/4de2bf4d9bcd299b69a3f7ee52732331199d993b/src/core/include/iDynTree/Core/Rotation.h#L313

I did the same for the visualization of vectors in the iDynTree visualizer: https://github.com/robotology/idyntree/blob/4de2bf4d9bcd299b69a3f7ee52732331199d993b/src/visualization/src/VectorsVisualization.cpp#L40-L45

[rob-mau]

I think we still need an if indeed in case $\hat{\boldsymbol{x}}^\top \hat{\boldsymbol{y}} = -1$ the following is not defined

Yes, the singularity in $\theta = \pi$ is not remuvable. Also evaluating $R$ as Stefano suggested we have this problem. When the two vectors are aligned, the normal axis is not uniquely defined and the cross product between them is equal to the zero vector. Indeterminate forms (0/0) appear in the Rodrigues' formula.

[traversaro]

@GiulioRomualdi is this ready to be merged, right?

[GiulioRomualdi]

yes