Add the possibility to assign more sensors to one link

[valentino-razza]

For the other project, we need to force the feet to be parallel while in contact with the ground. This leads to the need to assign more of one sensor to each foot: Xsens and IIT shoe data should be merged together for each foot link

[valentino-razza]

From a talk with @lrapetti, the main changes to be performed are

• index the "wearableTargets" map by ModelLinkName instead of TargetName;
• remove "wearableName", "targetType", and "sensorType" fields from WearableSensorTarget class;
• add a vector to the WearableSensorTarget class to include "wearableName", "targetType", and "sensorType" information keeping the definition order from the xml file;
• Adapt the code to the new structure

[valentino-razza]

In this branch, in addition to the previous changes, we introduce two novelties:

• to exploit the data from the IIT shoes;
• to create a new target type.

The shoe data is used to force the link rotation matrix. When the force z-component is above a threshold value, the link rotation matrix is changed to align the link z-axis with the world one. Otherwise, no operation is performed.

The new target type, named updateOnly, allows multiple sensors to be associated to a single link.

To define multiple sensors for a single link, the WEARABLE_SENSOR_TARGETS section of the xml file should have:

• a single entry for the link and one of the sensors, where the target type accounts for the overall data to be set for the link (not necessarily from the single sensor);
• the subsequent entries for the same link must have updateOnly as target type.

[valentino-razza]

From this comment, the link rotation has been performed through the following strategy.

Given the unitary norm vector $e$, we want to set the rotation matrix $R$ such that $R e = e$ (e.g., the z-axis of the link is parallel to the z-axis of the world).

We compute the vector $v = R e$. The angle between $v$ and $e$ is computed as $$ \alpha = \acos( v \cdot e) $$ It is worth noting that, since $e$ has unitary norm and $R$ is a rotation matrix, $v$ has unitary norm. From the cross product $n = e \times v$, we compute the direction of an orthogonal vector to both $e$ and $v$.

By defining a rotation matrix $R{n,-\alpha}$ such that the rotation axis is $n$ and the angle is $-\alpha$, the link rotation matrix is computed as $$R = R{n,-\alpha} R$$

[valentino-razza]

Due to the latest upgrades from the other project, we need to perform motions on inclined surfaces.

The overall library should work in the same way. We need to parameterize the floor orientation when we use the shoe data to enforce the feet link to be parallel to the ground.

We add a parameter to the XML retargeting config file to define the floor orientation. This is uniquely identified through the vector orthogonal to the floor itself. For backward compatibility, the floor orientation parameter is optional and its default value is the z-axis (0,0,1).

The new parameter is floorOrientationVector to be set into the human_state_provider device section. The expected value is a 3 element vector. If the parameter is not defined or if the vector size is wrong, the default value is set.

[valentino-razza]

The code in the branch feetConstraints/addVectorPublish accounts for the aforementioned features.

Closing this issue