Efficient motion planning for problems lacking optimal substructure

We consider the motion-planning problem of planning a collision-free path of a robot in the presence of risk zones. The robot is allowed to travel in these zones but is penalized in a super-linear fashion for consecutive accumulative time spent there. We suggest a natural cost function that balances path length and risk-exposure time. Specifically, we consider the discrete setting where we are given a graph, or a roadmap, and we wish to compute the minimal-cost path under this cost function. Interestingly, paths defined using our cost function do not have an optimal substructure. Namely, subpaths of an optimal path are not necessarily optimal. Thus, the Bellman condition is not satisfied and standard graph-search algorithms such as Dijkstra cannot be used. We present a path-finding algorithm, which can be seen as a natural generalization of Dijkstra's algorithm. Our algorithm runs in $O\left((n_B\cdot n) \log( n_B\cdot n) + n_B\cdot m\right)$ time, where~$n$ and $m$ are the number of vertices and edges of the graph, respectively, and $n_B$ is the number of intersections between edges and the boundary of the risk zone. We present simulations on robotic platforms demonstrating both the natural paths produced by our cost function and the computational efficiency of our algorithm

Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs

In this paper, we present Batch Informed Trees (BIT*), a planning algorithm based on unifying graph- and sampling-based planning techniques. By recognizing that a set of samples describes an implicit random geometric graph (RGG), we are able to combine the efficient ordered nature of graph-based techniques, such as A*, with the anytime scalability of sampling-based algorithms, such as Rapidly-exploring Random Trees (RRT). BIT* uses a heuristic to efficiently search a series of increasingly dense implicit RGGs while reusing previous information. It can be viewed as an extension of incremental graph-search techniques, such as Lifelong Planning A* (LPA*), to continuous problem domains as well as a generalization of existing sampling-based optimal planners. It is shown that it is probabilistically complete and asymptotically optimal. We demonstrate the utility of BIT* on simulated random worlds in $\mathbb{R}^2$ and $\mathbb{R}^8$ and manipulation problems on CMU's HERB, a 14-DOF two-armed robot. On these problems, BIT* finds better solutions faster than RRT, RRT*, Informed RRT*, and Fast Marching Trees (FMT*) with faster anytime convergence towards the optimum, especially in high dimensions.Comment: 8 Pages. 6 Figures. Video available at http://www.youtube.com/watch?v=TQIoCC48gp

Sampling-Based Methods for Factored Task and Motion Planning

This paper presents a general-purpose formulation of a large class of discrete-time planning problems, with hybrid state and control-spaces, as factored transition systems. Factoring allows state transitions to be described as the intersection of several constraints each affecting a subset of the state and control variables. Robotic manipulation problems with many movable objects involve constraints that only affect several variables at a time and therefore exhibit large amounts of factoring. We develop a theoretical framework for solving factored transition systems with sampling-based algorithms. The framework characterizes conditions on the submanifold in which solutions lie, leading to a characterization of robust feasibility that incorporates dimensionality-reducing constraints. It then connects those conditions to corresponding conditional samplers that can be composed to produce values on this submanifold. We present two domain-independent, probabilistically complete planning algorithms that take, as input, a set of conditional samplers. We demonstrate the empirical efficiency of these algorithms on a set of challenging task and motion planning problems involving picking, placing, and pushing

Intrinsically Motivated Goal Exploration Processes with Automatic Curriculum Learning

Intrinsically motivated spontaneous exploration is a key enabler of autonomous lifelong learning in human children. It enables the discovery and acquisition of large repertoires of skills through self-generation, self-selection, self-ordering and self-experimentation of learning goals. We present an algorithmic approach called Intrinsically Motivated Goal Exploration Processes (IMGEP) to enable similar properties of autonomous or self-supervised learning in machines. The IMGEP algorithmic architecture relies on several principles: 1) self-generation of goals, generalized as fitness functions; 2) selection of goals based on intrinsic rewards; 3) exploration with incremental goal-parameterized policy search and exploitation of the gathered data with a batch learning algorithm; 4) systematic reuse of information acquired when targeting a goal for improving towards other goals. We present a particularly efficient form of IMGEP, called Modular Population-Based IMGEP, that uses a population-based policy and an object-centered modularity in goals and mutations. We provide several implementations of this architecture and demonstrate their ability to automatically generate a learning curriculum within several experimental setups including a real humanoid robot that can explore multiple spaces of goals with several hundred continuous dimensions. While no particular target goal is provided to the system, this curriculum allows the discovery of skills that act as stepping stone for learning more complex skills, e.g. nested tool use. We show that learning diverse spaces of goals with intrinsic motivations is more efficient for learning complex skills than only trying to directly learn these complex skills

A nonlinear vehicle-structure interaction methodology with wheel-rail detachment and reattachment

. A vehicle-structure interaction methodology with a nonlinear contact formulation based on contact and target elements has been developed. To solve the dynamic equations of motion, an incremental formulation has been used due to the nonlinear nature of the contact mechanics, while a procedure based on the Lagrange multiplier method imposes the contact constraint equations when contact occurs. The system of nonlinear equations is solved by an efficient block factorization solver that reorders the system matrix and isolates the nonlinear terms that belong to the contact elements or to other nonlinear elements that may be incorporated in the model. Such procedure avoids multiple unnecessary factorizations of the linear terms during each Newton iteration, making the formulation efficient and computationally attractive. A numerical example has been carried out to validate the accuracy and efficiency of the present methodology. The obtained results have shown a good agreement with the results obtained with the commercial finite element software ANSY

Incremental Cycle Detection, Topological Ordering, and Strong Component Maintenance

We present two on-line algorithms for maintaining a topological order of a directed $n$-vertex acyclic graph as arcs are added, and detecting a cycle when one is created. Our first algorithm handles $m$ arc additions in $O(m^{3/2})$ time. For sparse graphs ($m/n = O(1)$), this bound improves the best previous bound by a logarithmic factor, and is tight to within a constant factor among algorithms satisfying a natural {\em locality} property. Our second algorithm handles an arbitrary sequence of arc additions in $O(n^{5/2})$ time. For sufficiently dense graphs, this bound improves the best previous bound by a polynomial factor. Our bound may be far from tight: we show that the algorithm can take $\Omega(n^2 2^{\sqrt{2\lg n}})$ time by relating its performance to a generalization of the $k$-levels problem of combinatorial geometry. A completely different algorithm running in $\Theta(n^2 \log n)$ time was given recently by Bender, Fineman, and Gilbert. We extend both of our algorithms to the maintenance of strong components, without affecting the asymptotic time bounds.Comment: 31 page

An incremental approach to the solution of global trajectory optimization problems

This paper presents an incremental approach to the solution of multiple gravity assist trajectories (MGA) with deep space maneuvers. The whole problem is decomposed in sub-problems that are solved incrementally. The solution of each sub-problem leads to a progressive reduction of the search space. Unlike other similar methods, the search for solutions of each sub-problem is performed through a stochastic approach. The resulting set of disconnected boxes is transformed into a connected collection of boxes through an affine transformation. For MGA problems, the incremental approach increases both the efficiency and reliability of the optimization process. Two relevant examples will illustrate the effectiveness of the proposed method