Merge pull request #56 from Simple-Robotics/topic/set_land_constraints

edantec · web-flow · commit dd71f120ebf3 · 2025-05-06T16:12:38.000+02:00
Add land_cstr boolean
diff --git a/bindings/expose-fulldynamics.cpp b/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;
   }
diff --git a/bindings/expose-kinodynamics.cpp b/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;
   }
diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py
@@ -74,6 +74,7 @@
     torque_limits=True,
     kinematics_limits=True,
     force_cone=False,
+    land_cstr=False
 )
 T = 50
 
diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py
@@ -75,6 +75,7 @@
     Wfoot=0.01,
     kinematics_limits=True,
     force_cone=False,
+    land_cstr=False
 )
 T = 50
 
diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py
@@ -81,6 +81,7 @@
     torque_limits=True,
     kinematics_limits=True,
     force_cone=True,
+    land_cstr=False
 )
 
 T = 100
diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py
@@ -89,6 +89,7 @@
     Wfoot=0.075,
     kinematics_limits=True,
     force_cone=False,
+    land_cstr=False
 )
 
 T = 100
diff --git a/include/simple-mpc/fulldynamics.hpp b/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;
diff --git a/include/simple-mpc/kinodynamics.hpp b/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
diff --git a/package.xml b/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>
 
diff --git a/pixi.lock b/pixi.lock
diff --git a/pixi.toml b/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"
diff --git a/src/fulldynamics.cpp b/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(
diff --git a/src/kinodynamics.cpp b/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};
 
diff --git a/tests/problem.cpp b/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);
 
diff --git a/tests/test_utils.cpp b/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;
 }

Original file line number	Diff line number	Diff line change
`@@ -39,6 +39,7 @@ namespace simple_mpc::python`
`39`	`39`	`conf.torque_limits = bp::extract<bool>(settings["torque_limits"]);`
`40`	`40`	`conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);`
`41`	`41`	`conf.force_cone = bp::extract<bool>(settings["force_cone"]);`
	`42`	`+ conf.land_cstr = bp::extract<bool>(settings["land_cstr"]);`
`42`	`43`
`43`	`44`	`return new FullDynamicsOCP(conf, model_handler);`
`44`	`45`	`}`
`@@ -126,6 +127,7 @@ namespace simple_mpc::python`
`126`	`127`	`settings["torque_limits"] = conf.torque_limits;`
`127`	`128`	`settings["kinematics_limits"] = conf.kinematics_limits;`
`128`	`129`	`settings["force_cone"] = conf.force_cone;`
	`130`	`+ settings["land_cstr"] = conf.land_cstr;`
`129`	`131`
`130`	`132`	`return settings;`
`131`	`133`	`}`
Original file line number	Diff line number	Diff line change
`@@ -28,6 +28,7 @@ namespace simple_mpc::python`
`28`	`28`
`29`	`29`	`conf.kinematics_limits = bp::extract<bool>(settings["kinematics_limits"]);`
`30`	`30`	`conf.force_cone = bp::extract<bool>(settings["force_cone"]);`
	`31`	`+ conf.land_cstr = bp::extract<bool>(settings["land_cstr"]);`
`31`	`32`
`32`	`33`	`return new KinodynamicsOCP(conf, model_handler);`
`33`	`34`	`}`
`@@ -51,6 +52,7 @@ namespace simple_mpc::python`
`51`	`52`	`settings["Wfoot"] = conf.Wfoot;`
`52`	`53`	`settings["kinematics_limits"] = conf.kinematics_limits;`
`53`	`54`	`settings["force_cone"] = conf.force_cone;`
	`55`	`+ settings["land_cstr"] = conf.land_cstr;`
`54`	`56`
`55`	`57`	`return settings;`
`56`	`58`	`}`
Original file line number	Diff line number	Diff line change
`@@ -74,6 +74,7 @@`
`74`	`74`	`torque_limits=True,`
`75`	`75`	`kinematics_limits=True,`
`76`	`76`	`force_cone=False,`
	`77`	`+ land_cstr=False`
`77`	`78`	`)`
`78`	`79`	`T = 50`
`79`	`80`
Original file line number	Diff line number	Diff line change
`@@ -75,6 +75,7 @@`
`75`	`75`	`Wfoot=0.01,`
`76`	`76`	`kinematics_limits=True,`
`77`	`77`	`force_cone=False,`
	`78`	`+ land_cstr=False`
`78`	`79`	`)`
`79`	`80`	`T = 50`
`80`	`81`
Original file line number	Diff line number	Diff line change
`@@ -81,6 +81,7 @@`
`81`	`81`	`torque_limits=True,`
`82`	`82`	`kinematics_limits=True,`
`83`	`83`	`force_cone=True,`
	`84`	`+ land_cstr=False`
`84`	`85`	`)`
`85`	`86`
`86`	`87`	`T = 100`
Original file line number	Diff line number	Diff line change
`@@ -89,6 +89,7 @@`
`89`	`89`	`Wfoot=0.075,`
`90`	`90`	`kinematics_limits=True,`
`91`	`91`	`force_cone=False,`
	`92`	`+ land_cstr=False`
`92`	`93`	`)`
`93`	`94`
`94`	`95`	`T = 100`