Add method to help calibrating multiple FT sensors attached to the sa…

…me submodel

src/core/include/iDynTree/Core/TestUtils.h

@@ -304,6 +304,7 @@ namespace iDynTree
         {
             if( !(fabs(vec1(i)-vec2(i)) < tol) )
             {
+                std::cerr << "========> " << fabs(vec1(i)-vec2(i)) << " " << tol << std::endl;
                 checkCorrect = false;
                 correctElements[i] = false;
             }

src/estimation/include/iDynTree/Estimation/ExtWrenchesAndJointTorquesEstimator.h

@@ -43,6 +43,13 @@ namespace iDynTree
  * The effect of gravity is considered by directly using the proper acceleration in the case
  * of the floating frame (proper acceleration can be directly measured by an accelerometer)
  * or by directly providing the gravity vector in the fixed frame case.
+ *
+ * Beside its main goal of estimation of external wrenches and joint torques, the class
+ * also provide methods that can be useful to calibrate the six-axis FT sensors of the robot.
+ * These methods are:
+ *   * ExtWrenchesAndJointTorquesEstimator::computeExpectedFTSensorsMeasurements
+ *   * ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics
+ * Details of each method can be found in the method documentation
  */
 class ExtWrenchesAndJointTorquesEstimator
 {
@@ -73,7 +80,7 @@ class ExtWrenchesAndJointTorquesEstimator
 
     estimateExternalWrenchesBuffers m_calibBufs;
     estimateExternalWrenchesBuffers m_bufs;
-    
+
     /**
      * Disable copy constructor and copy operator
      */
@@ -221,6 +228,9 @@ class ExtWrenchesAndJointTorquesEstimator
      * assumption about the simmetry of the robot configuration, the joint torques and
      * the external wrenches can be done.
      *
+     * \warning Before calling this method, either updateKinematicsFromFloatingBase or
+     *          updateKinematicsFromFixedBase must be called.
+     *
      * @param[in] unknowns the unknown external wrenches.
      * @param[out] predictedMeasures the estimate measures for the FT sensors.
      * @param[out] estimatedContactWrenches the estimated contact wrenches.
@@ -232,6 +242,70 @@ class ExtWrenchesAndJointTorquesEstimator
                                                     LinkContactWrenches & estimatedContactWrenches,
                                                     JointDOFsDoubleArray & estimatedJointTorques);
 
+    /**
+     * \brief For each submodel without any external wrench, computes the equation that relates the FT sensor measures with the kinematics-related known terms.
+     *
+     * For each submodel in which there are no external forces, we can write the following equation:
+     *
+     * \f[
+     *  A w = b
+     * \f]
+     * Where:
+     *   * \f$w\f$ is the vector of dimension 6*nrOfFTSensors obtained by stacking the FT sensors measures:
+     *   * \f$A\f$ is a matrix of size 6 x 6*nrOfFTSensors that depends on position in space of the FT sensors
+     *   * \f$b\f$ is a vector of size 6 that depends on the position, velocity, acceleration and gravity of each link in the submodel
+     *
+     * This function provides an easy way to compute A and B . Typically, these quantities are not used online
+     * during the estimation of external wrenches or  internal torques, but rather as an helper method when calibrating FT sensors.
+     *
+     * In the rest of the documentation, we will refer to this quantities:
+     *   * nrOfSubModels (\f$n_{sm}\f$): the number of submodels in which the model is divided, as induced by the FT sensors present in the model.
+     *   * nrOfSubModelsWithoutExtWrenches: the number of submodels on which there is no external wrench
+     *   * nrOfFTSensors (\f$n_{ft}\f$): the number of FT sensors present in the model
+     * In particular, the value of A and b for a given submodel is the following. First of all, for any submodel $sm$ with no external force,
+     * we can write (from Equation 4.19 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf, modified to account
+     * for all FT sensors in the submodel and to remove external wrenches):
+     *
+     * \f[
+     *   \sum_{s=1}^{n_ft}~\mu_{sm, ft}~\sigma_{sm, ft}~{}_{B_{sm}} X^{ft} \mathrm{f}^{meas}_{ft} = \sum_{L \in \mathbb{L}_{sm}} {}_{B_{sm}} X^{L} {}_L \phi_L
+     * \f]
+     *
+     * where:
+     *  * \f$ \mu_{sm, ft} \f$ is equal to \f$1\f$ if the sensor \f$ft\f$ is attached to the submodel \f$sm\f$, and \f$0\f$ otherwise
+     *  * \f$ \sigma_{sm, ft} \f$ is equal to \f$1\f$ if the sensor \f$ft\f$ is measuring the force applied on submodel \f$sm\f$ or \f$-1\f$ if it is measuring the force that the submodel excerts on its neighbor submodel
+     *  *\f$ {B_{sm}} \f$ is a frame in which this equation is expressed, that for this function it is the base link of the submodel.
+     *
+     *
+     * With this definitions, we can see that A_sm and b_sm for a given submodel \f$sm\f$ can be simply be defined as:
+     *
+     * \f$
+     * A_{sm} = \begin{bmatrix} \mu_{sm, ft0}~\sigma_{sm, ft(0)}~{}_C X^{ft(0)}  & \hdots & \mu_{sm, ft(n_ft-1)}~\sigma_{sm, ft(n_ft-1)}~X^{ft(n_ft-1)} \end{bmatrix}
+     * \f$
+     *
+     * \f$
+     * w_{sm} = \begin{bmatrix} \mathrm{f}^{meas}_{ft(0)}  \\ \vdots \\ \mathrm{f}^{meas}_{ft(n_ft-1)}  \end{bmatrix}
+     * \f$
+     *
+     * \f$
+     * b_{sm} = \sum_{L \in \mathbb{L}_{sm}} {}_C X^{L} {}^L \phi_L
+     * \f$
+     *
+     * \warning Before calling this method, either updateKinematicsFromFloatingBase or
+     *          updateKinematicsFromFixedBase must be called.
+     *
+     * @param[in] unknowns the unknown external wrenches, that is used to understand the submodels in which no external wrench is present
+     * @param[out] A vector of nrOfSubModelsWithoutExtWrenches matrices of size 6 x 6*nrOfFTSensors
+     * @param[out] b vector of nrOfSubModelsWithoutExtWrenches vectors of size 6
+     * @param[out] subModelIDs vector of size nrOfSubModelsWithoutExtWrenches of unsigned integers from 0 to nrOfSubModels-1, subModelIDs[i] specifies to which submodel the quantities A[i]
+     * @param[out] baseLinkIndeces vector of size nrOfSubModelsWithoutExtWrenches of iDynTree::LinkIndex from 0 to nrOfLinks-1, baseLinkIndeces[i] specifies the link in which the equation i is expressed
+     * @return true if all went well, false otherwise.
+     */
+    bool computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts & unknowns,
+                                                                     std::vector<iDynTree::MatrixDynSize>& A,
+                                                                     std::vector<iDynTree::VectorDynSize>& b,
+                                                                     std::vector<size_t>& subModelID,
+                                                                     std::vector<iDynTree::LinkIndex>& baseLinkIndeces);
+
     /**
      * \brief Estimate the external wrenches and the internal joint torques using the measurement of the internal F/T sensors.
      *

src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp

@@ -346,6 +346,197 @@ bool ExtWrenchesAndJointTorquesEstimator::computeExpectedFTSensorsMeasurements(c
     return ok;
 }
 
+Wrench computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics_ComputebHelper(const Model& model,
+                                                              const Traversal& subModelTraversal,
+                                                              const JointPosDoubleArray & jointPos,
+                                                              const LinkVelArray& linkVel,
+                                                              const LinkAccArray& linkProperAcc,
+                                                                    estimateExternalWrenchesBuffers& bufs)
+{
+    // First compute the known term of the estimation for each link:
+    // this loop is similar to the dynamic phase of the RNEA
+    // \todo pimp up performance as done in RNEADynamicPhase
+     for(int traversalEl = subModelTraversal.getNrOfVisitedLinks()-1; traversalEl >= 0; traversalEl--)
+     {
+         LinkConstPtr visitedLink = subModelTraversal.getLink(traversalEl);
+         LinkIndex    visitedLinkIndex = visitedLink->getIndex();
+         LinkConstPtr parentLink  = subModelTraversal.getParentLink(traversalEl);
+
+         const iDynTree::SpatialInertia & I = visitedLink->getInertia();
+         const iDynTree::SpatialAcc     & properAcc = linkProperAcc(visitedLinkIndex);
+         const iDynTree::Twist          & v = linkVel(visitedLinkIndex);
+         bufs.b_contacts_subtree(visitedLinkIndex) = I*properAcc + v*(I*v);
+
+         // Iterate on childs of visitedLink
+         // We obtain all the children as all the neighbors of the link, except
+         // for its parent
+         // \todo TODO this point is definitly Tree-specific
+         // \todo TODO this "get child" for is duplicated in the code, we
+         //            should try to consolidate it
+         for(unsigned int neigh_i=0; neigh_i < model.getNrOfNeighbors(visitedLinkIndex); neigh_i++)
+         {
+             LinkIndex neighborIndex = model.getNeighbor(visitedLinkIndex,neigh_i).neighborLink;
+             // Check if this neighbor is a child of the link according to this traversal
+             if( subModelTraversal.isParentOf(visitedLinkIndex,neighborIndex) )
+             {
+                 LinkIndex childIndex = neighborIndex;
+                 IJointConstPtr neighborJoint = model.getJoint(model.getNeighbor(visitedLinkIndex,neigh_i).neighborJoint);
+                 Transform visitedLink_X_child = neighborJoint->getTransform(jointPos,visitedLinkIndex,childIndex);
+
+                 // One term of the sum in Equation 5.20 in Featherstone 2008
+                 bufs.b_contacts_subtree(visitedLinkIndex) = bufs.b_contacts_subtree(visitedLinkIndex)
+                                                            + visitedLink_X_child*bufs.b_contacts_subtree(childIndex);
+             }
+         }
+
+         if( parentLink == 0 )
+         {
+             // If the visited link is the base of the submodel, the
+             // computed known terms is the known term of the submodel itself
+             return bufs.b_contacts_subtree(visitedLinkIndex);
+         }
+     }
+
+     // If we reach this point of the code, something is really really wrong
+     assert(false);
+     return Wrench::Zero();
+}
+
+
+// If it exists, get the link of the traversal to which the FT sensor is connected. If this link does not exists,
+// return nullptr otherwise
+// Warning: this function assumes that only one link in the traversal is connected to the FT sensors, so it is
+// suitable to be used with submodels generated by some code similar to:
+// std::vector<std::string> ftJointNames;
+// getFTJointNames(m_sensors,ftJointNames);
+// bool ok = m_submodels.splitModelAlongJoints(m_model,m_dynamicTraversal,ftJointNames);
+iDynTree::LinkConstPtr getLinkOfSubModelThatIsConnectedToFTSensors(const Traversal & subModelTraversal, iDynTree::SixAxisForceTorqueSensor * ftSens)
+{
+    for(size_t traversalEl = 0; traversalEl < subModelTraversal.getNrOfVisitedLinks(); traversalEl++)
+    {
+        iDynTree::LinkConstPtr visitedLink = subModelTraversal.getLink(traversalEl);
+
+        if (ftSens->isLinkAttachedToSensor(visitedLink->getIndex()))
+        {
+            return visitedLink;
+        }
+    }
+
+    return nullptr;
+}
+
+
+bool ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts & unknownWrenches,
+                                                                                                      std::vector<iDynTree::MatrixDynSize>& A,
+                                                                                                      std::vector<iDynTree::VectorDynSize>& b,
+                                                                                                      std::vector<size_t>& subModelIDs,
+                                                                                                      std::vector<iDynTree::LinkIndex>& baseLinkIndeces)
+{
+    if( !m_isModelValid )
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","computeExpectedFTSensorsMeasurements",
+                    "Model and sensors information not set.");
+        return false;
+    }
+
+    if( !m_isKinematicsUpdated )
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","computeExpectedFTSensorsMeasurements",
+                    "Kinematic information not set.");
+        return false;
+    }
+
+    // Compute nrOfSubmodels without external forces, and specifically which submodel
+    subModelIDs.resize(0);
+    size_t nrOfSubModels = m_submodels.getNrOfSubModels();
+    for(size_t sm=0; sm < nrOfSubModels; sm++)
+    {
+        bool subModelHasExternalWrenchOnIt = false;
+
+        // Iterate over all links of the submodel, and check if there is any external contact
+        const Traversal & subModelTraversal = m_submodels.getTraversal(sm);
+        for(size_t traversalEl = 0; traversalEl < subModelTraversal.getNrOfVisitedLinks(); traversalEl++)
+        {
+            LinkConstPtr visitedLink = subModelTraversal.getLink(traversalEl);
+            size_t nrOfContactForLink = unknownWrenches.getNrOfContactsForLink(visitedLink->getIndex());
+
+            if (nrOfContactForLink > 0)
+            {
+                subModelHasExternalWrenchOnIt = true;
+            }
+        }
+
+        if (!subModelHasExternalWrenchOnIt)
+        {
+            subModelIDs.push_back(sm);
+        }
+    }
+
+    size_t nrOfSubModelsWithoutExtWrenches = subModelIDs.size();
+    size_t nrOfFTSensors = m_sensors.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
+
+    // Resize quantities
+    baseLinkIndeces.resize(nrOfSubModelsWithoutExtWrenches);
+    A.resize(nrOfSubModelsWithoutExtWrenches);
+    b.resize(nrOfSubModelsWithoutExtWrenches);
+
+    for(size_t l=0; l < nrOfSubModelsWithoutExtWrenches; l++)
+    {
+        A[l].resize(6, 6*nrOfFTSensors);
+        b[l].resize(6);
+    }
+
+    // Populate quantities
+    for (size_t l = 0; l < subModelIDs.size(); l++)
+    {
+        size_t sm = subModelIDs[l];
+        const Traversal & subModelTraversal = m_submodels.getTraversal(sm);
+
+        // Assign baseLinkIndeces
+        baseLinkIndeces[l] = subModelTraversal.getBaseLink()->getIndex();
+
+        // Compute b vector
+
+        // First compute the known term of the estimation for each link:
+        // this loop is similar to the dynamic phase of the RNEA
+        toEigen(b[l]) = toEigen(computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics_ComputebHelper(m_model,subModelTraversal,m_jointPos,m_linkVels,m_linkProperAccs,m_bufs));
+
+        // Compute A matrix
+        Eigen::Map< Eigen::Matrix<double,Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > Aeig = toEigen(A[l]);
+        Aeig.setZero();
+
+        // As a first step, we need to compute the transform between each link and the base
+        // of its submodel (we are computing the estimation equation in the submodel base frame
+        computeTransformToTraversalBase(m_model,subModelTraversal,m_jointPos,m_bufs.subModelBase_H_link);
+
+        // Iterate over all FTs of the full model, and check if they are connected to the submodel
+        // TODO: if this O(n^2) loop turns out to be expensive, we can speed it up by caching somewhere
+        // the set of FT sensors that belong to a submodel
+        for(size_t ft=0; ft < nrOfFTSensors; ft++ )
+        {
+            ::iDynTree::SixAxisForceTorqueSensor * ftSens
+                = (::iDynTree::SixAxisForceTorqueSensor *) m_sensors.getSensor(::iDynTree::SIX_AXIS_FORCE_TORQUE,ft);
+
+            assert(ftSens != 0);
+
+            iDynTree::LinkConstPtr linkConnectedToFt = getLinkOfSubModelThatIsConnectedToFTSensors(subModelTraversal, ftSens);
+
+            if (linkConnectedToFt)
+            {
+                iDynTree::Matrix6x6 link_T_sens;
+                bool ok = ftSens->getWrenchAppliedOnLinkMatrix(linkConnectedToFt->getIndex(), link_T_sens);
+                IDYNTREE_UNUSED(ok);
+                assert(ok);
+                iDynTree::Transform subModelBase_T_link = m_bufs.subModelBase_H_link(linkConnectedToFt->getIndex());
+                Aeig.block<6,6>(0, 6*ft) = iDynTree::toEigen(subModelBase_T_link.asAdjointTransformWrench())*iDynTree::toEigen(link_T_sens);
+            }
+        }
+
+    }
+
+    return true;
+}
+
 bool ExtWrenchesAndJointTorquesEstimator::estimateExtWrenchesAndJointTorques(const LinkUnknownWrenchContacts& unknowns,
                                                                              const SensorsMeasurements& ftSensorsMeasures,
                                                                                    LinkContactWrenches& estimateContactWrenches,

src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp

@@ -13,6 +13,7 @@
 
 #include "testModels.h"
 
+#include <iDynTree/Core/EigenHelpers.h>
 #include <iDynTree/Core/TestUtils.h>
 
 
@@ -114,13 +115,15 @@ int main()
 
     setDOFsSubSetPositionsInDegrees(estimatorIMU.model(),dofNames,dofPositionsInDegrees,qj);
 
+
+    double accelerationOfGravity = 9.81;
     Vector3 gravityOnRootLink;
     gravityOnRootLink.zero();
-    gravityOnRootLink(2) = -9.81;
+    gravityOnRootLink(2) = -accelerationOfGravity;
 
     Vector3 properAccelerationInIMU;
     properAccelerationInIMU.zero();
-    properAccelerationInIMU(2) = 9.81;
+    properAccelerationInIMU(2) = accelerationOfGravity;
 
     Vector3 zero;
     zero.zero();
@@ -200,7 +203,48 @@ int main()
     }
 
     // The sum of the netWrenchesWithoutGravity of all the links should be equal to the sum of the external wrenches
-    ASSERT_EQUAL_SPATIAL_FORCE_TOL(momentumDerivative,externalForces,1e-8);
+    ASSERT_EQUAL_SPATIAL_FORCE_TOL(momentumDerivative,externalForces, 1e-8);
+
+    // Test computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics method
+    // First of all, we build the 6*nrOfFTsensors vector composed by the known FT sensors measures
+    size_t nrOfFTSensors = estimatorIMU.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE);
+    Eigen::VectorXd w(6*nrOfFTSensors);
+    w.setZero();
+
+    for(size_t ft=0; ft < nrOfFTSensors; ft++)
+    {
+        iDynTree::Wrench ftMeas;
+        sensOffsetIMU.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,ft,ftMeas);
+        w.segment<6>(6*ft) = iDynTree::toEigen(ftMeas);
+    }
+
+    // Then, we compute the A and b matrices
+    std::vector<iDynTree::MatrixDynSize> A;
+    std::vector<iDynTree::VectorDynSize> b;
+    std::vector<size_t> subModelIDs;
+    std::vector<iDynTree::LinkIndex> baseLinkIndeces;
+    ok = estimatorIMU.computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(fullBodyUnknowns,A,b,subModelIDs,baseLinkIndeces);
+    ASSERT_IS_TRUE(ok);
+
+    ASSERT_IS_TRUE(A.size() == b.size());
+    ASSERT_IS_TRUE(A.size() == subModelIDs.size());
+    ASSERT_IS_TRUE(A.size() == baseLinkIndeces.size());
+
+    std::cerr << "Testing computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics method" << std::endl;
+    for(size_t l=0; l < A.size(); l++)
+    {
+        std::cerr << "Testing submodel with base " << estimatorIMU.model().getLinkName(baseLinkIndeces[l])  << std::endl;
+
+        iDynTree::VectorDynSize bCheck(b[l].size());
+        toEigen(bCheck) = toEigen(A[l])*w;
+        /*
+        std::cerr << "A: " << toEigen(A[l]) << std::endl;
+        std::cerr << "w: " << w << std::endl;
+        std::cerr << "b: " << toEigen(b[l]) << std::endl;
+        std::cerr << "bCheck: " << toEigen(bCheck) << std::endl;*/
+        ASSERT_EQUAL_VECTOR_TOL(b[l], bCheck, 1e-7);
+    }
+
 
 
     return EXIT_SUCCESS;