# Learning Geometric Constraints
in Task and Motion Planning

Tianyu Ren  Computer Science Department, Technische Universität Darmstadt
Alexander Imani Cowen-Rivers  Huawei R&D London  Haitham Bou Ammar  Huawei R&D
London  Jan Peters  Computer Science Department, Technische Universität
Darmstadt

###### Abstract

Searching for bindings of geometric parameters in task and motion planning
(TAMP) is a finite-horizon stochastic planning problem with high-dimensional
decision spaces.A robot manipulator can only move in a subspace of its whole
range that is subjected to many geometric constraints. A TAMP solver usually
takes many explorations before finding a feasible binding set for each task.
It is favorable to learn those constraints once and then transfer them over
different tasks within the same work space. We address this problem by
representing constraint knowledge with transferable primitives and using
Bayesian optimization (BO) based on these primitives to guide binding search
in further tasks. Via semantic and geometric backtracking in TAMP, we
construct constraint primitives to encode the geometric constraints
respectively in a reusable form. Then we devise a BO approach to efficiently
utilize the accumulated constraints for guiding node expansion of a MCTS-based
binding planner. We further compose a transfer mechanism to enable free
knowledge flow between TAMP tasks. Results indicate that our approach reduces
the expensive exploration calls in binding search by 43.60to 71.69 when
compared to the baseline unguided planner.

##  I Introduction

Task and Motion Planning (TAMP) is a framework aiming to enable robotic
reasoning and acting in multi-stage long-horizon manipulation and mobility
tasks. TAMP solvers typically adhere to a two-level execution process. First,
high-level planners establish a sequence of symbolic operators (skeletons)
that allow arriving at a (symbolic) goal state. With those sequences at hand,
low-level motion planners then bind symbolic operators to metric motion
parameters (bindings), effectively rendering geometrically feasible strategies
for the environment.

