Changes to the way parameters are handled. Also Setups regarding ML

bit-bots · Jan 8, 2025 · ee9b43e · ee9b43e
1 parent 1eef281
commit ee9b43e
Show file tree

Hide file tree

Showing 6 changed files with 42 additions and 12 deletions.
diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml
@@ -75,7 +75,7 @@ walking:
       phase_reset:
         min_phase: 0.90
         foot_pressure:
-          active: True
+          active: False
           ground_min_pressure: 1.5
         effort:
           active: False
@@ -107,10 +107,12 @@ walking:
           i_clamp_min: 0.0
           i_clamp_max: 0.0
           antiwindup: False
+          threshold: 0.0
         roll:
           p: 0.0
           i: 0.0
           d: 0.0
           i_clamp_min: 0.0
           i_clamp_max: 0.0
           antiwindup: False
+          threshold: 0.0
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
@@ -13,6 +13,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "bitbots_quintic_walk/walk_utils.hpp"
+#include "bitbots_quintic_walk_parameters.hpp"
 #include "bitbots_splines/abstract_stabilizer.hpp"
 
 namespace bitbots_quintic_walk {
@@ -23,7 +24,8 @@ class WalkStabilizer : public bitbots_splines::AbstractStabilizer<WalkResponse>
   void reset() override;
   WalkResponse stabilize(const WalkResponse &response, const rclcpp::Duration &dt) override;
   WalkRequest adjust_step_length(WalkRequest request, const double imu_roll, const double imu_pitch,
-                                 double pitch_threshold, double roll_threshold, const rclcpp::Duration &dt);
+                                 double pitch_threshold, double roll_threshold, const rclcpp::Duration &dt,
+                                 walking::Params config_);
 
  private:
   rclcpp::Node::SharedPtr node_;

diff --git a/bitbots_motion/bitbots_quintic_walk/src/parameters.yaml b/bitbots_motion/bitbots_quintic_walk/src/parameters.yaml
@@ -372,11 +372,27 @@ walking:
           default_value: false
     step_length:
       pitch:
+        p:
+          type: double
+          description: "Proportional gain for the step adjust pitch controller"
+          default_value: 0.0
+        d:
+          type: double
+          description: "Derivative gain for the step adjust pitch controller"
+          default_value: 0.0
         threshold:
           type: double
           description: "Threshold to trigger step length adjustment"
           default_value: 0.0
       roll:
+        p:
+          type: double
+          description: "Proportional gain for the step adjust roll controller"
+          default_value: 0.0
+        d:
+          type: double
+          description: "Derivative gain for the step adjust roll controller"
+          default_value: 0.0
         threshold:
           type: double
           description: "Threshold to trigger step length adjustment"

diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp
@@ -181,7 +181,7 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) {
   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,
-      config_.node.step_length.roll.threshold, rclcpp::Duration::from_nanoseconds(1e9 * dt));
+      config_.node.step_length.roll.threshold, rclcpp::Duration::from_nanoseconds(1e9 * dt), config_);
   RCLCPP_WARN(node_->get_logger(), "New step size: %f, %f", request.linear_orders[0], request.linear_orders[1]);
 
   walk_engine_.setGoals(request);

diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp
@@ -25,10 +25,12 @@ void WalkStabilizer::reset() {
 
 WalkRequest WalkStabilizer::adjust_step_length(WalkRequest request, const double imu_roll, const double imu_pitch,
                                                double pitch_threshold, double roll_threshold,
-                                               const rclcpp::Duration& dt) {
+                                               const rclcpp::Duration& dt, walking::Params config_) {
   double adjustment_pitch = pid_step_length_adjustment_pitch_.computeCommand(imu_pitch, dt);
   double adjustment_roll = pid_step_length_adjustment_roll_.computeCommand(imu_roll, dt);
   RCLCPP_WARN(node_->get_logger(), "Adjustment pitch, roll: %f, %f", adjustment_pitch, adjustment_roll);
+  RCLCPP_WARN(node_->get_logger(), "p: %f,", config_.node.step_length.pitch.p);
+
   // adapt step length values based on PID controllers
   // we use a threshold to avoid unneeded steps
   if (fabs(adjustment_pitch) >= pitch_threshold) {

diff --git a/bitbots_simulation/bitbots_webots_sim/worlds/optimization_wolfgang.wbt b/bitbots_simulation/bitbots_webots_sim/worlds/optimization_wolfgang.wbt
@@ -1,4 +1,11 @@
-#VRML_SIM R2021b utf8
+#VRML_SIM R2022b utf8
+
+EXTERNPROTO "../protos/robots/Wolfgang/WolfgangOptimization.proto"
+#EXTERNPROTO "../protos/robots/Wolfgang/Wolfgang.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+
 DEF world_info WorldInfo {
   basicTimeStep 8
   optimalThreadCount 1
@@ -29,7 +36,7 @@ DEF world_info WorldInfo {
   ]
 }
 Viewpoint {
-  orientation 0.14 -0.605 -0.784 2.78
+  orientation 0.0550798 0.739203 -0.671227 0.221113
   position -2.20 4.06 1.5
 }
 TexturedBackground {
@@ -39,16 +46,17 @@ TexturedBackgroundLight {
   texture "stadium_dry"
 }
 
+Floor {
+  rotation 0 1 0 0
+  contactMaterial "grass"
+  size 100 100
+  tileSize 2 2
+}
+
 DEF amy WolfgangOptimization {
   translation -0.9968797400586715 2.9921102788692946 0.4274542359577024
   rotation 0.12198099937775954 0.12935172908461096 -0.9840674600725422 1.5869763401625028
   name "amy"
   controller "<extern>"
   supervisor TRUE
 }
-Floor {
-  rotation 1 0 0 1.57
-  contactMaterial "grass"
-  size 100 100
-  tileSize 2 2
-}