Improve secondary calibration

[lrapetti]

Some options of the secondary calibration are currently not working for some model (e.g. the chain tree calibration for the iCub model). Those problem should be investigated, and furthermore a general refactoring might be useful.

I'll first try to sum-up what is there, what is working and what not.

[lrapetti]

Up to now there are three calibration strategies:

• whole-body calibration
  command: calibrate
  It assumes the subject is assuming the zero configuration $\tilde{s}=s_0=0$ (i.e. the configuration in which the model joints are all zero), while for the base link is assumed being the measured one. At this point the secondary calibration matrix is computed for each link as follow:
  $$
  R_i^{II}=R_i(p_B(t),R_{B}(t),s(t))^TR_i(p_B(t),R_{B}(t),s_0)
  $$

• link calibration
  command: calibrate <linkName>
  It assumes that the given link $L$ is in zero configuration w.r.t. its child link. So given the set of joints $\mathcal{J}$ between child and parent link,
  $$
  \begin{equation}
  \tilde{s}_j=s_j(t) \ \forall j \notin \mathcal{J} \\
  \tilde{s}_j=0 \ \ otherwise
  \end{equation}
  $$
  so at this point we can define the secondary calibration matrixr the link $L$ as:
  $$
  R_L^{II}=R_L(p_B(t),R_{B}(t),s(t))^TR_L(p_B(t),R_{B}(t),\tilde{s})
  $$

• chain calibration
  command: calibrate <parentLinkName> <childLinkName>
  in this case the calibration is performed for a chain of links $\mathcal{L}$ from the parent link to the child link. It assumes that all the joint $\mathcal{J}$ involved in the chain are in the zero configuration.
  $$
  \begin{equation}
  \tilde{s}_j=s_j(t) \ \forall j \notin \mathcal{J} \\
  \tilde{s}_j=0 \ \ otherwise
  \end{equation}
  $$
  and the secondary calibration is computed for the links in $\mathcal{L}$ as:
  $$
  R_i^{II}=R_i(p_B(t),R_{B}(t),s(t))^TR_i(p_B(t),R_{B}(t),\tilde{s}) \ \forall i \in \mathcal{L}
  $$

[lrapetti]

Coming to what is not working, there are some problems with the current implemented calibration methods. link calibration* and chain calibration depends on subtrees created using getReducedModel function implemented in the HumanStateProvider which is supposed to create a sub-model of the given model considering the extrating the chain between a parent and a child link. The current implementation of this function has some weakness:

• chain calibration: the first problem is that the fake links used on iCub Teleoperation models (Add secondary calibration #164 (comment)). In fact when creating a reduced model with loadReducedModelFromFullModel the fixed joints connecting the fake links are not considered. Indeed the fake links are no longer detected as links but as frames. (this issue was already presented in Add secondary calibration #164 (comment)).
  So, since the problem has always been there, this functionality was never working, @kouroshD @S-Dafarra are you sure you were using this option back in time? Maybe you were just using calibrate head_fake?
• link calibration: the link calibration is done w.r.t. a parent link that is defined using createEndEffectorsPairs method. This methods create link pairs accordingly to the links for which we have sensors measurements and looking at the closest link. The methods for doing this are somehow complex and not straightforward, and at the end the parent-child pairs order might depend on the base definition.
  Moreover, a big drawback of link calibration is that it can not be applied to the base link since it has no parent links.

[lrapetti]

In order to overcome the weakness of the implemented calibration methods I thought about other possible calibration methods that might be implemented:

• Custom offset calibration: this was discussed with @kouroshD, we might simply manually send the offset calibration for given links. (this might be helpful for example for experiments with iCub robot on the pole)
• Relative calibration: in this case we can calibrate a given link w.r.t. another chosen link, assuming the subject is in the zero configuration (at list for the links involved). In fact we can simply get the relative transform between the two links in zero configuration. ${}^{L1}R^0_{L2}$ and then compute the secondary calibration for the chosen link as:
  $$
  R^{II}_{L2}=R_{L_2}^T*R_{L_1}{}^{L1}R^0_{L2}
  $$

I have done a preliminary implementation of this two methods, plus a refactoring of the previously implemented methods in https://github.com/lrapetti/human-dynamics-estimation/tree/improve-calibration, I'll try to test them and eventually open a PR depending on the results.

[S-Dafarra]

• @kouroshD @S-Dafarra are you sure you were using this option back in time? Maybe you were just using calibrate head_fake?

You are probably right. Not sure why I was working back then. I guess that by not taking into consideration the lower part, we did not spot any other issue.

[claudia-lat]

cc @kouroshD

[claudia-lat]

cc @lrapetti

[claudia-lat]

Almost ready with the review. Just waiting for a final test @lrapetti

[lrapetti]

Mereged #211, closing.