ExecutePolicy Class
Executes a policy as continuous, collision-checked motion on the arm and optional gripper. More...
Declaration
Included Headers
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, double link_padding) |
|
Start monitoring the planning scene for the run and read the commanded joints' current positions. More... | |
Private Member Functions Index
| 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_ |
| JTACClientInterface * | active_jtac_client_ = nullptr |
| GripperClientInterface * | active_gripper_client_ = nullptr |
| 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 for the next chunk over the GetActionChunk service, 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 policy is any node that serves the GetActionChunk ROS 2 service (moveit_pro_ml_msgs/srv/GetActionChunk); the serving node can wrap an in-process model adapter, a local GPU server, or a remote one. Every chunk is blended onto the running motion at the seam regardless of the policy; the seam blend is never delegated to the producer. When the policy runs real-time chunking (policy_uses_real_time_chunking), the Behavior additionally sends it the previous chunk's unexecuted tail and the state that chunk was anchored to, so it continues the committed motion and the blend has less to correct.
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_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 |
| link_padding | input | double |
| 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 |
Definition at line 420 of file execute_policy.hpp.
Public Member Typedefs
GripperClientInterface
|
Definition at line 424 of file execute_policy.hpp.
JTACClientInterface
|
Definition at line 423 of file execute_policy.hpp.
TrajectoryChunkValidator
|
Definition at line 425 of file execute_policy.hpp.
Public Constructors
ExecutePolicy()
|
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 producer as a GetActionChunk service client, 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 450 of file execute_policy.hpp, definition at line 771 of file execute_policy.cpp.
Public Member Functions
acquireSceneAndInitialState()
| 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.
link_padding Padding in meters added to the robot's collision geometry in the monitored scene. The validator applies it 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 494 of file execute_policy.hpp, definition at line 868 of file execute_policy.cpp.
Private Member Functions
buildActionChunkRequest()
|
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 671 of file execute_policy.hpp, definition at line 1236 of file execute_policy.cpp.
buildGripperPlan()
|
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 649 of file execute_policy.hpp, definition at line 1338 of file execute_policy.cpp.
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 499 of file execute_policy.hpp, definition at line 903 of file execute_policy.cpp.
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 497 of file execute_policy.hpp, definition at line 923 of file execute_policy.cpp.
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 711 of file execute_policy.hpp.
resolvePolicyChunk()
|
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 630 of file execute_policy.hpp, definition at line 1304 of file execute_policy.cpp.
streamPolicyChunks()
|
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 614 of file execute_policy.hpp, definition at line 1409 of file execute_policy.cpp.
warnOnControlPeriodMismatch()
|
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 684 of file execute_policy.hpp, definition at line 1842 of file execute_policy.cpp.
Private Member Attributes
active_gripper_client_
|
Definition at line 709 of file execute_policy.hpp.
active_jtac_client_
|
Definition at line 708 of file execute_policy.hpp.
future_
|
Definition at line 716 of file execute_policy.hpp.
gripper_client_
|
Definition at line 697 of file execute_policy.hpp.
halt_mutex_
|
Definition at line 707 of file execute_policy.hpp.
jtac_client_
|
Definition at line 692 of file execute_policy.hpp.
observation_source_
|
Definition at line 694 of file execute_policy.hpp.
producer_
|
Definition at line 691 of file execute_policy.hpp.
stop_source_
|
Definition at line 699 of file execute_policy.hpp.
validator_
|
Definition at line 693 of file execute_policy.hpp.
Public Static Functions
metadata()
| static |
Metadata for the MoveIt Pro Developer Tool.
- Returns
The Behavior metadata.
Declaration at line 463 of file execute_policy.hpp, definition at line 1869 of file execute_policy.cpp.
providedPorts()
| static |
Input ports that configure the chunk execution.
- Returns
The list of ports.
Declaration at line 457 of file execute_policy.hpp, definition at line 782 of file execute_policy.cpp.
The documentation for this class was generated from the following files:
Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.