/, /!)%0!,-%.1/, /!)%0!,-%.1
/, /!!/-/, /!!/-
!+,.(!).*"*(+/.!,%!)!!$)%'
!+*,.-
!+,.(!).*"*(+/.!,%!)!
.$'))%)#"*,'),,.%/'.! **.--%)#*)2#/,.%*).$'))%)#"*,'),,.%/'.! **.--%)#*)2#/,.%*)
+!-) *(+'%).*.%*)+!-) *(+'%).*.%*)
'%-$&-
/, /!)%0!,-%.1
!+--+/, /!! /
!+*,./(!,
&-'%-$.$'))%)#"*,'),,.%/'.! **.--%)#*)2#/,.%*)+!-) *(+'%).
*.%*)
!+,.(!).*"*(+/.!,%!)!!$)%'!+*,.-
+!,
$..+- *-'%+/, /!! /-.!$
$%- */(!).$-!!)( !0%''!.$,*/#$/, /!!/--!,0%!*".$!/, /!)%0!,-%.1%,,%!-
'!-!*)..!+/-+/, /!! /"*, %.%*)'%)"*,(.%*)
PATH PLANNING FOR PLANAR ARTICULATED
ROBOTS USING CONFIGURATION SPACES
AND
COMPLIANT MOTION
Elisha Sacks
Department
of
Computer Sciences
Purdue Universil:y
West Lafayette, IN 47907
CSD TR #00-011
July 2000
(Revised 9/02)
NOTE,
TITLE
CHANGE
Path planning for planar articulated robots using
configuration spaces and compliant motion
Elisha Sacks
Abs/rae/_This
paper
presents a pnlh planning algorithm for
an
artic-
ulated
planar
robot wllh
II
5WtiC
obstacle. The Blgorlthm selects
II
mbot
part.
findsa
path
to
lis
goalconfigurallon
by
sysl.ematic
conligumtion
space
!ielIIt:h, drags the entire robot nlong !he path using compllnnt mollon, and
repeals the cycle until
C\"l!ry
robot pllrt reoches lis goal. The
planner
Is
1l'5ted
on
11,000
random
problems,
which
span
do;r.e1\S
ofroboVobslacle
ge.
omelrilS with
up
10
43 moving
pnru
and
with narrow channels. II solvl'5
every
problem
In
seeonds, whcn.'llS
randomlzed
a1goriUuns
appear
10
fllil
on
1111
of
them.
KeyH'ords-palh
planning,
cunfigumtioD
space,
compliant
motion.
I.
INTRODUCTION
T
HIS paper presents a path planning algorithm for
an
artic-
ulated planar robot with a static obstacle.
The
task is
10
compute a valid robot path from a start configuration
to
a goal
configuration. A palh is valid when no two parts ever overlap
and every joint equation always holds. Path planning is crucial
in robot navigation and is important in robot manipulation, de-
sign for assembly, virtual prolotyping, computer graphics, and
computational biology.
Path planning is a subtask
of
motion planning. The largertask
is to devise a control policy thai drives a robot from a
stan
state
to a goal along a valid path. The policy must respect the robot
control authority and the task dynamics, and ideally should gen-
eratean
optimalpath. Motion planning is normally factored into
path planning followed by controller design
[1], although there
is current research
on
an integrated approach [2]. This paper
does not address motion planning issues beyond path planning.
Path planning has been studied extensively over the past
twemy years and many algorithms have been developed. The
algorithms can be formalized as searchinga configuration space
for a free.space path between
stan
and goal configurations.
Early work constructed exact
or
resolution complele free space
representations and searched them systematically [3]. The key
measure
of
complexity is the configuration space dimension,
which equals the number
of
independent
pan
trnnslations and
rotations.
The
worst-case computation time is exponential in
this dimension for planar and spatial systems. Practical algo-
rithms have been developed for dimension three, notably for
polygonal robots and obstacles {4], [5].
But
the approach ap-
pears impractical for dimension six, which is the minimum for
spatial planning
or
for multi-part planar planning.
The
impracticality
of
systematic search led
to
the develop-
ment
of
probabilistic algorithms that construct free-space road
Elisha Sacks
is
a Profcssor
of
Computer Science
at
Purdue
University,
WCSI
Lafayelle,
IN
47907,
USA.
This research benefilal
from
discussions with
Bruce
Don:lld,
Ananth
Grama,
Leo Joskowicz, Jean-elaude
Latombe,
Jean
Paul
Lau-
mond,
Mall
Mason,
Brian
Mirtich, and Jeff Trinkle.
S:lCks
was
supported
by
NSF
gr.uJ15
CCR-96176Ol1
and IIS.(I082339. by
Ute
Purdue Center forCompu-
tational
Image
Analysis
and
Scientific
VISualization.
by
a
Ford
University Re-
search Gran!,
by
lhc Ford
ADAPT
2000 project,
WId
by
gr.uJt
981S)(i
from
!he
Israeli
Academy
ofScience.
maps by random sampling [6]. Although extremely promising,
probabilistic algorithms have some drawbacks. The probability
of
finding a path when
one
exists
is
a function
of
geometriccon-
stants that are hard
to estimate. The constants are large when
the configuration space contains many narrowchannels, most
of
which do not link the startand goal configurations. Hence, there
is
no
efficient way
10
selectenough samples
to
guaranlee a spec-
ified error rate.
Nor
is there a way
10
prove that no path exists.
The running time is dominated by
pan
intersection tests, which
can
be
slow for curved parts. Robots with closed loops
of
joints
pose problems for random sampling because the joint equations
must hold at the sample configurations.
This paper presents a planning algorithm thal addresses the
impracticality
of
systematic search and the limitations
of
prob-
abilistic algorithms. The planner selects a robot part, finds a
path
to
its goal configuration by systematic configuration space
search, drags the entire robot along the path using complianl
motion, and repeats the cycle until every robot part reaches its
goal.
The
pan
selection heuristic focuses the planner on man-
ageable subtasks.
The
systematic search finds narrow channels.
The
dragging heuristic explores a tiny subset
of
the system con-
figuration space based on the intuition that the nonselected parts
should slide along the obstacle andalongeach olher. A heuristic
algorithm seems unavoidable given the exponential complexity
of
path planning. However, the algorithm is complete for one
moving part, which
is
a common case in robotics and
in
design
for assembly.
The algorithm has been implemented for general planar sys-
tems with curved parts. closed loops, and narrow channels. Pia·
nar systems are important and common, yet are more tractable
than spatial syslems. Many robots
are
planar
or
can
be
treated
as planar, including wheeled vehicles and legged robots that
op-
erate
in
buildings, on ships. and on roads. Most mechanical sys-
tems are planar. The planner has been validated on 11,000 ran-
dom
problems, which span dozens
of
roboflobstacle geometries
with
up
to
43 moving parts and with narrow channels.
It
solves
every problem in seconds, whereas randomized algorithms ap-
pear
to
fail on all
of
them.
The rest
of
the paper is organized as follows. Section 2 con-
tains a review
of
priorwork. Sections
J-5
describe the planner.
Seclion 6 assesses its performance: running times on sample
systems, a comparison with randomized algorithms, and limita-
tions. Section 7 contains conclusions and plans for fUlure work.
n.
PRIOR
WORK
Motion planning has spawned a large literature that is sur-
veyed by Latombe [7J. This section summarizes the portion that
is related
to path planning. The most commoncases are a planar
polygonal robot with a polygonal obstacle, a polyhedral robot
with a polyhedral obstacle, and a six degree-of-freedom robot
,
manipulalOf with a polyhedral obstacle.
Latombe
[3] describes
mostalgorithms in detail.
The
configuration space approach originates in the work
of
Lozano-P6rez
[8]
and underlies most path planning research.
The configuration space
of
a system
of
rigid parts
is
a differ-
entiable manifold that
encodes
their positions and orientations.
Poinls in this space, called configurations. are classified as free
when no two parts touch
and
as blocked when [wo parts over-
lap.
The
free and blocked configurations
fonn
open sets, called
free
and blocked space.
Their
common boundary, called con-
tact space, contains the conligurn.tions where two
or
more parts
touch without overlap and
the
other parts are free. Figure 3
shows the configuration space
of
a planar part with respecllO an
obstacle. which is three-dimensional and
can
be parameterized
by the position and orientation
of
lhepart. Contact space, drawn
in grey, consislS
of
two connected componenls. Freelblocked
space are the regions oUlSidelinside these components.
Complete planning algorithms based on e",act configuration
spacerepresentalions have been developed
for
parts bounded by
algebraic curve segmenls [9J.
The
condition that the parts
not
overlap yields multivariate polynomial inequalilies in the con-
figuration space coordinates.
The
equations are solved
by
al-
gebraic methods, such as cylindrical decomposition
or
Grtibner
basis calculation.
The
solulion
set
is searched for a path from
the start
(0
the goal configuration.
Canny
[10] presents an
exact planning algorithm that conslructs and searches a one-
dimensional subset
of
configuralion space called a road map.
None
of
the algorithms for algebraic curves has been imple-
mented. perhaps because
of
their intricacy and high compUla-
tional complexity. Complete, exact algorithms have proven ef-
fective for a polygonal
robol
with a polygonal obstacle [4], [5].
An alternative planning approach is to construct an approxi-
mate representation
of
free
space
and
to search
it
heuristically.
Lozano-Perez [8] decomposes the configuration space
of
a pla-
nar polygon into a stack
of
slices along the rotalion axis. Brooks
[11] decomposes the configuration space into a graph
of
free,
blocked, and mixed cuboids.
He
perfonns
A* search
for
a
piecewise linear path through the
free
nodes.
If
none
is found,
heuristics are used to select
mixed
nodes and lo split
them
into
smaller cuboids, the graph
is
updated,
and
the search is re-
peated. Takahashi and Schilling
[l2J
plan
for
a rectangularrobot
with a polygonal obstacle via heuristic search
of
the generalized
Voronoi diagram. Approximate approaches perform well in di-
mension three, as long as the part fits are
not
too light, but are
impractical in dimension six
or
higher because the number
of
cells
is
exponential in the dimension.
There
are also heuristicplanners
for
a polyhedral robot with a
polyhedral obstacle, which have six-dimensional configuration
spaces.
The
key
concepl
is a contact patch: a connected subset
of
contact space
where
the contact point lies on a fixed pair
of
robot/obstacle features (vertices. edges.
or
faces).
Donald
[l3)
constructs robot paths via A* search.
He
develops parametric
expressions for contact patches and their intersections, but does
notcompute the free spacetopology. His
planner
moves through
free
space and from patch to patch via heuristic motions, such
as sliding along the obstacle. Trinkle and
Hunter
[14] plan robot
manipulation by searching a graph
of
contact
fonnations-sets
of
touching robot/object feature
pairs-that
are akin to contaCl
patches. Joskowicz and Taylor [15J construct linearized con-
tact patches and infer patch transitions
for
a complex polyhedral
model
of
a prosthetic hip.
Recent path planning research focuses on probabilislic algo-
rithms. Potential field algorithms [16J, [17] numerically mini-
mize a potential function that is designed
to have a global min-
imum at the goal. Local
minima
are escaped by short random
walks.
Road
map
algorithms [6] generate many random con-
figurations,
prune
the blocked
ones,
and link adjacent free ones
into a graph (normally wilh line segments). Path planning is
performed by linking the inilial
and
goal configurations to the
graph then searching
for
a path between them.
Another approach is to build a roadmap between the startand
goal, rather than
over
the entire
free
space.
One
algorithm [18]
generates random trees rooted
at
the start and goal configura-
lions. Tree nodes are generaled and linked
in
the
same
way as
road map nodes. A path is found when a
node
in
one tree can
be linked to a
node
in the
other
tree. Another algorithm [19]
grows a
tree from lhe start configuration and test
for
paths from
each new
node
to the goal.
Randomized
optimization is used to
maximize the distance between nodes
and
to generate paths that
minimize the distance to the goal.
Barraquand
et
al [20J unify the treaunent
of
probabilistic al-
gorithms
and
prove
them
probabilistically complele.
Their
the-
ory predicts that cluttered envirollmenls
and
narrow passages
can require excessive numbers
of
samples, as has proven true.
Recenl
work
addresses this
problem
via improved sampling
methods [21]. [22]. Ji
and
Xiao
[23] develop a road map al-
gorithm for compliant motion between two polyhedra. Other
work [24], [25] develops efficient
sampling
methods
for
closed
loops
of
joints.
The
configurations
mUSl
satisfy the
joint
equa-
tions, which are coupled systems
of
algebraic equations
in
the
configuration space coordinates.
The
challenge is to cover the
solution space quickly and thoroughly.
Compliant motion is very useful in fine motion planning with
uncertainty [26], [27J, [28].
The
canonical task is to insert a peg
in a hole by adapting a nominal path
lO
work
with imperfect sen-
sors, imperfectactuators, inexact geometric models. and incom-
pleledynamical models. Compliant motion with force feedback
along a nominal insertion path
can
compensate
for
some
enors.
The
robot tries to follow a direct path to the goal.
When
the peg
hils the obstacle, the
robot
modifies its velocity by sublracting
the component that is normal to the obstacle.
The
challenge
is
lo constructa sequence
of
motions that achieves a planning goal
whenever the errors lie within given bounds. In this paper, com-
pliant motion allows a selected
robot
part to move in a specified
direction
by
assigning compliant velocities to the other parts.
Incomplete and imperfect infonnation is
nOl
addressed.
In
summary, prior
work
provides theorelical planning algo-
rithms
for
general systems,
complete
practical algorithms for
planar pairs, heuristic algorithms for spatial pairs, and proba-
bilistic algorithms
for
general syslems.
This
paper
describes a
heuristic planner for general planar systems based upon a com-
plete planner
for
planar
pairs.
III.
PATH PLANNING ALGORITHM
This seelion describes the input, output, and high-level stnlc-
ture
of
the path planner.
which
are summarized in Figure
1.
Input:
parts, obstacle, joints.
Slart,
goals, configuration
spaces.
While goals remain
Select goal.
ConstruCl graph plan.
Construct
full
path.
Output:
path that achieves goals.
Fig.
I.
Path
planning
algorithm.
The inputs are parts. an obstacle, joints, a start configuration,
goals, and configuration spaces. The output
is
a valid path from
the startconfiguration to a configuration that achieves the goals.
The next two seclions describe the main components
of
the plan-
ner: the graph plannerand the full planner.
A pan
is
a rigid body that consists
of
a stack of cross sec-
tions. A cross section
is
oblained by extruding a profile
in
the
xy
plane along an interval on the z axis. A profile
is
a circular
list
of
line and circle segments
in
which the head
of
each seg-
ment equals the
tail
of
the following segment_ Any planar part
can be modeled with a moderate number
of
line and circle seg-
ments according
to
our
survey
of
2500 mechanisms [29].
In
our
examples, every part has one cross section. Multiple sections
occur
in
some mechanical syslems and are needed to approxi.
mate spatial geometry.
The parts uanslate and rotate
in
the
xy
plane. The configu-
ration of a part
is
a biple
(x,y,8)
where
(x,y)
is
the position
of
the part frame
in
a global coordinate frame and 8
is
the angle
between !he frames. The obstacle consists
of
zero ormore stalic
parts. A part can
be connected
to
the obstacle
or
to
another part
by a revolute or a prismatic joint. The start configuration
is
a
valid system configuration, a list
of
pan
configurations, which
means that no
two
parts overlap and
all
joint equations are sat-
isfied.
The goals input
is
a list
of
part/goal configuralion pairs. A
single pair suffices to specify a goal configuration for a robol
end effector. The planner must generate a robot path that brings
the end effector to the goal and that leaves the other parts
in
an
arbilral)' valid configuration. Several input pair.; are required
to specify a complele configuration for a multi-part
robot_
The
planner must generate a path that achieves all the goals, hence
that brings the robot to the specified configuration. Intennediate
cases also arise where some, but not all, parts have goals, as
in
the room example below.
A configuration space
is
provided for every part/part and
part/obstacle pair. The configuration spaces are constructed
by
ourprogram [30]. The output
is
a boundaryrepresentation
of
the
free space
of
the first part
in
the the second part frame, which
is the global frame when the second part
is
the obstacle. The
boundary
is
encoded in a
co1ltact
graph whose nodes and links
represent contact patches and patch adjacencies. A patch
is
a
connectedsubset
of
contactspace where the contactpoint lies on
a
fixed
pair
of
part features (line segments, circle segments, or
segment endpoints). The surface equation
is
represented implic-
itly as
f(x,
y,
8)
= 0 and parametrically as
(x(u,8),y(u,
8), 9).
An adjacency occurs when
two
patches share a boundary
Clll"Ve.
The algorithm guarantees that every patch has four boundaries:
bottom and top curves
of
the fonn 8 = k and lefl and right
curves
of
the form (x(8),y(O),
8).
3
The algorithm achieves the goals iteratively.
It
maintains a
list
of
achieved goals, a list
of
failed goals, and a path. Initially.
the goal lists are empty and the start configuration
is
the sole
element
of
the pa!h. Each iteration selects an inpul goal and
Dies
to extend the path
lo
a configuration that achieves
it.
If
a path
is
found, the goal
is
moved to the achieved list and the failed goals
are returned to the input list. The reason for the second step
is
lhat path extension can clear the way for a formerly blocked
part to reach its goal.
If
a path
is
not found, the goal
is
moved
to the failed lisl. The iteration ends when the input list
is
empty.
The algorithm outputs lhe path when the failed list
is
emp!y and
reports failure otherwise.
Each iteration consists
of
three steps. Thefirst step selects the
input goal
81c
that minimizes the distance from the currentcon-
figuration
of
part
8.
which comes from the last configuration
of
the current path,
to
its goal configuration
c.
This greedy heuris-
tic tries the goal that appears easiest. The second step searches
the .sfobstacle contact graph for a path
to
lhe goal configuration,
meaning a pa!h where 8 avoids the obstacle and the other parts
are ignored. The third step integrates a velocity field thal moves
s along the path. holds
fixed
the parts with achieved goals, and
moves the olher pans compliantly.
If
8 reaches C, the integralor
output is returned. Otherwise. the conlact graph
is
searched for
another palh.
Figure 2 illustrates the planner. There are four parts: a robot
comprised
of
linkl and link2. a door, and a chair. The house
is
the obstacle. The door
is
attached to the house by a pin
joint. The goal is to move linkl to lhe displayed goal config-
uration. The robot starts outside, moves to the door and pushes
it open, removes the chair from the bedroom doorway. enters
the bedroom, and parks. The !hick black line is the projeclion
of
the link1 graph path. The path is generated by navigating
the link1/house configuration space without considering Iink2,
lhe door, and !he chair. The robot path
is
represented by se-
lecled snapshots. It is generated by dragging linkI along the
graph path and assigning link2. the door. and the chair compli-
ant velocities. The compliant velocities salisfy the Iink1flink2
and door/house joint equations, cause lhe door
lo
rotate when
linkl pushes
it,
cause the chair
to
move left when Iinkl pushes
it. and cause Iink2to follow the house profile.
IV.
GRAPH
PLANNER
The graph planner searches a configuration space for a path
from a start to a goal configuration.
It
tests
if
the start and
goal
lie
in
lhe same connected component
of
free space. which
is
a standard computational geometry operation.
If
not, it re-
pons that there
is
no
path.
If
so, it finds a path via
A*
search.
The search space consists
of
patch/configuration nodes where
the patch
is
in
the contact graph and the configuration lies on
the patch. The start and goal are represented by nodes with 0
patches. Search nodes are stored
in
a heap that
is
sorted by a
heuristic quality measure. The heap
is
initialized
to
a single
node with the stan configuration. The minimum node
is
re-
moved from lhe heap and its children are added to the heap.
Each untraversed neighbor
of
the minimum patch generales a
child whose configuration
is
the midpoint
of
the shared patch
boundary curve. There
is
also one child for the path that fol-
lows a straight line
mm
the minimum configuration
to
the goal