Although successful in many instances [1, 2, 3, 4, 5], symbolic binding in
TAMP is challenging, requiring resolutions to large-scale constraint
stochastic sequential decision-making problems. To illustrate, consider the
grasping example in Figure 1 in which a robot needs to transport a body from
region1 to region2 while avoiding environmental obstacles, e.g., obstacle1 or
obstacle2. The feasible skeleton of Table I requires binding decisions that,
first, choose a grasping direction
$\texttt{\\#dir1}\in\\{\texttt{front},\texttt{right},\texttt{top},\texttt{back},\texttt{left}\\}$
and then opt a target pose #pose12 $\in[0,1]^{3}$. Those bindings are
sequential such that a choice of #dir1 alters allowable pose states that can
subsequently affect later decision stages. Existing approaches trend to
address such binding search by modelling each task as a black-box optimization
problem [6, 7, 8]. In this case, different environmental constraints (e.g.,
those induced by obstacle1 and obstacle2) are merged together into a single
objective that is specified for a task instance. Therefore, experience about
these constraints cannot be shared by a second task.

To promote knowledge sharing in binding search, in this paper, we represent
environmental constraints respectively by constraint primitives (CPs). Based
on constraint primitives, we devise a Bayesian Optimization algorithm to
effectively guide the exploration binding search in TAMP. The main
contribution of this work is threefold:

  * •

we create constraint primitives each of which represents a piece of objective
geometric knowledge of the environment via semantic and geometric
backtracking;

  * •

we propose an efficient BO algorithm based on constraint primitives to guide
the exploration in the decision tree of binding search;

  * •

we devise a transfer learning mechanism to generalize accumulated constraint
primitives to new tasks with zero effort.

In three robot manipulation tasks, we show that the proposed method can
improve binding search efficiency over planners with quasi-random samplers. We
also provide a comparison with another Bayesian optimization approach that
does not use the constraint primitive representation to show the significant
improvement with transferred knowledge.

##  II Related work

Most existing studies solve binding search by optimizing a single objective
function with respect to a specific task. Geometric constraints of the
environment are implicitly modelled together in the objective. For addressing
such a finite-horizon optimization problem. MCTS (Monte Carlo tree search) or
UCT (Upper Confidence bounds for Trees) [9] are common choices. In [6], VOOT
(Voronoi optimistic optimization applied to trees) performs value-driven
sampling of the continuous binding space. It requires a deterministic
objective function so it is infeasible for most TAMP systems where sample-
based motion generators (e.g., RRT [10]) are extensively used. Kernel
Regression UCT proposed in [7] enables information sharing between similar
binding decisions through kernel regression, but it does not provide any
convergence or completeness guarantees. In eTAMP [11], PW-UCT (Upper
Confidence bounds for Trees with Progressive Widening) [12, 13] is used to
address the stochastic transition dynamics in planning. It ensures
probabilistic completeness in binding searching by observing the PW laws
prescribed in [13]. This approach however relies on random samplers for node
expansion in UCT. The resultant exploration is uninformative and inefficient.
In addition, as with other MCTS methods, PW-UCT have difficulties transfer its
experience to other tasks. It has to start from scratch for each TAMP task
even though there is apparent overlap in geometric constraints (e.g.,
$T_{\text{desk}}$ and $T_{\text{deskP}}$ in Fig. 4(a) share the same
constraint with obstacle3). In [8], the authors formulate binding search as a
black-box function optimization problem and propose a experience-based UCB
(Upper Confidence Bound) algorithm, BOX, to guide binding search. By
maintaining a score matrix with columns divided by binding decisions and rows
divided by task instances, BOX can reason with the correlation information
between different task instances so that it gets some capability to accumulate
binding experience and generalize it to unseen instances. However, this
framework only works with discrete binding spaces, and it only enables
knowledge sharing between different instances (with the same skeleton) instead
of different tasks (e.g., $T_{\text{desk}}$, $T_{\text{deskP}}$,
$T_{\text{regrasp}}$ in Fig. 4(a)). Moreover, the construction of the score
matrix requires substantial engineering and training before it can be used for
each task.

Instead of merging into a single objective, environmental constraints can be
respectively modeled and satisfied during binding search. With hand-coded
constraints, [14] formulates a pick-and-place task as a constraint
satisfaction problem (CSP) with discrete binding spaces and solves it with
off-the-shelf CSP solvers. By dependency analysis in semantic and geometric
spaces, [15] proposes a culprit detection mechanism to automatically identify
constraints in the robot environment. To make constraints more general and
reusable, authors in [15] use typed symbols instead of concrete instances to
describe constraints. Unfortunately, this method also requires discretization
of the binding space, which adds difficulties in application to practical
robot tasks. We are interested in knowledge transfer mechanism for binding
search that supports decision making in both continuous and discrete parameter
space. It should have no negative effects on the planner completeness and
should require no special training phases.

##  III Background

TABLE I: A feasible skeleton for $T_{\text{desk}}$ with its operators grouped by layers of a decision tree. Tree node layer |  Skeleton operators
---|---
decision1 |  $\texttt{Sample-grasp(body)}\to\texttt{\\#dir1}$
transition1 |  $\texttt{Inv-kin(body,pose0,\\#dir1)}\to\texttt{\\#config1}$ $\texttt{Plan-motion(config0,\\#config1)}\to\texttt{\\#traj01}$ Move-Pick(body,#traj01)
decision2 |  $\texttt{Sample-pose(body,region2)}\to\texttt{\\#pose12}$
transition2 |  $\texttt{Inv-kin(body,\\#pose12,\\#dir1)}\to\texttt{\\#config2}$ $\texttt{Plan-motion(\\#config1,\\#config2)}\to\texttt{\\#traj12}$ Move-Place(body,#traj12)

As the starting point of binding search, we assume a feasible skeleton for
$T_{\text{desk}}$ as shown in Table I, where pose0 is the initial pose of body
on region1, and config0 is the initial configuration of robot. The motion
parameters marked by # are those open decisions that demand concrete bindings.

###  III-A Zero-shot Task Generalization

The goal of zero-shot task generalization is to achieve task goals that are
not seen during training [24– 26].For evaluation we consider a zero-shot
generalization setup [25, 26] where only a subset of the task goals is
available during training, and the agent has to achieve a disjoint set of test
task goals.

###  III-B UCT-based Binding Search

Following [13], we model the binding search process as a decision tree. In a
similar way of how solution constraints are defined in [8], we group the
skeleton operators into two alternative layers (see Table I): the transition
layer and the decision layer. Operators in decision layers, such as Sample-
grasp and Sample-pose, always generate decisions with long-lasting effects and
they are especially crucial to the solution. We note the binding search
problem as $\text{BINDING}(Task,\langle\texttt{\\#d1,\\#d2,...}\rangle)$. An
decision tree for
$\text{BINDING}(T_{\text{desk}},\langle\texttt{\\#dir1,\\#pose12}\rangle)$ can
be plotted as Fig. 1.

(a) $T_{\text{desk}}$

(b) The decision tree for
$\text{BINDING}(T_{\text{desk}},\langle\texttt{\\#dir1,\\#pose12}\rangle)$

Figure 1: $T_{\text{desk}}$ with motion parameters
$\langle\texttt{\\#dir1,\\#pose12}\rangle$ to be bound by the decision tree.

The PW-UCT algorithm for binding search is outlined in Alg. 1 (please see
details in App. A-A). The planner should search for feasible bindings
$\bm{x}^{\star}$ (e.g.,
$\bm{x}^{\star}=\langle\texttt{front,(0.6,0.3,0.9)}\rangle$ in Fig. 1) until
the maximum attempts $N_{rollout}$ is reached. Before $\bm{x}^{\star}$, each
UCT rollout is concluded by a terminal state. A termination means at least one
geometric constraint is violated, and binding search must restart from the
root. A tree node at termination state will receive a reward
$r=0.1\left(d_{\text{end}}/d_{\text{total}}\right)+r_{\text{end}}$ , where
$d_{\text{end}}$ is the termination depth in UCT $d_{\text{total}}$ is the
total tree depth. $r_{\text{end}}=1$ when a feasible binding list is found,
otherwise $r_{\text{end}}=0$. This reward encourages the planner to go deeper
in the decision tree and to find more feasible bindings.

On SAMPLE-NEW-CHILD$(node)$, a new child node will be expanded from $node$
with a binding decision generated by a random sampler. Otherwise an existing
child node of $node$ will be selected by UCB criterion. The balance between
sampling new children and selecting existing children is regulated by
EXPANDABLE-BY-PW$(node)$. The basic PW idea is to limit the number of visits
for existing nodes. Despite its probabilistic completeness in theory, we have
observed in practice slow convergence to feasible solutions with this
approach. It is mainly due to the unguided random samplers used for
exploration. Without any information about the geometric constraints of the
environment, these samplers just enumerate bindings by brute force. We propose
to address this problem by replacing random samplers in Alg. 1 with Bayesian
optimisation routines.

Input: $N_{rollout}$

$node\leftarrow root$

while  _$count <N_{rollout}$_ do

while  _$node$ is not terminated and $node$ is not su_ do

ADD-VISIT($node$)

if  _EXPANDABLE-BY-PW $(node)$_ then

$node\leftarrow\text{SAMPLE-NEW-CHILD}(node)$

else

$node\leftarrow\text{SELECT-CHILD-BY-UCB}(node)$

BACK-PROPAGATE$(node)$

$node\leftarrow root$

$count\leftarrow count+1$

$\bm{x}^{\star}=$GET-FEASIBLE-BINDINGS$(root)$

if  _$\bm{x}^{\star}$ is not None_ then

return $\bm{x}^{\star}$

return $None$

Algorithm 1 UCT Binding Search

Commonly-used random samplers in tree node expansion (SAMPLE-NEW-CHILD in Alg.
1) lead to unguided explorations in UCT [12, 13]. Thus the binding planner
normally requires a large amount of simulation rollouts before a feasible
solution $\bm{x}^{\star}$ is found. In comparison, the framework we purpose
CP-BO exploits each failure point during the tree search and accumulate it as
a data point. These points are stored in a global dataset.

###  III-C Bayesian Optimization

As noted earlier, TAMP requires expensive exploration step in UCT. In the
context of TAMP we want to explore multiple constraints till we are feasible.
This optimisation problem differs from typical BO as we have fine-grained
information as to whether each individual constraint primitives in the
simulator was satisfied. Thus, for $n$ constraint primitives we have both the
overall information of whether the bindings were feasible for the task
$y=f(\bm{x})$ and for each primitive constraint $i$ we receive additional
information ($y^{\texttt{CP}}_{i}$) as to whether the constraint was
satisfied. Note, in order for feasibility within a task, all constraints that
compromise this task must be satisfied. Because of this additional
information, we can frame a novel Bayesian Optimisation problem as shown in
Eq. 1.

$\arg\max_{\bm{x}\in\mathcal{X}}f(\bm{x}),$ (1)

with $\bm{x}$ denoting a binding list of motion parameters, $\mathcal{X}$ a
bounded binding domain and $f:\mathcal{X}\rightarrow\mathcal{R}$ determines
whether a feasible binding is found ($\mathcal{P}(X=\text{Feasible})=1.0$) for
a skeleton with respect to all the geometric constraints.

To achieve this goal, BO algorithms operate in two steps. In the first, a
Bayesian surrogate model is learned, while in the second an acquisition
function is maximised to determine new bindings.

Step 1: To learn a Bayesian regression surrogate model, one typically places a
GP prior on the latent function, $f(\cdot)$, which is fully specified through
a mean function, $m(\bm{x})$, and a covariance function or kernel
$k_{\bm{\theta}}(\bm{x},\bm{x}^{\prime})$ with $\bm{\theta}$ representing
kernel hyper-parameters. The model specification is completed by defining a
likelihood. Here, practitioners typically assume that observations $y_{l}$
adhere to a Gaussian noise model such that $y_{l}=f(\bm{x}_{l})+\epsilon_{l}$
where $\epsilon_{l}\sim\mathcal{N}(0,\sigma_{\text{noise}}^{2})$. This, in
turn, generates a Gaussian likelihood of the form
$y_{l}|\bm{x}_{l}\sim\mathcal{N}(f_{l},\sigma_{\text{noise}}^{2})$ where we
use $f_{l}$ to denote $f(\bm{x}_{l})$ with
$f(\bm{x})\sim\mathcal{G}\mathcal{P}(m(\bm{x}),k_{\bm{\theta}}(\bm{x},\bm{x}^{\prime}))$.
To learn a Bayesian classification model, one must additionally warp the the
output to be between [0,1] via the standard Normal CDF $\Phi(x)$, using a
variational GP as we no longer preserve an analytic form our exact GP
objective.

Step 2: To determine bindings $\bm{x}^{\star}$, we typically maximise an
acquisition function $\alpha$, such as Upper Confidence Bound (UCB) using
either an evolutionary search (ES) or gradient approach [16]. However, for our
specific setting of multiple constraints which involve both continuous and
binary constraints, they do not work out the box, thus we introduce a new
acquisition function in Sec. IV-B.

$\bm{x}^{\star}=\arg\max_{\bm{x}}\alpha^{\bm{\theta}}_{\text{UCB}}(\bm{x}|\mathcal{D}),\
\ \
\alpha_{\text{UCB}}^{\bm{\theta}}(\bm{x}|\mathcal{D})=\mu(\bm{x};\bm{\theta})+\sqrt{\beta}\sigma(\bm{x};\bm{\theta})$
(2)

Where $\beta$ is a hyper-parameter that controls the exploration–exploitation
trade-off. In this work we use the ES method NSGA-II, to maximise the
acquisition functions.

##  IV Representing & Utilising Constraint Primitive Knowledge

To achieve guided exploration in binding tree search, firstly the geometric
constraints experienced in the previous simulation rollouts should be properly
represented as transferable knowledge that is easy to store and retrieve, as
described in Sec. IV-A. In IV-B we purpose our method Constraint Primitive
Bayesian Optimisation (CP-BO) to utilise the stored information from Sec. IV-A
to reduce calls to the (expensive) simulator. Lastly, in IV-C we then describe
how to transfer the knowledge stored from Sec. IV-A across tasks.

###  IV-A Causal Graph: Representing CP Knowledge

To transfer knowledge across tasks, we must make environmental constraints
independent from specific tasks and represent them as objective knowledge. We
propose to use constraint primitives (CP) to model the respective geometric
restrictions of the environment. For example in Fig. 1, the planner should
learn from the failure at t1.2: the unsuccessful inverse kinematic solver Inv-
kin indicates that right is not a good binding for #dir1 when body is at pose0
due to the collision with obstacle2. We describe a CP type by four elements:
the failure vertex (e.g., Inv-kin), the the responsible decision vertexes
(e.g., #dir1), the relevant context vertexes (e.g., {obstacle2,pose0}), and
other connection vertexes.

Encode CP by Causal Graph: A directional graph is a natural choice for
representing the causal relationships between the four elements of a CP type.
With directional graphs, we can compare CP types via examining the isomorphism
of their graphs and their corresponding vertexes. We plot in Fig. 2 the causal
graphs of some failure nodes in Fig. 1(b). Whenever a failure is detected
during binding searching, we utilize the skeleton to backtrack the semantic
relationships starting from the failure vertex, and meanwhile we use geometric
dependency chains rendered by the simulator to backtrack obstacles that result
in collisions. The directional edges in a causal graph represent input-output
as well as the collision relationships.

(a)
$\texttt{t1.2}\in\texttt{CP}_{\texttt{a}}(\langle\texttt{\\#dir}\rangle,\\{\texttt{obstacle2,pose0}\\})$

(b)
$\texttt{t1.4}\in\texttt{CP}_{\texttt{b}}(\langle\texttt{\\#dir}\rangle,\\{\texttt{obstacle1,pose0}\\})$

(c)
$\texttt{t2.1}\in\texttt{CP}_{\texttt{d}}(\langle\texttt{\\#dir,\\#pose}\rangle,\\{\texttt{shelf}\\})$

(d)
$\texttt{t2.3}\in\texttt{CP}_{\texttt{d}}(\langle\texttt{\\#dir,\\#pose}\rangle,\\{\texttt{shelf}\\})$

Figure 2: Constraint primitive points are represented by directional graphs
extracted from the semantic causal relationships provided by skeletons.
$\texttt{CP}_{\texttt{a}}$, $\texttt{CP}_{\texttt{b}}$,
$\texttt{CP}_{\texttt{d}}$ represent three different CP types.

CP types and CP points:  We further hash the causal graph by Weisfeiler Lehman
method [17] into a hexadecimal string and use it to constitute a unique ID for
a certain CP type: the type ID. CPs with the same type ID are considered
belong to the same CP type. We formally define a CP point of typeID as (3). It
indicates that the bindings of $\bm{x}=\langle
binding_{1},binding_{2},...\rangle$ in $\\{context_{1},context_{2},...\\}$ is
evaluated as $\bm{y}^{\texttt{CP}}_{\texttt{typeID}}$. Scalar
$\bm{y}^{\texttt{CP}}_{\texttt{typeID}}$ measures the severity of typeID being
breached. In its simplest implementation, we set
$\bm{y}^{\texttt{CP}}_{\texttt{typeID}}=-1$ for feasible CP points and
$\bm{y}^{\texttt{CP}}_{\texttt{typeID}}=0$ for the infeasible.

$\begin{split}\bm{y}=\texttt{CP}_{\texttt{typeID}}(\langle
binding_{1},binding_{2},...\rangle,\\{context_{1},context_{2},...\\})\end{split}$
(3)

The granularity of CP type is determined by the level of details encoded in
type ID. The more semantic and geometric features are included in type ID, the
finer the CP are subdivided. A definition should be detailed enough to divide
the constraints into groups in each of which the data points are comparable to
each other and useful covariance information can be extracted by Bayesian
optimization. For example in Fig. 2, $\texttt{CP}_{\texttt{a}}$ should be
distinguished from $\texttt{CP}_{\texttt{b}}$ since it is subjected to a
different obstacle; while $\texttt{CP}_{\texttt{c}}$ should is distinguished
from $\texttt{CP}_{\texttt{d}}$ as it has different decision arguments. On the
other hand, the CP representation should be general enough to enable knowledge
transfer across as many as tasks.

Figure 3: CP-BO assists binding search by guiding exploration (left) and
transferring knowledge (right).

For the TAMP tasks of concern, we can use a global CP dictionary
$\mathcal{D}=\\{\text{type ID}:=\ \mathcal{D}^{\texttt{CP}}_{\text{type
ID}}\\}$ to store CP points by CP types. With this knowledge container, each
failure in binding search is expected to contribute to the total understanding
of the environment. During binding search (see Fig 3 (left)), a failure point
$\texttt{point}_{\texttt{x}}$ is encoded to a type ID
$\texttt{type}_{\texttt{x}}$. If $\texttt{type}_{\texttt{x}}$ already exists
in $\mathcal{D}$, we add $\texttt{point}_{\texttt{x}}$ to the data set of
$\texttt{type}_{\texttt{x}}$; otherwise we create $\texttt{type}_{\texttt{x}}$
in $\mathcal{D}$ and initialize it with $\texttt{point}_{\texttt{x}}$.

###  IV-B Bayesian Optimisation Over Constraint Primitive Knowledge

When constraint primitives are collected, they can support binding decision
making thereafter.

Acquisition Function:  Given a set of $n$ constraints
$\texttt{CP}_{1},\ldots,\texttt{CP}_{n}$ and corresponding feasible sets of
constraint values $\texttt{F}_{1},\ldots,\texttt{F}_{n}$, and corresponding
constraint datasets
$\mathcal{D}^{\texttt{CP}}_{0},\ldots,\mathcal{D}^{\texttt{CP}}_{n}$, we
notice that our main objective 1 can be further decomposed with individual
constraint primitives, specifically
$\mathcal{P}^{\text{Feasible}}=\prod_{i}^{n}\mathcal{P}(\texttt{CP}_{\texttt{i}}\in\texttt{F}_{\texttt{i}}\mid\bm{x},\mathcal{D}^{\texttt{CP}}_{i})$.
Inspired by this decomposition, we introduce a novel acquisition function
named Minimum Probability of Feasibility (MPF) that allows us to successfully
find bindings with high probability of satisfying all constraints.

$\displaystyle\alpha^{\bm{\theta}}_{\text{MPF}}(\bm{x}|\mathcal{D})$
$\displaystyle=\min\Bigg{[}\mathcal{P}(\texttt{CP}_{1}\in\texttt{F}_{1}),\ldots,\mathcal{P}(\texttt{CP}_{n}\in\texttt{F}_{n})\Bigg{]},$

Where we use
$\mathcal{P}(\texttt{CP}_{1}\in\texttt{F}_{1})=\mathcal{P}(\texttt{CP}_{1}\in\texttt{F}_{1}\mid\bm{x},\mathcal{D})$
for compactness. To give further intuition as to the benefit of this
formulation, once we maximised our acquisition and found an $\bm{x}^{\star}$,
there exists a $\bm{p}^{\star}$ such that
$\alpha^{\bm{\theta}}_{\text{MPF}}(\bm{x}^{\star})=\bm{p}^{\star}$, where
$\mathcal{P}(\texttt{CP}_{\texttt{k}}\in\texttt{F}_{\texttt{k}})\geq\bm{p}^{\star}$
for $k=1,\ldots\,n$. With this new acquisition function, we can now purpose
our full Bayesian Optimisation algorithm CP-BO (Alg. 2) to suggest bindings
for UCT. Additionally, perform BO directly on the UCT reward function
(detailed in III-B) using the UCB acquisition 2, which we name MCTS-BO.

Input: Total number of constraints $N$, constraint dataset
$\mathcal{D}^{\texttt{CP}}_{i}=\\{\bm{x},\bm{y}^{\texttt{CP}}_{i}\\}_{l=1}^{n_{0}}$,
$\alpha^{\bm{\theta}}_{\text{MPF}}$.

while  _Not Feasible_ do

Fit a surrogate model to each constraint $i$’s dataset
$\mathcal{D}^{\texttt{CP}}_{i}$.

Find new bindings $\hat{\bm{x}}$ by maximising
$\alpha^{\bm{\theta}}_{\text{MPF}}$.

Evaluate new bindings by querying the simulator to acquire
[$\hat{\bm{y}}^{\texttt{CP}}_{0},\ldots,\hat{\bm{y}}^{\texttt{CP}}_{n}]=f(\hat{\bm{x}})$.

Update each constraint $i$’s dataset by creating
$\mathcal{D}^{\texttt{CP}}_{i}=\mathcal{D}^{\texttt{CP}}_{i}\cup\\{\hat{\bm{x}},\hat{\bm{y}}^{\texttt{CP}}_{i}\\}$.

Algorithm 2 Constraint Primitive Bayesian Optimisation (CP-BO)

(a) $T_{\text{deskP}}\rightarrow T_{\text{desk}}$,
$\\{T_{\text{deskP}},T_{\text{desk}}\\}\rightarrow T_{\text{regrasp}}$

(b) $\\{T_{\text{packP1}},T_{\text{packP2}},T_{\text{packP3}}\\}\rightarrow
T_{\text{pack}}$

Figure 4: The constraint primitives transferred across different tasks, as
indicated by the arrows. CP points relevant to the new tasks are automatically
activated and utilized by CP-BO to assist binding search.

###  IV-C Transferring Constraint Primitive Knowledge

In the binding search of another task, we encode each encountered failure as a
type ID and use it to query in the global CP dictionary $\mathcal{D}$ (see
Fig. 3 (right)). If it matches with existing IDs, certain groups of CP points
are activated and connected to their casual decision tree nodes. Then, we can
use these CP points to guide exploration at these decision tree nodes by CP-
BO. We note this knowledge transfer mechanism by CP-BO Transfer. CP-BO
Transfer is fully automated and its effectiveness depends on the amount of
relevant knowledge already stored in $\mathcal{D}$.

##  V Empirical evaluation

Figure 5: Evaluation results of the the baseline algorithm Quasi-random vs our
three proposed algorithms CP-BO, MCTS-BO and CP-BO Transfer across three
benchmark tasks. These box plots show the number of rollouts in the simulator
required to achieve feasibility, with confidence intervals (+-sd). From these
figures, we observe that both all our purposed CP-BO, MCTS-BO versions of PW-
UCT consistently outperform Quasi-random PW-UCT. Whilst CP-BO Transfer PW-UCT
offers a significant further reduction in rollouts in the simulator.

The aim of this section is to evaluate the advantages of our framework for
transfer learning in TAMP. We evaluate our proposed methods on three robot
tasks: three desk task ($T_{\text{desk}}$ in Fig. 1(a)) which has two binding
decisions ($\langle\texttt{\\#dir1,\\#pose12}\rangle$ in Fig. 1(a)), the re-
grasping task ($T_{\text{regrasp}}$) which has four binding decisions, and the
packing task ($T_{\text{pack}}$) which has three binding decisions 111See Fig.
4. Please see Appendix B for more details on tasks.. We also make the
following assumption:

###### Assumption 1

We assume that feasible skeletons are already given by symbolic planners, thus
only require correct binding of variables.

Binding Search Methods: We evaluate four methods for guiding node expansion in
PW-UCT, by assigning values to bindings. First, the baseline method, Quasi-
random, which uses a Voronoi sampler to bind variables and thus does not
collect or utilise the geometric constraint primitive knowledge outlined in
Sec. IV. MCTS-BO performs GP-UCB [18] directly on the reward function 1 of PW-
UCT, thus does not collect or utilise the geometric constraint primitive
knowledge as its collected dataset of bindings and reward are dependent on the
specific tasks success. Finally, we evaluate our proposed CP-BO which
constructs a dataset of geometric constraint primitive knowledge (Sec. IV-A),
and utilises it using BO (Sec. IV-B). Finally in CP-BO Transfer, we allow CP-
BO predecessor tasks to collect CP points into their respective datasets
$\mathcal{D}^{\texttt{CP}}_{\texttt{typeID}}$. To prevent CP-BO from using
previous knowledge, we reset $\mathcal{D}$ before each scenario.

Surrogate Model Design: For modelling the continuous UCT reward or modelling
the constraint primitives by a GP we use; a $\text{Mat\'{e}rn}(5/2)$
covariance function with automatic relevance detection and a constant mean
function. Additionally, we apply SMOTE to tackle the issue of unbalanced
classes in GP classification.

Transfer Learning Setup: For transfer learning, as shown in Fig. 4,
$T_{\text{desk}}$ has one predecessor task, $T_{\text{regrasp}}$ has two and
$T_{\text{pack}}$ has three. Each predecessor task is evaluated for 30
rollouts.

Each algorithm is evaluated on the benchmark tasks for 1000 seeds and we show
the results in Fig. 5. We observe that in most tasks that, even without
transferring knowledge, reduces rollouts in the simulator: shown by Quasi-
random performing the worst compared with CP-BO and MCTS-BO. Finally, in CP-BO
Transfer we see significant reductions in rollouts required to find a feasible
binding set. We observe a 71.69 % relative reduction on expensive simulation
calls vs the baseline Quasi-random on $T_{\text{desk}}$, on
$T_{\text{regrasp}}$ a 60.37% relative reduction and a 43.60 % relative
reduction on $T_{\text{pack}}$. We verify via that CP-BO acquired this gain is
derived solely from CP-BO Transfer representing, efficiently utilising and
transferring CP knowledge across tasks.

##  VI Conclusion

Based on a novel representation of the geometric constraints in TAMP, we
propose a CP-BO to guide the UCT exploration in binding search for solving
challenging TAMP tasks. Our empirical evaluation shows its effectiveness in a
variety of robot tasks. Especially when previous CP knowledge are available,
the CP-BO solvers can generate suggestions with higher quality from limited
data than the comparable task-dependant approach MCTS-BO. With CP-BO, the
planner is able to accumulate knowledge in a stream of tasks and to achieve a
constant transfer learning of the geometric environment.

##  Appendix A Additional Methodological background

###  A-A PW-UCT: MCTS for binding search

In this study, we use a reward function defined as
$r=0.1\left(d_{\text{end}}/d_{\text{total}}\right)+r_{\text{success}}$, where
$d_{\text{end}}$ is the depth in UCT where $node$ is terminated. The first
term of $r$ is a normalized depth that encourages the robot to avoid branches
where bindings could fail earlier. For the second term, $r_{\text{success}}=1$
when all bindings are successfully found, otherwise $r_{\text{success}}=0$. By
BACK-PROPAGATION$(node)$, the reward is assigned to $node$ and its parents as
a standard MCTS procdeure. In each rollout, ADD-VISIT$(node)$ increases the
visit number of $node$ by 1 and meanwhile updates the environment state. In a
PW law, a sufficient visiting number means that the value of an existing node
is estimated sufficiently well, then new nodes will be expanded to explore the
unreached decision space. In addition, if the decision at $node$ is finite,
PW-EXPANDABLE$(node)$ will prevent further node expansion after all choices
are enumerated. For further details we refer the readers to [11]

##  Appendix B Experiment details

###  B-A The TAMP tasks for evaluation

(a) $T_{\text{regrasp}}$

(b) $T_{\text{pack}}$

Figure 6: TAMP tasks for evaluation.

The regrasping task. For $T_{\text{regrasp}}$ as shown in Fig. 6(a), we have
the problem
BINDING$(T_{\text{regrasp}},\langle\\#dir1,\\#pose13,\\#dir2,\\#pose12\rangle)$
to solve. Compared to $T_{\text{desk}}$ in Fig. 1(a), $T_{\text{regrasp}}$ is
more challenging in binding search since it has a longer horizon with two more
decisions to make. To avoid collision with obstacles and meanwhile facilitate
the following regrasping motion, #pose13 of body in region3 should be
carefully chosen. Then the planner should search for a new grasping direction
#dir2 that enable a collision-free trajectory on the shelf later during the
placement in region2 while not colliding with obstacles on the table at the
moment.

The packing task. In $T_{\text{pack}}$ from Fig. 6(b), the mobile robot must
relocate body1, body2, body3 from region4 to region5 without causing any
collisions and drops of the movable bodies. The new locations of all the
bodies in region5: #pose1, #pose2, #pose3, should be strategically decided to
ensure that the first-arriving bodies don’t occupy too much space. To make the
task even more difficult, we set region5 25% smaller than region4. This
binding search problem is noted as
BINDING$(T_{\text{pack}},\langle\\#pose1,\\#pose2,\\#pose3\rangle)$.

###  B-B CP-BO with transferred knowledge

As shown in Fig. 4, the predecessor task of $T_{\text{desk}}$ is
$T_{\text{deskP}}$ while $T_{\text{regrasp}}$ have two predecessor tasks,
$T_{\text{deskP}}$ and $T_{\text{regrasp}}$. Similarly, we run three
predecessor tasks including $T_{\text{packP1}}$, $T_{\text{packP2}}$,
$T_{\text{packP3}}$ are ahead of $T_{\text{pack}}$. Each predecessor task is
evaluated for 30 rollouts. We do not expect that the surrogate models of CP-BO
is fully converged within such data-collection sessions, but give the Trans-
CP-BO algorithm a warm start.

## References

  * [1] L. P. Kaelbling and T. Lozano-Pérez, “Hierarchical task and motion planning in the now,” in 2011 IEEE International Conference on Robotics and Automation, pp. 1470–1477, IEEE, 2011.
  * [2] S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell, and P. Abbeel, “Combined task and motion planning through an extensible planner-independent interface layer,” in 2014 IEEE international conference on robotics and automation (ICRA), pp. 639–646, IEEE, 2014.
  * [3] N. T. Dantam, Z. K. Kingston, S. Chaudhuri, and L. E. Kavraki, “Incremental Task and Motion Planning: A Constraint-Based Approach.,” in Robotics: Science and systems, vol. 12, p. 00052, Ann Arbor, MI, USA, 2016.
  * [4] C. R. Garrett, T. Lozano-Pérez, and L. P. Kaelbling, “PDDLStream: Integrating symbolic planners and blackbox samplers via optimistic adaptive planning,” in Proceedings of the International Conference on Automated Planning and Scheduling, vol. 30, pp. 440–448, 2020.
  * [5] C. R. Garrett, R. Chitnis, R. Holladay, B. Kim, T. Silver, L. P. Kaelbling, and T. Lozano-Pérez, “Integrated task and motion planning,” arXiv preprint arXiv:2010.01083, 2020.
  * [6] B. Kim, K. Lee, S. Lim, L. Kaelbling, and T. Lozano-Pérez, “Monte carlo tree search in continuous spaces using voronoi optimistic optimization with regret bounds,” in Proceedings of the AAAI Conference on Artificial Intelligence, vol. 34, pp. 9916–9924, 2020.
  * [7] T. Yee, V. Lisỳ, and M. H. Bowling, “Monte carlo tree search in continuous action spaces with execution uncertainty.,” in IJCAI, pp. 690–697, 2016\.
  * [8] B. Kim, Z. Wang, L. P. Kaelbling, and T. Lozano-Pérez, “Learning to guide task and motion planning using score-space representation,” The International Journal of Robotics Research, vol. 38, no. 7, pp. 793–812, 2019\.
  * [9] L. Kocsis, C. Szepesvári, and J. Willemson, “Improved monte-carlo search,” Univ. Tartu, Estonia, Tech. Rep, vol. 1, 2006.
  * [10] S. M. LaValle et al., “Rapidly-exploring random trees: A new tool for path planning,” 1998.
  * [11] T. Ren, G. Chalvatzaki, and J. Peters, “Extended task and motion planning of long-horizon robot manipulation,” arXiv preprint arXiv:2103.05456, 2021\.
  * [12] A. Couëtoux, J.-B. Hoock, N. Sokolovska, O. Teytaud, and N. Bonnard, “Continuous upper confidence trees,” in International Conference on Learning and Intelligent Optimization, pp. 433–445, Springer, 2011.
  * [13] D. Auger, A. Couetoux, and O. Teytaud, “Continuous upper confidence trees with polynomial exploration–consistency,” in Joint European Conference on Machine Learning and Knowledge Discovery in Databases, pp. 194–209, Springer, 2013.
  * [14] T. Lozano-Pérez and L. P. Kaelbling, “A constraint-based method for solving sequential manipulation planning problems,” in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3684–3691, IEEE, 2014.
  * [15] F. Lagriffoul and B. Andres, “Combining task and motion planning: A culprit detection problem,” The International Journal of Robotics Research, vol. 35, no. 8, pp. 890–927, 2016.
  * [16] J. T. Wilson, F. Hutter, and M. P. Deisenroth, “Maximizing acquisition functions for bayesian optimization,” in Advances in Neural Information Processing Systems (NeurIPS), 2018.  Acceptance rate: 22
  * [17] N. Shervashidze, P. Schweitzer, E. J. Van Leeuwen, K. Mehlhorn, and K. M. Borgwardt, “Weisfeiler-lehman graph kernels.,” Journal of Machine Learning Research, vol. 12, no. 9, 2011.
  * [18] N. Srinivas, A. Krause, S. M. Kakade, and M. Seeger, “Gaussian process optimization in the bandit setting: No regret and experimental design,” arXiv preprint arXiv:0912.3995, 2009.

##  Appendix C Acknowledgments

….