From 0d888c4a0d839ae4d69437a76ac283dc1fb95eca Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sat, 18 Feb 2023 23:43:23 +0100 Subject: [PATCH 01/10] Add pitch capture steps --- .../include/bitbots_quintic_walk/walk_node.h | 3 ++ bitbots_quintic_walk/src/walk_node.cpp | 37 ++++++++++++++++--- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h index beb40058..0c635ba2 100644 --- a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h +++ b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h @@ -163,6 +163,9 @@ class WalkNode : public rclcpp::Node { double imu_pitch_vel_threshold_; double imu_roll_vel_threshold_; + double pitch_error_avg_ = 0; + std::vector> pitch_error_buffer_; + int odom_pub_factor_; double last_ros_update_time_; diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index 3f7a7e43..172fb90a 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -218,11 +218,10 @@ void WalkNode::publish_debug(){ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { WalkRequest request(current_request_); - // update walk engine response - if (got_new_goals_) { - got_new_goals_ = false; - walk_engine_.setGoals(request); - } + request.linear_orders[0] += pitch_error_avg_ * 0.15; + + walk_engine_.setGoals(request); + checkPhaseRestAndReset(); current_response_ = walk_engine_.update(dt); @@ -464,10 +463,36 @@ void WalkNode::imuCb(const sensor_msgs::msg::Imu::SharedPtr msg) { current_trunk_fused_pitch_ = imu_fused.fusedPitch; current_trunk_fused_roll_ = imu_fused.fusedRoll; - // get angular velocities + // Get angular velocities roll_vel_ = msg->angular_velocity.x; pitch_vel_ = msg->angular_velocity.y; + // Calculate average pitch angle error over a given ros duration + pitch_error_buffer_.push_back(std::make_pair( + current_trunk_fused_pitch_ - walk_engine_.getWantedTrunkPitch(), + msg->header.stamp)); + + + // Filter the buffer to only contain values within the last duration + pitch_error_buffer_.erase( + std::remove_if( + pitch_error_buffer_.begin(), + pitch_error_buffer_.end(), + [this, &msg](const std::pair &pair) { + auto pitch_error_duration = rclcpp::Duration::from_seconds(0.2); + return rclcpp::Time(msg->header.stamp) - pair.second > pitch_error_duration; + }), + pitch_error_buffer_.end()); + + + pitch_error_avg_ = std::accumulate( + pitch_error_buffer_.begin(), + pitch_error_buffer_.end(), + 0.0, + [](double sum, const std::pair &pair) { + return sum + pair.first; + }) / pitch_error_buffer_.size(); + if (imu_active_) { // compute the pitch offset to the currently wanted pitch of the engine double wanted_pitch = walk_engine_.getWantedTrunkPitch(); From a099fb0c05935f6df7ffd4ccf079c3e8647f1962 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sat, 18 Feb 2023 23:44:00 +0100 Subject: [PATCH 02/10] Add pitch capture steps II --- bitbots_quintic_walk/src/walk_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index 172fb90a..f3b0239f 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -218,7 +218,7 @@ void WalkNode::publish_debug(){ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { WalkRequest request(current_request_); - request.linear_orders[0] += pitch_error_avg_ * 0.15; + request.linear_orders[0] += pitch_error_avg_ * 0.13; walk_engine_.setGoals(request); From 79bb5d38c024ee9d8a3759e6a95d90301a436a0c Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sun, 19 Feb 2023 16:59:15 +0100 Subject: [PATCH 03/10] Add derivative to capture steps --- .../include/bitbots_quintic_walk/walk_node.h | 2 ++ bitbots_quintic_walk/src/walk_node.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h index 0c635ba2..c7102ac5 100644 --- a/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h +++ b/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.h @@ -164,6 +164,8 @@ class WalkNode : public rclcpp::Node { double imu_roll_vel_threshold_; double pitch_error_avg_ = 0; + double pitch_error_derivative_ = 0; + double pitch_error_integral_ = 0; std::vector> pitch_error_buffer_; int odom_pub_factor_; diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index f3b0239f..b79488ab 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -218,7 +218,7 @@ void WalkNode::publish_debug(){ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { WalkRequest request(current_request_); - request.linear_orders[0] += pitch_error_avg_ * 0.13; + request.linear_orders[0] += pitch_error_avg_ * 0.16 + pitch_error_derivative_ * 5; walk_engine_.setGoals(request); @@ -493,6 +493,12 @@ void WalkNode::imuCb(const sensor_msgs::msg::Imu::SharedPtr msg) { return sum + pair.first; }) / pitch_error_buffer_.size(); + // Calculate the derivative of the pitch error + pitch_error_derivative_ = (pitch_error_avg_ - last_pitch_error_avg) / 0.2; //TODO correct timing + + // Calculate the integral of the pitch error + pitch_error_integral_ += pitch_error_avg_ * 0.2; //TODO correct timing + if (imu_active_) { // compute the pitch offset to the currently wanted pitch of the engine double wanted_pitch = walk_engine_.getWantedTrunkPitch(); From ced2ad97ec766a0fa00a71a7bad2202721f45257 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sun, 19 Feb 2023 17:00:25 +0100 Subject: [PATCH 04/10] Fix derivative --- bitbots_quintic_walk/src/walk_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index b79488ab..bf0b7238 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -484,7 +484,10 @@ void WalkNode::imuCb(const sensor_msgs::msg::Imu::SharedPtr msg) { }), pitch_error_buffer_.end()); + // Save the last average pitch error + auto last_pitch_error_avg = pitch_error_avg_; + // Calculate the average pitch error pitch_error_avg_ = std::accumulate( pitch_error_buffer_.begin(), pitch_error_buffer_.end(), From 38acfd4af23b02f7f724eaaf7a7b4b1c2299c00a Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Sun, 19 Feb 2023 17:01:13 +0100 Subject: [PATCH 05/10] Add even more experimental ankle pitch offset --- bitbots_quintic_walk/src/walk_node.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index bf0b7238..f298c37e 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -230,7 +230,7 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { current_response_.current_fused_pitch = current_trunk_fused_pitch_; bitbots_msgs::msg::JointCommand command; - if (walk_engine_.getState()!=WalkState::IDLE){ + //if (walk_engine_.getState()!=WalkState::IDLE){ // get stabilized goals from stabilizer current_stabilized_response_ = stabilizer_.stabilize(current_response_, rclcpp::Duration::from_nanoseconds(1e9 * dt)); @@ -254,7 +254,27 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { command.velocities = vels; command.accelerations = accs; command.max_currents = pwms; - } + + auto left_ankle_pitch_index = std::find( + command.joint_names.begin(), + command.joint_names.end(), + "LAnklePitch"); + + auto right_ankle_pitch_index = std::find( + command.joint_names.begin(), + command.joint_names.end(), + "RAnklePitch"); + + // Add ankle pitch offset based on the integral of the pitch error + double integral_gain = 0.001; + // The above but inplace + command.positions[left_ankle_pitch_index - command.joint_names.begin()] += pitch_error_integral_ * integral_gain; + command.positions[right_ankle_pitch_index - command.joint_names.begin()] -= pitch_error_integral_ * integral_gain; + + // log values for debugging + RCLCPP_WARN(this->get_logger(), "Left ankle pitch goal: %f | Right ankle pitch goal: %f | Pitch error integral: %f | Offset: %f", command.positions[left_ankle_pitch_index - command.joint_names.begin()], command.positions[right_ankle_pitch_index - command.joint_names.begin()], pitch_error_integral_, pitch_error_integral_ * integral_gain); + //} + return command; } From 5b530d61e41e71529c5245d48d006355d410f1be Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Wed, 5 Jul 2023 15:32:08 +0200 Subject: [PATCH 06/10] feat(odometry): make scaling params dynamically configurable by usage of the generate_parameter_library https://github.com/PickNikRobotics/generate_parameter_library --- bitbots_odometry/CMakeLists.txt | 94 +++++++++++-------- bitbots_odometry/config/odometry_config.yml | 42 +++++++++ .../bitbots_odometry/motion_odometry.h | 14 ++- bitbots_odometry/launch/odometry.launch | 6 +- bitbots_odometry/package.xml | 1 + bitbots_odometry/src/motion_odometry.cpp | 71 +++++++------- 6 files changed, 143 insertions(+), 85 deletions(-) create mode 100644 bitbots_odometry/config/odometry_config.yml diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index db10c2cb..f1d64487 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -6,20 +6,26 @@ if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif () -find_package(tf2_eigen REQUIRED) -find_package(tf2 REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(message_filters REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ament_cmake REQUIRED) find_package(biped_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(bitbots_docs REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) find_package(rot_conv REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(Eigen3 REQUIRED) + +generate_parameter_library( + odometry_parameters + config/odometry_config.yml +) include_directories(include) @@ -28,37 +34,48 @@ add_compile_options(-Wall -Werror -Wno-unused) add_executable(odometry_fuser src/odometry_fuser.cpp) add_executable(motion_odometry src/motion_odometry.cpp) +target_link_libraries( + motion_odometry + rclcpp::rclcpp + odometry_parameters +) + ## Specify libraries to link a library or executable target against -ament_target_dependencies(motion_odometry - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs - biped_interfaces - geometry_msgs - bitbots_docs - rot_conv - sensor_msgs - tf2_ros - ament_cmake - rclcpp) +ament_target_dependencies( + motion_odometry + tf2_eigen + tf2 + nav_msgs + message_filters + tf2_geometry_msgs + biped_interfaces + geometry_msgs + bitbots_docs + rot_conv + sensor_msgs + tf2_ros + ament_cmake + generate_parameter_library + rclcpp +) -ament_target_dependencies(odometry_fuser - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs - biped_interfaces - geometry_msgs - bitbots_docs - rot_conv - sensor_msgs - tf2_ros - ament_cmake - rclcpp - Eigen3) +ament_target_dependencies( + odometry_fuser + tf2_eigen + tf2 + nav_msgs + message_filters + tf2_geometry_msgs + biped_interfaces + geometry_msgs + bitbots_docs + rot_conv + sensor_msgs + tf2_ros + ament_cmake + rclcpp + Eigen3 +) enable_bitbots_docs() @@ -71,5 +88,4 @@ install(TARGETS odometry_fuser install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - ament_package() diff --git a/bitbots_odometry/config/odometry_config.yml b/bitbots_odometry/config/odometry_config.yml new file mode 100644 index 00000000..0d8761b1 --- /dev/null +++ b/bitbots_odometry/config/odometry_config.yml @@ -0,0 +1,42 @@ +motion_odometry: + x_forward_scaling: { + type: double, + default_value: 1.25, + description: "Scaling factor in forward x direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + x_backward_scaling: { + type: double, + default_value: 0.95, + description: "Scaling factor in backward x direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + y_scaling: { + type: double, + default_value: 1.0, + description: "Scaling factor in y direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + yaw_scaling: { + type: double, + default_value: 1.4, + description: "Scaling factor for rotation in yaw direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + publish_walk_odom_tf: { + type: bool, + default_value: false, + description: "Should odom tf be published" + } diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 7b44cc1e..2456dfd3 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -10,9 +10,12 @@ #include #include #include +#include "odometry_parameters.hpp" using std::placeholders::_1; +namespace bitbots_odometry { + class MotionOdometry : public rclcpp::Node { public: MotionOdometry(); @@ -33,11 +36,10 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr odom_subscriber_; - double x_forward_scaling_; - double x_backward_scaling_; - double y_scaling_; - double yaw_scaling_; - bool publish_walk_odom_tf_; + // Declare parameter listener and struct from the generate_parameter_library + motion_odometry::ParamListener param_listener_; + // Datastructure to hold all parameters, which is build from the schema in the 'parameters.yaml' + motion_odometry::Params config_; void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); void jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg); @@ -50,3 +52,5 @@ class MotionOdometry : public rclcpp::Node { std::string current_support_link_; rclcpp::Time start_time_; }; + +} diff --git a/bitbots_odometry/launch/odometry.launch b/bitbots_odometry/launch/odometry.launch index b1d5d95c..4b6df72b 100644 --- a/bitbots_odometry/launch/odometry.launch +++ b/bitbots_odometry/launch/odometry.launch @@ -6,16 +6,12 @@ + - - - - - diff --git a/bitbots_odometry/package.xml b/bitbots_odometry/package.xml index 8d09bd8d..3a93cbe7 100644 --- a/bitbots_odometry/package.xml +++ b/bitbots_odometry/package.xml @@ -23,6 +23,7 @@ rot_conv bitbots_docs biped_interfaces + generate_parameter_library diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index b28efc02..9c469bab 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -1,12 +1,15 @@ #include -MotionOdometry::MotionOdometry() - : Node("MotionOdometry"), - tf_buffer_(std::make_unique(this->get_clock())), - br_(std::make_unique(this)), - tf_listener_(std::make_shared(*tf_buffer_, this)) { - this->declare_parameter("publish_walk_odom_tf", false); - this->get_parameter("publish_walk_odom_tf", publish_walk_odom_tf_); +namespace bitbots_odometry { + +MotionOdometry::MotionOdometry() : Node("MotionOdometry"), + param_listener_(get_node_parameters_interface()) { + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, this); + br_ = std::make_unique(this); + config_ = param_listener_.get_params(); + this->declare_parameter("base_link_frame", "base_link"); this->get_parameter("base_link_frame", base_link_frame_); this->declare_parameter("r_sole_frame", "r_sole"); @@ -15,27 +18,20 @@ MotionOdometry::MotionOdometry() this->get_parameter("l_sole_frame", l_sole_frame_); this->declare_parameter("odom_frame", "odom"); this->get_parameter("odom_frame", odom_frame_); - this->declare_parameter("x_forward_scaling", 1.0); - this->get_parameter("x_forward_scaling", x_forward_scaling_); - this->declare_parameter("x_backward_scaling", 1.0); - this->get_parameter("x_backward_scaling", x_backward_scaling_); - this->declare_parameter("y_scaling", 1.0); - this->get_parameter("y_scaling", y_scaling_); - this->declare_parameter("yaw_scaling", 1.0); - this->get_parameter("yaw_scaling", yaw_scaling_); - joint_update_time_= rclcpp::Time(0, 0, RCL_ROS_TIME); + + joint_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); current_support_state_ = -1; previous_support_state_ = -1; walk_support_state_sub_ = this->create_subscription("walk_support_state", - 1, - std::bind(&MotionOdometry::supportCallback, - this, _1)); + 1, + std::bind(&MotionOdometry::supportCallback, + this, _1)); kick_support_state_sub_ = this->create_subscription("dynamic_kick_support_state", - 1, - std::bind(&MotionOdometry::supportCallback, - this, _1)); + 1, + std::bind(&MotionOdometry::supportCallback, + this, _1)); joint_state_sub_ = this->create_subscription("joint_states", 1, @@ -57,12 +53,13 @@ MotionOdometry::MotionOdometry() void MotionOdometry::loop() { rclcpp::Time cycle_start_time = this->now(); + config_ = param_listener_.get_params(); //check if joint states were received, otherwise we can't provide odometry rclcpp::Duration joints_delta_t = this->now() - joint_update_time_; if (joints_delta_t.seconds() > 0.1) { // only warn if we did not just start as this results in unecessary warnings - if ((this->now() - start_time_).seconds() > 10){ + if ((this->now() - start_time_).seconds() > 10) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30000, "No joint states received. Will not provide odometry."); } @@ -73,9 +70,9 @@ void MotionOdometry::loop() { // but since we skip the double support phase, we basically take the timepoint when the double support phase is // finished. This means both feet did not move and this should create no offset. if ((current_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || + && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || (current_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { + && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { foot_change_time_ = current_support_state_time_; if (previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE) { previous_support_link_ = l_sole_frame_; @@ -99,12 +96,12 @@ void MotionOdometry::loop() { // scale odometry based on parameters double x = previous_to_current_support.getOrigin().x(); if (x > 0) { - x = x * x_forward_scaling_; + x = x * config_.x_forward_scaling; } else { - x = x * x_backward_scaling_; + x = x * config_.x_backward_scaling; } - double y = previous_to_current_support.getOrigin().y() * y_scaling_; - double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * yaw_scaling_; + double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); tf2::Quaternion q; q.setRPY(0, 0, yaw); @@ -134,12 +131,12 @@ void MotionOdometry::loop() { tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); double x = current_support_to_base.getOrigin().x(); if (current_odom_msg_.twist.twist.linear.x > 0) { - x = x * x_forward_scaling_; - }else{ - x = x * x_backward_scaling_; + x = x * config_.x_forward_scaling; + } else { + x = x * config_.x_backward_scaling; } - double y = current_support_to_base.getOrigin().y() * y_scaling_; - double yaw = tf2::getYaw(current_support_to_base.getRotation()) * yaw_scaling_; + double y = current_support_to_base.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); tf2::Quaternion q; q.setRPY(0, 0, yaw); @@ -151,7 +148,7 @@ void MotionOdometry::loop() { odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; odom_to_base_link_msg.header.frame_id = odom_frame_; odom_to_base_link_msg.child_frame_id = base_link_frame_; - if (publish_walk_odom_tf_) { + if (config_.publish_walk_odom_tf) { RCLCPP_WARN_ONCE(this->get_logger(), "Sending Tf from walk odometry directly"); br_->sendTransform(odom_to_base_link_msg); } @@ -217,9 +214,11 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) current_odom_msg_ = *msg; } +} + int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); From c60a30f61e733f7fbf0fc6cffca055ae71f032db Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Wed, 5 Jul 2023 15:32:08 +0200 Subject: [PATCH 07/10] feat(odometry): make scaling params dynamically configurable by usage of the generate_parameter_library https://github.com/PickNikRobotics/generate_parameter_library --- bitbots_odometry/CMakeLists.txt | 94 +++++++++++-------- bitbots_odometry/config/odometry_config.yml | 42 +++++++++ .../bitbots_odometry/motion_odometry.h | 14 ++- bitbots_odometry/launch/odometry.launch | 5 - bitbots_odometry/package.xml | 1 + bitbots_odometry/src/motion_odometry.cpp | 71 +++++++------- 6 files changed, 142 insertions(+), 85 deletions(-) create mode 100644 bitbots_odometry/config/odometry_config.yml diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index db10c2cb..f1d64487 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -6,20 +6,26 @@ if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif () -find_package(tf2_eigen REQUIRED) -find_package(tf2 REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(message_filters REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(ament_cmake REQUIRED) find_package(biped_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(bitbots_docs REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) find_package(rot_conv REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(Eigen3 REQUIRED) + +generate_parameter_library( + odometry_parameters + config/odometry_config.yml +) include_directories(include) @@ -28,37 +34,48 @@ add_compile_options(-Wall -Werror -Wno-unused) add_executable(odometry_fuser src/odometry_fuser.cpp) add_executable(motion_odometry src/motion_odometry.cpp) +target_link_libraries( + motion_odometry + rclcpp::rclcpp + odometry_parameters +) + ## Specify libraries to link a library or executable target against -ament_target_dependencies(motion_odometry - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs - biped_interfaces - geometry_msgs - bitbots_docs - rot_conv - sensor_msgs - tf2_ros - ament_cmake - rclcpp) +ament_target_dependencies( + motion_odometry + tf2_eigen + tf2 + nav_msgs + message_filters + tf2_geometry_msgs + biped_interfaces + geometry_msgs + bitbots_docs + rot_conv + sensor_msgs + tf2_ros + ament_cmake + generate_parameter_library + rclcpp +) -ament_target_dependencies(odometry_fuser - tf2_eigen - tf2 - nav_msgs - message_filters - tf2_geometry_msgs - biped_interfaces - geometry_msgs - bitbots_docs - rot_conv - sensor_msgs - tf2_ros - ament_cmake - rclcpp - Eigen3) +ament_target_dependencies( + odometry_fuser + tf2_eigen + tf2 + nav_msgs + message_filters + tf2_geometry_msgs + biped_interfaces + geometry_msgs + bitbots_docs + rot_conv + sensor_msgs + tf2_ros + ament_cmake + rclcpp + Eigen3 +) enable_bitbots_docs() @@ -71,5 +88,4 @@ install(TARGETS odometry_fuser install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) - ament_package() diff --git a/bitbots_odometry/config/odometry_config.yml b/bitbots_odometry/config/odometry_config.yml new file mode 100644 index 00000000..0d8761b1 --- /dev/null +++ b/bitbots_odometry/config/odometry_config.yml @@ -0,0 +1,42 @@ +motion_odometry: + x_forward_scaling: { + type: double, + default_value: 1.25, + description: "Scaling factor in forward x direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + x_backward_scaling: { + type: double, + default_value: 0.95, + description: "Scaling factor in backward x direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + y_scaling: { + type: double, + default_value: 1.0, + description: "Scaling factor in y direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + yaw_scaling: { + type: double, + default_value: 1.4, + description: "Scaling factor for rotation in yaw direction", + validation: { + bounds<>: [0.5, 1.5] + } + } + + publish_walk_odom_tf: { + type: bool, + default_value: false, + description: "Should odom tf be published" + } diff --git a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h index 7b44cc1e..2456dfd3 100644 --- a/bitbots_odometry/include/bitbots_odometry/motion_odometry.h +++ b/bitbots_odometry/include/bitbots_odometry/motion_odometry.h @@ -10,9 +10,12 @@ #include #include #include +#include "odometry_parameters.hpp" using std::placeholders::_1; +namespace bitbots_odometry { + class MotionOdometry : public rclcpp::Node { public: MotionOdometry(); @@ -33,11 +36,10 @@ class MotionOdometry : public rclcpp::Node { rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr odom_subscriber_; - double x_forward_scaling_; - double x_backward_scaling_; - double y_scaling_; - double yaw_scaling_; - bool publish_walk_odom_tf_; + // Declare parameter listener and struct from the generate_parameter_library + motion_odometry::ParamListener param_listener_; + // Datastructure to hold all parameters, which is build from the schema in the 'parameters.yaml' + motion_odometry::Params config_; void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg); void jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg); @@ -50,3 +52,5 @@ class MotionOdometry : public rclcpp::Node { std::string current_support_link_; rclcpp::Time start_time_; }; + +} diff --git a/bitbots_odometry/launch/odometry.launch b/bitbots_odometry/launch/odometry.launch index b1d5d95c..ef7e744b 100644 --- a/bitbots_odometry/launch/odometry.launch +++ b/bitbots_odometry/launch/odometry.launch @@ -10,12 +10,7 @@ - - - - - diff --git a/bitbots_odometry/package.xml b/bitbots_odometry/package.xml index 8d09bd8d..3a93cbe7 100644 --- a/bitbots_odometry/package.xml +++ b/bitbots_odometry/package.xml @@ -23,6 +23,7 @@ rot_conv bitbots_docs biped_interfaces + generate_parameter_library diff --git a/bitbots_odometry/src/motion_odometry.cpp b/bitbots_odometry/src/motion_odometry.cpp index b28efc02..9c469bab 100644 --- a/bitbots_odometry/src/motion_odometry.cpp +++ b/bitbots_odometry/src/motion_odometry.cpp @@ -1,12 +1,15 @@ #include -MotionOdometry::MotionOdometry() - : Node("MotionOdometry"), - tf_buffer_(std::make_unique(this->get_clock())), - br_(std::make_unique(this)), - tf_listener_(std::make_shared(*tf_buffer_, this)) { - this->declare_parameter("publish_walk_odom_tf", false); - this->get_parameter("publish_walk_odom_tf", publish_walk_odom_tf_); +namespace bitbots_odometry { + +MotionOdometry::MotionOdometry() : Node("MotionOdometry"), + param_listener_(get_node_parameters_interface()) { + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_, this); + br_ = std::make_unique(this); + config_ = param_listener_.get_params(); + this->declare_parameter("base_link_frame", "base_link"); this->get_parameter("base_link_frame", base_link_frame_); this->declare_parameter("r_sole_frame", "r_sole"); @@ -15,27 +18,20 @@ MotionOdometry::MotionOdometry() this->get_parameter("l_sole_frame", l_sole_frame_); this->declare_parameter("odom_frame", "odom"); this->get_parameter("odom_frame", odom_frame_); - this->declare_parameter("x_forward_scaling", 1.0); - this->get_parameter("x_forward_scaling", x_forward_scaling_); - this->declare_parameter("x_backward_scaling", 1.0); - this->get_parameter("x_backward_scaling", x_backward_scaling_); - this->declare_parameter("y_scaling", 1.0); - this->get_parameter("y_scaling", y_scaling_); - this->declare_parameter("yaw_scaling", 1.0); - this->get_parameter("yaw_scaling", yaw_scaling_); - joint_update_time_= rclcpp::Time(0, 0, RCL_ROS_TIME); + + joint_update_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); current_support_state_ = -1; previous_support_state_ = -1; walk_support_state_sub_ = this->create_subscription("walk_support_state", - 1, - std::bind(&MotionOdometry::supportCallback, - this, _1)); + 1, + std::bind(&MotionOdometry::supportCallback, + this, _1)); kick_support_state_sub_ = this->create_subscription("dynamic_kick_support_state", - 1, - std::bind(&MotionOdometry::supportCallback, - this, _1)); + 1, + std::bind(&MotionOdometry::supportCallback, + this, _1)); joint_state_sub_ = this->create_subscription("joint_states", 1, @@ -57,12 +53,13 @@ MotionOdometry::MotionOdometry() void MotionOdometry::loop() { rclcpp::Time cycle_start_time = this->now(); + config_ = param_listener_.get_params(); //check if joint states were received, otherwise we can't provide odometry rclcpp::Duration joints_delta_t = this->now() - joint_update_time_; if (joints_delta_t.seconds() > 0.1) { // only warn if we did not just start as this results in unecessary warnings - if ((this->now() - start_time_).seconds() > 10){ + if ((this->now() - start_time_).seconds() > 10) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 30000, "No joint states received. Will not provide odometry."); } @@ -73,9 +70,9 @@ void MotionOdometry::loop() { // but since we skip the double support phase, we basically take the timepoint when the double support phase is // finished. This means both feet did not move and this should create no offset. if ((current_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || + && previous_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE) || (current_support_state_ == biped_interfaces::msg::Phase::RIGHT_STANCE - && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { + && previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE)) { foot_change_time_ = current_support_state_time_; if (previous_support_state_ == biped_interfaces::msg::Phase::LEFT_STANCE) { previous_support_link_ = l_sole_frame_; @@ -99,12 +96,12 @@ void MotionOdometry::loop() { // scale odometry based on parameters double x = previous_to_current_support.getOrigin().x(); if (x > 0) { - x = x * x_forward_scaling_; + x = x * config_.x_forward_scaling; } else { - x = x * x_backward_scaling_; + x = x * config_.x_backward_scaling; } - double y = previous_to_current_support.getOrigin().y() * y_scaling_; - double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * yaw_scaling_; + double y = previous_to_current_support.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(previous_to_current_support.getRotation()) * config_.yaw_scaling; previous_to_current_support.setOrigin({x, y, 0}); tf2::Quaternion q; q.setRPY(0, 0, yaw); @@ -134,12 +131,12 @@ void MotionOdometry::loop() { tf2::fromMsg(current_support_to_base_msg.transform, current_support_to_base); double x = current_support_to_base.getOrigin().x(); if (current_odom_msg_.twist.twist.linear.x > 0) { - x = x * x_forward_scaling_; - }else{ - x = x * x_backward_scaling_; + x = x * config_.x_forward_scaling; + } else { + x = x * config_.x_backward_scaling; } - double y = current_support_to_base.getOrigin().y() * y_scaling_; - double yaw = tf2::getYaw(current_support_to_base.getRotation()) * yaw_scaling_; + double y = current_support_to_base.getOrigin().y() * config_.y_scaling; + double yaw = tf2::getYaw(current_support_to_base.getRotation()) * config_.yaw_scaling; current_support_to_base.setOrigin({x, y, current_support_to_base.getOrigin().z()}); tf2::Quaternion q; q.setRPY(0, 0, yaw); @@ -151,7 +148,7 @@ void MotionOdometry::loop() { odom_to_base_link_msg.header.stamp = current_support_to_base_msg.header.stamp; odom_to_base_link_msg.header.frame_id = odom_frame_; odom_to_base_link_msg.child_frame_id = base_link_frame_; - if (publish_walk_odom_tf_) { + if (config_.publish_walk_odom_tf) { RCLCPP_WARN_ONCE(this->get_logger(), "Sending Tf from walk odometry directly"); br_->sendTransform(odom_to_base_link_msg); } @@ -217,9 +214,11 @@ void MotionOdometry::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) current_odom_msg_ = *msg; } +} + int main(int argc, char **argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::Duration timer_duration = rclcpp::Duration::from_seconds(1.0 / 200.0); rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer(node, node->get_clock(), timer_duration, [node]() -> void {node->loop();}); From 3541aa32405dda346ba77bcc794522870a8302d5 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 6 Jul 2023 08:53:30 +0200 Subject: [PATCH 08/10] Add different odometry configs for different robots --- bitbots_odometry/CMakeLists.txt | 2 +- bitbots_odometry/config/odometry_config_amy.yaml | 7 +++++++ bitbots_odometry/config/odometry_config_donna.yaml | 7 +++++++ bitbots_odometry/config/odometry_config_jack.yaml | 7 +++++++ bitbots_odometry/config/odometry_config_melody.yaml | 7 +++++++ bitbots_odometry/config/odometry_config_rory.yaml | 7 +++++++ .../{odometry_config.yml => odometry_config_template.yaml} | 0 bitbots_odometry/launch/odometry.launch | 2 +- 8 files changed, 37 insertions(+), 2 deletions(-) create mode 100644 bitbots_odometry/config/odometry_config_amy.yaml create mode 100644 bitbots_odometry/config/odometry_config_donna.yaml create mode 100644 bitbots_odometry/config/odometry_config_jack.yaml create mode 100644 bitbots_odometry/config/odometry_config_melody.yaml create mode 100644 bitbots_odometry/config/odometry_config_rory.yaml rename bitbots_odometry/config/{odometry_config.yml => odometry_config_template.yaml} (100%) diff --git a/bitbots_odometry/CMakeLists.txt b/bitbots_odometry/CMakeLists.txt index f1d64487..4bd17893 100644 --- a/bitbots_odometry/CMakeLists.txt +++ b/bitbots_odometry/CMakeLists.txt @@ -24,7 +24,7 @@ find_package(tf2_ros REQUIRED) generate_parameter_library( odometry_parameters - config/odometry_config.yml + config/odometry_config_template.yaml ) include_directories(include) diff --git a/bitbots_odometry/config/odometry_config_amy.yaml b/bitbots_odometry/config/odometry_config_amy.yaml new file mode 100644 index 00000000..b78a958f --- /dev/null +++ b/bitbots_odometry/config/odometry_config_amy.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 1.4 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_donna.yaml b/bitbots_odometry/config/odometry_config_donna.yaml new file mode 100644 index 00000000..b78a958f --- /dev/null +++ b/bitbots_odometry/config/odometry_config_donna.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 1.4 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_jack.yaml b/bitbots_odometry/config/odometry_config_jack.yaml new file mode 100644 index 00000000..b78a958f --- /dev/null +++ b/bitbots_odometry/config/odometry_config_jack.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 1.4 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_melody.yaml b/bitbots_odometry/config/odometry_config_melody.yaml new file mode 100644 index 00000000..b78a958f --- /dev/null +++ b/bitbots_odometry/config/odometry_config_melody.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 1.4 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_rory.yaml b/bitbots_odometry/config/odometry_config_rory.yaml new file mode 100644 index 00000000..b78a958f --- /dev/null +++ b/bitbots_odometry/config/odometry_config_rory.yaml @@ -0,0 +1,7 @@ +motion_odometry: + ros__parameters: + x_forward_scaling: 1.25 + x_backward_scaling: 0.95 + y_scaling: 1.0 + yaw_scaling: 1.4 + publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config.yml b/bitbots_odometry/config/odometry_config_template.yaml similarity index 100% rename from bitbots_odometry/config/odometry_config.yml rename to bitbots_odometry/config/odometry_config_template.yaml diff --git a/bitbots_odometry/launch/odometry.launch b/bitbots_odometry/launch/odometry.launch index 4b6df72b..545c0731 100644 --- a/bitbots_odometry/launch/odometry.launch +++ b/bitbots_odometry/launch/odometry.launch @@ -6,7 +6,7 @@ - + From 2704d92fd32a94ff04fe18fd81a39afe05d84999 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 6 Jul 2023 08:55:09 +0200 Subject: [PATCH 09/10] Very basic tuning --- bitbots_odometry/config/odometry_config_amy.yaml | 2 +- bitbots_odometry/config/odometry_config_donna.yaml | 2 +- bitbots_odometry/config/odometry_config_jack.yaml | 2 +- bitbots_odometry/config/odometry_config_melody.yaml | 2 +- bitbots_odometry/config/odometry_config_rory.yaml | 2 +- bitbots_odometry/config/odometry_config_template.yaml | 4 ++-- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/bitbots_odometry/config/odometry_config_amy.yaml b/bitbots_odometry/config/odometry_config_amy.yaml index b78a958f..55021f9a 100644 --- a/bitbots_odometry/config/odometry_config_amy.yaml +++ b/bitbots_odometry/config/odometry_config_amy.yaml @@ -3,5 +3,5 @@ motion_odometry: x_forward_scaling: 1.25 x_backward_scaling: 0.95 y_scaling: 1.0 - yaw_scaling: 1.4 + yaw_scaling: 0.6 publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_donna.yaml b/bitbots_odometry/config/odometry_config_donna.yaml index b78a958f..55021f9a 100644 --- a/bitbots_odometry/config/odometry_config_donna.yaml +++ b/bitbots_odometry/config/odometry_config_donna.yaml @@ -3,5 +3,5 @@ motion_odometry: x_forward_scaling: 1.25 x_backward_scaling: 0.95 y_scaling: 1.0 - yaw_scaling: 1.4 + yaw_scaling: 0.6 publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_jack.yaml b/bitbots_odometry/config/odometry_config_jack.yaml index b78a958f..55021f9a 100644 --- a/bitbots_odometry/config/odometry_config_jack.yaml +++ b/bitbots_odometry/config/odometry_config_jack.yaml @@ -3,5 +3,5 @@ motion_odometry: x_forward_scaling: 1.25 x_backward_scaling: 0.95 y_scaling: 1.0 - yaw_scaling: 1.4 + yaw_scaling: 0.6 publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_melody.yaml b/bitbots_odometry/config/odometry_config_melody.yaml index b78a958f..55021f9a 100644 --- a/bitbots_odometry/config/odometry_config_melody.yaml +++ b/bitbots_odometry/config/odometry_config_melody.yaml @@ -3,5 +3,5 @@ motion_odometry: x_forward_scaling: 1.25 x_backward_scaling: 0.95 y_scaling: 1.0 - yaw_scaling: 1.4 + yaw_scaling: 0.6 publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_rory.yaml b/bitbots_odometry/config/odometry_config_rory.yaml index b78a958f..55021f9a 100644 --- a/bitbots_odometry/config/odometry_config_rory.yaml +++ b/bitbots_odometry/config/odometry_config_rory.yaml @@ -3,5 +3,5 @@ motion_odometry: x_forward_scaling: 1.25 x_backward_scaling: 0.95 y_scaling: 1.0 - yaw_scaling: 1.4 + yaw_scaling: 0.6 publish_walk_odom_tf: false diff --git a/bitbots_odometry/config/odometry_config_template.yaml b/bitbots_odometry/config/odometry_config_template.yaml index 0d8761b1..03d46145 100644 --- a/bitbots_odometry/config/odometry_config_template.yaml +++ b/bitbots_odometry/config/odometry_config_template.yaml @@ -28,10 +28,10 @@ motion_odometry: yaw_scaling: { type: double, - default_value: 1.4, + default_value: 0.6, description: "Scaling factor for rotation in yaw direction", validation: { - bounds<>: [0.5, 1.5] + bounds<>: [0.2, 2.5] } } From b2463cf5c0a6925f5627883d571201388b6ba589 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Thu, 6 Jul 2023 14:27:38 +0000 Subject: [PATCH 10/10] Revert "Add even more experimental ankle pitch offset" This reverts commit 38acfd4af23b02f7f724eaaf7a7b4b1c2299c00a. --- bitbots_quintic_walk/src/walk_node.cpp | 24 ++---------------------- 1 file changed, 2 insertions(+), 22 deletions(-) diff --git a/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_quintic_walk/src/walk_node.cpp index f298c37e..bf0b7238 100644 --- a/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_quintic_walk/src/walk_node.cpp @@ -230,7 +230,7 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { current_response_.current_fused_pitch = current_trunk_fused_pitch_; bitbots_msgs::msg::JointCommand command; - //if (walk_engine_.getState()!=WalkState::IDLE){ + if (walk_engine_.getState()!=WalkState::IDLE){ // get stabilized goals from stabilizer current_stabilized_response_ = stabilizer_.stabilize(current_response_, rclcpp::Duration::from_nanoseconds(1e9 * dt)); @@ -254,27 +254,7 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { command.velocities = vels; command.accelerations = accs; command.max_currents = pwms; - - auto left_ankle_pitch_index = std::find( - command.joint_names.begin(), - command.joint_names.end(), - "LAnklePitch"); - - auto right_ankle_pitch_index = std::find( - command.joint_names.begin(), - command.joint_names.end(), - "RAnklePitch"); - - // Add ankle pitch offset based on the integral of the pitch error - double integral_gain = 0.001; - // The above but inplace - command.positions[left_ankle_pitch_index - command.joint_names.begin()] += pitch_error_integral_ * integral_gain; - command.positions[right_ankle_pitch_index - command.joint_names.begin()] -= pitch_error_integral_ * integral_gain; - - // log values for debugging - RCLCPP_WARN(this->get_logger(), "Left ankle pitch goal: %f | Right ankle pitch goal: %f | Pitch error integral: %f | Offset: %f", command.positions[left_ankle_pitch_index - command.joint_names.begin()], command.positions[right_ankle_pitch_index - command.joint_names.begin()], pitch_error_integral_, pitch_error_integral_ * integral_gain); - //} - + } return command; }