Use last command to initialize state - fix oscillations & nervous steering

nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp

@@ -29,6 +29,7 @@ struct Trajectories
   Eigen::ArrayXXf x;
   Eigen::ArrayXXf y;
   Eigen::ArrayXXf yaws;
+  Eigen::ArrayXf costs;
 
   /**
     * @brief Reset state data

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -89,13 +89,15 @@ class MotionModel
         0).select(state.vx.col(i - 1) + max_delta_vx,
         state.vx.col(i - 1) - min_delta_vx);
 
-      state.vx.col(i) = state.cvx.col(i - 1)
+      state.cvx.col(i - 1) = state.cvx.col(i - 1)
         .cwiseMax(lower_bound_vx)
         .cwiseMin(upper_bound_vx);
+      state.vx.col(i) = state.cvx.col(i - 1);
 
-      state.wz.col(i) = state.cwz.col(i - 1)
+      state.cwz.col(i - 1) = 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);
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
@@ -105,9 +107,10 @@ class MotionModel
           0).select(state.vy.col(i - 1) + max_delta_vy,
           state.vy.col(i - 1) - min_delta_vy);
 
-        state.vy.col(i) = state.cvy.col(i - 1)
+        state.cvy.col(i - 1) = state.cvy.col(i - 1)
           .cwiseMax(lower_bound_vy)
           .cwiseMin(upper_bound_vy);
+        state.vy.col(i) = state.cvy.col(i - 1);
       }
     }
   }
@@ -161,14 +164,10 @@ class AckermannMotionModel : public MotionModel
    */
   void applyConstraints(models::ControlSequence & control_sequence) override
   {
-    const auto vx_ptr = control_sequence.vx.data();
-    auto wz_ptr = control_sequence.wz.data();
-    int steps = control_sequence.vx.size();
-    for(int i = 0; i < steps; i++) {
-      float wz_constrained = fabs(*(vx_ptr + i) / min_turning_r_);
-      float & wz_curr = *(wz_ptr + i);
-      wz_curr = utils::clamp(-1 * wz_constrained, wz_constrained, wz_curr);
-    }
+    const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_;
+    control_sequence.wz = control_sequence.wz
+      .max((-wz_constrained))
+      .min(wz_constrained);
   }
 
   /**

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -190,13 +190,13 @@ class Optimizer
    * @brief  Update velocities in state
    * @param state fill state with velocities on each step
    */
-  void updateStateVelocities(models::State & state) const;
+  void updateStateVelocities(models::State & state);
 
   /**
    * @brief  Update initial velocity in state
    * @param state fill state
    */
