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

xt::xtensor< float, 2 >	getOptimizedTrajectory ()
	Get the optimal trajectory 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 ()
	Reset the optimization problem to initial conditions.

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 (xt::xtensor< float, 2 > &trajectories, const xt::xtensor< float, 2 > &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 commant. More...

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

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

xt::xtensor< float, 1 >	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_checker	Object to check if goal is completed

Returns: TwistStamped of the MPPI control

Definition at line 134 of file optimizer.cpp.

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

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 166 of file optimizer.cpp.

◆ getControlFromSequenceAsTwist()

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

protected

Convert control sequence to a twist commant.

Parameters

stamp Timestamp to use

Returns: TwistStamped of command to send to robot base

Definition at line 396 of file optimizer.cpp.

◆ 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 455 of file optimizer.cpp.

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

Here is the caller graph for this function:

◆ getOptimizedTrajectory()

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

Get the optimal trajectory for a cycle for visualization.

Returns: Optimal trajectory

Definition at line 345 of file optimizer.cpp.

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

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 35 of file optimizer.cpp.

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

Here is the caller graph for this function:

◆ integrateStateVelocities() [1/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 313 of file optimizer.cpp.

◆ integrateStateVelocities() [2/2]

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

protected

Rollout velocities in state to poses.

Parameters

trajectories	to rollout
state	fill state

Definition at line 275 of file optimizer.cpp.

◆ 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 235 of file optimizer.cpp.

◆ 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 185 of file optimizer.cpp.

◆ 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 269 of file optimizer.cpp.

◆ 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 412 of file optimizer.cpp.

◆ 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 428 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 258 of file optimizer.cpp.

◆ 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 251 of file optimizer.cpp.

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 257 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()

◆ getOptimizedTrajectory()

◆ initialize()

◆ integrateStateVelocities() [1/2]

◆ integrateStateVelocities() [2/2]

◆ isHolonomic()

◆ prepare()

◆ propagateStateVelocitiesFromInitials()

◆ setMotionModel()

◆ setSpeedLimit()

◆ updateInitialStateVelocities()

◆ updateStateVelocities()

Member Data Documentation

◆ critics_data_