feat: add acceleration/deceleration limits

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)

include/mppi_controller/models/constraints.hpp

@@ -15,6 +15,8 @@
 #ifndef MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
 #define MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
 
+#include <cmath>
+
 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<float>(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<float>(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_

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

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_);
+        }
       }
     }
   }

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

include/mppi_controller/tools/utils.hpp

@@ -473,9 +473,16 @@ inline void savitskyGolayFilter(models::ControlSequence& control_sequence,
                                 std::array<mppi::models::Control, 4>& control_history,
                                 const models::OptimizerSettings& settings)
 {
-  // Savitzky-Golay Quadratic, 9-point Coefficients
-  xt::xarray<float> 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<float> 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;
 

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;
@@ -96,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<float>(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)
@@ -139,10 +144,7 @@ void Optimizer::reset()
     {static_cast<size_t>(settings_.time_steps), static_cast<size_t>(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)
@@ -184,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)
   {
@@ -233,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);
@@ -264,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<float>(state_.speed.linear.x);
+  const float speed_wz = static_cast<float>(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<float>(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<float>(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_;
@@ -287,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;
@@ -306,6 +370,55 @@ void Optimizer::applyControlSequenceConstraints()
   }
 
   motion_model_->applyConstraints(control_sequence_);
+
+  // Acceleration / deceleration constraints on the mean control sequence.
+  // 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)
+  {
+    float prev_vx = static_cast<float>(state_.speed.linear.x);
+    float prev_vy = isHolonomic() ? static_cast<float>(state_.speed.linear.y) : 0.0f;
+    float prev_wz = static_cast<float>(state_.speed.angular.z);
+
+    // 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) {
+      // t=0 uses controller_period; subsequent steps use model_dt
+      const float dt = (i == 0) ? s.controller_period : static_cast<float>(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);
+
+        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);
+        }
+      }
+
+      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);
+      }
+    }
+  }
+
+  // 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