add open loop option for MPPI

nav2_mppi_controller/README.md

@@ -61,6 +61,7 @@ This process is then repeated a number of times and returns a converged solution
  | 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. |
  | 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. |
 
+ | open_loop        | bool    | Default false. Useful when using low accelerations and when wheel odometry's latency causes issues in initial state estimation. |
 
 #### Trajectory Visualizer
  | Parameter             | Type   | Definition                                                                                                  |

nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp

@@ -38,6 +38,7 @@ struct OptimizerSettings
   unsigned int iteration_count{0u};
   bool shift_control_sequence{false};
   size_t retry_attempt_limit{0};
+  bool open_loop{false};
 };
 
 }  // namespace mppi::models

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -288,6 +288,8 @@ class Optimizer
     std::nullopt, std::nullopt};  /// Caution, keep references
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+
+  geometry_msgs::msg::Twist last_command_vel_;
 };
 
 }  // namespace mppi

nav2_mppi_controller/src/optimizer.cpp

@@ -99,6 +99,7 @@ void Optimizer::getParams()
   getParam(s.sampling_std.vy, "vy_std", 0.2f);
   getParam(s.sampling_std.wz, "wz_std", 0.4f);
   getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
+  getParam(s.open_loop, "open_loop", false);
 
   s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
   if (s.base_constraints.ax_min > 0.0) {
@@ -158,6 +159,10 @@ void Optimizer::reset(bool reset_dynamic_speed_limits)
   control_history_[2] = {0.0f, 0.0f, 0.0f};
   control_history_[3] = {0.0f, 0.0f, 0.0f};
 
+  if (settings_.open_loop) {
+    last_command_vel_ = geometry_msgs::msg::Twist();
+  }
+
   if (reset_dynamic_speed_limits) {
     settings_.constraints = settings_.base_constraints;
   }
@@ -213,6 +218,8 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
   utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
   auto control = getControlFromSequenceAsTwist(plan.header.stamp);
 
+  last_command_vel_ = control.twist;
+
   if (settings_.shift_control_sequence) {
     shiftControlSequence();
   }
@@ -256,7 +263,7 @@ void Optimizer::prepare(
   nav2_core::GoalChecker * goal_checker)
 {
   state_.pose = robot_pose;
-  state_.speed = robot_speed;
+  state_.speed = settings_.open_loop ? last_command_vel_ : robot_speed;
   state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
   path_ = utils::toTensor(plan);
   costs_.setZero();
@@ -347,8 +354,7 @@ void Optimizer::updateStateVelocities(
   propagateStateVelocitiesFromInitials(state);
 }
 
-void Optimizer::updateInitialStateVelocities(
-  models::State & state) const
+void Optimizer::updateInitialStateVelocities(models::State & state) const
 {
   state.vx.col(0) = static_cast<float>(state.speed.linear.x);
   state.wz.col(0) = static_cast<float>(state.speed.angular.z);

nav2_mppi_controller/test/optimizer_unit_tests.cpp

@@ -697,6 +697,64 @@ TEST(OptimizerTests, TestGetters)
   EXPECT_EQ(optimizer_tester.getSettings().model_dt, 0.05f);
 }
 
+TEST(OptimizerTests, Omni_openLoopMppiTest)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  OptimizerTester optimizer_tester;
+  node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
+  node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
+  node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.05));
+  node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(80));
+  node->declare_parameter("mppic.ax_max", rclcpp::ParameterValue(0.5));
+  node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(0.5));
+  node->declare_parameter("mppic.ay_max", rclcpp::ParameterValue(0.5));
+  node->declare_parameter("mppic.open_loop", rclcpp::ParameterValue(true));
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  optimizer_tester.initialize(node, "mppic", costmap_ros, tf_buffer, &param_handler);
+  optimizer_tester.resetMotionModel();
+  optimizer_tester.testSetOmniModel();
+
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 999;
+  geometry_msgs::msg::Twist robot_speed;
+  robot_speed.linear.x = 100.0;
+  robot_speed.angular.z = 100.0;
+  robot_speed.linear.y = 100.0;
+  nav_msgs::msg::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.poses.resize(17);
+
+  auto [cmd1, optimal_trajectory1] = optimizer_tester.evalControl(pose, robot_speed, path,
+    goal, nullptr);
+
+  EXPECT_LE(std::abs(cmd1.twist.linear.x),
+    optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ax_max);
+  EXPECT_LE(std::abs(cmd1.twist.angular.z),
+    optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().az_max);
+  EXPECT_LE(std::abs(cmd1.twist.linear.y),
+    optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ay_max);
+
+  auto [cmd2, optimal_trajectory2] = optimizer_tester.evalControl(pose, robot_speed, path,
+    goal, nullptr);
+
+  const double vx_delta = std::abs(cmd2.twist.linear.x - cmd1.twist.linear.x);
+  const double wz_delta = std::abs(cmd2.twist.angular.z - cmd1.twist.angular.z);
+  const double vy_delta = std::abs(cmd2.twist.linear.y - cmd1.twist.linear.y);
+
+  EXPECT_LE(vx_delta,
+    optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ax_max);
+  EXPECT_LE(wz_delta,
+    optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().az_max);
+  EXPECT_LE(vy_delta,
+    optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ay_max);
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);