diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp index 1f07efa069084534459b8f864711db17c0b6fe1b..e2649bf5e94bf4ae8ff1a03f27e45a015eeec613 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 83b36ed64aba6f1671b7a3b317b30a9b7fb5baaa..9a32db547672934dbc88ecbee58208961bca113a 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_;