ExecutePolicy Class

Executes a policy as continuous, collision-checked motion on the arm and optional gripper. More...

Declaration

class moveit_pro::behaviors::ExecutePolicy { ... }

Included Headers

#include <execute_policy.hpp>

Base class

class	AsyncBehaviorBase
	A base class for behaviors which need to asynchronously run a function that might take a long time to complete. More...

Public Member Typedefs Index

using	JTACClientInterface = jtac_client::JTACClientInterface
using	GripperClientInterface = gripper_client::GripperClientInterface
using	TrajectoryChunkValidator = chunked_trajectory_executor::TrajectoryChunkValidator

Public Constructors Index

	ExecutePolicy (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< BehaviorContext > &shared_resources, Deps deps={})
	Constructor for the ExecutePolicy Behavior. More...

Public Member Functions Index

tl::expected< SceneAndState, std::string >	acquireSceneAndInitialState (const std::vector< std::string > &joint_names, const collision_check_params::PaddingOverrides &padding)
	Start monitoring the planning scene for the run and read the commanded joints' current positions. More...

Private Member Functions Index

tl::expected< PolicyHandle, std::string >	resolvePolicy ()
tl::expected< bool, std::string >	doWork () override
	User-implemented function which handles executing the potentially-long-running process. More...
tl::expected< void, std::string >	doHalt () override
	Optionally implement additional work needed to cleanly interrupt the async process. More...
tl::expected< void, std::string >	streamPolicyChunks (chunked_trajectory_executor::ChunkedTrajectoryExecutor &executor, const StreamConfig &config, const std::stop_token &stop_token, const std::shared_ptr< std::atomic< bool > > &scene_changed)
	Request chunks from the policy and stitch them onto the controller until the run completes. More...
tl::expected< ResolvedPolicyChunk, std::string >	resolvePolicyChunk (const moveit_pro_ml_msgs::srv::GetActionChunk::Response &response, const StreamConfig &config) const
	Resolve one policy response into the arm motion to submit, plus the gripper split when a gripper is configured. More...
GripperPlan	buildGripperPlan (const ArmGripperChunk &gripper_chunk, std::int64_t conditioning_steps, int committed_action_steps, double seam_time, bool &warned_clamp) const
	Build the committed gripper plan for one chunk, clamped to the gripper joint's limits. More...
moveit_pro_ml_msgs::srv::GetActionChunk::Request	buildActionChunkRequest (const StreamConfig &config, const PolicyObservation &observation, bool first_request, std::int64_t conditioning_steps, double chunk_start_time, const RtcCarryover &rtc) const
	Assemble one GetActionChunk request from the current observation and the real-time-chunking carryover. More...
void	warnOnControlPeriodMismatch (const moveit_pro_ml_msgs::srv::GetActionChunk::Response &response, double dt, bool &warned) const
	Warn once per run when the policy's reported native control period disagrees with dt. More...
std::shared_future< tl::expected< bool, std::string > > &	getFuture () override
	Gets the shared future which is used to monitor the progress of the async process. More...

Private Member Attributes Index

std::unique_ptr< ActionChunkProducerInterface >	producer_
std::unique_ptr< JTACClientInterface >	jtac_client_
std::unique_ptr< TrajectoryChunkValidator >	validator_
std::unique_ptr< ObservationSourceInterface >	observation_source_
std::unique_ptr< GripperClientInterface >	gripper_client_
std::stop_source	stop_source_
std::mutex	halt_mutex_
ActionChunkProducerInterface *	active_producer_ = nullptr
JTACClientInterface *	active_jtac_client_ = nullptr
GripperClientInterface *	active_gripper_client_ = nullptr
collision_check_params::PaddingDiagnostics	padding_diagnostics_
std::shared_future< tl::expected< bool, std::string > >	future_

Public Static Functions Index

static BT::PortsList	providedPorts ()
	Input ports that configure the chunk execution. More...
static BT::KeyValueVector	metadata ()
	Metadata for the MoveIt Pro Developer Tool. More...

Description

Executes a policy as continuous, collision-checked motion on the arm and optional gripper.

