Use getFoobNb instead of calling size on various array

Author: earlaud

src/centroidal-dynamics.cpp

@@ -29,7 +29,7 @@ namespace simple_mpc
   , settings_(settings)
   {
     nx_ = 9;
-    nu_ = (int)model_handler_.getFeetFrameNames().size() * settings_.force_size;
+    nu_ = (int)model_handler_.getFeetNb() * settings_.force_size;
     control_ref_.resize(nu_);
     control_ref_.setZero();
     com_ref_.setZero();
@@ -107,7 +107,7 @@ namespace simple_mpc
 
   void CentroidalOCP::computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs)
   {
-    for (std::size_t i = 0; i < model_handler_.getFeetFrameNames().size(); i++)
+    for (std::size_t i = 0; i < model_handler_.getFeetNb(); i++)
     {
       if (settings_.force_size != force_refs.at(model_handler_.getFootFrameName(i)).size())
       {
@@ -124,7 +124,7 @@ namespace simple_mpc
     {
       throw std::runtime_error("Stage index exceeds stage vector size");
     }
-    if (pose_refs.size() != model_handler_.getFeetFrameNames().size())
+    if (pose_refs.size() != model_handler_.getFeetNb())
     {
       throw std::runtime_error("pose_refs size does not match number of end effectors");
     }

src/fulldynamics.cpp

@@ -215,7 +215,7 @@ namespace simple_mpc
 
   void FullDynamicsOCP::setReferencePoses(const std::size_t t, const std::map<std::string, pinocchio::SE3> & pose_refs)
   {
-    if (pose_refs.size() != model_handler_.getFeetFrameNames().size())
+    if (pose_refs.size() != model_handler_.getFeetNb())
     {
       throw std::runtime_error("pose_refs size does not match number of end effectors");
     }
@@ -275,7 +275,7 @@ namespace simple_mpc
   FullDynamicsOCP::setReferenceForces(const std::size_t t, const std::map<std::string, Eigen::VectorXd> & force_refs)
   {
     CostStack * cs = getCostStack(t);
-    if (force_refs.size() != model_handler_.getFeetFrameNames().size())
+    if (force_refs.size() != model_handler_.getFeetNb())
     {
       throw std::runtime_error("force_refs size does not match number of end effectors");
     }

src/inverse-dynamics/kinodynamics.cpp

@@ -18,7 +18,7 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
   const size_t nu = nv - 6;
 
   // Prepare foot contact tasks
-  const size_t n_contacts = model_handler_.getFeetFrameNames().size();
+  const size_t n_contacts = model_handler_.getFeetNb();
   const Eigen::Vector3d normal{0, 0, 1};
   const double weight = model_handler_.getMass() * 9.81;
   const double max_f = settings_.contact_weight_ratio_max * weight;

src/kinodynamics.cpp

@@ -31,7 +31,7 @@ namespace simple_mpc
   , settings_(settings)
   {
 
-    nu_ = nv_ - 6 + settings_.force_size * (int)model_handler_.getFeetFrameNames().size();
+    nu_ = nv_ - 6 + settings_.force_size * (int)model_handler_.getFeetNb();
     x0_ = model_handler_.getReferenceState();
     control_ref_.resize(nu_);
     control_ref_.setZero();
@@ -170,7 +170,7 @@ namespace simple_mpc
 
   void KinodynamicsOCP::setReferencePoses(const std::size_t t, const std::map<std::string, pinocchio::SE3> & pose_refs)
   {
-    if (pose_refs.size() != model_handler_.getFeetFrameNames().size())
+    if (pose_refs.size() != model_handler_.getFeetNb())
     {
       throw std::runtime_error("pose_refs size does not match number of end effectors");
     }
@@ -228,7 +228,7 @@ namespace simple_mpc
 
   void KinodynamicsOCP::computeControlFromForces(const std::map<std::string, Eigen::VectorXd> & force_refs)
   {
-    for (std::size_t i = 0; i < model_handler_.getFeetFrameNames().size(); i++)
+    for (std::size_t i = 0; i < model_handler_.getFeetNb(); i++)
     {
       if (settings_.force_size != force_refs.at(model_handler_.getFootFrameName(i)).size())
       {

src/ocp-handler.cpp

@@ -106,7 +106,7 @@ namespace simple_mpc
 
     Eigen::VectorXd force_ref(force_size);
     force_ref.setZero();
-    force_ref[2] = -model_handler_.getMass() * gravity / (double)model_handler_.getFeetFrameNames().size();
+    force_ref[2] = -model_handler_.getMass() * gravity / (double)model_handler_.getFeetNb();
 
     std::map<std::string, bool> contact_phase;
     std::map<std::string, pinocchio::SE3> contact_pose;