Skip to content

Fixes usages of deprecated control_toolbox constructors. #350

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
barulicm merged 1 commit into from
Jul 4, 2025
Merged

Fixes usages of deprecated control_toolbox constructors. #350

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 24 additions & 7 deletions ateam_kenobi/src/core/motion/motion_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <CGAL/squared_distance_2.h>
#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>
#include <ateam_msgs/msg/robot_motion_command.hpp>
#include <ateam_msgs/msg/robot_state.hpp>
Expand All @@ -45,6 +46,9 @@ CREATE_PARAM(double, "motion/pid/t_max", t_max, 4);


MotionController::MotionController()
: x_controller(0.0, 0.0, 0.0, 0.0, 0.0, control_toolbox::AntiWindupStrategy()),
y_controller(0.0, 0.0, 0.0, 0.0, 0.0, control_toolbox::AntiWindupStrategy()),
t_controller(0.0, 0.0, 0.0, 0.0, 0.0, control_toolbox::AntiWindupStrategy())
{
this->reset();
}
Expand Down Expand Up @@ -195,10 +199,23 @@ ateam_msgs::msg::RobotMotionCommand MotionController::get_command(

void MotionController::reset()
{
// TODO(anon): handle pid gains better
this->x_controller.initialize(2.8, 0.0, 0.002, 0.3, -0.3, true);
this->y_controller.initialize(2.8, 0.0, 0.002, 0.15, -0.15, true);
this->t_controller.initialize(2.5, 0.0, 0.0, 0.5, -0.5, true);
const auto u_max = std::numeric_limits<double>::infinity();

@chachmu chachmu Jul 4, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it be better to set these to like the max velocity or something just to make sure the controller doesn't go completely crazy?

@barulicm barulicm Jul 4, 2025

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You already do that clamping elsewhere for now, and I was trying to keep this changeset minimal.
We certainly could consider using their implementation of clamping instead in the future.

const auto u_min = -std::numeric_limits<double>::infinity();
control_toolbox::AntiWindupStrategy x_aws;
x_aws.type = control_toolbox::AntiWindupStrategy::LEGACY;
x_aws.i_max = 0.3;
x_aws.i_min = -0.3;
this->x_controller.initialize(2.8, 0.0, 0.002, u_max, u_min, x_aws);
control_toolbox::AntiWindupStrategy y_aws;
y_aws.type = control_toolbox::AntiWindupStrategy::LEGACY;
y_aws.i_max = 0.15;
y_aws.i_min = -0.15;
this->y_controller.initialize(2.8, 0.0, 0.002, u_max, u_min, y_aws);
control_toolbox::AntiWindupStrategy t_aws;
t_aws.type = control_toolbox::AntiWindupStrategy::LEGACY;
t_aws.i_max = 0.5;
t_aws.i_min = -0.5;
this->t_controller.initialize(2.5, 0.0, 0.0, u_max, u_min, t_aws);

this->progress = 0;
this->total_dist = 0;
Expand Down Expand Up @@ -263,7 +280,7 @@ void MotionController::set_t_pid_gain(GainType gain, double value)

void MotionController::set_x_pid_gains(double p, double i, double d)
{
control_toolbox::Pid::Gains gains;
control_toolbox::Pid::Gains gains = this->x_controller.get_gains();
gains.p_gain_ = p;
gains.i_gain_ = i;
gains.d_gain_ = d;
Expand All @@ -272,7 +289,7 @@ void MotionController::set_x_pid_gains(double p, double i, double d)

void MotionController::set_y_pid_gains(double p, double i, double d)
{
control_toolbox::Pid::Gains gains;
control_toolbox::Pid::Gains gains = this->y_controller.get_gains();
gains.p_gain_ = p;
gains.i_gain_ = i;
gains.d_gain_ = d;
Expand All @@ -281,7 +298,7 @@ void MotionController::set_y_pid_gains(double p, double i, double d)

void MotionController::set_t_pid_gains(double p, double i, double d)
{
control_toolbox::Pid::Gains gains;
control_toolbox::Pid::Gains gains = this->t_controller.get_gains();
gains.p_gain_ = p;
gains.i_gain_ = i;
gains.d_gain_ = d;
Expand Down