Collision detection or nearest-neighbor search?
On the computational bottleneck in
sampling-based motion planning⋆
Michal Kleinbort1⋆⋆, Oren Salzman2⋆⋆, and Dan Halperin1
1 Blavatnik School of Computer Science, Tel-Aviv University, Israel
2 Carnegie Mellon University, Pittsburgh PA 15213, USA
Abstract. The complexity of nearest-neighbor search dominates the
asymptotic running time of many sampling-based motion-planning al-
gorithms. However, collision detection is often considered to be the com-
putational bottleneck in practice. Examining various asymptotically op-
timal planning algorithms, we characterize settings, which we call NN-
sensitive, in which the practical computational role of nearest-neighbor
search is far from being negligible, i.e., the portion of running time taken
up by nearest-neighbor search is comparable, or sometimes even greater
than the portion of time taken up by collision detection. This reinforces
and substantiates the claim that motion-planning algorithms could sig-
nificantly benefit from efficient and possibly specifically-tailored nearest-
neighbor data structures. The asymptotic (near) optimality of these al-
gorithms relies on a prescribed connection radius, defining a ball around
a configuration q, such that q needs to be connected to all other config-
urations in that ball. To facilitate our study, we show how to adapt this
radius to non-Euclidean spaces, which are prevalent in motion planning.
This technical result is of independent interest, as it enables to compare
the radial-connection approach with the common alternative, namely,
connecting each configuration to its k nearest neighbors (k-NN). Indeed,
as we demonstrate, there are scenarios where using the radial connection
scheme, a solution path of a specific cost is produced ten-fold (and more)
faster than with k-NN.
1
Introduction
Given a robot R moving in a workspace W cluttered with obstacles, motion-
planning (MP) algorithms are used to efficiently plan a path for R, while avoid-
ing collision with obstacles [13, 36]. Prevalent algorithms abstract R as a point
⋆This work has been supported in part by the Israel Science Foundation (grant
no. 825/15), by the Blavatnik Computer Science Research Fund, and by the Hermann
Minkowski–Minerva Center for Geometry at Tel Aviv University. O. Salzman has
been also supported by the German-Israeli Foundation (grant no. 1150-82.6/2011),
by the National Science Foundation IIS (#1409003), Toyota Motor Engineering &
Manufacturing (TEMA), and the Office of Naval Research. Parts of this work were
done while O. Salzman was a student at Tel Aviv University.
⋆⋆M. Kleinbort and O. Salzman contributed equally to this paper.
arXiv:1607.04800v3  [cs.RO]  30 Oct 2016
in a high-dimensional space called the configuration space (C-space) X and plan
a path (curve) in this space. A point, or a configuration, in X represents a place-
ment of R that is either collision-free or not, subdividing X into the sets Xfree
and Xforb, respectively. Sampling-based algorithms study the structure of X by
constructing a graph, called a roadmap, which approximates the connectivity
of Xfree. The nodes of the graph are collision-free configurations sampled at ran-
dom. Two (nearby) nodes are connected by an edge if the straight line segment
connecting their configurations is collision-free as well.
Sampling-based MP algorithms are typically implemented using two primi-
tive operations: Collision detection (CD) [39], which is primarily used to deter-
mine whether a configuration is collision-free or not, and Nearest-neighbor (NN)
search, which is used to efficiently return the nearest neighbor (or neighbors) of a
given configuration. CD is also used to test if the straight line segment connecting
two configurations lies in Xfree—a procedure referred to as local planning (LP).
In this paper we consider both CD and LP calls when measuring the time spent
on collision-detection operations.
Fig. 1: Running-time breakdown of
the main primitive operations used
in MPLB [46] applied to the 3D-
Grid scenario (Fig. 2a). For additional
data, see Sec. 4. Best viewed in color.
Contribution The complexity of NN search
dominates the asymptotic running time of
many sampling-based MP algorithms How-
ever, the main computational bottleneck in
practical settings is typically considered to be
LP [13, 36]. In this paper we argue that this
may not always be the case. We describe set-
tings, which we call NN-sensitive, where the
(computational) role of NN search after finite
running-time is far from negligible and mer-
its the use of advanced and specially-tailored
data structures; see Fig. 1 for a plot demon-
strating this behavior. NN-sensitive settings may be due to (i) planners that
algorithmically shift the computational weight to NN search; (ii) scenarios in
which certain planners perform mostly NN search; or (iii) parameters’ values for
which certain planners spend the same order of running time on NN and CD.
Specifically, we focus on asymptotically (near) optimal MP algorithms. These
planners, which are the standard practice nowadays, are of specific interest as
they typically require more CD and NN calls than non-optimal planners. We
study the ratio between the overall time spent on NN search and CD after N
configurations were sampled. We observe situations where NN takes up to 100%
more time than CD in scenarios based on the Open Motion Planning Library [15];
on synthetic high-dimensional C-spaces we even observe a ratio of 4500%.
We mostly concentrate on the radial version of MP algorithms, where the
set of neighbors in the roadmap of a given configuration q includes all configu-
rations of maximal distance r from q. To do so in non-Euclidean C-spaces, we
derive closed-form expressions for the volume of a unit ball in several common
C-spaces. This technical result is of independent interest, as the lack of such
expressions seems to have thus far prevented the exploration and understand-
2
ing of these types of algorithms in non-Euclidean settings—most experimental
evaluation reported in the literature on the radial version of asymptotically-
optimal planners is limited to Euclidean settings only. We show empirically that
in certain scenarios, the radial version of an MP algorithm produces a solution
of specific cost more than ten times faster than the non-radial version, namely,
where each node is connected to its k nearest neighbors.
We emphasize that we are not the first to claim that in certain cases NN
may dominate the running of MP algorithms, see, e.g., [9]. However, we take a
systematic approach to characterize and analyze when this phenomenon occurs.
Throughout the paper we use the following notation: For an algorithm ALG,
let χALG(S) be the ratio between the overall time spent on NN search and CD
for a specific motion-planning problem after a set S of configurations was sam-
pled, where we assume that all other parameters of the problem, the workspace
and the robot, are fixed—see details below. Let χALG(N) be the expected value
of χALG(S) over all sample sets S of size N.
Organization We start with an overview of related work in Sec. 2 and continue
in Sec. 3 to summarize, for several algorithms, the computational complexity
in terms of NN search and CD. We show that asymptotically, as N tends to
infinity, χALG(N) tends to infinity as well. In Sec. 4 we point out several NN-
sensitive settings together with simulations demonstrating how χALG(N) behaves
in such settings. These simulations make use of the closed-form expressions of
the volume of unit balls, which are detailed in Sec. 5. Sec. 6 concludes with a
discussion and possible future work.
Remark. There are algorithms where a third type of operations (e.g., cost es-
timation), beyond CD and NN, takes the lion’s share of computation time. We
defer the discussion of such algorithms to future research.
2
Background and related work
We start by giving an overview of asymptotically (near) optimal MP algorithms
and continue with a description of CD and NN algorithms.
2.1
Asymptotically optimal sampling-based motion planning
A random geometric graph (RGG) G is a graph whose vertices are sampled at
random from some space X. Every two configurations are connected if their
distance is less than a connection radius rn (which is typically a function of the
number of nodes n in the graph). We are interested in a connection radius such
that, asymptotically, for any two vertices x, y, the cost of a path in the graph
connecting x and y converges to the minimal-cost path connecting them in X.
A sufficient condition to ensure this property is that [27]
rn ≥2η
µ(Xfree)
ζd
1/d 1
d
1/d log n
n
1/d
.
(1)
3
Here d is the dimension of X, µ(·) and ζd denote the Lebesgue measure (volume)
of a set and of the d-dimensional unit ball, respectively, and η ≥1 is a tuning
parameter that allows to balance between exploring unvisited regions of the C-
space and connecting visited regions. Alternatively, an RGG where every vertex
is connected to its kn ≥e(1 + 1/d) log n nearest neighbors will ensure similar
convergence properties [28]. Unless stated otherwise, we focus on RGGs of the
former type. Namely, where the set of neighbors of a node is chosen according
to Eq. 1. For a survey on additional models of RGGs, their properties and their
connection to sampling-based MP algorithms, see [50].
Most asymptotically-optimal planners sample a set of collision-free configu-
rations (either incrementally or in batches). This set of configurations induces
an RGG G or a sequence of increasingly dense RGGs {Gn} whose vertices are
the sampled configurations. Set G′ ⊆G to be the subgraph of G whose edges
represent collision-free motions. These algorithms construct a roadmap H ⊆G′.
PRM* and RRG [28], which were two of the three initial asymptotically-
optimal MP algorithms, call the local planner for all the edges of G. To increase
the convergence rate to high-quality solutions, algorithms such as RRT* [28],
RRT# [3], LBT-RRT [47], FMT* [27], MPLB [46], Lazy-PRM* [20], and BIT* [18]
call the local planner for a subset of the edges of G.
Reducing the number of LP calls is typically done by constructing G (using
nearest-neighbor operations only) and deciding for which edges to call the local
planner. Many of the algorithms mentioned do so by using graph operations
such as shortest-path computation. These operations often take a tiny fraction
of the time required for LP computation. One such example is MPLB which uses
shortest-path computation to obtain a lower bound on the cost-to-go of nodes.
Alternatively, dynamic shortest-path algorithms such as LPA* [34] are used to
maintain the cost-to-come of nodes when the graph undergoes a series of edge
insertions and deletions. Examples include Lazy-PRM* and LBT-RRT. In more
recent algorithms, such as FMT* and BIT*, NN and CD may not take the lion’s
share of computation time in practice.
Variants and extensions of the above algorithms include, among others, con-
structing sparse data structures [16, 48], taking into account dynamic obsta-
cles [43] and addressing kinodynamic planning [38, 49, 53].
2.2
Collision detection
Collision-detection algorithms are extensively used by sampling-based MP al-
gorithms in order to answer discrete collision queries or continuous ones. The
former test whether a sampled configuration is in collision with the workspace
obstacles, whereas the latter test whether a continuous path between two con-
figurations is collision-free.
Most CD algorithms are bound to certain types of models, where rigid polyhe-
dral models are the most common. They often allow answering proximity queries
as well (i.e., separation-distance computation or penetration-depth estimation).
Several software libraries for collision detection are publicly available [14, 35].
The most general of which is the Flexible Collision Library (FCL) [44] that inte-
grates several techniques for fast and accurate collision checking and proximity
4
computation. For polyhedral models, which are prevalent in MP settings, most
commonly-used techniques are based on bounding volume hierarchies (BVH).
Among the various types of bounding volumes, the most prominent are axis-
aligned bounding box, oriented bounding box, spheres, and swept sphere vol-
ume. The different types of bounding volumes differ in the tightness of fitting to
the bounded set and in the cost of an overlap test of two bounding volumes.
A collision query using BVHs may take O(m2) time in the worst case,
where m is the complexity of the obstacle polyhedra (recall that we assume
that the robot system has constant-description complexity). However, tighter
bounds may be obtained using methods tailored for large environments [14, 22].
Specifically, the time complexity is O(m logδ−1 m + s), where δ ∈{2, 3} is the
dimension of the workspace W and s is the number of intersections between the
bounding volumes. Other methods relevant to MP are continuous CD [29, 30, 55]
and algorithms tailored for dynamic environments where the objects undergo
rigid motion [14]. Additionally, efficient approaches that use graphics hardware
may accelerate the running time of CD queries significantly; see, e.g., [19]. For
a survey on the topic, see [39].
2.3
Nearest-neighbor methods: exact and approximate
Nearest-neighbor (NN) algorithms are frequently used in various domains. In
the most basic form of the problem we are given a set P of n points in a metric
space M = (X, ρ), where X is a set and ρ : X×X →R is a distance metric. Given
a query point q ∈X, we wish to efficiently report the nearest point p ∈P to q.
Immediate extensions include the k-nearest-neighbors (K-NN) and the r-near-
neighbors (R-NN) problems. The former reports the k nearest points of P to q,
whereas the latter reports all points of P within a distance r from q. Another
variant is the all-pairs r-near neighbors (AP) where, given a radius r, one has
to report all pairs of points in P of distance at most r.
In the plane, the NN search problem (also known as the post-office problem)
can be efficiently solved by constructing a Voronoi diagram of P in O(n log n)
time and preprocessing it to a linear-size point-location data structure in O(n log n)
time3. Queries are then answered in O(log n) time [7, 21]. However, for high-
dimensional point sets this approach becomes infeasible, as it is exponential in
the dimension d. This phenomenon is often termed “the curse of dimensional-
ity” [26].
An efficient data structure for low dimensional spaces4 is the kd-tree [6, 17],
whose expected query complexity is logarithmic in n under certain assumptions.
However, the constant factors hidden in the asymptotic query time depend expo-
nentially on the dimension d [5]. Another structure suitable for low-dimensional
spaces is the geometric near-neighbor access tree (GNAT); as claimed in [11],
typically the construction time is O(dn log n) and only linear space is required.
3 A simple randomized algorithm with expected O(n log n) time is described in [21].
There is a variety of more involved algorithms with worst-case O(n log n) time (see,
e.g., [31]).
4 We refer to a space as low dimensional when its dimension is at most a few dozens.
5
In order to overcome the so-called “curse of dimensionality”, numerous al-
gorithms, such as, cover trees [8] and randomly-oriented kd-trees [51], were pro-
posed. These methods adapt to the intrinsic dimension of the data, which is
often much smaller than that of the ambient dimension d.
All the aforementioned structures give an exact solution to the problem. How-
ever, many approximate algorithms exist, and often perform significantly faster
than the exact ones, especially when d is high. The approximate variant of the
basic NN search problem is to design a data structure that supports the follow-
ing operation: For any query q ∈X, if there exists p ∈P such that D(p, q) ≤r,
find a point p′ ∈P such that D(q, p′) ≤cr, where r, c > 0 are given real pa-
rameters. This problem is often referred to as (r, c)-NN [25]. The approximate
k-nearest neighbors and the approximate r-near neighbors search problems can
be defined similarly. Among the prominent approximate algorithms are Bal-
anaced box-decomposition trees (BBD-trees) [5], and Locality-sensitive hashing
(LSH) [26]. An improved variant of LSH, which uses data-dependent hashing,
was recently presented [2]. For a survey on approximate NN methods in high-
dimensional spaces, see [25].
In the context of MP, several specifically-tailored exact [24, 54] and approxi-
mate [32, 45] techniques were previously described. Note that the proofs provided
for the probabilistic completeness and asymptotic optimality of certain planners
(e.g., PRM*, RRT*) assume the use of exact NN queries [28]. Conveniently,
in [50] a theoretical justification for using approximate NN methods rather than
exact ones is proven for PRM*.
A structure based on random grids allows for efficiently answering approxi-
mate r-near neighbors queries in Euclidean d-dimensional space [1]. In [32] we
used this structure to obtain significant speedups in the construction time of cer-
tain asymptotically-optimal algorithms. In addition, we were able to converge
faster to the optimal solution, when the grid-based structure was used to answer
NN queries.
3
The asymptotic behavior of common MP algorithms
In this section we provide more background on the asymptotic complexity analy-
sis of various sampling-based MP algorithms. We then show that for both PRM-
type algorithms and RRT-type algorithms, the expected ratio between the time
spent on NN search and the time spent on CD goes to infinity as n →∞.
Throughout the paper we use the following notation: We denote by N the
total number of configurations sampled by the algorithm, and by n the number
of collision-free configurations in the roadmap. Let m denote the complexity of
the workspace obstacles and assume that the robot is of constant-description
complexity5.
6
Operation
Computational complexity Comments
NN
O(cd,ε · log n)
Approx. using BBD-trees [4]
Ω(log n)
Approx. under the partition trees
paradigm∗
R-NN
O(c′
d,ε + 2d log n + κ)
Approx. using BBD-trees [4]
Ω(log n +
1
ϵd−1 + κ)
Approx. under the partition trees
paradigm∗
AP
O(cAP · log n · (n + κ))
Approx. using RTG [1]
CD (for a robot
with a single rigid
part)
O(m logδ−1 m + s)
Assuming the workspace is Rδ [39]
CD
(for
a
sys-
tem with ℓrigid
parts)∗∗
O(ℓ2 + ℓm logδ−1 m + s)
Assuming the workspace is Rδ [39]
Table 1:
Summary of the complexity of typical primitive operations in sampling-based
algorithms. Here, κ denotes the expected number of neighbors returned by an NN query, δ
denotes the dimension of the workspace, and s denotes the number of intersections between
the bounding volumes. The constants cd,ε, c′
d,ε and cAP are (at least) exponential in d. RTG
is an approximate NN structure based on random grids.
∗The lower bounds for NN and R-NN are for the worst case query based on the partition
trees paradigm.
∗∗CD for a system with ℓrigid parts involves ℓcollision checks between parts and obstacles
and O(ℓ2) collision checks between the pairs of parts.
3.1
Complexity of common motion-planning algorithms
We start by summarizing the computational complexity of the primitive oper-
ations and continue to detail the computational complexity of a selected set of
algorithms. We assume familiarity with the planners that are discussed.
Complexity of primitive operations The main primitive operations that we
consider are (i) nearest-neigbhor operations (NN, R-NN and AP) and (ii) collision-
detection operations (CD and LP). Additionally, MP algorithms make use of
priority queues and graph operations. We assume, as is typically the case, that
the running time of these operations is negligible when compared to NN and CD.
Since many NN data structures require a preprocessing phase, the complexity
of a single query should consider the amortized cost of preprocessing. However,
since usually at least n NN or R-NN queries are performed, where n is the
number of points stored in the NN data structure, this amortized preprocessing
cost is asymptotically subsumed by the cost of a query.
5 The assumption that the robot is of constant-description complexity implies that
testing for self-collision can be done in constant time.
7
Table 1 summarizes the complexity of the main primitive operations. Note
that the bounds given for NN assume approximate methods6. Local planning
(LP), which is not mentioned in Table 1, is often implemented using multiple
CD operations along a densely-sampled C-space line-segment between two con-
figurations. Specifically, we assume that the planner is endowed with a fixed
parameter called STEP specifying the sampling density along edges. During LP,
edges of maximal length rn will be subdivided into ⌈rn/STEP⌉collision-checked
configurations (see also [36, p. 214]). Therefore, the complexity of a single LP
query can be bounded by O(rn · QCD), where QCD is the complexity of a single
CD query (here STEP is assumed to be constant).
Remark. Our analysis differs slightly from the one performed by Karaman and
Frazzoli [28] where LP is performed by checking if the C-space line segment
intersects the C-space obstacles. The latter requires an exact representation of
the C-space obstacles, whose complexity can be exponential in the dimension d.
We therefore assume, as is common in practice, that LP is performed as a series
of CD calls in the workspace.
Complexity of algorithms In order to choose which edges of G to explicitly
check for being free using the local planner, all algorithms need to determine
(i) which of the N nodes are collision free and (ii) what are the neighbors of
each node. Thus, these algorithms typically require N CD calls7 and either n
R-NN calls in the case of incremental algorithms, such as RRT*, LBT-RRT or
RRT#, or one AP call8 in the case of batch algorithms, such as sPRM*, Lazy-
sPRM* or FMT*.
To quantify the number of LP calls performed by each algorithm, note that
the expected number of neighbors of a node in G is Θ(ηd2d log n) [50]. Therefore,
if an algorithm calls the local planner for all (or for a constant fraction of)
the edges of G, then the expected number of LP calls will be Θ(ηd2dn log n).
Often, tighter bounds on the number of LP calls can be obtained. For instance,
for FMT* (and similarly for MPLB) the expected number of LP calls can be
bounded by O(n) (see, [27, Lemma C.2]).
3.2
The asymptotic behavior of the ratio χALG(N)
Let TCD(S) be the overall time spent on CD for a specific motion-planning
problem after a set S of configurations was sampled, where we assume, as before,
that all other parameters of the problem are fixed. Let TCD(N) be the expected
value of TCD(S) over all sample sets S of size N. We show here that the expected
6 Bounds for exact NN structures exist only for a subset of the prevalent methods and
may require prior assumptions on the point set.
7 To determine if a node is collision-free, one may use the framework by Bialkowski et
al. [9] which replaces CD calls with NN search. However, their framework assumes
that the CD provides a bound on the distance to the closest C-space obstacle, an
assumption that may not always hold. Thus, here we assume standard CD imple-
mentation.
8 AP can be replaced by a series of n R-NN queries.
8
value of the ratio χALG(N) over all sample sets of size N goes to infinty as N →∞
for both sPRM* and RRT*. Recall that we are interested in the expected value
of the ratio. We do that by looking at the ratio between a lower bound on the
time of NN and TCD(N), defined above.
To obtain a lower bound on the time of NN, we assume that the NN structure
being used is a j-ary tree for a constant j, in which the data points are kept
in the leaves. This is a reasonable assumption, as many standard NN structures
are based on trees [5, 6, 11, 17]. Performing n queries of NN (or R-NN) using
this structure, one for every data point, costs Ω(n log n), as each query involves
locating the leaf in which the query point lies. It is easy to show this both for
sPRM*, in which the NN structure is constructed given a batch of all data points,
and for RRT*, where the structure is constructed incrementally.
Additionally, we have the following lemma, whose proof is in [33]:
Lemma 1 If an algorithm uses a uniform set of samples and the C-space ob-
stacles occupy a constant fraction of the C-space, then n = Θ(N) almost surely.
Proof. Lemma 1 states that under mild assumptions, there exist constants N0 ∈
N+ and 0 < c1 ≤1 s.t. for every N > N0, n ≥c1N almost surely. Showing
that this holds for sPRM* requires using the Chernoffbound (see, e.g., [42,
Theorem 4.5]) and noting that the number n is a binomial random variable
B(N, p) with success probability p = µ(Xfree)/µ(X), which is a strictly-positive
constant by our assumption. In particular, for every constant δ ∈(0, 1) the event
n > (1 −δ)pN holds almost surely. In order to show that n is proportional to N
in RRT* as well, we supplement the latter observation with the fact that after
a finite number of samples, each new free sample will be added as a node to the
constructed tree. This is formalized in [28, Lemma 63].
⊓⊔
We use the following notation in the remainder of this section: For an oper-
ation OP let QOP denote the complexity of the operation and #OP denote the
number of times the operation is called.
The asymptotic value of χsPRM*(N) For sPRM* it holds that TCD(N) =
#CD · QCD + #LP · QLP. Clearly, #CD = N and QCD = O(m2) (an upper bound
on s from Table 1—the number of intersections between the bounding volumes).
In expectation we have that #LP = O(rd
n) and in addition QLP = O(rn · QCD).
Finally, recall that rn = Θ

