We present an experience-based planning framework called Thunder that learns to reduce computation time required to solve high-dimensional planning problems in varying environments. The approach is especially suited for large configuration spaces that include many invariant constraints, such as those found with whole body humanoid motion planning. Experiences are generated using probabilistic sampling and stored in a sparse roadmap spanner (SPARS), which provides asymptotically near-optimal coverage of the configuration space, making storing, retrieving, and repairing past experiences very efficient with respect to memory and time. The Thunder framework improves upon past experience-based planners by storing experiences in a graph rather than in individual paths, eliminating redundant information, providing more opportunities for path reuse, and providing a theoretical limit to the size of the experience graph. These properties also lead to improved handling of dynamically changing environments, reasoning about optimal paths, and reducing query resolution time. The approach is demonstrated on a 30 degrees of freedom humanoid robot and compared with the Lightning framework, an experience-based planner that uses individual paths to store past experiences. In environments with variable obstacles and stability constraints, experiments show that Thunder is on average an order of magnitude faster than Lightning and planning from scratch. Thunder also uses 98.8% less memory to store its experiences after 10,000 trials when compared to Lightning. Our framework is implemented and freely available in the Open Motion Planning Library.

One intuitive approach for planning robotic manipulation tasks is to first compute a plan for the manipulated object, as if capable to move on its own, then use the obtained plan to find a sequence of arm maneuvers to take the object along the computed plan. Motion planning queries would be performed in relatively low-dimensional configuration spaces, rather than the full configuration space of a robot's arms and grippers, resulting in a more efficient planning method. However, having a plan for the manipulated object does not guarantee there also exists a feasible sequence of maneuvers for the robot to do the manipulation. Knowing different possible grasps on the object increases the robots chance to find a sequence of grasp switches and maneuvers, but makes the search more time consuming as the grasps need to be tested for usefulness. In this paper, we develop a multi-level architecture for complex manipulation planning of rigid bodies which uses communication between the two levels, one for planning the manipulated object motion, the other to plan for the arms, to improve the efficiency of the method. We store grasp zones in the configuration space of the manipulated object as regions where a given grasp seems promising. We use grasp zones to guide searching for grasp switching maneuvers, and to avoid regions of configuration space where few good grasps exist.

Recent work in sampling-based motion planning has yielded several different approaches for computing good quality paths in high degree of freedom systems: path shortcutting methods that attempt to shorten a single solution path by connecting non-consecutive configurations, a path hybridization technique that combines portions of two or more solutions to form a shorter path, and asymptotically optimal algorithms that converge to the shortest path over time. This paper presents an extensible meta-algorithm that incorporates a traditional sampling-based planning algorithm with offline path shortening techniques to form an anytime algorithm which exhibits competitive solution lengths to the best known methods and optimizers. A series of experiments involving rigid motion and complex manipulation are performed as well as a comparison with asymptotically optimal methods which show the efficacy of the proposed scheme, particularly in high-dimensional spaces.

Most prior techniques for proximity computations are designed for synthetic models and assume exact geometric representations. However, real robots construct representations of the environment using their sensors, and the generated representations are more cluttered and less precise than synthetic models. Furthermore, this sensor data is updated at high frequency. In this paper, we present new collision- and distance-query algorithms, which can efficiently handle large amounts of point cloud sensor data received at real-time rates. We present two novel techniques to accelerate the computation of broad-phase data structures: 1) we present a progressive technique that incrementally computes a high-quality dynamic AABB tree for fast culling, and 2) we directly use an octree representation of the point cloud data as a proximity data structure. We assign a probability value to each leaf node of the tree, and the algorithm computes the nodes corresponding to high collision probability. In practice, our new approaches can be an order of magnitude faster than previous methods. We demonstrate the performance of the new methods on both synthetic data and on sensor data collected using a Kinect™ for motion planning for a mobile manipulator robot.

This paper describes an algorithm that considers uncertainty while solving the simultaneous task and motion planning (STAMP) problem. Information about uncertainty is transferred to the task planning level from the motion planning level using the concept of a task motion multigraph (TMM). TMMs were introduced in previous work to improve the efficiency of solving the STAMP problem for mobile manipulators. In this work, Markov Decision Processes are used in conjunction with TMMs to select sequences of actions that solve the STAMP problem such that the resulting solutions have higher probability of feasibility. Experimental evaluation indicates significantly improved probability of feasibility for solutions to the STAMP problem, compared to algorithms that ignore uncertainty information when selecting possible sequences of actions. At the same time, the efficiency due to TMMs is largely maintained.

Randomized planners, search-based planners, potential-field approaches and trajectory optimization based motion planners are just some of the types of approaches that have been developed for motion planning. Given a motion planning problem, choosing the appropriate algorithm to use is a daunting task even for experts since there has been relatively little effort in comparing the plans generated by the different approaches, for different problems. In this paper, we present a set of benchmarks and the associated infrastructure for comparing different types of motion planning approaches and algorithms. The benchmarks are specifically designed for robotics and include typical indoor human environments. We present example motion planning problems for single arm tasks. Our infrastructure is designed to be easily extensible to allow for the addition of new planning approaches, new robots, new environments and new metrics. We present results comparing the performance of several motion planning algorithms to validate the use of these benchmarks.

Robots executing practical tasks in real environments are often subject to multiple constraints. These constraints include orientation constraints: e.g., keeping a glass of water upright, torque constraints: e.g., not exceeding the torque limits for an arm lifting heavy objects, visibility constraints: e.g., keeping an object in view while moving a robot arm, etc. Rejection sampling, Jacobian projection techniques and optimization-based approaches are just some of the methods that have been used to address such constraints while computing motion plans for robots performing manipulation tasks. In this work, we present an approach to handling certain types of constraints in a manner that significantly increases the efficiency of existing methods. Our approach focuses on the sampling step of a motion planner. We implement this step as the drawing of samples from a set that has been computed in advance instead of the direct sampling of constraints. We show how our approach can be applied to different constraints: orientation constraints on the end-effector of an arm, visibility constraints and dual-arm constraints. We present simulated results to validate our method, comparing it to approaches that use direct sampling of constraints.

