diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml index 1c2e7ebf1..7fa2b8fbe 100644 --- 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,6 +107,7 @@ walking: i_clamp_min: 0.0 i_clamp_max: 0.0 antiwindup: False + threshold: 0.0 roll: p: 0.0 i: 0.0 @@ -114,3 +115,4 @@ walking: 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 index 1974afb53..06c6ed080 100644 --- 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 #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 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 index 79630ced3..e95ca3c1d 100644 --- 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 index b94b8a333..b645b80b3 100644 --- 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 index 36c123323..2abcb7943 100644 --- 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 index f650f3427..c507ac30c 100644 --- 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,6 +46,13 @@ 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 @@ -46,9 +60,3 @@ DEF amy WolfgangOptimization { controller "" supervisor TRUE } -Floor { - rotation 1 0 0 1.57 - contactMaterial "grass" - size 100 100 - tileSize 2 2 -} \ No newline at end of file