Learning Feasibility of Factored Nonlinear Programs in Robotic Manipulation Planning

In the discrete SAT and CSP literature, a minimal infeasible subset of constraints (also called Minimal Unsatisfiable Subset of Constraints or Minimal Unsatisfiable Core) is usually computed by solving a sequence of SAT and MAX-SAT problems [15, 16, 17].

In a general continuous domain, a minimal infeasible subset can be found by solving a linear number of problems [18]. This search can be accelerated with a divide and conquer strategy, with logarithmic complexity [19]. In convex and nonlinear optimization, we can find approximate minimal subsets by solving one optimization program with slack variables [20]. In contrast, our method uses learning to directly predict minimal infeasible subsets of variables and constraints, and can be combined with these previous approaches to reduce the computational time.

We use Graph Neural Networks (GNN) [21, 22, 23] for learning in graph-structured data. Different message passing and convolutions have been proposed, e.g. [24, 25]. Our architecture, targeted towards inference in factored nonlinear programs, is inspired by previous works that approximate belief propagation in factor graphs [26, 27, 28].

Recently, GNN models have been applied to solve NP-hard problems [29], Boolean Satisfaction [30], Max cut [31], constraint satisfaction [32], and discrete planning [33, 34, 35]. Compared to state-of-the-art solvers, learned models achieve competitive solution times and scalability but are outperformed in reliability and accuracy. To our knowledge, this is the first work to use a GNN model to predict the minimal infeasible subgraphs of a factored nonlinear program in a continuous domain.

In robotic manipulation planning, GNNs are a popular architecture to represent the relations between movable objects, because they provide a strong relational bias and a natural generalization to including additional objects in the scene.

For example, they have been used as problem encoding to learn policies for robotic assembly [36, 37] and manipulation planning [38], to learn object importance and guide task and motion planning [39], and to learn dynamical models and interactions between objects [40], [41]. Previous works often use object centric representations: the vertices of the graph represent the objects and the task is encoded in the initial feature vector of each variable. Alternatively, our model performs message passing using the structure of the nonlinear program of the manipulation sequence, achieving generalization to different manipulation sequences that fulfil different goals.

A Factored-NLP $G$ is a bipartite graph $G=(X\cup H,E)$ that models the dependencies between a set of variables $X=\{x_{i}\in\mathbb{R}^{n_{i}}\}$ and a set of constraints $H=\{h_{a}:\mathbb{R}^{m_{a}}\to\mathbb{R}^{m_{a}^{\prime}}\}$. Each constraint $h_{a}(x_{a})$ is a piecewise differentiable function evaluated on a (typically small) subset of variables $x_{a}\subseteq X$ (e.g. $x_{a}=\{x_{1},x_{4}\}$). The edges model the dependencies between variables and constraints $E=\{(x_{i},h_{a}):\text{constraint $h_{a}$ depends on variable $x_{i}$}\}$. Throughout this document, we will use the indices $i,j$ to denote variables and $a,b$ to denote constraints.

The underlying/associated nonlinear program is

The constraints $h_{a}(x_{a})\leq 0$ also include equality constraints (that can be written as $h_{a}(x_{a})\leq 0$ and $h_{a}(x_{a})\geq 0$).

A Factored-NLP $G$ is feasible ($\mathcal{F}(G)=1$) iff (1) has a solution, that is, there exists a value assignment $\bar{x}_{i}$ for each variable $x_{i}$ such that all constraints $h_{a}(\bar{x}_{a})$ are fulfilled. Otherwise, it is infeasible ($\mathcal{F}(G)=0$). This assignment can be computed with nonlinear optimization methods, such as Augmented Lagrangian or Interior Points. A minimal infeasible subgraph $M\subseteq G$ is an infeasible subset of variables and constraints whose any proper subset is feasible,

