From 9eb67820d728d0b1cb74a7a6157c2accb92e9a0c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 9 Sep 2024 23:28:02 -0700 Subject: [PATCH] Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes (#4664) * Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes Signed-off-by: Steve Macenski * fixing unit test for type implicit cast Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski (cherry picked from commit 7eb47d84012dd9e25988ce5c9c269280e6ee3dd1) --- .../nav2_mppi_controller/motion_models.hpp | 4 +- .../test/motion_model_tests.cpp | 78 +++++++++++++++++++ 2 files changed, 80 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 0c5671ea3e..7d84ba29df 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -112,8 +112,8 @@ class AckermannMotionModel : public MotionModel auto & vx = control_sequence.vx; auto & wz = control_sequence.wz; - auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_); - view = xt::sign(wz) * vx / min_turning_r_; + auto view = xt::masked_view(wz, (xt::fabs(vx) / xt::fabs(wz)) < min_turning_r_); + view = xt::sign(wz) * xt::fabs(vx) / min_turning_r_; } /** diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 4238fa03fb..f22424d226 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -164,6 +164,9 @@ TEST(MotionModelTests, AckermannTest) // VX equal since this doesn't change, the WZ is reduced if breaking the constraint EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); @@ -177,3 +180,78 @@ TEST(MotionModelTests, AckermannTest) // Check it cleanly destructs model.reset(); } + +TEST(MotionModelTests, AckermannReversingTest) +{ + models::ControlSequence control_sequence; + models::ControlSequence control_sequence2; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + control_sequence2.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + auto node = std::make_shared("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler, node->get_name()); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are non-empty for Ackermann Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence.vx(i) = -idx * idx * idx; // now reversing + control_sequence.wz(i) = idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } + + // Repeat with negative rotation direction + for (unsigned int i = 0; i != control_sequence2.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence2.vx(i) = -idx * idx * idx; // now reversing + control_sequence2.wz(i) = -idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence2 = control_sequence2; + model->applyConstraints(control_sequence2); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); + EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); + for (unsigned int i = 1; i != control_sequence2.wz.shape(0); i++) { + EXPECT_LT(control_sequence2.wz(i), 0.0); + } + + // Now, check the specifics of the minimum curvature constraint + EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); + for (unsigned int i = 1; i != control_sequence2.vx.shape(0); i++) { + EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2); + } + + // Check that Ackermann Drive is properly non-holonomic and parameterized + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +}