bugfix: use absolute values for adjustment. Initialize pid controller…

…s of walk_stabilizer correctly again
bit-bots · Dec 18, 2024 · 1eef281 · 1eef281
1 parent f7fda8a
commit 1eef281
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 9 deletions.
diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_stabilizer.hpp
@@ -1,6 +1,7 @@
 #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_STABILIZER_H_
 #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_STABILIZER_H_
 
+#include <math.h>
 #include <rot_conv/rot_conv.h>
 
 #include <Eigen/Geometry>

diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
@@ -177,13 +177,7 @@ void WalkNode::publish_debug() { visualizer_.publishDebug(current_stabilized_res
 bitbots_msgs::msg::JointCommand WalkNode::step(double dt) {
   WalkRequest request(current_request_);
 
-  // update walk engine response
-  if (got_new_goals_) {
-    // got_new_goals_ = false;
-  }
-
-  // Apply capture step
-
+  // Apply step length adjustment
   RCLCPP_WARN(node_->get_logger(), "Original step size: %f, %f", request.linear_orders[0], request.linear_orders[1]);
   request = stabilizer_.adjust_step_length(
       request, current_trunk_fused_roll_, current_trunk_fused_pitch_, config_.node.step_length.pitch.threshold,

diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp
@@ -10,6 +10,8 @@ WalkStabilizer::WalkStabilizer(rclcpp::Node::SharedPtr node)
       pid_step_length_adjustment_roll_(node, "node.step_length.roll") {
   pid_step_length_adjustment_pitch_.initPid();
   pid_step_length_adjustment_roll_.initPid();
+  pid_trunk_fused_pitch_.initPid();
+  pid_trunk_fused_roll_.initPid();
 
   reset();
 }
@@ -29,10 +31,10 @@ WalkRequest WalkStabilizer::adjust_step_length(WalkRequest request, const double
   RCLCPP_WARN(node_->get_logger(), "Adjustment pitch, roll: %f, %f", adjustment_pitch, adjustment_roll);
   // adapt step length values based on PID controllers
   // we use a threshold to avoid unneeded steps
-  if (adjustment_pitch >= pitch_threshold) {
+  if (fabs(adjustment_pitch) >= pitch_threshold) {
     request.linear_orders[0] += adjustment_pitch;
   }
-  if (adjustment_roll >= roll_threshold) {
+  if (fabs(adjustment_roll) >= roll_threshold) {
     request.linear_orders[1] += adjustment_roll;
   }
   return request;