Fix mppi bidirectional settings

nav2_bringup/params/nav2_params.yaml

@@ -113,6 +113,7 @@ controller_server:
       ax_max: 3.0
       ax_min: -3.0
       ay_max: 3.0
+      ay_min: -3.0
       az_max: 3.5
       vx_std: 0.2
       vy_std: 0.2

nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp

@@ -30,6 +30,7 @@ struct ControlConstraints
   float wz;
   float ax_max;
   float ax_min;
+  float ay_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, 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::ControlConstraints base_constraints{0.0f, 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, 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

@@ -75,14 +75,22 @@
     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 min_delta_vy = model_dt_ * control_constraints_.ay_min;
     float max_delta_wz = model_dt_ * control_constraints_.az_max;
 
     unsigned int n_cols = state.vx.cols();
 
     for (unsigned int i = 1; i < n_cols; i++) {
+      Eigen::ArrayXf lower_bound_vx = (state.vx.col(i - 1) >
+        0).select(state.vx.col(i - 1) + min_delta_vx,
+        state.vx.col(i - 1) - max_delta_vx);
+      Eigen::ArrayXf upper_bound_vx = (state.vx.col(i - 1) >
+        0).select(state.vx.col(i - 1) + max_delta_vx,
+        state.vx.col(i - 1) - min_delta_vx);
+
       state.cvx.col(i - 1) = state.cvx.col(i - 1)
-        .cwiseMax(state.vx.col(i - 1) + min_delta_vx)
-        .cwiseMin(state.vx.col(i - 1) + max_delta_vx);
+        .cwiseMax(lower_bound_vx)
+        .cwiseMin(upper_bound_vx);
       state.vx.col(i) = state.cvx.col(i - 1);
 
       state.cwz.col(i - 1) = state.cwz.col(i - 1)
@@ -91,9 +99,15 @@
       state.wz.col(i) = state.cwz.col(i - 1);
 
       if (is_holo) {
+        Eigen::ArrayXf lower_bound_vy = (state.vy.col(i - 1) >
+          0).select(state.vy.col(i - 1) + min_delta_vy,
+          state.vy.col(i - 1) - max_delta_vy);
+        Eigen::ArrayXf upper_bound_vy = (state.vy.col(i - 1) >
+          0).select(state.vy.col(i - 1) + max_delta_vy,
+          state.vy.col(i - 1) - min_delta_vy);
         state.cvy.col(i - 1) = state.cvy.col(i - 1)
-          .cwiseMax(state.vy.col(i - 1) - max_delta_vy)
-          .cwiseMin(state.vy.col(i - 1) + max_delta_vy);
+          .cwiseMax(lower_bound_vy)
+          .cwiseMin(upper_bound_vy);
         state.vy.col(i) = state.cvy.col(i - 1);
       }
     }
@@ -114,7 +128,7 @@
 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};
+    0.0f, 0.0f};
 };
 
 /**

nav2_mppi_controller/src/optimizer.cpp

@@ -75,6 +75,7 @@ void Optimizer::getParams()
   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.ay_min, "ay_min", -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);
@@ -89,6 +90,14 @@ void Optimizer::getParams()
       "Sign of the parameter ax_min is incorrect, consider setting it negative.");
   }
 
+  if (s.base_constraints.ay_min > 0.0) {
+    s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min;
+    RCLCPP_WARN(
+      logger_,
+      "Sign of the parameter ay_min is incorrect, consider setting it negative.");
+  }
+
+
   getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
 
   s.constraints = s.base_constraints;
@@ -245,6 +254,7 @@ void Optimizer::applyControlSequenceConstraints()
   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 min_delta_vy = s.model_dt * s.constraints.ay_min;
   float max_delta_wz = s.model_dt * s.constraints.az_max;
   float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0));
   float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0));
@@ -259,7 +269,11 @@ void Optimizer::applyControlSequenceConstraints()
   for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
     float & vx_curr = control_sequence_.vx(i);
     vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
-    vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
+    if(vx_last > 0) {
+      vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
+    } else {
+      vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr);
+    }
     vx_last = vx_curr;
 
     float & wz_curr = control_sequence_.wz(i);
@@ -270,7 +284,11 @@ void Optimizer::applyControlSequenceConstraints()
     if (isHolonomic()) {
       float & vy_curr = control_sequence_.vy(i);
       vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
-      vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last + max_delta_vy, vy_curr);
+      if(vy_last > 0) {
+        vy_curr = utils::clamp(vy_last + min_delta_vy, vy_last + max_delta_vy, vy_curr);
+      } else {
+        vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last - min_delta_vy, vy_curr);
+      }
       vy_last = vy_curr;
     }
   }

nav2_mppi_controller/test/optimizer_unit_tests.cpp

@@ -177,13 +177,14 @@ class OptimizerTester : public Optimizer
     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 min_delta_vy = settings_.constraints.ay_min * 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), 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.vy(0, 1), std::clamp(0.5, 1.0 + min_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
@@ -225,16 +226,18 @@ TEST(OptimizerTests, BasicInitializedFunctions)
   node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
   node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
   node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(3.0));
+  node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(3.0));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
   ParametersHandler param_handler(node);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
   optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
 
-  // Test value of ax_min, it should be negative
+  // Test value of ax_min, ay_min it should be negative
   auto & constraints = optimizer_tester.getControlConstraints();
   EXPECT_EQ(constraints.ax_min, -3.0);
+  EXPECT_EQ(constraints.ay_min, -3.0);
 
   // Should be empty of size batches x time steps
   // and tests getting set params: time_steps, batch_size, controller_frequency
@@ -530,6 +533,7 @@ TEST(OptimizerTests, updateStateVelocitiesTests)
   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.ay_min", 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", "", true);

nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml

@@ -61,6 +61,7 @@ controller_server:
       ax_max: 3.0
       ax_min: -3.0
       ay_max: 3.0
+      ay_min: -3.0
       az_max: 3.5
       vx_std: 0.2
       vy_std: 0.2