Add method to help calibrating multiple FT sensors attached to the same submodel

[traversaro]

This PR adds the ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics method, that can be used to compute for each submodel without any external wrench, the equation that relates the FT sensor measures with the kinematics-related known terms. This function can be useful to perform calibration of multiple FT connected to the same submodel without any external force, even if multiple links (that belong to different submodels) have external forces on it.

For more theorical details, check the ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics documentation, and for details on how to use the class check the added tests.

[HosameldinMohamed]

Hi @traversaro thanks a lot for this implementation. I went through the code and I more or less understood it, but I have some curiosities:

• Since the matrix $A$ and the vector $b$ are computed to obtain the expected FT wrenches, why provide $A$ and $b$ instead of computing $w_{sm}$ by the pseudo-inverse? In this way, the function will be compatible with the method computeExpectedFTSensorsMeasurements(), especially since you checked in the unit test that the expected computed with the latter provides a correct solution for the A and b pair computed by the new method.

[HosameldinMohamed]

Probably it's better to highlight here that this represents the expected measures from the FTs, not the actual measures we get from the sensor,,

[traversaro]

Do you have any idea how to formulate the sentence better/more understandable? Assuming that the multibody model is correct (that is kind of the assumption of the whole class) that sentence is correct. Unless we introduce somewhere the concept of "expected measures" (that we need to define somewhere) and "actual measures", I am not sure if I am able to make this sentence more clear.

[HosameldinMohamed]

No I think it's clear enough to me once I read through the whole explanation.

I was just expecting a similar structure to the computeExpectedFTSensorsMeasurements() method in my mind. But now I see they are serving different goals.

[HosameldinMohamed]

I'm curious to see if pinv(A[l])*b[l] = w with the A matrices contain many zeros that correspond to not being attached to FTs.

[traversaro]

Note that depending on the rank and shape of of $A$, we can't say that pinv(A[l])*b[l] = w, but just that pinv(A[l])*b[l] = pinv(A[l])*A[l]*w.

[traversaro]

• Since the matrix A and the vector b are computed to obtain the expected FT wrenches, why provide A and b instead of computing wsm by the pseudo-inverse? In this way, the function will be compatible with the method computeExpectedFTSensorsMeasurements(), especially since you checked in the unit test that the expected computed with the latter provides a correct solution for the A and b pair computed by the new method.

The main reason is that in this way we can plug this information in any learning/identification/calibration algorithm. Let's say that you are in the case in which you have a model in which they are two FT sensors, connected in a chain:

(code: ExampleFTs.zip )

For this model, let's say that you have a dataset composed by $i = 1 ... N$ data points, and:

• The submodels induced by the FT sensors in the models are ${SM}_1 = \lbrace L_1 \rbrace$, ${SM}_2 = \lbrace L_2, L_3\rbrace$ , ${SM}_3 = \lbrace L_4 \rbrace$.
• $r_1^i \in \mathbb{R}^6$, $r_2^i \in \mathbb{R}^6$ raw measurement values of sensors $S_1$ and $S_2$
• $c^i \subseteq \mathcal{P}(\lbrace L_1, L_2, L_3, L_4 \rbrace)$ is the set of the link in contacts for the data point $i$ (Note: $\mathcal{P}(\lbrace L_1, L_2, L_3, L_4 \rbrace)$ is the power set (i.e. the set of all possible subsets) of the set of the links)
• ${}^i \phi_i$ the inertial and gravitational force-torque for each link, compueted using the updateKinematicsFromFloatingBase or updateKinematicsFromFixedBase method

And you want to learn the parameters for the following generic non-linear calibration models for the FT sensors (this models could be linear models, polynomial models, feedforward Neural networks):

$$ \mathrm{f}^i_1 = \mathrm{f}_1 (r_1^i, \theta_1) $$

$$ \mathrm{f}^i_2 = \mathrm{f}_2 (r_1^i, \theta_2) $$

Using the method introduced in this PR, you can write the following cost function to minimize:

$$ C(\theta_1, \theta_2) = \sum_{i=1}^{N} \sum_{sm=1}^3 \left[ {SM}_{sm} \cap c^i_{sm} = \emptyset \right] \left| A^i_j \begin{bmatrix} \mathrm{f}_1 (r_1^i, \theta_1) \\ \mathrm{f}_2 (r_1^i, \theta_2) \end{bmatrix} - b^i_j \right|^2 $$

Where:

• Where $A^i_j$ and $b^i_j$ are the matrix and vector computed by the ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics method
• $\left[ P \right]$ are the Iverson bracket, i.e. $\left[ P \right] = 1$ if P is true $0$ otherwise.
• ${SM}_{sm} \cap c^i = \emptyset$ is just a fancy mathematical term to say "there is no contact in submodel $sm$ (specifically, it says: the interesection between the set of link in contact $c^i$ and of the links of submodel $sm$ ${SM}_{sm}$ is empty)
• I left out any weight for simplifity, that in a real implementation need to be added.

The identification problem could then be solved by:

$$ (\theta_1^*, \theta_2^*) = argmin_{(\theta_1, \theta_2} C(\theta_1, \theta_2) $$

If the method gave in input directly the $\mathrm{f}^i_1$ and $\mathrm{f}^i_2$ computed by solving the pseudoinverse, you could not write (unless I am missing something, that is possible) the cost function that $C(\theta_1, \theta_2)$.

Note that in specific cases (for example if both $L_1$ and $L_4$ are in in contact, i.e. $c^i = \lbrace L_1, L_4 \rbrace$) the size of $A$ is $6 \times 12$, and so for sure there are infinite solution that minimize the least square residual.

[traversaro]

I am not excited about the naming, but let's merge this, we can always change names later.

[HosameldinMohamed]

Can we generate MATLAB bindings for this method?

Maybe you could give me pointers on how to do it?

[traversaro]

Good idea, so we see if they work! MATLAB bindings generation is done by an action, let me trigger it.

[HosameldinMohamed]

Good idea, so we see if they work! MATLAB bindings generation is done by an action, let me trigger it.

Greate! Thanks a lot!