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,41 @@ 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.u(i) = state.cu(i-1)
+      // => we start from current robot speed (state.u(0)) and then apply u_virt (state.cu(i-1))
+      // but we also need to constrain u_virt before applying it based on static limits (max vel & curvature) to compute
+      // u_app
+      // then state.u(i) = u_app(i-1)
+      // ==>  x_k = F(x_k-1, u_app_k-1) = F(x_k-1, g(u_virt_k-1))
+
+      // u_app(0) should be limited on acceleration relative to either feedback (state.u(0)) as done now
+      // TODO 3 or ideally based on last published command
+
+      // state.vx.col(i) = state.cvx.col(i - 1)
+      //   .cwiseMax(lower_bound_vx)
+      //   .cwiseMin(upper_bound_vx);
+
+      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);
+
+      // 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);
+
+      // constrain u_virt 0 & 1 to make sure accelerations are limited at the first step (state.vx(0) is from feedback)
+      // also constrain u_virt_1 as this is published as command
+      // if (i <= 2)
+      // {
+      //   state.cvx.col(i - 1) = state.vx.col(i);
+      //   state.cwz.col(i - 1) = state.wz.col(i);
+      // }
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
@@ -105,9 +133,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 +190,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

@@ -145,6 +145,8 @@ class Optimizer
    */
   void optimize();
 
+  void computeControlSequenceAccel(const models::ControlSequence& control_sequence);
+
   /**
    * @brief Prepare state information on new request for trajectory rollouts
    * @param robot_pose Pose of the robot at given time
@@ -190,13 +192,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 +282,9 @@ class Optimizer
   models::Path path_;
   geometry_msgs::msg::Pose goal_;
   Eigen::ArrayXf costs_;
+  geometry_msgs::msg::TwistStamped prev_control_twist_;
+  models::ControlSequence prev_control_sequence_;
+  Eigen::Array3f initial_velocities_;
 
   CriticData critics_data_ = {
     state_, generated_trajectories_, path_, goal_,

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -563,6 +563,16 @@ inline void savitskyGolayFilter(
     };
 
   // Filter trajectories
+
+  // std::cout << "**** SGF:\n";
+  // std::cout << "wz before: " << control_sequence.wz(Eigen::seq(0, 9)).transpose() << "\n";
+  // std::cout << "control_history ";
+  // for (const auto& e : control_history)
+  // {
+  //   // std::cout << e.wz << " , ";
+  // }
+  // std::cout << std::endl;
+
   const models::ControlSequence initial_control_sequence = control_sequence;
   applyFilterOverAxis(
     control_sequence.vx, initial_control_sequence.vx, control_history[0].vx,
@@ -574,6 +584,14 @@ inline void savitskyGolayFilter(
     control_sequence.wz, initial_control_sequence.wz, control_history[0].wz,
     control_history[1].wz, control_history[2].wz, control_history[3].wz);
 
+  // std::cout << "after wz: " << control_sequence.wz(Eigen::seq(0, 9)).transpose() << "\n";
+  // std::cout << "control_history ";
+  // for (const auto& e : control_history)
+  // {
+  //   // std::cout << e.wz << " , ";
+  // }
+  // std::cout << std::endl;
+
   // Update control history
   unsigned int offset = settings.shift_control_sequence ? 1 : 0;
   control_history[0] = control_history[1];

nav2_mppi_controller/src/critic_manager.cpp

@@ -67,6 +67,11 @@ std::string CriticManager::getFullName(const std::string & name)
 void CriticManager::evalTrajectoriesScores(
   CriticData & data) const
 {
+  // std::cout << "evalTrajectoriesScores:" << std::endl;
+  // std::cout << "data.state.vx: " << data.state.vx(Eigen::seq(0, 9), 0).transpose() << "\n";
+  // std::cout << "data.state.cvx: " << data.state.cvx(Eigen::seq(0, 9), 0).transpose() << "\n";
+  // std::cout << "data.trajectories.x: " << data.trajectories.x(Eigen::seq(0, 9), 0).transpose() << "\n";
+
   for (const auto & critic : critics_) {
     if (data.fail_flag) {
       break;

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() *
@@ -92,6 +96,8 @@ void ConstraintCritic::score(CriticData & data)
       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() * weight_).eval();
     }
+    // std::cout << "constraint_critic costs after: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl;
+
     return;
   }
 }