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]

Is there something less than warning? It often happens that you don't care too much about the length of the arrow before displaying it. Having a warning continuously printed might be annoying

[traversaro]

I agree, if you display data from a sensor (i.e. force sensor) it makes total sense to have a near 0 value, there is any other reason to have this print/check?

[GiulioRomualdi]

Done 4bdfa9c

[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:

idyntree/src/core/include/iDynTree/Core/Rotation.h

Line 313 in 4de2bf4

static Rotation RotAxis(const Direction & direction, const double angle);

I did the same for the visualization of vectors in the iDynTree visualizer:

idyntree/src/visualization/src/VectorsVisualization.cpp

Lines 40 to 45 in 4de2bf4

	iDynTree::toEigen(rotationAxis) = iDynTree::toEigen(yDirection).cross(iDynTree::toEigen(vectorDirection));
	rotationAxis.Normalize();
	double rotationAngle = std::acos(iDynTree::toEigen(vectorDirection).dot(iDynTree::toEigen(yDirection)));
	iDynTree::Rotation arrowRotationMatrix = iDynTree::Rotation::RotAxis(rotationAxis, rotationAngle);

[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