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.

Sign up for GitHub

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

Jump to bottom

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

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

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
137f75d
Initial commit
Ayush1285 Jun 6, 2024
a2c11e6
Corrected to Eigen Array
Ayush1285 Jun 6, 2024
8df6757
updated motion model with eigen
Ayush1285 Jun 8, 2024
0f3a0f8
Replaced xtensor with eigen in Optimizer, NoiseGenerator and all Util…
Ayush1285 Jun 10, 2024
3e0bcd4
updated critics with Eigen
Ayush1285 Jun 18, 2024
f244e0a
optimized Eigen::Array implementation
Ayush1285 Aug 12, 2024
96b14d1
added comment
Ayush1285 Aug 14, 2024
62eb959
Updated path align critic and velocity deadband critic with Eigen
Ayush1285 Aug 16, 2024
3d8f987
Updated cost critic and constraint critic with eigen
Ayush1285 Aug 19, 2024
bc5b377
Updated utils test with Eigen
Ayush1285 Aug 19, 2024
5fb8006
Reverted unnecessary changes and fixed static instance in Noise gener…
Ayush1285 Aug 20, 2024
46baa06
changes std::abs to fabs, clamp to min-max
Ayush1285 Aug 23, 2024
a336814
Converted tests to Eigen
Ayush1285 Sep 2, 2024
93d017b
Complete conversion from xtensor to Eigen
Ayush1285 Sep 3, 2024
0f82afe
fixed few review comments
Ayush1285 Sep 5, 2024
b41d519
Merge branch 'main' into eigen_mppi
Ayush1285 Sep 10, 2024
ca61afc
Fixed linters and few review comments
Ayush1285 Sep 11, 2024
61a1d51
Fixed mis-merge of AckermannReversingTest
Ayush1285 Sep 11, 2024
63b1e61
fixed gtest assertion
Ayush1285 Sep 11, 2024
dfae9d5
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Sep 14, 2024
ea052a8
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Sep 16, 2024
286b3c0
Fixed optimizer_unit_tests and related issues
Ayush1285 Sep 16, 2024
d6418bd
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Oct 1, 2024
2c53db4
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Oct 2, 2024
847f837
Fixed all the unit tests and critic tests, all unit tests passing loc…
Ayush1285 Oct 2, 2024
f9ed129
fixed few review comments
Ayush1285 Oct 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 10 additions & 21 deletions nav2_mppi_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,6 @@
cmake_minimum_required(VERSION 3.5)
project(nav2_mppi_controller)

add_definitions(-DXTENSOR_ENABLE_XSIMD)
add_definitions(-DXTENSOR_USE_XSIMD)

set(XTENSOR_USE_TBB 0)
set(XTENSOR_USE_OPENMP 0)
set(XTENSOR_USE_XSIMD 1)

find_package(ament_cmake REQUIRED)
find_package(angles REQUIRED)
find_package(geometry_msgs REQUIRED)
Expand All @@ -24,8 +17,12 @@ find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(xsimd REQUIRED)
find_package(xtensor REQUIRED)
find_package(Eigen3 REQUIRED)
Copy link
Member

@SteveMacenski SteveMacenski Sep 4, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

@Ayush1285 Ayush1285 Sep 5, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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

Copy link
Member

@SteveMacenski SteveMacenski Sep 5, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

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?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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"}};

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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.


include_directories(
include
${EIGEN3_INCLUDE_DIR}
)

nav2_package()

