20-25% performance improvement in MPPI controller using Eigen library for computation. #4621

Ayush1285 · 2024-08-14T20:56:39Z

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 20-25% 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

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

… files Signed-off-by: Ayush1285 <[email protected]>

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

mergify · 2024-08-14T20:57:20Z

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

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

mergify · 2024-08-16T20:30:40Z

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

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

mergify · 2024-08-19T18:13:28Z

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

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

mergify · 2024-08-19T18:24:27Z

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

nav2_mppi_controller/CMakeLists.txt

nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

nav2_mppi_controller/src/critics/constraint_critic.cpp

nav2_mppi_controller/src/optimizer.cpp

…ator Signed-off-by: Ayush1285 <[email protected]>

mergify · 2024-08-20T19:55:22Z

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

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

mergify · 2024-08-23T17:46:40Z

@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 · 2024-08-23T18:17:03Z

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 · 2024-08-23T19:07:20Z

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

mergify · 2024-09-02T20:23:20Z

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

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

Ayush1285 · 2024-09-03T17:58:57Z

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 · 2024-09-03T18:22:16Z

@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 · 2024-09-04T16:19:21Z

nav2_mppi_controller/CMakeLists.txt

 find_package(ament_cmake REQUIRED)
-find_package(xsimd REQUIRED)
-find_package(xtensor REQUIRED)
+find_package(Eigen3 REQUIRED)


Are the various check_cxx_compiler_flag useful for Eigen? Any other ones that would be good for Eigen specifically (those are ones pointed out by xtensor)?

-mfma is necessary for fast floating point fused multiply-add operations. Answer by one of the Eigen Core maintainer, He has also mentioned enabling OpenMP for multi-threading. And it seems there is no harm in keeping ISA flags(SSE/AVX).

One interesting find, I tried removing the -fast-math flag and it speeds up the performance by a big margin.
For 2000 * 50 size:
xtensor: 11.6 ms avg.
Eigen with fast-math flag: 8.9 ms
Eigen without fast-math flag: 7.5 ms

And it seems there is no harm in keeping ISA flags(SSE/AVX).

There actually is, ARM based processors have inconsistent support for them or versions, so it makes release difficult. If they don't impact performance positively, we should remove them and removes an entire vector of potential problems (which we're currently running into in #4380 and a few other places in the past). If they're important, lets keep them but I had hoped this would be a good source of removal of problems with Eigen, but alas such is life 😆

I'd be curious about OpenMP's changes in performance! We could make that a build option if it helps and those that want to use it can!

https://stackoverflow.com/questions/56547557/basic-ways-to-speed-up-a-simple-eigen-program https://github.com/owlbarn/eigen/blob/master/README.md there are some remarks here on AVX/fast-math, mfma. Might be worth doing a bit of research on Eigen-specific compiler optimizations, it looks like a rich vein and what was good for xtensor might not be right for Eigen.

Perhaps -O3? We do that with Smac Planner since it helps so much, I hardly want people using it without a high level of optimization

For fast-math, it might be worth testing on a couple of CPUs if you have them to make sure (if not, I can also test on my side, I have a few on my benchtop). What experiment are you running to get that performance change and/or does it include all the critics?

If they're important, lets keep them but I had hoped this would be a good source of removal of problems with Eigen

On my CPU only -mfma and -O3 impacted performance positively(and removal of fast-math also). There was no impact of AVX and SSE flags.

For fast-math, it might be worth testing on a couple of CPUs if you have them to make sure (if not, I can also test on my side, I have a few on my benchtop). What experiment are you running to get that performance change and/or does it include all the critics?

