From bb7d90be22afe9ad9b82dc7af647f41b98db631f Mon Sep 17 00:00:00 2001 From: eegeugur Date: Wed, 15 Oct 2025 23:03:29 +0300 Subject: [PATCH 1/4] Preserve gaussian proposals and enforce accel limits at command boundary --- .../nav2_mppi_controller/motion_models.hpp | 18 ++++--- nav2_mppi_controller/src/optimizer.cpp | 49 ++++++++++++++----- 2 files changed, 49 insertions(+), 18 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..6875bde70ed 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,19 @@ 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.vx.col(i) = state.vx.col(i) + .cwiseMax(control_constraints_.vx_min) + .cwiseMin(control_constraints_.vx_max); - 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); + state.wz.col(i) = state.wz.col(i) + .cwiseMax(-control_constraints_.wz) + .cwiseMin(control_constraints_.wz); if (is_holo) { auto lower_bound_vy = (state.vy.col(i - 1) > @@ -106,10 +110,12 @@ 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); + state.vy.col(i) = state.vy.col(i) + .cwiseMax(-control_constraints_.vy) + .cwiseMin(control_constraints_.vy); } } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 013b29de502..cfc4391774f 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -300,14 +300,39 @@ void Optimizer::applyControlSequenceConstraints() float max_delta_vy = s.model_dt * s.constraints.ay_max; float min_delta_vy = s.model_dt * s.constraints.ay_min; float max_delta_wz = s.model_dt * s.constraints.az_max; - float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0)); - float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0)); - control_sequence_.vx(0) = vx_last; - control_sequence_.wz(0) = wz_last; + + float& vx0 = control_sequence_.vx(0); + vx0 = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx0); + if (state_.speed.linear.x > 0.0f) { + vx0 = utils::clamp(state_.speed.linear.x + min_delta_vx, + state_.speed.linear.x + max_delta_vx, vx0); + } else { + vx0 = utils::clamp(state_.speed.linear.x - max_delta_vx, + state_.speed.linear.x - min_delta_vx, vx0); + } + + float& wz0 = control_sequence_.wz(0); + wz0 = utils::clamp(-s.constraints.wz, s.constraints.wz, wz0); + wz0 = utils::clamp(state_.speed.angular.z - max_delta_wz, + state_.speed.angular.z + max_delta_wz, wz0); + + if (isHolonomic()) { + float& vy0 = control_sequence_.vy(0); + vy0 = utils::clamp(-s.constraints.vy, s.constraints.vy, vy0); + if (state_.speed.linear.y > 0.0f) { + vy0 = utils::clamp(state_.speed.linear.y + min_delta_vy, + state_.speed.linear.y + max_delta_vy, vy0); + } else { + vy0 = utils::clamp(state_.speed.linear.y - max_delta_vy, + state_.speed.linear.y - min_delta_vy, vy0); + } + } + + float vx_last = control_sequence_.vx(0); + float wz_last = control_sequence_.wz(0); float vy_last = 0; if (isHolonomic()) { - vy_last = utils::clamp(-s.constraints.vy, s.constraints.vy, control_sequence_.vy(0)); - control_sequence_.vy(0) = vy_last; + vy_last = control_sequence_.vy(0); } for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) { @@ -484,22 +509,22 @@ void Optimizer::updateControlSequence() auto & s = settings_; auto vx_T = control_sequence_.vx.transpose(); - auto bounded_noises_vx = state_.cvx.rowwise() - vx_T; + auto noises_vx = state_.cvx.rowwise() - vx_T; 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 * (noises_vx.rowwise() * vx_T).rowwise().sum()).eval(); if (s.sampling_std.wz > 0.0f) { auto wz_T = control_sequence_.wz.transpose(); - auto bounded_noises_wz = state_.cwz.rowwise() - wz_T; + auto 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(); + costs_ += (gamma_wz * (noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); } if (is_holo) { auto vy_T = control_sequence_.vy.transpose(); - auto bounded_noises_vy = state_.cvy.rowwise() - vy_T; + auto noises_vy = state_.cvy.rowwise() - vy_T; const float gamma_vy = s.gamma / (s.sampling_std.vy * s.sampling_std.vy); - costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval(); + costs_ += (gamma_vy * (noises_vy.rowwise() * vy_T).rowwise().sum()).eval(); } auto costs_normalized = costs_ - costs_.minCoeff(); From 7585923a40d9a0843a02271c99305bb1a6d3cbbb Mon Sep 17 00:00:00 2001 From: eegeugur Date: Thu, 16 Oct 2025 19:25:07 +0300 Subject: [PATCH 2/4] Command the computed control for the current state --- 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 cfc4391774f..092c0a16f9f 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -546,13 +546,11 @@ 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); 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 0d5fc885151438619762402e97a62261bf778ebc Mon Sep 17 00:00:00 2001 From: eegeugur Date: Fri, 17 Oct 2025 11:47:22 +0300 Subject: [PATCH 3/4] Fix acceleration violation after SG filter --- .../include/nav2_mppi_controller/optimizer.hpp | 5 +++++ .../include/nav2_mppi_controller/tools/utils.hpp | 14 +------------- nav2_mppi_controller/src/optimizer.cpp | 13 ++++++++++++- 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 1914abde647..4335dc3b89f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -230,6 +230,11 @@ class Optimizer */ void updateControlSequence(); + /** + * @brief Update control history with the final command + */ + void updateHistory(); + /** * @brief Convert control sequence to a twist command * @param stamp Timestamp to use 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 4a3cc6b7c06..4c5efb313a7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -431,12 +431,10 @@ inline float posePointAngle( * @brief Apply Savisky-Golay filter to optimal trajectory * @param control_sequence Sequence to apply filter to * @param control_history Recent set of controls for edge-case handling - * @param Settings Settings to use */ inline void savitskyGolayFilter( models::ControlSequence & control_sequence, - std::array & control_history, - const models::OptimizerSettings & settings) + std::array & control_history) { // Savitzky-Golay Quadratic, 9-point Coefficients Eigen::Array filter = {-21.0f, 14.0f, 39.0f, 54.0f, 59.0f, 54.0f, 39.0f, 14.0f, @@ -498,16 +496,6 @@ inline void savitskyGolayFilter( applyFilterOverAxis( control_sequence.wz, initial_control_sequence.wz, control_history[0].wz, control_history[1].wz, control_history[2].wz, control_history[3].wz); - - // Update control history - unsigned int offset = settings.shift_control_sequence ? 1 : 0; - control_history[0] = control_history[1]; - control_history[1] = control_history[2]; - control_history[2] = control_history[3]; - control_history[3] = { - control_sequence.vx(offset), - control_sequence.vy(offset), - control_sequence.wz(offset)}; } /** diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 092c0a16f9f..668582ddfa2 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -192,6 +192,9 @@ std::tuple Optimizer::evalCon do { optimize(); + utils::savitskyGolayFilter(control_sequence_, control_history_); + applyControlSequenceConstraints(); + updateHistory(); optimal_trajectory = getOptimizedTrajectory(); switch (trajectory_validator_->validateTrajectory( optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal)) @@ -210,7 +213,6 @@ std::tuple Optimizer::evalCon } } while (fallback(critics_data_.fail_flag || !trajectory_valid)); - utils::savitskyGolayFilter(control_sequence_, control_history_, settings_); auto control = getControlFromSequenceAsTwist(plan.header.stamp); if (settings_.shift_control_sequence) { @@ -543,6 +545,15 @@ void Optimizer::updateControlSequence() applyControlSequenceConstraints(); } +void Optimizer::updateHistory() { + control_history_[0] = control_history_[1]; + control_history_[1] = control_history_[2]; + control_history_[2] = control_history_[3]; + control_history_[3] = {control_sequence_.vx(0), + isHolonomic() ? control_sequence_.vy(0) : 0.0f, + control_sequence_.wz(0)}; +} + geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist( const builtin_interfaces::msg::Time & stamp) { From d293e576cb3acb0ed44a49b2561dc12fd9cd08b6 Mon Sep 17 00:00:00 2001 From: eegeugur Date: Fri, 17 Oct 2025 14:25:32 +0300 Subject: [PATCH 4/4] Update SG filter test --- nav2_mppi_controller/test/utils_test.cpp | 29 ++++++++++++------------ 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 70733e37db4..5e154e18156 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -310,20 +310,21 @@ TEST(UtilsTests, SmootherTest) history[0].wz = 0.0; history_init = history; - models::OptimizerSettings settings; - settings.shift_control_sequence = false; // so result stores 0th value in history - - savitskyGolayFilter(noisey_sequence, history, settings); - - // Check history is propagated backward - EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02); - EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02); - EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02); - - // Check history element is updated for first command - EXPECT_NEAR(history[3].vx, 0.2, 0.05); - EXPECT_NEAR(history[3].vy, 0.0, 0.035); - EXPECT_NEAR(history[3].wz, 0.23, 0.02); + savitskyGolayFilter(noisey_sequence, history); + + // History should remain unchanged + EXPECT_NEAR(history_init[0].vx, history[0].vx, 1e-6); + EXPECT_NEAR(history_init[0].vy, history[0].vy, 1e-6); + EXPECT_NEAR(history_init[0].wz, history[0].wz, 1e-6); + EXPECT_NEAR(history_init[1].vx, history[1].vx, 1e-6); + EXPECT_NEAR(history_init[1].vy, history[1].vy, 1e-6); + EXPECT_NEAR(history_init[1].wz, history[1].wz, 1e-6); + EXPECT_NEAR(history_init[2].vx, history[2].vx, 1e-6); + EXPECT_NEAR(history_init[2].vy, history[2].vy, 1e-6); + EXPECT_NEAR(history_init[2].wz, history[2].wz, 1e-6); + EXPECT_NEAR(history_init[3].vx, history[3].vx, 1e-6); + EXPECT_NEAR(history_init[3].vy, history[3].vy, 1e-6); + EXPECT_NEAR(history_init[3].wz, history[3].wz, 1e-6); // Check that path is smoother float smoothed_val{0}, original_val{0};