Given a graph $G$ and a subset of variables $X^{\prime}\subseteq X$, a variable-induced subgraph $G[X^{\prime}]=(X^{\prime}\cup H^{\prime},E^{\prime})$ with $H^{\prime}=\{h_{a}\in H:\text{Neigh}_{G}(h_{a})\subseteq X^{\prime}\}$ is the subgraph spanned by the variables $X^{\prime}$. Intuitively, $G[X^{\prime}]$ contains the variables $X^{\prime}$ and all the constraints that can be evaluated with these variables. In this work, we consider only minimal subgraphs in the form of variable-induced subgraphs, i.e. $M=G[X^{\prime}]\subseteq G$, because it enables a more compact representation (our approach can be adapted to predict general subgraphs if required, changing the proposed variable classification to constraint classification in Sec III-B).

A minimal infeasible subgraph is connected and a supergraph $\tilde{M}\supseteq M$ of an infeasible subgraph $M$ is also infeasible. A factored-NLP $G$ can contain multiple infeasible subgraphs, and a variable $x_{i}\in X$ can belong to multiple infeasible subgraphs.

Let $\Phi_{G}=\{M_{r}\subseteq G\}$ be the set of minimal infeasible subgraphs of a Factored-NLP $G$. Instead of learning the mapping $\phi:G\to\Phi_{G}$ directly, we propose to learn an over-approximation $\tilde{\phi}$ that can efficiently be framed as binary variable classification.

We first introduce the variable-feasibility function $\psi(x_{i};G)$ that assigns a label $y_{i}\in\{0,1\}$ to each variable $x_{i}\in X$. $y_{i}=0$ if $x_{i}$ belongs to some infeasible subgraph $M_{r}$, and $y_{i}=1$ otherwise. Given such a labelled graph, we can recover the infeasible subgraphs approximately by computing the connected components on the subgraph induced by the variables with label $0$, i.e. $G\left[\{x_{i}\in X:y_{i}=0\}\right]$. Thus, we define the approximate mapping as,

where CCA denotes a connected component analysis.

The approximate mapping $\tilde{\phi}$ is exact, i.e. $\tilde{\phi}=\phi$, if the infeasible subgraphs are disconnected. If two or more of the infeasible subgraphs are connected, it returns their union as a minimal infeasible sugraph, i.e. $\cup\tilde{\phi}=\cup\phi$, which over-approximates the size of the original minimal infeasible subgraph. Our neural model will be trained to imitate the labels of the variable-feasibility function $\psi$.

We emphasize that learning the approximate function $\tilde{\phi}$ is not a real limitation. First, because the prediction will be integrated into an algorithm that can further reduce the size of the infeasible subgraph, if it is not already minimal, as shown later in Sec. III-D. Second, because in practice finding small infeasible subgraphs, as opposed to strictly minimal, is already useful in the applications. Finally, note that $\phi$ can be converted to a multiclass variable classification $f(x_{i};G)=y\subseteq\{1,\ldots,R\}$, where each variable can belong to multiple classes – but this would require a complex, and potentially intractable, permutation invariant formulation.

A fundamental idea of our method is to use the structure of the Factored-NLP for message passing with Graph Neural Networks (GNN) to learn the variable-feasibility $\psi(x_{i};G)$.

Each variable vertex $x_{i}\in X$ has a feature vector $z_{i}\in\mathbb{R}^{n_{z}}$ that is updated with the incoming messages of the neighbour constraints. $z_{i}$ is initialized with $z_{i}^{0}$ to encode semantic and continuous information of the variable $x_{i}$ (an example on how to initialize the features in manipulation planning is shown in Sec. IV-B). The update rule follows a two-step process: first, each constraint computes and sends back a message to each neighbour variable, which depends on the current features of all the neighbour variables. Second, each variable aggregates the information of the incoming messages from the constraints and updates its feature vector. A graphical representation is shown in Fig 2.

