MPPI: Clip path up to in-place rotation #5648

tonynajjar · 2025-10-23T15:37:04Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

Description of contribution in a few bullet points

With SMACLattice I now have in-place rotations in my path and want to force MPPI to prune up until these rotations. The concept is similar to how cusps are handled.

Inside the inversion-detection logic, I added the detection of in-place rotations and the optional pruning of the path up until that point
if both enforce_path_rotation and enforce_path_inversion are true, the path will be clipped to the first occurrence

Description of documentation updates required from your changes

Added new parameter, so need to add that to default configs and documentation page

Description of how this change was tested

Need to write/adapt tests

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
Should this be backported to current distributions? If so, tag with backport-*.

codecov · 2025-10-23T16:16:31Z

Codecov Report

❌ Patch coverage is 57.69231% with 22 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...oller/include/nav2_mppi_controller/tools/utils.hpp	55.00%	18 Missing ⚠️
nav2_mppi_controller/src/path_handler.cpp	66.66%	4 Missing ⚠️

Files with missing lines	Coverage Δ
...nclude/nav2_mppi_controller/tools/path_handler.hpp	`100.00% <ø> (ø)`
nav2_mppi_controller/src/path_handler.cpp	`83.56% <66.66%> (-0.27%)`	⬇️
...oller/include/nav2_mppi_controller/tools/utils.hpp	`90.41% <55.00%> (-7.29%)`	⬇️

... and 8 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

SteveMacenski · 2025-10-23T20:05:40Z

@mini-1235 this seems highly related to your controller server refactor PR.

SteveMacenski

I think this can be simplified in both the main util function implementation & the integration by having the rotational checks be inserted in the inversion checks / tolerance validation methods as an optional setting to check for in-place rotations.

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

nav2_mppi_controller/src/path_handler.cpp

Signed-off-by: Tony Najjar <[email protected]>

mini-1235 · 2025-10-28T11:23:28Z

@mini-1235 this seems highly related to your controller server refactor PR.

Sorry I just saw this

Hi @tonynajjar, in #5446 I actually added in-place roation check in findFirstPathInversion

navigation2/nav2_util/include/nav2_util/path_utils.hpp

Lines 107 to 119 in c669b68

    
           if ( 
        
             (hypot(oa_x, oa_y) == 0.0 && 
        
             path.poses[idx - 1].pose.orientation != 
        
             path.poses[idx].pose.orientation) 
        
             || 
        
             (hypot(ab_x, ab_y) == 0.0 && 
        
             path.poses[idx].pose.orientation != 
        
             path.poses[idx + 1].pose.orientation)) 
        
           { 
        
             // returning the distance since the points overlap 
        
             // but are not simply duplicate points (e.g. in place rotation) 
        
             return idx + 1; 
        
           }

Could you take a look and see if that also works for your case?

tonynajjar · 2025-10-28T12:17:48Z

Could you take a look and see if that also works for your case?

@mini-1235 thanks for the heads-up. I won't be able to try out the whole PR but I took a look and there are some differences in functionality:

I added the param minimum_rotation_angle, for the user to potentially ignore very small rotations. e.g. small rotations occur with SMACLattice when rotation_penalty is very small
nitpick: hypot(oa_x, oa_y) == 0.0 maybe better to compare with a very small number e.g. 1e-3, depending on how the planner generates these in-place rotations

mini-1235 · 2025-10-28T14:16:45Z

Thanks @tonynajjar, that makes sense! Will update in the PR accordingly

SteveMacenski · 2025-11-25T18:43:58Z

@mini-1235 should your SimplePathHandler PR have this updated to respect?

SteveMacenski · 2025-11-25T18:39:55Z

nav2_mppi_controller/src/path_handler.cpp

  }
