A factored Nonlinear Program (Factored-NLP) explicitly models the dependencies between a set of continuous variables and nonlinear constraints, providing an expressive formulation for relevant robotics problems such as manipulation planning or simultaneous localization and mapping. When the problem is over-constrained or infeasible, a fundamental issue is to detect a minimal subset of variables and constraints that are infeasible. Previous approaches require solving several nonlinear programs, incrementally adding and removing constraints, and are thus computationally expensive. In this paper, we propose a graph neural architecture that predicts which variables and constraints are jointly infeasible. The model is trained with a dataset of labeled subgraphs of Factored-NLPs, and importantly, can make useful predictions on larger factored nonlinear programs than the ones seen during training. We evaluate our approach in robotic manipulation planning, where our model is able to generalize to longer manipulation sequences involving more objects and robots, and different geometric environments. The experiments show that the learned model accelerates general algorithms for conflict extraction (by a factor of 50) and heuristic algorithms that exploit expert knowledge (by a factor of 4).

Task and Motion Planning has made great progress in solving hard sequential manipulation problems. However, a gap between such planning formulations and control methods for reactive execution remains. In this paper we pro-pose a model predictive control approach dedicated to robustly execute a single sequence of constraints, which corresponds to a discrete decision sequence of a TAMP plan. We decompose the overall control problem into three sub-problems (solving for sequential waypoints, their timing, and a short receding horizon path) that each is a non-linear program solved online in each MPC cycle. The resulting control strategy can account for long-term interdependencies of constraints and reactively plan for a timing-optimal transition through all constraints. We additionally propose phase backtracking when running constraints of the current phase cannot be fulfilled, leading to a fluent re-initiation behavior that is robust to perturbations and interferences by an experimenter.

Long-horizon manipulation tasks require joint reasoning over a sequence of discrete actions and their associated continuous control parameters. While Task and Motion Planning (TAMP) approaches are capable of generating motion plans that account for this joint reasoning, they usually assume full knowledge about the environment (e.g. in terms of shapes, poses of objects) and often require computation times not suitable for real-time control.To overcome this, we propose a learning framework where a high-level reasoning network predicts, based on an image of the scene, a sequence of discrete actions and the parameter values of their associated low-level controllers. These controllers are parameterized in terms of a learned energy function, leading to time-invariant controllers for each phase. We train the whole framework end-to-end using a dataset of TAMP solutions computed using Logic Geometric Programming. A key feature is that the reasoning network determines the parameters of the controllers jointly, such that the overall task can be solved. Despite having no explicit representation of the geometry nor pose of the objects in the scene, our network is still able to accomplish geometrically precise manipulation tasks, including handovers and an accurate pointing task where the parameters of early actions are tightly coupled with those of later actions. Video: https://youtu.be/AcPWRTkr3_g

We present a hierarchical planning and control framework that enables an agent to perform various tasks and adapt to a new task flexibly. Rather than learning an individual policy for each particular task, the proposed framework, DISH, distills a hierarchical policy from a set of tasks by representation and reinforcement learning. The framework is based on the idea of latent variable models that represent high-dimensional observations using low-dimensional latent variables. The resulting policy consists of two levels of hierarchy: (i) a planning module that reasons a sequence of latent intentions that would lead to an optimistic future and (ii) a feedback control policy, shared across the tasks, that executes the inferred intention. Because the planning is performed in low-dimensional latent space, the learned policy can immediately be used to solve or adapt to new tasks without additional training. We demonstrate the proposed framework can learn compact representations (3- and 1-dimensional latent states and commands for a humanoid with 197- and 36-dimensional state features and actions) while solving a small number of imitation tasks, and the resulting policy is directly applicable to other types of tasks, i.e., navigation in cluttered environments.

