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

Merged
merged 57 commits into from
Jan 15, 2025
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
57 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
8e609ab
Merged main and resolved merge conflict
Ayush1285 Dec 20, 2024
6f01214
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Dec 24, 2024
78f1d61
Fixing strided trajectory columns
Ayush1285 Dec 26, 2024
aab0b55
fixed lint error
Ayush1285 Dec 26, 2024
7ad850a
Merge branch 'ros-navigation:main' into eigen_mppi
Ayush1285 Jan 14, 2025
4dd35f1
merged into main
Ayush1285 Jan 15, 2025
cc1b48b
Fixed merge
Ayush1285 Jan 15, 2025
3be2a61
Merge branch 'eigen_mppi' of github.com:Ayush1285/navigation2 into ei…
Ayush1285 Jan 15, 2025
3460d5b
Fixed optimizer benchmark with latest api changes
Ayush1285 Jan 15, 2025
7820a41
fixed build error
Ayush1285 Jan 15, 2025
141bb6a
Fixed new util_test
Ayush1285 Jan 15, 2025
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
Next Next commit
Initial commit
Signed-off-by: Ayush1285 <ayush.singhftw@gmail.com>
  • Loading branch information
Ayush1285 committed Jun 6, 2024
commit 137f75db538eab0e48cf490ec6330d9b0c3b3dc4
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::VectorXf vx;
Eigen::VectorXf vy;
Eigen::VectorXf 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::VectorXf::Zero(time_steps);
vy = Eigen::VectorXf::Zero(time_steps);
wz = Eigen::VectorXf::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::VectorXf x;
Eigen::VectorXf y;
Eigen::VectorXf 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::VectorXf::Zero(size);
y = Eigen::VectorXf::Zero(size);
yaws = Eigen::VectorXf::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::MatrixXf vx;
Eigen::MatrixXf vy;
Eigen::MatrixXf wz;

xt::xtensor<float, 2> cvx;
xt::xtensor<float, 2> cvy;
xt::xtensor<float, 2> cwz;
Eigen::MatrixXf cvx;
Eigen::MatrixXf cvy;
Eigen::MatrixXf 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::MatrixXf::Zero(batch_size, time_steps);
vy = Eigen::MatrixXf::Zero(batch_size, time_steps);
wz = Eigen::MatrixXf::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::MatrixXf::Zero(batch_size, time_steps);
cvy = Eigen::MatrixXf::Zero(batch_size, time_steps);
cwz = Eigen::MatrixXf::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::MatrixXf x;
Eigen::MatrixXf y;
Eigen::MatrixXf 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::MatrixXf::Zero(batch_size, time_steps);
y = Eigen::MatrixXf::Zero(batch_size, time_steps);
yaws = Eigen::MatrixXf::Zero(batch_size, time_steps);
}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,7 @@
#include "nav2_mppi_controller/models/state.hpp"
#include "nav2_mppi_controller/models/constraints.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/xmath.hpp>
#include <xtensor/xmasked_view.hpp>
#include <xtensor/xview.hpp>
#include <xtensor/xnoalias.hpp>
#pragma GCC diagnostic pop
#include <Eigen/Dense>

#include "nav2_mppi_controller/tools/parameters_handler.hpp"

Expand Down Expand Up @@ -87,11 +79,11 @@ class MotionModel
float min_delta_vx = model_dt_ * control_constraints_.ax_min;
float max_delta_vy = model_dt_ * control_constraints_.ay_max;
float max_delta_wz = model_dt_ * control_constraints_.az_max;
for (unsigned int i = 0; i != state.vx.shape(0); i++) {
for (unsigned int i = 0; i != state.vx.rows(); i++) {
float vx_last = state.vx(i, 0);
float vy_last = state.vy(i, 0);
float wz_last = state.wz(i, 0);
for (unsigned int j = 1; j != state.vx.shape(1); j++) {
for (unsigned int j = 1; j != state.vx.cols(); j++) {
float & cvx_curr = state.cvx(i, j - 1);
cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
state.vx(i, j) = cvx_curr;
Expand Down
18 changes: 6 additions & 12 deletions nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,7 @@
#include <string>
#include <memory>

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

#include "rclcpp_lifecycle/lifecycle_node.hpp"

Expand Down Expand Up @@ -56,7 +50,7 @@ namespace mppi
*/
class Optimizer
{
public:
public:
/**
* @brief Constructor for mppi::Optimizer
*/
Expand Down Expand Up @@ -108,7 +102,7 @@ class Optimizer
* @brief Get the optimal trajectory for a cycle for visualization
* @return Optimal trajectory
*/
xt::xtensor<float, 2> getOptimizedTrajectory();
Eigen::MatrixXf getOptimizedTrajectory();

/**
* @brief Set the maximum speed based on the speed limits callback
Expand Down Expand Up @@ -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::MatrixXf & trajectories,
const Eigen::MatrixXf & state) const;

/**
* @brief Update control sequence with state controls weighted by costs
Expand Down Expand Up @@ -256,7 +250,7 @@ class Optimizer
std::array<mppi::models::Control, 4> control_history_;
models::Trajectories generated_trajectories_;
models::Path path_;
xt::xtensor<float, 1> costs_;
Eigen::VectorXf costs_;

CriticData critics_data_ =
{state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,13 +21,7 @@
#include <mutex>
#include <condition_variable>

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

#include "nav2_mppi_controller/models/optimizer_settings.hpp"
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
Expand Down Expand Up @@ -99,9 +93,9 @@ class NoiseGenerator
*/
void generateNoisedControls();

xt::xtensor<float, 2> noises_vx_;
xt::xtensor<float, 2> noises_vy_;
xt::xtensor<float, 2> noises_wz_;
Eigen::MatrixXf noises_vx_;
Eigen::MatrixXf noises_vy_;
Eigen::MatrixXf noises_wz_;

mppi::models::OptimizerSettings settings_;
bool is_holonomic_;
Expand Down
Loading