Structured deep generative models for sampling on constraint manifolds in sequential manipulation
Anonymous Author(s)
AfﬁliationAddress email
Abstract: Sampling efﬁciently on constraint manifolds is a core problem inrobotics.
We propose Deep Generative Constraint Sampling (DGCS), which combines a deep generative model for sampling close to a constraint manifold with anonlinear constrained optimization to project onto the constraint manifold.
Thegenerative model is conditioned on the problem instance, taking a scene image asinput, and it is trained with a dataset of solutions and a novel analytic constraintterm.
To further improve the precision and diversity of samples, we extend theapproach to exploit a factorization of the constrained problem.
We evaluate ourapproach in two problems of robotic sequential manipulation in cluttered environments.
Experimental results demonstrate that our deep generative model producesdiverse and precise samples and outperforms heuristic warmstart initialization.
Introduction
We consider the problem of sampling points on a constraint manifold, i.e. ﬁnding diverse solutionsof a nonlinear mathematical program without costs.
Such problems arise throughout robotics, inparticular in solving sequential manipulation problems, as it will be the focus in our applications.
We assume that the constraints are given in terms of piecewise differentiable equalities and inequalities.
This allows us to leverage constrained optimization methods to project a randomly sampledconﬁgurations onto the manifold.
However, for highly non-linear constraints and disconnected solution manifolds, a local optimization method can get trapped in local optima and fail to ﬁnd a feasiblesolution.
Further, our aim is to generate a diverse and covering set of points on the constraint manifold, and to reduce the computation time needed by the local optimization.
The crucial challengetherefore is to ﬁrst sample diversely and close to the constraint manifold , so that the local optimizercan efﬁciently project to solutions covering the constraint manifold.
We follow a learning-based approach, where we assume that a dataset of feasible points for various problem instances is available.
Reusing precomputed data on similar problems is a promisingapproach [1, 2, 3].
A fundamental challenge is that the mapping between problem instance and thefeasible manifold is extremely nonlinear and discontinuous [4, 5].
Our approach, therefore, is to train a generative neural model to map a problem instance – given interms of a scene image – to a randomized sample close to the constraint manifold for that instance.
An image-based prediction of solutions to manipulation problems has recently received attention[1, 6, 7, 8, 9] as it provides a ﬁxed size parametrization (image resolution) that encodes the obstaclesand the objects to be interacted, without need to engineer problem instance features.
Importantly, itis a natural way to encode non-ﬁxed number of objects.
Generative Adversarial Networks (GANs) [10, 11] and Variational Autoencoders (VAEs) [12] haveproposed a powerful methodology for training such a generative model and shown great potentialto represent complex distributions in high dimensional space.
This work adopts the training objective and method of GANs, which is to minimize the divergence between the generative and targetdistributions.
That is, given the training data being the diverse samples on the constraint manifold, the deep generative model is trained to produce seeds that lie close to the manifold conditioned onthe current problem, represented with an image of the scene.
These seeds are then used as initialization (warmstart) of a subsequent optimization procedure that projects them to the manifold to ﬁnd afeasible solution.
Despite the expressive power of function approximators and recent improvements in deep generativemodels, they still have limitations in generating samples from a highly nonlinear and multimodal distribution.
Inspired by previous works exploiting an underlying sparse structure of robotic sequentialmanipulation problems [13, 14, 15, 16] we propose an extension of the above system that leverages a factorization of the constraint problem to model this multimodal distribution more efﬁciently.
Sampling is decomposed into a sequence, where for each factor we can train a separate conditionalgenerative model.
Sequencing these conditional generative models increases sample efﬁciency ofthe training and the multi-modality of the distributions it can represent.
We demonstrate the system in the context of robotic sequential manipulation, where the problemis to ﬁnd a sequence of feasible mode-switch conﬁgurations, e.g., robot’s poses when it is pickingor placing an object.
These problems are deﬁned by a set of continuous variables representingsequences of robot conﬁgurations, object poses, and relative transformations, with nonlinear, piecewise differentiable constraints, e.g., for grasping, placement, collision avoidance and kinematics,which give a natural factorization of the overall problem. Sampling a diverse set of mode-switchconﬁgurations efﬁciently is essential as an inner module of task and motion planning, e.g. to providewaypoints for subsequent trajectory optimization.
In summary, our core contributions are the following:
• We introduce Deep Generative Constraint Sampling (DGCS), which combines deep generative models with nonlinear optimization to efﬁciently generate diverse solutions of constraint problems.
The generative model is trained conditional to a scene image to be applicable to varying problem instances.
As an application, we produce mode-switch conﬁgurations in sequential manipulation.•
We extend the standard data-based training of the generative model with analytical featuresof the feasible manifold to achieve both, diversity and sample accuracy.•
We extend our system to exploit a factorization of the constraint problem, if given, to further improve modelling of multi-modal distributions with disconnected support and reducesample complexity.
Related Work
Generative Models in Robotics Recently, deep generative models have been successfully appliedin robotics, specially in settings where the problem is represented directly with images or pointclouds.
For example, for generating grasp of complex objects directly from images [17, 18].
In thecontext of motion planning [19, 20, 21], and planning [22, 23, 24], generative models are used forsampling informative collision free conﬁgurations which greatly improve running time of samplingbased algorithms.
In this line of research the general aim is to train a network to directly predictsolutions to a problem.
However, none of these work addresses sampling on a high-dimensionalconstraint manifold, or predicting full multi-robot manipulation sequences.
To achieve accuratesampling, we combine learned generative models with local optimization for constraint projection.
Warmstart in Nonlinear optimization In robotics, nonlinear optimization is used to sample fromconstraint manifolds and optimize trajectories [25, 26, 27].
The goal of recent data-based approachesis to learn a warmstart to reduce online computation time [2, 28].
In settings where the nonlinearprograms can be represented with mixed integer constraints, [29, 30, 3, 31] learn an assignment tothe integer constraints and run subsequent optimization.
In comparison to our method, integer formulations use a discrimative model and are easier to train but are difﬁcult to generalize to problemswithout a clear integer structure.
Recently [32] applies GANs for inverse kinematics, adding forward kinematics into the network training.
In contrast, we use general analytical features in the costterm.
Moreover, our framework is conditioned on an image representation, and scales to sequentialmanipulation.
Sampling on a Constraint Manifold
The problem we address is to generate samples from a manifold M τ parametrized by a ﬁxed dimensional (but potentially large, e.g. images) problem parameter τ ,
[Equation]
[Equation] where φ eq ( x ; τ ) : R n × R m → R n e and φ ineq ( x ; τ ) : R n × R m → R n i are nonlinear vector-valuedfunctions, that are piecewise differentiable.
τ ∈ R m represents the current problem instance andparametrizes all the constraints.
In the context of robotic manipulation, τ represents all the objectsin the scene (position, size, shape...), x are robot and object degrees of freedom that are subject toconstraints, and φ eq , φ ineq are (in)equality constraints that describe the problem’s objectives such ascollision avoidance, grasping, kinematic, and pose constraints.
A generative model x ∼ P ( τ ) that produces feasible solutions (samples x ∈ M τ ) is built from twocomponents (see Alg. 1): a randomized seed x 0 generation and a constrained optimization algorithmthat projects the seed x 0 to the solution manifold (2).
[Equation]
In our current implementation, we solve (2) approximately, by running a nonlinear optimizer fromstarting point x 0 on the feasibility problem: ﬁnd x s.t φ eq ( x ; τ ) = 0; φ ineq ( x ; τ ) ≤ 0 .
The initialization and internal regularization of the optimizer provides an implicit regularization with respectto x 0 .
The projection step is a non-convex optimization problem that has no guarantees to producea feasible sample; especially for a complex sequential manipulation problem, the optimizationlandscape often has substantial local optima induced by constraints, making non-linear projection prone to fail unless seeds are close to thesolution manifold.
To address such difﬁculty, we train a seeding distribution P θ ( τ ) to approximate a reference distribution of feasible solutions P r ( τ ) , so that it cangenerate diverse samples close to the parametric manifold M τ which the optimizer can easilyproject.
An example is shown in Fig.
Algorithm
Training Deep Generative Models to Sample on Constraint Manifolds
Our deep generative distribution is ˜ x ∼ P θ ( τ ) with ˜ x
= G θ ( z, τ ) , z ∼ P z where P z is a multidimensional Gaussian and G θ is a neural network with parameters θ .
In contrast to the standard application of adversarial generative models (images generation), in oursetting we also have an analytical description of the support of the target distribution, namely thefeatures φ ( x ; τ ) =
[ φ eq ( x ; τ ) , max (0 , φ ineq ( x, τ ))] that describe M τ .
We could consider a data-freegradient based optimization of the analytical constraint violation as
[Equation]
[Equation]
However, this is extremely ill-posed and converges to the “deterministic mapping” local minimum
G θ ( z, τ ) =
G θ,τ ∀ z , losing the potential to generate a diverse distribution.
To enforce sample diversity, we regularize with respect to a reference distribution P r that is diverseand has its support on the manifold, i.e., E x ∼ P r || φ ( x ; τ ) ||
2 = 0 .
Namely, we formulate the problemas:
[Equation]
where W is the Wasserstein distance, and β ∈ R > 0 .
Although the analytical term does not provide additional information than the support of P r , as will be shown in the experiment section, itscontribution is important in the context of function approximation and stochastic gradient descent innon-convex optimization.
4.1 Wasserstein Distance and Adversarial Formulation
The Wasserstein-1 (Earth Mover distance) between two probability distributions P r and P θ (5) isdeﬁned intuitively as the cost of geometrically moving the mass from one distribution to the otherunder an optimal transport plan,
[Equation]
where Π( P r , P θ ) denotes the set of all joint distributions γ ( x, y ) with marginals P r and P θ .
Compared to other notions of distance such as Jensen-Shannon Divergence or Total Variation, adversarial generative models with Wasserstein distances are known to have good practical stability andconvergence and are less prone to mode collapse [11, 33].
Furthermore, the Wasserstein distanceprovides a nice interpretation in our application, because it takes into account the geometric distancebetween two distributions; the minimization of the geometric distance (norm in conﬁguration space)to a distribution supported on the manifold improves the success rate of the subsequent projectionprocedure.
We minimize the objective function (4) with the Wasserstein GAN [11, 33] formulation.
Using
Kantorovich Duality, the original formulation (5) is transformed into a minimax game between thecritic network D and the generator G , which are trained simultaneously with stochastic gradientdescent.
Speciﬁcally, the minimax problem (with our analytical feature) is:
[Equation]
where ˆ x are samples between x and ˜ x .
Our reference distribution P r ( τ ) consists of a discrete setof solution/problem-pairs D
= { ( x i , τ i ) } .
This data is computed ofﬂine by solving a large set ofproblems, with a focus on getting diverse samples.
Structured Generative Model by Exploiting Factorization
The previous sections treated x as a single variable.
While the approach for training a single generative model ˜ x
= G θ ( x ; τ ) , z ∼ P z is powerful, we can further improve the scalability to largeproblems by exploiting a given factorization of x and sequentially decomposing the sampling procedure into a sequence of conditional samplings, as in Bayesian networks [34].
p t a t b q ar a q aq bq b 1 r b
Kin
Kin Kin
Kin
Position Grasp
Grasp (a) Handover t p q 2 q
Kin Kin
Grasp
Position (b) Pick and Place t p q 2 q(c) Pick and Place
We assume the conﬁguration x = { x 1 , ..., x N } can be factored into N vector variables.
Similarly, the constraints (equalities and inequalities) φ ( x ; τ ) are factored into L sets of constraints,{ φ 1 , φ 2 , . .
. , φ L } with φ j : R a j → R b j where a j , b j ∈ N are the dimensions of the domain andco-domain of the constraint φ j , and each constraint φ j depends only on a subset of variables.
[Equation]
The set of constraints now deﬁnes a factored mathematical program without costs, also called constraint graph.
Such factorization naturally arise in many applications, where each variable has somesemantic and geometric meaning.
Fig.
2 shows some examples of such constraint graphs in thecontext of robotics sequential manipulation.
Variables correspond to robot joint conﬁguration q ,mobile robots base pose r , relative transformations between objects and grippers t and object position p .
These variables are then coupled by the kinematic chain, collision avoidance, grasp, andpose constraints.
The pick-and-place graph in Fig.
2b is the fundamental building block of sequential manipulation.
Longer and more complex tasks such as a handover (Fig. 2a) or an assembly canbe represented by chaining the basic pick and place with additional constraints.
5.1 Directed Graphical Model and Sequential Sampling
The factored structure directly implies a factorization of the joint probability distribution P ( τ ) fromwhich we want to sample.
As in Bayesian networks we can sequentialize sampling if we decide onan ordering of variables that corresponds to a directed acyclic graph.
Instead of learning a single
G θ to produce a full assignment with z ∼ P z , x = G θ ( z ; τ ) , we learn a conditional model foreach factor by using the corresponding marginal distributions of the original data, and the subset ofcorresponding analytical features in the constraint graph.
We illustrate the beneﬁts of this factorization with the Pick-and-Place problem.
The joint distribution for this problem is factorized into (we omit conditioning on τ for clarity)
[Equation]
where p is the placement position of the object, t is the relative transformation between objectand gripper and q 1 , q 2 are robot joint conﬁgurations at pick and place.
The factorization exploitsconditional independence between ( q 1 , p ) given t and ( q 1 , q 2 ) given t .
We leverage this structure bytraining a sequence of conditional samplers p ∼ P p , t ∼ P t ( p ) , q 1 ∼ P q 1 ( t ) , q 2 ∼ P q 2 ( t, p ) , see Fig.
2c and Fig. This factorization can be easily extended to longer manipulation sequences, withthe ordering p → r → t → q . See Appendix A for more examples of sampling orders and details.
As a side note, using marginal distributions of a jointly consistent dataset is necessary, as only themarginal contains useful information of whether a partial assignment will admit a full solution.
For example, when sampling p ∼ P p , we want to fulﬁl the two following constraints (conditioned onthe scene): a) position ( p ) = 0 ; and b) ∃ ( t, q 1 , q 2 )
s.t ( t, q 1 , q 2 , p ) is feasible.
The second constraintcannot be evaluated efﬁciently, but is modelled by the marginal distribution of the data.5.2 Note on the advantage of factorization for modelling mulimodality
Generating a distribution with disconnected supports with a deep generative model that receives acontinuous input noise z ∼ P z would require inﬁnite gradients in G θ and can only be done approximately.
In these cases, training is unstable and very sensitive to hyperparameters and architecture.
We can model disconnected distribution more effectively by factoring the full joint probability asa sequence of smaller, conditional modules, as is conﬁrmed by our experimental results in Section6.3.
The sequencing still requires that each module is able to produce some degree of multimodality.
However, once a module in the sequence receives a disconnected input in the form of conditioning,it can successfully produce a disconnected output.
As we chain modules with the ability to generatea small amount of disconnected components given a continuous input, the number of possible disconnected components of the output grows exponentially with the number of modules in sequence.
Furthermore, from a practical perspective, training smaller modules turned out to be more effective.
For instance, we observed that the analytical feature
|| φ ( x ; τ ) ||
2 of the joint problem can providebadly-conditioned gradients when evaluated far from the manifold.
This issue is alleviated whenconsidering only subsets of constraints and variables.
In our preliminary experiments, we also evaluated GAN frameworks that have a mechanism to model disconnected distribution [35, 36] (andrequire an estimate of the number of disconnected components) but did not ﬁnd signiﬁcant improvement.
Experiments
6.1 Image based Problem representation
We use an image-based representation of the problem instance τ that consists of the depth image andmasks.
Speciﬁcally, τ = { d, m 1 , m 2 , m 3 } , where d is the depth image, and m 1 , m 2 , m 3 are threemasks that contain, respectively, information from the initial object pose, goal pose or placementregion, and obstacles, see Fig. 4.
In the factored approach, each generative module receives as inputonly the “relevant” masks, e.g. the sampler for the robot pick conﬁguration receives a mask of theobstacles and the initial conﬁguration, but not the goal pose.
The main strength of the image representation is that it can generalize to different object shapes andchanging number of obstacles and shapes.
Moreover, a depth camera is easily available, approximatemasks can be computed with image segmentation techniques and provides a good representation ofsequential manipulation problems on tabletop scenarios.
6.2 Scenarios
We consider three different manipulation tasks that involve object manipulation with stable grasps.
• Pick-and-Place : A robot has to pick an object and place it on a rectangular region of the
table.
Fig.
• Assembly : two mobile robots have to pick an object each and assemble them.
The assembly is not unique, but modelled as a manifold with constraints on rotation and position: objects
[Table]
have to be perpendicular and make stable contact with predeﬁned faces of the cubes (similarto a T shape).
Fig.
• Handover : Two mobile robots have to collaborate to move the object from the initial to thegoal position doing a handover.
Fig.
All these tasks are performed on a cluttered table with a varying number (between 3 and 5) ofobstacles.
The grasp between the gripper and the object is modelled with a two ﬁnger gripper (e.g.
Franka Panda Gripper) that constrains the position and orientation.
The training dataset consists of4000 pairs of problem-solution computed ofﬂine with a user-deﬁned sampling sequence to ensurediversity.
The problems consist of 64x128x4 images (input of network) and their correspondingenvironment (for analytical error term in training and nonlinear optimization).
The variability between instances is deﬁned by the number, position and size of obstacles, the sizeand position of the objects and the goal conﬁguration.
The instances of the evaluation and traindataset come from the same distribution.
See the Appendix A for details about the structure of thenetworks and implementation and a collection of samples generated by our deep generative models.
6.3 Ablation Study
The Pick-and-Place scenario is used for an ablation study of the proposed generative model.
Weanalyze the contribution of the factored structure ( Big NN vs Structure NN ) and analytical error term( +analytical ).
We evaluate precision and coverage of seed samples (output from deep generativemodels) and solutions (after projection with nonlinear optimization) by generating 4000 samples foreach new instance (30 in total).
We report the metrics:
• Error : the constraint violation || φ
(˜ x ; τ ) || 2 (unitless, lower is better).
• Precision : average nearest neighbour distance to a reference dataset.
It models how closesamples are to the real data (lower is better).• Coverage : average nearest neighbour distance from the reference dataset to the computedsamples.
It describes how good the reference dataset is covered by the learned distribution(lower is better).
Results are shown in Table 1.
When analyzing the seeds (output of the generative model) our twocontributions are essential to achieve small constraint violation and good coverage.
In terms ofsolutions, only the networks with structure provide good coverage and seeds from the model withstructure and analytical term have higher probability of leading to a solution (better success rate).
6.4 Benchmark: Generative Model in Nonlinear optimization
The Assembly and Handover scenarios are used to compare our generative model against two baseline methods for warmstarting (seeding) nonlinear optimizers.
We analyze the number of solvedproblems and the number of necessary optimization runs.
Measuring the number of solved nonlin- [Figure] [Figure] [Figure]
e [Figure] I
[Figure] [Figure]
Number
[Figure]
[Figure]
e r
[Figure]
I n s t a
[Figure]
ear programs is an indirect way to evaluate coverage and sample quality, as both are fundamentalto solve a diverse set of problems with a nonlinear optimizer and prevent convergence to infeasiblepoints.
We compare our complete model (deep generative model with structure and analytical errorterm), in short deep , with baselines:
• Rand: Randomized initial guess around a reference position.
• Rand Data: Choosing samples x from the training dataset at random.
The initial point isthus a feasible sample for another problem of the same family.
This is actually a strongbaseline, because it provides diverse informative initial seeds.
We evaluate the generative model (deep generative model + optimization) on 200 problems from theevaluation dataset.
The experiments are repeated 10 times and we report mean and variance.
Weﬁrst report how many optimization trials (each trial has an independent starting point) are necessaryto solve each of the test instances, and plot the histogram of the mean value.
Unsolved problems areassumed to be solved with 10 trials (maximum number trials).
Fig.
In both scenarios, the proposed deep generative model outperforms the baseline warmstarts, signiﬁcantly reducing number of trials required to solve the instances: in average (across problems),from (Rand Data, 3.86 ± 1.29) to (Deep, 2.77 ± 1.79) in Handover and from (Rand Data, 3.99 ± 1.44)(Deep, 2.07 ± 1.38) in Assembly .
To complete the analysis, we also show the cumulative number ofproblems solved as we increase the number of optimization trials in Fig.
7 of the Appendix A. Thecomputational overhead of evaluating the neural network is small (we produce 10 samples in 8 mswith a GPU), while most of the time is spent in optimization runs that converge to infeasible points. Time spent (in seconds) to complete the benchmarks are: (Deep, 949 ± 32), (Rand Data, 1746 ± 76)in Assembly ; and (Deep, 1246 ± 26), (Rand Data, 1715 ± 58) in Handover .
Conclusion
In this work, we propose Deep Generative Constraint Sampling (DGCS), a new approach to samplefrom a constraint manifold to tackle problems in robotic sequential manipulation.
Our frameworkcombines a deep generative sampling model, conditioned on an image based representation of theproblem, and a nonlinear optimizer to project samples onto the manifold.
We further extendedthe approach to exploit a given factorization of the problem, by training a sequence of conditionalgenerative models rather than a full joint generator.
Our empirical results conﬁrm that the inclusionof analytic constraints in the training of the generative model, as well as exploiting the factorizationof a given problem signiﬁcantly improve the efﬁciency, diversity and precision of the samplingapproach.
Using the trained generative models we can outperform heuristic warmstart strategies.
A limitation of our approach is that training requires a dataset of solutions to different instancesof the same problem class.
As future work, the proposed graphical structure could be exploitedto provide generalization across different problem classes (i.e. different type and number of constraints and variables) by sharing and combining the sampling modules of the sequence.
Our currentframework combines generative sampling using a neural network with subsequent projection usingconstrained optimization.
A promising future direction is to explore whether it is possible to embedthe optimization algorithm as a last layer of the generative model, while keeping good coverage andmultimodality.