Add 6D foot in model handler

earlaud · earlaud · commit b3e583a1bc76 · 2025-07-31T16:08:38.000+02:00
diff --git a/benchmark/go2.cpp b/benchmark/go2.cpp
@@ -40,10 +40,10 @@ 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.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("FL_foot", ref_FL_foot);
   model_handler.setFootReferencePlacement("FR_foot", ref_FR_foot);
   model_handler.setFootReferencePlacement("RL_foot", ref_RL_foot);
diff --git a/benchmark/talos.cpp b/benchmark/talos.cpp
@@ -72,8 +72,8 @@ 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);
+  model_handler.add6DFoot("left_sole_link", base_joint);
+  model_handler.add6DFoot("right_sole_link", base_joint);
 
   model_handler.setFootReferencePlacement(
     "left_sole_link", pinocchio::SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
diff --git a/bindings/expose-robot-handler.cpp b/bindings/expose-robot-handler.cpp
@@ -23,7 +23,8 @@ namespace simple_mpc
       bp::class_<RobotModelHandler>(
         "RobotModelHandler", bp::init<const pinocchio::Model &, const std::string &, const std::string &>(
                                bp::args("self", "model", "reference_configuration_name", "base_frame_name")))
-        .def("addFoot", &RobotModelHandler::addFoot)
+        .def("addPointFoot", &RobotModelHandler::addPointFoot)
+        .def("add6DFoot", &RobotModelHandler::add6DFoot)
         .def("setFootReferencePlacement", &RobotModelHandler::setFootReferencePlacement)
         .def("difference", &RobotModelHandler::difference)
         .def("getBaseFrameName", &RobotModelHandler::getBaseFrameName)
diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py
@@ -23,10 +23,10 @@
 # Create Model and Data handler
 model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
 
-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.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("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py
@@ -14,10 +14,10 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(robot_wrapper.model, "standing", base_joint_name)
-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.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("FL_foot", pin.XYZQUATToSE3(np.array([ 0.17, 0.15, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("FR_foot", pin.XYZQUATToSE3(np.array([ 0.17,-0.15, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("RL_foot", pin.XYZQUATToSE3(np.array([-0.24, 0.15, 0.0, 0,0,0,1])))
diff --git a/examples/talos_centroidal.py b/examples/talos_centroidal.py
@@ -15,8 +15,8 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addFoot("left_sole_link",  base_joint_name)
-model_handler.addFoot("right_sole_link", base_joint_name)
+model_handler.add6DFoot("left_sole_link",  base_joint_name)
+model_handler.add6DFoot("right_sole_link", base_joint_name)
 model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 
diff --git a/examples/talos_fulldynamics.py b/examples/talos_fulldynamics.py
@@ -22,8 +22,8 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addFoot("left_sole_link",  base_joint_name)
-model_handler.addFoot("right_sole_link", base_joint_name)
+model_handler.add6DFoot("left_sole_link",  base_joint_name)
+model_handler.add6DFoot("right_sole_link", base_joint_name)
 model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 
diff --git a/examples/talos_kinodynamics.py b/examples/talos_kinodynamics.py
@@ -15,8 +15,8 @@
 
 # Create Model and Data handler
 model_handler = RobotModelHandler(rmodel, reference_configuration_name, base_joint_name)
-model_handler.addFoot("left_sole_link",  base_joint_name)
-model_handler.addFoot("right_sole_link", base_joint_name)
+model_handler.add6DFoot("left_sole_link",  base_joint_name)
+model_handler.add6DFoot("right_sole_link", base_joint_name)
 model_handler.setFootReferencePlacement("left_sole_link", pin.XYZQUATToSE3(np.array([ 0.0, 0.1, 0.0, 0,0,0,1])))
 model_handler.setFootReferencePlacement("right_sole_link", pin.XYZQUATToSE3(np.array([ 0.0,-0.1, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
diff --git a/include/simple-mpc/inverse-dynamics.hpp b/include/simple-mpc/inverse-dynamics.hpp
@@ -83,7 +83,7 @@ namespace simple_mpc
     tsid::InverseDynamicsFormulationAccForce formulation_;
 
     std::vector<bool> active_tsid_contacts_;
-    std::vector<tsid::contacts::ContactPoint> tsid_contacts;
+    std::vector<std::shared_ptr<tsid::contacts::ContactBase>> tsid_contacts;
     std::shared_ptr<tsid::tasks::TaskJointPosture> postureTask_;
     std::shared_ptr<tsid::tasks::TaskSE3Equality> baseTask_;
     std::shared_ptr<tsid::tasks::TaskJointPosVelAccBounds> boundsTask_;
diff --git a/include/simple-mpc/robot-handler.hpp b/include/simple-mpc/robot-handler.hpp
@@ -27,6 +27,13 @@ namespace simple_mpc
    */
   struct RobotModelHandler
   {
+    enum FootType
+    {
+      POINT,
+      SIX_D
+    };
+    typedef Eigen::Matrix<double, 4, 3> ContactPointsMatrix;
+
   private:
     /**
      * @brief Model to be used by ocp
@@ -49,6 +56,16 @@ namespace simple_mpc
      */
     std::vector<std::string> feet_names_;
 
+    /**
+     * @brief Is the foot a contact point or a 6D contact
+     */
+    std::vector<FootType> feet_types_;
+
+    /**
+     * @brief List of contact points for each 6D feets
+     */
+    std::map<size_t, Eigen::Matrix<double, 4, 3>> feet_contact_points_;
+
     /**
      * @brief Ids of the frames to be in contact with the environment
      */
@@ -77,15 +94,34 @@ namespace simple_mpc
       const Model & model, const std::string & reference_configuration_name, const std::string & base_frame_name);
 
     /**
-     * @brief
+     * @brief Create a point foot, that can apply 3D force to the ground. (in comparison to 6D foot)
      *
      * @param foot_name Frame name that will be used a a foot
      * @param reference_frame_name Frame to which the foot reference
      * frame will be attached.
      *
+     * @return the foot number
+     *
      * @note The foot placement will be set by default using the reference configuration of the RobotModelHandler
      */
-    FrameIndex addFoot(const std::string & foot_name, const std::string & reference_frame_name);
+    size_t addPointFoot(const std::string & foot_name, const std::string & reference_frame_name);
+
+    /**
+     * @brief Create a point foot, that can apply 6D wrench to the ground. (in comparison to point foot)
+     *
+     * @param foot_name Frame name that will be used a a foot
+     * @param reference_frame_name Frame to which the foot reference
+     * frame will be attached.
+     * @param contactPoints 3D positions (in the local frame) of the foot 4 extremum point
+     *
+     * @return the foot number
+     *
+     * @note The foot placement will be set by default using the reference configuration of the RobotModelHandler
+     */
+    size_t add6DFoot(
+      const std::string & foot_name,
+      const std::string & reference_frame_name,
+      const ContactPointsMatrix & contactPoints);
 
     /**
      * @brief Update the reference placement of the foot wrt to the frame it is attached to
@@ -115,6 +151,16 @@ namespace simple_mpc
       return size_t(std::find(feet_names_.begin(), feet_names_.end(), foot_name) - feet_names_.begin());
     }
 
+    FootType getFootType(size_t i) const
+    {
+      return feet_types_[i];
+    }
+
+    const ContactPointsMatrix & get6DFootContactPoints(size_t i) const
+    {
+      return feet_contact_points_.at(i);
+    }
+
     const std::vector<FrameIndex> & getFeetIds() const
     {
       return feet_ids_;
@@ -159,6 +205,16 @@ namespace simple_mpc
     {
       return model_;
     }
+
+  private:
+    /**
+     * @brief Common operation to perform to add a foot (of any type) : Create the reference frame and set it default
+     * pose using the default pose of the robot
+     *
+     * @param foot_name Name of the frame foot
+     * @param reference_frame_name Name of the frame where the reference frame of the foot will be attached
+     */
+    void addFootFrames(const std::string & foot_name, const std::string & reference_frame_name);
   };
 
   class RobotDataHandler
diff --git a/src/inverse-dynamics.cpp b/src/inverse-dynamics.cpp
@@ -1,4 +1,6 @@
 #include <simple-mpc/inverse-dynamics.hpp>
+#include <tsid/contacts/contact-6d.hpp>
+#include <tsid/contacts/contact-point.hpp>
 
 using namespace simple_mpc;
 
@@ -15,7 +17,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
   const size_t nv = model.nv;
   const size_t nu = nv - 6;
 
-  // Prepare contact point task
+  // Prepare foot contact tasks
   const size_t n_contacts = model_handler_.getFeetNames().size();
   const Eigen::Vector3d normal{0, 0, 1};
   const double weight = model_handler_.getMass() * 9.81;
@@ -24,13 +26,30 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
   for (int i = 0; i < n_contacts; i++)
   {
     const std::string frame_name = model_handler.getFootName(i);
-    // Create contact point
-    tsid::contacts::ContactPoint & contact_point =
-      tsid_contacts.emplace_back(frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f);
-    // Set contact parameters
-    contact_point.Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
-    contact_point.Kd(2.0 * contact_point.Kp().cwiseSqrt());
-    contact_point.useLocalFrame(false);
+    switch (model_handler.getFootType(i))
+    {
+    case RobotModelHandler::FootType::POINT: {
+      auto contact_point = std::make_shared<tsid::contacts::ContactPoint>(
+        frame_name, robot_, frame_name, normal, settings_.friction_coefficient, min_f, max_f);
+      contact_point->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
+      contact_point->Kd(2.0 * contact_point->Kp().cwiseSqrt());
+      contact_point->useLocalFrame(false);
+      tsid_contacts.push_back(contact_point);
+      break;
+    }
+    case RobotModelHandler::FootType::SIX_D: {
+      auto contact_6D = std::make_shared<tsid::contacts::Contact6d>(
+        frame_name, robot_, frame_name, model_handler_.get6DFootContactPoints(i), normal,
+        settings_.friction_coefficient, min_f, max_f);
+      contact_6D->Kp(settings_.kp_contact * Eigen::VectorXd::Ones(3));
+      contact_6D->Kd(2.0 * contact_6D->Kp().cwiseSqrt());
+      tsid_contacts.push_back(contact_6D);
+      break;
+    }
+    default: {
+      assert(false);
+    }
+    }
     // By default contact is not active (will be by setTarget)
     active_tsid_contacts_.push_back(false);
   }
@@ -117,9 +136,23 @@ 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, 1);
+      }
+      switch (model_handler_.getFootType(i))
+      {
+      case RobotModelHandler::FootType::POINT: {
+        std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[i])->setForceReference(f_target.row(i));
+        break;
+      }
+      case RobotModelHandler::FootType::SIX_D: {
+        std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[i])->setForceReference(f_target.row(i));
+        break;
       }
-      tsid_contacts[i].setForceReference(f_target.row(i));
+      default: {
+        assert(false);
+      }
+      }
+      tsid_contacts[i];
       active_tsid_contacts_[i] = true;
     }
     else
@@ -146,7 +179,22 @@ void KinodynamicsID::solve(
   {
     if (active_tsid_contacts_[i])
     {
-      tsid_contacts[i].setReference(data_handler_.getFootPose(i));
+      switch (model_handler_.getFootType(i))
+      {
+      case RobotModelHandler::FootType::POINT: {
+        std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[i])
+          ->setReference(data_handler_.getFootPose(i));
+        break;
+      }
+      case RobotModelHandler::FootType::SIX_D: {
+        std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[i])
+          ->setReference(data_handler_.getFootPose(i));
+        break;
+      }
+      default: {
+        assert(false);
+      }
+      }
     }
   }
 
diff --git a/src/robot-handler.cpp b/src/robot-handler.cpp
@@ -24,7 +24,7 @@ namespace simple_mpc
     mass_ = pinocchio::computeTotalMass(model_);
   }
 
-  FrameIndex RobotModelHandler::addFoot(const std::string & foot_name, const std::string & reference_frame_name)
+  void RobotModelHandler::addFootFrames(const std::string & foot_name, const std::string & reference_frame_name)
   {
     feet_names_.push_back(foot_name);
     feet_ids_.push_back(model_.getFrameId(foot_name));
@@ -48,8 +48,24 @@ namespace simple_mpc
     const pinocchio::SE3 default_placement = data.oMf[reference_frame_id].actInv(data.oMf[frame_id]);
 
     setFootReferencePlacement(foot_name, default_placement);
+  }
 
-    return frame_id;
+  size_t RobotModelHandler::addPointFoot(const std::string & foot_name, const std::string & reference_frame_name)
+  {
+    addFootFrames(foot_name, reference_frame_name);
+    feet_types_.push_back(FootType::POINT);
+    const size_t foot_nb = feet_types_.size();
+    return foot_nb;
+  }
+
+  size_t RobotModelHandler::add6DFoot(
+    const std::string & foot_name, const std::string & reference_frame_name, const ContactPointsMatrix & contactPoints)
+  {
+    addFootFrames(foot_name, reference_frame_name);
+    feet_types_.push_back(FootType::SIX_D);
+    const size_t foot_nb = feet_types_.size();
+    feet_contact_points_.insert({foot_nb, contactPoints});
+    return foot_nb;
   }
 
   void RobotModelHandler::setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot)
diff --git a/tests/robot_handler.cpp b/tests/robot_handler.cpp
@@ -48,7 +48,7 @@ BOOST_AUTO_TEST_CASE(model_handler)
   // Add feet
   for (size_t i = 0; i < feet_names.size(); i++)
   {
-    model_handler.addFoot(feet_names.at(i), base_frame);
+    model_handler.addPointFoot(feet_names.at(i), base_frame);
     model_handler.setFootReferencePlacement(feet_names.at(i), feet_refs.at(i));
   }
 
@@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(model_handler)
   // Check that the default foot pose actually match the reference configuration
   {
     const std::string foot_name = "FR_calf";
-    model_handler.addFoot(foot_name, base_frame);
+    model_handler.addPointFoot(foot_name, base_frame);
     // Do not set foot reference placement to check the default one
 
     RobotDataHandler data_handler(model_handler);
diff --git a/tests/test_utils.cpp b/tests/test_utils.cpp
@@ -66,8 +66,8 @@ RobotModelHandler getTalosModelHandler()
   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);
+  model_handler.addPointFoot("left_sole_link", base_joint);
+  model_handler.addPointFoot("right_sole_link", base_joint);
   model_handler.setFootReferencePlacement(
     "left_sole_link", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0., 0.1, 0.)));
   model_handler.setFootReferencePlacement(
@@ -91,10 +91,10 @@ RobotModelHandler getSoloHandler()
   RobotModelHandler model_handler(model, "straight_standing", base_joint);
 
   // Add feet
-  model_handler.addFoot("FR_FOOT", base_joint);
-  model_handler.addFoot("FL_FOOT", base_joint);
-  model_handler.addFoot("HR_FOOT", base_joint);
-  model_handler.addFoot("HL_FOOT", base_joint);
+  model_handler.addPointFoot("FR_FOOT", base_joint);
+  model_handler.addPointFoot("FL_FOOT", base_joint);
+  model_handler.addPointFoot("HR_FOOT", base_joint);
+  model_handler.addPointFoot("HL_FOOT", base_joint);
   model_handler.setFootReferencePlacement(
     "FR_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, -0.1, 0.)));
   model_handler.setFootReferencePlacement(