$\mu_{a\to i}\in\mathbb{R}^{n_{\mu}}$ is the message from constraint $a$ to variable $i$. $[\oplus z_{i}]_{i}$ denotes concatenation. $N(a)=\text{Neigh}_{G}(h_{a})$ is the ordered set of variables connected to the constraint $h_{a}$. $N(i)=\text{Neigh}_{G}(x_{i})$ is the set of constraints connected to variable $x_{i}$. AGG is an aggregation function, e.g. max, sum, mean or weighted average. We use max (element-wise) in our implementation. Update and $\texttt{Message}_{a}$ are small MLPs (Multilayer Perceptron) with learnable parameters. Likewise the nonlinear constraints in the Factored-NLP are not permutation invariant or symmetric, the features $z_{i}$ have to be concatenated in a predefined order $N(a)$ when evaluating $\texttt{Message}_{a}$.

The function Update is shared by all vertices (which generalizes to Factored-NLPs with additional variables). The function $\texttt{Message}_{a}$ is shared between different constraints of the Factored-NLP that represent the same mathematical function, i.e. $\texttt{Message}_{a}=\texttt{Message}_{b}~{}\text{iff}~{}h_{a}(x)=h_{b}(x)$ (which generalizes to Factored-NLPs with additional constraints). For example, in manipulation planning, all constraints that model collisions between objects will share the same Message MLP.

The message passing update (4) is performed $K$ times, starting from the initial feature vectors $z_{i}^{0}$. The feature vectors after $K$ iterations are used for feasibility prediction with a small MLP classifier.

The parameters of the classifier, message and update networks are trained end-to-end to minimize the weighted binary cross entropy loss between the prediction $\hat{y}_{i}$ and the variable-feasibility labels $y_{i}$.

To account for the approximation of our variable classification formulation, and small prediction errors, we can integrate the learned classifier into a classical algorithm to detect minimal infeasible subgraphs.

We assume the user provides the Solve and Reduce routines, that respectively check if a Factored-NLP is feasible and compute a minimal infeasible subset of an infeasible graph. Reduce is an expensive routine, as it requires solving several nonlinear programs adding and removing constraints. The number of evaluated NLPs (and therefore the computation time) depends on the size of the input graph: linear on the total number of variables using [18], or logarithmic [19].

Our algorithm is shown in Alg. 1. The GNN model is evaluated on the input Factored-NLP and computes a feasibility scores $\hat{y_{i}}$ for each variable. Iteratively increasing the classification threshold $\delta$, we select the candidate infeasible variables $X_{\delta}$ using the current threshold $\delta$. We generate candidate infeasible subgraphs with a connected component analysis on the variable-induced subgraph $G[X_{\delta}]$, that are evaluated with Solve. Once an infeasible subgraph is found, we use Reduce to get a minimal infeasible subgraph.

A non-learning approach runs solve and reduce directly on the input Factored-NLP. Therefore, the acceleration in our algorithm comes from evaluating these routines with small (ideally minimal) candidates. Alg. 1 can be extended to compute several minimal infeasible subgraphs by removing the break instruction (line 1) and adding a special check before solving a candidate subgraph (line 1), to avoid solving for a supergraph of a found infeasible subgraph.

A Factored-NLP models the motion of robots and objects that is implied by a sequence of high-level actions (such as pick and place) as a nonlinear optimization/feasibility problem. It contains variables that represent the configuration of objects and robots at each time step. We focus on the keyframe or mode-switch problem, that considers only the configurations at the beginning and end of each motion phase (e.g. when picking or placing an object), but not the trajectory between them. This follows a common problem decomposition used in Task and Motion Planning, where path feasibility is evaluated afterwards with trajectory optimization or sampling based motion planning [42, 1].

We include three types of variables: robot configurations, object absolute positions and object relative positions with respect to the parent frame. Nonlinear constraints model grasping (Grasp in Fig. 3), kinematic switches (Kin), valid placements (Pos), collision avoidance (brown squares), reference position (Ref), time consistency (Equal), and geometric consistency between relative and absolute pose (PoseDiff). The structure of the NLP is similar to previous factored formulations [7, 42, 43, 44] but less compact (one can formulate the nonlinear program without explicitly introducing the absolute position of objects). However, this is necessary to formulate Factored-NLPs of diverse manipulation sequences using only few types of nonlinear constraints. Because each type of constraint will correspond to a different Message network, this formulation is crucial to enable generalization of the GNN model. In Fig. 3, we show the Factored-NLP that corresponds to the sequence (pick object B with robot Q), (pick object B with robot W from robot Q), (place object B on top of object A with Robot W).

