Path Hug Critic

[silanus23]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5037
Primary OS tested on	Ubuntu
Robotic platform tested on	nav2 bringup pack
Does this PR contain AI generated software?	unit tests
Was this PR description generated by AI software?	nope

---

Description of contribution in a few bullet points

After other PR I have tried to add path hug critic to path align critic. Refactored a bit. It looks too complex now can be simplified a bit now. New functionalites added to critics are:

• I have splitted critic to 2. Arclength is previous version I didn't touch it as much possible.
• To not calculate each segment's longthitude everytime, I have added a function updatePathCache it calculates euclidean distance every time path renewed so we don't have to recalculate it in every iteration.
• I have added computeMinDistancePath to calculate the minimum distance to path. It takes a hint from the previous traj point so it can search and find only in search window.
• Robot can go back so it splits search window and starts search from back. As a next step we can add a lookback parameter too.
• The code advances trajectory by trajectory.
• Code assumes traj[0] is current position.

Description of documentation updates required from your changes

Path Align critic's page needs refactorization

Description of how this change was tested

I tried to isolate effects of critic by only testing it with FollowPath and making it's weight 3. I have seen some visual improvements. Further testing could be helpfull.

---

Future work that may be required in bullet points

• Advices and helps about TODO's in code would be very helpfull.
• Usage of eval should be researched.
• A PoC video.
• Seperating power and weight for each mode.
• Consider adding a lookback param instead dividing search window
• Create a diagram for migration guide.

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

[mergify]

@silanus23, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[silanus23]

@mini-1235 If you could give this PR a look I would be very gratefull.

[mini-1235]

@silanus23 Thanks for mentioning, but I don't have the bandwidth to go through all your PRs at the moment. I would like to focus first on the BT node (which I have already reviewed), and then I will look into the costmap plugin or MPPI critic next week.

These PRs look like good additions, and I want to make sure they solve the original issue and don't break the existing behavior.

I tried to isolate effects of critic by only testing it with FollowPath and making it's weight 3. I have seen some visual improvements. Further testing could be helpfull.

For your PRs, it would be very helpful if you could include a before/after video demonstrating the behavior change. That would make it much easier to understand the current status without having to rely only on code review or running the tests myself.

[silanus23]

There is still a room for improvement but I think current version is a good approach and a starting point.
I have recorded bags of tracking_error_feedback and analyzed results I put the image above and here are test conditions:

Results:

7/9 runs show improvement (mean: ~17%, max: ~27%, std: ~21%)
Runs 7-9 (weight=30, stride=2) show most consistent gains: 24-38% mean error reduction
Run 5 outlier was caused by high CPU usage triggering re-SLAM - fixed in current version and hasn't reoccurred

Test configuration:

Runs 1-6: stride=4, decreasing weights (run 6: weight=30)
Runs 7-9: stride=2, weight=30

Not:
CPU tests are on the way too.

[codecov]

Codecov Report

❌ Patch coverage is 97.24138% with 4 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
..._mppi_controller/src/critics/path_align_critic.cpp	96.58%	4 Missing ⚠️

Files with missing lines	Coverage Δ
...nav2_mppi_controller/critics/path_align_critic.hpp	100.00% <100.00%> (ø)
...oller/include/nav2_mppi_controller/tools/utils.hpp	97.50% <100.00%> (+0.03%)	⬆️
..._mppi_controller/src/critics/path_align_critic.cpp	95.62% <96.58%> (+1.77%)	⬆️

... and 19 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[silanus23]

https://drive.google.com/drive/folders/1nfrqVMZcp--n5_tBdhfYqg-eJUJUrH_s

Above link provides linke for ros2 bags. I will clean unnecssary tests and fill some comment gaps today alongside lint errors today. Please let me do before close review.

Here is the comparisions that I have made after some optimizations. Story is :
1/3 : In wiiide open fields ending with corners

4-5: Zig zag corridor where robot needs to turn around before starting

6-7: Only zig zag corridor.

Main optimization is terminating search if traj. point is close enough.