2η (log n/n)1/d
= Θ
(log n/n)1/d

. Therefore,
TCD(N) = N · QCD + O(ηd2dn log n) · QLP
= N · QCD + O(ηd2dn log n) · O(rn · QCD)
= O(m2N) + O(m2N 1−1/d log1+1/d N) = O(m2N).
(2)
As Ω(n log n) is a valid lower bound on the overall complexity of NN, there
exists a constant c2 > 0 s.t. the time for NN for a roadmap with n nodes is at
least c2n log n. Moreover, since TCD(N) = O(m2N) then there exists a constant
c3 > 0 s.t. the overall time for CD is at most c3m2N.
9
Thus, using Lemma 1,
χsPRM*(N) ≥c2n log n
c3m2N
≥c′ log N
m2
,
(3)
where c′ > 0 is a constant. Observing that the above fraction goes to infinity as
N goes to infinity, we obtain that limN→∞χsPRM*(N) = ∞, as anticipated.
We note that although m is assumed to be constant, we leave it in our analysis
to emphasize its effect on χALG(N).
The asymptotic value of χRRT*(N) Since RRT* is an incremental algorithm
and not a batch one, we consider the time spent on CD in the Nth iteration,
which is QCD + #LP · QLP. Therefore,
TCD(N) =
XN
i=1

