Switch from std::vector to Eigen Matrix to avoid runtime in FootTrajectoryGenerator

include/qrw/FootTrajectoryGenerator.hpp

@@ -84,7 +84,7 @@ class FootTrajectoryGenerator {
   double lockTime_;   // Target lock before the touchdown
   double vertTime_;   // Duration during which feet move only along Z when taking off and landing
 
-  std::vector<int> feet;  // Column indexes of feet currently in swing phase
+  Eigen::Matrix<int, 1, 4> feet;  // Column indexes of feet currently in swing phase
   Vector4 t0s;            // Elapsed time since the start of the swing phase movement
   Vector4 t_swing;        // Swing phase duration for each foot
 

src/FootTrajectoryGenerator.cpp

@@ -8,7 +8,7 @@ FootTrajectoryGenerator::FootTrajectoryGenerator()
       k_mpc(0),
       maxHeight_(0.0),
       lockTime_(0.0),
-      feet(),
+      feet(Eigen::Matrix<int, 1, 4>::Zero()),
       t0s(Vector4::Zero()),
       t_swing(Vector4::Zero()),
       targetFootstep_(Matrix34::Zero()),
@@ -183,34 +183,40 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
 
 void FootTrajectoryGenerator::update(int k, MatrixN const &targetFootstep) {
   if ((k % k_mpc) == 0) {
-    // Indexes of feet in swing phase
-    feet.clear();
-    for (int i = 0; i < 4; i++) {
-      if (gait_->getCurrentGait()(0, i) == 0) feet.push_back(i);
-    }
+    // Status of feet
+    feet = gait_->getCurrentGait().row(0).cast <int> ();
+
     // If no foot in swing phase
-    if (feet.size() == 0) return;
+    if (feet.sum() == 4) return;
 
     // For each foot in swing phase get remaining duration of the swing phase
-    for (int j = 0; j < (int)feet.size(); j++) {
-      int i = feet[j];
-      t_swing[i] = gait_->getPhaseDuration(0, feet[j], 0.0);  // 0.0 for swing phase
-      double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
-      t0s[i] = std::max(0.0, value);
+    for (int i = 0; i < 4; i++) {
+      if (feet(0, i) == 0)
+      {
+        t_swing[i] = gait_->getPhaseDuration(0, i, 0.0);  // 0.0 for swing phase
+        double value = t_swing[i] - (gait_->getRemainingTime() * k_mpc - ((k + 1) % k_mpc)) * dt_wbc - dt_wbc;
+        t0s[i] = std::max(0.0, value);
+      }
     }
   } else {
     // If no foot in swing phase
-    if (feet.size() == 0) return;
+    if (feet.sum() == 4) return;
 
     // Increment of one time step for feet in swing phase
-    for (int i = 0; i < (int)feet.size(); i++) {
-      double value = t0s[feet[i]] + dt_wbc;
-      t0s[feet[i]] = std::max(0.0, value);
+    for (int i = 0; i < 4; i++) {
+      if (feet(0, i) == 0)
+      {
+        double value = t0s[i] + dt_wbc;
+        t0s[i] = std::max(0.0, value);
+      }
     }
   }
 
-  for (int i = 0; i < (int)feet.size(); i++) {
-    updateFootPosition(feet[i], targetFootstep.col(feet[i]));
+  for (int i = 0; i < 4; i++) {
+    if (feet(0, i) == 0)
+    {
+      updateFootPosition(i, targetFootstep.col(i));
+    }
   }
   return;
 }