Skip to content

Commit a33e8d2

Browse files
Ayush1285Ayush1285
authored
45-50% performance improvement in MPPI controller using Eigen library for computation. (#4621)
* Initial commit Signed-off-by: Ayush1285 <[email protected]> * Corrected to Eigen Array Signed-off-by: Ayush1285 <[email protected]> * updated motion model with eigen Signed-off-by: Ayush1285 <[email protected]> * Replaced xtensor with eigen in Optimizer, NoiseGenerator and all Util files Signed-off-by: Ayush1285 <[email protected]> * updated critics with Eigen Signed-off-by: Ayush1285 <[email protected]> * optimized Eigen::Array implementation Signed-off-by: Ayush1285 <[email protected]> * added comment Signed-off-by: Ayush1285 <[email protected]> * Updated path align critic and velocity deadband critic with Eigen Signed-off-by: Ayush1285 <[email protected]> * Updated cost critic and constraint critic with eigen Signed-off-by: Ayush1285 <[email protected]> * Updated utils test with Eigen Signed-off-by: Ayush1285 <[email protected]> * Reverted unnecessary changes and fixed static instance in Noise generator Signed-off-by: Ayush1285 <[email protected]> * changes std::abs to fabs, clamp to min-max Signed-off-by: Ayush1285 <[email protected]> * Converted tests to Eigen Signed-off-by: Ayush1285 <[email protected]> * Complete conversion from xtensor to Eigen Signed-off-by: Ayush1285 <[email protected]> * fixed few review comments Signed-off-by: Ayush1285 <[email protected]> * Fixed linters and few review comments Signed-off-by: Ayush1285 <[email protected]> * Fixed mis-merge of AckermannReversingTest Signed-off-by: Ayush1285 <[email protected]> * fixed gtest assertion Signed-off-by: Ayush1285 <[email protected]> * Fixed optimizer_unit_tests and related issues Signed-off-by: Ayush1285 <[email protected]> * Fixed all the unit tests and critic tests, all unit tests passing locally Signed-off-by: Ayush1285 <[email protected]> * fixed few review comments Signed-off-by: Ayush1285 <[email protected]> * Fixed CostCritic issue and added test for shiftColumn method Signed-off-by: Ayush1285 <[email protected]> * Added test for new functions Signed-off-by: Ayush1285 <[email protected]> * Removed compiler flags Signed-off-by: Ayush1285 <[email protected]> * updated test to check first and last columns Signed-off-by: Ayush1285 <[email protected]> * Addressed few review comments Signed-off-by: Ayush1285 <[email protected]> * Changed the obstacle critic implementation to the original way. Updated optimizer_benchmark test with critics and params Signed-off-by: Ayush1285 <[email protected]> * Fixed bugs Signed-off-by: Ayush1285 <[email protected]> * Fixed linter Signed-off-by: Ayush1285 <[email protected]> * Added clamp util function Signed-off-by: Ayush1285 <[email protected]> * Fixed bug Signed-off-by: Ayush1285 <[email protected]> * Fixed review comments: Added utils::clamp method Signed-off-by: Ayush1285 <[email protected]> * Fixing strided trajectory columns Signed-off-by: Ayush1285 <[email protected]> * fixed lint error Signed-off-by: Ayush1285 <[email protected]> * Fixed merge Signed-off-by: Ayush1285 <[email protected]> * Fixed optimizer benchmark with latest api changes Signed-off-by: Ayush1285 <[email protected]> * fixed build error Signed-off-by: Ayush1285 <[email protected]> * Fixed new util_test Signed-off-by: Ayush1285 <[email protected]> --------- Signed-off-by: Ayush1285 <[email protected]>
1 parent 5ff8cc7 commit a33e8d2

40 files changed

+952
-872
lines changed

‎nav2_mppi_controller/CMakeLists.txt‎

Lines changed: 10 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,6 @@
11
cmake_minimum_required(VERSION 3.5)
22
project(nav2_mppi_controller)
33

4-
add_definitions(-DXTENSOR_ENABLE_XSIMD)
5-
add_definitions(-DXTENSOR_USE_XSIMD)
6-
7-
set(XTENSOR_USE_TBB 0)
8-
set(XTENSOR_USE_OPENMP 0)
9-
set(XTENSOR_USE_XSIMD 1)
10-
114
find_package(ament_cmake REQUIRED)
125
find_package(angles REQUIRED)
136
find_package(geometry_msgs REQUIRED)
@@ -24,30 +17,19 @@ find_package(tf2 REQUIRED)
2417
find_package(tf2_geometry_msgs REQUIRED)
2518
find_package(tf2_ros REQUIRED)
2619
find_package(visualization_msgs REQUIRED)
27-
find_package(xsimd REQUIRED)
28-
find_package(xtensor REQUIRED)
20+
find_package(Eigen3 REQUIRED)
21+
22+
include_directories(
23+
include
24+
${EIGEN3_INCLUDE_DIR}
25+
)
2926

3027
nav2_package()
3128

3229
include(CheckCXXCompilerFlag)
3330

34-
check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512)
35-
check_cxx_compiler_flag("-msse4.2" COMPILER_SUPPORTS_SSE4)
36-
check_cxx_compiler_flag("-mavx2" COMPILER_SUPPORTS_AVX2)
3731
check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA)
3832