The Behavior gathers the current observation (joint positions and camera images), asks a policy handle for the next GetActionChunk response, and feeds it to a ChunkedTrajectoryExecutor that blends it onto the running motion at the seam, checks it against the commanded joints' position, velocity, and acceleration limits and for collision of the robot with itself and with the world, and stitches it onto the controller. It requests the next chunk before the committed motion drains so the robot normally does not pause at chunk boundaries, and stops cleanly once total_action_steps points have run, when the policy reports no more actions, or when the Behavior is halted. A policy response reporting an error, or one whose status is unset or not defined by the contract, stops the robot the same way and fails the Behavior, so an Objective can branch on a policy-side failure.

A chunk that is still not ready when the committed motion ends does not fail the run: every controller goal ends in a rest tail, so the robot rests in place with the goal, and with it the force-torque monitoring, still active, and the chunk seams into that rest when it arrives, resuming from the last executed action rather than skipping the actions the pause covered. The first pause of a run publishes a warning, and a run that paused reports how often and for how long when it ends, also when the policy call itself fails or times out; a first chunk slow enough to pause counts its whole wait as paused time. The motion budget counts only executed policy motion: neither the wait for the first chunk nor time spent paused counts toward total_action_steps. A policy call that fails or times out (policy_call_timeout) fails the run, as does the rare case where the chunk cannot be re-anchored when it arrives.

The collision checks read a planning scene that is monitored for the whole run and honor its AllowedCollisionMatrix. The matrix and the set of attached objects are read once at run start and not refreshed, so any contact the task means to make, such as a grasp or a place, must be allowed with a collision rule before this Behavior starts. Motion already sent to the controller but not yet executed is re-validated against the current scene when a world-geometry update arrives, and again each time the Behavior requests the next chunk from the policy; a violation cancels the controller goals, the controller decelerates to rest, and the Behavior fails, reporting the contact.

The check of the robot against the world pads the robot links by the monitored scene's link padding unless collision_check_params overrides it, and pads the world objects only when collision_check_params sets a world padding. While the deprecated link_padding port is set, its value is the link padding. The check of the robot against itself is never padded.

Supply a live policy handle from LoadPi05Policy or ConnectPolicyService. Each invocation starts a fresh producer from that resident policy; a handle another ExecutePolicy is running fails busy without starting motion. Halt cancels the controller and gripper goals and the producer, then waits for an in-process inference pass to finish before returning. The deprecated policy_service_name input remains available only when policy is unwired. When policy_uses_real_time_chunking is enabled, requests carry the previous chunk's unexecuted tail and anchor state. Capability validation belongs to the policy adapter; native pi0.5 currently rejects RTC. Every chunk is still blended and validated by the executor.

The arm and the gripper are driven on separate paths from the same chunk. The arm joints are streamed to the trajectory controller; the gripper, when one is configured, is the joint named by gripper_joint_name, which the chunk must carry as its one joint beyond the arm group (only single-DOF grippers are supported), and is commanded through the GripperCommand action server named by gripper_command_action_name, streamed step by step across each committed chunk so it tracks the policy's gripper motion rather than holding one value per chunk. The two gripper ports are set together or both left empty. With a gripper configured, the joint's sensed position is appended to the robot state sent to the policy after the arm group's joints, so the policy observes the gripper it commands; the executor and the controller still command the arm joints only. Leaving both ports empty runs the arm only and leaves the gripper to a separate Behavior. Each gripper target is checked for finiteness and clamped to the gripper joint's position limits. The gripper's links are part of the collision check at the pose the monitored scene currently reports, so the gripper geometry is considered, but the commanded gripper opening itself is not re-validated per step the way the arm chunk is. A halt cancels both the arm controller goals and any in-flight gripper goal; a position controller may still hold its last gripper setpoint after a cancel, so a halt is not guaranteed to reopen the gripper.

Data Port Name	Port Type	Object Type
policy	input	PolicyHandle
policy_service_name	input	std::string
joint_group_name	input	std::string
total_action_steps	input	int
prompt	input	std::string
committed_action_steps	input	int
dt	input	double
num_interpolation_points	input	int
collision_check_params	input	moveit_studio_msgs::msg::CollisionCheckParams
goal_path_tolerance	input	double
policy_tracking_weight	input	double
policy_uses_real_time_chunking	input	bool
guidance_horizon	input	int
controller_action_name	input	std::string
absolute_force_torque_threshold	input	std::vector<double>
policy_call_timeout	input	double
image_names	input	std::vector<std::string>
image_topics	input	std::vector<std::string>
joint_states_topic	input	std::string
gripper_command_action_name	input	std::string
gripper_joint_name	input	std::string
link_padding	input	double (deprecated)

