adding addMeasuredForce to the formulation bindings

include/tsid/bindings/python/formulations/formulation.hpp

@@ -27,6 +27,7 @@
 #include "tsid/contacts/contact-6d.hpp"
 #include "tsid/contacts/contact-point.hpp"
 #include "tsid/contacts/contact-two-frame-positions.hpp"
+#include "tsid/contacts/measured-6Dwrench.hpp"
 #include "tsid/tasks/task-joint-posture.hpp"
 #include "tsid/tasks/task-se3-equality.hpp"
 #include "tsid/tasks/task-com-equality.hpp"
@@ -127,7 +128,9 @@ struct InvDynPythonVisitor
         .def("checkContact", &InvDynPythonVisitor::checkContact,
              bp::args("name", "HQPOutput"))
         .def("getContactForce", &InvDynPythonVisitor::getContactForce,
-             bp::args("name", "HQPOutput"));
+             bp::args("name", "HQPOutput"))
+        .def("addMeasuredForce", &InvDynPythonVisitor::addMeasuredForce,
+               bp::args("measured_force"));
   }
   static pinocchio::Data data(T& self) {
     pinocchio::Data data = self.data();
@@ -273,6 +276,9 @@ struct InvDynPythonVisitor
                                          const solvers::HQPOutput& sol) {
     return self.getContactForces(name, sol);
   }
+  static bool addMeasuredForce(T& self, contacts::Measured6Dwrench measured_force) {
+     return self.addMeasuredForce(measured_force);
+}
 
   static void expose(const std::string& class_name) {
     std::string doc = "InvDyn info.";

tests/python/test_Measured6DWrench.py

@@ -22,10 +22,10 @@
 frameName = "RAnkleRoll"
 contact = tsid.Measured6Dwrench(
     "Measured6Dwrench", robot, frameName)
-wrench = np.asarray([1.5, 1.5, 1.5, 2.5, 2.5, 2.5])
+wrench = np.asarray([1.5, 1.5, 1.5, 2.5, 2.5, 2.5]) # Some random wrench
 contact.setMeasuredContactForce(wrench)
 measured_wrench = contact.measuredContactForce
 print(measured_wrench)
 
 invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
-invdyn.addMeasuredForce(contact)
+invdyn.addMeasuredForce(contact)