Thanks.

[SteveMacenski]

What was the deviation limit you set to get those results? Also would want to know the runtime impact of this as well on performance -- but generally those metrics tell me you're on the right track!

It looks like you end up replacing alot of the PathAlign logic though - maybe this is good as another critic so we keep some legacy behavior? It seems sensible to me that if using tracking error (or even without it, just a different method from the tracking-error mindset) there's some changes in the way things are structured and therefore applying a different strategy. In that case, we'd want to keep the original around for those that use it today and we can always create a new (improved) critic which would also make testing easier

[silanus23]

❯ python3 critic_analyzer.py crit_logs.txt
MPPI CRITIC PERFORMANCE ANALYSIS

PathAlignCritic:
  Average: 0.843 ms
  Std Dev: 0.372 ms
  Min:     0.614 ms
  Max:     4.167 ms
  Count:   265
  Near Max (≥90%): 1 times (0.4%)

PathHugCritic:
  Average: 0.769 ms
  Std Dev: 0.292 ms
  Min:     0.442 ms
  Max:     3.143 ms
  Count:   267
  Near Max (≥90%): 1 times (0.4%)

Above is a critic analyzer I have compared it with path align.

Here is the logs link
https://drive.google.com/file/d/1kCf0Q-U6WP3XxMk5MdCy-zgYSRli0sZZ/view?usp=drive_link

Here is a video

https://drive.google.com/file/d/161QaCOEU1vLY45UGgYg-6OVIJmmhigmL/view?usp=drive_link

I am not sure if this will be exactly like this but I have tried couple of times and see the same results. I am basically drawing a U shape with a basic python node.

Below new ros2 bags to prove. Please forget first 2 in this and above images. Last 4 here are taken from a zig zag corridor in tb4 .

I know currently the implementation has many missing points. But I wanted to make sure the path is correct before moving with tthe polishing. So please focus on formula and results. I wouldn't want to waste your time.

About that max improvement going wron on 1 run: I don't think it's fully about this critic cause we have to stride in order to gain a bit of a performance and that run was still better on average. I have tried some other methods.

• I have tried to predict where the next hint is via trajectory velocity and path point density. It was very low on cpu usage but improvements were half of the current

• I have tried to take the search window longthitude as segment index number. It worked but it wasn't so nav2 style.

• I use exponential search around the hint to find window because theoretically it's better and since the probably close to the hint better than binary search. Seen this while trying.

Last detail : Why do I re-find path start point everytime? Cause caching it was decreasing the quality especially if 1 time it falls to wrong side it was creating too much inconsistency and this was forcing me to increase parameters making the usage worse.

[SteveMacenski]

Maybe I've just lost the thread here - but how does this use the tracking error and scoring MPPI to stay within set tracking error bounds? I don't see that message / information used here.

I can see your metrics here on the path tracking error reduction, but that wasn't really what the aim was at the moment. I'm also wondering for staying that accurate on the path if you have an issue with deviating due to obstacles blocking and seems like the robot may be going much slower and stopping in order to maintain the tight distance (though may just be an artifact of tuning - and use of path align both).

Maybe something to hold onto for us to look into another time, but I think that should be a separate ticket on a separate topic to improve more precise path tracking behavior and the trade offs we want to make therein.

[silanus23]

I am using the same mathematicall approach that is used in while publishing tracking error in the path_utils file. I didn't use that directly, the inputs of it were diffirent and I wanted to use eigen directly. I only made tiny improvements to fit into mppi's logic better, those above things are just side improvements I have tried or placed.

I scored every trajectory point based on distance from path, even when already close. I guess this took away the robot's breathing gap. I can change this directly to a distance limited approach and scoring high only if robot deviates too much. This would make a better fit to your vision making this more bound oriented I guess.

About the video, I have used normal tune + path hug params in the yaml in PR. The slowness is likely from this. I guess I overstretched a bit while trying to prove this works.Would making this limit based solve the problem or should I try diffirent approach?

[silanus23]