O(m2) + O(ηd2d log i(log i/i)1/d) · O(m2)

= O(m2N) + O
 
m2ηd2d XN
i=1
 
log i
log i
i
1/d!!
.
Note that since f(i) = log i(log i/i)1/d is bounded from above by some constant b,
then PN
i=1 f(i) can be upper bounded by O(Nb). Thus, TCD(N) = O(m2N)
and, accordingly, χRRT*(N) ≥c′′ log N
m2
, where c′′ > 0 is a constant. Therefore,
limN→∞χRRT*(N) = ∞. In summary,
Proposition 2 The values χsPRM*(N) and χRRT*(N) tend to infinity as N →∞.
From a theoretical standpoint, NN search determines the asymptotic running
time of typical sampling-based MP algorithms. In contrast, the common expe-
rience is that CD dominates the running time in practice. However, we show in
the remainder of the paper that in a variety of special situations NN search is a
non-negligible factor in the running-time in practice.
4
Nearest-neighbor sensitive settings
In this section we describe settings where the computational role of NN search
in practice is far from negligible, even for a relatively small number of samples.
We call these settings NN-sensitive. For each such setting we empirically demon-
strate this behavior. In Sec. 4.1 we describe our experimental methodology and
outline properties common to all our experiments. Each of the subsequent sec-
tions is devoted to a specific type of NN-sensitivity.
4.1
Experimental methodology
In our experiments, we ran the Open Motion Planning Library (OMPL 1.1) [15]
on a 2.5GHz×4 Intel Core i5 processor with 8GB of memory. Each reported
10
(a)
(b)
(c)
(d)
Fig. 2: Scenarios used in experiments. (a) 3D Grid, (b) 2D Random polygons, (c) 3D
Cubicles and (d) dD Hypercube with a centered obstacle. Start and target configurations
for a robot are depicted in green and red, respectively. Scenarios (b) and (c) are provided
with the OMPL distribution. More details are provided in the body of the paper.
result is averaged over fifty (50) runs and includes error bars which denote the
20’th and 80’th percentiles. The scenarios used are depicted in Fig. 2.
Several of our experiments are in non-Euclidean C-spaces, which in turn re-
quire a closed-form expression for ζd, the measure (volume) of the d-dimensional
unit ball (see Eq. 1). In Sec. 5 we describe a general approach to compute this
value together with a heuristic that makes the computed radius effective in prac-
tice. This heuristic is used in all the experiments presented in this section.
What do we measure? Recall that our main thesis in this paper is that while
the folklore in motion planning is that the running time of sampling-based plan-
ning algorithms in practice is strongly dominated by collision detection, we (and
others) observe that quite often the time taken up by NN-search is significant,
and not rarely larger than the time taken up by collision detection. Therefore,
our primary measure is wall time, namely the running time spent on the differ-
ent primitives as gauged in standard clock time (to distinguish from CPU-time
measurement or other more system-specific measurements like number of float-
ing point operations). The principal reason for doing that is that wall time is
what matters most in practice. This, for example, will affect the response time
of a planner used by a robot system. One may argue that this measurement
may only be meaningful for a very limited suite of software tools used by mo-
tion planners. However, we use state-of-the-art tools that are used by many.
There is not such an abundance of efficient stable software tools for this pur-
pose, and most researchers in the field seem to use a fairly small set of tools
for collision detection and nearest-neighbor search. This said, we still provide
additional measurements for each experiment—the average number of basic op-
erations. These measurements should allow people who come up with their own
(or specialized) motion-planning primitives to assess what will be the effect of
their special primitives on the overall running time of the algorithms in practice.
11
4.2
NN-sensitive algorithms
In recent years, several planners were introduced, which algorithmically shift
some of the computational cost from CD to NN search. Two such examples are
Lazy-PRM* [20] and MPLB [46], though lazy planners were described before
(e.g., [10]). Both algorithms delay local planning by building an RGG G over a
set of samples without checking if the edges are collision free. Then, they employ
graph-search algorithms to find a solution. To construct G only NN queries are
required. Moreover, using these graph-search algorithms dramatically reduces
the number of LP calls. Thus, in many cases (especially as the number of samples
grows) the weight of CD is almost negligible with respect to that of NN.
Specifically, Lazy-PRM* iteratively computes the shortest path in G between
the start and target configurations using a dynamic single source shortest path
algorithm. LP is called only for the edges of the path. If some are found to be
in collision, they are removed from the graph. This process is repeated until a
solution is found or until the source and target do not lie in the same connected
component. We use a batch variant of Hauser’s Lazy-PRM* algorithm [20], which
we denote by Lazy-sPRM*. This variant constructs the roadmap in the same
fashion as sPRM* does but delays LP to the query phase.
MPLB uses G to compute lower bounds on the cost between configurations to
tightly estimate the cost-to-go [46]. These bounds are then used as a heuristic to
guide the search of an anytime version of FMT* [27]. The bounds are computed
by running a shortest-path algorithm over G from the target to the source. Fig. 1
(on page 2) presents the amount of NN, CD and other operations used by MPLB
running on the 3D Grid scenario for two robots translating and rotating in space
that need to exchange their positions (Fig. 2a). Fig. 3 plots the ratio χMPLB(N)
as a function of the number of valid samples n and Table 2 provides the average
number of basic operation calls performed for different values of n. Clearly, with
several thousands of iterations, which are required for obtaining a high-quality
solution, NN dominates the running time of the algorithm.
Additional experiments demonstrating the behavior of NN-sensitive algo-
rithms can be found in [9, 20, 46].
0.0
0.2
0.4
0.6
0.8
1.0
1.2
1.4
1.6
MPLB(N)
0
5
10
15
20
25
30
Number of samples (in thousands)
Fig. 3: χMPLB(N) as a function of n demon-
strating an NN-sensitive algorithm.
n
#R-NN #CD
#LP
#CD in LP
1,600
3.1K
4.2K
9.5K
159K
3,200
6.3K
8.5K
20.8K 308K
6,400
12.7K 16.9K 44.2K 598K
12,800 25.5K 33.9K 86.7K 1,105K
Table 2: Average number of calls for the
main primitive operations for different val-
ues of n for MPLB.
12
4.3
NN-sensitive scenarios
A scenario S = (W, R) is defined by a workspace W and a robot system R. The
robot system R may, in turn, be a set of ℓsingle constant-description complexity
robots operating simultaneously in W. Let the dimension d of S be the dimension
of the C-space induced by R, and, hence, d = Θ(ℓ).9 Let the complexity of S be
the complexity m of the workspace obstacles. Note that CD is affected by ℓ, as
both robot-obstacle and robot-robot collisions should be considered. Therefore,
the bound on the complexity of a CD operation is: O(ℓ· m2 + ℓ2), see Sec. 3.1.
We next show how the role of NN may increase when (i) the dimension of S
increases or (ii) the complexity of S decreases.
The effect of the dimension d In Section 3.2 we show that as the number of
samples tends to infinity, NN dominates the running time of the algorithm. A
natural question to ask is “what happens when we fix the number of samples and
increase the dimension?” The different structure of RRT* and sPRM* merits a
different answer for each algorithm.
RRT* Here, we show that the NN sensitivity grows with the number of unsuc-
cessful iterations10. This implies that if the number of unsuccessful iterations
grows with the dimension, so will χRRT*(N). Indeed, we demonstrate this phe-
nomenon in the 3D cubicles scenario (Fig. 2c). Note that in this situation the
effect of d is indirect.
To better discuss our results we define two types of LP operations: the first is
called when the algorithm attempts to grow the tree towards a random sample
while the second is called during the rewiring step. We denote the former type
of LP calls by LP-A and the latter by LP-B and note that LP-A will occur every
iteration while LP-B will occur only in successful ones.
We use ℓtranslating and rotating L-shaped robots. We gradually increase ℓ
from two to six, resulting in a C-space of dimension d = 6ℓ. Robots are placed in
different sections of the workspace and can reach their target with little robot-
robot interaction. We fix the number of iterations N and measure χRRT*(N)
as a function of d. The results for several values of N are depicted in Fig. 4.
Additionally, Table 3 shows the average number of operation calls for various
values of d.
As d grows, the number of unsuccessful iterations grows (see #LP-A in Ta-
ble 3). This growth, which is roughly linear with respect to d induces a linear
increase in χRRT*(N) for a given N (see Fig. 4). Furthermore, the slope of this
line increases with N which further demonstrates the fact that for a fixed d,
limN→∞χRRT*(N) = ∞.
Finally, Fig. 5 depicts the time decomposition of the main primitives as a
function of d, for N = 80K. Clearly, as d grows most of the time is spent on NN
calls due to the increase in the portion of unsuccessful iterations.
9 In the case of a single ℓ-link robot, the robot is not of constant-description complexity.
Thus, the dimension of S is d = Θ(ℓ). For simplicity of exposition we ignore here
the case of many non-constant multi-link robots.
10 Here, an iteration is said to be unsuccessful when the RRT* tree is not extended.
13
0.0
0.5
1.0
1.5
2.0
RRT*(N)
15
20
25
30
35
Dimension
N = 80K
N = 40K
N = 20K
Fig. 4: χRRT*(N) as a function of d in
the 3D Cubicles scenario (Fig. 2c), when
fixing the number N of iterations.
d #NN #R-NN #CD #LP-A #LP-B #CD in LP
12 80K 10.2K 80K 80K
325K 4,235K
24 80K 7.5K
80K 80K
724K 4,135K
36 80K 5.7K
80K 80K
886K 3,812K
Table 3: Average number of calls for the main
primitive operations for different values of d, for
N = 80K iterations.
sPRM* Here, the NN sensitivity of the algorithm is more complex. The reason,
roughly speaking, is that for a fixed n, the expected value of the number κ of
reported neighbors is Θ(2dn log n). Thus, in expectation, κ grows exponentially
in d. However, for large enough values of d, we have κ = Θ(n2), since each node
can have at most n neighbors. Interestingly, this means that the computational
cost of the overall NN time shifts from finding the set of nearest neighbors to
reporting it.
We attempt to formalize the above discussion but emphasize that this (crude)
analysis is only useful to try and explain trends. The reason being is that we
consider finite values of n and d while using asymptotic bounds. Clearly in this
regime, constants may play a significant role, a fact that we neglect.
Set κ to be the total number of nearest neighbors computed by the algorithm.
Following the above discussion, for a fixed n and increasing values of d, the ex-
pected value of κ grows from O(n log n) to O(n2). As κ grows, LP will dominate
the running time of TCD(N) (and not CD as asymptotically occurs in 2). Fur-
thermore, for large values of d, we have that rn ≈c for some constant c. Thus,
TCD(N) ≈cn2. For NN, as we discussed, the computational cost will shift from
finding the set of nearest neighbors to reporting it. Thus, for large values of d,
the cost of NN will be proportional to n2. Combining the two observations, we
can conjecture that for large values of d, we have that χsPRM*(N) will converge
to some constant.
According to the above analysis we expect to see an initial increase of χsPRM*(N)
followed by a convergence to a constant value. The increase in χsPRM*(N) is due
0.0
0.2
0.4
0.6
0.8
1.0
Time decomposition [%]
15
20
25
30
35
Dimension
else
LP-B
CD + LP-A
R-NN
NN
Fig. 5: Time breakdown of the main primi-
tive operations in RRT* running on the 3D
Cubicles scenario (Fig. 2c) as a function
of d, for N = 80K iterations. Best viewed
in color.
14
0.0
0.5
1.0
1.5
2.0
2.5
sPRM*(N)
5
10
15
20
25 30
35
40
45 50
Dimension
rNN,
= 0.5
rNN,
= 0.25
rNN,
= 0
(a)
0
9
18
27
36
45
sPRM*(N)
5
10
15
20
25 30
35
40
45 50
Dimension
kNN,
= 0.5
kNN,
= 0.25
kNN,
= 0
(b)
Fig. 6: χsPRM*(N) as a function of d in the dD Hypercube scenario (Fig. 2d) for a roadmap
with n = 5000 vertices. Experiments with obstacle measures µ of 0, 0.25 and 0.5 are
displayed. The connection strategies used in (a) and (b) are R-NN and K-NN, respectively.
to the increasing complexity of finding the set of nearest neighbors, which grows
with the dimension11. A possible decrease will occur as the computational weight
“shifts” to reporting the set of neighbors followed by an asymptotic convergence
to a constant value, for large values of d.
Aiming to test this conjecture, we solved a planning problem for a point robot
moving in the d-dimensional unit hypercube containing a hyper-cubicle obstacle
of a specific measure µ centered at the middle of the workspace (Fig. 2d). Note
that a single CD call in this setting takes O(d) time. We set n = 5000 and used
sPRM* to plan a path for a point robot from (0, . . . , 0) ∈Rd to (1, . . . , 1) ∈Rd.
We gradually increased d and measured χsPRM*(N). We repeated the experiment
for obstacle volumes µ: 0, 0.25, and 0.5, keeping the volume of the room fixed.
Indeed, the expected trend can be seen clearly in our results in Fig. 6a. We
note, however, that the average number of neighbors of a node reached the value
of 90%, only for d ≥30. Moreover, for larger value of µ we saw a decrease in
the average number of reported neighbors. This may explain the fact that the
peak of the plot for µ = 0.5 is obtained for a larger value of d, then the one
for µ = 0. Additionally, as we increase µ, the ratio obtains higher values, before
it starts converging. We repeated the experiment while using K-NN instead of
R-NN queries. The results, depicted in Fig. 6b, show a growth in the ratio as a
function of d by 2000%. This is not surprising, as the standard value of k that
is commonly used is proportional to log n, and is smaller by a factor of 2d than
the expected number of neighbors returned by an R-NN query.
The effect of the geometric complexity m of the obstacles Recall that
a collision query may take O(m2) in the worst case. For small values of m,
this becomes negligible with respect to other primitive operations, such as NN.
In order to demonstrate this effect we ran the following experiment which is
11 Here, we assume, as is common in the literature, that the cost of finding the set of
NN grows with the dimension.
15
0
1
2
3
4
5
6
7
LBT-RRT(N)
200
400
600
800 1000 1200 1400 1600
Number of triangles in obstacle mesh (m)
Set #1
Set #2
Fig. 7: χLBT-RRT(N) as a function of m in
workspaces of increasing obstacle complex-
ity based on the 2D Random polygons sce-
nario (Fig. 2b). The presented plots are for
N = 4K iterations in two different randomly-
generated experiment sets.
m
#NN #R-NN #CD #LP
#CD in LP
266
4K
2.5K
4K
8.1K
496K
704
4K
1.7K
4K
7.7K
352K
1,262 4K
0.99K 4K
24.5K 271K
1,476 4K
0.7K
4K
28.7K 232K
Table 4: Average number of calls for the
main primitive operations for different values
of m (chosen arbitrarily).
based on the 2D Random polygons scenario (Fig. 2b). We created two sequences
of increasing geometric-complexity (growing m) environments. Each sequence
was constructed as follows: we start with the empty environment and incremen-
tally add random polygons from Fig. 2b until all the polygons in Fig. 2b have
been added. We then placed eight robots12 that need to change their positions,
and ran LBT-RRT (with approximation factor ε = 0.4) for a fixed N. Fig. 7
plots χLBT-RRT(N) as a function of m for two sets of environments. As antici-
pated, the ratio in both sets of environments decays polynomially as m grows.
Average operation counts, given in Table 4, reveal an increase in the number
of LP calls as m grows. A possible explanation is that when there are very few
polygonal obstacles in this environment, almost no rewiring is performed as the
cost-to-come of each node in the LBT-RRT tree is close to optimal. However,
the number of CD calls within LP operations decreases as m grows since many
edges are invalid. Note that each CD call becomes more costly as a function of
m, thus, the overall time spent on CD does not change significantly as a function
of m. Finally, since it is more difficult to grow the roadmap in the presence of
obstacles, less R-NN calls are initiated, thus, reducing the time spent on NN.
4.4
NN-sensitive parameters
Typically, when MP algorithms are evaluated, this is done after a careful tuning
of their parameters. However, in practice, certain algorithms might be used with
a sub-optimal set of parameters. This may be either due to lack of knowledge,
errors, or the inability to choose the right set of parameters.
In all planning
algorithms, one of the critical user-defined parameters, is the step size (STEP);
see Sec. 3. Using STEP which is too small may cause LP to be over-conservative
and costly. Choosing larger values which are still appropriate for the scenario at
hand allows to decrease the portion of time spent on CD checks.
12 We use eight robots, as this is the smallest number of robots where the trend is
easily discernible.
16
We demonstrate how RRT* becomes NN-sensitive under certain step-size
values, by testing the effect of the step size on χRRT*(N). We ran RRT* for a fixed
number of iterations N = 25K on the 3D Cubicles scenario (Fig. 2c). In order
to modify the step size in OMPL, one needs to specify a state validity-checking
resolution. This value, which we denote by RES, is specified as a fraction of the
space’s extent, that is, the maximal possible distance between two configurations
in the C-space. Using larger values of RES may yield paths that are invalid. Thus,
when increasing RES, we also used a model of the robot which was inflated
accordingly to ensure that all paths are collision free (see [36, Ch.5.3.4]). One
can see (Fig. 8) that there is a linear correlation between RES and χRRT*(N).
Indeed, Table 5 shows that when RES increases by a factor of 2, the number of
CD calls within LP operations decreases by roughly 2. Maybe more interesting
is that using the default OMPL value of 1%, CD takes roughly twenty times
more than NN. By changing this value (and also using an inflated model of the
robot), CD takes less than three times the amount of time spent on NN.
0.0
0.1
0.2
0.3
0.4
0.5
RRT*(N)
0.0
0.02
0.04
0.06
0.08
0.1
RES
Fig. 8:
χRRT*(N) as a function of the
state validity-checking resolution RES for
RRT* running on the 3D Cubicles scenario
(Fig. 2c).
RES #NN #R-NN #CD #LP
#CD in LP
0.01 25K 2.9K
25K 36.1K 715K
0.02 25K 3K
25K 36.5K 384K
0.04 25K 3K
25K 36.4K 197K
Table 5: Average number of calls for the
main primitive operations for different val-
ues of m (chosen arbitrarily).
5
Asymptotically-optimal motion-planning using R-NN
In this section we address an existing gap in the literature of sampling-based MP
algorithms: How to use Eq. 1 in non-Euclidean spaces, which are prevalent in
motion planning. Specifically, we derive closed-form expressions for the volume
of the unit ball in several common C-spaces and distance metrics and discuss
how to effectively use this value.
Closing this gap allows to evaluate the connection scheme of an algorithm.
Namely, should one choose connections using R-NN or K-NN. In NN-sensitive
settings this choice may have a dramatic effect on the performance of the algo-
rithm since (i) the number of reported neighbors may differ and (ii) the cost of
the two query types for a certain NN data structure may be different. Indeed,
we show empirically that there are scenarios where using R-NN, a solution path
of a specific cost is produced ten-fold (and more) faster than with K-NN.
17
5.1
Well-behaved spaces and the volume of balls
Recall that X denotes a C-space and that given a set A ⊆X, µ(A) denotes the
Lebesgue measure of A. Let ρ : X × X →R denote a distance metric and let
Bρ
X (r, x) := {y ∈X|ρ(x, y) ≤r} and Sρ
X (r, x) := {y ∈X|ρ(x, y) = r} denote
the ball and sphere of radius r (defined using ρ) centered at x ∈X, respectively.
Finally, let Bρ
X (r) := µ (Bρ
X (r, 0)) and Sρ
X (r) := µ (Sρ
X (r, 0)). We will often omit
the superscript ρ or the subscript X when they will be clear from the context.
We now define the notion of a well-behaved space in the context of metrics;
for a detailed discussion on well-behaved spaces see [40]. In such spaces there is
a derivative relationship between S(r) and B(r). Formally,
Definition 3 A space X is well behaved when ∂BX (r)
∂r
= SX (r). Conversely, we
say that X is well behaved when
R
ϱ∈[0,r] SX (ϱ)dϱ = BX (r).
We continue with the definition of a compound space which is the Cartesian
product of two spaces. Let X1, X2 be two C-spaces with distance metrics ρ1, ρ2,
respectively. Define X = X1 × X2 to be their compound space. We adopt a
common way13 to define the (weighted) distance metric over X, when using
weights w1, w2 ∈R+ and some constant p [36, Chapter 5]:
ρX = (w1ρp
1 + w2ρp
2)1/p .
(4)
The following Lemma states that the volume of balls in a compound space
X = X1 × X2 where X1 is well behaved can be expressed analytically.
Lemma 4 Following the above notation, if X1 is well behaved then
BX1×X2(r) =
Z
ϱ∈[0,r/w1/p
1
]
SX1(ϱ) · BX2
 rp −w1ϱp
w2
1/p!
dϱ.
(5)
Proof. By definition, BX (r) =
R
x∈BX (r) dx. Using Fubini’s Theorem [41],
BX1×X2(r) =
Z
x1∈BX1(r/w1/p
1
)



Z
x2∈BX2
 
rp−w1xp
1
w2
1/p! dx2


dx1.
The inner integral is simply the volume of a ball of radius

rp−w1xp
1
w2
1/p
in X2.
In addition, we know that X1 is well behaved, thus BX1(r) =
R
x1∈BX1(r) dx =
R
ϱ∈[0,r] SX1(ϱ)dϱ. By changing the integration variable, substituting the inner
integral and using the fact that X1 is well behaved we obtain Eq. 5.
⊓⊔
13 Eq. 4 is often used due to its computational efficiency and simplicity. However,
alternative methods exist, which exhibit favorable properties such as invariance to
rotation of the reference frame; see, e.g., [12].
18
5.2
Volume of balls in common C-spaces
In this section we demonstrate how to use Lemma 4 for some common C-spaces.
For a full list of C-spaces for which a closed-form expression for BX (r) was
derived, we refer the reader to the appendix.
SE(3)—The C-space of a spatial translating and rotating rigid body. Recall that
SE(3) = R3 × SO(3). We define the weighted distance metric of SE(3) according
to Eq. 4 with p = 1 (a common choice in MP [15]). In the appendix we show
that the following holds:
SR3 = 4πr2,
BSO(3)(r) = π(2r −sin 2r)
Using the fact that R3 is well behaved and by Lemma 4 we obtain that:
BSE(3)(r) =
Z
ϱ∈[0,r/w1]
(4πϱ2) · π
2(r −w1ϱ)
w2
−sin 2(r −w1ϱ)
w2

