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
Open
Show file tree
Hide file tree
Changes from 14 commits
Commits
Show all changes
46 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
cdd489f
Merged the latest SG Filter changes
Ayush1285 Oct 5, 2024
0327754
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Oct 16, 2024
2f8bd63
Fixed CostCritic issue and added test for shiftColumn method
Ayush1285 Oct 16, 2024
55f7fae
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Oct 20, 2024
902a72d
Added test for new functions
Ayush1285 Oct 20, 2024
aa732d2
Removed compiler flags
Ayush1285 Oct 20, 2024
526436c
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Nov 18, 2024
1862c1c
updated test to check first and last columns
Ayush1285 Nov 20, 2024
7a398ff
Addressed few review comments
Ayush1285 Nov 20, 2024
082aab9
Merge branch 'eigen_mppi' of github.com:Ayush1285/navigation2 into ei…
Ayush1285 Nov 20, 2024
85df29c
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Nov 20, 2024
b94dad3
Merge branch 'eigen_mppi' of github.com:Ayush1285/navigation2 into ei…
Ayush1285 Nov 20, 2024
e81afc5
Changed the obstacle critic implementation to the original way. Updat…
Ayush1285 Nov 20, 2024
b78606d
Fixed bugs
Ayush1285 Nov 23, 2024
510d43c
Fixed linter
Ayush1285 Nov 23, 2024
15ee9ce
Added clamp util function
Ayush1285 Nov 23, 2024
78e6d94
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Nov 23, 2024
1e3835e
Fixed bug
Ayush1285 Nov 23, 2024
00f77e1
Fixed review comments: Added utils::clamp method
Ayush1285 Nov 27, 2024
61652cd
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Dec 5, 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
20 changes: 5 additions & 15 deletions nav2_mppi_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,22 +1,12 @@
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)

# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major
# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major

find_package(ament_cmake REQUIRED)
find_package(xsimd REQUIRED)
find_package(xtensor REQUIRED)
find_package(Eigen3 REQUIRED)
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved

include_directories(
include
${EIGEN3_INCLUDE_DIR}
)

set(dependencies_pkgs
Expand Down Expand Up @@ -95,8 +85,8 @@ set(libraries mppi_controller mppi_critics)

foreach(lib IN LISTS libraries)
target_compile_options(${lib} PUBLIC -fconcepts)
target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS}
target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd)
target_include_directories(${lib} PUBLIC) # ${OpenMP_INCLUDE_DIRS}
target_link_libraries(${lib})
ament_target_dependencies(${lib} ${dependencies_pkgs})
endforeach()

Expand All @@ -116,7 +106,7 @@ if(BUILD_TESTING)
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
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 Down
2 changes: 1 addition & 1 deletion nav2_mppi_controller/benchmark/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ foreach(name IN LISTS BENCHMARK_NAMES)
${dependencies_pkgs}
)
target_link_libraries(${name}
mppi_controller mppi_critics benchmark
Ayush1285 marked this conversation as resolved.
Show resolved Hide resolved
mppi_controller benchmark mppi_critics
)

target_include_directories(${name} PRIVATE
Expand Down
4 changes: 1 addition & 3 deletions nav2_mppi_controller/benchmark/controller_benchmark.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,7 @@
#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 <Eigen/Dense>

#include "nav2_mppi_controller/motion_models.hpp"
#include "nav2_mppi_controller/controller.hpp"
Expand Down
7 changes: 2 additions & 5 deletions nav2_mppi_controller/benchmark/optimizer_benchmark.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,7 @@
#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 <Eigen/Dense>

#include "nav2_mppi_controller/optimizer.hpp"
#include "nav2_mppi_controller/motion_models.hpp"
Expand Down Expand Up @@ -118,8 +116,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"}};
std::vector<std::string> critics = {};
Ayush1285 marked this conversation as resolved.
Show resolved Hide resolved

prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@

#include "nav2_mppi_controller/tools/path_handler.hpp"
#include "nav2_mppi_controller/optimizer.hpp"
#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
//#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
Ayush1285 marked this conversation as resolved.
Show resolved Hide resolved
#include "nav2_mppi_controller/models/constraints.hpp"
#include "nav2_mppi_controller/tools/utils.hpp"

Expand Down Expand Up @@ -117,7 +117,7 @@ class MPPIController : public nav2_core::Controller
std::unique_ptr<ParametersHandler> parameters_handler_;
Optimizer optimizer_;
PathHandler path_handler_;
TrajectoryVisualizer trajectory_visualizer_;
//TrajectoryVisualizer trajectory_visualizer_;

bool visualize_;
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,7 @@
#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 <Eigen/Dense>

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/goal_checker.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
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,7 @@
#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
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 = Eigen::ArrayXf::Zero(time_steps);
vy = Eigen::ArrayXf::Zero(time_steps);
wz = Eigen::ArrayXf::Zero(time_steps);
}
};

Expand Down
19 changes: 7 additions & 12 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp
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 = Eigen::ArrayXf::Zero(size);
y = Eigen::ArrayXf::Zero(size);
yaws = Eigen::ArrayXf::Zero(size);
}
};

Expand Down
32 changes: 14 additions & 18 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
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 = Eigen::ArrayXXf::Zero(batch_size, time_steps);
vy = Eigen::ArrayXXf::Zero(batch_size, time_steps);
wz = Eigen::ArrayXXf::Zero(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 = Eigen::ArrayXXf::Zero(batch_size, time_steps);
cvy = Eigen::ArrayXXf::Zero(batch_size, time_steps);
cwz = Eigen::ArrayXXf::Zero(batch_size, time_steps);
}
};
} // namespace mppi::models
Expand Down
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 = Eigen::ArrayXXf::Zero(batch_size, time_steps);
y = Eigen::ArrayXXf::Zero(batch_size, time_steps);
yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps);
}
};

Expand Down
Loading