The structure of the Factored-NLP implicitly encodes the number of objects, robots and the high-level action sequence (e.g. which robots pick which objects). The geometric description of the environment is encoded locally in the initial feature vector of each variable $z_{i}^{0}$. Specifically, the initial feature vector includes the information of unary constraints (i.e. constraints evaluated only on a single variable, which are then not added to the message passing architecture), additional semantic class information (for example, whether the variable represents an object or a robot, but without including a notion of time index or entity), and geometric information that is relevant for the constraints (for example, the size of the objects). The dimension of $z_{i}^{0}$ is fixed, and shorter feature vectors are padded with zeros.

For example, suppose that the Factored-NLP of Fig. 3 is evaluated in a scene where robot $Q$ is at pose $T_{Q}=$ $[\,0.32,\,0.41,\,0.56,\,0.707,\,0,\,0,\,0.707]$ , the start position of object $A$ is $T_{A}=[0.35,\,0.4,\,0.5,\,0.707,\,0,\,0,\,0.707]$ and object $A$ is a box of size $S_{A}=[0.2,\,0.3,\,0.2]$. Then $z^{0}$ of variables $\{q_{0},q_{1},q_{2},q_{3}\}$ is $[1,\,0,\,0,\,0,\,0,\,0,T_{Q}]$ where the first 6 components are a one-hot vector to indicate that it is a robot. The $z^{0}$ of $\{a_{0},a_{1},a_{2},a_{3}\}$ is $[0,\,1,\,0,\,0,\,0,\,0,\,T_{A}]$ , where first components indicate that it is a relative pose with respect to the reference position. The $z^{0}$ of $\{A_{0},A_{1},A_{2},A_{3}\}$ is $[0,\,0,\,1,\,0,\,0,\,0,\,S_{A},\,0,\,0,\,0,\,0]$ to indicate that it is an absolute position of an object of size $S_{A}$.

We evaluate our model in robotic sequential manipulation. The high-level goal is to build towers and rearrange blocks in different configurations, in scenarios that contain several and varying number of blocks, robots and movable obstacles, at different positions, see Fig. 4 and 5. The following setting is used to generate the train dataset (4800 Factored-NLPs):

$\bullet$ Five movable objects: 3 blocks and 2 obstacles. Both types of objects have collisions constraints, but obstacles are bigger and usually block grasps or placements. $\bullet$ Two robots: 7-DOF Panda robot arms, that can pick and place objects using a top grasp. $\bullet$ Different geometric scenes: the position of the objects, robots and tables is randomized. $\bullet$ Manipulation sequences of length 4 to 7.

To evaluate the generalization capabilities of the learned model, we consider three additional datasets:

$\bullet$ +Robots: we add an additional robot. $\bullet$ +Blocks: we add two additional blocks. $\bullet$ +Actions: it contains Factored-NLPs with longer manipulation sequences (length of 8 to 10).

For training the GNN model we need a set of Factored-NLPs with labelled variables to indicate whether they belong to a minimal infeasible subset. First, we generate a set of interesting high-level action sequences. Second, we evaluate the manipulation sequences on random geometric scenes to generate Factored-NLPs (including the initial feature vectors). To compute the feasibility labels, we adapt the conflict extraction algorithm of [7] to find up to 10 minimal infeasible subgraphs.

We compare our model (GNN) against a Multilayer Perceptron (MLP) and a sequential model (MLP-SEQ), trained with the same dataset.