I have made it to threshold based tracking error elliminator only.

Comparison videos:

Without: (https://drive.google.com/file/d/1qhEjPC4melmXeuVJF4SCZwCbdtr84xfO/view?usp=drive_link)

With: (https://drive.google.com/file/d/1sFgq0Jap8YTXx6d00T3v_uiUAT7TUPnA/view?usp=drive_link)

As seen in the videos, it handles U-turns better than the current setup. The robot's speed can be improved with more careful tuning of the parameters.

Testing with a script listening to tracking_error_feedback shows that the robot now exceeds the set limit 10x less than the current implementation.

Parameters used in video:

max_allowed_distance: 0.05

violation_penalty_scale: 10.0

TL;DR: It calculates candidate trajectories, finds the tracking_error value for every point, and scores it according to the max_allowed_distance. The only logic change is in the index search, which now uses a search-based approach to meet the requirements of the MPC speed.

[SteveMacenski]

The robot's speed can be improved with more careful tuning of the parameters.

Can you show that? I suppose its not always just a matter of tuning, sometimes when we developed the original plugins for MPPI we had to try 6+ formulations to find one that really worked well and didn't have other trade offs that were unacceptable.

TL;DR: It calculates candidate trajectories, finds the tracking_error value for every point, and scores it according to the max_allowed_distance. The only logic change is in the index search, which now uses a search-based approach to meet the requirements of the MPC speed.

I think this is where maybe we are getting lost. I totally support better path tracking behavior if that comes with reasonable trade offs (we can always have the Path Align critic for those that want it; more options is always good). For instance, if we say this one is for very accurate tracking but may not be as flexible to go around obstacles -- or something like that. For people that want stricter path tracking behavior. Assuming we can get that and deal with the velocity issues.

So hands down, I support this line of thinking. However...

What I was looking for in the ticket was the following:

• Using the tracking error information to penalize the trajectory to stay in bounds
• Meaning either/both (a) setting a critical_cost high value for trajectory samples that violate the constraint, (b) optionally incentivize the robot to not get close to it by applying a penalty when getting close to the boundary / continuously in the 'tunnel' to better be along the path / etc. It may not be trying to get it to stay strictly aligned, but penalizing when it gets close to the boundary to repel it. But that's worth testing to see if setting a continuous cost based on distance or only starting the cost when its sufficiently far away is better. I suspect having a 'grace' area where we don't penalize costs would work better to give the system more flexibility and dynamicism when we're talking about optimizing for very small cost differences near the path. Its really more about keeping it away from the boundary and definitely from violating it.

I think your current work is more focused on very accurate path tracking which is related but not exactly the same thing. For many situations, you may want the robot to be able to deviate within the bounds flexibly, but never violate it. So you would only want (a). In other situations, you may want the robot to generally follow the path well, but also be able to get around small issues within tracking bounds, so you'd want (b). In other situations, you want strict path tracking -- which is more what your current work focuses on.

[silanus23]

Current one had major changes.
It takes averages optionally. Default mod only looks for a strict tracking error corridor. If use_soft_repulsion param true it's adding averages too to those who don't go beyond strict one. Soft repulsion increases linearly.

An important thing to note is If there ain't no trajectory inside boundaries. It's giving a warn and either making 0 for every or just giving averages according soft repulsion parameter. I am not sure about this behaviour. If exceeding limits should I add average regardless?

I hope this time I get it correctly. Sorry for my previous confusions.

Note:
I have tested this but still there is a room for little bit push to see how much weight can soft repulsion has without slowing down etc. etc..

This is close to ready review but not. I am just trying to be sure my direction is correct. So please let me finish fully before full review.

[silanus23]

0.75 Error limit

PathAlign:
https://drive.google.com/file/d/1u8EAqQ7n_RWvLtfmEuBGnIvPgPLy21hW/view?usp=drive_link
PathHugCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
trajectory_point_step: 2
threshold_to_consider: 0.5
search_window: 0.2
lookahead_distance: 0.35
max_allowed_distance: 0.075
collision_cost: 100000.0
use_soft_repulsion: true
grace_distance: 0.01
PathHug:
https://drive.google.com/file/d/189p3MXKEKpWDilMfrs4_xcjbKKTG28Wi/view?usp=drive_link

0.50 Error limit

PathAlign:
https://drive.google.com/file/d/1iG53wtv5ecfshyx6J1rTPcLNvu5uRmuP/view?usp=drive_link
PathHugCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
trajectory_point_step: 2
threshold_to_consider: 0.5
search_window: 0.2
lookahead_distance: 0.55
max_allowed_distance: 0.05
collision_cost: 100000.0
use_soft_repulsion: true
grace_distance: 0.01
PathHug:
https://drive.google.com/file/d/12X35XArpCoO_wa2srQk4N3Xrmts9x1qR/view?usp=drive_link

  PathHugCritic:
    enabled: true
    cost_power: 1
    cost_weight: 3.0
    trajectory_point_step: 2
    threshold_to_consider: 0.5
    search_window: 0.2
    lookahead_distance: 0.45
    max_allowed_distance: 0.05
    collision_cost: 100000.0
    use_soft_repulsion: true
    grace_distance: 0.01

PathHug:
https://drive.google.com/file/d/1wE04mlJG5xlvAF9C84X0U_DlvAQe1xEm/view?usp=drive_link

With Path Hug robot was struggling to turn around so I have made PathFollowCritic's weight 7.0 including Path Align tests to equalize.

PathHugCritic: enabled: true cost_power: 1 cost_weight: 3.0 trajectory_point_step: 2 threshold_to_consider: 0.5 search_window: 0.1 lookahead_distance: 0.35 max_allowed_distance: 0.075 collision_cost: 100000.0 use_soft_repulsion: true grace_distance: 0.01

PathHugCritic: Samples : 235 Average : 1.344 ms Std Dev : 0.560 ms 
Min : 0.506 ms p50 : 1.340 ms p95 : 2.292 ms 
Max : 3.330 ms Near-max (≥90% of max): 1 (0.4%) 

PathAlignCritic: Samples : 235 Average : 1.109 ms Std Dev : 0.406 ms 
Min : 0.497 ms p50 : 1.183 ms p95 : 1.725 ms Max : 1.953 ms Near-max (≥90% of max): 8 (3.4%) 

Result: PathAlignCritic is 1.21x FASTER than PathHugCritic on average.
 ================================================================================

Next steps and notes

• Hard boundary enforcement is fundamentally limited by localization noise and path replanning. Current version is the closest practical approach to strict enforcement.

• As you can see speed is not a problem when tested without PathAlign .

• From my observations it's providing benefits at max_allowed_distance at 0.75 and lower but I think 0.5 is the sweet spot about benefits.

• Unfortunately even with the light weight combination above it's still slower than PathAlign and it gets slower quickly with lookahead_distance

• search_window below 0.2 needs more testing, particularly at tight max_allowed_distance values.

• I know you generally want this cross tracking feature to be distance oriented instead path point oriented but in critics we are dealing with a lot smaller values. Maybe in search_window and/or lookahead_distance can be index oriented in here. Cause search_window is going to be between 2 or 6 at worst anyway.

• When it comes to lookahead_distance it's a bit more complicated it increases point count too much on curved paths. I am aware of that corners need more points to stabilize but current distance based approach is creating spikes. We can use that computation gap to evaluate more points while usage staying stable. There maybe a sweet spot in here that we can use.

• Maybe there might be a dynamic point number finder with less computation than distance but more flexible than point number based system. I am really not sure if this is possible.

• Would a policy like if 2 points too close (defined by a parameter) don't bother evaluating both work? This elliminate too closely clustered points could be done in cumulative distance finding phase so search window can benefit it and we don't need to check this again again.

What do you think should I give it a try or is there a better way I can't see? Maybe just leave this side to user they can still get good restriction with low usage but it would be a bit stuttery. BTW when it moves smoother it becomes more resiliant to noise.