Preserve gaussian proposals and enforce accel limits at command boundary

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -89,15 +89,19 @@ class MotionModel
         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)
+      state.vx.col(i) = state.cvx.col(i - 1)
         .cwiseMax(lower_bound_vx)
         .cwiseMin(upper_bound_vx);
-      state.vx.col(i) = state.cvx.col(i - 1);
+      state.vx.col(i) = state.vx.col(i)
+        .cwiseMax(control_constraints_.vx_min)
+        .cwiseMin(control_constraints_.vx_max);
 
-      state.cwz.col(i - 1) = state.cwz.col(i - 1)
+      state.wz.col(i) = state.cwz.col(i - 1)
         .cwiseMax(state.wz.col(i - 1) - max_delta_wz)
         .cwiseMin(state.wz.col(i - 1) + max_delta_wz);
-      state.wz.col(i) = state.cwz.col(i - 1);
+      state.wz.col(i) = state.wz.col(i)
+        .cwiseMax(-control_constraints_.wz)
+        .cwiseMin(control_constraints_.wz);
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
@@ -106,10 +110,12 @@ class MotionModel
         auto 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)
+        state.vy.col(i) = state.cvy.col(i - 1)
           .cwiseMax(lower_bound_vy)
           .cwiseMin(upper_bound_vy);
-        state.vy.col(i) = state.cvy.col(i - 1);
+        state.vy.col(i) = state.vy.col(i)
+          .cwiseMax(-control_constraints_.vy)
+          .cwiseMin(control_constraints_.vy);
       }
     }
   }

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -230,6 +230,11 @@ class Optimizer
    */
   void updateControlSequence();
 
