RMPD – A Recursive Mid-Point Displacement Algorithm for Path Planning
Fangda Li and Ankit V. Manerikar and Avinash C. Kak
School of Electrical and Computer Engineering, Purdue University
465 Northwestern Avenue
West Lafayette, Indiana 47907
{li1208, amanerik, kak}@purdue.edu
Abstract
Motivated by what is required for real-time path planning,
the paper starts out by presenting RMPD, a new recursive
“local” planner founded on the key notion that, unless made
necessary by an obstacle, there must be no deviation from
the shortest path between any two points, which would nor-
mally be a straight line path in the conﬁguration space. Sub-
sequently, we increase the power of RMPD by introducing
the notion of cost-awareness into the algorithm to improve
the path quality – this is done by associating obstacle and
smoothness costs with the currently selected path points and
factoring those costs in choosing the best points for the next
iteration. In this manner, the overall strategy in the cost-aware
form of RMPD, cRMPD, combines the computational efﬁ-
ciency made possible by the recursive RMPD planner with
the cost efﬁcacy of a stochastic trajectory optimizer to rapidly
produce high-quality local collision-free paths. Based on the
test cases we have run, our experiments show that cRMPD
can reduce planning time by up to two orders of magnitude
as compared to RRT-Connect, while still maintaining a path
length optimality equivalent to that of RRT*.
1
Introduction
Path planning has been an important area of research in
robotics over the last several decades. Path planning algo-
rithms have important uses in robotic assembly (Nof 1999),
autonomous driving (Kuwata et al. 2009), kinematic and dy-
namic control for robots, etc.
In traditional approaches to path planning, one ﬁrst over-
lays a grid of points on the conﬁguration space and then
develops an obstacle-free path incrementally from the start
conﬁguration to the goal conﬁguration, going from one grid
point to the next in the process. Subsequently, a search is
carried out over the paths thus discovered to ﬁnd the opti-
mal path that connects the goal conﬁguration with the start
conﬁguration. These algorithms are known to work well in
low-dimensional conﬁguration spaces. However, as the di-
mensionality of the space increases, they extract a large per-
formance penalty.
More recently, sampling based algorithms have gained
considerable prominence. The basic idea of such algorithms
is to sample the conﬁguration space at randomly selected
Copyright c⃝2018, Association for the Advancement of Artiﬁcial
Intelligence (www.aaai.org). All rights reserved.
Figure 1: RMPD uses recursion to efﬁciently ﬁnd a detour
around an obstacle. Shown as p1
m and p2
m are the mid-points
of two candidate path segments, each of which contains at
least one in-collision point. The detouring collision-free re-
placements for the mid-points are shown as p1
f and p2
f. The
dashed circles represents the search hypersphere for middle
point replacement.
points until a connection of the local pathways thus con-
structed can lead one from the start conﬁguration to the goal
conﬁguration. These are best exempliﬁed by the Probabilis-
tic Roadmap Method (PRM) (Kavraki et al. 1996), the Ran-
domized Potential Fields (Barraquand and Latombe 1993),
and the Rapidly-exploring Random Tree (RRT) (LaValle
1998). The important factors that account for the popular-
ity of such algorithms include : (1) They can be used with
greater ease in high dimensional conﬁguration spaces com-
pared to the traditional algorithms; (2) The algorithms based
on PRM and RRT can be shown to be probabilistically com-
plete; (3) The ease in merging partially developed solutions
in order to respond simultaneously to multiple path planning
queries; etc.
In our laboratory we have been exploring the use of RRT
and RRT-Connect (Kuffner and LaValle 2000) algorithms
for real-time motion planning as needed for automatic prun-
ing of dormant trees — a crucial step in growing healthy
apple orchards. Our main concern has been the limited ex-
tent to which a straightforward application of RRT-Connect,
as originally formulated by its authors, can generate smooth
and concise paths in obstacle-dense regions.
To address this shortcoming of the randomized algo-
arXiv:1709.00488v2  [cs.RO]  26 Feb 2018
rithms, we propose a new approach here. The ﬁrst part of
our approach consists of a novel “local” planner that is based
on the key notion that, unless made necessary by an obsta-
cle, there must be no deviation from the shortest path be-
tween any two points, which would normally be a straight
line path in the conﬁguration space. We refer to this basic
path planner as the RMPD algorithm or the single-path “Re-
cursive Mid-Point Displacement” algorithm. Its basic idea is
recursive: Check each sampling point on the path connect-
ing two end-points for being collision free. If that condition
is not satisﬁed at any sampling point, move the mid-point to
a collision-free location to create a detour, divide the result-
ing path into sub-paths and attempt to ﬁnd a collision-free
detour for each such sub-path. If the detour is again in colli-
sion, further divide the sub-path, and so on.
Subsequently, we increase the power of RMPD by includ-
ing cost-awareness in the algorithm. This is done by modify-
ing the basic search strategy of RMPD in a manner similar to
that of STOMP (Stochastic Trajectory Optimization Motion
Planning) (Kalakrishnan et al. 2011) in order to steer the it-
erative sampling distributions towards cost-optimal regions
in the sampling space. As a consequence, the cost-aware
RMPD, which we denote cRMPD, not only optimizes for
computational efﬁciency during path-planning but does so
with a degree of cost-awareness in producing collision-free
paths between the source and the goal points.
Following (Kalakrishnan et al. 2011) and (Ratliff et al.
2009), the cost function incorporated in the cRMPD planner
consists of an obstacle cost calculated from a local potential
ﬁeld as well as a smoothness cost to avoid rapid accelera-
tions. This cost is calculated for each sampled point in the
current iteration and is used to update the sampling distribu-
tion in the subsequent search. It would seem that, while one
can expect this cost-function weighted sampling strategy to
improve the cost efﬁciency of the planner, it would do so by
slowing down the planner. It is interesting to note, however,
that the overall time efﬁciency of the cRMPD planner does
not suffer because the search for obstacle-free paths is more
likely to be biased towards collision-free regions of the con-
ﬁguration space. That search bias results in the ﬁnal paths
to be discovered in a shorter time. We will demonstrate this
effect through our experimental results.
In the experimental section, we demonstrate our algo-
rithm using both 2D bitmap and 3D environments with nar-
row passages that must be traversed by a path from the start
state to the goal state. In addition, we evaluate the perfor-
mance of cRMPD using a simulation of apple tree pruning.
Based on the test cases we have run, we observe that cRMPD
signiﬁcantly reduces the average path length and also the
control effort required. Furthermore, cRMPD reduces the
planning time by two orders of magnitude in some of the
test cases vis-a-vis the other algorithms chosen for compar-
ison.
In the rest of the paper, Section 2 presents the related work
that is most relevant to cRMPD. In particular, this section
provides an overview of sampling based algorithms along
with the related past research that has focused on addressing
the shortcomings of these algorithms in exploring narrow
passages as well as their cost-effectiveness. Subsequently,
Section 3 presents our cRMPD algorithm. Section 4 presents
the experimental results that demonstrate the efﬁciency of
the cRMPD algorithm vis-a-vis its main competition. Fi-
nally, we conclude in Section 5.
2
Related Work
We start with an overview of the sampling based algorithms
for path planning. Such algorithms are generally either
graph-based or tree-based. Graph based motion planning al-
gorithms, with Probabilistic Roadmap Method (Kavraki et
al. 1996) being the most representative, are widely used in
multi-query motion planning scenarios. A “roadmap” only
needs to be constructed once by such planners for static
workspaces; the same roadmap can subsequently be used to
handle multiple queries. On the other hand, tree based algo-
rithms, such as the Rapidly-exploring Random Tree (RRT)
(LaValle 1998), build space ﬁlling trees with biased growth
toward large unexplored regions.
From the standpoint of efﬁciency, a problem with ran-
domized sampling based algorithms, such as RRT, is that
they tend to carry out excessive unnecessary sampling as
only a tiny fraction of what is explored in the conﬁgura-
tion space is eventually used for constructing the solution
path. Many approaches have been proposed to address this
problem. For example, for paths in the vicinity of narrow
passages, one can increase the odds of a path traversing
those passages by directly biasing the sampling to favor
obstacle-dense regions (Lee et al. 2012; Hsu et al. 2003;
Zhang and Manocha 2008; Urmson and Simmons 2003) or
by actively reducing the sampling in wide-open free spaces
(Lee et al. 2012; Urmson and Simmons 2003; Kuffner and
LaValle 2000; Clifton et al. 2008). The ﬁrst strategy is re-
ferred to by “Narrow-passage Favored Sampling” and the
second by “Open-Space Selective Sampling”, respectively.
In what follows, we will provide brief reviews of these two
strategies.
Narrow-passage Favored Sampling aims explicitly to
sample more densely in potentially narrow regions of the
conﬁguration space. In order to identify such regions, Hsu et
al. has proposed a bridge test in which a bridge is deﬁned as
a line segment consisting of two in-collision endpoints and
one free middle point (Hsu et al. 2003). Other approaches
such as by (Zhang and Manocha 2008) propose a retraction-
based modiﬁcation to RRT, which generates dense samples
near obstacles at the cost of a higher computational over-
head. Retracting a sample in this context refers to the pro-
cess of resampling around the sample with a bias to improve
upon the current choice of the sample with respect to a cer-
tain metric. The contribution reported in (Lee et al. 2012)
attempts to integrate both these methods by retracting the
samples that pass the bridge test.
On the other hand, the main idea of Open-space Selec-
tive Sampling is to implicitly bias the sampling toward nar-
rower regions in the conﬁguration space by biasing it against
wide-open regions. An approximate approach for doing so is
proposed in (Lee et al. 2012) in which a free hypersphere is
associated with every non-contact RRT node and samples
falling within any of the free hyperspheres are discarded.
In the bidirectional RRT algorithm, namely RRT-Connect
(Kuffner and LaValle 2000), a greedy path extension func-
tion decreases the need for excessive sampling in the open
regions of the conﬁguration space. Moreover, with the same
path extension logic, an arbitrary number of RRTs can be
grown simultaneously to increase the pace of exploration in
narrow regions, as described in (Clifton et al. 2008).
Our algorithm incorporates both strategies mentioned
above in an intuitive manner. By aggressively connect-
ing samples with straight lines, whenever that is possible,
RMPD signiﬁcantly diminishes the over exploration ten-
dency of RRT. At the same time, RMPD explores more
densely in the vicinity of narrow regions through the detour-
ing logic triggered by in-collision samples.
In general, despite being probabilistic complete, the tra-
ditional formulations of sampling based algorithms remain
agnostic regarding the costs associated with the paths. For-
tunately, it has been known for some time that it is possible
to include cost considerations in sampling based approaches
to motion planning (Karaman and Frazzoli 2011). While the
contribution in (Karaman and Frazzoli 2011) can serve as a
guide to combining cost considerations with sampling based
planners, the paths produced still need post-processing in
order to be viable. In that context, it is interesting to note
that the trajectory optimization based approach as incorpo-
rated in CHOMP (Ratliff et al. 2009) requires no such post-
processing. CHOMP uses Hamiltonian cost optimization to
generate smooth motion paths. However, the gradient de-
scent method utilized in CHOMP often result in entrapment
in local minima.
To get around the problems caused by getting trapped in
local minima, Kalakrishnan et al. developed the STOMP
planner that uses stochastic sampling to search for cost op-
timal paths (Kalakrishnan et al. 2011). We use this method
to transform RMPD into the cost-aware algorithm, cRMPD.
cRMPD combines the time-efﬁciency of sampling-based
path search with the cost-effectiveness of an optimal-control
planner to rapidly generate local cost-aware paths for solv-
ing motion planning problems. In Section 4, we compare
the performance of cRMPD with the other algorithms men-
tioned in this section.
3
RMPD – Recursive Mid-Point
Displacement Algorithm
In this section, we propose a novel path planning algorithm
we call RMPD, for Recursive Mid-Point Displacement, that
is based on a simple path construction idea: Don’t deviate
from a straight-line unless absolutely necessary on account
of the obstacles.
We ﬁrst present the base version of the algorithm, which
focuses on overcoming local obstacles without much com-
putational overhead. In order to ﬁnd a path, RMPD connects
any two nodes with a straight line, which is checked for in-
collision property using a local planner. If this property is
found to be true at any sampling point on the straight-line
path, tangential detours around the obstacle are made recur-
sively by replacing the line’s middle point.
Subsequently we substitute the collision-free middle point
search in RMPD with a cost-aware exploration. The derived
algorithm, cRMPD, thereby inherits the time efﬁciency of
RMPD and further improves the path quality by incorporat-
ing a numerical cost function.
3.1
Single-path RMPD
In contrast to the retraction methods described in (Zhang
and Manocha 2008; Lee et al. 2012), our basic algorithm,
RMPD, navigates its way around obstacles with inexpensive
resampling, while leveraging the idea of divide and conquer.
As the recursion goes deeper, RMPD focuses on ﬁnding a
warped collision-free replacement for a shorter path segment
within its local area.
The steps of RMPD are presented in Algorithm 1. Using
the notation shown in Figure 1, given a start point ps and
a goal point pg in a conﬁguration space, the algorithm ﬁrst
validates ps and pg and returns with failure if either ps or pg
is in collision. If both states are valid, RMPD invokes itself
recursively to populate an initially empty path θ rooted at
ps. If the condition returned by the terminating recursion is
true, we have found a valid path θ.
Algorithm 1 RMPD(θ, ps, pg)
1 if ps or pg is in collision
2
return false
3 if (ps, pg) is in collision
4
pm ⇐(ps + pg)/2
5
if pm is in collision
6
σ ∝∥ps −pg∥
7
pf ⇐GaussianFreeStateSampler(pm, σ)
8
else
9
pf ⇐pm
10
return RMPD(θ, ps, pf) and RMPD(θ, pf, pg)
11 else
12
θ.append(pg)
13
return true
As a key step, RMPD recursively breaks a path segment
into two potentially collision-free sub-paths and appends the
middle point to the ﬁnal path, with the main consideration
behind employing the middle point for displacement being
simplicity. In line 3 of Algorithm 1, RMPD ﬁrst attempts to
connect ps and pg directly with a straight line (ps, pg) using
a bidirectional local planner, which validates the segment by
marching from the middle point to the two ends in an alter-
nating fashion. If no obstacle is encountered, the function
simply adds pg to θ as a waypoint in the ﬁnal path (line 12).
On the other hand, if a path segment (ps, pg) encounters an
obstacle at any sampling point, RMPD tries to make a tan-
gential detour around the obstacle by breaking (ps, pg) into
two halves.
First, pm, the middle point of ps and pg, is checked
for
collision.
In
case
of
collision,
pm
is
replaced
by
a
nearby
collision-free
point
pf,
returned
by
GaussianFreeStateSampler(pm, σ)
(line
7).
The
standard deviation of the Gaussian sampler is set propor-
tional to the distance between ps and pg, which ensures
locality of the search for a free middle point. If the maxi-
mum number of iterations is reached before the discovery
of a free point, the sampler simply returns its last failed
candidate, which causes the validity check in the next level
of recursion to fail (line 1). Finally, RMPD recursively
invokes itself with both sub-paths, (ps, pf) and (pf, pg)
(line 10). Recursion terminates automatically when the
waypoint states in θ connect ps to pg successfully with
consecutive collision-free path segments.
In the implementation of RMPD, we specify the max
number of waypoints in the path, Nmax. If the initial ps and
pg are still not connected after having Nmax waypoints in
θ, we deem that RMPD invocation to have failed to ﬁnd a
solution.
3.2
Cost-Aware RMPD (cRMPD)
While the time efﬁciency of a planner is desirable in real-
time planning scenarios, path quality is yet another im-
portant discriminating factor. In this section, we introduce
a cost-aware modiﬁcation to the search strategy for the
middle point in RMPD as implemented by the call to
GaussianFreeStateSampler in Line 7 of Algorithm 1.
As opposed to keeping only the ﬁrst feasible sample in the
basic RMPD, cRMPD computes the cost of a sampled point
regardless of its validity and then aggregates the cost infor-
mation with those of other nearby points for the purpose of
choosing the best candidate for the next middle point.
cRMPD
is
implemented
by
replacing
the
call
to
GaussianFreeStateSampler
in
Line
7
of
Algo-
rithm 1 by a call to CostAwareFreeStateSampler
whose pseudo code is shown in Algorithm 2. Note
that CostAwareFreeStateSampler includes a call to
GaussianSampler in line 8 of Algorithm 2. For the
difference between GaussianFreeStateSampler used
in RMPD and GaussianSampler used in cRMPD, the
former carries out repeated sampling until a collision free
point is found, whereas the latter returns a sample regardless
of its collision property.
In
every
iteration,
the
Gaussian
sampler
(GaussianSampler) with mean pm and standard de-
viation σ ﬁrst generates K random samples (lines 7 and 8).
Similar to what is done in RMPD, the standard deviation σ
of the Gaussian sampler is proportional to the straight line
length. Subsequently, the differences between the random
samples and the current middle point are added together
using exponentiated weights to produce the update δp (line
10). The sampling distribution in the next iteration is cen-
tered at the updated middle point, which is essentially the
weighted average of the previous K samples. The weights
are computed with the softmax function that is based on the
cost of each sample f(pi) (line 9), where h is a constant.
These weights can also be interpreted as probabilities used
in calculating the expectation of the true gradient. Using the
exponentiated weights as shown amounts to implementing
the EGD (Estimated Gradient Descent) method presented
in (Kalakrishnan et al. 2011). The search for the optimal
middle point using this approach is repeated until either
the maximum number of attempts is reached or the cost
converges.
Algorithm 2 CostAwareFreeStateSampler(ps, pm, pg)
1 c ⇐+∞
2 δp ⇐0
3 σ ∝|ps −pg|
4 do
5
cprev ⇐c
6
pm ⇐pm + δp
7
for i = 1, ..., K
8
pi ⇐GaussianSampler(pm, σ)
9
wi ⇐
e−hf(pi)
PK
j e−hf(pj )
10
δp ⇐PK
i wi(pi −pm)
11
c ⇐f(pm)
12 while cprev −c > ϵ
13 return pm
In a manner similar to STOMP, through the loop in lines
4 through 12, cRMPD employs a Monte Carlo approach for
the cost function minimization. Moreover, if one regards the
problem of ﬁnding an optimal middle point as a special case
of STOMP with only three waypoints, the random explo-
ration strategy on trajectories by adding multivariate Gaus-
sian noise is then analogous to Gaussian sampling for dis-
covering the best middle point.
While cRMPD can accommodate any arbitrary cost
function, in the interest of computational efﬁciency in a
sampling-based context, the cost function f(p) should be
only state dependent and should be computable in constant
time. Following the cost functions often used in trajectory
optimization algorithms (Ratliff et al. 2009; Kalakrishnan et
al. 2011), the cost function in cRMPD consists of a clearance
term and a smoothness term:
f(p) = fclr(p) + λfsmt(p).
(1)
The clearance term, necessary for ﬁnding tangential detours
around the obstacles, is taken from a signed distance ﬁeld
in the conﬁguration space. The magnitude of fclr(p) rep-
resents the distance from p to the closest point of opposite
in-collision property1:
fclr(p) = sgn(p) · min{|p −pi| | pi ∈P−sgn(p)}.
(2)
The polarity function sgn(p) returns either 1 or −1 such that
fclr is positive when p is in collision.
1What that means is that if p is in-collision, meaning it is inside
an obstacle, then we seek the closest point that is collision free. On
the other hand, if p is collision-free, we seek that point that is on
the nearest obstacle.
The smoothness term is deﬁned as:
fsmt(p) = |ps −p| + |p −pg| −|ps −pg|.
(3)
Basically, fsmt punishes any middle point that deviates
from the original straight line path, and thereby incentivizes
shorter and smoother paths.
The cost-aware sampling strategy of the cRMPD planner
allows it to be likened to the STOMP planner (Kalakrish-
nan et al. 2011), albeit with one main distinction. The dis-
tinguishing factor between the cRMPD and STOMP plan-
ner is the number of waypoints accounted for during the
iterative sampling – a STOMP planner samples a new set
of n-point trajectories for each iteration, where n is ﬁxed
throughout the path planning operation. On the other hand,
a cRMPD planner recursively samples path points to replace
the middle-point of a path segment to form a collision-free
path. Thus, whereas CostAwareFreeStateSampler can
be considered as a special case of STOMP in which the
number of waypoints is ﬁxed to be three, in cRMPD the
total number of waypoints increases with each iteration as
required by the planner. This ﬂexibility in the number of
waypoints therefore allows cRMPD to adjust discretization
of the path to suit the complexity of the workspace.
4
Experiments
In this section, we compare the performance of cRMPD with
other sampling-based algorithms, namely RRT (LaValle
1998), RRT-Connect (Kuffner and LaValle 2000), RRT*
(Karaman and Frazzoli 2011) and PRM (Kavraki et al.
1996). The algorithms are benchmarked on three planning
problems: point robot on a 2D bitmap, the Piano Mover’s
problem, and the Twistcooler problem. The planning sce-
narios are chosen to be rich in narrow passages such as the
middle hole in Twistcooler and the narrow corridor in Piano
Mover. The maximum path-planning time was set to be suf-
ﬁciently high to allow RRT* to converge and a near-optimal
cost path could be found. We show that cRMPD achieves a
more favorable trade-off between time efﬁciency and path
quality than RRT and its popular variants. Subsequently, the
performance of cRMPD is evaluated against the trajectory
optimizer STOMP in a tree pruning benchmark. We demon-
strate that cRMPD is capable of generating smooth paths
with quantitative metrics comparable to those by the trajec-
tory optimizer, while using much less time.
4.1
Experimental Setup
We have implemented cRMPD in C++ within the OMPL
(Sucan, Moll, and Kavraki 2012) framework. OMPL also
provides optimized implementations for the competing
sampling-based algorithms — this ensures fairness when
comparing experimental results. For the tree pruning bench-
mark, we used the publicly available STOMP implemen-
tation (Kalakrishnan 2011) within the MoveIt! (Sucan and
Chitta 2011) framework. All experiments were repeated 30
times on a 2.30 GHz Intel i7 processor with 8 GB of RAM
and the average results are presented.
For all the paths produced by sampling-based planners,
including cRMPD, all quantitative measurements are ex-
tracted after post-processing. This post-processing includes
iterative path shortcutting followed by B-spline ﬁtting per
the OMPL implementation. Although the paths produced
by the different planners in our comparative evaluation
are more or less similar with and without post-processing,
we only present the results on smoothed paths since post-
processing is a crucial step for any geometric planner to
generate physically executable paths (Elbanhawi and Simic
2014). As long as this post-processing step is identical for
all the planners in a comparative evaluation, no one plan-
ner gets any advantage. Additionally, we do the following to
compute the smoothness of a path θ:
1. Upsample the path uniformly to M waypoints, so that
every two consecutive waypoints are equally spaced in the
conﬁguration space Rn.
2. Apply an M ×M second-order ﬁnite differencing matrix
A to the interpolated path θ′ ∈RM×n (Equation 5).
3. Sum the norms of the M vectors in θ′ to obtain the ﬁnal
smoothness value qsmt.
A =


