Contact motion task can be set as equality from user

Author: earlaud

include/simple-mpc/inverse-dynamics.hpp

@@ -53,6 +53,9 @@ namespace simple_mpc
       DEFINE_FIELD(double, w_posture, -1.)        // Disabled by default
       DEFINE_FIELD(double, w_contact_motion, -1.) // Disabled by default
       DEFINE_FIELD(double, w_contact_force, -1.)  // Disabled by default
+
+      ///< Are the contact motion = 0 handled as a hard contraint (true) or a cost (if false)
+      DEFINE_FIELD(bool, contact_motion_equality, false)
     };
 
     KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, double control_dt, const Settings settings);

src/inverse-dynamics.cpp

@@ -146,7 +146,9 @@ void KinodynamicsID::setTarget(
     {
       if (!active_tsid_contacts_[i])
       {
-        formulation_.addRigidContact(*tsid_contacts[i], settings_.w_contact_force, settings_.w_contact_motion, 1);
+        formulation_.addRigidContact(
+          *tsid_contacts[i], settings_.w_contact_force, settings_.w_contact_motion,
+          settings_.contact_motion_equality ? 0 : 1);
       }
       switch (model_handler_.getFootType(i))
       {