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
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
Prev Previous commit
Next Next commit
Fixed optimizer_unit_tests and related issues
Signed-off-by: Ayush1285 <ayush.singhftw@gmail.com>
  • Loading branch information
Ayush1285 committed Sep 16, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
commit 286b3c0b863cd1558929707807b51aa4d5a122ec
Original file line number Diff line number Diff line change
@@ -39,7 +39,7 @@ namespace mppi
struct CriticData
{
const models::State & state;
models::Trajectories & trajectories;
const models::Trajectories & trajectories;
const models::Path & path;

Eigen::ArrayXf & costs;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
Original file line number Diff line number Diff line change
@@ -41,9 +41,9 @@ struct ControlSequence

void reset(unsigned int time_steps)
{
vx = Eigen::ArrayXf::Zero(time_steps);
vy = Eigen::ArrayXf::Zero(time_steps);
wz = Eigen::ArrayXf::Zero(time_steps);
vx.setZero(time_steps);
vy.setZero(time_steps);
wz.setZero(time_steps);
}
};

Original file line number Diff line number Diff line change
@@ -35,9 +35,9 @@ struct Path
*/
void reset(unsigned int size)
{
x = Eigen::ArrayXf::Zero(size);
y = Eigen::ArrayXf::Zero(size);
yaws = Eigen::ArrayXf::Zero(size);
x.setZero(size);
y.setZero(size);
yaws.setZero(size);
}
};

Original file line number Diff line number Diff line change
@@ -46,13 +46,13 @@ struct State
*/
void reset(unsigned int batch_size, unsigned int 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);
vx.setZero(batch_size, time_steps);
vy.setZero(batch_size, time_steps);
wz.setZero(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);
cvx.setZero(batch_size, time_steps);
cvy.setZero(batch_size, time_steps);
cwz.setZero(batch_size, time_steps);
}
};
} // namespace mppi::models
Original file line number Diff line number Diff line change
@@ -35,9 +35,9 @@ struct Trajectories
*/
void reset(unsigned int batch_size, unsigned int 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);
x.setZero(batch_size, time_steps);
y.setZero(batch_size, time_steps);
yaws.setZero(batch_size, time_steps);
}
};

40 changes: 25 additions & 15 deletions nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
Original file line number Diff line number Diff line change
@@ -696,24 +696,34 @@ struct Pose2D
float x, y, theta;
};

// TODO(Ayush1285) Re-factor this utility method to
// perform in-place column shifting operation
/**
* @brief Shift the columns of a Eigen Array
* @param e Eigen expression or Array
* @param shift Number of columns by which array will be shifted
* ex: -1 means shifting columns to right by 1 place
* @return shifted Eigen Array
* @brief Shift the columns of a 2D Eigen Array or scalar values of
* 1D Eigen Array by 1 place.
* @param e Eigen Array
* @param direction direction in which Array will be shifted.
* 1 for shift in right direction and -1 for left direction.
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
*/
template<class T>
auto rollColumns(T && e, std::ptrdiff_t shift)
inline void shiftColumnsByOnePlace(Eigen::Ref<Eigen::ArrayXXf> e, int direction)
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
{
shift = shift >= 0 ? shift : e.cols() + shift;
auto flat_size = shift * e.rows();
Eigen::ArrayXXf cpyMatrix(e.rows(), e.cols());
std::copy(e.data(), e.data() + flat_size, std::copy(
e.data() + flat_size, e.data() + e.size(), cpyMatrix.data()));
return cpyMatrix;
int size = e.size();
if((e.cols() == 1 || e.rows() == 1) && size > 1) {
auto start_ptr = direction == 1 ? e.data() + size - 2 : e.data() + 1;
auto end_ptr = direction == 1 ? e.data() : e.data() + size - 1;
while(start_ptr != end_ptr) {
*(start_ptr + direction) = *start_ptr;
start_ptr -= direction;
}
*(start_ptr + direction) = *start_ptr;
} else {
auto start_ptr = direction == 1 ? e.data() + size - 2 * e.rows() : e.data() + e.rows();
auto end_ptr = direction == 1 ? e.data() : e.data() + size - e.rows();
auto span = e.rows();
while(start_ptr != end_ptr) {
std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
start_ptr -= (direction * span);
}
std::copy(start_ptr, start_ptr + span, start_ptr + direction * span);
}
}

