Multilevel motion planning: A fiber bundle formulation

Abstract

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 two to six orders of magnitude and underline the efficiency of multilevel motion planners and the benefit of exploiting multilevel abstractions using the terminology of fiber bundles.

1. Introduction

As human beings, we often tackle complex problems by employing abstraction hierarchies (Simon, 1969; Ballard, 2015). Abstraction hierarchies, however, are not only helpful for us, but they can also be used by robots to solve high-dimensional motion planning problems—efficiently, and with strong guarantees (Orthey and Toussaint, 2019; Reid et al., 2019; Ichter and Pavone, 2019; Vidal et al., 2019; Gochev et al., 2012).

However, abstractions in robot motion planning are difficult to model. The state spaces involved are usually continuous, high-dimensional, and might contain intricate constraints (Konidaris, 2019). It is often unclear how to model abstractions over such state spaces, how such abstractions can be efficiently exploited, and how we can keep completeness, or optimality guarantees.

To tackle this problem, we introduce the framework of fiber bundles (Steenrod, 1951; Lee, 2003) to robot motion planning. Fiber bundles are a convenient way to model multilevel abstractions, because they provide the useful concepts of bundle sections and restrictions. Both bundle sections and restrictions allow us to develop novel planning algorithms to exploit them for efficient sampling. Figure 1 illustrates these two notions, which we will introduce in more detail later. On the left, we show an abstraction (a projection) from the torus T² to the circle S¹. A path on the circle (magenta) imposes a path restriction (dark gray) on the torus T². On this path restriction, we can find path sections (green), which are paths which project onto the path on the circle. The path on the circle acts here as a guide to quickly find solution paths. On the right of Figure 1, we show the same scenario for the Mobius strip, which is a non-trivial fiber bundle (Möbius, 1858). A tree (green) is shown on the path restriction (dark gray), where planning is restricted by the path on the circle (magenta). By constructing sections and restrictions, we can create novel motion planners, which are able to quickly find relevant regions in state space to plan in. This allows those planner to efficiently solve high-dimensional motion planning problems. An overview of our approach is shown in Figure 2.

Figure 1.

Efficient search over fiber bundles by exploiting path restrictions and sections. Left: A fiber bundle T² → S¹ which abstracts the Torus to a base space (circle). A path on the base space (magenta) imposes a path restriction (dark gray) on the Torus, on which we can find a section (green). Right: A non-trivial fiber bundle from the Mobius strip to a circle, with path restriction (dark gray), and a planned tree (green).

Figure 2.

Overview about the contributions of this paper (gray boxes are input). We tackle motion planning in high-dimensional state spaces by imposing multilevel abstractions. Those abstractions are modeled as fiber bundles (Section 4), from which bundle primitives are derived (Section 5), and the bundle planners QRRT, QRRT*, QMP, and QMP* are created (Section 6). Those bundle planners efficiently exploit bundle primitives. Given a new query in a high-dimensional state space, this allows us to plan and optimize motions, even in scenarios where non-bundle planners fail.

1.1. Our contributions

Our work builds on prior publications at the International Conference on Intelligent Robots and Systems (IROS) (Orthey et al., 2018) and the International Symposium on Robotics Research (ISRR) (Orthey and Toussaint, 2019). Our contributions over this prior work are:

1. We propose to formulate multilevel motion planning problems using the terminology of fiber bundles, and introduce the particularly useful notions of bundle sections and bundle restrictions.

2. Based on this formulation, we improve upon previous bundle planners QRRT and QMP and develop two new bundle planners QRRT* and QMP*.

3. We show QRRT* and QMP* to be probabilistically complete and asymptotically optimal by inheritance from RRT*, and PRM*.

4. We define primitive methods on fiber bundles and conduct a meta-analysis to find the best implementation of those methods.

5. We provide open source implementations of fiber bundles and bundle planners (together with a high-level introduction, a tutorial, and demos), which is freely available in the open motion planning library (OMPL).

6. We evaluate QMP, QMP*, QRRT, and QRRT* by comparing them against planners from OMPL on four low-dimensional scenarios ranging from 2 degrees of freedom (dof) to 7-dof, and on eight high-dimensional scenarios ranging from 21-dof to 100-dof.

Our evaluations show that our planners QMP, QMP*, QRRT, and QRRT* can efficiently exploit fiber bundles. While they are competitive in low-dimensional spaces, they are particularly useful in high-dimensional spaces, where other planners have difficulty finding solutions. We show that for high-dimensional state spaces, our bundle space planners can provide runtime improvements by up to two to six order of magnitude.

2. Related work

We provide a brief overview on motion planning with focus on optimal planning. We then discuss multilevel motion planning by discussing how our approach of fiber bundles is connected to existing research. In particular, we stress the point that fiber bundles often contribute additional vocabulary, which we can exploit to develop novel methods, simplify notations and better structure our code. We finish by reviewing complementary approaches to fiber bundles and we discuss what our approach adds to existing approaches in (optimal) multilevel motion planning.

2.1. Motion planning

To solve motion planning problems, we need to develop algorithms to find paths through the state space of a robot (Lozano-Pérez, 1983). Searching such a state space is NP-hard (Canny, 1988), but we can often efficiently find solutions using sampling-based algorithms (LaValle, 2006; Salzman, 2019), where we randomly sample states and connect them to a graph (Kavraki et al., 1996) or to a tree (Lavalle, 1998). Many variations are possible, for example, using bidirectional trees growing from start and goal state (Kuffner and LaValle, 2000; LaValle and Kuffner Jr, 2001), lazy evaluation of edges (Bohlin and Kavraki, 2000; Mandalika et al., 2019), sparse graphs (Siméon et al., 2000; Jaillet and Siméon, 2008), safety certificates (Bialkowski et al., 2016), or deterministic sampling sequences (Janson et al., 2018; Palmieri et al., 2019).

Often, we like to find an optimal path by minimizing an optimization objective (Karaman and Frazzoli, 2011). To find optimal paths, we could transfer ideas from classical planning like lazy edge evaluation (Hauser, 2015) or sparse graphs (Dobson and Bekris, 2014). However, cost function landscapes (Jaillet et al. 2010) often provide additional information we can exploit. Examples include informed sets to prune irrelevant states (Gammell et al., 2018, 2020) or fast marching trees to grow trees outward in cost to come space (Janson et al., 2015). Recently, sampling-based motion planning algorithms have also been extended to address zero-measure constraints (Kingston et al., 2019), implicit constraints (Jaillet and Porta, 2013), dynamic constraints (Li et al., 2016), or dynamic environments (Otte and Frazzoli, 2016).

All those algorithms can robustly solve many planning problems, provide formal guarantees like probabilistic completeness, or asymptotic optimality, and have been verified in a wide variety of applications (LaValle, 2006; Şucan et al., 2012). However, as we show in Section 8, we often cannot use them to solve high-dimensional planning problems in a reasonable amount of time (like less than 60 s). We believe additional information is required to solve those problems efficiently. A possible candidate for this additional information are multiple levels of abstraction.

2.2. Multilevel motion planning

In multilevel motion planning, we impose a multilevel abstraction on the state space and we develop algorithms which exploit this abstraction. While several models for multilevel motion planning have been put forward, we propose to use fiber bundles. To justify this decision, we show their relation to alternative modeling approaches and provide clues to the additional value they bring to the table.

2.2.1. Quotient spaces

Fiber bundles are related to quotient spaces (Orthey et al., 2018; Orthey and Toussaint, 2019; Brandao and Havoutis, 2020), latent spaces (Ichter and Pavone, 2019), or subspaces (Reid et al., 2020) in that we can represent those spaces as the base space of a fiber bundle. We can often create such a base space by taking the quotient of an equivalence class (Pappas et al., 2000). Using the ideas of base spaces, there are two interesting special cases. First, we can use base spaces to simplify a nonholonomic state space to a holonomic state space (Sekhavat et al., 1998; Vidal et al., 2019). Often, having a path on the base space is enough to find a global solution, in particular if some sort of smoothness constraint is imposed (Vidal et al., 2019; Hönig et al., 2018). Second, we can use sequences of base spaces to simplify multi-robot planning problems (Erdmann and Lozano-Perez, 1987; Siméon et al., 2002; Solovey and Halperin, 2014). We can often solve such problems efficiently by graph coordination. In graph coordination, we first plan a graph on each individual robot subspace, then we combine them using specialized algorithms like sub-dimensional expansion (Wagner and Choset, 2015) or directional oracles (Solovey et al., 2016; Shome et al., 2020). This is different from our approach, in that graphs are constructed independently, while we construct them sequentially, similar to a prioritization of robots (Erdmann and Lozano-Perez, 1987; Van Den Berg and Overmars, 2005; Ma et al., 2019).

While numerous works exist to exploit sequences of base spaces (Zhang et al., 2009; Vidal et al., 2019), we like to highlight two algorithms. First, the Manhattan-like rapidly-exploring random tree (ML-RRT) (Cortés et al., 2008; Nguyen et al., 2018), where path sections are computed similar to the L1 interpolation we advocate. However, the ML-RRT approach differs from ours, in that we use a different collision checking function for the base space and we give formal guarantees using restriction sampling. Second, the hierarchical bidirectional fast marching tree (HBFMT) algorithm (Reid et al., 2019, 2020), where restriction sampling is used on sequences of subspaces. Similar to our approach, Reid et al. (2020) prove HBFMT to be almost-surely asymptotically optimal by inheritance from BFMT*. Our approach is similar in that we develop asymptotically optimal algorithms based on RRT* and PRM*. However, contrary to both Reid et al. (2020) and Jaillet and Siméon (2008), we use quotient spaces instead of subspaces, we support manifolds instead of only Euclidean spaces, we support multiple robots with nonholonomic constraints, and we provide a variable path bias for restriction sampling (contrary to a fixed path bias Reid et al. (2020)). We also differ by providing a recursive path section method which we show to quickly find sections even in high-dimensional state spaces.

2.2.2. Constraint relaxations

Fiber bundles are related to constraint relaxations (Boyd and Vandenberghe, 2004; Roubíček, 2011), in that we can often model constraint relaxations as a particular type of fiber bundle, that is, a bundle with an admissible projection, which does not reduce the dimensionality. We can often create constraint relaxations by increasing the free space (Hsu et al., 2006), by retracting the obstacle geometry (Saha et al., 2005), or by shrinking robot links sequentially to zero (Baginski, 1996). While constraint relaxations often do not decrease the dimensionality, there are, however, extensions which do decrease the dimensionality, like progressive relaxations (Ferbach and Barraquand, 1997) or iterative constraint relaxations (Bayazit et al., 2005). In both methods, we either remove links or robots from the problem and we can use them to model the same multilevel abstractions as we can do with fiber bundles. However, by using fiber bundles, we can add additional insights like path sections and restriction sampling.

Closely related to relaxations are projections (Şucan and Kavraki 2009, 2011; Röwekämper et al., 2013; Luna et al., 2020). Projections are a component of fiber bundles, which we use to project the state space onto a lower-dimensional base space. Contrary to quotient spaces, projections are often not required to be admissible but can even be random (Şucan and Kavraki, 2009). A noteworthy approach is projection using adaptive dimensionality (Vahrenkamp et al., 2008; Gochev et al., 2012, 2013), where projections remove degrees of freedom (dof). We can remove dofs of a robot by having a fixed projection (Gochev et al., 2012; Yu et al., 2020) or by adjusting the projection depending on which links are closest to obstacles (Yoshida, 2005; Kim et al., 2015). While similar to fiber bundles, both Yoshida (2005) and Kim et al. (2015) emphasize the role of distances in workspace to choose a multilevel abstraction, which is an interesting complementary approach to ours. We differ, however, by supporting multiple robots, nonholonomic constraints, and by providing asymptotic optimality guarantees.

2.2.3. Admissible heuristics

Fiber bundles are related to admissible heuristics (Pearl, 1984; Persson and Sharf, 2014; Aine et al., 2016), in that we can use metrics on the lower-dimensional base space as admissible heuristics (Passino and Antsaklis, 1994) to guide search on the state space. This is closely related to the idea of computing lower bounds for planning problems (Salzman and Halperin, 2016). When using sequences of fiber bundles, we basically use tighter and tighter lower bounds on the real solution. Our approach differs, however, in that we do not consider inadmissible heuristics, which we could combine with admissible heuristics to often speed up planning (Aine et al., 2016; Tonneau et al., 2018).

While there are many ways to define admissible heuristics (Aine et al., 2016), we believe there are two main approaches for the case of continuous state spaces, namely, low-dimensional sampling and guide paths. In low-dimensional sampling (Şucan and Kavraki, 2009), we first sample on a lower-dimensional base space, then use those samples to restrict sampling on the state space. There are two main approaches. First, we can select sequences of subspaces of the state space (Xanthidis et al., 2018), then sample them by selecting the subspaces based on the density of samples. Second, we can use workspace sampling (Van Den Berg and Overmars, 2005; Zucker et al., 2008; Rickert et al., 2014; Luna et al., 2020), where state space samples are taken from the restriction of collision-free sets in workspace. We can do workspace sampling by focusing on narrow passages (Van Den Berg and Overmars, 2005), or by selecting promising points on the robot and guiding them through the workspace (Luna et al., 2020). Our approach is similar in that we use lower-dimensional sampling on the base space and we select base spaces based on a density criterion (Xanthidis et al., 2018). However, we differ by smoothly changing between path and graph restriction sampling, and by using a recursive path section method to efficiently find solution paths.

Closely related to low-dimensional sampling is the concept of guide paths (Tonneau et al., 2018; Ha et al., 2019). A guide path is a solution on the base space, which we use to restrict sampling on the state space (Palmieri et al., 2016). Guide paths are often used in contact planning (Bretl, 2006; Tonneau et al., 2018), where we can often give sufficiency conditions on when a feasible section exists (Grey et al., 2017). When no feasible section exists, some methods fail while other gradually shift towards graph restriction sampling (Grey et al., 2017). It is also possible to compute multiple guide paths which increase our chance to find a feasible section (Vonásek and Pěniĝka, 2019; Ha et al., 2019; Orthey et al., 2020). While we also sample along guide paths (path restriction sampling), we differ in two ways. First, we use adaptive restriction sampling to gradually change sampling from path to graph restriction, whereby we guarantee asymptotic optimality. Second, we use a recursive path section method to quickly find feasible path sections in high-dimensional state spaces.

2.3. Exploiting additional information

Fiber bundles are a way to exploit additional information. Other approaches, complementary to fiber bundles, exists. One approach is region-based decomposition. In a region-based decomposition, the problem is divided into regions in which planning becomes computationally efficient (Toussaint and Lopes, 2017; Orthey et al., 2020). Such an approach can be done in two ways. First, the workspace can be divided (Plaku et al., 2010; Vega-Brown and Roy, 2018), for example, using subdivision grids (Plaku, 2015), Delaunay decompositions (Plaku et al., 2010), or convex regions (Deits and Tedrake, 2014; Vega-Brown and Roy, 2018). Second, the solution path space can be divided (Farber, 2008), for example, by using the notion of homotopy classes (Munkres, 2000), where two paths are considered to be equivalent if we can deform them into each other. Homotopy classes are closely related to the notion of topological complexity (Farber, 2017), the minimal number of regions in state space which are collapsible into a point (null-homotopic). Several practical solutions exists to compute path homotopy classes, like the H-value (Bhattacharya et al., 2012; Bhattacharya and Ghrist, 2018), simplicial complices (Pokorny et al., 2016a), task projections (Pokorny et al., 2016b), or mutual crossings of robots (Mavrogiannis and Knepper, 2016). However, all those approaches often become computationally intractable for high-dimensional systems, multiple robots, or nonholonomic constraints. Fiber bundles are a complementary effort to organize regions on different levels of abstraction (Orthey and Toussaint, 2020).

Apart from region-based decompositions, we identify three other methods to exploit additional information. First, we can exploit distance information in workspace to compute sets of feasible states (Quinlan, 1994), which can be used to plan safe motions (Bialkowski et al., 2016), or to compute covers of free space (Lacevic et al., 2016; Lacevic and Osmankovic, 2020). Second, we can exploit differentiable constraints when available (Toussaint et al., 2018; Henkel and Toussaint, 2020). Third, we can exploit alternative state space representations, for example, by using topology-preserving mappings (Zarubin et al., 2012; Ivan et al., 2013). This is complementary to our approach, in that Zarubin et al. (2012) tries to find alternative representations of a state space, while we concentrate on finding simplifications of a given space.

2.4. Fiber bundles and prior approaches

Fiber bundle planners exploit a number of projections to accelerate planning performance. Prior approaches using projections are the KPIECE planner (Şucan and Kavraki, 2009), which uses a projection onto a simplified space, and the SBL planner (Sánchez and Latombe, 2003a), which plans using a simplified grid of the state space. However, most prior approaches are limited in the number of projections (Şucan and Kavraki, 2009; Cortés et al., 2008), the number of robots (Vidal et al., 2019), use only holonomic constraints (Zhang et al., 2009), use only Euclidean spaces (Reid et al., 2019, 2020), or work only in specific situations (Gochev et al., 2012; Kim et al., 2015). Instead, we can apply fiber bundles to any manifold space (we show it for compound spaces including the special Euclidean and orthogonal groups in 2 d and 3 d), any finite number of projections (up to 98 in our evaluations), any finite number of robots (up to eight in our evaluations) and any nonholonomic constraint (for Dubin’s state spaces in our evaluations). With fiber bundles, we also provide a shared vocabulary, which can unify methods like path restriction sampling (Zhang et al., 2009; Tonneau et al., 2018; Vidal et al., 2019), or graph restriction sampling (Grey et al., 2017; Orthey et al., 2018; Reid et al., 2020). Since we also provide an open source implementation in OMPL, we can benchmark different multilevel strategies (Appendix C) and we can show the benefit of fiber bundles compared to classical motion planners (Section 8).

3. Background on optimal motion planning

Let R₁, …, R _M be M robots with associated (component) state spaces Y₁, …, Y _M , respectively. We can combine the robots into one generalized robot R with associated (composite) state space X = Y₁ ×⋯ × Y _M .

To each state space X, we add two complementary structures. First, we add a constraint function ϕ: X → {0, 1} on X which takes an element x in X and returns zero if x is feasible and one otherwise. Examples of constraints are joint limits, self-collisions, environment-robot collisions and robot-robot collisions. Second, we add a steering function ψ, which takes two elements x₁ and x₂ in X as input and returns a path steering the robot from x₁ to x₂ (while potentially ignoring constraints). We denote a state space X together with the constraint function ϕ and the steering function ψ as a planning space (X, ϕ, ψ). The planning space implicitly defines the free state space as X_f = {x ∈ X∣ϕ(x) = 0}.

Given a planning space, we define a motion planning problem as a tuple $\left(x_I,X_G,X\right)$ . To solve a motion planning problem, we need to develop an algorithm to find a path from the initial state x _I ∈ X_f to a desired goal region X _G ⊆ X_f. Often, we are not only interested in some path, but in a path which optimizes a cost functional $c:X^I\to {\mathbb{R}}_{\ge 0}$ whereby I is the unit interval and X ^I is the set of continuous paths from I to X with finite length (Karaman and Frazzoli, 2011; Janson et al., 2018). We define the optimal motion planning problem as finding a path from x _I to X _G minimizing the cost functional c.

4. Multilevel motion planning

Let X be a state space and let $X_K\stackrel{{\pi }_{K-1}}{{\displaystyle \to }}\dots \stackrel{{\pi }_1}{{\displaystyle \to }}{X}_1$ be a multilevel abstraction of X such that X _K = X, and π _k are projections from a state space X _k to a state space X_k−1. Each projection π _k : X_k−1 → X _k is modeled as a fiber bundle (see Section 4.1). Given a start configuration x _I ∈ X _K , a goal region X _G ⊆ X _K , and an objective cost functional c, we define the optimal multilevel motion planning problem as the tuple (x _I , X _G , X₁, …, X _K ) asking us to find a path from x _I to X _G while minimizing the cost c. Thus, by defining an optimal multilevel motion planning problem, we generalize optimal motion planning (Section 3) by adding additional information.

In the following sections, we discuss the framework of fiber bundles which provides us with the concepts of bundle restrictions and bundle sections. Those concepts will be used to define primitive methods (Section 5), which are fundamental to create bundle planners which exploit those primitive methods and plan efficiently over fiber bundles (Section 6).

4.1. Fiber bundle formulation

To model multiple levels of abstractions of state spaces, we use the framework of fiber bundles (Steenrod, 1951; Husemoller, 1966; Lee, 2003). A fiber bundle is a tuple (F, X, B, π), consisting of the total space X, the fiber space F, the base space B and the projection map $\pi :X\to B.$ (1)

The mapping π needs to fulfill two properties:

1. Union of Fibers: The total space X is a (disjoint) union of copies of the fiber space F, parameterized by the base space B (Lee, 2003). This means that, if we take any element b in B, the preimage π⁻¹(b) is isomorphic to the fiber space F.

2. Local Product Space: The total space X locally equals the product space B × F. This means, if we take any element b in B, there exists a neighborhood U (an open set containing b) such that the preimage π⁻¹(U) is homeomorphic to U × F (Lee, 2003).

In other words, a fiber bundle locally has the structure of a product space, and π provides a projection from the total space X to a “parameterization” of fibers in B. This local product structure and the projection π aligns with the terms of equivalence classes and quotient spaces as described in Appendix A. Our main motivation for leveraging the terminology of fiber bundles are the notions we introduce next.

4.2. Bundle restrictions

Given a fiber bundle (F, X, B, π), and a subset U ⊆ B, a bundle restriction ${X|}_U\subseteq X$ is the subset of the total space that projects to U. This set ${X|}_U={\pi }^{-1}\left(U\right)$ is called the restriction of X to U (Tu, 2017).

We consider three kinds of restrictions. First, given a point x _B on the base space, we use its restriction ${F|}_{x_B}={\pi }^{-1}\left(\left\{x_B\right\}\right)$ . Note that we call ${F|}_{x_B}$ a fiber as it is, by definition, isomorphic to F (Assertion 1.). We visualize this in Figure 3 (Left). Second, given a path p _B : I → B on the base space, with I = [0, 1] the unit interval, we have its restriction ${X|}_{p_B}={\pi }^{-1}\left(\left\{{\mathit{p}}_B\left(t\right):t\in I\right\}\right)$ (Figure 3, Middle). And third, given a graph G _B on the base space, we have the graph restriction π⁻¹(G _B ) ⊆ X, where we unproject the union of all its vertices and edges (Figure 3, Right).

Figure 3.

Bundle sections on fiber bundle X → B with base path {b₁, b₂, b₃, b₄, b₅}. We show three interpolated sections on the bundle space: L2 section (solid line), L1 fiber first section (dashed line), and L1 fiber last section (dotted line).

We use these three restrictions for different computations. First, we use point restrictions (fibers) to lift base space elements up to the total space (Section 4.3). Second, we use path restrictions to quickly compute sections, which are paths on the total space constraint to the path restriction (Section 4.3 and Section 5.4). Third, we use graph restrictions to formulate restriction sampling, that is, sampling restricted to elements of the total space that project onto the base space graph (Section 5.1). It is important to note that restriction sampling is dense in the free total space, if the graph on B is dense. We use this denseness property to prove probabilistic completeness and asymptotic optimality (Section 7).

4.3. Bundle sections

Given a fiber bundle (F, X, B, π) and a subset U ⊆ B of the base space, a section of U is a map s: U → X such that π(s(u)) = u (Lee, 2003). In other words, while a restriction ${X|}_U$ unprojects U to all elements x ∈ X that project to U, a section maps each u ∈ U to just one specific element x ∈ X that projects to u (It also follows, that $s\left(u\right)\in {X|}_U$ for any section s.). We define useful cases of sections in the following.

4.3.1. Lift

When U contains only a single element {b}, we call the section a lift. A lift s(b) takes as input an element b in B and returns an element x on the total space. We often like to single out a specific element x by additionally choosing a fiber space element f, whereby we overload the lift as s(b, f). In the case of X being a product space, we define the lift as s(b, f) = (b, f). However, if X → B is not trivial, the base space element b defines an isomorphic transformation of the fiber space (including the fiber element f), which in turn uniquely defines the element x. In this work, all bundle spaces are trivial except the Mobius strip. For the Mobius strip, we define the transformation as a linear transformation, involving a translation of the fiber space (here: the unit [0, 1] interval) around the circle while simultaneously rotating the fiber space.

4.3.2. Path section

When the subset U is an interval, we call the section a path section. A path section of a path p _B : I → B is itself a path p: I → X such that p _B = π◦p.

Our algorithms will aim to find feasible path section, that is, feasible unprojections of paths in the base space to paths in the full space. We use three interpolation methods to this end. All three methods take as input a base path p _B and two total space elements x₁ and x₂ in X. Let π _F be the projection of the total space onto the the fiber space (i.e., orthogonal to the base space projection π). We then compute fiber space elements f₁ = π _F (x₁) and f₂ = π _F (x₂) that introduce coordinates along the fibers, and which we use to interpolate. Each method differs by how we interpolate between f₁ and f₂ along the path restriction π⁻¹(p _B ) (see also Figure 4).

Figure 4.

Bundle sections on fiber bundle X → B with base path fb1; b2; b3; b4; b5g. We show three interpolated sections on the bundle space: L2 section (solid line), L1 fiber first section (dashed line) and L1 fiber last section (dotted line).

4.3.3. L2 section

To interpolate a section, we can use a straightforward L² section. To interpolate an L² section, we use the shortest path under the L²-norm, which is simply the linear interpolation $l_{L^2}\left(t\right)=\left(1-t\right)f_1+t\left(f_2-f_1\right).$ (2)

We then compute the section as $\mathit{p}\left(t\right)=s\left({\mathit{p}}_B\left(t\right),l_{L^2}\left(t\right)\right)$ (3)by lifting each path base element to the bundle space. We use the L² section mainly to compute quotient space metrics (Section 5.2).

4.3.4. L1 section

An alternative to L² sections are L¹ sections. In an interpolation with an L¹ section, we compute the section as $\mathit{p}\left(t\right)=s\left({\mathit{p}}_B\left(t\right),l_{L^1}\left(t\right)\right)$ (4)with the interpolation $l_{L^1}=\{\begin{array}{ll}{f}_1,& \text{if} t<\frac{1}{2}\\ f_2,& \text{if} t\ge \frac{1}{2}\end{array}$ (5)

We use two flavors of L¹ sections. The first flavor is fiber first (FF) sections, where we use the adjusted base path as ${\mathbf{p}}_{\text{FF}}\left(t\right)=\{\begin{array}{ll}{\mathbf{p}}_B\left(0\right),& \text{if}\hspace{0.17em}t<\frac{1}{2}\\ {\mathbf{p}}_B\left(2\left(t-\frac{1}{2}\right)\right),& \text{if}\hspace{0.17em}t\ge \frac{1}{2}\end{array}$ (6)

The second flavor is L¹ fiber last (FL) sections, where we use the base path as ${\mathbf{p}}_{\text{FL}}\left(t\right)=\{\begin{array}{ll}{\mathbf{p}}_B\left(2t\right),& \text{if}\hspace{0.17em}t<\frac{1}{2}\\ {\mathbf{p}}_B\left(1\right),& \text{if}\hspace{0.17em}t\ge \frac{1}{2}\end{array}$ (7)

Both fiber first and fiber last L¹ sections are cornerstones of our find section method, which we will use alternately to find feasible sections (see Section 5.4 for details).

4.4. Bundle sequences

With a fiber bundle, we model a single state space abstraction. To model multiple levels of abstraction, we can chain fiber bundles into sequences. A fiber bundle sequence is a tuple (X_1:K, F_1:K−1, π_1:K−1) such that the k-th base space is equal to the k − 1-th total space. We write such a sequence as $X_K\stackrel{{\pi }_{K-1}}{{\displaystyle \to }}{X}_{K-1}\stackrel{{\pi }_{K-2}}{{\displaystyle \to }}\dots \stackrel{{\pi }_1}{{\displaystyle \to }}{X}_1$ (8)whereby we call the space X _k the k-th bundle space.

4.5. Admissible fiber bundles

An important type of fiber bundles are the ones using admissible projections (Orthey et al., 2018). An admissible projection is a projection preserving the feasibility of a state. This is important to preserve probabilistic completeness and asymptotic optimality. We next define admissible projections and discuss the corresponding notion of admissible fiber bundles.

Let ϕ and ϕ _B be constraint functions on X and B, respectively. Given the constraint functions, we can define the free total space X_f and the free base space B_f (see Section 3). For an admissible projection, we require the projection mapping to fulfill the first two requirements (Assertions 1 and 2 above) plus the following third requirement:

3. Admissible: The projection mapping does not invalidate solutions. This means, if we map the free state space X_f via π onto the base space, then the image π(X_f) is a subset of the free base space B_f. Or, equivalently, ϕ _B (π(x)) ≤ ϕ(x) for any x ∈ X (Orthey and Toussaint 2019).

If a projection mapping is admissible w.r.t. given ϕ and ϕ _B , we call the fiber bundle an admissible fiber bundle. Analogously, if a sequence of fiber bundles contains only admissible projections, we call it an admissible fiber bundle sequence. It is important to note that admissibility is a requirement, if we like to prove probabilistic completeness or asymptotic optimality.

Using admissible fiber bundle (sequences), we thus can tie together the notions of quotient spaces, constraint relaxations and admissible heuristics. First, we can interpret fiber bundles as a generalization of constraint relaxations (Orthey and Toussaint, 2019), where paths on the base space are lower bound estimates on solution paths on the total space. Second, we can use a solution on the base space as an admissible heuristic (Aine et al., 2016) and exploit it by using either restriction sampling, by using a quotient space (base space) metric (Passino and Antsaklis, 1994; Pearl, 1984), or by computing sections along a given base space path (Zhang et al., 2009).

4.6. Examples of fiber bundles

To make the discussion more concrete, we discuss two (multilevel) abstractions which are often used in motion planning.

4.6.1. Prioritized multi-robot motion planning

To plan motions for multiple robots, we can prioritize the robots (Erdmann and Lozano-Perez, 1987; Ma et al., 2019). Given M robots, we can order them, then plan for the first robot and use its motion as a constraint on the motion of the next robot. We can formalize this as a fiber bundle sequence $Y_{1:M}\stackrel{{\pi }_{M-1}}{{\displaystyle \to }}{Y}_{1:M-1}\stackrel{{\pi }_{M-2}}{{\displaystyle \to }}\cdots \stackrel{{\pi }_1}{{\displaystyle \to }}{Y}_1$ (9)whereby Y _m is the state space of the m-th robot and Y_1:m is the Cartesian product of the state spaces Y₁, …, Y _m . In the fiber bundle sequence, we remove, in each step, the configuration space and the geometry of the least important robot. We can then either plan a path in Y₁ and use it as a constraint for the next robot (i.e., finding a feasible section in the path restriction). This is known as path coordination (Siméon et al., 2002). Or we use the graph on Y₁ to restrict sampling for the remaining robots, which is known as graph coordination (Svestka and Overmars, 1998). In practice, we can realize graph coordination either by using an oracle to guide expansion (Solovey et al., 2016) or by expanding dimensionality when conflicts arise (Wagner and Choset, 2015).

4.6.2. Tangent bundle and path-velocity decomposition

When planning for a dynamical system, we often can simplify planning using a tangent bundle decomposition. Given a state space X we impose a tangent bundle $X=TM=M\times {\mathbb{R}}^n$ with projection $M\times {\mathbb{R}}^n\stackrel{\pi }{{\displaystyle \to }}M$ (10)whereby n = dim M, ${\mathbb{R}}^n$ is the fiber space and M is the base space. We call M the configuration space and TM the tangent bundle. Planning on tangent bundles often follows a two-step approach. First, we compute a path p _M on the configuration space M (the base space) avoiding obstacles and self-collisions. Second, we compute a velocity along the path, that is, a time reparameterization. Such a time reparameterization is a path section of p _M and we can find such a section by solving a convex optimization problem (Bobrow et al., 1985), which we can solve efficiently (Pham and Pham, 2018). To guarantee completeness, however, we need to either plan on the full tangent bundle TM (LaValle and Kuffner Jr, 2001) or track valid speed profiles along paths on M (Pham et al., 2017).

5. Primitive methods on fiber bundles

To exploit fiber bundles, we derive a set of primitive methods. This includes methods to sample the base space, to compute a metric, to select a bundle space to grow next, and to rapidly find a feasible section. To implement each method, we develop several different strategies and discuss how they influence the algorithms. To select the best strategies for each algorithm, we perform a meta-analysis in Appendix C.

To use the primitive methods, we assume that every bundle space X _k has access to the following fiber bundle specific functions:

1. A fiber space F _k = X _k /X_k−1

2. A base space projection π _k : X _k → X_k−1

3. A fiber space projection ${\pi }_k^F:X_k\to F_k$

4. Projected start state $x_I^k$ and goal region $X_G^k$

5. A graph G _k = (V _k , E _k ) containing |V _k | vertices and |E _k | edges

The primitives will be used in the development of the bundle planners (Section 6), where we exploit primitives for improved planning performance.

5.1. Restriction sampling

In restriction sampling, we sample states on the total space X _k by sampling exclusively in the graph restriction induced by the graph on the base space X_k−1 (see Section 4.2), as we detail in Alg. 1. We first check if the base space X_k−1 exists (Line 1.1). If it does not exists, we revert to a standard sampling method like uniform sampling (Line 1.6). If it does exists, we first sample a base space element (Line 1.2), then use it to sample a fiber space element (Line 1.3) and finally lift the base space element to the bundle space using the fiber space element (Line 1.4). The lift operation depends on if the bundle is trivial, in which case we just concatenate base element and fiber element. If the bundle is non-trivial (like the Mobius strip), we use the base element to index the correct fiber space, then use the fiber element to index the correct bundle space element (see Section 4.3).

To implement the Sample function, we use uniform sampling of the space. However, other sampling techniques are certainly possible, like Gaussian sampling (Boor et al., 1999), obstacle-based sampling (Amato et al., 1998), bridge sampling (Hsu et al., 2003), maximum clearance (Wilmarth et al., 1999), quasi-random (Branicky et al., 2001), utility-based (Burns and Brock, 2005), or deterministic sampling (Janson et al., 2018; Palmieri et al., 2019). To guarantee probabilistic completeness and asymptotic optimality, we only need to verify that those sequences are dense.

The main method of restriction sampling is the SampleBase method. In the SampleBase method, we sample the graph G_k−1 on the base space. While numerous methods exist to sample a graph (Leskovec and Faloutsos, 2006), we found five methods particularly important.

5.1.1. Random vertex sampling

First, we can choose a vertex at random, which we refer to as Random Vertex (RV) sampling (Leskovec and Faloutsos, 2006). In RV sampling, we choose a random integer between 1 and |V| which uniquely defines a vertex on the graph G. This sampling is particularly fast (O(1) operations), but might be overly constrictive if we sample from a tree or a graph with long edges. However, for large graphs, this sampling procedure is often the only alternative to not slow down sampling.

5.1.2. Random edge sampling

Second, we can choose an edge at random, then pick a state on this edge, a method we refer to as Random Edge (RE) sampling (Leskovec and Faloutsos, 2006). This method requires two operations, first to pick an edge, then to pick a number between 0 and 1 to determine the state on the edge. This method seems to be superior if the graph is sparse and has long edges, in particular edges going through narrow passages.

5.1.3. Random degree vertex sampling

Third, we can choose a vertex at random, but biased towards vertices with a low degree (number of outgoing edges). We refer to this as Random Degree-Vertex (RDV) sampling. With RDV sampling, we bias samples to vertices which are either in tight corners or inside of narrow passages. Vertices in large open passages often have many neighbors and thereby a large degree. This method, however, requires to update a probability function which tracks the degrees of each vertex.

5.1.4. Path restriction sampling

Fourth, we can choose a sample on the lowest cost path on the graph, a method we refer to as path restriction (PR) sampling. We can utilize PR sampling in two ways. Either, we sample on the path restriction with a fixed probability β_fixed. This is similar to the fixed tunnel radius proposed by Reid et al. (2019). Or, we first sample exclusively on the path restriction, then gradually decay towards the fixed path bias. We call this method PR decay sampling.

PR decay sampling allows us to model a change in belief. It is often true that the shortest path on the base space contains a feasible section, which we should search for by exclusively sampling on the path restriction (Orthey et al., 2018). If we do not find a valid section, we should gradually dismiss our belief that a section exists and try to sample the graph restriction instead. To model this change in belief, we use an exponential decay function to smoothly transition from probability 1 down to the fixed probability β_fixed using a decay constant λ. See Appendix B for the definition of exponential decay.

Before using PR decay sampling, we simplify the path. Simplifying the path is similar to the local path refinement method (Zhang et al., 2009), where a path is optimized to increase its clearance. For this operation, we use a simple short-cutting path optimizer, which does not slow down planning in high-dimensional spaces.

We use a path optimizer with a cost term for path length.

5.1.5. Neighborhood sampling

Fifth, we can choose a sample not directly on the graph, but in an epsilon neighborhood. We refer to this as neighborhood (NBH) sampling. NBH sampling is helpful when there is a path through a narrow passage which comes close to its boundary. Those paths often do not have a feasible section. Instead, if we would perturbate the path slightly, we can often find a path admitting a feasible section. With NBH sampling, we first sample a configuration x exactly on the graph, and then sample a second configuration x′ which we sample uniformly in an epsilon ball around x. In practice, we use an exponential decay (Appendix B) to smoothly vary the size of the neighborhood from zero up to epsilon. With NBH sampling, we can often solve problems where a solution through a narrow passage has few or no samples, while using nearby samples allows us a bit more wriggle room. Note that instead of uniform epsilon sampling, we could also use a Gaussian distribution with mean x and epsilon variance (Reid et al., 2019). However, in preliminary testing, we could not observe a difference between them.

5.2. Bundle space metric

An essential component of bundle algorithms are the nearest neighbor computations, which depend on choosing a good metric function. We discuss two possible metrics, the intrinsic bundle metric (ignoring the base space) and the quotient space metric (exploiting the base space).

5.2.1. Intrinsic bundle metric

To straightforwardly attach a metric to the bundle space, we use the geodesic distance between two points while ignoring the base space. We compute this intrinsic metric on X as $d\left(x_1,x_2\right)=d_X\left(x_1,x_2\right)$ (11)

While this is a naive way to compute the metric, we note that using base space information is often costly, and the total space metric is often good enough (Orthey and Toussaint, 2019).

5.2.2. Quotient space metric

If a base space is available, we can consider it as a quotient space, on which we can define a quotient space metric (Guo et al., 2019). To define a quotient space metric between two states, we first project both states onto the base space, compute a shortest path p _B using the base graph and then interpolate an L² section along the path restriction ${X|}_{p_B}$ (see Section 4.3).

In particular, given two points x₁ and x₂ in X _k , we project them onto the base space X_k−1 to yield b₁ = π(x₁) and b₂ = π(x₂). We then compute the nearest vertices v₁ and v₂ on the graph G_k−1 and we compute a path on G_k−1 between v₁ and v₂ using the A* algorithm with the intrinsic base space metric as an admissible heuristic. Finally, we use the fiber space projection of x₁ and x₂ to compute fiber space elements f₁ = π _F (x₁) and f₂ = π _F (x₂), which we use to integrate an L² section (Section 4.3). We then compute the bundle space metric as $d\left(x_1,x_2\right)=\{\begin{array}{ll}{d}_X\left(x_1,x_2\right)& v_1=v_2\\ \begin{array}{l}{d}_F\left(f_1,f_2\right)\\ +d_B\left(y_1,v_1\right)\\ +d_B\left(y_2,v_2\right)\\ +d_{{\mathit{G}}_k}\left(v_1,v_2\right)\end{array}& \text{otherwise}\end{array}$ (12)with d _F being the fiber space metric (L²), d _B the base space metric and $d_{{\mathit{G}}_k}$ the length of the shortest path on G _k between vertices under the base space metric.

While the quotient space metric is more mathematically sound, there are two practical problems. First, computing this metric is costly, because we need to perform a graph search operation. Second, the graph on the base space might not yet be dense, thereby potentially returning values leading to an inadmissible heuristic, which in turn would mislead the planner on the bundle space.

5.3. Bundle space importance

In each iteration of multilevel motion planning, we make a choice about expanding a graph by selecting a level. To select a level, we attach an importance function to each bundle space, which we use to rank the bundle spaces. We develop three different importance strategies.

5.3.1. Uniform

In uniform importance, we select all bundle spaces an equal amount of times. This is similar to round-robin change, similar to scheduling operations (Russell and Norvig, 2002). Here we use a slight variation, where we compute the importance based on the number of vertices, thereby ensuring a uniform expansion of each level. In particular, for bundle space X _k with graph G _k and |V _k | vertices, we compute its importance as 1/|V _k | + 1.

5.3.2. Exponential

To densely cover spaces with higher dimensions, we usually require more samples. In general, the sampling density is proportional to N^1/d where N is the number of samples and d the dimensionality (Hastie et al., 2009). Therefore, we should select the space with the lowest density first, thereby guaranteeing equal sampling density across all spaces. We can compute an exponential importance as 1/|V _k |^1/d + 1 which reflects an exponential increase of samples in higher dimensions. This idea is similar to the selection of bundle spaces using a geometric progression (Xanthidis et al., 2018). This is also related to multilevel monte carlo (Giles, 2015) and sparse grid methods (Bungartz and Griebel, 2004).

5.3.3. Epsilon greedy

Whenever we find a graph connecting initial and goal state on the base space, it seems reasonable to greedily exploit this graph to find a path on the bundle space. This strategy is not complete, since the graph might not yet contain a feasible section (see Section 4.2). We can, however, create a complete algorithm by extending the base space with an epsilon probability while extending the bundle space the rest of the time. We compute this as $f\left(k\right)=\{\begin{array}{ll}{ϵ}^{K-k}-{ϵ}^{K-k+1}& k>1\\ {ϵ}^{K-1}& \text{otherwise}\end{array}$ (13)whereby k is the bundle space level and K is the total number of bundle spaces. We then compute the importance for the k-th bundle space as 1/|V _k |/f(k) + 1, reflecting our desire to expand recent levels more aggressively.

5.4. Finding path sections

Finding path sections quickly and reliably is one of the cornerstones of all bundle planners. In this section, we use the interpolation methods of Section 4.3 to develop a recursive path section algorithm, which we depict in Alg. 2. For this to work, we need to have at least a base space (Line 2.1). We then compute the shortest path on the base space (Line 2.2) and recursively compute a section, either by starting from an L1 fiber first section (Line 2.3) or if unsuccessful, by starting from an L1 fiber last section (Line 2.5).

To recursively compute a section, we show the pseudocode in Alg. 3. We terminate the algorithm if we reach a certain depth d_MAX (Line 3.3) or if we reach the goal region (Line 3.8). Inside each recursion iteration, we interpolate an L1 section, either fiber first (if FF is true) or fiber last (if FF is false) (Line 3.6). We then propagate the system along the section while valid (Line 3.7) and return the last valid state.

If we do not reach the goal state with the last valid state, we do up to b_MAX sidesteps along the fiber space. Sidestepping means that we project the last valid state onto the base space (Line 3.11), then sample a random fiber space element (Line 3.13) and lift the states to the bundle space to obtain a state x _k (Line 3.14). We then check if we can move from the last valid state to the state x _k (Line 3.15). Since both states have the same base space projection, we call this a sidestep (i.e., a step orthogonal to the base space). If the motion is valid, we clip the remaining base path (Line 3.17) and recursively call the algorithm (Line 3.18). In the recursion call, we increase the depth, use the clipped base path segment and change the interpolation method from fiber first to fiber last. We change the interpolation at this point, because we observe an alternation between interpolation methods to substantially improve runtime.

5.4.1. Nonholonomic constraints

In the case of holonomic constraints, we can use the L¹ interpolation (Line 3.6) and the base space segment (Line 3.17) to follow the path restriction exactly. However, if we have nonholonomic constraints, we often cannot follow the path restriction exactly, in particular if the base space path is piece-wise linear. Note that a base space path is often piece-wise linear if we do not impose additional smoothness assumptions (Vidal et al., 2019; Hönig et al., 2018).

To still compute path sections over piece-wise linear base space paths in the nonholonomic case, we do a two-phase approach. First, we compute the interpolation values as in Section 4.3, but only at discrete points, which provides us with a set of points on the bundle space. Second, we interpolate between those points by using the nonholonomic steering function. While we might deviate from the base path restriction, we follow, however, the base path restriction as close as the steering function allows us. This approach is similar to the idea of interpolating waypoints with dynamically feasible path segments, which has been done for flying quadrotors (Richter et al., 2016) and for underwater vehicles (Yu et al., 2019). However, we differ by first interpolating values for the fiber spaces along the base space path. The remaining computation in Alg. 3 remains exactly as in the holonomic case.

6. Bundle space motion planners

To solve a multilevel motion planning problem, we develop a set of algorithms generalizing existing motion planners to fiber bundles. All those planners share the same high-level structure, which we call a BundlePlanner (Alg. 4). In the BundlePlanner method, we first initialize a priority queue sorted by the importance of each bundle space (Line 4.1). We then iterate over all bundle spaces, try to find a section on the k-th bundle space (Line 4.3) and then push the k-th bundle space into the priority queue (Line 4.4). We then execute the while loop while a planner terminate condition (PTC) is not fulfilled for the k-th bundle space (Line 4.5). Inside the loop, we select the most important bundle space, grow the graph or tree and push the space back into the queue (Line 4.6 to 4.8). We terminate if the PTC for the K-th bundle space has been fulfilled. This means we either terminate successfully, found the problem to be infeasible or reach a timelimit.

All bundle space algorithms are alike in sharing the same high-level structure; each bundle space algorithm differs in their Grow function (Line 4.7) and their primitive methods (Section 5).

6.1. Bundle planner variants

The BundlePlanner algorithm is used to develop novel algorithms by changing the Grow function. To implement the Grow function, we can utilize almost any single-level planning algorithm. In our case, we use the algorithms RRT, RRT*, PRM, and PRM* (please consult Table 1 for abbreviations of algorithms).

Table 1.

List of Motion planning Algorithms Used in Experimental Section. Properties of the Algorithms are: Supporting Fiber Bundles (FB), Being Probabilistically Complete (PC), and Being Asymptotically (Near-)Optimal (AnO).

Motion planner	Description	Origin paper	FB	PC	AnO
QRRT	Rapidly-exploring random quotient space trees	(Orthey and Toussaint 2019)	x	x
QMP	Quotient space roadmap planner	(Orthey et al. 2018)	x	x
QRRT*	Optimal version of QRRT	This paper	x	x	x
QMP*	Optimal version of QMP	This paper	x	x	x
PRM	Probabilistic roadmap planner	(Kavraki et al. 1996)		x
PRM*	Optimal version of PRM	(Karaman and Frazzoli 2011)		x
LazyPRM*	Optimal version of LazyPRM	(Karaman and Frazzoli 2011)		x
SPARS	Sparse roadmap spanners	(Dobson and Bekris 2014)		x	x
SPARS2	SPARS without dense graph	(Dobson and Bekris 2014)		x	x
RRT	Rapidly-exploring random tree	(Lavalle 1998)		x
RRTConnect	Bidirectional RRT	(Kuffner and LaValle 2000)		x
RRT*	Optimal version of RRT	(Karaman and Frazzoli 2011)		x
LazyRRT	Lazy edge evaluation RRT	(Kuffner and LaValle 2000)		x
TRRT	Transition-based RRT	(Jaillet et al. 2010)		x
BiTRRT	Bidirectional TRRT	(Jaillet et al. 2010)		x
LBTRRT	Lower bound tree RRT	(Salzman and Halperin 2016)		x	x
RRTX	RRT with pseudo-optimal tree	(Otte and Frazzoli 2016)		x	x
RRT#	RRT sharp	(Arslan and Tsiotras 2013)		x	x
InformedRRT*	Informed search RRT*	(Gammell et al. 2014)		x	x
SORRT*	Sorted InformedRRT*	(Gammell et al. 2014)		x	x
SBL	Single-query bidirectional lazy PRM	(Sánchez and Latombe 2003b)		x
SST	Stable sparse RRT	(Li et al. 2016)		x	x
STRIDE	Search tree with resolution independent density estimation	(Gipson et al. 2013)		x
FMT	Fast marching tree	(Janson et al. 2015)		x	x
BFMT	Bidirectional FMT	(Janson et al. 2015)		x	x
BIT*	Batch informed trees	(Gammell et al. 2020)		x	x
ABIT*	Advanced BIT*	(Strub and Gammell 2020)		x	x
EST	Expansive spaces planner	(Hsu et al. 1999)		x
BiEST	Bidirectional EST	(Hsu et al. 1999)		x
ProjEST	Projection EST	(Hsu et al. 1999)		x
KPIECE	Kinodynamic motion planning By interior-exterior cell exploration	(Şucan and Kavraki 2009)		x
BKPIECE	Bidirectional KPIECE	(Şucan and Kavraki 2009)		x
LBKPIECE	Lazy BKPIECE	(Şucan and Kavraki 2009)		x
PDST	Path-directed subdivision tree	(Ladd and Kavraki 2004)		x

All grow functions in a multilevel versions of our algorithms differ from their single-level version in four points. First, we replace uniform sampling by restriction sampling, as we detail in Section 5.1. Algorithms might differ in how we implement graph sampling in restriction sampling. Second, when pushing a new bundle space into the priority queue, we check for a feasible section over the solution path on the last bundle space, as we detail in Section 5.4. This computation is equivalent for each bundle planner. Third, we rank bundle spaces based on a selection criterion, which we detail in Section 5.3. Algorithms might differ in the type of selection criterion we employ. Fourth, we adjust the metric on the bundle space, which affects both nearest neighbors computation and the steering method, as we detail in Section 5.2. While different metrics are possible (Orthey et al., 2018), we use the intrinsic bundle metric for all algorithms (as determined by our meta-analysis in Appendix C).

6.2. QRRT

In Alg. 5, we show the QRRT algorithm. We previously introduced QRRT in Orthey and Toussaint (2019). We differ here by using an exponential importance primitive (Section 5.3.2) and by adding the find section primitive (Section 5.4). The remaining structure, however, remains unchanged. In detail, we sample a random element from the bundle space (Line 5.1) using restriction sampling (Section 5.1). We then choose the nearest vertex from the tree (Line 5.2) and steer from the nearest to the random element (Line 5.3). We then check if the motion is collision-free and add the new state to the tree. Note that we stop steering if the distance goes above a threshold, similar to RRT (LaValle and Kuffner Jr, 2001).

6.3. QRRT*

While QRRT performs well in our evaluations, we can improve upon QRRT by developing an asymptotic optimal version. We call this QRRT* and depict the algorithm in Alg. 6. By developing QRRT*, we generalize RRT* (Karaman and Frazzoli, 2011) to multiple levels of abstraction. To implement QRRT*, we use one iteration of QRRT (Line 6.1), then compute k nearest neighbors of the new state (Line 6.2). We choose the k as k = k_RRT log(N) whereby N is the number of vertices in the tree (Karaman and Frazzoli, 2011). The parameter k_RRT can be chosen based on the dimension of the problem (Karaman and Frazzoli, 2011; Kleinbort et al., 2019).

After computing k nearest neighbors, we perform two rewire operations (this dicussion follows closely Salzman and Halperin (2016)). First, we rewire the nearest neighbors to the new state (Line 5.4). Second, we rewire the new state to the nearest neighbors (Line 5.7). We show the rewire operation in Alg. 7. Inside the rewire algorithm, we update the incoming edge of state y by checking if the cost to come from state x (cost from initial state to x) plus the cost to go from x to y is smaller than the cost to come for state y. In that case, we update the graph by removing all incoming edges into y and adding a directed edge from x to y. Contrary to similar implementations (Karaman and Frazzoli, 2011; Salzman and Halperin, 2016), we also update the tree G _k such that we can use the same restriction sampling method for each algorithm. While the grow method is similar to the RRT* method (Salzman and Halperin, 2016), we note that much of the complexity is encapsulated in the primitive methods (Section 5), which we use to sample, to compute distances, to find sections and to choose a bundle space to grow next.

6.4. QMP

In Alg. 8, we show the QMP algorithm, which we introduced in Orthey et al. (2018). In the QMP algorithm, we differ from QRRT by not growing a tree, but a graph (Kavraki et al., 1996). QMP generalizes PRM in the sense that QMP becomes equivalent to PRM when we choose a single-level abstraction. The algorithm QMP as presented here differs slightly from its original conception (Orthey et al., 2018) in three points. First, we use the epsilon greedy importance (Section 5.3.2) instead of uniform importance to select a bundle space to expand. Second, we use the intrinsic bundle metric (Section 5.2) instead of the quotient space metric, which we found to not scale well to high-dimensional state spaces (see Appendix C). Third, we use the FindSection method to quickly check for sections (Section 5.4).

6.5. QMP*

QMP* is similar as QMP, but we use a different k in each iteration to choose the nearest neighbors. This k is chosen such that the resulting algorithm is almost-surely asymptotically optimal (Karaman and Frazzoli, 2011). In general we use k = k_PRM log(N) with N being the number of vertices in the graph. See also Solovey and Kleinbort (2020) for recent developments on choosing the parameter k_PRM.

6.6. Open source implementation

To make the algorithms freely available, we provide implementations in C/C++, which we split into two frameworks. The first framework is a graphical user interface (GUI) where users can specify fiber bundles by providing URDF (Unified Robotic Description Format) files for each level and specify the bundle structure in an XML (Extensible Markup Language) file. We then provide functionalities to step through each level and to visualize the lowest cost path on each level. The code is freely available on github. ¹

The second framework is the actual implementation of fiber bundles, bundle algorithms, and primitives, which we implement as a submodule of the Open Motion Planning Library (OMPL) (Şucan and Kavraki, 2009). In particular, we encapsulate our code as an OMPL planner class, which we can use for benchmarking (Moll et al., 2015) or analysis. This code is part of OMPL version 1.6.0 and includes a high-level introduction, a tutorial, and additional demos. ²

7. Analysis of bundle planners

Let $X_K\stackrel{{\pi }_{K-1}}{{\displaystyle \to }}\dots \stackrel{{\pi }_1}{{\displaystyle \to }}{X}_1$ be a fiber bundle sequence. We like to prove that, on this fiber bundle sequence, the algorithms QRRT, QRRT*, QMP, and QMP* are probabilistically complete (PC) and that QRRT* and QMP* are asymptotically optimal (AO).

To prove those properties, we use two methods. First, we state three assumptions on the importance function and the datastructures, which we use to establish that restriction sampling is dense. Second, we argue that the bundle algorithms, when using restriction sampling, inherit the PC and AO properties from their single-level counterpart.

7.1. Assumptions

We require three assumptions to hold true.

1. The importance function of each bundle space (Section 5.3) monotonically converges to zero (we select every bundle space infinitely many times)

2. Restriction sampling is dense in X₁

3. If restriction sampling is dense, the graph on the k-th bundle space is space-filling in the connected initial component

whereby the connected initial component is the set of points in X _k which are path-connected3 to π _k (x _I ), that is, to the projection of the initial state onto the k-th bundle space. A graph is said to be space-filling in a set U, if for any x in U there exists a path in the graph starting at x _I and converges to x (Kuffner and LaValle 2011) (in the limit when running time goes to infinity).

7.2. Proof that restriction sampling is dense

When stripping down to the essentials, we observe that the bundle planners differ on the last level from non-multilevel planners by replacing uniform sampling with restriction sampling. While uniform sampling is dense in the complete state space, restriction sampling differs, in that we can prove it to be dense in the connected initial component.

To prove denseness, we need some notations. First, a set U is dense in X if the intersection of U with any non-empty open subset V of X is non-empty (Munkres, 2000). We abbreviate this by saying that a set is dense if its closure cl(U), the smallest closed set containing U, contains the space X. When using a sequence of samples α₁, α₂, …, we can interpret the sequence as a set $A=\left\{{\alpha }_i\mid i\in \mathbb{N}\right\}$ . We can then say that the sequence is dense in the space X if the closure cl(A) contains X (or is equal to).

Let I _k be the connected initial component(on the bundle space k) and let A _k be a restriction sampling sequence. To prove A _k to be dense in I _k , we choose an arbitrary set U in I _k . We then prove that there will be a non-empty intersection of U with A _k , that is, given enough time, we will at least sample once from U. Our proof is inductive, that is, we prove it to be true for k = 1, then use this to inductively argue for arbitrary k.

In a preliminary version of the proof (Orthey and Toussaint, 2019), we showed restriction sampling to be dense in the free state space, which is true only if there is a single connected component. To make the proof more general, we replace the free state space here with the connected initial component.

Theorem 1

A _k is dense in I _k for k ≥ 1.

Proof

By induction for k = 1, A₁ is dense in X₁ by assumption and therefore dense in I₁ since I₁ ⊆ X₁. For the induction step, we can assume A_k−1 to be dense in I_k−1. Let U be a non-empty open subset of I _k . Since U is open, π_k−1(U) is open (by property of fiber bundle). By induction assumption there exists a y in A_k−1 ∩ π_k−1(U). Consider an open set V of the preimage ${\pi }_{k-1}^{-1}\left(y\right)$ . Since A _k is dense in ${\pi }_{k-1}^{-1}\left(A_{k-1}\right)$ (by definition of restriction sampling), there exists an x in A _k ∩ V which is a subset of U. Since U was arbitrary, A _k is dense in I _k . □

Due to Theorem 1, we observe that restriction sampling differs from uniform sampling by removing states which cannot be feasible. Therefore, algorithms using restriction sampling maintain all their properties, which we can inherit.

7.3. Inheritance of probabilistic completeness

A motion planning algorithm is probabilistically complete, if the probability that the algorithm will find a path (if one exists) goes to one as time goes to infinity. This property has been proven for sampling-based planners, in the case of a graph (Svestka, 1996) including the case of a tree (Kuffner and LaValle, 2000).

Probabilistic completeness follows in our case directly from the assumptions and our proof that restriction sampling is dense. In particular, let us assume a given motion planning problem to be feasible and containing a solution in the interior of the free space. Since restriction sampling is dense, by assumption, we have a space-filling graph containing a path starting at the initial state and converging to the goal state.

In the grow functions of QRRT, QRRT*, QMP, and QMP*, we directly implement the corresponding versions of RRT, RRT*, PRM, and PRM*, which all have been shown to be probabilistically complete (see corresponding papers listed in Tab. 1). They therefore necessarily need to construct a space-filling graph (tree) (Kuffner and LaValle, 2011) and all bundle space planners, when using restriction sampling, inherit the probabilistic completeness property.

7.4. Inheritance of asymptotical optimality

An algorithm is (almost-surely) asymptotically (near-)optimal (AnO) (Karaman and Frazzoli, 2011; Salzman and Halperin, 2016) if it converges to a cost at most (1 + ϵ) times the cost of the optimal path. An algorithm is (almost-surely) asymptotically optimal if it is AnO with ϵ = 0.

Similar to probabilistic completeness, we argue that QRRT* and QMP* are asymptotically optimal, since this property is inherited from RRT* and PRM* (Karaman and Frazzoli, 2011), respectively. This is true, since on the last level, we only change the sampling function from uniform to restriction sampling. Since we showed restriction sampling to be dense and we will select the last bundle space infinitely many times, we can be sure that the optimality properties are kept intact. Note that this line of reasoning is slightly different from the proof of asymptotic optimality for HBFMT (Reid et al., 2020), where Reid et al., (2020) define a probability l with which they switch to use uniform sampling, thereby guaranteeing optimality by actually reverting to BFMT. We, however, rely on the denseness property of restriction sampling, thereby avoiding an uniform extension step.

Detailed proofs of asymptotic optimality for sampling-based planner can be found in Karaman and Frazzoli (2011). See also Salzman and Halperin (2016) and Solovey and Kleinbort (2020) for a treatise of asymptotic near-optimality.

8. Evaluation

To show the wide applicability of fiber bundles and bundle algorithms, we apply them to a broad range of planning scenarios. In particular, we evaluate our algorithms on four low-dimensional and eight high-dimensional planning scenarios, including computer animation, pregrasping, multi-robot coordination, and nonholonomic constraints. The dimensionality of the state spaces in the high-dimensional case ranges from 21-dof (box folding) to 100-dof (hypercube). We compare our algorithms with available algorithms implemented in the Open Motion Planning Library (OMPL) as of May 2020 (Moll et al., 2015). References and details of those algorithms are shown in Table 1. All algorithms, except QMP, QMP*, QRRT, and QRRT*, do not use the additional information which fiber bundles provide. We like to show that fiber bundles are helpful to solve scenarios which are near unsolvable using classical sampling-based methods Kavraki et al. (1996); Kuffner and LaValle (2000)

Evaluation Metrics: For all scenarios, we let each algorithm run 10 times with a cut-off time limit of 60s. For the low-dimensional scenarios, we report a success-cost plot showing convergence rate and success rate over time. In this case, we let the algorithms run for 60s and query their current best cost with a 100Hz update frequency (i.e., every 0.01s). For the high-dimensional scenarios, we run two separate evaluations. First, a pure runtime evaluation, where we compare the average runtime on each scenario, comparing against all available OMPL planners. In this case, planners run until they either find a solution or the cut-off time limit has been reached. Second, we report on a success-cost plot for our algorithms against four well performing algorithms from OMPL, namely, RRTConnect, RRT*, BIT*, and LBTRRT. In this case, all algorithms are run for 60s with best cost queries at 100 Hz update frequency.

Hardware: Concerning hardware, we use a 4-core laptop running Ubuntu 18.04 with 20GB of RAM to run the runtime evaluation on the high-dimensional planning problems. For the low-dimensional planning problems and the cost function evaluation on the high-dimensional problems, we use a 4-core laptop running Ubuntu 16.04 with 8GB of RAM. Concerning parameters, our algorithms are set as follows. For the FindSection method, we use d_MAX = 3 and b_MAX = 10. For path restriction sampling, we use the decay constant λ = 10⁻³ and fixed probability β_fixed = 0.1. For QRRT, we use a maximal distance range of 0.2μ whereby μ is the measure of the space (same value as in RRT or RRTConnect). For QMP, we use k = 10 to compute nearest neighbors (same as in PRM). For QMP*, we use the optimal number of nearest neighbors in each iteration as in PRM* (Solovey and Kleinbort, 2020). The choice of primitive methods has been independently optimized using a meta-analysis (See Appendix C). We set any other parameters to be equivalent to the corresponding single-level planner.

State Spaces: In all scenarios, the state spaces of the robots are modeled using the following mathematical spaces. For rigid bodies, we use SE(2) and SE(3), the special Euclidean group in two and three dimensions, respectively (Selig, 2004). The spaces in those groups model all rotations and translations applicable to a rigid body in two or three dimensions. For rotating joints, we use SO(2) and SO(3), the special orthogonal group. The spaces in those groups model all rotations about a fixed point of the robot. For all other kinematic chains with rotational joints and joint limits, we use the Euclidean space ${\mathbb{R}}^n$ of n dimensions.

8.1. Low-dimensional motion planning

In the low-dimensional motion planning evaluation, we evaluate QMP, QMP*, QRRT, and QRRT* against RRTConnect, RRT*, BIT*, and LBTRRT. This is done on four low-dimensional planning problems as shown in Figure 6. We let all planners run until time out and collect both time to find the first solution and solution cost over time.

8.1.1. 2-dof disk problem (2 levels)

The first scenario is a 2-dof disk problem, where a small disk robot needs to traverse a square with a narrow passage in the middle. For our bundle algorithms, we use a projection onto a smaller inscribed disk with half the radius of the original disk (see Figure 6). This creates a fiber bundle as ${\mathbb{R}}^2\to {\mathbb{R}}^2$ (14)

The evaluation results are shown in Figure 5 (Upper left). RRTConnect performs best in terms of quickest convergence to one hundred percent success rate, while BIT* performs best by converging the fastest to the optimal solution. All bundle planners can successfully solve this problem with competitive results both in terms of success rate (QRRT, QMP), and in terms of cost convergence (QRRT*, QMP*).

Figure 5.

Success-cost plots of the four low-dimensional planning scenarios. (a) 02D disk, (b) 06D Piano Mover’s problem, (c) 07D Planar Manipulator, and (d) 06D Double L-Shape.

8.1.2. 3-dof piano mover’s problem (2 levels)

The second scenario is the piano mover’s problem (Schwartz and Sharir, 1983), where a piano has to be moved on a planar floor from one side of a house to the other side. As shown in Figure 6, we impose a fiber bundle by inscribing a simpler shape into the original piano, thus imposing a fiber bundle as $SE\left(2\right)\to SE\left(2\right)$ (15)

Figure 6.

Four scenarios for low-dimensional planning. Start configuration of robot (green) is shown alongside goal configuration (red) when applicable. In each figure, the robot is shown on the original space (left), and with the first projection applied (right), where the original robot is shown with a transparent color.

The evaluation results are shown in Figure 5 (Upper right). BIT* and RRTConnect outperform in terms of success rate, while BIT* also converges quickest to a low-cost solution. All bundle planners perform slightly worse, but still competitive in terms of runtime and cost convergence.

8.1.3. 7-dof planar manipulator (4 levels)

In the third scenario, we evaluate the planners on a 7-dof planar manipulator task, as shown in Figure 6 (Lower left). For this scenario, we impose four levels of abstractions, where we first project the 7-dof robot onto a 4-dof robot by removing the last three links. We then project onto a 2-dof robot by removing two links and finally we project onto a 1-dof robot by removing one link. The resulting fiber bundle can be written as $\begin{array}{l}SO\left(2\right)\times {\mathbb{R}}^6\to SO\left(2\right)\times {\mathbb{R}}^3\\ \to SO\left(2\right)\times {\mathbb{R}}^1\to SO\left(2\right).\end{array}$ (16)

The evaluation results in Figure 5 (Lower left) show that RRTConnect and LBTRRT perform best in terms of success rate, while LBTRRT converges quickest in terms of solution cost. Both QMP and QMP* perform competitively in terms of success rate and QMP* terms of cost convergence. QRRT has slightly worse performance in terms of success rate, but still solves the problem. QRRT*, however, does not solve all runs of this problem.

8.1.4. 6-dof drone (2 levels)

In the fourth scenario, a drone has to traverse two trees to reach a goal state. The state space is SE(3) with additional constraints on roll and pitch, but not yaw (to prevent impossible maneuvers). We impose a fiber bundle as $SE\left(3\right)\to {\mathbb{R}}^3$ (17)

by inscribing a small sphere inside the drone, thereby reducing the state space to ${\mathbb{R}}^3$ . The evaluation results in Figure 5 (Lower right) show RRTConnect to converge quickest in success rate, with LBTRRT being fastest in cost convergence. All bundle planners solve this problem, but QMP* returns a slightly larger final cost compared to the lowest cost found.

Figure 7.

Hypercube scenario comparison of algorithms STRIDE and QRRT.

8.2. High-dimensional motion planning

For the high-dimensional planning scenarios, we conduct two evaluations. First, we run a large set of planners from OMPL until a first solution is found (or a timeout occurs) and report on the runtime. Those results are evaluated for all available planners in OMPL, if they are applicable to the problem at hand. This case is discussed in Section 8.2.1 up to Section 8.2.8. Second, we run the eight planners QMP, QMP*, QRRT, QRRT*, RRTConnect, RRT*, BIT*, and LBTRRT on each scenario until the timeout occurs. We collect both success rate and cost over time and plot those results as success-cost graphs. This case is discussed in Section 8.3. Note that each case uses a different hardware setup as mentioned in Section 8.

8.2.1. 100-dof hypercube (98 levels)

The hypercube (Gipson et al., 2013) is a classical motion planning benchmark, where we need to move a point robot in an n-dimensional cube X = [0,1] ⁿ from x _I = (0, …, 0) to x _G = (1, …, 1). We allow the robot to move only along corridors of size ϵ = 0.1 along the edges of the cube as shown in Figure 8(a). For more details see Gipson et al. (2013). As a fiber bundle, we choose the sequence of reductions ${\left[\mathrm{0,1}\right]}^n\to {\left[\mathrm{0,1}\right]}^{n-1}\to \dots \to {\left[\mathrm{0,1}\right]}^2$ (18)where the constraint function is the constraint function of the corresponding cube.

Figure 8.

Runtime benchmarks on the first four high-dimensional planning scenarios. (a) 37-dof pre-grasp, (b) Benchmark, (c) 48-dof drones, (d) Benchmark, (e) 54-dof kraken animation, (f) Benchmark, (g) 72-dof manipulators, and (h) Benchmark.

Prior work showed solutions to 25-dimensional cubes in around 100s (Gipson et al., 2013). Here, we attempt to solve a 100-dimensional cube version. The benchmarks are shown in Figure 8(b). All bundle planners have an average runtime of less than 0.1s. Also the non-bundle planner SPARS2 terminates with a runtime of around 0.2s. However, we note that SPARS2 terminates with a probabilistic infeasibility proof, that is, they declare this problem infeasible. Only QRRT, QMP and their star versions can solve this problem in the time limit given. While we terminate all planner at 60s, we can provide a rough estimate of performance improvement of QRRT compared to STRIDE (which outperforms PRM, KPIECE, EST, and RRT (Gipson et al., 2013)). To do that, we let STRIDE run on the n = {3, …, 9} dimensional version of the cube, then we extrapolate the results by fitting a cubic curve (see Figure 7). Comparing the extrapolation to QRRT at the dimension 100, we observe that QRRT performs around six orders of magnitude better than STRIDE.

8.2.2. 21-dof box folding (5 levels)

To automate deliveries or assemble production pieces, we often need to compute folding motions. Here we concentrate on computing the folding motion of a small packaging box with 21-dof (Figure 8(c)). Such problems are challenging, because parts of the box have to fit into small narrow passages, which is challenging for sampling-based planners. We use a fiber bundle sequence as $\begin{array}{ll}SE\left(2\right)\times {\mathbb{R}}^{18}& \to SE\left(2\right)\times {\mathbb{R}}^{16}\to SE\left(2\right)\times {\mathbb{R}}^{13}\\ & \to SE\left(2\right)\times {\mathbb{R}}^{10}\to SE\left(2\right)\times {\mathbb{R}}^7\\ & \to SE\left(2\right)\end{array}$ (19)which corresponds to the removal of (1) flaps on lid, (2) lid, (3) right side, (4) left side, (5) back/front elements. We show the benchmarks in Figure 8(d). The best performing algorithm is QMP with 0.68s of planning time. QRRT performs worse with around 6.4s. We discuss this performance difference in Section 9. Both QMP and QRRT together with QMP* outperform all other planning algorithms, that is, no OMPL algorithm was able to solve this scenario in our timelimit.

8.2.3. 24-dof Dubins cars crossing (3 levels)

With several companies pushing towards autonomous driving, we need increasingly more efficient algorithms to coordinate multiple car-like robots under nonholonomic constraints. We concentrate here on the problem of planning motions for eight Dubins cars (Dubins, 1957), which are cars with constant forward speed, which we can steer left or right. The cars start on different ends of a crossroad (in reverse direction) and we need them to cross the road while avoiding each other (Figure 8(e)). We impose a fiber bundle as $SE{\left(2\right)}^8\to {\left({\mathbb{R}}^2\right)}^8\to {\left({\mathbb{R}}^2\right)}^4$ (20)which corresponds to the reduction onto a disk inscribed in the car and the removal of the upper four robots, respectively. We show the benchmark in Figure 8(f). QRRT performs best with a planning time of 0.28s closely followed by QRRT* (0.29s) and QMP (1.77s). QMP* performs less well with 12.41s of planning time. Except EST with planning time of around 54s, there was no non-bundle algorithm able to solve this coordination problem in the timelimit given.

8.2.4. 30-dof airport (15 levels)

While coordinating motions for multiple cars are essential for traffic coordination, we often need to coordinate multiple vehicles in 3D under nonholonomic constraints. One particular instance of this problem is an airport, in which we might need to coordinate cars, planes, and zeppelins, each with different state spaces and different possible nonholonomic constraints. Here, we use a scenario with three trucks, 1 zeppelin, 1 propeller plane, 1 airplane while taxiing ⁴ and two airplanes while flying (see Figure 8(g)). This scenario is particularly challenging, since all vehicles have nonholonomic constraints except the zeppelin. We model the dynamics of the trucks and the planes as Dubins car and Dubins airplane (LaValle, 2006), respectively. Note that arbitrary dynamically constraints could be imposed, but there are implementations of Dubins car and airplane spaces available in OMPL, which makes them also useable with other algorithms in the library. We use a prioritization-like abstraction as $\begin{array}{l}SE{\left(2\right)}^4\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ {\mathbb{R}}^2\times SE{\left(2\right)}^3\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ SE{\left(2\right)}^3\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ {\mathbb{R}}^2\times SE{\left(2\right)}^2\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ SE{\left(2\right)}^2\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ {\mathbb{R}}^2\times SE\left(2\right)\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ SE\left(2\right)\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ {\mathbb{R}}^2\times SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ SE\left(3\right)\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^3\to \\ {\mathbb{R}}^3\times {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^2\to \\ {\left({\mathbb{R}}^3\times SO\left(2\right)\right)}^2\to \\ {\mathbb{R}}^3\times \left({\mathbb{R}}^3\times SO\left(2\right)\right)\to \\ \left({\mathbb{R}}^3\times SO\left(2\right)\right)\to \\ {\mathbb{R}}^3\end{array}$ (21)where the first four SE(2) spaces represent the three trucks and the taxiing airplane. The SE(3) space represents the zeppelin and the remaining three spaces of ${\mathbb{R}}^3\times SO\left(2\right)$ represent the two flying airplanes and the propeller plane, respectively. Each projection either projects an SE(2) space by using the simpler robots of a nested disk, by removing a robot completely (and its geometry) or by nesting an inscribed sphere. The benchmarks are shown in Figure 8(h). The best performing planner are QRRT (0.52s), QMP (0.99s) and QMP* (0.94s). QRRT* performs significantly worse with a planning time of around 58s, which suggest that it could not completely solve this problem in the time allocated. Besides the bundle planner, we also observe that RRTConnect shows competitive results with 4.5s of planning time.

8.2.5. 37-dof pregrasp (3 levels)

Manipulation of objects is a challenging task for robots (Dafle et al., 2018; Driess et al., 2020), in particular if we have to deal with realistic hands with many dofs. We concentrate here on computing a pregrasp for a 37-dof shadow-hand robot mounted on a KUKA LWR robot. We define the problem as finding a pregrasp for the grasping of a small glass, as we depict in Figure 9(a).We impose a fiber bundle as ${\mathbb{R}}^{31}\to {\mathbb{R}}^{18}\to {\mathbb{R}}^{13}$ (22)which corresponds to a reduction by first removing all fingers except thumb and index finger and second removing the thumb. The benchmark for this problem is shown in Figure 9(b). Both QMP and QMP* perform best with around 6.81s and 12.36s of planning time. In this scenario, no non-bundle planner can solve this problem. Please note that the planner QRRT and QRRT* perform around 44s and 48s. We discuss this performance further in Section 9.

8.2.6. 48-dof drones (8 levels)

Planning motions for multiple quadrotors (Hönig et al., 2018) is essential for drone delivery, in disaster response scenarios and for entertainment purposes. We consider here the problem of coordinating the motion of eight drones which have to traverse a small forest-like environment as shown in Figure 9(c). We use the fiber bundle $SE{\left(3\right)}^8\to SE{\left(3\right)}^7\to \cdots \to SE\left(3\right)$ (23)which corresponds to a prioritization of the drones, that is, in each projection we remove one robot. The benchmarks are shown in Figure 9(d). While the best algorithm is QRRT (0.14s) closely followed by QMP (0.15s) and QMP* (0.16s), we observe that also RRTConnect and BFMT show competitive performances with 0.59s and 6.05s, respectively.

8.2.7. 54-dof Kraken animation (17 levels)

Computer animation is an important application of planning algorithms (Plaku et al., 2018). In animations for movies, an animator would probably insert keyframes to guide the planning of motions. However, if we like to compute animations online, for example, for a computer game, we require fast planning algorithms.

We show here the problem of animating a 54-dof Kraken-like robot (see Figure 9(e)), which has to wrap its arms around a sailing ship. We use a fiber bundle reduction as $\begin{array}{ll}SE\left(3\right)\times {\mathbb{R}}^{48}& \to SE\left(3\right)\times {\mathbb{R}}^{45}\to SE\left(3\right)\times {\mathbb{R}}^{42}\\ & \to \dots \to \\ SE\left(3\right)\times {\mathbb{R}}^6& \to SE\left(3\right)\times {\mathbb{R}}^3\to SE\left(3\right)\end{array}$ (24)which corresponds to the removal of each arm (6-dof revolute joints) on each stage, whereby we first remove the last three links (removal of 3-dof) and then remove the remaining arm (3-dof). We show the benchmark in Figure 9(f). We observe that both QRRT (0.20 s) and QMP (0.23 s) perform below 1s to find a feasible solution. Next comes QMP* with a planning time of 6.21 s. The next best non-bundle planner is BiTRRT with a performance of around 22 s planning time. The performance of the bundle planner QRRT is thus two orders of magnitude better than the next best non-bundle planner.

8.2.8. 72-dof manipulators (3 levels)

When automating construction work (Hartmann et al., 2020) or warehouse operations (Salzman and Stern, 2020; Eppner et al., 2016), we often need to coordinate multiple robots with many dofs. Here, we consider the coordination of eight KUKA manipulators on disk-shaped mobile bases. Each manipulator starts around a circle and needs to change position with its antipodal partner (see Figure 9(g)). We impose a fiber bundle as ${\left(SE\left(2\right)\times {\mathbb{R}}^6\right)}^8\to {\left({\mathbb{R}}^2\right)}^8\to {\left({\mathbb{R}}^2\right)}^4$ (25)which corresponds to the removal of arms and the removal of the upper half of the robots. The benchmarks are shown in Figure 9(h). We observe that QRRT solves this problem in 3.65 s while QRRT* requires 19 s. Only one non-bundle planner is able to terminate on average before the timelimit: RRTConnect with around 39 s seconds of planning time. Note that this problem is difficult for QMP (57 s) and QMP* (50 s) which perform worse than RRTConnect.

Figure 9.

Runtime benchmarks on the last four high-dimensional planning scenarios.

8.3. Cost analysis of high-dimensional scenarios

So far, planners have been evaluated with respect to runtime. To also evaluate the cost convergence property, we compare both QRRT* and QMP* on all eight high-dimensional scenarios to QMP, QRRT, BIT*, RRT*, LBTRRT, and RRTConnect. The results are shown in Figure 10.

Figure 10.

Success-cost plots of the eight high-dimensional planning scenarios.

Let us detail the performance of each algorithm class. First, the non-bundle space planners are only able to tackle two out of eight scenarios. RRTConnect is able to solve the airport and the drones scenario by quickly converging to 100% success rate. In the drones scenario, RRTConnect also finds good, low-cost solutions before any other planner has even found a single solution. However, apart from RRTConnect, the planners RRT*, BIT*, and LBTRRT are not applicable to any of the scenarios with no solved run during the time budget.

Second, the bundle space planners QMP, QMP*, QRRT, and QRRT* are able to tackle all eight scenarios. For the hypercube, QMP, QRRT, and QRRT* quickly find a solution, but are not able to improve upon it. QMP* finds a solution slightly later, but is able to continuously improve upon it. In the box folding task, QMP* is able to solve 90% of the cases while converging quickly to a low-cost solution. Both QRRT and QMP have lower success rates, but find on average a low-cost solution. QRRT*, however, is not able to adequately solve this problem with a success rate of 10%. For the crossing cars scenario, all bundle planners reach 100% success rate with both QRRT* and QMP* converging to low-cost solutions over time. For the airport scenario, QRRT and QMP reach 100% success rate, while both QRRT* and QMP* reach only 80% and 30%, respectively. In terms of cost convergence, QMP* is not able to improve the initial solution cost and has a large cost variance as indicated by the large shaded region around the average cost.

In the Shadow-hand scenario, QMP, and QMP* reach 90% and 70% success rate, while QRRT, and QRRT* reach only 40% and 20%. While QMP* is able to improve the solutions slightly, it has a large variance around the average cost. For the drones scenarios, both QMP and QMP* reach 100% with QMP* converging over time to good low-cost solutions. QRRT is competitive with 90% success rate and low-cost average solution as indicated by the cross in the cost plot. However, QRRT* is only able to solve 10% of the runs. For the octopus scenario, QMP, QMP*, and QRRT reach 100% success rate, while QRRT* only reaches 20%. QMP* shows quick, and low-variance convergence to an optimal solution. Finally, in the mobile manipulators scenario, QRRT* and QRRT reach 90% success rate, while QMP* reaches 20% and QMP fails to find any solutions. QRRT* is also able to converge quickly over time, reaching a solution cost significantly below solution costs from QRRT, and RRTConnect.

9. Discussion

From the preceding evaluation section, we have supporting evidence to draw three broad conclusions. First, it is difficult to solve high-dimensional planning problems with classical (non-bundle) motion planning algorithms. This should not be surprising, since the problem is known to be NP-hard (Hopcroft et al., 1984; Canny, 1988; Solovey, 2020) and the spaces to contain multiple narrow passages (Lozano-Pérez and Wesley, 1979; Salzman et al., 2013).

Second, we can often quickly and reliably solve high-dimensional planning problems by exploiting fiber bundles. We believe there are three primary contributing factors. First, we have expansions of narrow passages. If we project a narrow passage onto a base space, we often observe the narrow passage to increase its volume relative to the surrounding space. We thereby increase our chance to sample narrow passages on the base space, which we can use to guide sampling on the total space (Orthey and Toussaint, 2019). Second, we have the removal of infeasible preimages. If we find a point on the base space to be infeasible, we can remove their preimage from the bundle space, thereby removing regions which cannot be feasible (Orthey et al., 2018). Third, we have dedicated methods to exploit admissible heuristics. If we have a path on the base space, we can often quickly find solutions using the recursive path section method or by using path restriction sampling (Zhang et al., 2009). By staying on the path restriction, we exploit the information from the base space, similar to how we would exploit an admissible cost-to-go heuristic in a discrete search scenario (Pearl, 1984; Aine et al., 2016).

Third, the cost analysis showed that bundle space planners can successfully converge to low-cost solutions in high-dimensional spaces. However, this seems to only hold true for QMP*, which outperforms QRRT* in terms of cost convergence in seven out of eight scenarios, as shown in Section 8.3. QRRT*, however, has inferior performance compared to QMP* and only outperforms QMP* in the mobile manipulators scenario. We believe this is due to QRRT* using tree rewiring, which is an expensive operation. Instead, QMP* does not rely on such an operation and is better suited to tackle high-dimensional spaces.

While our evaluation seems to corroborate those statements, we also like to discuss two limiting issues. The first issue are evaluation outlier, which seemingly contradict our statements. We discuss what they are and what we can do about them. The second issue is our reliance on pre-specification of fiber bundles, which we do for this work manually. We discuss options to automatically specify them in the future.

9.1. Evaluation outlier

From the evaluations, we observe that we often can find solutions over multilevel abstractions quickly and reliably. However, we observe three noteworthy exceptions. First, we observe that QRRT performs below 3s on every enviroment, except the 37-dof pregrasp (43s) and the box folding task (8s). The cost analysis further shows that QRRT is often not able to reach the 100% success rate. We believe those environments to be challenging for QRRT, because they are examples of ingress problems, that is, problems where we need to enter a narrow passage, similar to a Bugtrap (Yershova et al., 2005). Such problems could be overcome in future work by developing a bidirectional version of QRRT, by using biased sampling towards narrow passages (Yang and Lavalle, 2004), or by selectively expanding states at the frontier of the tree (Yershova et al., 2005; Denny et al., 2020).

Second, we observe QRRT* to perform worse by an order of magnitude compared to QRRT on five out of eight environments. The cost analysis corroborate this observation by showing that QRRT* performs worse in cost convergence on seven out of eight environments when compared against QMP*. We believe the rewiring of the tree in Alg. 6 slows down planning over multilevel abstractions. In the future, we could overcome this by either postpone rewiring of the tree until a solution is found or by exploiting informed sets (Gammell et al., 2014), which are admissible lower bounds on the optimal solution. It could also be fruitful to investigate the connection between quotient space metrics and the geometric shape of informed sets, which we could use as admissible heuristics (Gammell et al., 2020).

Third, we observe that the non-bundle planner RRTConnect performs competitively on the 30-dof airport and the 48-dof drones environment. Also BFMT performs competitively on 48-dof drones. It seems, we could solve both problems without using fiber bundles. We believe this to happen because both scenarios involve SE(3) state spaces, where narrow passages might be rarer than in SE(2) scenarios. In those environments, we therefore have enough volume to quickly find valid samples, which we can exploit using RRTConnect, or BFMT. However, we believe fiber bundles are still needed. First, we do not know if RRTConnect or BFMT would still perform well if we further increase dimensionality. Second, only by using bundle planners can we consistently and reliably find solutions in all environments. Third, fiber bundles are often the only option if we want to rapidly establish infeasibility or organize local minima over high-dimensional state spaces (Orthey et al., 2020). It is, however, necessary to investigate how narrow passages slow down planning and how we could overcome them using fiber bundles. We previously conducted some evaluations in that direction for the QRRT planner (Orthey and Toussaint, 2019).

9.2. Specifying fiber bundles

For each problem, fiber bundles have to be specified manually. This is problematic, since there is no clear guideline on how to select fiber bundles for a specific problem. This could be overcome by optimizing over a primitive set of fiber bundles. To create a primitive set of fiber bundles, we could use the largest inscribed sphere for a rigid body, the removal of links from a chain, or the removal of nonholonomic constraints from a dynamical system. We can then search the landscape of such primitive fiber bundles to find an efficient fiber bundle for a specific robot and a specific set of environments. A recent study by Brandao and Havoutis (2020) shows promising results in that direction by using evolutionary algorithms to select an abstraction. It could also be promising to use workspace information to select a fiber bundle (Yoshida, 2005), either by choosing joints which can actuate links of interest through the workspace (Luna et al., 2020) or by choosing a bundle on-the-fly based on which links are closest to obstacles (Kim et al., 2015). We thereby could choose different fiber bundles for large rooms, for narrow passages or for ingress tasks. However, in those cases, we would need to consider fiber bundles with changing dimensions, which are in general given by the concept of a sheaf (Bredon, 2012).

10. Conclusion

We modeled multilevel motion planning problems using the framework of fiber bundles. To exploit fiber bundles, we developed a set of bundle primitives, and the bundle planners QRRT* and QMP*, which we showed to be probabilistically complete and asymptotically optimal. We also extended the existing bundle planners QRRT (Orthey and Toussaint, 2019) and QMP (Orthey et al., 2018) using an exponential importance criterion and a recursive L1 path section method (Figure 1). We conducted a meta-analysis to find the best implementation of the bundle primitives, including graph sampling, metric, importance selection, and path section methods. Using the bundle planners, we robustly and efficiently solved challenging high-dimensional motion planning problems, from 21-dof to 100-dof. We also showed competitive results for low-dimensional scenarios, and we showed QMP* to be superior in cost convergence for high-dimensional scenarios.

However, we believe there is still room for improvement. In particular, runtime could be further reduced by developing a bidirectional version of QRRT (LaValle and Kuffner Jr, 2001), by improving convergence using informed sets (Gammell et al., 2014), by investigating novel path section optimization methods (Zhang et al., 2009), and by automatically searching fiber bundles to exploit (Kim et al., 2015; Brandao and Havoutis, 2020)—that is, with respect to a given bundle algorithm (Orthey and Toussaint, 2019). We also believe it is worthwhile to investigate the connection to complementary approaches, like computing neighborhoods (Lacevic and Osmankovic, 2020) and exploiting sufficiency conditions (Grey et al., 2017).

However, despite room for improvements, we showed that bundle planners can efficiently exploit fiber bundles. By exploiting fiber bundles, bundle planners outperformed existing planners often by up to two orders of magnitude, occasionally up to six orders of magnitude. Thus, we believe to not only have contributed to solving multilevel planning problems in the now, but also to have contributed tools and insights to investigate high-dimensional state spaces in the future.