Ensure bindings for computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics are working fine

bindings/iDynTree.i

@@ -36,6 +36,7 @@
 %{
 /* Note : always include headers following the inheritance order */
 #include <cmath>
+#include <cstddef>
 
 //Utils
 #include "iDynTree/Utils.h"
@@ -140,6 +141,25 @@
 
 %}
 
+// Wrap the std::vector<iDynTree::MatrixDynSize> params
+%template(MatrixDynSizeVector) std::vector<iDynTree::MatrixDynSize>;
+
+// Wrap the std::vector<iDynTree::VectorDynSize> params
+%template(VectorDynSizeVector) std::vector<iDynTree::VectorDynSize>;
+
+// Wrap the:
+// * std::vector<std::ptrdiff_t>,
+// * std::vector<iDynTree::LinkIndex>,
+// * std::vector<iDynTree::JointIndex>,
+// * std::vector<iDynTree::DOFIndex>,
+// * std::vector<iDynTree::FrameIndex>,
+// * std::vector<iDynTree::TraversalIndex>,
+// params
+namespace std {
+    typedef ::ptrdiff_t ptrdiff_t;
+}
+%template(IndexVector) std::vector<std::ptrdiff_t>;
+
 //Wrap std::vector<BerdySensors>
 namespace std {
     %template(BerdySensors) vector<iDynTree::BerdySensor>;

bindings/python/tests/CMakeLists.txt

@@ -8,3 +8,4 @@ add_test(NAME PythonGeometryTests COMMAND ${Python3_EXECUTABLE} geometry.py)
 add_test(NAME PythonJointTests COMMAND ${Python3_EXECUTABLE} joints.py)
 add_test(NAME PythonHelperTests COMMAND ${Python3_EXECUTABLE} helpers.py)
 add_test(NAME PythonModelLoaderTests COMMAND ${Python3_EXECUTABLE} modelloader.py)
+add_test(NAME PythonExtWrenchesAndJointTorquesEstimatorUnitTest COMMAND ${Python3_EXECUTABLE} ExtWrenchesAndJointTorquesEstimatorUnitTest.py)

bindings/python/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.py

@@ -0,0 +1,122 @@
+import sys
+
+sys.path.append("../../python/")
+sys.path.append("../../../lib/python/")
+sys.path.append("../../../lib/python/Debug/")
+
+import os
+import unittest
+import idyntree.swig as iDynTree;
+import numpy as np
+import random
+
+class ExtWrenchesAndJointTorquesEstimatorUnitTest(unittest.TestCase):
+    def tol(self):
+        return 1e-10;
+
+    def places(self):
+        return 7;
+
+    def checkApproxEqual(self,val1,val2,msg):
+        self.assertAlmostEqual(val1,val2,self.places(),msg);
+
+
+    def checkVecApproxEqual(self,p1,p2,msg):
+        msgMore = "val1 = " + str(p1) + " and val2 = " + str(p2)
+        for i in range(0,len(p1)):
+            self.checkApproxEqual(p1[i],p2[i],msg+":"+msgMore);
+
+    def testcomputeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(self):
+
+        # Define considered joints
+        consideredJoints = iDynTree.StringVector()
+        consideredJoints.push_back("torso_pitch")
+        consideredJoints.push_back("torso_roll")
+        consideredJoints.push_back("torso_yaw")
+        consideredJoints.push_back("neck_pitch")
+        consideredJoints.push_back("neck_roll")
+        consideredJoints.push_back("neck_yaw")
+        consideredJoints.push_back("l_shoulder_pitch")
+        consideredJoints.push_back("l_shoulder_roll")
+        consideredJoints.push_back("l_shoulder_yaw")
+        consideredJoints.push_back("l_elbow")
+        consideredJoints.push_back("r_shoulder_pitch")
+        consideredJoints.push_back("r_shoulder_roll")
+        consideredJoints.push_back("r_shoulder_yaw")
+        consideredJoints.push_back("r_elbow")
+        consideredJoints.push_back("l_hip_pitch")
+        consideredJoints.push_back("l_hip_roll")
+        consideredJoints.push_back("l_hip_yaw")
+        consideredJoints.push_back("l_knee")
+        consideredJoints.push_back("l_ankle_pitch")
+        consideredJoints.push_back("l_ankle_roll")
+        consideredJoints.push_back("r_hip_pitch")
+        consideredJoints.push_back("r_hip_roll")
+        consideredJoints.push_back("r_hip_yaw")
+        consideredJoints.push_back("r_knee")
+        consideredJoints.push_back("r_ankle_pitch")
+        consideredJoints.push_back("r_ankle_roll")
+
+        # Load model
+        estimator = iDynTree.ExtWrenchesAndJointTorquesEstimator()
+        testModel = os.path.join(os.path.dirname(__file__), "../../../../src/tests/data/iCubDarmstadt01.urdf")
+        ok = estimator.loadModelAndSensorsFromFileWithSpecifiedDOFs(testModel, consideredJoints)
+        self.assertTrue(ok)
+
+        # Set random state
+        dofs = estimator.model().getNrOfDOFs()
+        q = iDynTree.JointPosDoubleArray.FromPython([random.random() for i in range(0, dofs)])
+        dq = iDynTree.JointDOFsDoubleArray.FromPython([random.random() for i in range(0, dofs)])
+        ddq = iDynTree.JointDOFsDoubleArray.FromPython([random.random() for i in range(0, dofs)])
+        grav = iDynTree.Vector3.FromPython([0.0, 0.0, -9.81])
+
+        root_link_index = estimator.model().getFrameIndex("root_link")
+        estimator.updateKinematicsFromFixedBase(q,dq,ddq,root_link_index,grav)
+
+        # Set unknown contact forces
+        fullBodyUnknowns = iDynTree.LinkUnknownWrenchContacts(estimator.model())
+        unknown = iDynTree.UnknownWrenchContact()
+        unknown.unknownType = iDynTree.FULL_WRENCH
+        unknown.contactPoint.zero()
+        fullBodyUnknowns.addNewContactInFrame(estimator.model(), root_link_index, unknown)
+
+        # Compute expected FT measures
+        # First of all, we build the 6*nrOfFTsensors vector composed by the known FT sensors measures
+        sensOffset = iDynTree.SensorsMeasurements(estimator.model().sensors())
+        estimatedContactWrenches = iDynTree.LinkContactWrenches(estimator.model())
+        estimatedJointTorques = iDynTree.JointDOFsDoubleArray(estimator.model())
+
+        ok = estimator.computeExpectedFTSensorsMeasurements(fullBodyUnknowns, sensOffset, estimatedContactWrenches, estimatedJointTorques)
+        self.assertTrue(ok)
+
+        nrOfFTSensors = estimator.model().sensors().getNrOfSensors(iDynTree.SIX_AXIS_FORCE_TORQUE)
+
+        w = np.zeros(6*nrOfFTSensors)
+        for ft in range(0, nrOfFTSensors):
+            ft_meas = iDynTree.Wrench()
+            sensOffset.getMeasurement(iDynTree.SIX_AXIS_FORCE_TORQUE, ft, ft_meas)
+            w[range(6*ft, 6*ft+6)] = ft_meas.toNumPy()
+
+        A = iDynTree.MatrixDynSizeVector()
+        b = iDynTree.VectorDynSizeVector()
+        subModelIDs = iDynTree.IndexVector()
+        baseLinkIndeces = iDynTree.IndexVector()
+
+
+        # Test computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics
+        ok = estimator.computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(fullBodyUnknowns, A, b, subModelIDs, baseLinkIndeces)
+        self.assertTrue(ok)
+        self.assertEqual(A.size(), b.size())
+        self.assertEqual(A.size(), subModelIDs.size())
+        self.assertEqual(A.size(), baseLinkIndeces.size())
+
+        for i in range(0, A.size()):
+            bNumPy = b[i].toNumPy()
+            bCheck = A[i].toNumPy()@w
+            self.checkVecApproxEqual(bNumPy, bCheck, "computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics check failed")
+
+
+if __name__ == '__main__':
+    # initalize the seed to have predictable results
+    random.seed(0)
+    unittest.main()

src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h

@@ -64,6 +64,8 @@ class ExtWrenchesAndJointTorquesEstimator
     LinkTraversalsCache m_kinematicTraversals;
 
     JointPosDoubleArray m_jointPos;
+    JointDOFsDoubleArray m_jointVel;
+    JointDOFsDoubleArray m_jointAcc;
     LinkVelArray m_linkVels;
     LinkAccArray m_linkProperAccs;
     LinkNetExternalWrenches m_linkNetExternalWrenches;
@@ -190,9 +192,9 @@ class ExtWrenchesAndJointTorquesEstimator
      *                       expressed in the specified frame orientation.
      * @return true if all went ok, false otherwise.
      */
-    bool updateKinematicsFromFloatingBase(const JointPosDoubleArray  & jointPos,
-                                          const JointDOFsDoubleArray & jointVel,
-                                          const JointDOFsDoubleArray & jointAcc,
+    bool updateKinematicsFromFloatingBase(const VectorDynSize  & jointPos,
+                                          const VectorDynSize & jointVel,
+                                          const VectorDynSize & jointAcc,
                                           const FrameIndex & floatingFrame,
                                           const Vector3 & properClassicalLinearAcceleration,
                                           const Vector3 & angularVel,
@@ -209,9 +211,9 @@ class ExtWrenchesAndJointTorquesEstimator
      * @param[in] gravity the gravity acceleration vector, expressed in the specified fixed frame.
      * @return true if all went ok, false otherwise.
      */
-    bool updateKinematicsFromFixedBase(const JointPosDoubleArray  & jointPos,
-                                       const JointDOFsDoubleArray & jointVel,
-                                       const JointDOFsDoubleArray & jointAcc,
+    bool updateKinematicsFromFixedBase(const VectorDynSize  & jointPos,
+                                       const VectorDynSize & jointVel,
+                                       const VectorDynSize & jointAcc,
                                        const FrameIndex & fixedFrame,
                                        const Vector3 & gravity);
 
@@ -305,7 +307,7 @@ class ExtWrenchesAndJointTorquesEstimator
     bool computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts & unknowns,
                                                                      std::vector<iDynTree::MatrixDynSize>& A,
                                                                      std::vector<iDynTree::VectorDynSize>& b,
-                                                                     std::vector<size_t>& subModelID,
+                                                                     std::vector<std::ptrdiff_t>& subModelID,
                                                                      std::vector<iDynTree::LinkIndex>& baseLinkIndeces);
 
     /**

src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp

@@ -40,6 +40,8 @@ ExtWrenchesAndJointTorquesEstimator::ExtWrenchesAndJointTorquesEstimator():
     m_dynamicTraversal(),
     m_kinematicTraversals(),
     m_jointPos(),
+    m_jointVel(),
+    m_jointAcc(),
     m_linkVels(),
     m_linkProperAccs(),
     m_linkNetExternalWrenches(),
@@ -88,7 +90,9 @@ bool ExtWrenchesAndJointTorquesEstimator::setModel(const Model& _model)
     m_model.computeFullTreeTraversal(m_dynamicTraversal);
     m_kinematicTraversals.resize(m_model);
 
-
+    m_jointPos.resize(m_model);
+    m_jointVel.resize(m_model);
+    m_jointAcc.resize(m_model);
     m_linkVels.resize(m_model);
     m_linkProperAccs.resize(m_model);
     m_linkIntWrenches.resize(m_model);
@@ -187,9 +191,9 @@ const SubModelDecomposition& ExtWrenchesAndJointTorquesEstimator::submodels() co
 }
 
 
-bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFixedBase(const JointPosDoubleArray& jointPos,
-                                                                        const JointDOFsDoubleArray& jointVel,
-                                                                        const JointDOFsDoubleArray& jointAcc,
+bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFixedBase(const VectorDynSize& jointPos,
+                                                                        const VectorDynSize& jointVel,
+                                                                        const VectorDynSize& jointAcc,
                                                                         const FrameIndex& fixedFrame,
                                                                         const Vector3& gravity)
 {
@@ -211,9 +215,10 @@ bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFixedBase(const Jo
     return updateKinematicsFromFloatingBase(jointPos,jointVel,jointAcc,fixedFrame,properClassicalAcceleration,zero,zero);
 }
 
-bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFloatingBase(const JointPosDoubleArray& jointPos,
-                                                                           const JointDOFsDoubleArray& jointVel,
-                                                                           const JointDOFsDoubleArray& jointAcc,
+
+bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFloatingBase(const VectorDynSize& jointPos,
+                                                                           const VectorDynSize& jointVel,
+                                                                           const VectorDynSize& jointAcc,
                                                                            const FrameIndex& floatingFrame,
                                                                            const Vector3& properClassicalLinearAcceleration,
                                                                            const Vector3& angularVel,
@@ -232,6 +237,26 @@ bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFloatingBase(const
         return false;
     }
 
+    if (m_jointPos.size() != jointPos.size())
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","updateKinematicsFromFloatingBase","Wrong size of input joint positions.");
+        return false;
+    }
+
+    if (m_jointVel.size() != jointVel.size())
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","updateKinematicsFromFloatingBase","Wrong size of input joint velocities.");
+    }
+
+    if (m_jointAcc.size() != jointAcc.size())
+    {
+        reportError("ExtWrenchesAndJointTorquesEstimator","updateKinematicsFromFloatingBase","Wrong size of input joint accelerations.");
+    }
+
+    m_jointPos = jointPos;
+    m_jointVel = jointVel;
+    m_jointAcc = jointAcc;
+
     // Get link of the specified frame
     LinkIndex floatingLinkIndex = m_model.getFrameLink(floatingFrame);
 
@@ -255,16 +280,15 @@ bool ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFloatingBase(const
     base_acc_link = link_H_frame*base_acc_frame;
     base_classical_acc_link.fromSpatial(base_acc_link,base_vel_link);
 
+
     // Propagate the kinematics information
     bool ok = dynamicsEstimationForwardVelAccKinematics(m_model,m_kinematicTraversals.getTraversalWithLinkAsBase(m_model,floatingLinkIndex),
                                                         base_classical_acc_link.getLinearVec3(),
                                                         base_vel_link.getAngularVec3(),
                                                         base_classical_acc_link.getAngularVec3(),
-                                                        jointPos,jointVel,jointAcc,
+                                                        m_jointPos,m_jointVel,m_jointAcc,
                                                         m_linkVels,m_linkProperAccs);
 
-    // Store joint positions
-    m_jointPos = jointPos;
 
     m_isKinematicsUpdated = ok;
     return ok;
@@ -427,7 +451,7 @@ iDynTree::LinkConstPtr getLinkOfSubModelThatIsConnectedToFTSensors(const Travers
 bool ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts & unknownWrenches,
                                                                                                       std::vector<iDynTree::MatrixDynSize>& A,
                                                                                                       std::vector<iDynTree::VectorDynSize>& b,
-                                                                                                      std::vector<size_t>& subModelIDs,
+                                                                                                      std::vector<std::ptrdiff_t>& subModelIDs,
                                                                                                       std::vector<iDynTree::LinkIndex>& baseLinkIndeces)
 {
     if( !m_isModelValid )

src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp

@@ -214,7 +214,7 @@ int main()
     // 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<std::ptrdiff_t> subModelIDs;
     std::vector<iDynTree::LinkIndex> baseLinkIndeces;
     ok = estimatorIMU.computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(fullBodyUnknowns,A,b,subModelIDs,baseLinkIndeces);
     ASSERT_IS_TRUE(ok);

src/model/include/iDynTree/JointState.h

@@ -29,6 +29,7 @@ namespace iDynTree
         void resize(const iDynTree::Model & model);
 
         bool isConsistent(const iDynTree::Model & model) const;
+        JointPosDoubleArray& operator=(const iDynTree::VectorDynSize& input);
 
         ~JointPosDoubleArray();
     };
@@ -47,6 +48,7 @@ namespace iDynTree
         void resize(const iDynTree::Model & model);
 
         bool isConsistent(const iDynTree::Model & model) const;
+        JointDOFsDoubleArray& operator=(const iDynTree::VectorDynSize& input);
 
         ~JointDOFsDoubleArray();
     };

src/model/src/JointState.cpp

@@ -36,6 +36,13 @@ bool JointPosDoubleArray::isConsistent(const Model& model) const
     return (this->size() == model.getNrOfPosCoords());
 }
 
+JointPosDoubleArray& JointPosDoubleArray::operator=(const iDynTree::VectorDynSize& input)
+{
+    iDynTree::VectorDynSize* thisUpCast = static_cast<iDynTree::VectorDynSize*>(this);
+    *thisUpCast = input;
+    return *this;
+}
+
 JointPosDoubleArray::~JointPosDoubleArray()
 {
 
@@ -68,6 +75,13 @@ bool JointDOFsDoubleArray::isConsistent(const Model& model) const
     return (this->size() == model.getNrOfDOFs());
 }
 
+JointDOFsDoubleArray& JointDOFsDoubleArray::operator=(const iDynTree::VectorDynSize& input)
+{
+    iDynTree::VectorDynSize* thisUpCast = static_cast<iDynTree::VectorDynSize*>(this);
+    *thisUpCast = input;
+    return *this;
+}
+
 JointDOFsDoubleArray::~JointDOFsDoubleArray()
 {