The MLP computes $\hat{y}_{i}=\text{MLP}(\tilde{z}^{0}_{i},A,C)$. $\tilde{z}_{i}^{0}=[z_{i}^{0},t_{i},e_{i}]$ is the feature vector of the variable we want to classify. It concatenates the feature vector $z_{i}^{0}$ used in the GNN, with the time index $t_{i}$ of the variable, and a parametrization that defines the entity $e_{i}$ (for instance, we represent an object with its starting pose – one hot encodings could not generalize to more objects). $A$ is the encoding of the whole action sequence, using small vectors to encode each token, e.g. {“pick”, “block1”, “l_gripper”, “table”}. To account for sequences of different length, we fix a maximum length and add padding. $C$ is the scene parametrization and contains the position/shapes of all possible objects and robots. We also evaluate MLP-SEQ, a sequential model $\text{MLP}(\tilde{z}^{0}_{i},\text{SEQ}(A),C)$ that encodes the action sequence with a recurrent network (Gated Recurrent Units).

We first evaluate the accuracy of the models to predict if a variable belongs to a minimal infeasible subset, see Tab. I. Our GNN model outperforms the alternative architectures, both in the original Train Data and, specially, in the extension datasets. Our model keeps a constant $\sim$95% success rate across all datasets, while the performance of MLP and MLP-SEQ drops to 48% and 75%. We also evaluate the accuracy of our model to predict infeasible subgraphs, using the proposed method that combines variable classification and connected component analysis, using the initial threshold for classification $\delta=0.5$. Our model outperforms MLP and MLP-SEQ, and finds between 70% and 57% of the infeasible subgraphs, and 30%-50% of the predicted subgraphs are minimal, see Tab. II. Between 34%-48% of the predicted infeasible graphs are actually feasible (not shown in the table due to space limitation).

MLP, MLP-SEQ and GNN have the same information to make the predictions. The Factored-NLP is a deterministic mapping of the action sequence and the geometric scene. Although a MLP could learn this mapping, our experiments show that the representation does not emerge naturally – confirming that a structured model yields better generalization.

We analyze the time required to find one minimal infeasible subgraph in an infeasible Factored-NLP with algorithms:

• •

  Oracle, which executes a single call to Solve and Reduce with a minimal infeasible subgraph as input.

• •

  General {1,2}, which are generic algorithms for conflict extraction: General 1 uses constraint filtering [18], and General 2 uses QuickXplain [19].

• •

  Expert is a heuristic algorithm for conflict extraction in manipulation planning [8]. It exploits the temporal structure, domain relaxations, and the convergence of the optimizer to quickly discover the conflicts.

• •

  GNN+{e,g1} combines the prediction of our GNN model with either Expert or General 1. Expert and General 1 are used as Reduce in Alg. 1.

Results are shown in Table III. GNN+g1 is 60-120x faster than General 2 (which is faster than General 1). This highlights the benefits of our approach in domains where we can compute a dataset using General offline, and train the model to get an order-of-magnitude improvement in new problems. GNN+g1 is 4-5x faster than the Expert algorithm, and only 1.2x slower than an oracle. Moreover, the acceleration provided by GNN is maintained in all the datasets. This confirms the good accuracy and generalization of the architecture seen in the classification results. As a side note, Expert is faster than General 2 because it solves a lot of small feasible NLPs first, until it finds one that is infeasible (which is faster than solving infeasible NLPs).

We demonstrate the benefits of neural accelerated conflict extraction inside the GraphNLP Planner [8] for solving TAMP problems. The planner iteratively generates high-level plans, detects infeasible subgraphs, and encodes this information back into the logical description of the problem.

For this evaluation, we define 10 high-level goals for each setting corresponding to the +Actions, +Robots, and +Blocks datasets, and report the total sum (including the 10 goals) of the number of solved NLPs and the computational time in the conflict extraction component of TAMP solver. GNN+e (which is more robust than GNN+g1 in this setting) takes only (8.33s, 511 NLPs), (9.83s, 603 NLPs), and (63.9s, 1979 NLPs) for each scenario, and is between 2 and 3 times faster than the expert algorithm, which requires (24.2s, 731 NLPs), (38.7s, 1116 NLPs), and (137.9s, 2554 NLPs).