inline auto point_corrected_yaws(
6 changes: 3 additions & 3 deletions nav2_mppi_controller/src/critics/cost_critic.cpp
Original file line number Diff line number Diff line change
@@ -127,13 +127,13 @@ void CostCritic::score(CriticData & data)
int strided_traj_rows = data.trajectories.x.rows();
int outer_stride = strided_traj_rows * trajectory_point_step_;

const auto traj_x = Eigen::Map<Eigen::ArrayXXf, 0,
const auto traj_x = Eigen::Map<const Eigen::ArrayXXf, 0,
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
Eigen::Stride<-1, -1>>(data.trajectories.x.data(), strided_traj_rows, strided_traj_cols,
Eigen::Stride<-1, -1>(outer_stride, 1));
const auto traj_y = Eigen::Map<Eigen::ArrayXXf, 0,
const auto traj_y = Eigen::Map<const Eigen::ArrayXXf, 0,
Eigen::Stride<-1, -1>>(data.trajectories.y.data(), strided_traj_rows, strided_traj_cols,
Eigen::Stride<-1, -1>(outer_stride, 1));
const auto traj_yaw = Eigen::Map<Eigen::ArrayXXf, 0,
const auto traj_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
Eigen::Stride<-1, -1>(outer_stride, 1));

12 changes: 7 additions & 5 deletions nav2_mppi_controller/src/critics/path_align_critic.cpp
Original file line number Diff line number Diff line change
@@ -99,11 +99,13 @@ void PathAlignCritic::score(CriticData & data)
int strided_traj_cols = data.trajectories.x.cols() / trajectory_point_step_ + 1;
int outer_stride = strided_traj_rows * trajectory_point_step_;
// Get strided trajectory information
const auto T_x = Eigen::Map<Eigen::ArrayXXf, 0, Eigen::Stride<-1, -1>>(data.trajectories.x.data(),
strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
const auto T_y = Eigen::Map<Eigen::ArrayXXf, 0, Eigen::Stride<-1, -1>>(data.trajectories.y.data(),
strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
const auto T_yaw = Eigen::Map<Eigen::ArrayXXf, 0,
const auto T_x = Eigen::Map<const Eigen::ArrayXXf, 0,
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
Eigen::Stride<-1, -1>>(data.trajectories.x.data(),
strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
const auto T_y = Eigen::Map<const Eigen::ArrayXXf, 0,
Eigen::Stride<-1, -1>>(data.trajectories.y.data(),
strided_traj_rows, strided_traj_cols, Eigen::Stride<-1, -1>(outer_stride, 1));
const auto T_yaw = Eigen::Map<const Eigen::ArrayXXf, 0,
Eigen::Stride<-1, -1>>(data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
Eigen::Stride<-1, -1>(outer_stride, 1));
const auto traj_sampled_size = T_x.cols();
33 changes: 20 additions & 13 deletions nav2_mppi_controller/src/optimizer.cpp
Original file line number Diff line number Diff line change
@@ -213,11 +213,15 @@ void Optimizer::prepare(

void Optimizer::shiftControlSequence()
{
control_sequence_.vx = utils::rollColumns(control_sequence_.vx, 1);
control_sequence_.wz = utils::rollColumns(control_sequence_.wz, 1);
auto size = control_sequence_.vx.size();
utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);
utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);
control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);

if (isHolonomic()) {
control_sequence_.vy = utils::rollColumns(control_sequence_.vy, 1);
utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
}
}

@@ -243,6 +247,11 @@ void Optimizer::applyControlSequenceConstraints()
s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy));
float wz_last = std::min(
s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz));
control_sequence_.vx(0) = vx_last;
control_sequence_.wz(0) = wz_last;
if (isHolonomic()) {
control_sequence_.vy(0) = vy_last;
}

for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
float & vx_curr = control_sequence_.vx(i);
@@ -313,10 +322,10 @@ void Optimizer::integrateStateVelocities(
last_yaw = curr_yaw;
}

