Skip to content

Commit 8dbc929

Browse files
chanhhoang99chanhhoang99
authored
add open loop option for MPPI (#5617)
* add open loop option for MPPI Signed-off-by: Chanh Hoang <[email protected]> * Change parameter description. Fix linting Signed-off-by: Chanh Hoang <[email protected]> * Storing last command velocity Signed-off-by: Chanh Hoang <[email protected]> * trailing white space removal Signed-off-by: Chanh Hoang <[email protected]> * code style fix Signed-off-by: Chanh Hoang <[email protected]> * fix code style divergence Signed-off-by: Chanh Hoang <[email protected]> * update unit test open_loop option for optimizer Signed-off-by: Chanh Hoang <[email protected]> * fix ament uncrusify Signed-off-by: Chanh Hoang <[email protected]> * refactor Signed-off-by: Chanh Hoang <[email protected]> * fix build error Signed-off-by: Chanh Hoang <[email protected]> * add unit test for initial last_command_vel value Signed-off-by: Chanh Hoang <[email protected]> * code coverage fix Signed-off-by: Chanh Hoang <[email protected]> * move open loop option to prepare() function Signed-off-by: Chanh Hoang <[email protected]> * Refactor unit test for optimizer mppi open loop option Signed-off-by: Chanh Hoang <[email protected]> * fix unit test build fail Signed-off-by: Chanh Hoang <[email protected]> * linitng Signed-off-by: Chanh Hoang <[email protected]> * fix build error Signed-off-by: Chanh Hoang <[email protected]> --------- Signed-off-by: Chanh Hoang <[email protected]>
1 parent 976fbeb commit 8dbc929

File tree

5 files changed

+71
-3
lines changed

5 files changed

+71
-3
lines changed

‎nav2_mppi_controller/README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ This process is then repeated a number of times and returns a converged solution
6161
| publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. |
6262
| publish_critics_stats | bool | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. |
6363

64+
| open_loop | bool | Default false. Useful when using low accelerations and when wheel odometry's latency causes issues in initial state estimation. |
6465

6566
#### Trajectory Visualizer
6667
| Parameter | Type | Definition |

‎nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ struct OptimizerSettings
3838
unsigned int iteration_count{0u};
3939
bool shift_control_sequence{false};
4040
size_t retry_attempt_limit{0};
41+
bool open_loop{false};
4142
};
4243

4344
} // namespace mppi::models

‎nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -288,6 +288,8 @@ class Optimizer
288288
std::nullopt, std::nullopt}; /// Caution, keep references
289289

290290
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
291+
292+
geometry_msgs::msg::Twist last_command_vel_;
291293
};
292294

293295
} // namespace mppi

‎nav2_mppi_controller/src/optimizer.cpp‎

Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,7 @@ void Optimizer::getParams()
9999
getParam(s.sampling_std.vy, "vy_std", 0.2f);
100100
getParam(s.sampling_std.wz, "wz_std", 0.4f);
101101
getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
102+
getParam(s.open_loop, "open_loop", false);
102103

