From 999b019e5b7f2075df6fd950d33830a9e358ceff Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 1 Sep 2025 16:26:00 +0200 Subject: [PATCH 01/35] [tmp] debug accel limits --- .../nav2_mppi_controller/optimizer.hpp | 2 + nav2_mppi_controller/src/optimizer.cpp | 49 +++++++++++++++++++ 2 files changed, 51 insertions(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e70cfcab786..37c599337f5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/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 diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 97a16249814..7df029af7a7 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -211,6 +211,12 @@ std::tuple Optimizer::evalCon } while (fallback(critics_data_.fail_flag || !trajectory_valid)); utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); + + std::cout << "Control Sequence After SGF:\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) { @@ -220,6 +226,31 @@ std::tuple Optimizer::evalCon return std::make_tuple(control, optimal_trajectory); } +void Optimizer::computeControlSequenceAccel(const models::ControlSequence& control_sequence) +{ + auto & s = settings_; + + std::cout << std::endl; + for (long int i = 1; i < control_sequence.vx.size(); ++i) { + // Compute accelerations + float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt; + float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; + + // Check if accelerations exceed constraints + if (std::abs(ax) > s.constraints.ax_max) { + std::cout << "Acceleration constraint violated at index " << i << ":\n"; + std::cout << "vx[i-1]: " << control_sequence.vx(i - 1) << ", vx[i]: " << control_sequence.vx(i) << ", ax: " << ax << "\n"; + } + + if (std::abs(wz_accel) > s.constraints.az_max) { + std::cout << "Angular acceleration constraint violated at index " << i << ":\n"; + std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; + } + } + std::cout << std::endl; +} + + void Optimizer::optimize() { for (size_t i = 0; i < settings_.iteration_count; ++i) { @@ -294,6 +325,13 @@ void Optimizer::applyControlSequenceConstraints() { auto & s = settings_; + // 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_); + 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; @@ -337,6 +375,12 @@ void Optimizer::applyControlSequenceConstraints() } motion_model_->applyConstraints(control_sequence_); + + // Debugging output for control sequence after motion model constraints + std::cout << "Control Sequence After Motion Model Constraints:\n"; + std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; + std::cout << "wz: " << control_sequence_.wz.transpose() << "\n"; + computeControlSequenceAccel(control_sequence_); } void Optimizer::updateStateVelocities( @@ -514,6 +558,11 @@ void Optimizer::updateControlSequence() control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat; } + // Debugging output for control sequence before motion model constraints + std::cout << "Control Sequence Before Motion Model Constraints:\n"; + std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; + std::cout << "wz: " << control_sequence_.wz.transpose() << "\n"; + applyControlSequenceConstraints(); } From f801ca17560fb343dff2dbbd8ca8547488e3d27c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 2 Sep 2025 12:30:03 +0200 Subject: [PATCH 02/35] [tmp] more debug prints --- .../nav2_mppi_controller/tools/utils.hpp | 18 ++++++++++++++++++ nav2_mppi_controller/src/critic_manager.cpp | 5 +++++ .../src/critics/constraint_critic.cpp | 10 ++++++++++ nav2_mppi_controller/src/optimizer.cpp | 19 ++++++++++++------- 4 files changed, 45 insertions(+), 7 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index f518f16e169..ea6591b92c3 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/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]; diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index d6acd320b05..7126ea9c417 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/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; diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index bce8bdc7fca..db8ed640e0e 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -83,7 +83,15 @@ void ConstraintCritic::score(CriticData & data) auto & vx = data.state.vx; auto & wz = data.state.wz; float min_turning_rad = acker->getMinTurningRadius(); + + std::cout << "constraint_critic acker vx: " << data.state.vx(Eigen::seq(0, 9), 0).transpose() << std::endl; + std::cout << "constraint_critic acker wz: " << data.state.wz(Eigen::seq(0, 9), 0).transpose() << std::endl; + auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f); + std::cout << "constraint_critic acker wz_safe: " << wz_safe(Eigen::seq(0, 9), 0).transpose() << std::endl; + std::cout << "constraint_critic acker out_of_turning_rad_motion: " << out_of_turning_rad_motion(Eigen::seq(0, 9), 0).transpose() << std::endl; + + std::cout << "constraint_critic costs before: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl; 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 +100,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; } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 7df029af7a7..96072599091 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -212,7 +212,7 @@ std::tuple Optimizer::evalCon utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); - std::cout << "Control Sequence After SGF:\n"; + 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_); @@ -330,7 +330,7 @@ void Optimizer::applyControlSequenceConstraints() 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_); + // computeControlSequenceAccel(control_sequence_); float max_delta_vx = s.model_dt * s.constraints.ax_max; float min_delta_vx = s.model_dt * s.constraints.ax_min; @@ -376,11 +376,11 @@ void Optimizer::applyControlSequenceConstraints() motion_model_->applyConstraints(control_sequence_); - // Debugging output for control sequence after motion model constraints - std::cout << "Control Sequence After Motion Model Constraints:\n"; - std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; - std::cout << "wz: " << control_sequence_.wz.transpose() << "\n"; - computeControlSequenceAccel(control_sequence_); + // // Debugging output for control sequence after motion model constraints + // std::cout << "Control Sequence After Motion Model Constraints:\n"; + // std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; + // std::cout << "wz: " << control_sequence_.wz.transpose() << "\n"; + // computeControlSequenceAccel(control_sequence_); } void Optimizer::updateStateVelocities( @@ -545,11 +545,16 @@ 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; From 6369739ef4dd5ff8c57df4e58965f6ee5b807e97 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 2 Sep 2025 12:11:45 +0200 Subject: [PATCH 03/35] [mppi] fix division by zero leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run --- nav2_mppi_controller/src/critics/constraint_critic.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index db8ed640e0e..962db2ac293 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -82,16 +82,19 @@ void ConstraintCritic::score(CriticData & data) if (acker != nullptr) { auto & vx = data.state.vx; auto & wz = data.state.wz; - float min_turning_rad = acker->getMinTurningRadius(); + const float min_turning_rad = acker->getMinTurningRadius(); std::cout << "constraint_critic acker vx: " << data.state.vx(Eigen::seq(0, 9), 0).transpose() << std::endl; std::cout << "constraint_critic acker wz: " << data.state.wz(Eigen::seq(0, 9), 0).transpose() << std::endl; - auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f); + const float epsilon = 1e-6f; + auto wz_safe = wz.abs().max(epsilon); // Replace small wz values with epsilon to avoid division by 0 + auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz_safe)).max(0.0f); std::cout << "constraint_critic acker wz_safe: " << wz_safe(Eigen::seq(0, 9), 0).transpose() << std::endl; std::cout << "constraint_critic acker out_of_turning_rad_motion: " << out_of_turning_rad_motion(Eigen::seq(0, 9), 0).transpose() << std::endl; std::cout << "constraint_critic costs before: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl; + 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() * From 907068a4ddfa75d700188fa781d3d223869283ad Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 2 Sep 2025 12:16:53 +0200 Subject: [PATCH 04/35] [mppi] apply savitskyGolayFilter before control constraints otherwise, the filter may result in control_sequence which violates the hard kinematic and acceleration constraints --- nav2_mppi_controller/src/optimizer.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 96072599091..7188e864826 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -210,8 +210,6 @@ std::tuple Optimizer::evalCon } } while (fallback(critics_data_.fail_flag || !trajectory_valid)); - utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); - std::cout << "Control Sequence End:\n"; std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; std::cout << "wz: " << control_sequence_.wz.transpose() << "\n"; @@ -289,7 +287,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; @@ -563,10 +561,7 @@ void Optimizer::updateControlSequence() control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat; } - // Debugging output for control sequence before motion model constraints - std::cout << "Control Sequence Before Motion Model Constraints:\n"; - std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; - std::cout << "wz: " << control_sequence_.wz.transpose() << "\n"; + utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); applyControlSequenceConstraints(); } From d28860dff1432b83a3185df61cddceb4e307a045 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 4 Sep 2025 09:12:31 +0300 Subject: [PATCH 05/35] [tmp] comment out some prints --- .../nav2_mppi_controller/tools/utils.hpp | 30 +++++++++---------- .../src/critics/constraint_critic.cpp | 12 ++++---- nav2_mppi_controller/src/optimizer.cpp | 23 +++++++------- 3 files changed, 33 insertions(+), 32 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index ea6591b92c3..d4f97413a33 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -564,14 +564,14 @@ 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; + // 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( @@ -584,13 +584,13 @@ 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; + // 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; diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 962db2ac293..33981c2272d 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -84,16 +84,16 @@ void ConstraintCritic::score(CriticData & data) auto & wz = data.state.wz; const float min_turning_rad = acker->getMinTurningRadius(); - std::cout << "constraint_critic acker vx: " << data.state.vx(Eigen::seq(0, 9), 0).transpose() << std::endl; - std::cout << "constraint_critic acker wz: " << data.state.wz(Eigen::seq(0, 9), 0).transpose() << std::endl; + // std::cout << "constraint_critic acker vx: " << data.state.vx(Eigen::seq(0, 9), 0).transpose() << std::endl; + // std::cout << "constraint_critic acker wz: " << data.state.wz(Eigen::seq(0, 9), 0).transpose() << std::endl; const float epsilon = 1e-6f; auto wz_safe = wz.abs().max(epsilon); // Replace small wz values with epsilon to avoid division by 0 auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz_safe)).max(0.0f); - std::cout << "constraint_critic acker wz_safe: " << wz_safe(Eigen::seq(0, 9), 0).transpose() << std::endl; - std::cout << "constraint_critic acker out_of_turning_rad_motion: " << out_of_turning_rad_motion(Eigen::seq(0, 9), 0).transpose() << std::endl; + // std::cout << "constraint_critic acker wz_safe: " << wz_safe(Eigen::seq(0, 9), 0).transpose() << std::endl; + // std::cout << "constraint_critic acker out_of_turning_rad_motion: " << out_of_turning_rad_motion(Eigen::seq(0, 9), 0).transpose() << std::endl; - std::cout << "constraint_critic costs before: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl; + // std::cout << "constraint_critic costs before: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl; if (power_ > 1u) { data.costs += ((((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + @@ -103,7 +103,7 @@ 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; + // std::cout << "constraint_critic costs after: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl; return; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 7188e864826..d4de1f4f00f 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -211,8 +211,8 @@ std::tuple 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"; + // 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); @@ -232,18 +232,19 @@ void Optimizer::computeControlSequenceAccel(const models::ControlSequence& contr for (long int i = 1; i < control_sequence.vx.size(); ++i) { // Compute accelerations float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt; - float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; + // float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; // Check if accelerations exceed constraints - if (std::abs(ax) > s.constraints.ax_max) { + constexpr float epsilon = 1e-4f; + if (std::abs(ax) > s.constraints.ax_max + epsilon) { std::cout << "Acceleration constraint violated at index " << i << ":\n"; std::cout << "vx[i-1]: " << control_sequence.vx(i - 1) << ", vx[i]: " << control_sequence.vx(i) << ", ax: " << ax << "\n"; } - if (std::abs(wz_accel) > s.constraints.az_max) { - std::cout << "Angular acceleration constraint violated at index " << i << ":\n"; - std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; - } + // if (std::abs(wz_accel) > s.constraints.az_max + epsilon) { + // std::cout << "Angular acceleration constraint violated at index " << i << ":\n"; + // std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; + // } } std::cout << std::endl; } @@ -543,15 +544,15 @@ 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"; + // 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"; + // 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; From 24ddf03922d59d8cb5d391c848e9962f20374fe5 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 4 Sep 2025 09:14:59 +0300 Subject: [PATCH 06/35] [tmp] rearrange accel check --- nav2_mppi_controller/src/optimizer.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index d4de1f4f00f..23da0a9fccd 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -230,21 +230,20 @@ void Optimizer::computeControlSequenceAccel(const models::ControlSequence& contr std::cout << std::endl; for (long int i = 1; i < control_sequence.vx.size(); ++i) { - // Compute accelerations - float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt; - // float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; + constexpr float epsilon = 1e-4f; // Check if accelerations exceed constraints - constexpr float epsilon = 1e-4f; + float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt; if (std::abs(ax) > s.constraints.ax_max + epsilon) { std::cout << "Acceleration constraint violated at index " << i << ":\n"; std::cout << "vx[i-1]: " << control_sequence.vx(i - 1) << ", vx[i]: " << control_sequence.vx(i) << ", ax: " << ax << "\n"; } - // if (std::abs(wz_accel) > s.constraints.az_max + epsilon) { - // std::cout << "Angular acceleration constraint violated at index " << i << ":\n"; - // std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; - // } + float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; + if (std::abs(wz_accel) > s.constraints.az_max + epsilon) { + std::cout << "Angular acceleration constraint violated at index " << i << ":\n"; + std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; + } } std::cout << std::endl; } From 7009019a413def0a6957504ec5d996473eaa6fb6 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 9 Sep 2025 10:25:51 +0200 Subject: [PATCH 07/35] [tmp] comment out more prints --- nav2_mppi_controller/src/critic_manager.cpp | 8 ++++---- nav2_mppi_controller/src/optimizer.cpp | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 7126ea9c417..abdd50e2443 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -67,10 +67,10 @@ 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"; + // 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) { diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 23da0a9fccd..d8b0a7d97b6 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -210,10 +210,10 @@ std::tuple Optimizer::evalCon } } while (fallback(critics_data_.fail_flag || !trajectory_valid)); - std::cout << "Control Sequence End:\n"; + // 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_); + // computeControlSequenceAccel(control_sequence_); auto control = getControlFromSequenceAsTwist(plan.header.stamp); @@ -324,10 +324,10 @@ void Optimizer::applyControlSequenceConstraints() auto & s = settings_; // 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"; + // 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_); float max_delta_vx = s.model_dt * s.constraints.ax_max; From 52a43159deea8e41232e2242d7345f769760ed3b Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 22 Sep 2025 09:38:55 +0000 Subject: [PATCH 08/35] Revert "Unclamp noise velocity. (#5266)" This reverts commit fb25b2fcc4fa41d00754a048753acb3ef6ee52d3. --- .../include/nav2_mppi_controller/motion_models.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index c10684b0c79..3115cc4e0a1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/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) > @@ -104,10 +106,10 @@ 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.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); } } } From 13195ed3c2191958c05d778ba6b287c05b9f3212 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 6 Oct 2025 14:16:59 +0200 Subject: [PATCH 09/35] [mppi] color trajectories by cost Color trajectories by costs by adding a red component which is inversely proportional ti the costs of the visualized trajectories (the lower the cost the more red the trajectory) --- .../models/trajectories.hpp | 1 + nav2_mppi_controller/src/optimizer.cpp | 1 + .../src/trajectory_visualizer.cpp | 19 ++++++++++++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp index 24dbcb7f69c..73379fb1e87 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/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 diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index d8b0a7d97b6..29de58f2410 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -255,6 +255,7 @@ void Optimizer::optimize() generateNoisedTrajectories(); critic_manager_.evalTrajectoriesScores(critics_data_); updateControlSequence(); + generated_trajectories_.costs = costs_; } } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 6960b5676c6..796c36f93a6 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -111,6 +111,23 @@ void TrajectoryVisualizer::add( const float shape_1 = static_cast(n_cols); points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); + std::vector> 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(sorted_costs.size()); + float color_component = 1.0f; + std::map 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(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); From a3f4b4b0ff6de3c511ec9831e0bbaadb16fdcb8a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 6 Oct 2025 14:17:32 +0200 Subject: [PATCH 10/35] [clean] pub unconstrained optimal trajectory --- .../nav2_mppi_controller/controller.hpp | 4 ++- .../nav2_mppi_controller/optimizer.hpp | 8 ++++-- nav2_mppi_controller/src/controller.cpp | 22 +++++++++++++-- nav2_mppi_controller/src/optimizer.cpp | 28 ++++++++++++++----- .../test/optimizer_unit_tests.cpp | 16 +++++------ 5 files changed, 58 insertions(+), 20 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 1c52df96f68..afea526db40 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -110,7 +110,8 @@ class MPPIController : public nav2_core::Controller void visualize( nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp, - const Eigen::ArrayXXf & optimal_trajectory); + const Eigen::ArrayXXf & optimal_trajectory, + const Eigen::ArrayXXf & optimal_trajectory_unconstrained); std::string name_; nav2::LifecycleNode::WeakPtr parent_; @@ -118,6 +119,7 @@ class MPPIController : public nav2_core::Controller std::shared_ptr costmap_ros_; std::shared_ptr tf_buffer_; nav2::Publisher::SharedPtr opt_traj_pub_; + nav2::Publisher::SharedPtr opt_traj_unconstrained_pub_; std::unique_ptr parameters_handler_; Optimizer optimizer_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 37c599337f5..bdc411a5d7b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -109,14 +109,15 @@ class Optimizer * @brief Get the optimal trajectory for a cycle for visualization * @return Optimal trajectory */ - Eigen::ArrayXXf getOptimizedTrajectory(); + Eigen::ArrayXXf getOptimizedTrajectory(const models::ControlSequence& control_sequence) const; /** * @brief Get the optimal control sequence for a cycle for visualization * @return Optimal control sequence */ const models::ControlSequence & getOptimalControlSequence(); - + const models::ControlSequence & getOptimalControlSequenceUnconstrained(); + const Eigen::ArrayXXf & getOptimalTrajectoryUnconstrained(); /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use @@ -277,12 +278,15 @@ class Optimizer models::State state_; models::ControlSequence control_sequence_; + models::ControlSequence control_sequence_virtual_; std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; geometry_msgs::msg::Pose goal_; Eigen::ArrayXf costs_; + Eigen::ArrayXXf optimal_trajectory_unconstrained_; + CriticData critics_data_ = { state_, generated_trajectories_, path_, goal_, costs_, settings_.model_dt, false, nullptr, nullptr, diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index a9fab10c85e..eb848d5e952 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -52,6 +52,9 @@ void MPPIController::configure( "~/optimal_trajectory"); } + opt_traj_unconstrained_pub_ = node->create_publisher( + "~/optimal_trajectory_unconstrained"); + RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -124,8 +127,21 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( opt_traj_pub_->publish(std::move(trajectory_msg)); } +// if (publish_optimal_trajectory_ && opt_traj_unconstrained_pub_->get_subscription_count() > 0) { +// std_msgs::msg::Header trajectory_header; +// trajectory_header.stamp = cmd.header.stamp; +// trajectory_header.frame_id = costmap_ros_->getGlobalFrameID(); + +// auto trajectory_msg = utils::toTrajectoryMsg( +// optimizer_.getOptimalTrajectoryUnconstrained(), +// optimizer_.getOptimalControlSequenceUnconstrained(), +// optimizer_.getSettings().model_dt, +// trajectory_header); +// opt_traj_unconstrained_pub_->publish(std::move(trajectory_msg)); +// } + if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory, optimizer_.getOptimalTrajectoryUnconstrained()); } return cmd; @@ -134,10 +150,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( void MPPIController::visualize( nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp, - const Eigen::ArrayXXf & optimal_trajectory) + const Eigen::ArrayXXf & optimal_trajectory, + const Eigen::ArrayXXf & optimal_trajectory_unconstrained) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); + trajectory_visualizer_.add(optimal_trajectory_unconstrained, "Optimal Trajectory Unconstrained", cmd_stamp); trajectory_visualizer_.visualize(std::move(transformed_plan)); } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 29de58f2410..3f3fc392553 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -188,11 +188,13 @@ std::tuple Optimizer::evalCon { prepare(robot_pose, robot_speed, plan, goal, goal_checker); Eigen::ArrayXXf optimal_trajectory; + Eigen::ArrayXXf optimal_trajectory_unconstraint; bool trajectory_valid = true; do { optimize(); - optimal_trajectory = getOptimizedTrajectory(); + optimal_trajectory = getOptimizedTrajectory(control_sequence_); + optimal_trajectory_unconstrained_ = getOptimizedTrajectory(control_sequence_virtual_); switch (trajectory_validator_->validateTrajectory( optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal)) { @@ -211,8 +213,8 @@ std::tuple 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"; + // // 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); @@ -497,18 +499,18 @@ void Optimizer::integrateStateVelocities( } } -Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() +Eigen::ArrayXXf Optimizer::getOptimizedTrajectory(const models::ControlSequence& control_sequence) const { const bool is_holo = isHolonomic(); Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); Eigen::Array trajectories = Eigen::Array(settings_.time_steps, 3); - sequence.col(0) = control_sequence_.vx; - sequence.col(1) = control_sequence_.wz; + sequence.col(0) = control_sequence.vx; + sequence.col(1) = control_sequence.wz; if (is_holo) { - sequence.col(2) = control_sequence_.vy; + sequence.col(2) = control_sequence.vy; } integrateStateVelocities(trajectories, sequence); @@ -520,6 +522,17 @@ const models::ControlSequence & Optimizer::getOptimalControlSequence() return control_sequence_; } + +const models::ControlSequence & Optimizer::getOptimalControlSequenceUnconstrained() +{ + return control_sequence_virtual_; +} + +const Eigen::ArrayXXf & Optimizer::getOptimalTrajectoryUnconstrained() +{ + return optimal_trajectory_unconstrained_; +} + void Optimizer::updateControlSequence() { const bool is_holo = isHolonomic(); @@ -562,6 +575,7 @@ void Optimizer::updateControlSequence() control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat; } + control_sequence_virtual_ = control_sequence_; utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); applyControlSequenceConstraints(); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index eec9ea0ec57..9057d1ed4fa 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -249,14 +249,14 @@ TEST(OptimizerTests, BasicInitializedFunctions) EXPECT_EQ(trajs.x.cols(), 50); EXPECT_TRUE(trajs.x.isApproxToConstant(0.0f)); - optimizer_tester.resetMotionModel(); - optimizer_tester.testSetOmniModel(); - auto traj = optimizer_tester.getOptimizedTrajectory(); - EXPECT_EQ(traj(5, 0), 0.0); // x - EXPECT_EQ(traj(5, 1), 0.0); // y - EXPECT_EQ(traj(5, 2), 0.0); // yaw - EXPECT_EQ(traj.rows(), 50); - EXPECT_EQ(traj.cols(), 3); + // optimizer_tester.resetMotionModel(); + // optimizer_tester.testSetOmniModel(); + // auto traj = optimizer_tester.getOptimizedTrajectory(); + // EXPECT_EQ(traj(5, 0), 0.0); // x + // EXPECT_EQ(traj(5, 1), 0.0); // y + // EXPECT_EQ(traj(5, 2), 0.0); // yaw + // EXPECT_EQ(traj.rows(), 50); + // EXPECT_EQ(traj.cols(), 3); optimizer_tester.reset(); optimizer_tester.shutdown(); From 89a2fcb3f34178fec6d4b6c64b83cc511efd47a0 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 26 Sep 2025 09:03:41 +0000 Subject: [PATCH 11/35] Reapply "Unclamp noise velocity. (#5266)" This reverts commit 52a43159deea8e41232e2242d7345f769760ed3b. --- .../include/nav2_mppi_controller/motion_models.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 3115cc4e0a1..c10684b0c79 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -89,15 +89,13 @@ 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.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); if (is_holo) { auto lower_bound_vy = (state.vy.col(i - 1) > @@ -106,10 +104,10 @@ 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); } } } From 76ae5a56289426f2edd2393a4bf6a5b6bd3ed361 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 7 Oct 2025 09:49:47 +0200 Subject: [PATCH 12/35] [mppi] implement control cost from paper --- nav2_mppi_controller/src/optimizer.cpp | 38 ++++++++++++++++++++++---- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3f3fc392553..2154598ce65 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -538,16 +538,42 @@ void Optimizer::updateControlSequence() const bool is_holo = isHolonomic(); auto & s = settings_; - auto vx_T = control_sequence_.vx.transpose(); - auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; + // Paper + auto vx = state_.cvx; // K×N + std::cout << "Vx: size " << vx.rows() << "x" << vx.cols() << std::endl; + auto ux = control_sequence_.vx; // Nx1 + std::cout << "ux: size " << ux.rows() << "x" << ux.cols() << std::endl; + + // u_t^T * v_t (paper) = v_t^T * u_t per sample + // Eigen::VectorXf cross = (Vx * ux).eval(); // (KxN) * (Nx1) -> (Kx1) + // Since all vars are Eigen::Array, need to do element-wise multiplication and then sum rows + Eigen::ArrayXf cross_vx = (vx.rowwise() * ux.transpose()) // K×N, broadcast ux over rows + .rowwise() + .sum(); // Kx1 + std::cout << "cross_vx: " << cross_vx.rows() << "x" << cross_vx.cols() << " : " << cross_vx(Eigen::seq(0, 9)).transpose() << "\n"; + + // original mppi + auto vx_T = control_sequence_.vx.transpose(); // 1xN + auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; // KxN + auto costs_vx = ((bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); + // costs_ += (gamma_vx * (ux_T * vx).rowwise().sum()).eval(); + std::cout << "costs_vx: " << costs_vx.rows() << "x" << costs_vx.cols() << " : " << costs_vx(Eigen::seq(0, 9)).transpose() << "\n"; + 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 * cross_vx; if (s.sampling_std.wz > 0.0f) { - auto wz_T = control_sequence_.wz.transpose(); - auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; + // auto wz_T = control_sequence_.wz.transpose(); // 1xN + // auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; // KxN 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 * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); + + auto wz = state_.cwz; + auto uz = control_sequence_.wz; // Nx1 + Eigen::ArrayXf cross_wz = (wz.rowwise() * uz.transpose()) // (K×N), broadcast ux over rows + .rowwise() + .sum(); + costs_ += gamma_wz * cross_wz; } if (is_holo) { From f5bc2814366bf3f9034ac4fcce66a3dad35ac78f Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 7 Oct 2025 09:50:05 +0200 Subject: [PATCH 13/35] Revert "[mppi] implement control cost from paper" This reverts commit 944a5471cc892566a02dd91cf50faaa3cbdbb1af. --- nav2_mppi_controller/src/optimizer.cpp | 38 ++++---------------------- 1 file changed, 6 insertions(+), 32 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 2154598ce65..3f3fc392553 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -538,42 +538,16 @@ void Optimizer::updateControlSequence() const bool is_holo = isHolonomic(); auto & s = settings_; - // Paper - auto vx = state_.cvx; // K×N - std::cout << "Vx: size " << vx.rows() << "x" << vx.cols() << std::endl; - auto ux = control_sequence_.vx; // Nx1 - std::cout << "ux: size " << ux.rows() << "x" << ux.cols() << std::endl; - - // u_t^T * v_t (paper) = v_t^T * u_t per sample - // Eigen::VectorXf cross = (Vx * ux).eval(); // (KxN) * (Nx1) -> (Kx1) - // Since all vars are Eigen::Array, need to do element-wise multiplication and then sum rows - Eigen::ArrayXf cross_vx = (vx.rowwise() * ux.transpose()) // K×N, broadcast ux over rows - .rowwise() - .sum(); // Kx1 - std::cout << "cross_vx: " << cross_vx.rows() << "x" << cross_vx.cols() << " : " << cross_vx(Eigen::seq(0, 9)).transpose() << "\n"; - - // original mppi - auto vx_T = control_sequence_.vx.transpose(); // 1xN - auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; // KxN - auto costs_vx = ((bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); - // costs_ += (gamma_vx * (ux_T * vx).rowwise().sum()).eval(); - std::cout << "costs_vx: " << costs_vx.rows() << "x" << costs_vx.cols() << " : " << costs_vx(Eigen::seq(0, 9)).transpose() << "\n"; - + auto vx_T = control_sequence_.vx.transpose(); + auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx); - costs_ += gamma_vx * cross_vx; + costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); if (s.sampling_std.wz > 0.0f) { - // auto wz_T = control_sequence_.wz.transpose(); // 1xN - // auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; // KxN + auto wz_T = control_sequence_.wz.transpose(); + auto bounded_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(); - - auto wz = state_.cwz; - auto uz = control_sequence_.wz; // Nx1 - Eigen::ArrayXf cross_wz = (wz.rowwise() * uz.transpose()) // (K×N), broadcast ux over rows - .rowwise() - .sum(); - costs_ += gamma_wz * cross_wz; + costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); } if (is_holo) { From d48ff1672e71bb78ca963ab9140fe7824f1685b1 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 8 Oct 2025 11:21:32 +0200 Subject: [PATCH 14/35] constrain u_virt_0,1 to force acceleration constraints -> reduces oscillations a lot -> no accelerations violations --- .../include/nav2_mppi_controller/motion_models.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index c10684b0c79..8a2a07bdc41 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -97,6 +97,14 @@ class MotionModel .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, From 34049edc46613344cf3cb8e2faca30ca9b810cbd Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 8 Oct 2025 15:54:39 +0200 Subject: [PATCH 15/35] [tmp] only constrain before publish --- .../nav2_mppi_controller/motion_models.hpp | 32 ++++++++++++--- nav2_mppi_controller/src/optimizer.cpp | 39 +++++++++++++++++-- 2 files changed, 62 insertions(+), 9 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 8a2a07bdc41..67886c5e77e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -89,21 +89,43 @@ 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) + // .cwiseMax(lower_bound_vx) + // .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)) + + // TODO 2 constrains state.u based on static limits (max vel & curvature) to compute u_app + + // 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 (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) > diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3f3fc392553..7afc66a6908 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -237,13 +237,13 @@ void Optimizer::computeControlSequenceAccel(const models::ControlSequence& contr // Check if accelerations exceed constraints float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt; if (std::abs(ax) > s.constraints.ax_max + epsilon) { - std::cout << "Acceleration constraint violated at index " << i << ":\n"; + std::cout << "****Acceleration constraint violated at index " << i << ":\n"; std::cout << "vx[i-1]: " << control_sequence.vx(i - 1) << ", vx[i]: " << control_sequence.vx(i) << ", ax: " << ax << "\n"; } float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; if (std::abs(wz_accel) > s.constraints.az_max + epsilon) { - std::cout << "Angular acceleration constraint violated at index " << i << ":\n"; + std::cout << "***Angular acceleration constraint violated at index " << i << ":\n"; std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; } } @@ -308,7 +308,12 @@ void Optimizer::shiftControlSequence() control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2); control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2); - if (isHolonomic()) { + // 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); } @@ -338,8 +343,16 @@ 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; + // --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 1 only constrain the published controls (u0 & u1), but keep the unconstrained sequence for warm start + // 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; @@ -347,8 +360,18 @@ void Optimizer::applyControlSequenceConstraints() 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(state_.speed.linear.x); + float wz_last = static_cast(state_.speed.angular.z); + + float vy_last = 0; + if (isHolonomic()) { + vy_last = static_cast(state_.speed.linear.y); + } - 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) { @@ -359,10 +382,18 @@ 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); From b78c77423b9e3ae6e6466308dc843597e7a32905 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 11:17:27 +0200 Subject: [PATCH 16/35] [tmp] use unconstrained u_virt for warm start --- nav2_mppi_controller/src/optimizer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 7afc66a6908..0eb558d28f1 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -302,6 +302,8 @@ void Optimizer::prepare( void Optimizer::shiftControlSequence() { + control_sequence_ = control_sequence_virtual_; + auto size = control_sequence_.vx.size(); utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); utils::shiftColumnsByOnePlace(control_sequence_.wz, -1); @@ -606,8 +608,8 @@ void Optimizer::updateControlSequence() control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat; } - control_sequence_virtual_ = control_sequence_; utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); + control_sequence_virtual_ = control_sequence_; applyControlSequenceConstraints(); } From c4afab0b78baad60d9accac93504849b5c445a76 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 14:43:47 +0200 Subject: [PATCH 17/35] [mppi] constrain velocity & curvature in predict --- .../nav2_mppi_controller/motion_models.hpp | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 67886c5e77e..af864ab8e2f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -127,6 +127,17 @@ class MotionModel // state.cwz.col(i - 1) = state.wz.col(i); // } + // also apply velocity limits constrains + state.vx.col(i) = state.vx.col(i) + .cwiseMax(control_constraints_.vx_min) + .cwiseMin(control_constraints_.vx_max); + + state.wz.col(i) = state.wz.col(i) + .cwiseMax(-control_constraints_.wz) + .cwiseMin(control_constraints_.wz); + + + // also constrain wz base if (is_holo) { auto lower_bound_vy = (state.vy.col(i - 1) > 0).select(state.vy.col(i - 1) + min_delta_vy, @@ -138,8 +149,14 @@ class MotionModel 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) + .cwiseMax(-control_constraints_.vy) + .cwiseMin(control_constraints_.vy); } } + + applyConstraints(state); } /** @@ -153,6 +170,7 @@ class MotionModel * @param control_sequence Control sequence to apply constraints to */ virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} + virtual void applyConstraints(models::State & /*state*/) {} protected: float model_dt_{0.0}; @@ -201,6 +219,28 @@ class AckermannMotionModel : public MotionModel } } + void applyConstraints(models::State & state) override + { + std::cout << "\n\t wz Before applyConstraints:\n\t\t" << state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl; + // auto tmp_state = state; + // for(unsigned int i = 0; i < state.vx.cols(); i++) { + // auto wz_constrained = state.vx.col(i).abs() / min_turning_r_; + // state.wz.col(i) = state.wz.col(i) + // .cwiseMax(-1 * wz_constrained) + // .cwiseMin(wz_constrained); + // } + + std::cout << "\n\t wz with loop:\n\t\t" << state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl; + + // vectorized version + const auto wz_constrained = tmp_state.vx.abs() / min_turning_r_; + tmp_state.wz = tmp_state.wz + .max((-wz_constrained)) + .min(wz_constrained); + + std::cout << "\n\t wz without loop:\n\t\t" << tmp_state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl; + } + /** * @brief Get minimum turning radius of ackermann drive * @return Minimum turning radius From afa6acdf5760cc2f5ea18cbc30342f7111d226ec Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 14:44:54 +0200 Subject: [PATCH 18/35] [mppi] clean up: constrain velocity & curvature in predict --- .../nav2_mppi_controller/motion_models.hpp | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index af864ab8e2f..e67e567c8a1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -221,24 +221,10 @@ class AckermannMotionModel : public MotionModel void applyConstraints(models::State & state) override { - std::cout << "\n\t wz Before applyConstraints:\n\t\t" << state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl; - // auto tmp_state = state; - // for(unsigned int i = 0; i < state.vx.cols(); i++) { - // auto wz_constrained = state.vx.col(i).abs() / min_turning_r_; - // state.wz.col(i) = state.wz.col(i) - // .cwiseMax(-1 * wz_constrained) - // .cwiseMin(wz_constrained); - // } - - std::cout << "\n\t wz with loop:\n\t\t" << state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl; - - // vectorized version - const auto wz_constrained = tmp_state.vx.abs() / min_turning_r_; - tmp_state.wz = tmp_state.wz + const auto wz_constrained = state.vx.abs() / min_turning_r_; + state.wz = state.wz .max((-wz_constrained)) .min(wz_constrained); - - std::cout << "\n\t wz without loop:\n\t\t" << tmp_state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl; } /** From 1f8507294b1f79c53500876b5b36d4437f070c1a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 15:54:00 +0200 Subject: [PATCH 19/35] [tmp] only constrain control_seq(0,1) in applyControlSequenceConstraints --- .../include/nav2_mppi_controller/motion_models.hpp | 2 +- nav2_mppi_controller/src/optimizer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index e67e567c8a1..6da22c00846 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -211,7 +211,7 @@ class AckermannMotionModel : public MotionModel { const auto vx_ptr = control_sequence.vx.data(); auto wz_ptr = control_sequence.wz.data(); - int steps = control_sequence.vx.size(); + int steps = 2;//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); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 0eb558d28f1..1dd77c21ef0 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -373,7 +373,7 @@ void Optimizer::applyControlSequenceConstraints() vy_last = static_cast(state_.speed.linear.y); } - for (unsigned int i = 0; i != control_sequence_.vx.size(); i++) { + for (unsigned int i = 0; i < 2; 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) { From 2f2b546ed56cfaa0e10474bc8037f69b1b6f9f7a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 15:54:22 +0200 Subject: [PATCH 20/35] Revert "[tmp] only constrain control_seq(0,1) in applyControlSequenceConstraints" This reverts commit ad50f9280d60e69ae956356b52ac71901abca619. --- .../include/nav2_mppi_controller/motion_models.hpp | 2 +- nav2_mppi_controller/src/optimizer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 6da22c00846..e67e567c8a1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -211,7 +211,7 @@ class AckermannMotionModel : public MotionModel { const auto vx_ptr = control_sequence.vx.data(); auto wz_ptr = control_sequence.wz.data(); - int steps = 2;//control_sequence.vx.size(); + 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); diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 1dd77c21ef0..0eb558d28f1 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -373,7 +373,7 @@ void Optimizer::applyControlSequenceConstraints() vy_last = static_cast(state_.speed.linear.y); } - for (unsigned int i = 0; i < 2; 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) { From fa9153596a0bbb637a179ef0892f9259c557311d Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 16:36:44 +0200 Subject: [PATCH 21/35] Undo "[tmp] use unconstrained u_virt for warm start" --- nav2_mppi_controller/src/optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 0eb558d28f1..5f061543114 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -302,7 +302,7 @@ void Optimizer::prepare( void Optimizer::shiftControlSequence() { - control_sequence_ = control_sequence_virtual_; + // control_sequence_ = control_sequence_virtual_; auto size = control_sequence_.vx.size(); utils::shiftColumnsByOnePlace(control_sequence_.vx, -1); From f8e8f6de69a309bc9ae7f01a757e053d37c5b0b7 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 17:12:15 +0200 Subject: [PATCH 22/35] Reapply constrain u_virt_0,1 to force acceleration constraints --- .../include/nav2_mppi_controller/motion_models.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index e67e567c8a1..16de53d0dae 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -121,11 +121,11 @@ class MotionModel // 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 (i <= 2) + { + state.cvx.col(i - 1) = state.vx.col(i); + state.cwz.col(i - 1) = state.wz.col(i); + } // also apply velocity limits constrains state.vx.col(i) = state.vx.col(i) From d3eb7aa892489a0268d45bd78b7fad3647ebd03c Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 9 Oct 2025 17:12:36 +0200 Subject: [PATCH 23/35] [tmp] debugging changes to be removed --- .../nav2_mppi_controller/motion_models.hpp | 2 - .../nav2_mppi_controller/optimizer.hpp | 2 + nav2_mppi_controller/src/optimizer.cpp | 88 +++++++++++++++---- 3 files changed, 75 insertions(+), 17 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 16de53d0dae..bd304620967 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -101,8 +101,6 @@ class MotionModel // 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)) - // TODO 2 constrains state.u based on static limits (max vel & curvature) to compute u_app - // 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 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index bdc411a5d7b..d379097a2d0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -284,6 +284,8 @@ 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::ArrayXXf optimal_trajectory_unconstrained_; diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 5f061543114..f2638c96b9a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -192,6 +192,9 @@ std::tuple 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(control_sequence_); optimal_trajectory_unconstrained_ = getOptimizedTrajectory(control_sequence_virtual_); @@ -223,6 +226,33 @@ std::tuple Optimizer::evalCon shiftControlSequence(); } + { + auto & s = settings_; + constexpr float epsilon = 1e-4f; + + // Check if accelerations exceed constraints + double dt = (control.header.stamp.sec - prev_control_twist_.header.stamp.sec) + + 1e-9 * (control.header.stamp.nanosec - prev_control_twist_.header.stamp.nanosec); + + float ax = (control.twist.linear.x - prev_control_twist_.twist.linear.x) / s.model_dt; + float ax_real = (control.twist.linear.x - prev_control_twist_.twist.linear.x) / dt; + if (std::abs(ax) > s.constraints.ax_max + epsilon) { + std::cout << "Acceleration constraint violated from last command " << ":\t"; + std::cout << "vx[i]: " << control.twist.linear.x << ", vx[i-1]: " << prev_control_twist_.twist.linear.x + << ", ax: " << ax << " real dt: " << dt << " real ax " << ax_real << "\n" ; + } + float wz = (control.twist.angular.z - prev_control_twist_.twist.angular.z) / s.model_dt; + float wz_real = (control.twist.angular.z - prev_control_twist_.twist.angular.z) / dt; + if (std::abs(wz) > s.constraints.az_max + epsilon) { + std::cout << "Angular Acceleration constraint violated from last command " << ":\t"; + std::cout << "wz[i]: " << control.twist.angular.z << ", wz[i-1]: " << prev_control_twist_.twist.angular.z + << ", az: " << wz << " real dt: " << dt << " real az " << wz_real << "\n" ; + } + } + + prev_control_twist_ = control; + prev_control_sequence_ = control_sequence_; + return std::make_tuple(control, optimal_trajectory); } @@ -253,6 +283,9 @@ 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_); @@ -302,6 +335,15 @@ 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(); @@ -314,8 +356,7 @@ void Optimizer::shiftControlSequence() // << 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()) - { + if (isHolonomic()) { utils::shiftColumnsByOnePlace(control_sequence_.vy, -1); control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2); } @@ -333,6 +374,10 @@ 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"; @@ -340,17 +385,23 @@ void Optimizer::applyControlSequenceConstraints() // 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 1 only constrain the published controls (u0 & u1), but keep the unconstrained sequence for warm start // 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)); @@ -384,18 +435,18 @@ 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; - } + // 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 (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); @@ -410,10 +461,9 @@ void Optimizer::applyControlSequenceConstraints() motion_model_->applyConstraints(control_sequence_); - // // Debugging output for control sequence after motion model constraints // std::cout << "Control Sequence After Motion Model Constraints:\n"; - // std::cout << "vx: " << control_sequence_.vx.transpose() << "\n"; - // std::cout << "wz: " << control_sequence_.wz.transpose() << "\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_); } @@ -433,6 +483,9 @@ void Optimizer::updateInitialStateVelocities( if (isHolonomic()) { state.vy.col(0) = static_cast(state.speed.linear.y); } + + // 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( @@ -622,6 +675,11 @@ geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( auto vx = control_sequence_.vx(offset); auto wz = control_sequence_.wz(offset); + // 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(offset); return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID()); From 8b6bcf77a02e6e71857419c681d8b0207fc721f2 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 13 Oct 2025 13:32:46 +0200 Subject: [PATCH 24/35] [tmp] undo changes to predict & applyControlSequenceConstraints --- .../nav2_mppi_controller/motion_models.hpp | 32 +++++++++---------- nav2_mppi_controller/src/optimizer.cpp | 6 ++-- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index bd304620967..bd567066eb0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -119,20 +119,20 @@ class MotionModel // 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 (i <= 2) + // { + // state.cvx.col(i - 1) = state.vx.col(i); + // state.cwz.col(i - 1) = state.wz.col(i); + // } - // also apply velocity limits constrains - state.vx.col(i) = state.vx.col(i) - .cwiseMax(control_constraints_.vx_min) - .cwiseMin(control_constraints_.vx_max); + // // also apply velocity limits constrains + // state.vx.col(i) = state.vx.col(i) + // .cwiseMax(control_constraints_.vx_min) + // .cwiseMin(control_constraints_.vx_max); - state.wz.col(i) = state.wz.col(i) - .cwiseMax(-control_constraints_.wz) - .cwiseMin(control_constraints_.wz); + // state.wz.col(i) = state.wz.col(i) + // .cwiseMax(-control_constraints_.wz) + // .cwiseMin(control_constraints_.wz); // also constrain wz base @@ -148,13 +148,13 @@ class MotionModel .cwiseMax(lower_bound_vy) .cwiseMin(upper_bound_vy); - state.vy.col(i) = state.cvy.col(i - 1) - .cwiseMax(-control_constraints_.vy) - .cwiseMin(control_constraints_.vy); + // state.vy.col(i) = state.cvy.col(i - 1) + // .cwiseMax(-control_constraints_.vy) + // .cwiseMin(control_constraints_.vy); } } - applyConstraints(state); + // applyConstraints(state); } /** diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index f2638c96b9a..0d385b716d0 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -402,7 +402,6 @@ void Optimizer::applyControlSequenceConstraints() // 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)); @@ -413,8 +412,8 @@ void Optimizer::applyControlSequenceConstraints() 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(state_.speed.linear.x); float wz_last = static_cast(state_.speed.angular.z); @@ -423,8 +422,9 @@ void Optimizer::applyControlSequenceConstraints() if (isHolonomic()) { vy_last = static_cast(state_.speed.linear.y); } + */ - for (unsigned int i = 0; i != control_sequence_.vx.size(); i++) { + for (unsigned int i = 1; 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) { From bf606a3b6db3a20cc0aef1fd08ac97e91bbdf1e7 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 13 Oct 2025 13:33:46 +0200 Subject: [PATCH 25/35] [tmp] use prev u_pub for accel constraints --- nav2_mppi_controller/src/optimizer.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 0d385b716d0..bca54318ac4 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -477,11 +477,14 @@ void Optimizer::updateStateVelocities( void Optimizer::updateInitialStateVelocities( models::State & state) const { - state.vx.col(0) = static_cast(state.speed.linear.x); - state.wz.col(0) = static_cast(state.speed.angular.z); + // state.vx.col(0) = static_cast(state.speed.linear.x); + // state.wz.col(0) = static_cast(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(state.speed.linear.y); + // state.vy.col(0) = static_cast(state.speed.linear.y); + state.vy.col(0) = control_sequence_.vy(0); } // std::cout << "updateInitialStateVelocities: (" << state.speed.linear.x << " , " << state.speed.angular.z << ")\n" From 2482d0828243c3314d0bb71237168bae26a40f58 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 13 Oct 2025 14:45:39 +0200 Subject: [PATCH 26/35] [tmp] constrain before publish w.r.t to prev u_pub --- .../nav2_mppi_controller/optimizer.hpp | 5 ++-- nav2_mppi_controller/src/optimizer.cpp | 26 ++++++++++++------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index d379097a2d0..730389745d1 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -193,13 +193,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 @@ -286,6 +286,7 @@ class Optimizer Eigen::ArrayXf costs_; geometry_msgs::msg::TwistStamped prev_control_twist_; models::ControlSequence prev_control_sequence_; + Eigen::Array3f initial_velocities_; Eigen::ArrayXXf optimal_trajectory_unconstrained_; diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index bca54318ac4..60977c409b9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -400,8 +400,8 @@ void Optimizer::applyControlSequenceConstraints() // --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)); @@ -412,19 +412,20 @@ void Optimizer::applyControlSequenceConstraints() 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(state_.speed.linear.x); - float wz_last = static_cast(state_.speed.angular.z); + // float vx_last = static_cast(state_.speed.linear.x); + // float wz_last = static_cast(state_.speed.angular.z); + float vx_last = initial_velocities_(0); + float wz_last = initial_velocities_(2); float vy_last = 0; if (isHolonomic()) { - vy_last = static_cast(state_.speed.linear.y); + // vy_last = static_cast(state_.speed.linear.y); + 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) { @@ -468,14 +469,14 @@ 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(state.speed.linear.x); // state.wz.col(0) = static_cast(state.speed.angular.z); @@ -487,6 +488,11 @@ void Optimizer::updateInitialStateVelocities( 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; } From 05f2891d63017fdae9e053603c38635f8d0a4b51 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 13 Oct 2025 15:03:14 +0200 Subject: [PATCH 27/35] Revert PR 5266 again :) --- .../nav2_mppi_controller/motion_models.hpp | 32 +++++++++++-------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index bd567066eb0..8f7b5a6efec 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -89,10 +89,10 @@ 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) - // .cwiseMax(lower_bound_vx) - // .cwiseMin(upper_bound_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.u(i) = state.cu(i-1) // => we start from current robot speed (state.u(0)) and then apply u_virt (state.cu(i-1)) @@ -104,18 +104,18 @@ class MotionModel // 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.vx.col(i) = state.cvx.col(i - 1) + // .cwiseMax(lower_bound_vx) + // .cwiseMin(upper_bound_vx); - 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); + + // 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 @@ -144,9 +144,13 @@ 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); + // 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) // .cwiseMax(-control_constraints_.vy) From 4f411b4b0a1f9c9337ee5a7ae4d85e984de7df18 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 17 Oct 2025 16:50:21 +0200 Subject: [PATCH 28/35] [mppi] clean ackermann constraints --- .../include/nav2_mppi_controller/motion_models.hpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 8f7b5a6efec..c7ef2d22ac7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -211,14 +211,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); } void applyConstraints(models::State & state) override From e36d9362638618a4bd8bd1c445700daae0895d25 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 17 Oct 2025 17:16:09 +0200 Subject: [PATCH 29/35] [mppi] publish u0 instead of u1 --- nav2_mppi_controller/src/optimizer.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 60977c409b9..78f5bd6ef7d 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -679,10 +679,8 @@ void Optimizer::updateControlSequence() 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); // 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" @@ -690,7 +688,7 @@ geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( // << control_sequence_.wz(Eigen::seq(0, 9)).transpose() << std::endl; if (isHolonomic()) { - auto vy = control_sequence_.vy(offset); + auto vy = control_sequence_.vy(0); return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID()); } From af63f9da2c16132a62426d7b52493323100390ad Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Fri, 17 Oct 2025 16:18:42 +0200 Subject: [PATCH 30/35] constrain turning radius in predict --- .../include/nav2_mppi_controller/motion_models.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index c7ef2d22ac7..e48dde1332f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -158,7 +158,7 @@ class MotionModel } } - // applyConstraints(state); + applyConstraints(state); } /** From aadf3c2590b842d919e0cb30934c0109a30b3cdd Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 23 Oct 2025 12:34:56 +0200 Subject: [PATCH 31/35] Revert "[clean] pub unconstrained optimal trajectory" This reverts commit a3f4b4b0ff6de3c511ec9831e0bbaadb16fdcb8a. --- .../nav2_mppi_controller/controller.hpp | 4 +-- .../nav2_mppi_controller/optimizer.hpp | 8 ++---- nav2_mppi_controller/src/controller.cpp | 22 ++------------- nav2_mppi_controller/src/optimizer.cpp | 27 +++++-------------- .../test/optimizer_unit_tests.cpp | 16 +++++------ 5 files changed, 20 insertions(+), 57 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index afea526db40..1c52df96f68 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -110,8 +110,7 @@ class MPPIController : public nav2_core::Controller void visualize( nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp, - const Eigen::ArrayXXf & optimal_trajectory, - const Eigen::ArrayXXf & optimal_trajectory_unconstrained); + const Eigen::ArrayXXf & optimal_trajectory); std::string name_; nav2::LifecycleNode::WeakPtr parent_; @@ -119,7 +118,6 @@ class MPPIController : public nav2_core::Controller std::shared_ptr costmap_ros_; std::shared_ptr tf_buffer_; nav2::Publisher::SharedPtr opt_traj_pub_; - nav2::Publisher::SharedPtr opt_traj_unconstrained_pub_; std::unique_ptr parameters_handler_; Optimizer optimizer_; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 730389745d1..8ce9afc2e9e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -109,15 +109,14 @@ class Optimizer * @brief Get the optimal trajectory for a cycle for visualization * @return Optimal trajectory */ - Eigen::ArrayXXf getOptimizedTrajectory(const models::ControlSequence& control_sequence) const; + Eigen::ArrayXXf getOptimizedTrajectory(); /** * @brief Get the optimal control sequence for a cycle for visualization * @return Optimal control sequence */ const models::ControlSequence & getOptimalControlSequence(); - const models::ControlSequence & getOptimalControlSequenceUnconstrained(); - const Eigen::ArrayXXf & getOptimalTrajectoryUnconstrained(); + /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use @@ -278,7 +277,6 @@ class Optimizer models::State state_; models::ControlSequence control_sequence_; - models::ControlSequence control_sequence_virtual_; std::array control_history_; models::Trajectories generated_trajectories_; models::Path path_; @@ -288,8 +286,6 @@ class Optimizer models::ControlSequence prev_control_sequence_; Eigen::Array3f initial_velocities_; - Eigen::ArrayXXf optimal_trajectory_unconstrained_; - CriticData critics_data_ = { state_, generated_trajectories_, path_, goal_, costs_, settings_.model_dt, false, nullptr, nullptr, diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index eb848d5e952..a9fab10c85e 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -52,9 +52,6 @@ void MPPIController::configure( "~/optimal_trajectory"); } - opt_traj_unconstrained_pub_ = node->create_publisher( - "~/optimal_trajectory_unconstrained"); - RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -127,21 +124,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( opt_traj_pub_->publish(std::move(trajectory_msg)); } -// if (publish_optimal_trajectory_ && opt_traj_unconstrained_pub_->get_subscription_count() > 0) { -// std_msgs::msg::Header trajectory_header; -// trajectory_header.stamp = cmd.header.stamp; -// trajectory_header.frame_id = costmap_ros_->getGlobalFrameID(); - -// auto trajectory_msg = utils::toTrajectoryMsg( -// optimizer_.getOptimalTrajectoryUnconstrained(), -// optimizer_.getOptimalControlSequenceUnconstrained(), -// optimizer_.getSettings().model_dt, -// trajectory_header); -// opt_traj_unconstrained_pub_->publish(std::move(trajectory_msg)); -// } - if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory, optimizer_.getOptimalTrajectoryUnconstrained()); + visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); } return cmd; @@ -150,12 +134,10 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( void MPPIController::visualize( nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp, - const Eigen::ArrayXXf & optimal_trajectory, - const Eigen::ArrayXXf & optimal_trajectory_unconstrained) + const Eigen::ArrayXXf & optimal_trajectory) { trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); - trajectory_visualizer_.add(optimal_trajectory_unconstrained, "Optimal Trajectory Unconstrained", cmd_stamp); trajectory_visualizer_.visualize(std::move(transformed_plan)); } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 78f5bd6ef7d..290abc819fb 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -188,7 +188,6 @@ std::tuple Optimizer::evalCon { prepare(robot_pose, robot_speed, plan, goal, goal_checker); Eigen::ArrayXXf optimal_trajectory; - Eigen::ArrayXXf optimal_trajectory_unconstraint; bool trajectory_valid = true; do { @@ -196,8 +195,7 @@ std::tuple Optimizer::evalCon // << 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(control_sequence_); - optimal_trajectory_unconstrained_ = getOptimizedTrajectory(control_sequence_virtual_); + optimal_trajectory = getOptimizedTrajectory(); switch (trajectory_validator_->validateTrajectory( optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal)) { @@ -216,8 +214,8 @@ std::tuple 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"; + // 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); @@ -594,18 +592,18 @@ void Optimizer::integrateStateVelocities( } } -Eigen::ArrayXXf Optimizer::getOptimizedTrajectory(const models::ControlSequence& control_sequence) const +Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); Eigen::Array trajectories = Eigen::Array(settings_.time_steps, 3); - sequence.col(0) = control_sequence.vx; - sequence.col(1) = control_sequence.wz; + sequence.col(0) = control_sequence_.vx; + sequence.col(1) = control_sequence_.wz; if (is_holo) { - sequence.col(2) = control_sequence.vy; + sequence.col(2) = control_sequence_.vy; } integrateStateVelocities(trajectories, sequence); @@ -617,17 +615,6 @@ const models::ControlSequence & Optimizer::getOptimalControlSequence() return control_sequence_; } - -const models::ControlSequence & Optimizer::getOptimalControlSequenceUnconstrained() -{ - return control_sequence_virtual_; -} - -const Eigen::ArrayXXf & Optimizer::getOptimalTrajectoryUnconstrained() -{ - return optimal_trajectory_unconstrained_; -} - void Optimizer::updateControlSequence() { const bool is_holo = isHolonomic(); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 9057d1ed4fa..eec9ea0ec57 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -249,14 +249,14 @@ TEST(OptimizerTests, BasicInitializedFunctions) EXPECT_EQ(trajs.x.cols(), 50); EXPECT_TRUE(trajs.x.isApproxToConstant(0.0f)); - // optimizer_tester.resetMotionModel(); - // optimizer_tester.testSetOmniModel(); - // auto traj = optimizer_tester.getOptimizedTrajectory(); - // EXPECT_EQ(traj(5, 0), 0.0); // x - // EXPECT_EQ(traj(5, 1), 0.0); // y - // EXPECT_EQ(traj(5, 2), 0.0); // yaw - // EXPECT_EQ(traj.rows(), 50); - // EXPECT_EQ(traj.cols(), 3); + optimizer_tester.resetMotionModel(); + optimizer_tester.testSetOmniModel(); + auto traj = optimizer_tester.getOptimizedTrajectory(); + EXPECT_EQ(traj(5, 0), 0.0); // x + EXPECT_EQ(traj(5, 1), 0.0); // y + EXPECT_EQ(traj(5, 2), 0.0); // yaw + EXPECT_EQ(traj.rows(), 50); + EXPECT_EQ(traj.cols(), 3); optimizer_tester.reset(); optimizer_tester.shutdown(); From 6a172a09220a92d63bd9688d662f83c48a2fac6e Mon Sep 17 00:00:00 2001 From: Adi Vardi <57910756+adivardi@users.noreply.github.com> Date: Wed, 22 Oct 2025 20:11:19 +0200 Subject: [PATCH 32/35] Clean fix division by zero (from PR 5636) --- nav2_mppi_controller/src/critics/constraint_critic.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 33981c2272d..3d1d90cd6ad 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -84,16 +84,9 @@ void ConstraintCritic::score(CriticData & data) auto & wz = data.state.wz; const float min_turning_rad = acker->getMinTurningRadius(); - // std::cout << "constraint_critic acker vx: " << data.state.vx(Eigen::seq(0, 9), 0).transpose() << std::endl; - // std::cout << "constraint_critic acker wz: " << data.state.wz(Eigen::seq(0, 9), 0).transpose() << std::endl; - const float epsilon = 1e-6f; - auto wz_safe = wz.abs().max(epsilon); // Replace small wz values with epsilon to avoid division by 0 + 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); - // std::cout << "constraint_critic acker wz_safe: " << wz_safe(Eigen::seq(0, 9), 0).transpose() << std::endl; - // std::cout << "constraint_critic acker out_of_turning_rad_motion: " << out_of_turning_rad_motion(Eigen::seq(0, 9), 0).transpose() << std::endl; - - // std::cout << "constraint_critic costs before: " << data.costs(Eigen::seq(0, 9)).transpose() << std::endl; if (power_ > 1u) { data.costs += ((((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) + From 1822d1bd3c9acff885b0d53ba0511ef20e5481aa Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 23 Oct 2025 12:54:58 +0200 Subject: [PATCH 33/35] Revert "[mppi] constrain velocity & curvature in predict" This reverts commit c4afab0b78baad60d9accac93504849b5c445a76. --- .../nav2_mppi_controller/motion_models.hpp | 29 ------------------- 1 file changed, 29 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index e48dde1332f..4efd5f1cb3f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -125,17 +125,6 @@ class MotionModel // state.cwz.col(i - 1) = state.wz.col(i); // } - // // also apply velocity limits constrains - // state.vx.col(i) = state.vx.col(i) - // .cwiseMax(control_constraints_.vx_min) - // .cwiseMin(control_constraints_.vx_max); - - // state.wz.col(i) = state.wz.col(i) - // .cwiseMax(-control_constraints_.wz) - // .cwiseMin(control_constraints_.wz); - - - // also constrain wz base if (is_holo) { auto lower_bound_vy = (state.vy.col(i - 1) > 0).select(state.vy.col(i - 1) + min_delta_vy, @@ -148,17 +137,8 @@ class MotionModel .cwiseMax(lower_bound_vy) .cwiseMin(upper_bound_vy); state.vy.col(i) = 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) - // .cwiseMax(-control_constraints_.vy) - // .cwiseMin(control_constraints_.vy); } } - - applyConstraints(state); } /** @@ -172,7 +152,6 @@ class MotionModel * @param control_sequence Control sequence to apply constraints to */ virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {} - virtual void applyConstraints(models::State & /*state*/) {} protected: float model_dt_{0.0}; @@ -217,14 +196,6 @@ class AckermannMotionModel : public MotionModel .min(wz_constrained); } - void applyConstraints(models::State & state) override - { - const auto wz_constrained = state.vx.abs() / min_turning_r_; - state.wz = state.wz - .max((-wz_constrained)) - .min(wz_constrained); - } - /** * @brief Get minimum turning radius of ackermann drive * @return Minimum turning radius From 2ee9e062e6710c25310d9a0abc66f108ed7d86c4 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 23 Oct 2025 13:03:04 +0200 Subject: [PATCH 34/35] remove all the commented out code --- .../nav2_mppi_controller/motion_models.hpp | 26 ----- .../nav2_mppi_controller/tools/utils.hpp | 18 ---- nav2_mppi_controller/src/critic_manager.cpp | 5 - .../src/critics/constraint_critic.cpp | 2 - nav2_mppi_controller/src/optimizer.cpp | 94 +------------------ 5 files changed, 1 insertion(+), 144 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 4efd5f1cb3f..3659efcb6bb 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/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, diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index d4f97413a33..f518f16e169 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/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]; diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index abdd50e2443..d6acd320b05 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/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; diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 3d1d90cd6ad..882216b5e9e 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/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; } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 290abc819fb..201584656bb 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -191,9 +191,6 @@ std::tuple 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 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(state_.speed.linear.x); - // float wz_last = static_cast(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(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(state.speed.linear.x); - // state.wz.col(0) = static_cast(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(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()); From 3e42f4bbb2b8969843f788788b7bc4eac2e67477 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Thu, 23 Oct 2025 13:03:52 +0200 Subject: [PATCH 35/35] remove debugging prints & code --- .../nav2_mppi_controller/optimizer.hpp | 4 -- nav2_mppi_controller/src/optimizer.cpp | 53 ------------------- 2 files changed, 57 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 8ce9afc2e9e..17d74c974c8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -145,8 +145,6 @@ 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 @@ -282,8 +280,6 @@ 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_ = { diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 201584656bb..a50a2b8cbd9 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -216,61 +216,9 @@ std::tuple Optimizer::evalCon shiftControlSequence(); } - { - auto & s = settings_; - constexpr float epsilon = 1e-4f; - - // Check if accelerations exceed constraints - double dt = (control.header.stamp.sec - prev_control_twist_.header.stamp.sec) - + 1e-9 * (control.header.stamp.nanosec - prev_control_twist_.header.stamp.nanosec); - - float ax = (control.twist.linear.x - prev_control_twist_.twist.linear.x) / s.model_dt; - float ax_real = (control.twist.linear.x - prev_control_twist_.twist.linear.x) / dt; - if (std::abs(ax) > s.constraints.ax_max + epsilon) { - std::cout << "Acceleration constraint violated from last command " << ":\t"; - std::cout << "vx[i]: " << control.twist.linear.x << ", vx[i-1]: " << prev_control_twist_.twist.linear.x - << ", ax: " << ax << " real dt: " << dt << " real ax " << ax_real << "\n" ; - } - float wz = (control.twist.angular.z - prev_control_twist_.twist.angular.z) / s.model_dt; - float wz_real = (control.twist.angular.z - prev_control_twist_.twist.angular.z) / dt; - if (std::abs(wz) > s.constraints.az_max + epsilon) { - std::cout << "Angular Acceleration constraint violated from last command " << ":\t"; - std::cout << "wz[i]: " << control.twist.angular.z << ", wz[i-1]: " << prev_control_twist_.twist.angular.z - << ", az: " << wz << " real dt: " << dt << " real az " << wz_real << "\n" ; - } - } - - prev_control_twist_ = control; - prev_control_sequence_ = control_sequence_; - return std::make_tuple(control, optimal_trajectory); } -void Optimizer::computeControlSequenceAccel(const models::ControlSequence& control_sequence) -{ - auto & s = settings_; - - std::cout << std::endl; - for (long int i = 1; i < control_sequence.vx.size(); ++i) { - constexpr float epsilon = 1e-4f; - - // Check if accelerations exceed constraints - float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt; - if (std::abs(ax) > s.constraints.ax_max + epsilon) { - std::cout << "****Acceleration constraint violated at index " << i << ":\n"; - std::cout << "vx[i-1]: " << control_sequence.vx(i - 1) << ", vx[i]: " << control_sequence.vx(i) << ", ax: " << ax << "\n"; - } - - float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt; - if (std::abs(wz_accel) > s.constraints.az_max + epsilon) { - std::cout << "***Angular acceleration constraint violated at index " << i << ":\n"; - std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n"; - } - } - std::cout << std::endl; -} - - void Optimizer::optimize() { for (size_t i = 0; i < settings_.iteration_count; ++i) { @@ -571,7 +519,6 @@ void Optimizer::updateControlSequence() } utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); - control_sequence_virtual_ = control_sequence_; applyControlSequenceConstraints(); }