Add land_cstr boolean

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>
 

pixi.toml

@@ -20,6 +20,7 @@ libboost-python-devel = ">=1.80.0"
 python = "3.11"
 eigenpy = ">=3.10.1"
 aligator = ">=0.13"
+proxsuite = ">=0.7.2"
 benchmark = ">=1.8.0"
 pytest = ">=8.3.0"
 matplotlib = ">=3.9"

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(

src/kinodynamics.cpp

@@ -129,7 +129,7 @@ namespace simple_mpc
 
           FunctionSliceXpr vel_slice = FunctionSliceXpr(frame_vel, vel_id);
           stm.addConstraint(vel_slice, EqualityConstraint());
-          if (land_constraint.at(name))
+          if (settings_.land_cstr and land_constraint.at(name))
           {
             std::vector<int> frame_id = {2};
 

tests/problem.cpp

@@ -46,7 +46,7 @@ BOOST_AUTO_TEST_CASE(fulldynamics)
   CostStack * cs = dynamic_cast<CostStack *>(&*sm.cost_);
 
   BOOST_CHECK_EQUAL(cs->components_.size(), 6);
-  BOOST_CHECK_EQUAL(sm.numConstraints(), 4);
+  BOOST_CHECK_EQUAL(sm.numConstraints(), 3);
 
   fdproblem.createProblem(model_handler.getReferenceState(), 100, 6, settings.gravity[2], true);
 

tests/test_utils.cpp

@@ -140,6 +140,7 @@ FullDynamicsSettings getFullDynamicsSettings(RobotModelHandler model_handler)
   settings.torque_limits = true;
   settings.kinematics_limits = true;
   settings.force_cone = true;
+  settings.land_cstr = false;
 
   return settings;
 }
@@ -186,6 +187,7 @@ KinodynamicsSettings getKinodynamicsSettings(RobotModelHandler model_handler)
   settings.Wfoot = 0.075;
   settings.kinematics_limits = true;
   settings.force_cone = true;
+  settings.land_cstr = false;
 
   return settings;
 }