Definition at line 337 of file execute_policy.hpp.

Public Member Typedefs

GripperClientInterface

using moveit_pro::behaviors::ExecutePolicy::GripperClientInterface = gripper_client::GripperClientInterface

Definition at line 341 of file execute_policy.hpp.

JTACClientInterface

using moveit_pro::behaviors::ExecutePolicy::JTACClientInterface = jtac_client::JTACClientInterface

Definition at line 340 of file execute_policy.hpp.

TrajectoryChunkValidator

using moveit_pro::behaviors::ExecutePolicy::TrajectoryChunkValidator = chunked_trajectory_executor::TrajectoryChunkValidator

Definition at line 342 of file execute_policy.hpp.

Public Constructors

ExecutePolicy()

moveit_pro::behaviors::ExecutePolicy::ExecutePolicy (const std::string & name, const BT::NodeConfiguration & config, const std::shared_ptr< BehaviorContext > & shared_resources, Deps deps={})

Constructor for the ExecutePolicy Behavior.

Parameters

name	Behavior instance name, set by the behavior tree factory.
config	Behavior tree configuration, set by the behavior tree factory.
shared_resources	Resources shared between Behaviors, such as the node and the robot model.
deps	Injected collaborators for testing. Each null entry is built from the ports at run start: the jtac_client as a default JTACClient, the validator as a collision validator reading the live monitored planning scene, the observation_source as a topic subscriber, and the gripper_client as a GripperClient created only when gripper_command_action_name is set.

Declaration at line 366 of file execute_policy.hpp, definition at line 643 of file execute_policy.cpp.

Public Member Functions

acquireSceneAndInitialState()

tl::expected< ExecutePolicy::SceneAndState, std::string > moveit_pro::behaviors::ExecutePolicy::acquireSceneAndInitialState (const std::vector< std::string > & joint_names, const collision_check_params::PaddingOverrides & padding)

virtual

Start monitoring the planning scene for the run and read the commanded joints' current positions.

Production starts a PlanningSceneMonitor that stays alive for the whole run, so the collision checks follow the world as it changes. Tests override this to supply canned initial positions with no live scene (and inject their own validator).

Parameters

joint_names	Commanded joints, defining the order of initial_joint_positions.
padding	Link and world object paddings in meters applied to the monitored scene; a disengaged member keeps the scene's own padding. The validator applies them when checking the robot against the world, but not when checking the robot against itself.

Returns

The running scene monitor and the initial positions, or an error if the scene could not be fetched.

Declaration at line 410 of file execute_policy.hpp, definition at line 749 of file execute_policy.cpp.

Private Member Functions

buildActionChunkRequest()

moveit_pro_ml_msgs::srv::GetActionChunk::Request moveit_pro::behaviors::ExecutePolicy::buildActionChunkRequest (const StreamConfig & config, const PolicyObservation & observation, bool first_request, std::int64_t conditioning_steps, double chunk_start_time, const RtcCarryover & rtc)

Assemble one GetActionChunk request from the current observation and the real-time-chunking carryover.

The request carries the current observation; the seam state is used only internally by the executor for spline anchoring, so the robot_state field is always the state observed now.

Parameters

config	The resolved run configuration (prompt, commanded joints, timestep, chunking mode).
observation	The current sensed joint positions and camera frames.
first_request	True on the run's first request, so a stateful policy resets its episode.
conditioning_steps	Leading chunk steps the policy freezes to the previous chunk, sent as the inference delay.
chunk_start_time	The chunk's start on the trajectory clock, the time its first action stands for. The unexecuted tail of the previous chunk is sliced here so its actions and the new chunk's actions pair up on the same control ticks.
rtc	The previous chunk's carryover, sent only when the policy runs real-time chunking.

Returns

The assembled request.

Declaration at line 589 of file execute_policy.hpp, definition at line 1233 of file execute_policy.cpp.

buildGripperPlan()

ExecutePolicy::GripperPlan moveit_pro::behaviors::ExecutePolicy::buildGripperPlan (const ArmGripperChunk & gripper_chunk, std::int64_t conditioning_steps, int committed_action_steps, double seam_time, bool & warned_clamp)

