feat(mppi): fix for gradual drift

Co-authored-by: Cursor <cursoragent@cursor.com>

Author: PanConChicharron

planning/autoware_mppi_optimizer/README.md

@@ -69,6 +69,7 @@ Various features can be disabled by changing the following parameters set in `mp
 ignore_obstacles: true
 ignore_drivable_area: true
 force_cold_start_each_step: true
+use_last_control_as_nominal: true
 ```
 
 Then rebuild / restart the diffusion planner and compare live MPPI to offline retune.
@@ -79,12 +80,14 @@ Notes:
 - `ignore_drivable_area` is retained as an ablation flag; on this stack boundary crash is already
   disabled in the cost (`isEgoOutsideDrivableArea` always false).
 - `force_cold_start_each_step` only resets tracking counters / arc-length (control is already
-  re-seeded from the reference via `updateImportanceSampler(u_nom)` each cycle).
+  re-seeded via `updateImportanceSampler(u_nom)` each cycle).
+- `use_last_control_as_nominal` warm-starts `u_nom` from the shifted previous optimized control
+  sequence when available; otherwise (and on cold start) reseeds from the diffusion reference.
 
 ### Replay only
 
 ```bash
-ros2 run autoware_mppi_optimizer mppi_debug_visualizer.py -- \
+ros2 run autoware_diffusion_planner mppi_debug_visualizer.py -- \
   --log-dir "$HOME/.cache/autoware/mppi_debug_log"
 ```
 