Eigen::ArrayXf yaw_cos = utils::rollColumns(traj_yaws, -1).cos();
Eigen::ArrayXf yaw_sin = utils::rollColumns(traj_yaws, -1).sin();
yaw_cos(0) = cosf(initial_yaw);
yaw_sin(0) = sinf(initial_yaw);
utils::shiftColumnsByOnePlace(traj_yaws, 1);
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
traj_yaws(0) = initial_yaw;
Eigen::ArrayXf yaw_cos = traj_yaws.cos();
Eigen::ArrayXf yaw_sin = traj_yaws.sin();

auto dx = (vx * yaw_cos).eval();
auto dy = (vx * yaw_sin).eval();
@@ -352,12 +361,10 @@ void Optimizer::integrateStateVelocities(
for(unsigned int i = 1; i != n_cols; i++) {
trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt;
}

auto yaw_cos = (utils::rollColumns(trajectories.yaws, -1).cos()).eval();
auto yaw_sin = (utils::rollColumns(trajectories.yaws, -1).sin()).eval();

yaw_cos.col(0) = cosf(initial_yaw);
yaw_sin.col(0) = sinf(initial_yaw);
utils::shiftColumnsByOnePlace(trajectories.yaws, 1);
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
trajectories.yaws.col(0) = initial_yaw;
auto yaw_cos = trajectories.yaws.cos().eval();
auto yaw_sin = trajectories.yaws.sin().eval();

auto dx = (state.vx * yaw_cos).eval();
auto dy = (state.vx * yaw_sin).eval();
3 changes: 1 addition & 2 deletions nav2_mppi_controller/test/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -12,7 +12,6 @@ set(TEST_NAMES
optimizer_unit_tests
)


foreach(name IN LISTS TEST_NAMES)
ament_add_gtest(${name}
${name}.cpp
@@ -44,4 +43,4 @@ target_link_libraries(critics_tests
)
if(${TEST_DEBUG_INFO})
target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO)
endif()
endif()
39 changes: 19 additions & 20 deletions nav2_mppi_controller/test/optimizer_unit_tests.cpp
Original file line number Diff line number Diff line change
@@ -106,12 +106,12 @@ class OptimizerTester : public Optimizer
{
reset();

EXPECT_TRUE(state_.vx.isApprox(Eigen::ArrayXXf::Zero(1000, 50)));
EXPECT_TRUE(control_sequence_.vx.isApprox(Eigen::ArrayXf::Zero(50)));
EXPECT_TRUE(state_.vx.isApproxToConstant(0.0f));
EXPECT_TRUE(control_sequence_.vx.isApproxToConstant(0.0f));
EXPECT_EQ(control_history_[0].vx, 0.0);
EXPECT_EQ(control_history_[0].vy, 0.0);
EXPECT_NEAR(costs_.sum(), 0, 1e-6);
EXPECT_TRUE(generated_trajectories_.x.isApprox(Eigen::ArrayXXf::Zero(1000, 50)));
EXPECT_TRUE(generated_trajectories_.x.isApproxToConstant(0.0f));
}

bool fallbackWrapper(bool fail)
@@ -247,7 +247,7 @@ TEST(OptimizerTests, BasicInitializedFunctions)
auto trajs = optimizer_tester.getGeneratedTrajectories();
EXPECT_EQ(trajs.x.rows(), 1000);
EXPECT_EQ(trajs.x.cols(), 50);
EXPECT_TRUE(trajs.x.isApprox(Eigen::ArrayXXf::Zero(1000, 50)));
EXPECT_TRUE(trajs.x.isApproxToConstant(0.0f));

optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
@@ -401,7 +401,7 @@ TEST(OptimizerTests, shiftControlSequenceTests)
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);

// Test shiftControlSequence by setting the 2nd value to something unique to neighbors
auto sequence = optimizer_tester.grabControlSequence();
auto & sequence = optimizer_tester.grabControlSequence();
sequence.reset(100);
sequence.vx(0) = 9999;
sequence.vx(1) = 6;
@@ -491,34 +491,34 @@ TEST(OptimizerTests, applyControlSequenceConstraintsTests)
// in motion_models_test.cpp
optimizer_tester.resetMotionModel();
optimizer_tester.testSetOmniModel();
auto sequence = optimizer_tester.grabControlSequence();
auto & sequence = optimizer_tester.grabControlSequence();

