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.

SteveMacenski · 2025-10-23T20:07:36Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

+      path.poses[start_idx].pose.position.y;
+    float initial_translation = sqrtf(ab_x * ab_x + ab_y * ab_y);
+
+    if (initial_translation >= translation_threshold) {


This should be some very small epsilon, right? 1e-3 or so? Would we really need this to be a parameter? An in-place rotation should be unambiguously in the same spot

SteveMacenski · 2025-10-23T20:08:23Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

+    }
+
+    // Start of potential rotation sequence
+    float start_yaw = tf2::getYaw(path.poses[start_idx].pose.orientation);


Can't we just compare the heading if they're within 1e-3 of each other if they're different. If so, that's an in-place rotation? It really seems like this should be much simpler.

SteveMacenski · 2025-10-23T20:09:30Z

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

+ * @param rotation_threshold Minimum total rotation to consider significant
+ * @return Index after the rotation sequence if found, path size if no rotation detected
+ */
+inline unsigned int findFirstPathRotation(


For consideration: Do we really need this to be another function? We could just have this be a possible inversion check on findFirstPathInversion with a bool for check_rotations (probably a better argument name, but you get the idea). If so, we check if there's an inplace rotation to return.

SteveMacenski · 2025-10-23T20:11:53Z

nav2_mppi_controller/src/path_handler.cpp

  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
 }

+bool PathHandler::isWithinRotationTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)


Ditto in the path handler -- could we have a cool for if we check rotations in the existing isWithinInversionTolerances as to not complicate this more? I think the in-place rotation could count as something to prune for. Maybe we re-name "inversion" with something else ("discontinuity"?) or honestly leave as-is but have an arg for whether to include the in-place rotation check

SteveMacenski · 2025-10-23T20:12:44Z

nav2_mppi_controller/src/path_handler.cpp

+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+  return distance <= rotation_xy_tolerance_ && fabs(angle_distance) <= rotation_yaw_tolerance_;


Would you want the in-place rotation vs inversion to be any different? If not, then that would also speak to removing duplication so the same tolerance parameters can be used either way :-) Thats what you have the defaults set to, so that's making me think that's a good idea

Personally at this point no but I can imagine it can be desirable. For simplicity I removed it for now

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 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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

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

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

tonynajjar commented Oct 23, 2025 •

edited

Loading

Uh oh!

codecov bot commented Oct 23, 2025 •

edited

Loading

Uh oh!

SteveMacenski commented Oct 23, 2025

Uh oh!

SteveMacenski left a comment

Uh oh!

SteveMacenski Oct 23, 2025

Uh oh!

SteveMacenski Oct 23, 2025

Uh oh!

SteveMacenski Oct 23, 2025

Uh oh!

SteveMacenski Oct 23, 2025

Uh oh!

SteveMacenski Oct 23, 2025

Uh oh!

tonynajjar Oct 28, 2025

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!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

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!

SteveMacenski Oct 23, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Oct 23, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Oct 23, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Oct 23, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Oct 23, 2025

Choose a reason for hiding this comment

Uh oh!

tonynajjar Oct 28, 2025

Choose a reason for hiding this comment

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!

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