Add secondary calibration for Xsens

[lrapetti]

The objective is to add a secondary calibration to perform on top of xsens calibration in case the suit got miscalibrated. The idea is to have a known orientation and compute the calibration transform from the measured orientation.

In particular, the known position will be considered the position such that the joints are set to zero. In case of calibrating a single link, the known position will be measured by setting to zero the joints from the parent to the given link (e.g. when calibrating the head, all the neck joint are considered zero). When setting the whole body instead, the base position will be considered the one given by xsens, while all the joints are set to be zero.

[lrapetti]

The implementation can be found at robotology/human-dynamics-estimation/add-secondary-calibration and has been tested for the single links (not tested for whole-body).

In order to trigger the calibration, you can access the HumanStateProvider rpc port and the following command are available:

• help: give information regarding the available commands
• calibrate : perform the whole-body calibration. Posture of the subject should be npose (zero configuration of the robot).
• calibrate linkName: perform the calibration for the given link. Posture of the subject should be such that the joints between the given link and the parent link are zero in the robot configuration. N.B. the calibration can be performed only for those link on which we are mapping sensor measurements, i.e. the fake link that can be found in the configuration.
• reset: remove all the secondary calibration matrices.
• reset linkName: remove the secondary calibration for the given link.

[lrapetti]

cc @Yeshasvitvs @kouroshD @S-Dafarra

[S-Dafarra]

The result is pretty cool and already avoided us to recalibrate the suit a couple of times. I wonder whether it is possible to add a command to calibrate a whole subtree, e.g. when calibrating the torso, also the arms and the head are modified accordingly.
Indeed, we noticed that by recalibrating the torso orientation, the arms were getting skewed.

[lrapetti]

I wonder whether it is possible to add a command to calibrate a whole subtree, e.g. when calibrating the torso, also the arms and the head are modified accordingly.

implemented in 7719914

It is now possible to calibrate a chain, assuming having the configuration where all the joints in the chain are zero, with the command:
calibrate firstLinkName childLinkName
(e.g. calibrate chest_fake r_hand_fake)

Note that this functionality has not been extensively tested and the code is not really clean at this stage.

[S-Dafarra]

That's interesting! How should we use it to calibrate the chest and both arms contemporaneously?

[lrapetti]

That's interesting! How should we use it to calibrate the chest and both arms contemporaneously?

I would say that, given that we may be interested in calibrating also the head at the same time, you can try with the whole body calibration

yarp rpc /HumanStateProvider/rpc:i
> calibrate

[claudia-lat]

Please @lrapetti open the relate PR.

[lrapetti]

Please @lrapetti open the relate PR.

PR opened at #164, but still requires cleanup

[traversaro]

the links are set to zero.

Link or joints?

[lrapetti]

the links are set to zero.

Link or joints?

Joints, thanks I have fixed it.

[lrapetti]

merged #164
Closing.