Main algorithm optimizer of the MPPI Controller. More...

#include <nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp>

Collaboration diagram for mppi::Optimizer:

[legend]

Public Member Functions
	Optimizer ()=default
	Constructor for mppi::Optimizer.

	~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. More...

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, const geometry_msgs::msg::Pose &goal, nav2_core::GoalChecker *goal_checker)
	Compute control using MPPI algorithm. More...

models::Trajectories &	getGeneratedTrajectories ()
	Get the trajectories generated in a cycle for visualization. More...

Eigen::ArrayXXf	getOptimizedTrajectory ()
	Get the optimal trajectory for a cycle for visualization. More...

const models::ControlSequence &	getOptimalControlSequence ()
	Get the optimal control sequence for a cycle for visualization. More...

void	setSpeedLimit (double speed_limit, bool percentage)
	Set the maximum speed based on the speed limits callback. More...

void	reset (bool reset_dynamic_speed_limits=true)
	Reset the optimization problem to initial conditions. More...

const models::OptimizerSettings &	getSettings () const
	Get the motion model time step. More...

Protected Member 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, const geometry_msgs::msg::Pose &goal, nav2_core::GoalChecker *goal_checker)
	Prepare state information on new request for trajectory rollouts. More...

void	getParams ()
	Obtain the main controller's parameters.

void	setMotionModel (const std::string &model)
	Set the motion model of the vehicle platform. More...

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. More...

void	updateInitialStateVelocities (models::State &state) const
	Update initial velocity in state. More...

void	propagateStateVelocitiesFromInitials (models::State &state) const
	predict velocities in state using model for time horizon equal to timesteps More...

void	integrateStateVelocities (models::Trajectories &trajectories, const models::State &state) const
	Rollout velocities in state to poses. More...

void	integrateStateVelocities (Eigen::Array< float, Eigen::Dynamic, 3 > &trajectories, const Eigen::ArrayXXf &state) const
	Rollout velocities in state to poses. More...

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 command. More...

bool	isHolonomic () const
	Whether the motion model is holonomic. More...

void	setOffset (double controller_frequency)
	Using control frequencies 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. More...

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_

geometry_msgs::msg::Pose	goal_

Eigen::ArrayXf	costs_

CriticData	critics_data_

rclcpp::Logger	logger_ {rclcpp::get_logger("MPPIController")}
	Caution, keep references.

Detailed Description

Main algorithm optimizer of the MPPI Controller.

Definition at line 51 of file optimizer.hpp.

Member Function Documentation

◆ evalControl()

geometry_msgs::msg::TwistStamped mppi::Optimizer::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.

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	Given Goal pose to reach.
goal_checker	Object to check if goal is completed

Returns: TwistStamped of the MPPI control

Definition at line 162 of file optimizer.cpp.

References fallback(), getControlFromSequenceAsTwist(), optimize(), prepare(), and shiftControlSequence().

Referenced by nav2_mppi_controller::MPPIController::computeVelocityCommands().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fallback()

bool mppi::Optimizer::fallback ( bool fail )

protected

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

Parameters

fail	Whether the system failed to recover from

Definition at line 194 of file optimizer.cpp.

References reset().

Referenced by evalControl().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getControlFromSequenceAsTwist()

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

protected

Convert control sequence to a twist command.

Parameters

stamp Timestamp to use

Returns: TwistStamped of command to send to robot base

Definition at line 482 of file optimizer.cpp.

References isHolonomic().

Referenced by evalControl().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getGeneratedTrajectories()

models::Trajectories & mppi::Optimizer::getGeneratedTrajectories ( )

Get the trajectories generated in a cycle for visualization.

Returns: Set of trajectories evaluated in cycle

Definition at line 542 of file optimizer.cpp.

Referenced by nav2_mppi_controller::MPPIController::visualize().

Here is the caller graph for this function:

◆ getOptimalControlSequence()

const models::ControlSequence & mppi::Optimizer::getOptimalControlSequence ( )

Get the optimal control sequence for a cycle for visualization.

Returns: Optimal control sequence

Definition at line 437 of file optimizer.cpp.

Referenced by nav2_mppi_controller::MPPIController::computeVelocityCommands().

Here is the caller graph for this function:

◆ getOptimizedTrajectory()

Eigen::ArrayXXf mppi::Optimizer::getOptimizedTrajectory ( )

Get the optimal trajectory for a cycle for visualization.

Returns: Optimal trajectory

Definition at line 419 of file optimizer.cpp.

References integrateStateVelocities(), and isHolonomic().

Referenced by nav2_mppi_controller::MPPIController::computeVelocityCommands(), and nav2_mppi_controller::MPPIController::visualize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSettings()

const models::OptimizerSettings& mppi::Optimizer::getSettings ( ) const

inline

Get the motion model time step.

Returns: Time step of the model

Definition at line 131 of file optimizer.hpp.