// Test boundary of limits
sequence.vx = Eigen::ArrayXf::Ones(50);
sequence.vy = 0.75 * Eigen::ArrayXf::Ones(50);
sequence.wz = 2.0 * Eigen::ArrayXf::Ones(50);
optimizer_tester.applyControlSequenceConstraintsWrapper();
EXPECT_TRUE(sequence.vx.isApprox(Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.vy.isApprox(0.75 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.wz.isApprox(2.0 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.vx.isApproxToConstant(1.0f));
EXPECT_TRUE(sequence.vy.isApproxToConstant(0.75f));
EXPECT_TRUE(sequence.wz.isApproxToConstant(2.0f));

// Test breaking limits sets to maximum
sequence.vx = 5.0 * Eigen::ArrayXf::Ones(50);
sequence.vy = 5.0 * Eigen::ArrayXf::Ones(50);
sequence.wz = 5.0 * Eigen::ArrayXf::Ones(50);
optimizer_tester.applyControlSequenceConstraintsWrapper();
EXPECT_TRUE(sequence.vx.isApprox(Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.vy.isApprox(0.75 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.wz.isApprox(2.0 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.vx.isApproxToConstant(1.0f));
EXPECT_TRUE(sequence.vy.isApproxToConstant(0.75f));
EXPECT_TRUE(sequence.wz.isApproxToConstant(2.0f));

// Test breaking limits sets to minimum
sequence.vx = -5.0 * Eigen::ArrayXf::Ones(50);
sequence.vy = -5.0 * Eigen::ArrayXf::Ones(50);
sequence.wz = -5.0 * Eigen::ArrayXf::Ones(50);
optimizer_tester.applyControlSequenceConstraintsWrapper();
EXPECT_TRUE(sequence.vx.isApprox(-1.0 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.vy.isApprox(-0.75 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.wz.isApprox(-2.0 * Eigen::ArrayXf::Ones(50)));
EXPECT_TRUE(sequence.vx.isApproxToConstant(-1.0f));
EXPECT_TRUE(sequence.vy.isApproxToConstant(-0.75f));
EXPECT_TRUE(sequence.wz.isApproxToConstant(-2.0f));
}

TEST(OptimizerTests, updateStateVelocitiesTests)
@@ -611,14 +611,14 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
models::State state;
state.reset(1000, 50);
models::Trajectories traj;
traj.reset(1000, 50);
state.vx = 0.1 * Eigen::ArrayXXf::Ones(1000, 50);
state.vx.col(0) = Eigen::ArrayXf::Zero(1000);
state.vy = Eigen::ArrayXXf::Zero(1000, 50);
state.wz = Eigen::ArrayXXf::Zero(1000, 50);

optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
EXPECT_TRUE(traj.y.isApprox(Eigen::ArrayXXf::Zero(1000, 50)));
EXPECT_TRUE(traj.yaws.isApprox(Eigen::ArrayXXf::Zero(1000, 50)));
EXPECT_TRUE(traj.y.isApproxToConstant(0.0f));
EXPECT_TRUE(traj.yaws.isApproxToConstant(0.0f));
for (unsigned int i = 0; i != traj.x.cols(); i++) {
EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3);
}
@@ -628,7 +628,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
state.vy.col(0) = Eigen::ArrayXf::Zero(1000);
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);

EXPECT_TRUE(traj.yaws.isApprox(Eigen::ArrayXXf::Zero(1000, 50)));
EXPECT_TRUE(traj.yaws.isApproxToConstant(0.0f));
for (unsigned int i = 0; i != traj.x.cols(); i++) {
EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3);
EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3);
@@ -643,7 +643,6 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
float x = 0;
float y = 0;
for (unsigned int i = 1; i != traj.x.cols(); i++) {
std::cout << i << std::endl;
x += (0.1 /*vx*/ * cosf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
y += (0.1 /*vx*/ * sinf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;

Loading