Track a moving pose using MPC. More...

#include <mpc_pose_tracking.hpp>

Inheritance diagram for moveit_pro::behaviors::MPCPoseTracking:

[legend]

Collaboration diagram for moveit_pro::behaviors::MPCPoseTracking:

[legend]

Public Member Functions
	MPCPoseTracking (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< moveit_pro::behaviors::BehaviorContext > &shared_resources)

Public Member Functions inherited from moveit_pro::behaviors::MPCBehaviorBase< MPCPoseTracking >
	MPCBehaviorBase (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< moveit_pro::behaviors::BehaviorContext > &shared_resources)

Public Member Functions inherited from moveit_pro::behaviors::internal::MPCBehaviorBase
	MPCBehaviorBase (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< moveit_pro::behaviors::BehaviorContext > &shared_resources, const std::function< tl::expected< void, std::string >(const BT::TreeNode &)> &set_residual_ports)

	~MPCBehaviorBase () override

tl::expected< void, std::string >	doHalt () override
	Optionally implement additional work needed to cleanly interrupt the async process.

BT::NodeStatus	onStart () final
	Claims the process-wide right to execute MPC, then sets up the problem from the Behavior's ports.

tl::expected< bool, std::string >	doWork () final
	Runs the MPC loop and releases the claim taken by onStart() on every exit path. Final for that reason.

virtual bool	finished ()
	Determines whether the behavior has finished execution.

std::shared_future< tl::expected< bool, std::string > > &	getFuture () override
	Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member.

Public Member Functions inherited from moveit_pro::behaviors::AsyncBehaviorBase
	AsyncBehaviorBase (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< BehaviorContext > &shared_resources)

	~AsyncBehaviorBase () override=default

BT::NodeStatus	onStart () override
	Required implementation of BT::StatefulActionNode::onStart().

BT::NodeStatus	onRunning () override
	Required implementation of BT::StatefulActionNode::onRunning().

void	onHalted () override
	Required implementation of BT::StatefulActionNode::onHalted().

void	resetStatus ()
	Resets the internal status of this node.

void	notifyCanHalt ()
	Called when runAsync() finishes to notify onHalted() that the async process has finished.

Public Member Functions inherited from moveit_pro::behaviors::SharedResourcesNode< BT::StatefulActionNode >
	SharedResourcesNode (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< BehaviorContext > &behavior_context)
	Constructor for SharedResourcesNode. Called by BT::BehaviorTreeFactory when creating a new behavior tree containing this node.

const std::shared_ptr< BehaviorContext > &	getBehaviorContext () const

Static Public Member Functions
static BT::PortsList	providedPorts ()

static auto	getResiduals ()
	Define the residuals used by this MPC behavior.

static BT::KeyValueVector	metadata ()

Static Public Member Functions inherited from moveit_pro::behaviors::MPCBehaviorBase< MPCPoseTracking >
static tl::expected< void, std::string >	setResidualParameter (const BT::TreeNode &tree_node)

static BT::PortsList	providedPorts ()

Static Public Member Functions inherited from moveit_pro::behaviors::internal::MPCBehaviorBase
static BT::PortsList	providedProblemParametersPorts ()
	Provides the list of BT ports required for problem parameters.

static BT::PortsList	providedSolverParametersPorts ()
	Provides the list of BT ports required for solver parameters.

Additional Inherited Members
Public Attributes inherited from moveit_pro::behaviors::internal::MPCBehaviorBase
trajectory_msgs::msg::JointTrajectory	trajectory_msg
	Trajectory message to be populated by MPC.

rclcpp_action::Client< control_msgs::action::FollowJointTrajectory >::SharedPtr	client
	Action client to send trajectories to a joint trajectory controller.

std::shared_ptr< moveit_pro::planning_scene_monitor::PlanningSceneMonitor >	planning_scene_monitor
	Shared pointer to the planning scene monitor used for monitoring and updating the planning scene.

moveit_pro::base::RobotModelConstPtr	robot_model
	Constant pointer to the robot model used for planning and execution.

std::function< tl::expected< void, std::string >(const BT::TreeNode &)>	set_residual_ports
	Callback to set the residual parameters from the BT ports.

Detailed Description

Track a moving pose using MPC.

This behavior uses Model Predictive Control (MPC) to track a target pose in real-time. The behavior optimizes several residuals simultaneous to achieve effective tracking.

Target Pose

The target pose is provided through the target_pose and target_twist input ports. The target pose must be in world frame. The target pose is tracked by a site defined in the MuJoCo model, e.g. end_effector.

Optimization Problem

The optimization problem aims to minimize the following cost: $l(x, u) = \sum_{i=1}^M w_i \cdot \|r_i(x, u)\|$ where $w_i$ is a non-negative weight for each residual functions, and $r_i(x, u)$ is the residual function. The goal of the optimization is to minimize the sum of the weighted residual norms. This site tracking behavior optimizes the following residuals.

Cartesian velocity
Cartesian acceleration:
Joint velocity and acceleration.
Force/torque constraints.
Site tracking errors. For further details on the residual functions, please refer to the documentation of each residual function.

Tuning Tips

Adjust the weights of the residual functions to prioritize specific constraints.
As a starting point, the cost of position error should be 10x that of the velocity error, and the velocity error should be 10x that of the acceleration error.

Constructor & Destructor Documentation

◆ MPCPoseTracking()

moveit_pro::behaviors::MPCPoseTracking::MPCPoseTracking	(	const std::string &	name,
		const BT::NodeConfiguration &	config,
		const std::shared_ptr< moveit_pro::behaviors::BehaviorContext > &	shared_resources
	)

Member Function Documentation

◆ getResiduals()

static auto moveit_pro::behaviors::MPCPoseTracking::getResiduals ( )

inlinestatic

Define the residuals used by this MPC behavior.

This method allows the use of a MPCProblem instance to add residual-specific ports.

Returns: A BT::PortsList containing the required ports for the behavior.

◆ metadata()

BT::KeyValueVector moveit_pro::behaviors::MPCPoseTracking::metadata ( )

static

◆ providedPorts()

BT::PortsList moveit_pro::behaviors::MPCPoseTracking::providedPorts ( )