traversaro
commented
10 months ago
- 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 theAandbpair 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
updateKinematicsFromFloatingBaseorupdateKinematicsFromFixedBasemethod
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, \theta2) = \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::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematicsmethod - $\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.
HosameldinMohamed
This PR adds the
ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematicsmethod, 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::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematicsdocumentation, and for details on how to use the class check the added tests.