Robots often have to operate in discrete partially observable worlds, where the state of the world is only observable at runtime. To react to different world states, robots need contingencies. To find contingencies, prior work developed the path tree optimization (PTO) method, which computes motion contingencies by constructing a tree of motion paths in belief space. In this paper, we extend upon PTO by enabling camera-based belief space planning through an extension of the open motion planning library (OMPL). By leveraging this extension, we develop an improved camera-based state sampler and an efficient open-source implementation of PTO. This version of PTO supports a virtual camera, non-euclidean state spaces, and different state samplers. We evaluate this improved version of PTO on four realistic scenarios with a virtual camera in up to 10-dimensional state spaces. In our evaluations, we compare PTO both with a default and with the new camera-based state sampler. The results indicate that the camera-based state sampler improves success rates in 3 out of 4 scenarios while having a significant lower memory footprint. Our work thus makes an important step in advancing belief-space planning and provides researchers with an open source tool to use, modify, and benchmark belief-space planning methods.

Robotic construction assembly planning aims to find feasible assembly sequences as well as the corresponding robot-paths and can be seen as a special case of task and motion planning (TAMP). As construction assembly can well be parallelized, it is desirable to plan for multiple robots acting concurrently. Solving TAMP instances with many robots and over a long time-horizon is challenging due to coordination constraints, and the difficulty of choosing the right task assignment. We present a planning system which enables parallelization of complex task and motion planning problems by iteratively solving smaller subproblems. Combining optimization methods to jointly solve for manipulation constraints with a sampling-based bi-directional space-time path planner enables us to plan cooperative multi-robot manipulation with unknown arrival-times. Thus, our solver allows for completing subproblems and tasks with differing timescales and synchronizes them effectively. We demonstrate the approach on multiple construction case-studies to show the robustness over long planning horizons and scalability to many objects and agents. Finally, we also demonstrate the execution of the computed plans on two robot arms to showcase the feasibility in the real world.

We present a motion planner for planning through space-time with dynamic obstacles, velocity constraints, and unknown arrival time. Our algorithm, Space-Time RRT*(ST-RRT*), is a probabilistically complete, bidirectional motion planning algorithm, which is asymptotically optimal with respect to the shortest arrival time. We experimentally evaluate ST-RRT* in both abstract (2D disk, 8D disk in cluttered spaces, and on a narrow passage problem), and simulated robotic path planning problems (sequential planning of 8DoF mobile robots, and 7DoF robotic arms). The proposed planner outperforms RRT-Connect and RRT* on both initial solution time, and attained final solution cost. The code for ST-RRT* is available in the Open Motion Planning Library (OMPL).

Optimal sampling based motion planning and trajectory optimization are two competing frameworks to generate optimal motion plans. Both frameworks have complementary properties: Sampling based planners are typically slow to converge, but provide optimality guarantees. Trajectory optimizers, however, are typically fast to converge, but do not provide global optimality guarantees in nonconvex problems, e.g. scenarios with obstacles. To achieve the best of both worlds, we introduce a new planner, BITKOMO, which integrates the asymptotically optimal Batch Informed Trees (BIT*) planner with the K-Order Markov Optimization (KOMO) trajectory optimization framework. Our planner is anytime and maintains the same asymptotic optimality guarantees provided by BIT*, while also exploiting the fast convergence of the KOMO trajectory optimizer. We experimentally evaluate our planner on manipulation scenarios that involve high dimensional configuration spaces, with up to two 7-DoF manipulators, obstacles and narrow passages. BITKOMO performs better than KOMO by succeeding even when KOMO fails, and it outperforms BIT* in terms of convergence to the optimal solution.

