Disable heuristic if gait isStatic is true

src/FootstepPlanner.cpp

@@ -165,16 +165,19 @@ void FootstepPlanner::computeNextFootstep(int i, int j, Vector6 const& b_v, Vect
 
   double t_stance = gait_->getPhaseDuration(i, j, 1.0);  // 1.0 for stance phase
 
-  // Add symmetry term
-  nextFootstep_.col(j) = t_stance * 0.5 * b_v.head(3);
+  if (!gait_->getIsStatic())
+  {
+    // Add symmetry term
+    nextFootstep_.col(j) = t_stance * 0.5 * b_v.head(3);
 
-  // Add feedback term
-  nextFootstep_.col(j) += params_->k_feedback * (b_v.head(3) - b_vref.head(3));
+    // Add feedback term
+    nextFootstep_.col(j) += params_->k_feedback * (b_v.head(3) - b_vref.head(3));
 
-  // Add centrifugal term
-  Vector3 cross;
-  cross << b_v(1) * b_vref(5) - b_v(2) * b_vref(4), b_v(2) * b_vref(3) - b_v(0) * b_vref(5), 0.0;
-  nextFootstep_.col(j) += 0.5 * std::sqrt(h_ref / g) * cross;
+    // Add centrifugal term
+    Vector3 cross;
+    cross << b_v(1) * b_vref(5) - b_v(2) * b_vref(4), b_v(2) * b_vref(3) - b_v(0) * b_vref(5), 0.0;
+    nextFootstep_.col(j) += 0.5 * std::sqrt(h_ref / g) * cross;
+  }
 
   // Legs have a limited length so the deviation has to be limited
   nextFootstep_(0, j) = std::min(nextFootstep_(0, j), L);

src/Joystick.cpp

@@ -211,7 +211,6 @@ void Joystick::update_v_ref_gamepad(int k, bool gait_is_static, Vector6 h_v)
 
   // Heavily filtered joystick velocity to be used as a trigger for the switch trot/static
   v_ref_heavy_filter_ = gp_alpha_vel_heavy_filter * v_gp_ + (1 - gp_alpha_vel_heavy_filter) * v_ref_heavy_filter_;
-  std::cout << v_ref_heavy_filter_.transpose() << std::endl;
 
   // Low pass filter to slow down the changes of position when moving the joysticks
   p_ref_ = gp_alpha_pos * p_gp_ + (1 - gp_alpha_pos) * p_ref_;