Add acceleration constraints in the MPPI controller

nav2_bringup/params/nav2_params.yaml

@@ -98,6 +98,10 @@ controller_server:
       time_steps: 56
       model_dt: 0.05
       batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      az_max: 3.5
       vx_std: 0.2
       vy_std: 0.2
       wz_std: 0.4

nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp

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

nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp

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

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -16,9 +16,11 @@
 #define NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_
 
 #include <cstdint>
+#include <string>
 
 #include "nav2_mppi_controller/models/control_sequence.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/models/constraints.hpp"
 
 // xtensor creates warnings that needs to be ignored as we are building with -Werror
 #pragma GCC diagnostic push
@@ -52,6 +54,17 @@ 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
@@ -70,12 +83,30 @@ class MotionModel
     // }
 
     const bool is_holo = isHolonomic();
+    float max_delta_vx = model_dt_ * control_constraints_.ax_max;
+    float min_delta_vx = model_dt_ * control_constraints_.ax_min;
+    float max_delta_vy = model_dt_ * control_constraints_.ay_max;
+    float 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++) {
-        state.vx(i, j) = state.cvx(i, j - 1);
-        state.wz(i, j) = state.cwz(i, j - 1);
+        float & 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;
+
+        float & 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) {
-          state.vy(i, j) = state.cvy(i, j - 1);
+          float & 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;
         }
       }
     }
@@ -92,6 +123,11 @@ class MotionModel
    * @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};
 };
 
 /**

nav2_mppi_controller/src/optimizer.cpp

@@ -77,11 +77,23 @@ void Optimizer::getParams()
   getParam(s.base_constraints.vx_min, "vx_min", -0.35f);
   getParam(s.base_constraints.vy, "vy_max", 0.5f);
   getParam(s.base_constraints.wz, "wz_max", 1.9f);
+  getParam(s.base_constraints.ax_max, "ax_max", 3.0f);
+  getParam(s.base_constraints.ax_min, "ax_min", -3.0f);
+  getParam(s.base_constraints.ay_max, "ay_max", 3.0f);
+  getParam(s.base_constraints.az_max, "az_max", 3.5f);
   getParam(s.sampling_std.vx, "vx_std", 0.2f);
   getParam(s.sampling_std.vy, "vy_std", 0.2f);
   getParam(s.sampling_std.wz, "wz_std", 0.4f);
   getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
 
+  s.base_constraints.ax_max = std::abs(s.base_constraints.ax_max);
+  if (s.base_constraints.ax_min > 0) {
+    s.base_constraints.ax_min = -1.0 * s.base_constraints.ax_min;
+    RCLCPP_WARN(
+      logger_,
+      "Sign of the parameter ax_min is incorrect, consider setting it negative.");
+  }
+
   getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
 
   s.constraints = s.base_constraints;
@@ -243,6 +255,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_);
 }
 
@@ -414,6 +449,7 @@ void Optimizer::setMotionModel(const std::string & model)
               "Model " + model + " is not valid! Valid options are DiffDrive, Omni, "
               "or Ackermann"));
   }
+  motion_model_->initialize(settings_.constraints, settings_.model_dt);
 }
 
 void Optimizer::setSpeedLimit(double speed_limit, bool percentage)

nav2_mppi_controller/test/optimizer_unit_tests.cpp

@@ -176,13 +176,17 @@ class OptimizerTester : public Optimizer
     EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
 
     // propagateStateVelocitiesFromInitials
+    float max_delta_vx = settings_.constraints.ax_max * settings_.model_dt;
+    float min_delta_vx = settings_.constraints.ax_min * settings_.model_dt;
+    float max_delta_vy = settings_.constraints.ay_max * settings_.model_dt;
+    float max_delta_wz = settings_.constraints.az_max * settings_.model_dt;
     propagateStateVelocitiesFromInitials(state);
     EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
     EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
     EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
-    EXPECT_NEAR(state.vx(0, 1), 0.75, 1e-6);
-    EXPECT_NEAR(state.vy(0, 1), 0.5, 1e-6);
-    EXPECT_NEAR(state.wz(0, 1), 0.1, 1e-6);
+    EXPECT_NEAR(state.vx(0, 1), std::clamp(0.75, 5.0 + min_delta_vx, 5.0 + max_delta_vx), 1e-6);
+    EXPECT_NEAR(state.vy(0, 1), std::clamp(0.5, 1.0 - max_delta_vy, 1.0 + max_delta_vy), 1e-6);
+    EXPECT_NEAR(state.wz(0, 1), std::clamp(0.1, 6.0 - max_delta_wz, 6.0 + max_delta_wz), 1e-6);
 
     // Putting them together: updateStateVelocities
     state.reset(1000, 50);
@@ -196,9 +200,9 @@ class OptimizerTester : public Optimizer
     EXPECT_NEAR(state.vx(0, 0), -5.0, 1e-6);
     EXPECT_NEAR(state.vy(0, 0), -1.0, 1e-6);
     EXPECT_NEAR(state.wz(0, 0), -6.0, 1e-6);
-    EXPECT_NEAR(state.vx(0, 1), -0.75, 1e-6);
-    EXPECT_NEAR(state.vy(0, 1), -0.5, 1e-6);
-    EXPECT_NEAR(state.wz(0, 1), -0.1, 1e-6);
+    EXPECT_NEAR(state.vx(0, 1), std::clamp(-0.75, -5.0 + min_delta_vx, -5.0 + max_delta_vx), 1e-6);
+    EXPECT_NEAR(state.vy(0, 1), std::clamp(-0.5, -1.0 - max_delta_vy, -1.0 + max_delta_vy), 1e-6);
+    EXPECT_NEAR(state.wz(0, 1), std::clamp(-0.1, -6.0 - max_delta_wz, -6.0 + max_delta_wz), 1e-6);
   }
 
   geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwistWrapper()
@@ -519,6 +523,10 @@ TEST(OptimizerTests, updateStateVelocitiesTests)
   node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
   node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60));
   node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
+  node->declare_parameter("mppic.ax_max", rclcpp::ParameterValue(3.0));
+  node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(-3.0));
+  node->declare_parameter("mppic.ay_max", rclcpp::ParameterValue(3.0));
+  node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(3.5));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", "dummy_costmap", true);
   ParametersHandler param_handler(node);