diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i index e206389d6d..bb14aaa404 100644 --- a/bindings/iDynTree.i +++ b/bindings/iDynTree.i @@ -36,6 +36,7 @@ %{ /* Note : always include headers following the inheritance order */ #include +#include //Utils #include "iDynTree/Utils.h" @@ -140,6 +141,25 @@ %} +// Wrap the std::vector params +%template(MatrixDynSizeVector) std::vector; + +// Wrap the std::vector params +%template(VectorDynSizeVector) std::vector; + +// Wrap the: +// * std::vector, +// * std::vector, +// * std::vector, +// * std::vector, +// * std::vector, +// * std::vector, +// params +namespace std { + typedef ::ptrdiff_t ptrdiff_t; +} +%template(IndexVector) std::vector; + //Wrap std::vector namespace std { %template(BerdySensors) vector; diff --git a/bindings/python/tests/CMakeLists.txt b/bindings/python/tests/CMakeLists.txt index 1359bf3689..e82f35404e 100644 --- a/bindings/python/tests/CMakeLists.txt +++ b/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) diff --git a/bindings/python/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.py b/bindings/python/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.py new file mode 100644 index 0000000000..ee540e0720 --- /dev/null +++ b/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() diff --git a/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h b/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h index b879080b8d..3338373456 100644 --- a/src/estimation/include/iDynTree/ExtWrenchesAndJointTorquesEstimator.h +++ b/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& A, std::vector& b, - std::vector& subModelID, + std::vector& subModelID, std::vector& baseLinkIndeces); /** diff --git a/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp b/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp index 33e8f55ee7..ca3370fcfe 100644 --- a/src/estimation/src/ExtWrenchesAndJointTorquesEstimator.cpp +++ b/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& A, std::vector& b, - std::vector& subModelIDs, + std::vector& subModelIDs, std::vector& baseLinkIndeces) { if( !m_isModelValid ) diff --git a/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp b/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp index 922f3d72b8..ed30c1905f 100644 --- a/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp +++ b/src/estimation/tests/ExtWrenchesAndJointTorquesEstimatorUnitTest.cpp @@ -214,7 +214,7 @@ int main() // Then, we compute the A and b matrices std::vector A; std::vector b; - std::vector subModelIDs; + std::vector subModelIDs; std::vector baseLinkIndeces; ok = estimatorIMU.computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(fullBodyUnknowns,A,b,subModelIDs,baseLinkIndeces); ASSERT_IS_TRUE(ok); diff --git a/src/model/include/iDynTree/JointState.h b/src/model/include/iDynTree/JointState.h index 182b558d7b..2c888ec297 100644 --- a/src/model/include/iDynTree/JointState.h +++ b/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(); }; diff --git a/src/model/src/JointState.cpp b/src/model/src/JointState.cpp index 727edf725e..48e3d0ea2b 100644 --- a/src/model/src/JointState.cpp +++ b/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(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(this); + *thisUpCast = input; + return *this; +} + JointDOFsDoubleArray::~JointDOFsDoubleArray() {