1
0
0
0
0
0
−2
1
0
· · ·
0
0
0
1
−2
1
0
0
0
...
...
...
0
0
0
1
−2
1
0
0
0
· · ·
0
1
−2
0
0
0
0
0
1


(4)
¨θ′ = Aθ
(5)
Note that qsmt measures the total amount of acceleration or
joint effort needed by the robot to traverse the path.
Lastly, as for the choice of the parameters λ = 0.5,
h = 5.0 and Nmax = 100, these were determined em-
pirically. Choosing K carefully is critical to the success of
the algorithm. As the value of K increases, one tends to get
shorter and smoother paths but at the expense of longer plan-
ning time. We noticed in our experiments that the beneﬁts of
a larger value for K saturate at around K = 10. So we have
set K to this value in all experiments with cRMPD. Addi-
tionally, setting σ in the CostAwareFreeStateSampler
to be 1/6 of the distance between the start and the goal (line
3 in Algorithm 2) yielded good results and was used for all
experiments.
4.2
Point Robot on 2D Bitmap
The point robot, with 2 translational DoF, in the bitmap case
studied by us must navigate from the bottom left corner of
the map to the top right corner, as shown in Figure 2a. The
environment contains a cluster of obstacles that are placed to
form a narrow direct passage to the goal along the diagonal,
while creating an open region near the bottom right corner.
In this benchmark, every run is given a 0.5s quantum, which
is sufﬁcient for the cost of RRT* to converge. Through the
quantitative results in Table 1, we observe that although RRT
and RRT-Connect score higher with regard to planning time,
they are likely to choose suboptimal path segments in the
open regions of the conﬁguration space. This observation
is best supported by the comparison on path lengths. On
(a)
(b)
(c)
(d)
(e)
(f)
Figure 2: Spatial layouts for the four benchmarks and selected results. (a) A sample solution by cRMPD on the 2D bitmap.
The approximated clearance cost of a sample is indicated by the color. (b) The Piano Mover’s problem. (c) The Twistcooler
Problem. (d) Mesh of the scanned apple tree. To prune a branch, the end effector must reach the pink marker while staying
aligned to the blue arrow. (e) An example pruning pose used in the benchmark. (f) An example path found by cRMPD.
Table 1: 2D point robot benchmark results. Every column is
normalized against the best performer to extract the relative
performance (# CC = number of collision checks).
Time
Length
# CC
RRT
1.07
1.15
1.36
RRT-C
1.00
1.15
1.23
RRT*
427.34
1.00
43.28
cRMPD
31.84
1.03
1.00
the other hand, navigating through the narrow diagonal pas-
sage poses little challenge for the cRMPD planner. With a
straight line initialization, cRMPD achieves a competitive
average path length comparable to what is returned by RRT*
although RRT* takes more than 10 times longer to ﬁnd the
path. Also note that our state-dependent cost function allows
cRMPD to save on the number of collision checks.
4.3
Path Planning in 3D Workspaces
To examine the characteristics of cRMPD in 3D workspaces,
we study two rigid-body motion planning problems. The
set of transformations for all rigid-body motions in 3D
workspaces forms the special Euclidean manifold, SE(3).
This manifold spans a 6-dimensional space consisting of two
subspaces: a 3D Euclidean space for translations and another
3D space in the special Orthogonal group SO(3) that de-
scribes rotations.
It is noteworthy that the distance between any two points
on the SE(3) manifold is deﬁned as a simple summation of
distances in the two subspaces: ℓ2-norm in the Euclidean
subspace and arc-length on the SO(3) manifold (Kuffner
2004). To avoid the complications created by averaging the
states in SE(3), as required by the EGD (estimated gradi-
ent descent) calculation in line 10 of Algorithm 2, cRMPD
adopts a greedy strategy for choosing the middle point – the
updated middle point in each iteration is simply the one with
the lowest cost among the K samples. As the reader will see
shortly, this works well in practice.
Of the two 3D path planning problems we ﬁrst show re-
sults for the classic Piano Mover’s problem, as seen in Fig-
ure 2c, where a piano must be maneuvered through the living
room and the narrow corridor at the top right. The results,
presented in Table 2, show that cRMPD surpasses its com-
petitors by a signiﬁcant margin with respect to nearly all the
metrics. While being more than 10 times faster than RRT-
Connect, cRMPD still returns the shortest and smoothest
path, even compared to the optimal planner RRT*.
In the second 3D path planning problem, the Twistcooler
problem, a 5-link rigid-body robot must navigate through a
small hole in a barrier in the middle while there exist large
Table 2: Relative performance for the Piano Mover’s prob-
lem (SR = Success Rate and qsmt = Smoothness Cost).
Time
Length
qsmt
# CC
SR (%)
RRT
32.74
1.25
8.75
71.46
80.0
RRT-C
15.17
1.22
7.78
38.55
100.0
RRT*
61.35
1.02
8.01
186.28
73.3
PRM
37.62
1.21
9.20
137.29
76.7
cRMPD
1.00
1.00
1.00
1.00
96.7
Table 3: Relative performance for the Twistcooler problem.
Time
Length
qsmt
# CC
SR (%)
RRT
279.01
1.49
16.23
430.05
96.7
RRT-C
428.22
1.55
15.49
665.52
73.3
RRT*
746.27
1.00
11.74
152.96
60.0
PRM
52.81
1.52
21.62
882.87
100.0
cRMPD
1.00
1.00
1.00
1.00
90.0
open spaces on the two sides of the barrier. The Twistcooler
problem is deceivingly challenging: the small hole in the
middle and the stretched shape of the robot form a elon-
gated narrow passage in the conﬁguration space. For RRT
and its variants, a typical run lasts several hundreds of sec-
onds before a solution is found. We did try to combine those
planners with obstacle-based samplers that generate samples
close to obstacles2, yet no major improvement in planning
time was observed. The PRM planner achieved the highest
success rate and improved time efﬁciency, however it did so
with compromised path smoothness and the most number of
collision checks.
As shown by the benchmarking results tabulated in Ta-
ble 3, cRMPD possesses superior ability to navigate through
narrow passages. Quantitatively speaking, cRMPD is able to
produce a path within a time span that is one order of mag-
nitude shorter than any other planner. In addition, cRMPD
is able to do so without compromising the path quality, as
demonstrated the ﬁnal solution length and smoothness.
4.4
Dormant Apple Tree Pruning
We now show results for a real-world problem: dormant
apple tree pruning with a 6-DoF robot arm. The mesh of
the tree is obtained by scanning a real dormant tree us-
ing an RGB-D sensor. To prune a branch, the end effec-
tor of the robot arm must reach the base of that branch
in a perpendicular direction, as demonstrated by the ex-
ample pruning pose in Figure 2e. This benchmark con-
sists of 13 queries in total and each planner is tasked to
solve the query 10 times with a maximum allowable time
of 5s. Interestingly, the thin spindly trees would present
challenges in the form of non-convex cost surfaces for any
optimization-based planner, including STOMP and cRMPD.
2An obstacle-based sampler ﬁrst takes two samples, one valid
and the other invalid. Subsequently, it interpolates from the invalid
sample to the valid sample, returning the ﬁrst valid sample encoun-
tered (2012).
Table 4: Relative performance on the tree pruning bench-
mark.
Time
Length
qsmt
SR (%)
RRT
17.28
1.47
2.16
91.5
RRT-C
1.00
1.12
15.49
100.0
RRT*
37.55
1.19
1.34
93.1
STOMP
24.75
1.00
1.00
41.5
cRMPD
1.81
1.07
1.68
100.0
In the case of cRMPD, our experiments have shown that
when the EGD begins with a naive middle point initial-
ization, namely pm = (ps + pg)/2, subsequent middle
points often descend quickly into a local minimum that is
not collision-free, resulting in a failure of the planner. Along
similar lines, the consequence of this non-convexity is also
reﬂected in the low success rate of STOMP, shown in Ta-
ble 4. To address this shortcoming, a high-quality initial-
ization is of crucial importance. Therefore, before the in-
vocation of CostAwareFreeStateSampler, cRMPD ﬁrst
takes K samples around the middle point and compute their
costs. Subsequently, the seed point pm that is being passed
to the CostAwareFreeStateSampler call is the one with
the lowest cost among the K samples.
The greedy initialization strategy of cRMPD as described
above has been shown to work effectively. As one can tell
from the results in Table 4, the trade-off between time ef-
ﬁciency and path quality is evident. Whereas STOMP of-
fers the best path quality, it is prone to failure and entails a
large computational burden. The opposite of this conclusion
holds true for RRT-Connect. On the other hand, cRMPD of-
fers us a better trade-off between path quality and computa-
tional burden. Not only can cRMPD answer the queries with
a 100% success rate, it also achieves near optimum perfor-
mance in terms of planning time and path length.
4.5
Limitations
Despite out-performing the other well-known planners in
our comparative study, cRMPD has some limitations of its
own. First, if a middle point is chosen poorly, it will cause
subsequent middle points to diverge from the optimal path.
Since cRMPD is a single-path planner, a middle point, once
chosen, can no longer be changed in later iterations and re-
mains in the ﬁnal path. We believe that this shortcoming
could be addressed by using cRMPD only as a local plan-
ner in a probabilistically complete top-level planner, such as
RRT. Furthermore, launching multiple instances of cRMPD
simultaneously in a multi-threaded fashion also would help
get around the difﬁculties that may arise with the possible
suboptimality of the middle points.
5
Conclusion
Best known randomized sampling based algorithms for path
planning, while possessing the highly desirable property of
probabilistic completeness, unfortunately tend to carry out
unnecessary randomized explorations in open spaces and
branch out slowly in narrow passages. Our “local” planner
RMPD has the ability to efﬁciently bypass local obstacles
using inexpensive resampling, which accelerates excursions
into narrow passages through a divide-and-conquer strategy.
Additionally, cRMPD, our cost-aware version of RMPD,
takes advantage of estimated gradient descent to produce
a cost-optimal middle point. cRMPD uses a Monte Carlo
sampling strategy where the current sampling distribution
is constantly steered to low-cost samples in the previous it-
eration. Our experimental results demonstrate that cRMPD
possesses superior qualities with regard to time efﬁciency
and path quality. This makes cRMPD a powerful new ap-
proach to path planning.
6
Acknowledgement
The authors would like to thank the anonymous reviewers
for their insightful feedback. This project was supported by
the USDA Specialty Crop Research Initiative (SCRI).
References
[Barraquand and Latombe 1993] Barraquand,
J.,
and
Latombe, J.-C.
1993.
Nonholonomic multibody mobile
robots: Controllability and motion planning in the presence
of obstacles. Algorithmica 10(2-4):121.
[Clifton et al. 2008] Clifton, M.; Paul, G.; Kwok, N.; Liu, D.;
and Wang, D.-L. 2008. Evaluating performance of multiple
RRTs. In Mechtronic and Embedded Systems and Applica-
tions, 2008. MESA 2008. IEEE/ASME International Confer-
ence on, 564–569. IEEE.
[Elbanhawi and Simic 2014] Elbanhawi, M., and Simic, M.
2014.
Sampling-based robot motion planning: A review.
IEEE Access 2:56–77.
[Hsu et al. 2003] Hsu, D.; Jiang, T.; Reif, J.; and Sun, Z.
2003. The bridge test for sampling narrow passages with
probabilistic roadmap planners. In Robotics and Automa-
tion, 2003. Proceedings. ICRA’03. IEEE International Con-
ference on, volume 3, 4420–4426. IEEE.
[Kalakrishnan et al. 2011] Kalakrishnan,
M.;
Chitta,
S.;
Theodorou, E.; Pastor, P.; and Schaal, S. 2011. STOMP:
Stochastic trajectory optimization for motion planning. In
Robotics and Automation (ICRA), 2011 IEEE International
Conference on, 4569–4574. IEEE.
[Kalakrishnan 2011] Kalakrishnan, M.
2011.
STOMP
motion planner.
http://wiki.ros.org/stomp_
motion_planner. Accessed: 2017-11-20.
[Karaman and Frazzoli 2011] Karaman, S., and Frazzoli, E.
2011.
Sampling-based algorithms for optimal motion
planning.
The international journal of robotics research
30(7):846–894.
[Kavraki et al. 1996] Kavraki, L. E.; Svestka, P.; Latombe,
J.-C.; and Overmars, M. H. 1996. Probabilistic roadmaps
for path planning in high-dimensional conﬁguration spaces.
IEEE transactions on Robotics and Automation 12(4):566–
580.
[Kuffner and LaValle 2000] Kuffner, J. J., and LaValle, S. M.
2000. RRT-connect: An efﬁcient approach to single-query
path planning.
In Robotics and Automation, 2000. Pro-
ceedings. ICRA’00. IEEE International Conference on, vol-
ume 2, 995–1001. IEEE.
[Kuffner 2004] Kuffner, J. J. 2004. Effective sampling and
distance metrics for 3d rigid body path planning. In Robotics
and Automation, 2004. Proceedings. ICRA’04. 2004 IEEE
International Conference on, volume 4, 3993–3998. IEEE.
[Kuwata et al. 2009] Kuwata, Y.; Teo, J.; Fiore, G.; Kara-
man, S.; Frazzoli, E.; and How, J. P. 2009. Real-time mo-
tion planning with applications to autonomous urban driv-
ing.
IEEE Transactions on Control Systems Technology
17(5):1105–1118.
[LaValle 1998] LaValle, S. M. 1998. Rapidly-exploring ran-
dom trees: A new tool for path planning.
[Lee et al. 2012] Lee, J.; Kwon, O.; Zhang, L.; and Yoon, S.-
e. 2012. SR-RRT: Selective retraction-based rrt planner. In
Robotics and Automation (ICRA), 2012 IEEE International
Conference on, 2543–2550. IEEE.
[Nof 1999] Nof, S. Y. 1999. Handbook of industrial robotics,
volume 1. John Wiley & Sons.
[Ratliff et al. 2009] Ratliff, N.; Zucker, M.; Bagnell, J. A.;
and Srinivasa, S. 2009. CHOMP: Gradient optimization
techniques for efﬁcient motion planning. In Robotics and
Automation, 2009. ICRA’09. IEEE International Conference
on, 489–494. IEEE.
[Sucan and Chitta 2011] Sucan, I. A., and Chitta, S. 2011.
MoveIt! http://moveit.ros.org.
[Sucan, Moll, and Kavraki 2012] Sucan, I. A.; Moll, M.; and
Kavraki, L. E. 2012. The open motion planning library.
IEEE Robotics & Automation Magazine 19(4):72–82.
[Urmson and Simmons 2003] Urmson, C., and Simmons, R.
2003.
Approaches for heuristically biasing RRT growth.
In Intelligent Robots and Systems, 2003.(IROS 2003). Pro-
ceedings. 2003 IEEE/RSJ International Conference on, vol-
ume 2, 1178–1183. IEEE.
[Zhang and Manocha 2008] Zhang, L., and Manocha, D.
2008.
An efﬁcient retraction-based RRT planner.
In
Robotics and Automation, 2008. ICRA 2008. IEEE Interna-
tional Conference on, 3743–3750. IEEE.