From 2a75e5d0729638b620ddfbbf5da0998a5b1f9020 Mon Sep 17 00:00:00 2001 From: Vincent Belpois <vincent.belpois@gmail.com> Date: Wed, 24 Apr 2024 21:23:36 +0200 Subject: [PATCH] Didn't limit the correct speed I think --- .../config/omnidirectional_controller.yaml | 4 ++-- .../src/omnidirectional_controller.cpp | 16 ++++++++++------ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/ezbot_robot/config/omnidirectional_controller.yaml b/src/ezbot_robot/config/omnidirectional_controller.yaml index 9550bed..d3db8c8 100644 --- a/src/ezbot_robot/config/omnidirectional_controller.yaml +++ b/src/ezbot_robot/config/omnidirectional_controller.yaml @@ -35,7 +35,7 @@ omnidirectional_controller: use_stamped_vel: false linear_has_velocity_limits: true - linear_min_velocity: -1.0 - linear_max_velocity: 1.0 + linear_min_velocity: -0.1 + linear_max_velocity: 0.1 diff --git a/src/omnidirectional_controllers/src/omnidirectional_controller.cpp b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp index 9bb49a8..517dc73 100644 --- a/src/omnidirectional_controllers/src/omnidirectional_controller.cpp +++ b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp @@ -437,6 +437,10 @@ controller_interface::return_type OmnidirectionalController::update( } Twist command = *cmd_vel_; + double & linear_x_command = command.twist.linear.x; + double & linear_y_command = command.twist.linear.y; + double & angular_command = command.twist.angular.z; + if (odom_params_.open_loop) { odometry_.updateOpenLoop( @@ -486,12 +490,12 @@ controller_interface::return_type OmnidirectionalController::update( auto & last_command = previous_commands_.back().twist; auto & second_to_last_command = previous_commands_.front().twist; - limiter_linear_.limit(command.twist.linear.x, second_to_last_command.linear.x, last_command.linear.x, + limiter_linear_.limit(linear_x_command, second_to_last_command.linear.x, last_command.linear.x, period.seconds()); - limiter_linear_.limit(command.twist.linear.y, second_to_last_command.linear.y, last_command.linear.y, + limiter_linear_.limit(linear_y_command, second_to_last_command.linear.y, last_command.linear.y, period.seconds()); - limiter_angular_.limit(command.twist.angular.z, second_to_last_command.angular.z, last_command.angular.z, + limiter_angular_.limit(angular_command, second_to_last_command.angular.z, last_command.angular.z, period.seconds()); @@ -500,9 +504,9 @@ controller_interface::return_type OmnidirectionalController::update( // Compute wheels velocities: RobotVelocity body_vel_setpoint; - body_vel_setpoint.vx = cmd_vel_->twist.linear.x; - body_vel_setpoint.vy = cmd_vel_->twist.linear.y; - body_vel_setpoint.omega = cmd_vel_->twist.angular.z; + body_vel_setpoint.vx = linear_x_command; + body_vel_setpoint.vy = linear_y_command; + body_vel_setpoint.omega = angular_command; std::vector<double> wheels_angular_velocity; wheels_angular_velocity = omni_robot_kinematics_.getWheelsAngularVelocities(body_vel_setpoint);