Backport bidirectional settings #4954 (#5260)

Signed-off-by: Tatsuro Sakaguchi <[email protected]>

Author: Tacha-S

nav2_bringup/params/nav2_params.yaml

@@ -102,6 +102,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

@@ -31,6 +31,7 @@ struct ControlConstraints
   float ax_max;
   float ax_min;
   float ay_max;
+  float ay_min;
   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

@@ -86,14 +86,19 @@ class MotionModel
     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;
     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++) {
         float & cvx_curr = state.cvx(i, j - 1);
-        cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
+        if (vx_last > 0) {
+          cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
+        } else {
+          cvx_curr = std::clamp(cvx_curr, vx_last - max_delta_vx, vx_last - min_delta_vx);
+        }
         state.vx(i, j) = cvx_curr;
         vx_last = cvx_curr;
 
@@ -104,7 +109,11 @@ class MotionModel
 
         if (is_holo) {
           float & cvy_curr = state.cvy(i, j - 1);
-          cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy);
+          if (vy_last > 0) {
+            cvy_curr = std::clamp(cvy_curr, vy_last + min_delta_vy, vy_last + max_delta_vy);
+          } else {
+            cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last - min_delta_vy);
+          }
           state.vy(i, j) = cvy_curr;
           vy_last = cvy_curr;
         }
@@ -127,7 +136,7 @@ class MotionModel
 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

@@ -80,6 +80,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);
@@ -94,6 +95,13 @@ 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;
@@ -267,13 +275,18 @@ 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 = 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);
+    if (vx_last > 0) {
+      vx_curr = std::clamp(vx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
+    } else {
+      vx_curr = std::clamp(vx_curr, vx_last - max_delta_vx, vx_last - min_delta_vx);
+    }
     vx_last = vx_curr;
 
     float & wz_curr = control_sequence_.wz(i);
@@ -282,7 +295,11 @@ void Optimizer::applyControlSequenceConstraints()
 
     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);
+      if (vy_last > 0) {
+        vy_curr = std::clamp(vy_curr, vy_last + min_delta_vy, vy_last + max_delta_vy);
+      } else {
+        vy_curr = std::clamp(vy_curr, vy_last - max_delta_vy, vy_last - min_delta_vy);
+      }
       vy_last = vy_curr;
     }
   }

nav2_mppi_controller/test/optimizer_unit_tests.cpp

@@ -186,13 +186,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
@@ -234,16 +235,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", "", "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
@@ -539,6 +542,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", "", "dummy_costmap", true);