From 5e676d51681edad012f804387c3938ff620ef0dd Mon Sep 17 00:00:00 2001 From: Nisarg Date: Wed, 3 Jun 2026 12:45:47 +0900 Subject: [PATCH 1/2] add acceleration limits --- cfg/MPPIController.cfg | 5 ++ .../mppi_controller/models/constraints.hpp | 49 +++++++++++++++++-- include/mppi_controller/motion_models.hpp | 15 ++++++ src/optimizer.cpp | 34 ++++++++++++- 4 files changed, 97 insertions(+), 6 deletions(-) diff --git a/cfg/MPPIController.cfg b/cfg/MPPIController.cfg index cc94737..bc2a496 100644 --- a/cfg/MPPIController.cfg +++ b/cfg/MPPIController.cfg @@ -25,6 +25,11 @@ grp_robot.add("wz_max", double_t, 0, "Max WZ (rad/s)", 1.9, 0.0, 10.0) grp_robot.add("max_vel_trans", double_t, 0, "Max resultant Vx and Vy", 3.0, 0.0, 10.0) +grp_robot.add("max_accel_trans", double_t, 0, "Max translational acceleration (m/s^2); 0 = disabled", 0.0, 0.0, 50.0) +grp_robot.add("max_decel_trans", double_t, 0, "Max translational deceleration/braking (m/s^2); 0 = disabled", 0.0, 0.0, 50.0) +grp_robot.add("max_accel_angular", double_t, 0, "Max angular acceleration (rad/s^2); 0 = disabled", 0.0, 0.0, 100.0) +grp_robot.add("max_decel_angular", double_t, 0, "Max angular deceleration/braking (rad/s^2); 0 = disabled", 0.0, 0.0, 100.0) + grp_robot.add("vx_std", double_t, 0, "Sampling standard deviation for VX", 0.2, 0.0, 4.0) grp_robot.add("vy_std", double_t, 0, "Sampling standard deviation for VY", 0.2, 0.0, 4.0) grp_robot.add("wz_std", double_t, 0, "Sampling standard deviation for WZ", 0.4, 0.0, 4.0) diff --git a/include/mppi_controller/models/constraints.hpp b/include/mppi_controller/models/constraints.hpp index de3f595..4e69ef9 100644 --- a/include/mppi_controller/models/constraints.hpp +++ b/include/mppi_controller/models/constraints.hpp @@ -15,6 +15,8 @@ #ifndef MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ #define MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ +#include + namespace mppi::models { @@ -24,11 +26,15 @@ namespace mppi::models */ struct ControlConstraints { - double vx_max; - double vx_min; - double vy; - double wz; - double max_vel_trans; + double vx_max = 0.0; + double vx_min = 0.0; + double vy = 0.0; + double wz = 0.0; + double max_vel_trans = 0.0; + double max_accel_trans = 0.0; // 0 = disabled + double max_decel_trans = 0.0; // 0 = disabled + double max_accel_angular = 0.0; // 0 = disabled + double max_decel_angular = 0.0; // 0 = disabled }; /** @@ -42,6 +48,39 @@ struct SamplingStd double wz; }; +/** + * @brief Clamp a velocity step to respect acceleration/deceleration limits. + * Magnitude-based: |v| increasing = acceleration, |v| decreasing = deceleration. + * A limit of 0 means disabled (no clamping applied). + */ +inline float clampByAccel(float v_prev, float v_curr, double max_accel, double max_decel, float dt) +{ + if (max_accel <= 0.0 && max_decel <= 0.0) { + return v_curr; + } + const float abs_prev = std::fabs(v_prev); + const float abs_curr = std::fabs(v_curr); + if (abs_curr > abs_prev) { + if (max_accel > 0.0) { + const float allowed = abs_prev + static_cast(max_accel) * dt; + if (abs_curr > allowed) { + return std::copysign(allowed, v_curr); + } + } + } else { + if (max_decel > 0.0) { + float allowed = abs_prev - static_cast(max_decel) * dt; + if (allowed < 0.0f) { + allowed = 0.0f; + } + if (abs_curr < allowed) { + return std::copysign(allowed, v_prev); + } + } + } + return v_curr; +} + } // namespace mppi::models #endif // MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ diff --git a/include/mppi_controller/motion_models.hpp b/include/mppi_controller/motion_models.hpp index 4e19f84..4de0ceb 100644 --- a/include/mppi_controller/motion_models.hpp +++ b/include/mppi_controller/motion_models.hpp @@ -87,6 +87,21 @@ class MotionModel { state.vx(j, i) = cvx_curr * scale; state.vy(j, i) = cvy_curr * scale; } + + // Apply acceleration / deceleration constraints per batch trajectory + if (control_constraints_.max_accel_trans > 0.0 || control_constraints_.max_decel_trans > 0.0) { + state.vx(j, i) = models::clampByAccel( + state.vx(j, i - 1), state.vx(j, i), + control_constraints_.max_accel_trans, control_constraints_.max_decel_trans, model_dt_); + state.vy(j, i) = models::clampByAccel( + state.vy(j, i - 1), state.vy(j, i), + control_constraints_.max_accel_trans, control_constraints_.max_decel_trans, model_dt_); + } + if (control_constraints_.max_accel_angular > 0.0 || control_constraints_.max_decel_angular > 0.0) { + state.wz(j, i) = models::clampByAccel( + state.wz(j, i - 1), state.wz(j, i), + control_constraints_.max_accel_angular, control_constraints_.max_decel_angular, model_dt_); + } } } } diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 63d3d3e..54e2417 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -81,7 +81,11 @@ void Optimizer::setParams(const mppi_controller::MPPIControllerConfig& config) s.base_constraints.vx_min = config.vx_min; s.base_constraints.vy = config.vy_max; s.base_constraints.wz = config.wz_max; - s.base_constraints.max_vel_trans = config.max_vel_trans; + s.base_constraints.max_vel_trans = config.max_vel_trans; + s.base_constraints.max_accel_trans = config.max_accel_trans; + s.base_constraints.max_decel_trans = config.max_decel_trans; + s.base_constraints.max_accel_angular = config.max_accel_angular; + s.base_constraints.max_decel_angular = config.max_decel_angular; s.sampling_std.vx = config.vx_std > kMinPositive ? config.vx_std : kMinPositive; s.sampling_std.vy = config.vy_std > kMinPositive ? config.vy_std : kMinPositive; s.sampling_std.wz = config.wz_std > kMinPositive ? config.wz_std : kMinPositive; @@ -306,6 +310,34 @@ void Optimizer::applyControlSequenceConstraints() } motion_model_->applyConstraints(control_sequence_); + + // Acceleration / deceleration constraints on the mean control sequence. + // Step 0 is compared against the current robot velocity (state_.speed). + const auto& c = s.constraints; + if (c.max_accel_trans > 0.0 || c.max_decel_trans > 0.0 || + c.max_accel_angular > 0.0 || c.max_decel_angular > 0.0) + { + float prev_vx = static_cast(state_.speed.linear.x); + float prev_vy = isHolonomic() ? static_cast(state_.speed.linear.y) : 0.0f; + float prev_wz = static_cast(state_.speed.angular.z); + const float dt = static_cast(s.model_dt); + + for (unsigned int i = 0; i < control_sequence_.vx.shape(0); ++i) { + control_sequence_.vx(i) = models::clampByAccel( + prev_vx, control_sequence_.vx(i), c.max_accel_trans, c.max_decel_trans, dt); + prev_vx = control_sequence_.vx(i); + + if (isHolonomic()) { + control_sequence_.vy(i) = models::clampByAccel( + prev_vy, control_sequence_.vy(i), c.max_accel_trans, c.max_decel_trans, dt); + prev_vy = control_sequence_.vy(i); + } + + control_sequence_.wz(i) = models::clampByAccel( + prev_wz, control_sequence_.wz(i), c.max_accel_angular, c.max_decel_angular, dt); + prev_wz = control_sequence_.wz(i); + } + } } void Optimizer::updateStateVelocities(models::State& state) const From 26b4aa76870fe39b85698bb1b548c548289e7251 Mon Sep 17 00:00:00 2001 From: Nisarg Date: Thu, 4 Jun 2026 15:27:25 +0900 Subject: [PATCH 2/2] fixes for acccceleration --- .../models/optimizer_settings.hpp | 2 + include/mppi_controller/optimizer.hpp | 10 +- include/mppi_controller/tools/utils.hpp | 13 +- src/optimizer.cpp | 137 ++++++++++++++---- 4 files changed, 129 insertions(+), 33 deletions(-) diff --git a/include/mppi_controller/models/optimizer_settings.hpp b/include/mppi_controller/models/optimizer_settings.hpp index b78122b..a945626 100644 --- a/include/mppi_controller/models/optimizer_settings.hpp +++ b/include/mppi_controller/models/optimizer_settings.hpp @@ -31,6 +31,7 @@ struct OptimizerSettings models::ControlConstraints constraints{ 0, 0, 0, 0, 0}; models::SamplingStd sampling_std{ 0, 0, 0 }; double model_dt{ 0 }; + float controller_period{ 0.0f }; double temperature{ 0 }; double gamma{ 0 }; int batch_size{ 0 }; @@ -39,6 +40,7 @@ struct OptimizerSettings bool shift_control_sequence{ false }; int retry_attempt_limit{ 0 }; bool open_loop{ false }; + unsigned int sgf_order{ 2u }; }; } // namespace mppi::models diff --git a/include/mppi_controller/optimizer.hpp b/include/mppi_controller/optimizer.hpp index 927c229..1f3164a 100644 --- a/include/mppi_controller/optimizer.hpp +++ b/include/mppi_controller/optimizer.hpp @@ -205,6 +205,12 @@ class Optimizer */ void generateNoisedTrajectories(); + /** + * @brief Apply inter-iteration dynamic feasibility constraints on the + * first control sequence element before noise generation + */ + void applyControlSequenceInterIterationConstraints(); + /** * @brief Apply hard vehicle constraints on control sequence */ @@ -269,10 +275,10 @@ class Optimizer bool isHolonomic() const; /** - * @brief Using control frequency and time step size, determine if trajectory + * @brief Using control period and time step size, determine if trajectory * offset should be used to populate initial state of the next cycle */ - void setOffset(double controller_frequency); + void setOffset(double controller_period); /** * @brief Perform fallback behavior to try to recover from a set of trajectories in collision diff --git a/include/mppi_controller/tools/utils.hpp b/include/mppi_controller/tools/utils.hpp index 284ee2a..867883f 100644 --- a/include/mppi_controller/tools/utils.hpp +++ b/include/mppi_controller/tools/utils.hpp @@ -473,9 +473,16 @@ inline void savitskyGolayFilter(models::ControlSequence& control_sequence, std::array& control_history, const models::OptimizerSettings& settings) { - // Savitzky-Golay Quadratic, 9-point Coefficients - xt::xarray filter = { -21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0 }; - filter /= 231.0; + // Savitzky-Golay filter coefficients, 9-point window + xt::xarray filter; + if (settings.sgf_order == 1) { + // Degree-1 (linear): uniform moving average with more aggressive smoothing + filter = { 1.0f/9, 1.0f/9, 1.0f/9, 1.0f/9, 1.0f/9, 1.0f/9, 1.0f/9, 1.0f/9, 1.0f/9 }; + } else { + // Degree-2 (quadratic): standard 9-point SG coefficients + filter = { -21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0 }; + filter /= 231.0; + } const unsigned int num_sequences = control_sequence.vx.shape(0) - 1; diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 54e2417..a994558 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -100,16 +100,17 @@ void Optimizer::setParams(const mppi_controller::MPPIControllerConfig& config) s.retry_attempt_limit, s.base_constraints.vx_max, s.base_constraints.vx_min, s.base_constraints.vy, s.base_constraints.wz, s.sampling_std.vx, s.sampling_std.vy, s.sampling_std.wz); + s.controller_period = static_cast(1.0 / config.controller_frequency); + setMotionModel(config.motion_model); setVisualize(config.visualize); - setOffset(config.controller_frequency); + setOffset(s.controller_period); noise_generator_.setParams(config); reset(); } -void Optimizer::setOffset(double controller_frequency) +void Optimizer::setOffset(double controller_period) { - const double controller_period = 1.0 / controller_frequency; constexpr double eps = 1e-6; if ((controller_period + eps) < settings_.model_dt) @@ -143,10 +144,7 @@ void Optimizer::reset() {static_cast(settings_.time_steps), static_cast(3)}); optimal_trajectory_.fill(0.0f); - if (settings_.open_loop) - { - last_command_vel_ = geometry_msgs::Twist(); - } + last_command_vel_ = geometry_msgs::Twist(); critic_manager_.updateConstraints(settings_.constraints); if (trajectory_validator_ && settings_.model_dt > 0.0 && settings_.time_steps > 0) @@ -188,10 +186,7 @@ uint32_t Optimizer::evalControl(const geometry_msgs::PoseStamped& robot_pose, co utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); cmd_vel = getControlFromSequenceAsTwist(plan.header); - if (settings_.open_loop) - { - last_command_vel_ = cmd_vel.twist; - } + last_command_vel_ = cmd_vel.twist; if (settings_.shift_control_sequence) { @@ -237,8 +232,36 @@ bool Optimizer::fallback(bool fail, uint32_t& error) void Optimizer::prepare(const geometry_msgs::PoseStamped& robot_pose, const geometry_msgs::Twist& robot_speed, const nav_msgs::Path& plan) { + if (settings_.open_loop) { + state_.speed = last_command_vel_; + } else { + // Predict state one controller_period forward toward the last command to compensate + // for the latency between measurement and when this command will take effect. + // Clamp to physically achievable range so prediction never exceeds dynamics. + const auto& c = settings_.constraints; + const float dt = settings_.controller_period; + state_.speed = robot_speed; + if (c.max_accel_trans > 0.0 || c.max_decel_trans > 0.0) { + state_.speed.linear.x = std::clamp( + last_command_vel_.linear.x, + robot_speed.linear.x - dt * c.max_decel_trans, + robot_speed.linear.x + dt * c.max_accel_trans); + if (isHolonomic()) { + state_.speed.linear.y = std::clamp( + last_command_vel_.linear.y, + robot_speed.linear.y - dt * c.max_decel_trans, + robot_speed.linear.y + dt * c.max_accel_trans); + } + } + if (c.max_accel_angular > 0.0 || c.max_decel_angular > 0.0) { + state_.speed.angular.z = std::clamp( + last_command_vel_.angular.z, + robot_speed.angular.z - dt * c.max_decel_angular, + robot_speed.angular.z + dt * c.max_accel_angular); + } + } + state_.pose = robot_pose; - state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed; path_ = utils::toTensor(plan); costs_.fill(0); critics_data_.trajectories_in_collision.assign(costs_.shape(0), false); @@ -268,12 +291,54 @@ void Optimizer::shiftControlSequence() void Optimizer::generateNoisedTrajectories() { + applyControlSequenceInterIterationConstraints(); noise_generator_.setNoisedControls(state_, control_sequence_); noise_generator_.generateNextNoises(); updateStateVelocities(state_); integrateStateVelocities(generated_trajectories_, state_); } +void Optimizer::applyControlSequenceInterIterationConstraints() +{ + // Enforce t=0 to be dynamically feasible from the current speed for inter-iteration feasibility. + // Re-centers the noise distribution at t=0, which is still information-theoretically sound. + auto& s = settings_; + const float dt = s.controller_period; + const float speed_vx = static_cast(state_.speed.linear.x); + const float speed_wz = static_cast(state_.speed.angular.z); + + if (s.shift_control_sequence) { + // When shifting, vx(0) is "now" (not sent), so pin it to current speed so + // vx(1) is at most one constraint step away. + control_sequence_.vx(0) = speed_vx; + control_sequence_.wz(0) = speed_wz; + if (isHolonomic()) { + control_sequence_.vy(0) = static_cast(state_.speed.linear.y); + } + } else { + // When not shifting, vx(0) is the sent command — clamp it to the feasible envelope. + const auto& c = s.constraints; + if (c.max_accel_trans > 0.0 || c.max_decel_trans > 0.0) { + control_sequence_.vx(0) = models::clampByAccel( + speed_vx, control_sequence_.vx(0), + c.max_accel_trans, c.max_decel_trans, dt); + } + if (c.max_accel_angular > 0.0 || c.max_decel_angular > 0.0) { + control_sequence_.wz(0) = models::clampByAccel( + speed_wz, control_sequence_.wz(0), + c.max_accel_angular, c.max_decel_angular, dt); + } + if (isHolonomic()) { + const float speed_vy = static_cast(state_.speed.linear.y); + if (c.max_accel_trans > 0.0 || c.max_decel_trans > 0.0) { + control_sequence_.vy(0) = models::clampByAccel( + speed_vy, control_sequence_.vy(0), + c.max_accel_trans, c.max_decel_trans, dt); + } + } + } +} + bool Optimizer::isHolonomic() const { return is_holonomic_; @@ -291,17 +356,12 @@ void Optimizer::applyControlSequenceConstraints() control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max); control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz); - //max_vel_trans constraint + // max_vel_trans constraint float max_vel_trans = s.constraints.max_vel_trans; - - for (unsigned int i = 1; i != control_sequence_.vx.shape(0); i++) { + for (unsigned int i = 0; i != control_sequence_.vx.shape(0); i++) { float vx_curr = control_sequence_.vx(i); float vy_curr = control_sequence_.vy(i); - float wz_curr = control_sequence_.wz(i); - - // Apply max_vel_trans constraint float speed = std::hypot(vx_curr, vy_curr); - if (speed > max_vel_trans) { float scale = max_vel_trans / speed; control_sequence_.vx(i) = vx_curr * scale; @@ -312,7 +372,9 @@ void Optimizer::applyControlSequenceConstraints() motion_model_->applyConstraints(control_sequence_); // Acceleration / deceleration constraints on the mean control sequence. - // Step 0 is compared against the current robot velocity (state_.speed). + // Initialize from current robot speed (state_.speed) for inter-iteration feasibility. + // Use controller_period for t=0 (time until the sent command takes effect), + // then switch to model_dt for the rest of the MPC horizon. const auto& c = s.constraints; if (c.max_accel_trans > 0.0 || c.max_decel_trans > 0.0 || c.max_accel_angular > 0.0 || c.max_decel_angular > 0.0) @@ -320,24 +382,43 @@ void Optimizer::applyControlSequenceConstraints() float prev_vx = static_cast(state_.speed.linear.x); float prev_vy = isHolonomic() ? static_cast(state_.speed.linear.y) : 0.0f; float prev_wz = static_cast(state_.speed.angular.z); - const float dt = static_cast(s.model_dt); + + // When shifting, t=0 is "now" — pin it to current speed so that t=1 (the sent + // command) is one constraint step away. + if (s.shift_control_sequence) { + control_sequence_.vx(0) = prev_vx; + control_sequence_.wz(0) = prev_wz; + if (isHolonomic()) { + control_sequence_.vy(0) = prev_vy; + } + } for (unsigned int i = 0; i < control_sequence_.vx.shape(0); ++i) { - control_sequence_.vx(i) = models::clampByAccel( + // t=0 uses controller_period; subsequent steps use model_dt + const float dt = (i == 0) ? s.controller_period : static_cast(s.model_dt); + + if (c.max_accel_trans > 0.0 || c.max_decel_trans > 0.0) { + control_sequence_.vx(i) = models::clampByAccel( prev_vx, control_sequence_.vx(i), c.max_accel_trans, c.max_decel_trans, dt); - prev_vx = control_sequence_.vx(i); + prev_vx = control_sequence_.vx(i); - if (isHolonomic()) { - control_sequence_.vy(i) = models::clampByAccel( + if (isHolonomic()) { + control_sequence_.vy(i) = models::clampByAccel( prev_vy, control_sequence_.vy(i), c.max_accel_trans, c.max_decel_trans, dt); - prev_vy = control_sequence_.vy(i); + prev_vy = control_sequence_.vy(i); + } } - control_sequence_.wz(i) = models::clampByAccel( + if (c.max_accel_angular > 0.0 || c.max_decel_angular > 0.0) { + control_sequence_.wz(i) = models::clampByAccel( prev_wz, control_sequence_.wz(i), c.max_accel_angular, c.max_decel_angular, dt); - prev_wz = control_sequence_.wz(i); + prev_wz = control_sequence_.wz(i); + } } } + + // Apply again to ensure acceleration constraints don't violate specialty limits (e.g. Ackermann) + motion_model_->applyConstraints(control_sequence_); } void Optimizer::updateStateVelocities(models::State& state) const