Make setReferenceFootPlacement use foot index instead of name for coherence and performance (if needed)

Author: EtienneAr

benchmark/go2.cpp

@@ -44,10 +44,10 @@ int main()
   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);
-  model_handler.setFootReferencePlacement("RR_foot", ref_RR_foot);
+  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);
 

benchmark/talos.cpp

@@ -76,9 +76,11 @@ int main()
   model_handler.addQuadFoot("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.)));
+    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);
 

include/simple-mpc/robot-handler.hpp

@@ -77,12 +77,12 @@ namespace simple_mpc
       const ContactPointsMatrix & contactPoints);
 
     /**
-     * @brief Update the reference placement of the foot wrt to the frame it is attached to
+     * @brief Update the placement of the foot reference frame wrt to the joint/frame it is attached to
      *
      * @param foot_name Foot frame name
      * @param refMfoot Placement in the local frame
      */
-    void setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot);
+    void setFootReferencePlacement(size_t foot_nb, const SE3 & refMfoot);
 
     /**
      * @brief Perform a finite difference on the sates.
@@ -107,7 +107,7 @@ namespace simple_mpc
 
     size_t getFeetNb() const
     {
-      return size(feet_ids_);
+      return size(feet_frame_ids_);
     }
 
     size_t getFootNb(const std::string & foot_frame_name) const
@@ -128,7 +128,7 @@ namespace simple_mpc
 
     const std::vector<FrameIndex> & getFeetFrameIds() const
     {
-      return feet_ids_;
+      return feet_frame_ids_;
     }
 
     const std::vector<std::string> & getFeetFrameNames() const
@@ -148,12 +148,12 @@ namespace simple_mpc
 
     FrameIndex getFootFrameId(size_t foot_nb) const
     {
-      return feet_ids_.at(foot_nb);
+      return feet_frame_ids_.at(foot_nb);
     }
 
     FrameIndex getFootRefFrameId(size_t foot_nb) const
     {
-      return feet_ref_frame_ids.at(foot_nb);
+      return feet_ref_frame_ids_.at(foot_nb);
     }
 
     double getMass() const
@@ -211,12 +211,12 @@ namespace simple_mpc
     /**
      * @brief Ids of the frames to be in contact with the environment
      */
-    std::vector<FrameIndex> feet_ids_;
+    std::vector<FrameIndex> feet_frame_ids_;
 
     /**
      * @brief Ids of the frames that are reference position for the feet
      */
-    std::vector<FrameIndex> feet_ref_frame_ids;
+    std::vector<FrameIndex> feet_ref_frame_ids_;
 
     /**
      * @brief Base frame id

src/robot-handler.cpp

@@ -26,8 +26,10 @@ namespace simple_mpc
 
   void RobotModelHandler::addFootFrames(const std::string & foot_name, const std::string & reference_frame_name)
   {
+    const size_t new_foot_index = getFeetNb();
+
     feet_frame_names_.push_back(foot_name);
-    feet_ids_.push_back(model_.getFrameId(foot_name));
+    feet_frame_ids_.push_back(model_.getFrameId(foot_name));
 
     // Create reference frame
     FrameIndex reference_frame_id = model_.getFrameId(reference_frame_name);
@@ -38,7 +40,7 @@ namespace simple_mpc
     auto frame_id = model_.addFrame(new_frame);
 
     // Save foot id
-    feet_ref_frame_ids.push_back(frame_id);
+    feet_ref_frame_ids_.push_back(frame_id);
 
     // Set placement to default value
     pinocchio::Data data(model_);
@@ -47,7 +49,7 @@ namespace simple_mpc
 
     const pinocchio::SE3 default_placement = data.oMf[reference_frame_id].actInv(data.oMf[frame_id]);
 
-    setFootReferencePlacement(foot_name, default_placement);
+    setFootReferencePlacement(new_foot_index, default_placement);
   }
 
   size_t RobotModelHandler::addPointFoot(const std::string & foot_name, const std::string & reference_frame_name)
@@ -68,9 +70,9 @@ namespace simple_mpc
     return foot_nb;
   }
 
-  void RobotModelHandler::setFootReferencePlacement(const std::string & foot_name, const SE3 & refMfoot)
+  void RobotModelHandler::setFootReferencePlacement(size_t foot_nb, const SE3 & refMfoot)
   {
-    model_.frames[model_.getFrameId(foot_name + "_ref", pinocchio::OP_FRAME)].placement = refMfoot;
+    model_.frames[feet_ref_frame_ids_.at(foot_nb)].placement = refMfoot;
   }
 
   Eigen::VectorXd RobotModelHandler::difference(const ConstVectorRef & x1, const ConstVectorRef & x2) const

tests/robot_handler.cpp

@@ -46,10 +46,10 @@ BOOST_AUTO_TEST_CASE(model_handler)
   RobotModelHandler model_handler(model, default_conf_name, base_frame);
 
   // Add feet
-  for (size_t i = 0; i < feet_names.size(); i++)
+  for (size_t foot_nb = 0; foot_nb < feet_names.size(); foot_nb++)
   {
-    model_handler.addPointFoot(feet_names.at(i), base_frame);
-    model_handler.setFootReferencePlacement(feet_names.at(i), feet_refs.at(i));
+    model_handler.addPointFoot(feet_names.at(foot_nb), base_frame);
+    model_handler.setFootReferencePlacement(foot_nb, feet_refs.at(foot_nb));
   }
 
   /*********/

tests/test_utils.cpp

@@ -93,13 +93,13 @@ RobotModelHandler getSoloHandler()
   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.getFootNb("FR_FOOT"), SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, -0.1, 0.)));
   model_handler.setFootReferencePlacement(
-    "FL_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, 0.1, 0.)));
+    model_handler.getFootNb("FL_FOOT"), SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(0.1, 0.1, 0.)));
   model_handler.setFootReferencePlacement(
-    "HR_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, -0.1, 0.)));
+    model_handler.getFootNb("HR_FOOT"), SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, -0.1, 0.)));
   model_handler.setFootReferencePlacement(
-    "HL_FOOT", SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, 0.1, 0.)));
+    model_handler.getFootNb("HL_FOOT"), SE3(Eigen::Quaternion(0., 0., 0., 1.), Eigen::Vector3d(-0.1, 0.1, 0.)));
 
   return model_handler;
 }