Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
optimizer.cpp
1 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
2 // Copyright (c) 2025 Open Navigation LLC
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #include "nav2_mppi_controller/optimizer.hpp"
17 
18 #include <limits>
19 #include <memory>
20 #include <stdexcept>
21 #include <string>
22 #include <vector>
23 #include <cmath>
24 #include <chrono>
25 
26 #include "nav2_core/controller_exceptions.hpp"
27 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
28 #include "nav2_ros_common/node_utils.hpp"
29 #include "nav2_ros_common/tf2_factories.hpp"
30 
31 namespace mppi
32 {
33 
35  nav2::LifecycleNode::WeakPtr parent, const std::string & name,
36  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
37  nav2::TransformBuffer::SharedPtr tf_buffer,
38  ParametersHandler * param_handler)
39 {
40  parent_ = parent;
41  name_ = name;
42  costmap_ros_ = costmap_ros;
43  costmap_ = costmap_ros_->getCostmap();
44  parameters_handler_ = param_handler;
45  tf_buffer_ = tf_buffer;
46 
47  auto node = parent_.lock();
48  logger_ = node->get_logger();
49 
50  getParams();
51 
52  critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_);
53  noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_);
54 
55  // This may throw an exception if not valid and fail initialization
56  nav2::declare_parameter_if_not_declared(
57  node, name_ + ".TrajectoryValidator.plugin",
58  rclcpp::ParameterValue("mppi::DefaultOptimalTrajectoryValidator"));
59  std::string validator_plugin_type = nav2::get_plugin_type_param(
60  node, name_ + ".TrajectoryValidator");
61  validator_loader_ = std::make_unique<pluginlib::ClassLoader<OptimalTrajectoryValidator>>(
62  "nav2_mppi_controller", "mppi::OptimalTrajectoryValidator");
63  trajectory_validator_ = validator_loader_->createUniqueInstance(validator_plugin_type);
64  trajectory_validator_->initialize(
65  parent_, name_ + ".TrajectoryValidator",
66  costmap_ros_, parameters_handler_, tf_buffer, settings_);
67  RCLCPP_INFO(logger_, "Loaded trajectory validator plugin: %s", validator_plugin_type.c_str());
68 
69  reset();
70 }
71 
73 {
74  noise_generator_.shutdown();
75 }
76 
78 {
79  std::string motion_model_name;
80 
81  auto & s = settings_;
82  auto getParam = parameters_handler_->getParamGetter(name_);
83  auto getParentParam = parameters_handler_->getParamGetter("");
84 
85  // Reject dynamic updates to kinematic params when speed limit is active
86  auto kinematic_guard = [this](
87  const rclcpp::Parameter & param,
88  rcl_interfaces::msg::SetParametersResult & result) {
89  if (isSpeedLimitActive()) {
90  result.successful = false;
91  if (!result.reason.empty()) {
92  result.reason += "\n";
93  }
94  result.reason += "Rejected dynamic update to '" + param.get_name() +
95  "': speed limit is active. Clear the speed limit first.";
96  }
97  };
98 
99  const std::vector<std::string> kinematic_params = {
100  "vx_max", "vx_min", "vy_max", "wz_max"};
101  for (const auto & p : kinematic_params) {
102  parameters_handler_->addPreCallback(name_ + "." + p, kinematic_guard);
103  }
104 
105  auto batch_size_guard = [](
106  const rclcpp::Parameter & param,
107  rcl_interfaces::msg::SetParametersResult & result)
108  {
109  const auto value = param.as_int();
110  if (value <= 0 || value > 100000) {
111  result.successful = false;
112  if (!result.reason.empty()) {
113  result.reason += "\n";
114  }
115  result.reason += "MPPI batch_size must be in [1, 100000]";
116  if (value > 100000) {
117  result.reason += ". If you need a larger value, contact the maintainers to discuss "
118  "extending this limit";
119  }
120  }
121  };
122  parameters_handler_->addPreCallback(name_ + ".batch_size", batch_size_guard);
123 
124  auto time_steps_guard = [](
125  const rclcpp::Parameter & param,
126  rcl_interfaces::msg::SetParametersResult & result)
127  {
128  const auto value = param.as_int();
129  if (value <= 0 || value > 1000) {
130  result.successful = false;
131  if (!result.reason.empty()) {
132  result.reason += "\n";
133  }
134  result.reason += "MPPI time_steps must be in [1, 1000]";
135  if (value > 1000) {
136  result.reason += ". If you need a larger value, contact the maintainers to discuss "
137  "extending this limit";
138  }
139  }
140  };
141  parameters_handler_->addPreCallback(name_ + ".time_steps", time_steps_guard);
142 
143  getParam(s.model_dt, "model_dt", 0.05f);
144  getParam(s.model_delay_vx, "model_delay_vx", 0.0f);
145  getParam(s.model_delay_vy, "model_delay_vy", 0.0f);
146  getParam(s.model_delay_wz, "model_delay_wz", 0.0f);
147  getParam(s.clamp_raw_controls, "clamp_raw_controls", false);
148  getParam(s.time_steps, "time_steps", 56);
149  getParam(s.batch_size, "batch_size", 1000);
150  getParam(s.iteration_count, "iteration_count", 1);
151  getParam(s.temperature, "temperature", 0.3f);
152  getParam(s.gamma, "gamma", 0.015f);
153  getParam(s.base_constraints.vx_max, "vx_max", 0.5f);
154  getParam(s.base_constraints.vx_min, "vx_min", -0.35f);
155  getParam(s.base_constraints.vy, "vy_max", 0.5f);
156  getParam(s.base_constraints.wz, "wz_max", 1.9f);
157  getParam(s.base_constraints.ax_max, "ax_max", 3.0f);
158  getParam(s.base_constraints.ax_min, "ax_min", -3.0f);
159  getParam(s.base_constraints.ay_max, "ay_max", 3.0f);
160  getParam(s.base_constraints.ay_min, "ay_min", -3.0f);
161  getParam(s.base_constraints.az_max, "az_max", 3.5f);
162  getParam(s.sampling_std.vx, "vx_std", 0.2f);
163  getParam(s.sampling_std.vy, "vy_std", 0.2f);
164  getParam(s.sampling_std.wz, "wz_std", 0.4f);
165  getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
166  getParam(s.open_loop, "open_loop", false);
167  getParam(s.sgf_order, "sgf_order", 2);
168  if (s.sgf_order < 1 || s.sgf_order > 2) {
169  RCLCPP_WARN(logger_, "sgf_order must be 1 or 2, defaulting to 2");
170  s.sgf_order = 2;
171  }
172 
173  s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
174  if (s.base_constraints.ax_min > 0.0) {
175  s.base_constraints.ax_min = -1.0 * s.base_constraints.ax_min;
176  RCLCPP_WARN(
177  logger_,
178  "Sign of the parameter ax_min is incorrect, consider setting it negative.");
179  }
180 
181  if (s.base_constraints.ay_min > 0.0) {
182  s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min;
183  RCLCPP_WARN(
184  logger_,
185  "Sign of the parameter ay_min is incorrect, consider setting it negative.");
186  }
187 
188  getParam(motion_model_name, "motion_model", std::string("diff_drive"));
189 
190  if (s.batch_size == 0u || s.batch_size > 100000u) {
191  RCLCPP_WARN(
192  logger_, "batch_size must be in [1, 100000], defaulting to 1000. "
193  "If you need a larger value, contact the maintainers to discuss extending this limit");
194  s.batch_size = 1000u;
195  }
196  if (s.time_steps == 0u || s.time_steps > 1000u) {
197  RCLCPP_WARN(
198  logger_, "time_steps must be in [1, 1000], defaulting to 56. "
199  "If you need a larger value, contact the maintainers to discuss extending this limit");
200  s.time_steps = 56u;
201  }
202 
203  s.constraints = s.base_constraints;
204 
205  setMotionModel(motion_model_name);
206  parameters_handler_->addPostCallback([this]() {reset();});
207 
208  double controller_frequency;
209  getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static);
210  s.controller_period = static_cast<float>(1.0 / controller_frequency);
211  setOffset(s.controller_period);
212 }
213 
214 void Optimizer::setOffset(double controller_period)
215 {
216  constexpr double eps = 1e-6;
217 
218  if ((controller_period + eps) < settings_.model_dt) {
219  RCLCPP_WARN(
220  logger_,
221  "Controller period is less then model dt, consider setting it equal");
222  } else if (abs(controller_period - settings_.model_dt) < eps) {
223  RCLCPP_INFO(
224  logger_,
225  "Controller period is equal to model dt. Control sequence "
226  "shifting is ON");
227  settings_.shift_control_sequence = true;
228  } else {
230  "Controller period more then model dt, set it equal to model dt");
231  }
232 }
233 
234 void Optimizer::reset(bool reset_dynamic_speed_limits)
235 {
236  state_.reset(settings_.batch_size, settings_.time_steps);
237  control_sequence_.reset(settings_.time_steps);
238  control_history_[0] = {0.0f, 0.0f, 0.0f};
239  control_history_[1] = {0.0f, 0.0f, 0.0f};
240  control_history_[2] = {0.0f, 0.0f, 0.0f};
241  control_history_[3] = {0.0f, 0.0f, 0.0f};
242 
243  last_command_vel_ = geometry_msgs::msg::Twist();
244 
245  if (reset_dynamic_speed_limits) {
246  settings_.constraints = settings_.base_constraints;
247  }
248 
249  costs_.setZero(settings_.batch_size);
250  generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
251 
252  noise_generator_.reset(settings_, isHolonomic());
253  motion_model_->setConstraints(settings_.constraints, settings_.model_dt,
254  settings_.model_delay_vx, settings_.model_delay_vy, settings_.model_delay_wz,
255  settings_.clamp_raw_controls);
256  motion_model_->clearCommandHistory();
257  trajectory_validator_->initialize(
258  parent_, name_ + ".TrajectoryValidator",
259  costmap_ros_, parameters_handler_, tf_buffer_, settings_);
260 
261  RCLCPP_INFO(logger_, "Optimizer reset");
262 }
263 
265 {
266  return motion_model_->isHolonomic();
267 }
268 
270 {
271  // Speed limit is active when current constraints differ from base constraints.
272  // This occurs when setSpeedLimit() has modified the velocity/acceleration limits.
273  const auto & base = settings_.base_constraints;
274  const auto & curr = settings_.constraints;
275  return base.vx_max != curr.vx_max ||
276  base.vx_min != curr.vx_min ||
277  base.vy != curr.vy ||
278  base.wz != curr.wz;
279 }
280 
281 std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalControl(
282  const geometry_msgs::msg::PoseStamped & robot_pose,
283  const geometry_msgs::msg::Twist & robot_speed,
284  const nav_msgs::msg::Path & plan,
285  const geometry_msgs::msg::Pose & goal,
286  nav2_core::GoalChecker * goal_checker)
287 {
288  prepare(robot_pose, robot_speed, plan, goal, goal_checker);
289  Eigen::ArrayXXf optimal_trajectory;
290  bool trajectory_valid = true;
291 
292  do {
293  optimize();
294  optimal_trajectory = getOptimizedTrajectory();
295  switch (trajectory_validator_->validateTrajectory(
296  optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal))
297  {
298  case mppi::ValidationResult::SOFT_RESET:
299  trajectory_valid = false;
300  RCLCPP_WARN(logger_, "Soft reset triggered by trajectory validator");
301  break;
302  case mppi::ValidationResult::FAILURE:
304  "Trajectory validator failed to validate trajectory, hard reset triggered.");
305  case mppi::ValidationResult::SUCCESS:
306  default:
307  trajectory_valid = true;
308  break;
309  }
310  } while (fallback(critics_data_.fail_flag || !trajectory_valid));
311 
312  auto control = getControlFromSequenceAsTwist(plan.header.stamp);
313 
314  last_command_vel_ = control.twist;
315 
316  if (settings_.shift_control_sequence) {
318  }
319 
320  return std::make_tuple(control, optimal_trajectory);
321 }
322 
324 {
325  for (size_t i = 0; i < settings_.iteration_count; ++i) {
326  costs_.setZero();
328  critic_manager_.evalTrajectoriesScores(critics_data_);
330  }
331 }
332 
333 bool Optimizer::fallback(bool fail)
334 {
335  // Keep retry history scoped to this optimizer instance.
336  if (!fail) {
337  fallback_count_ = 0;
338  return false;
339  }
340 
341  reset(false /*Don't reset zone-based speed limits after fallback*/);
342 
343  if (++fallback_count_ > settings_.retry_attempt_limit) {
344  fallback_count_ = 0;
345  throw nav2_core::NoValidControl("Optimizer fail to compute path");
346  }
347 
348  return true;
349 }
350 
352  const geometry_msgs::msg::PoseStamped & robot_pose,
353  const geometry_msgs::msg::Twist & robot_speed,
354  const nav_msgs::msg::Path & plan,
355  const geometry_msgs::msg::Pose & goal,
356  nav2_core::GoalChecker * goal_checker)
357 {
358  state_.pose = robot_pose;
359  if (settings_.open_loop) {
360  state_.speed = last_command_vel_;
361  } else {
362  // Predict state one controller_period forward toward the last command to compensate
363  // for the latency between measurement and when this command will take effect.
364  // Clamp to physically achievable range so prediction never exceeds dynamics.
365  const auto & c = settings_.constraints;
366  const double dt = settings_.controller_period;
367  float max_delta_vx = dt * c.ax_max;
368  float min_delta_vx = dt * c.ax_min;
369  float max_delta_wz = dt * c.az_max;
370  state_.speed = robot_speed;
371  state_.speed.linear.x = utils::clampVelocityByAccel(
372  robot_speed.linear.x, last_command_vel_.linear.x, min_delta_vx, max_delta_vx);
373  state_.speed.angular.z = utils::clampVelocityByAccel(
374  robot_speed.angular.z, last_command_vel_.angular.z, -max_delta_wz, max_delta_wz);
375  if (isHolonomic()) {
376  float max_delta_vy = dt * c.ay_max;
377  float min_delta_vy = dt * c.ay_min;
378  state_.speed.linear.y = utils::clampVelocityByAccel(
379  robot_speed.linear.y, last_command_vel_.linear.y, min_delta_vy, max_delta_vy);
380  }
381  // Predict the robot pose at dt in future
382  motion_model_->predictPose(state_.pose.pose, robot_speed, dt);
383  }
384  state_.local_path_length = nav2_util::geometry_utils::calculate_path_length(plan);
385  path_ = utils::toTensor(plan);
386  costs_.setZero(settings_.batch_size);
387  goal_ = goal;
388  critics_data_.fail_flag = false;
389  critics_data_.goal_checker = goal_checker;
390  critics_data_.motion_model = motion_model_;
391  critics_data_.furthest_reached_path_point.reset();
392  critics_data_.path_pts_valid.reset();
393 }
394 
396 {
397  auto size = control_sequence_.vx.size();
398  utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);
399  utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);
400  control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
401  control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);
402 
403  if (isHolonomic()) {
404  utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
405  control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
406  }
407 }
408 
410 {
412  noise_generator_.setNoisedControls(state_, control_sequence_);
413  noise_generator_.generateNextNoises();
414  updateStateVelocities(state_);
415  integrateStateVelocities(generated_trajectories_, state_);
416 }
417 
419 {
420  // Enforce t=0 to be dynamically feasible from the current speed for inter-iteration feasibility
421  // Re-centers the distribution at t=0, but still applied in a information theoretic sound way
422  auto & s = settings_;
423  float first_dt = s.controller_period;
424  float max_delta_vx = first_dt * s.constraints.ax_max;
425  float min_delta_vx = first_dt * s.constraints.ax_min;
426  float max_delta_vy = first_dt * s.constraints.ay_max;
427  float min_delta_vy = first_dt * s.constraints.ay_min;
428  float max_delta_wz = first_dt * s.constraints.az_max;
429 
430  float speed_vx = static_cast<float>(state_.speed.linear.x);
431  float speed_wz = static_cast<float>(state_.speed.angular.z);
432  if (s.shift_control_sequence) {
433  // When shifting, vx(0) is not sent and represents 'now'
434  // so that vx(1), the sent command, needs to be only one step away.
435  control_sequence_.vx(0) = speed_vx;
436  control_sequence_.wz(0) = speed_wz;
437  if (isHolonomic()) {
438  control_sequence_.vy(0) = static_cast<float>(state_.speed.linear.y);
439  }
440  } else {
441  // When not shifting, vx(0) is the sent command, clamp to the feasible envelope
442  control_sequence_.vx(0) = utils::clampVelocityByAccel(
443  speed_vx, control_sequence_.vx(0), min_delta_vx, max_delta_vx);
444  control_sequence_.wz(0) = utils::clampVelocityByAccel(
445  speed_wz, control_sequence_.wz(0), -max_delta_wz, max_delta_wz);
446  if (isHolonomic()) {
447  float speed_vy = static_cast<float>(state_.speed.linear.y);
448  control_sequence_.vy(0) = utils::clampVelocityByAccel(
449  speed_vy, control_sequence_.vy(0), min_delta_vy, max_delta_vy);
450  }
451  }
452 }
453 
455 {
456  auto & s = settings_;
457 
458  // Apply constraints to set the optimal control sequence within bounds
459  motion_model_->applyConstraints(control_sequence_);
460 
461  // Use controller_period for t=0 to realistically model physical limits
462  float first_dt = s.controller_period;
463  float max_delta_vx = first_dt * s.constraints.ax_max;
464  float min_delta_vx = first_dt * s.constraints.ax_min;
465  float max_delta_vy = first_dt * s.constraints.ay_max;
466  float min_delta_vy = first_dt * s.constraints.ay_min;
467  float max_delta_wz = first_dt * s.constraints.az_max;
468 
469  // Initialize as the current speed to create inter-iteration dynamic feasibility
470  float vx_last = static_cast<float>(state_.speed.linear.x);
471  float wz_last = static_cast<float>(state_.speed.angular.z);
472  float vy_last = isHolonomic() ? static_cast<float>(state_.speed.linear.y) : 0.0f;
473 
474  // When shifting, vx(0) is "now" and not sent. Pin it so vx(1), the sent command,
475  // is exactly one constraint step from current speed when shift_control_sequence
476  if (s.shift_control_sequence) {
477  control_sequence_.vx(0) = vx_last;
478  control_sequence_.wz(0) = wz_last;
479  if (isHolonomic()) {
480  control_sequence_.vy(0) = vy_last;
481  }
482  }
483 
484  for (unsigned int i = 0; i != control_sequence_.vx.size(); i++) {
485  // After first timestep, switch to MPC model_dt for intra-iteration feasibility
486  if (i == 1) {
487  max_delta_vx = s.model_dt * s.constraints.ax_max;
488  min_delta_vx = s.model_dt * s.constraints.ax_min;
489  max_delta_vy = s.model_dt * s.constraints.ay_max;
490  min_delta_vy = s.model_dt * s.constraints.ay_min;
491  max_delta_wz = s.model_dt * s.constraints.az_max;
492  }
493 
494  float & vx_curr = control_sequence_.vx(i);
495  vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
496  vx_curr = utils::clampVelocityByAccel(vx_last, vx_curr, min_delta_vx, max_delta_vx);
497  vx_last = vx_curr;
498 
499  float & wz_curr = control_sequence_.wz(i);
500  wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr);
501  wz_curr = utils::clampVelocityByAccel(wz_last, wz_curr, -max_delta_wz, max_delta_wz);
502  wz_last = wz_curr;
503 
504  if (isHolonomic()) {
505  float & vy_curr = control_sequence_.vy(i);
506  vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
507  vy_curr = utils::clampVelocityByAccel(vy_last, vy_curr, min_delta_vy, max_delta_vy);
508  vy_last = vy_curr;
509  }
510  }
511 
512  // Apply again to ensure accel constraints don't violate specialty limits
513  motion_model_->applyConstraints(control_sequence_);
514 }
515 
517  models::State & state) const
518 {
521 }
522 
524 {
525  state.vx.col(0) = static_cast<float>(state.speed.linear.x);
526  state.wz.col(0) = static_cast<float>(state.speed.angular.z);
527 
528  if (isHolonomic()) {
529  state.vy.col(0) = static_cast<float>(state.speed.linear.y);
530  }
531 }
532 
534  models::State & state) const
535 {
536  motion_model_->predict(state);
537 }
538 
540  Eigen::Array<float, Eigen::Dynamic, 3> & trajectory,
541  const Eigen::ArrayXXf & sequence) const
542 {
543  float initial_yaw = static_cast<float>(tf2::getYaw(state_.pose.pose.orientation));
544 
545  const auto vx = sequence.col(0);
546  const auto wz = sequence.col(1);
547 
548  auto traj_x = trajectory.col(0);
549  auto traj_y = trajectory.col(1);
550  auto traj_yaws = trajectory.col(2);
551 
552  const size_t n_size = traj_yaws.size();
553  if (n_size == 0) {
554  return;
555  }
556 
557  float last_yaw = initial_yaw;
558  for (size_t i = 0; i != n_size; i++) {
559  last_yaw += wz(i) * settings_.model_dt;
560  traj_yaws(i) = last_yaw;
561  }
562 
563  Eigen::ArrayXf yaw_cos = traj_yaws.cos();
564  Eigen::ArrayXf yaw_sin = traj_yaws.sin();
565  utils::shiftColumnsByOnePlace(yaw_cos, 1);
566  utils::shiftColumnsByOnePlace(yaw_sin, 1);
567  yaw_cos(0) = cosf(initial_yaw);
568  yaw_sin(0) = sinf(initial_yaw);
569 
570  auto dx = (vx * yaw_cos).eval();
571  auto dy = (vx * yaw_sin).eval();
572 
573  if (isHolonomic()) {
574  auto vy = sequence.col(2);
575  dx = (dx - vy * yaw_sin).eval();
576  dy = (dy + vy * yaw_cos).eval();
577  }
578 
579  float last_x = state_.pose.pose.position.x;
580  float last_y = state_.pose.pose.position.y;
581  for (size_t i = 0; i != n_size; i++) {
582  last_x += dx(i) * settings_.model_dt;
583  last_y += dy(i) * settings_.model_dt;
584  traj_x(i) = last_x;
585  traj_y(i) = last_y;
586  }
587 }
588 
590  models::Trajectories & trajectories,
591  const models::State & state) const
592 {
593  auto initial_yaw = static_cast<float>(tf2::getYaw(state.pose.pose.orientation));
594  const size_t n_cols = trajectories.yaws.cols();
595 
596  Eigen::ArrayXf last_yaws = Eigen::ArrayXf::Constant(trajectories.yaws.rows(), initial_yaw);
597  for (size_t i = 0; i != n_cols; i++) {
598  last_yaws += state.wz.col(i) * settings_.model_dt;
599  trajectories.yaws.col(i) = last_yaws;
600  }
601 
602  Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos();
603  Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin();
604  utils::shiftColumnsByOnePlace(yaw_cos, 1);
605  utils::shiftColumnsByOnePlace(yaw_sin, 1);
606  yaw_cos.col(0) = cosf(initial_yaw);
607  yaw_sin.col(0) = sinf(initial_yaw);
608 
609  auto dx = (state.vx * yaw_cos).eval();
610  auto dy = (state.vx * yaw_sin).eval();
611 
612  if (isHolonomic()) {
613  dx -= state.vy * yaw_sin;
614  dy += state.vy * yaw_cos;
615  }
616 
617  Eigen::ArrayXf last_x = Eigen::ArrayXf::Constant(
618  trajectories.x.rows(),
619  state.pose.pose.position.x);
620  Eigen::ArrayXf last_y = Eigen::ArrayXf::Constant(
621  trajectories.y.rows(),
622  state.pose.pose.position.y);
623 
624  for (size_t i = 0; i != n_cols; i++) {
625  last_x += dx.col(i) * settings_.model_dt;
626  last_y += dy.col(i) * settings_.model_dt;
627  trajectories.x.col(i) = last_x;
628  trajectories.y.col(i) = last_y;
629  }
630 }
631 
633 {
634  const bool is_holo = isHolonomic();
635  Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2);
636  Eigen::Array<float, Eigen::Dynamic, 3> trajectories =
637  Eigen::Array<float, Eigen::Dynamic, 3>(settings_.time_steps, 3);
638 
639  sequence.col(0) = control_sequence_.vx;
640  sequence.col(1) = control_sequence_.wz;
641 
642  if (is_holo) {
643  sequence.col(2) = control_sequence_.vy;
644  }
645 
646  integrateStateVelocities(trajectories, sequence);
647  return trajectories;
648 }
649 
651 {
652  return control_sequence_;
653 }
654 
656 {
657  const bool is_holo = isHolonomic();
658  auto & s = settings_;
659 
660  auto vx_T = control_sequence_.vx.transpose();
661  auto bounded_noises_vx = state_.cvx.rowwise() - vx_T;
662  const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx);
663  costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
664 
665  if (s.sampling_std.wz > 0.0f) {
666  auto wz_T = control_sequence_.wz.transpose();
667  auto bounded_noises_wz = state_.cwz.rowwise() - wz_T;
668  const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz);
669  costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
670  }
671 
672  if (is_holo) {
673  auto vy_T = control_sequence_.vy.transpose();
674  auto bounded_noises_vy = state_.cvy.rowwise() - vy_T;
675  const float gamma_vy = s.gamma / (s.sampling_std.vy * s.sampling_std.vy);
676  costs_ += (gamma_vy * (bounded_noises_vy.rowwise() * vy_T).rowwise().sum()).eval();
677  }
678 
679  auto costs_normalized = costs_ - costs_.minCoeff();
680  const float inv_temp = 1.0f / s.temperature;
681  auto softmaxes = (-inv_temp * costs_normalized).exp().eval();
682  softmaxes /= softmaxes.sum();
683 
684  auto softmax_mat = softmaxes.matrix();
685  control_sequence_.vx = state_.cvx.transpose().matrix() * softmax_mat;
686  control_sequence_.wz = state_.cwz.transpose().matrix() * softmax_mat;
687 
688  if (is_holo) {
689  control_sequence_.vy = state_.cvy.transpose().matrix() * softmax_mat;
690  }
691 
692  utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
693 
695 }
696 
697 geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
698  const builtin_interfaces::msg::Time & stamp)
699 {
700  unsigned int offset = settings_.shift_control_sequence ? 1 : 0;
701 
702  auto vx = control_sequence_.vx(offset);
703  auto wz = control_sequence_.wz(offset);
704  auto vy = isHolonomic() ? control_sequence_.vy(offset) : 0.0f;
705 
706  // Update the command history for the motion model's latency compensation mechanism
707  motion_model_->pushCommandHistory(vx, vy, wz);
708 
709  if (isHolonomic()) {
710  return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());
711  }
712 
713  return utils::toTwistStamped(vx, wz, stamp, costmap_ros_->getBaseFrameID());
714 }
715 
716 void Optimizer::setMotionModel(const std::string & motion_model_name)
717 {
718  auto node = parent_.lock();
719  const std::string plugin_ns = name_ + "." + motion_model_name;
720  std::string plugin_type;
721  motion_model_loader_ =
722  std::make_unique<pluginlib::ClassLoader<MotionModel>>(
723  "nav2_mppi_controller", "mppi::MotionModel");
724 
725  try {
726  plugin_type = nav2::get_plugin_type_param(node, plugin_ns);
727  motion_model_ = motion_model_loader_->createSharedInstance(plugin_type);
728  motion_model_->initialize(parameters_handler_, plugin_ns);
729  motion_model_->setConstraints(settings_.constraints, settings_.model_dt,
730  settings_.model_delay_vx, settings_.model_delay_vy, settings_.model_delay_wz,
731  settings_.clamp_raw_controls);
732  } catch (const pluginlib::PluginlibException & ex) {
734  std::string("Failed to load motion model plugin '") + motion_model_name +
735  "': " + ex.what());
736  }
737 
738  RCLCPP_INFO(logger_, "Loaded motion model plugin: %s", plugin_type.c_str());
739 }
740 
741 void Optimizer::setSpeedLimit(double speed_limit, bool percentage)
742 {
743  auto & s = settings_;
744  if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
745  s.constraints.vx_max = s.base_constraints.vx_max;
746  s.constraints.vx_min = s.base_constraints.vx_min;
747  s.constraints.vy = s.base_constraints.vy;
748  s.constraints.wz = s.base_constraints.wz;
749  } else {
750  if (percentage) {
751  // Speed limit is expressed in % from maximum speed of robot
752  double ratio = speed_limit / 100.0;
753  s.constraints.vx_max = s.base_constraints.vx_max * ratio;
754  s.constraints.vx_min = s.base_constraints.vx_min * ratio;
755  s.constraints.vy = s.base_constraints.vy * ratio;
756  s.constraints.wz = s.base_constraints.wz * ratio;
757  } else {
758  // Speed limit is expressed in absolute value
759  double ratio = speed_limit / s.base_constraints.vx_max;
760  s.constraints.vx_max = s.base_constraints.vx_max * ratio;
761  s.constraints.vx_min = s.base_constraints.vx_min * ratio;
762  s.constraints.vy = s.base_constraints.vy * ratio;
763  s.constraints.wz = s.base_constraints.wz * ratio;
764  }
765  }
766  motion_model_->setConstraints(settings_.constraints, settings_.model_dt,
767  settings_.model_delay_vx, settings_.model_delay_vy, settings_.model_delay_wz,
768  settings_.clamp_raw_controls);
769 }
770 
772 {
773  return generated_trajectories_;
774 }
775 
776 } // namespace mppi
void on_configure(nav2::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr< nav2_costmap_2d::Costmap2DROS >, ParametersHandler *)
Configure critic manager on bringup and load plugins.
void evalTrajectoriesScores(CriticData &data)
Score trajectories by the set of loaded critic functions.
void reset(mppi::models::OptimizerSettings &settings, bool is_holonomic)
Reset noise generator with settings and model types.
void initialize(mppi::models::OptimizerSettings &settings, bool is_holonomic, const std::string &name, ParametersHandler *param_handler)
Initialize noise generator with settings and model types.
void setNoisedControls(models::State &state, const models::ControlSequence &control_sequence)
set noised control_sequence to state controls
void shutdown()
Shutdown noise generator thread.
void generateNextNoises()
Signal to the noise thread the controller is ready to generate a new noised control for the next iter...
void updateStateVelocities(models::State &state) const
Update velocities in state.
Definition: optimizer.cpp:516
const models::ControlSequence & getOptimalControlSequence()
Get the optimal control sequence for a cycle for visualization.
Definition: optimizer.cpp:650
void setMotionModel(const std::string &model)
Set the motion model of the vehicle platform.
Definition: optimizer.cpp:716
Eigen::ArrayXXf getOptimizedTrajectory()
Get the optimal trajectory for a cycle for visualization.
Definition: optimizer.cpp:632
void reset(bool reset_dynamic_speed_limits=true)
Reset the optimization problem to initial conditions.
Definition: optimizer.cpp:234
rclcpp::Logger logger_
Caution, keep references.
Definition: optimizer.hpp:331
void prepare(const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::Twist &robot_speed, const nav_msgs::msg::Path &plan, const geometry_msgs::msg::Pose &goal, nav2_core::GoalChecker *goal_checker)
Prepare state information on new request for trajectory rollouts.
Definition: optimizer.cpp:351
std::tuple< geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf > evalControl(const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::Twist &robot_speed, const nav_msgs::msg::Path &plan, const geometry_msgs::msg::Pose &goal, nav2_core::GoalChecker *goal_checker)
Compute control using MPPI algorithm.
Definition: optimizer.cpp:281
void integrateStateVelocities(models::Trajectories &trajectories, const models::State &state) const
Rollout velocities in state to poses.
Definition: optimizer.cpp:589
void applyControlSequenceInterIterationConstraints()
Apply inter-iteration dynamic feasibility constraints on the first control sequence element before no...
Definition: optimizer.cpp:418
void updateControlSequence()
Update control sequence with state controls weighted by costs using softmax function.
Definition: optimizer.cpp:655
bool isSpeedLimitActive() const
Check if a dynamic speed limit is currently active.
Definition: optimizer.cpp:269
void generateNoisedTrajectories()
updates generated trajectories with noised trajectories from the last cycle's optimal control
Definition: optimizer.cpp:409
bool fallback(bool fail)
Perform fallback behavior to try to recover from a set of trajectories in collision.
Definition: optimizer.cpp:333
bool isHolonomic() const
Whether the motion model is holonomic.
Definition: optimizer.cpp:264
models::Trajectories & getGeneratedTrajectories()
Get the trajectories generated in a cycle for visualization.
Definition: optimizer.cpp:771
void updateInitialStateVelocities(models::State &state) const
Update initial velocity in state.
Definition: optimizer.cpp:523
void shutdown()
Shutdown for optimizer at process end.
Definition: optimizer.cpp:72
void optimize()
Main function to generate, score, and return trajectories.
Definition: optimizer.cpp:323
void setOffset(double controller_period)
Using control period and time step size, determine if trajectory offset should be used to populate in...
Definition: optimizer.cpp:214
void setSpeedLimit(double speed_limit, bool percentage)
Set the maximum speed based on the speed limits callback.
Definition: optimizer.cpp:741
void shiftControlSequence()
Shift the optimal control sequence after processing for next iterations initial conditions after exec...
Definition: optimizer.cpp:395
void applyControlSequenceConstraints()
Apply hard vehicle constraints on control sequence.
Definition: optimizer.cpp:454
void getParams()
Obtain the main controller's parameters.
Definition: optimizer.cpp:77
void propagateStateVelocitiesFromInitials(models::State &state) const
predict velocities in state using model for time horizon equal to timesteps
Definition: optimizer.cpp:533
geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time &stamp)
Convert control sequence to a twist command.
Definition: optimizer.cpp:697
void initialize(nav2::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr< nav2_costmap_2d::Costmap2DROS > costmap_ros, nav2::TransformBuffer::SharedPtr tf_buffer, ParametersHandler *dynamic_parameters_handler)
Initializes optimizer on startup.
Definition: optimizer.cpp:34
Handles getting parameters and dynamic parameter changes.
void addPostCallback(T &&callback)
Set a callback to process after parameter changes.
void addPreCallback(const std::string &name, T &&callback)
Set a callback to process before parameter changes.
auto getParamGetter(const std::string &ns)
Get an object to retrieve parameters.
Function-object for checking whether a goal has been reached.
A control sequence over time (e.g. trajectory)
State information: velocities, controls, poses, speed.
Definition: state.hpp:32
void reset(unsigned int batch_size, unsigned int time_steps)
Reset state data.
Definition: state.hpp:48
Candidate Trajectories.
void reset(unsigned int batch_size, unsigned int time_steps)
Reset state data.