\section{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 find 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 first open a drawer, then grasp the utensil, then place it in the drawer, and finally close the drawer.

Classical planning algorithms are capable of efficiently and broadly searching state spaces and planning over long-horizons \cite{lavalle2001randomized,karaman2011sampling}.
However, they have difficulty in the presence of complex, stochastic dynamics, high-dimensional systems, and so-called narrow passages (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 
\cite{andrychowicz2017hindsight,bacon2017option,fox2017multi,eysenbach2018diversity,xu2018neural}.
However, many such approaches are limited to short-horizons \cite{nachum2018data,gupta2019relay}.

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. \ref{fig:teaser}). 
In this work, we define 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 environment.
Broadly-Exploring, Local-policy Trees\xspace (BELT\xspace) is thus able to create long-horizon plans with sequential, complex tasks.

\begin{figure}[b!]
\begin{subfigure}{.232\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_init}  
  \caption{Initialize}
  \label{fig:toy_init}
\end{subfigure}
\begin{subfigure}{.24\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_iter}  
  \caption{Sample a task and rollout}
  \label{fig:toy_iter}
\end{subfigure}
\begin{subfigure}{.246\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_search}  
  \caption{Iterate}
  \label{fig:toy_search}
\end{subfigure}
\begin{subfigure}{.24\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_soln}  
  \caption{Return solution}
  \label{fig:toy_soln}
\end{subfigure}
\caption{BELT\xspace plans long-horizon, sequential trajectories via a task-conditioned tree and task-model.}
\label{fig:teaser}
\end{figure}

\subsection{Related Work.}

\textit{Planning Algorithms} are capable of efficiently, and globally, exploring state spaces to plan long-horizon trajectories which avoid collision \cite{lavalle2001randomized,karaman2011sampling,li2015sparse}.
When robots must interact with the world, planning algorithms often consider the task and motion planning setting.
Approaches in this setting generally plan in a hierarchical manner by defining task-primitives and planning over them, for example, via trees or optimization-based methods \cite{kaelbling2010hierarchical, kaelbling2013integrated, hauser2009integrating, nau2003shop2, wolfe2010combined, toussaint2015logic}.
These methods perform well in long-horizon planning, but are limited by their task representations, the dimensionality of the search space, and the ability to execute high-dimensional, complex tasks robustly.

\textit{Model-Free Learning} offers a promising direction to execute such tasks, though theses methods have difficulty reasoning over long horizons.
To enable long-horizon performance, previous works often enforce hierarchy or embed policies within classical structure.
Hierarchical reinforcement learning \cite{bacon2017option,fox2017multi,vezhnevets2017feudal} works to temporally extend planning at lower levels, reducing the horizon the high-level policy must reason over \cite{nachum2019does}.
Recent work on goal-conditioned reinforcement learning learns to efficiently achieve arbitrary goal states \cite{kaelbling1993learning, andrychowicz2017hindsight}.
This casts the problem for the high-level policy as finding a sequence of goal states \cite{nachum2018data, gupta2019relay, nair2019hierarchical, nair2018visual}.
This also allows the embedding of policies in classical motion planning structures \cite{faust2018prm,chiang2019rl,savinov2018semi,paxton2017combining,eysenbach2019search,zhang2018composable,ichter2018learning,qureshi2019motion}.
Search on the replay buffer (SoRB) \cite{eysenbach2019search} for example builds a searchable graph of the replay buffer given a learned distance metric for a goal-conditioned policy.
In contrast to SoRB and other previous work, BELT\xspace 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.

\textit{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, \cite{kaiser2019model, schrittwieser2019mastering}.
To extend the performance of models to longer-horizons, models can be trained for multi-step prediction \cite{hafner2019learning} and planned over \cite{hafner2019dream}.
Others use models within classical algorithms, such as sampling-based motion planning \cite{ichter2019robot} or control \cite{watter2015embed,levine2019prediction}.
In this work, we leverage a task-conditioned 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.

\subsection{Statement of Contributions.}

The contribution of this paper is an algorithm Broadly-Exploring, Local-policy Trees\xspace (BELT\xspace) that is capable of planning long-horizon, sequential tasks in complex environments. 
BELT\xspace leverages 
(\textbf{1} -- Sections \ref{sec:alg} and \ref{sec:bias}) the state space exploration abilities of planning through an RRT-inspired search,
(\textbf{2} -- Section \ref{sec:play}) the local guidance of a learned task-conditioned policy, capable of executing complex tasks in high-dimensional spaces, and 
(\textbf{3} -- Section \ref{sec:model}) the long-horizon propagation and reasoning of a learned, temporally extended task-conditioned model.
BELT\xspace is demonstrated with a learned goal-conditioned policy in a realistic environment to plan effectively, significantly outperforming baselines.
Furthermore, these plans are directly executable by the learned policy.

\section{Broadly-Exploring, Local-policy Trees\xspace}\label{sec:alg}

We begin with a general outline of BELT\xspace and then discuss concrete implementations of algorithmic subroutines in Sections \ref{sec:play} - \ref{sec:model}.
Briefly, BELT\xspace 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 efficient global-exploration of RRTs, the local directed actions of a learned task-conditioned policy, and the sequential reasoning of a model.

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.
The only requirements of the task space $\mathcal{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 $\mathcal{X}$ with a goal-conditioned policy.
To allow for general task definitions, a success criteria $\texttt{Success}$ is used that is considered a ``black box''. Given a trajectory $\{x_\text{init}, ..., x_T\}$, $\texttt{Success}$ outputs if the trajectory satisfies all necessary conditions.
This formulation allows for a general problem definition that includes goal-conditioned planning, but also allows for transient tasks that cannot be expressed by a goal state. 
The environment considered in this work is 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.

\begin{figure}[t]
\centering
\includegraphics[width=0.9\textwidth]{belt_teaser.pdf}
\caption{BELT\xspace with Play Latent Motor Plans \cite{lynch2020learning} learns a general, goal-conditioned policy as well as a model from teleoperation data. Given a long-horizon task, BELT\xspace translates this into an RRT-inspired, model-based tree search through the space, where each edge represents a single task sampled from demonstration data. Trajectories are verified successful via a trajectory-wise success check on the model and executed by the policy.}
\end{figure}\label{fig:belt_teaser}

The search is outlined in Algorithm \ref{alg} and BELT\xspace is shown with progressively more concreteness in Figs.~\ref{fig:teaser}~-~\ref{fig:play_belt}. Fig.~\ref{fig:teaser} shows a hypothetical form of BELT\xspace with an explicit task space, Fig.~\ref{fig:belt_teaser} shows BELT\xspace with a goal-conditioned policy that is learned from play \cite{lynch2020learning}, and Fig.~\ref{fig:play_belt} shows BELT\xspace as applied to the experimental environment used herein. A new planning problem is defined by an initial state, $x_\text{init}$, and by success criteria $\texttt{Success}$.
The search begins by initializing a tree $\mathcal{T}$ with $x_\text{init}$ as its root and iteratively adding task-conditioned edges.
For each iteration $i$, a state $x_\text{expand} \in \mathcal{T}$ within the tree is selected, along with a task $z_i \in \mathcal{Z}$.
BELT\xspace requires a sampleable task space $\mathcal{Z}$ and task-conditioned policy, $\pi(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 \ref{sec:play}.
The goal of the selection of the tree node $x_\text{expand}$ is to guide the search towards unexplored regions and to exploit promising paths; this is discussed in detail in Section \ref{sec:bias}.
Given the task $z$ and state $x_\text{expand}$, the policy is then propagated for $T$ timesteps via a model $p(x_{t+1} | x_t, a_t)$ to create trajectory $\{x_\text{expand}, x_1, ..., x_{T-1}, x_\text{new}\}$. 
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 \ref{sec:model}.
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_\text{expand}$ to $x_\text{new}$ is checked for success with $\texttt{Success}$. 
This search continues for $N$ iterations, at which time the lowest-cost path marked as successful is returned if it exists.

\begin{figure}[t]
\centering
\begin{subfigure}{.388\textwidth}
  \centering
  \includegraphics[width=\linewidth]{tree_lmp_model.pdf}  
  \caption{Policy takes as input a goal state, the current state and samples from its latent ``plan'' space. This is then input to the model.}
  \label{fig:play_train}
\end{subfigure}
\begin{subfigure}{.53\textwidth}
  \centering
  \includegraphics[width=\linewidth]{sideways_tree_lmp_search.pdf}  
  \caption{At each iteration BELT\xspace samples a goal from demonstration data and a tree state and rolls out a task-conditioned model to add edges to the tree. The solution trajectory is selected as the lowest cost solution, as verified by the binary success check $\texttt{Success}$. Larger figure  in Appendix Section \ref{sec:full_figs}.}
  \label{fig:search}
\end{subfigure}
\caption{Broadly-Exploring, Local-policy Trees\xspace with a Play-LMP policy (Section \ref{sec:play}).}
\label{fig:play_belt}
\end{figure}

\begin{algorithm}
\small
\caption{Broadly-Exploring, Local-policy Trees\xspace}
\begin{algorithmic}[1]
\Require initial state $x_\text{init}$, sample count $N$, and success check $\texttt{Success}$
    \State Initialize tree $\mathcal{T}$ with state $x_\text{init}$.
    \For{$i$ in $1:N$}  
        \State Sample a task $z_i$ from task space $\mathcal{Z}$.\label{line:sample_task} \Comment{Discussed in Section \ref{sec:play}}
        \State Select a state $x_\text{expand}$ from $\mathcal{T}$ to expand from. \label{line:sample_node} \Comment{Discussed in Section \ref{sec:bias}}
        \For{$t$ in $1:T$}
        \State Run policy $a_t \sim \pi(a_t | x_t, z_i)$.  \Comment{Discussed in Section \ref{sec:play}}
        \State Rollout model $x_{t+1} \sim p(x_{t+1} | x_t, a_t)$. \Comment{Discussed in Section \ref{sec:model}}
        \EndFor
        \State Update $\mathcal{T}$ with state $x_i$, trajectory $\{x_\text{expand}, ..., x_i\}$, task $z_i$, and $\texttt{Success}(\{x_\text{init}, ..., x_i\})$.
    \EndFor \label{loop}
    \State \Return minimum time successful trajectory or failure if none
\end{algorithmic}\label{alg}
\end{algorithm}

\subsection{Learning a Latent Task Space and Task-Conditioned Policy From Play}\label{sec:play}

In this work we implement BELT\xspace with a general-purpose goal-reaching policy at the low level, called Learning from Play (LfP) \cite{lynch2020learning}. To learn from ``play'' we assume access to an unsegmented teleoperated play dataset $\mathcal{D}$ of semantically meaningful behaviors provided by users, without a set of predefined tasks in mind. For example, a user might open and close doors, rearrange objects on a desk, or simply tidy up the scene.

To learn control, this long temporal state-action log $\mathcal{D} = \{(x_t, a_t)\}^{\infty}_{t=0}$ is relabeled \cite{andrychowicz2017hindsight,kaelbling1993learning}, 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_{\operatorname{play}} = \{(\tau, x_g)_i\}^{D_{\operatorname{play}}}_{i=0}$, where each example consists of a goal state $x_g$ and a demonstration $\tau = \{(x_0, a_0), ... \}$ solving for the goal.
These can be fed to a simple maximum likelihood goal conditioned imitation objective, 
$\mathcal{L}_{\operatorname{LfP}} = \mathbb{E}_{(\tau,\ x_g) \sim D_{\operatorname{play}}}\left[\sum^{|\tau|}_{t=0}\operatorname{log} \pi _{\theta}\left( a_{t}| x_t, x_g\right)\right]$, to learn policy $\pi _{\theta}\left( a_{t}| x_t, x_g\right)$, parameterized by $\theta$. 
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 $\pi _{\theta}\left( a_{t}| x_t, x_g\right)$. For direct comparison to prior work, we use Play Supervised Latent Motor Plans (Play-LMP) \cite{lynch2020learning}. Play-LMP addresses the inherent multimodality in free-form imitation datasets. Concretely it is a sequence-to-sequence conditional variational autoencoder (seq2seq CVAE), autoencoding 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. \ref{fig:play_train}.
See \cite{lynch2020learning} for details.

\subsection{Biasing the Search}\label{sec:bias}

Given a goal-conditioned policy, as Play-LMP is, RRT \cite{lavalle2001randomized} is used with a few adaptions to broadly search the state space, chose achievable edge tasks, and bias towards low-cost paths (Fig. \ref{fig:bias}).

To broadly search the state space, a key component of RRT's ability to efficiently 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 \cite{lavalle2001randomized}.
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\xspace, we seek to leverage a Voronoi bias via a temporal distance $\Delta t$ between states  $x_i, x_j \in \mathcal{X}$ as the L2 distance is not a good distance for this problem.
For example, consider the block in Fig. \ref{fig:search}. 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 significant distance.
As the temporal distance is not easily quantifiable, we learn to predict it with a temporal distance classifier, $\tau_\text{tdc}(x_i, x_j)$, with $K$ exponential categories corresponding to temporal distance intervals, $d_k \in \{[0,1), [1,2), [2,4), [4,8), [8,16), ..., [128,256)\}$ based on \cite{aytar2018playing}.
The temporal distance classifier is implemented as a neural network and trained over policy rollouts to predict the distribution over class labels via a cross-entropy classification loss, $\mathcal{L}_\text{tdc} = -\sum^K_{l=1} y_l \log(\hat{y}_l)$ with $\hat{y} = \tau_\text{tdc}(x_i, x_j)$,
where $y$ and $\hat{y}$ are the true and predicted label distributions respectively.
Briefly, 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 first selecting a goal state  from a demonstration dataset from rollouts of the policy $x_\text{sample} \in \mathcal{D_\pi}$.
The temporal distance is then predicted between $x_\text{sample}$ and each node in the tree.
All nodes below a cutoff temporal distance $d_\text{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_\text{expand}$.
This biases the tree search towards nodes that are capable of reaching the goal state and towards nodes with efficient trajectories. 
It further allows a Voronoi bias by temporal distance.
The value of $d_\text{cutoff}$ represents a trade-off between exploiting more promising paths (larger values of $d_\text{cutoff}$) and exploring new ones (smaller values of $d_\text{cutoff}$); this trade-off is studied experimentally in the Appendix.
This procedure modifies lines \ref{line:sample_task} and \ref{line:sample_node} of Algorithm \ref{alg}.

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.

\begin{figure}[b!]
\centering
\begin{subfigure}{.4\textwidth}
  \centering
  \includegraphics[width=\linewidth]{tree_bias}  
  \caption{Biasing the tree search}
  \label{fig:bias}
\end{subfigure}
\begin{subfigure}{.45\textwidth}
  \centering
  \includegraphics[width=\linewidth]{models.pdf}  
  \caption{Task- and action-models}
  \label{fig:models}
\end{subfigure}
\begin{subfigure}{\textwidth}
  \centering
  \includegraphics[width=\linewidth]{model_comparison_1.pdf}  
  \caption{Task-models, action-model, and simulator our experiment environment (\href{https://youtu.be/zCJpNPn0BZQ?t=162}{video link}). Larger figure available in Appendix Section \ref{sec:full_figs}.}
  \label{fig:models_data}
\end{subfigure}
\caption{(\ref{fig:bias}) shows the bias used by BELT\xspace to choose the tree state $x_\text{expand}$ once $x_\text{sample}$ has been sampled. The blue region shows how the temporal distance between states may differ from the L2 distance, necessitating learning a temporal distance classifier. The choice between state 1 and 3 demonstrates the bias towards lower cost paths: though state 1 is closer to $x_\text{sample}$, node 3 has a much lower cost to come, and thus state 3 is selected.
(\ref{fig:models}-\ref{fig:models_data}) shows the two types of models used in this work, an action- and a task-conditioned model. 
The action-conditioned model often exhibits compounding errors as it is recursively applied along the trajectory, while the task-conditioned model avoids this by temporally extending the prediction and conditioning on the fixed task for the edge.
This compounding error can be seen in the second task (block lifting) where the action-model becomes unstable and the end effector flails (\href{https://youtu.be/zCJpNPn0BZQ?t=162}{video link}).}
\end{figure}

\subsection{Learned Task-Conditioned Model}\label{sec: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. \ref{fig:models} and \ref{fig:models_data}.
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\xspace 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_\text{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 the where the robot will be after $T$ timesteps, i.e., $p_\text{task}(x_{t+T} | x_t, z)$.
This accounts for the closed-loop nature of the policy for a fixed 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 $\mathcal{L}_{p_\text{action}} = \sum_{t=0}^{T-1} ||\hat{x}_{t+1} - x_{t+1}||$ with $\hat{x}_{t+1} \sim p_\text{action}(x_t, a_t)$, while the task-model seeks to minimize $\mathcal{L}_{p_\text{task}} = ||\hat{x}_T - x_T||$ with $\hat{x}_T \sim p_\text{task}(x_0, z)$.

\section{Experimental Results}\label{sec:result}

\subsection{Experimental Setup}\label{sec:setup}

The experiments herein are based on a simulated ``playground environment'' \citep{lynch2020learning} (Fig~\ref{fig:search} and discussed in more detail in the Appendix) built in Mujoco \citep{todorov2012mujoco}. 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. The robot is a position-controlled 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.

\textbf{Long-horizon evaluation} We evaluate our model and its baselines on large set of challenging long-horizon manipulation tasks as in \cite{lynch2020language}, 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 \cite{lynch2020learning, lynch2020language} 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. \ref{fig:chain4}). 

Note that prior work \cite{lynch2020language} assumes a human is in the loop providing each next subtask to the agent in the sequence. In our setup, an agent must provide \textit{itself} subgoals to achieve the multi-stage behavior. Other prior long-horizon evaluations, such as \cite{gupta2019relay}, describe the long-horizon task as a final goal state to reach. While this valid for many useful manipulation tasks, many are impossible to infer from the final 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 final 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 specification scheme---an indicator function, defined 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.

\subsection{Algorithmic Parameters and Baselines}

\textit{Baselines.} 
We compare against two baselines that use the same underlying Play-supervised Latent Motor Plan (Play-LMP) \cite{lynch2020learning} policy.
We compare to a model-based planner (cross entropy method (CEM) \cite{rubinstein2013cross}) 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 refit to successful trajectories.
We also compare to a Play-LMP policy that is given a single goal-state that satisfies all the subtasks (this single goal state existed for 36\% of the chains).
This baseline demonstrates the limitations of the model-free policy to execute long-horizon tasks.
Finally, we also compare to an ``oracle'' LMP policy, which is given the four chain tasks in a row, demonstrating an upper bound of what the BELT\xspace plan may achieve given the underlying policy.

\textit{Parameters.} 
The low-level policy is trained with the same play logs collected in \cite{lynch2020learning}; $\sim$7h of teleoperated play relabeled into $D_{\operatorname{play}}$, containing $\sim$10M short-horizon demonstrations, each 1-2 seconds (see video examples of this data \href{\lfpwebsite/assets/mp4/play_data516x360.mp4}{here}).
BELT\xspace used 2500 samples drawn from demonstration data of the policy rollouts.
$\tau_\text{tdc}$ was trained on this dataset to an accuracy of 82\% and $d_\text{cutoff}$ was set to $<64$.
The edges were rolled out with a randomly drawn timestep of $T\in\{32, 64, 96\}$, as inspired by \cite{li2015sparse}.
More detail on the BELT\xspace's algorithmic parameters and tradeoffs are detailed in the Appendix.

\subsection{Results}

We begin by comparing the overall performance of BELT\xspace, different models, and the baselines in Table \ref{tab:all} on the chain tasks and Fig. \ref{fig:trajs} shows several planned trajectories.
We study each algorithm's performance on the axis of finding 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\xspace 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 even feasible for most.
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 \cite{hafner2019learning}.

BELT\xspace 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\xspace's ability to both search the space of solutions and find feasible plans.
The task model solves 16\% fewer problems than directly using the simulator.

\begin{table}[b]
\small
\centering
\begin{tabular}{r c c c c} 
 Algorithm & Model & Solution Found & Success Rate & Feasible \\ 
 \hline
 Chain-LMP (Oracle) & -- & -- & 32\% & 87\% \\ 
 LMP & -- & -- & 6\% & 21\% \\ 
 CEM & Simulator& 3\% & 40\% & 58\% \\ 
 CEM & Task-Model & 11\% & 7\% & 15\% \\ 
 BELT\xspace & Action-Model & 100\% & 3\% & 8\% \\ 
 BELT\xspace & Task-Model & 66\% & 27\% & 56\%\\ 
 BELT\xspace & Simulator & 82\% & 26\% & 68\%\\ 
\end{tabular}
\caption{Success rate and robustness for BELT\xspace, CEM, and LMP. Plan robustness is measured by the percent of solutions executed successfully (success rate) and if there was at least one successful in nine attempts (feasible).
}
\label{tab:all}
\end{table}

In Table \ref{tab:tasks}, we analyze which tasks proved most difficult for BELT\xspace, 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 difficult 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 difficulty 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 \pi$ radians, which is particularly difficult for the model.
This indicates a need for an angular representation such as quaternions or axis-angles.


\begin{table}[t]
{\small
\centering
\begin{tabular}{l | c c c | c c c} 
 & \multicolumn{3}{c |}{Solution Found} & \multicolumn{3}{c}{Feasible} \\
 & BELT\xspace & BELT\xspace & & BELT\xspace & BELT\xspace & \\ 
 Chain Contains Task & Simulator & Task-Model & Ratio & Simulator & Task-Model & Ratio\\ 
 \hline
overall & 82\% & 66\%	& 0.81 & 	68\% & 56\% & 	0.82 \\
button, push any  & 73\% & 54\%	& 0.74 & 	\textbf{63\%} & \textbf{32\%} & 	\textbf{0.50} \\
drawer open/close & 82\% & 70\%	& 0.85 & 	69\% & 64\% & 	0.93 \\
grasp & 92\% &	77\%	& 0.84	& 73\% &	70\%	& 0.95 \\
knock & 91\% & 71\%	& 0.78 & 	58\% & 55\% & 	0.95 \\
rotate left & \textbf{93\%} & \textbf{0\%}	& \textbf{0.00} & 	\textbf{38\%} & -- & -- \\
rotate right & 89\% & 78\%	& 0.88 & 	77\% & 74\% & 	0.97 \\
shelf, in/out & 77\% &	56\% &	0.72 &	57\% &	54\% &	0.95 \\
slider, open/close   & 81\% & 63\%	& 0.78 & 	67\% & 58\% & 	0.87 \\
sweeping & 76\% & 64\%	& 0.84 & 	65\% & 76\% & 	1.17 \\
\end{tabular}
\caption{Task performance breakdown. The task-model particularly has difficulty planning ``rotate left'' tasks due to an angle discontinuity, and the policy has trouble executing the task for the same reason.
The task-model plans execute poorly with any buttons, likely because the model makes predictions from goal states and the button tasks are transient, thus not often appearing in goal states.}
\label{tab:tasks}
}
\end{table}

\begin{figure}[t]
\centering
\includegraphics[width=1.0\textwidth]{figs/lrtamp_planned_trajectories_few.pdf}
\caption{Plans from BELT\xspace, demonstrating its ability to plan long-horizon, sequential trajectories (\href{https://youtu.be/zCJpNPn0BZQ?t=193}{video link}). 
}
\label{fig:trajs}
\end{figure}

\section{Conclusion and Future Work}\label{sec:conclusion}

\textit{Conclusions.} This work presented BELT\xspace, an algorithm capable of planning long-horizon tasks.
BELT\xspace 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.

\textit{Real-world Experiments.} 
This method builds upon a scalable approach to learning goal-conditioned low-level policies (\cite{lynch2020learning}), 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 classifiers such as \cite{singh2019endtoend}. Hence we estimate that there is a reasonable and scalable path to real-world experiments and plan to explore this in future work.

\textit{Application to Dynamic and Unknown Environments.}
In settings where the environment is dynamic or not fully observed, BELT\xspace plans may be limited in horizon. In these settings, the BELT\xspace tree can be reused to quickly replan, either by rewiring based on changes or by planning trajectories back into the tree in case of plan divergence. Furthermore, concepts from BELT\xspace can be used to form a graph and maintain a representation of possible paths through the environment.

\textit{Future Work.} 
In the future we wish to show how BELT\xspace can be extended well beyond the dataset and to other task representations and models.
We further wish to study how it can be used quickly in replanning, and particularly how the existing tree may be reused.
Finally, we wish to study methods for generating not just any feasible plan, but robust and efficient plans.

\clearpage

\acknowledgments{The authors wish to thank Karol Hausman, Vincent Vanhoucke, Alexander Toshev, and Aleksandra Faust for helpful discussions.}


{\small

\section{Introduction}
	
    Submission to CoRL 2020 will be entirely electronic, via a web site (not email). Information about the submission process and \LaTeX{} templates are available on the conference web site at \url{http://www.robot-learning.org/}. For camera ready submission, use the \texttt{final} option for the \texttt{\textbackslash usepackage} command. 


\section{Citations}
\label{sec:citations}

	Citations can be made using either \textbackslash citep\{\} or \textbackslash citet\{\}, depending from the appropriateness. To avoid the citation moving to the next line, it is often a good practice to replace the space before with a tilde (\~{}) character.
	Example 1: ``CoRL is the best conference ever~\citep{Calandra2016}.''
	Example 2: ``\citet{Calandra2016} proved, both theoretically and numerically, that CoRL is the best conference ever.''
	

\section{Experimental Results}
\label{sec:result}

	Nam dui ligula, fringilla a, euismod sodales, sollicitudin vel, wisi. Morbi auctor lorem non justo.
	Nam lacus libero, pretium at, lobortis vitae, ultricies et, tellus. Donec aliquet, tortor sed accumsan
	bibendum, erat ligula aliquet magna, vitae ornare odio metus a mi. Morbi ac orci et nisl hendrerit
	mollis. 

	Suspendisse ut massa. Cras nec ante. Pellentesque a nulla. Cum sociis natoque penatibus
	et magnis dis parturient montes, nascetur ridiculus mus. Aliquam tincidunt urna. Nulla ullamcorper
	vestibulum turpis. Pellentesque cursus luctus mauris.
	
	Nulla malesuada porttitor diam. Donec felis erat, congue non, volutpat at, tincidunt tristique, libero.
	Vivamus viverra fermentum felis. Donec nonummy pellentesque ante. Phasellus adipiscing semper 	elit. 
	Proin fermentum massa ac quam. Sed diam turpis, molestie vitae, placerat a, molestie nec, leo.
	Maecenas lacinia. Nam ipsum ligula, eleifend at, accumsan nec, suscipit a, ipsum. Morbi blandit
	ligula feugiat magna. Nunc eleifend consequat lorem. Sed lacinia nulla vitae enim. Pellentesque tin-
	cidunt purus vel magna. Integer non enim. Praesent euismod nunc eu purus. Donec bibendum quam
	in tellus. Nullam cursus pulvinar lectus. Donec et mi. Nam vulputate metus eu enim. Vestibulum
	pellentesque felis eu massa.
	Quisque ullamcorper placerat ipsum. Cras nibh. Morbi vel justo vitae lacus tincidunt ultrices. 
	
	Lorem
	ipsum dolor sit amet, consectetuer adipiscing elit. In hac habitasse platea dictumst. Integer tempus
	convallis augue. Etiam facilisis. Nunc elementum fermentum wisi. Aenean placerat. Ut imperdiet,
	enim sed gravida sollicitudin, felis odio placerat quam, ac pulvinar elit purus eget enim. Nunc vitae
	tortor. Proin tempus nibh sit amet nisl. Vivamus quis tortor vitae risus porta vehicula.


\section{Conclusion}
\label{sec:conclusion}

	Nam dui ligula, fringilla a, euismod sodales, sollicitudin vel, wisi. Morbi auctor lorem non justo.
	Nam lacus libero, pretium at, lobortis vitae, ultricies et, tellus. Donec aliquet, tortor sed accumsan
	bibendum, erat ligula aliquet magna, vitae ornare odio metus a mi. Morbi ac orci et nisl hendrerit
	mollis. Suspendisse ut massa. Cras nec ante. Pellentesque a nulla. Cum sociis natoque penatibus
	et magnis dis parturient montes, nascetur ridiculus mus. Aliquam tincidunt urna. Nulla ullamcorper
	vestibulum turpis. Pellentesque cursus luctus mauris.
	Nulla malesuada porttitor diam. Donec felis erat, congue non, volutpat at, tincidunt tristique, libero.
	Vivamus viverra fermentum felis. Donec nonummy pellentesque ante. Phasellus adipiscing semper
	elit. Proin fermentum massa ac quam. Sed diam turpis, molestie vitae, placerat a, molestie nec, leo.
	Maecenas lacinia. Nam ipsum ligula, eleifend at, accumsan nec, suscipit a, ipsum. Morbi blandit
	ligula feugiat magna. Nunc eleifend consequat lorem. Sed lacinia nulla vitae enim. Pellentesque tin-
	cidunt purus vel magna. Integer non enim. Praesent euismod nunc eu purus. Donec bibendum quam
	in tellus. Nullam cursus pulvinar lectus. Donec et mi. Nam vulputate metus eu enim. Vestibulum
	pellentesque felis eu massa.
	Quisque ullamcorper placerat ipsum. Cras nibh. Morbi vel justo vitae lacus tincidunt ultrices. Lorem
	ipsum dolor sit amet, consectetuer adipiscing elit. In hac habitasse platea dictumst. Integer tempus
	convallis augue. Etiam facilisis. Nunc elementum fermentum wisi. Aenean placerat. Ut imperdiet,
	enim sed gravida sollicitudin, felis odio placerat quam, ac pulvinar elit purus eget enim. Nunc vitae
	tortor. Proin tempus nibh sit amet nisl. Vivamus quis tortor vitae risus porta vehicula.



\clearpage
\acknowledgments{If a paper is accepted, the final camera-ready version will (and probably should) include acknowledgments. All acknowledgments go at the end of the paper, including thanks to reviewers who gave useful comments, to colleagues who contributed to the ideas, and to funding agencies and corporate sponsors that provided financial support.}



\section{Introduction}
	
    Submission to CoRL 2020 will be entirely electronic, via a web site (not email). Information about the submission process and \LaTeX{} templates are available on the conference web site at \url{http://www.robot-learning.org/}. For camera ready submission, use the \texttt{final} option for the \texttt{\textbackslash usepackage} command. 


\section{Citations}
\label{sec:citations}

	Citations can be made using either \textbackslash citep\{\} or \textbackslash citet\{\}, depending from the appropriateness. To avoid the citation moving to the next line, it is often a good practice to replace the space before with a tilde (\~{}) character.
	Example 1: ``CoRL is the best conference ever~\citep{Calandra2016}.''
	Example 2: ``\citet{Calandra2016} proved, both theoretically and numerically, that CoRL is the best conference ever.''
	

\section{Experimental Results}
\label{sec:result}

	Nam dui ligula, fringilla a, euismod sodales, sollicitudin vel, wisi. Morbi auctor lorem non justo.
	Nam lacus libero, pretium at, lobortis vitae, ultricies et, tellus. Donec aliquet, tortor sed accumsan
	bibendum, erat ligula aliquet magna, vitae ornare odio metus a mi. Morbi ac orci et nisl hendrerit
	mollis. 

	Suspendisse ut massa. Cras nec ante. Pellentesque a nulla. Cum sociis natoque penatibus
	et magnis dis parturient montes, nascetur ridiculus mus. Aliquam tincidunt urna. Nulla ullamcorper
	vestibulum turpis. Pellentesque cursus luctus mauris.
	
	Nulla malesuada porttitor diam. Donec felis erat, congue non, volutpat at, tincidunt tristique, libero.
	Vivamus viverra fermentum felis. Donec nonummy pellentesque ante. Phasellus adipiscing semper 	elit. 
	Proin fermentum massa ac quam. Sed diam turpis, molestie vitae, placerat a, molestie nec, leo.
	Maecenas lacinia. Nam ipsum ligula, eleifend at, accumsan nec, suscipit a, ipsum. Morbi blandit
	ligula feugiat magna. Nunc eleifend consequat lorem. Sed lacinia nulla vitae enim. Pellentesque tin-
	cidunt purus vel magna. Integer non enim. Praesent euismod nunc eu purus. Donec bibendum quam
	in tellus. Nullam cursus pulvinar lectus. Donec et mi. Nam vulputate metus eu enim. Vestibulum
	pellentesque felis eu massa.
	Quisque ullamcorper placerat ipsum. Cras nibh. Morbi vel justo vitae lacus tincidunt ultrices. 
	
	Lorem
	ipsum dolor sit amet, consectetuer adipiscing elit. In hac habitasse platea dictumst. Integer tempus
	convallis augue. Etiam facilisis. Nunc elementum fermentum wisi. Aenean placerat. Ut imperdiet,
	enim sed gravida sollicitudin, felis odio placerat quam, ac pulvinar elit purus eget enim. Nunc vitae
	tortor. Proin tempus nibh sit amet nisl. Vivamus quis tortor vitae risus porta vehicula.


\section{Conclusion}
\label{sec:conclusion}

	Nam dui ligula, fringilla a, euismod sodales, sollicitudin vel, wisi. Morbi auctor lorem non justo.
	Nam lacus libero, pretium at, lobortis vitae, ultricies et, tellus. Donec aliquet, tortor sed accumsan
	bibendum, erat ligula aliquet magna, vitae ornare odio metus a mi. Morbi ac orci et nisl hendrerit
	mollis. Suspendisse ut massa. Cras nec ante. Pellentesque a nulla. Cum sociis natoque penatibus
	et magnis dis parturient montes, nascetur ridiculus mus. Aliquam tincidunt urna. Nulla ullamcorper
	vestibulum turpis. Pellentesque cursus luctus mauris.
	Nulla malesuada porttitor diam. Donec felis erat, congue non, volutpat at, tincidunt tristique, libero.
	Vivamus viverra fermentum felis. Donec nonummy pellentesque ante. Phasellus adipiscing semper
	elit. Proin fermentum massa ac quam. Sed diam turpis, molestie vitae, placerat a, molestie nec, leo.
	Maecenas lacinia. Nam ipsum ligula, eleifend at, accumsan nec, suscipit a, ipsum. Morbi blandit
	ligula feugiat magna. Nunc eleifend consequat lorem. Sed lacinia nulla vitae enim. Pellentesque tin-
	cidunt purus vel magna. Integer non enim. Praesent euismod nunc eu purus. Donec bibendum quam
	in tellus. Nullam cursus pulvinar lectus. Donec et mi. Nam vulputate metus eu enim. Vestibulum
	pellentesque felis eu massa.
	Quisque ullamcorper placerat ipsum. Cras nibh. Morbi vel justo vitae lacus tincidunt ultrices. Lorem
	ipsum dolor sit amet, consectetuer adipiscing elit. In hac habitasse platea dictumst. Integer tempus
	convallis augue. Etiam facilisis. Nunc elementum fermentum wisi. Aenean placerat. Ut imperdiet,
	enim sed gravida sollicitudin, felis odio placerat quam, ac pulvinar elit purus eget enim. Nunc vitae
	tortor. Proin tempus nibh sit amet nisl. Vivamus quis tortor vitae risus porta vehicula.



\clearpage
\acknowledgments{If a paper is accepted, the final camera-ready version will (and probably should) include acknowledgments. All acknowledgments go at the end of the paper, including thanks to reviewers who gave useful comments, to colleagues who contributed to the ideas, and to funding agencies and corporate sponsors that provided financial support.}



\section{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 find 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 first open a drawer, then grasp the utensil, then place it in the drawer, and finally close the drawer.

Classical planning algorithms are capable of efficiently and broadly searching state spaces and planning over long-horizons \cite{lavalle2001randomized,karaman2011sampling}.
However, they have difficulty in the presence of complex, stochastic dynamics, high-dimensional systems, and so-called narrow passages (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 
\cite{andrychowicz2017hindsight,bacon2017option,fox2017multi,eysenbach2018diversity,xu2018neural}.
However, many such approaches are limited to short-horizons \cite{nachum2018data,gupta2019relay}.

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. \ref{fig:teaser}). 
In this work, we define 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 environment.
Broadly-Exploring, Local-policy Trees\xspace (BELT\xspace) is thus able to create long-horizon plans with sequential, complex tasks.

\begin{figure}[b!]
\begin{subfigure}{.232\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_init}  
  \caption{Initialize}
  \label{fig:toy_init}
\end{subfigure}
\begin{subfigure}{.24\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_iter}  
  \caption{Sample a task and rollout}
  \label{fig:toy_iter}
\end{subfigure}
\begin{subfigure}{.246\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_search}  
  \caption{Iterate}
  \label{fig:toy_search}
\end{subfigure}
\begin{subfigure}{.24\textwidth}
  \centering
  \includegraphics[width=\linewidth]{toy_soln}  
  \caption{Return solution}
  \label{fig:toy_soln}
\end{subfigure}
\caption{BELT\xspace plans long-horizon, sequential trajectories via a task-conditioned tree and task-model.}
\label{fig:teaser}
\end{figure}

\subsection{Related Work.}

\textit{Planning Algorithms} are capable of efficiently, and globally, exploring state spaces to plan long-horizon trajectories which avoid collision \cite{lavalle2001randomized,karaman2011sampling,li2015sparse}.
When robots must interact with the world, planning algorithms often consider the task and motion planning setting.
Approaches in this setting generally plan in a hierarchical manner by defining task-primitives and planning over them, for example, via trees or optimization-based methods \cite{kaelbling2010hierarchical, kaelbling2013integrated, hauser2009integrating, nau2003shop2, wolfe2010combined, toussaint2015logic}.
These methods perform well in long-horizon planning, but are limited by their task representations, the dimensionality of the search space, and the ability to execute high-dimensional, complex tasks robustly.

\textit{Model-Free Learning} offers a promising direction to execute such tasks, though theses methods have difficulty reasoning over long horizons.
To enable long-horizon performance, previous works often enforce hierarchy or embed policies within classical structure.
Hierarchical reinforcement learning \cite{bacon2017option,fox2017multi,vezhnevets2017feudal} works to temporally extend planning at lower levels, reducing the horizon the high-level policy must reason over \cite{nachum2019does}.
Recent work on goal-conditioned reinforcement learning learns to efficiently achieve arbitrary goal states \cite{kaelbling1993learning, andrychowicz2017hindsight}.
This casts the problem for the high-level policy as finding a sequence of goal states \cite{nachum2018data, gupta2019relay, nair2019hierarchical, nair2018visual}.
This also allows the embedding of policies in classical motion planning structures \cite{faust2018prm,chiang2019rl,savinov2018semi,paxton2017combining,eysenbach2019search,zhang2018composable,ichter2018learning,qureshi2019motion}.
Search on the replay buffer (SoRB) \cite{eysenbach2019search} for example builds a searchable graph of the replay buffer given a learned distance metric for a goal-conditioned policy.
In contrast to SoRB and other previous work, BELT\xspace 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.

\textit{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, \cite{kaiser2019model, schrittwieser2019mastering}.
To extend the performance of models to longer-horizons, models can be trained for multi-step prediction \cite{hafner2019learning} and planned over \cite{hafner2019dream}.
Others use models within classical algorithms, such as sampling-based motion planning \cite{ichter2019robot} or control \cite{watter2015embed,levine2019prediction}.
In this work, we leverage a task-conditioned 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.

\subsection{Statement of Contributions.}

The contribution of this paper is an algorithm Broadly-Exploring, Local-policy Trees\xspace (BELT\xspace) that is capable of planning long-horizon, sequential tasks in complex environments. 
BELT\xspace leverages 
(\textbf{1} -- Sections \ref{sec:alg} and \ref{sec:bias}) the state space exploration abilities of planning through an RRT-inspired search,
(\textbf{2} -- Section \ref{sec:play}) the local guidance of a learned task-conditioned policy, capable of executing complex tasks in high-dimensional spaces, and 
(\textbf{3} -- Section \ref{sec:model}) the long-horizon propagation and reasoning of a learned, temporally extended task-conditioned model.
BELT\xspace is demonstrated with a learned goal-conditioned policy in a realistic environment to plan effectively, significantly outperforming baselines.
Furthermore, these plans are directly executable by the learned policy.

\section{Broadly-Exploring, Local-policy Trees\xspace}\label{sec:alg}

We begin with a general outline of BELT\xspace and then discuss concrete implementations of algorithmic subroutines in Sections \ref{sec:play} - \ref{sec:model}.
Briefly, BELT\xspace 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 efficient global-exploration of RRTs, the local directed actions of a learned task-conditioned policy, and the sequential reasoning of a model.

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.
The only requirements of the task space $\mathcal{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 $\mathcal{X}$ with a goal-conditioned policy.
To allow for general task definitions, a success criteria $\texttt{Success}$ is used that is considered a ``black box''. Given a trajectory $\{x_\text{init}, ..., x_T\}$, $\texttt{Success}$ outputs if the trajectory satisfies all necessary conditions.
This formulation allows for a general problem definition that includes goal-conditioned planning, but also allows for transient tasks that cannot be expressed by a goal state. 
The environment considered in this work is 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.

\begin{figure}[t]
\centering
\includegraphics[width=0.9\textwidth]{belt_teaser.pdf}
\caption{BELT\xspace with Play Latent Motor Plans \cite{lynch2020learning} learns a general, goal-conditioned policy as well as a model from teleoperation data. Given a long-horizon task, BELT\xspace translates this into an RRT-inspired, model-based tree search through the space, where each edge represents a single task sampled from demonstration data. Trajectories are verified successful via a trajectory-wise success check on the model and executed by the policy.}
\end{figure}\label{fig:belt_teaser}

The search is outlined in Algorithm \ref{alg} and BELT\xspace is shown with progressively more concreteness in Figs.~\ref{fig:teaser}~-~\ref{fig:play_belt}. Fig.~\ref{fig:teaser} shows a hypothetical form of BELT\xspace with an explicit task space, Fig.~\ref{fig:belt_teaser} shows BELT\xspace with a goal-conditioned policy that is learned from play \cite{lynch2020learning}, and Fig.~\ref{fig:play_belt} shows BELT\xspace as applied to the experimental environment used herein. A new planning problem is defined by an initial state, $x_\text{init}$, and by success criteria $\texttt{Success}$.
The search begins by initializing a tree $\mathcal{T}$ with $x_\text{init}$ as its root and iteratively adding task-conditioned edges.
For each iteration $i$, a state $x_\text{expand} \in \mathcal{T}$ within the tree is selected, along with a task $z_i \in \mathcal{Z}$.
BELT\xspace requires a sampleable task space $\mathcal{Z}$ and task-conditioned policy, $\pi(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 \ref{sec:play}.
The goal of the selection of the tree node $x_\text{expand}$ is to guide the search towards unexplored regions and to exploit promising paths; this is discussed in detail in Section \ref{sec:bias}.
Given the task $z$ and state $x_\text{expand}$, the policy is then propagated for $T$ timesteps via a model $p(x_{t+1} | x_t, a_t)$ to create trajectory $\{x_\text{expand}, x_1, ..., x_{T-1}, x_\text{new}\}$. 
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 \ref{sec:model}.
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_\text{expand}$ to $x_\text{new}$ is checked for success with $\texttt{Success}$. 
This search continues for $N$ iterations, at which time the lowest-cost path marked as successful is returned if it exists.

\begin{figure}[t]
\centering
\begin{subfigure}{.388\textwidth}
  \centering
  \includegraphics[width=\linewidth]{tree_lmp_model.pdf}  
  \caption{Policy takes as input a goal state, the current state and samples from its latent ``plan'' space. This is then input to the model.}
  \label{fig:play_train}
\end{subfigure}
\begin{subfigure}{.53\textwidth}
  \centering
  \includegraphics[width=\linewidth]{sideways_tree_lmp_search.pdf}  
  \caption{At each iteration BELT\xspace samples a goal from demonstration data and a tree state and rolls out a task-conditioned model to add edges to the tree. The solution trajectory is selected as the lowest cost solution, as verified by the binary success check $\texttt{Success}$. Larger figure  in Appendix Section \ref{sec:full_figs}.}
  \label{fig:search}
\end{subfigure}
\caption{Broadly-Exploring, Local-policy Trees\xspace with a Play-LMP policy (Section \ref{sec:play}).}
\label{fig:play_belt}
\end{figure}

\begin{algorithm}
\small
\caption{Broadly-Exploring, Local-policy Trees\xspace}
\begin{algorithmic}[1]
\Require initial state $x_\text{init}$, sample count $N$, and success check $\texttt{Success}$
    \State Initialize tree $\mathcal{T}$ with state $x_\text{init}$.
    \For{$i$ in $1:N$}  
        \State Sample a task $z_i$ from task space $\mathcal{Z}$.\label{line:sample_task} \Comment{Discussed in Section \ref{sec:play}}
        \State Select a state $x_\text{expand}$ from $\mathcal{T}$ to expand from. \label{line:sample_node} \Comment{Discussed in Section \ref{sec:bias}}
        \For{$t$ in $1:T$}
        \State Run policy $a_t \sim \pi(a_t | x_t, z_i)$.  \Comment{Discussed in Section \ref{sec:play}}
        \State Rollout model $x_{t+1} \sim p(x_{t+1} | x_t, a_t)$. \Comment{Discussed in Section \ref{sec:model}}
        \EndFor
        \State Update $\mathcal{T}$ with state $x_i$, trajectory $\{x_\text{expand}, ..., x_i\}$, task $z_i$, and $\texttt{Success}(\{x_\text{init}, ..., x_i\})$.
    \EndFor \label{loop}
    \State \Return minimum time successful trajectory or failure if none
\end{algorithmic}\label{alg}
\end{algorithm}

\subsection{Learning a Latent Task Space and Task-Conditioned Policy From Play}\label{sec:play}

In this work we implement BELT\xspace with a general-purpose goal-reaching policy at the low level, called Learning from Play (LfP) \cite{lynch2020learning}. To learn from ``play'' we assume access to an unsegmented teleoperated play dataset $\mathcal{D}$ of semantically meaningful behaviors provided by users, without a set of predefined tasks in mind. For example, a user might open and close doors, rearrange objects on a desk, or simply tidy up the scene.

To learn control, this long temporal state-action log $\mathcal{D} = \{(x_t, a_t)\}^{\infty}_{t=0}$ is relabeled \cite{andrychowicz2017hindsight,kaelbling1993learning}, 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_{\operatorname{play}} = \{(\tau, x_g)_i\}^{D_{\operatorname{play}}}_{i=0}$, where each example consists of a goal state $x_g$ and a demonstration $\tau = \{(x_0, a_0), ... \}$ solving for the goal.
These can be fed to a simple maximum likelihood goal conditioned imitation objective, 
$\mathcal{L}_{\operatorname{LfP}} = \mathbb{E}_{(\tau,\ x_g) \sim D_{\operatorname{play}}}\left[\sum^{|\tau|}_{t=0}\operatorname{log} \pi _{\theta}\left( a_{t}| x_t, x_g\right)\right]$, to learn policy $\pi _{\theta}\left( a_{t}| x_t, x_g\right)$, parameterized by $\theta$. 
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 $\pi _{\theta}\left( a_{t}| x_t, x_g\right)$. For direct comparison to prior work, we use Play Supervised Latent Motor Plans (Play-LMP) \cite{lynch2020learning}. Play-LMP addresses the inherent multimodality in free-form imitation datasets. Concretely it is a sequence-to-sequence conditional variational autoencoder (seq2seq CVAE), autoencoding 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. \ref{fig:play_train}.
See \cite{lynch2020learning} for details.

\subsection{Biasing the Search}\label{sec:bias}

Given a goal-conditioned policy, as Play-LMP is, RRT \cite{lavalle2001randomized} is used with a few adaptions to broadly search the state space, chose achievable edge tasks, and bias towards low-cost paths (Fig. \ref{fig:bias}).

To broadly search the state space, a key component of RRT's ability to efficiently 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 \cite{lavalle2001randomized}.
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\xspace, we seek to leverage a Voronoi bias via a temporal distance $\Delta t$ between states  $x_i, x_j \in \mathcal{X}$ as the L2 distance is not a good distance for this problem.
For example, consider the block in Fig. \ref{fig:search}. 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 significant distance.
As the temporal distance is not easily quantifiable, we learn to predict it with a temporal distance classifier, $\tau_\text{tdc}(x_i, x_j)$, with $K$ exponential categories corresponding to temporal distance intervals, $d_k \in \{[0,1), [1,2), [2,4), [4,8), [8,16), ..., [128,256)\}$ based on \cite{aytar2018playing}.
The temporal distance classifier is implemented as a neural network and trained over policy rollouts to predict the distribution over class labels via a cross-entropy classification loss, $\mathcal{L}_\text{tdc} = -\sum^K_{l=1} y_l \log(\hat{y}_l)$ with $\hat{y} = \tau_\text{tdc}(x_i, x_j)$,
where $y$ and $\hat{y}$ are the true and predicted label distributions respectively.
Briefly, 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 first selecting a goal state  from a demonstration dataset from rollouts of the policy $x_\text{sample} \in \mathcal{D_\pi}$.
The temporal distance is then predicted between $x_\text{sample}$ and each node in the tree.
All nodes below a cutoff temporal distance $d_\text{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_\text{expand}$.
This biases the tree search towards nodes that are capable of reaching the goal state and towards nodes with efficient trajectories. 
It further allows a Voronoi bias by temporal distance.
The value of $d_\text{cutoff}$ represents a trade-off between exploiting more promising paths (larger values of $d_\text{cutoff}$) and exploring new ones (smaller values of $d_\text{cutoff}$); this trade-off is studied experimentally in the Appendix.
This procedure modifies lines \ref{line:sample_task} and \ref{line:sample_node} of Algorithm \ref{alg}.

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.

\begin{figure}[b!]
\centering
\begin{subfigure}{.4\textwidth}
  \centering
  \includegraphics[width=\linewidth]{tree_bias}  
  \caption{Biasing the tree search}
  \label{fig:bias}
\end{subfigure}
\begin{subfigure}{.45\textwidth}
  \centering
  \includegraphics[width=\linewidth]{models.pdf}  
  \caption{Task- and action-models}
  \label{fig:models}
\end{subfigure}
\begin{subfigure}{\textwidth}
  \centering
  \includegraphics[width=\linewidth]{model_comparison_1.pdf}  
  \caption{Task-models, action-model, and simulator our experiment environment (\href{https://youtu.be/zCJpNPn0BZQ?t=162}{video link}). Larger figure available in Appendix Section \ref{sec:full_figs}.}
  \label{fig:models_data}
\end{subfigure}
\caption{(\ref{fig:bias}) shows the bias used by BELT\xspace to choose the tree state $x_\text{expand}$ once $x_\text{sample}$ has been sampled. The blue region shows how the temporal distance between states may differ from the L2 distance, necessitating learning a temporal distance classifier. The choice between state 1 and 3 demonstrates the bias towards lower cost paths: though state 1 is closer to $x_\text{sample}$, node 3 has a much lower cost to come, and thus state 3 is selected.
(\ref{fig:models}-\ref{fig:models_data}) shows the two types of models used in this work, an action- and a task-conditioned model. 
The action-conditioned model often exhibits compounding errors as it is recursively applied along the trajectory, while the task-conditioned model avoids this by temporally extending the prediction and conditioning on the fixed task for the edge.
This compounding error can be seen in the second task (block lifting) where the action-model becomes unstable and the end effector flails (\href{https://youtu.be/zCJpNPn0BZQ?t=162}{video link}).}
\end{figure}

\subsection{Learned Task-Conditioned Model}\label{sec: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. \ref{fig:models} and \ref{fig:models_data}.
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\xspace 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_\text{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 the where the robot will be after $T$ timesteps, i.e., $p_\text{task}(x_{t+T} | x_t, z)$.
This accounts for the closed-loop nature of the policy for a fixed 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 $\mathcal{L}_{p_\text{action}} = \sum_{t=0}^{T-1} ||\hat{x}_{t+1} - x_{t+1}||$ with $\hat{x}_{t+1} \sim p_\text{action}(x_t, a_t)$, while the task-model seeks to minimize $\mathcal{L}_{p_\text{task}} = ||\hat{x}_T - x_T||$ with $\hat{x}_T \sim p_\text{task}(x_0, z)$.

\section{Experimental Results}\label{sec:result}

\subsection{Experimental Setup}\label{sec:setup}

The experiments herein are based on a simulated ``playground environment'' \citep{lynch2020learning} (Fig~\ref{fig:search} and discussed in more detail in the Appendix) built in Mujoco \citep{todorov2012mujoco}. 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. The robot is a position-controlled 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.

\textbf{Long-horizon evaluation} We evaluate our model and its baselines on large set of challenging long-horizon manipulation tasks as in \cite{lynch2020language}, 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 \cite{lynch2020learning, lynch2020language} 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. \ref{fig:chain4}). 

Note that prior work \cite{lynch2020language} assumes a human is in the loop providing each next subtask to the agent in the sequence. In our setup, an agent must provide \textit{itself} subgoals to achieve the multi-stage behavior. Other prior long-horizon evaluations, such as \cite{gupta2019relay}, describe the long-horizon task as a final goal state to reach. While this valid for many useful manipulation tasks, many are impossible to infer from the final 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 final 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 specification scheme---an indicator function, defined 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.

\subsection{Algorithmic Parameters and Baselines}

\textit{Baselines.} 
We compare against two baselines that use the same underlying Play-supervised Latent Motor Plan (Play-LMP) \cite{lynch2020learning} policy.
We compare to a model-based planner (cross entropy method (CEM) \cite{rubinstein2013cross}) 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 refit to successful trajectories.
We also compare to a Play-LMP policy that is given a single goal-state that satisfies all the subtasks (this single goal state existed for 36\% of the chains).
This baseline demonstrates the limitations of the model-free policy to execute long-horizon tasks.
Finally, we also compare to an ``oracle'' LMP policy, which is given the four chain tasks in a row, demonstrating an upper bound of what the BELT\xspace plan may achieve given the underlying policy.

\textit{Parameters.} 
The low-level policy is trained with the same play logs collected in \cite{lynch2020learning}; $\sim$7h of teleoperated play relabeled into $D_{\operatorname{play}}$, containing $\sim$10M short-horizon demonstrations, each 1-2 seconds (see video examples of this data \href{\lfpwebsite/assets/mp4/play_data516x360.mp4}{here}).
BELT\xspace used 2500 samples drawn from demonstration data of the policy rollouts.
$\tau_\text{tdc}$ was trained on this dataset to an accuracy of 82\% and $d_\text{cutoff}$ was set to $<64$.
The edges were rolled out with a randomly drawn timestep of $T\in\{32, 64, 96\}$, as inspired by \cite{li2015sparse}.
More detail on the BELT\xspace's algorithmic parameters and tradeoffs are detailed in the Appendix.

\subsection{Results}

We begin by comparing the overall performance of BELT\xspace, different models, and the baselines in Table \ref{tab:all} on the chain tasks and Fig. \ref{fig:trajs} shows several planned trajectories.
We study each algorithm's performance on the axis of finding 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\xspace 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 even feasible for most.
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 \cite{hafner2019learning}.

BELT\xspace 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\xspace's ability to both search the space of solutions and find feasible plans.
The task model solves 16\% fewer problems than directly using the simulator.

\begin{table}[b]
\small
\centering
\begin{tabular}{r c c c c} 
 Algorithm & Model & Solution Found & Success Rate & Feasible \\ 
 \hline
 Chain-LMP (Oracle) & -- & -- & 32\% & 87\% \\ 
 LMP & -- & -- & 6\% & 21\% \\ 
 CEM & Simulator& 3\% & 40\% & 58\% \\ 
 CEM & Task-Model & 11\% & 7\% & 15\% \\ 
 BELT\xspace & Action-Model & 100\% & 3\% & 8\% \\ 
 BELT\xspace & Task-Model & 66\% & 27\% & 56\%\\ 
 BELT\xspace & Simulator & 82\% & 26\% & 68\%\\ 
\end{tabular}
\caption{Success rate and robustness for BELT\xspace, CEM, and LMP. Plan robustness is measured by the percent of solutions executed successfully (success rate) and if there was at least one successful in nine attempts (feasible).
}
\label{tab:all}
\end{table}

In Table \ref{tab:tasks}, we analyze which tasks proved most difficult for BELT\xspace, 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 difficult 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 difficulty 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 \pi$ radians, which is particularly difficult for the model.
This indicates a need for an angular representation such as quaternions or axis-angles.


\begin{table}[t]
{\small
\centering
\begin{tabular}{l | c c c | c c c} 
 & \multicolumn{3}{c |}{Solution Found} & \multicolumn{3}{c}{Feasible} \\
 & BELT\xspace & BELT\xspace & & BELT\xspace & BELT\xspace & \\ 
 Chain Contains Task & Simulator & Task-Model & Ratio & Simulator & Task-Model & Ratio\\ 
 \hline
overall & 82\% & 66\%	& 0.81 & 	68\% & 56\% & 	0.82 \\
button, push any  & 73\% & 54\%	& 0.74 & 	\textbf{63\%} & \textbf{32\%} & 	\textbf{0.50} \\
drawer open/close & 82\% & 70\%	& 0.85 & 	69\% & 64\% & 	0.93 \\
grasp & 92\% &	77\%	& 0.84	& 73\% &	70\%	& 0.95 \\
knock & 91\% & 71\%	& 0.78 & 	58\% & 55\% & 	0.95 \\
rotate left & \textbf{93\%} & \textbf{0\%}	& \textbf{0.00} & 	\textbf{38\%} & -- & -- \\
rotate right & 89\% & 78\%	& 0.88 & 	77\% & 74\% & 	0.97 \\
shelf, in/out & 77\% &	56\% &	0.72 &	57\% &	54\% &	0.95 \\
slider, open/close   & 81\% & 63\%	& 0.78 & 	67\% & 58\% & 	0.87 \\
sweeping & 76\% & 64\%	& 0.84 & 	65\% & 76\% & 	1.17 \\
\end{tabular}
\caption{Task performance breakdown. The task-model particularly has difficulty planning ``rotate left'' tasks due to an angle discontinuity, and the policy has trouble executing the task for the same reason.
The task-model plans execute poorly with any buttons, likely because the model makes predictions from goal states and the button tasks are transient, thus not often appearing in goal states.}
\label{tab:tasks}
}
\end{table}

\begin{figure}[t]
\centering
\includegraphics[width=1.0\textwidth]{figs/lrtamp_planned_trajectories_few.pdf}
\caption{Plans from BELT\xspace, demonstrating its ability to plan long-horizon, sequential trajectories (\href{https://youtu.be/zCJpNPn0BZQ?t=193}{video link}). 
}
\label{fig:trajs}
\end{figure}

\section{Conclusion and Future Work}\label{sec:conclusion}

\textit{Conclusions.} This work presented BELT\xspace, an algorithm capable of planning long-horizon tasks.
BELT\xspace 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.

\textit{Real-world Experiments.} 
This method builds upon a scalable approach to learning goal-conditioned low-level policies (\cite{lynch2020learning}), 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 classifiers such as \cite{singh2019endtoend}. Hence we estimate that there is a reasonable and scalable path to real-world experiments and plan to explore this in future work.

\textit{Application to Dynamic and Unknown Environments.}
In settings where the environment is dynamic or not fully observed, BELT\xspace plans may be limited in horizon. In these settings, the BELT\xspace tree can be reused to quickly replan, either by rewiring based on changes or by planning trajectories back into the tree in case of plan divergence. Furthermore, concepts from BELT\xspace can be used to form a graph and maintain a representation of possible paths through the environment.

\textit{Future Work.} 
In the future we wish to show how BELT\xspace can be extended well beyond the dataset and to other task representations and models.
We further wish to study how it can be used quickly in replanning, and particularly how the existing tree may be reused.
Finally, we wish to study methods for generating not just any feasible plan, but robust and efficient plans.

\clearpage

\acknowledgments{The authors wish to thank Karol Hausman, Vincent Vanhoucke, Alexander Toshev, and Aleksandra Faust for helpful discussions.}


{\small