-  void updateInitialStateVelocities(models::State & state) const;
+  void updateInitialStateVelocities(models::State & state);
 
   /**
    * @brief predict velocities in state using model
@@ -280,6 +280,7 @@ class Optimizer
   models::Path path_;
   geometry_msgs::msg::Pose goal_;
   Eigen::ArrayXf costs_;
+  Eigen::Array3f initial_velocities_;
 
   CriticData critics_data_ = {
     state_, generated_trajectories_, path_, goal_,

nav2_mppi_controller/src/critics/constraint_critic.cpp

@@ -82,8 +82,12 @@ void ConstraintCritic::score(CriticData & data)
   if (acker != nullptr) {
     auto & vx = data.state.vx;
     auto & wz = data.state.wz;
-    float min_turning_rad = acker->getMinTurningRadius();
-    auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f);
+    const float min_turning_rad = acker->getMinTurningRadius();
+
+    const float epsilon = 1e-6f;
+    auto wz_safe = wz.abs().max(epsilon);  // Replace small wz values to avoid division by 0
+    auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz_safe)).max(0.0f);
+
     if (power_ > 1u) {
       data.costs += ((((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) +
         out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() *

nav2_mppi_controller/src/optimizer.cpp

@@ -210,7 +210,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) {
@@ -226,6 +225,7 @@ void Optimizer::optimize()
     generateNoisedTrajectories();
     critic_manager_.evalTrajectoriesScores(critics_data_);
     updateControlSequence();
+    generated_trajectories_.costs = costs_;
   }
 }
 
@@ -258,7 +258,7 @@ void Optimizer::prepare(
   state_.pose = robot_pose;
   state_.speed = robot_speed;
   path_ = utils::toTensor(plan);
-  costs_.setZero();
+  costs_.setZero(settings_.batch_size);
   goal_ = goal;
 
   critics_data_.fail_flag = false;
@@ -299,17 +299,17 @@ 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;
+
+  // limit acceleration between current initial_velocities_ and first control in the sequence
+  float vx_last = initial_velocities_(0);
+  float wz_last = initial_velocities_(2);
+
   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 = initial_velocities_(1);
   }
 
-  for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
+  for (unsigned int i = 0; 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);
     if(vx_last > 0) {
@@ -340,21 +340,25 @@ void Optimizer::applyControlSequenceConstraints()
 }
 
 void Optimizer::updateStateVelocities(
-  models::State & state) const
+  models::State & state)
 {
   updateInitialStateVelocities(state);
   propagateStateVelocitiesFromInitials(state);
 }
 
 void Optimizer::updateInitialStateVelocities(
-  models::State & state) const
+  models::State & state)
 {
-  state.vx.col(0) = static_cast<float>(state.speed.linear.x);
-  state.wz.col(0) = static_cast<float>(state.speed.angular.z);
+  state.vx.col(0) = control_sequence_.vx(0);
+  state.wz.col(0) = control_sequence_.wz(0);
 
   if (isHolonomic()) {
-    state.vy.col(0) = static_cast<float>(state.speed.linear.y);
+    state.vy.col(0) = control_sequence_.vy(0);
   }
+
+  initial_velocities_(0) = control_sequence_.vx(0);
+  initial_velocities_(1) = control_sequence_.vy(0);
+  initial_velocities_(2) = control_sequence_.wz(0);
 }
 
 void Optimizer::propagateStateVelocitiesFromInitials(
@@ -514,19 +518,19 @@ void Optimizer::updateControlSequence()
     control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat;
   }
 
+  utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
+
   applyControlSequenceConstraints();
 }
 
 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/src/trajectory_visualizer.cpp

@@ -111,6 +111,23 @@ void TrajectoryVisualizer::add(
   const float shape_1 = static_cast<float>(n_cols);
   points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_));
 
+  std::vector<std::pair<size_t, float>> sorted_costs;
+  sorted_costs.reserve(std::ceil(n_rows / trajectory_step_));
+
+  for (size_t i = 0; i < n_rows; i += trajectory_step_) {
+    sorted_costs.emplace_back(i, trajectories.costs(i));
+  }
+  std::sort(sorted_costs.begin(), sorted_costs.end(),
+            [](const auto & a, const auto & b) { return a.second < b.second; });
+
+  const float step = 1.0f / static_cast<float>(sorted_costs.size());
+  float color_component = 1.0f;
+  std::map<size_t, float> cost_color_map;
+  for (const auto & pair : sorted_costs) {
+    cost_color_map[pair.first] = color_component;
+    color_component -= step;
+  }
+
   for (size_t i = 0; i < n_rows; i += trajectory_step_) {
     for (size_t j = 0; j < n_cols; j += time_step_) {
       const float j_flt = static_cast<float>(j);
@@ -119,7 +136,7 @@ void TrajectoryVisualizer::add(
 
       auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03);
       auto scale = utils::createScale(0.03, 0.03, 0.03);
-      auto color = utils::createColor(0, green_component, blue_component, 1);
+      auto color = utils::createColor(cost_color_map[i], green_component, blue_component, 1);
       auto marker = utils::createMarker(
         marker_id_++, pose, scale, color, frame_id_, marker_namespace);