39-
if(COMPILER_SUPPORTS_AVX512)
40-
add_compile_options(-mno-avx512f)
41-
endif()
42-
43-
if(COMPILER_SUPPORTS_SSE4)
44-
add_compile_options(-msse4.2)
45-
endif()
46-
47-
if(COMPILER_SUPPORTS_AVX2)
48-
add_compile_options(-mavx2)
49-
endif()
50-
5133
if(COMPILER_SUPPORTS_FMA)
5234
add_compile_options(-mfma)
5335
endif()
@@ -63,10 +45,9 @@ add_library(mppi_controller SHARED
6345
src/path_handler.cpp
6446
src/trajectory_visualizer.cpp
6547
)
66-
target_compile_options(mppi_controller PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
48+
target_compile_options(mppi_controller PUBLIC -O3)
6749
target_include_directories(mppi_controller
6850
PUBLIC
69-
${xsimd_INCLUDE_DIRS}
7051
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
7152
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
7253
target_link_libraries(mppi_controller PUBLIC
@@ -84,9 +65,6 @@ target_link_libraries(mppi_controller PUBLIC
8465
tf2_geometry_msgs::tf2_geometry_msgs
8566
tf2_ros::tf2_ros
8667
${visualization_msgs_TARGETS}
87-
xtensor
88-
xtensor::optimize
89-
xtensor::use_xsimd
9068
)
9169

9270
add_library(mppi_critics SHARED
@@ -102,10 +80,9 @@ add_library(mppi_critics SHARED
10280
src/critics/twirling_critic.cpp
10381
src/critics/velocity_deadband_critic.cpp
10482
)
105-
target_compile_options(mppi_critics PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
83+
target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
10684
target_include_directories(mppi_critics
10785
PUBLIC
108-
${xsimd_INCLUDE_DIRS}
10986
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
11087
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
11188
target_link_libraries(mppi_critics PUBLIC
@@ -122,9 +99,6 @@ target_link_libraries(mppi_critics PUBLIC
12299
tf2_geometry_msgs::tf2_geometry_msgs
123100
tf2_ros::tf2_ros
124101
${visualization_msgs_TARGETS}
125-
xtensor
126-
xtensor::optimize
127-
xtensor::use_xsimd
128102
)
129103
target_link_libraries(mppi_critics PRIVATE
130104
pluginlib::pluginlib
@@ -150,7 +124,7 @@ if(BUILD_TESTING)
150124
ament_find_gtest()
151125

152126
add_subdirectory(test)
153-
# add_subdirectory(benchmark)
127+
add_subdirectory(benchmark)
154128
endif()
155129

156130
ament_export_libraries(${libraries})
@@ -168,7 +142,7 @@ ament_export_dependencies(
168142
tf2_geometry_msgs
169143
tf2_ros
170144
visualization_msgs
171-
xtensor
145+
Eigen3
172146
)
173147
ament_export_include_directories(include/${PROJECT_NAME})
174148
ament_export_targets(nav2_mppi_controller)

‎nav2_mppi_controller/benchmark/controller_benchmark.cpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,9 @@
1313
// limitations under the License.
1414

1515
#include <benchmark/benchmark.h>
16+
17+
#include <Eigen/Dense>
18+
1619
#include <string>
1720

1821
#include <geometry_msgs/msg/pose_stamped.hpp>
@@ -24,10 +27,6 @@
2427
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
2528
#include <nav2_core/goal_checker.hpp>
2629

27-
#include <xtensor/xarray.hpp>
28-
#include <xtensor/xio.hpp>
29-
#include <xtensor/xview.hpp>
30-
3130
#include "nav2_mppi_controller/motion_models.hpp"
3231
#include "nav2_mppi_controller/controller.hpp"
3332

‎nav2_mppi_controller/benchmark/optimizer_benchmark.cpp‎

Lines changed: 17 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,8 @@
1414

1515
#include <benchmark/benchmark.h>
1616

17+
#include <Eigen/Dense>
18+
1719
#include <string>
1820
#include <vector>
1921

@@ -27,10 +29,6 @@
2729
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
2830
#include <nav2_core/goal_checker.hpp>
2931

30-
#include <xtensor/xarray.hpp>
31-
#include <xtensor/xio.hpp>
32-
#include <xtensor/xview.hpp>
33-
3432
#include "nav2_mppi_controller/optimizer.hpp"
3533
#include "nav2_mppi_controller/motion_models.hpp"
3634

@@ -51,8 +49,8 @@ void prepareAndRunBenchmark(
5149
bool consider_footprint, std::string motion_model,
5250
std::vector<std::string> critics, benchmark::State & state)
5351
{
54-
int batch_size = 300;
55-
int time_steps = 12;
52+
int batch_size = 2000;
53+
int time_steps = 56;
5654
unsigned int path_points = 50u;
5755
int iteration_count = 2;
5856
double lookahead_distance = 10.0;
@@ -91,16 +89,17 @@ void prepareAndRunBenchmark(
9189
nav2_core::GoalChecker * dummy_goal_checker{nullptr};
9290

9391
for (auto _ : state) {
94-
optimizer->evalControl(pose, velocity, path, dummy_goal_checker);
92+
optimizer->evalControl(pose, velocity, path, path.poses.back().pose, dummy_goal_checker);
9593
}
9694
}
9795

9896
static void BM_DiffDrivePointFootprint(benchmark::State & state)
9997
{
10098
bool consider_footprint = true;
10199
std::string motion_model = "DiffDrive";
102-
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
103-
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
100+
std::vector<std::string> critics = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
101+
{"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
102+
{"PreferForwardCritic"}};
104103

105104
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
106105
}
@@ -109,19 +108,20 @@ static void BM_DiffDrive(benchmark::State & state)
109108
{
110109
bool consider_footprint = true;
111110
std::string motion_model = "DiffDrive";
112-
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
113-
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
111+
std::vector<std::string> critics = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
112+
{"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
113+
{"PreferForwardCritic"}};
114114

115115
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
116116
}
117117

118-
119118
static void BM_Omni(benchmark::State & state)
120119
{
121120
bool consider_footprint = true;
122121
std::string motion_model = "Omni";
123-
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
124-
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
122+
std::vector<std::string> critics = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
123+
{"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
124+
{"PreferForwardCritic"}};
125125

126126
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
127127
}
@@ -130,8 +130,9 @@ static void BM_Ackermann(benchmark::State & state)
130130
{
131131
bool consider_footprint = true;
132132
std::string motion_model = "Ackermann";
133-
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
134-
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
133+
std::vector<std::string> critics = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
134+
{"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
135+
{"PreferForwardCritic"}};
135136

136137
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
137138
}

‎nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp‎

Lines changed: 3 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,11 @@
1515
#ifndef NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
1616
#define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
1717

18+
#include <Eigen/Dense>
19+
1820
#include <memory>
1921
#include <vector>
2022

21-
// xtensor creates warnings that needs to be ignored as we are building with -Werror
22-
#pragma GCC diagnostic push
23-
#pragma GCC diagnostic ignored "-Warray-bounds"
24-
#pragma GCC diagnostic ignored "-Wstringop-overflow"
25-
#include <xtensor/xtensor.hpp>
26-
#pragma GCC diagnostic pop
27-
2823
#include "geometry_msgs/msg/pose_stamped.hpp"
2924
#include "nav2_core/goal_checker.hpp"
3025
#include "nav2_mppi_controller/models/state.hpp"
@@ -48,7 +43,7 @@ struct CriticData
4843
const models::Path & path;
4944
const geometry_msgs::msg::Pose & goal;
5045

51-
xt::xtensor<float, 1> & costs;
46+
Eigen::ArrayXf & costs;
5247
float & model_dt;
5348

5449
bool fail_flag;

‎nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp‎

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -20,13 +20,6 @@
2020
#include <vector>
2121
#include <pluginlib/class_loader.hpp>
2222

23-
// xtensor creates warnings that needs to be ignored as we are building with -Werror
24-
#pragma GCC diagnostic push
25-
#pragma GCC diagnostic ignored "-Warray-bounds"
26-
#pragma GCC diagnostic ignored "-Wstringop-overflow"
27-
#include <xtensor/xtensor.hpp>
28-
#pragma GCC diagnostic pop
29-
3023
#include "geometry_msgs/msg/twist.hpp"
3124
#include "geometry_msgs/msg/twist_stamped.hpp"
3225

‎nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp‎

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,7 @@
1515
#ifndef NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
1616
#define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
1717

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

2520
namespace mppi::models
2621
{
@@ -40,15 +35,15 @@ struct Control
4035
*/
4136
struct ControlSequence
4237
{
43-
xt::xtensor<float, 1> vx;
44-
xt::xtensor<float, 1> vy;
45-
xt::xtensor<float, 1> wz;
38+
Eigen::ArrayXf vx;
39+
Eigen::ArrayXf vy;
40+
Eigen::ArrayXf wz;
4641

4742
void reset(unsigned int time_steps)
4843
{
49-
vx = xt::zeros<float>({time_steps});
50-
vy = xt::zeros<float>({time_steps});
51-
wz = xt::zeros<float>({time_steps});
44+
vx.setZero(time_steps);
45+
vy.setZero(time_steps);
46+
wz.setZero(time_steps);
5247
}
5348
};
5449

‎nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp‎

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -15,12 +15,7 @@
1515
#ifndef NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
1616
#define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
1717

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

2520
namespace mppi::models
2621
{
@@ -31,18 +26,18 @@ namespace mppi::models
3126
*/
3227
struct Path
3328
{
34-
xt::xtensor<float, 1> x;
35-
xt::xtensor<float, 1> y;
36-
xt::xtensor<float, 1> yaws;
29+
Eigen::ArrayXf x;
30+
Eigen::ArrayXf y;
31+
Eigen::ArrayXf yaws;
3732

3833
/**
3934
* @brief Reset path data
4035
*/
4136
void reset(unsigned int size)
4237
{
43-
x = xt::zeros<float>({size});
44-
y = xt::zeros<float>({size});
45-
yaws = xt::zeros<float>({size});
38+
x.setZero(size);
39+
y.setZero(size);
40+
yaws.setZero(size);
4641
}
4742
};
4843

‎nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp‎

Lines changed: 14 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -15,16 +15,12 @@
1515
#ifndef NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
1616
#define NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
1717

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

2520
#include <geometry_msgs/msg/pose_stamped.hpp>
2621
#include <geometry_msgs/msg/twist.hpp>
2722

23+
2824
namespace mppi::models
2925
{
3026

@@ -34,13 +30,13 @@ namespace mppi::models
3430
*/
3531
struct State
3632
{
37-
xt::xtensor<float, 2> vx;
38-
xt::xtensor<float, 2> vy;
39-
xt::xtensor<float, 2> wz;
33+
Eigen::ArrayXXf vx;
34+
Eigen::ArrayXXf vy;
35+
Eigen::ArrayXXf wz;
4036

41-
xt::xtensor<float, 2> cvx;
42-
xt::xtensor<float, 2> cvy;
43-
xt::xtensor<float, 2> cwz;
37+
Eigen::ArrayXXf cvx;
38+
Eigen::ArrayXXf cvy;
39+
Eigen::ArrayXXf cwz;
4440

4541
geometry_msgs::msg::PoseStamped pose;
4642
geometry_msgs::msg::Twist speed;
@@ -50,13 +46,13 @@ struct State
5046
*/
5147
void reset(unsigned int batch_size, unsigned int time_steps)
5248
{
53-
vx = xt::zeros<float>({batch_size, time_steps});
54-
vy = xt::zeros<float>({batch_size, time_steps});
55-
wz = xt::zeros<float>({batch_size, time_steps});
49+
vx.setZero(batch_size, time_steps);
50+
vy.setZero(batch_size, time_steps);
51+
wz.setZero(batch_size, time_steps);
5652

57-
cvx = xt::zeros<float>({batch_size, time_steps});
58-
cvy = xt::zeros<float>({batch_size, time_steps});
59-
cwz = xt::zeros<float>({batch_size, time_steps});
53+
cvx.setZero(batch_size, time_steps);
54+
cvy.setZero(batch_size, time_steps);
55+
cwz.setZero(batch_size, time_steps);
6056
}
6157
};
6258
} // namespace mppi::models

0 commit comments

Comments
 (0)