remove all the commented out code

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -94,37 +94,11 @@ class MotionModel
         .cwiseMin(upper_bound_vx);
       state.vx.col(i) = state.cvx.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) >
           0).select(state.vy.col(i - 1) + min_delta_vy,

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -563,16 +563,6 @@ 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,
@@ -584,14 +574,6 @@ 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,11 +67,6 @@ 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

@@ -96,8 +96,6 @@ 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;
   }
 }

nav2_mppi_controller/src/optimizer.cpp

@@ -191,9 +191,6 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
   bool trajectory_valid = true;
 
   do {
-    // std::cout << "\n\t control_sequence_ Before optimize:\n\t\t"
-    //           << control_sequence_.vx(Eigen::seq(0, 5)).transpose() << "\n\t"
-    //           << control_sequence_.wz(Eigen::seq(0, 5)).transpose() << "\n\t" << std::endl;
     optimize();
     optimal_trajectory = getOptimizedTrajectory();
     switch (trajectory_validator_->validateTrajectory(
@@ -213,11 +210,6 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
     }
   } while (fallback(critics_data_.fail_flag || !trajectory_valid));
 
-  // std::cout << "Control Sequence End:\n";
-  // std::cout << "vx: " << control_sequence_.vx.transpose() << "\n";
-  // std::cout << "wz: " << control_sequence_.wz.transpose() << "\n";
-  // computeControlSequenceAccel(control_sequence_);
-
   auto control = getControlFromSequenceAsTwist(plan.header.stamp);
 
   if (settings_.shift_control_sequence) {
@@ -281,9 +273,6 @@ void Optimizer::computeControlSequenceAccel(const models::ControlSequence& contr
 
 void Optimizer::optimize()
 {
-  // std::cout << "optimize: control_sequence_:\n\t\t"
-  //           << control_sequence_.vx(Eigen::seq(0, 9)).transpose() << "\n\t\t"
-  //           << control_sequence_.wz(Eigen::seq(0, 9)).transpose() << std::endl;
   for (size_t i = 0; i < settings_.iteration_count; ++i) {
     generateNoisedTrajectories();
     critic_manager_.evalTrajectoriesScores(critics_data_);
@@ -333,27 +322,12 @@ void Optimizer::prepare(
 
 void Optimizer::shiftControlSequence()
 {
-  // std::cout << "shiftControlSequence:\n\t" //\tsettings_.shift_control_sequence" << settings_.shift_control_sequence
-  //           << "\n\t control_sequence_ Before:\n\t\t"
-  //           << control_sequence_.vx(Eigen::seq(0, 5)).transpose() << "\n\t"
-  //           << control_sequence_.wz(Eigen::seq(0, 5)).transpose() << "\n\t"
-  //           /*<< control_sequence_.vx(Eigen::seq(Eigen::last -5, Eigen::last)).transpose()*/<< std::endl;
-  // std::cout << "\n\t control_sequence_virtual_ Before:\n\t\t"
-  //           << control_sequence_virtual_.vx(Eigen::seq(0, 5)).transpose() << "\n\t"
-  //           << control_sequence_virtual_.wz(Eigen::seq(0, 5)).transpose() << "\n\t" << std::endl;
-
-  // control_sequence_ = control_sequence_virtual_;
-
   auto size = control_sequence_.vx.size();
   utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);
   utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);
   control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
   control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);
 
-  // std::cout << "\n\t control_sequence_ After:\n\t\t" << control_sequence_.vx(Eigen::seq(0, 5)).transpose() << "\n\t\t"
-  //           << control_sequence_.wz(Eigen::seq(0, 5)).transpose() << "\n\t\t"
-  //           /*<< control_sequence_.vx(Eigen::seq(Eigen::last -5, Eigen::last)).transpose()*/<< std::endl;
-
   if (isHolonomic()) {
     utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
     control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
@@ -372,54 +346,18 @@ void Optimizer::applyControlSequenceConstraints()
 {
   auto & s = settings_;
 
-  // std::cout << "Control Sequence Before Motion Model Constraints:\n";
-  // std::cout << "vx: " << control_sequence_.vx(Eigen::seq(0, 9)).transpose() << "\n";
-  // std::cout << "wz: " << control_sequence_.wz(Eigen::seq(0, 9)).transpose() << "\n";
-
-  // Debugging output for constraint values
-  // std::cout << "****Acceleration Constraints:\n";
-  // std::cout << "ax_max: " << s.constraints.ax_max << ", ax_min: " << s.constraints.ax_min << "\n";
-  // std::cout << "ay_max: " << s.constraints.ay_max << ", ay_min: " << s.constraints.ay_min << "\n";
-  // std::cout << "az_max: " << s.constraints.az_max << "\n";
-  // computeControlSequenceAccel(control_sequence_);
-
-  // std::cout << "applyControlSequenceConstraints: \n  state.u_app \n\t" << state_.vx(Eigen::seq(0, 2), Eigen::seq(0, 2))
-  //           << "\n\t" << state_.wz(Eigen::seq(0, 2), Eigen::seq(0, 2)) << std::endl
-  //           << "\n  state.u_virt\n\t" << state_.cvx(Eigen::seq(0, 2), Eigen::seq(0, 2)) << "\n\t"
-  //           << state_.cwz(Eigen::seq(0, 2), Eigen::seq(0, 2)) << "\n  ctrl_seq: \n\t"
-  //           << control_sequence_.vx(Eigen::seq(0, 5)).transpose() << "\n\t"
-  //           << control_sequence_.wz(Eigen::seq(0, 5)).transpose() << std::endl;
-
   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;
 
-  // --tried 1 limit ctrl_seq_(0) based on accel_limit from current robot speed (= state.vx(0,0))  (instead of in predict -> see it still accel issue)
-  // TODO 4 or ideally based on last published command
-  // at this point, control_sequence_ contains the softmax mean of state_.cu (u_virt)]
-/*
-  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 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;
-  }
-*/
-  // limit acceleration between current feedback speed and first control in the sequence
-  // float vx_last = static_cast<float>(state_.speed.linear.x);
-  // float wz_last = static_cast<float>(state_.speed.angular.z);
+  // 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 = static_cast<float>(state_.speed.linear.y);
     vy_last = initial_velocities_(1);
   }
 
@@ -434,18 +372,10 @@ void Optimizer::applyControlSequenceConstraints()
     vx_last = vx_curr;
 
     float & wz_curr = control_sequence_.wz(i);
-    // if (i==0)
-    // {
-    //   std::cout << "control_sequence_.wz(0) BEFORE: " << control_sequence_.wz(i) << std::endl;
-    // }
     wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr);
     wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr);
     wz_last = wz_curr;
 
-    // if (i==0)
-    // {
-    //   std::cout << "control_sequence_.wz(0) AFTER: " << control_sequence_.wz(i) << std::endl;
-    // }
     if (isHolonomic()) {
       float & vy_curr = control_sequence_.vy(i);
       vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
@@ -459,11 +389,6 @@ void Optimizer::applyControlSequenceConstraints()
   }
 
   motion_model_->applyConstraints(control_sequence_);
-
-  // std::cout << "Control Sequence After Motion Model Constraints:\n";
-  // std::cout << "vx: " << control_sequence_.vx(Eigen::seq(0, 9)).transpose() << "\n";
-  // std::cout << "wz: " << control_sequence_.wz(Eigen::seq(0, 9)).transpose() << "\n";
-  // computeControlSequenceAccel(control_sequence_);
 }
 
 void Optimizer::updateStateVelocities(
@@ -476,23 +401,16 @@ void Optimizer::updateStateVelocities(
 void Optimizer::updateInitialStateVelocities(
   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);
   }
 
-  // save for later
   initial_velocities_(0) = control_sequence_.vx(0);
   initial_velocities_(1) = control_sequence_.vy(0);
   initial_velocities_(2) = control_sequence_.wz(0);
-
-  // std::cout << "updateInitialStateVelocities: (" << state.speed.linear.x << "  ,  " << state.speed.angular.z << ")\n"
-  //  << state.vx(0, Eigen::seq(0, 5)) << "\n " << state.wz(0, Eigen::seq(0, 5)) << std::endl;
 }
 
 void Optimizer::propagateStateVelocitiesFromInitials(
@@ -639,16 +557,11 @@ void Optimizer::updateControlSequence()
     costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval();
   }
 
-  // std::cout << "costs_: " << costs_(Eigen::seq(0, 9)).transpose() << "\n";
-
   auto costs_normalized = costs_ - costs_.minCoeff();
   const float inv_temp = 1.0f / s.temperature;
   auto softmaxes = (-inv_temp * costs_normalized).exp().eval();
   softmaxes /= softmaxes.sum();
 
-  // std::cout << "costs_normalized: " << costs_normalized(Eigen::seq(0, 9)).transpose() << "\n";
-  // std::cout << "softmaxes: " << softmaxes(Eigen::seq(0, 9)).transpose() << "\n";
-
   auto softmax_mat = softmaxes.matrix();
   control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat;
   control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat;
@@ -669,11 +582,6 @@ geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
   auto vx = control_sequence_.vx(0);
   auto wz = control_sequence_.wz(0);
 
-  // std::cout << "getControlFromSequenceAsTwist:\n\tsettings_.shift_control_sequence" << settings_.shift_control_sequence
-  //           << "\n\t (vx , wz): (" << vx << ", " << wz << ")\n\t control_sequence_:\n\t\t"
-  //           << control_sequence_.vx(Eigen::seq(0, 9)).transpose() << "\n\t\t"
-  //           << control_sequence_.wz(Eigen::seq(0, 9)).transpose() << std::endl;
-
   if (isHolonomic()) {
     auto vy = control_sequence_.vy(0);
     return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());