dϱ
= π2
3
1
w3
1w2

2r4 −6w2
2r2 + 3w4
2 −3w4
2 cos 2r
w2

.
(6)
SE(2)×SE(2)—The C-space of two translating and rotating planar robots. The
metric considered in this case is
ρSE(2)×SE(2) = w1ρR2 + w2ρS1 + w3ρR2 + w4ρS1.
We cannot compute the volume of a ball in a straightforward fashion as we do
not have a representation of SSE(2)(r). Thus, we start by computing the volume
of a ball in S1 × SE(2). In the appendix we show the following:
SS1(r) = 2,
BSE(2)(r) = 2π
3
1
w2
3w4
r3.
Since S1 is well behaved, we obtain,
BS1×SE(2)(r) =
Z
ϱ∈[0,r/w2]
2 · 2π
3
1
w2
3w4
r −w2ϱ
1
3
dϱ = π
3
1
w2w2
3w4
r4.
We now use the fact that SE(2) × SE(2) = R2 × (S1 × SE(2)) in order to
compute the value of BSE(2)×SE(2)(r). As we show in the appendix, SR2(r) = 2πr
and R2 is well behaved. Thus, we obtain:
BSE(2)×SE(2)(r) =
Z
ϱ∈[0,r/w1]
2πϱ · π
3
(r −w1ϱ)4
w2w2
3w4
dϱ = π2
45
1
w2
1w2w2
3w4
r6.
19
X
x
rn
B(rn, x)
(a)
1.0
1.1
1.2
1.3
1.4
1.5
Solution cost
0
10
20
30
40
50
60
70
80
Time [sec]
K-NN
R-NN Heuristic
(b)
Fig. 9: R-NN heuristic (a) A two-dimensional C-space for which Assumption 5 does not
hold. (b) Solution cost (optimal cost has a value of 1) as a function of time for Lazy-sPRM*
running in the Cubicles scenario (Fig. 2c). The dashed line visualizes the difference in time
for obtaining a solution of cost 1.28 between R-NN heuristic and K-NN.
5.3
Effective use of R-NN in MP algorithms in practice
We now discuss how to effectively use the radial connection scheme of asymp-
totically (near) optimal MP algorithms. We first describe a common scenario for
which the computed radii are practically useless, and continue by suggesting a
simple heuristic to overcome this problem.
The proofs of asymptotic optimality provided by Karaman and Frazzoli [28]
and by Janson et al. [27] rely on the following implicit assumption:
Assumption 5 For x ∈X, w.h.p. BX (r, x) = µ(BX (r, x) ∩X).
This assumption does not hold when the center of the ball is close to the bound-
ary of the C-space X. The reason why this assumption may be used in the afore-
mentioned proofs is that the proofs consider balls of radii proportional to rn
which tends to zero as n →∞. This, in turn, implies that, for a given path σ,
there exits a certain number of samples n0 for which the radius rn0 is sufficiently
small and hence the sequence of balls of radius rn0 covering σ does not intersect
the boundary of X.
In many common settings, Assumption 5 does not hold for practical values
of n. Consider, for example, Fig. 9a, which depicts a two-dimensional rectangular
C-space where one dimension is significantly larger than the other. For small
values of n, any ball of radius rn intersects the boundary of X (as the ball
B(rn, x), drawn in orange, for which B(rn, x)\X ̸= ∅). As a result, the number of
configurations of distance at most rn from a configuration x might be too small,
and this, in turn, may cause the roadmap of n vertices to remain disconnected.
We start by formally describing the setting and propose a heuristic to choose a
larger radius. Let X = X1×X2 be a d-dimensional compound C-space and assume
that µ(X1) ≥µ(X2). Let d1, d2 denote the dimensions of X1, X2, respectively.
Finally, let ρmax(X2) be the maximal distance between any two points in X2 and
assume that ρ = w1ρ1 + w2ρ2. When Assumption 5 does not hold, as in Fig. 9,
the intuition is that the “effective dimension” of our C-space is closer to d1 than
to d1 + d2. If rn > w2ρmax(X2) then ∀x ∈X BX (rn, x) > µ(BX (rn, x) ∩X). In
20
such cases, we suggest to project all points to X1 and use the critical connection
radius that we would have used had the planning occurred in X1.
To evaluate the proposed heuristic, we ran Lazy-sPRM* on the Cubicles
scenario (Fig. 2c) using R-NN with and without the heuristic, and also using K-
NN strategy (with the standard kn value). We measured the cost of the solution
path as a function of the running time. As depicted in Fig. 9b, the heuristic
was able to find higher-quality solutions using less samples, resulting in a ten-
fold speedup in obtaining a solution of a certain cost, when compared to K-NN.
Moreover, R-NN without the heuristic was practically inferior, as it was not able
to find a solution even for large values of n; results omitted.
6
Discussion and future work
We have described settings where the computational role of NN search after
a finite time is far from negligible when compared to that of CD. We have
characterized these settings and demonstrated this phenomenon empirically. By
developing improved NN-search techniques that are tailored for MP, the overall
running time of planners in NN-sensitive settings can be significantly reduced.
Identifying NN-sensitive settings is not always straightforward. Especially in
the case of NN-sensitive parameters, where one needs to determine whether a
set of parameters is sub-optimal for the problem at hand. It would be interesting
to identify additional NN-sensitive parameters, perhaps algorithm-specific ones
that are less general. Another open problem is whether it is possible to develop a
parameter-tuning technique (or a heuristic) such that the computational weight
of NN is increased. Then, by using NN techniques tailored to MP, the overall
performance of the algorithm can be significantly improved.
References
[1] Aiger, D., Kaplan, H., Sharir, M.: Reporting neighbors in high-dimensional Eu-
clidean space. SIAM J. Comput. 43(4), 1363–1395 (2014)
[2] Andoni, A., Razenshteyn, I.P.: Optimal data-dependent hashing for approximate
near neighbors. In: STOC. pp. 793–801 (2015)
[3] Arslan, O., Tsiotras, P.: Use of relaxation methods in sampling-based algorithms
for optimal motion planning. In: ICRA. pp. 2413–2420 (2013)
[4] Arya, S., Mount, D.M.: Approximate range searching. Computational Geometry:
Theory and Applications 17(3-4), 135–152 (2000)
[5] Arya, S., Mount, D.M., Netanyahu, N.S., Silverman, R., Wu, A.Y.: An optimal
algorithm for approximate nearest neighbor searching in fixed dimensions. Journal
of the ACM 45(6), 891–923 (1998)
[6] Bentley, J.L.: Multidimensional binary search trees used for associative searching.
Commun. ACM 18, 509–517 (1975)
[7] de Berg, M., Cheong, O., van Kreveld, M., Overmars, M.: Computational Geom-
etry: Algorithms and Applications. Springer-Verlag, 3rd edn. (2008)
[8] Beygelzimer, A., Kakade, S., Langford, J.: Cover trees for nearest neighbor. In:
International Conference on Machine Learning (ICML). pp. 97–104 (2006)
21
[9] Bialkowski, J., Otte, M.W., Karaman, S., Frazzoli, E.: Efficient collision checking
in sampling-based motion planning via safety certificates. I. J. Robotic Res. 35(7),
767–796 (2016)
[10] Bohlin, R., Kavraki, L.E.: Path planning using lazy PRM. In: ICRA. pp. 521–528
(2000)
[11] Brin, S.: Near neighbor search in large metric spaces. In: VLDB. pp. 574–584
(1995)
[12] Chirikjian, G.S., Zhou, S.: Metrics on motion and deformation of solid models. J.
Mech. Des. 120(2), 252–261 (1998)
[13] Choset, H., Lynch, K.M., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L.E.,
Thrun, S.: Principles of Robot Motion: Theory, Algorithms, and Implementation.
MIT Press (June 2005)
[14] Cohen, J.D., Lin, M.C., Manocha, D., Ponamgi, M.: I-COLLIDE: an interactive
and exact collision detection system for large-scale environments. In: Symposium
on Interactive 3D Graphics. pp. 189–196, 218 (1995)
[15] S¸ucan, I.A., Moll, M., Kavraki, L.E.: The Open Motion Planning Library. IEEE
Robotics & Automation Magazine 19(4), 72–82 (2012)
[16] Dobson, A., Bekris, K.E.: Sparse roadmap spanners for asymptotically near-
optimal motion planning. I. J. Robotic Res. 33(1), 18–47 (2014)
[17] Friedman, J.H., Bentley, J.L., Finkel, R.A.: An algorithm for finding best matches
in logarithmic expected time. ACM Trans. Math. Softw. 3(3), 209–226 (1977)
[18] Gammell, J.D., Srinivasa, S.S., Barfoot, T.D.: Informed RRT*: Optimal sampling-
based path planning focused via direct sampling of an admissible ellipsoidal heuris-
tic. In: IROS. pp. 2997–3004 (2014)
[19] Govindaraju, N.K., Redon, S., Lin, M.C., Manocha, D.: CULLIDE: interactive
collision detection between complex models in large environments using graphics
hardware. In: Graphics Hardware. pp. 25–32. Eurographics Association (2003)
[20] Hauser, K.: Lazy collision checking in asymptotically-optimal motion planning.
In: ICRA. pp. 2951–2957 (2015)
[21] Hemmer, M., Kleinbort, M., Halperin, D.: Optimal randomized incremental con-
struction for guaranteed logarithmic planar point location. Comput. Geom. 58,
110–123 (2016)
[22] Hubbard, P.M.: Approximating polyhedra with spheres for time-critical collision
detection. ACM Trans. Graph. 15(3), 179–210 (1996)
[23] Huynh, D.Q.: Metrics for 3d rotations: Comparison and analysis. Journal of Math-
ematical Imaging and Vision 35(2), 155–164 (2009)
[24] Ichnowski, J., Alterovitz, R.: Fast nearest neighbor search in SE(3) for sampling-
based motion planning. In: WAFR. pp. 197–214 (2014)
[25] Indyk, P.: Nearest neighbors in high-dimensional spaces. In: Goodman, J.E.,
O’Rourke, J. (eds.) Handbook of Discrete and Computational Geometry, chap. 39,
pp. 877–892. CRC Press LLC, Boca Raton, FL, 2nd edn. (2004)
[26] Indyk, P., Motwani, R.: Approximate nearest neighbors: Towards removing the
curse of dimensionality. In: STOC. pp. 604–613 (1998)
[27] Janson, L., Schmerling, E., Clark, A.A., Pavone, M.: Fast marching tree: A fast
marching sampling-based method for optimal motion planning in many dimen-
sions. I. J. Robotic Res. 34(7), 883–921 (2015)
[28] Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion plan-
ning. I. J. Robotic Res. 30(7), 846–894 (2011)
[29] Kim, S., Redon, S., Kim, Y.J.: Continuous collision detection for adaptive simu-
lation of articulated bodies. The Visual Computer 24(4), 261–269 (2008)
22
[30] Kim, Y.J., Redon, S., Lin, M.C., Manocha, D., Templeman, J.: Interactive contin-
uous collision detection using swept volume for avatars. Presence 16(2), 206–223
(2007)
[31] Kirkpatrick, D.G.: Optimal search in planar subdivisions. SIAM J. Comput. 12(1),
28–35 (1983)
[32] Kleinbort, M., Salzman, O., Halperin, D.: Efficient high-quality motion planning
by fast all-pairs r-nearest-neighbors. In: ICRA. pp. 2985–2990 (2015)
[33] Kleinbort, M., Salzman, O., Halperin, D.: Collision detection or nearest-neighbor
search? On the computational bottleneck in sampling-based motion planning.
CoRR abs/1607.04800 (2016)
[34] Koenig, S., Likhachev, M., Furcy, D.: Lifelong planning A*. Artif. Intell. 155(1-2),
93–146 (2004)
[35] Larsen, E., Gottschalk, S., Lin, M.C., Manocha, D.: Fast proximity queries with
swept sphere volumes. Tech. rep., Department of Computer Science, University
of North Carolina (1999), TR99-018
[36] LaValle, S.M.: Planning Algorithms. Cambridge University Press (2006)
[37] Li, S.: Concise formulas for the area and volume of a hyperspherical cap. Asian
Journal of Mathematics and Statistics 4(1), 66–70 (2011)
[38] Li, Y., Littlefield, Z., Bekris, K.E.: Sparse methods for efficient asymptotically
optimal kinodynamic planning. In: WAFR. pp. 263–282 (2014)
[39] Lin, M.C., Manocha, D.: Collision and proximity queries. In: Goodman, J.E.,
O’Rourke, J. (eds.) Handbook of Discrete and Computational Geometry, chap. 35,
pp. 767–786. CRC Press LLC, Boca Raton, FL, 2nd edn. (2004)
[40] Marichal, J.L., Dorff, M.: Derivative relationships between volume and surface
area of compact regions in Rd. Rocky Mountain J. Math. 37(2), 551–571 (2007)
[41] Mazzola, G., Milmeister, G., Weissmann, J.: Comprehensive Mathematics for
Computer Scientists 2, chap. 31, pp. 87–95. Springer, Berlin, Heidelberg (2005)
[42] Mitzenmacher, M., Upfal, E.: Probability and Computing: Randomized Algo-
rithms and Probabilistic Analysis. Cambridge University Press, New York, NY,
USA (2005)
[43] Otte, M., Frazzoli, E.: RRT-X: Asymptotically optimal single-query sampling-
based motion planning with quick replanning. I. J. Robotic Res. (2016), to appear
[44] Pan, J., Chitta, S., Manocha, D.: FCL: A general purpose library for collision and
proximity queries. In: ICRA. pp. 3859–3866 (2012)
[45] Plaku, E., Kavraki, L.E.: Quantitative analysis of nearest-neighbors search in
high-dimensional sampling-based motion planning. In: WAFR. pp. 3–18 (2006)
[46] Salzman, O., Halperin, D.: Asymptotically-optimal motion planning using lower
bounds on cost. In: ICRA. pp. 4167–4172 (2015)
[47] Salzman, O., Halperin, D.: Asymptotically near-optimal RRT for fast, high-
quality, motion planning. IEEE Trans. Robotics 32(3), 473–483 (2016)
[48] Salzman, O., Shaharabani, D., Agarwal, P.K., Halperin, D.: Sparsification of
motion-planning roadmaps by edge contraction. I. J. Robotic Res. 33(14), 1711–
1725 (2014)
[49] Schmerling, E., Janson, L., Pavone, M.: Optimal sampling-based motion planning
under differential constraints: The driftless case. In: ICRA. pp. 2368–2375 (2015)
[50] Solovey, K., Salzman, O., Halperin, D.: New perspective on sampling-based motion
planning via random geometric graphs. Robots Science and Systems (RSS) (2016)
[51] Vempala, S.: Randomly-oriented k-d trees adapt to intrinsic dimension. In: Foun-
dations of Software Technology and Theoretical Computer Science (FSTTCS).
pp. 48–57 (2012)
23
[52] Wang, X.: Volumes of generalized unit balls. Mathematics Magazine 78(5), 390–
395 (2005)
[53] Webb, D.J., van den Berg, J.: Kinodynamic RRT*: Asymptotically optimal mo-
tion planning for robots with linear dynamics. In: ICRA. pp. 5054–5061 (2013)
[54] Yershova, A., LaValle, S.M.: Improving motion-planning algorithms by efficient
nearest-neighbor searching. IEEE Trans. Robotics 23(1), 151–157 (2007)
[55] Zhang, X., Redon, S., Lee, M., Kim, Y.J.: Continuous collision detection for ar-
ticulated models using Taylor models and temporal culling. ACM Transactions
on Graphics (TOG) 26(3), 15 (2007)
24
A
Volume of ball in common C-spaces
In this section we derive closed-form expressions for the volume of the unit ball in
many common C-spaces and distance metrics. We start with the d-dimensional
Euclidean space Rd (considering two different metrics) and continue with the d-
dimensional unit sphere embedded in Rd+1 with geodesic distance as the metric.
These results are summarized in Table 6. We then continue to consider a set of
compound spaces for which the metric is defined by setting p = 1 in Eq. 4. This
choice follows the way metrics are defined in OMPL [15] for compound spaces.
All results can be extended straightforwardly to different values of p.
L2—Euclidean space with Euclidean metric Arguably, the most simple space
to be considered is the Euclidean space. Here X = Rd, and given two points
x, y ∈X such that x = (x1, . . . , xd), xi ∈R (similarly for y), the distance
metric is the standard Euclidean metric. Namely, ρ(x, y) =
P
i(xi −yi)2
1/2.
We denote by L2(d) the d-dimensional Euclidean space where ρ is the Euclidean
metric.
Computing the volume of the unit ball in this space is a well studied prob-
lem. For a detailed discussion on properties of the unit ball see https://en.
wikipedia.org/wiki/Volume_of_an_n-ball.
We have that,
BL2(2d)(r) = πd
d! r2d;
BL2(2d+1)(r) = 2(d! )(4π)d
(2d + 1)! r2d+1;
SL2(d)(r) = d
r Bd,r.
It is straightforward to see that L2(d) is a well-behaved space (according to
Definition 3).
L1—Euclidean space with Manhattan metric We consider the Euclidean space
X = Rd with a different distance metric, namely the Manhattan metric. Here
ρ(x, y) = P
i |xi −yi|. This is less common in motion-planning applications, but
serves as a simple example to several concepts that will be used. We denote
by L1(d) the d-dimensional Euclidean space where ρ is the Manhattan metric.
(r, 0)
(0, r)
(r, 0, 0)
(0, r, 0)
(0, 0, r)
Fig. 10: Balls of radii r in L1. Two dimensional ball (left) and one octant of a three
dimensional ball (right).
25
Computing the volume of the unit ball in this space is also well-studied (see,
e.g., [52]). Here we have that BL1(d)(r) =
2d/d!


