Learning to Correct Mistakes: Backjumping in Long-Horizon Task and Motion Planning
Abstract: As robots become increasingly capable of manipulation and long-term autonomy, long-horizon task and motion planning problems are becoming increasingly important.
A key challenge in such problems is that early actions in the plan may make future actions infeasible.
When reaching a dead-end in the search, most existing planners use backtracking, which exhaustively reevaluates motionlevel actions, often resulting in inefﬁcient planning, especially when the search depth is large.
In this paper, we propose to learn backjumping heuristics which identify the culprit action directly using supervised learning models to guide the task-level search.
Based on evaluations on two different tasks, we ﬁnd that our method signiﬁcantly improves planning efﬁciency compared to backtracking and also generalizes to problems with novel numbers of objects.
Introduction
Integrated task and motion planning (TAMP [1]) is a framework for making sequential decisions in robotic tasks.
Solving TAMP problems involves a hybrid search over a sequence of discrete actions ( e.g. , which object to manipulate) and their continuous motion parameters ( e.g. , with which pose to grasp the object).
One important practical challenge is that for long-horizon tasks with a large number of objects, the search space becomes intractable due to a large depth and branching factor.
In this work we address the challenges of long-horizon TAMP where early actions in the plan may make future actions infeasible.
For example, as shown in Fig. 1, if the robot places the ﬁrst few objects near the cabinet entrance in a packing task, or on the target retrieval path in a navigation among movable obstacles (NAMO [2]) task, the problems become infeasible, unless the culprit actions that placed objects in obstructive positions are corrected.
When planning fails, identifying culprit actions is desirable as the search can focus on correcting the culprit actions, ignoring irrelevant actions.
Generating explanations for failure has been explored to guide search [3, 4, 5].
In TAMP, geometric failures guide the task-level planner, often done via backtracking to try a different alternative for the immediately prior action in the plan [6].
As backtracking exhaustively explores the search tree (Fig. 2(b)), correcting the culprit action can involve an exponential number of evaluations of all the intermediate actions.
The complexity becomes especially daunting in TAMP because the continuous motion parameters induce an inﬁnite branching factor for each action.
In the constraint satisfaction literature [7], backjumping has been introduced to alleviate the complexity of backtracking by taking a short cut to an ancestor action.
In discrete settings, it may be possible to jump back to the culprit action directly without precluding any possible solutions.
However, because of the inﬁnite branching factor in TAMP, conclusively identifying the culprit action is generally not possible.
Even estimating the culprit requires solving the original backtracking problem, which can be very time-consuming (see details in Sec. 4).
We thus propose a learning approach by shifting the computational burden to the training phase where training data is collected by solving backtracking problems.
Our observation is that because the true culprit action always exists in any failure cases, learning models designed to exploit this may predict the culprit accurately.
Speciﬁcally, we explore two frameworks to learn backjumping heuristics: (1) imitation learning which directly predicts the culprit from all previous actions, and (2) plan feasibility which indirectly predicts the culprit by checking if the solution can be found after each action.
Our proposed learning process is a domain-independent way of learning domainspeciﬁc heuristics.
We summarize our contributions as follows:
• We propose two backjumping heuristic learning methods to improve the efﬁciency of solving long-horizon TAMP problems.
We also present two algorithms containing learning methods as a subcomponent, which treats the continuous parameters differently.
• Our empirical results show that our methods improve planning efﬁciency by 40% for packing and 99% for NAMO, both against backtracking.
Furthermore, by incorporating a graph neural network (GNN [8]) into the learning model, our methods can generalize to problems with novel numbers of objects while outperforming backtracking consistently.
Preliminaries
We introduce the TAMP notation that will be used to deﬁne our problem and the sequence-beforespecify strategy that we improve on.
2.1 G-TAMP notation
Although our method can potentially be applied to a broader class of TAMP problems [1], we focus on a particular subclass of TAMP called geometric TAMP (G-TAMP [9]) for clarity of presentation.
In G-TAMP, a mobile manipulator is tasked with moving multiple objects to target regions among movable obstacles.
We assume quasi-static dynamics of the world 1 , deterministic effects of actions, and fully-observable environments.
Even with these assumptions, the proposed problem is already hard (NP-hard explained in Sec. 7), but relaxing these assumptions may still be possible by introducing belief-space planning [10, 11], which we leave as future work.
Formally, a G-TAMP problem is deﬁned by a tuple < M , R , O , F, T, G , s 0 > where M is a set of movable objects , R is a set of target regions , O is a set of operators , F is a feasibility checking function , T is a transition function , G is a goal set , and s 0 is an initial state .
The goal is to ﬁnd a sequence of grounded and reﬁned operators 2 starting from s 0 to reach a state s ∈ G .
• M , R and s :
In G-TAMP, the environment consists of any number of ﬁxed objects such as tables, N M movable objects M = { m i } N M i =1 such as cups, and N R target regions R = { r j } N R j =1 .
We denote the world state by s which consists of the robot conﬁguration and the pose of each movable object.
• O : Operators in G-TAMP move one object to a region, such as P ICKAND P LACE and P USH .
Each reﬁned operator o ( d = ( m i , r j ) , c = ( q, g ))
∈ O consists of (1) the discrete parameter d ∈ M×R specifying which object to move and which region to move it to, and (2) the continuous parameters c composed of a grasp g ∈ SE (3) 3 on m i and a placement pose q ∈ SE (3) in r j . • F and T :
The feasibility of an operator can be determined by evaluating constraints ( e g. , reachability and collision avoidance) using an external motion planner.
We denote this feasibility evaluation as a Boolean-valued function F ( s, o ) → { 0 , 1 } .
If the operator is feasible, a deterministic transition function T ( s, o ) determines the new state s (cid:48) .
• G : A goal set G is deﬁned as a conjunction of predicates, such as I N C ABINET ( m i , r j = C ABINET ) which becomes T RUE if m i is stably located in the cabinet.
G-TAMP problems can alternatively be seen as hybrid constraint satisfaction problems, as shown in
Fig. 2(a).
Circles represent variables whose domains are either continuous ( s k and c k ) or discrete ( d k ), and squares represent constraints ( F k ).
When the plan length is K , we ﬁnd values for sets of variables { d k } K − 1 k =0 and { c k } K − 1 k =0 such that all constraints { F k } K − 1 k =0 are satisﬁed and s K ∈ G .
2.2 Sequence- before -specify strategy
Sequence-before-satisfy [4, 12, 13, 14, 15, 16, 17, 1] is one strategy to solve TAMP problems which this work is based on.
The strategy consists of two stages: (1)
In the sequencing stage, the strategy ﬁnds plan skeleton s [18] reaching the goal set G symbolically only, i.e. , ( d k ) K − 1 k =0 are computed but their continuous counterparts ( c k ) K − 1 k =0 are left unspeciﬁed.
In the specifying stage, the strategy chooses a plan skeleton often based on AI planning heuristics and reﬁne s ( c k ) K − 1 k =0 satisfying the constraints and a goal condition.
For notational simplicity, in the rest of the paper, we use m k to represent the object moved by operator o k .
We treat objects that are not included in a grounded plan as ﬁxed objects.
Note that the two stages repeat until either ﬁnding a solution or reporting no solution, generating multiple plan skeletons.
Our method (Sec. 4 and Sec. 5) accommodates these multiple plan skeletons by learning a speciﬁc model for each plan skeleton.
In this section we focus on the case of a single plan skeleton for ease of presentation.
Backtracking search is generally used to ﬁnd values of ( c k ) K − 1 k =0 in the specifying stage by constructing a search tree rooted from c 0 (Fig. 2(b)).
Since the domain of c k is continuous, the strategy uses sampling ( e.g. , uniform sampling from the domain of c k [19]) to obtain N reﬁned values to select from.
Index k corresponds to both the level of the tree and the step in the plan skeleton.
When the assigned values of ( c 0 , ..., c k − 1 ) are inconsistent with all sampled values of c k ( i.e. , violating the constraint F k ), we say the search hits a dead-end and denote the dead-end level by k d .
The search then backtracks to a parent node c k − 1 to assign another value and retries the consistency check with c k .
If successful, a child node c k +1 is evaluated at the next level, otherwise another value of c k −is attempted.
Backtracking repeats this process exhaustively until it ﬁnds values of ( c k ) K − 1 k =
that are consistent, i.e. , the solution of a problem.
Problem Description
When the planning horizon is long, the above backtracking-based strategy becomes intractable as the search space increases exponentially.
Our goal in this work is to adopt the idea of backjumping [7] to effectively reduce the search space and achieve efﬁcient planning in TAMP.
In contrast to backtracking which only backtracks level by level when dead-ends persist, we can speed up the search by backjumping multiple levels (Fig. 2(b)).
A backjump is said to be safe if it does not preclude any solutions.
Speciﬁcally, at level k d , safe jump corresponds to an ancestor level k < k d such that attempting all possible values of its descendants at all levels between k and k d does not resolve the dead-end.
We denote all safe-jump levels by a set K .
The larger the jump, the better, because larger jumps avoid more computation.
We denote the largest safe jump by maximum jump , i.e. , k (cid:63) = min K .
Correspondingly, we call c k (cid:63) a culprit variable responsible for making all values of c k being inconsistent with a partial plan.
The objective is to ﬁnd k (cid:63) to backjump safely and maximally whenever a dead-end is met at level k d , thus improving the overall efﬁciency of the sequence-before-specify strategy.
Backjump Learning Methods
The constraint network in Fig. 2(a) contains a chain structure with respect to constraints F k by considering d k , c k , and s k as one large variable node for each k .
Because of this topological structure, no constraints connect variables that are more than one level apart.
Moreover, we can only estimate k (cid:63) as we use sampling to handle continuous domains.
For these reasons, k (cid:63) identiﬁcation using backjumping is inherently challenging in TAMP.
It is however possible to ﬁnd an approximation of k (cid:63) with backtracking since it evaluates all possible combinations of assignments using all sampled values from the root to level k d .
Also, since k (cid:63) always exists for any dead-end situations and is unique , we can hope to gather less noisy labeled data with a sufﬁcient amount of samples.
Leveraging these observations, we propose two alternative supervised learning methods to identify k (cid:63) , where training labels are gathered by solving tasks from the same distribution using backtracking.
The robot conﬁguration is used in an external motion planner to check the feasibility of a corresponding reﬁned operator, but it is not used in our learning models.
We thus deﬁne a new state notation ¯ s consisting of poses of movable objects only.
4.1 Imitation learning
When the search hits a dead-end at level k d , we ﬁrst propose a predictor to directly predict the maximum jump k (cid:63) in the domain of { 0 , . . .
, k d − 1 } , from the sequence of states (¯ s 1 , . . .
, ¯ s k d ) and the geometric attributes of a movable object m k d − 1 ( e.g. , size).
In our model, each state ¯ s is represented by a fully-connected graph whose nodes are the poses of movable objects and each edge is the relative pose between each pair of objects.
As shown in Figure 3(a), to predict k (cid:63) , our model (1) extracts the state features from each state in the sequence using the same GNN, (2) computes the temporal features across the sequence of state features using a bidirectional recurrent neural network (bRNN [20]) or multi-head attentions [21], (3) extracts the movable object features from m k d with MLP1 (multilayer perceptron), and (4) with pairs of temporal features and object features as inputs, applies the same prediction network MLP to predict the likelihood of each step being k (cid:63) .
The model is trained to minimize the cross-entropy loss between the predicted likelihood of being k (cid:63) and the ground-truth k (cid:63) label.
The k (cid:63) labels are collected from the backtracking tree search.
At any k d , we record its current state trajectory as (¯ s 1 , . . .
, ¯ s k d ) .
When the backtracking successfully reaches the level k d for the ﬁrst time after the dead-end, we record its state trajectory up to level k d as (¯ s (cid:48) 1 , . .
, ¯ s (cid:48) k d ) .
Let ¯ s i and ¯ s (cid:48)
i be the i -th element from the corresponding trajectory sequences.
The k (cid:63) is the ﬁrst level where two trajectories diverge, i.e. , k (cid:63) = arg min i (cid:8) i ∈ { 0 , ...,
[Equation]
[Equation]
4.2 Plan feasibility
Besides directly estimating k (cid:63) , we investigate an alternative counterfactual approach that learns a binary classiﬁer to predict whether a reﬁned partial plan at each level contributes to a dead-end.
We use the predicted labels ( i.e. , either feasible or infeasible) to identify which level corresponds to k (cid:63) .
Speciﬁcally, with a sequence of states ( s 1 , . . .
, s k d ) that faces a dead-end at level k d , we start with swhere only c 0 is reﬁned ( i.e. , a value is assigned) but the rest of the variables ( c k ) k d k =1 are not reﬁned yet.
Then, the classiﬁer predicts whether ﬁnding a consistent assignment of values for ( c k ) k d k = is feasible.
If infeasible, it implies that the placement of m 0 is a culprit action making the rest of variables ( c k ) k d k =1 inconsistent with c 0 .
By deﬁnition, level 0 becomes the maximum jump k (cid:63) .
If feasible, we continue with s 2 where c 0 and c 1 are reﬁned only and the classiﬁer predicts feasibility for ( c k ) k d k =2 .
Likewise, we iteratively apply the classiﬁer to predict feasibility in an ascending order of k ; we stop if infeasibility is predicted and output the corresponding step as k (cid:63) , or continue to the next step otherwise.
Let k (cid:48) ∈ { 1 , ..., k d } be the step where ( c k ) k (cid:48) − 1 k =0 are reﬁned and feasibility for ( c k ) k d k = k (cid:48) is to be evaluated.
As shown in Figure 3(b), to predict whether a level k (cid:48) <
k d is a safe jump, our model (1) extracts the state features from ¯ s k (cid:48) using GNN, (2) extracts the object features from ( m k ) k d k = k (cid:48) each with MLP1, (3) computes the temporal feature from the state features and object features using a unidirectional RNN or multi-head attention (the inputs to the attention are also masked in a way to make the computation unidirectional) and (4) applies the classiﬁcation network MLP2 to the temporal feature to predict whether ﬁnding consistent values for c k d is feasible.
For model training, note that plan feasibility can be trained on any pair of state s k (cid:48) and future variables ( c k (cid:48) , . .
c k ) where c k does not have to be a dead-end, thus making it relatively easy to generate a large amount of training data.
For any state s k (cid:48) ( i.e. , assigned values of ( c k ) k (cid:48) − 1 k =0 in the search tree), if its subtree reaches a level k ≥ k (cid:48) , the plan feasibility for ( c k (cid:48) , . . .
, c k ) is 1, orotherwise.
The model is trained to minimize the binary cross-entropy loss between the predicted and ground-truth feasibility for ( c k (cid:48) , . . .
, c k ) .
Backjumping Algorithms
In this section, we present our algorithm that leverages the proposed learning methods as backjumping heuristics to guide the search.
The overall algorithm is similar to backtracking except that, at dead-ends, backtracking is replaced by backjumping with k (cid:63) predicted by the trained model.
Since we sample a ﬁnite number of values for ( c k ) K − 1 k [-=
-]=0 from their continuous domains, we need a mechanism to sample more values if a solution is not found with those currently available values.
We propose two versions of the algorithm: (1) the batch sampling method, and (2) the forgetting method, differing by how the sampling process is treated.
The pseudocodes of both algorithms are presented in the appendix.
In batch sampling, we ﬁrst draw a batch of N samples for each variable of ( c k ) K − 1 k =0 , and then ﬁnd a consistent sequence of values using the backjumping algorithm.
If a solution is not found before the search tree is exhausted, we draw another batch of samples to construct a new search tree.
Since the search tree maintains the same set of values, memoization techniques such as no-goods 5 can be applied to further accelerate the search, which we leave as future work.
The forgetting method on the other hand does not keep previously sampled values, but discards them and redraws new samples each time the search visits a different level in the search tree.
Thus, memoization is unavailable in the forgetting method, although it may potentially explore the continuous search space effectively.
Both algorithms are run until the time limit is reached.
It is an open question which of the two methods is more theoretically beneﬁcial ( e.g. , in terms of convergence rate); we instead show the performance of both methods empirically.
Evaluation
This section reports on experiments designed to evaluate the following hypotheses: (1) How efﬁciently can our backjumping methods ﬁnd a solution in comparison with backtracking?
(2) How well does our model generalize to different numbers of objects?
(3) Which of the backjumping algorithms between batch sampling and forgetting performs more efﬁciently?
We evaluate these hypotheses on the packing and NAMO tasks, as described next.
6.1 Evaluation tasks
Our evaluation tasks are implemented in the PyBullet [22] simulator where a PR2 robot is used as a mobile manipulator (see Fig. 1).
We use PDDL [23, 24] to ﬁnd plan skeletons in the sequencing stage and bidirectional RRT [25] as an external motion planner [26] in the specifying stage.
In the packing task, the objective is to move all objects located randomly on the right tables into the cabinet on the left.
For a P ICKAND P LACE operator where the target region r j is the cabinet, we deﬁne the sampling domain as the 2D base plane of the cabinet to draw placement pose samples p .
As the interior space of the cabinet can be accessed from one side only and is not spacious enough, objects placed near the entrance may make placing future objects infeasible.
In the NAMO task, there are 10 movable boxes (in blue) located randomly in the vicinity of the robot’s path and 27 ﬁxed boxes (in brown) in the room and the goal is to move the target box (in red) to the goal region (in gray).
As blue boxes block the path to reach the target box, the robot must clear them by relocating them in their vicinity.
We also deﬁne the domain for sampling box placement poses p from a circular arc on the ﬂoor computed with respect to the robot base frame.
When moving back to the goal region, with the target box in hand, the robot is unlikely to ﬁnd a feasible motion plan if blue boxes were relocated to blocking positions.
Unlike the packing task where objects move from one ﬁxed object to another ﬁxed object, objects are moved to and from the same ﬁxed object ( i.e. , the ﬂoor) in NAMO.
6.2 Implementation details
For both backtracking and backjumping, we use N = 30 for the packing task and N = 4 for the
NAMO task.
We denote our method and architecture combinations as:
• IL-RNN and IL-Attn : Imitation learning with RNN and with multi-head attention, respectively.
• PF-RNN and PF-Attn : Plan feasibility with RNN and with multi-head attention, respectively.
The architecture and training details can be found in the appendix.
For PF, as the trained model outputs a probability of being either feasible or infeasible, we additionally introduce a threshold (cid:15) to make a classiﬁcation decision robust.
We apply the model to each step and record a predicted probability of being feasible at step k as p k .
We select k (cid:63) as the ﬁrst step whose p k < (cid:15) .
(cid:15) is computed by averaging between the maximum and minimum values of p k , where k is an element from (0 , . .
, k d − 1) .
Empirically, we ﬁnd that introducing this adaptive threshold (cid:15) predicts k (cid:63) more accurately than a ﬁxed threshold, e.g. , setting (cid:15) = 0 .
All our reported results are obtained by training with 3 different seeds.
We observe in our supervised learning setting that changing the seed to train the model does not affect the performance much.
In Sec.
6.5, we show that the forgetting algorithm empirically outperforms batch sampling.
Thus, we use the forgetting algorithm to obtain results for planning efﬁciency and generalization.
We include complete results for batch sampling in the appendix.
6.3 Planning efﬁciency
We collect data from 500 problems with 10 objects for packing and from 250 problems for NAMO.
New 100 problems and 50 problems are used to test packing and NAMO, respectively.
To measure planning efﬁciency, we consider the number of nodes visited in the search tree, where fewer nodes imply fewer feasibility checks, leading to faster planning.
As shown in the top rows of
Table 1, in both tasks, our backjumping methods are signiﬁcantly more efﬁcient than backtracking.
We also show how closely our backjumping ˆ k (cid:63) approximates the ground-truth k (cid:63) in the appendix.
In NAMO, imitation learning outperforms plan feasibility.
We conjecture that movable objects in
NAMO do not affect each other ( i.e. , relocating one box does not affect the feasibility of relocating another box), making it easier for imitation learning to ﬁnd ˆ k (cid:63) close to k (cid:63) .
We also show in the appendix that some methods still greatly outperform backtracking with fewer training data, and that the performances in Table 1 can further be improved with more training data.
[Table]
[Table]
6.4 Generalization
We examine whether our method can generalize to a novel number of objects.
In particular, we train our model using 10 objects and test with more objects ( i [Table] e. , 11 and 12 objects).
Note that adding more objects to the packing task is more challenging than removing objects as it leads to more dead-ends and harder estimation of k (cid:63) , due to insufﬁcient space in the cabinet [Table]
As shown in the middle rows of Table [Table] our methods still signiﬁcantly outperform backtracking and the performance ratios of our methods and backtracking remain similar to those in the top rows where the number of objects is the same for both training and testing [Table]
with the only exception of PF
Attn in Packing (11) and (12).
We conjecture that the use of GNNs allows our learning models to handle novel numbers of objects.
6.5 Comparison of backjumping algorithms
As shown in the bottom row of Table 1, evaluated on the same set of 100 problems in packing with 10 objects, batch sampling is about 2 ∼ 3 times slower than forgetting, while still outperforming backtracking.
Although forgetting empirically exceeds batch sampling in our evaluation, we point out in Sec.
5 that enhancing batch sampling is potentially achievable by leveraging ideas from the constraint satisfaction literature [7].
Related Work
In TAMP [27, 10, 28, 4, 16], geometric failures are often used to provide feedback to task-level planning to guide its search.
However, this feedback in literature is mostly local [5] meaning that the failure is involved with a particular sequence of actions.
In this work, we propose a global feedback that considers the entire plan skeleton by leveraging backjumping.
The minimum constraint removal (MCR) problem [3] is related to our problem as its objective is to ﬁnd a parsimonious set of objects to remove in order to reach a goal region.
Our problem is a generalization to MCR in that a goal placement of an object is not deﬁned and that objects are not removed but relocated.
Since MCR is NP-hard, our problem also falls into the NP-hard category.
The most relevant papers to our work are the culprit detection problem [5] and the work [12]; both papers address shortcomings of backtracking.
A method proposed in the culprit detection problem generates global feedback from failures in answer set programming [29] but it pre-discretizes the state which may ignore certain motion constraints.
The work [12] improves the efﬁciency of backtracking by jointly optimizing parameters such as trajectory, grasp, and placement pose; their work uses trajectory optimization, which is a competing framework for sampling-based motion planning our method is based on.
Learning other heuristics to improve planning efﬁciency has been proposed in the TAMP and motion planning communities, such as predicting feasibility at the motion level [30, 31, 32] and guiding sampling [33, 34].
Instead, our approach learns task-level heuristics by analyzing long-horizon dependencies , which can potentially complement existing heuristics to improve efﬁciency further.
Limitations
Besides future work discussed in the paper, some limitations of the presented work are as follows:
(1) We consider incorporating backjumping with a given single plan skeleton only, not with a set of possible plan skeletons, which may be critical when objects have substantially different sizes.
(2) We test on relatively simple geometric objects and assume known 3D CAD models.
A more realistic geometry of objects, e.g. , constructed by point clouds, in our framework, is a future direction.
(3) We evaluate with the same geometry of obstacles ( i
e. , the same cabinet in packing and the same arrangement of ﬁxed objects in NAMO).
Future work would be to achieve generalization to novel geometry of obstacles drawn from some distribution.
Conclusion
In this paper, we present learning frameworks to learn backjumping heuristics from data to identify the culprit action aimed at improving the efﬁciency of solving TAMP problems.
Our experimental results demonstrate our method exceeds backtracking by far in terms of planning efﬁciency and its generalization to problems with novel numbers of objects. Acknowledgments
This work has taken place in the Learning Agents Research Group (LARG) at UT Austin.
LARG research is supported in part by NSF (CPS-1739964, IIS-1724157, FAIN-2019844), ONR (N0001418-2243), ARO (W911NF-19-2-0333), DARPA, GM, Bosch, and UT Austin’s Good Systems grand challenge.
Peter Stone serves as the Executive Director of Sony AI America and receives ﬁnancial compensation for this work.
The terms of this arrangement have been reviewed and approved by the
University of Texas at Austin in accordance with its policy on objectivity in research.