Sparse roadmaps are important to compactly represent state spaces, to determine problems to be infeasible and to terminate in finite time. However, sparse roadmaps do not scale well to high-dimensional planning problems. In prior work, we showed improved planning performance on high-dimensional planning problems by using multilevel abstractions to simplify state spaces. In this work, we generalize sparse roadmaps to multilevel abstractions by developing a novel algorithm, the sparse multilevel roadmap planner (SMLR). To this end, we represent multilevel abstractions using the language of fiber bundles, and generalize sparse roadmap planners by using the concept of restriction sampling with visibility regions. We argue SMLR to be probabilistically complete and asymptotically near-optimal by inheritance from sparse roadmap planners. In evaluations, we outperform sparse roadmap planners on challenging planning problems, in particular problems which are high-dimensional, contain narrow passages or are infeasible. We thereby demonstrate sparse multilevel roadmaps as an efficient tool for feasible and infeasible high-dimensional planning problems.

Sampling-based planning methods often become inefficient due to narrow passages. Narrow passages induce a higher runtime, because the chance to sample them becomes vanishingly small. In recent work, we showed that narrow passages can be approached by relaxing the problem using admissible lower dimensional projections of the state space. Those relaxations often increase the volume of narrow passages under projection. Solving the relaxed problem is often efficient and produces an admissible heuristic we can exploit. However, given a base path, i.e., a solution to a relaxed problem, there are currently no tailored methods to efficiently exploit the base path. To efficiently exploit the base path and thereby its admissible heuristic, we develop section patterns, which are solution strategies to efficiently exploit base paths in particular around narrow passages. To coordinate section patterns, we develop the pattern dance algorithm, which efficiently coordinates section patterns to reactively traverse narrow passages. We combine the pattern dance algorithm with previously developed multilevel planning algorithms and benchmark them on challenging planning problems like the Bugtrap, the double L-shape, an egress problem, and on four pregrasp scenarios for a 37 degrees-of-freedom shadow hand mounted on a KUKA LWR robot. Our results confirm that section patterns are useful to efficiently solve high-dimensional narrow passage motion planning problems.

A motion planning algorithm computes the motion of a robot by computing a path through its configuration space. To improve the runtime of motion planning algorithms, we propose to nest robots in each other, creating a nested quotient-space decomposition of the configuration space. Based on this decomposition we define a new roadmap-based motion planning algorithm called the Quotient-space roadMap Planner (QMP). The algorithm starts growing a graph on the lowest dimensional quotient space, switches to the next quotient space once a valid path has been found, and keeps updating the graphs on each quotient space simultaneously until a valid path in the configuration space has been found. We show that this algorithm is probabilistically complete and outperforms a set of state-of-the-art algorithms implemented in the open motion planning library (OMPL).

We introduce a novel notion for lowering the dimensionality of motion planning problems: Irreducibility. Irreducibility of a configuration space trajectory τ means: We cannot find another configuration space trajectory τ', such that the swept volume of τ' is included in the swept volume of τ. The main contribution of our work is twofold: First, we show that motion planning in the space of irreducible trajectories is complete. Second, we show that we can construct reducible subspaces by reasoning about the inherent hierarchical structure of open kinematic chains. Using those theoretical results, we proceed by analytically defining a 7-dimensional irreducible configuration subspace for the humanoid robot HRP-2 under some assumptions. To show its practical importance, we solve a high-dimensional pin-hole problem for HRP-2 from the scratch.

Solving complex robot manipulation tasks requires to combine motion generation on the geometric level with planning on a symbolic level. On both levels robotics research has developed a variety of mature methodologies, including geometric motion planning and motion primitive learning on the motor level as well as logic reasoning and relational Reinforcement Learning methods on the symbolic level. However, their robust integration remains a great challenge. In this paper we approach one aspect of this integration by optimizing the motion primitives on the geometric level to be as consistent as possible with their symbolic predictions. The so optimized motion primitives increase the probability of a “successful” motion-meaning that the symbolic prediction was indeed achieved. Conversely, using these optimized motion primitives to collect new data about the effects of actions the learnt symbolic rules becomes more predictive and deterministic.