Class Optimizer

Class Documentation

class Optimizer

Main algorithm optimizer of the MPPI Controller.

Public Functions

Optimizer() = default

Constructor for mppi::Optimizer.

inline ~Optimizer()

Destructor for mppi::Optimizer.

void initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string &name, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros, ParametersHandler *dynamic_parameters_handler)

Initializes optimizer on startup.

Parameters:

  • parent – WeakPtr to node

  • name – Name of plugin

  • costmap_ros – Costmap2DROS object of environment

  • dynamic_parameter_handler – Parameter handler object

void shutdown()

Shutdown for optimizer at process end.

geometry_msgs::msg::TwistStamped evalControl(const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::Twist &robot_speed, const nav_msgs::msg::Path &plan, nav2_core::GoalChecker *goal_checker)

Compute control using MPPI algorithm.

Parameters:

  • robot_pose – Pose of the robot at given time

  • robot_speed – Speed of the robot at given time

  • plan – Path plan to track

  • goal_checker – Object to check if goal is completed

Returns:

TwistStamped of the MPPI control

models::Trajectories &getGeneratedTrajectories()

Get the trajectories generated in a cycle for visualization.

Returns:

Set of trajectories evaluated in cycle

xt::xtensor<float, 2> getOptimizedTrajectory()

Get the optimal trajectory for a cycle for visualization.

Returns:

Optimal trajectory

void setSpeedLimit(double speed_limit, bool percentage)

Set the maximum speed based on the speed limits callback.

Parameters:

  • speed_limit – Limit of the speed for use

  • percentage – Whether the speed limit is absolute or relative

void reset()

Reset the optimization problem to initial conditions.

Protected Functions

void optimize()

Main function to generate, score, and return trajectories.

void prepare(const geometry_msgs::msg::PoseStamped &robot_pose, const geometry_msgs::msg::Twist &robot_speed, const nav_msgs::msg::Path &plan, nav2_core::GoalChecker *goal_checker)

Prepare state information on new request for trajectory rollouts.

Parameters:

  • robot_pose – Pose of the robot at given time

  • robot_speed – Speed of the robot at given time

  • plan – Path plan to track

  • goal_checker – Object to check if goal is completed

void getParams()

Obtain the main controller’s parameters.

void setMotionModel(const std::string &model)

Set the motion model of the vehicle platform.

Parameters:

model – Model string to use

void shiftControlSequence()

Shift the optimal control sequence after processing for next iterations initial conditions after execution.

void generateNoisedTrajectories()

updates generated trajectories with noised trajectories from the last cycle’s optimal control

void applyControlSequenceConstraints()

Apply hard vehicle constraints on control sequence.

void updateStateVelocities(models::State &state) const

Update velocities in state.

Parameters:

state – fill state with velocities on each step

void updateInitialStateVelocities(models::State &state) const

Update initial velocity in state.

Parameters:

state – fill state

void propagateStateVelocitiesFromInitials(models::State &state) const

predict velocities in state using model for time horizon equal to timesteps

Parameters:

state – fill state

void integrateStateVelocities(models::Trajectories &trajectories, const models::State &state) const

Rollout velocities in state to poses.

Parameters:

  • trajectories – to rollout

  • state – fill state

void integrateStateVelocities(xt::xtensor<float, 2> &trajectories, const xt::xtensor<float, 2> &state) const

Rollout velocities in state to poses.

Parameters:

  • trajectories – to rollout

  • state – fill state

void updateControlSequence()

Update control sequence with state controls weighted by costs using softmax function.

geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time &stamp)

Convert control sequence to a twist commant.

Parameters:

stamp – Timestamp to use

Returns:

TwistStamped of command to send to robot base

bool isHolonomic() const

Whether the motion model is holonomic.

Returns:

Bool if holonomic to populate y axis of state

void setOffset(double controller_frequency)

Using control frequence and time step size, determine if trajectory offset should be used to populate initial state of the next cycle.

bool fallback(bool fail)

Perform fallback behavior to try to recover from a set of trajectories in collision.

Parameters:

fail – Whether the system failed to recover from

Protected Attributes

rclcpp_lifecycle::LifecycleNode::WeakPtr parent_
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_
nav2_costmap_2d::Costmap2D *costmap_
std::string name_
std::shared_ptr<MotionModel> motion_model_
ParametersHandler *parameters_handler_
CriticManager critic_manager_
NoiseGenerator noise_generator_
models::OptimizerSettings settings_
models::State state_
models::ControlSequence control_sequence_
std::array<mppi::models::Control, 4> control_history_
models::Trajectories generated_trajectories_
models::Path path_
xt::xtensor<float, 1> costs_
CriticData critics_data_ = {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}
rclcpp::Logger logger_ = {rclcpp::get_logger("MPPIController")}

Caution, keep references.