Referenced by nav2_mppi_controller::MPPIController::computeVelocityCommands().

Here is the caller graph for this function:

◆ initialize()

void mppi::Optimizer::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

Definition at line 32 of file optimizer.cpp.

References getParams(), mppi::NoiseGenerator::initialize(), isHolonomic(), logger_, mppi::CriticManager::on_configure(), and reset().

Referenced by nav2_mppi_controller::MPPIController::configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ integrateStateVelocities() [1/2]

void mppi::Optimizer::integrateStateVelocities	(	Eigen::Array< float, Eigen::Dynamic, 3 > &	trajectories,
		const Eigen::ArrayXXf &	state
	)		const

protected

Rollout velocities in state to poses.

Parameters

trajectories	to rollout
state	fill state

Definition at line 328 of file optimizer.cpp.

References isHolonomic().

Here is the call graph for this function:

◆ integrateStateVelocities() [2/2]

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

protected

Rollout velocities in state to poses.

Parameters

trajectories	to rollout
state	fill state

Definition at line 378 of file optimizer.cpp.

References isHolonomic().

Referenced by generateNoisedTrajectories(), and getOptimizedTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isHolonomic()

bool mppi::Optimizer::isHolonomic ( ) const

protected

Whether the motion model is holonomic.

Returns: Bool if holonomic to populate y axis of state

Definition at line 157 of file optimizer.cpp.

Referenced by applyControlSequenceConstraints(), getControlFromSequenceAsTwist(), getOptimizedTrajectory(), initialize(), integrateStateVelocities(), reset(), shiftControlSequence(), updateControlSequence(), and updateInitialStateVelocities().

Here is the caller graph for this function:

◆ prepare()

void mppi::Optimizer::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
	)

protected

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

Definition at line 213 of file optimizer.cpp.

Referenced by evalControl().

Here is the caller graph for this function:

◆ propagateStateVelocitiesFromInitials()

void mppi::Optimizer::propagateStateVelocitiesFromInitials ( models::State & state ) const

protected

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

Parameters

state fill state

Definition at line 322 of file optimizer.cpp.

Referenced by updateStateVelocities().

Here is the caller graph for this function:

◆ reset()

void mppi::Optimizer::reset ( bool reset_dynamic_speed_limits = true )

Reset the optimization problem to initial conditions.

Parameters

Whether to reset the constraints to its base values

Definition at line 135 of file optimizer.cpp.

References isHolonomic(), logger_, mppi::NoiseGenerator::reset(), mppi::models::State::reset(), and mppi::models::Trajectories::reset().

Referenced by fallback(), getParams(), initialize(), and nav2_mppi_controller::MPPIController::reset().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setMotionModel()

void mppi::Optimizer::setMotionModel ( const std::string & model )

protected

Set the motion model of the vehicle platform.

Parameters

model Model string to use

Definition at line 498 of file optimizer.cpp.

Referenced by getParams().

Here is the caller graph for this function:

◆ setSpeedLimit()

void mppi::Optimizer::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

Definition at line 515 of file optimizer.cpp.

Referenced by nav2_mppi_controller::MPPIController::setSpeedLimit().

Here is the caller graph for this function:

◆ updateInitialStateVelocities()

void mppi::Optimizer::updateInitialStateVelocities ( models::State & state ) const

protected

Update initial velocity in state.

Parameters

state fill state

Definition at line 311 of file optimizer.cpp.

References isHolonomic().

Referenced by updateStateVelocities().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateStateVelocities()

void mppi::Optimizer::updateStateVelocities ( models::State & state ) const

protected

Update velocities in state.

Parameters

state fill state with velocities on each step

Definition at line 304 of file optimizer.cpp.

References propagateStateVelocitiesFromInitials(), and updateInitialStateVelocities().

Referenced by generateNoisedTrajectories().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ critics_data_

CriticData mppi::Optimizer::critics_data_

protected

Initial value:

= {
    state_, generated_trajectories_, path_, goal_,
    costs_, settings_.model_dt, false, nullptr, nullptr,
    std::nullopt, std::nullopt}

Definition at line 274 of file optimizer.hpp.

The documentation for this class was generated from the following files:

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
nav2_mppi_controller/src/optimizer.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ evalControl()

◆ fallback()

◆ getControlFromSequenceAsTwist()

◆ getGeneratedTrajectories()

◆ getOptimalControlSequence()

◆ getOptimizedTrajectory()

◆ getSettings()

◆ initialize()

◆ integrateStateVelocities() [1/2]

◆ integrateStateVelocities() [2/2]

◆ isHolonomic()

◆ prepare()

◆ propagateStateVelocitiesFromInitials()

◆ reset()

◆ setMotionModel()

◆ setSpeedLimit()

◆ updateInitialStateVelocities()

◆ updateStateVelocities()

Member Data Documentation

◆ critics_data_