Build the committed gripper plan for one chunk, clamped to the gripper joint's limits.

The gripper runs on its own path alongside the arm, so this picks out the gripper targets for the same window of chunk steps the arm executor commits this iteration: one target per committed step. Those targets come straight from the policy and skip the arm's limit checks, so each is clamped to the gripper joint's position limits before it is commanded.

Parameters

gripper_chunk	The chunk's split-out gripper positions and joint name.
conditioning_steps	Steps the seam advanced past the chunk start, dropped from the front.
committed_action_steps	How many of the remaining steps to commit.
seam_time	The committed window's start on the trajectory clock.
warned_clamp	Set true once a clamp warning has been published, so the run warns at most once. Every target is clamped regardless; only an overshoot past the tolerated band warns.

Returns

The per-step gripper plan for this window.

Declaration at line 567 of file execute_policy.hpp, definition at line 1335 of file execute_policy.cpp.

doHalt()

tl::expected< void, std::string > moveit_pro::behaviors::ExecutePolicy::doHalt ()

virtual

Optionally implement additional work needed to cleanly interrupt the async process.

The default implementation of this function is a no-op which will not interrupt the async process. This will mean that onHalted will wait until the process finishes before returning.

Returns

Return an empty tl::expected<void, std::string> if the additional work to halt was successful. Return an error message if something failed while doing additional work to halt.

Declaration at line 417 of file execute_policy.hpp, definition at line 785 of file execute_policy.cpp.

doWork()

tl::expected< bool, std::string > moveit_pro::behaviors::ExecutePolicy::doWork ()

virtual

User-implemented function which handles executing the potentially-long-running process.

This function is called within an async process in a separate thread.

Returns

A tl::expected which contains a bool indicating task success if the process completed successfully or was canceled, or an error message if the process failed unexpectedly.

Declaration at line 415 of file execute_policy.hpp, definition at line 863 of file execute_policy.cpp.

getFuture()

std::shared_future< tl::expected< bool, std::string > > & moveit_pro::behaviors::ExecutePolicy::getFuture ()

inline virtual

Gets the shared future which is used to monitor the progress of the async process.

Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member.

This exists to prevent destruction of the derived class while the async process is still in-progress. If the derived class is destroyed, the definitions of the functions used within doWork() will be destroyed too, which will result in the virtual functions in the base class being called instead and cause a fault.

This function will force derived classes to add an instance of this type and return a reference to it. The base class can then use this virtual function to access the shared future in functions like onStart.

By adding this virtual function we're properly demonstrating how this future depends on things from the derived class and the natural flow of object lifetimes will do the hard work for us. The std::shared_future destructor will get the value of the future before the derived class is destructed assuming it's the last reference to the shared state. Doing it this way means neither the base nor derived class should need to implement a destructor which is a nice property to have.

Returns

Returns the shared_future, which should be owned by the child class.

Definition at line 635 of file execute_policy.hpp.

resolvePolicy()

tl::expected< PolicyHandle, std::string > moveit_pro::behaviors::ExecutePolicy::resolvePolicy ()

Declaration at line 414 of file execute_policy.hpp, definition at line 809 of file execute_policy.cpp.

resolvePolicyChunk()

tl::expected< ExecutePolicy::ResolvedPolicyChunk, std::string > moveit_pro::behaviors::ExecutePolicy::resolvePolicyChunk (const moveit_pro_ml_msgs::srv::GetActionChunk::Response & response, const StreamConfig & config)

Resolve one policy response into the arm motion to submit, plus the gripper split when a gripper is configured.

With no gripper the chunk must match the arm joint group; with a gripper it is split into the arm joints and the configured gripper joint. Splitting here, before the arm is committed, means a chunk that omits or misnames the gripper joint fails before any motion reaches the controller.

Parameters

response	The policy response carrying the chunk.
config	The resolved run configuration (commanded joints, dof, gripper action name).

Returns

The resolved chunk, or an error if the chunk does not match the configured joints.

Declaration at line 548 of file execute_policy.hpp, definition at line 1301 of file execute_policy.cpp.

streamPolicyChunks()

tl::expected< void, std::string > moveit_pro::behaviors::ExecutePolicy::streamPolicyChunks (chunked_trajectory_executor::ChunkedTrajectoryExecutor & executor, const StreamConfig & config, const std::stop_token & stop_token, const std::shared_ptr< std::atomic< bool > > & scene_changed)

