Add Inverse dynamics class using TSID

.gitignore

@@ -33,4 +33,9 @@
 
 .cache/
 build*/
-/__pycache__/
+__pycache__/
+
+
+.vscode
+CMakeFiles/
+CMakeCache.txt

CMakeLists.txt

@@ -95,8 +95,8 @@ endif()
 include(dependencies.cmake)
 
 # Main Library
-file(GLOB mpc_SOURCE CONFIGURE_DEPENDS src/*.cpp)
-file(GLOB mpc_HEADER CONFIGURE_DEPENDS include/${PROJECT_NAME}/*.hpp)
+file(GLOB_RECURSE mpc_SOURCE CONFIGURE_DEPENDS src/*.cpp)
+file(GLOB_RECURSE mpc_HEADER CONFIGURE_DEPENDS include/${PROJECT_NAME}/*.hpp)
 
 add_library(${PROJECT_NAME} SHARED ${mpc_HEADER} ${mpc_SOURCE})
 target_include_directories(
@@ -112,6 +112,7 @@ target_link_libraries(
     aligator::aligator
     example-robot-data::example-robot-data
     ndcurves::ndcurves
+    tsid::tsid
 )
 target_link_libraries(${PROJECT_NAME} PUBLIC OpenMP::OpenMP_CXX)
 set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER "${mpc_HEADER}")

README.md

@@ -81,5 +81,6 @@ source install/setup.bash
 * [Aligator](https://github.com/edantec/aligator) temporary_fix branch
 * [example-robot-data](https://github.com/Gepetto/example-robot-data)
 * [ndcurves](https://github.com/loco-3d/ndcurves)
+* [tsid](https://github.com/stack-of-tasks/tsid)
 * (optional) [eigenpy](https://github.com/stack-of-tasks/eigenpy)>=3.9.0 (Python bindings)
 * (optional) [bullet](https://github.com/bulletphysics/bullet3) (Simulation examples)

benchmark/go2.cpp

@@ -40,14 +40,14 @@ int main()
   ref_FR_foot.translation() = Eigen::Vector3d(0.17, -0.15, 0.0);
   ref_RL_foot.translation() = Eigen::Vector3d(-0.24, 0.15, 0.0);
   ref_RR_foot.translation() = Eigen::Vector3d(-0.24, -0.15, 0.0);
-  model_handler.addFoot("FL_foot", base_joint_name);
-  model_handler.addFoot("FR_foot", base_joint_name);
-  model_handler.addFoot("RL_foot", base_joint_name);
-  model_handler.addFoot("RR_foot", base_joint_name);
-  model_handler.setFootReferencePlacement("FL_foot", ref_FL_foot);
-  model_handler.setFootReferencePlacement("FR_foot", ref_FR_foot);
-  model_handler.setFootReferencePlacement("RL_foot", ref_RL_foot);
-  model_handler.setFootReferencePlacement("RR_foot", ref_RR_foot);
+  model_handler.addPointFoot("FL_foot", base_joint_name);
+  model_handler.addPointFoot("FR_foot", base_joint_name);
+  model_handler.addPointFoot("RL_foot", base_joint_name);
+  model_handler.addPointFoot("RR_foot", base_joint_name);
+  model_handler.setFootReferencePlacement(model_handler.getFootNb("FL_foot"), ref_FL_foot);
+  model_handler.setFootReferencePlacement(model_handler.getFootNb("FR_foot"), ref_FR_foot);
+  model_handler.setFootReferencePlacement(model_handler.getFootNb("RL_foot"), ref_RL_foot);
+  model_handler.setFootReferencePlacement(model_handler.getFootNb("RR_foot"), ref_RR_foot);
 
   RobotDataHandler data_handler(model_handler);
 
@@ -109,7 +109,6 @@ int main()
   int T_fly = 30;
   int T_contact = 5;
   simple_mpc::MPCSettings mpc_settings;
-  mpc_settings.ddpIteration = 1;
   mpc_settings.support_force = -gravity[2] * model_handler.getMass();
   mpc_settings.TOL = 1e-4;
   mpc_settings.mu_init = 1e-8;

benchmark/talos.cpp

@@ -72,13 +72,16 @@ int main()
   RobotModelHandler model_handler(model, "half_sitting", base_joint);
 
   // Add feet
-  model_handler.addFoot("left_sole_link", base_joint);
-  model_handler.addFoot("right_sole_link", base_joint);
+  Eigen::Matrix<double, 4, 3> foot_quad{{0.1, 0.075, 0}, {-0.1, 0.075, 0}, {-0.1, -0.075, 0}, {0.1, -0.075, 0}};
+  model_handler.addQuadFoot("left_sole_link", base_joint, foot_quad);
+  model_handler.addQuadFoot("right_sole_link", base_joint, foot_quad);
 
   model_handler.setFootReferencePlacement(
-    "left_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
+    model_handler.getFootNb("left_sole_link"),
+    pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
   model_handler.setFootReferencePlacement(
-    "right_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
+    model_handler.getFootNb("right_sole_link"),
+    pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., -0.1, 0.)));
 
   RobotDataHandler data_handler(model_handler);
 
@@ -152,36 +155,36 @@ int main()
   for (std::size_t i = 0; i < 10; i++)
   {
     std::map<std::string, bool> contact_state;
-    contact_state.insert({model_handler.getFootName(0), true});
-    contact_state.insert({model_handler.getFootName(1), true});
+    contact_state.insert({model_handler.getFootFrameName(0), true});
+    contact_state.insert({model_handler.getFootFrameName(1), true});
     contact_states.push_back(contact_state);
   }
   for (std::size_t i = 0; i < 50; i++)
   {
     std::map<std::string, bool> contact_state;
-    contact_state.insert({model_handler.getFootName(0), true});
-    contact_state.insert({model_handler.getFootName(1), false});
+    contact_state.insert({model_handler.getFootFrameName(0), true});
+    contact_state.insert({model_handler.getFootFrameName(1), false});
     contact_states.push_back(contact_state);
   }
   for (std::size_t i = 0; i < 10; i++)
   {
     std::map<std::string, bool> contact_state;
-    contact_state.insert({model_handler.getFootName(0), true});
-    contact_state.insert({model_handler.getFootName(1), true});
+    contact_state.insert({model_handler.getFootFrameName(0), true});
+    contact_state.insert({model_handler.getFootFrameName(1), true});
     contact_states.push_back(contact_state);
   }
   for (std::size_t i = 0; i < 50; i++)
   {
     std::map<std::string, bool> contact_state;
-    contact_state.insert({model_handler.getFootName(0), false});
-    contact_state.insert({model_handler.getFootName(1), true});
+    contact_state.insert({model_handler.getFootFrameName(0), false});
+    contact_state.insert({model_handler.getFootFrameName(1), true});
     contact_states.push_back(contact_state);
   }
   for (std::size_t i = 0; i < 10; i++)
   {
     std::map<std::string, bool> contact_state;
-    contact_state.insert({model_handler.getFootName(0), true});
-    contact_state.insert({model_handler.getFootName(1), true});
+    contact_state.insert({model_handler.getFootFrameName(0), true});
+    contact_state.insert({model_handler.getFootFrameName(1), true});
     contact_states.push_back(contact_state);
   }
 

bindings/CMakeLists.txt

@@ -9,11 +9,11 @@ set(
   expose-robot-handler.cpp
   expose-problem.cpp
   expose-mpc.cpp
-  expose-qp.cpp
   expose-interpolate.cpp
   expose-centroidal.cpp
   expose-fulldynamics.cpp
   expose-kinodynamics.cpp
+  expose-inverse-dynamics.cpp
   expose-friction-compensation.cpp
 )
 

bindings/expose-inverse-dynamics.cpp

@@ -0,0 +1,97 @@
+#include <eigenpy/eigenpy.hpp>
+
+#include "simple-mpc/inverse-dynamics/centroidal-id.hpp"
+#include "simple-mpc/inverse-dynamics/kinodynamics-id.hpp"
+
+namespace simple_mpc
+{
+  namespace python
+  {
+    namespace bp = boost::python;
+
+    Eigen::VectorXd solveKinoProxy(
+      KinodynamicsID & self,
+      const double t,
+      const Eigen::Ref<const Eigen::VectorXd> & q_meas,
+      const Eigen::Ref<const Eigen::VectorXd> & v_meas)
+    {
+      Eigen::VectorXd tau_res(self.model_handler_.getModel().nv - 6);
+      self.solve(t, q_meas, v_meas, tau_res);
+      return tau_res;
+    }
+
+    Eigen::VectorXd getAccelerationsKinoProxy(KinodynamicsID & self)
+    {
+      Eigen::VectorXd a(self.model_handler_.getModel().nv);
+      self.getAccelerations(a);
+      return a;
+    }
+
+    Eigen::VectorXd solveCentroidalProxy(
+      CentroidalID & self,
+      const double t,
+      const Eigen::Ref<const Eigen::VectorXd> & q_meas,
+      const Eigen::Ref<const Eigen::VectorXd> & v_meas)
+    {
+      Eigen::VectorXd tau_res(self.model_handler_.getModel().nv - 6);
+      self.solve(t, q_meas, v_meas, tau_res);
+      return tau_res;
+    }
+
+    Eigen::VectorXd getAccelerationsCentroidalProxy(CentroidalID & self)
+    {
+      Eigen::VectorXd a(self.model_handler_.getModel().nv);
+      self.getAccelerations(a);
+      return a;
+    }
+
+    void setTarget_CentroidalID(
+      CentroidalID & self,
+      const Eigen::Ref<const Eigen::Vector<double, 3>> & com_position,
+      const Eigen::Ref<const Eigen::Vector<double, 3>> & com_velocity,
+      const CentroidalID::FeetPoseVector & feet_pose,
+      const CentroidalID::FeetVelocityVector & feet_velocity,
+      const std::vector<bool> & contact_state_target,
+      const std::vector<CentroidalID::TargetContactForce> & f_target)
+    {
+      self.setTarget(com_position, com_velocity, feet_pose, feet_velocity, contact_state_target, f_target);
+    }
+
+    void exposeInverseDynamics()
+    {
+      bp::class_<KinodynamicsID::Settings>("KinodynamicsIDSettings", bp::init<>(bp::args("self")))
+        .def_readwrite("friction_coefficient", &KinodynamicsID::Settings::friction_coefficient)
+        .def_readwrite("contact_weight_ratio_max", &KinodynamicsID::Settings::contact_weight_ratio_max)
+        .def_readwrite("contact_weight_ratio_min", &KinodynamicsID::Settings::contact_weight_ratio_min)
+        .def_readwrite("kp_base", &KinodynamicsID::Settings::kp_base)
+        .def_readwrite("kp_posture", &KinodynamicsID::Settings::kp_posture)
+        .def_readwrite("kp_contact", &KinodynamicsID::Settings::kp_contact)
+        .def_readwrite("w_base", &KinodynamicsID::Settings::w_base)
+        .def_readwrite("w_posture", &KinodynamicsID::Settings::w_posture)
+        .def_readwrite("w_contact_motion", &KinodynamicsID::Settings::w_contact_motion)
+        .def_readwrite("w_contact_force", &KinodynamicsID::Settings::w_contact_force)
+        .def_readwrite("contact_motion_equality", &KinodynamicsID::Settings::contact_motion_equality);
+
+      bp::class_<KinodynamicsID, boost::noncopyable>(
+        "KinodynamicsID", bp::init<const simple_mpc::RobotModelHandler &, double, const KinodynamicsID::Settings>(
+                            bp::args("self", "model_handler", "control_dt", "settings")))
+        .def("setTarget", &KinodynamicsID::setTarget)
+        .def("solve", &solveKinoProxy)
+        .def("getAccelerations", &getAccelerationsKinoProxy);
+
+      bp::class_<CentroidalID::Settings, bp::bases<KinodynamicsID::Settings>>(
+        "CentroidalIDSettings", bp::init<>(bp::args("self")))
+        .def_readwrite("kp_com", &CentroidalID::Settings::kp_com)
+        .def_readwrite("kp_feet_tracking", &CentroidalID::Settings::kp_feet_tracking)
+        .def_readwrite("w_com", &CentroidalID::Settings::w_com)
+        .def_readwrite("w_feet_tracking", &CentroidalID::Settings::w_feet_tracking);
+
+      bp::class_<CentroidalID, boost::noncopyable>(
+        "CentroidalID", bp::init<const simple_mpc::RobotModelHandler &, double, const CentroidalID::Settings>(
+                          bp::args("self", "model_handler", "control_dt", "settings")))
+        .def("setTarget", &setTarget_CentroidalID)
+        .def("solve", &solveCentroidalProxy)
+        .def("getAccelerations", &getAccelerationsCentroidalProxy);
+    }
+  } // namespace python
+} // namespace simple_mpc