Add land_cstr boolean

Author: edantec

bindings/expose-fulldynamics.cpp

@@ -39,6 +39,7 @@ namespace simple_mpc::python
     conf.torque_limits = bp::extract<bool>(settings["torque_limits"]);
     conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
     conf.force_cone = bp::extract<bool>(settings["force_cone"]);
+    conf.land_cstr = bp::extract<bool>(settings["land_cstr"]);
 
     return new FullDynamicsOCP(conf, model_handler);
   }
@@ -126,6 +127,7 @@ namespace simple_mpc::python
     settings["torque_limits"] = conf.torque_limits;
     settings["kinematics_limits"] = conf.kinematics_limits;
     settings["force_cone"] = conf.force_cone;
+    settings["land_cstr"] = conf.land_cstr;
 
     return settings;
   }

bindings/expose-kinodynamics.cpp

@@ -28,6 +28,7 @@ namespace simple_mpc::python
 
     conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);
     conf.force_cone = bp::extract<bool>(settings["force_cone"]);
+    conf.land_cstr = bp::extract<bool>(settings["land_cstr"]);
 
     return new KinodynamicsOCP(conf, model_handler);
   }
@@ -51,6 +52,7 @@ namespace simple_mpc::python
     settings["Wfoot"] = conf.Wfoot;
     settings["kinematics_limits"] = conf.kinematics_limits;
     settings["force_cone"] = conf.force_cone;
+    settings["land_cstr"] = conf.land_cstr;
 
     return settings;
   }

examples/go2_fulldynamics.py

@@ -74,6 +74,7 @@
     torque_limits=True,
     kinematics_limits=True,
     force_cone=False,
+    land_cstr=False
 )
 T = 50
 

examples/go2_kinodynamics.py

@@ -75,6 +75,7 @@
     Wfoot=0.01,
     kinematics_limits=True,
     force_cone=False,
+    land_cstr=False
 )
 T = 50
 

examples/talos_fulldynamics.py

@@ -81,6 +81,7 @@
     torque_limits=True,
     kinematics_limits=True,
     force_cone=True,
+    land_cstr=False
 )
 
 T = 100

examples/talos_kinodynamics.py

@@ -89,6 +89,7 @@
     Wfoot=0.075,
     kinematics_limits=True,
     force_cone=False,
+    land_cstr=False
 )
 
 T = 100

include/simple-mpc/fulldynamics.hpp

@@ -49,6 +49,7 @@ namespace simple_mpc
     bool torque_limits;
     bool kinematics_limits;
     bool force_cone;
+    bool land_cstr;
 
     // Control limits
     Eigen::VectorXd umin;

include/simple-mpc/kinodynamics.hpp

@@ -47,6 +47,7 @@ namespace simple_mpc
     // Constraint
     bool kinematics_limits;
     bool force_cone;
+    bool land_cstr;
   };
 
   class KinodynamicsOCP : public OCPHandler

package.xml

@@ -14,6 +14,7 @@
   <depend>boost</depend>
   <depend>pinocchio</depend>
   <depend>proxsuite-nlp</depend>
+  <depend>proxsuite</depend>
   <depend>aligator</depend>
   <depend>std_msgs</depend>
 

src/fulldynamics.cpp

@@ -171,7 +171,7 @@ namespace simple_mpc
           stm.addConstraint(wrench_residual, NegativeOrthant());
         }
 
-        if (land_constraint.at(name))
+        if (settings_.land_cstr and land_constraint.at(name))
         {
           FrameVelocityResidual velocity_residual = FrameVelocityResidual(
             space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name),
@@ -187,7 +187,7 @@ namespace simple_mpc
             space.ndx(), model_handler_.getModel(), actuation_matrix_, cms, prox_settings_, name, settings_.mu);
           stm.addConstraint(friction_residual, NegativeOrthant());
         }
-        if (land_constraint.at(name))
+        if (settings_.land_cstr and land_constraint.at(name))
         {
           std::vector<int> vel_id = {0, 1, 2};
           FrameVelocityResidual velocity_residual = FrameVelocityResidual(