Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 46 commits into
base: main
Choose a base branch
from

Conversation

Ayush1285
Copy link
Contributor

@Ayush1285 Ayush1285 commented Aug 14, 2024

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

Copy link
Contributor

mergify bot commented Aug 14, 2024

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

Copy link
Contributor

mergify bot commented Aug 16, 2024

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

Copy link
Contributor

mergify bot commented Aug 19, 2024

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

Copy link
Contributor

mergify bot commented Aug 19, 2024

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

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

nav2_mppi_controller/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_mppi_controller/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_mppi_controller/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_mppi_controller/CMakeLists.txt Show resolved Hide resolved
nav2_mppi_controller/CMakeLists.txt Outdated Show resolved Hide resolved
nav2_mppi_controller/src/critics/constraint_critic.cpp Outdated Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Outdated Show resolved Hide resolved
Copy link
Contributor

mergify bot commented Aug 20, 2024

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

Copy link
Contributor

mergify bot commented Aug 23, 2024

@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
Copy link
Member

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
Copy link
Contributor Author

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.

Signed-off-by: Ayush1285 <[email protected]>
Copy link
Contributor

mergify bot commented Sep 2, 2024

@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
Copy link
Contributor Author

Ayush1285 commented Sep 3, 2024

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.

Copy link
Contributor

mergify bot commented Sep 3, 2024

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

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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!

nav2_mppi_controller/CMakeLists.txt Show resolved Hide resolved
nav2_mppi_controller/benchmark/optimizer_benchmark.cpp Outdated Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Outdated Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Outdated Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Outdated Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Show resolved Hide resolved
Signed-off-by: Ayush1285 <[email protected]>
Copy link
Contributor

mergify bot commented Sep 9, 2024

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

Copy link
Contributor

mergify bot commented Sep 10, 2024

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

@Ayush1285 Ayush1285 changed the title 20-25% performance improvement in MPPI controller using Eigen library for computation. 45-50% performance improvement in MPPI controller using Eigen library for computation. Nov 20, 2024
@Ayush1285
Copy link
Contributor Author

Ayush1285 commented Nov 20, 2024

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
Copy link
Member

SteveMacenski commented Nov 21, 2024

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"

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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!

Signed-off-by: Ayush1285 <[email protected]>
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It continues to be odd to me that we have 3 lines in the main optimizer that don't have codecov showing even though they're sequential operations with lines that are said to be covered. I assume its a compiler optimization artifact that these lines are optimized out, but it is curious to me. Nothing for you to worry about though unless this sparks a thought.

+   auto yaw_sin = trajectories.yaws.sin().eval();
+   auto dy = (state.vx * yaw_sin).eval();
+   auto dy = (vx * yaw_sin).eval();

I retriggered CI, it should be green now.


OK, next steps:

  • Steve: do some sanity testing of the various things to make sure they're still working as I expect: (1) each critic, (2) optimizer, (3) motion models, and (4) constraint compliance on the output optimal trajectory. Run for a couple of hours.
  • Steve: Performance benchmarking :-)
  • Steve: Ask another non-me party & an ARM user to review the software changes and the performance on their practically applied platform as a second verification

Anything else you feel should be done here - or are you happy with this as is? 😄 🥳

nav2_mppi_controller/src/optimizer.cpp Outdated Show resolved Hide resolved
nav2_mppi_controller/src/optimizer.cpp Outdated Show resolved Hide resolved
@Ayush1285
Copy link
Contributor Author

Ayush1285 commented Nov 27, 2024

Anything else you feel should be done here

  1. System tests are again failing consistently. In the GPS Waypoint Follower test, it passes the first waypoint, but after that action, the server refuses to accept the goal. Is it a generic failure? I've tested manually locally with multiple waypoints; it was okay there.
    Edit: System tests are passing now.
  2. If we can test this on ARM platforms facing issues for xtensor, then it would be great to know if Eigen works there. Although we have removed all the SIMD extension flags, but as I know Eigen has explicit extrinsic written for SIMD extensions and then it is conditionally compiled based on whatever is supported by the processor. So, I think we might face problems here. Binaries generated by x86 might not work as desired on those ARM platforms.

@SteveMacenski
Copy link
Member

On ARM: I added that to my TODO list to find a deployed user, I have a couple in mind. I'll plan on doing benchmarking and the analysis steps in my first bullet this week

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A migration guide entry on docs.nav2.org is necessary since this is such a big change. Add in the improvement metrics too!

@Ayush1285
Copy link
Contributor Author

A migration guide entry on docs.nav2.org is necessary since this is such a big change. Add in the improvement metrics too!

Yes, I was thinking that once you've done your testing and performance benchmarking, I'll add an entry on docs.nav2.org.

@SteveMacenski
Copy link
Member

SteveMacenski commented Dec 4, 2024

Performance testing based on the Nav2 bringup default settings and differential drive model:

  • Average for xtensor: 10.58ms
  • Average for Eigen: 7.31ms
  • Improvement: 30.9%

Moreover from that, a major benefit of just being on Eigen as an extremely well supported math library which I think will be a better solution long term with possible GPU bindings / general releases on Ubuntu distros

High-level initial smoke testing looks good - no issues to report. I'm going to continue to dig into each of the critics just to prove to myself that they are working properly - but what I'm seeing from my testing, I'm not expecting to find anything.

Edit: completed the critic & constraints testing, all looks good.

While I'm doing that, I'm inviting a few beta testers to the party to have them test it out as well from a deployed end-user's perspective. I think you can start on the docs update :-)


BTW, I made a pretty big breaking change in #4779, you need to pull in Main to make CI work or with the latest in related packages :-) Its conflict free though, so easy enough

size_t n_size = traj_yaws.size();

traj_yaws(0) = wz(0) * settings_.model_dt + initial_yaw;
float last_yaw = traj_yaws(0);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would you agree this is the same and more concise?

  float last_yaw = initial_yaw;
  for(size_t i = 0; i != n_size; i++) {
    float & curr_yaw = traj_yaws(i);
    curr_yaw = last_yaw + wz(i) * settings_.model_dt;
    last_yaw = curr_yaw;
  }

If so, this and the X/Y entries below it could be updated as such :-)

The other implementation this function could also be made similarly if the last_val variable is introduced

Copy link
Contributor Author

@Ayush1285 Ayush1285 Dec 5, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, this is the same and more concise. But since everything is working fine after testing, I'd suggest to keep it same. I'll open another PR after this for handling all the nit-pick changes and possible micro optimizations. What do you think about this? @SteveMacenski

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure thing :-)

@Ayush1285
Copy link
Contributor Author

@SteveMacenski Opened the PR for docs changes : Docs changes PR

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants