Andreas Orthey

Servet B. Bayraktar, Andreas Orthey, Marc Toussaint

Object extraction tasks often occur in disassembly problems, where bolts, screws, or pins have to be removed from tight, narrow spaces. In such problems, the distance to the environment is often on the millimeter scale. Sampling-based planners can solve such problems and provide completeness guarantees. However, sampling becomes a bottleneck, since almost all motions will result in collisions with the environment. To overcome this problem, we propose a novel scale-invariant sampling strategy which explores the configuration space using a grow-shrink search to find useful, high-entropy sampling scales. Once a useful sampling scale has been found, our framework exploits this scale by using a principal components analysis (PCA) to find useful directions for object extraction. We embed this sampler into a multi-arm bandit rapidly-exploring random tree (MAB-RRT) planner and test it on eight challenging 3D object extraction scenarios, involving bolts, gears, rods, pins, and sockets. To evaluate our framework, we compare it with classical sampling strategies like uniform sampling, obstacle-based sampling, and narrow-passage sampling, and with modern strategies like mate vectors, physics-based planning, and disassembly breadth first search. Our experiments show that scale-invariant sampling improves success rate by one order of magnitude on 7 out of 8 scenarios. This demonstrates that scale-invariant sampling is an important concept for general purpose object extraction in disassembly tasks.

Constantinos Chamzas, Carlos Quintero-Peña, Zachary Kingston et al.

Recently, there has been a wealth of development in motion planning for robotic manipulation new motion planners are continuously proposed, each with their own unique strengths and weaknesses. However, evaluating new planners is challenging and researchers often create their own ad-hoc problems for benchmarking, which is time-consuming, prone to bias, and does not directly compare against other state-of-the-art planners. We present MotionBenchMaker, an open-source tool to generate benchmarking datasets for realistic robot manipulation problems. MotionBenchMaker is designed to be an extensible, easy-to-use tool that allows users to both generate datasets and benchmark them by comparing motion planning algorithms. Empirically, we show the benefit of using MotionBenchMaker as a tool to procedurally generate datasets which helps in the fair evaluation of planners. We also present a suite of 40 prefabricated datasets, with 5 different commonly used robots in 8 environments, to serve as a common ground to accelerate motion planning research.

Andreas Orthey, Florian T. Pokorny, Marc Toussaint

In this extended abstract, we report on ongoing work towards an approximate multimodal optimization algorithm with asymptotic guarantees. Multimodal optimization is the problem of finding all local optimal solutions (modes) to a path optimization problem. This is important to compress path databases, as contingencies for replanning and as source of symbolic representations. Following ideas from Morse theory, we define modes as paths invariant under optimization of a cost functional. We develop a multi-mode estimation algorithm which approximately finds all modes of a given motion optimization problem and asymptotically converges. This is made possible by integrating sparse roadmaps with an existing single-mode optimization algorithm. Initial evaluation results show the multi-mode estimation algorithm as a promising direction to study path spaces from a topological point of view.

Valentin Noah Hartmann, Andreas Orthey, Danny Driess et al.

Robotic assembly planning enables architects to explicitly account for the assembly process during the design phase, and enables efficient building methods that profit from the robots' different capabilities. Previous work has addressed planning of robot assembly sequences and identifying the feasibility of architectural designs. This paper extends previous work by enabling planning with large, heterogeneous teams of robots. We present a planning system which enables parallelization of complex task and motion planning problems by iteratively solving smaller subproblems. Combining optimization methods to 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 case-studies to show the robustness over long planning horizons and scalability to many objects and agents of our algorithm. Finally, we also demonstrate the execution of the computed plans on two robot arms to showcase the feasibility in the real world.

Marie-Therese Khoury, Andreas Orthey, Marc Toussaint

Contact-based motion planning for manipulation, object exploration or balancing often requires finding sequences of fixed and sliding contacts and planning the transition from one contact in the environment to another. However, most existing algorithms concentrate on the control and learning aspect of sliding contacts, but do not embed the problem into a principled framework to provide guarantees on completeness or optimality. To address this problem, we propose a method to extend constraint-based planning using contact transitions for sliding contacts. Such transitions are elementary operations required for whole contact sequences. To model sliding contacts, we define a sliding contact constraint that permits the robot to slide on the surface of a mesh-based object. To exploit transitions between sliding contacts, we develop a contact transition sampler, which uses three constraint modes: contact with a start surface, no contact and contact with a goal surface. We sample these transition modes uniformly which makes them usable with sampling-based planning algorithms. Our method is evaluated by testing it on manipulator arms of two, three and seven internal degrees of freedom with different objects and various sampling-based planning algorithms. This demonstrates that sliding contact constraints could be used as an elementary method for planning long-horizon contact sequences for high-dimensional robotic systems.

Andreas Orthey, Marc Toussaint

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.

Andreas Orthey, Marc Toussaint

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.

Andreas Orthey, Sohaib Akbar, Marc Toussaint

High-dimensional motion planning problems can often be solved significantly faster by using multilevel abstractions. While there are various ways to formally capture multilevel abstractions, we formulate them in terms of fiber bundles. Fiber bundles essentially describe lower-dimensional projections of the state space using local product spaces, which allows us to concisely describe and derive novel algorithms in terms of bundle restrictions and bundle sections. Given such a structure and a corresponding admissible constraint function, we develop highly efficient and asymptotically-optimal sampling-based motion planning methods for high-dimensional state spaces. Those methods exploit the structure of fiber bundles through the use of bundle primitives. Those primitives are used to create novel bundle planners, the rapidly-exploring quotient-space trees (QRRT*), and the quotient-space roadmap planner (QMP*). Both planners are shown to be probabilistically complete and almost-surely asymptotically optimal. To evaluate our bundle planners, we compare them against classical sampling-based planners on benchmarks of four low-dimensional scenarios, and eight high-dimensional scenarios, ranging from 21 to 100 degrees of freedom, including multiple robots and nonholonomic constraints. Our findings show improvements up to 2 to 6 orders of magnitude and underline the efficiency of multilevel motion planners and the benefit of exploiting multilevel abstractions using the terminology of fiber bundles.

Andreas Orthey, Marc Toussaint

Multi-robot motion planning problems often have many local minima. It is essential to visualize those local minima such that we can better understand, debug and interact with multi-robot systems. Towards this goal, we present the multi-robot motion explorer, an algorithm which extends previous results on multilevel Morse theory by introducing a component-based framework, where we reduce multi-robot configuration spaces by reducing each robots component space using fiber bundles. Our algorithm exploits this component structure to search for and visualize local minima. A user of the algorithm can specify a multilevel abstraction and an optimization algorithm. We use this information to incrementally build a local minima tree for a given problem. We demonstrate this algorithm on several multi-robot systems of up to 20 degrees of freedom.

Andreas Orthey, Benjamin Frész, Marc Toussaint

Motion planning problems often have many local minima. Those minima are important to visualize to let a user guide, prevent or predict motions. Towards this goal, we develop the motion planning explorer, an algorithm to let users interactively explore a tree of local-minima. Following ideas from Morse theory, we define local minima as paths invariant under minimization of a cost functional. The local-minima are grouped into a local-minima tree using lower-dimensional projections specified by a user. The user can then interactively explore the local-minima tree, thereby visualizing the problem structure and guide or prevent motions. We show the motion planning explorer to faithfully capture local minima in four realistic scenarios, both for holonomic and certain non-holonomic robots.

Andreas Orthey, Marc Toussaint

Motion planning problems can be simplified by admissible projections of the configuration space to sequences of lower-dimensional quotient-spaces, called sequential simplifications. To exploit sequential simplifications, we present the Quotient-space Rapidly-exploring Random Trees (QRRT) algorithm. QRRT takes as input a start and a goal configuration, and a sequence of quotient-spaces. The algorithm grows trees on the quotient-spaces both sequentially and simultaneously to guarantee a dense coverage. QRRT is shown to be (1) probabilistically complete, and (2) can reduce the runtime by at least one order of magnitude. However, we show in experiments that the runtime varies substantially between different quotient-space sequences. To find out why, we perform an additional experiment, showing that the more narrow an environment, the more a quotient-space sequence can reduce runtime.

Andreas Orthey, Olivier Roussel, Olivier Stasse et al.

The motion of a mechanical system can be defined as a path through its configuration space. Computing such a path has a computational complexity scaling exponentially with the dimensionality of the configuration space. We propose to reduce the dimensionality of the configuration space by introducing the irreducible path --- a path having a minimal swept volume. The paper consists of three parts: In part I, we define the space of all irreducible paths and show that planning a path in the irreducible path space preserves completeness of any motion planning algorithm. In part II, we construct an approximation to the irreducible path space of a serial kinematic chain under certain assumptions. In part III, we conduct motion planning using the irreducible path space for a mechanical snake in a turbine environment, for a mechanical octopus with eight arms in a pipe system and for the sideways motion of a humanoid robot moving through a room with doors and through a hole in a wall. We demonstrate that the concept of an irreducible path can be applied to any motion planning algorithm taking curvature constraints into account.

Andreas Orthey, Adrien Escande, Eiichi Yoshida

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).