This paper presents a kinodynamic motion planner, i.e., Kinodynamic Motion Planning by Interior-Exterior Cell Exploration (KPIECE), which is specifically designed for systems with complex dynamics, where integration backward in time is not possible, and speed of computation is important. A grid-based discretization is used to estimate the coverage of the state space. The coverage estimates help the planner detect the less-explored areas of the state space. An important characteristic of this discretization is that it keeps track of the boundary of the explored region of the state space and focuses exploration on the less covered parts of this boundary. Extensive experiments show that KPIECE provides significant computational gain over existing state-of-the-art methods and allows us to solve some harder, previously unsolvable problems. For some problems, KPIECE is shown to be up to two orders of magnitude faster than existing methods and use up to 40 times less memory. A shared memory parallel implementation is presented as well. This implementation provides better speedup than an embarrassingly parallel implementation by taking advantage of the evolving multicore technology.

This paper introduces the concept of a task motion multigraph, a data structure that can be used to reveal a difficulty specific to mobile manipulation: the possibility of planning in different state spaces in order to achieve the same goal. The different options reflect the mobile manipulator's ability to use different hardware components to perform a required task. For instance, a humanoid robot can open a door with its left arm or with its right arm. Thus, motion planning can be performed in the left arm's state space or in the right arm's state space. Given the specification of a task, it is shown how to encode the available motion planning options in a task motion multigraph. An algorithm that computes sequences of motion plans for mobile manipulators using the newly introduced notion is presented and evaluated. The algorithm makes use of information from the task motion multigraph to prioritize the spaces for which motion plans are computed. Experimental results show that reduced planning times can be obtained when considering the available planning options.

This paper addresses the problem of computing the sequence of motion plans necessary for a mobile manipulator to execute a given task. In our previous work, we have demonstrated that computational advantages can be obtained when solving this problem by using the notion of a task motion multigraph (TMM). TMMs represent the state spaces that correspond to various hardware components of the robot, and they convey this information to the motion planning level. In this paper, we present and evaluate an algorithm that further exploits TMMs and explores multiple state spaces simultaneously. Since tasks to be performed by mobile manipulators often allow solutions that use only a subset of the robot's hardware components, motion plans can be found in lower dimensional state spaces. The resulting solutions tend to be shorter, more natural and faster to compute. We show that when planning under geometric constraints only, information gained while exploring lower dimensional spaces can be reused to obtain solutions in higher dimensional spaces, if necessary. The reuse of information implicitly provides the ability to compute decoupled motion plans. If solutions are not found while planning in a decoupled fashion, the algorithm resorts to planning in the robot's full state space. Our experiments indicate speedups of 200% and solutions up to four times shorter when compared to an analogous approach that does not employ TMMs.

Single-query sampling-based motion planners are an efficient class of algorithms widely used today to solve challenging motion planning problems. This paper exposes the common core of these planners and presents a tutorial for their implementation. A set of ideas extracted from algorithms existing in the literature is presented. In addition, lower level implementation details that are often skipped in papers due to space limitations are discussed. The purpose of the paper is to improve our understanding of single-query sampling-based motion planners and motivate our community to explore avenues of research that lead to significant improvements of such algorithms.

We present a novel combination of motion planning techniques to compute motion plans for robotic arms. We compute plans that move the arm as close as possible to the goal region using sampling-based planning and then switch to a trajectory optimization technique for the last few centimeters necessary to reach the goal region. This combination allows fast computation and safe execution of motion plans even when the goals are very close to objects in the environment. The system incorporates realtime sensory inputs and correctly deals with occlusions that can occur when robot body parts block the sensor view of the environment. The system is tested on a 7 degree-of-freedom robot arm with sensory input from a tilting laser scanner that provides 3D information about the environment.

This paper presents significant steps towards the online integration of 3D perception and manipulation for personal robotics applications. We propose a modular and distributed architecture, which seamlessly integrates the creation of 3D maps for collision detection and semantic annotations, with a real-time motion replanning framework. To validate our system, we present results obtained during a comprehensive mobile manipulation scenario, which includes the fusion of the above components with a higher level executive.

Sampling-based motion planners are often used to solve very high-dimensional planning problems. Many recent algorithms use projections of the state space to estimate properties such as coverage, as it is impractical to compute and store this information in the original space. Such estimates help motion planners determine the regions of space that merit further exploration. In general, the employed projections are user-defined, and to the authors' knowledge, automatically computing them has not yet been investigated. In this work, the feasibility of offline-computed random linear projections is evaluated within the context of a state-of-the art sampling-based motion planning algorithm. For systems with moderate dimension, random linear projections seem to outperform human intuition. For more complex systems it is likely that non-linear projections would be better suited.

This paper provides proof-of-concept that state-of-the-art sampling-based motion planners that are tightly integrated with a physics-based simulator can compute paths that can be executed by a physical robotic system. Such a goal has been the subject of intensive research during the last few years and reflects the desire of the motion planning community to produce paths that are directly relevant to realistic mechanical systems and do not need a huge post-processing step in order to be executed on a robotic platform. To evaluate this approach, a recently developed motion planner is used to compute paths for a modular robot constructed from seven modules. These paths are then executed on hardware and compared with the paths predicted by the planner. For the system considered, the planner prediction and the paths achieved by the physical robot match, up to small errors. This work reveals the potential of modern motion planning research and its implications in the design and operation of complex robotic platforms.