From 45ca5968d91f6c8654f15c221e01453532c00c4b Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Mon, 18 Oct 2021 13:42:48 +0200
Subject: [PATCH] Disable heuristic if gait isStatic is true
---
src/FootstepPlanner.cpp | 19 +++++++++++--------
src/Joystick.cpp | 1 -
2 files changed, 11 insertions(+), 9 deletions(-)
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index 1f07efa0..e2649bf5 100644
--- a/src/FootstepPlanner.cpp
+++ b/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);
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index 83b36ed6..9a32db54 100644
--- a/src/Joystick.cpp
+++ b/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_;
--
GitLab