Propper use of tail and head for static size slices

Author: earlaud

include/simple-mpc/inverse-dynamics.hpp

@@ -157,8 +157,8 @@ namespace simple_mpc
       const pinocchio::SE3 base_pose(
         pinocchio::SE3::Quaternion(q_target[6], q_target[3], q_target[4], q_target[5]), q_target.head<3>());
       tsid::math::SE3ToVector(base_pose, sampleBase_.pos);
-      sampleBase_.setDerivative(v_target.head(6));
-      sampleBase_.setSecondDerivative(a_target.head(6));
+      sampleBase_.setDerivative(v_target.head<6>());
+      sampleBase_.setSecondDerivative(a_target.head<6>());
       baseTask_->setReference(sampleBase_);
 
       // Foot contacts

src/centroidal-dynamics.cpp

@@ -219,8 +219,8 @@ namespace simple_mpc
     QuadraticResidualCost * qca = cs->getComponent<QuadraticResidualCost>("angular_mom_cost");
     AngularMomentumResidual * cfa = qca->getResidual<AngularMomentumResidual>();
 
-    v.head(3) = cfm->getReference() / model_handler_.getMass();
-    v.tail(3) = cfa->getReference() / model_handler_.getMass();
+    v.head<3>() = cfm->getReference() / model_handler_.getMass();
+    v.tail<3>() = cfa->getReference() / model_handler_.getMass();
     return v;
   }
 
@@ -234,8 +234,8 @@ namespace simple_mpc
     QuadraticResidualCost * qca = cs->getComponent<QuadraticResidualCost>("angular_mom_cost");
     AngularMomentumResidual * cfa = qca->getResidual<AngularMomentumResidual>();
 
-    cfm->setReference(velocity_base.head(3) * model_handler_.getMass());
-    cfa->setReference(velocity_base.tail(3) * model_handler_.getMass());
+    cfm->setReference(velocity_base.head<3>() * model_handler_.getMass());
+    cfa->setReference(velocity_base.tail<3>() * model_handler_.getMass());
   }
 
   const Eigen::VectorXd CentroidalOCP::getPoseBase(const std::size_t t)
@@ -252,7 +252,7 @@ namespace simple_mpc
     CostStack * cs = getCostStack(t);
     QuadraticResidualCost * qrc = cs->getComponent<QuadraticResidualCost>("com_cost");
     CentroidalCoMResidual * cfr = qrc->getResidual<CentroidalCoMResidual>();
-    com_ref_ = pose_base.head(3);
+    com_ref_ = pose_base.head<3>();
     cfr->setReference(com_ref_);
   }
 
@@ -292,14 +292,14 @@ namespace simple_mpc
   void CentroidalOCP::setReferenceState(const std::size_t t, const ConstVectorRef & x_ref)
   {
     assert(x_ref.size() == 9 && "x_ref not of the right size");
-    setPoseBase(t, x_ref.head(3));
-    setVelocityBase(t, x_ref.tail(6));
+    setPoseBase(t, x_ref.head<3>());
+    setVelocityBase(t, x_ref.tail<6>());
   }
 
   const ConstVectorRef CentroidalOCP::getReferenceState(const std::size_t t)
   {
-    x0_.head(3) = getPoseBase(t);
-    x0_.tail(6) = getVelocityBase(t);
+    x0_.head<3>() = getPoseBase(t);
+    x0_.tail<6>() = getVelocityBase(t);
     return x0_;
   }
 

src/fulldynamics.cpp

@@ -343,7 +343,7 @@ namespace simple_mpc
   {
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
-    return qc->getTarget().head(7);
+    return qc->getTarget().head<7>();
   };
 
   void FullDynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base)
@@ -355,7 +355,7 @@ namespace simple_mpc
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
     x0_ = getReferenceState(t);
-    x0_.head(7) = pose_base;
+    x0_.head<7>() = pose_base;
     qc->setTarget(x0_);
   }
 

src/kinodynamics.cpp

@@ -287,7 +287,7 @@ namespace simple_mpc
   {
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
-    return qc->getTarget().head(7);
+    return qc->getTarget().head<7>();
   };
 
   void KinodynamicsOCP::setPoseBase(const std::size_t t, const ConstVectorRef & pose_base)
@@ -299,7 +299,7 @@ namespace simple_mpc
     CostStack * cs = getCostStack(t);
     QuadraticStateCost * qc = cs->getComponent<QuadraticStateCost>("state_cost");
     x0_ = getReferenceState(t);
-    x0_.head(7) = pose_base;
+    x0_.head<7>() = pose_base;
     qc->setTarget(x0_);
   }
 

src/mpc.cpp

@@ -288,9 +288,9 @@ namespace simple_mpc
         -(data_handler_->getRefFootPose(name).translation()[1] - data_handler_->getBaseFramePose().translation()[1]);
       twist_vect_[1] =
         data_handler_->getRefFootPose(name).translation()[0] - data_handler_->getBaseFramePose().translation()[0];
-      next_pose_.head(2) = data_handler_->getRefFootPose(name).translation().head(2);
-      next_pose_.head(2) += (velocity_base_.head(2) + velocity_base_[5] * twist_vect_)
-                            * (settings_.T_fly + settings_.T_contact) * settings_.timestep;
+      next_pose_.head<2>() = data_handler_->getRefFootPose(name).translation().head<2>();
+      next_pose_.head<2>() += (velocity_base_.head<2>() + velocity_base_[5] * twist_vect_)
+                              * (settings_.T_fly + settings_.T_contact) * settings_.timestep;
       next_pose_[2] = data_handler_->getFootPose(name).translation()[2];
 
       foot_trajectories_.updateTrajectory(

src/ocp-handler.cpp

@@ -132,7 +132,7 @@ namespace simple_mpc
 
     if (terminal_constraint)
     {
-      createTerminalConstraint(x0.head(3));
+      createTerminalConstraint(x0.head<3>());
     }
   }
 } // namespace simple_mpc

src/robot-handler.cpp

@@ -121,9 +121,9 @@ namespace simple_mpc
   RobotDataHandler::CentroidalStateVector RobotDataHandler::getCentroidalState() const
   {
     RobotDataHandler::CentroidalStateVector x_centroidal;
-    x_centroidal.head(3) = data_.com[0];
-    x_centroidal.segment(3, 3) = data_.hg.linear();
-    x_centroidal.tail(3) = data_.hg.angular();
+    x_centroidal.head<3>() = data_.com[0];
+    x_centroidal.segment<3>(3) = data_.hg.linear();
+    x_centroidal.tail<3>() = data_.hg.angular();
     return x_centroidal;
   }