Add acceleration constraints to MPPI controller

cfg/MPPIController.cfg

@@ -15,6 +15,12 @@ grp_robot.add("vy_max", double_t, 0, "Max VY (m/s)", 0.5, 0.0, 100.0)
 grp_robot.add("vx_min", double_t, 0, "Min VX (m/s)", -0.35, -10.0, 0.0)
 grp_robot.add("vy_min", double_t, 0, "Min VY (m/s)", 0.5, 0.0, 10.0)
 grp_robot.add("wz_max", double_t, 0, "Max WZ (rad/s)", 1.9, 0.0, 10.0)
+
+grp_robot.add("ax_max", double_t, 0, "Max Acc x (m/s^2)", 3.0, 0.0, 10.0)
+grp_robot.add("ax_min", double_t, 0, "Min Acc x (m/s^2)", -3.0, -10.0, 0.0)
+grp_robot.add("ay_max", double_t, 0, "Max Acc y (m/s^2)", 3.0, 0.0, 10.0)
+grp_robot.add("az_max", double_t, 0, "Max Acc z (m/s^2)", 3.0, 0.0, 10.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

@@ -28,6 +28,10 @@ struct ControlConstraints
   double vx_min;
   double vy;
   double wz;
+  double ax_max;
+  double ax_min;
+  double ay_max;
+  double az_max;
 };
 
 /**

include/mppi_controller/models/optimizer_settings.hpp

@@ -27,8 +27,8 @@ namespace mppi::models
  */
 struct OptimizerSettings
 {
-  models::ControlConstraints base_constraints{ 0, 0, 0, 0 };
-  models::ControlConstraints constraints{ 0, 0, 0, 0 };
+  models::ControlConstraints base_constraints{ 0, 0, 0, 0, 0, 0, 0, 0 };
+  models::ControlConstraints constraints{ 0, 0, 0, 0, 0, 0, 0, 0 };
   models::SamplingStd sampling_std{ 0, 0, 0 };
   double model_dt{ 0 };
   double temperature{ 0 };

include/mppi_controller/models/state.hpp

@@ -28,13 +28,13 @@ namespace mppi::models {
  * @brief State information: velocities, controls, poses, speed
  */
 struct State {
-  xt::xtensor<float, 2> vx;
-  xt::xtensor<float, 2> vy;
-  xt::xtensor<float, 2> wz;
+  xt::xtensor<double, 2> vx;
+  xt::xtensor<double, 2> vy;
+  xt::xtensor<double, 2> wz;
 
-  xt::xtensor<float, 2> cvx;
-  xt::xtensor<float, 2> cvy;
-  xt::xtensor<float, 2> cwz;
+  xt::xtensor<double, 2> cvx;
+  xt::xtensor<double, 2> cvy;
+  xt::xtensor<double, 2> cwz;
 
   geometry_msgs::PoseStamped pose;
   geometry_msgs::Twist speed;
@@ -43,13 +43,13 @@ struct State {
    * @brief Reset state data
    */
   void reset(unsigned int batch_size, unsigned int time_steps) {
-    vx = xt::zeros<float>({batch_size, time_steps});
-    vy = xt::zeros<float>({batch_size, time_steps});
-    wz = xt::zeros<float>({batch_size, time_steps});
+    vx = xt::zeros<double>({batch_size, time_steps});
+    vy = xt::zeros<double>({batch_size, time_steps});
+    wz = xt::zeros<double>({batch_size, time_steps});
 
-    cvx = xt::zeros<float>({batch_size, time_steps});
-    cvy = xt::zeros<float>({batch_size, time_steps});
-    cwz = xt::zeros<float>({batch_size, time_steps});
+    cvx = xt::zeros<double>({batch_size, time_steps});
+    cvy = xt::zeros<double>({batch_size, time_steps});
+    cwz = xt::zeros<double>({batch_size, time_steps});
   }
 };
 }  // namespace mppi::models

include/mppi_controller/motion_models.hpp

@@ -27,6 +27,7 @@
 #include "mppi_controller/AckermannConfig.h"
 #include "mppi_controller/models/control_sequence.hpp"
 #include "mppi_controller/models/state.hpp"
+#include "mppi_controller/models/constraints.hpp"
 
 namespace mppi {
 
@@ -46,22 +47,51 @@ class MotionModel {
    */
   virtual ~MotionModel() = default;
 
+  /**
+    * @brief Initialize motion model on bringup and set required variables
+    * @param control_constraints Constraints on control
+    * @param model_dt duration of a time step
+    */
+  void initialize(const models::ControlConstraints & control_constraints, float model_dt)
+  {
+    control_constraints_ = control_constraints;
+    model_dt_ = model_dt;
+  }
+
   /**
    * @brief With input velocities, find the vehicle's output velocities
    * @param state Contains control velocities to use to populate vehicle
    * velocities
    */
   virtual void predict(models::State& state) {
-    using namespace xt::placeholders;  // NOLINT
-    xt::noalias(xt::view(state.vx, xt::all(), xt::range(1, _))) =
-        xt::view(state.cvx, xt::all(), xt::range(0, -1));
-
-    xt::noalias(xt::view(state.wz, xt::all(), xt::range(1, _))) =
-        xt::view(state.cwz, xt::all(), xt::range(0, -1));
-
-    if (isHolonomic()) {
-      xt::noalias(xt::view(state.vy, xt::all(), xt::range(1, _))) =
-          xt::view(state.cvy, xt::all(), xt::range(0, -1));
+    const bool is_holo = isHolonomic();
+    double max_delta_vx = model_dt_ * control_constraints_.ax_max;
+    double min_delta_vx = model_dt_ * control_constraints_.ax_min;
+    double max_delta_vy = model_dt_ * control_constraints_.ay_max;
+    double max_delta_wz = model_dt_ * control_constraints_.az_max;
+
+    for (unsigned int i = 0; i != state.vx.shape(0); i++) {
+      float vx_last = state.vx(i, 0);
+      float vy_last = state.vy(i, 0);
+      float wz_last = state.wz(i, 0);
+      for (unsigned int j = 1; j != state.vx.shape(1); j++) {
+        double & cvx_curr = state.cvx(i, j - 1);
+        cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
+        state.vx(i, j) = cvx_curr;
+        vx_last = cvx_curr;
+
+        double & cwz_curr = state.cwz(i, j - 1);
+        cwz_curr = std::clamp(cwz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz);
+        state.wz(i, j) = cwz_curr;
+        wz_last = cwz_curr;
+
+        if (is_holo) {
+          double & cvy_curr = state.cvy(i, j - 1);
+          cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy);
+          state.vy(i, j) = cvy_curr;
+          vy_last = cvy_curr;
+        }
+      }
     }
   }
 
@@ -71,12 +101,18 @@ class MotionModel {
    */
   virtual bool isHolonomic() = 0;
 
+
   /**
    * @brief Apply hard vehicle constraints to a control sequence
    * @param control_sequence Control sequence to apply constraints to
    */
   virtual void applyConstraints(models::ControlSequence& /*control_sequence*/) {
   }
+
+  protected:
+    float model_dt_{0.0};
+  models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
+    0.0f};
 };
 
 /**

src/optimizer.cpp

@@ -61,6 +61,10 @@ void Optimizer::setParams(const mppi_controller::MPPIControllerConfig& config)
   s.retry_attempt_limit = config.retry_attempt_limit;
   s.base_constraints.vx_max = config.vx_max;
   s.base_constraints.vx_min = config.vx_min;
+  s.base_constraints.ax_max = config.ax_max;
+  s.base_constraints.ax_min = config.ax_min;
+  s.base_constraints.ay_max = config.ay_max;
+  s.base_constraints.az_max = config.az_max;
   s.base_constraints.vy = config.vy_max;
   s.base_constraints.wz = config.wz_max;
   s.sampling_std.vx = config.vx_std;
@@ -239,6 +243,29 @@ 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);
 
+  float max_delta_vx = s.model_dt * s.constraints.ax_max;
+  float min_delta_vx = s.model_dt * s.constraints.ax_min;
+  float max_delta_vy = s.model_dt * s.constraints.ay_max;
+  float max_delta_wz = s.model_dt * s.constraints.az_max;
+  float vx_last = control_sequence_.vx(0);
+  float vy_last = control_sequence_.vy(0);
+  float wz_last = control_sequence_.wz(0);
+  for (unsigned int i = 1; i != control_sequence_.vx.shape(0); i++) {
+    float & vx_curr = control_sequence_.vx(i);
+    vx_curr = std::clamp(vx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
+    vx_last = vx_curr;
+
+    float & wz_curr = control_sequence_.wz(i);
+    wz_curr = std::clamp(wz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz);
+    wz_last = wz_curr;
+
+    if (isHolonomic()) {
+      float & vy_curr = control_sequence_.vy(i);
+      vy_curr = std::clamp(vy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy);
+      vy_last = vy_curr;
+    }
+  }
+
   motion_model_->applyConstraints(control_sequence_);
 }
 
@@ -418,6 +445,7 @@ void Optimizer::setMotionModel(const int model)
       motion_model_ = std::make_shared<DiffDriveMotionModel>();
       break;
   }
+  motion_model_->initialize(settings_.constraints, settings_.model_dt);
   is_holonomic_ = motion_model_->isHolonomic();
 }