Broadly-Exploring, Local-Policy Trees for Long-Horizon Task Planning
Abstract: Long-horizon planning in realistic environments requires the ability to reason over sequential tasks in high-dimensional state spaces with complex dynamics.
Classical motion planning algorithms, such as rapidly-exploring random trees, are capable of efﬁciently exploring large state spaces and computing longhorizon, sequential plans.
However, these algorithms are generally challenged with complex, stochastic, and high-dimensional state spaces as well as in the presence of small, topologically complex goal regions, which naturally emerge in tasks that interact with the environment.
Machine learning offers a promising solution for its ability to learn general policies that can handle complex interactions and high-dimensional observations.
However, these policies are generally limited in horizon length.
Our approach, Broadly-Exploring, Local-policy Trees (BELT), merges these two approaches to leverage the strengths of both through a task-conditioned, model-based tree search.
BELT uses an RRT-inspired tree search to efﬁciently explore the state space.
Locally, the exploration is guided by a task-conditioned, learned policy capable of performing general short-horizon tasks.
This task space can be quite general and abstract; its only requirements are to be sampleable and to well-cover the space of useful tasks.
This search is aided by a task-conditioned model that temporally extends dynamics propagation to allow long-horizon search and sequential reasoning over tasks.
BELT is demonstrated experimentally to be able to plan long-horizon, sequential trajectories with a goal conditioned policy and generate plans that are robust.
Introduction
For a robot to plan complex long-horizon tasks in realistic environments, it must be capable of reasoning over sequences of subtasks.
It must broadly search the space of possible trajectories while considering both what the subtasks are and the order in which they are accomplished.
It must handle the complex and high dimensional state space and dynamics of the real world.
Consider, for example, a robot cleaning a kitchen.
A planner must ﬁnd trajectories to put a utensil away, to wash a pot, to throw away trash, and more.
Each of these tasks requires the ability to understand and execute lower-level tasks involved, as well as their order; to put a utensil away, the robot must ﬁrst open a drawer, then grasp the utensil, then place it in the drawer, and ﬁnally close the drawer.
Classical planning algorithms are capable of efﬁciently and broadly searching state spaces and planning over long-horizons [1, 2].
However, they have difﬁculty in the presence of complex, stochastic dynamics, high-dimensional systems, so-called narrow passages, and small, complex goal regions (which tasks naturally create).
Machine learning has emerged as the state of the art for problems with high-dimensional observations (e.g., pixels) and challenging dynamics (e.g., environment interaction).
Furthermore, it allows learning policies for general and abstract tasks [3, 4, 5, 6, 7].
However, many such approaches are limited to short-horizons [8, 9].
We propose a method which leverages the best of both approaches, a tree search which explores the state space in an RRT-like manner, but where edges maintain a consistent task executed by a learned, task-conditioned policy (Fig. 1).
In this work, we deﬁne task in a general sense, with a requirement only that the task space is sampleable and well-covers the space of useful actions; for example, this includes goal-conditioned policies, explicit tasks, learned skills, and latent task embeddings.
The local, task-conditioned policy is thus capable of executing complex short-horizon tasks.
The skeleton of the RRT search allows rapid exploration of the state space.
This exploration is aided by a task-conditioned model, capable of rolling out long-horizon trajectories, which can then be planned over and reasoned about in a model-based manner to the limit of the known, predictable
[Figure]
environment.
Broadly-Exploring, Local-policy Trees (BELT) is thus able to create long-horizon plans with sequential, complex tasks.
1.1 Related Work.
Planning Algorithms are capable of efﬁciently, and globally, exploring state spaces to plan longhorizon trajectories which avoid collision [1, 2, 10].
Within robotics, as the robot must interact with the world, planning algorithms often consider the task and motion planning setting (contrasting with AI planning).
Approaches in this setting generally plan in a hierarchical manner by deﬁning explicit task-primitives, symbolic languages, or transition logic constraints.
These methods then plan over them, for example, via trees or optimization-based methods [11, 12, 13, 14, 15, 16, 17].
These methods perform well in long-horizon planning, but are limited by their task representations, the dimensionality of the search space, and their ability to execute high-dimensional, complex tasks robustly.
BELT approaches such problems through minimal constraints and structure on the task space, instead relying on learned local policies to guide the search towards task relevant regions.
Model-Free Learning offers a promising direction to execute such tasks, though theses methods have difﬁculty reasoning over long horizons.
To enable long-horizon performance, previous works often enforce hierarchy or embed policies within classical structure.
Hierarchical reinforcement learning [4, 5, 18] works to temporally extend planning at lower levels, reducing the horizon the high-level policy must reason over [19].
Recent work on goal-conditioned reinforcement learning learns to efﬁciently achieve arbitrary goal states [20, 3].
This casts the problem for the high-level policy as ﬁnding a sequence of goal states [8, 9, 21, 22].
This also allows the embedding of policies in classical motion planning structures [23, 24, 25, 26, 27, 28, 29, 30].
Search on the replay buffer (SoRB) [27] for example builds a searchable graph of the replay buffer given a learned distance metric for a goalconditioned policy.
In contrast to SoRB and other previous work, BELT uses a tree search equipped with a task-conditioned model to enable searching for plans outside of previous experience and is capable of solving non-goal-conditioned tasks with its model-based success check.
Model-based Planning allows longer-horizon, sequential planning by moving reasoning to the model level.
In this way planning can be performed by rolling out models and evaluating such rollouts, [31, 32].
To extend the performance of models to longer-horizons, models can be trained for multistep prediction [33] and planned over [34].
Others use models within classical algorithms, such as sampling-based motion planning [35] or control [36, 37].
In this work, we leverage a taskconditioned model to temporally extend the model prediction over edges with consistent tasks, resulting in more robust long-range performance.
We additionally use the model to broadly search the state space via an RRT-inspired search.
1.2 Statement of Contributions.
The contribution of this paper is an algorithm Broadly-Exploring, Local-policy Trees (BELT) that is capable of planning long-horizon, sequential tasks in complex environments.
BELT leverages ( 1 – Sections 2 and 2.2) the state space exploration abilities of planning through an RRT-inspired search, ( 2 – Section 2.1) the local guidance of a learned task-conditioned policy, capable of executing complex tasks in high-dimensional spaces, and ( 3 – Section 2.3) the long-horizon propagation and reasoning of a learned, temporally extended task-conditioned model.
BELT is demonstrated with a learned goal-conditioned policy in a realistic environment to plan effectively, signiﬁcantly outperforming baselines.
Furthermore, these plans are directly executable by the policy.
[Figure]
Broadly-Exploring, Local-policy Trees
We begin with a general outline of BELT and then discuss concrete implementations of algorithmic subroutines in Sections 2.1 - 2.3.
Brieﬂy, BELT searches for long-horizon, sequential plans via a model-based, task-conditioned tree search.
This search is built on a backbone of a Rapidly-exploring
Random Tree (RRT) with a local, learned task-conditioned policy.
This approach leverages the efﬁcient global-exploration of RRTs, the local directed actions of a learned task-conditioned policy, and the sequential reasoning of a model.
Problem Statement.
In this work we refer to a task and task space in a general sense and we consider planning in environments which are predictable and known, with a fully observed state space X .
The only requirements of the task space Z is that it is sampleable and well-covers the space of useful actions.
For example, this can be explicitly given as a set of tasks, implicitly learned via a latent task space, or structured as the state space X with a goal-conditioned policy.
To allow for general task deﬁnitions, a success criteria Success is used that is considered a “black box”.
Given a trajectory { x init , ..., x T } , Success outputs if the trajectory satisﬁes all necessary conditions.
This formula- tion allows for a general problem deﬁnition that includes goal-conditioned planning, but also allows for transient tasks that cannot be expressed by a goal state.
The cost used in this work, referred to interchangeably with distance, is the time of a trajectory, though the approach is general to other costs.
The environment considered in this work is continuous, known, and static, with the exception of motion as a result of interaction with the robot.
The complexity of the environment arises generally from high dimensional states (e.g., many objects) and challenging interactions (e.g., grasping) and the sequentiality arises from tasks which are bottlenecked by previous task completion.
BELT.
The search is outlined in Algorithm 1 and BELT is shown with progressively more concreteness in Figs.
Fig.
1 shows a hypothetical form of BELT with an explicit task space, Fig.shows BELT with a goal-conditioned policy that is learned from play [38], and Fig.
3 shows BELT as applied to the experimental environment used herein.
A new planning problem is deﬁned by an initial state, x init , and by success criteria Success .
The search begins by initializing a tree T with x init as its root (Line 1) and iteratively adding task-conditioned edges.
For each iteration i , a state x expand 2 T within the tree is selected, along with a task
z
i 2 Z (Lines 3-4).
BELT requires a sampleable task space Z and task-conditioned policy, ⇡ ( a t | x t , z i ) , which outputs an action a t at time t given the current state x t and task z i .
The goal of the task space is to well cover the state space and guide the search towards useful actions; the task space and policy used herein are discussed in detail in Section 2.1.
The goal of the selection of the tree node x expand is to guide the search towards unexplored regions and to exploit promising paths; this is discussed in detail in Section 2.2.
Given the task z i and state x expand , the policy is then propagated for T timesteps via a model p ( x t +1 | x t , a t ) to create trajectory { x expand , x 1 , ..., x T  1 , x T } , saving the ﬁnal state x new = x T (Lines 6-7).
The goal of the model is to provide accurate predictions of trajectories given a task (in the absence of known dynamics); the model used herein is discussed in more detail in Section 2.3.
In this way, each edge may be thought of having a temporally extended consistent intent, such as attempting a single task or reaching a goal.
This edge is then added to the tree and the trajectory from x init to x expand to x new is checked for success with Success (Line 8).
This search continues for N iterations, at which time the lowest-cost path marked as successful is returned if it exists (Line 9).
[Figure]
grasping
[Figure]
from tree T (L4) state space X
[Figure]
(L
[Figure] [Figure]
[Figure] [Figure]
: N
Section 2.
Section 2. Section 2.
Rollout model
[Table]
[Table]
2.1 Learning a Task Space and Task-Conditioned Policy From Play
In this work we implement BELT with a general-purpose goal-reaching policy at the low level, called Learning from Play (LfP) [38].
To learn from “play” we assume access to an unsegmented teleoperated play dataset D of semantically meaningful behaviors provided by users, without a set of predeﬁned tasks in mind.
For example, a user might open and close doors, rearrange objects, or simply tidy the scene.
To learn control, this long temporal state-action log D = { ( x t , a t ) } 1 t =0 is relabeled [3, 20], treating each visited state in the dataset as a “reached goal state”, with the preceding states and actions treated as optimal behavior for reaching that goal.
Relabeling yields a dataset of D play = { ( ⌧ , x g ) i } D play i =0 , where each example consists of a goal state x g and a demonstration ⌧ = { ( x 0 , a 0 ) , ... } solving for the goal.
These can be fed to a simple maximum likelihood goal conditioned imitation objective,
L [Equation] = E
[Equation]
[Equation] play [Equation] i , to learn policy ⇡ ✓ ( a t | x t , x g ) , parameterized by ✓ .
The motivation behind this collection is to allow play to fully cover the state space using prior human knowledge, then to distill short-horizon behaviors into a single goal-directed policy.
We could in principle use any goal-directed imitation network architecture to implement the policy ⇡ ✓ ( a t | x t , x g ) .
For direct comparison to prior work, we use Play Supervised Latent Motor Plans (Play-LMP) [38].
Play-LMP addresses the inherent multimodality in free-form imitation datasets.
Concretely it is a sequence-to-sequence conditional variational autoencoder (seq2seq CVAE), au- toencoding contextual demonstrations through a latent “plan” space.
The decoder is a policy trained to reconstruct input actions, conditioned on state x t , goal x g , and an inferred plan z for how to get from x t to x g .
At test time, Play-LMP takes a goal as input, and infers and follows plan z internally.
A t-SNE of this plan space is shown in Fig. 3a.
See [38] for details.
With Play-LMP as our policy, the task space Z is the union of the goal x g and the latent plan z .
2.2 Biasing the Search
Given a goal-conditioned policy, as Play-LMP is, RRT [1] is used with a few adaptions to broadly search the state space, chose achievable edge tasks, and bias towards low-cost paths (Fig. 4a).
To broadly search the state space, a key component of RRT’s ability to efﬁciently explore is what is known as the Voronoi bias.
For an RRT, this bias states that at each iteration, the selection of the tree node for expansion is proportional to the volume of its Voronoi region, meaning nodes in larger unexplored regions are more likely to be selected and thus RRTs rapidly explore [1].
To select a node in RRT, a sample is drawn from the state space and the nearest node within the tree is selected as the expansion node.
For BELT, we seek to leverage a Voronoi bias via a temporal distance  t between states x i , x j 2 X as the L2 distance is not a good distance for this problem.
For example, consider the block in Fig.
3b.
If it is inside the drawer or on top of the desk, the L2 distance is quite small, but to connect these two states requires opening the drawer, grasping the block, placing the block, and closing the drawer – a signiﬁcant distance.
As the temporal distance is not easily quantiﬁable, we learn to predict it with a temporal distance classiﬁer, ⌧ tdc ( x i , x j ) , with K exponential categories corresponding to temporal distance intervals, d k 2 { [0 , 1) , [1 , 2) , [2 , 4) , [4 , 8) , [8 , 16) , ..., [128 , 256) } based on [39].
The temporal distance classiﬁer is implemented as a neural network and trained over policy rollouts to predict the distribution over class labels via a cross-entropy classiﬁcation loss,
L tdc =
P
Kl =1 y l log(ˆ y l ) with ˆ y = ⌧ tdc ( x i , x j ) , where y and ˆ y are the true and predicted label distributions respectively.
Brieﬂy, we note that we also considered a temporal distance regressor, but found it considerably less accurate.
The selection of an expansion state thus proceeds by ﬁrst selecting a goal state from a demonstration dataset from rollouts of the policy x sample 2 D ⇡ .
The temporal distance is then predicted between x sample and each node in the tree.
All nodes below a cutoff temporal distance d cutoff are then added to a set of possible nodes to expand from.
Within this set the node with the lowest-cost to come is selected for expansion, x expand .
This biases the tree search towards nodes that are capable of reaching the goal state and towards nodes with efﬁcient trajectories.
It further allows a Voronoi bias by temporal distance.
The value of d cutoff represents a trade-off between exploiting more promising paths (larger values of d cutoff ) and exploring new ones (smaller values of d cutoff ); this trade-off is studied experimentally in the Appendix Section 5.3.
This procedure modiﬁes lines 3 and 4 of Algorithm 1.
We note that a common and effective approach to biasing search in sampling-based motion planning is to goal bias, i.e., biasing samples towards states within the goal region.
As success often requires full sequences of trajectories rather than individual states we do not generally have access to goal states in this problem setting and thus we do not use goal biasing of this form.
2.3 Learning a Task-Conditioned Model
Given a policy, the tree search plans via a model.
In this work we consider three options for models: a simulator, an action-conditioned model, and a task-conditioned model, each shown in Fig.
4b and 4c.
Access to a simulator or the exact dynamics is assumed for much of the motion planning community when planning (unless explicitly planning under uncertainty).
This acts as an upper bound herein and tests the ability of BELT to explore the state space and plan sequential tasks given a perfect model.
It also shows bounds on the robustness of our policy.
The action-conditioned model learns to predict the next state p action ( x t +1 | x t , a t ) given the current state x t and action a t .
For a given trajectory, this is recursively applied for T timesteps.
The task-conditioned model learns a prediction of where the robot will be after T timesteps, i.e., p task ( x t + T | x t , z ) .
This accounts for the closedloop nature of the policy for a ﬁxed task and temporally extends the prediction to reduce recursive errors.
For the Play LMP policy, the task conditioning z corresponds to a goal state and a latent plan.
Each model is represented as a neural network and trained from a dataset of demonstration policy rollouts to minimize the L2 reconstruction loss.
For a given rollout for task z , states { x 0 , x 1 , ..., x T } , and actions { a 0 , a 1 , ...a T  1 } , the action-model seeks to minimize L p action =
P
[-T  1 t =
 ||-][Equation]
x [-t +
  x t +
-][Equation]
| with ˆ x t + ⇠ p action ( x t , a t ) , while the task-model seeks to minimize L p task
=
|| ˆ x T  x T || with ˆ x
T ⇠ p task ( x 0 , z ) .
[Figure]
Experimental Results
The experiments herein are based on a simulated “playground environment” [38] (Discussed in more detail in the Appendix and shown in Appendix Fig 7) built in Mujoco [40].
The robot is a positioncontrolled 8-DOF robotic arm and gripper.
The robot receives as observations at each timestep of the cartesian position and orientation of all objects including its end effector.
The agent performs high-frequency, closed-loop control, sending continuous position, rotation, and gripper commands to its arm at 30 Hz.
The environment is known and static (unless the direct result of interaction with the robot) and contains a desk with a sliding door and drawer that can be opened and closed as well as a movable rectangular block and 3 buttons that can be pushed to turn on different colored lights (for a total of 19 continuous dimensions, detailed in full in Appendix 5.1).
Long-horizon evaluation.
We evaluate our model and its baselines on large set of challenging longhorizon manipulation tasks as in [41], which require several subtasks to be executed in order.
Here, multi-stage tasks are constructed by considering all valid N-stage transitions between a set of 18 core short-horizon manipulation tasks.
See [38, 41] for a complete discussion of the 18 tasks, which span different families, e.g. pushing buttons, opening and closing doors/drawers, grasping and lifting a block, etc.
We evaluate on “Chain-4”, which results in 925 unique task chains (Appendix Fig. 7).
Note that prior work [41] assumes a human is in the loop providing each next subtask to the agent in the sequence.
In our setup, an agent must provide itself subgoals to achieve the multi-stage behavior.
Other prior long-horizon evaluations, such as [9], describe the long-horizon task as a ﬁnal goal state to reach.
While this valid for many useful manipulation tasks, many are impossible to infer from the ﬁnal goal state alone.
For example, in this environment, when a button is no longer pressed down, the corresponding light turns off.
In this way, the ﬁnal state for task “1) open drawer, 2) push green button, 3) close drawer” may appear to an agent as “no change to the environment”.
We consider a more general task speciﬁcation scheme—an indicator function, deﬁned over the full agent trajectory, which returns true if the full desired sequence was executed correctly and false otherwise.
We refer to this here as the “success detector”, which our high-level agent plans against.
3.1 Algorithmic Parameters and Baselines
Baselines.
We compare against two baselines that use the same underlying Play-supervised Latent
Motor Plan (Play-LMP) [38] policy.
We compare to a model-based planner (cross entropy method (CEM) [42]) and a Play-LMP with given goal state.
CEM was implemented in the task space to select four sequential tasks.
The tasks were then rolled out with the simulator and task-model.
The distribution was then reﬁt to successful trajectories.
We also compare to a Play-LMP policy that is given a single goal-state that satisﬁes all the subtasks (this single goal state existed for 36% of the chains).
This baseline demonstrates that a simple approach of using long-horizon goals with a policy trained on short-horizons performs poorly and that BELT is able to plan long-horizons with local policies.
Finally, we also compare to an “oracle” LMP policy, Chain-LMP, which is given the four chain tasks in a row, demonstrating an upper bound of what the BELT plan may achieve given the performance of the underlying policy.
Parameters.
The low-level policy is trained with the same play logs collected in [38]; ⇠ 7h of teleoperated play relabeled into D play , containing ⇠ 10M short-horizon demonstrations, each 1-2 seconds (see video for examples of this data).
The TDC and models were trained on demonstration data for ⇠ 100k rollouts.
⌧ tdc was trained on this dataset to an accuracy of 82% and d cutoff was set to < 64 based on analysis in the next section.
The edges were rolled out with a randomly drawn timestep of T 2 { 32 , 64 , 96 } , as inspired by [10].
BELT used 2500 goal samples drawn from demonstration data of policy rollouts, rather than the play data generated by human operators.
We note that we also tried samples from the play dataset, but found many of the goals unreachable by the policy, and overall worse performance.
See the Appendix for detail on parameters and tradeoffs.
Metrics.
We evaluated the performance of BELT across three metrics intended to capture both its ability to plan and how robust those plans are.
“Solution Found” is the rate at which the planner returns that it has found a solution that satisﬁes the success criteria, capturing an algorithm’s ability to explore and plan.
“Success Rate” is the rate at which the solution is successfully executed if it is found (in this work we rollout successful solutions nine times), capturing how robust the plan is.
This robustness may be with respect to a stochastic policy, errors in the learned model, or a stochastic environment.
“Feasibility” is the rate at which any of the replays for a given solution was able to solve the problem, capturing whether the plan was feasible (partially isolating the impact of an inconsistent policy).
These are discussed in more detail in Appendix Section 5.7.
3.2 Results
We begin by comparing the overall performance of BELT, different models, and the baselines in
Table 1 on the chain tasks and Fig.
5 shows several planned trajectories.
We study each algorithm’s performance on the axis of ﬁnding solutions (based on the algorithm output) and on robustness.
Chain-LMP shows the upper bound of robustness of the policy to repeat four successive tasks.
CEM with the simulator and LMP with a single-goal state fail to solve most chains.
CEM with the model solves more tasks, but the solutions are not robust as the search seems to exploit inaccuracies in the model.
BELT with the action model too exploits model inaccuracies – the model quickly becomes unstable as it is recursively rolled out over every timestep.
Solutions are found for all chains, but the solutions are not robust or feasible for most, possibly due to impossible states input to the success detectors.
The task model performs much better due to its temporally extended prediction, though we note multistep predictions hold promise to improve the action-conditioned results [33].
BELT with the task model and the simulator perform well, solving 66% and 82% of the problems respectively, and replay at similar rates and only slightly worse than the oracle.
This demonstrates
BELT’s ability to both search the space of solutions and ﬁnd feasible plans.
The task model solves 16% fewer problems than directly using the simulator.
[Table]
[Table] [Table]
In Table
[Table]
we analyze which tasks proved most difﬁcult for BELT, and particularly where the task-conditioned model and the simulator performance most diverged.
For both the simulator and task-model, the button presses proved to be part of the most difﬁcult chains to plan and execute.
This is a because the button presses are transient and do not leave a permanent state change, which is particularly challenging for a goal-conditioned low-level policy to well cover.
The task-model has difﬁculty with chains with the “rotate left” task on the block, while the simulator plans quite well over them but fail to execute many.
This is a result of the discontinuity of angles, i.e., if the block starts at 0 radians and is rotated left, the angle jumps to 2 ⇡ radians, which is particularly difﬁcult for the model.
This indicates a need for an angular representation such as quaternions or axis-angles.
Conclusion and Future Work
Conclusions.
This work presented BELT, an algorithm capable of planning long-horizon tasks.
BELT leverages the rapid state space exploration of RRT, the robust and general local performance of learned task-conditioned policies, and the long-horizon reasoning of a task-conditioned model.
It is demonstrated in a realistic, complex, manipulation environment with a goal-conditioned policy to be capable of planning consistently and robustly.
Real-world Experiments.
This method builds upon a scalable approach to learning goal-conditioned low-level policies ([38]), which were demonstrated to yield high success rate on the 18-task benchmark using onboard sensors only.
While these results were obtained in simulation, the scalability of the local policy learning makes real-world deployment promising.
Our work also relies on task success detectors, real-world experiments could make use of learned success classiﬁers such as [43].
Hence we estimate that there is a reasonable and scalable path to real-world experiments and plan to explore this in future work.
Future Work.
In the future we wish to show how BELT can be extended well beyond the dataset and to other task representations, state representations (e.g., images), and models.
We further wish to study how it can be used in replanning, particularly how the tree may be reused, as discussed in
Appendix Section 5.8.
We also wish to study how BELT can be used with constraints on obstacles, states, or model feasibility.
Finally, we wish to study methods for generating not just any feasible plan, but robust and efﬁcient plans.
Acknowledgments
The authors wish to thank Karol Hausman, Vincent Vanhoucke, Alexander Toshev and Aleksandra
Faust for helpful discussions.