45-50% performance improvement in MPPI controller using Eigen library for computation.

[Ayush1285]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4237 #3351
Primary OS tested on	Ubuntu
Robotic platform tested on	NA, Optimizer Benchmark
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Replaced xtensor with Eigen library for computation.
• Performance increased by 45-50% with footprint and critics enabled.
• Currently WIP

Description of documentation updates required from your changes

---

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

[mergify]

This pull request is in conflict. Could you fix it @Ayush1285?

[mergify]

@Ayush1285, 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).

[mergify]

@Ayush1285, 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).

[mergify]

@Ayush1285, 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).

[SteveMacenski]

I did a pretty fast review, so far from complete on each detail, but a good starting point!

[mergify]

@Ayush1285, 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).

[mergify]

@Ayush1285, 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).

[SteveMacenski]

Let me know here when you want me to take a look again! I'm quite excited for this work - even if for no reason than to move to Eigen + 10% performance boost, since Eigen's release and support is much more known than xtensor's

[Ayush1285]

Let me know here when you want me to take a look again! I'm quite excited for this work - even if for no reason than to move to Eigen + 10% performance boost, since Eigen's release and support is much more known than xtensor's

Sure, I'm trying a few optimizations and will push changes once I'm done.

[mergify]

@Ayush1285, 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).

[Ayush1285]

Let me know here when you want me to take a look again! I'm quite excited for this work - even if for no reason than to move to Eigen + 10% performance boost, since Eigen's release and support is much more known than xtensor's

I've completed the migration to Eigen. But we need to make sure that functionality-wise everything is correct or not. I'll run tests and ensure that all of them are passing. Meanwhile, you can take a look at the latest changes.

[mergify]

@Ayush1285, 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).

[SteveMacenski]

I did a first look through (didn't analyze in detail the math in the critics, but higher level programming items first), but generally looks good to me with some details to answer!

[SteveMacenski]

Worth having a util for clamp that implements this instead of putting inline for each location? It is prone to copy+paste errors

[SteveMacenski]

I think this is still nice-to-have, but I won't block for it

[SteveMacenski]

@Ayush1285 thoughts on adding this? it would make things quite a bit more readable and less error prone for future modifiations

[Ayush1285]

We can add an util function. What will it implement? Clamping for all x, y, and z values, or just wrapping std::min(..., std::max())?

[SteveMacenski]

Just a wrapper around min(max())

[mergify]

@Ayush1285, 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).

[mergify]

This pull request is in conflict. Could you fix it @Ayush1285?

[Ayush1285]

@SteveMacenski It seems system tests are passing now after fixing the CostCritic issue but dwb_critics tests failed, Is it a flaky test? On my local system, it is passing.

[codecov]

Codecov Report

Attention: Patch coverage is 83.49515% with 51 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
..._mppi_controller/src/critics/constraint_critic.cpp	21.05%	15 Missing ⚠️
...ontroller/src/critics/velocity_deadband_critic.cpp	25.00%	12 Missing ⚠️
...2_mppi_controller/src/critics/obstacles_critic.cpp	70.58%	5 Missing ⚠️
..._mppi_controller/src/critics/path_angle_critic.cpp	85.71%	3 Missing ⚠️
...i_controller/src/critics/prefer_forward_critic.cpp	40.00%	3 Missing ⚠️
nav2_mppi_controller/src/optimizer.cpp	96.25%	3 Missing ⚠️
nav2_mppi_controller/src/critics/cost_critic.cpp	80.00%	2 Missing ⚠️
..._mppi_controller/src/critics/goal_angle_critic.cpp	60.00%	2 Missing ⚠️
nav2_mppi_controller/src/critics/goal_critic.cpp	71.42%	2 Missing ⚠️
..._mppi_controller/src/critics/path_align_critic.cpp	81.81%	2 Missing ⚠️
... and 2 more

Files with missing lines	Coverage Δ
...er/include/nav2_mppi_controller/critic_manager.hpp	100.00% <ø> (ø)
...e/nav2_mppi_controller/models/control_sequence.hpp	100.00% <100.00%> (ø)
...oller/include/nav2_mppi_controller/models/path.hpp	100.00% <100.00%> (ø)
...ller/include/nav2_mppi_controller/models/state.hpp	100.00% <100.00%> (ø)
...clude/nav2_mppi_controller/models/trajectories.hpp	100.00% <100.00%> (ø)
...ler/include/nav2_mppi_controller/motion_models.hpp	100.00% <100.00%> (ø)
...troller/include/nav2_mppi_controller/optimizer.hpp	100.00% <ø> (ø)
...ude/nav2_mppi_controller/tools/noise_generator.hpp	100.00% <ø> (ø)
...v2_mppi_controller/tools/trajectory_visualizer.hpp	100.00% <ø> (ø)
...oller/include/nav2_mppi_controller/tools/utils.hpp	97.67% <100.00%> (+0.46%)	⬆️
... and 14 more

... and 3 files with indirect coverage changes

---

🚨 Try these New Features:

• Flaky Tests Detection - Detect and resolve failed and flaky tests

[SteveMacenski]

Back from ROSCon / ROS-I meetings now. Reviewing the software / remaining open items today. I think next steps from there are for me to play with this on my machine as well and:

• do some sanity testing of the various critics to make sure they're still working as I expect
• do some functional sanity checking on the math for the (1) optimizer, (2) motion models, and (3) constraint compliance on the output optimal trajectory. These are software changes I want to scrutinize a little more functionally after a code review for my own piece of mind
• Performance benchmarking :-)
• Ask another non-me party to review the software changes and the performance on their practically applied platform as a second verification

Then good to merge and very publicly thank you for this amazing work 🎆

Is there anything else you think needs to be done here or is planned that we should factor in?

I'm noticing on the code coverage report that there are a couple of lines that are no longer being tested that the UX is saying were covered before in the Obstacle Critic and possibly Optimizer (?). Its just a few stray lines, but makes my eyebrows raise a little bit since I'm not sure how that's possible if the tests are unchanged and they appear to be things that should be triggering 🤷

Please go through all open comments (you may need to expand some the GitHub UX collapses) as some are open questions I want the 👍 on that you checked on those things. There are 5 above this comment :-)

[SteveMacenski]

I got to everything except for the main optimizer file, I ran out of time :(

[SteveMacenski]

Completed the full review

[Ayush1285]

Is there anything else you think needs to be done here or is planned that we should factor in?

@SteveMacenski Verifying the impact of the fast-math compiler flag needs to be done. On my machine, removing fast-math and keeping fma flag resulted in sped up for Eigen.

I'm noticing on the code coverage report that there are a couple of lines that are no longer being tested that the UX is saying were covered before in the Obstacle Critic and possibly Optimizer (?). Its just a few stray lines, but makes my eyebrows raise a little bit since I'm not sure how that's possible if the tests are unchanged and they appear to be things that should be triggering 🤷

I'll take a look at it.

[SteveMacenski]

To prevent us from scrolling up through old comments before the last review: the only one left open is the Clamp question where I responded "yes, just min(max()) to make it more ergonomic and help reduce likely issues with later modifications from other users"

[SteveMacenski]

Otherwise, my only open other things are the clamping util, copy in the util return, stride either mistake or my misunderstanding, and reviewing the test coverage

Everything else is on me to do some functional testing for external validation and this is finally good to be merged! I'm very optimistic we can get this in before the end of the year!