Currently, I have only one CPU :(. You can try it on your CPUs if possible. I'm running optimizer_benchmark/BM_diffDrive with all these critics loaded: {{"ConstraintCritic"}, {"CostCritic"}, {"PathAlignCritic"}, {"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};

There actually is, ARM based processors have inconsistent support for them or versions, so it makes release difficult.

Oh, I wasn't aware of this. Once merge conflicts and build errors are resolved, then we can test on multiple CPUs with different flags.

nav2_mppi_controller/benchmark/optimizer_benchmark.cpp

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

SteveMacenski · 2024-09-04T16:26:11Z

nav2_mppi_controller/src/optimizer.cpp

-  xt::view(control_sequence_.wz, -1) =
-    xt::view(control_sequence_.wz, -2);
-
+  control_sequence_.vx = utils::rollColumns(control_sequence_.vx, 1);


Does the roll also handle setting the last element to the second to last element?

No, I'll add that part. I'll have to re-implement the rollColumns method because it is not shifting the values in place. Compiler is moving the values of the temporary matrix created inside this function, and this is causing bad memory handling, I think.

nav2_mppi_controller/src/optimizer.cpp

SteveMacenski · 2024-09-04T16:28:47Z

nav2_mppi_controller/src/optimizer.cpp

    vx_last = vx_curr;

    float & wz_curr = control_sequence_.wz(i);
-    wz_curr = std::clamp(wz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz);


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

nav2_mppi_controller/src/optimizer.cpp

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

mergify · 2024-09-09T17:59:58Z

@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 · 2024-09-10T06:28:40Z

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

SteveMacenski · 2024-09-10T18:29:47Z

I think you may not have done the last rebase / pull correctly. I'm seeing changes in files that don't relate to this PR. It could be due to the conflict and Git's attempt to show me the diffs it can with that in place, but that would need to be resolved before I can review again unfortunately :(

Its a 3k line PR with 169 files, vast majority are unrelated

mergify · 2024-09-10T19:34:04Z

@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 · 2024-09-11T17:00:04Z

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

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

Ayush1285 · 2024-09-11T17:31:16Z

I think you may not have done the last rebase / pull correctly. I'm seeing changes in files that don't relate to this PR. It could be due to the conflict and Git's attempt to show me the diffs it can with that in place, but that would need to be resolved before I can review again unfortunately :(

Its a 3k line PR with 169 files, vast majority are unrelated

I've corrected the bad rebase/merge. Lint errors are also fixed.

mergify · 2024-09-11T17:35:40Z

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

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

SteveMacenski · 2024-09-12T15:38:45Z

This still appears to have a few test failures - though you're right that linting looks good now 👍

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

SteveMacenski · 2024-09-12T15:45:35Z

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -202,8 +196,8 @@ class Optimizer
   * @param state fill state
   */
  void integrateStateVelocities(
-    xt::xtensor<float, 2> & trajectories,
-    const xt::xtensor<float, 2> & state) const;
+    Eigen::Array<float, -1, 3> & trajectories,


Is there a typedef for -1 and dynamic so that an onlooker knows this means dynamic size? I.e. Eigen::Dynamic

We can use Eigen::Dynamic instead of -1, that would be more self-explanatory.

nav2_mppi_controller/test/CMakeLists.txt

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

SteveMacenski · 2024-09-16T19:15:12Z

Should I have this another review? I know you're actively working and don't want to pester you mid-dev cycle :-)

nav2_mppi_controller/src/noise_generator.cpp

Ayush1285 · 2024-09-16T21:06:26Z

Should I have this another review? I know you're actively working and don't want to pester you mid-dev cycle :-)

Actually, I am also busy in other deliverables. So, I'm not able to allocate much time. But I'll let you know in 3-4 days maybe, once I've finished fixing the test failures and other review comments.

…ally Signed-off-by: Ayush1285 <[email protected]>

SteveMacenski · 2024-10-02T16:20:47Z

Please request a review or otherwise comment when you want me to take a look again :-)

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

Ayush1285 · 2024-10-02T16:28:59Z

Please request a review or otherwise comment when you want me to take a look again :-)

Yes, I've requested for the review. All unit tests are passing now, but system tests are still failing.

SteveMacenski · 2024-10-02T19:02:49Z

Retriggering the job - that could just be flaky. I do want to see coverage results before I review so I can use that in my review process. So I'll review once that is uploaded on the job success. I don't think there's an action item for you (just waiting on the build).

mergify · 2024-10-03T18:40:10Z

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

Ayush1285 added 7 commits June 6, 2024 23:47

Initial commit

137f75d

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

Corrected to Eigen Array

a2c11e6

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

updated motion model with eigen

8df6757

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

Replaced xtensor with eigen in Optimizer, NoiseGenerator and all Util…

0f3a0f8

… files Signed-off-by: Ayush1285 <[email protected]>