Request chunks from the policy and stitch them onto the controller until the run completes.

Loops until total_action_steps points of motion have run, the policy reports no more actions, a response fails the run (an error, unset, or contract-undefined status), or the run is halted, requesting the next chunk before the committed motion drains. The controller is cancelled on every exit path. Called once per run after the executor has been reset to its stationary hold.

Parameters

executor	The executor holding the controller client and the running trajectory.
config	The resolved run configuration.
stop_token	Halt signal flipped by doHalt().
scene_changed	Set by the scene monitor's update callback when the world geometry changes; the loop consumes it and re-validates the committed motion against the current scene.

Returns

Nothing on a clean stop, or an error describing the first failed step.

Declaration at line 532 of file execute_policy.hpp, definition at line 1406 of file execute_policy.cpp.

warnOnControlPeriodMismatch()

void moveit_pro::behaviors::ExecutePolicy::warnOnControlPeriodMismatch (const moveit_pro_ml_msgs::srv::GetActionChunk::Response & response, double dt, bool & warned)

Warn once per run when the policy's reported native control period disagrees with dt.

The chunk is played at the configured dt, so a native rate that differs from it scales every commanded joint velocity. A contract-violating negative period is flagged but not acted on.

Parameters

response	The policy response carrying the reported control period.
dt	The configured control period the chunk is played at.
warned	Set true once a warning has been published, so the run warns at most once.

Declaration at line 602 of file execute_policy.hpp, definition at line 1852 of file execute_policy.cpp.

Private Member Attributes

active_gripper_client_

GripperClientInterface* moveit_pro::behaviors::ExecutePolicy::active_gripper_client_ = nullptr

Definition at line 628 of file execute_policy.hpp.

active_jtac_client_

JTACClientInterface* moveit_pro::behaviors::ExecutePolicy::active_jtac_client_ = nullptr

Definition at line 627 of file execute_policy.hpp.

active_producer_

ActionChunkProducerInterface* moveit_pro::behaviors::ExecutePolicy::active_producer_ = nullptr

Definition at line 626 of file execute_policy.hpp.

future_

std::shared_future<tl::expected<bool, std::string> > moveit_pro::behaviors::ExecutePolicy::future_

Definition at line 640 of file execute_policy.hpp.

gripper_client_

std::unique_ptr<GripperClientInterface> moveit_pro::behaviors::ExecutePolicy::gripper_client_

Definition at line 615 of file execute_policy.hpp.

halt_mutex_

std::mutex moveit_pro::behaviors::ExecutePolicy::halt_mutex_

Definition at line 625 of file execute_policy.hpp.

jtac_client_

std::unique_ptr<JTACClientInterface> moveit_pro::behaviors::ExecutePolicy::jtac_client_

Definition at line 610 of file execute_policy.hpp.

observation_source_

std::unique_ptr<ObservationSourceInterface> moveit_pro::behaviors::ExecutePolicy::observation_source_

Definition at line 612 of file execute_policy.hpp.

padding_diagnostics_

collision_check_params::PaddingDiagnostics moveit_pro::behaviors::ExecutePolicy::padding_diagnostics_

Definition at line 633 of file execute_policy.hpp.

producer_

std::unique_ptr<ActionChunkProducerInterface> moveit_pro::behaviors::ExecutePolicy::producer_

Definition at line 609 of file execute_policy.hpp.

stop_source_

std::stop_source moveit_pro::behaviors::ExecutePolicy::stop_source_

Definition at line 617 of file execute_policy.hpp.

validator_

std::unique_ptr<TrajectoryChunkValidator> moveit_pro::behaviors::ExecutePolicy::validator_

Definition at line 611 of file execute_policy.hpp.

Public Static Functions

metadata()

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

static

Metadata for the MoveIt Pro Developer Tool.

Returns

The Behavior metadata.

Declaration at line 379 of file execute_policy.hpp, definition at line 1879 of file execute_policy.cpp.

providedPorts()

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

static

Input ports that configure the chunk execution.

Returns

The list of ports.

Declaration at line 373 of file execute_policy.hpp, definition at line 653 of file execute_policy.cpp.

---

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

• execute_policy.hpp
• execute_policy.cpp

---

Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.