+  /**
+   * @brief Update control history with the final command
+   */
+  void updateHistory();
+
   /**
    * @brief Convert control sequence to a twist command
    * @param stamp Timestamp to use

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -431,12 +431,10 @@ inline float posePointAngle(
  * @brief Apply Savisky-Golay filter to optimal trajectory
  * @param control_sequence Sequence to apply filter to
  * @param control_history Recent set of controls for edge-case handling
- * @param Settings Settings to use
  */
 inline void savitskyGolayFilter(
   models::ControlSequence & control_sequence,
-  std::array<mppi::models::Control, 4> & control_history,
-  const models::OptimizerSettings & settings)
+  std::array<mppi::models::Control, 4> & control_history)
 {
   // Savitzky-Golay Quadratic, 9-point Coefficients
   Eigen::Array<float, 9, 1> filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f,
@@ -498,16 +496,6 @@ inline void savitskyGolayFilter(
   applyFilterOverAxis(
     control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
     control_history[1].wz, control_history[2].wz, control_history[3].wz);
-
-  // Update control history
-  unsigned int offset = settings.shift_control_sequence ? 1 : 0;
-  control_history[0] = control_history[1];
-  control_history[1] = control_history[2];
-  control_history[2] = control_history[3];
-  control_history[3] = {
-    control_sequence.vx(offset),
-    control_sequence.vy(offset),
-    control_sequence.wz(offset)};
 }
 
 /**

nav2_mppi_controller/src/optimizer.cpp

@@ -192,6 +192,9 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
 
   do {
     optimize();
+    utils::savitskyGolayFilter(control_sequence_, control_history_);
+    applyControlSequenceConstraints();
+    updateHistory();
     optimal_trajectory = getOptimizedTrajectory();
     switch (trajectory_validator_->validateTrajectory(
       optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal))
@@ -210,7 +213,6 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
     }
   } while (fallback(critics_data_.fail_flag || !trajectory_valid));
 
-  utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
   auto control = getControlFromSequenceAsTwist(plan.header.stamp);
 
   if (settings_.shift_control_sequence) {
@@ -300,14 +302,39 @@ void Optimizer::applyControlSequenceConstraints()
   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));
-  control_sequence_.vx(0) = vx_last;
-  control_sequence_.wz(0) = wz_last;
+
+  float& vx0 = control_sequence_.vx(0);
+  vx0 = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx0);
+  if (state_.speed.linear.x > 0.0f) {
+    vx0 = utils::clamp(state_.speed.linear.x + min_delta_vx,
+      state_.speed.linear.x + max_delta_vx, vx0);
+  } else {
+    vx0 = utils::clamp(state_.speed.linear.x - max_delta_vx,
+      state_.speed.linear.x - min_delta_vx, vx0);
+  }
+
+  float& wz0 = control_sequence_.wz(0);
+  wz0 = utils::clamp(-s.constraints.wz, s.constraints.wz, wz0);
+  wz0 = utils::clamp(state_.speed.angular.z - max_delta_wz,
+    state_.speed.angular.z + max_delta_wz, wz0);
+
+  if (isHolonomic()) {
+    float& vy0 = control_sequence_.vy(0);
+    vy0 = utils::clamp(-s.constraints.vy, s.constraints.vy, vy0);
+    if (state_.speed.linear.y > 0.0f) {
+      vy0 = utils::clamp(state_.speed.linear.y + min_delta_vy,
+             state_.speed.linear.y + max_delta_vy, vy0);
+    } else {
+      vy0 = utils::clamp(state_.speed.linear.y - max_delta_vy,
+             state_.speed.linear.y - min_delta_vy, vy0);
+    }
+  }
+
+  float vx_last = control_sequence_.vx(0);
+  float wz_last = control_sequence_.wz(0);
   float vy_last = 0;
   if (isHolonomic()) {
-    vy_last = utils::clamp(-s.constraints.vy, s.constraints.vy, control_sequence_.vy(0));
-    control_sequence_.vy(0) = vy_last;
+    vy_last = control_sequence_.vy(0);
   }
 
   for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
@@ -484,22 +511,22 @@ void Optimizer::updateControlSequence()
   auto & s = settings_;
 
   auto vx_T = control_sequence_.vx.transpose();
-  auto bounded_noises_vx = state_.cvx.rowwise() - vx_T;
+  auto noises_vx = state_.cvx.rowwise() - vx_T;
   const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx);
-  costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
+  costs_ += (gamma_vx * (noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
 
   if (s.sampling_std.wz > 0.0f) {
     auto wz_T = control_sequence_.wz.transpose();
-    auto bounded_noises_wz = state_.cwz.rowwise() - wz_T;
+    auto noises_wz = state_.cwz.rowwise() - wz_T;
     const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz);
-    costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
+    costs_ += (gamma_wz * (noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
   }
 
   if (is_holo) {
     auto vy_T = control_sequence_.vy.transpose();
-    auto bounded_noises_vy = state_.cvy.rowwise() - vy_T;
+    auto noises_vy = state_.cvy.rowwise() - vy_T;
     const float gamma_vy = s.gamma / (s.sampling_std.vy * s.sampling_std.vy);
-    costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval();
+    costs_ += (gamma_vy * (noises_vy.rowwise() * vy_T).rowwise().sum()).eval();
   }
 
   auto costs_normalized = costs_ - costs_.minCoeff();
@@ -518,16 +545,23 @@ void Optimizer::updateControlSequence()
   applyControlSequenceConstraints();
 }
 
+void Optimizer::updateHistory() {
+  control_history_[0] = control_history_[1];
+  control_history_[1] = control_history_[2];
+  control_history_[2] = control_history_[3];
+  control_history_[3] = {control_sequence_.vx(0),
+                         isHolonomic() ? control_sequence_.vy(0) : 0.0f,
+                         control_sequence_.wz(0)};
+}
+
 geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
   const builtin_interfaces::msg::Time & stamp)
 {
-  unsigned int offset = settings_.shift_control_sequence ? 1 : 0;
-
-  auto vx = control_sequence_.vx(offset);
-  auto wz = control_sequence_.wz(offset);
+  auto vx = control_sequence_.vx(0);
+  auto wz = control_sequence_.wz(0);
 
   if (isHolonomic()) {
-    auto vy = control_sequence_.vy(offset);
+    auto vy = control_sequence_.vy(0);
     return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());
   }
 

nav2_mppi_controller/test/utils_test.cpp

@@ -310,20 +310,21 @@ TEST(UtilsTests, SmootherTest)
   history[0].wz = 0.0;
   history_init = history;
 
-  models::OptimizerSettings settings;
-  settings.shift_control_sequence = false;  // so result stores 0th value in history
-
-  savitskyGolayFilter(noisey_sequence, history, settings);
-
-  // Check history is propagated backward
-  EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02);
-  EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02);
-  EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02);
-
-  // Check history element is updated for first command
-  EXPECT_NEAR(history[3].vx, 0.2, 0.05);
-  EXPECT_NEAR(history[3].vy, 0.0, 0.035);
-  EXPECT_NEAR(history[3].wz, 0.23, 0.02);
+  savitskyGolayFilter(noisey_sequence, history);
+
+  // History should remain unchanged
+  EXPECT_NEAR(history_init[0].vx, history[0].vx, 1e-6);
+  EXPECT_NEAR(history_init[0].vy, history[0].vy, 1e-6);
+  EXPECT_NEAR(history_init[0].wz, history[0].wz, 1e-6);
+  EXPECT_NEAR(history_init[1].vx, history[1].vx, 1e-6);
+  EXPECT_NEAR(history_init[1].vy, history[1].vy, 1e-6);
+  EXPECT_NEAR(history_init[1].wz, history[1].wz, 1e-6);
+  EXPECT_NEAR(history_init[2].vx, history[2].vx, 1e-6);
+  EXPECT_NEAR(history_init[2].vy, history[2].vy, 1e-6);
+  EXPECT_NEAR(history_init[2].wz, history[2].wz, 1e-6);
+  EXPECT_NEAR(history_init[3].vx, history[3].vx, 1e-6);
+  EXPECT_NEAR(history_init[3].vy, history[3].vy, 1e-6);
+  EXPECT_NEAR(history_init[3].wz, history[3].wz, 1e-6);
 
   // Check that path is smoother
   float smoothed_val{0}, original_val{0};