updated critics with Eigen

3e0bcd4

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

optimized Eigen::Array implementation

f244e0a

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

added comment

96b14d1

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

Updated path align critic and velocity deadband critic with Eigen

62eb959

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

Updated cost critic and constraint critic with eigen

3d8f987

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

Updated utils test with Eigen

bc5b377

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

SteveMacenski requested changes Aug 19, 2024

View reviewed changes

Reverted unnecessary changes and fixed static instance in Noise gener…

5fb8006

…ator Signed-off-by: Ayush1285 <[email protected]>

changes std::abs to fabs, clamp to min-max

46baa06

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

Converted tests to Eigen

a336814

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

Complete conversion from xtensor to Eigen

93d017b

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

SteveMacenski requested changes Sep 4, 2024

View reviewed changes

fixed few review comments

0f82afe

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

Ayush1285 force-pushed the eigen_mppi branch from d1f9b87 to 0f82afe Compare September 10, 2024 19:28

Merge branch 'main' into eigen_mppi

b41d519

Ayush1285 added 2 commits September 11, 2024 22:49

Fixed linters and few review comments

ca61afc

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

Fixed mis-merge of AckermannReversingTest

61a1d51

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

fixed gtest assertion

63b1e61

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

SteveMacenski requested changes Sep 12, 2024

View reviewed changes

Ayush1285 and others added 3 commits September 14, 2024 19:34

Merge branch 'ros-navigation:main' into eigen_mppi

dfae9d5

Merge branch 'ros-navigation:main' into eigen_mppi

ea052a8

Fixed optimizer_unit_tests and related issues

286b3c0

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

SteveMacenski reviewed Sep 16, 2024

View reviewed changes

nav2_mppi_controller/src/noise_generator.cpp Outdated Show resolved Hide resolved

Ayush1285 and others added 3 commits October 2, 2024 00:11

Merge branch 'ros-navigation:main' into eigen_mppi

d6418bd

Merge branch 'ros-navigation:main' into eigen_mppi

2c53db4

Fixed all the unit tests and critic tests, all unit tests passing loc…

847f837

…ally Signed-off-by: Ayush1285 <[email protected]>

fixed few review comments

f9ed129

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

Ayush1285 marked this pull request as ready for review October 2, 2024 16:26

Ayush1285 requested a review from SteveMacenski October 2, 2024 16:27

20-25% performance improvement in MPPI controller using Eigen library for computation. #4621

Are you sure you want to change the base?

20-25% performance improvement in MPPI controller using Eigen library for computation. #4621

Conversation

Ayush1285 commented Aug 14, 2024 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

mergify bot commented Aug 14, 2024

mergify bot commented Aug 16, 2024

mergify bot commented Aug 19, 2024

mergify bot commented Aug 19, 2024

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

mergify bot commented Aug 20, 2024

mergify bot commented Aug 23, 2024

SteveMacenski commented Aug 23, 2024

Ayush1285 commented Aug 23, 2024

mergify bot commented Sep 2, 2024

Ayush1285 commented Sep 3, 2024 • edited Loading

mergify bot commented Sep 3, 2024

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Sep 4, 2024 • edited Loading

Choose a reason for hiding this comment

Ayush1285 Sep 5, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Sep 5, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented Sep 9, 2024

mergify bot commented Sep 10, 2024

SteveMacenski commented Sep 10, 2024 • edited Loading

mergify bot commented Sep 10, 2024

mergify bot commented Sep 11, 2024

Ayush1285 commented Sep 11, 2024

mergify bot commented Sep 11, 2024

SteveMacenski commented Sep 12, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski commented Sep 16, 2024

Ayush1285 commented Sep 16, 2024

SteveMacenski commented Oct 2, 2024

Ayush1285 commented Oct 2, 2024

SteveMacenski commented Oct 2, 2024

mergify bot commented Oct 3, 2024

Ayush1285 commented Aug 14, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

Ayush1285 commented Sep 3, 2024 •

edited

Loading

SteveMacenski Sep 4, 2024 •

edited

Loading

Ayush1285 Sep 5, 2024 •

edited

Loading

SteveMacenski Sep 5, 2024 •

edited

Loading

SteveMacenski commented Sep 10, 2024 •

edited

Loading