@@ -106,7 +109,7 @@ with diffusion reference (cyan), logged MPPI (red), and retuned MPPI (green):
 
 ```bash
 # Option A — visualizer with retune panel
-ros2 run autoware_mppi_optimizer mppi_debug_visualizer.py -- \
+ros2 run autoware_diffusion_planner mppi_debug_visualizer.py -- \
   --log-dir "$HOME/.cache/autoware/mppi_debug_log" \
   --enable-retune \
   --params-yaml $(ros2 pkg prefix autoware_mppi_optimizer)/share/autoware_mppi_optimizer/config/mppi_optimizer.param.yaml

planning/autoware_mppi_optimizer/config/mppi_optimizer.param.yaml

@@ -2,23 +2,21 @@
   ros__parameters:
     # Vehicle actuation dynamics are loaded from $(vehicle_model)_description/config/simulator_model.param.yaml
 
-    # MPPI controller / FirstOrderDubinsBicycleCostParams
-    # (see first_order_dubins_mppi_interface.cu, first_order_dubins_bicycle_cost.cuh)
-    lambda: 3000.0
+    # FirstOrderDubinsBicycleCostParams (see first_order_dubins_bicycle_cost.cuh)
+    lambda: 14000.0
     desired_speed: 2.5
     speed_coeff: 500.0
-    track_coeff: 1000.0
-    heading_coeff: 500.0
+    track_coeff: 3000.0
+    heading_coeff: 1000.0
     crash_coeff: 100000.0
     boundary_threshold: 0.8
     boundary_threshold_left: -1.0
     boundary_threshold_right: -1.0
     lateral_acceleration_coeff: 500.0
-    lateral_jerk_coeff: 3000.0
-    longitudinal_jerk_coeff: 1000.0
-    accel_cmd_coeff: 200.0
-    steer_cmd_coeff: 1000.0
-    steer_rate_coeff: 5000.0
+    lateral_jerk_coeff: 1000.0
+    longitudinal_jerk_coeff: 10.0
+    accel_cmd_coeff: 0.0
+    steer_cmd_coeff: 10.0
     goal_pos_coeff: 1000.0
     goal_speed_coeff: 200.0
     goal_yaw_coeff: 500.0
@@ -34,3 +32,6 @@
     ignore_obstacles: false
     ignore_drivable_area: true
     force_cold_start_each_step: false
+    
+    # Warm-start u_nom from shifted previous optimized controls (else reseed from DP each cycle).
+    use_last_control_as_nominal: true

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/first_order_dubins_mppi_cost_params.hpp

@@ -34,6 +34,7 @@ struct FirstOrderDubinsMppiCostParams
   float boundary_threshold_right{-1.0F};
   float accel_cmd_coeff{0.0F};
   float steer_cmd_coeff{0.0F};
+  
   float steer_rate_coeff{0.0F};
   float lateral_acceleration_coeff{300.0F};
   float lateral_jerk_coeff{300.0F};

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/first_order_dubins_mppi_interface.hpp

@@ -112,9 +112,13 @@ class FirstOrderDubinsMppiInterface
 
   /**
    * @brief Ablation options to mirror mppi_offline_retune conditions in online sim.
+   * @param use_last_control_as_nominal When true and a previous optimized control sequence
+   *        exists, seed u_nom by shifting that sequence (warm start) instead of reseeding
+   *        from the diffusion reference every cycle.
    */
   void setAblationOptions(
-    bool ignore_obstacles, bool ignore_drivable_area, bool force_cold_start_each_step);
+    bool ignore_obstacles, bool ignore_drivable_area, bool force_cold_start_each_step,
+    bool use_last_control_as_nominal = false);
 
   /**
    * @brief Copy per-rollout raw costs and normalized importance weights from the last
@@ -137,7 +141,8 @@ class FirstOrderDubinsMppiInterface
    * @brief Track a diffusion-planner reference (poses + velocities) with one MPPI step.
    *
    * Uses the diffusion trajectory directly as the MPPI reference horizon (x, y, yaw, v),
-   * seeds u_nom from the reference trajectory controls each cycle, and returns the MPPI-predicted
+   * seeds u_nom from the previous optimized controls when use_last_control_as_nominal is set
+   * (otherwise from the reference trajectory), and returns the MPPI-predicted
    * feasible state rollout that best tracks that reference.
    *
    * @param input Reference trajectory from the diffusion planner (map frame).

planning/autoware_mppi_optimizer/include/autoware/mppi_optimizer/first_order_dubins_mppi_runtime_options.hpp

@@ -29,6 +29,8 @@ struct FirstOrderDubinsMppiRuntimeOptions
   bool ignore_obstacles{false};
   bool ignore_drivable_area{false};
   bool force_cold_start_each_step{false};
+  /** Warm-start u_nom from shifted previous optimized controls (else reseed from DP each cycle). */
+  bool use_last_control_as_nominal{false};
 };
 
 }  // namespace autoware::mppi_optimizer

planning/autoware_mppi_optimizer/src/first_order_dubins/first_order_dubins_mppi_interface.cu

@@ -53,7 +53,9 @@ namespace
 constexpr int kMppiHorizon = 80;
 constexpr int kRefHorizon = kMppiHorizon;
 constexpr float kDt = 0.1F;
+constexpr size_t kMaxIter = 10;
 constexpr int kNumRollouts = 32 * 1024;
+constexpr float kLambda = 1500.0F;
 constexpr float kInitArcLength = 1.5F;
 constexpr size_t kTrackingIndexResetThreshold = 15U;
 // constexpr int kMaxVizRollouts = 200;  // rollout viz disabled
@@ -338,6 +340,39 @@ void buildRolloutVisualization(
 }
 #endif
 
+/// @brief check if the given trajectory needs to be optimized
+[[nodiscard]] bool is_optimization_required(const autoware::mppi_optimizer::Trajectory & trajectory)
+{
+  const auto is_stopping = std::find_if(
+                             trajectory.points.begin(), trajectory.points.end(),
+                             [&](const autoware_planning_msgs::msg::TrajectoryPoint & p) {
+                               return p.longitudinal_velocity_mps < 0.02;
+                             }) != trajectory.points.end();
+  auto length = 0.0;
+  for (auto i = 0UL; i + 1 < trajectory.points.size(); ++i) {
+    length += std::hypot(
+      trajectory.points[i].pose.position.x - trajectory.points[i + 1].pose.position.x,
+      trajectory.points[i].pose.position.y - trajectory.points[i + 1].pose.position.y);
+  }
+  const auto is_short = length < 4.0;
+  return !is_stopping || !is_short;
+}
+
+/// @brief override initial 0 velocities with engage velocities
+void set_initial_engage_velocity(autoware::mppi_optimizer::Trajectory & trajectory)
+{
+  constexpr auto engage_velocity = 0.25;
+  if (trajectory.points.size() < 3) return;
+  const auto wants_to_move = std::find_if(
+                               trajectory.points.begin(), trajectory.points.end(),
+                               [&](const autoware_planning_msgs::msg::TrajectoryPoint & p) {
+                                 return p.longitudinal_velocity_mps > engage_velocity;
+                               }) != trajectory.points.end();
+  if (wants_to_move && trajectory.points[0].longitudinal_velocity_mps < 0.05) {
+    trajectory.points[0].longitudinal_velocity_mps = engage_velocity;
+    trajectory.points[1].longitudinal_velocity_mps = engage_velocity;
+  }
+}
 }  // namespace
 
 struct FirstOrderDubinsMppiInterface::Impl
@@ -375,6 +410,8 @@ struct FirstOrderDubinsMppiInterface::Impl
   bool ignore_obstacles{false};
   bool ignore_drivable_area{false};
   bool force_cold_start_each_step{false};
+  /** Warm-start u_nom from shifted previous u_opt when available. */
+  bool use_last_control_as_nominal{false};
 
   Impl() : feedback(&model, kDt), sampler(SAMPLER::SAMPLING_PARAMS_T{}) {}
 
@@ -432,8 +469,7 @@ struct FirstOrderDubinsMppiInterface::Impl
     sampler = SAMPLER(sp);
 
     controller = std::make_unique<Mppi>(
-      &model, &cost, &feedback, &sampler, kDt, 1, user_cost_params_.lambda, 0.0F, kMppiHorizon,
-      u_nom);
+      &model, &cost, &feedback, &sampler, kDt, kMaxIter, kLambda, 0.0F, kMppiHorizon, u_nom);
     auto cp = controller->getParams();
     cp.dynamics_rollout_dim_ = dim3(32, 2, 1);
     cp.cost_rollout_dim_ = dim3(32, 2, 1);
@@ -471,6 +507,15 @@ struct FirstOrderDubinsMppiInterface::Impl
     sim_time = 0.0F;
   }
 
+  void seedNominalControlFromLastOptimized()
+  {
+    // Drop the control already applied at the previous cycle; hold the terminal command.
+    for (int t = 0; t < kMppiHorizon - 1; ++t) {
+      u_nom.col(t) = u_opt.col(t + 1);
+    }
+    u_nom.col(kMppiHorizon - 1) = u_opt.col(kMppiHorizon - 1);
+  }
+
   void seedNominalControlFromDiffusionReference(
     const Trajectory & reference, const size_t start_idx)
   {
@@ -509,6 +554,16 @@ struct FirstOrderDubinsMppiInterface::Impl
     }
   }
 
+  void seedNominalControl(const Trajectory & reference, const size_t start_idx)
+  {
+    // After a tracking reset, step_count is 0 and u_opt was cleared — fall back to DP seed.
+    if (use_last_control_as_nominal && step_count > 0) {
+      seedNominalControlFromLastOptimized();
+      return;
+    }
+    seedNominalControlFromDiffusionReference(reference, start_idx);
+  }
+
   void updateDiffusionReference(
     const Trajectory & reference, const Odometry & odometry,
     const std::optional<geometry_msgs::msg::AccelWithCovarianceStamped> & acceleration,
@@ -539,7 +594,7 @@ struct FirstOrderDubinsMppiInterface::Impl
       resetTrackingState();
     }
     tracking_start_idx = new_start_idx;
-    seedNominalControlFromDiffusionReference(reference, tracking_start_idx);
+    seedNominalControl(reference, tracking_start_idx);
 
     const float ego_yaw = yawFromOdometry(odometry);
     const float ego_v = static_cast<float>(odometry.twist.twist.linear.x);
@@ -708,9 +763,9 @@ void FirstOrderDubinsMppiInterface::setRuntimeOptions(
   setDebugTrajectoryLogging(
     options.enable_debug_trajectory_log, options.debug_trajectory_log_directory);
   setAblationOptions(
-    options.ignore_obstacles, options.ignore_drivable_area, options.force_cold_start_each_step);
+    options.ignore_obstacles, options.ignore_drivable_area, options.force_cold_start_each_step,
+    options.use_last_control_as_nominal);
 }
-
 void FirstOrderDubinsMppiInterface::setDebugTrajectoryLogging(
   const bool enable, const std::string & directory)
 {
@@ -723,20 +778,22 @@ void FirstOrderDubinsMppiInterface::setDebugTrajectoryLogging(
 
 void FirstOrderDubinsMppiInterface::setAblationOptions(
   const bool ignore_obstacles, const bool ignore_drivable_area,
-  const bool force_cold_start_each_step)
+  const bool force_cold_start_each_step, const bool use_last_control_as_nominal)
 {
   if (!impl_) {
     throw std::runtime_error("FirstOrderDubinsMppiInterface implementation is missing");
   }
   impl_->ignore_obstacles = ignore_obstacles;
   impl_->ignore_drivable_area = ignore_drivable_area;
   impl_->force_cold_start_each_step = force_cold_start_each_step;
+  impl_->use_last_control_as_nominal = use_last_control_as_nominal;
   RCLCPP_INFO(
     mppiLogger(),
     "MPPI ablation options: ignore_obstacles=%s ignore_drivable_area=%s "
-    "force_cold_start_each_step=%s",
+    "force_cold_start_each_step=%s use_last_control_as_nominal=%s",
     ignore_obstacles ? "true" : "false", ignore_drivable_area ? "true" : "false",
-    force_cold_start_each_step ? "true" : "false");
+    force_cold_start_each_step ? "true" : "false",
+    use_last_control_as_nominal ? "true" : "false");
 }
 
 bool FirstOrderDubinsMppiInterface::copySampleCostDistribution(
@@ -796,9 +853,14 @@ FirstOrderDubinsMppiOptimizationResult FirstOrderDubinsMppiInterface::optimizeTr
     throw std::runtime_error("FirstOrderDubinsMppiInterface implementation is missing");
   }
   FirstOrderDubinsMppiOptimizationResult result;
-  if (input.points.size() < 2U) {
+  const auto not_enough_input_points = input.points.size() < 2U;
+  const auto optimization_required = is_optimization_required(input);
+  if (not_enough_input_points || !optimization_required) {
     RCLCPP_WARN(
-      mppiLogger(), "MPPI skipped: trajectory has %zu points (need >= 2)", input.points.size());
+      mppiLogger(), "MPPI skipped: %s",
+      not_enough_input_points ? "trajectory has fewer than 2 points"
+                              : "trajectory does not require optimization");
+
     result.trajectory = input;
     result.debug.reference_trajectory = input;
     result.debug.optimized_trajectory = input;
@@ -889,6 +951,8 @@ FirstOrderDubinsMppiOptimizationResult FirstOrderDubinsMppiInterface::optimizeTr
     std::chrono::duration<double, std::milli>(std::chrono::steady_clock::now() - start_time)
       .count();
 
+  set_initial_engage_velocity(output);
+
   result.trajectory = output;
   result.debug.reference_trajectory = input;
   result.debug.optimized_trajectory = output;

planning/autoware_mppi_optimizer/src/first_order_dubins_mppi_runtime_options_ros.cpp

@@ -40,6 +40,8 @@ void declare_first_order_dubins_mppi_runtime_options(
   node.declare_parameter(param_name(prefix, "ignore_drivable_area"), defaults.ignore_drivable_area);
   node.declare_parameter(
     param_name(prefix, "force_cold_start_each_step"), defaults.force_cold_start_each_step);
+  node.declare_parameter(
+    param_name(prefix, "use_last_control_as_nominal"), defaults.use_last_control_as_nominal);
 }
 
 FirstOrderDubinsMppiRuntimeOptions get_first_order_dubins_mppi_runtime_options(
@@ -55,6 +57,8 @@ FirstOrderDubinsMppiRuntimeOptions get_first_order_dubins_mppi_runtime_options(
     node.get_parameter(param_name(prefix, "ignore_drivable_area")).as_bool();
   options.force_cold_start_each_step =
     node.get_parameter(param_name(prefix, "force_cold_start_each_step")).as_bool();
+  options.use_last_control_as_nominal =
+    node.get_parameter(param_name(prefix, "use_last_control_as_nominal")).as_bool();
   return options;
 }
 

planning/autoware_mppi_optimizer/src/mppi_optimizer.cpp

@@ -26,6 +26,7 @@
 #include <memory>
 #include <optional>
 #include <stdexcept>
+#include <string>
 
 namespace autoware::mppi_optimizer
 {