Broadly-Exploring, Local-Policy Trees for Long-Horizon Task Planning
Anonymous Author(s)
AfﬁliationAddress email
Abstract: Long-horizon planning in realistic environments requires the ability toreason over sequential tasks in high-dimensional state spaces with complex dynamics.
Classical motion planning algorithms, such as rapidly-exploring randomtrees, are capable of efﬁciently exploring large state spaces and computing longhorizon, sequential plans.
However, these algorithms are generally challengedwith complex, stochastic, and high-dimensional state spaces as well as in thepresence of small, topologically complex goal regions, which naturally emergein tasks that interact with the environment.
Machine learning offers a promisingsolution for its ability to learn general policies that can handle complex interactions and high-dimensional observations.
However, these policies are generallylimited in horizon length.
Our approach, Broadly-Exploring, Local-policy Trees(BELT), merges these two approaches to leverage the strengths of both througha task-conditioned, model-based tree search.
BELT uses an RRT-inspired treesearch to efﬁciently explore the state space.
Locally, the exploration is guidedby a task-conditioned, learned policy capable of performing general short-horizontasks.
This task space can be quite general and abstract; its only requirementsare to be sampleable and to well-cover the space of useful tasks.
This search isaided by a task-conditioned model that temporally extends dynamics propagationto allow long-horizon search and sequential reasoning over tasks.
BELT is demonstrated experimentally to be able to plan long-horizon, sequential trajectories witha 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 ofreasoning over sequences of subtasks.
It must broadly search the space of possible trajectorieswhile considering both what the subtasks are and the order in which they are accomplished.
It musthandle the complex and high dimensional state space and dynamics of the real world.
Consider, forexample, a robot cleaning a kitchen.
A planner must ﬁnd trajectories to put a utensil away, to washa pot, to throw away trash, and more.
Each of these tasks requires the ability to understand andexecute lower-level tasks involved, as well as their order; to put a utensil away, the robot must ﬁrstopen 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, stochasticdynamics, 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 problemswith 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 exploresthe state space in an RRT-like manner, but where edges maintain a consistent task executed by alearned, task-conditioned policy (Fig. 1).
In this work, we deﬁne task in a general sense, witha 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 taskembeddings.
The local, task-conditioned policy is thus capable of executing complex short-horizontasks.
The skeleton of the RRT search allows rapid exploration of the state space.
This exploration isaided by a task-conditioned model, capable of rolling out long-horizon trajectories, which can then
[Figure]
be planned over and reasoned about in a model-based manner to the limit of the known, predictableenvironment.
Broadly-Exploring, Local-policy Trees (BELT) is thus able to create long-horizonplans 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 interactwith the world, planning algorithms often consider the task and motion planning setting (contrastingwith AI planning).
Approaches in this setting generally plan in a hierarchical manner by deﬁningexplicit task-primitives, symbolic languages, or transition logic constraints. These methods thenplan 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 tasksrobustly.
BELT approaches such problems through minimal constraints and structure on the taskspace, 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 havedifﬁculty reasoning over long horizons.
To enable long-horizon performance, previous works oftenenforce 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-levelpolicy must reason over [19].
Recent work on goal-conditioned reinforcement learning learns toefﬁ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 classicalmotion 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 equippedwith a task-conditioned model to enable searching for plans outside of previous experience and iscapable 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 modellevel.
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, suchas 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 thestate 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) thatis 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-inspiredsearch, ( 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 propagationand reasoning of a learned, temporally extended task-conditioned model.
BELT is demonstratedwith a learned goal-conditioned policy in a realistic environment to plan effectively, signiﬁcantlyoutperforming 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 algorithmicsubroutines in Sections 2.1 - 2.3.
Brieﬂy, BELT searches for long-horizon, sequential plans via amodel-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 theefﬁ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 considerplanning in environments which are predictable and known, with a fully observed state space X .
Theonly requirements of the task space Z is that it is sampleable and well-covers the space of usefulactions.
For example, this can be explicitly given as a set of tasks, implicitly learned via a latent taskspace, or structured as the state space X with a goal-conditioned policy.
To allow for general taskdeﬁ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- 103 tion allows for a general problem deﬁnition that includes goal-conditioned planning, but also allowsfor transient tasks that cannot be expressed by a goal state.
The cost used in this work, referred tointerchangeably with distance, is the time of a trajectory, though the approach is general to othercosts.
The environment considered in this work is continuous, known, and static, with the exceptionof 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.
2shows BELT with a goal-conditioned policy that is learned from play [38], and Fig.
3 shows BELTas applied to the experimental environment used herein.
A new planning problem is deﬁned by aninitial state, x init , and by success criteria Success .
The search begins by initializing a tree T with 115 x init as its root (Line 1) and iteratively adding task-conditioned edges.
For each iteration i , a state 116 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 118 t given the current state x t and task z i .
The goal of the task space is to well cover the state spaceand guide the search towards useful actions; the task space and policy used herein are discussed indetail in Section 2.1.
The goal of the selection of the tree node x expand is to guide the search towardsunexplored regions and to exploit promising paths; this is discussed in detail in Section 2.2.
Giventhe 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 124 goal of the model is to provide accurate predictions of trajectories given a task (in the absence ofknown dynamics); the model used herein is discussed in more detail in Section 2.3.
In this way, eachedge may be thought of having a temporally extended consistent intent, such as attempting a singletask or reaching a goal.
This edge is then added to the tree and the trajectory from x init to x expandto x new is checked for success with Success (Line 8).
This search continues for N iterations, atwhich 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] [Table]
[Table] [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 unsegmentedteleoperated play dataset D of semantically meaningful behaviors provided by users, without a set 134 of predeﬁned tasks in mind.
For example, a user might open and close doors, rearrange objects, orsimply 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 137 each visited state in the dataset as a “reached goal state”, with the preceding states and actions treatedas optimal behavior for reaching that goal.
Relabeling yields a dataset of D play = { ( ⌧ , x g ) i } D play i =0 , 139 where each example consists of a goal state x g and a demonstration ⌧ = { ( x 0 , a 0 ) , ... } solving for 140 the goal.
These can be fed to a simple maximum likelihood goal conditioned imitation objective,
L LfP =
E ( ⌧ , x g ) ⇠ D play h P | ⌧ | t =0 log ⇡ ✓ ( a t | x t , x g ) 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 priorhuman 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 146 (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 trainedto reconstruct input actions, conditioned on state x t , goal x g , and an inferred plan z for how to getfrom 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 broadlysearch 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 isknown as the Voronoi bias.
For an RRT, this bias states that at each iteration, the selection of the treenode 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 nodein RRT, a sample is drawn from the state space and the nearest node within the tree is selected as theexpansion node.
For BELT, we seek to leverage a Voronoi bias via a temporal distance  t betweenstates x i , x j 2 X as the L2 distance is not a good distance for this problem.
For example, consider 163 the block in Fig.
3b.
If it is inside the drawer or on top of the desk, the L2 distance is quite small, butto connect these two states requires opening the drawer, grasping the block, placing the block, andclosing the drawer – a signiﬁcant distance.
As the temporal distance is not easily quantiﬁable, welearn 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 overpolicy 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 labeldistributions 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 demonstrationdataset from rollouts of the policy x sample 2 D ⇡ .
The temporal distance is then predicted between 175 x sample and each node in the tree.
All nodes below a cutoff temporal distance d cutoff are then addedto a set of possible nodes to expand from.
Within this set the node with the lowest-cost to come isselected for expansion, x expand .
This biases the tree search towards nodes that are capable of reachingthe 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 studiedexperimentally 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 planningis to goal bias, i.e., biasing samples towards states within the goal region.
As success often requiresfull sequences of trajectories rather than individual states we do not generally have access to goalstates 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.
4band 4c.
Access to a simulator or the exact dynamics is assumed for much of the motion planningcommunity when planning (unless explicitly planning under uncertainty).
This acts as an upperbound herein and tests the ability of BELT to explore the state space and plan sequential tasks givena perfect model.
It also shows bounds on the robustness of our policy.
The action-conditioned modellearns 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 194 trajectory, this is recursively applied for T timesteps.
The task-conditioned model learns a predictionof where the robot will be after T timesteps, i.e., p task ( x t + T | x t , z ) .
This accounts for the closed- 196 loop nature of the policy for a ﬁxed task and temporally extends the prediction to reduce recursiveerrors.
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 policyrollouts 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 =
||
ˆ x t +
x t +
|| 201 with ˆ x t +1 ⇠ 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]
[Figure]
Experimental Results
The experiments herein are based on a simulated “playground environment” [38] (Discussed in moredetail 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 ofthe cartesian position and orientation of all objects including its end effector.
The agent performshigh-frequency, closed-loop control, sending continuous position, rotation, and gripper commandsto its arm at 30 Hz.
The environment is known and static (unless the direct result of interaction withthe robot) and contains a desk with a sliding door and drawer that can be opened and closed as wellas 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 coreshort-horizon manipulation tasks.
See [38, 41] for a complete discussion of the 18 tasks, which spandifferent families, e.g. pushing buttons, opening and closing doors/drawers, grasping and lifting ablock, 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 inthe 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 stateto 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 greenbutton, 3) close drawer” may appear to an agent as “no change to the environment”.
We consider amore 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 referto this here as the “success detector”, which our high-level agent plans against.
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 toselect four sequential tasks.
The tasks were then rolled out with the simulator and task-model.
Thedistribution was then reﬁt to successful trajectories.
We also compare to a Play-LMP policy thatis given a single goal-state that satisﬁes all the subtasks (this single goal state existed for 36% ofthe chains).
This baseline demonstrates that a simple approach of using long-horizon goals with apolicy trained on short-horizons performs poorly and that BELT is able to plan long-horizons withlocal policies.
Finally, we also compare to an “oracle” LMP policy, Chain-LMP, which is given thefour chain tasks in a row, demonstrating an upper bound of what the BELT plan may achieve giventhe performance of the underlying policy.
Parameters.
The low-level policy is trained with the same play logs collected in [38]; ⇠ 7h ofteleoperated play relabeled into D play , containing ⇠ 10M short-horizon demonstrations, each 1-2 243 seconds (see video for examples of this data).
The TDC and models were trained on demonstrationdata for ⇠ 100k rollouts.
⌧ tdc was trained on this dataset to an accuracy of 82% and d cutoff was set 245 to < 64 based on analysis in the next section.
The edges were rolled out with a randomly drawntimestep of T 2 { 32 , 64 , 96 } , as inspired by [10].
BELT used 2500 goal samples drawn from 247 demonstration data of policy rollouts, rather than the play data generated by human operators.
Wenote that we also tried samples from the play dataset, but found many of the goals unreachable bythe 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 itsability to plan and how robust those plans are.
“Solution Found” is the rate at which the plannerreturns that it has found a solution that satisﬁes the success criteria, capturing an algorithm’s abilityto explore and plan. “Success Rate” is the rate at which the solution is successfully executed if itis found (in this work we rollout successful solutions nine times), capturing how robust the planis. This robustness may be with respect to a stochastic policy, errors in the learned model, or astochastic environment. “Feasibility” is the rate at which any of the replays for a given solution wasable to solve the problem, capturing whether the plan was feasible (partially isolating the impact ofan 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’sperformance 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.
CEMwith the simulator and LMP with a single-goal state fail to solve most chains.
CEM with the modelsolves more tasks, but the solutions are not robust as the search seems to exploit inaccuracies in themodel.
BELT with the action model too exploits model inaccuracies – the model quickly becomesunstable as it is recursively rolled out over every timestep.
Solutions are found for all chains, but thesolutions are not robust or feasible for most, possibly due to impossible states input to the successdetectors.
The task model performs much better due to its temporally extended prediction, thoughwe 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 problemsrespectively, 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 solves16% fewer problems than directly using the
[Table]
[Table]
[Table] In Table [Table] we analyze which tasks proved most difﬁcult for BELT, and particularly where thetask-conditioned model and the simulator performance most diverged.
For both the simulator andtask-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, whichis particularly challenging for a goal-conditioned low-level policy to well cover.
The task-model hasdifﬁculty with chains with the “rotate left” task on the block, while the simulator plans quite wellover them but fail to execute many.
This is a result of the discontinuity of angles, i.e., if the blockstarts at 0 radians and is rotated left, the angle jumps to 2 ⇡ radians, which is particularly difﬁcult forthe 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 performanceof learned task-conditioned policies, and the long-horizon reasoning of a task-conditioned model.
Itis demonstrated in a realistic, complex, manipulation environment with a goal-conditioned policy tobe capable of planning consistently and robustly.
Real-world Experiments.
This method builds upon a scalable approach to learning goal-conditionedlow-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 scalabilityof the local policy learning makes real-world deployment promising.
Our work also relies on tasksuccess 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 planto explore this in future work.
Future Work.
In the future we wish to show how BELT can be extended well beyond the datasetand to other task representations, state representations (e.g., images), and models.
We further wishto 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 feasibleplan, but robust and efﬁcient plans.