Expand Down Expand Up @@ -63,10 +60,9 @@ add_library(mppi_controller SHARED
src/path_handler.cpp
src/trajectory_visualizer.cpp
)
target_compile_options(mppi_controller PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
target_compile_options(mppi_controller PUBLIC -fconcepts -O3)
target_include_directories(mppi_controller
PUBLIC
${xsimd_INCLUDE_DIRS}
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(mppi_controller PUBLIC
Expand All @@ -84,9 +80,6 @@ target_link_libraries(mppi_controller PUBLIC
tf2_geometry_msgs::tf2_geometry_msgs
tf2_ros::tf2_ros
${visualization_msgs_TARGETS}
xtensor
xtensor::optimize
xtensor::use_xsimd
)

add_library(mppi_critics SHARED
Expand All @@ -102,10 +95,9 @@ add_library(mppi_critics SHARED
src/critics/twirling_critic.cpp
src/critics/velocity_deadband_critic.cpp
)
target_compile_options(mppi_critics PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
target_include_directories(mppi_critics
PUBLIC
${xsimd_INCLUDE_DIRS}
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
target_link_libraries(mppi_critics PUBLIC
Expand All @@ -122,9 +114,6 @@ target_link_libraries(mppi_critics PUBLIC
tf2_geometry_msgs::tf2_geometry_msgs
tf2_ros::tf2_ros
${visualization_msgs_TARGETS}
xtensor
xtensor::optimize
xtensor::use_xsimd
)
target_link_libraries(mppi_critics PRIVATE
pluginlib::pluginlib
Expand All @@ -150,7 +139,7 @@ if(BUILD_TESTING)
ament_find_gtest()

add_subdirectory(test)
# add_subdirectory(benchmark)
Ayush1285 marked this conversation as resolved.
Show resolved Hide resolved
add_subdirectory(benchmark)
endif()

ament_export_libraries(${libraries})
Expand All @@ -168,7 +157,7 @@ ament_export_dependencies(
tf2_geometry_msgs
tf2_ros
visualization_msgs
xtensor
Eigen3
)
ament_export_include_directories(include/${PROJECT_NAME})
ament_export_targets(nav2_mppi_controller)
Expand Down
7 changes: 3 additions & 4 deletions nav2_mppi_controller/benchmark/controller_benchmark.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@
// limitations under the License.

#include <benchmark/benchmark.h>

#include <Eigen/Dense>

#include <string>

#include <geometry_msgs/msg/pose_stamped.hpp>
Expand All @@ -24,10 +27,6 @@
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
#include <nav2_core/goal_checker.hpp>

#include <xtensor/xarray.hpp>
#include <xtensor/xio.hpp>
#include <xtensor/xview.hpp>

#include "nav2_mppi_controller/motion_models.hpp"
#include "nav2_mppi_controller/controller.hpp"

Expand Down
8 changes: 3 additions & 5 deletions nav2_mppi_controller/benchmark/optimizer_benchmark.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

#include <benchmark/benchmark.h>

#include <Eigen/Dense>

#include <string>
#include <vector>

Expand All @@ -27,10 +29,6 @@
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
#include <nav2_core/goal_checker.hpp>

#include <xtensor/xarray.hpp>
#include <xtensor/xio.hpp>
#include <xtensor/xview.hpp>

#include "nav2_mppi_controller/optimizer.hpp"
#include "nav2_mppi_controller/motion_models.hpp"

Expand Down Expand Up @@ -121,7 +119,7 @@ static void BM_Omni(benchmark::State & state)
bool consider_footprint = true;
std::string motion_model = "Omni";
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};

prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
}
Expand Down
11 changes: 3 additions & 8 deletions nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,16 +15,11 @@
#ifndef NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
#define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_

#include <Eigen/Dense>

#include <memory>
#include <vector>

// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xtensor.hpp>
#pragma GCC diagnostic pop

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/goal_checker.hpp"
#include "nav2_mppi_controller/models/state.hpp"
Expand All @@ -47,7 +42,7 @@ struct CriticData
const models::Trajectories & trajectories;
const models::Path & path;

xt::xtensor<float, 1> & costs;
Eigen::ArrayXf & costs;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
float & model_dt;

bool fail_flag;
Expand Down
7 changes: 0 additions & 7 deletions nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,13 +20,6 @@
#include <vector>
#include <pluginlib/class_loader.hpp>

// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xtensor.hpp>
#pragma GCC diagnostic pop

#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"

Expand Down
19 changes: 7 additions & 12 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,7 @@
#ifndef NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
#define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_

// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xtensor.hpp>
#pragma GCC diagnostic pop
#include <Eigen/Dense>

namespace mppi::models
{
Expand All @@ -40,15 +35,15 @@ struct Control
*/
struct ControlSequence
{
xt::xtensor<float, 1> vx;
xt::xtensor<float, 1> vy;
xt::xtensor<float, 1> wz;
Eigen::ArrayXf vx;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
Eigen::ArrayXf vy;
Eigen::ArrayXf wz;

void reset(unsigned int time_steps)
{
vx = xt::zeros<float>({time_steps});
vy = xt::zeros<float>({time_steps});
wz = xt::zeros<float>({time_steps});
vx.setZero(time_steps);
vy.setZero(time_steps);
wz.setZero(time_steps);
}
};

Expand Down
19 changes: 7 additions & 12 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,7 @@
#ifndef NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
#define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_

// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xtensor.hpp>
#pragma GCC diagnostic pop
#include <Eigen/Dense>

namespace mppi::models
{
Expand All @@ -31,18 +26,18 @@ namespace mppi::models
*/
struct Path
{
xt::xtensor<float, 1> x;
xt::xtensor<float, 1> y;
xt::xtensor<float, 1> yaws;
Eigen::ArrayXf x;
Eigen::ArrayXf y;
Eigen::ArrayXf yaws;

/**
* @brief Reset path data
*/
void reset(unsigned int size)
{
x = xt::zeros<float>({size});
y = xt::zeros<float>({size});
yaws = xt::zeros<float>({size});
x.setZero(size);
y.setZero(size);
yaws.setZero(size);
}
};

Expand Down
32 changes: 14 additions & 18 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,16 +15,12 @@
#ifndef NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
#define NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_

// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xtensor.hpp>
#pragma GCC diagnostic pop
#include <Eigen/Dense>

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>


namespace mppi::models
{

Expand All @@ -34,13 +30,13 @@ namespace mppi::models
*/
struct State
{
xt::xtensor<float, 2> vx;
xt::xtensor<float, 2> vy;
xt::xtensor<float, 2> wz;
Eigen::ArrayXXf vx;
Eigen::ArrayXXf vy;
Eigen::ArrayXXf wz;

xt::xtensor<float, 2> cvx;
xt::xtensor<float, 2> cvy;
xt::xtensor<float, 2> cwz;
Eigen::ArrayXXf cvx;
Eigen::ArrayXXf cvy;
Eigen::ArrayXXf cwz;

geometry_msgs::msg::PoseStamped pose;
geometry_msgs::msg::Twist speed;
Expand All @@ -50,13 +46,13 @@ struct State
*/
void reset(unsigned int batch_size, unsigned int time_steps)
{
vx = xt::zeros<float>({batch_size, time_steps});
vy = xt::zeros<float>({batch_size, time_steps});
wz = xt::zeros<float>({batch_size, time_steps});
vx.setZero(batch_size, time_steps);
vy.setZero(batch_size, time_steps);
wz.setZero(batch_size, time_steps);

cvx = xt::zeros<float>({batch_size, time_steps});
cvy = xt::zeros<float>({batch_size, time_steps});
cwz = xt::zeros<float>({batch_size, time_steps});
cvx.setZero(batch_size, time_steps);
cvy.setZero(batch_size, time_steps);
cwz.setZero(batch_size, time_steps);
}
};
} // namespace mppi::models
Expand Down
20 changes: 7 additions & 13 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,7 @@
#ifndef NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_
#define NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_

// xtensor creates warnings that needs to be ignored as we are building with -Werror
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#include <xtensor/xtensor.hpp>
#include <xtensor/xview.hpp>
#pragma GCC diagnostic pop
#include <Eigen/Dense>

namespace mppi::models
{
Expand All @@ -32,18 +26,18 @@ namespace mppi::models
*/
struct Trajectories
{
xt::xtensor<float, 2> x;
xt::xtensor<float, 2> y;
xt::xtensor<float, 2> yaws;
Eigen::ArrayXXf x;
Eigen::ArrayXXf y;
Eigen::ArrayXXf yaws;

/**
* @brief Reset state data
*/
void reset(unsigned int batch_size, unsigned int time_steps)
{
x = xt::zeros<float>({batch_size, time_steps});
y = xt::zeros<float>({batch_size, time_steps});
yaws = xt::zeros<float>({batch_size, time_steps});
x.setZero(batch_size, time_steps);
y.setZero(batch_size, time_steps);
yaws.setZero(batch_size, time_steps);
}
};

Expand Down
Loading
Loading