· rd. Of specific interest are
the two- and three-dimensional cases where (see Fig. 10),
BL1(2)(r) = 2r2;
BL1(3)(r) = 4
3r3;
SL1(2)(r) = 4
√
2r;
SL1(3)(r) = 2
√
3r2.
Note that this C-space is not well behaved. Consider for example the two-
dimensional case. Clearly,
∂BL1(2)(r)
∂r
= ∂2r2
∂r
= 4r ̸= 4
√
2r = SL1(2)(r).
However, as described in [40], for any d-dimensional C-space X the transforma-
tion s(r) = d · BX (r)/SX (r) can be applied. Expressing the volume of a ball
and a sphere using s(r), instead of r, yields representations for which the ball-
sphere derivative property holds. We call these representations canonical and
denote them by ˜BX (s(r)) and ˜SX (s(r)), respectively. Lemma 4 can then be used
even if a space is not well behaved, assuming that one can produce analytic
representations of ˜BX (s(r)) and ˜SX (s(r)).
Sd—Unit Sphere with geodesic distance Let X be the unit hyper-sphere Sd em-
bedded in Rd+1, i.e., X = Sd ≡{q ∈Rd+1 | P
i x2
i = 1}. We use geodesic
distance as our metric: ρ(q1, q2) = arccos |q1 · q2|. We study this space as it will
allow us to obtain results for the spaces representing rotations in two and three
dimensions. It is well known that S1 ∼= SO(2) where A ∼= B denotes that A
is homeomorphic to B. When considered as the set of unit quaternions with
absolute value 1, S3 ≃SO(3) where A ≃B denotes that A is diffeomorphic
to B. See, e.g., [36, Chapter 4] for an in-depth discussion on different topological
structures and their connection to motion-planning configuration spaces. Choos-
ing geodesic distances is a common choice to compare rotations, see [23] for a
comparative study on different distance metrics for rotations. Furthermore, the
d-dimensional torus T(d) is defined as the Cartesian product of d unit circles
namely, T(d) =
S1
d. This is of interest as the configuration space of a planar
articulated robot with m links is R2 × T(m). Thus, we concentrate on Sd for
d = 1, 2 and 3.
Li [37] gives a general formula for the area of a hyper-spherical cap which is
exactly BSd(r) in our setting. Specifically, we have that
BS1(r) = 2r;
BS2(r) = 2π(1 −cos r);
BS3(r) = π (2r −sin(2r)) .
To compute SSd(r), note that the intersection of a hyperplane perpendicular
to one of the axes with a d-dimensional hyper-sphere is a hyper-ball of dimension
d −1. The radius of this ball is exactly sin r and we need its surface area (see
Fig. 11). Thus, we have that
SS1(r) = 2;
SS2(r) = 2π sin r;
SS3(r) = 4π sin2 r.
Note that S1 is well behaved while S2 and S3 are not. Using s(r) = 2(1 −
cos r)/sin r = 2 tan r
2 one can obtain the canonical forms for S2. Similarly, using
s(r) = 3(2r −sin(2r))/(4 sin2 r) the canonical forms for S3 can be obtained.
26
̸
= r
p
sin r
q
dist(p, q) = r
h = 1 −cos r
Fig. 11: Visualization of a spherical cap on the unit sphere S2 embedded in R3.
SE(2)—The configuration space of a planar translating and rotating rigid body
Recall that SE(2) = L2(2) × S1, the volume of a sphere in L2(2) is 2πr, while
the volume of a ball in S1 is 2r. In addition, recall that L2(2) is well behaved.
Using Eq. 4 as our distance metric and using Lemma 4 we have
BSE(2)(r) =
Z
ϱ∈[0,r/w1]
(2πϱ)(2(r −w1ϱ)/w2)dϱ = 2π
3
1
w2
1w2
r3.
(7)
T(d)—The configuration space of a kinematic chain in the plane Recall that
T(1) = S1 and that T(d) = S1 × T(d −1). Furthermore, the volume of a sphere
in S1 is 2 while the volume of a ball in S1 is 2r. In addition, recall that S1 is well
behaved. We start with the simple case of a two-dimensional torus T(2). Here,
using Eq. 4 as our distance metric and using Lemma 4 we have
BT(2)(r) =
Z
ϱ∈[0,r/w1]
2 · 2r −w1ϱ
w2
dϱ =
2
w1w2
r2.
(8)
C-space L2(2) L2(3) L1(2) L1(3) S1
S3
B(r)
πr2
4
3πr3
2r2
4
3r3
2r π(2r −sin(2r)
S(r)
2πr
4πr2 4
√
2r 2
√
3r2 2
4π sin2 r
B(1)
π
4
3π
2
4
3
2
π(2 −sin(2)
Table 6: Summary of B(r), S(r) and B(1) for several C-spaces.
27
In general, the volume of a ball in T(d) using the distance metric ρT(d) =
P
i wiρS1 is computed by using the recursive definition of T(n) to obtain
BT(d)(r) =
 
2d
d!
Y
i
1
wi
!
· rd.
(9)
L2(d) × L2(d)—The configuration space of two translating robots Considering
the Cartesian product of two Euclidean spaces comes naturally in the case of
two translating robots in Rd for d ∈{2, 3}. Recall that ∀d L2(d) is well behaved.
Thus, for the case of d = 2 and using Eq. 4 as our metric we have
BL2(2)×L2(2)(r) =
Z
ϱ∈[0,r/w1]
(2πϱ) · π
r −w1ϱ
w2
2
dϱ = π2
6
1
w2
1w2
2
r4.
(10)
For the case of d = 3 and using Eq. 4 as our metric we have
BL2(3)×L2(3)(r) =
Z
ϱ∈[0,r/w1]
(4πϱ2) · 4π
3
r −w1ϱ
w2
3
dϱ = 4π2
45
1
w3
1w3
2
r6.
(11)
L2(2)m—The configuration space of m translating planar robots We general-
ize the above results for the case of m translating planar robots. The metric
considered is ρ(L2(2))m = Σm
i=1wiρL2(2).
By generalizing the above approach, we have that
B(L2(2))m = Cmπm ·
 m
Y
i=1
1
w2
i
!
· r2m,
(12)
where Cm =
Cm−1
m(2m−1) and C1 = 1.
L2(3)m—The configuration space of m translating spatial robots For the case of
m translating spatial robots, when the metric considered is ρ(L2(3))m = Σm
i=1wiρL2(3)
and using similar arguments, we have that
B(L2(3))m = Cmπm ·
 m
Y
i=1
1
w3
i
!
· r3m,
(13)
Where Cm =
8Cm−1
3m(3m−1)(3m−2) and C1 = 4/3.
(SE(2))m—The configuration space of m translating and rotating planar robots
We generalize the result we obtained for two robots for the case of m translating
and rotating planar robots. The metric considered is ρ(SE(2))m = Σm
i=1w2i−1ρL2(2)+
w2iρS1.
We have that
B(SE(2))m = Cmπm ·
 m
Y
i=1
1
w2
2i−1w2i
!
· r3m,
(14)
where Cm =
4Cm−1
3m(3m−1)(3m−2) and C1 = 2/3.
28