Track a moving pose using MPC. More...

#include <mpc_pose_tracking.hpp>

Inheritance diagram for moveit_studio::behaviors::MPCPoseTracking:

[legend]

Collaboration diagram for moveit_studio::behaviors::MPCPoseTracking:

[legend]

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

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

Public Member Functions inherited from moveit_studio::behaviors::internal::MPCBehaviorBase
	MPCBehaviorBase (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< moveit_studio::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 () override

tl::expected< bool, std::string >	doWork () override
	User-implemented function which handles executing the potentially-long-running process.

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_studio::behaviors::AsyncBehaviorBase
	AsyncBehaviorBase (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< BehaviorContext > &shared_resources)

virtual	~AsyncBehaviorBase ()=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.

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

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_studio::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_studio::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_studio::behaviors::internal::MPCBehaviorBase
std::shared_future< tl::expected< bool, std::string > >	future_
	Classes derived from AsyncBehaviorBase must have this shared_future as a class member.

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< planning_scene_monitor::PlanningSceneMonitor >	planning_scene_monitor
	Shared pointer to the planning scene monitor used for monitoring and updating the planning scene.

moveit::core::RobotModelConstPtr	robot_model
	Constant pointer to the robot model used for planning and execution.

std::string	planning_group
	The name of the planning group used for motion planning.

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

Protected Member Functions inherited from moveit_studio::behaviors::AsyncBehaviorBase
void	notifyCanHalt ()
	Called when runAsync() finishes to notify onHalted() that the async process has finished.

Protected Attributes inherited from moveit_studio::behaviors::SharedResourcesNode< BT::StatefulActionNode >
std::shared_ptr< BehaviorContext >	shared_resources_

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_studio::behaviors::MPCPoseTracking::MPCPoseTracking	(	const std::string &	name,
		const BT::NodeConfiguration &	config,
		const std::shared_ptr< moveit_studio::behaviors::BehaviorContext > &	shared_resources
	)

Member Function Documentation

◆ getResiduals()

static auto moveit_studio::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_studio::behaviors::MPCPoseTracking::metadata ( )

static

◆ providedPorts()

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