103104
s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
104105
if (s.base_constraints.ax_min > 0.0) {
@@ -158,6 +159,10 @@ void Optimizer::reset(bool reset_dynamic_speed_limits)
158159
control_history_[2] = {0.0f, 0.0f, 0.0f};
159160
control_history_[3] = {0.0f, 0.0f, 0.0f};
160161

162+
if (settings_.open_loop) {
163+
last_command_vel_ = geometry_msgs::msg::Twist();
164+
}
165+
161166
if (reset_dynamic_speed_limits) {
162167
settings_.constraints = settings_.base_constraints;
163168
}
@@ -213,6 +218,8 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
213218
utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
214219
auto control = getControlFromSequenceAsTwist(plan.header.stamp);
215220

221+
last_command_vel_ = control.twist;
222+
216223
if (settings_.shift_control_sequence) {
217224
shiftControlSequence();
218225
}
@@ -256,7 +263,7 @@ void Optimizer::prepare(
256263
nav2_core::GoalChecker * goal_checker)
257264
{
258265
state_.pose = robot_pose;
259-
state_.speed = robot_speed;
266+
state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed;
260267
state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
261268
path_ = utils::toTensor(plan);
262269
costs_.setZero();
@@ -347,8 +354,7 @@ void Optimizer::updateStateVelocities(
347354
propagateStateVelocitiesFromInitials(state);
348355
}
349356

350-
void Optimizer::updateInitialStateVelocities(
351-
models::State & state) const
357+
void Optimizer::updateInitialStateVelocities(models::State & state) const
352358
{
353359
state.vx.col(0) = static_cast<float>(state.speed.linear.x);
354360
state.wz.col(0) = static_cast<float>(state.speed.angular.z);

‎nav2_mppi_controller/test/optimizer_unit_tests.cpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -697,6 +697,64 @@ TEST(OptimizerTests, TestGetters)
697697
EXPECT_EQ(optimizer_tester.getSettings().model_dt, 0.05f);
698698
}
699699

700+
TEST(OptimizerTests, Omni_openLoopMppiTest)
701+
{
702+
auto node = std::make_shared<nav2::LifecycleNode>("my_node");
703+
OptimizerTester optimizer_tester;
704+
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
705+
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
706+
node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.05));
707+
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(80));
708+
node->declare_parameter("mppic.ax_max", rclcpp::ParameterValue(0.5));
709+
node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(0.5));
710+
node->declare_parameter("mppic.ay_max", rclcpp::ParameterValue(0.5));
711+
node->declare_parameter("mppic.open_loop", rclcpp::ParameterValue(true));
712+
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
713+
"dummy_costmap", "", true);
714+
std::string name = "test";
715+
ParametersHandler param_handler(node, name);
716+
rclcpp_lifecycle::State lstate;
717+
costmap_ros->on_configure(lstate);
718+
auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
719+
optimizer_tester.initialize(node, "mppic", costmap_ros, tf_buffer, &param_handler);
720+
optimizer_tester.resetMotionModel();
721+
optimizer_tester.testSetOmniModel();
722+
723+
geometry_msgs::msg::PoseStamped pose;
724+
pose.pose.position.x = 999;
725+
geometry_msgs::msg::Twist robot_speed;
726+
robot_speed.linear.x = 100.0;
727+
robot_speed.angular.z = 100.0;
728+
robot_speed.linear.y = 100.0;
729+
nav_msgs::msg::Path path;
730+
geometry_msgs::msg::Pose goal;
731+
path.poses.resize(17);
732+
733+
auto [cmd1, optimal_trajectory1] = optimizer_tester.evalControl(pose, robot_speed, path,
734+
goal, nullptr);
735+
736+
EXPECT_LE(std::abs(cmd1.twist.linear.x),
737+
optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ax_max);
738+
EXPECT_LE(std::abs(cmd1.twist.angular.z),
739+
optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().az_max);
740+
EXPECT_LE(std::abs(cmd1.twist.linear.y),
741+
optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ay_max);
742+
743+
auto [cmd2, optimal_trajectory2] = optimizer_tester.evalControl(pose, robot_speed, path,
744+
goal, nullptr);
745+
746+
const double vx_delta = std::abs(cmd2.twist.linear.x - cmd1.twist.linear.x);
747+
const double wz_delta = std::abs(cmd2.twist.angular.z - cmd1.twist.angular.z);
748+
const double vy_delta = std::abs(cmd2.twist.linear.y - cmd1.twist.linear.y);
749+
750+
EXPECT_LE(vx_delta,
751+
optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ax_max);
752+
EXPECT_LE(wz_delta,
753+
optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().az_max);
754+
EXPECT_LE(vy_delta,
755+
optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ay_max);
756+
}
757+
700758
int main(int argc, char **argv)
701759
{
702760
::testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)