Existing work on sequential manipulation planning and trajectory optimization typically assumes the robot, environment and tools to be given. However, in particular in industrial applications, it is highly interesting to ask, what would be an optimal robot design, tool shape, or robot station geometry for a particular ensemble of manipulation tasks. To tackle this problem we propose a formulation to jointly optimize over static design parameters and the sequential manipulation trajectory. We can include optimization objectives such as penalizing velocities (path length) and joint torques. Our evaluations show that design optimization can significantly improve on such metrics. For instance, in a wrench tool demonstration scenario we show that the shape of the wrench tool as well as design of the robot can be optimized to allow for exerting a necessary external torque with minimal effort.

In this paper, we propose a deep neural network that predicts the feasibility of a mixed-integer program from visual input for robot manipulation planning. Integrating learning into task and motion planning is challenging, since it is unclear how the scene and goals can be encoded as input to the learning algorithm in a way that enables to generalize over a variety of tasks in environments with changing numbers of objects and goals. To achieve this, we propose to encode the scene and the target object directly in the image space.Our experiments show that our proposed network generalizes to scenes with multiple objects, although during training only two objects are present at the same time. By using the learned network as a heuristic to guide the search over the discrete variables of the mixed-integer program, the number of optimization problems that have to be solved to find a feasible solution or to detect infeasibility can greatly be reduced.

Logic-Geometric Programming (LGP) is a powerful motion and manipulation planning framework, which represents hierarchical structure using logic rules that describe discrete aspects of problems, e.g., touch, grasp, hit, or push, and solves the resulting smooth trajectory optimization. The expressive power of logic allows LGP for handling complex, large-scale sequential manipulation and tool-use planning problems. In this paper, we extend the LGP formulation to stochastic domains. Based on the control-inference duality, we interpret LGP in a stochastic domain as fitting a mixture of Gaussians to the posterior path distribution, where each logic pro le defines a single Gaussian path distribution. The proposed framework enables a robot to prioritize various interaction modes and to acquire interesting behaviors such as contact exploitation for uncertainty reduction, eventually providing a composite control scheme that is reactive to disturbance.

We present a representation learning algorithm that learns a low-dimensional latent dynamical system from high-dimensional sequential raw data, e.g., video. The framework builds upon recent advances in amortized inference methods that use both an inference network and a refinement procedure to output samples from a variational distribution given an observation sequence, and takes advantage of the duality between control and inference to approximately solve the intractable inference problem using the path integral control approach. The learned dynamical model can be used to predict and plan the future states; we also present the efficient planning method that exploits the learned low-dimensional latent dynamics. Numerical experiments show that the proposed path-integral control based variational inference method leads to tighter lower bounds in statistical model learning of sequential data. Supplementary video: https://youtu.be/xCp35crUoLQ

This work presents a multiscale framework to solve a class of stochastic optimal control problems in the context of robot motion planning and control in a complex environment. In order to handle complications resulting from a large decision space and complex environmental geometry, two key concepts are adopted: (a) a diffusion wavelet representation of the Markov chain for hierarchical abstraction of the state space; and (b) a desirability function-based representation of the Markov decision process (MDP) to efficiently calculate the optimal policy. In the proposed framework, a global plan that compressively takes into account the long time/length-scale state transition is first obtained by approximately solving an MDP whose desirability function is represented by coarse scale bases in the hierarchical abstraction. Then, a detailed local plan is computed by solving an MDP that considers wavelet bases associated with a focused region of the state space, guided by the global plan. The resulting multiscale plan is utilized to finally compute a continuous-time optimal control policy within a receding horizon implementation. Two numerical examples are presented to demonstrate the applicability and validity of the proposed approach.

This work presents an efficient method to solve a class of continuous-time, continuous-space stochastic optimal control problems of robot motion in a cluttered environment. The method builds upon a path integral representation of the stochastic optimal control problem that allows computation of the optimal solution through sampling and estimation process. As this sampling process often leads to a local minimum especially when the state space is highly non-convex due to the obstacle field, we present an efficient method to alleviate this issue by devising a proposed topological motion planning algorithm. Combined with a receding-horizon scheme in execution of the optimal control solution, the proposed method can generate a dynamically feasible and collision-free trajectory while reducing concern about local optima. Illustrative numerical examples are presented to demonstrate the applicability and validity of the proposed approach.