+
+  if (enforce_path_rotation_) {
+    getParam(minimum_rotation_angle_, "minimum_rotation_angle", 0.785);


Is 45 deg sensible for this limit by default?

Also parameter guide updates required for this param addition (among others)

Also for simplicity, I think this can go into the if (enforce_path_inversion_ || enforce_path_rotation_) conditional. If !enforce_path_rotation_ then it won't be used anyway.

SteveMacenski · 2025-11-25T18:41:57Z

nav2_mppi_controller/src/path_handler.cpp

+      float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(
+        global_plan_up_to_inversion_, rotation_check);


Suggested change

float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;

inversion_locale_ = utils::removePosesAfterFirstInversion(

global_plan_up_to_inversion_, rotation_check);

float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;

inversion_locale_ = utils::removePosesAfterFirstInversion(

global_plan_up_to_inversion_, rotation_threshold);

I think the check implies its a boolean when its really a threshold

SteveMacenski · 2025-11-25T18:42:10Z

nav2_mppi_controller/src/path_handler.cpp

+  float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(
+    global_plan_up_to_inversion_, rotation_check);


Suggested change

float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;

inversion_locale_ = utils::removePosesAfterFirstInversion(

global_plan_up_to_inversion_, rotation_check);

float rotation_threshold = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;

inversion_locale_ = utils::removePosesAfterFirstInversion(

global_plan_up_to_inversion_, rotation_threshold);

SteveMacenski · 2025-11-25T18:43:07Z

nav2_mppi_controller/src/path_handler.cpp

+
+  // Find and restrict to the first rotation or inversion constraint
+  float rotation_check = enforce_path_rotation_ ? minimum_rotation_angle_ : 0.0f;
+  inversion_locale_ = utils::removePosesAfterFirstInversion(


Isn't this missing the if for if we are even using path inversions (+ your new rotations)?

SteveMacenski · 2025-11-25T18:45:13Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

-  if (first_after_inversion == path.poses.size()) {
-    return 0u;
+
+  const unsigned int first_constraint = findFirstPathInversion(cropped_path, rotation_threshold);


Rename to first_after_constraint

SteveMacenski · 2025-11-25T18:48:06Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

-    float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
-    if (dot_product < 0.0f) {
-      return idx + 1;
+  unsigned int first_constraint = path.poses.size();


first_after_constraint as well

SteveMacenski · 2025-11-25T18:51:25Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

+ * @return the first point after the inversion or rotation found in the path
 */
-inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
+inline unsigned int findFirstPathInversion(


Not a required change, but I think this would be more compact (efficient, sure, though mostly for compactness) if we could combine this into a single loop. There's much duplication and really the only difference is that the minimum length is 3 rather than 4 (which could be hand waved away perhaps?)

SteveMacenski · 2025-11-25T18:51:56Z

Pretty small peanuts, should be able to merge after the next set of changes.

@mini-1235 please review yourself :-)

mini-1235 · 2025-11-26T10:57:54Z

@mini-1235 should your SimplePathHandler PR have this updated to respect?

Yes, I forgot to push that update upstream

@mini-1235 please review yourself :-)

Will review tomorrow / Friday

SteveMacenski requested changes Oct 23, 2025

View reviewed changes

PR fixes

4d72845

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar force-pushed the in-place-rotation-crop branch from 2a1060f to 4d72845 Compare October 27, 2025 17:35

refactor

8995617

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar requested a review from SteveMacenski October 28, 2025 09:45

SteveMacenski requested changes Nov 25, 2025

View reviewed changes

MPPI: Clip path up to in-place rotation #5648

Are you sure you want to change the base?

MPPI: Clip path up to in-place rotation #5648

Conversation

tonynajjar commented Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

codecov bot commented Oct 23, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski commented Oct 23, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mini-1235 commented Oct 28, 2025

Uh oh!

tonynajjar commented Oct 28, 2025

Uh oh!

mini-1235 commented Oct 28, 2025

Uh oh!

SteveMacenski commented Nov 25, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Nov 25, 2025

Uh oh!

mini-1235 commented Nov 26, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

tonynajjar commented Oct 23, 2025 •

edited

Loading

codecov bot commented Oct 23, 2025 •

edited

Loading