Skip to content

Unique PID Parameters for each Robot #2513

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
shourikb merged 6 commits into from
Mar 11, 2026
Merged

Unique PID Parameters for each Robot #2513

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
79d7b3c
started the process, fix the .hpp file
shourikb Mar 2, 2026
da1333c
should all work now
shourikb Mar 4, 2026
d5b8450
added a callback to update the params when they are changed in rqt
shourikb Mar 9, 2026
a3119af
Fix Code Style On unique-params (#2515)
github-actions[bot] Mar 9, 2026
762f23a
removed comments
shourikb Mar 11, 2026
7199b1f
Merge branch 'unique-params' of https://github.com/RoboJackets/robocu…
shourikb Mar 11, 2026
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: 19 additions & 12 deletions src/rj_control/include/rj_control/motion_control.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,17 +23,6 @@

namespace control {

DECLARE_FLOAT64(params::kMotionControlParamModule, max_acceleration);
DECLARE_FLOAT64(params::kMotionControlParamModule, max_velocity);
DECLARE_FLOAT64(params::kMotionControlParamModule, rotation_kp);
DECLARE_FLOAT64(params::kMotionControlParamModule, rotation_ki);
DECLARE_FLOAT64(params::kMotionControlParamModule, rotation_kd);
DECLARE_INT64(params::kMotionControlParamModule, rotation_windup);
DECLARE_FLOAT64(params::kMotionControlParamModule, translation_kp);
DECLARE_FLOAT64(params::kMotionControlParamModule, translation_ki);
DECLARE_FLOAT64(params::kMotionControlParamModule, translation_kd);
DECLARE_INT64(params::kMotionControlParamModule, translation_windup);

namespace testing {

class MotionControlTest;
Expand Down Expand Up @@ -79,7 +68,7 @@ class MotionControl {
*/
void update_params();

static void set_velocity(MotionSetpoint* setpoint, rj_geometry::Twist target_vel);
void set_velocity(MotionSetpoint* setpoint, rj_geometry::Twist target_vel);

int shell_id_;

Expand Down Expand Up @@ -107,6 +96,24 @@ class MotionControl {
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_x_pub_;
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_y_pub_;
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_heading_pub_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;

// Robot-specific PID gains
double translation_kp_;
double translation_ki_;
double translation_kd_;

double rotation_kp_;
double rotation_ki_;
double rotation_kd_;

// Shared limits
double max_velocity_;
double max_acceleration_;
double max_angular_velocity_;

int translation_windup_;
int rotation_windup_;
};

} // namespace control
107 changes: 69 additions & 38 deletions src/rj_control/src/motion_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,35 +6,66 @@ using planning::RobotInstant;
using rj_geometry::Pose;
using rj_geometry::Twist;

DEFINE_FLOAT64(params::kMotionControlParamModule, max_acceleration, 3.0,
"Maximum acceleration limit (motion control) (m/s^2)");
DEFINE_FLOAT64(params::kMotionControlParamModule, max_velocity, 2.4,
"Maximum velocity limit (motion control) (m/s)");
DEFINE_FLOAT64(params::kMotionControlParamModule, max_angular_velocity, 5.0,
"Maximum angular velocity limit (motion control) (rad/s)");
DEFINE_FLOAT64(params::kMotionControlParamModule, rotation_kp, 10.0,
"Kp for rotation ((rad/s)/rad)");
DEFINE_FLOAT64(params::kMotionControlParamModule, rotation_ki, 0.0,
"Ki for rotation ((rad/s)/(rad*s))");
DEFINE_FLOAT64(params::kMotionControlParamModule, rotation_kd, 0.9,
"Kd for rotation ((rad/s)/(rad/s))");
DEFINE_INT64(params::kMotionControlParamModule, rotation_windup, 0,
"Windup limit for rotation (unknown units)");
DEFINE_FLOAT64(params::kMotionControlParamModule, translation_kp, 0.6,
"Kp for translation ((m/s)/m)");
DEFINE_FLOAT64(params::kMotionControlParamModule, translation_ki, 0.0,
"Ki for translation ((m/s)/(m*s))");
DEFINE_FLOAT64(params::kMotionControlParamModule, translation_kd, 0.3,
"Kd for translation ((m/s)/(m/s))");
DEFINE_INT64(params::kMotionControlParamModule, translation_windup, 0,
"Windup limit for translation (unknown units)");

MotionControl::MotionControl(int shell_id, rclcpp::Node* node)
: shell_id_(shell_id),
angle_controller_(0, 0, 0, 50, 0),
drawer_(
node->create_publisher<rj_drawing_msgs::msg::DebugDraw>(viz::topics::kDebugDrawTopic, 10),
fmt::format("motion_control/{}", std::to_string(shell_id))) {
std::string param_prefix = fmt::format("robot_{}", std::to_string(shell_id_));

// populate params
// robot specific
node->get_parameter(param_prefix + ".translation_kp", translation_kp_);
node->get_parameter(param_prefix + ".translation_ki", translation_ki_);
node->get_parameter(param_prefix + ".translation_kd", translation_kd_);
node->get_parameter(param_prefix + ".rotation_kp", rotation_kp_);
node->get_parameter(param_prefix + ".rotation_ki", rotation_ki_);
node->get_parameter(param_prefix + ".rotation_kd", rotation_kd_);

// shared between robots
node->get_parameter("translation_windup", translation_windup_);
node->get_parameter("rotation_windup", rotation_windup_);
node->get_parameter("max_velocity", max_velocity_);
node->get_parameter("max_acceleration", max_acceleration_);
node->get_parameter("max_angular_velocity", max_angular_velocity_);

param_callback_handle_ = node->add_on_set_parameters_callback(
[this, param_prefix](const std::vector<rclcpp::Parameter>& params) {
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;

for (const auto& param : params) {
const auto& name = param.get_name();

if (name == param_prefix + ".translation_kp")
translation_kp_ = param.as_double();
else if (name == param_prefix + ".translation_ki")
translation_ki_ = param.as_double();
else if (name == param_prefix + ".translation_kd")
translation_kd_ = param.as_double();
else if (name == param_prefix + ".rotation_kp")
rotation_kp_ = param.as_double();
else if (name == param_prefix + ".rotation_ki")
rotation_ki_ = param.as_double();
else if (name == param_prefix + ".rotation_kd")
rotation_kd_ = param.as_double();
else if (name == "translation_windup")
translation_windup_ = param.as_int();
else if (name == "rotation_windup")
rotation_windup_ = param.as_int();
else if (name == "max_velocity")
max_velocity_ = param.as_double();
else if (name == "max_acceleration")
max_acceleration_ = param.as_double();
else if (name == "max_angular_velocity")
max_angular_velocity_ = param.as_double();
}

return result;
});

motion_setpoint_pub_ = node->create_publisher<MotionSetpoint::Msg>(
topics::motion_setpoint_topic(shell_id_), rclcpp::QoS(1));
target_state_pub_ = node->create_publisher<RobotState::Msg>(
Expand Down Expand Up @@ -188,9 +219,9 @@ void MotionControl::run(const RobotState& state, const planning::Trajectory& tra

void MotionControl::set_velocity(MotionSetpoint* setpoint, Twist target_vel) {
// Limit Velocity
target_vel.linear().clamp(PARAM_max_velocity);
target_vel.linear().clamp(max_velocity_);
target_vel.angular() =
std::clamp(target_vel.angular(), -PARAM_max_angular_velocity, PARAM_max_angular_velocity);
std::clamp(target_vel.angular(), -max_angular_velocity_, max_angular_velocity_);

// make sure we don't send any bad values
if (Eigen::Vector3d(target_vel).hasNaN()) {
Expand All @@ -211,20 +242,20 @@ void MotionControl::set_velocity(MotionSetpoint* setpoint, Twist target_vel) {

void MotionControl::update_params() {
// Update PID parameters
position_x_controller_.kp = static_cast<float>(PARAM_translation_kp);
position_x_controller_.ki = static_cast<float>(PARAM_translation_ki);
position_x_controller_.kd = static_cast<float>(PARAM_translation_kd);
position_x_controller_.setWindup(PARAM_translation_windup);

position_y_controller_.kp = static_cast<float>(PARAM_translation_kp);
position_y_controller_.ki = static_cast<float>(PARAM_translation_ki);
position_y_controller_.kd = static_cast<float>(PARAM_translation_kd);
position_y_controller_.setWindup(PARAM_translation_windup);

angle_controller_.kp = static_cast<float>(PARAM_rotation_kp);
angle_controller_.ki = static_cast<float>(PARAM_rotation_ki);
angle_controller_.kd = static_cast<float>(PARAM_rotation_kd);
angle_controller_.setWindup(PARAM_rotation_windup);
position_x_controller_.kp = static_cast<float>(translation_kp_);
position_x_controller_.ki = static_cast<float>(translation_ki_);
position_x_controller_.kd = static_cast<float>(translation_kd_);
position_x_controller_.setWindup(translation_windup_);

position_y_controller_.kp = static_cast<float>(translation_kp_);
position_y_controller_.ki = static_cast<float>(translation_ki_);
position_y_controller_.kd = static_cast<float>(translation_kd_);
position_y_controller_.setWindup(translation_windup_);

angle_controller_.kp = static_cast<float>(rotation_kp_);
angle_controller_.ki = static_cast<float>(rotation_ki_);
angle_controller_.kd = static_cast<float>(rotation_kd_);
angle_controller_.setWindup(rotation_windup_);
}

void MotionControl::reset() {
Expand Down
49 changes: 43 additions & 6 deletions src/rj_param_utils/config/real_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,52 @@

/control:
ros__parameters:
robot_0:
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
robot_1:
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
robot_2:
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
robot_3:
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
robot_4:
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
robot_5:
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
max_acceleration: 2.0
max_angular_velocity: 1.0
max_velocity: 0.5
rotation_kd: 0.9
rotation_ki: 0.0
rotation_kp: 10.0
rotation_windup: 0
translation_kd: 0.45
translation_ki: 0.0
translation_kp: 3.0
translation_windup: 0
use_sim_time: false

Expand Down
49 changes: 43 additions & 6 deletions src/rj_param_utils/config/sim_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,52 @@

/control:
ros__parameters:
robot_0:
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
robot_1:
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
robot_2:
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
robot_3:
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
robot_4:
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
robot_5:
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
max_acceleration: 1.5
max_angular_velocity: 1.0
max_velocity: 2.0
rotation_kd: 0.0
rotation_ki: 0.0
rotation_kp: 8.0
rotation_windup: 0
translation_kd: 0.0
translation_ki: 0.0
translation_kp: 4.0
translation_windup: 0
use_sim_time: false

Expand Down
Loading