f An 9

Fae <
its
= es! |ioe te :
Pie
re
¢
bs
: ea
Algorithmic and Computational Robotics:
New Directions
Algorithmic and Computational Robotics:
New Directions

The Fourth Workshop on the Algorithmic

Foundations of Robotics

edited by
Bruce Randall Donald
Dartmouth College

Kevin M. Lynch
Northwestern University

Daniela Rus
Dartmouth College

of
6 ioe 2000
A K Peters NEIR The Workshop on the Algorithmic
Natick, Massachusetts = | Foundations of Robotics
Editorial, Sales, and Customer Service Office

A K Peters, Ltd.
63 South Avenue
Natick, MA 01760

Copyright © 2001 by A K Peters, Ltd.

All rights reserved. No part of the material protected by this copyright notice
may be reproduced or utilized in any form, electronic or mechanical,
including photocopying, recording, or by any information storage and
retrieval system, without written permission from the copyright owner.

Library of Congress Cataloging-in-Publication Data

Workshop on the Algorithmic Foundations of Robotics (4th : 2000 : Dartmouth College)

Algorithmic and computational robotics : new directions : the fourth Workshop on the
Algorithmic Foundations of Robotics / edited by Bruce Donald, Kevin Lynch, Daniela Rus.
Gp. em.
Workshop held March 16-18, 2000, Dartmouth College.
Includes bibliographical references and index.
ISBN 1-56881-125-X (alk. paper)
1. Robotics--Congresses. 2. Algorithms--Congresses. I. Donald, Bruce R. II. Lynch,
Kevin (Kevin M.) III. Rus, Daniela. IV. Title.

TJ210.3 .W664 2000

629.8'92--dce21 00-049355

Cover illustration: Mechanical drawing of Leonardo da Vinci.

From Leonardo da Vinci, Cod. Atl., fol. 357 r-a.Reynal and Company, New York,
p.498. Copyright in Italy by the Istituto Geografico De Agostini— Novara — 1956.

Printed in the United States of America

05 04 03 02 O1 IO. StH by Zh BB al
Contents

Foreword vu
Participants

Meso-Scale Self-Assembly
D. H. Gracias, I. Choi, M. Weck and G. M. Whitesides
Robust Geometric Computing in Motion
D. Halperin

Controlled Module Density Helps Reconfiguration Planning 23

A. Nguyen, L. Guibas, and M. Yim

Positioning Symmetric and Non-Symmetric Parts Using Radial and Constant Force Fields 37
F. Lamirauz and L. Kavraki
Complete Distributed Coverage of Rectilinear Environments 51
Z. J. Butler, A. A. Rizzi, and R. L. Hollis
Closed-Loop Distributed Manipulation Using Discrete Actuator Arrays 63
J. E. Luntz, W. Messner, and H. Choset
Kinematic Tolerance Analysis with Configuration Spaces 73
L. Joskowicz and E. Sacks
Deformable Free Space Tilings for Kinetic Collision Detection 83
P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang
Real-time Global Deformations 97
Y. Zhuang and J. Canny
Motion Planning for Kinematic Stratified Systems with Application to Quasi-Static
Legged Locomotion and Finger Gaiting 109
B. Goodwine and J. Burdick
Manipulation of Pose Distributions 127
M. Moll and M.A. Erdmann
Image Guided Surgery 143
E. Grimson, M. Leventon, L. Lorigo, T.Kapur, O. Faugeras, R. Kikinis, R. Keriven,
A. Nabavi, and C-F. Westin

Pulling Motion Based Tactile Sensing 157

M. Kaneko and T. Tsuji
vi Contents

Compensatory Grasping with the Parallel Jaw Gripper 169

T. Zhang, G. Smith, and K. Goldberg
Optimal Planning for Coordinated Vehicles 181
A. Bicchi and L. Pallottino
An Efficient Approximation Algorithm for Weighted Region Shortest Path Problem 191
J. Reif and Z. Sun
Synthesis and Regulation of Cyclic Behaviors 205
E. Klavins, D.E. Koditschek, and R. Ghrist
A Framework for Steering Dynamic Robotic Locomotion Systems 221
J. P. Ostrowski and K. A. McIsaac
A Kinematics-Based Probabilistic Roadmap Method for Closed Chain Systems 233
L. Han and N. M. Amato
Randomized Kinodynamic Motion Planning with Moving Obstacles 247
D. Hsu, R. Kindel, J.C. Latombe, and S. Rock
On Random Sampling in Contact Configuration Space 265
X. Ji and J. Xiao
Randomized Path Planning for a Rigid Body Based on Hardware Accelerated Voronoi Sampling 279
C. Pisula, K. Hoff, M. Lin, and D. Manocha
Rapidly-Exploring Random Trees: Progress and Prospects 293
S. M. LaValle and J. J. Kuffner
Encoders for Spherical Motion Using Discrete Optical Sensors 309
E. Scheinerman, G. S. Chirikjian, and D. Stein
Notes on Visibility Roadmaps and Path Planning 317
J.P. Laumond and T. Siméon
AutoBalancer: An Online Dynamic Balance Compensation Scheme for Humanoid Robots 329
S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba, and H. Inoue
Coupled Oscillators for Legged Robots 341
M. D. Berkemeier

Reliable Mobile Robot Navigation From Unreliable Visual Cues 349

A. J. Briggs, D. Scharstein, and S. D. Abbott
Toward Real-Time Motion Planning in Changing Environments 363
P. Leven and S. Hutchinson

Graphical Construction of Time Optimal Trajectories for Differential Drive Robots 377
D. J. Balkcom and M. T. Mason
Foreword

Robot algorithms are abstractions of computational processes that control or reason about motion and
perception in the physical world. The computation may be implemented in software, hard-wired elec-
tronics, biomolecular mechanisms, or purely mechanical devices. Because actions in the physical world
are subject to physical laws and geometric constraints, the design and analysis of robot algorithms
raises a unique combination of questions in control theory, computational and differential geometry, and
computer science. Algorithms serve as a unifying theme in the multi-disciplinary field of robotics.
The Fourth International Workshop on the Algorithmic Foundations of Robotics (WAFR) brought
together a group of about sixty researchers to discuss recent trends and important future directions of
research on the algorithmic foundations of robotics. Held at Dartmouth College on March 16-18, 2000,
the workshop was chaired by Bruce Randall Donald (Dartmouth), Kevin Lynch (Northwestern), and
Daniela Rus (Dartmouth). The workshop consisted of six invited talks and twenty-four contributed
presentations in a single track. Each paper was rigorously refereed by the program chairs plus at
least three members of the program committee. The program committee consisted of the conference
chairs plus Pankaj Agarwal (Duke), Srinivas Akella (Rensselaer Polytechnic Institute), Nancy Amato
(Texas A&M), Antonio Bicchi (University of Pisa), Kamal Kant Gupta (Simon Fraser), Leslie Kaelbling
(MIT), Makoto Kaneko (University of Hiroshima), David Kriegman (UIUC), Steve LaValle (Iowa State),
Dinesh Manocha (UNC), Jim Ostrowski (University of Pennsylvania), John Reif (Duke), and Elisha
Sacks (Purdue).
Topics reported at WAFR 2000 included geometric algorithms, minimalist and underactuated robot-
ics, robot controllability, manufacturing and assembly, holonomic and nonholonomic motion planning,
manipulation planning, sensor-based planning, task-level planning, grasping, navigation, biomimetics,
medical robotics, self-assembly, modular and reconfigurable robots, and distributed manipulation. This
book contains the proceedings of WAFR 2000. A number of these papers are clearly destined to be
landmarks in the field. For example, the paper “Meso-Scale Self-Assembly” by Gracias, Choi, Weck, and
Whitesides was voted number three in the “Top Five Must Read Papers Recommended by the Carnegie
Mellon School of Computer Science Faculty” for the year 2000. It is worth noting that numbers one
and two were each written more than 40 years ago, and “Meso-Scale Self-Assembly” was voted ahead
of number four, Judge Jackson’s findings of fact against Microsoft. This demonstrates the value of
interdisciplinary interaction at workshops such as WAFR.
We are very grateful to Dartmouth College and Sandia National Laboratories for their generous
financial support of WAFR. We would like to thank all participants for their contributions. We thank
Dartmouth Project Assistant David Bellows for his work in organizing WAFR and in typesetting the
book and Fred Henle for his advice and assistance with AT@X and image enhancement.

Bruce Randall Donald

Kevin M. Lynch
Daniela Rus
June, 2000
 ee heaymm ~
mn Oe anit B. Cray: ;

pes i (Reve \ 2 i ~*~ fmt risers ; hi

7 _ A—

wens ze Pine) Hemeeroniey, Lawns taniegiies b> aaniratteda ms eave Ho

eae et Hotei nore T lie leotidg dt af Ok odgy
Behe Invicndeat ebony we .cureicadiam walucmlorooil (a=ise :
WS) ctaiesare> oiriwmery han eweel dendoni of iia oa
a Siany'sOthe ni Savtrean lo sebasiduzes en
Mites wilt of onend? acrivtion « ay wey watiniglA wimiia 30
whplestiitAIS Os Aco teen ayesdlaaliry2 branpinensti rooted pees affa
ant boa ebuary gre BOD Gt wriknisseer Wie Jods le got eal
satin’. piuolatiacs oa
§ itnll .« vi duct:ny mepeteh rh wy.tut pe wf) acd we
Pere.) arvelt ebpsiitfrat) incall
2 Tekin acral ‘9d buals / saw qi :
MBs atlak Ualivint cit 30 letalanon qoidwbew adT Pp panties dl)a
eimte td behets, Tess yO lO esw weg ths dae) slyoke 6 ot eaeliads
an atpninoy ineiying od) 68) )xomer prety 1q Whi te sander moult
bei miitiimente
Gh)r ellen A. pivhitsa Soares Seay A Wi
aes i! is A jaune) (net It vate errLig?
TF) rishi Reon mY, oe:
Se CHINN ,
hutietrl bivad .amideorilt jo vitenay iat) “cise A-oicialle
ene
op nd Ral eie M46 Cer) Liewo: eG ob. (AU) arsenal =a
é niet Aw?
= ouaareys

Pap atshy Liotgubasetnie yew Abs DMipyoi219 tg baiti Ow SSAW ne borage as qe

whey14s); (3 Paeaeiedicn fae iigarmd cites bra wdhwindiugen Sacer *pe
eter 4 awincyivae ier ae egetenolate ast at-daed YUL Loend-oamee peilmale & aL
: aah) ti ara iheditiaitioon bas wbubonr vWdrpnaay- lise sotto sue iT
a we Wag oe!) lo wdamn A NS Wists, eyuibsoooia Sills
ad) magia vr
focal,
qlee: eA LC lav iomteesal a eee bork oRprisapa det, Pe f
oll lebures ae ~
pa}
erga d Seon datsToney Sy
GL” art? ai seth) toelouy hovev enw
== io
va dy sot dita af fi Oe, 109% Set TOL “lao soi?
— 4h) Taluqine)Ve tp =
‘% ii &in * ine RAGE Rene” ta ume elivty O} ed) geotr untiiw cores
' Lit
ody vastaFa met eit Mow maiess Joel Wo » yithett «epee =
rs J eins Mobile Rerbet ti SAL
tse hae egertelion i& Dota
7 * elrabattrsinds A btigperrntnel MiniisHebwme ben sgolion)
Stuoathatl] ll wm
Souls a7 fale ideo tists wi Biliiel (ad ace ile rmd) ot ld bivew
sig ;
enlt“Labiamgys itt bag BisPh MAMI A Le oid wit wenllatt Livet] vcmpélen
A Minti egarmnd Hey Kel Uodiiw «oud tatedl ts Sabena
lon ay Dittedt: Ppkiy tw ti Dill pee —
tw Hie iyrt
a us nJ lV ny

mri] ilnus "i

TAN oereyT,
Participants

Pankaj Agarwal Duke University

Srinivas Akella Rensselaer Polytechnic Institute
Nancy Amato Texas A&M
Boris Aronov Polytechnic University
Devin Balkcom Carnegie Mellon University
Matthew Berkemeier Utah State University
Antonio Bicchi University of Pisa
Amy Briggs Middlebury College
Herve Bronimann Polytechnic University
Joel Burdick California Institute of Technology
Zack Butler Carnegie Mellon University
Ming C. Lin University of North Carolina Chapel Hill
Greg Chirikjian Johns Hopkins University
Anne Collins Duke University
Bruce Randall Donald Dartmouth College
Michael Erdmann Carnegie Mellon University
Robert Ghrist Georgia Institute of Tech.
Ken Goldberg University of California Berkeley
W. Eric L. Grimson MIT
Kamal Gupta Simon Fraser University
John H. Reif Duke University
Dan Halperin Tel Aviv University
Li Han Texas A&M
John Hollerbach University of Utah
David Hsu Stanford University
Wesley Huang Rensselaer Polytechnic Institute
Seth Hutchinson University of Ilinois
Leonidas J. Guibas Stanford University
Xuerong Ji University of North Carolina Charlotte
Satoshi Kagami University of Tokyo
Fumio Kanehiro University of Tokyo
Makoto Kaneko Hiroshima University
Lydia Kavraki Rice University
Eric Klavins University of Michigan
Daniel Koditschek University of Michigan
James Kuffner University of Tokyo
Steven LaValle Iowa State University
Florent Lamiraux LAAS-CNRS
Jean-Claude Latombe Stanford University
Jean-Paul Laumond LAAS-CNRS
Kevin M. Lynch Northwestern University
Dinesh Manocha University of North Carolina Chapel Hill
Matt Mason Carnegie Mellon University
William Messner Carnegie Mellon University
Mark Moll Carnegie Mellon University
An Nguyen Stanford University
Jim Ostrowski University of Pennsylvania
Elon Rimon Technion University Israel
Alfred Rizzi Carnegie Mellon University
Daniela Rus Dartmouth College
Elisha Sacks Purdue University
Daniel Scharstein Middlebury College
Edward Scheinerman Johns Hopkins University
A. Frank van der Stappen Utrecht University
Ileana Streinu Smith College
Robert Sun Duke University
George Whitesides Harvard University
Jing Xiao University of North Carolina Charlotte
Mark Yim Xerox PARC
Tao (Mike) Zhang University of California Berkeley
Li Zhang Stanford University
Yan Zhuang University of California Berkeley
Meso-Scale Self-Assembly
David H. Gracias, Harvard University, Cambridge, MA
Insung Choi, Harvard University, Cambridge, MA
Marcus Weck, Harvard University, Cambridge, MA
George M. Whitesides, Harvard University, Cambridge, MA

This paper describes the application of self-assembly to floating at the interface between a hydrophobic liquid
the formation of structured aggregates (both static and (perfluorodecalin, PFD) and a hydrophilic one (wa-
dynamic) of millimeter sized objects. We argue that ter) [6, 1]. In this system, PFD wets the hydrophobic
MEso-scale Self-Assembly (MESA) provides a frame- faces and forms “positive” menisci (menisci that extend
work for fabricating two- and three- dimensional struc- above the plane of the interface); water wets the hy-
tures (with objects ranging in size between 10 nm and drophilic faces and forms “negative” menisci (menisci
10 mm), and illustrates broadly important principles that extend below the plane). Menisci of the same
underlying the behavior of complex, natural systems. shape interact attractively with each other, while a
mismatch in the shape of the menisci results in weak or
1 Introduction repulsive interactions between the objects. Since capil-
lary forces have decay lengths on the order of millime-
Self-Assembly is the spontaneous organization of mole- ters to nanometers, they can be used to assemble ob-
cules or objects, under steady state or equilibrium jects across this range of sizes. (ii) The second system
conditions, into stable aggregates, by non-covalent uses MESA to form three-dimensional (3D) aggregates.
forces; these aggregates are not necessarily at the In this system liquid films (e.g. of low melting-point
global minimum in energy [14, 15]. Meso-Scale Self- alloys [9], or polymeric adhesives [12]) coat the faces
Assembly (MESA) refers to self assembly carried out of the assembling units. These films have a high in-
with objects ranging in size from 10 nm to 10 mm terfacial free energy in contact with water. When two
using forces having lateral extension with similar size faces come into contact, the films coalesce; this coales-
(8; 6,.1,.9, 12, 17, 13, 7, 4, 11}. MESA bridges the cence minimizes the exposed area of the interface and
gap between molecular self-assembly (e.g. crystalliza- decreases the free energy of the system.
tion [16]; and protein-ligand recognition [10]) and We also describe a system based on dynamical self-
the conventional fabrication of macroscopic machines. assembly [2] that incorporates a combination of attrac-
This paper briefly outlines the most important con- tive and repulsive forces. In this system, millimeter-
cepts governing self-assembly at these length scales. It sized magnetic disks are placed at a liquid-air interface,
also describes the application of MESA in (a) fabri- and subjected to the magnetic field produced by a ro-
cating artificial crystals and three dimensional struc- tating permanent magnet. The attractive forces occur
tures [9, 12, 17], (b) mimicking molecular self assem- between the magnetic units and the rotating magnet.
bly [4, 11, 3, 5], and (c) designing dynamical systems The repulsive forces are due to hydrodynamic interac-
that show tailored complexity [2]. tions associated with vortices in the fluid in the vicinity
of the individual spinning magnets.
2 Forces

We begin by describing two static systems; in these 3 Two-Dimensional MESA

systems, the capillary interactions between individual
units are responsible for MESA. (i) One system uses We assembled two-dimensional (2D) aggregates using
MESA to form 2D aggregates. It employs selective pat- pieces made of PDMS, an organic polymer: poly-
terning of the faces of the object into hydrophilic and dimethylsiloxane; (density = 1.05 g/mL) at the PFD
hydrophobic sets. These objects self assemble, while (density = 1.91 g/mL) /water (density = 1.00 g/mL)
 D. H. Gracias, I. Choi, M. Weck, and G. M. Whitesides

OOO00O0
Figure 1: Photographs of self-assembled arrays of hydrophobically patterned hexagons, floating in the plane of the PF'D 4
water interface, and agitated by an orbital shaker. All the hexagons have centrosymmetric patterns of hydrophobic faces;
the dark faces are hydrophobic, while the light ones are hydrophilic. (A) Two faces (opposite) of individual hexagons are
hydrophobic; this pattern results in a linear array on assembly. (B) Alternate faces of individual hexagons are hydrophobic;
this pattern generates a porous 2D array. (C) All the faces of individual hexagons are hydrophobic; this pattern generates a
2D, close-packed array.

uuu
| CM
Figure 2: 3D assemblies containing 300 cubes. All the faces of the individual cubes were coated with a hydrophobic
polymer; assembly was carried out in water.

interface [6, 1, 7, 4, 11]. The PDMS objects assembled regular arrays (crystals) of millimeter sized polymeric
upon agitation using an orbital shaker. We chose this polyhedra. The faces of these polyhedra were coated
interface for four reasons: (a) PDMS does not swell in with either a photcurable adhesive [12] or molten sol-
contact with either liquid; (b) capillary forces at the in- der [9]. When these liquid-coated polyhedra were sus-
terface are strong (interfacial energy=0.05 J/m7); (c) pended in water and agitated, collisions between them
PDMS has a density intermediate between that of wa- allowed contact and coalescence of the liquid films; this
ter and PFD, and the pieces therefore float at the in- coalescence minimized the interfacial area between the
terface; (d) a thin film of PFD remains between the hydrophobic liquid and water, and provided the ther-
faces of the objects when they assemble, and acts as modynamic driving force for the self-assembly. In ex-
a lubricant; this film allows the objects to move lat- periments using pieces coated with photocurable adhe-
erally relative to one another, and minimizes the free sive, after self-assembly was complete, the structures
energy of the system. Figure 1 shows photographs of were exposed to UV radiation; this exposure cured the
aggregates formed with different hexagonal units at the lubricant and gave the aggregates sufficient mechanical
PFD/water interface. strength that they could be manipulated and charac-
terized. Figure 2 shows assembled 3D structures that
4 Three-Dimensional MESA
contain 300 cubes; these structures formed from sus-
MESA also provides an alternate strategy for the fab- pensions of 1000 cubes, using a photocurable adhesive.
rication of 3D microstructures. We assembled large, Figure 3 shows 3D porous structures formed by the as-
Meso-Scale Self-Assembly
3

A ited by molecules and to use them to join mesoscale

objects. Examples of processes that have been ab-
stracted from organic and biological chemistry and
used in MESA include hierarchical [4] and templated
[3] self-assembly, and DNA recognition [5]. We de-
scribe results of these experiments carried out at the
PFD / water interface through the interaction of hy-
drophilic and hydrophobic menisci as described earlier.
The optical photographs in Figure 4 illustrate the
translation of the molecular concepts of hierarchi-
cal and templated self-assembly into the mesoscopic
world. The results shown in Figure 4A demonstrate
hierarchical self-assembly. Two interactions of differ-
ent strengths were responsible for the self-assembly.
Initially, a strong capillary interaction between hy-
drophobic faces formed structures containing four com-
ponents; a weaker interaction between the concave
hydrophobic faces subsequently caused aggregation
into larger structures. Figure 4B illustrates tem-
plated MESA. These objects interacted among them-
selves through concave hydrophobic regions; the self-
assembly was directed using round, hydrophilic ob-
jects (the templates). On addition of the templat-
ing structures, the system formed the inclusion struc-
tures shown in Figure 4B; inclusion of these templates
strengthened and extended the hexagonal lattice in
these structures. We observed that the formation of
the cyclic structures began with the pre-orientation of
the trefoil objects around the circular template, fol-
lowed by the self-assembly of these trefoil objects.

6 Biomimetic MESA: Models of DNA

Recognition
Figure 3: 3D self-assembly of porous structures using a
high surface tension, low-melting point solder. Polyhedra on
Figure 5A, B, and C are photographs illustrating a
the left, (dark regions represent areas covered with solder)
model of sequence-specific assembly: this model is
assembled into the structures shown on the right.
based on sequence specific molecular recognition in
DNA and RNA. In particular it is based on a long
sembly of polyhedra whose faces were covered with a strand, a specific sequence of which can be recog-
solder having high interfacial tension with water. nized by a short probe with a complementary sequence.
These strands are based on individual objects contain-
ing hydrophobic lock and key functionalities that are
5 Molecule-Mimetic Self-Assembly: connected via a PDMS thread. The two strands associ-
Hierarchical and Templated MESA ated by pairing at the complementary sequences, with
probabilities that clearly illustrate the principle, but
One of the most interesting applications of MESA is as are much lower than corresponding molecular systems.
a vehicle with which to abstract and illustrate impor- This process is the first, primitive step toward mod-
tant concepts in self-assembly modeled on those exhib- eling the formation of double-stranded DNA by the
 D. H. Gracias, I. Choi, M. Weck, and G. M. Whitesides

Figure 4: (A) Hierarchical MESA in 2D. Initially four pieces (one (w) and three (x)) aggregate via strong capillary
interactions at the hydrophobically patterned faces (dark regions). A weaker capillary interaction between two concave
hydrophobic faces enables these small aggregates to grow into larger structures. (B) Templuted MESA in 2D. Eleven circular
pieces (z), act as templates around which 87 pieces (y), assemble into cyclic hexamers through capillary interactions at the
hydrophobic patterned faces (dark regions).

association of short sequences of single-stranded DNA. and spinning at the liquid-air interface. Circulardisks
Although this model has obvious limitations, it con- made up of PDMS doped with magnetite (1 mm inner
tains several features analogous to molecular systems, diameter, 400 wm thick) were placed in a dish filled
including cooperativity, directionality, and sequence with a liquid (typically ethylene glycol/water or glyc-
specificity. erin/water with kinematic viscosities 1-50 cp). A bar
magnet (5.6 cm x 4 cm x 1 cm) was then rotated be-
7 Dynamic Self-Assembly low the dish ( 2.4 cm below the air-liquid interface),
at an angular velocity of 200-1200 r.p.m. Figure 6 is
We have also examined dynamic self-assembly systems, a schematic diagram of the experimental set-up. Fig-
that is, systems that develop order only when dissipat- ure 7 shows the dynamic patterns formed with different
ing energy. These systems are particularly interest- numbers of disks. A variety of patterns were observed,
ing for their possible relevance to issues of complexity ranging from unnucleated (n<5) to nucleated (n>5) to
and emergence and ultimately, perhaps to understand- bistable patterns (n=10, 12). The wealth of phenom-
ing very complex systems, such as the cell. Here we ena displayed by this system makes it a good one to
describe the formation of dynamic patterns of unex- study complexity: the interactions involved are simple
pected regularity and complexity by millimeter-sized enough to be studied in detail but the patterns are far
magnetic disks subjected to a rotating magnetic field from obvious.
Meso-Scale Self-Assembly

Figure 6: The scheme of the experimental setup for the

dynamic self-assembly. A bar magnet rotates below a dish
filled with liquid. Magnetically doped disks are placed on
the interface, and are fully immersed in the liquid except
for their top surface. The disks spin around their azes.
A magnetic force (Fm) attracts the disks towards the center
of the dish, and hydrodynamic force (F},) pushes them apart
from each other.
Figure 5: Examples of a two-dimensional model for
sequence-specific self-assembly. In all three cases, a long ECS-9729405), the National Institutes of Health (grant
strand and a short probe were placed at the water/PFD GM 30367), and DARPA. M.W. thanks the German
interface and swirled at a frequency of 60 r.p.m. on an Academic Exchange Service (DAAD) for a postdoc-
orbital shaker. These objects interacted through hydropho- toral fellowship.
bic lock and key structures. Self-assembly between the long
strand and the probe at a pre-determined recognition site
took place with high probability, in less than two hours. F'g- References
ures show the self-assembled structures based on sequence-
specific recognition between (a) a 22-membered single strand [1] A. Terfort, N. Bowden, and G. M. Whitesides. Three-
dimensional self-assembly of millimeter-scale compo-
and a seven-membered probe, (b) a larger 50-membered
nents. Nature, 386:162-164, 1997.
single strand and an eight-membered probe, and (c) the
largest single strand examined (100-membered) and a nine- [2] B. A. Grzybowski, H. A. Stone and G. M. Whitesides.
membered probe. Dynamic self-assembly of magnetized, millimeter-sized
objects rotating at the liquid-air interface. Submitted.

Acknowledgments [3] I. S. Choi, M. Weck, B. Xu, N. L. Jeon and G. M.

Whitesides. Mesoscopic templated self-assembly at the
Financial support was provided in part by the Na- fluid-fluid interface. Langmuir. (In Press).
tional Science Foundation (grant CHE-9801358 and
 D. H. Gracias, I. Choi, M. Weck, and G. M. Whitesides

Figure 7: The dynamic patterns formed by various numbers (n) of disks rotating at the ethylene glycol-air interface,
27 mm above the plane of the external magnet. The disks are composed of polyethylene shell (white) of outer diameter
1.27 mm, filled with PDMS doped with 25 % w/w of magnetite (black core). All disks spin around their centers at w=700
r.p.m, and the entire aggregate slowly (<2 r.p.m.) precesses around its center. For n<5, the aggregates do not have a
“nucleus”-all disks are precessing on the rim of a circle. For n>5, nucleated structures appear. For n=10 and n=12, the
patterns are bistable in the sense that the two observed patterns interconvert irregularly with time. For n=19, the hexagonal
pattern (left) appears only above w 800 r.p.m., but can be “annealed” down to 700_r.p.m. by slowly decreasing the spinning
rate. Without annealing, a less symmetric pattern exists at w=700 r.p.m.

[4] I.S. Choi, N. Bowden and G. M. Whitesides. Macro- the “capillary bond”. Journal of the American Chem-
scopic, hierarchical, two-dimensional self-assembly. ical Society, 121:5373-5391, 1999.
Angewandte Chemie International Edition, 38:3078—
3081, 1999. P. A. Kralchevsky and K. Nagayama. Capillary forces
between colloidal particles. Langmuir, 10:23-36, 1994.
[5cometM. Weck, I. S. Choi, N. L. Jeon and G. M. Whitesides.
Assembly of mesoscopic analogs of nucleic acids. Jour- T. L. Breen, J. Tien, S. R. J. Oliver, T. Hadzic and G.
nal of the American Chemical Society. (In Press). M. Whitesides. Design and self-assembly of open, reg-
ular, 3d mesostructures. Science, 284:948-951, 1999.
[6] N. Bowden, A. Terfort, J. Carbeck and G. M. White-
[10] M. M. C. A. L. Lehninger, D. L. Nelson. . Principles
sides. Self-assembly of mesoscale objects into ordered of Biochemistry 2nd Ed. Worth Publishers, 1993.
two-dimensional arrays. Science, 276:233-235, 1997.
[11] I. S$. Choi, N. Bowden and G. M. Whitesides. Shape-
[7] N. Bowden, I. S$. Choi, B. A. Grzybowski and G. selective recognition and self-assembly of mm-scale
M. Whitesides. Mesoscale self-assembly of hexagonal components. Journal of the American Chemical So-
plates using lateral capillary forces: Synthesis using ciety, 121:1754-1755, 1999.
Meso-Scale Self-Assembly

[12] T. L. B. J. Tien and G. M. Whitesides. Crystalliza- [15] M. V. M. M. Mrksich and G. M. Whitesides. Using
tion of millimeter-scale objects with use of capillary Self-Assembled Monolayers to Study the Interactions
forces. Journal of the American Chemical Society , of Man-Made Materials with Proteins; 2nd ed. Landes
120:12670—12671, 1998. Co.: Austin, TX, 1997.

[13] NEB AN] X2 , Isaacs,’ DN. Chin and *G. M.

[16] J. W. Mullin. Crystallization. London, Butterworths,
Whitesides. Self-assembling Systems on Scales from
1961.
Nanometers to Millimeters: Design and Discovery. D.
N. Reinhoudt, John Wiley and Sons, New York, 1999.
[17] W. T. S. Huck, J. Tien and G. M. Whitesides. Three-
[14] J.-M. Lehn. Supramolecular Chemistry: Concepts and dimensional mesoscale self-assembly. Journal of the
Perspectives: A Personal Account. VCH: Weiheim, American Chemical Society, 120:8267—8268, 1998.
1995.
 ‘i ' es ie

" a plo af she fis

7 Pea Pr ay pth veh

me coyrmids 4)
Robust Geometric Computing in Motion
Dan Halperin, Tel Aviv University, Israel

Transforming a geometric algorithm into an effective study concerning complexity measures, the arithmetic
computer program is a difficult task. This transfor- model and the treatment of degenerate input.
mation is made particularly hard by the basic assump- In this paper we discuss the gap between the theory
tions of most theoretical geometric algorithms concern- and practice of geometric algorithms. We then describe
ing complexity measures and (more crucially) the han- efforts to settle this gap and facilitate the successful
dling of robustness issues. The paper starts with a dis- implementation of geometric algorithms in general and
cussion of the gap between the theory and practice of of algorithms for geometric arrangements and motion
geometric algorithms, together with a brief review of planning in particular.
existing solutions to some of the problems that this di-
Most of the algorithms developed in Computational
chotomy brings about.
Geometry aim for efficient worst-case asymptotic run-
We then turn to an overview of the CGAL project and ning time and storage. The computational model used
library. The CGAL project is a joint effort by a number is the so-called real RAM model that allows for infinite
of research groups in Europe and Israel to produce a ro- precision arithmetic operations [64]. Every operation
bust software library of geometric algorithms and data on a (small) constant number of simple geometric ob-
structures. The library is now available for use with jects (such as line segments, circles, planes) takes ‘unit’
significant functionality. We describe the main goals time. In addition the input to the algorithm is assumed
and results of the project. to be in general position, namely degenerate configura-
The central part of the paper is devoted to arrange- tions are excluded. For example, no three of a set of
ments (i.e., space subdivisions induced by geometric ob- input points may lie on a single line. We collectively
jects) and motion planning. We concentrate on the refer to the issues of arithmetic precision and the treat-
maps and arrangements part of the CGAL library. Then ment of degenerate input as robustness issues.
we describe two packages developed on top of CGAL for Each of the assumptions we have just mentioned
constructing robust geometric primitives for motion al- needs to be revised in practice. Some of the prob-
gorithms. lems are not unique to geometric algorithms, such as
the fact that the standard asymptotic measures of al-
gorithm performance may hide prohibitively large con-
1 Introduction
stants. This pitfall has been observed many times in
geometry, for example in the context of range search-
For over two decades, research in Computational
ing [37], vertical decomposition [50], and construction
Geometry has yielded numerous efficient algorithms
of Minkowski sums [4]; the latter example will be de-
and data structures for solving problems arising in a
scribed in more detail in the sequel.
diversity of areas from statistics and chemistry to GIS
and robotics [42],[54],[68]. Whereas the original focus Sometimes the worst-case resource bounds are de-
of this field was almost exclusively theoretical, these terringly high since they cover the treatment of even
days a lot of attention is given to practical solutions the most pathological non-realistic input instances.
and their software implementation. Transforming a This has led to the study of realistic input models
theoretical geometric algorithm into an effective run- (a.k.a. “fatness”). Making additional assumptions on
ning program is a hard task. The difficulties in this the input (based on the specific nature of a problem
process are rooted in the assumptions of the theoretical at hand) and tailoring the algorithms accordingly can
10 D. Halperin

result in much more efficient algorithms that are often A solution is given by using exact arithmetic (see,
simpler [5],[27],[48],[76], [77]. e.g., [17],[18],{20],[25],[80]). We can use rational num-
bers and maintain the numerator and denominator as
The unit-cost model for operations on a constant
unlimited length integers (supported by several soft-
number of simple geometric objects is also question-
ware packages). Precise predicates are also supported
able, especially when we aim for robust computation.
when they involve square roots [30] or even taking the
Already in two-dimensions and for collections of simple
k-th root [19],[59].
algebraic curves (e.g., circular arcs), algorithms need to
determine the sign of a polynomial of a rather high de- Exact arithmetic predicates can be rather time con-
gree [14]. Carrying out one such operation robustly suming. There are several adaptive evaluation schemes
may be costly. Redesigning algorithms so that they that save computing time by resorting to expensive
use lower-degree predicates was recently proposed as a computations only when they cannot determine the
means to enhance robustness [13],[14]. correct answer by simpler means like standard com-
puter arithmetic. One form of adaptive evaluation is a
Indeed, robustness issues seem to be the most criti-
floating-point filter which makes use of the hardware-
cal in the passage from theory to practice in geometric
supported floating-point arithmetic at the initial stage.
algorithms. Ignoring these issues can result in unreli-
For the use of such techniques in exact geometric com-
able or incorrect programs. This has led to an intensive
puting see [29],{40],|60],[74]. The packages described in
study in recent years. We briefly mention the main ap-
Sections 3 and 4 make extensive use of LEDA’s filters
proaches to handling robustness issues next and refer
[60] to speed up their running time; see for example the
the reader to recent surveys on the topic [69],[79] for
experimental results reported in [39] for a comparison
further information.
of exact arithmetic with or without filters.
Let us illustrate the problem of arithmetic precision
If we are not using exact arithmetic, precision prob-
in geometric computing. Consider the two polygons on
lems become especially noticeable at or near degen-
the left-hand side of Figure 1: does the small polygon
eracies. Burnikel et al. propose to handle degeneracies
fit inside the cavity in the larger polygon? We find the
directly [21] for certain two-dimensional problems. In
answer by computing the Minkowski sum of the larger
three- and higher-dimensional problems, however, di-
polygon and a copy of the smaller polygon rotated by
rectly handling degeneracies is an extremely tedious
180° degrees (the details are deferred to Section 4.1).
and error-prone task. Symbolic perturbation was sug-
Suppose the answer is yes as the figure implies. To
gested as a general means to overcome degeneracies
give a correct answer we must be able to identify a
[32],[34],[78] provided that exact arithmetic is avail-
singular point on the boundary of the Minkowski sum
able. If one insists on using only fixed-precision arith-
(in the middle of the sum on the right-hand side of
metic then several methods that approximate the geo-
Figure 1) which is the intersection point of many line
metric objects were proposed to guarantee robust-
segments. The standardly available computer arith-
ness and/or remove degeneracies; see, for example,
metic and number types could not in general give such
answers. [43] ,[50], [53] ,[61],[75).
Because of all the difficulties in implementing
geometric algorithms (as discussed above) several
computational-geometry groups have decided to put
up a carefully designed and implemented library with
an emphasis on robustness and generality [63]. This
resulted in CGAL: The Computational Geometry Al-
gorithms Library.
The CGAL project! officially started in September
1996. The actual work on the library’s kernel started
Figure 1: The small polygon can fit into the cavity in the
larger polygon (left-hand side) as indicated by a singular “The CGAL project and its successor project GALIA were
point in the middle of the Minkowski sum of the larger poly- both funded by the European Union. Throughout the paper
gon with a rotated copy of the small one (right-hand side). we refer to both projects as the CGAL project.
Robust Geometric Computing in Motion
11
earlier [35] and drew on the experience from still ear- Aviv University (Israel) and Utrecht University (The
lier projects including: the geometric part of LEDA Netherlands).
[59], C++gal ?, PlaGeo and SpaGeo?, and the XYZ-
workbench [70]. In the next section we give an overview Among the major goals in the design of the library
of the CGAL project and library, the main design goals were robustness, generality, and efficiency [16],[35],[36].
In general robustness is achieved in CGAL through
that guided the development of the library and a brief
exact computation of geometric predicates, which often
description of its various parts.
require the use of special number types. The user is free
CGAL is definitely not the only software develop- to choose different number types such as the machine
ment effort of computational geometry algorithms. Im- float or double, but then the algorithms are not always
plementations of geometric algorithms have been re- guaranteed to produce the correct output. There is a
ported in the literature and were made available for clear indication in CGAL under which conditions the
various problems; see [7] and [8]. These include li- algorithms are certified to work correctly.
braries and workbench efforts. Schirra [69] comments
Number types could be easily imported from other
that as far as libraries/workbenches are concerned, two
packages such as LEDA or The GNU Multiple Preci-
have paid special attention to robustness issues: The
sion Arithmetic Library [44] and smoothly used with
XYZ-workbench and LEDA.
CGAL. The ability of the user to specify different num-
This paper concentrates on arrangements and mo- ber types for the same algorithm is one aspect of the
tion planning. Arrangements are space subdivisions generality of the library. The project has adapted
induced by geometric objects and they are closely re- [16] the so-called generic programming paradigm that
lated to the study of collision-free path planning for makes heavy use of the C++ template mechanism [9].
objects moving among obstacles. Each of these top- In addition to number types users can choose a repre-
ics, as well as the connection between them, has been sentation type for point coordinates: Cartesian or ho-
the subject of extensive research. We give more back- mogeneous. Major flexibility and generality is achieved
ground about these areas and point to bibliography in through the use of ‘traits classes’. A traits class brings
the relevant sections. In Section 3 we focus on the maps together all the data types and operations needed by
and arrangements part of the library which has been a certain algorithm and is passed as an additional pa-
developed at Tel Aviv University. We explain the con- rameter to the algorithm. In Section 3.2 we illustrates
nection between arrangements and motion planning in how traits classes enhance the generality of the code
Section 4 and describe two packages developed on top for representing two-dimensional subdivisions.
of CGAL that provide robust primitives for motion algo-
For a detailed discussion of the above and additional
rithms. Concluding remarks and directions for further
goals in the design of CGAL see [16],[35],[36].
research are given in Section 5.
Notice that there is no global strategy for handling
degenerate input in CGAL. If an algorithm is claimed
2 The CGAL Project and Library
to perform correctly as long as exact predicates are
The Computational Geometry Algorithms Library provided, this means in particular that it could han-
CGAL is a software library of robust geometric algo- dle degenerate input. While this is often doable and
even desirable for two-dimensional problems [21] it
rithms and data structures.
may be unnecessary or extremely difficult for higher-
The participating groups in the CGAL project are dimensional problems. In Section 4.2 we propose an
from ETH Zurich (Switzerland), The Free Univer- efficient method for removing degeneracies from three-
sity Berlin (Germany), Halle University (Germany), dimensional arrangements.
INRIA Sophia-Antipolis (France), Max Planck Insti-
CGAL consists of three parts. The first is the ker-
tute Saarbrucken (Germany), RISC Linz (Austria), Tel
nel, which consists of primitive constant-size geometric
2p. Avnaim, C++gal: A C++ library for geometric al- objects and predicates on them (e.g., points and ori-
gorithms, INRIA Sophia-Antipolis, 1994.
entation test for points). The second part, the basic
3G.-J. Giezeman, PlaGeo, a library for planar geome-
try, and SpaGeo, a library for spatial geometry, Utrecht
library, consists of a large collection of fundamental
University, 1994. algorithms (e.g., constructing convex hulls) and data
12 D. Halperin

structures (e.g., kd-trees) parametrized by trait classes. more information about arrangements see, for example,
The maps and arrangements package that we describe [3},[31],[46], [73].
in the next section is part of the basic library. The third Arrangements are a useful tool in the study of geo-
part incorporates non-geometric support facilities such metric problems in robotics and other areas. We use
as I/O support for debugging and for interfacing CGAL them to partition a space into cells such that certain
to various visualization tools. The full functionality of invariants are maintained in each cell. The objects in
the library can be found in the manuals of the vari- S that define the arrangement are the loci of critical
ous parts [24]. Additional documentation includes an points in space where the invariants change. Arrange-
installation guide and a “getting started” manual guid- ments lead to efficient solutions in, among other areas,
ing the novice how to use the library through a series
robot motion planning (as we will explain in more de-
of program examples.
tail in Section 4.1), assembly planning, and visibility
CGAL can be used as a stand-alone library. However, problems [46],[73].
there are close connections between CGAL and LEDA—
Motivated by these applications the Tel Aviv site
the library of efficient data structures and algorithms.
of the CGAL project has been responsible for the de-
There are provisions in CGAL for easy use of LEDA’s
velopment of a package to support the construction
special number types, graphical window and graphical
and manipulation of arrangements of general curves.
output to a postscript file. For a succinct listing of the
Our two-dimensional package is described in this sec-
principal differences between CGAL and the geometric
tion; many technical details that we omit here can be
part of LEDA see [60, Section 9.11].
found in the CGAL user manual [24, Part II, Chapters 8
through 10] and in [39] and [51]. In Section 4.2 we de-
3 Maps and Arrangements scribe implementations for three-dimensional arrange-
ments of triangles and of polyhedral surfaces built on
Given a collection S$ of geometric objects (such as lines,
top of CGAL.
planes, or spheres) the arrangement A(S) is the sub-
division of the space where these objects reside into
cells as induced by the objects in S. Figure 2 illus- 3.1 From Maps to Arrangements
trates a planar arrangement of segments which consists
of vertices, edges and faces: a vertex is either a segment
Subdivisions could be used in CGAL in one of four lev-
els, where each higher level is built on top of the previ-
endpoint or the intersection point of two (or more) seg-
ments, an edge is a maximal portion of a segment not
ous one: (i) topological maps (which are not necessarily
containing any vertex, and a face is a maximal region planar), (ii) planar maps, (iii) planar maps with inter-
of the plane not containing any vertex or edge. We sections, and (iv) arrangements. We will restrict our
assume below some familiarity with arrangements; for description here to planar maps and arrangements.
A planar map in CGAL is defined as the subdivi-
sion of the plane into cells by a collection of pairwise
interior-disjoint z-monotone curves. The choice to re-
strict the curves to be z-monotone at this level makes
it easy to apply algorithmic methods such as the verti-
cal decomposition [28] to otherwise general curves. The
planar map level is the natural level to represent a sim-
ple plane subdivision such as a geographic map or a
room with polygonal obstacles.

;
Dee
x Planar maps in CGAL support traversal over faces,
edges and vertices of the map, traversal over the edges
of a face and around a vertex and efficient point loca-
tion (namely identifying the feature of a map where a
query point is located). They are dynamic and support
Figure 2: An arrangement of segments. insertion of new edges into or removal of edges out of an
Robust Geometric Computing in Motion
13

ing the geometric representation of the objects (Ho-

mogeneous, Cartesian) and flexibility in choosing the
geometric kernel (LEDA, CGAL, or a user-defined ker-
nel). The traits class enables the users to employ the
maps and arrangements package with different kinds
of curves that they define (as required by their appli-
cations). The only restriction is that they obey the
predefined interface.
The planar map traits class defines the two basic
geometric objects of the map: the point and the z-
monotone curve. In addition several types of predicates
are required: For example, a predicate that returns
whether a point is above, below, or on a given curve,
Figure 3: An arrangement of circles [52]. and a predicate that compares the y-coordinate of two
curves at a given x-coordinate.*

existing map. The representation is based on the Dou- We supply ready-made traits for line segments, cir-
bly Connected Edge List (DCEL) [28, Chapter 2]. This cular arcs, and polylines while using various number
representation belongs to a family of edge-based data types. We mainly rely on LEDA’s special number types
structures in which each edge is represented as a pair of for efficient exact arithmetic.
opposite half edges. The representation supports inner
components (holes) inside the faces. 3.3 Point Location

The curves at the arrangement level are the most Point location is a basic service required from a subdi-
general: they may intersect and are not necessarily x- vision data structure: Given a query point p report the
monotone. The arrangement level employs the planar face of the subdivision containing p. Planar maps (and
map level and preprocesses the input curves by decom- the levels above them) provide this service. We provide
posing each one of them into z-monotone subcurves, three algorithms for point location as well as a mech-
and further subdivides them at their intersection points anism for the users to supply their own point-location
with the other curves. A data structure which we call algorithm.
the hierarchy tree maintains the curve-history of each
The algorithms we have implemented are: (i) a naive
of the edges in the final planar map (namely the orig-
algorithm, which goes over all the edges in the map to
inal curves and sub-curves in which this edge is con-
find the location of the query point; (ii) an efficient
tained). In addition to the planar-map functionality,
algorithm (the default one) which is based on the ran-
the arrangement level supports overlapping curves and
domized incremental construction of a search struc-
the traversal of all the curves containing a query edge.
ture through the vertical decomposition of the map
[15],{62],[72], and (iii) a “walk-along-a-line” (walk, for
3.2 The Separation of Combinatorics and
short) algorithm that is an improvement over the naive
Numerics
approach: it finds the point’s location by walking along
As mentioned in Section 2, algorithms in CGAL are a vertical line from “infinity” towards the query point.
passed a so-called traits class that contains the needed We remind the reader that our point location imple-
data types and operations. For planar maps, the traits mentation handles general finite planar maps. The
class is an abstract interface of predicates and functions subdivision is not necessarily monotone (each face’s
that wraps the access of an algorithm to the geomet- boundary is a union of z-monotone chains) nor con-
ric (rather than combinatorial) inner representation. nected. In addition the input may be x-degenerate.
We have formulated the requirements from the planar We have observed an interesting behavior of the
map’s traits so they make as few assumptions on the point-location algorithms: During the construction of
curves as possible. Packing those predicates and func-
tions under one traits class gives flexibility in choos- 4The full list of requirements can be found in [24].
14 D. Halperin

a map (or an arrangement), using the walk algo-

rithm is faster than using the presumably fast vertical-
decomposition based search structure. The latter an-
swers queries in logarithmic time whereas the walk
algorithm may require up to linear time per query.
The explanation to this phenomenon is the significant
overhead in constructing the search structure (which
does not express itself as strongly in the asymptotic
bounds). See [39] for a comparison of the performance
of our point location algorithms.
Besides the naive algorithm, implementing the point-
location algorithms for possibly degenerate arrange-
ments of arbitrary curves is non-trivial. Already the
walk algorithm raises difficulties; Mehlhorn and Naher
describe these difficulties for their walk point-location
in (straight edge) Delaunay triangulations [60]. The
naive point location was used in our implementation to
verify the correctness of the other algorithms through
the use of a so-called “double checker”: It is a debug
point-location strategy that to the planar map pack-
age looks as a regular strategy but for every query it
runs two different algorithms, compares their results
and notifies the developer in case of a mismatch.
We have also developed an adaptive point location
scheme for arrangements of parametric curves [52].
The idea is to bound a curve inside a polygon (which
serves as a polygonal approximation of the curve) and
perform operations on the resulting arrangement of
polygons. If we are unable to tell which original curves
lie on the boundary of a face containing a query point,
we refine the polygonal approximation until we can an-
swer the query; see Figure 4 for an illustration on a
collection of Bézier curves. This scheme makes use of
the arrangement polygonal-line traits.
Figure 4: An arrangement of polygons each bounding a
3.4 On-line Zone Construction Bézier curve before (a) and after (b) the first point location;
(c) displays the underlying arrangement of Bézier curves
We conclude this section with another problem on pla- and in bold line the curves that bound the face containing
nar arrangements of lines for which we implemented the query point.
and compared four different algorithms: identifying the
faces of an arrangement that are intersected by an addi- led to useful observations regarding the use of number
tional curve 7 (these faces are called the zone of y); the types in arrangement computation and about the ben-
curve is given to the algorithm on-line piece by piece. efits of caching the results of geometric computation,
This work is motivated by the efficient exploration of both constructions and predicates, to save exact com-
the area bisectors of a polygon [11] related to using putation time (for example, once computed we retain
MEMS arrays for part orienting [12]. The experiments the intersection point of two lines for later use by the
°The use of “strategy” here refers to the strategy pat- algorithm instead of recomputing it). An experimental
tern [41]. comparison between the algorithms is reported in [6].
Robust Geometric Computing in Motion
15
4 Robust Motion Primitives We distinguish three types of points in the configu-
ration space according to the placements of the robot
Arrangements are useful in solving motion planning that they represent: free placements, where the robot
problems. In this section we explain this connection does not intersect any obstacle, forbidden placements,
and describe two tools that are based on arrangements where the robot intersects the interior of an obstacle,
for producing robust primitives for motion algorithms. and semi-free placements, where the robot is in con-
The first tool constructs exact polygonal Minkowski tact with the boundary of an obstacle, but does not
sums. Minkowski sums describe configuration space intersect the interior of any obstacle. The collection
obstacles for translational motion planning and related of all the points in the configuration space that repre-
problems. Following the typical CGAL approach ro- sent semi-free robot placements partitions the configu-
bustness of the tool is achieved through the use of exact ration space into free regions and forbidden regions. In
arithmetic. Moreover, we directly handle degeneracies; a motion-planning problem with two degrees of free-
the solution depicted in Figure 1 exemplifies the han- dom, for instance, the points representing semi-free
dling of a degenerate configuration by our package. placements of the robot lie on several curves in a two-
dimensional configuration space. We call these curves
The second tool computes approzimate three- constraint curves. Note that each such curve is in-
dimensional swept volumes, which describe the space duced by the contact of a robot boundary feature and
occupied by a polyhedron as it is moved along a trajec- an obstacle boundary feature. We are therefore inter-
tory in space. Swept volumes have many applications ested in studying the partitioning of 2D space by a col-
including the verification that the motion of an object lection of constraint curves and this is where motion
along a certain trajectory can be carried out without planning and the study of arrangements meet. This
collision with the environment objects. In applications discussion extends to motion planning with more de-
where very high precision is not required, we propose a grees of freedom and higher dimensional arrangements
method which perturbs the surfaces that determine the and has been the subject of an intensive study; see e.g.,
swept volume so as to remove all possible degeneracies
and which allows for computing robustly when using
[22],[45],{49],(71),{73].
There has been much work on obtaining sharp
the standard floating-point arithmetic.
bounds on the size of the Minkowski sum of two sets
in two and three dimensions, and on developing fast
4.1 Exact Polygonal Minkowski Sums
algorithms for computing Minkowski sums. If P is a
Given two sets P and Q in R?, their Minkowski sum (or polygonal set with m vertices and Q is another polyg-
vector sum), denoted by P © Q, is the set {p+ q|p€ onal set with n vertices, then P © Q is a portion of
P,q € Q}. Minkowski sums are used in a wide range the arrangement of O(mn) constraint segments, where
of applications, including robot motion planning [58], each segment is the Minkowski sum of a vertex of P
assembly planning [47], and computer-aided design and and an edge of Q, or vice versa. Therefore the size of
manufacturing (CAD/CAM) [33]. P@Q is O(m?n?) and it can be computed within that
time; this bound is tight in the worst case [55]; see Fig-
Consider, for example, a planar setting with an ob- ure 5. The sum has lower worst-case complexity when
stacle P and a robot Q that moves by translation. We one of the polygons or both are convex.
can choose a reference point r rigidly attached to Q
and suppose that Q is placed such that the reference
point coincides with the origin. If we let Q’ denote a
copy of Q rotated by 180° degrees, then P 6 Q’ is the
locus of placements of the point r where PQ #9. In
the study of motion planning the space of all possible
placements of the reference point is called the configu-
ration space of Q and this sum is called a configuration
space obstacle because @ collides with P when trans- Figure 5: P and Q are polygons with horizontal and verti-
lated along a path 7 exactly when the point r, moved cal teeth with m and n vertices respectively. The complexity
along 7, intersects P © Q’. of P@®Q is O(m?n’).
16 D. Halperin

rere tersection of many edges or the overlap of edges. For

ee \
further details on the algorithms and a comparison of
ova
7 oe
\\
\
their performance, see [38].
Besant
4 Sati
ith
As the bound 0(m?n?) indicates, even for input
polygons with a moderate number of vertices the out-
put can be huge. This, together with our choice to
use exact arithmetic, necessitates speeding up the al-
gorithm in other ways. We discovered that the choice of
Figure 6: Tight passage: the desired target placement for the decomposition in Step 1 can have a dramatic effect
the small polygon is inside the inner room defined by the on the running time of the Minkowski-sum algorithms.
larger polygon (left-hand side). In the configuration space (Note that in the asymptotic complexity measures the
(right-hand side) the only possible path to achieve this target choice of decomposition is meaningless: every reason-
passes through the line segment emanating from the hole in able decomposition, including an arbitrary triangula-
the Minkowski sum [38]. tion, produces subpolygons of total complexity O(k)
where k is the complexity of the polygon.) This led us
We devised and implemented three algorithms for to investigate what constitute good decompositions for
computing the Minkowski sum of two polygonal sets efficient construction of Minkowski sums [4].
[4], [38]. Our main goal was to produce a robust and We studied and experimented with various well-
exact implementation. This goal was achieved by em- known decompositions as well as with several new de-
ploying the CGAL planar map package (Section 3) while composition schemes. Among our findings are that in
using exact number types. We are currently using our general: (i) triangulations are too costly (although they
software to solve translational motion planning prob- can be produced quickly, they considerably slow down
lems in the plane. We are able to compute collision- the Minkowski-sum computation), (ii) what constitutes
free paths even in environments cluttered with obsta- a good decomposition for one of the input polygons
cles, where the robot could only reach a destination depends on the other input polygon—consequently, we
placement by moving through tight passages, practi- developed a procedure for simultaneously decomposing
cally moving in contact with the obstacle boundaries. the two polygons such that a “mixed” objective func-
See Figure 6 for an illustration. This is in contrast with tion is minimized, (iii) there are optimal decomposition
most existing motion planning software for which tight algorithms that significantly expedite the Minkowski-
or narrow passages constitute a significant hurdle. We sum computation, but the decomposition itself is ex-
remark that our tool can only handle translational mo- pensive to compute — in such cases simple heuristics
tion; steps towards robust handling of polygon rotation that approximate the optimal decomposition perform
are discussed in [23]. very well. The decomposition methods, the experi-
The input to our algorithms consists of two polygo- ments, and the conclusions drawn from them are re-
nal sets P and Q (each being an arbitrary collection ported in [4].
of simple polygons), with a total of m and n ver-
tices respectively. Our algorithms consist of the fol- 4.2 Degeneracy-Free Approximate Swept
lowing three steps: (1) Decompose the polygons of P Volumes
into convex subpolygons P), P2,...,P; and the poly-
gons of Q into convex subpolygons Qi, Q2,...,Qz, (2) A swept volume is the geometric space occupied by an
For each i € [l..s] and for each j € [1..t] compute object moving along a prescribed trajectory in a given
the Minkowski subsum P; ®Q,;, and (3) Construct the time interval. The motion can be translational and
union of all the subsum polygons P; 6 Q; computed rotational. We call the moving object a generator and
in Step 2; the output is represented as a planar map. its motion a sweep. See Figure 7 for an illustration.
The three algorithms differ in the way they compute Swept volumes play an important role in many geo-
the union (Step 3). The ability to discover the semi-free metric applications, such as geometric modeling, robot
placement of Figure 1 or the tight passage of Figure 6 workspace computation, numerical control cutter path
requires the program to identify the simultaneous in- generation, and assembly design. There is rich liter-
Robust Geometric Computing in Motion
17

cies in three-dimensional arrangements is an arduous

task. Moreover, because of the nature of the problem
at hand and the approximate solution we have chosen
for it, the recognition of all the degeneracies in itself is
irrelevant. Identification of all the degeneracies is only
meaningful as a way to extract a consistent topology of
the outer cell boundary—but we will obtain the same
goal in a much simpler way. We apply a “controlled”
perturbation to the surfaces in P so that all degenera-
cies are removed [66],[67]. One of our main goals here
Figure 7: The volume swept by a square moving along a is to allow to manipulate the swept volume robustly
helical trajectory. with standard floating-point arithmetic. We describe
our scheme next.
ature on swept volumes which is beyond the scope of Since we aim to use standard floating-point arith-
this paper to review; see, e.g., [1] for a bibliography. metic, we are unable to tell for sure whether a degener-
acy exists. We can only tell that a potential degeneracy
Consider the following question: Given an assembly
exists. To define such a potential degeneracy formally,
of parts, a specific part P in the assembly, and a poten-
we use a resolution parameter, € > 0, which is a small
tial path a for removing P out of the assembly—can
positive real number. Two polyhedral features (e.g., a
P be moved along 7 without colliding with the other
vertex and a non-incident facet) are assumed too close
parts of the assembly? We can answer this question by
(and therefore potentially degenerate) whenever they
computing the swept volume of P along zm and check it
are less than € away from each other. We assume that
for collision with the other parts. We were specifically
€ is given as an input parameter according to the ma-
motivated by problems in large assemblies and manual
chine precision, the type of the arithmetic operations
removal of parts for maintenance and repair.
and the depth of the expression tree [69]. In our al-
We chose to implement an algorithm proposed by gorithm this tree’s depth is a small constant, thus € is
Abrams and Allen [2] which handles polyhedral bod- a constant that can be determined and bounded inde-
ies moving along an arbitrary trajectory in a motion pendently of the input size.
that can be translational and rotational, and outputs Next we define a perturbation radius 6, which is
a polyhedral approximation of the appropriate swept proven to be sufficiently small (6 depends on ¢€ and the
volume; we will refer to this algorithm as ASW (ap- input polyhedral surfaces). Any polyhedral surface in
proximate swept volume). The algorithm generates a P or any feature thereof will be perturbed by at most
set P of polyhedral surfaces such that the boundary of 6. Specifically, each vertex of any surface in P is moved
the desired swept volume is the boundary of the outer by a Euclidean distance of at most 6 from its original
cell of the arrangements A(P). (ASW ignores voids.) placement.
Our overall plan was: (i) use ASW to produce the set
Our perturbation scheme is “controlled” in two ways.
P and then (ii) use an implementation of the space First, by determining the size of 6 we set a tradeoff that
sweep algorithm [26], adapted from triangles to poly-
controls the magnitude of the perturbation versus the
hedral surfaces, to compute the outer cell of A(P). efficiency of the computation of the perturbation— the
However, the algorithm described in [26] assumes larger the 6 the faster the computation. Second, unlike
general position (its implementation already has to various popular heuristic perturbation schemes (e.g.,
handle a large number of cases even when assuming “heuristic epsilons” [69]), our perturbation guarantees
general position). Abrams and Allen also report that that the resulting collection of polyhedral surfaces is
they could not find robust software for arrangements degeneracy free by carrying out a controlled incremen-
that could lead to a robust implementation of their so- tal insertion process where we do not proceed to the
lution [2]. Unlike two-dimensional arrangements where next iteration before the arrangement induced by the
directly handling degeneracies is a satisfying solution subcollection of surfaces or subsurfaces of P we inserted
(as in Section 4.1 above), full treatment of degenera- so far is degeneracy free. Successful completion of the
18 D. Halperin

process is guaranteed for 6 values above a threshold arrangements comparable in their robustness and gen-
which is determined by the analysis of the procedure. erality to the two-dimensional package described in
Section 3. A major obstacle to achieving this goal is
The thesis [65] contains a detailed report on all the the treatment of degeneracies (as far as precision is
degeneracies arising in arrangements of polyhedral sur- concerned we could use exact arithmetic wherever pos-
faces and the swept volume computation, gives bounds
sible). We mention next several possible directions to
on the perturbation radius as a function of € and the in-
confront degeneracies in arrangements. Although we
put polyhedral surfaces, and describes the implementa-
point out the difficulties and shortcomings of each di-
tion of the scheme together with experimental results.
rection we only mention directions which we believe
Our implementation of the swept volume algorithm are feasible, perhaps in restricted form (that is, only
uses CGAL in various ways (geometric primitives, pred- in three dimensions or only for arrangements of certain
icates, affine transformations, assertions, and precon- simple types of objects).
ditions testing). Most notably it uses the polyhedral
surface package developed by Kettner [57]. e Certifying a correct result only for input in gen-
eral position. This is feasible (in the sense that as-
This work is an extension of an earlier work on
serting general position is easier than computing
arrangements of spheres as they arise in molecular
modeling [50]. In both cases (molecular modeling and a degenerate arrangement) but impractical since
swept volumes) we show by experiments [50], [67] that quite often the input of physical-world problems
also on highly degenerate input (as depicted in Fig- is not in general position.
ure 8) the controlled perturbation scheme works effi- e Directly handling all degeneracies (as we do in the
ciently even when 6 is fairly small.
two-dimensional case). This seems to be a tremen-
dous task already for arrangements of triangles in
5 Conclusions three dimensions. What is missing is a system-
atic and concise way to express and compute the
We described advancement in robust implementation topology of the arrangement at the neighborhood
of geometric algorithms. After reviewing the CGAL of degeneracies.
project and library we concentrated on the maps and
e Applying symbolic perturbation (see the Introduc-
arrangements part of CGAL and showed how this and
other parts of CGAL are useful for devising robust prim-
tion). In this solution the difficulty seems to be
itives for motion planning. in the interpretation of the output (postprocess-
ing) [69].
Regarding arrangements, the next goal would be
to develop packages for three- and higher-dimensional e Tightly approximating the arrangement, possibly
using only fixed precision arithmetic (as in Sec-
tion 4.2). There are many conceivable ways to
achieve this goal, one of which is extending the
scheme that we propose above, controlled pertur-
bation, to other objects and to higher dimensions.
Implementing the scheme is not difficult. How-
ever, giving a theoretical guarantee for its viability
for arrangements of complex objects is a difficult
task [65].

For motion planning, supporting only two- and

(currently partially) three-dimensional arrangements
means we could solve robustly and accurately prob-
Figure 8: Swept volume with many intersections (and po- lems with a limited number of degrees of freedom®
tential degeneracies) in the middle [66]. °The swept volume application described in Section 4.2
Robust Geometric Computing in Motion
19
Progress in the implementation of algorithms for Ministry of Science), and by the Hermann Minkowski
higher-dimensional arrangements would result in ro- — Minerva Center for Geometry at Tel Aviv University.
bust algorithms for systems with more degrees of free-
dom. Notably, probabilistic roadmap (PRM) tech-
References
niques already offer a solution for motion planning
problems with many degrees of freedom [10],[56]. How- [1] K. Abdel-Malek. Swept volumes: Bibliography.
ever, densely cluttered environments and narrow pas- www.icaen.uiowa.edu/~ amalek/sweep/bibliog.htm.
sages in the workspace are difficult for PRM tech-
[2] S. Abrams and P. Allen. Swept volumes and their use
niques; in fact tight passages are impossible to iden- in viewpoint computation in robot work-cells. In Proc.
tify without exact predicates. We anticipate that hy- IEEE Intl. Sympos. on Assembly and Task Planning,
bridizing the two approaches, namely PRMs and ex- pages 188-193, 1995.
act (or tightly approximate) arrangements, will lead to
[3] P. Agarwal and M. Sharir. Arrangements. In J.-R.
stronger motion planning algorithms. Sack and J. Urrutia, editors, Handbook of Computa-
tional Geometry, pages 49-119. Elsevier Science Pub-
Finally, note that CGAL offers much additional func- lishers B.V. North-Holland, Amsterdam, 1999.
tionality that could be useful for algorithms in robotics
and motion planning, including convex hulls, Voronoi [4] P. K. Agarwal, E. Flato, and D. Halperin.
diagrams, triangulations of point sets, various opti- Polygon decomposition for efficient con-
struction of Minkowski sums. Manuscript.
mization algorithms (e.g., smallest enclosing sphere), www.math.tau.ac.il/“flato/TriminkWeb, Tel Aviv
and multi-dimensional search structures. University, 1999.

[5] P. K. Agarwal, M. Katz, and M. Sharir. Comput-

Acknowledgements ing depth orders for fat objects and related problems.
Comput. Geom. Theory Appl., 5:187-206, 1995.
The work described in this paper was done by or has [6] Y. Aharoni, D. Halperin, I. Hanniel, S. Har-Peled, and
been made possible by the efforts of many individuals— C. Linhart. On-line zone construction in arrangements
the participants of the CGAL and GALIA projects. The of lines in the plane. In Proc. of the 8rd Workshop of
full list of participating sites is given at the beginning Algorithm Engineering, volume 1668 of Lecture Notes
Comput. Sci., pages 139-153. Springer-Verlag, 1999.
of Section 2.
[7] N. Amenta. Directory of computational geometry soft-
The major contributions at the Tel Aviv site which ware. www.geom.umn.edu/software/cglist /.
are described in Sections 3 and 4 have been made by
Eyal Flato, Iddo Hanniel, Oren Nechushtan, and Sigal [8] N. Amenta. Computational geometry software. In
J. E. Goodman and J. O’Rourke, editors, Handbook
Raab. Others who contributed at Tel Aviv are: Sariel
of Discrete and Computational Geometry, chapter 52,
Har-Peled, Chaim Linhart, Michal Ozery, and more pages 951-960. CRC Press LLC, Boca Raton, FL,
recently Eti Ezra, Shai Hirsch, and Eli Packer. The 1997.
author also thanks Stefan Schirra and Lutz Kettner
[9] M. Austern. Generic Programming and the STL —
for their comments on a draft of this paper.
Using and Extending the C++ Standard Template L1-
Work reported in this paper has been supported brary. Addison-Wesley, 1999.
by ESPRIT IV LTR Projects No. 21957 (CGAL) and [10] J. Barraquand, L. E. Kavraki, J. C. Latombe, T.-Y.
No. 28155 (GALIA), by the USA-Israel Binational Sci- Li, R. Motwani, and P. Raghavan. A random sampling
ence Foundation, by The Israel Science Foundation scheme for robot path planning. Internat. J. Robot.
founded by the Israel Academy of Sciences and Human- Res., 16(6):759-774, 1997.

ities (Center for Geometric Computing and its Applica- [11] K.-F. Béhringer, B. Donald, and D. Halperin. On the
tions), by a Franco-Israeli research grant “factory of the area bisectors of a polygon. Discrete Comput. Geom.,
future” (monitored by AFIRST/France and The Israeli 22:269-285, 1999.

approximately solves a problem with six degrees of free- [12] K.-F. Béhringer, B. R. Donald, and N. C. MacDon-
dom. However for solving motion planning problems ezactly ald. Upper and lower bounds for programmable vector
through the usage of arrangements, we need k-dimensional fields with applications to MEMS and vibratory plate
arrangements for problems with k degrees of freedom. part feeders. In J.-P. Laumond and M. H. Overmars,
20 D. Halperin

editors, Algorithms for Robotic Motion and Manipu- [26] M. de Berg, L. J. Guibas, and D. Halperin. Verti-
lation, pages 255-276. A. K. Peters, Wellesley, MA, cal decompositions for triangles in 3-space. Dzscrete
1996. Comput. Geom., 15:35-61, 1996.

[27] M. de Berg, M. J. Katz, A. F. van der Stappen, and

[13] J.-D. Boissonnat and F. P. Preparata. Robust plane
J. Vleugels. Realistic input models for geometric algo-
sweep for intersecting segments. Report TR 3270, IN-
RIA, Sophia Antipolis, Sept. 1997. rithms. In Proc. 13th Annu. ACM Sympos. Comput.
Geom., pages 294-303, 1997.
[14] J.-D. Boissonnat and J. Snoeyink. Efficient algo-
rithms for line and curve segments intersection using [28] M. de Berg, M. van Kreveld, M. Overmars, and
restricted prediactes. In Proc. 15th Annu. ACM Sym- O. Schwarzkopf. Computational Geometry: Algo-
pos. Comput. Geom., 1999. rithms and Applications. Springer-Verlag, Heidelberg,
Germany, 1997.
[15] J.-D. Boissonnat and M. Yvinec. Algorithmic Geome-
try. Cambridge University Press, UK, 1998. [29] O. Devillers and F. P. Preparata. A probabilistic
analysis of the power of arithmetic filters. Discrete
amd Computational Geometry, 20:523-547, 1998.
[16] H. Bronniman, L. Kettner, S. Schirra, and
R. Veltkamp. Applications of the generic program-
ming paradigm in the design of CGAL. ‘Technical [30] T. Dubé, K. Ouchi, and C.-K. Yap. Tutorial for the
real/expr package. 1996.
Report MPI-I-98-1-030, Max-Planck-Insitut fiir Infor-
matik, 66123 Saarbriicken, Germany, 1998.
[31] H. Edelsbrunner. Algorithms in Combinatorial Geom-
etry, volume 10 of EATCS Monographs on Theoretical
[17] H. Brénnimann, I. Emiris, V. Pan, and S. Pion.
Computer Science. Springer-Verlag, Heidelberg, West
Computing exact geometric predicates using modular
Germany, 1987.
arithmetic with single precision. In Proc. 18th Annu.
ACM Sympos. Comput. Geom., pages 174-182, 1997. [32] H. Edelsbrunner and E. P. Miicke. Simulation of sim-
plicity: A technique to cope with degenerate cases in
[18] H. Broénnimann and M. Yvinec. Efficient exact eval- geometric algorithms. ACM Trans. Graph., 9(1):66-
uation of signs of determinants. In Proc. 13th Annu. 104, 1990.
ACM Sympos. Comput. Geom., pages 166-173, 1997.
[33] G. Elber and M.-S. Kim, editors. Special Issue of Com-
[19] C. Burnikel, R. Fleischer, K. Mehlhorn, and S. Schirra. puter Aided Design: Offsets, Sweeps and Minkowski
Efficient exact geometric computation made easy. In Sums, volume 31. 1999.
Proc. 15th Annu. ACM Sympos. Comput. Geom.,
pages 341-350, 1999. [34] I. Z. Emiris, J. F. Canny, and R. Seidel. Efficient per-
turbations for handling geometric degeneracies. Algo-
[20] C. Burnikel, J. Konnemann, K. Mehlhorn, S. Naher, rithmica, 19(1—2):219-242, Sept. 1997.
S. Schirra, and C. Uhrig. Exact geometric computa-
tion in LEDA. In Proc. 11th Annu. ACM Sympos. [35] A. Fabri, G. Giezeman, L. Kettner, S. Schirra, and
Comput. Geom., pages C18-C19, 1995. S. Schonherr. The CGAL kernel: A basis for geo-
metric computation. In M. C. Lin and D. Manocha,
[21] C. Burnikel, K. Mehlhorn, and S. Schirra. On de- editors, Proc. 1st ACM Workshop on Appl. Comput.
generacy in geometric computations. In Proc. 5th Geom., volume 1148 of Lecture Notes Comput. Sci.,
ACM-SIAM Sympos. Discrete Algorithms, pages 16- pages 191-202. Springer-Verlag, 1996.
23, 1994.
[36] A. Fabri, G. Giezeman, L. Kettner, S. Schirra, and
[22] J. Canny. The Complexity of Robot Motion Planning. S. Schénherr. On the design of CGAL, the Com-
ACM — MIT Press Doctoral Dissertation Award Se- putational Geometry Algorithms Library. Technical
ries. MIT Press, Cambridge, MA, 1987. Report MPI-I-98-1-007, Max-Planck-Institut Inform..,
1998. ‘To appear in Software — Practice and Experi-
[23] J. Canny, B. R. Donald, and E. K. Ressler. A rational ence.
rotation method for robust geometric algorithms. In
Proc. 8th Annu. ACM Sympos. Comput. Geom., pages [37] P.-O. Fjallstrém, J. Petersson, L. Nilsson, and
251-260, 1992. Z. Zhong. Evaluation of range searching methods for
contact searching in mechanical engineering. Internat.
The CGAL User Manual, Version 2.1, 2000. J. Comput. Geom. Appl., 8:67-83, 1998.
www.cs.uu.nl/CGAL.
[38] E. Flato and D. Halperin. Robust and efficient con-
K. L. Clarkson. Safe and effective determinant eval- struction of planar Minkowski sums. In Abstracts
uation. In Proc. 83rd Annu. IEEE Sympos. Found. 16th European Workshop Comput. Geom., pages 85-
Comput. Sci., pages 387-395, Oct. 1992. 88, Eilat, 2000.
Robust Geometric Computing in Motion
21

[39] E. Flato, D. Halperin, I. Hanniel, and O. Nechush-

[52] I. Hanniel and D. Halperin. Two-dimensional arrange-
tan. The design and implementation of planar maps ments in CGAL and adaptive point location for para-
in CGAL. In Proc. of the 3rd Workshop of Algorithm metric curves. Manuscript. Tel Aviv University, 2000.
Engineering, volume 1668 of Lecture Notes Comput.
Sci., pages 154-168. Springer-Verlag, 1999. [53] J. Hobby. Practical segment intersection with fi-
nite precision output. Comput. Geom. Theory Appl.,
[40] S: Fortune and C. J. Van Wyk. Static analysis yields 13:199-214, 1999.
efficient exact integer arithmetic for computational
geometry. ACM Trans. Graph., 15(3):223-248, July [54] W. D. Jones. Computational geometry, a community
1996. bibliography. Overview at
sal.cs.uiuc.edu/“jeffe/compgeom/compgeom.html.
[41] E. Gamma, R. Helm, R. Johnson, and J. Vlissides. De-
sign Patterns — Elements of Reusable Object-Oriented [55] A. Kaul, M. A. O’Connor, and V. Srinivasan. Com-
Software. Addison-Wesley, 1995. puting Minkowski sums of regular polygons. In Proc.
3rd Canad. Conf. Comput. Geom., pages 74-77, Aug.
[42] J. E. Goodman and J. O’Rourke, editors. Handbook 1991.
of Discrete and Computational Geometry. CRC Press
LLC, Boca Raton, FL, 1997. [56] L. Kavraki, P. Svestka, J. Latombe, and M. Overmars.
Probabilistic roadmaps for fast path planning in high
[43] M. Goodrich, L. J. Guibas, J. Hershberger, and dimensional configuration spaces. [EEE Tr. on Rob.
P. Tanenbaum. Snap rounding line segments efficiently and Autom., 12:566-580, 1996.
in two and three dimensions. In Proc. 13th Annu.
ACM Sympos. Comput. Geom., pages 284-293, 1997. [57] L. Kettner. Designing a data structure for polyhedral
surfaces. In Proc. 14th Annu. ACM Sympos. Comput.
Geom., pages 146-154, 1998.
[44] T. Granlund. GNU MP, The GNU Multiple Precision
Arithmetic Library, version 2.0.2, June 1996.
[58] J.-C. Latombe. Robot Motion Planning. Kluwer Aca-
demic Publishers, Boston, 1991.
[45] D. Halperin. Robot motion planning and the single
cell problem in arrangements. Journal of Intelligent
and Robotic Systems, 11:45-65, 1994. [59] kK. Mehlhorn, S. Naher, M. Seel, and C. Uhrig. The
LEDA User Manual, Version 4.1. Max-Planck-Insitut
fiir Informatik, 66123 Saarbriicken, Germany, 1999.
[46] D. Halperin. Arrangements. In J. E. Goodman and
J. O’Rourke, editors, Handbook of Discrete and Com-
putational Geometry, chapter 21, pages 389-412. CRC
[60] K. Melhorn and S. Naher. The LEDA Platform of
Combinatorial and Geometric Computing. Cambridge
Press LLC, Boca Raton, FL, 1997. University Press, 1999.

[47] D. Halperin, J.-C. Latombe, and R. H. Wilson. A

[61] V. Milenkovic. Verifiable implementations of geo-
general framework for assembly planning: The mo- metric algorithms using finite precision arithmetic.
tion space approach. In Proc. 14th ACM Symp. on In D. Kapur and J. L. Mundy, editors, Geometric
Comput. Geom., pages 9-18, 1998. Reasoning. North-Holland, Amsterdam, Netherlands,
1988.
[48] D. Halperin and M. H. Overmars. Spheres, molecules
and hidden surface removal. Comput. Geom. Theory [62] K. Mulmuley. Computational Geometry: An Introduc-
Appl., 11:83-102, 1998. tion Through Randomized Algorithms. Prentice Hall,
Englewood Cliffs, NJ, 1994.
[49] D. Halperin and M. Sharir. Arrangements and their
applications in robotics: Recent developments. In [63] M. H. Overmars. Designing the Computational Geom-
K. Goldberg, D. Halperin, J.-C. Latombe, and R. Wil- etry Algorithms Library CGAL. In Proc. 1st ACM
son, editors, Proc. Workshop Algorithmic Found. Ro- Workshop on Appl. Comput. Geom., volume 1148 of
bot., pages 495-511. A. K. Peters, Wellesley, MA, 1995. Lecture Notes Comput. Sci., pages 113-119. Springer-
Verlag, May 1996.
[50] D. Halperin and C. R. Shelton. A perturbation scheme
for spherical arrangements with application to molec- [64] F. P. Preparata and M. I. Shamos. Computational
ular modeling. Comput. Geom. Theory Appl., 10:273- Geometry: An Introduction. Springer-Verlag, New
287, 1998. York, NY, 1985.

I. Hanniel. The design and implementation of planar [65] S. Raab. Controlled perturbation for arrangements of
[51] polyhedral surfaces with application to swept volumes.
arrangements of curves in CGAL. M.Sc. thesis, Dept.
Comput. Sci., Tel Aviv University, Tel Aviv, Israel, M.Sc. thesis, Dept. Comput. Sci., Bar lan University,
2000. In preparation. Ramat Gan, Israel, 1999.
22 D. Halperin

[66] S. Raab. Controlled perturbation of arrangements of [73] M. Sharir and P. Agarwal. Davenport-Schinzel Se-
polyhedral surfaces with application to swept volumes. quences and Their Geometric Applications. Cam-
In Proc. 15th Annu. ACM Sympos. Comput. Geom., bridge University Press, 1995.
1999.
[74] J. Shewchuk. Adaptive robust floating-point arith-
[67] S. Raab and D. Halperin. Controlled perturbation of metic and fast robust geometric predicates. Discrete
arrangements of polyhedral surfaces with application Comput. Geom., 18:305-363, 1997.
to swept volumes. Full version, manuscript, Tel Aviv
University, 2000. [75] K. Sugihara. On finite-precision representations of
geometric objects. J. Comput. Syst. Sct., 39:236—-247,
[68] J.-R. Sack and J. Urrutia, editors. Handbook of Com- 1989.
putational Geometry. North-Holland, 1999.
[76] A. F. van der Stappen. Motion Planning amidst Fat
[69] S. Schirra. Robustness and precision issues in geomet- Obstacles. Ph.D. dissertation, Dept. Comput. Sci.,
ric computation. In J.-R. Sack and J. Urrutia, editors, Utrecht Univ., Utrecht, Netherlands, 1994.
Handbook of Computational Geometry, pages 597-632.
Elsevier Science Publishers B.V. North-Holland, Am- [77] A. F. van der Stappen, D. Halperin, and M. H. Over-
sterdam, 1999. mars. The complexity of the free space for a robot
moving amidst fat obstacles. Comput. Geom. Theory
[70] P. Schorn. Robust algorithms in a program library for Appl., 3:353-373, 1993.
geometric computation. Ph.D. thesis, ETH Ziirich,
Switzerland, 1991. Report 9519. [78] C. K. Yap. A geometric consistency theorem for a
symbolic perturbation scheme. J. Comput. Syst. Sci.,
[71] J. T. Schwartz and M. Sharir. On the “piano movers” 40(1):2-18, 1990.
problem II: General techniques for computing topolog-
ical properties of real algebraic manifolds. Adv. Appl. [79] C. K. Yap. Robust geometric computation. In J. E.
Math., 4:298-351, 1983. Goodman and J. O’Rourke, editors, Handbook of Dis-
crete and Computational Geometry, chapter 35, pages
[72] R. Seidel. A simple and fast incremental randomized 653-668. CRC Press LLC, Boca Raton, FL, 1997.
algorithm for computing trapezoidal decompositions
and for triangulating polygons. Comput. Geom. The- [80] C. K. Yap. Towards exact geometric computation.
ory Appl., 1(1):51-64, 1991. Comput. Geom. Theory Appl., 7(1):3-23, 1997.
Controlled Module Density Helps
Reconfiguration Planning
An Nguyen, Stanford University, Stanford, CA
Leonidas J. Guibas, Stanford University, Stanford, CA
Mark Yim, Xerox Palo Alto Research Center, Palo Alto, CA

In modular reconfigurable systems, individual modules modular reconfigurable robot offers many advantages
are capable of limited motion due to blocking and con- over conventional robots [7]. Such a robot has high
nectivity constraints, yet the entire system has a large fault. tolerance, since each module can be replaced by
number of degrees of freedom. The combination of these any other module in the robot; the homogeneity of the
two facts makes motion planning for such systems ex- modules allows low cost mass production; finally, the
ceptionally challenging. In this paper we present two robot can assume a wide variety of conformations or
results that shed some light on this problem. First we shapes, and thus can be easily adapted to performing
show that, for a robotic system consisting of hexagonal numerous tasks.
2D modules, the absence of a single excluded config- A reconfigurable robot changes its shape by movy-
uration is sufficient to guarantee the feasibility of the ing one or more of its modules locally. The motion of
motion planning problem (for any two connected con- each individual module in the configuration must sat-
figurations with the same number of modules). We also isfy certain constraints, as illustrated in Figure 1. In
provide an analysis of the number of steps in which the general, there are two types of constraints: the connec-
reconfiguration can be accomplished. Second, we ar- tivity constraint requires all modules to stay connected
gue that skeletal metamodules, which are scaffolding- at all times to a fixed base (for power distribution,
like structures in 2D built out of normal modules, of- inter-module communication, etc), and the blocking
fer an interesting alternative. General shapes can be constraint requires a module not to collide with other
built out of these metamodules and, unlike the case for modules during its motion. A module may also need
shapes built directly out of modules, a metamodule can to support itself on its neighboring modules in order
collapse and pass through the interior of its neighboring to move.
metamodules, thus eliminating all blocking constraints.
Though each module can have very limited motion,
This tunneling capability makes the motion planning
or none at all, the large number of modules present
problem easier and allows faster reconfiguration as well,
means that the system has many degrees of freedom —
by providing a higher bandwidth conduit, the interior
in general, roughly proportional to the number of mod-
of the shape, through which the modules can flow. The
ules present. The homogeneity of the modules further
conclusion of our work is that it is worthwhile to study
complicates the reconfiguration problem, as it is not
subclasses of shapes that (1) approximate closely arbi-
clear which module in the original configuration should
trary shapes, while also (2) simplifying significantly the
go to which location in the final configuration. Thus
motion planning problem.
motion planning for modular reconfigurable robots is
a challenging combinatorial task; in fact, under several
1 Introduction scenarios, the local blocking constraints can make the
task infeasible.
A modular reconfigurable robot, sometimes known as
a metamorphic robot consists of many identical mod- In this paper we propose to address this problem by
ules, each with limited ability to connect, disconnect, focusing on selected subsets of the allowed module con-
and move around its neighbors. Originally proposed by figurations. By imposing certain constraints that limit
Yim [12], Chirikjian [2], and Murata [7], these robots how locally packed the modules can be, we demonstrate
have since been further studied by these and several that the reconfiguration problem can become always
other authors [13, 14, 1, 4, 6, 5, 8, 9, 10, 11, 15]. A feasible, or at least significantly easier. We give two
24 A. Nguyen, L. Guibas, and M. Yim

2 Related Work

Here we survey briefly earlier related work on recon-

figurable robots. Murata et al. built a “self-assembling
machine” [7]. This machine is a robot constructed out
of 2D fractum modules. The reconfiguration of such
a robot is done with random local motions to improve
the “fitness” of a configuration with respect to the final
configuration. Due to the randomness of the planning,
Figure 1: This figure shows a reconfigurable robot com- the convergence is slow, and not practical for large
posed of rigid hexagonal modules. Each module can move problems [7]. Pamecha et al. [9] built a robot from reg-
to a nearby empty space by rotating itself around a corner ular hexagonal modules. The hexagonal module is de-
it shares with some other supporting module. The allowable signed so that it has no blocking constraints. They, too,
motions of a module are restricted. The base, always shown solved the reconfiguration problem using simulated an-
in black in this and all subsequent figures, can never move. nealing to improve the matching distance to the final
The module at D cannot move anywhere since such motion configuration. Upper and lower bounds on the opti-
would leave the configuration disconnected. The module at mal number of moves required were also given [10, 3].
A cannot move to B around supporting module E’ because Murata and Kotay et al. independently built 3D mod-
such motion is blocked by F. The module at A cannot move ular robots using 3D units [8] and 3D molecules [6]
to C either, because there 1s no module supporting the move. and discussed motion planning for their robots. Rus
and Vona [11] built a system out of cubic compress-
ible modules called Crystalline Atoms. They gave a
results, both for modular reconfigurable robots whose melt and grow algorithm to do the motion planning
modules are rigid and correspond to a regular tiling of between any two configurations. Another way to build
space. First, we show that for robots composed of 2D 3D modular robots was proposed by Yim et al. [14],
rigid hexagonal modules on a planar lattice, the recon- using a 3D rhombic dodecahedron as the key element.
figuration problem is always solvable if both the initial Zhang et al. [15] gave several heuristic algorithms for
and final configurations do not contain the excluded distributed parallel motion planning of such an robot.
pattern (occupied, empty, occupied) parallel to any of
The extant literature has not addressed the issue of
the three axis of the lattice. We also analyze the com-
the feasibility of the motion planning problem. Sev-
plexity of our morphing algorithm in this case. Second,
eral of the proposed robots including Murata’s frac-
we propose the use of a metamodule structure for a 2D
tum modules [8] and Yim’s rhombic dodecahedron [14]
hexagonal lattice. A metamodule is effectively a scaled
(with 3, 5, or 7-sided constraints), admit of infeasi-
copy of the original module, built out of the original
ble reconfiguration problems. Though in some cases
modules but with free space in its interior. Structures
the feasibility can be restored by building flexible mod-
built entirely out of these metamodules are easier to
ules to overcome the neighbor blocking constraints, this
reconfigure, as a metamodule has the ability to fold
adds mechanical and control complexity, and therefore
up and tunnel through any of its neighbors, thus re-
cost to the system.
moving the troublesome blocking constraints for the
original modules.
3 The Reconfiguration Problem
In both cases there is still a very rich space of al-
lowed robot configurations. Our intuition is that the A fundamental problem for a robot of this type is to
shapes the robot needs to assume so as to perform var- plan how to accomplish its reconfiguration. Given ini-
ious functions can be constructed or, at least, well- tial and final configurations, we would like to compute
approximated by configurations satisfying the stated the motions of the individual modules in the config-
packing constraints. Thus, by restricting ourselves to uration that transform the initial configuration into
such less dense structures we gain simplicity in motion the final one. In theory, reconfiguration can be done
planning without any significant sacrifice in function- by searching the graph of all possible configurations,
ality. with edges between two configurations representing the
Controlled Module Density
25
reachability from one to other in a single step of some
module in the configuration. This searching approach
is not practical since the number of possible configura-
tions grows exponentially with the number of modules
in the configuration. The approach is thus intractable,
even for robots with modest numbers of modules.
Note that the optimal number of steps to transform
one configuration to another is a metric defined on the
space of all configurations. We refer to this number
as the distance in the configuration space. Any opti-
Figure 3: Here is an example of an immobile configura-
mal planner can be used to compute this distance, and
tion. The black module at one end of the chain is the fixed
conversely, any way to compute this distance function base, and the module at the other end cannot move because
yields a way to do the motion planning. The compu- of blocking constraint. All other modules cannot move be-
tation of the optimal metric function would probably cause of connectivity constraint.
be as hard as the motion planning itself. Current ap-
proaches for this problem define heuristic approxima-
tions to this function that can be computed quickly pack the disk before being able to move some module
and potentially decrease the distance to the final con- into the empty hole.
figuration. Before we discuss further difficulties, note that a ro-
bot may be immobile, with none of its modules able to
4 Difficulty with Motion Planning move without violating their motion constraints. Such
immobile configurations are typically the result of the
It is unfortunate that in the presence of motion con- blocking constraints (in practice the connectivity con-
straints, two configurations that are very close in most straint always exists, but it cannot cause immobiliza-
intuitive distance notions may not be at all close to tion by itself.) An example of immobilization for a ro-
each other in the configuration space. For example, bot composed of rigid hexagons is shown in Figure 3.
consider two solid disk-like shapes packed with mod- Robots made of fracta by Murata et al. or rhombic
ules except for a single-module hole near their centers, dodecahedron by Yim et al. with 7-sided blocking con-
but in a slightly different location in the two disks, see straints are known to have immobile configurations.
Figure 2. These shapes are approximately the same by Robots made of rhombic dodecahedra with 3- and 5-
virtually all geometric and topological measures. Yet, sided constraints can also be immobile. The construc-
the number of motion steps required to transform one tion of such immobile configurations is similar to that
disk to another is large, since any motion plan must un- of the configuration shown in Figure 3.
If the initial or final configuration is immobile, it will
be impossible to find a motion plan between the two
configurations. In general, the existence of an immo-
bile configuration is likely to make the reconfiguration
problem harder, even when it is feasible. There ex-
ist immobile configurations where a slight change in
a configuration can make it mobile again — thus the
distance in the configuration space between two config-
urations can be very sensitive to local perturbations.
It would be hard for a heuristic measure to capture
this delicate distinction. Another problem with im-
mobile configurations is that there may exist an am-
Figure 2: Configurations that are approximately the same mobile branch even in a mobile configuration. Any
in geometric and topological sense may be far apart in the motion planning involving immobile branches would
configuration space. require construction or destruction of the branches,
26 A. Nguyen, L. Guibas, and M. Yim

Figure 4: An example of an immobile branch in a mobile Figure 5: The above configuration is inadmissible because
configuration. the empty grid B is between two occupied grids A and C.
If a module is added to the configuration at B, the configu-
ration becomes admissible. The configuration also becomes
and it would be hard to define a heuristic that cap-
admissible if the module at A or C is removed.
tures that.
Given the difficulty for solving the motion planning
Proposition 1 Any admissible configuration can be
problem in general, it is desirable to restrict ourselves
transformed into a straight line configuration.
to a simpler problem. As already mentioned, we pro-
pose to restrict ourselves to a subset of all possible con- As an immediate corollary of proposition 1:
figurations, large enough to adequately approximate an
arbitrary configuration, yet restricted enough to make Corollary 2 Any admissible configuration can be
the motion planning more tractable. transformed into any other admissible configuration
with the same number of modules.
5 Reconfiguration for Planar Hexago-
Proof of proposition 1:
nal Modules
We construct a transformation from an admissible
We consider the case of a reconfigurable robot com- configuration to a straight configuration in two phases.
posed of rigid hexagonal modules in the plane, as illus- First, we transform the admissible configuration to an
trated in Figure 1. We assume that all configurations admissible chain configuration (to be defined below),
of interest must be connected, and that a particular and then convert that chain to a straight line configu-
module, called the base, is immobile. The base can be ration.
used for transmitting power and communication into Note that the complement of an admissible configu-
the system. We will refer to a possible location for a ration in the plane consists of one or more connected
module as a grid cell, or simply a grid. The only mo- components, exactly one of which is infinite. We call
tion allowed for a module is the rigid rotation around a this infinite component the outer free space (with re-
vertex it shares with some other module in the config- spect to the configuration). The module boundary line
uration, subject to the condition that such rotation is segments separating the outer free space and the config-
not blocked by any module nor leaves the configuration uration are called the outer boundary, and the modules
disconnected. in the configuration that border the outer free space are
We call a robot configuration admissible if it is con- said to be on the outer boundary.
nected and there is no empty grid cell directly in be- For ease of discussion, we assume that the hexagon
tween two occupied grid cells in the configuration; see lattice is oriented as in the previous figures. We use lat-
Figure 5. Admissible configurations have a very nice tice coordinates with the x-axis in the minus 30 degree
property, as shown in the following proposition. direction, and the y-axis in the vertical direction.
Controlled Module Density
27
In the first phase, we use a greedy algorithm. Let
us choose an extreme module in the configuration, say
choose among all modules with maximal x coordinate
the one with maximal y coordinate. Clearly, this mod-
ule is on the outer boundary of the configuration. We
grow a “tail” by repeatedly locating available modules
that can be moved towards the extreme module we just
selected, moving them there and then appending them
to the end of this growing tail. This process ends when
there is no module available for extending the tail.
The search for a module to add to the tail is done Figure 6: This figure is used in the proof of lemma 3. A
greedily. Starting from the extreme position, we search is a grid on the outer boundary of a configuration, and B
among all modules on the outer boundary of the config- and C' are on the outer free space. If a module, moving
uration, say in the counter-clockwise direction, for the in the counter-clockwise direction along the outer boundary
first module that can move clockwise without violating of the configuration, can reach B but cannot reach C, then
any motion constraints. We then let that module roll the lemma asserts that the module at A can move counter-
into the free space, along the outer boundary toward clockwise to either K, or I. Other grid labels in the figure
the extreme position, and then along the existing tail are used in the proof of the lemma.
until it reaches the tip. The feasibility of such a motion
is guaranteed with the help of the following lemma:
In both situations, the module at A can move
Lemma 3 A module moving along the outer bound- counter-clockwise, and thus lemma 3 is proved.
ary of an admissible configuration in counter-clockwise Since we choose the first module that can move
(clockwise) direction can only be blocked at a position counter-clockwise, the lemma guarantees that such a
at which its blocker module is capable of moving in the module can move counter-clockwise without ever be-
that same direction. ing blocked toward the extreme module. The module
can then move to the tip of the constructed tail. We
Proof of lemma 3:
need to make sure, however, that the configuration is
We only consider the case when the direction is admissible at the end of each move.
counter-clockwise. The clockwise case is similar. See Note that the removal of a module from an admissi-
Figure 6. ble configuration always leaves it admissible as long as
Say that in the original configuration, before the it is still connected. We only need to ensure that when
moving module moves to B, grid A is occupied, and we add the module to the tip of the tail, the configu-
grids B and C are empty. The admissibility of the ration is still admissible. Since the tail points right up
configuration implies that grids D and F are empty. from a module at an extreme position, most modules
Thus the only reason that a moving module cannot in the tail are too far to affect the admissibility of the
move forward from B to C is that E is occupied. The rest of the configuration. We need to be cautious only
admissibility again dictates that H and J are empty. when building the first few modules in the tail.
There are now two possibilities: Let the coordinates of the extreme module be
(rz, yo). It is easy to verify that the following scheme
e If K is occupied, then L is empty, and thus the for building the tail will keep the configuration admis-
module at A can roll counter-clockwise over K to sible, see Figure 7:
I.
e if (xo — 1, yo + 1) is empty, the tail can be build at
e If K is empty, then there must be a module at J
(xo +1, yo +1), (20 +2, Yo + 2), (Zo +3, Yo +3), -- 53
so that A is not disconnected from the rest of the
configuration. Grid M must then be empty, and e if (x—1, yo +1) is occupied, and (ro —1, Yo) is also
the module at A can roll counter-clockwise over occupied, the tail can be build at (2, yo +1), (to+
module at J to K. ++55
1,yo+1), (zo + 2, Yo + 2); (v0 + 3, Yo + 3),
28 A. Nguyen, L. Guibas, and M. Yim

that there is a module capable of moving without con-

straints. This module is on the outer boundary of the
new configuration and thus is on the outer boundary
of the original configuration. ©
Proof of lemma 4:
In this proof, we use the term loop for a circular
list of distinct modules, each being a neighbor to the
Figure 7: There are three different types of tails. In each
modules located right before and after it in the circular
sub-figure, the grid shaded with slanted lines is the extreme
list. See Figure 8.
module, gray grid(s) are other occupied grids, and grids
shaded with bricks are grids in the tail to be constructed.

e if (vo — 1, yo + 1) is occupied, but (zo — 1, yo) is

empty, then (zo — 1, yo — 1) is also empty, and in
order for (xo, yo) to be connected, (xo, yo—1) is oc-
eealg|
cupied. The tail can be build at (xo +1, yo), (zo +
2, 40+ 1);(0 +3, 40+) j++ ote
'
oP
The first phase eventually ends. At that time, no paca

easisssceressetees
piece in the configuration is free to move except the
CHOC Ha HO-C
module at the end of the constructed tail.
2,0,0,8 28,8485
Lemma 4 The configuration at the end of phase 1 is a
chain with one end at the end of the newly constructed
e266 OB GB oe e-G=o
tail and with the other end at the fixed base. Figure 8: The black loop represents the largest loop in the
configuration. A is the loop connector for the fixed base.
Before we prove lemma 4, let us prove:
B is the loop connector for the the tail. C is an extreme
Lemma 5 Any admissible configuration has a module module on the loop that has no motion constraints for its
on the outer boundary that can move without any mo- motion to the outer free space. D is an extreme module on
tion constraint. the loop, but is a connector. A and B happen to be extreme
modules of the loop.
Proof of lemma 5:
Consider the configuration obtained after phase 1.
We call a module, say X, in a configuration a connec-
Assume, for the sake of contradiction, that this config-
tor if the removal of X from the configuration makes
uration is not a chain, so there is a loop in the configu-
the configuration disconnected. For a connector X, we
ration. Among all the loops, choose the one enclosing
define the branch at X as the subconfiguration consist-
the largest number of grids. Call this loop £. From
ing of X and the component that becomes disconnected
any module, say X, outside of loop £, there is one or
from the base upon the removal of X.
more paths connecting the module to the loop, all of
Consider an extreme module in the configuration which must share a common module Y contacting the
that is different from the fixed base. This module is loop (otherwise, the loop can be augmented to enclose
on the outer boundary, and its motion (clockwise or more grids inside.) We call Y the loop connector for X.
counter-clockwise) into the outer free space doesn’t vi- The removal of Y would make X disconnected from CL,
olate any blocking constraint. If this module is not a and thus make the configuration disconnected. Thus,
connector, it is the desired module. a loop connector is a connector as previously defined.
If it is a connector, consider the branch at this con- Consider an extreme module M in loop L. Without
nector as a configuration, with the connector as the loss of generality, we can assume that WM is neither the
fixed base. By an inductive argument, we can assume loop connector for the tip of the tail constructed, nor
Controlled Module Density
29
the loop connector for the fixed base (if the loop con- Lemma 6 The number of tail relocation steps in phase
nectors for those modules are defined). We can make 2 is O(n'/2)
this assumption, since a loop has more than two ex-
treme modules, and thus we can always choose an ex- Proof: At any moment, consider the configuration ex-
treme module different from the above two connectors. cluding the tail. Assume that this configuration has
Note that with our choice of £, module M must be on m, (m < n) modules, then its diameter d (measured
the outer boundary of the configuration. in terms of grid modules) is at least O(m!/?). Let P
and @ be two modules with distance d. It is easy to
There are two possible cases:
see that in two tail relocations, by relocating the tail
to P, then to Q (or to Q, then to P), the tail has at
e If M has no neighboring module outside of the least d more modules; the remaining configuration has
loop then, due to its extremality, M can move O(m?/?) less modules. Straightforward analysis shows
into the outer free space without any motion con- that the number of tail relocations must be at most
straints. O(n'/?), ©
From lemma 6, the number of steps in the second
e If M has as a neighbor module N outside of the
phase is O(n°/?). This worse case can be achieved for
loop, then M is a loop connector for N. Consider
a winding chain of n modules around the fixed base.
the branch at M as a configuration with fixed base
M. By lemma 5, there is a module on the bound- This completes our proof of proposition 1. ©
ary of this new configuration that can move into
the outer free space of the new configuration. It 6 Robots Composed of Metamodules
is clear that this module can move into the outer
free space of the original configuration. In this section, we explore a different approach to re-
configuration planning. Instead of restricting ourselves
to admissible configurations, we consider arbitrary con-
Thus, in both cases, we have a contradiction: there is figurations that can be built out of a different building
a module capable of moving to the tail built at the end block, a skeletal metamodule or in short a metamod-
of phase 1. This proves that the assumption we made ule. A skeletal metamodule is a collection of modules
is false, and thus lemma 4 holds. In other words, the in a scaffolding-like structure, designed with the pur-
greedy algorithm in phase 1 transforms any admissible pose of relaxing the basic module blocking constraints.
configuration into an admissible chain. ¢ The metamodules themselves pack in a lattice struc-
In phase 2, we show that it is possible to transform ture; the ones we consider have an overall shape that
an admissible chain configuration into a straight line is a scaled-up copy of the basic module shape.
configuration. The chain configuration may be wind- Figure 9 shows a robot of rigid hexagons, built from
ing around the fixed base, and a simple way to make skeletal metamodules. The metamodule in Figure 9
it straight is to unwind it. Starting with the extreme still has a hexagonal shape, but it is an enlarged copy
module we used in phase 1, we repeatedly choose a dif- of the original module shape and has an empty interior.
ferent extreme module on the chain, then build a tail at A metamodule can decompose itself into modules,
this location. The tail will have more and more mod- which can move to another nearby location, then re-
ules after each iteration, since the previous extreme assemble themselves back into a metamodule. This can
module and all the modules in the previous tail are in happen on the surface of the structure, just like with
the new tail. Eventually, the fixed base becomes one of regular modules, but now with no blocking constraints.
the extreme modules, and the tail contains all modules More interestingly, in a configuration of metamodules
other than the fixed base. that are sufficiently large, a metamodule can move to
If the original configuration has n modules, the first a neighboring empty position through the interior of
phase would take O(n”) steps since each module moves a supporting metamodule neighboring both the meta-
at_ most O(n) steps. In the second phase, the cost for module and the nearby empty location. This can be
each tail relocation is O(n”). The number of tail relo- achieved by moving out of the way certain modules of
cation can be bounded by the following lemma: the supporting metamodule, thus creating a gate to
 A. Nguyen, L. Guibas, and M. Yim

sestatatata!
HOE erereveveverere

| ae
4 beets
7,

Se ae

o.8, aoe”.

sestraseces

Figure 9: This figure illustrates a configuration of sev-

eral hexagonal metamodules. Each metamodule consists of
86 hexagonal modules, yet collectively behaves like a single
hezagonal module. Unlike a regular hexagonal module, a
SreeSe
metamodule have no blocking constraints, and can move by
tunneling through other metamodules.

its interior, and then allowing modules of the moving

metamodule to go inside. The supporting metamodule
can then close the gate and open another gate toward
the empty location, letting the modules inside it move
acu
_! 2,0 | 8, Sotetes
2,00 tfcic: 8.7
cate
Bet,
out and to assemble back to a metamodule again; see Cetetetetetetetetetetetetetata.
Figure 10. This neighbor to neighbor motion is again
without any blocking constraint. In the same way a Figure 10: This figure illustrates the process in which a
metamodule can actually tunnel through the interior of metamodule, collapsed inside another metamodule, comes
the structure and then reassemble itself when it reaches out to form a metamodule again. On the top we see a
the boundary on the other side. Thus the metamodule metamodule with another collapsed metamodule inside it.
can be thought of as a module of a new metamorphic The grids labeled by letters form the gate that opens to let
robot, but now this module has no blocking constraints the collapsed metamodule out. On the bottom we see the
and can pass through the interior of other modules. second metamodule almost fully assembled. Its final mod-
ules are in the grids labeled by numbers — after they come
As the above example shows, metamodules can be out the transformation is complete.
built out of regular reconfigurable robot modules and
then they themselves can be treated as building blocks
for a reconfigurable robot. Any sufficiently large shape stead of a module as the basic unit. Thus metamodules
built out of basic modules can be well approximated by configurations can be used in lieu of basic module con-
metamodules, or an exact scaled version of the original figurations, with little loss in modeling power.
shape can be constructed directly out of metamodules Metamodules have several advantages over standard
(if scale is not important) by using a metamodule in- modules. If we have a heuristic algorithm to do motion
Controlled Module Density
3l
planning for a reconfigurable robot of hexagon mod-
ules, that algorithm can be used directly for a robot of
hexagonal metamodules. Furthermore, heuristic mea-
sures for the distance between two configurations will
most likely result in better quality plans in the meta-
module case, as the blocking constraints have been
eliminated. For example, the heuristics proposed by
Chirikjian and Pamecha to do motion planning [10],
and the bound on the number of steps [3] are valid for
metamodule configurations.
Metamodules also have the additional tunneling ca-
Figure 12: The two local combinations of modules on the
pability. Besides moving on the boundary of the con-
left are prohibited in a fat configuration. The dark and light
figuration, like regular modules, metamodules have the
gray grids represent occupied and unoccupied grids respec-
flexibility to go through the interior of other modules
tively. The configuration on the right is not fat, because
and thus they can make shortcuts through the config-
the fat condition is violated in two different places. If two
uration. If we let many modules move at the same
modules are added to the configuration at B and C, the con-
time, the interior is now available to provide a much
figuration will become fat.
higher bandwidth channel for the motion than is pos-
sible on the surface alone. This tunneling capability of the right, then reconstruct the metamodules on top.
metamodules can be also used to create compressible This can be thought of as a shifting of the top layer
modules, such as those proposed by Rus and Vona [11]. over the bottom layer by one module, in one time step.
The overall effect of the operation is to move the left-
6.1 Restricted Configurations of Metamodules most metamodule of the top layer all the way to the
In this section, we propose a new way to do parallel right in one time step, independent of the number of
motion planning for a restricted class of metamodule metamodules on the layers.
configurations. This combines the idea of metamod- We will use the above operation as a basic step for
ules and that of restricted configurations. We allow our motion planning. It is preferable that the config-
modules to move concurrently to speed up reconfigu- uration we deal with is not too skinny. We restrict
ration. The approach is motivated by the work of Rus ourselves to a new class of fat configurations in which
and Vona [11]. two local combinations of metamodules, as explained in
Figure 12, are prohibited. This restriction essentially
Suppose that we have two layers of metamodules as
requires that a fat configuration has locally, width two
shown in Figure 11. If we keep the metamodules on
or more; again, it should be clear that this restriction
the bottom layer fixed, we can collapse all metamod-
has little affect on the class of shapes that can be ap-
ules on the top layer, move the modules in parallel to
proximated. We will use the term nonfat for configu-
rations that are not fat.
Fat configurations have a nice property, as shown in
the following proposition:
Proposition 7 Any fat configuration of n metamod-
ules can be transformed into a straight line fat config-
uration in O(n) parallel steps.

As an immediate corollary of proposition 7:

Figure 11: The top layer of metamodules can move to the
Corollary 8 Any fat configuration of n metamodules
right over the bottom layers in one parallel tume step. This
can be transformed into any other fat configuration
is equivalent to moving the metamodule on the top layer
with the same number of modules in O(n) parallel
from the extreme left position to the extreme right position steps.
in one parallel step.
32 A. Nguyen, L. Guibas, and M. Yim

PE of one of its neighboring metamodule leaves the

Hs
ein a configuration still connected and fat again. In this

Sk OA
case, we move both modules to the tip of the tail
in two consecutive time steps.

Yule
ae
wa Cyd,6
Note that the fixed base should not be in any layer

7, eeeeeee
being shifted, and this is guaranteed since there are
y modules surrounding the fixed base at all times. Also,
although the configuration at the end of each step may
3337 Olen a not be fat, this only happens at the tip of the tail (when
the tail first touches some other part of the configura-
tion.) For the analysis, it is safe to assume that the
Figure 13: The metamodules in a fat configuration can
configuration is always fat, since we can pretend that
move to generate the rounded straight line to the right. The
the tip of the tail is not yet there when we search for
algorithm applied on this configuration first moves A, then
the next metamodule.
moves both B and C (and breaks the only loop in the con-
figuration), then moves D, F, and E to the tail. Before further analysis, let us give some intuition
behind the two cases above. In the first case, the op-
eration takes away a metamodule on the boundary,
Proof of proposition 7: making the configuration thinner. Since the configu-
Without loss of generality, it suffices to show that ration is fat, the topology of the configuration does not
we can transform a fat configuration to a fat straight change. In the second case, the configuration has some
line configuration with a rounded end (shown shaded thin cross section in a loop. The removal of a module
with slanted lines in Figure 13.) Furthermore, we can in that thin section makes the configuration nonfat,
assume that the six metamodules surrounding the base yet the additional metamodule removal eliminates the
metamodule are already in the original configuration. skinny cross section. The loop is disconnected, and the
This is so we can preprocess the original configuration topology of the configuration changes, see Figure 13.
and postprocess the final configuration to the desired
We claim that the above greedy algorithm moves all
form in O(n) time.
metamodules in the configuration to the tail. Let us as-
We again will use a greedy algorithm. Starting from sume for the sake of contradiction that there are some
the tip of the tail constructed so far, we search in the metamodules not in the tail when the greedy algorithm
counter-clockwise direction along the boundary of the fails to find any metamodules to move. Consider the
configuration to find the first metamodule not already free space, the complement of the configuration. If
in the tail satisfying either of the following two condi- there are two or more connected components of the
tions:
free space, consider the smallest distance between the
e The removal of the metamodule leaves the config- component contacting the tail and all remaining com-
uration connected and fat. In this case, we use the ponents. This distance is not one, since we assume
parallel shearing operation to shift an entire layer that the configuration at the beginning of each search
of the configuration on the boundary, with the net is always fat. If this distance is two, the second condi-
effect of moving the selected metamodule in O(1) tion would be applicable to remove the two metamod-
steps to the tip of the tail. If the layer on the ules, merging the two components of the free space. If
boundary includes metamodules in both the up- the distance is three or more everywhere, clearly the
per and lower layers of the tail, we do the shearing first condition would have been used to remove further
in two steps, the first shearing uses only one of the metamodules. Thus the free space is connected. In
two layers of the tail, and the second shearing uses order for the first condition not to be applicable, the
the remaining layer. metamodules not in the tail are on one or more thin
chains originating from the tail. This is a contradic-
e The removal of the metamodule leaves the configu- tion, since the metamodules at the tip of these chains
ration connected but nonfat, and further removal would have been removed by the first condition.
Controlled Module Density
33
The O(n) number of parallel steps follows trivially,
since the remaining configuration has one less meta-
module after every O(1) time steps. o

6.2 Reconfiguration by Tunneling

If the metamodules are large enough to allow tunneling,

then another method can be used for reconfiguring an
arbitrary configuration into a line, in O(n) steps. The
intuition here is that a metamodule at a boundary of
a configuration can follow a tunneling path through a
configuration emerging at a growing tail that is forming
a line (and consequently between two arbitrary config-
urations). Every metamodule in that path can imme-
diately follow this leader metamodule and emerge at
the end of the tail. The appropriate metaphor here is
of a turning a rubber glove inside out by pushing in at
the fingers and having them come out the other end,
one at a time, appended to each other in a straight line,
see Figure 14.
The first step is to impose a tree structure on the
configuration. This is done by labeling the metamod-
ules with increasing numbers in a breadth first traver-
sal sequence, starting with the metamodule where the
tail will grow as number 0). The breadth first number-
ing can be achieved with simple incremental message
passing.
The following algorithm is purely local, in that
every metamodule runs the same program (a small fi-
nite state machine) and no communication is required
between metamodules except for knowledge of their
neighboring metamodules and their numbers (in the
labeling).
There are four states that every metamodule can be
in:

e S; — the metamodule is part of the original struc-

ture (not tunneling nor part of the tail)

e S>. — the metamodule is tunneling towards the 0

metamodule. Figure 14: Several snapshots of the tunneling process as
it transforms the top configuration to a straight line. Meta-
e S3; — the tunneling metamodule has reached the
modules tunnel toward the base, then extend out to form a
0 metamodule and is now tunneling in the tail.
straight line. In these figures, light gray grids contain regu-
e S, — the metamodule has grown the tail and so lar metamodules, and dark gray grids contain metamodules
has stopped tunneling. with another metamodule tunneling inside them.

There are three rules for transitioning between

states:
34 A. Nguyen, L. Guibas, and M. Yim

e T; (S14 S2): If the metamodule is or becomes a tree for another convoy to pass through. In both cases
leaf metamodule, it starts to tunnel toward 0. another module, the blocker of M from a different sub-
tree, is moving though the branch node for every time
e T> (Sz +> $3): If the metamodule has tunneled step that our module M has to wait. But once a meta-
into the 0 metamodule, it starts to tunnel to the module has been the blocker of M, it can never block
tip of the tail. M again at a branch node, as it has now become an
ancestor of M in the tree. Thus no node needs more
e T3 (S3 +4 54): If the metamodule has reached the
than O(n) moving or waiting steps to reach its final
end of the tail, it exits and forms a normal non-
tunneling metamodule growing the tail. destination.
Since we move from the leaves of the branch inward,
Any metamodule that is a leaf will move up the tree connectivity is never broken. Furthermore this algo-
by tunneling inside the branches towards 0. Moving rithm operates in a very simple and purely local fash-
up the tree is accomplished by examining the number ion. All of this assumes that only one metamodule
of the metamodules neighboring the current metamod- can be “tunneled” inside another metamodule at one
ule that the tunneling metamodule is contained in and time. Metamodules with larger interiors would have
targeting the neighbor with the lowest number as the more “bandwidth” which would mean less traffic prob-
direction to tunnel. When a leaf tunnels up a branch lems and fewer steps.
of the tree, the former second-to-last metamodule be-
comes the last metamodule and thus a leaf, so it starts
7 Conclusion
to move up.
There are three cases where a metamodule in state In this paper we have explored certain restrictions on
S> will not be able to tunnel into its neighbor: 1) when the allowed structure of reconfigurable robots with the
it is being tunneled itself, 2) when another metamod- aim of making the motion planning problem easier,
ule is inside the target metamodule and has not exited without unduly confining the set of shapes the robot
because it is waiting for another metamodule to move, can assume. Though all our results are in 2D, we be-
and 3) when a metamodule with a higher number wants lieve that appropriate extensions to 3D are possible,
to enter the same target metamodule. The last case is and in some cases straightforward. For example, we
an arbitrary prioritization (or right of way) convention. can build 3D rhombic dodecahedron metamodules that
Once a metamodule reaches the 0 metamodule, it at- consist of an edge skeleton of a scaled copy of the ba-
tempts to form the tail by tunneling down the tail until sic RD module. Because the parallelogram facets of
it emerges at the end, thus growing the tail. these metamodules are actually solid only along their
At every step, at least one metamodule is either mov- edges, other collapsed metamodules can enter, tunnel
ing up the tree, or growing the tail. In fact, every meta- through, and exit a given metamodule easily, without
module will either be a conduit for other metamodules any of the gate opening or closing operations we need
to pass through or will itself be tunneling through an- in 2D. In 3D as well, the traffic management of convoys
other metamodule after O(n) steps. While there is of modules tunneling through the robot interior seems
always a convoy tunneling towards the root or growing easier than in 2D. Since the tunneling reconfiguration
the tail, some convoys may be waiting at branch nodes algorithm imposes a tree structure which is indepen-
for other convoys to finish their tunneling. Clearly a dent of the dimensionality of the physical modules, it
metamodule only moves O(n) steps before it reaches will work for this 3D RD case as well.
its eventual goal position in the tail, as its motion fol-
lows a path in the tree and through the tail. What is Acknowledgments
more interesting is that a metamodule M also never
waits a total of more than O(n) steps before reaching Leonidas Guibas and An Nguyen were supported in
its destination. We can see this as follows. After the part by National Science Foundation grants CCR-
initial O(n) steps, metamodule M waits only when it 9623851 and IRI-9619625, as well as by US Army
is being tunneled through, or when it is part of a tun- MURI grant DAAH04—96-1-0007. An Nguyen was also
neling convoy that is waiting at a branch node up the supported by a Stanford Graduate Fellowship.
Controlled Module Density 35

References
[9] Pamecha, A. et al., “Design and Implementation
of Metamorphic Robots” in Proceedings of the 1996
[1] Casal, A., Yim, M., “Self-Reconfiguration Planning ASME Design Engineering Technical Conference and
for a Class of Modular Robots,” SPIE Symposium Computers in Engineering Conference, Aug 1996.
on Intelligent Systems and Advanced Manufacturing,
Sept. 1999.
[10] Pamecha, A., Chirikjian, G., “Useful Metrics for
[2] Chirikjian, G., “Kinematics of a Metamorphic Ro- Modular Robot Motion Planning,” in JEEE Trans-
actions on Robotics and Automation, Vol. 13, No 4,
botic System,” in Pre. 1994 IEEE Int. Conf. Robotics
and Automation, San Diego, CA. August 1997.

[3] Chirikjian, G., Pamecha, A., “Bounds for Self-

[11] Rus, D., Vona, M., “Self-reconfiguration Planning
Reconfiguration of Metamorphic Robots,” JHU with Compressible Unit Modules,” in Proceedings of
Technical Report, RMS-9-95-1. the 1999 IEEE International Conference on Robotics
& Automation, May 1999.
[4] Chirikjian, G. et al., “Evaluating Efficiency of Self-
Reconfiguration in a Class of Modular Robots,” in
Journal of Robotic Systems, June 1996. [12] Yim, M., “A Reconfigurable Modular Robot with
Many Degrees of Locomotion,” in Proc. 1993 JSME
Kotay, K., et al., “The Self-reconfiguring Robotic Int. Conf. Advanced Mechatronics.
Molecule: Design and Control Algorithms,” Algo-
rithmic Foundations of Robotics, 1998.
[13] Yim, M., “Locomotion With a Unit-Modular Re-
configurable Robot”, Stanford University PhD The-
Kotay, K. et al., “The Self-reconfiguring Robotic
Molecule,” in Proceedings of the 1998 IEEE In-
sis, 1994.
ternational Conference on Robotics & Automation,
May 1998. [14] Yim, M. et al., “Rhombic Dodecahedron Shape
for Self-Assembling Robots,” Xerox PARC, SPL
Murata, S. et al., “Self-Assembling Machine”, in Pro- TechReport P9710777, 1997.
ceedings of the 1994 IEEE International Conference
on Robotics & Automation, May 1994.
[15] Zhang, Y., et al., “Distributed Control for 3D
Shape Metamorphosis,” submitted Autonomous Ro-
[8] Murata, S. et al., “A 3-D Self-Reconfigurable Struc-
bots Journal, special issue on self-reconfigurable ro-
ture”, in Proceedings of the 1998 IEEE International
Conference on Robotics & Automation, May 1998. bots, 1999.
 e = > :

| vita ucrak abe er ere

cre <>
ROEM: ee ;
x ea Fie

ialrind Pomnton
si 09
—— iw ‘A

magebari i
PI ie
Se icike
ee ie ~

pes, er ee ay Sa ry
nw oe acti We Syren h a “s ri i A
prbtnatigl wale cin bean
oe a eg (itl. See aes rpeteeas ;
uma oomad Pied: wh)» Bie
a) cae 2 OF Ca ae a Ee
eeeay tenes Ree ins Fon af) ae, par .
aed eoiE kite as)
a 7

, a (ies ae james ii

ae feae

yao sition “or ae

@ i
we
r
er?
=
>
4

F Rows ud 5 «aive-+eig

bee | ae ete AD ar obieth

nA i= -_ om m4 bnd. sole any
am
ra al inalpat..qgd & 1m Oe
Eye ae 4
Jinn. tl VS @ Bp Get gare
mie # te 7 a \ edibans x
i i!’ aah @ Uy Say toe «
rife ple Pome ‘

IT Lis ely. Thy 2 ey

cote be ies “ a ®
Jen afclue rieeasigitiie lle
¥ hi Halts’ Oe a Frag

AckniG ~Join peep

— MW ae

td aarti Thee pling 7 : -

ow Siggy yehe alltel
“0 ert ; , a —-~ &
ws < 1 , é 'a<e- .
ML NP igrigs , Tatplete,
ih Sala Ss: 42S ene
Positioning Symmetric and Non-Symmetric Parts
using Radial and Constant Force Fields
Florent Lamiraux, LAAS-CNRS, Toulouse, France
Lydia E. Kavraki, Rice University, Houston, TX

Part positioning is an important task in manufactur- squeezes performed by two parallel jaw grippers. Given
ing. New approaches have been proposed to perform a polygonal part, the paper described an algorithm to
this task using force fields implemented on an active compute the best sequence of squeezes that uniquely
surface. A part placed on such a surface is subjected to orients the convex hull of the part no matter what the
a resultant force and torque and moves toward a stable initial configuration is. In [16], the parts are on a con-
equilibrium configuration. Such force fields can be im- veyor belt and rotate by contact with passive fences.
plemented using different technologies. In this paper,
Another approach is based on force fields imple-
we study the combination of a unit radial field with
mented in an horizontal plane [6, 13]. A part lying
a small constant force field. In prior work we proved
on the field is subjected to a resultant force and torque
that such a combination can uniquely position a class of
that move the part toward a stable equilibrium con-
non-symmetric parts. In this paper, we propose a more
figuration if one exists. A series of papers (see [4] for
complete modeling of this combination which allows us
detailed references) established the fundamentals for
to devise a method to determine all the equilibrium con-
part manipulation using force fields. Current technol-
figurations of a part in the above force fields. This
ogy permits the implementation of certain force fields
method works for both symmetric and non-symmetric
in the microscale with MEMS actuators [6] or air jets
parts. Beyond the method, this paper reports a com-
[2] and in the macroscale using mechanical devices [14].
prehensive study of the action of radial and constant Vibrating plates can also be used to produce certain
potential fields over parts with an original characteri-
force fields in the plane [3].
zation of local minima of the lifted potential field.
In work which is summarized in [4, 6] the properties
1 Introduction of force fields that are suitable for sensorless manipula-
tion were analyzed and manipulation strategies were
During manufacturing, parts typically stored in boxes
proposed. Several fields were investigated including
have to be manipulated and oriented before assembly. the squeeze, radial, and inertial fields and combina-
This task is critical in manufacturing since it strongly tions thereof. Notably, the algorithm of [9] still applies
affects the productivity of the assembly line. Orien- when the jaw grippers are replaced by squeeze force
tation has been traditionally performed by vibratory fields. Later work [10] introduced the elliptic potential
bowl feeders. However, these devices are designed for field which gave rise to two stable equilibria for non
a given part and need to be modified if the shape of symmetric parts.
the part changes.
Recent work has investigated alternative ways of ori- Complexity and Uniqueness of Positioning
enting parts, and emphasis has been placed on simple
programmable devices [1, 6, 9, 15, 14]. Part positioning There are two main issues when positioning’ parts us-
without sensing has become very popular over the past ing force fields. The first one is the number of basic
few years since it is easy to implement and the meth- operations, (i.e., number of force fields used) and the
ods proposed can be very robust [1, 2, 3, 6, 7, 8, 9, 16]. second one is the number of distinct positions that the
Our work brings new contributions in this domain. 1Orienting a part consists in specifying the orientation
One of the pioneering papers in this area, [9], pro- of the part. Positioning a part consists in specifying the
posed to orient a polygonal part by a sequence of position and orientation.
38 F. Lamiraux and L. Kavraki

strategy yields. Both issues have been addressed previ- these partial derivatives. An algorithm to determine all
ously and work has been done both on finding shorter the stable equilibrium configurations of a part is then
sequences of operations [6] and on finding a unique op- given. Finally, an example of positioning a symmetric
eration that positions a part. In [10], a quadratic po- part is presented. We conclude in Section 5.
tential field is used to position parts. Any part with
distinct principal moments of inertia has exactly two 2 Notation and Definitions
stable equilibrium configurations in these fields. The
other parts have undefined orientation. Addressing the This section puts forward the notation and definitions
above issues at the same time would consist in finding used in the paper. We first define the action of a force
a single field that uniquely positions a part. It was field over a part and the notion of a stable equilibrium
conjectured in [6] that the combination of a unit ra- configuration in this context. Then we outline some
dial field with a constant force field would fulfill this properties of force fields deriving from a potential func-
task. In [5], we proved that this conjecture is true un- tion. Most of these definitions have been introduced
der the condition that when the part is subjected to in [4].
the unit radial field only, the center of mass of the part
If F is a set, we denote by int(#) and OF the interior
in equilibrium is not at the origin of the field. Thus,
and boundary of E. Throughout the paper, r denotes
for almost any non-symmetric part, there exists a force
a point of the plane of coordinates (€,7) in a frame
field that uniquely positions this part.
attached to the plane. We consider a part in the plane
occupying surface S with center of mass G at (0,0)
Contributions and Organization of the Paper
in a reference configuration qg. We suppose that S$
Symmetric parts like a rectangle or a regular polygon is a compact set and that its boundary QS is a zero-
for instance do not fulfill the above condition: When measure subset of the plane. The configuration space
placed in a unit radial field, at equilibrium, the center of the part isC = R?xS!, where S! is the unit circle. A
of mass of the part will coincide with the origin of the configuration q = (z,y,@) € C corresponds to a rigid-
unit radial field, due to symmetry. The goal of this pa- body transformation yq in the plane transforming r =
per is to determine all the stable equilibrium configura- (€,7) into
tions of a general part, symmetric or not, subjected to
the combination of a unit radial field with a small con- _ { «+€&cosé—nsin#é
stant field. There are two main contributions in this Pa(t) = (y + €sin6 + n cos ).
paper. The first one is a characterization of the sta-
We denote by Sq the subset occupied by the part in
ble equilibrium configurations of a part using a general
configuration q.
modeling based on the curve of translational equilibria
for fixed orientations and its partial derivatives. This When the part is placed on a force field f(r), the
modeling is a generalization of results previously pub- resultant force and torque it is subjected to are the
lished [5, 12]. The second contribution is a method to following:
compute the above mentioned partial derivatives and
an algorithm that determines all the equilibrium con-
an = T [ f(r)dr (1)
figurations of a given part. q

This paper is organized as follows. In Section 2, we

give some notation and definitions. In Section 3, we
= cS | i (r—rg) x f(r)dr
S qa
(2)

first define the unit radial field that we then combine

with a constant field. We define the equilibrium curve where rg = (z, y) is the position of the center of mass
as the locus of the translational equilibria for fixed ori- G, when the part is at configuration q.
entation. After pointing out the smoothness of this We say that q is an equilibrium configuration if the
curve, we state the central result of the paper that ex- resultant force and torque vanish at q. Moreover, an
presses the stable equilibrium configurations of a part equilibrium configuration q is said to be stable if, sub-
w.r.t. the equilibrium curve and its partial derivatives. jected to a small perturbation, the part stays in the
Section 4 first describes the practical computations of neighborhood of q.
Positioning Symmetric and Non-Symmetric Parts
39
Potential Fields The class of force fields deriving
from a potential function are very helpful in mechanics
and electrostatics since the equilibrium configurations
of a point-particle subjected to such a field are exactly
the local minima of the potential field. This property
extends naturally for “reasonable” force fields from the
plane to the configuration space via the notion of lifted
potential field [4].

Definition 1 (Lifted Potential Field)

Let f(r) be a force field in the plane deriving from a
potential function w, i.e., f(r) = —Vu(r). The function

U(a)= f u(x)ar=[weata)yar
qa
Figure 1: Parameterization of C with the system of coor-
dinates q = (X,Y,9). Yq corresponds to a translation of
over C is called the lifted potential field induced by uw. (X,Y) followed by a rotation of angle @ about the origin.

The lifted potential field U(q) is thus defined by in- 3.1 Unit Radial Field
tegrating the value of the plane potential field over the
surface Sq occupied by the part in configuration q. We call unit radial field the force field f(r) = —7.This
In [6] some properties of the lifted potential field are field has constant magnitude, is oriented toward the
established. The most interesting among these proper- origin of the frame attached to the plane and derives
ties is that the partial derivatives of the lifted potential from the potential function:
field w.r.t. x, y and @ are exactly the opposite of re-
spectively the coordinates of the resultant force and v(r) = |[rll = ve? +77. (3)
the resultant torque. v is clearly symmetric by rotation about the origin.

(a) = —Fe(a) Thus if q’ is obtained from q by a rotation about the

origin, the values of the lifted potential field at q and

a = -F@)
q’ are the same. To take fully advantage of this prop-
erty, we are going to use a new system of coordinates
(X,Y,@) for the configuration of the part, illustrated
(a) = -M(a). in Figure 1 and defined by:

The main consequence of these equalities is that sta- xXx x cos

+ ysin@
ble equilibrium configurations are equivalent to local va —xsin@ + ycosé.
minima of the lifted potential. For this reason, we fo-
cus our attention in the following sections on the local
Expressed in this system of coordinates, the lifted
minima of the lifted potential field induced by radial
potential field V corresponding to v depends only on
and constant fields. X and Y and can be written as follows:

3 Radial and Constant Fields Vax e.0 / u(X + &,¥ +n)d€édn. (4)
Ss
In this section we study the properties of combinations
of a unit radial force field with a small constant force Because of the independence on @, we will consider V
field. Both fields derive from a potential. We give a as a function of (X,Y) only. Notice that when @ = 0,
method to determine the stable equilibrium configura- X =z and Y = y. V(X,Y) = V(a,y,0) can thus
tions (i.e., local minima) of a part subjected to such be considered as the lifted potential field of the part
a combination in the general case. See also [5], where in translation. This interpretation can be helpful to
some of the material below was originally developed. understand some of the forthcoming developments.
40 F. Lamiraux and L. Kavraki

Smoothness of V In the following sections, we will singularity of v at the origin affects the smoothness of
use a lot the partial derivatives of V. For this reason, V only where the boundary of the part passes above
we state in this section the differentiability properties this singularity.
of this function.
Minimum of V and Pivot Point Due to symme-
C is partitioned into three subsets. The configura- try, the unit radial field obviously cannot uniquely po-
tions for which the origin of the radial field is in the sition a part. However, for a fixed value of 8, any part
interior of the part, the configuration for which the ori- in translation has a unique stable equilibrium. This
gin is outside the part and the configurations for which property is a consequence of Proposition 3 below. This
the origin is on the boundary of the part. We denote proposition was also given in [5]. It is repeated here for
respectively C*”, C°“*, and C°°""4 these subsets. To de- completeness. Notice that it provides a different proof
cide in which of these subsets a configuration (X, Y, 6) for the existence and uniqueness of the pivot point de-
lies, we need only to consider (X,Y) in the new system fined below than the one given in [4].
of coordinates. (see Figure 3.1 for intuition):
Proposition 3 V verifies the following properties:
qec™ S (—X,—-Y) € int(S)
q e (out Se
Cx. ) =Y) ¢ S
(i) The Hessian of V Hess V(X,Y) is positive defi-
q E Cbound van
(-X,-Y) € OS. nite everywhere in R?,

As far as differentiability is concerned, V has the fol- (ii) V has a unique local minimum over R?.
lowing properties.
Proof: As Hess V is a 2 by 2 matrix, (i) is equivalent
Proposition 2 (i) V is of class C® over C” and to:
over CO, tr Hess V(X,Y) > 0
2
(it) V is of class C? over C and VAG) CR eer ery CP y 0
where tr and det are respectively the trace and determi-
nant operators. According to (7-9) and (3), the second
BEY) =f FER+EY tnlaedn 6) order partial derivatives of V are:
a°V lis ee (Yee
+ ee
SyOGY) =f Fe +EY tnldedn a0) = | para are
o"V <$ (X +)?
SCY) =f Sx +EY tn dln (7) a) = free vem
av ee) £= g (X -(X+9W +)
Spry) = f Sex +EY tnldien © OXOY
axoy + €)? + (Y +n)?)3/2 d§dn.

It is isp from these expressions that

oan thSEX +6Y + nba, (9) tr Hess V = od + ee is positive everywhere. The
determinant of the Has of V:
The classical results of integration theory, regarding O°V OV OV
differentiating inside the integral cannot be applied as det Hess V(X,Y) = (
OX? OY? (Sxay? Si
such to the unit radial field because of the non differen-
tiability at the origin. The proof of this proposition is is the sum of two terms, each of which is a product
based on a family vp, of approximations of v, C™ every- of two integrals over S. Replacing these products by
where and equal to the radial field outside the disc of integrals over the Cartesian product S? = S x S, we
radius h centered at the origin. We do not give here get
the proof of this Proposition. This proof can be found
in [11]. (/FG naga) (/a(n) =
Ss s)
It is interesting to notice that the lifted potential
field V is C® although v is not even differentiable. The ifF(€.,m)9(2, n2)déi dni d&2dno.
Positioning Symmetric and Non-Symmetric Parts
4]
If we condense the notation as follows, X;, = (X+&),
Y; = (Y + m) for i = 1,2, and omit d&;dnidé2dn2, we
have:

det Hess V(X,Y)

2172
= i ee eee
go (AT HY? )9/2(X2 + Y2)3/2 ae

/ X1Y1X2Yo
ga (Xt + ¥7)8/2(X2 + ¥2)3/2
3 ij Mo Xe XG eye
Figure 2: In a unit radial field, at the equilibrium con-
S2 (Xx? at: eT OE: oteMowlehe
figuration corresponding to 0 = 0, the center of mass is at
In the first integral, (X,Y) and (Xo, Y2) have a sym- (Xo, Yo).
metric role and can be switched so that X?Y? can be
replaced by $(X?Y? + X3Y?) and: the unit radial field. In this section, we perturb the
1 f X?Ys + X3Y? — 2X1Yi X2Ye radial field by adding a small constant field in order to
det Hess V(X, Y) break the symmetry. We are going to show that for
2 ea (ATA VS) AG + ¥2)8? each fixed orientation 6 of the part, the corresponding
att (X1Y2 — X2¥i)? lifted potential field has a unique minimum in (X,Y).
2 Jig (XP + Y?)3/2(X3 + ¥2)8/2 When now @ varies, the curve of these minima is C!
SG).
and C if the pivot point is not on the boundary of the
Thus Hess V is positive definite everywhere. This en- part. We call this curve the equilibrium curve. Then
sures us that if V has a local minimum, it is unique. we will give a characterization of the local minima of
the lifted potential using the equilibrium curve.
Moreover, as u(r) tends toward infinity when ||r|| tends
toward infinity, V(X, Y) also tends toward infinity as We consider now the following potential function in
(X,Y) diverges. This property implies the existence of the plane (Figure 3):
a local minimum of V. oO
u(r) = v(r) + 6n
We denote by (Xo, Yo) the unique minimum of V.
The set of equilibrium configurations of the part under where v is the unit radial field and 6 is a positive con-
the radial field is the following {(Xo, Yo,0),@ € S*}. stant. The second term 67 corresponds to the constant
Let us express this curve in the standard system of
coordinates:

x = Xocosé —Yosiné (10)

y = Xosind+ Yocosé. (11)
The stable equilibrium configurations are obtained by < se"

rotation of the part about the origin of the radial field. se

SEEESSES RANTS x
PVRBEREEE OS

The point of the part situated at the origin in these con-

Sw
SASS

figurations is called the pivot point and denoted P [6].

In the stable equilibrium configuration corresponding
to 9 = 0 (remember that in this case, X =z, Y = y-)s
the center of mass is translated to (Xo, Yo). Thus in
configuration qo, the pivot point is at (—Xo, —Yo) (see
Figure 2).

3.2 Radial-Constant Field

the existence and Figure 3: Potential function of the combination of a unit

The previous section established
radial force field with a constant force field.
uniqueness of the pivot point for a part subjected to
42 F. Lamiraux and L. Kavraki

force field —6(0, 1). The lifted potential field for a given From (12), as V is of class C? (Proposition 2), F’ is
value of 6 is expressed as follows in the (X, Y,@) system of class C!. By definition, the equilibrium curve min-
of coordinates: imizes the lifted potential field for fixed @ and 6 and
therefore fits the following implicit representation:
Us(X, Y,0) = V(X, Y) + 6|S| (sin? X + cos Y) (12)
where |S| is the area of S. For clarity purposes, we F(X*,Y*, 0,6) =0.
define the following functions
The differential of the partial function F¢,5 of the
Cesc ry UX, V0,0) =a X,Y,0) variables (X,Y) is exactly the Hessian of V. From
Proposition 3, this differential is invertible everywhere.
where the variables we put in the subscript are consid-
According to the implicit function theorem, these
ered constant. conditions imply that X* and Y* can be expressed
Minimum of Us; and Equilibrium Curve The as C! functions of (6,6). As the equilibrium curve is
second term of the right hand side of Expression (12) unique, these C! functions are necessarily the formerly
is linear in (X,Y). For this reason, Ups has the same defined X*(6,6) and Y*(@, 6).
second order partial derivatives as V and Hess Ug.5 =
Hess V is positive definite (Proposition 3). For any (ii) If the pivot point is not on the boundary of the
fixed value of 9 and 6 < 1, u(r) tends toward infinity part, —(X*(6,0), ¥*(0,0)) ¢0S. By the continuity of
with |r|. Thus Ug5(X,Y) tends toward infinity with X* and Y%, there exists a 69 such that for any 8 € S!
(X,Y) and Ug has a unique local minimum. We de- and 0 < 6 < 609, —(X*(0,6), Y*(@,6)) ¢ OS. In other
note by (X*(0,6),Y*(8,6)) this local minimum. We words, if we follow the equilibrium curve for a small
can express it in the standard system of coordinates 6, the origin of the field remains completely inside or
by: completely outside the part and (X*, Y*, 0,6) remains
a*(0,6) = cos6 X*(0,6) —sin@ Y*(8,6) (13) in a domain where F is smooth (from Proposition 2).
Therefore, according to the implicit function theorem,
y"(0,6) = sin? X*(0,6) + cos@ Y*(6,6). (14) X* and Y* are also smooth. Relations (13) and (14)
For each value of 6 < 1, these local minima define a imply that «* and y* have the same differentiability
curve of parameter @ that we call equilibrium curve. properties as X* and Y~*. Oo
We are going to show now that this curve is of class
C?. It is of class C® for small values of 6 if the pivot From now on, we will assume that the pivot point
point is not on the boundary of the part. is not on the boundary of the part, so that the partial
derivatives of the equilibrium curves are all defined for
Notice that (X*,Y*)(0,0) = (Xo, Yo) is the min-
small 6.
imum of V and therefore is independent of @. The
smoothness of the equilibrium curve will prove useful
in the sequel where its partial derivatives are used. Properties of the equilibrium curve We now
point out a property of the equilibrium curves that
Proposition 4 The equilibrium curve is smooth. will constitute the basis of our method to determine
the local minima of U. For a fixed value of 6 the local
(i) X*, Y*, x* and y* are continuously differentiable.
minima of U are obviously on the equilibrium curve
(ii) if (—X0, -Yo) ¢ OS (i.e., the pivot point is not on associated to 6. We are going to show that these local
the boundary of the part), there exists dg such that minima are the points where (x*, y*) crosses the y axis
A", Y*,.a*, and y* are C™ over S' x [0,do). from z <0 tox > 0. For that, we define:

Proof: (i) This proposition is a direct result of the Us (0) = Us(X*(8,

6), Y*(0, 6), 8)
implicit function theorem. Indeed, if we define the fol-
lowing function from R* into R?: to be the minimum value of the lifted potential field
. for given 0 and 6. The variation of Ux(@) along the
8U (X,Y, 0,6)
equilibrium curve is given by the following proposition.
Positioning Symmetric and Non-Symmetric Parts
43
Proposition 5 For any @ € S!, 3.3. Computation of the Stable Equilibrium
dU; Configurations of a Part for Small 6
dé
(0) = 6|S|2*(0,6).
Tn [5], we proved that for a part with distinct pivot
Proof: For clarity, we omit 6 in the notation of this point and center of mass (Xo, Yo) 4 (0,0), the stable
proof. By definition Uj (0) = Us(X* (0), ¥*(0), 0). Dif- equilibrium configuration is unique. The proof is based
ferentiating this expression w.r.t. to 6 leads to: on the expression (10) of the equilibrium curve of pa-
dU; ce ake .
9) = axe (X*0),¥"0),0)S (0) + Aes rameter 6 = 0 and on the continuity of x*(0,6) and its
partial derivatives. More precisely, x*(0,0) vanishes for
OUs i, A dy* two values of : 0; and 62 = 0, +7. x*(0,0) is increas-
oy (X°(),¥ (8), 8) (8) + ing at one of these two values (say 62) and decreasing
St
0 %
(X"(0),¥"(0),8)
Bis at the other one:

= Tt (x°(6),¥*(6),6)
= 6|S|(cos@
X*(0) —sin@ Y*(@))
= 6|S|«*(6) By continuity of x* and ae we proved that for small
values of 6, 2*(0,6) still vanishes only twice, around 6;
using expression (12) and the fact that the partial and 4 and therefore the stable equilibrium configura-
derivatives of Us w.r.t. X and Y vanish at (X*,Y%*). tion is unique.
O
When the pivot point and the center of mass are the
This proposition leads directly to the following prop- same (that is when (Xo, Yo) = (0,0)), this reasoning
erty. does not apply since from (10) and (13), 2*(0,0) = 0
for all 6. The following theorem however, states that
Proposition 6 For any fixed value of 6 < 1, the two the criterion applied previously to x*(6,0) can be ap-
following properties are equivalent: plied to the first non-uniformly zero partial derivative
(i) (X,Y,8@) ts a local minimum of U5, of z* w.r.t. 6, evaluated for 6 = 0: "2" (6,0). More
(7) eA A A030) 2Y¥=.Y, (0,6) and. the precisely, if Fe (6,0) has only simple roots, 7.e., van-
equilibrium curve crosses the y-axis from ishes with non zero slope then for small 6, x*(6,6) also
left to right when @ increases: has only simple roots close to the roots of —— (0,0)
a*(6,5) =0 and 22-(0,5) > 0. (Figure 5). Among those roots, some represent stable
Figure 4 shows the value of the lifted potential along equilibrium configurations, the other ones are unstable.
the equilibrium curve for a given part. Theorem 7 For a given part, if there exists an
We are going now to devise a method for computing integer n > 0 such that:
these equilibrium configurations for small values of 6
and for any kind of part, symmetric or not. (i) for anyk such thatO0<k<n-—1,
22" (9,0) = 0 uniformly over S!,
(it) oan (0,0) vanishes only at a finite number
of points (61,...,42m), and
n+1_*
(iti) for anyl<Il<m, Fe (421, 0) = 0,
m+1*
U§
(8) oe (821-1, 0) < 0,

then for small values of 6, the part has exactly m

stable equilibrium configurations. These configurations
x
converge toward (Xo, Yo, 921) in the (X,Y,@) system of
coordinates, 1 <1 <m, when 6 tends toward 0.
Figure 4: Along the equilibrium curve, the variation of the
value of the lifted potential is proportional to x*. Thus the
m+1_.*

Proof: Let us first notice that a5 (4, 0) represents

stable equilibrium configurations of the part are those where
the slope at 6, of a=" (0,0) seen as a function of 6.
the equilibrium curve crosses the y-axis from left to right.
44 F. Lamiraux and L. Kavraki

uniformly over S!. If we take a ees 6 in the interval

2—~ (9,5) as a function
(021 —@, 02: +a), and we consider ek
of 6 that we temporarily denote by f(6), the above
equality can be rewritten:

‘Spt

Moreover, (15) implies that V6 € [0, 41],

OF f
ai (5) >0 ;
ss (17)
Te
Figure 5: The first non-uniformly zero partial derivative
Therefore, using Taylor-Lagrange formula, for any 6 €
oo (0,0) of x* w.r.t. 6 provides all the information about
[0,51], there exists @, 0 < @ < 1 such that:
the local minima of Us for small values of 6. The values of
6 where this function vanishes with positive slope (02 and
(8) = = eee (18)
n—-1 of 6k oe” 6”
64 on this example) are close (by continuity) to the val-
ues where x*(0,6) vanishes with positive slope for small 6.
According to Proposition 6, these values of 0 are stable equt-
librium configurations.
anf
; Dee (19)
This establishes that for any 6 < 06, and any
Therefore condition (iii) simply means that Te (6,0) 6 € (0g, — a, 09: + a), 1<l<m, oz" (0,6) > 0. Thus
changes sign at each 6, for 1 < p < 2m. for a fixed 6 € [0,6,], the function 7*(6,6) of @ is
All the partial derivatives of x«* are continuous. increasing over (62; — a,42; + a) and cannot vanish
Thus, from condition (iii), there exist two positive more than once over this interval. Using the same
numbers a and 6, such that for any positive real num-
reasoning, we can establish that 2*(@,6) is decreasing
ber 6 < 6; and any integer / between 1 and m,
over the intervals (62:1 — a, 621-1 + a) and cannot
Cio *
VOE (O21 = a, 821 SF a), =-(6,6) >0 vanish more than once in each of them either.
? (15)
VO € (@2-1 — a, O2-1 + a), see (6,6) < 0.

Let us denote by J the union of the intervals of 0 2. From condition (i),when 0 ite in J, Tz (6,0)
defined above: ---~(0, 0)|admits
0,05 +) a positive eons bound over J, md we denote by M:
I= U (Op
1<ps2m Nak

and by J = S! \ I the complement of J as shown on

M = min {os (8, 0) sert>o (20)
06"
Figure 5.
From the uniform continuity of oe
&=
= ~ (0,6) over the com-
The proof consists of two parts:
pact set J x [0,6,], there exists d2, 0 < dg < 6; such
1. We first prove that over each interval pecstntine that for any 6 € [0,62] and any 6 € J,
I, for small fixed 6, the OE Ge.ae of x* keeps a
|
O" x* O"z*
constant sign and therefore x* frit: only once
over each of these intervals.

2. Then, we show that, for small 6, x* does not vanish

over J.
Sar) >0 (21)
1. Differentiating the equation defined in (i) w.r.t. 9,
we get that, for any k < n—1, We can apply again Taylor-Lagrange relation. To keep
Okt1e * the same notation, f(6) denotes now 2*(6,6) consid-
aoaae
0006 (9) = 0 ered as a function of 6 for a fixed value of @ € J.
Positioning Symmetric and Non-Symmetric Parts
45
From condition (i) we get that condition (16) is sat- To compute an expression of a& (6,0) we need thus
isfied and from (21) that aL (65) does not vanish over ak xcs ky-*
to compute expressions of S<. “0,0) and (0,0).
(0, 62] and thus keeps a constant sign over this interval.
Condition (17) (or its counterpart a +(5) < 0) is thus By definition, (X*,Y*) minimizes Us. Thus the
satisfied over [0,63]. We can reuse (18) and (19) (re- partial derivatives Ua.
OX and se
ees> vanish at (X*, Y*).
placing > by < if ot (6) < 0) to conclude that «* (0, 5) Using expression (12), this Sehsneer is equivalent to:
Bee not vanish over [0,62] and has the same sign as
OV
a= (0,0). ax (9,9), ¥"(4,6)) +6|S|sind = 0
If we oye consider a fixed 6 < 69 and if we re OVE. a
call that ae (0,0) changes sign between two subin- ay (0,6), ¥*(8,6))+6|S|cos@ = 0.
tervals constituting J, we can conclude that «*(6,6)
vanishes exactly once over each (6; — a,6; +a), 1 < By successively differentiating this system w.r.t. 6 and
I < 2m at a value of @ that we denote by 6;(6). by evaluating the eee system at 6 = 0 we will
The stable equilibrium configurations of the part are get expressions of24" (6,0) and ae (0,0). We show
expressed in the (X,Y,@) system of coordinates by below the first step of this process by computing an ex-
(X*(@21( 6), Y*(21(5),
6), 5), O27). Since a can be cho- pression of a (9,0). Then we will address the general
sen as small as desired by making 6 tend toward case with higher order eure) 0).
0, lims.0 A21(6) = 6:. X*, Y* being continuous,
the stable equilibrium configurations converge toward Distinct Pivot Point and Center of Mass If
(Xo, Yo, 21) as 6 tends toward 0. O (Xo, Yo) 4 (0,0), we express this vector in polar co-
ordinates: (Xo, Yo) = (pcosy, psiny), p > 0. Then,
Let us point out that in most ee the first non-
from (10),
uniformly zero partial derivative2
aaa~(6, 0) has simple
x* (0,0) = pcos(@ + wp).
roots (we will see later that these partial derivatives are
trigonometric polynomials in 9) and Theorem 7 can x*(8,0) vanishes with positive slope for 9 = = —y and
be used. This theorem extends results from [12]. In Theorem 7 (with n = 0) permits to conchae! that when
the next section we use Theorem 7 to compute all the 6 tends toward 0, the stable sete agate configuration
equilibrium configurations of a part under the radial- is unique and tends toward (Xo, Vaneoeau in the new
constant force field. system of coordinates, that is (0,—p,3m
5 _ 4) in the
standard system of coordinates.
4 An Algorithm to Compute all Equi-
librium Configurations of a Part Same Pivot Point and Center of Mass We as-
sume now that (Xo,Yo) = (0,0). Let us differentiate
This section is organized as follows. First we explain once the above system w.r.t. 6. We get:
how to iteratively compute expressions of the partial
derivatives Fs (6,0) w.r.t. the partial derivatives of 0°V OX* OV OY,
ee eee eeeee in@ = 0O (22
the lifted radial potential field V evaluated at (0,0): ax? os +axey oo + lolsin 2
ee 0). These expressions enable us to com- ev.VSOAY
ax" 4 OV
ev oay* 6 = 0 (23
pute the equilibrium configurations of a part using The- DXOY 06.1 av? op 1 IPlo 23)
orem 7. We describe all the necessary operations in the
where the partial derivatives of V are evaluated at
form of an algorithm. Then we make some comments
(X*(0,5),¥*(0,5)) and 2X, o*~ are evaluated at
about the computation of axtoee (0, 0). Finally, an
(6,65). If we take 6 = 0 in this system, we get:
example illustrates the algorithm.

4.1 Expressions of a2" (6,0) Hess V (0,0) (2. a )= —|S| (oe i (24)
Differentiating Equation (13) yields:
) According to Proposition 3, si above saint is in-
ae)
= 6) =cos@ yAGE (6,5) —sin@ Sak (0, 6). 2 (0, 0)
vertible, so that expressions of x (0, 0) and ox
46 F. Lamiraux and L. Kavraki

and thus of 92" (6,0) are obtained by inversion of sys-

minima + 0
tem (24). Let us notice that these expressions are //Pivot point and See of mass distinct.
trigonometric polynomials of 0. If (2X(0,0) #0) or (24(0,0) 4 0)
(Xo, Yo) + Suances Y))
To get higher order derivatives of X* and Y*, we
(p, wb) + Se ee Yo)
need to differentiate several times (24). Let us point minima + {(0, —p,= — w)}
out a property of this system. exit;
endif;
Proposition 8 Differentiating k—1 times system (22- //Same pivot point and center of mass.
23) (k > 2) yields a system of the form: x0] + 0;
expr({1] < expression (22-23);

;
Hess V(X*(0,6), :
Y*(0,6)) (oe
Re (0,8)
6,6) \_a fay
n < 0;
while (z[n] = 0) do
nent;
system < evaluate(expr[n],6 = 0);
(25) (X[n],¥ [ne giiwrtets
where Ex, is a polynomial expression of variables
solve(system,(5*-(6, 0), aa (8,0)));
ox" (0,6), 2X (0,6) forl < 1 < k-1. The x{n] + cos6X[n] — sin 6 Y [n};
coefficients of this polynomial are functions of the expr|n + 1] « differentiate(expr[n],d);
axiayias(X"(9,5),¥*(9,8)), for2 <i < k+1 and od;
xe + differentiate(x[n], A);
Os 7 51. (01, ..-;9m) < solve(x[n] = 0, 6);
for each 7 between 1 and m do
Proof: This proposition can be easily proved by in- If evaluate(xe,,9 = 4:1) > 0
duction. For k = 2, if we differentiate once (22-23), we minima <- minima U {(0, 0, 4:)}
find that the proposition is satisfied with: endif
od;
pee Vso + 2a + Vie ¥7?
2K Var XT? + WV aXTY + Voa¥7? Table 1: Algorithm that computes the local minima of the
lifted radial-constant field Us for small 6.

where Vij = oro (X*(0,0),¥*(0)0), XPS

ox (9,4), Yi = a ~ (0,6).
form instead of writing them here. However, it is im-
portant to notice the structure of the successive rela-
Now, we assume that the proposition is satisfied at
tions expressed by (25). If we at le = 0 in these equa-
order k, that is Ey in (25) is of the correct form. Let us
differentiate again this equality w.r.t. 6. The left-hand tions, we get linear systems in (24*
Bo“ (6,0), ae ie(@,0)).
side becomes: These systems_are invertible since Hess V is positive
definite. By inverting these systems terete, we get

Hess V(X*(0, 5), Y*(0, 6))

oe
oe
(6,5) +
successive pexProns of a (0,0) and2—e “~(6,0) and
“pgret (9; 6) thus of mae
2 (6,0) w.r.t. the partial derivatives of V
at (0,0). It can be verified that these expressions are
VaoXd + VeaYy Vo 1X7 + VieYy )(X; )
Sc eR PTETR polynomials in 6. This iterative proce-
VaixXg + Vie ViaXt + Vosyy ve dure is the core of the algorithm presented below.
Note that the first term of this sum is exactly the left
hand side of (25) at order k + 1. The second term can 4.2 The Algorithm
be included in E41: it is of the correct form.

It remains to check that if EF, is of the form described Using the above developments, the roots of the first
in the proposition, so is F,41. This test is straightfor- non-uniformly zero o 2"(6,0) can be computed by the
ward. Oo algorithm presented in Table 1. This algorithm needs
the partial derivatives of V evaluated at (0,0) and a
The expressions involved in the above proposition function, minimize, that computes the unique mini-
are rather complex. That is why we only give their mum of V.
Positioning Symmetric and Non-Symmetric Parts
AT
The Partial Derivatives of V. Ifthe part is polyg- Inverting this system yields:
onal, we can express exactly the partial derivatives of
V and thus evaluate them for any (X,Y). In this case, OX* ol ey? _(|si
55 (O20). =5 sind 35 CAV AY cos 6.
minimize can numerically minimize V using a gradient
method and return an approximation of (Xo, Yo).
Substituting these equalities in (13) gives as expected:
If the part is not a polygon, the partial derivatives of
V can be computed numerically. We do not give details x°(0;0)= 0:
about these numerical computations. Some discussion
can be found in [11]. Now, differentiating again (22-23) and substituting 6 =
0, we get:
4.3 Example
0? X*
Let us compute the stable equilibrium configurations ABER + V3,0X7? + Wei XTY +VieYy? = 0
of the part represented in Figure 6. For this polygo- OPY* *2
—=-
52 + Vo1Xi? + 2Vi,2X7YT + Vos3¥7 = a0)
nal part, symmetric by rotation of angle 5, we have
computed using Maple the expressions Ole av and oY
These expressions are very long, we cannot Tenet them where Vi; = s2z5¥7(0,0), Xt = 94°(6,0), Yi" =
here. Differentiating symbolically these expressions, we o** (0,0) and oe and ee are evaluated at (0,0).
obtained higher order derivatives of V that we evalu- From this system, we extract expressions of the second-
ated at (0,0): order partial derivatives of X* and Y*:

FY (0,0) = 2%(0,0) = O7x*

Fen (Os0}=0
ary?
oe (O20)}* 10:

S70 argsinh 3 — Argsinh 5) Thus: Pye

—52 (0,0)=0.

ore Ue To get the third-order partial derivatives of X* and

Bx 09) = Gyagy0)= 0 Y*, we need to differentiate twice (22-23) w.r.t. 6 and

take 6 = 0. This yields two equations of the form:

OX" (6,0) +
A= a)
oY (0,0)ee o¥ (0.0) = oe—3 oey* (8,0) +
=r a A
ar CAD aeaye 09)pitt where ... stands for already evaluated expressions.

va lac a
OxX-0Y? 2
From this system, we get:

O°? x* sin 0((2\/2 + 6) cos” 6 + V2 — 6)

We apply now the successive steps of the algorithm
706° (6,0) = 204g——-¥"
A
described in Table 1 to the part. y= cos 6((2:/2 + 6) cos? 6 — 3/2)
58 KOO = = 2040) eal
To ey the notation, let us write 4 =
g¥ (0,0) = $5(0,0). Substituting 6 = 0 in (22-23) and finally
yields:
oP
Ges )= 1024/2 + 3072 sin 40.
0 0)+|S|sind = 0 063 >’ ie

gece a 0) + |S|cos@ = 0. We recall that A = 8v10 (Argsinh 3 — Argsinh 3) > 0.

00
48 F. Lamiraux and L. Kavraki

tially supported by NSF IRI-970228 and NSF CISE

(0,4)
§A1728-21122N.

(1,1)
(4,0) References

[1] S. Akella, W. Huang, K. Lynch, and M. Mason. Pla-

nar manipulation on a conveyor with a one joint ro-
bot. In G. Giralt and G. Hirzinger, editors, Interna-
tional Symposium on Robotics Research, pages 265—
Figure 6: Polygonal part symmetric by rotation of 3. 276. Springer, London, 1996.

[2] D. Biegelsen, W. Jackson, A. Berlin, and P. Cheung.

x* satisfies the hypotheses of Theorem 7 for n = Air jet arrays for precision positional control of flexible
a o (6, 0) vanishes 8 times, 4 times with negative media. In Proc. Int. Conf. on Micromechatronics for
Information and Precision Equipment, pages 631-634,
slope and 4 times with positive slope (for 9 = 13,
Tokyo, Japan, 1997.
i = 0,1,2,3). We conclude that for small values of 6,
the part has 4 stable equilibrium positions converging [3] K. Bohringer, V. Bhatt, and K. Goldberg. Sensorless
toward (0,0,75) when 6 tends toward 0. The part is manipulation using transverse vibrations of a plate.
thus positioned up to part symmetry by the force field. In Proc. IEEE Int. Conf. on Rob. and Autom., pages
1989-1996, 1995.

5 Conclusion [4] K. Bohringer, B. Donald, and N. MacDonald. Pro-

grammable vector fields for distributed manipulation,
with application to mems actuator arrays and vibra-
We have proposed a method to compute all stable equi-
tory part feeders. International Journal on Robotics
librium configurations of a part subjected to the combi- Research, 18:168-200, Feb. 1999.
nation of a unit radial force field with a small constant
field. This method is general and can be applied to any [5] K.-F. Bohringer, B. R. Donald, L. E. Kavraki, and
part, symmetric or not. In addition, this paper reports F. Lamiraux. Part orientation with one or two stable
a comprehensive study of the action of radial and small equilibria using programmable vector fields. to appear
in the IEEE Transactions on Robotics and Automa-
constant potential fields over parts. It proposes an in- tion.
teresting characterization of local minima of the lifted
potential field using the equilibrium curve and its par- [6] K.-F. Bohringer, B. R. Donald, and N. C. MacDon-
tial derivatives for 6 = 0. Some questions remain open ald. Upper and lower bounds for programmable vec-
however. All these results are asymptotic for small 6. tor fields with applications to MEMS and vibratory
plate parts feeders. In J. Laumond and M. Overmars,
The general case with any 6 between 0 and 1 is far
editors, Algorithms for Robotic Motion and Manipula-
more difficult since we do not have any expression of tion, pages 255-276, Natick, MA, 1997. AK Peters.
the equilibrium curve in general and the stable equi-
librium configurations are the roots of the equilibrium [7] J. Canny and K. Y. Goldberg. Risc for industrial ro-
curve. For this case, numerical methods will probably botics: recent results and open problems. In Proc.
IEEE Int. Conf. on Rob. and Autom., pages 1951-
be necessary. The rate of convergence under the fields
1958, 1994.
described in this paper, as well as under previously
proposed fields, is an open question. [8] M. Erdmann and M. Mason. An exploration of sen-
sorless manipulation. IEEE Tr. on Rob. and Autom.,
4(4):369-379, 1988.
Acknowledgments
[9] K. Y. Goldberg. Orienting polygonal parts without
This work was performed while Florent Lamiraux was sensors. Algorithmica, 10:201—225, 1993.
with the Department of Computer Science at Rice Uni-
versity. The authors are grateful to Karl Bohringer and [10] L. E. Kavraki. Part orientation with programmable
vector fields: Two stable equilibria for most parts. In
Bruce Donald for many discussions. Work on this pa- Proc. IEEE Int. Conf. on Robotics and Automation,
per by Lydia Kavraki and Florent Lamiraux was par- pages 2446-2451, 1997.
Positioning Symmetric and Non-Symmetric Parts 49

[11] F. Lamiraux and L. Kavraki. Part positioning using a discrete distributed actuator array. In P. Agarwal,
radial and constant fields: Modeling and computation L. E. Kavraki, and M. Mason, editors, Robotics: The
of all equilibrium configurations. in preparation. Algorithmic Perspective, pages 35-47. AK Peters, Nat-
ick, MA, 1998.
[12] F. Lamiraux and L. E. Kavraki. Positioning symmet-
ric parts using a combination of a unit-radial and a
constant force fields. to appear in the IEEE Interna- [15] D. S$. Reznik and J. Canny. Universal part manipula-
tional conference on Robotics and Automation 2000, tion in the plane with a single horizontally-vibrating
2000. plate. In P. Agarwal, L. E. Kavraki, and M. Mason,
editors, Robotics: The Algorithmic Perspective, pages
[13] W. Liu and P. Will. Part manipulation on an intelli- 35-47. AK Peters, Natick, MA, 1998.
gent motion surface. In International Conference on
Intelligent Robots and Systems, pages 399-404, Pitts-
burg, PA, Aug. 1995. IEEE/RSJ. [16] J. Wiegley, K. Goldberg, MM. Peshkin, and
M. Brokowski. A complete algorithm for de-
[14] J. Luntz, W. Messner, and H. Choset. Velocity field signing passive fences to orient parts. In Proc. Int.
design for parcel manipulation on the virtual vehicule, Conf. on Rob. and Autom., pages 1133-1139, 1996.
 hetodh jail, wires « 7
YW hoe deren, a
1 ir

Lap ticst ‘tla a

ot e
bar) stag 4 le noitenidnsa
i a Se ip ai
aime are‘aoe
® aol,
ater
~ & wen
A pale ate? Cj tinea,
deity mescorerrr ei Pe. hide
i> ao AtaS sent an
& Pi j HE= hE :
eneme snide Sim eee
Lika ye

; ne aN Tia;
a
eae ee
” smal oh d
peru s
Oar « iy bow * ; nn
; Cis ve 4 »
3

if ' eo J ‘ s P. :

vs
— oe
2 Ps
}4omsal}) hw ior Be dete Ss
PA palin mui. - 11 ee i
od atiec desl besthe count Sm
a ey al Yoni tory 2 Chimes = Te
rr u —_
SS, &.
; Soi mar fe WT w atyall iyi tony
ie a ee
— pee rt mal mds: Qe
er —
ig] icy} te
: y i iQ .
4 (eh : Hibingert” TS i once
10 OF Wy Thrwitiioo, Bit Fajen Celi ee ahi ots wlbde Vasa ae
bof the aia, re Vial ma, Lh
a oe
mo!mbhe
*
(ree, oe PR ‘Di 7
-
a! uw hae i Cipary) 5
are
ee
ey 4

’ 1M twin J SLUT FA i in4 ' a) a

“" hy :
/ pat fae
2 vw Ore. Phi
_
!
ayy f aj ¢
, Oh ma

é Veawe
- Rs

i tha
i mais ier .
e i seit Fi : tual

i y
"
vi
let Pr
Pia Nc f wy te

g wii (
-

® i” 1

tw. I
eal
~ é

‘
"en
~
Complete Distributed Coverage
of Rectilinear Environments

Zack J. Butler, Carnegie Mellon University, Pittsburgh, PA

Alfred A. Rizzi, Carnegie Mellon University, Pittsburgh, PA
Ralph L. Hollis, Carnegie Mellon University, Pittsburgh, PA

Complete coverage of an unknown environment is a stead to use a strict geometric algorithm about which
valuable skill for a variety of robot tasks such as floor completeness can be proven for any environment of a
cleaning and mine detection. Additionally, for a team given class. Tasks which may be accomplished by mul-
of robots, the ability to cooperatively perform such a tiple robots introduce additional complexity, since each
task can significantly improve their efficiency. This point in the environment need only be reached by one
paper presents a complete algorithm DCR (distributed of several robots, and in order to cooperate, the robots
coverage of rectilinear environments) which gives ro- must know (or discover) each other’s location. How-
bots this ability. DCR is applicable to teams of square ever, using multiple robots gives the potential for in-
robots operating in finite rectilinear environments and creased efficiency in terms of total time required.
executes independently on each robot in the team, di- While a number of sensor-based coverage algorithms
recting the individual robots so as to cooperatively cover have been proposed, in most, the algorithm begins by
their shared environment relying only on intrinsic con- assuming the environment to be simply shaped (e.g.
tact sensing to detect boundaries. DCR exploits the simply connected, monotone, convex, etc.). To cover
structure of this environment along with reliable posi- its environment, the robot may then execute a simple
tion sensing to become the first algorithm capable of coverage path until it discovers evidence that contra-
generating cooperative coverage without the use of ei- dicts the initial assumption, at which point one of sev-
ther a central controller or knowledge of the robots’ eral strategies is used to ensure coverage on all sides
initial positions. We present a completeness proof of of the newly discovered obstacle. An algorithm pre-
DCpr, which shows that the team of robots will always sented by Lumelsky et al. in [2] and extended in [3]
completely cover their environment. DCR has also been produces coverage of C? environments for robots with
implemented successfully in simulation, and future ez- finite non-zero sensing radius by recursively building a
tensions are presented which will enable instantiation subroutine stack to ensure all areas of the environment
on a real-world system. are covered. This algorithm does not explicitly build
a map, in contrast to sensor-based coverage work by
1 Introduction Acar [4] based on a planned coverage strategy outlined
in [5]. In [4], a cellular decomposition of the environ-
The coverage problem, that of planning a path for a ment is constructed and used to form a graph which
sensor, effector, or robot to reach every point in an in turn is used to plan coverage — when a specific cell
environment, is one that appears in a number of do- has been covered, the robot uses the structure of the
mains. The problem of sensor-based coverage, that of graph to plan a path to an unexplored area, and when
planning such a path from sensor data in the absence of the graph has no unexplored edges, coverage is com-
a priori information about the environment, is limited plete. The cellular decomposition approach of [5] also
to robotics, but also applies to a number of different inspired the algorithm presented in [6], which in turn is
tasks. What is common to all these problems, whether the basis for the current work. In [6], we presented an
a spray painting task on a known surface or a mine algorithm for coverage of rectilinear environments by a
detection task with little or no initial information, is single robot using only intrinsic contact sensing. This
a need for assurance of complete coverage. For known algorithm also explicitly leveraged the degeneracies of
areas, a path can be correctly generated off-line [1], the environment (degenerate in the C” sense) by de-
but in the sensor-based case, the usual solution is in- composing the free space into a set of rectangular cells.
52 Z. Butler, A. Rizzi, and R. Hollis

In contrast to the more commonly studied coverage playing soccer [10]) without proof of the correctness or
tasks mentioned above, the inspiration for the current completeness of the individual or group algorithms.
work comes from a manufacturing environment. The On the other hand, research into algorithms for com-
minifactory, an automated assembly system under de- plete coverage of an environment by cooperating robots
velopment in the Microdynamic Systems Laboratory’, has so far used a central controller deploying robots
has been built within a framework that provides for from known locations, which is not satisfactory for the
rapid design, programming, and deployment [7]. minifactory problem. For example, Gage’s work [11]
uses random walks by a large team with a common
A minifactory includes several types of independent
home position to generate probabilistically complete
robots, but this work concentrates on the couriers,
small tethered robots that operate on a set of tileable coverage. A fairly abstract algorithm presented by Rao
platens which form the factory floor. The couriers have et al. [12] uses a small team of point-sized robots with
micron-level position sensing but only intrinsic contact infinite range sensing to build a visibility graph of a
sensing to detect the boundaries of their environment. polygonal environment. In contrast, work by Rekleitis
In addition, each is equipped with an upward-pointing et al. [13] uses cooperating robots with mutual remote
optical sensor to locate LED beacons placed on over- sensing abilities, but with explicit cooperation to re-
head robots as calibration targets. One of the tasks duce mapping errors rather than to increase efficiency.
for the couriers is to collectively explore the as-built
factory from unknown initial positions to generate a 2 DCr: Overview
complete factory map. This task has led to the investi-
gation of coverage algorithms for teams of robots, with DCR is an algorithm developed for square robots that
the restrictive environment providing a simplified do- use only intrinsic contact sensing, and produces com-
main to consider. The algorithm developed here there- plete coverage of any finite rectilinear environment
fore applies to teams of square robots with intrinsic while using cooperation between robots to produce cov-
contact sensing operating in a shared, connected recti- erage more efficiently. It consists of three distinct com-
linear environment with finite boundary and area. In ponents, shown schematically in Figure 1. The first,
addition, the robots in the team will not know their CCrRm, covers the environment by incrementally build-
relative initial positions or orientations, however, due ing a cellular decomposition C (C = {Co,...,C,}).
to the structure of the environment, their orientation It uses only C and the robot’s current position p =
will be one of four distinct values (i.e. with axes aligned (Px, Py) (i.e. time-based history is not used) to direct
with the environment boundaries) and cannot change. the robot to continue coverage. CC ry is based on
the work in [6], and performs coverage without taking
Like the work described here, previous work in dis- into account any other robots in its team. However,
tributed robotics has presented the use of a common with the other components of DCR properly designed,
algorithm executed by each robot in a team (without a it performs coverage equally capably in a cooperative
central controller) to achieve a specific task [8, 9, 10].
For example, in the work of Donald et al. [8], several
distributed algorithms were presented to perform a co-
operative manipulation task. There, however, the goal
was to recast a simple provable algorithm in such a
way that explicit communication was unnecessary, but
could rather be implicit in the task mechanics. In our
data \ colleague|
work, the environment is static, and so this reduction info /

is not available, and the underlying algorithm (single- “Feature”

“handler
data, Sie
robot coverage) is much more complex. Other work on messages
other| robot 2
decentralized control of cooperative mobile robots has D C R (robot 1)

generally focused on the creation of a certain group

behavior (as simple as foraging [9] or as complex as Figure 1: A schematic rendition of the algorithm DCr, a
copy of which is run independently by each robot performing
‘More information at http://www.cs.cmu.edu/~msl coverage.
Complete Distributed Coverage of Rectilinear Environments
53

Figure 2: Examples of (a) the sweep-invariant decomposition, (b) an oriented rectilinear decomposition, and (c) a possible
generic rectilinear decomposition of a rectilinear environment.

setting. The second part of DC, the feature handler, Figure 2b. This decomposition is produced by extend-
derives pre-specified types of features from C and com- ing all vertical edges (the x values of these edges are
municates with other robots in the team to determine called interesting points, analogous to critical points of
the relative position of the various robots. Finally, the a sweep through a C? environment) to form cell bound-
overseer takes incoming data from colleagues and al- aries, and can be incrementally constructed and easily
ters C. This is done without the explicit “knowledge” covered with the use of seed-sowing paths as described
of CCrm, but because CCryy uses no state other than below. This decomposition is produced by DCR when
C and p, the overseer can alter C to incorporate the performing coverage without colleagues.
new data, and as long as this is done such that C re- Multiple robots performing cooperative coverage in a
mains in a state admissible to CCry (as described in shared environment will not necessarily have the same
Sec. 2.4), coverage continues. orientation, and therefore will not necessarily build the
same oriented rectilinear decomposition. This means
2.1 Cell Decompositions under DCR that neither of the above decompositions can be built
in a cooperative setting. Instead, the cell decomposi-
Before discussing the makeup of DC’p, it may be in-
tions that are constructed under DC p fall into a class
structive to describe the type of cell decompositions
referred to here as generic rectilinear decompositions
that will be constructed. This discussion will focus
(GRDs), an example of which can be seen in Figure 2c.
only on the geometry of the cells in the decomposi-
A GRD consists of cells that are rectangular and su-
tion, but in each type of decomposition, every cell also
persets of cells of the SID. In addition, in a valid GRD,
includes “connections” (geometric references) to each
no two cells have overlapping area (Vi, j7:C; NC; = 0)
of its neighbors that allow for the straightforward cre-
and all cells contain connections to all neighbors across
ation of an adjacency graph of the decomposition. One
their common edges. It should also be noted that dur-
way to uniquely decompose a known rectilinear envi-
ing the performance of DCR, a valid GRD may consist
ronment is the sweep-invariant decomposition (SID), of multiple disconnected components. This is because
as shown in Figure 2a. In a SID, each obstacle or robots only share completed cells, but may meet in an
boundary edge is extended until a perpendicular wall area that neither has completed. Also, for a given envi-
is reached, with all extended edges defining cell bound- ronment, different initial positions of robots may lead
aries — as a result, a cell’s edge may be determined by to different GRDs, since the nature and timing of the
an arbitrarily distant boundary component. Thus, it is cooperation may change.
not possible to incrementally construct the SID with-
out splitting cells that have already been completed, 2.2 CCrm Description
since determining a cell’s extent could require knowl-
edge of features that are arbitrarily distant from the To perform cooperative coverage, each robot must first
cell itself. This difficulty of incremental construction be able to perform coverage by itself. This is the job of
makes sensor-based coverage based on the SID imprac- CCrm, asensor-based coverage algorithm for a rectan-
tical. A different, nearly unique type of decomposi- gular robot with only intrinsic contact sensing operat-
tion is the oriented rectilinear decomposition, shown in ing in rectilinear environments. It operates in cycles,
54 Z. Butler, A. Rizzi, and R. Hollis

Figure 3: Some examples of how CCrm discovers and localizes interesting points.

with the length of each cycle being a single straight-line 3b, which uses similar rules, although in this case a
trajectory of the robot. At the beginning of each cycle, placeholder Hp is built rather than a new cell.
it examines the structure of C around the current posi- Since the first applicable rule determines the next
tion, and uses an ordered list of rules to determine the trajectory, rules for interesting points are tested first,
next trajectory (both direction and distance) required followed by the seed-sowing rule, as follows:
to continue coverage. The rules are structured so as to
produce complete coverage. Once a trajectory is sub- 1. If p is in a cell, call that cell C. and continue at
mitted to the underlying robot, the robot moves until rule 2. Otherwise, if p, is within w (the width of
one of three types of coverage events occurs. A cov- the robot) of a complete cell, go into that cell.
erage event occurs when the maximal distance of the 2. If C. has a side edge with finite uncertainty, move
trajectory is achieved, a collision takes place, or con- to localize that edge.
tact that is to be maintained with a wall is lost. CCry 3. If C. has a side edge at a known position that
then updates C given the type of event, chooses a new is not completely explored, investigate the closest
trajectory, and the cycle repeats. unexplored point in it.
Under CC ry, each cell C; is described by its mini- 4. If C. has unknown floor or ceiling, go in —y or +y
mum known and maximum potential extents, C;,, and respectively.
C;,, respectively. Associated with each edge of the cell 5. If C. is not covered from its left edge to its right
is a list of intervals, each of which is a connected line edge (note that if either edge is unknown, this will
segment describing the cell’s neighbor across that edge. always be true), continue seed-sowing.
Each interval therefore points to a wall, another cell or
a placeholder, a line segment denoting the entrance to If none of these rules apply to C,, then it must be
unexplored area. complete. A cell is defined as complete when each of
its edges are at known location and are spanned by a
To cover each cell, CCry generates a seed-sowing set of intervals, and its interior has been completely
path as shown in the leftmost portion of Figure 3a, in covered with seed-sowing strips. If C. is complete, the
which the robot travels along paths parallel to its y following rules are used, in order:
axis and as far apart as the width of the robot. These
continue until an interesting point is detected, such as 6. If there is an incomplete cell in C, plan a path to
in the middle portion of Figure 3a. When this occurs, it and follow the first step of that path.
CCrm updates C based on the type of coverage event. 7. If C. has a placeholder neighbor, build a new cell
In the case of Figure 3a, a new cell (C1) is added around from it and enter the new cell.
p with uncertainty in the boundary between it and the 8. Choose a placeholder from C, plan a path to the
previous cell. A rule then fires based on this uncer- cell it adjoins and take the first step of the path.
tainty to localize the interesting point before coverage
continues in the new cell. Another case of detection and Path planning is done by implicitly creating a graph
localization of an interesting point is shown in Figure from the adjacency relationships of the cells in C, and
Complete Distributed Coverage of Rectilinear Environments
59
searching that graph for a path to the destination. The robots. However, the claiming of area does not effect
search is done depth-first from the destination back to completeness of DC’, so it can be implemented only
p, and the destination is chosen deterministically, so when desired.
that even though the plan is regenerated after each
trajectory, the planned path will remain the same as
2.3 Feature Handler Description
the plan is executed. Traversal of each cell is done sim-
ply with straight line motions. Finally, if none of these
The feature handler is quite independent from the
rules fire, there must be no placeholders or incomplete
other two components in its behavior. Its task is to
cells, and so C must be completely covered and its use the data in C to generate colleague relationships
boundary explored, meaning coverage is complete. between robots, however, it does not alter C. There-
Once cooperation is achieved, a cell may have an- fore, the specific types of features and algorithms used
other (complete) cell above or below it, as seen in Fig- by the feature handler can change from one system to
ure 2c. To allow CCrm to continue seed-sowing in the next without affecting the rest of DCr. For exam-
such cells, a construct called an exploration boundary ple, in the current implementation, the feature handlers
has been developed. Exploration boundaries are vir- look for distances between unlabeled landmarks (bea-
tual walls placed at the floor and ceiling of each cell cons) that are common to two robots’ maps. However,
obtained from a colleague, and have the property of other feature types such as wall lengths or beacon la-
allowing the robot to pass though them only when the bels (if present) could also be used, depending on the
robot is in a complete cell. This allows CCry to per- particular system. The generic behavior of a feature
form seed-sowing in a cell that does not have walls at handler is to inform all other robots in the team of
its floor or ceiling, as shown in Figure 4a, but does the values for all instances of a specified feature type
not impede path planning once the cell is complete, as in C. When two robots discover matching features,
shown in Figure 4b. It should also be noted that in their feature handlers symmetrically compute the rela-
any case, a completed cell will always have an attached tive transforms between their local coordinate systems
floor and ceiling, where an attached cell edge is one using appropriate geometric algorithms. These trans-
that is adjacent to a wall or another complete cell (not forms are then used by the overseer when incorporating
a placeholder) at every point. data obtained from colleagues.

A second addition made to CCry that only has im- The other mandatory job of the feature handler is
pact during cooperation is that a robot can “claim” a to transfer data to all colleagues at the correct times.
placeholder as it builds a cell from it. The robot passes When a colleague relationship is formed, all complete
this claim on to the overseers of its colleagues, and so cells must be given to the colleague, and when a cell in
the other robots in the team will not travel to that area C first becomes complete, that cell must be reported
to perform coverage. This helps minimize the double to all colleagues. This ensures that the information
covering of area, but also implies trustworthiness of the available to each robot is consistent, which in turn
maintains the attached edge property of cells described
above.

2.4 Overseer Description

The overseer has the task of incorporating all data from

colleagues into C, a job complicated by the require-
ment that C must remain admissible to CCry. The
addition of an incoming cell Cnew to C is done in two
stages. In the first stage, zero or more new cells are
added to C to account for the area of Crew. Then, for
each added cell, its intervals are assigned to walls, ex-
Figure 4: The effects of an exploration boundary (dash-dot
isting cells or newly created placeholders. An example
line) when the robot is in (a) an incomplete cell and (b) a
of the action of the overseer is shown in Figure 5.
complete cell.
56 Z. Butler, A. Rizzi, and R. Hollis

(a) (b) (c) (d)

Figure 5: An example of adding new area by the overseer, in which the initial cell decomposition is depicted in Figure 5a
and the incoming cell Cnew in Figure 5b. The dot in each section of the figure represents a common real-world point.

The cell Cw arrives described in the coordinate sys- possibly also provided by the sender of Chew). Each
tem of the sending robot, and so it is first transformed new cell is then intersected with every cell C; € Cine.
into the local coordinate system using the transform This intersection process is designed to retain the com-
provided by the feature handler. Also at this time, all plete Crew and eliminate any overlap with incomplete
intervals in Cre, that do not point to walls are mod- cells (shrinking or deleting the incomplete cells as nec-
ified to point to “unidentified free space” rather than essary). Since an incomplete cell must be attached on
a specific cell or placeholder, since any such neighbor its floor and ceiling, C,-~ cannot be taller than any
information in C,,ew is meaningful only to the robot C; € Cine. The intersection is therefore performed as
that sent it. follows (note that it is not recursive, since each Crew
To determine the area of cells to be added to is now a fixed size):
C,C.om is defined as the set of all complete cells in © VC; © Cine:
C, and Cine = C — Coom. For the example in Figure
— If Ci, NCnew = 9, do nothing.
Cranes oscis and. Cine = 102). Crew 1s then
— If Ci,, NCnew = 0, reduce C;, so that it does
intersected with each cell in Cogm as follows:
not overlap Chew, skip to next Cj.
— If. Cyn Chas = Grew; TeplaceCamih- (4,
e VC; ‘S eet
skip to next C;.
— If C; NCrew = 9, do nothing.
— If Crew is the same height as C;, there must be
— If Crew is wider (larger in 2) than C;:
a partial overlap in the x direction, so reduce
* If Crew,right > Cirignts make a copy of C;,, (on either its left or right as appropriate)
Crew Called C,, set Cr jefe = Cirignt, and tOLabDut Crew:
call the overseer with C,.
— Otherwise, there must be partial overlap in
* Similarly (note no else here) for the y direction:
Creuleyt << Crise:
* If Crew,ceit < Ci,ceit, replace C; with a cell
* If Crew is also taller than C;, make a copy
Cs; set Oz ficor = Unew,ceil and keep only
Gf Crew Called Cz, set, Cz te: = Citepn and
placeholders attached to C,, and create an
Coz,right = Ci,rignt, then call the overseer
interval in C, to point to Chew.
with C,.
* Similarly (again no else) for Crew, floor >
— Else if Crew is taller than C;, perform similar
Ci, floor:
tests on top and bottom (no third test).
In the example, this intersection process results in
If Crew (or its descendants) survive this process (such the decomposition shown in Figure 5d. Finally, each
as the area shown in Figure 5c, added to C as C3 in unassigned interval 7in C,ew is given the correct neigh-
Figure 5d), it will consist only of area new to C, i.e. bor(s). This is done by determining which cell’s max-
Crew ™Ceom = 9. In addition, whether or not the area imum extent (if any) is across from the two ends of i
has been divided, each cell will still have at least two (these cells are denoted Cop and Cyt). i is then as-
attached edges (either walls or other complete cells, signed as follows:
Complete Distributed Coverage of Rectilinear Environments
57
e If Crop = Coot = 0, build a new placeholder H,,
equal in size to i and set i’s neighbor to H,,. (begin\ Seea:sowing xD _ S)
e If Gey = Coot #~ 0:
A Ri sinhA exploration /
— If Ciop is complete, set i’s neighbor to Cie:
Also, find the interval in Crop that corre-
pep OT
sponds to 7 and connect it to Chew. initial
— If Cop is incomplete and i is horizontal, split Codge exp
2, connect it to Crop for iM Crop, and build a
placeholder for 1M Ciop,-
Figure 6: A simplified version of the FSM representation
— If Crop is incomplete and i is vertical, connect
of CCrm. Contents of the nodes are described in the con-
i to Crop over the y extent of Crop, as long as
text of the proof. Gray dots represent the completion of
Crop, is within one robot width of i, build a
a cell.
placeholder otherwise. If Cropn adjoins Crew,
find the corresponding interval in Cio) and
connect it to Chey. starting from the initial condition of an empty map.
Each motion has from one to three (uncontrollable)
© If Crop # Coot, this should only occur if 7 is hor-
outcomes, each of which is a different type of coverage
izontal and Cop and Cy: are each either an in-
event and is represented by a transition in the FSM.
complete cell or 9. In this case, connect i where
Completeness is then demonstrated by showing that
adjacent to Ctop,, Coot, to those cells, and build
there are no terminal states other than complete cov-
placeholders for the remainder of 7.
erage, and no cycles that do not result in a monoton-
ically increasing measure of progress toward coverage.
tae Eroor In addition, since states of the FSM are uniquely deter-
mined by the robot’s current cell C,, the FSM is valid
To prove that DCR leads to complete coverage of any for any valid GRD that includes C, (no matter how it
finite rectilinear environment by any number of robots was constructed). A simplified version of the FSM, in
(in the absence of interrobot collisions), it is first nec- which some nodes consist of several states and transi-
essary to show the completeness of CCry, since DCR tions, is shown in Figure 6. Note that all loops in Fig-
run by a single robot is exactly CCry. We then show ure 6 contain a cell completion event, which indicates
that any cooperation regardless of its timing and na- progress toward complete coverage. While the details
ture does not interfere with the progress of CCra,. of the FSM are beyond the scope of this presentation,
These statements, combined with the reactive nature a brief description of each node may give some insight
of CCry (and therefore the decoupled nature of cov- to the structure of the FSM as well as CC ry, itself.
erage and cooperation under DCR), imply that DCR
is complete. Node A contains the states that describe seed-
sowing. In the absence of another incomplete cell over-
Proposition 1 CCry continues coverage to comple- lapping the current cell, seed-sowing continues, cycling
tion in a finite rectilinear environment in the absence through the four states in Node A, until an interest-
of cooperation that alters the robot’s current cell. ing point is discovered. This discovery can be made in
five different ways, each leading to one of five successor
Completeness of CC gy is shown through the con- states of Node A.
struction and analysis of a finite state machine (FSM)
which represents all possible behavior of CCry. To Node B contains a single state, the situation shown
construct the FSM, an equivalence relation is defined in the middle of Figure 3a. From this state, a single
over all possible cell decompositions C and robot po- motion completes the previous cell, and the robot is
sitions p such that any two (C,p) pairs that cause the then in a cell with one known and partially explored
same rule to be applied in the same way are considered edge. All states for which the current cell has this
equivalent. The resulting equivalence classes define the structure are contained in Node C, in which the robot
states of the FSM. All possible motions of the robot un- explores this first side of the cell. Node C contains
der CCrm (without cooperation) are then enumerated, two cycles which correspond to the exploration of that
58 Z. Butler, A. Rizzi, and R. Hollis

edge monotonically up or down (+y or —y). As the ex- described above. Global correctness is demonstrated
ploration progresses, the robot may create and extend by showing that the area of any cells added to C is cor-
wall intervals and/or placeholders, but there is never a rect and that all mutual connections between cells are
cause to reverse direction. This exploration may also constructed correctly. Local correctness is then shown
include attachment of the current cell to neighboring via an enumeration of the possible effects on the robot’s
cells, which is done correctly in any valid GRD. Once current cell by the overseer.
this exploration is complete, seed-sowing once again Proof that added area is correct relies on the fact
begins in this new cell. that all complete cells in a GRD are supersets of SID
Nodes D and E each represent a single state which cells (including the cell obtained by the overseer Crew).
will always lead to Node F. Node D represents a state Therefore, VC; € Ccom; (Cnew—Ci) is also a superset of
in which a corner is discovered by losing contact while SID cells. To confirm that the area added by the over-
moving along the floor or ceiling of a cell during seed- seer from Crew is in fact (Crew — Ci), Cnew is written
sowing. Node E represents the case shown in Figure as C; UC; UC;,, where C; is the area to the left of C;,
3b, in which a placeholder is built and the final edge of C, the area to the right of C;, and C,, the remainder
a cell localized. In both of these cases, the cell’s edge is of Crew. C; and C, will be fed back to the overseer
then at a known location. Node F in turn applies to all if non-null, at which point they will remain unchanged
cells where all but one edge are known and explored. It relative to C;. If Cj, is larger than C;, it will also be
is similar to Node C, in that it contains several states given to the overseer, but will be subject to the ver-
that describe the exploration of an edge, and the robot tical intersection test, which in turn sends C,, — C;
moves monotonically along the edge. There are some to the overseer. Otherwise, C,, C C;, or equivalently
internal differences between the nodes, and when the Cm —C; = 9. In either case, the total area added based
exploration is complete in Node F, so is the cell, with on Crew and C; is (Cy) UCU [Cm — Ci]) = (Crew — Ci).
one exception. This exception is for the first cell (Co), For incomplete cells {C; : C; € Cine, Ci Crew 4 9},
for which exploration of its first known edge is also de- it must then be shown that after Cney is added, all
scribed by Node F, leading to Node X. This can only known edges of C; lie on edges of the SID. Since in all
happen once, however, so this cycle cannot be repeat- cases, edges of C; that are moved will be coincident
edly traversed without completing a cell. with edges of Crew, which is a valid GRD cell, this
Finally, Node X describes the state where the ro- condition is also satisfied.
bot’s current cell is complete, as well as when the first To show that each added cell is correctly connected
edge of Cp has been explored. In the latter case, since to its neighbors, it must be shown that determining
Co is not yet complete, CCryy returns to Node A to the neighbors at each end of an interval (as is done
perform seed-sowing toward the other side of Co. Oth- by the overseer) is sufficient to determine its overall
erwise, there are three possibilities. If there is an in- disposition. This is in turn true if no interval is adja-
complete cell in C, the robot will enter it and restart cent to more than two cells, and no cell lies adjacent
seed-sowing in Node A. Otherwise, if a placeholder ex- to an interval without reaching one end of it. Proofs of
ists, CCry will build a cell from the placeholder and these statements rely on the property that each com-
begin to explore the cell’s near edge as described by plete cell in a GRD will always have two attached op-
Node C. Finally, if and only if Node X is reached and posing edges.
no incomplete cells or placeholders exist, coverage is
To show that no cell can lie in the middle of an in-
complete and DCR terminates.
terval 7, assume that such a cell exists (call it C,). By
Proposition 2 The action of the overseer always definition, C,, must not have a neighbor on the side that
leaves (C,p) in the domain of CCrm. attaches to 7, otherwise that neighbor would be present
instead of Crew. Cz must therefore have neighbors on
This proposition has both global and local (to p) im- its sides perpendicular to i. However, if these neigh-
plications. Globally, the overseer must always produce bors are walls, i will end at the edges of C,, which
a valid GRD, since this is assumed in Proposition 1. In contradicts the original assumption. Otherwise, the
addition, the local construction of C must be such that two neighbors must themselves have neighbors in the
C and p form a state which is represented in the FSM directions perpendicular to 7. Eventually this chain of
Complete Distributed Coverage of Rectilinear Environments
59

Call relation
acrst
OMG,Ge 0Ovedt) wan
DE Crew

Os CAIealebov elap)
Grorlan toca ps cay
[wate Taoeffect
[Crew
Cc, =Co, top/bottom
Creal 'CeiCy Ce,
[call subsumedreplaced
Continue in small C
middle replaced as above, but see case 3 in text
It is possible for the intervals on an edge of C, to be modified by the addition of an
adjacent cell, but this will not change the state of CCrm except perhaps to complete Ce.

Table 1: Effects of the overseer on the robot’s current cell C..

cells must adjoin a wall, at which point i’s neighbor is shown in Figure 7a, and is handled by connecting
will be a cell, not null as assumed. the interval in Crew to C. even though the minimum
Proving that an interval 7 will not adjoin more than extents of the two cells do not adjoin. This connec-
two cells is also shown by contradiction. First, assume tion is prescribed in Section 2.4. This puts CCry in
7 is a vertical interval in C;,e~ adjacent to two existing Node X, but allows the robot to reenter C,, as it is re-
cells. The two cells (C, and C,, complete or incom- quired to do, since C, is still incomplete. CCry then
plete) must therefore be unattached on the side facing resumes seed-sowing, since C, still does not extend to
Crew. Ca and Cz, must therefore have attached floors Crew. Case 2, shown in Figure 7b, is handled by rule
and ceilings (including their mutual edge), and both 1 of CCrm (this is in fact the only case in which this
must therefore have been created by a robot with the rule produces motion). Now in Node X, a move in the
same sweep direction. But this is not possible, as one x direction will always succeed, since Cre, must be as
would have to be created first (even if both came from tall as C., and if there was a wall between p and C,,
different robots), and would therefore have to extend it would have been discovered already. The robot then
to a true wall, not just to the other cell. Two cells like continues from this complete cell. Finally, Case 3 in
this C, and C;, therefore cannot exist, and so a verti- Table 1 is one in which multiple incomplete cells are
cal interval cannot have more than one neighbor. For created, such as in Figure 5d, which could potentially
a horizontal interval, the argument works exactly the cause problems for seed-sowing. However, these new
cells will always share a known edge, and coverage can
same way for complete cells. However, in this case it is
only continue away from that edge. Also, the poten-
possible to have at most two incomplete cells adjacent
tial failure of seed-sowing under CCpy, can only occur
to i if and only if they are Co and C}\.
when moving through the known but unexplored edge
Finally, to show that the action of the overseer does of an incomplete cell. Therefore, the robot will always
not leave the robot in a position from which coverage complete one of the new cells (even with successors)
cannot continue, the possible actions of the overseer and return to the other without triggering a failure of
in the neighborhood of p must be investigated. If the CCrm.-
robot is in a complete cell at the time of cooperation,
this cell will not be changed, and so the state of CCrm
is likewise unchanged. Otherwise, the robot’s current Proposition 3 Propositions 1 and 2 are sufficient to
cell C. is incomplete, and so the possible intersections prove completeness of DCR.
of Crew and C, must be enumerated. However, Crew
must be no taller than C,,,, since any known neighbors Proposition 2 ensures that regardless of the input to
above and below C. must be walls or complete cells. the overseer, CCry will always find itself in the FSM
The enumeration of all cases of intersection of C, and described in Proposition 1, from which it will continue
Crew is therefore as shown in Table 1, with the resul- to perform coverage, making monotonic progress. Also,
tant state of CCry also dependent on whether p is since the overseer can only increase the area spanned
within Crew. by Ccom (or leave it unchanged), the act of coopera-
Three of the results from this intersection are non- tion also describes monotonic progress toward complete
trivial, and are presented here in more detail. Case 1 coverage.
60 Z. Butler, A. Rizzi, and R. Hollis

Area covered by robot OS robot 1% both &

Figure 8: Screenshots of a two-robot coverage run in progress: (a) Initial positions (b) Just after colleague relationship is
formed (c) Each robot exploring a different region (d) Coverage is complete.

4 Implementation / Example respect to time or total distance for the pair, it does
show that each robot spends less time covering than it
DCR has been implemented in simulation, and a series would without cooperation.
of screen shots of a single run are shown in Figure 8. In addition to the simulation, CCry has been suc-
The simulation gives an overall view of the environ- cessfully implemented in the minifactory environment
ment as coverage progresses, and also displays a view on a single courier, and the relationship of the algo-
(not shown here) of the cell decomposition internal to rithm to the underlying robot control will remain the
each robot. In the simple example shown here, the two same in DCr. However, some extensions to DCR will
robots Ro and R,; performed coverage independently ease its transition into a real-world robot system. For
until a time just before Figure 8b was taken, at which example, the simulation currently incorporates small
point their feature handlers decided that sufficient in- non-cumulative position error, which is allowed for in
formation was present in their maps to determine their most cases, but not in a rigorous (or even completely
relative position. Note that at this point, Ro is no correct) manner. Further analysis of the effects of this
longer performing seed-sowing over the full width of and other types of uncertainty will make DCR ap-
the environment. By the time Figure 8c was taken, plicable to a wider variety of robot systems. Also,
each robot was working on a different area of the envi- while the simulation can effect collisions between ro-
ronment, after which they ended up in the same area. bots, DCR currently uses simple methods to attempt
R, then claimed the area at the lower left before Ro to avoid colleagues and make progress. These meth-
could, and so Ro simply waited in a “safe” place for Ry ods often succeed, but are prone to failure in complex
to finish. Finally, when coverage is complete in Figure environments, and more intelligent strategies must be
8d, note that only about half of the area has been vis- developed, preferably ones that allow the completeness
ited by both robots. While clearly not optimal with proof to be retained with minimal modification. Our
Complete Distributed Coverage of Rectilinear Environments
61
eventual goal is the implementation of DC on the
[4] E. Acar and H. Choset, “Critical point sensing in
minifactory hardware to verify its utility in a real-world unknown environments for mapping,” in JEEE Int’l
system. Conf. on Robotics and Automation, 2000.

[5] H. Choset and P. Pignon, “Coverage path planning:

5 Conclusion The boustrophedon decomposition,” in Intl. Conf. on
Field and Service Robotics, 1997.
In this paper, an algorithm DC has been presented
with which a team of independent robots can cooper-
[6] Z. J. Butler, A. A. Rizzi, and R. L. Hollis, “Contact
sensor-based coverage of rectilinear environments,” in
atively cover their shared environment. It comprises Proc. of IEEE Int’l Symposium on Intelligent Control,
a reactive coverage algorithm CCry which operates Sept. 1999.
without explicit knowledge of cooperation and two ad-
ditional components (the feature handler and the over-
[7] A. A. Rizzi, J. Gowdy, and R. L. Hollis, “Agile assem-
bly architecture: An agent-based approach to mod-
seer) which maintain cooperative relationships with ular precision assembly systems,” in Proc. of IEEE
other robots to increase efficiency of the team. This Int'l. Conf. on Robotics and Automation, pp. 1511-
1516, April 1997.
decoupling of coverage from cooperation enabled the
completeness proof of DCR presented in this paper, [8] B. R. Donald, J. Jennings, and D. Rus, “Informa-
demonstrating that any team of square robots with in- tion invariants for distributed manipulation,” Jnter-
trinsic contact sensing can successfully cover a finite national Journal of Robotics Research, vol. 16, no. 5,
pp. 673-702, 1997.
rectilinear environment efficiently.
[9] A. Drogoul and J. Ferber, “From Tom Thumb to the
Acknowledgments dockers: Some experiments with foraging robots,” in
From Animals to Animats IT, pp. 451-460, MIT Press,
1993.
The authors would like to thank Howie Choset and
Ercan Acar for helpful discussions about coverage. [10] P. Stone and M. Veloso, “Task decomposition, dy-
This research was funded in part by NSF grant DMI- namic role assignment and low-bandwidth communi-
cation for real-time strategic teamwork,” Artificial In-
9527190. Zack Butler was supported in part by an NSF
telligence, vol. 110, pp. 241-273, June 1999.
Graduate Research Fellowship.
[11] D. W. Gage, “Randomized search strategies with im-
perfect sensors,” in Mobile Robots VIII, pp. 270-279,
References 1993.
[1] Y. S. Suh and K. Lee, “NC milling tool path gener- [12] N. Rao, V. Protopopescu, and N. Manickam, “Coop-
ation for arbitrary pockets defined by sculptured sur- erative terrain model acquisition by a team of two or
faces,” Computer Aided Design, vol. 22, no. 5, pp. 273— three point-robots,” in Proc. of IEEE Int’l. Conf. on
284, 1990. Robotics and Automation, pp. 1427-1433, April 1996.
[2])toa}V. Lumelsky, S. Mukhopadhyay, and K. Sun, “Dy- [13] I. Rekleitis, G. Dudek, and E. Milios, “Multi-robot
namic path planning in sensor-based terrain acqui- exploration of an unknown environment, efficiently re-
sition,” IEEE Trans. on Robotics and Automation, ducing the odometry error,” in Int’l Joint Conf. in Ar-
vol. 6, no. 4, pp. 462-472, 1990. tificial Intelligence, (Nagoya, Japan), pp. 1340-1345,
August 1997.
[3] S. Hert, S. Tiwari, and V. Lumelsky, “A terrain cov-
ering algorithm for an AUV,” Autonomous Robots,
vol. 3, pp. 91-119, 1996.
 : Tee

ae
" baue ata *

‘ \ 7 v ' a '
7 . Pelee wrsar a
BAH law oath A
iD esi ~e
oo” beshvoubnyRRP OGISIC. sueldtio al
i ae ae ao 10 iad poor eb eseg on
Ld ae enrqmncy fl jsut
in ‘piguate eine
a mmileror dai ygih¥> rcp
she FD bia cut habih socal Ya
sabres
pa eae *

it ai Ly 4ha Pit.

: ,
tee iat y
* i if

a i 7 ey —)
% rnd © bas tieeernd -A ae
Y ONYoIaiIKTee mires wihtycbh ———
7d Bartdah oi paynink saver
“ pad 4 fey fifties! . a Pe en
tT ee beats Hie ls os fos
rié ata e pa: Lilt “tins t iT t- coat yey ms ~~ iD Wcad ot , " ”

Heinen nnyrsnieleg sinkd) Co. PE top a eat mT i? abw datune

: nea). | 381,0% ourei) tear re pia Ad, wa) wey ' . ; -_ta 4 mm abet ee
which OTS ES et OEY levy oil in) ae uF sh Tei praepegayy bine talsued
doa
- tics /: ol
ue , eS sg he
ad cere t ap ooh lw fate ae Prin Wt
bideMeai "sere saclay fr
: si ide
“are ines
hai
: iy in© Matin, 14 tiny
Mempomiiernton! © hae
ty Pl Si
aa Mv Aust sf w 1g an” sa ne ws geitgs ea it ena
FE ef sifort La: iy testy4s ‘hy Ni dedly hits HH Paha fh etA
eea ee recy ei ‘o> Bi Petit eel.
Ge sie’ nipass Redaansine
eT saa i a APT be heigl noitentatlyate ~<
a Mi ie as vd 7 Palgtt - ita? tt T tefl yr fiat De mc ah oil }
Pte)ced Wainy: al v's
‘vil Hees Tye ie ” Slams eran: ae
+ )
re Seariren OF AR outa, ae wpa sft> ive ea A, Baa
|
~~ lean ree
:
PAG MplsAla wtrgy
PUL, Wes fatenved |)
Ad —ana ema aii pee
THE reseed
‘3 Ce hia, 5
— = ao
: a rk7 Oise Vo 54 j : Presta Se ae on
Peay [POF lam mek hi bg mg : my
Ch@ @tyv| Pasnew Hy Hie . bs
iy te rie corey helmde
ie ae ' us dit ‘
. i kas ‘ , #7 i
Phe any i hei i
i Shi
Closed-Loop Distributed Manipulation
Using Discrete Actuator Arrays
Jonathan E. Luntz, University of Michigan, Ann Arbor, MI
William Messner, Carnegie Mellon University, Pittsburgh, PA
Howie Choset, Carnegie Mellon University, Pittsburgh, PA

Distributed manipulation systems induce motions on While supporting the object, each cell provides a trac-
objects through the application of many external forces. tion force on it, and the combined action of all the cells
An actuator array performs distributed manipulation supporting the object determines the motion of the
using a planar array of many small stationary actua- object. Current applications of actuator arrays range
tors (which we call cells) each of which applies a force from micromechanical systems transporting pieces of
in the plane to larger objects which rest atop many silicon wafer to macroscopic arrays of motorized wheels
cells at once. An actuator array can transport and transporting cardboard boxes.
orient flat objects in the plane. The authors have de-
Actuator arrays represent a recent development in
veloped a table-top scale actuator array consisting of
robotic manipulation. Typical work addresses the task
many motorized wheels. In such a macroscopic array,
of bringing an object to a particular position and orien-
a fairly small number of cells support the object. The tation using open-loop modes of operation. The action
work of the authors builds upon and extends the work of the cells is pre-programmed and constant, establish-
of researchers in microelectromechanical (MEMS) ac- ing a force field in which the object moves [1, 2]. These
tuator arrays by explicitly modeling the discreteness in analyses considered microelectromechanical (MEMS)
the system, including the set of supports, distribution scale applications, where the large number of actua-
of weight, and generation of traction forces and by us- tors justifies the assumption that the forces from the
ing the resulting model to directly design classes of ac- discrete actuators are a continuous force field applied
tuation fields (sets of wheel speeds). Using a constant over the area of the object. Under this assumption, re-
(open-loop) wheel velocity field, discreteness causes un- searchers used potential field theory to predict motions
desirable behavior such as unstable rotational equilib- and stable poses of objects.
ria, suggesting the use of object feedback. Discrete
In this work, we examine a macroscopic actuator
distributed control algorithms are derived by inverting
array, the Modular Distributed Manipulator System
the dynamics of manipulation (the relationship between
(MDMS). Each cell of the MDMS is made up of a
wheels speeds and forces on the object) to come up
pair of motorized roller wheels which together can ap-
with wheel velocity fields which effect the desired forces
ply a directable traction force to objects such as card-
and moments on the object. These algorithms reduce
board boxes. The MDMS uses distributed control and
the many-input-three-output control problem to a three-
therefore is fully programmable. An 18 cell prototype
input-three-output control problem. Using these algo-
MDMS has been built and tested. On the MDMS, as
rithms, the authors demonstrate the stability of closed-
few as four or six cells support an object such that
loop discrete distributed manipulation.
continuous approximations fail and we must account
for discreteness when analyzing the MDMS.
1 Introduction
Discreteness causes imprecision in open-loop manip-
An actuator array performs distributed manipulation ulation, particularly in the orientation of objects. For
where many small stationary elements (which we call example, under certain modeling assumptions appro-
cells) cooperate to manipulate larger objects. Objects priate to the MDMS, the net moment acting on an ob-
lie on a regular array as they are transported and ori- ject is not a direct function of the object’s orientation,
ented. Many cells support the object simultaneously, and orientation can only be done to cell resolution [5].
and as it moves, the set of supporting cells changes. In addition, some objects having stable rotational equi-
64 J. E. Luntz, W. Messner, and H. Choset

libria on a continuous array have unstable rotational

equilibria on a discrete array [4]. Therefore, closed-
loop object control is necessary to precisely orient ob-
jects. In a closed-loop mode of operation, the action of
the cells updates based on the object’s motion. Feed-
back may be obtained from local sensing at each cell or
from some global sensor, such as a vision system. Other
work has been done in closed-loop distributed manipu-
Figure 1: Flezible supports act as springs to distribute the
lation [6] using vibrating plates to generate continuous
force fields acting on small (point) objects rather than load.
pointwise forces acting on large objects.
equations combined with n instances of this compati-
This paper focuses on the derivation of closed-loop
bility constraint form a set of n+3 equations and n+3
manipulation strategies for the MDMS. Section 2 re-
unknowns from which we can solve for N as a function
views the discrete dynamics of manipulation on the
of object position [5].
MDMS from the previous WAFR conference. Section 3
derives closed-loop policies by inverting the dynamics.
Section 4 analyzes the dynamics and stability of closed-
loop manipulation. Concluding remarks are made in
Section 5.
where the matrix B contains the positions of the cells
currently supporting the object.
2 Dynamics of Manipulation
(eee
As the MDMS manipulates an object, the object passes B= BA ven Lg, (2)
from one set of supporting cells to the next. To under- Y1 +++ Un
stand the motion of the object, we examine the planar
dynamics of an object while it rests on a single arbi- The horizontal forces on the object are derived from
trary set of cells. To compute the traction forces on the normal forces through the use of a viscous-type
the object from each wheel, we first compute the sup- friction law (see Figure 2). The horizontal force from
porting (normal) forces and then apply a viscous-type
each cell Fi is proportional to a coefficient of friction p,
friction law.
that cell’s normal force N;, and the vector difference
Consider an object of weight W, whose center of between the velocity of the wheel and the velocity of
mass is located at Xo, = [| Tem. Yem |1 resting on the object at the point of the cell.
n cells. The object is supported by vertical normal
forces N = [ Ny N,, | whose determination re-
quires consideration of equilibrium of the object in both
fix=u(V-Xonto| 99](R-Xm))M @
the vertical (z) direction and in rotation about the x
where w is the rotation speed of the object. This trac-
and y axes. Equilibrium provides three equations, and
tion force from each cell is summed over all the cells.
since we have n supports, the system is statically in-
determinant, and we must consider flexibility in the
system.

We assume each support is a linear spring. Physi-

cally, this flexibility is either a flexible suspension under
each wheel as shown in Figure 1 or, as in the prototype,
flexibility in the surface of the bottom of the object.
We assume the bottom of the object is flat such that
the spring deflections (and hence the normal forces)
distribute linearly under the object. The 3 equilibrium Figure 2: Interaction between wheel and object.
Closed-Loop Distributed Manipulation - Discrete Actuator Arrays
65

Define a wheel velocity matrix V as 3 Position and Orientation Feedback

vf Teck]-m mo
Pale Vana, + + Our strategy to implement position and orientation
feedback is to apply velocity fields which reduce the
array to a single “virtual” actuator capable of gener-
Summing vectorially, the net horizontal force is ating a desired force and torque on the object. Since
force and torque change as the object moves, we must
Pea VON TS UX on W (5) continuously recompute and adjust the wheel speeds.
We assume sensing of the object’s position and orienta-
Observe that the net horizontal force is not a func- tion is done externally, for example, by a vision system.
tion of the object’s rotation speed - the terms multiply- It is desirable to distribute the computation to reduce
ing w are identically zero [5]. Furthermore, the second communication such that each cell need only be aware
term in this equation is a constant linear damping term. of the desired net force and torque and its location rel-
Substituting N from Equation 1 yields ative to the center of the object, and not the state of
each cell.
0 0
f = p»WVBT (BBT) | 1 0 |Xen 3.1 Applying Both Force and Torque
0 1 We compute velocity fields by inverting the relation-
ks ship between wheel speeds and net force and torque.
As a first attempt, we specify both a force and torque
=] 1 eS
to compute the velocity field. Without loss of general-
+ WVB" (BB) 0 |—“uXemW (6)
ity, we set the origin of the system to lie at the center of
0
Seas re the object such that the cells move rather than the ob-
a. ject. Also, we ignore the damping terms in Equations 6
and 7. For the purpose of feedback control, since the
The 2 x 2 spring constant matrix k,, and the offset damping is mainly a function of the object’s motion,
force vector f, are constant while the object rests on a we treat damping as a property of the manipulated ob-
particular set of supports. ject rather than the actuators. The expressions for the
We compute the net torque similarly [5] resulting action of our virtual actuator (i.e. the net force and
in the following expression for the net torque acting torque on the object) reduce to:
on the object as a function of position and rotational 0 0
speed (and set of supports). f =f, =»WVB" (BBT)"'| 1 0 | and
1 00 om
7 =yWRB'(BB') *|0|+pWRB(BB') [10] Xem (8)
0 01 fi
7 =T) = )W RB! (BB‘) ” |0
0
>
To Ks,

tex (f+, Xe) —w(€NT-W RE, Xem) (7) We can rewrite these equations in terms of the
stacked velocity vector formed by stacking the trans-
where R; = X;xV; and %; = XX; = (x? +47). The poses of the z and y component rows of V.
vector ks, relates torque to position, and 7, is a scalar io.
constant torque. The net applied torque and damp- i a UW i

ing are both effectively position dependent. The term To

multiplying w is always negative, and hence dissipative

100/000j000
(although not constant). Note that while an object 000|1 00000
rests on a particular set of supports, torque on the ob- d00}000[100
ject is not a function of orientation. This is important
for determining stable orientations.
66 J. E. Luntz, W. Messner, and H. Choset

yr y
oa
Fa (9 )
eS SE See Oe ee ee a ee

where D, and Dy are diagonal matrices containing the OF beh ed ee ee ee

x and y components of the cell positions. We pseudo- x

ae en ee
~ ee

invert this underconstrained set of equations to solve

for the stacked velocity vector.
dh Rr ee te ah
vi Ltt) eet p\-1 os
GTt|
ve =A
pW (QQ
( ) fo,
i (10)
Figure 3: The uniform field applies an arbitrary net force
Unfortunately, it is not possible to algebraically reduce on an object with no net torque.
qr (QQ™)™* to a more useful form. Each cell’s wheel
speeds depend on the positions of all cells currently
supporting the object. Therefore, while a field com- Figure 3 shows the resulting uniform field. Our
puted in such a manner will provide the desired force methodology ensures that the net force is independent
and torque, its computation cannot be distributed, and of the set of supports such that we can apply the same
a centralized controller must give speed commands to net force without knowing which cells the object rests
each cell. This is not practical for a large system be- on. The velocities of the cells are decoupled, and are
cause of bandwidth limitations. only functions of the net force to be applied. This field
easily operates under distributed control, where a cen-
3.2 Superposition of Fields tralized controller need only broadcast the desired net
Because traction force from each cell varies linearly force to all the cells.
with wheel speeds, net forces and torque add with the This uniform field can be shown to apply no torque.
superposition of velocity fields. Therefore, we adopt While it may seem intuitive that this be true, it is
the strategy of superimposing two fields: a field to ap- not trivial. If we specified each cell’s force rather than
ply a force without a torque and a field to apply a wheel speeds, there would be a net torque when the ob-
torque without a force, where each field operates un- ject centroid and the centroid of the supporting cells
der distributed control. did not coincide. Since the traction force from each cell
To compute a field to apply a force with no torque, depends on the supporting force, however, the distrib-
we invert the relationship between wheel speeds and ution of weight ensures that all moments balance. The
net force. By using the Penrose pseudo-inverse, we algebraic proof is omitted.
obtain the velocity field which provides the desired
The same methodology used to compute the uni-
net force with the minimum sum of squares of wheel
form field applies to the application of an arbitrary
speeds. This minimizes extraneous motions, and hope-
torque. The expression for the net torque in terms of
fully (with no guarantee) the resulting field generates
no net torque.
the stacked velocity vector is

The expression for the net force in terms of the

To = uW [1 0 0](BB7)
ad Ve
stacked velocity vector is B | -Dy|Da Fr (13)
for ae 11 0.000 BB") Bl 0sxn Vr
bs EW) oo 0lL00 Osun (BBT)- F|
Vy
(11)
We pseudo-invert this underconstrained set of equa-
tions to solve for the stacked velocity vector.

Flan omR) fe os
We pseudo-invert this underconstrained set of equa-
tions to solve for the stacked velocity vector.

Vey (Bt
Va pW On
This expression can be algebraically reduced since D,
and Dy are diagonal matrices. After some algebra, the
Closed-Loop Distributed Manipulation - Discrete Actuator Arrays
67
aL
’ ’ ? , , we = 3
This expression, in general is not equal to zero and gen-
erates an extra force which was not intended by this
ee field. This disturbance force (fa) is dependent on the
TARReee
it Fe oe
AS See ; application of torque. While it can be shown that there
LSS JAN ee Sd Reale Xx
| are many positions and orientations at which this dis-
@ a 4 4 a se - a

turbance force vanishes [3] and its effect may be small,

« 8 a a x 2 > = we desire an alternate method for generating a torque
without generating a disturbance force.
Figure 4: The computed rotational field applies an arbi-
trary net torque on an object.
3.3. Known Torque Direction Method

To derive a field to generate a torque with no force,

wheel velocities decouple and the velocity of each wheel
becomes we exploit the distribution of weight among the cells
1 vi to compute wheel velocities in such a way that no net
force is applied. Rotational equilibrium of the object
iti (15) about the x and y axes ensures that the normal forces
are distributed such that

Figure 4 shows the resulting computed rotational

field. The vector formed by each cell’s two wheel speeds o=| z Ay ih fe] mM
(17)
Y1 Un Nn
is perpendicular to its position relative to the center
of the object with a magnitude inversely proportional
to distance. Again, our methodology ensures that the We can represent the net force on the object as
net torque is independent of the set of supports. This
V, Ray; Ny;
field also operates under distributed control, where a
centralized controller need only broadcast the desired f=n| ae
if, WE aeOSV | N, (18)
torque and current object location to all the cells.
This rotational field, however, has two problems. These two equations are of the same form with the
First, since the wheels speeds are inversely propor- position matrix replaced by the velocity matrix. If we
tional to their distance from the object center, some set the wheel velocity matrix to match the position
wheel speed commands may become arbitrarily large, matrix, the net force will be zero. In addition, we can
and the wheel speeds will saturate. In practice, only swap the two rows of the velocity matrix and negate
one cell will be close enough to the object center to the top row while maintaining zero force to generate
saturate, and its contribution will be limited. the following velocity field.
The second problem is that a net force is applied
V;. = -ay; and Vi, = 0%; (19)
by the rotational field. We obtain the expression for
the net force by substituting Equations 15 into Equa- where a is a constant with which we can scale the
tion 11. Considering the x component of Equation 11 field. This is a kinematic rotational field since the
(the y component is handled similarly), the expression wheel speeds vary linearly with position as they do in
for the net force is rigid body rotation as shown in Figure 5. This field is
1 = decoupled and operates under distributed control. The
pW xit+y;z net torque this field applies is
fo, = »W [100] (BBT)'B
Pe ss ie NE(y? + <7) (20)
> Yi

This torque is not constant; it changes with both ob-

wee? |(16)
[100]] os doa? Soaiys||X nes ject position and set of supports. However, because
yu ae
ay: Dy? each cell contributes positively to the summation of
68 J. E. Luntz, W. Messner, and H. Choset

function from position reference input z, to the ob-

ject’s position @,, under proportional gain Ky, is

a « >

Xem(8) is Re
X,(s) ms? + upWs + Ky, (21)

(Actually, a pair of transfer functions exist, one for

xz and one for y.) The closed-loop transfer function
from the disturbance input, fa, (from the computed
rotational field), to object position is
Figure 5: The kinematic rotational field applies a torque
Aen 8) 1
of known direction without a net force. a (22)
Fy,(s) ms? +pWs+t Ky,
torques, the direction of the torque is known. There-
fore, we eliminate the disturbance force generated by In the case of the kinematic rotational field, the po-
the computed rotational field at the expense of uncer- sition loop is independent of the orientation loop and is
tainty in the applied torque magnitude. This field is already stable. In the case of the computed rotational
still useful for feedback by setting a constant a and field, the position loop (which is a stable linear time
multiplying Equation 20 by the desired torque. invariant system with a disturbance) is asymptotically
stable as long as the disturbance asymptotically ap-
proaches zero. Assuming the orientation loop is itself
4 Closed-Loop Dynamics and Stability
asymptotically stable, the orientation error (and there-
Given these three superimposable manipulation fields, fore the torque command) asymptotically approaches
we can now implement separate proportional control zero. The disturbance force asymptotically approaches
feedback loops for position and orientation as shown zero since it varies in proportion to the torque com-
in Figure 6. The two loops are completely independent mand. Therefore, the entire manipulation dynamics
except for the disturbance force generated by the orien- are asymptotically stable as long as the orientation loop
tation loop in the case of the computed rotational field. is itself asymptotically stable.

Because the force applied by the uniform field is pre-

4.1 Rotational Stability Under The Computed
cise, and the damping term from Equation 6 is constant
Rotational Field
and linear, the position loop has simple linear second
order time invariant dynamics. (The other terms in The closed-loop rotational dynamics under the com-
Equation 6 are identically zero because the origin of puted rotational field have a linear form, with mass,
the field follows the object.) The closed-loop transfer spring, and damping coefficients, but the damping is a
function of the set of supports and the object’s trans-
lational position. The computed rotational field is
such that the torque applied is precise (ignoring sat-
Es PositionDynamics
e,°8 . een

uration) and with a proportional gain of K; is equal

to K,(6, — 0). The rotational damping coefficient
from Equation 7 is simply uXN Tor equivalently,
>>Ni (a? + y?), which refer to as b(Xem(t),0). The
cna ———
other terms (and the second portion of the damping
coefficient) in Equation 7 are identically zero since the
origin of the field follows the object. The rotational
dynamics of the object under the computed rotational
Figure 6: Feedback control structure. Superimposable fun-
field therefore are
damental fields allow separate position (top) and orienta-
tion (bottom) loops.
JO + b(Xem(t); 0)6+K,0 = K,6, (23)
Closed-Loop Distributed Manipulation - Discrete Actuator Arrays
69

Figure 9: The orientation loop for the computed rota-

Figure 7: Rotational damping is a sector nonlinearity tional field is a second order system with (nonlinear) vari-
where rotation speed (w) maps to damping torque (Ta). In able damping coefficient b(Xem(t), 0).
this ecample, the nonlinearity lies in the sector (0,2).
nonlinearity is a function 6(t,r) such that kjx? <
®(t,x) < kgx? for all t > 0. The damping sector
nonlinearity is a variable gain, ky < b(Xem(t),) < ke
such that (¢,6) = $(Xem(t),0)0. Define the disk
D(k,,k2) as the circle he. center lies on the real
axis and crosses the real axis at a5 and ores as shown
by the circle in Figure 13. The circle criterion states
Figure 8: Standard form of a system to be analyzed by the
that a system of the form shown in Figure 8 with a
circle criterion
linear system G(s) with p unstable poles connected in
feedback by a sector nonlinearity, @(t, x), will be glob-
The damping coefficient b(Xem(t),0) is not a func- ally asymptotically stable if the Nyquist plot of the
tion of the wheel speeds, but it is a function of the forward-path linear system encircles the disk D(k1, kz)
position of the object and the set of supports (and im- exactly p times counter-clockwise.
plicitly the orientation). The coefficient changes dis- Under the computed rotational field, the orientation
continuously, but is bounded both above and below. loop readily transforms into the standard form of a
This nonlinearity is deterministic, but is time-varying system to be analyzed by the circle criterion. For zero
since it depends on the object position (which is inde- reference, the block diagram in Figure 9 is equivalent
pendent of orientation). to the form in Figure 10. It can be shown that the
While it is difficult to come up with good upper and Nyquist plot of the forward path always lies in the
lower bounds on the damping coefficient, we can base right half plane and cannot enter or encircle the disk
extremely conservative bounds on the facts that the D(k1,k2). Since there are no unstable poles in this
object is of finite size (contained in a circle of radius forward path, by the circle criterion, the closed-loop
Tmax) and that all of the supporting forces are positive. orientation dynamics are globally asymptotically sta-
These limits ensure that the damping force is a sector
nonlinearity (lying in a sector (k;,k2) such that ki <
b(Xem(t),0) < k2). Figure 7 shows an example of such
a sector nonlinearity.
Figure 9 shows the nonlinear orientation feed-
back loop for the computed rotational field. State-
dependent damping makes the orientation loop truly
nonlinear, although it is instantaneously linear such
that we may perform some block diagram manipula-
tions such as passing gains through the nonlinearity
and combining the nonlinearity in parallel with itself. Figure 10: The rearranged orientation feedback loop under
the computed rotational field has linear forward path and the
We can show that this loop is stable using the cir-
sector nonlinearity b(Xcm,9) in feedback.
cle criterion for the sector nonlinearity [7]. A sector
70 J. E. Luntz, W. Messner, and H. Choset

Figure 11: Using the kinematic rotation field, a second sector nonlinearity, atr(Xem, 9), is introduced to the orientation
feedback loop.

ble under the computed rotational field for all values

of proportional gain. Since this loop is asymptotically
stable, the entire dynamics will be stable.

4.2 Rotational Stability under the Kinematic

Rotational Field

Figure 11 shows the orientation loop using the kine-

matic rotation field. Because the applied torque (Equa-
tion 20), is not precise, an additional nonlinearity is in-
troduced from the desired torque to the applied torque.
The applied torque varies from the desired torque by
a state dependent gain which we call a;(Xem,6). This
coefficient has the same form as b(Xem,0) within the
multiplicative constant a. Since there are now two sec-
tor nonlinearities, it is more difficult to manipulate the
Figure 12: The block diagram for the orientation feedback
block diagram to the proper form to apply the circle
loop with the kinematic rotational field can be rearranged
criterion.
to apply the circle criterion. The two nonlinearities are in
Figure 12 shows the rearranged version of this loop. parallel (left) and since they have the same form, can be
To rearrange the loop, assume the input is zero and combined (right).
combine the gain Kg with the integrator block. Since
the two nonlinearities differ only by a multiplicative
constant, a, factor out a and define Kg = akg. The transformations.) Therefore, to satisfy the circle crite-
two nonlinearities (one with the integrator block), now rion, the Nyquist plot must encircle the disk D(kj, ko)
twice.
called ¢(Xem,0), are in parallel. Since ¢(Xem,9) is
a pointwise-linear state-dependent time-varying gain,
combine the two parallel paths to form the block dia- Figure 13 shows the Nyquist plot of G’(s). The
gram at the right of Figure 12. The transfer function solid portion of the Nyquist plot follows the function
of the forward path is now yeh, [Kr where x and y refer to the real and imag-

s+ Ko inary components of G(s). The dashed portion of the

G"(s) = 5 (24) Nyquist plot loops around counterclockwise at infinity.
This figure shows that given values of k; and ko there
Since G’(s) has two poles at the origin, we circumvent is a range of gains K% for which the disk D(ky, ko) is
the origin by passing the Nyquist contour to the right encircled twice. The square root curve shifts inward
which treats these two poles as unstable. (Alterna- as the gain is increased, and after a certain gain, the
tively, one could shift the two poles slightly away from Nyquist plot enters the disk and we can no longer guar-
the origin by applying appropriate block diagram loop antee the closed-loop system to be stable.
Closed-Loop Distributed Manipulation - Discrete Actuator Arrays
al

Therefore, determining bounds k; and ky on

$(Xem;@), allows the selection of a gain to guarantee
global asymptotic stability of the rotational closed-loop
system under the kinematic rotation field. It is inter-
esting to examine limiting cases for these bounds. In
the limit where kj = ky, the denominator in Equa-
tion 26 goes to zero, and the system will be stable at
any gain. This is expected since this is simply the
linear case. In the limit where ky >> k,, the bound be-
comes Kp < thn which may be a very small number.
Therefore, it is important to have good bounds on the
nonlinearity to allow a reasonable range of gains.

We do not_ currently have good bounds on

’(Xem,9) = pXN7, although we know finite bounds
exist. The bounds on this nonlinearity are functions of
the size and shape of an object. For a given object, we
could obtain exact bounds by computationally moving
the object over its entire configuration space (which
repeats on a regular array).

Alternatively, we could obtain better bounds with a

Figure 13: Application of the circle criterion to the orien-
more general, geometric analysis for a class of objects
tation loop using the kinematic rotational field. The circle
(namely rectangular objects). This would require an
represents the disk D(ki,k2) where ki and kz provide the
analysis of the discrete sampling of the object’s shape
bounds on the sector nonlinearity. The solid and dashed
over its range of motion, which is quite difficult in itself,
path shows the Nyquist plot in the complex plane of the lin-
and not within the scope of this work. Better bounds,
ear forward path. The dashed portion of the path represents
however, would allow for the use of higher gains while
an encirclement at infinity. The solid portion of the path is
guaranteeing stability. Also, the circle criterion itself
of the form of a square root. Note that D(ki,k2) is encir-
only provides a sufficient condition for stability. We
cled twice. believe that the closed-loop system under the kinematic
rotation field is stable at all gains.
Rather than solve the cubic equation to determine
the range of gains for which the Nyquist plot does not
enter the disk, we can obtain a sufficient condition for 4.3. Closed-Loop Simulation
asymptotic stability by guaranteeing that the Nyquist
plot will not enter the square circumscribing the disk Figure 14 shows simulations of the closed-loop system
under both rotational fields. Under the computed rota-
(shown by the dotted square in Figure 13). The upper
tional field (left) the object reaches equilibrium exactly
right corner of the square is located at
at both the desired position and orientation. Since
this is approximately a set of mass-damper systems,
Tee Colas a ko — ki .
(25)
ie 8te
25k", fae eee
by adjusting the gains, we can adjust the overshoot
and settling time of each component of the closed-loop
response. The curvature of the path of the center of
This point lies inside the square root portions of the the object (which otherwise would have been straight)
Nyquist plot if the following condition holds. demonstrates the effect of the disturbance force. Under
the kinematic rotational field (right) the object moves
by. kaka 2 Ak,ke : in a straight line to the equilibrium position, and the
Ki ~ 2kike (26) rotational motion is greatly unaffected by the torque
(kz — kx) uncertainty.
72 J. E. Luntz, W. Messner, and H. Choset

back methods apply. Because of the design methodol-

ogy used, these fields eliminate the dependence of the
dynamics on the set of supports. Because of the use
of distributed control, limitations are present in these
strategies in the form of coupling from torque to force,
or in the form of torque uncertainty. We proved that
these limitations do not affect closed-loop stability nor
do they hinder precision of equilibria, but they may
affect closed-loop performance.
Our future efforts will be toward performance guar-
antees for closed-loop manipulation using distributed
control obtained by generating good bounds on the dis-
crete nonlinearities and applying robust control meth-
ods. We will also extend the feedback methods pre-
sented here to include not only manipulation to a single
position and orientation, but also path and trajectory
following with multiple objects simultaneously.

References

[1] K. Bohringer, B. Donald, R. Mihailovich, and N. Mac-

Donald. A theory of manipulation and control for mi-
crofabricated actuator arrays. In Proceedings, IEEE
International Conference on Robotics and Automation,
1994.

[2] L. Kavraki. Part orientation with programmable vector

fields: Two stable equilibria for most parts. In Proceed-
ings, IEEE International Conference on Robotics and
Automation, 1997.

[3] J. Luntz, W. Messner, and H. Choset. Discreteness

issues of manipulation using actuator arrays. In Work-
shop on Distributed Manipulation, IEEE International
Conference on Robotics and Automation, 1999.
Figure 14: Simulation of the closed-loop system under the
[4] J. Luntz, W. Messner, and H. Choset. Open loop ori-
computed rotational field (top) and the kinematic rotational
entability of objects on actuator arrays. In Proceed-
field (bottom). The object is shown as the moving rectangle, ings, IEEE International Conference on Robotics and
with a curve tracing the motion of it’s center. Each cell is Automation., 1999.
marked with an ’X’.
(5
Pear J. Luntz, W. Messner, and H. Choset. Velocity field
design for parcel manipulation on the modular distrib-
uted manipulator system. In Proceedings, IEEE In-
5 Conclusions ternational Conference on Robotics and Automation.,
1999.

In this paper we have derived closed-loop control [6] D. Reznik, E. Moshkoich, and J. Canny. Building a
strategies for manipulating objects. While more com- universal planar manipulator. In Proceedings, Work-
plicated to implement than open-loop control, these shop on Distributed Manipulation at the International
Conference on Robotics and Automation, 1999.
strategies address precision limitations apparent in the
open-loop operation of discrete actuator arrays. We [7] M. Vidyasagar. Nonlinear Systems Analysis. Prentice-
derived a class of fields which reduce the array of actu- Hall Inc., Englewood Cliffs, NJ, 1978.
ators to a single virtual actuator for which simple feed-
Kinematic Tolerance Analysis with Configuration Spaces
Leo Joskowicz, The Hebrew University, Jerusalem, Israel
Elisha Sacks, Purdue University, West Lafayette, IN

This paper is a survey of our research on kinematic the analysis. These limitations cause significant risks
tolerance analysis of mechanical systems with paramet- and uncertainties despite large analysis efforts.
ric part tolerances. We present a general algorithm
The major steps in tolerance analysis are tolerance
for planar systems and illustrate it with a design case
specification, variation modeling, and sensitivity analy-
study. The algorithm constructs a variation model for
sis. Tolerance specification defines the allowable varia-
the system, derives worst-case bounds on the variation,
tion in the shapes and configurations of the parts of a
and helps designers find unexpected failure modes, such
system. The most common are parametric and geomet-
as jamming and blocking. The variation model is a gen-
ric tolerance specifications [16, 25]. Variation model-
eralization of the configuration space representation of
ing produces mathematical models that map tolerance
nominal part contacts. The algorithm handles general specifications to system variations. Sensitivity analy-
planar systems of curved parts with contact changes, sis estimates the worst-case and statistical variations
including open and closed kinematic chains. It con- of critical properties in the model for given part varia-
structs a variation model for each interacting pair of tions. Designers iterate through these steps to synthe-
parts then derives the overall system variation at a size systems that work reliably and that optimize other
given configuration by composing the pairwise variation design criteria, such as cost and durability.
models via sensitivity analysis and linear programming.
We demonstrate the algorithm on a gear selector mech- The tolerancing objectives are to produce designs
that can be assembled and that function correctly de-
anism in an automotive transmission with 100 func-
spite manufacturing variation. In assembly toleranc-
tional parameters. The analysis, which takes less than
ing, general part variations must be modeled, so geo-
a minute on a workstation, indicates that the critical
metric tolerance specifications are the norm. Statis-
kinematic variation occurs in third gear and identifies
tical sensitivity analysis is appropriate because guar-
the parameters that cause the variation.
anteed assembly is more expensive than discarding a
few defective products. Most algorithms perform tol-
1 Introduction erance analysis on the final assembled configuration [2],
although recent research explores toleranced assembly
sequencing [12] and fixturing [4]. In functional toler-
This paper describes our research in kinematic toler-
ancing, the relevant part variations occur in functional
ance analysis of mechanical systems. The task is to
features whose descriptions are parametric. Paramet-
estimate the worst-case or average error in critical sys-
ric tolerances, which are simpler than geometric tol-
tem parameters due to manufacturing variation. This
erances, are best suited to capture these variations.
analysis plays a key role in improving design quality
Worst-case analysis is most appropriate because func-
and in reducing development time. In current practice,
tional failures that occur after product delivery can be
tolerance analysis is an imperfect, difficult, and time
unacceptable.
consuming activity. To keep the analysis affordable
and on time, only those aspects of a design that are pre- Our research addresses functional kinematic toler-
sumed to be critical are considered. These typically in- ance analysis. Kinematic tolerancing is the most im-
clude safety items, selected clearances, and areas where portant form of functional tolerancing because kine-
part interferences are expected. Unproven assump- matic function, which is described by motion con-
tions and simplifications are often made to speed up straints due to part contacts, largely determines me-
74 L. Joskowicz and E. Sacks

chanical system function. The task is to compute the

variation in the part motions due to variations in the
tolerance parameters. Variation modeling derives the
functional relationship between the tolerance parame-
ters and the system kinematic function. Sensitivity
analysis determines the variation of this function over
the allowable parameter values.
We illustrate kinematic tolerance analysis on an in-
termittent gear mechanism (Figure 1). The mechanism
consists of a constant-breath cam, a follower with two
pawls, and a gear with inner teeth. The cam and the
gear are mounted on a fixed frame and rotate around
their centers; the follower is free. Rotating the cam
causes the follower to rotate in step and to reciprocate
along its length (the horizontal axis in the figure). The
follower engages a gear tooth with one pawl (snapshot
a), rotates the gear 57 degrees, disengages, rotates in-
dependently for 5 degrees while the gear dwells (snap-
shot b), then engages the gear with its opposite pawl
and repeats the cycle (snapshot c). The mechanical
function is conversion of rotary motion into alternate
rotation and dwell. The dwell time is determined by
the gear tooth spacing. Quantitative tolerance analysis
bounds the variation in the gear rotation, which is the
critical parameter in precision indexing. Qualitative
analysis detects failure modes, such as jamming, when
one pawl cannot disengage because the other prema-
turely touches the gear.
Creating a variation model is the limiting factor in
kinematic tolerance analysis. In most cases, the an-
alyst has to formulate and solve systems of algebraic
equations to obtain the relationship between the tol-
erance parameters and the kinematic function. The
analysis grows much harder when the system topology
changes, that is, when different parts interact at var-
ious stages of the work cycle, such as in the example
above. Contact changes represent qualitative changes
in the system function. They occur in the nominal
function of higher pairs, such as gears, cams, clutches,
and ratchets. Part variation produces unintended con-
tact changes in systems whose nominal designs pre-
scribe permanent contacts, such as joint play in link-
ages. The analysis has to determine which contacts
occur at each stage of the work cycle, to derive the
()
resulting kinematic functions, and to identify poten- Figure 1: Intermittent gear mechanism: (a) upper fol-
tial failure modes due to unintended contact changes, lower pawl engaged, (b) follower disengaged, (c) lower pawl
such as play, under-cutting, interference, and jamming. engaged.
Once the variation model is obtained, sensitivity analy-
Kinematic Tolerance Analysis
75
sis can be performed by linearization, statistical analy- tions between parts and studying their kinematic vari-
sis, or Monte Carlo simulation to quantify the variation ation [3]. Most commercial computer-aided tolerancing
in each mode [2]. systems include this capability for planar and spatial
We have developed a general kinematic tolerance mechanisms [24]. These methods are impractical for
analysis algorithm that addresses these issues [20]. The systems with many contact changes, such as a chain
algorithm constructs a variation model for the system, drive, and can miss failure modes due to unforeseen
derives worst-case bounds on the variation, and helps contact changes. Our method overcomes these limita-
designers find failure modes. The variation model is a tions by automating variation modeling and analysis
generalization of our configuration space representation for general planar systems.
of the nominal part contacts. The algorithm handles
general planar systems of curved parts with contact 3 Configuration Space
changes, including open and closed kinematic chains.
It analyzes systems with 50 to 100 parameters in un- We model nominal kinematic function within the con-
der a minute, which permits interactive tolerancing of figuration space representation of rigid body interac-
detailed functional models. tion. Configuration space is a general representation
for systems of rigid parts that is widely used in robot —
In this paper, we survey our research on kinematic
motion planning [11, 15]. We construct a configuration
tolerance analysis [9, 19, 20, 21]. The diverse prior
space for each pair of interacting parts in the mechani-
results are presented in an integrated manner with re-
cal system. The configuration space is a manifold with
alistic examples. We describe the configuration space
one coordinate per part degree of freedom. Interac-
representation, explain its role in tolerance analysis,
tions of pairs of fixed-axes planar parts are modeled
and outline the analysis algorithm for general planar
with two-dimensional spaces [18], whereas interactions
systems. We demonstrate the algorithm on an indus-
between general planar pairs are modeled with three-
trial application: Tolerancing a gear selector mecha-
dimensional spaces [17]. In both cases, points specify
nism with 100 functional parameters. We conclude
the relative configuration (position and orientation) of
with a discussion of future work and of applications
one part with respect to the other. We perform con-
in path planning with geometric uncertainty.
tact analysis by computing a configuration space for
each pair of parts.
2 Previous Work
Configuration space partitions into three disjoint sets
that characterize part interaction: Blocked space where
Previous work on kinematic tolerance analysis of me-
the parts overlap, free space where they do not touch,
chanical systems falls into three increasingly general
and contact space where they touch without overlap.
categories: static (small displacement) analysis, kine-
Blocked space represents unrealizable configurations,
matic (large displacement) analysis of fixed contact
free space represents independent part motions, and
systems, and kinematic analysis of systems with con-
contact space represents motion constraints due to part
tact changes. Static analysis of fixed contacts, also
contacts. The spaces have useful topological proper-
referred to as tolerance chain or stack-up analysis, is
ties. Free and blocked space are open sets whose com-
the most common. It consists of identifying a criti-
mon boundary is contact space. Contact space is a
cal dimensional parameter (a gap, clearance, or play),
closed set comprised of algebraic patches that repre-
building a tolerance chain based on part configurations
sent contacts between pairs of part features. Patch
and contacts, and determining the parameter variabil-
boundary curves represent simultaneous contacts be-
ity range using vectors, torsors, or matrix transforms
tween two pairs of part features.
[5, 26]. Recent research explores static analysis with
contact changes [1, 4, 8]. Configurations where unex- We illustrate these concepts on the gear/follower pair
pected failures occur can easily be missed because the of the intermittent gear mechanism (Figure 2). We
software leaves their detection to the user. Kinematic compute the configuration space of the gear relative to
analysis of fixed contact mechanical systems, such as the follower. The gear frame is at the center of the
linkages, has been thoroughly studied in mechanical outer boundary circle and its x axis is horizontal in
engineering [6]. It consists of defining kinematic rela- part a. The follower frame is at the center of the inner
76 L. Joskowicz and E. Sacks

square profile and its z axis is parallel to the pawls.

The configuration space coordinates are the position
(u,v) and the orientation ~ (in radians) of the gear
frame in the follower frame.
We first examine two configuration space slices where
the gear translates at a fixed orientation of ~ = 0 and
w = 1/32. The distance between these orientations is
maximal because the configuration space is ~-periodic
with period 7/16. Free space is white, blocked space
is grey, and contact space is black. At ~ = 0, the free
space consists of three connected components (the con-
figuration shown in the figure is in the middle one). All
part motions stay in the component containing their
starting configuration. The complex shape of the con-
tact space encodes the way that the gear can slide along
the follower pawls. At 7 = 7/32, the free space con-
sists of a single component. The contact curves have
changed because different gear teeth can touch the fol-
lower pawls at this orientation. The narrow channel
between the upper and lower regions is traversed when
the gear disengages one pawl and engages the other.
The configuration space of a pair is a complete repre-
sentation of the part contacts. Contacts between pairs
of features correspond to contact patches (curve seg-
ments in two dimensions and surface patches in three).
The patch geometry encodes the motion constraint and
the patch boundary encodes the contact change condi-
tions. Part motions correspond to paths in configura-
tion space. A path is legal if it lies in free and contact
space, but illegal if it intersects blocked space. Con-
tacts occur at configurations where the path crosses
from free to contact space, break where it crosses from
contact to free space, and change where it crosses be-
tween neighboring contact patches.
Figure 3 shows a detail of the three-dimensional con-
figuration space where the gear translates and rotates.
The labels a, b, and c mark the configurations dis-
played in Figure 1. The configuration follows the mo-
tion path (shown in grey) up (increasing u) along the
highlighted patch, into the page (decreasing v), then
left (increasing w). It then leaves the patch, crosses free
space (a black region), and enters another patch. (A
more detailed description of the indexing mechanism,
including a faulty variant, appears elsewhere [22}).
The configuration space representation generalizes
w= m/32
from pairs of parts to systems with more than two
Figure 2: Gear/follower configuration space slices. parts. A system of n planar parts has a 3n-dimensional
Kinematic Tolerance Analysis
17

which depend on the tolerance parameters. As the pa-

rameters vary around their nominal values, the contact
patches vary in a band around the nominal contact
space, which we call the contact zone. The contact
zone defines the kinematic variation in each contact
configuration: Every pair that satisfies the part toler-
ances generates a contact space that lies in the contact
zone. Kinematic variations do not occur in free config-
urations because the parts do not interact.
We illustrate contact zones on the gear/follower
pair. The gear is parameterized by the inner radius
r; = 15.7mm, the outer radius r, = 16.4mm, and
the ratio ~ = 0.3 between the angular width of a
tooth and the angular spacing between teeth. The fol-
lower is parameterized by the cam radius r. = 5mm
(which determines the size of the inner square), the
pawl thickness w = 0.5mm, and the length 1 = 9.8mm
Figure 3: Detail of the gear/follower configuration space. of the arms. The worst-case parameter variations are
The horizontal aris is the u translation, the axis into the +0.01mm. The full configuration space has a three-
page the v translation, and the vertical axis the w rotation. dimensional contact zone that is hard to visualize. In-
stead, we examine configuration space slices, which
configuration space whose points specify the n part have planar contact zones (Figure 4). The contact zone
configurations. A system configuration is free when is bounded by the curves that surround the nominal
no parts touch, is blocked when two parts overlap, and contact curves. Its width varies with the sensitivity
is in contact when two parts touch and no parts over- of the nominal contact configuration to the tolerance
lap. Computing the complete high-dimensional mech- parameters.
anism configuration space is impractical. Instead, we Each contact patch generates a region in the contact
construct the relevant system contact patches from the zone that represents the kinematic variation in the cor-
pairwise spaces. responding feature contact. The region boundaries en-
We have developed a configuration space compu- code the worst-case kinematic variation over the allow-
tation program for planar pairs whose part bound- able parameter variations. They are smooth functions
aries consist of line segments and circular arcs [17, 18]. of the tolerance parameters and of the part configura-
These features suffice for most engineering applications tions in each region. They are typically discontinuous
with the exception of involute gears and precision cams, at patch boundaries because the adjacent patches de-
which are best handled by specialized methods [7, 14]. pend on different parameters, as can be seen in the gaps
The program computes an exact representation of con- between adjacent contact zone boundary curves in Fig-
tact space: A graph whose nodes represent contact ure 4. The variation at boundary configurations is the
patches and whose arcs represent patch adjacencies. maximum over the neighboring patch variations. The
Each node contains a contact function that evaluates to contact zone regions represent the quantitative kine-
zero on the patch, is positive in nearby free configura- matic variation, while the relations among regions rep-
tions, and is negative in nearby blocked configurations. resent qualitative variations, such as possible jamming,
Each graph arc contains.a parametric representation of under-cutting, and interference. For example, the nar-
the boundary curve between its incident patches. row channel in the center of free space can close, which
prevents the follower from switching between pawl con-
4 Kinematic Variation tacts, because the contact zones of the channel sides
overlap.
We model kinematic variation by generalizing configu-
ration spaces to toleranced parts. The contact patches The contact zone is obtained from the parametric
of a pair are parameterized by the touching features, part models and the nominal contact patches. Each
78 L. Joskowicz and E. Sacks

where the partial derivatives of g are evaluated at

(u,v, %) and 6p; is the ith element of dp. This equation
approximates the portion of the perturbed patch near
the configuration (u,v, 7) with a plane.
The left side of Equation 2 specifies the normal di-
rection of the perturbed contact patch, which is inde-
pendent of the parameter variations. The right side
specifies the distance between the perturbed and the
nominal patch, which is the kinematic variation, for
any allowable parameter variation |; < dp; < uj with
1; <0 and u; > 0. The worst-case kinematic variation
(largest distance) occurs when the right side is maximal
or minimal. It is maximal when every term is maximal,
i.e., when Op; = |; when gp; > 0 and dp; = u; otherwise.
Switching u; and 1; yields the minimal value.

The derivatives 0g/Op; measure the sensitivity of the

contact configuration to the tolerance parameters. De-
signers can make small changes in the kinematic func-
tion by changing the parameters in accordance with the
sensitivities. The following table shows the sensitivi-
ties of the gear/follower pair to the design parameters
in the w = 0 slice.

parameter | maximum minimum average

(32 The values are from 84 contact configurations. Lin-

ear interpolation between these configurations approx-
Figure 4: Contact zones for gear/follower configuration imates the entire contact space to three significant dig-
space slices. its, hence the sample sensitivities are representative of
the entire space. The average pawl thickness sensitiv-
patch satisfies a contact equation g(u, v,~) = 0, which ity is four times that of the other parameters, which
we rewrite as g(u,v,¥~,p) = 0 to make explicit the makes it a good candidate for small design changes.
dependence on the vector p of tolerance parameters.
A parameter perturbation of dp leads to a perturbed The contact zone model generalizes from pairs to
patch that satisfies systems. The contact space is a semi-algebraic set in
configuration space: a collection of points, curves, sur-
g(u+ du, v + dv, + dv, p + dp) = 0. (1) faces, and higher dimensional components. As the tol-
Following the standard tolerancing approximation erance parameters vary around their nominal values,
the components vary in a band around the nominal
which considers only the first-order effects on kinematic
contact space, which is a higher-dimensional analog of
variation, we obtain the linear expression:
the three-dimensional contact zone of a pair. It is im-
Og
5, ouatt+ O98 il eh,xt roa
= ou + refi Og (2) practical to compute the full contact zone, so we sample
it at critical configurations. These configurations can
Kinematic Tolerance Analysis
79
be specified by a designer or can by derived by simu-
lating the system function and sampling periodically.
Several simulations may be needed to sample all the
operating modes of the system.
We compute the system variation at a sample config-
uration by determining which pairs of parts are in con-
tact, obtaining the corresponding parameterized con-
tact equations from the pairwise configuration spaces,
and solving a linear optimization problem. The vari-
ables are the part coordinate variations (52;, dy;, 60;)
and the tolerance parameters p;. The constraints come
from the tolerances and from the contact patches.
The tolerances provide two constraints per parameter
l; < pi < wi. We collect the contact equations into a
vector equation:
g(x, p) =0 (3)
with x part coordinates and p tolerance parameters.
We linearize the contact equations around the current
ie
configuration and the nominal parameter values to get: valve body

Dxgox + Dpgdp = 0 (4)

with Dxg the Jacobian matrix with respect to x and

Dpf the Jacobian matrix with respect to p. This equa-
tion is the system analog of Equation 2. It approxi- pin 2
mates the portion of the perturbed configuration space N

near x with a hyper-plane. Additional constraints

model driving motions, part play, and dynamical ef-
fects. The objective functions are the maxima and min-
ima of the coordinate variations. We solve one linear
Figure 5: CAD drawing of gear selector mechanism (top)
program for each function to obtain the system varia-
and cam/pin/piston assembly (bottom).
tion. The details and examples appear elsewhere [20].
of yet-to-be discovered tolerance problems can stop
5 Case Study: The Gear Selector production. Many tolerance problems reflect incor-
Mechanism rect simplifying assumptions where engineers ignored
an important feature or where the geometric complex-
We have applied the kinematic tolerance analysis pro- ity of the parts, their motions, and their interactions
gram to automotive power train design where the produced unexpected effects.
shortcomings of the current tolerance analysis practice
are acute [23]. The orders to purchase the transfer We summarize the analysis of the cam/piston assem-
lines, machines, and tools are placed when the tol- bly in a gearshift mechanism. Figure 5 shows the four
erance analysis begins. When the tolerance analysis main parts of the cam/piston assembly: the rooster
reveals a problem, modifications to the design often cam, the pin, the piston, and the valves body. There
require very expensive changes to the manufacturing are three moving parts, each with one degree of free-
process. Engineers must conduct a statistical analysis dom, that form two 2D configuration spaces. The cam
to determine if the probability of trouble is acceptable, rotates around an axis at its center. Its angular posi-
decrease the part tolerances, which increases costs, or tion is controlled by the shift stick (not shown). Its side
accept higher warranty costs and customer dissatisfac- pin is mounted on the piston’s left end and causes it
tion. During production, downstream consequences to slide back and forth inside the valves body, which is
80 L. Joskowicz and E. Sacks

fixed. The different piston positions open and close the

conducts on the valve. The pin, which is spring-loaded, 280,
temporarily locks the rooster cam in one of seven set-

rae
tings labeled 1, 2, 3, D, N, R, and P. Each of the cam
settings determines a nominal opening of the valves.
The angular position of the cam is determined by the
pin that pushes the lower cam profile. Variations in
the pin, piston, and cam shapes and positions affect
the piston displacement and thus the valve opening.
The kinematic tolerance analysis task is to determine
the maximum variation of the piston displacement for
each cam setting. It is also important to determine
which feature variations contribute the most to the
piston variation: the cam axial position, its profile, the
pin radius, or others. The many part features, complex 0
kinematic relations, and contact changes make manual -0.6 6
3
analysis impractical. 261.6
We obtained the nominal boundary representation
model of the gear selector cam subassembly from Ford.
We constructed a parametric model of the subassem-
bly by adding variation parameters to the functional
features of the parts. For the cam, we toleranced the
line segments that form the tooth sides, the small arc
segments that form the tooth tips, the arc segments
that connect the teeth, and the circular pin that en-
gages the piston. For the piston, we toleranced the
two vertical segments that are in contact with the cam
pin. For the pin, we toleranced the single, circular fea-
ture. Line segments were toleranced by varying the
coordinates of the two endpoints; arc segments were 260 A)
0.35 0.39
toleranced by varying the radius and the center coor-
dinates. To account for uncertainties in the position
Figure 6: Comb/piston configuration space and contact
of the rotation axes, we also toleranced the centers of zone detail.
rotation of the cam and the pin. Since we chose the
piston as the reference part of the assembly, there was
no need to tolerance the orientation of its translation worst-case variation of between 0.41mm and 0.45 mm
axis. The model has in total 86 tolerance parameters over the functional range of the cam angle, 6. In the
for the cam, 8 for the piston, 5 for the pin, and 99 cam/pin zone, the pin orientation, w, has a worst-case
overall. We assigned every parameter an independent variation of between 0.013 radians and 0.018 radians.
tolerance of +0.1 mm. Constructing the parametric We computed the kinematic variation of the system at
models and inputting them to the program took one the configuration @ = 0.371 radians, w = —0.0008 radi-
person two hours. ans, x = 260.8 mm where the gear selector is in third
gear (Figure 5). The worst-case variation of x is 0.9
To determine the kinematic variations, we computed mm—roughly half from each pair. The main factors
the nominal configuration spaces and the contact zones
in the cam/piston variation are the cam center hori-
of the cam/piston and cam/pin pairs, as shown in Fig-
zontal position (25%), vertical position (25%), tooth
ures 6 and 7. The computation took about 20 seconds, base x position (25%), and the x coordinates of the
using a Lisp program running on an Indigo 2 Worksta- piston vertical segments (10%). The cam/pin varia-
tion. In the cam/piston zone, the piston offset, x, has a
Kinematic Tolerance Analysis
81
60) misalignment due to manufacturing variation, assem-
bly error, or wear. We have developed a configu-
ration space computation algorithm for spatial pairs
whose parts move along fixed axes [10]. The con-
tact zone model applies to these spaces. To imple-
ment it, we need to program the partial derivatives of
the contact functions, g(u,v,p), for every type of con-
tact, e.g. plane/cylinder, cylinder/sphere, and cylin-
der/circle. We are also studying tolerance synthesis
within the configuration space representation and are
extending the algorithm to variational part models pro-
duced by modern CAD tools, such as ProEngineer and
Catia.
Kinematic tolerance analysis has potential relevance
to robot path planning with uncertain geometry. The
analysis provides a detailed understanding of how small
variations in geometry effect nominal contact relations.
This knowledge could be useful in compliant motion
and in manipulation where the robot plans based on
the domain geometry. The maximum material estimate
represents a single point in the contact zone, hence
provide less information than the full zone. In coarse
0.35 0 wD path planning, the extra information is probably un-
necessary because the robot wants to avoid obstacles
Figure 7: Comb/pin configuration space and contact zone robustly, not to interact with them.
detail.
Acknowledgments
tion is evenly distributed among the parameters of the
touching features and the part centers of rotation. Ralf Schultheiss and Uwe Hinze collaborated in the
case study. Sacks is supported by NSF grants CCR-
9617600 and CCR-9505745 and by the Purdue Center
6 Conclusion for Computational Image Analysis and Scientific Vi-
sualization. Joskowicz is supported by a grant from
We described a kinematic tolerance analysis algorithm the Authority for Research and Development, The He-
for mechanical systems with parametric part tolerances brew University of Jerusalem, Israel. Both are sup-
and presented a design case study. The algorithm con- ported by a Ford University Research Grant, the Ford
structs a variation model for the system, derives worst- ADAPT200 project, and by grant 98/536 from the Is-
case bounds on the variation, and helps designers find raeli Academy of Science.
unexpected failure modes, such as jamming and block-
ing. We have applied our program to detailed paramet- References
ric models of a variety mechanisms including a Geneva
cam pair, a 35mm camera shutter, a movie camera film [1] Eric Ballot and Pierre Bourdet. A computation
method for the consequences of geometric errors in
advance, and a micro-mechanical gear discriminator.
mechanisms. In Proc. of the 5th CIRP Int. Seminar
In all cases, the program produced useful and interest- on Computer-Aided Tolerancing, Toronto, 1997.
ing qualitative and quantitative results.
[2] K. W. Chase and A. R. Parkinson. A survey of re-
We plan to extend the scope of our algorithm to search in the application of tolerance analysis to the
spatial tolerance analysis of planar systems. Planar design of mechanical assemblies. Research in Engi-
pairs must be analyzed as spatial pairs to study axis neering Design, 3(1):23-37, 1991.
82 L. Joskowicz and E. Sacks

Spatial planning: A configuration

[3] Kenneth Chase, Spencer Magleby, and Charles [15] T. Lozano-Pérez.
Glancy. A comprehensive system for computer-aided space approach. In JEEE Transactions on Computers,
tolerance analysis of 2d and 3d mechanical assemblies. volume C-32, pages 108-120. IEEE Press, 1983.
In Proc. of the 5th CIRP Int. Seminar on Computer-
Aristides A. G. Requicha. Mathematical definition
Aided Tolerancing, Toronto, 1997. [16] Manufacturing Review,
of tolerance specifications.
[4at Jingliang Chen, Ken Goldberg, Mark Overmars, Dan 6(4):269-274, 1993.
Halperin, Karl Bohringer, and Yan Zhuang. Shape tol-
[17] Elisha Sacks. Practical sliced configuration spaces for
erance in feeding and fixturing. In P.K. Agarwal, L. E.
curved planar pairs. International Journal of Robotics
Kavraki, and M. T. Mason, editors, Robotics, The Al-
Research, 18(1):59-63, January 1999.
gorithmic Perspective: 3rd Workshop on Algorithmic
Foundations of Robotics (WAFR). A. K. Peters, 1998. Elisha Sacks and Leo Joskowicz. Computational kine-
[18]
matic analysis of higher pairs with multiple con-
[5] André Clemént, Alain Riviére, Phillipe Serré, and tacts. Journal of Mechanical Design, 117(2(A)):269—
Catherine Valade. The ttrs: 13 constraints for dimen- 277, June 1995.
sioning and tolerancing. In Proc. of the 5th CIRP Int.
Seminar on Computer-Aided Tolerancing, ‘Toronto, [19] Elisha Sacks and Leo Joskowicz. Parametric kinematic
1997. tolerance analysis of planar mechanisms. Computer-
Aided Design, 29(5):333-342, 1997.
[6 G. Erdman, Arthur. Modern Kinematics: develop-
ments in the last forty years. John Wiley and Sons, [20] Elisha Sacks and Leo Joskowicz. Parametric kine-
1993. matic tolerance analysis of general planar systems.
Computer-Aided Design, 30(9):707—714, August 1998.
Max Gonzales-Palacios and Jorge Angeles. Cam
Synthesis. Kluwer Academic Publishers, Dordrecht, [21] Elisha Sacks, Leo Joskowicz, Ralf Schultheiss, and
Boston, London, 1993. Uwe Hinze. Computer-assisted kinematic tolerance
analysis of a gear selector mechanism with the con-
(8 eet
Matsamoto Inui and Masashiro Miura. Configuration figuration space method. In 25th ASME Design Au-
space based analysis of position uncertainties of parts tomation Conference, Las Vegas, 1999.
in an assembly. In Proc. of the 4th CIRP Int. Seminar
on Computer-Aided Tolerancing, 1995. [22] Elisha Sacks, Charles Pisula, and Leo Joskowicz. Visu-
alizing three-dimensional configuration spaces for me-
[9pect Leo Joskowicz, Elisha Sacks, and Vijay Srinivasan. chanical design. Computer Graphics and Applications,
Kinematic tolerance analysis. Computer-Aided De- 19(5):50-53, 1999.
sign, 29(2):147-157, 1997. reprinted in [13].
[23] Ralf Schultheiss and Uwe Hinze. Detect the unex-
(10] Ku-Jim Kim, Elisha Sacks, and Leo Joskowicz. Kine-
pected - how to find and avoid unexpected tolerance
problems in mechanisms. In Global consistency of
matic analysis of spatial fixed-axes pairs using con-
tolerances, Proc. of the 6th CIRP Int. Seminar on
figuration spaces. Technical Report CSD-TR, 99-036,
Computer-Aided Tolerancing, F. van Houten and H.
Purdue University, 1999.
Kals eds., Kluwer, 1999.
[11 — Jean-Claude Latombe. Robot Motion Planning.
[24] O.W. Solomons, F. van Houten, and H. Kals. Current
Kluwer Academic Publishers, Boston, 1991. status of cat systems. In Proc. of the 5th CIRP Int.
Seminar on Computer-Aided Tolerancing, Toronto,
(12 Jean-Claude Latombe and Randall Wilson. Assem- 1997.
bly sequencing with toleranced parts. In Third ACM
Symposium on Solid Modeling and Applications, 1995. [25] Herbert Voelcker. A current perspective on tolerancing
and metrology. Manufacturing Review, 6(4):258-268,
(13 Jean-Paul Laumond and Mark Overmars, editors. Al- 1993.
gorithms for Robotic Motion and Manipulation. A. K.
Peters, Boston, MA, 1997. [26] Daniel Whitney, Olivier Gilbert, and Marek Jastrzeb-
ski. Representation of geometric variations using ma-
(14 ul Faydor L. Litvin. Gear Geometry and Applied Theory. trix transforms for statistical tolerance analysis. Re-
Prentice Hall, New Jersey, 1994. search in Engineering Design, 6(4):191-210, 1994.
Deformable Free Space Tilings for
Kinetic Collision Detection

Pankaj K. Agarwal, Duke University, Durham, NC

Julien Basch, Stanford University, Stanford, CA
Leonidas J. Guibas, Stanford University, Stanford, CA
John Hershberger, Mentor Graphics Corp., Wilsonville, OR
Li Zhang, Stanford University, Stanford, CA

We present kinetic data structures for detecting col- collision response, collision detection remains one of
lisions between a set of polygons that are moving the bottlenecks in such a system. A commonly used ap-
continuously in the plane. Unlike classical collision- proach to expedite the collision detection between com-
detection methods that rely on bounding volume hi- plex shapes is based on hierarchies of simple bounding
erarchies, our method is based on deformable tilings volumes surrounding each of the objects. For a given
of the free space surrounding the polygons. The ba- placement of two non-intersecting objects, their respec-
sic shape of our tiles is that of a pseudo-triangle, a tive hierarchies are refined only to the coarsest level at
shape sufficiently flexible to allow extensive deforma- which the primitive shapes in the two hierarchies can
tion, yet structured enough to make detection of self- be shown to be pairwise disjoint.
collisions easy. We show different schemes for main- Motion in the physical world is in general continuous
taining pseudo-triangulations using the framework of over time, and many systems attempt to speed up col-
kinetic data structures, and analyze their performance. lision checking by exploiting this temporal coherence,
Specifically, we first describe an algorithm for main- instead of repeating a full collision check ab initio
taining a pseudo-triangulation of a point set, and show at each time step [22]. Swept volumes in space or
that this pseudo-triangulation changes only quadrati- space-time have been used towards this goal [6, 17, 19].
cally many times if points move along algebraic arcs Though fixed time-sampling is customary for motion
of constant degree. We then describe an algorithm for integration, collisions tend to be rather irregularly
maintaining a pseudo-triangulation of a set of convex spaced over time. If we know precisely the motion laws
polygons. Finally, we extend our algorithm to the gen- of the objects, then it makes sense to try to predict ex-
eral case of maintaining a pseudo-triangulation of a set actly when collisions will happen, instead of hoping to
of moving simple polygons. These methods can be ex- locate them with time sampling. There have been a few
tended to situations where the polygons deform as well theoretical papers in computational geometry along
as move. these lines [8, 13, 23], but their results are not so useful
in practice because they use complex data structures
1 Introduction and are only applicable for limited types of motion.
In this paper, we consider the situation where we
Collision detection between moving objects is a fun- have multiple moving or deforming polygonal objects
damental problem in computational simulations of the in the plane. Such a setting can arise in various simu-
physical world. Because of its universality, it has been lation and motion planning tasks. For example, in sev-
studied by several different communities, including ro- eral manufacturing processes, the mechanical parts are
botics, computer graphics, computer-aided design, and modeled as planar polygons [1, 5, 4] for parts feeding
computational geometry. Several methods have been and orienting tasks. In motion planning for multiple
developed for the case of rigid bodies moving freely in mobile robots amidst possibly moving obstacles, the
two and three dimensions. Though a physical simula- robots and obstacles are usually projected to the floor,
tion involves several other computational tasks, such as and the problem is simplified to a two-dimensional
motion dynamics integration, graphics rendering, and problem to improve efficiency. More importantly, we
84 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

hope that extensions of the major techniques developed that the self-intersection of a cell of the decomposi-
in this paper will be able to handle three dimensional tion is easy to detect, the decomposition is simple to
objects. update when a self-intersection occurs, and the decom-
position conforms to the motion of the polygons so
Recently, Basch et al. [2] and Erickson et al. [9] pre-
sented algorithms for detecting collision between two
that self-intersections do not happen too many times.
An obvious choice for the decomposition is a trian-
polygons using the kinetic data structures framework,
gulation of the free space. Such a triangulation was
which was originally introduced by Basch et al. [3, 12].
These kinetic algorithms avoid many of the problems
used by Held et al. [16] to track a moving object flying
among fixed obstacles. Although a triangulation sat-
that arise in fixed time-sampling methods. A kinetic
isfies the first two criteria, it contains too many cells
data structure, or KDS for short, is built on the idea
and therefore has to be updated frequently when all
of maintaining a discrete attribute of objects in mo-
the defining shapes are in motion. We will therefore
tion by animating a proof of its correctness through
use a pseudo-triangulation as the decomposition, which
time. The proof consists of a set of elementary con-
has considerably fewer cells compared to a triangula-
ditions, called certificates, based on the kinds of tests
tion. Pseudo-triangles can flex as the objects move,
performed by ordinary geometric algorithms —- CCW
and therefore the combinatorial structure of the tiling
(counterclockwise) tests in our case. Those certificates
needs fewer updates. At the same time, the cells in a
that can fail as a result of the motion of the polygons
pseudo-triangulation have sufficiently simple shapes so
are placed in an event queue, ordered according to their
that their self-intersections are easy to detect and the
earliest failure time. When a certificate fails, the proof
needs to be updated. Unless a collision has occurred, triangulation is easy to update. Pseudo-triangulations
we perform this update and continue the simulation. have been used in the past, but primarily for various
In contrast to fixed time step methods, for which the visibility problems [7, 21]. An additional benefit of
fastest moving object gates the time step for the entire our structure is that it can gracefully adapt to object
system, a kinetic method is based on events (the certifi- shapes that are themselves flexible. As our polygons
cate failures) that have a natural significance in terms move they can also deform and change shape. This ad-
of the problem being addressed (collision detection in ditional flexibility impacts our data structure primarily
this case). on the number of events it has to process — but its ba-
sic nature and certification remains unchanged.
Unlike the previous hierarchy-based algorithms, the
algorithm by Basch et al. [2] maintains a decomposi- In Section 2, we describe our model for motion, de-
tion of the common exterior of the two moving poly- fine pseudo-triangulations, and describe the certificates
gons (similar approach was used by Mount [20] for the needed to maintain a pseudo-triangulation. In Sec-
static problem of intersection detection.). The cells of tion 3, we first describe how to maintain a pseudo-
this decomposition deform continuously as the objects triangulation for a set P of n moving points in the
move. As long as all the cells in the decomposition re- plane, and show that it can be maintained in an output-
main disjoint, the decomposition itself acts as a KDS sensitive manner. For low-degree algebraic motion, the
proof of non-collision between the objects. At certain pseudo-triangulation changes about O(n”) times. We
times, cells become invalid because they self-intersect can further refine the pseudo-triangulation to main-
and the decomposition has to be modified. Unfortu- tain a triangulation of P, which changes about O(n"/*)
nately, in their approach an extension to collision de- times if each point in P is moving with fixed velocity.
tection between many polygons is very expensive — To our knowledge, this is the first triangulation (with-
such a decomposition has to be built separately for out Steiner points) that changes a sub-cubic number of
every pair of polygons. times, even for linear motions.
In this paper we present an algorithm for detecting In Section 4, we describe a scheme for maintain-
collision between arbitrary sets of simple polygons as ing the pseudo-triangulation of a set P of k disjoint
they move and/or deform in the plane. As in [2], we convex polygons with a total of n vertices. We show
maintain a decomposition of the common exterior of that the greedy vertical pseudo-triangulation proposed
polygons, the free space, into deformable tiles. Ide- by Pocchiola and Vegter [21] can be maintained effi-
ally, we would like to maintain a decomposition so ciently. A nice feature of this (or any minimal) pseudo-
Deformable Free Space Tilings for Kinetic Collision Detection
85
triangulation is that the number of cells is only O(k), cessfully addressed. Though this paper describes how
which is typically much smaller than n, the total com- to implement the deformable tiling idea only in 2D, we
plexity of the polygons. On the other hand, the size are hopeful that 3D extensions will also be possible.
of any triangulation has to be at least Q(n). We will
show that we need a kinetic data structure of size O(k)
2 Models of Motion and Pseudo-
to maintain this pseudo-triangulation. But unlike the
set of points case, we do not have sharp bounds on the Triangulations
number of events.
In this section we discuss our models of motion, de-
Finally, in Section 5, we combine our algorithm for fine pseudo-triangulations, and discuss how we main-
the convex case with the one in [2] to construct a tain them as objects move continuously.
pseudo-triangulation for a set of pairwise-disjoint sim-
ple polygons moving in the plane. It can easily be 2.1 Models of Motion
updated when a certificate fails. The number of certifi-
cates needed to maintain the correctness of the pseudo- There are two reasons for defining motion models.
triangulation is within an O(log n) factor of the size of a First, we need to be able to compute the certificate fail-
minimum link subdivision separating the polygons [24]. ure times. Second, we wish to be able to give bounds
By this property, our separation proof automatically on the maximum number of events that can happen.
adapts to the complexity of the relative placement of We describe the model for which we bound the number
the polygons, and its size will vary between O(k) (when of events first.
the polygons are far from each other) and O(n) (when A rigid polygon P is described at rest by a refer-
they are closely intertwined). A compact separation ence point o, by an orthonormal basis (x,y), and by
proof is important when objects are allowed to change the coordinates (pz,p,) of each vertex p of P in this
their motion plan unpredictably since the number of orthonormal frame centered at 0. An algebraic motion
certificates represents the cost of recomputing the event of P is given by a trajectory o(t) of the reference point
times. Recently, Kirkpatrick et al. [18] were able to and by an orthonormal basis (x(t), y(t)), such that the
suggest a method that kinetically maintains a decom- coordinates of 0, x,y are algebraic functions of time of
position of the free space whose size is within a constant bounded degree. The position of v at time t, denoted
factor of the size of a minimum link subdivision. Unlike by v(t), is o(t) + p2x(t) + pyy(t). If P is a point, then
our method, in which the tiling is determined only by o is the same as P and the vectors x, y are not needed.
the current positions of the polygons, in their method, We will use P(t) to denote the polygon P at time t.
the tiling depends on the history of the motions; this The degree of motion of P is the maximum degree of
makes it hard to analyze the efficiency of their method. a polynomial defining the coordinates of 0;, 7;, y;, for
1<i<k. If the degree of motion is 1, we say that
Traditionally, collision detection has been divided
the motion is linear. We call a motion translational if
into two phases: the broad phase, in which one uses
the coordinates of vectors 2;,y;, for alll <i <k, are
a simple-minded algorithm (typically a bounding box
constant.
check) to determine which pairs of objects might collide
and thus need further testing, and the narrow phase, in Although the above model differs from that used in
which these candipdate pairs get a more detailed colli- practice in describing rotational motion, it is flexible
sion test using a sophisticated algorithm. Note that our enough to approximate arbitrary motions to any de-
approach based on deformable free space tilings com- sired accuracy, for a limited time. Note, however, that
pletely obviates this distinction. The tiling effectively an algebraic rotation has necessarily non-uniform an-
‘hides’ features of objects that are far away and treats gular velocity, and can cover only a constant number of
the objects as equivalent to their convex hulls. As the full turns. If we parameterize the basis (x(t), y(t)) ap-
objects get closer and more intertwined, their features propriately, we can represent algebraically some special
get progressively revealed and participate in collision cases, such as when all the objects rotate with angu-
checks. Furthermore our framework can be extended in lar velocities that are rational multiples of a common
a straightforward way to deformable objects, a setting angular velocity. Rotations with general angular veloc-
that no previous collision detection method had suc- ities can either be approximated using power series, or
86 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

the model can be extended to handle such rotations,

as long as the total number of full turns is bounded.
What is crucial for our analyses is that our kinetic cer-
tificates can fail only a bounded number of times each
during the course of the motions (this is what is called
a pseudo-algebraic motion).
For a deformable polygon, we assume that its defor-
mation is described by the motion of its vertices. An al-
gebraic deformation is a deformation where the motion
of each vertex can be described by an algebraic func-
tion of bounded degree. Further, we also assume that
basic structural properties of shapes are preserved dur-
ing the motion — deformable conver polygons always
have convex shapes during the entire deformation, and
the shapes of deformable simple polygons are always
simple. In the latter case, we need to be able to detect
self-collisions in order to insure simplicity. As we shall
see, our methods can be used to detect self-collisions (ii)
as well.
Figure 1: (i) The examples of some pseudo-triangles and
For a real collision detection system or simulation, non-pseudo-triangles. The first two figures are pseudo-
the equations of motion will typically be given by par- triangles, but the others are not. (ii) An example of pseudo
tial differential equations or bounds on acceleration. triangulation of a polygon with holes.
The kinetic setting applies to such situations as long
aS we can compute or give lower bounds on the failure the same as the vertices of S, and each edge of 7(S)
time of the certificates we are concerned with. In the is either an edge of 0S or a segment whose interior lies
remaining of this paper, we assume that such a com- inside S; edges of the latter type are called diagonals.
putation takes O(1) time.
Let P = {Pi,...,P,} be a set of k simple polygons
2.2 Pseudo-Triangulation with a total of n vertices. We allow the polygons to
be degenerate, in the sense that each P; might be a
A pseudo-triangle in the plane is a simple polygon point or a line segment, but we assume that all poly-
whose boundary consists of three concave chains, called gons are bounded. We also assume that the objects are
side chains, that join at their endpoints. The three in general position. Let P and P° denote the convex
endpoints of a pseudo-triangle are called corners. The hull and interior of P, respectively. We define the free
edges incident upon the corners are called corner edges space F(P) of P to be P\P°. The boundary of F(P),
(Figure 1 (i)). For a vertex p on the boundary of a denoted by OF (P), consists of polygon boundaries and
pseudo-triangle, we denote pr, pr be the predecessor convex hull edges.
and the successor of p along OA in the counterclockwise
Since a polygon (even with degenerate holes) can al-
direction.
ways be triangulated and a triangulated planar subdi-
Let S be a polygon with holes; some of the holes vision is obviously a pseudo-triangulation, a pseudo-
may be degenerate, i.e., they can be points or seg- triangulation of F(P) always exists. A pseudo-
ments. A pseudo-triangulation T(S) of S is a planar triangulation 7 is called minimal if the union of any
subdivision of the closure of S, so that each face of two faces in T is not a pseudo-triangle. If T is not
T(S) is a pseudo-triangle and so that the interior of minimal, then there exists a diagonal d in JT so that T
each face lies in the interior of S; see Figure 1 (ii) for remains a pseudo-triangulation after the removal of d
an example. In other words, 7(S) is a collection of from it. Although minimal pseudo-triangulations are
pseudo-triangles with pairwise-disjoint interiors, each not unique, we can prove the following result on the
lying inside S, that cover S. The vertices of T(S) are size of minimal pseudo-triangulations of F(P).
Deformable Free Space Tilings for Kinetic Collision Detection
87
Lemma 1 Let P be a set of k pairwise-disjoint simple delete the edge pp, and merge the triangle pprpR into
polygons. If m of these polygons are points, and there the pseudo-triangle A;. If both ppr, ppp are diagonal
are a total of r reflex vertices in P, then a pseudo- edges, we then delete one of ppz or ppp: which edge
triangulation of F(P) is minimal if and only if it con- is deleted depends on the specific pseudo-triangulation
tains 2k —m-+r—2 pseudo-triangles. we are maintaining; we will explain the choice for each
pseudo-triangulation later in the paper.
2.3 Maintaining a Pseudo-Triangulation
When the corner certificate of g fails (Figure 2(iii)),
We are interested in maintaining 7(P), a pseudo- we collapse the corner. Suppose when the concave cer-
triangulation of F(P), as F(P) deforms continuously. tificate corresponding to q fails, gz lies on the edge
Specifically, we want to maintain T7(P) as a kinetic qqr. We then delete the edge gqr and add the edge
data structure. As mentioned in the introduction, this qdiqr to maintain the pseudo-triangularity of A. This
is accomplished by maintaining a proof of the correct- process also effectively adds the point gz to the side
ness of 7(P), which consists of a small set of elemen- chain of the other pseudo-triangle incident to the edge
tary conditions called certificates. As long as the cer- qqr- Since there is only one way to update the pseudo-
tificates remain valid, the current combinatorial struc- triangulation when a corner certificate fails, we will not
ture of 7 (P) is valid. It is only when a certificate fails describe updates for this case in the following sections.
that 7(P) needs to be updated. A certificate failure is In addition, we also have to maintain the boundary
called an event. All the events are placed in an event
OF(P). The edges of OF(P) \ OP are automatically
queue according to their failure time. The structure is
maintained by the algorithm. By regarding OP as a
then maintained by processing those events one by one.
concave chain with respect to the exterior of P, it can
The efficiency of such a data structure depends on the
also be maintained using reflex certificates for each ver-
size of the proof, the number of events, and the number tex on the hull. We omit the details here.
of certificates that need to be updated at each event.
Since the pseudo-triangulation is not a canonical
In our set-up, we certify that each face in T(P) is
structure (there may be many pseudo-triangulations
a pseudo-triangle and that the faces cover F(P). In
of F(P)), it is not clear which pseudo-triangulation we
view of the definition of 7(P), we need to certify the
are maintaining. To avoid this ambiguity, we first de-
following two conditions for each pseudo-triangle A in
scribe a static algorithm that constructs a “specific”
T(P) (see Figure 2(i)):
pseudo-triangulation of F(P). We then assume that,
at any time t, the kinetic data structure maintains the
1. Each side chain C' is concave, i.e., for each inte-
pseudo-triangulation that the static algorithm would
rior vertex p of C, the angle Zprppr > x (or the
have constructed on P at time t. To maintain this in-
signed area of Ap, ppp is negative). We call these
variant, we need additional certificates. When these
certificates reflex certificates.
certificates fail, we usually update the structure by the
2. The side chains of a pseudo-triangle A join only flipping operation: For any diagonal edge e, consider
at their endpoints, i.e., for each corner vertex q the two pseudo-triangles A;,Ay that contain e. The
of A, the angle Zgzqqr > 0 (or the signed area union of A;, Ay forms a pseudo-quadrangle O—a cell
of Agzggr is positive). We call such certificates whose boundary consists of four concave chains. Be-
corner certificates. sides e, there is exactly one other diagonal edge inside
©, which is called the shadow edge of e. The quadran-
When the reflex certificate of p fails (Figure 2(ii)), we gle © can be decomposed into two pseudo-triangles in
connect the vertices pr, pr, adjacent to p, by an edge to two ways, one by adding e and the other by adding e’s
maintain the pseudo-triangularity of A. To maintain shadow edge. The flipping operation replaces e by e’.
the pseudo-triangulation, we may also have to update
other pseudo-triangles, depending on the edges adja-
cent to p. If both edges adjacent to p are boundary 3 Pseudo-Triangulation for Points
edges, then we add a triangle pprpr to T(P). If only
one edge, say ppr, is a diagonal edge, then ppz is ad- In this section, we assume P to be a set of n points.
jacent to another pseudo-triangle A;. In this case, we we define the incremental pseudo-triangulation for P,
88 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

(iii)

Figure 2: The certificates to prove pseudo-triangularity.

and show how to maintain it efficiently. We also show u(p)

how it can be refined to maintain a triangulation of P.

3.1 Incremental Pseudo-Triangulation

A pseudo-triangulation for points can be built in an in-

cremental manner as follows. We first sort the points in
d(p)
increasing order of their z-coordinates. Suppose that (i)
P1,P25-++5Pn is the sorted sequence, and denote by P; Figure 3: The incremental pseudo-triangulation of a point
the first 7 points. We insert the points from left to right. set. In (i), the sweep line has just passed p; (ii) shows the
Upon inserting p;, we draw the two tangent segments
final pseudo-triangulation.
from p; to the convex hull P;_;. Denote by u(pz) and
d(p,) the left endpoints of the upper and lower tangent
segments to P;,_1 from pz, respectively (Figure 3). The Lemma 2 As long the x-ordering of the points in P
concave chain on Px_1 between u(p,) and d(p,) and does not change, the reflex and corner certificates cer-
line segments p,u(p,), Ped(px) form a pseudo-triangle tify the incremental pseudo-triangulation.
A(px) — its boundary consists of a concave chain and
In view of this lemma, as long as the x-ordering does
two single edges. Let C(p;) denote the concave chain
on A(p,). The three corners of A(p,) are the points not change, we have to update JPT(P) only when a
Pr, A(pr), and u(p,). After processing every point in reflex or corner certificate fails. When a corner event
P, we obtain a pseudo-triangulation, which is called
fails, the structure can be updated as described in Sec-
the incremental pseudo-triangulation (IPT) of P. This tion 2.3. When a reflex certificate fails, we have two
process resembles to the classical incremental construc- choices of which edge to delete — such choice is not
tion of convex hull [10, 11]. difficult, and we omit the details in this version of the
paper.

3.2 Maintaining the IPT Next, we consider the case in which two points ex-
change in 2-order (we call this an z-event). Although
As the points move, IPT(P) changes continuously, such an event does not affect the validity of the pseudo-
but its combinatorial structure changes only at discrete triangulation, it does cause a change to IPT(P) since
times. the incremental ordering of the points changes. Sup-
pose that p passes q from the left at time t and p is
Deformable Free Space Tilings for Kinetic Collision Detection
89
u(p)
through p and q; otherwise, o3(t) is undefined. Since
the motion has constant degree, the x-order of a pair
of points can switch only a constant number of times.
Thus, each 3(t) consists of a constant number of arcs,
all of which are portions of a fixed-degree polynomial.
Consider the family of functions

®, = {¢7|¢€P,q#
pd}.
By our assumptions, any two arcs in ®, intersect a
constant number of times, say, s — 2 times. For a point
q to be d(p) at time t, ¢%(t) must have the largest
value among all the points in P. This is to say, the
number of changes to d(p) is the same as the combina-
torial complexity of the upper envelope of ®,, which is
bounded by A;(n). (As(m) is the maximum length of
an (n,s) Davenport-Schinzel sequence and is roughly
d(q)
linear.) Similarly, we can bound the number of changes
(ii) to u(p) by As(n). Summing over all the points in P,
Figure 4: (i) Two points exchange in x-order. (ii) Fan we thus have:
triangulation of IPT (P). Theorem 4 When the points of P move algebraically
with constant degree, IPT(P) changes O(n X;5(n))
above g (Figure 4). Let r be d(p) just before time ft, times, where s is a constant that depends on the de-
and let r’ be u(q) immediately after time t. Then the gree of the motion.
structure is updated by switching the edge rp with the
edge r’q — a flipping operation defined in Section 2.3. 3.4 Fan Triangulation of P
The point r’ can be found by computing a tangent seg- It is easy to obtain a triangulation of P from IPT (P).
ment from g to the concave chain between u(p) and We connect each point p to every interior point on
d(q), which can be done in O(log n) time. C(p), thereby creating a fan inside each A(p). We
Notice that in this description, each event changes call such a triangulation the fan triangulation of P.
IPT(P). Thus, the method is output-sensitive. We (Figure 4 (ii)). Although IPT(P) changes only nearly
have shown that quadratically many times, we are not able to prove a
similar bound on the number of changes in the fan tri-
Theorem 3 The IPT of n points can be maintained in angulation. It is easy to verify that a reflex or corner
an output-sensitive manner. Each update of the struc- event causes only O(1) changes to the fan triangula-
ture takes O(log n) time. tion. The problem comes from the z-ordering events,
which we process by switching a fan of p to a fan of q,
In the following, we shall bound the number of or vice versa (Figure 4 (ii)). The cost of such switching
changes to JPT(P) for points in constant degree alge- is proportional to the length of the chain between r and
braic motion. r’, where r and 7’ are the same as defined in Section 3.2.
In the worst case, it might be O(n). This gives us a
3.3. Combinatorial Changes to JPT(P) naive bound of O(n*) on the number of changes to the
of fan triangulation.
We now obtain an upper bound on the number
combinatorial changes to IPT(P), i.e., the number of For linear motion, we can obtain a better upper
events, under the assumption that the degree of mo- bound on the number of changes to the fan triangu-
tion of P is fixed. We define a function ¢%(t) for two lation by a global argument. To our knowledge, this is
distinct points p,q € P: if at time t, q is to the left the first triangulation for which a sub-cubic bound can
of p, then $%(t) is the slope of the line that passes be proved, even for linear motions.
90 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

Theorem 5 For points in linear motion, the num- make a right turn if it is possible. Call the trajectory
ber of changes to the fan triangulation is bounded by of such a point a pseudo-concave chain. Clearly, in
O(n*/3,(n)), where s is a constant. a line arrangement, this definition gives us exactly a
concave chain. We can prove the following.
In the following, we prove Theorem 5. For each point
p, denote by Ha(p) the half-space below the line passing Lemma 7 The lower envelope of the arrangement of
through p and r. Consider a point v € P on the concave =, consists of yy, disjoint (pseudo) concave chains.
chain C between r and r’ at the time t when an z-event Proof: Imagine that a point travels on the lower en-
happens. It can be shown that at least one of u(v) and velope of E,. It needs to stop only if it encounters an
d(v), say d(v), is adjacent to v in C at time t. endpoint of an arc. Such an endpoint corresponds to a
Since v lies between r and r’, the points p,q must discontinuity of the function €? for some p, i.e., corre-
lie below the line that passes through v and d(v), sponds to p coming in or going out of Hq(v). Further,
ie., p,q € Ha(v). Furthermore, there is no point in the endpoint is on the lower envelope. This happens
P12 Ha(v) to the left of p and g. Otherwise, it would Yy times. Oo
contradict that v appears on the concave chain between
Combining the Lemma 6 and 7, we are able to prove
u(p) and d(q). We define the function €?(t) to be the
Theorem 5.
x-coordinate of p if pis in Ha(v) and to the right of v at
time t and undefined otherwise. Set =, = {€? |p € P}. Proof: (Theorem 5) If the points move linearly with
The point v is on the concave chain between r and r’ a constant velocity, then the z-coordinate of a point is a
when p,q exchange their x-ordering if and only if €?(t) linear function of time. Thus, the lower envelope of =,
and €4(t) are both defined and they are the small- consists of 7, disjoint concave chains in an arrangement
est amongst all the €,’s at time ft, ie., if the point of n lines. According to the results in [14, 15], the total
(t, €?(t))(= (t,€2(t))) is a vertex on the lower enve- complexity of these chains is bounded by:
lope of =,. Denote by 6, the complexity of the lower
envelope of &=,. The number of changes to the fan tri-
by = O(max (ny,/*, n7/92/)).
angulation in this case is thus bounded by }?,-p dv. Therefore, the total complexity is bounded by:
Unfortunately, we cannot use a Davenport-Schinzel
+e Op = by, O(max (ny}/3 ,n?/342/3)) |
sequence argument to prove a near-linear bound on 6, vEP v
because the graphs of functions €2(t) may consist of
Q(n) arcs. In fact, there are examples in which the By Lemma 6, >7,, % = O(nd;(n)). Thus, >>, 6, is
graphs of functions in =, consist of Q(n) arcs in total. maximized when 7, = O(A;(n)), for all v’s. This gives
We therefore need a more refined analysis to bound us the bound of O(n4/*,(n)). Oo
On.
We do not know whether it is possible to extend this
When a point g enters or leaves Hq(v), it may or may
result to higher degree algebraic motions or whether
not appear on the lower envelope of =,. For a point
this bound is tight.
v € P, denote by 7, the number of arc endpoints that
appear on the lower envelope of =,.
4 Pseudo-Triangulation for Convex
Lemma 6 )0 ep Ww = O(nAs(n)). Polygons
Proof: When an event of this type happens, /PT(P) We now consider the case in which P is a set of k
changes. Thus, in total, it is bounded by O(nA,(n)) by convex polygons with a total of n vertices. We de-
Theorem 4. oO scribe a different pseudo-triangulation for P, called the
greedy vertical pseudo-triangulation, which is based on
Consider a point that travels along the edges of the notions developed by Pocchiola and Vegter [21]. They
arrangement A(=) of a set & of t-monotone algebraic introduced the greedy pseudo-triangulation as a tool
arcs. The point moves t-monotonically along an edge in to compute the visibility complex of a set of convex
A(=). When it comes to an intersection between two polygons. We adapt their algorithm for our collision
arcs, it either continues to travel on the same arc or detection application.
Deformable Free Space Tilings for Kinetic Collision Detection
91

(i) (ii) (iii)

Figure 5: (i) Intersecting and non-intersecting tangents: $1, 82 are intersecting, but 81,83 and 82,83 are non-intersecting
tangents. (ii) Greedy vertical pseudo-triangulation. (iit) Left-to-right property. The solid segment is an edge in GVPT .
The dotted ones are the free tangents it crosses.

4.1 Greedy Pseudo-Triangulation for Convex a pseudo-triangulation of F(P) consisting of 2k — 2

Polygons pseudo-triangles. By Lemma 1, this procedure con-
structs a minimal pseudo-triangulation.
There are exactly four bi-tangent segments between
any two disjoint convex polygons. A _ bi-tangent is
For any line segment s, define @(s) to be the mini-
mum angle by which we have to rotate s in counter-
called free if it does not intersect the interior of any
clockwise direction so that it becomes vertical. We
object in P. Let s and s’ be two free bi-tangents with
order all the free tangents in increasing order of 6(-).
a common endpoint v, which is a vertex of a polygon
The greedy pseudo-triangulation created using this or-
Pe€P. There are three disjoint wedges formed by the
dering is called the greedy vertical pseudo-triangulation
polygon edges incident upon v and between s and s’.
(GVPT) and is denoted by G(P) (Figure 5 (ii)). It
We say that s and s’ cross at v if each of these wedges
is shown in [21] that G(P) can be constructed in
spans an angle less than 7; see Figure 5.1 We say that
O(klogn) time, provided that each polygon is repre-
two free bi-tangents intersect if either they share an
sented as an array storing its vertices in a clockwise (or
interior point or they have a common endpoint and
counterclockwise) order.
they cross at that endpoint. For any linear ordering
~< on the free tangents, we can build a corresponding The following local rule determines whether a free
greedy triangulation 7.(P) as follows. We first sort bi-tangent is in G(P).
the tangents by < ordering. We then scan through Lemma 8 (Left-to-right property) A bi-tangent
this list and maintain a set S of tangents that we have segment s is in G(P) if and only if 0(s) < 0(s') for
added to the pseudo-triangulation so far. At each step, each free bi-tangent s’ intersected by s (Figure 5 (ttt)).
we pick the next tangent segment s in the sorted se-
quence and check whether it intersects any tangents Recall that if we replace an edge in a pseudo-
in S. If s does not intersect any segment of S, we triangulation with its shadow edge, we still obtain a
add it to S; otherwise, we discard s. After processing pseudo-triangulation. It turns out that in G(P) the
all the segments, we obtain a set of non-intersecting shadow edge of an edge s is minimal in the ordering <
free tangents. It is shown in [21] that |S| = 3k — 3, among all free bi-tangents crossing s. This implies the
and that together with the object boundaries, S forms following property:

‘Intuitively, if we smooth the polygon P by taking the Lemma 9 (Local property) Suppose that s is an
Minkowski sum of P with a disk of a sufficiently small ra- edge in T.(P). Denote by <' the same ordering as
dius, say 6, and we translate s and s’ by at most 6 so that < except assigning s as the maximum element of =’.
they become tangents to the resulting polygons at their end-
points, then s and s’ cross at v if and only if the translated
Then T-<:(P) can be obtained from T.(P) by replacing
copies of s and s’ intersect. s with its shadow edge.
92 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

Figure 6: The reflex(from left to right) and corner(from right to left) events and the updates. When a reflex certificate
fails, we choose the edge with smaller slope, as shown in the right figure.

By this lemma, if the ordering of an edge changes, Further, one of the corner or reflex certificates must fail
we just perform a local flip operation to fix the greedy when such an event happens. oO
pseudo-triangulation.
When a corner or reflex certificate fails, we can up-
4.2 Greedy Pseudo-Triangulation
date G(P) as described in Section 2.3, except for one
subtle issue in the case of reflex certificates. When
Maintenance
an interior vertex p of a side chain ... prppRr... ceases
We now describe how to maintain G(P) as a KDS so to be a reflex vertex, we add the edge pppr and we
that it can be updated efficiently as the polygons in have to delete one of the edges pzp and ppr. Instead
P move or deform. As described in Section 2.3, we of deleting one of them arbitrarily, we delete prp if
maintain corner and reflex certificates for each pseudo- O(pip) > O(ppr); see Figure 6.
triangle in G(P). In addition, for each diagonal edge Next, we consider diagonal certificates. If the cer-
s of G(P), we maintain a diagonal certificate to certify tificate of a diagonal edge s fails, then either s or its
that 0(s) < 0(s’), where s’ is the shadow edge of s shadow edge (see Section 2 for the definition) is verti-
in G(P). This adds the requirement to maintain the cal. When such an event happens, the slope ordering
shadow edges although such edges do not appear in of an edge jumps from the minimum to maximum, or
G(P). The shadow edges can also be maintained using vice versa. By the local property of greedy pseudo-
corner and reflex certificates. We will show that these triangulations, we can simply perform a flipping oper-
certificates are sufficient to maintain G(P). ation to s to maintain G(P).
As in Section 3, we can prove the following analogue Since G(P) has O(k) diagonals and pseudo-triangles,
to Lemma 2. the number of corner and diagonal certificates is O(k).
We assume that the polygons remain convex at all
Lemma 10 J[f no diagonal certificate fails, then G(P)
times, therefore it. suffices to maintain reflex certificates
changes only when one of the corner or reflex certifi-
for a vertex only if it is adjacent to a diagonal edge,
cates fails.
i.e., a free bi-tangent. The number of such vertices is
Proof: The greedy pseudo triangulation can change also O(k). Hence, we obtain the following:
only when a tangent stops or starts to be free, when Theorem 11 G(P) can be maintained by using a
the slope ordering of two intersecting tangents changes,
structure with O(k) certificates. Each event can be
or when two intersecting free tangents stop or start processed in time O(log n).
to intersect. Without any diagonal certificate failure,
all these cases are reduced to when three objects are This data structure works even if the polygons de-
collinear. (three objects are collinear if there is a line form continuously over time, as long as they remain
tangent to all of them.) Suppose that the collinear convex at all times, though the number of events might
objects are P;, P2, P3 and the line @ is tangent to be affected. The rigid motion can give us better bounds
them in that order. Consider the line segments s1,89,53 as stated in Theorem 13.
on £ that connect P; P2, P2P3, and P3P,, respectively.
By the left to right property, for such a collinearity to 4.3 Combinatorial changes to G(P)
change G(P), it must be the case that exactly two of Next, we bound the number of changes to G(P) if the
these edges are in G(P) right before the event happens. degree of motion of P is fixed. It can be shown that
Deformable Free Space Tilings for Kinetic Collision Detection
93
a corner or reflex certificate fails when three polygons
of P become collinear, i.e., a line is tangent to three
polygons. Hence, a combinatorial change in G(P) hap-
pens only when three polygons of P are collinear or
when a bi-tangent becomes vertical. The number of
such events can be bounded by the following lemma.

Lemma 12 Suppose that P,, P2, P; are conver poly-

gons with n1,n2,n3 vertices, respectively, and the de-
gree of their motion is constant. They can become
collinear O(n, + n2 +3) and O(nynz + nyn3 + n2N3)
times for translational and rigid motions, respectively.
The bi-tangent between P;, P can become vertical O(1)
and O(n, + nz) times for translational and rigid mo-
tions, respectively.

Applying this lemma to all triplets of P, we obtain

the following weak upper bounds on the number of (ii)
changes to G(P).
Figure 7: (i) The relative geodesic cycle of P pinned at the
Theorem 13 Let P be a set of k polygons with a total vertex p. (ii) The external relative geodesic triangualtion.
ofn vertices. G(P) changes O(kn?) times if the degree The thickened cycle is the geodesice cycle corresponding to
of motion of P is fixed. If the motion is translational, the root node of T(P).
the number of changes is only O(k?n). If the poly-
gons may deform, the number of changes is bounded by
compute a pseudo-triangulation of each of them sepa-
O(n3). rately. We triangulate each P as in [2]. Following [2],
we define the pinned relative geodesic cycle of P, with
5 Pseudo-Triangulation for Simple respect to a pinning point set B (a subset of the ver-
Polygons tices of P), to be the shortest cycle in F(P) that passes
through the vertices in B and has the same homotopy
In this section, we consider the case in which P is a type as OP. The pinned relative conver hull of P with
set of k pairwise-disjoint simple polygons with a to- respect to B is the region enclosed by the pinned rela-
tal of n vertices. We describe a method for main- tive geodesic cycle (Figure 7(i)). If B contains all the
taining a pseudo-triangulation of P by combining our vertices of P, the pinned relative convex hull of P is
algorithm for convex polygons with the algorithm by P itself. Consider a balanced binary tree T(P) with
Basch et al. [2] for two simple polygons. the vertices of P as leaves, in the order they appear on
OP; each internal node stores the vertex from its left
5.1 The Mixed Pseudo-Triangulation child. We can then build O(log |P|) pinned relative geo-
desic cycles, where the pinning set of each cycle is the
For each P € P, define the relative geodesic cycle C(P)
set of vertices at a given level of T(P). By overlaying
of P to be the shortest cycle in F(P) with the same
these cycles, we obtain a pseudo-triangulation of PNP
homotopy type as OP. The region enclosed by C(P) is
(Figure 7(ii)). This pseudo-triangulation is called the
called the relative convex hull and is denoted by P. We
external relative geodesic triangulation. Refer to [2] for
decompose F(P) into two parts Fi(P) = F(P)N(UP)
the maintenance and bounds on the number of changes
and Fo(P) = cl(F(P) \ Uint(P)). We compute
of this structure.
pseudo-triangulations T(Fi(P)) and 7(F2(P)) sepa-
rately. These two triangulations together give the Now, we consider F2(P). In the following, we gener-
mized pseudo-triangulation of P. alize the greedy pseudo-triangulation, defined in Sec-
First, we consider F;(P). Since the interiors of all tion 4, to a connected polygonal subdivision. Consider
the relative convex hulls are pairwise disjoint, we can a connected polygonal region F. A vertex v of F is
94 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

called a corner vertex if the interior angle at v is less

than 180°; otherwise, v is called a reflex vertex. For a
point p on OF, a line segment pq in F is called tangent
to OF at p if p is a corner vertex or the line passing
through p,q locally supports OF at p. (The intuition
behind the definition of “tangent” for corner vertices
comes from the case in which a corner is formed by two
separate convex objects whose boundaries touch. Then
a “tangent” segment to the corner is indeed tangent to
one of the two convex objects.) For two vertices p,q on
OF, the line segment pq is called a free bi-tangent if pq
lies in F and is tangent to OF at both p and g. We now
construct the greedy vertical pseudo-triangulation of
F, as described in Section 4. It can be shown that the
algorithm constructs a minimal pseudo-triangulation of
F. The following lemma bounds the number of pseudo-
triangles in such a pseudo-triangulation.

Lemma 14 [f OF consists of k connected components

and m corner vertices, the number of pseudo-triangles
in the greedy pseudo triangulation of F is O(k +m).

The relative geodesic triangulation T(Fi(P)) and

greedy pseudo-triangulation 7 (F2(P)) together form a
pseudo-triangulation of F(P), which we call the mized
pseudo-triangulation (Figure 8). Next, we bound the
Figure 8: The mized pseudo-triangulation. (i) Fi(P)
size of the mixed pseudo-triangulation.
and F2(P) are shaded differently. (ii) Mixed pseudo-
We call a diagonal edge of the mixed pseudo- triangulation; thick edges denote the boundary between
triangulation a bridge edge if it connects two differ- Fi(P) and F2(P).
ent polygons of P. As we will see later, bridge edges
play an important role in maintaining the pseudo-
triangulation. Then, the number of bridge edges is bounded by the
number of edges in the graph G. The graph G is planar,
Lemma 15 0F2(P) consists of O(k) connected com- but it may contain duplicate edges. By bounding the
ponents and O(k) bridge edges and corner vertices. number of faces with only two edges on the boundary,
we can still bound the number of edges of G, thus the
Proof: Clearly, the number of connected components
number of bridge edges, by O(k). Oo
of Fo(P) is bounded by O(k), as P contains k disjoint
objects. Consider a connected component F of Fo(P) Lemmas 14 and 15 imply the following.
and a corner vertex p on OF. Suppose that the two
adjacent vertices to p on OF are qj,qo. One of q; and Corollary 16 The greedy triangulation T(Fo(P)) of
gz must be on the same object as p, and the other Fo(P) has O(k) pseudo-triangles.
must be on a different object. This implies that the
number of corner vertices is bounded by the number of Next, we bound the size of T(F;(P)). We will focus
bridge edges on OF. The number of bridge edges on on the bridge edges in Fi4(P). Basch et al. [2] have
P is clearly bounded by O(k). For those edges not on shown that any line segment lying inside F(P) crosses
the convex hull, consider the planar graph G where a O(log n) bridge edges, and thus the number of bridge
node in G corresponds to an object in P and an edge edges is O(r logn), where T is the size of the min-link
between two nodes corresponds to a bridge edge on separator between two simple polygons. Here, we gen-
OF2(P) that connects the corresponding two objects. eralize their result to our setup by arguing that any
Deformable Free Space Tilings for Kinetic Collision Detection
95
free line segment can cross at most two relative convex 5.2 Mixed Pseudo-Triangulation Maintenance
hulls.
As shown in [2], the external relative geodesic trian-
Lemma 17 Any free line segment crosses O(log n) gulation can be maintained by corner and reflex cer-
bridge edges of T(F,(P)). tificates only, and there are local rules to decide which
edge to select when a reflex certificate fails. There-
Proof: Consider a free line segment pq. Suppose that fore, the mixed triangulation can be easily maintained,
pq crosses m relative convex hulls. As in [2], pq can although it comprises two completely different struc-
cross O(m log n) bridge edges. We claim that pq in- tures. As argued before, the number of certificates in
tersects OP, the boundary of the relative convex hull the certification structure of 7(P) is proportional to
of a polygon, P, in at most one point. Indeed, if pg the number of bridge edges in T(P). According to
intersects OP at two points, then we can shortcut OP Lemmas 15 and 18, we may bound the certificates for
between these two points, thereby contradicting the as- certifying 7 (P) as follows.
sumption that OP is a geodesic. This implies that pq
can intersect at most two relative convex hulls, which
Theorem 20 The number of certificates in T(P) is
completes the proof of the lemma. oO
bounded by O(Klogn), where « is the number of edges
in the minimum link subdivision of P. For each poly-
A minimum link subdivision is a polygonal subdivi- gon P in P, the number of certificates involving P is
sion of the plane, using as few (line segment) edges as bounded by O(7r(P) logn+k), where r(P) is the number
possible, such that each P € P is contained in its own of edges in the minimum link separator of P.
face of the subdivision. A minimum link separator for A trivial bound on the number of changes to the
a polygon P € P is asimple polygon homotopic to 0P mixed pseudo-triangulation for polygons in rigid mo-
with as few edges as possible. tion is O(n?), which is the number of times when three
Consider any Jordan cycle C in F(P) that is homo- vertices become collinear.
topic to OP. Clearly, C’ must intersect all the bridge
edges incident to P because C separates P from other 6 Conclusions
objects in P. Suppose that C consists of 7 line seg-
ments. Since each line segment on C crosses at most We have shown how to efficiently maintain free space
O(log n) bridge edges connected to P and each bridge pseudo-triangulations for points, as well as convex and
edge is crossed at least once, we can conclude that the simple polygons moving around in the plane. In ad-
number of bridge edges incident to P is bounded by dition to collision detection, the pseudo-triangulation
O(rlogn). Therefore, we have the following result on structure provides a simple way for walking around
the number of bridge edges. the free space and performing operations such as ray
shooting and other visibility queries. Unlike any pre-
Lemma 18 The number of bridge edges in T(Fi(P)) vious collision detection method, our deformable tiling
is bounded by O(K log n), where k is the number of edges can be adapted to work for deformable objects, thus
in a minimum link subdivision of P. For each polygon enabling new applications where simulation of flexible
P in P, the number of bridge edges connected to P is shapes is important. An extension of our ideas to 3D
bounded by O(r(P) logn), where T(P) is the number of presents the next obvious challenge.
edges in a minimum link separator of P.
Acknowledgments
Thus, we obtain the following bounds on the size of
the mixed pseudo-triangulation by combining Corol- Pankaj Agarwal was supported in part by National
lary 16 and Lemma 18. Science Foundation research grant CCR-93-01259, by
Army Research Office MURI grant DAAH04-96-1-
Theorem 19 The mized pseudo-triangulation has size 0013, by a Sloan fellowship, by a National Science
O(n), and among all the diagonal edges, only Foundation NYI award and matching funds from Xe-
O(klogn) edges are bridge edges, where k is the num- rox Corp, and by a grant from the U.S.-Israeli Bi-
ber of edges in a minimum link subdivision of P. national Science Foundation. Julien Basch, Leonidas
96 P. Agarwal, J. Basch, L. Guibas, J. Hershberger, and L. Zhang

Guibas, and Li Zhang were supported in part by Na- [10] R. L. Graham. An efficient algorithm for determining
tional Science Foundation grant CCR-9623851 and by the convex hull of a finite planar set. Inform. Process.
Lett., 1:182-133, 1972.
US Army MURI grant DAAH04-96-1-0007. We wish
to acknowledge the contribution of Otfried Cheong and [11] R. L. Graham and F. F. Yao. Finding the convex hull
Mark de Berg to preliminary work related to part of of a simple polygon. J. Algorithms, 4:324-331, 1983.
this paper. L. J. Guibas. Kinetic data structures: A state of the
[12]
art report. In Proc. 38rd Workshop on Algorithmic
Foundations of Robotics, pages 191-209, 1998.
References
[13] P. Gupta, R. Janardan, and M. Smid. Fast algorithms
[1] E. M. Arkin, M. Held, and J. S. B. Mitchell. Man- for collision and proximity problems involving mov-
ufacturing: An application domain for computational ing geometric objects. Comput. Geom. Theory Appl.,
geometry. In First Regional Symposium on Manufac- 6:371-391, 1996.
turing Science and Technology, pages 29-36, October
12-13 1995. [14] D. Halperin and M. Sharir. On disjoint concave chains
in arrangements of (pseudo) lines. Inform. Process.
J. Basch, J. Erickson, L. J. Guibas, J. Hershberger, Lett., 40(4):189-192, 1991.
and L. Zhang. Kinetic collision detection for two sim-
ple polygons. In Proc. 9th ACM-SIAM Sympos. Dis- [15] D. Halperin and M. Sharir. Corrigendum: On disjoint
concave chains in arrangements of (pseudo) lines. Jn-
crete Algorithms, pages 102-111, 1999.
form. Process. Lett., 51:53-56, 1994.
[3patsyJ. Basch, L. J. Guibas, and J. Hershberger. Data struc- [16] M. Held, J. T. Klosowski, and J. S. B. Mitchell. Eval-
tures for mobile data. In Proc. 8th ACM-SIAM Sym- uation of collision detection methods for virtual real-
pos. Discrete Algorithms, pages 747-756, 1997. ity fly-throughs. In Proc. 7th Canad. Conf. Comput.
Geom., pages 205-210, 1995.
[4(ee R.-P. Berretty, K. Goldberg, M. Overmars, and F. V.
der Stappen. On fence design and the complexity of [17] P. M. Hubbard. Collision detection for interactive
push plans for orienting parts. In Proc. 18th Annu. graphics applications. JEEE Trans. Visualization and
ACM Sympos. Comput. Geom., pages 21-29, 1997. Computer Graphics, 1(3):218-230, Sept. 1995.

[5 al K.-F. Bohringer, B. R. Donald, and N. C. MacDon- [18] D. Kirkpatrick, J. Snoeyink, and B. Speckmann. Ki-
ald. Upper and lower bounds for programmable vector netic collision detection for simple polygons. In To
fields with applications to MEMS and vibratory plate appear in 16th Sympos. Comput. Geom., 2000.
part feeders. In J.-P. Laumond and M. H. Overmars,
editors, Algorithms for Robotic Motion and Manipu- [19] B. Mirtich. Impulse-based Dynamic Simulation of
Rigid Body Systems. Ph.D. thesis, Dept. Elec. Engin.
lation, pages 255-276. A. K. Peters, Wellesley, MA,
Comput. Sci., Univ. California, Berkeley, CA, 1996.
1996.
[20] D. M. Mount. Intersection detection and separators
[6] S. Cameron. Collision detection by four-dimensional for simple polygons. In Proc. 8th Annu. ACM Sympos.
intersection testing. In Proc. IEEE Internat. Conf. Comput. Geom., pages 303-311, 1992.
Robot. Autom., pages 291-302, 1990.
[21] M. Pocchiola and G. Vegter. Topologically sweeping
B. Chazelle, H. Edelsbrunner, M. Grigni, L. J. Guibas, visibility complexes via pseudo-triangulations. Dis-
J. Hershberger, M. Sharir, and J. Snoeyink. Ray crete Comput. Geom., 16:419-453, Dec. 1996.
shooting in polygons using geodesic triangulations. Al-
gorithmica, 12:54-68, 1994. [22] M. K. Ponamgi, D. Manocha, and M. C. Lin. Incre-
mental algorithms for collision detection between gen-
[8 D. Eppstein and J. Erickson. Raising roofs, crashing
coomeaet
eral solid models. In Proc. ACM SIGGRAPH Sympos.
cycles, and playing pool: Applications of a data struc- Solid Modeling, pages 293-304, 1995.
ture for finding pairwise interactions. In Proc. 14th
Annu. ACM Sympos. Comput. Geom., pages 58-67, [23] E. Schémer and C. Thiel. Efficient collision detection
1998. for moving polyhedra. In Proc. 11th Annu. ACM Sym-
pos. Comput. Geom., pages 51-60, 1995.
[9] J. Erickson, L. J. Guibas, J. Stofi, and L. Zhang. [24] S. Suri. Minimum link paths in polygons and related
Separation-sensitive kinetic collision detection for con- problems. Ph.D. thesis, Dept. Comput. Sci., Johns
vex objects. In Proc. 9th ACM-SIAM Sympos. Dis- Hopkins Univ., Baltimore, MD, 1987.
crete Algorithms, pages 327-336, 1999.
Real-time Global Deformations
Yan Zhuang, University of California, Berkeley, CA
John Canny, University of California, Berkeley, CA

Real-time simulation and animation of 3D global de- 1 and 2). To avoid such distortions, we simulate global
formations is the bottleneck of many applications, such deformations using nonlinear finite element methods
as a surgical simulator. In this paper, we present a (Section 3).
system that simulates physically realistic large deforma-
Secondly, we achieve real-time simulation by restrict-
tions of soft objects in real-time. We achieve the phys-
ing time steps, which are not constants, to a small
ical realism by modeling the global deformation using
set of values. Finite element methods usually re-
geometrically nonlinear finite element methods. We ob-
quire solving a large sparse linear system at each time
tain real-time dynamic simulation of models of reason- step. The restriction of time steps leads to a small
able complexity by preprocessing the LU-factorization number of possible matrices to invert. This enables
of a small number of large matrices. To reduce the us to pre-compute all inverse matrices, represented
time and space required for such a preprocess, and the by its LU-factorization. With the precomputed LU-
time of the back-substitution, we apply modified nested factorization, only back-substitution is needed at each
dissection to reorder the finite element mesh. We also time step of the simulation. In Section 5, we dis-
introduce an efficient method that handles deformable cuss our modified nested dissection algorithm, which
object collisions with almost no extra cost. reduces both the time for LU-factorization and that of
back-substitution.
1 Introduction Finally, we propose an efficient collision handling
method for FEM in Section 6. Simulating deformable
Physically realistic modeling and manipulation of de- object collisions using a penalty method [21] requires
formable objects has been the bottleneck of many ap- tiny time steps to generate visually satisfactory anima-
plications, such as human tissue modeling, character tions. A general impulse collision [1] is considered more
animation, surgical simulation, etc. Among the poten- efficient and accurate but still requires more computa-
tial applications, a virtual surgical training system is tional power than collision-free dynamics. In Section
the most demanding for the real-time performance be- 6 we present an extremely simple and efficient. colli-
cause of the requirement of real-time interaction with sion time integration scheme, which makes the time
virtual human tissue. integration of collision dynamics as cheap as that of
In this paper, we address the bottleneck problem of collision-free dynamics.
real-time simulation of physically realistic large global
deformations of 3D objects. In particular we apply 2 Related Work
the finite element method (FEM) to model such de-
formation. By global deformation, we mean deforma- Our work of modeling and simulating a deformable ob-
tions, such as large twisting or bending of an object, ject falls into the realm of physically based modeling.
which involve the entire body, in contrast to poking Witkin et al. [25] summarizes the methods and princi-
and squeezing, which involve a relatively small region ples of physically based modeling, which has emerged
of the deformable object. as an important new approach to computer animation
and computer graphics modeling.
First, we point out that the application of linear elas-
tic finite element methods to simulating large global In general, there are two different approaches to
deformation leads to unacceptable distortions (Figure modeling deformable objects: The mass spring model
 Y. Zhuang and J. Canny
98

fixed left end and a free right end. The middle image
Figure 1: The left image shows a beam at its initial configuration with a
shows the distorted deformation under gravity, using linear strain. The right image shows the undistorted deformation,
under the same gravitational force, using quadratic strain (equation (5) and (6)).

and the finite element model. Gibson and Mirtich [9] Terzopoulos et al. [21, 20, 22] applies both finite dif-
gives a comprehensive review of this subject. ference and finite element methods in modeling elasti-
cally deformable objects. Celniker et al. [15] applies
The mass spring model has had good success in cre-
FEM to generate primitives that build continuous de-
ating visually satisfactory animations. Waters [23] uses
formable shapes designed to support a new free-form
a spring model to create a realistic 3D facial expression.
modeling paradigm. Pieper et al. [16] applies FEM
Provot et al. [18] describes a 2D model for animating
to computer-aided plastic surgery. Chen [3] animates
cloth, using double cross springs. Promayon et al. [17]
human muscle using a 20 node hexahedral FEM mesh.
presents a mass-spring model of 3D deformable objects
and develops some control techniques.
Keeve et al. [11] develops a static anatomy-based fa-
cial tissue model for surgical simulation using the FEM.
Despite the success in some animation applications, Most recently, Cotin et al. [5] presents real-time elas-
the mass spring models do not model the underlying tic deformation of soft tissues for surgery simulation,
physics accurately, which makes it unsuitable for simu- which only simulates static deformations.
lations that require more accuracy. The structure of
James and Pai [10] model real-time static local defor-
the mass spring is often application dependent and
mations using the boundary element method (BEM).
hard to interpret. The animation results often vary
BEM has the advantage of solving a smaller system
dramatically with different spring structures. The dis-
because it only deals with degrees of freedom on the
tribution of the mass to nodes is somewhat (if not com-
surface of the model. However, the resulting system is
pletely) arbitrary. Despite its inaccuracy, it does not
dense. It is difficult to apply boundary element method
have visual distortion and it is computationally cheap
to model non-homogeneous material.
to integrate over time because the system is, by its very
nature, a set of independent algebraic equations, which Our work differs from the previous work by either
requires no matrix inversions to solve. one or all of the following: (1) We simulate large global
deformations instead of small local deformations; (2)
As an alternative, finite element methods (FEM)
we simulate the dynamic behavior of soft objects rather
model the continuum much more accurately and their
than the static deformation.
underlying mathematics are well studied and devel-
oped. Another similar method is the finite difference
3 Nonlinear Elasticity with FEM
method, which is less accurate but simpler and appro-
priate for some applications. Indeed a linear finite dif- By global deformations, we mean deformations that are
ference method over a uniform mesh is just a special large and involve the entire body, such as high ampli-
case of FEM. Its accuracy and mathematical rigorous- tude bending and twisting (Figure 1 and 2). These
ness make FEM a better choice for applications such types of deformation often occur to soft objects, such
as surgical simulations. as tissue in surgical simulations.
Real-time Global Deformations
99

Figure 2: The bottom of the object is fixed and its top is twisted. The top in the left image is distorted (grown bigger)
because it is simulated using linear elasticity. The right image shows that the same distortion does not occur with nonlinear
elasticity.

The theory of elasticity is a fundamental discipline displacements at vertices of the mesh, called nodes,
in studying continuum material. It consists of a consis- will be calculated. The values at other points within
tent set of differential equations that uniquely describe the element are interpolated by continuous functions,
the state of stress, strain and displacement of each usually low order polynomials, using the nodal values.
point within an elastic deformable body. It consists of The global equations (the relationship between all the
equilibrium equations relating the stresses; kinemat- nodal values) are obtained by assembling elementwise
ics equations relating the strains and displacements, equations by imposing inter-element continuity of the
constitutive equations relating the stresses and strains; solution and balancing of inter-element forces.! This
and boundary conditions relating to the physical do- essentially requires solving the following system of dif-
main. The theory was first developed by Louis-Marie- ferential equations:
Henri Navier, Dimon-Denis Poisson, and George Green
in the first half of the 19th century [24].
Synthesizing those equations allows us to establish a Mii + Du+R(u) =F (1)
relationship between the deformation of the object and
the exerted forces. However an analytic expression of where u is the 3n-dimensional nodal displacement vec-
such relationship is impossible, except for a small num- tor; u and u, the respective velocity and acceleration
ber of simple problems. Finite element methods (FEM) vectors; F, the external force vector; M, the 3n x 3n
are one way to solve such a set of differential equations. mass matrix; D, the damping matrix; and R(u), the
From now on, we will discuss elasticity within the con- internal force vectors due to deformation. n is the num-
text of finite element methods. ber of nodes in the FEM model [29].
When the geometry of the deformable object is com- To our best knowledge the published research ((16,
plicated, it is impossible to obtain an analytic solution 3, 11, 5]) assumes small deformations in their virtual
of an elastic deformation. FEM solves this problem environment. The most simulated deformations are
by subdividing the object into small sub-domains with those caused by squeezing and poking at a relatively
simple shapes (tetrahedra, hexahedra, etc.), called fi- small surface region. The small deformation assump-
nite elements. The sub-division (mesh) does not only tion leads to the often used linear elasticity model,
approximate the original geometry, but also leads to a which is based on the following linear strain approx-
discrete representation of the deformation. imations:
In particular, we apply a displacement based finite el-
ement method to simulate such deformation. Namely 1Detailed discussions of FEM can be found in [19, 28].
 Y. Zhuang and J. Canny
100

Ou Ow Ow
Ex
~ Ox (2) Ou Ov Ou Ou Ov Ov
ee
Moy = Bas Tapas
de Oy Ox Oy o}

_ au,
Tey Oy Ox
(3)
The other 4 terms of the strain are defined simi-
larly. It is easy to verify that the above nonlinear strain
handles arbitrary large rigid body motions correctly.
where z, y and z are the independent variables of the
Namely no artificial strain will be introduced when we
cartesian frame, and u, v and w are the corresponding subject the object to a rigid body motion.
displacement variables at the given point. Other terms
of the strain at point (x,y, Z), €y, €z, Yyz and Yza, are This quadratic strain makes (1) a nonlinear system,
defined similarly. in which the internal force R(u) is no longer a linear
term of nodal displacements. If we solve this nonlinear
This linear strain makes the internal force vector lin- system using an implicit integration scheme such as [2],
ear with respect to nodal displacement vector. Namely real time simulation is impossible for reasonably large
it simplifies Equation (1) to the following linear system: meshes.
We observe that a soft material such as live tis-
Mi+Diu+Ku=F (4) sue has small stiffness in all directions (not necessar-
ily isotropic). This makes explicit time integration
This allows a preprocessing step that computes the schemes appropriate because we can take large time
constant stiffness matrix K and its LU factorization. steps. We apply the explicit Newmark scheme to equa-
This preprocessing step has been the key factor to real- tion (1), which leads to the following equations:
time performance in previous works, such as [5], which
animates deformations using a sequence of static equi- 1
Tre ee | aT AN or sun At, (7)
libria.
The problem with this linear strain approximation
is that it does not model finite rotation correctly. As i . Ra t i
(M+ 5 AtnD)iin+1 = Frti-R(un41)—D(tnt5tinAtn)
a result, it introduces distortions when large global de-
formations occur (Figure 1 and 2), because global de- (8)
formations usually involve finite rotation of part of the
object relative to the rest of it. i : Lie z
Un+1 = Up + 3 (un a5 G1 )AG, (9)
To further illustrate this distortion, let us subject
an undeformed object to a rigid body rotation. Ap- Newmark scheme converts a nonlinear system to 3
parently, the rotation should not introduce any defor- linear systems (7), (8) and (9). The order of updating
mation to the object. Namely the strain at any point is (7), (8) and then (9).
within the object should be zero. However equations
(2) and (3) give a nonzero strain. This “artificial” 4 Preprocessing the Inverse Matrices
strain leads to distortion, because the body has to de-
form in a certain way to balance the stress caused by The bottleneck of Newmark scheme is solving Equa-
such an “artificial strain”. tion (8). It requires inverting a large sparse matrix
M + tAtnD. This matrix is not a constant matrix
To avoid the distortion, as shown in Figures (1) and
because the time step At, may vary over time. In-
(2), we model the deformation using the exact strain,
verting a different large sparse matrix makes real time
which is quadratic as following:
performance impossible.
To achieve real-time performance, Zhuang and
Canny [27] approximated this matrix with its row-
lumped diagonal matrix. This is equivalent to diag-
onalizing both the mass matrix M and the damping
Real-time Global Deformations
101

middle 4 middle
left right

(a)
Figure 3: (a) The dissector in the regular nested dissection algorithm is a layer of nodes. (b) The modified nested dissection
algorithm uses a layers of elements (shaded), cut by a plane (or a line in 2D), as the dissector.

matrix D. The diagonalization of M is acceptable be- only requires back-substitution. The time for back-
cause it still preserves the global inertia property of substitution is determined by the number of nonzeros
the object, although it does not preserve the local mo- in the LU-factorization of (M + 5At,D). In Section
ment of inertia. The diagonalization of the damping 5, we discuss how to reduce number of nonzeros in the
matrix may lose important viscous elasticity property LU-factorization.
of the material. For simulations that require more
physical realism, diagonalization of matrix D is not 5 Nested Dissection
appropriate.
A typical finite element simulation has to solve a large
In this section, we propose a different treatment by
sparse linear system of large number of nonzero entries.
preprocessing. The matrices M and D are contants.
For example, a 10 x 10 x 10 linear hexahedral mesh for
The only variable is At,. The time step At, depends
3D linear elasticity gives a sparse matrix of 3993 x 3993
on the stability requirement of the system and the col-
with 242435 non-zeros, which is about 1.5% of the size
lision handling requirement. Instead of approximating
of a dense matrix with the same dimensions. To solve
these two matrices as [27] does, we restrict the time
such a system efficiently, we have to avoid operating on
step At, to a small set of values. Let T' be the largest
zeros aS much as we can. However how efficiently we
time step allowed, we define the restricted set of al-
can do so largely depends on the sparsity of the matrix:
lowed time steps as {T'/2*|i = 0,1,...,m}. We choose
the position of the nonzero entries.
the value m such that T/2” < Tin, where Tin is the
minimum time step in the worst case. Given a finite element model of a physical problem,
the values of non-zeros of (M + 5At,D) are deter-
By restricting time steps to such a small set of values, mined by the underlying model, while the positions
we only have m+ 1 possible matrices needed for the of those non-zeros are determined by the indices of
entire simulation. We can therefore pre-compute the the variables. For convenience, let us denote matrix
m + 1 inverse matrices before the simulation begins. (M + $At,D) by A. The entry (i,7) of A is nonzero
Instead of precomputing the inverse of matrix if and only if the variable xz; and x; are related. Given
(M + $AtnD), we precompute its LU-factorization. such a sparse matrix A = LU, the LU-factorization
Given the LU-factorization, solving Equation (8) takes O(>, dj) space and O()), d?) time, where d; is
 _|
Y. Zhuang and J. Canny
102

Right

Zeros

ci
(a)
500 1000
B: _
1500
Pt
ome
i

: 4°
”

Sie
# arees

4
7000an 3000
PT 115 ee

tea

= 316692

(c)
*i
3500
si

aeB
00
ees

mr
4500 500)

Figure 4: (a) The block structure of sparse matriz A after the first dissection. (b) The sparse matrix structure generated
by nested dissection. (c) The sparse matrix ordering using non-gird nested dissection.

the number of non-zeros in each column vector of £ has one degree of freedom.? A mesh of n nodes leads
[8]. If we assume that no exact numerical cancella- to a sparse matrix A of size n x n. An entry (7,7) is
tions can occur, £ will have non-zeros below the diag- nonzero if and only if the node i and 7 are in the same
onal everywhere that A does. We define fills to be the element.
below-diagonal entries in which £ is nonzero and the For the mesh generated on regular grid (3(a)) The
corresponding entry of A is zero.” regular nested dissection [6] algorithm recursively di-
Different ordering of the row and column vectors of vide the unordered nodes into 3 groups: left, middle
the matrix A has no effect on the underlining physical and right. The group middle is just a set of nodes that
problem that we are solving. However it dramatically completely separate left and right. This algorithm or-
change the number of fills. In order to reduce the space ders the nodes such that its sparse matrix has fractal
and running time for LU-factorization and the time of sparsity as shown in Figure 4(b). After the first step
the corresponding back-substitution, we would like to of recursion, we immediately get 2 blocks of zeros as
minimize the number of fills. Unfortunately finding the shown in Figure 4(a), because left and right are not
order that gives the smallest fills is an NP-complete directly related.
problem [26]. Unfortunately regular nested dissection algorithm
For sparse matrix that arises from regular finite requires a finite element mesh defined on regular grid.
element mesh, George[6] proposed a heuristic called For an unstructured mesh, it would be difficult to find
nested dissection for ordering the variables of the sys- middle to dissect the mesh. In computer graphics, most
tem such that it gives a small number of fills. meshes are unstructured. This motivated us to extend
regular nested dissection to unstructured finite element
Unfortunately a FEM mesh is often unstructured. In mesh.
this section, we propose a modified nested dissection
that works on any unstructured finite element mesh. Instead of separating the set of unordered nodes us-
ing a layer of nodes, we “cut” the mesh by a axis-
aligned plane. It is easy to compute the set of elements
5.1 Modified Nested Dissection
cut by this plane. We let middle be the set of unordered
nodes in the elements cut by this plane and continue
For the simplicity of the presentation, let us consider
recursively as the regular nested dissection.
a 2-dimensional finite element mesh, where each node
’This can be easily generalized to multiple degrees of
7A is symmetric. freedom and 3-dimensional meshes.
Real-time Global Deformations
103

number
fills
of

time
LUfactorization

= ue n n n — . 4 ——_= 4 .
0 500 1000 1500 2000 2500 3000 3500 4000 4500 1500 2000 2500 3000 3500 4000 4500
size (n) size (n)

(a) (b)
Figure 5: The result for minimum degree ordering is plotted in dashed line, non-grid nested dissection in solid line, and
the standard nested dissection in dash-dot line. (a) Number of fills. (b) Running time.

This modified nested dissection can be applied to any Before calling this function the first time, we initial-
unstructured finite element meshes, including tetra- ize each entry of the permutation array perm to -1 (or-
hedral meshes. Figure 3(b) shows one step of such der unassigned), and we compute the centroid of each
a dissection. It also leads to the block structure as element. The function Find-dissector simply computes
shown in Figure 4(a), except that the size of the M- the 3 medians along z, y, and z direction and compare
block is bigger. At the end, we still get an ordering the number of elements cut by the axis-aligned planes
that gives a sparse matrix with fractal sparsity (Fig- thru the medians and return as the dissector the plane
ure 4(c)). Due to the larger size of matrix M, the with the minimum cut.
two “wings” (M — left and M — right) are wider at
each level. 5.2 Running Time
The pseudo code of the modified nested dissection is
It takes O(n) time to find the median given a list of n
as following: numbers [4]. The depth of the recursion is apparently
Modified-nested-dissection(E, top, perm) O(log n). Thus the total running time is O(n logn).
if E€.length = 0 then
return. 5.3. Numerical Experiments
else if €.length = 1 then
for each node j in E[0] do In order to measure the performance of the modified
if perm|j| != —1 do
nested dissection algorithm, we compare its fills and
perm|j] = top
LO Dida LU-factorization time with that of regular nested dis-
endif section and that of minimum-degree algorithm [12, 7].
else All the matrices are derived from a 3-dimension fi-
Find-dissector(€). nite element mesh of different size. The comparison
middle = all the elements cut by the dissector.
of number of fills is listed in Table 1 and that of the
left = elements to the “left” of the dissector.
right = elements to the “right” of the dissector. LU-factorization time is listed in Table 2.
Modified-nested-dissection(middle, top, perm). Minimum-degree ordering is an alternative ordering
Modified-nested-dissection(left, top, perm).
proposed to handle general matrix. It is an greedy al-
Modified-nested-dissection(right, top, perm).
endif gorithm that does the ordering directly on the connec-
 Y. Zhuang and J. Canny
104

nnz random | minimum modified nested

Test
degree nested-dissection dissection

6a
[375 | 15285 |25722|
12384
52131
6|1176_[ 60360 332766
60888
487701 |117900 |
fad
[9 [1728 [94266
Pe |
2187 286929

Pis_[ 2700 [156632

3000 |176700|__|
3300 |195630|__|
P16]-3630_[21e58a] =i
Par_[ 3003 [242535 «| 100817 861705 610515
266008 [ 1262097 979875 684378

Table 1: Number offills for different ordering.

tivity graph defined by the matrix. Our numerical ex- The popular penalty methods [21, 20, 22] model the
periments show that our modified nested dissection al- collision by adding an artificial spring of large stiffness
gorithm has an apparent advantage over the minimum- at the point of collision. This stiff spring requires tiny
degree ordering, in both space and running time. Our integration time steps to stably simulate a collision.
modified nested dissection algorithm produces an order Various experiments show that the ratio between a col-
that has less fills, than the minimum-degree ordering, lision free integration time step and that of a penalty
in 17 of 18 tests, while it has a better LU-factorization collision is on the order of hundreds if not more.
time in all 18 test. The result is plotted in Figure 5. This tempts us to develop new collision-handling
This suggests that the geometry of the mesh gives bet- methods that avoid adding extra artificial stiffness into
ter heuristic than its connectivity graph. the system. We will illustrate our collision-handling
Also it is worth noting that the modified nested method, using a special case: collision between a rigid
dissection ordering requires significantly less time for body and a single node of the FEM mesh of the de-
LU-factorization while only having slightly less fills formable body (Figure 6). Later in this section, we will
than the minimum-degree ordering. This shows that show that it is straightforward to extend this method
the modified nested dissection leads to better sparsity: to handle general collisions of deformable objects.
Non-zeros are more optimally positioned in the matrix. Consider the collision between a moving deformable
body and a moving rigid body (Figure 6). To sim-
6 Collision Handling plify the discussion, we use the moving frame attached
to the moving rigid body instead of the fixed world
For collisions involving deformable objects, the colli- frame. Namely all quantities are relative to the mov-
sion time can be assumed finite (unlike the instanta- ing rigid body. Assume that at time t,, the node p on
neous collision of rigid bodies). This allows a larger the deformable object, with relative velocity ¥(p)n, is
time step for numerical integration. colliding with the rigid surface of outward normal n.
Real-time Global Deformations
105

LU factorization time (seconds)

nnz | random modified nested
degree nested-dissection
Fd ldRCT ad

[6 [1176-60360
[8 [1556 | 82956
[9 [tras |94366-| 86.30 [8
|
4.52
138870
2700
95630

Table 2: Time measured on HP9000/715.

By Equation (9), we get:

rigid surface js :
~
ve deformable body
are
‘
eetn
2
eae
2V(D)n
Atn
If we choose Atp+1 = Atn, by Equation (7), we have:

Un+2 -n=u,-n (12)

This shows that the non-penetration constraint is en-

Figure 6: A flexible body collides with a rigid body. forced after two time steps, because there is no relative
motion of the deformable body normal to the surface
The non-penetration constraint requires that the nor- of the rigid body.
mal component of the relative velocity of point p drops This collision-handling integration scheme can be
to zero at the moment of collision in the moving frame. considered a special case of impulse [1]. For rigid body
Unlike a rigid body collision, the flexible body will collisions, an impulse requires extremely small time
maintain contact with the rigid body for a nonzero steps for numerical integration because the rigid body
period of time. We enforce the non-penetration con- collision is considered to occur instantaneously. How-
straint at node p by setting the normal component of ever, for deformable body collisions, the collision time
V(p)n+1 to zero as following: is finite. By delaying the non-penetration constraint
by two time steps, we are able to integrate the impulse
V(p)n41 = V(p)n + (¥(P)n - YA (10) using large time steps.
 Y. Zhuang and J. Canny
106

This collision integration scheme can be generalized better algorithm using sweeping-line (plane). By us-
to collisions between deformable bodies and collisions ing a sweeping-line (plane) approach, we may be able
that involve multiple point contacts. Multiple point to find a “thin” layer of nodes (rather than a layer of
collisions are modeled as a set of simultaneous single elements) to separate the mesh. This way, we would
point collisions. be able to generalize the regular nested dissection algo-
rithm to unstructured mesh without giving away any
Unlike a general impulse [1, 13, 14], we do not have
performance. We are currently studying this approach.
to distinguish the case that the colliding deformable
objects quickly bounce away from each other and that We also introduced an efficient collision constraint.
one sticks to or slides on the surface of the other. The This constraint enables us to simulate the collisions
bouncing collision, the sticking and sliding contacts, with little extra computation, compared to a collision
are handled by exactly the same collision integration free simulation step. Our experiments show that this
constraint. This collision constrain adds little extra collision constraint handles collision much more effi-
cost to the dynamic simulation. cient than penalty method.
Currently our system is able to produce real-time
7 Conclusions and Future Work graphics for a mesh of several hundred vertices. We
are interfacing our system to haptic devices, such as a
We presented a simulation system that simulates global Phantom, so that users can interact with deformable
3D deformations in real-time. Due to the distortion as- objects in real-time.
sociated with linear strain, we simulate the global de-
formation using geometrically nonlinear finite element Acknowledgments
methods. The nonlinear FEM formulation is derived
from the application of the nonlinear exact strain. Supported by a Multi-Disciplinary Research Initiative
It is in general too expensive to solve such a nonlinear grant for 3D Visualization, sponsored by BMDO with
FEM system in real time. In order to achieve real-time support from ONR. We thank Panayiotis Papadopou-
performance, we pre-compute the LU-factorization of los for sharing with us his FEM expertise and Jonathan
a small number of large sparse matrices. Such pre- Shewchuk for his insight in 3D meshing.
processing is possible because we restrict the time steps
to a small set of values. Our experiments show that References
usually we only need no more than 3 different values
for time steps. [1] David Baraff and Andrew Witkin. Dynamic simula-
tion of non-penetrating flexible bodies. In Computer
To reduce the time and space for LU-factorization Graphics: Proceedings of SIGGRAPH, pages 303-308.
and the time of back-substitution, we apply nested ACM, 1992.
dissection to reorder the vertices in the finite element
mesh. Such a reordering does not change the physi- [2] David Baraff and Andrew Witkin. Large steps in cloth
simulation. In Computer Graphics: Proceedings of
cal model that we are simulating. But it dramatically
SIGGRAPH, pages 303-308. ACM, 1998.
reduce the number of nonzeros in the LU-factorization.
We modified the regular nested dissection algorithm [3] David Chen. Pump It Up: Computer Animation of
a Biomechanically Basded Model of Muscle Using the
so that it works on unstructured finite element mesh. Finite Element Method. PhD thesis, MIT, 1992.
The modified nested dissection ordering takes 30% to
50% more time for the LU-factorization than the reg- [4(Aes Thomas H. Cormen, Charles E. Leiserson, and
ular nested dissection, however it is more general than Ronald L. Rivest. Introduction to Algorithms. The
the regular nested dissection: it is able to handle any MIT Press, 10 edition, 1993.
unstructured finite element mesh. [5 Stéphane Cotin, Hervé Delingette, and Nicholas Ay-
Our current implementation simply uses a cutting ache. Real-time elastic deformations of soft tissues
plane and separate the mesh using the elements inter- for surgery simulation. IEEE Transcation on Visual-
ization and Computer Graphics, 5(1):62-73, January-
sected by the cutting plane. It seems that there is a March 1999.
Real-time Global Deformations
107

[6] Alan George. Nested dissection of a regular finite

[17] E. Promayon, P. Baconnier, and C. Puech. Physically-
element mesh. SIAM Jounal of Numerical Analysis, based deformations constrained in displacements and
10(2), 1973. volume. In EFUROGRAPHICS, 1996.

[7] Alan George and Joseph W. H. Liu. A fast implemen- [18] X. Provot. Deformation constrains in a mass-spring
tation of the minimum degree algorithm using quotient model to describe rigid cloth behavior. Computer In-
graphs. ACM Transaction on Mathematical Software, terface, 1995.
6(3), September 1980.
[19] J. N. Reddy. An Introduction to the Finite Element
Alan George and Joseph W. H. Liu. Computer Method. McGraw-Hill, Inc., 2nd edition, 1993.
Solution of Large Sparse Positive Definite Systems.
Prentice-Hall, Inc., 1981. [20] D. Terzopoulos and K. Fleischer. Modeling inelas-
tic deformation: Viscoelasticity, plasticity, fracture.
Sarah F. Gibson and Brian Mirtich. A servey of de- Computer Graphics, 22, August 1988.
formable models in computer graphics. Technical Re-
port TR-97-19, Mitsubishi Electric Research Labora-
[21] D. Terzopoulos, J. Platt, A. Barr, and K. Fleischer.
Elastically deformable models. Computer Graphics,
tories, Cambridge, MA, November 1997. 21, July 1987.

Doug L. James and Dinesh K. Pai. Artdefo: Accu- [22] D. Terzopoulos and K. Waters. Physically-based facial
rate real time deformable objects. Computer Graphics: modeling, analysis and animation. Journal of Visual-
Proceedings of Siggraph, pages 65-72, August 1999. ization and Computer Animation, 1990.

[11] E. Keeve, S. Girod, P. Pfeifle, and B. Girod. Anatomy- [23] K. Waters. A muscle model for animating three-
based facial tissue modeling using the finite element dimensional facial expression. Computer Graphics,
method. JEEE Visualization, 1996. 21(4), July 1987.

[12] Joseph W. H. Liu. Modification of minimum-degree [24] H. M. Westergaard. Theory of Elasticity and Plastic-
algorithm by multiple elemination. ACM Transaction ity. Dover Publications, Inc., 1964.
on Mathematical Software, 11(2), June 1985.
[25] A. Witkin and et al. . An introduction to physically

[13] Brian Mirtich and John Canny. Impulse-based dy-

namic simulation. In K. Goldberg, D. Halperin,
based modeling. Course Notes, 1993.

[26] M Yannakakis. Computing the minimum fill-in is np-

J.C. Latombe, and R. Wilson, editors, The Algorithm complete. SIAM Journal of Algrbraic Discrete Meth-
Foundations of Robotics. A. K. Peters, Boston, MA, ods, 2, 1981.
1995. Proceedings from the workshop held in Febru-
ary, 1994. [27] Yan Zhuang and John Canny. Real-time simulation of
physically realistic global deformations. JEEE Visual-
Brian Mirtich and John Canny. Impulse-based sim- ization: Late Breaking Hot Topics, October 1999.
ulation of rigid bodies. In Symposium on Interactive
3D Graphics, New York, 1995. ACM Press. [28] O. C. Zienkiewicz and R. L. Taylor. The Finite El-
ement Method: Basic Formulation and Linear Prob-
lems, volume 1. McGraw-Hill Book Company, 4th
[15] G. Celniker nad G. Gossard. Deformable curve and
edition, 1989. linear finite element method, linear elas-
surface finite elements for free form shage design.
Computer Graphics, 25(4), 1991. ticity.

[29] O. C. Zienkiewicz and R. L. Taylor. The Finite El-

[16] S. Peiper, J Rosen, and D. Zeltzer. Interactive graph-
ement Method: Solid and Fluid Mechanics Dynam-
ics for plastic surgery: A task-level analysis and im- ics and Non-Linearity, volume 2. McGraw-Hill Book
plementation. In Symposium on Interactive 3D Graph- Company, 4th edition, 1989.
ics, 1992.
 cy ASA} 9 ne AA le Qe eee
eel ph mean, icalae ; "|a:7)
- . ; abe} :

ei tie den? Lot, ates 7 wendit le

Javits gr Satria iy ba wet) ig Gombeyiere ma
creom Trad tunel gee A yegy) uae ’ DAR tee

f tg Stee Lae alta

reG@uio aide Sil AN See Aare. Hh 7

at Tain a} ne al

con Ket" ite hh owed ie ar Ee ; a

ee Th bowtie Pg ged a ie es
aye!) Doe wall Aeaecasiie A TOY sien
YaaRs “eh Mir! apdite ae 4 agicina 0
i ae srohiicd_ i
z, gyi 0 ve | aren Oe Sie slim Giettamasdenalt Ides “
Lokal, ;a ms irda Ui alien ae nec
by wldA gst
uae" A ai ites: Grill ere, era rh PorLiata pepe bs
tah)

ar. iT its ;
4 te, ahaa he Here b ‘ues waht" ¢ ety e a8 trp.~
tit = igdapey

gigs: erg ne eee eM a ea Pity a

— iesec eh, vtet” (has * Sp guemnns’: Baal tretear”

me poll
ih a
ea
—e es
e =e ste eoie Tor ea al
y “ti «i

hi shetesfrT cosine
Nab. - _

BRL shaw one dab babricia mana) P

: m » Desig’: enw iesd oud 4 At
vt Tapeh) Meuiartt m te nsarisesi ante i) Aa aii)
<i \ereriny:, MR?heh we Hye Tee me, * vo Shaws
| ‘seg .f ae A, aay | yt et ae : axfeus\ \ ob

Thea afe - nt:

ae aaa= 3a urfl de sits Tie 7 uidd aC iled ieSaGh al Gee onan ae
‘
AS
et PPS Baers: ery
?
ae
a _—s <yereete
ay lel eit ues peel Et Aye ti ug Sahay tet? do Saber doit
iat m Pe Pts diitiais 8) G be SE eae ater ead a etle
Ls oa oe (NET e rs hte at 1.4 ny ‘a , teas i : a EN, oad woh ie i.
; pak debt ATM vats) US 4 owhufal 18) hii ria Seed ozs bese :
Re eres: Greely sid igge'!. 2D} it DAUR UG ieee] >hia is
ee
= WaT al RMT Bemphy eile carralitrles (i
*S hn ae —
Cr as eb 1 te lak ase 0 est F > wy ani +» @e A va

PAST How er) Wale he, saltn “hey 13 ie) Ly a bins

| DEPOT” 2 Grete trays) oO —— “1 Site ay Teal —— hs
aw vy EE Mraltiter ab yaks ys hate SY oe een we
Fry Oi ‘ : y
Motion Planning for Kinematic Stratified Systems
with application to Quasi-Static Legged Locomotion
and Finger Gaiting
Bill Goodwine, University of Notre Dame, Notre Dame, IN
Joel W. Burdick, California Institute of Technology, Pasadena, CA

We present a general motion planning algorithm for multi-fingered hand manipulation. The method is in-
robotic systems with a “stratified” configuration space. dependent of the number of legs (or fingers) and many
Such systems include quasi-static legged robots and other aspects of a robot’s morphology. In the legged lo-
kinematic models of object manipulation by finger repo- comotion context, it is distinct from previous planning
sitioning. Our method extends a nonlinear motion methods in that it is not based on foot placement con-
planning algorithm for smooth systems to the stratified cepts, and therefore the computationally burdensome
case, where the relevant dynamics are not smooth. The calculation of foot placement can be avoided. Instead,
method does not depend upon the number of legs or fin- our approach focuses on control inputs.
gers, nor is it based on foot placement or finger place-
ment concepts. Examples demonstrate the method.

1 Introduction

This paper considers the motion planning problem for

systems whose governing equations of motion impose
a “stratified” structure on the system’s configuration
space (see Section 3). Stratification naturally arises
in the context of legged locomotion and object ma-
nipulation via finger repositioning. These operations
are characterized in part by the system making and
breaking contact with its environment. The configura-
tion spaces (or c-space) of these systems are “stratified”
into subsets that correspond to different contact states.
The governing dynamical equations depend upon the
contact state, and the dynamical equations are discon-
tinuous during the making and breaking of contact.

The goal of our motion planning scheme is to deter-

mine the control inputs (e.g., mechanism joint variable
Figure 1: (a) Schematic of simple herapod robot; (b) De-
trajectories) which will steer the walking robot from a
starting to a desired final configuration, or to manipu- finition of kinematic variables.
late the grasped object from an initial to a final config-
uration via a combination of finger repositioning and As a concrete example of when such a planner is
finger motions. The planner must simultaneously plan needed, consider the hexapod in Figure 1 (this model
the mechanism’s motion during a single contact state, will be explored in Section 5). Each leg has only two
as well as determine when to change contact states. degrees of freedom—the robot can only lift its legs up
This paper presents a general motion planning method- and down and move them forward and backward. Con-
ology for this class of systems, which includes all quasi- ventional hexapods are designed with three indepen-
static legged locomotors and many kinematic models of dent degrees of freedom per leg. The limited control
 W. Goodwine and J. Burdick
110

authority in this design may be desirable in practical stages of legged robot system design. While the tech-
situations because it decreases the robot’s mechanical niques outlined in this paper are equally applicable for
complexity. This leg geometry can also probably be im- locomotion and hand manipulation, the bulk of the pa-
plemented at very small size scales using MEMS tech- per will focus on locomotion, with the application to
nology. However, such decreased kinematic complex- hand manipulation briefly sketched at the end of the
ity comes at the cost of requiring a more sophisticated paper. An interesting observation, which is not ex-
control and motion planning theory. Note that for this plored in this paper, is that our technique is equally
robot, it is not immediately clear if it can move “side- applicable to both locomotion and manipulation.
ways.” There is a vast literature on the analysis and control
The issue of this mechanism’s ability to move side- of legged robotic locomotion. Prior efforts have typ-
ways is the controllability problem. In Refs. [9, 8] we ically focused either on a particular morphology (e.g.
present controllability tests for stratified systems. We biped [17], quadruped [19, 2], or hexaped [32]) or a par-
assume in this paper that a given system is controllable ticular locomotion assumption (e.g. quasi-static [32] or
in the stratified sense. Otherwise, it is not possible to hopping [25]). Less effort has been devoted to uncover-
track an arbitrary trajectory. Given the assumption ing principles that span all morphologies and assump-
of controllability, this paper addresses how to plan the tions. Some general results do exist. For example, the
robot’s leg (or finger) movements so that it can approx- bifurcation analysis in Ref. [5], many optimal control
imately follow a given trajectory. A conventional “foot- results such as those in Ref. [3] and the fundamental
placement” approach, where the foot can be placed as conservation of momentum and energy results that un-
necessary to implement vehicle motion, will clearly not derlie Raibert’s hopping results [25] have general ap-
work for the hexapod of Figure 1, because sideways leg plicability. However, none of these methods directly
placement is impossible. use the inherent geometry of stratified configuration
Our approach is motivated by the method of Laffer- spaces to formulate results which span morphologies
riere and Sussmann [18] for motion planning for a class and assumptions. Our work makes a novel connec-
of nonlinear kinematic systems whose equations of mo- tion with recent advances in nonlinear geometric con-
tion are smooth. However, since legged robots (and trol theory. We believe that this connection is a useful
grasping hands) intermittently make and break con- and necessary step towards establishing a solid basis
tact, their equations of motion are not smooth. Hence, for locomotion engineering.
the method of Ref. [18] cannot be directly applied. In contrast to robotic legged locomotion, many re-
Section 3 introduces the notion of a stratified c-space, sults in robotic grasping and manipulation are formu-
which is decomposed into various subspaces (or strata) lated in a manner that is independent of the morphol-
depending upon which combination of feet are in con- ogy of the gripper [24]. Vast efforts have been directed
tact with the ground. We extend the approach of Ref. toward the analysis of grasp stability and force closure
[18] by using the stratified c-space structure in a novel (26, 27, 31], motion planning assuming continuous con-
way. It is likely that other methods for steering smooth tact [20, 33, 13] and haptic interfaces and other sens-
systems (such as Ref. [22]) can be similarly extended by ing [6, 29, 28]. Finger gaiting, where fingers make and
adopting our framework. A main contribution of this break contact with the object has been less extensively
work is the introduction of a geometric framework that considered. Finger gaiting has been implemented in
supports the extension of prior nonholonomic motion certain instances [23, 15, 7] and also partially consid-
planning techniques to this class of systems. ered theoretically [14, 4, 10]. Perhaps the approach
Our approach is general and thus works indepen- which most closely mirrors that of the subject of this
dently of the number of legs (fingers). It may be true proposal is in [24] where notions of controllability and
that for a given quasi-static legged robot, one could de- observability from “standard” control theory are ap-
velop a specific motion planner that would perform as plied to grasping (however, these results are limited to
well, or possibly better, than the technique described the linear case and do not allow for fingers to intermit-
in this paper. The key advantage of this approach is tently contact the object).
its generality. It is particularly well suited to the task Section 2 briefly reviews standard ideas, and summa-
of quickly designing a planner during the preliminary rizes the motion planning method of Ref. [18]. Section
Motion Planning for Kinematic Stratified Systems
1!
3 introduces our notion of a stratified c-space. Section Theorem 1 Given two smooth vector fields 91, 92 the
4 presents our algorithm in the context of quasi-static composition of their exponentials is given by:
legged locomotion, while Section 5 applies this algo-
rithm to the system of Figure 1. Section 6 sketches et @92 — e91t92+3191,92]+
73([91,[91,92]]—-[92,[91,92]])-- (4)
the application of these ideas to multi-fingered hand
manipulation, and presents an example. where the remaining terms may be found by equating
terms in the (non-commutative) formal power series
2 Background on the right- and left-hand sides.

We assume the reader is familiar with the basic nota- 2.1 Trajectory Generation for Smooth
tion and formalism of differential geometry and non- Systems
linear control theory, as in Ref. [16]. The following
This section reviews the motion planning method of
definitions and classical theorems are reviewed so that
Ref. [18] for smooth kinematic systems described by a
the starting point of our development will be clear.
single equation having the form of Equation (1).
The equations of motion for smooth kinematic non-
A nonholonomic control system often does not have
holonomic systems take the form of a driftless nonlin-
enough controls to directly drive the system along a
ear affine control system evolving on a configuration
given trajectory, i.e., the number m in Equation (1)
manifold, M:
is less than the c-space dimension. In the method of
= Gi(2)Uy +---+Gm(t)Um xceEM. (1) Ref. [18], this deficit is managed by using an “extended
system,” where “fictitious controls,” corresponding to
Since we restrict our analysis to quasi-static locomotion
higher order Lie bracket motions, are added. If enough
and kinematic models of multi-fingered manipulation,
Lie brackets are added to the system to span all pos-
the governing equations of motion will piecewise take
sible motion directions (which is possible if the system
the form of Equation (1) on each strata. Recall that
is locally controllable), then the motion planning prob-
the Lie bracket between two control vector fields, gi (z) lem becomes trivial for the extended system.
and g2(x), is computed as:
The extended system is constructed by adding Lie
(91(2),92(2)] = P8247)
9,(q)— HX 9 (a) bracket directions to the original system from Equa-
tion (1),
and can be interpreted as the leading order term that
results from the sequence of flows ES BU ER Bab GO Ge aS)

G29 0 G-% 0 bf 0 G(x) = (91, g2](x) + O(€*), (2) where 6; = g; for? = 1)...,m, and the 0n41,-.-,05
correspond to higher order Lie brackets of the g;, cho-
where $2 (x9) represents the solution of the differential
sen so that dim(span{b,,...,6;}) = dim(Z,M). The
equation £ = gi(x) at time € starting from Zo.
vs are called fictitious inputs since they may not cor-
Campbell-Hausdorff Formula. The flow along respond with any actual system inputs. The higher
the vector field g; can be considered by its formal ex- order Lie brackets must belong to the Philip Hall basis
ponential of g;, denoted by: (30, 21] for the Lie algebra. The control inputs v' which
2 steer the extended system can be found as follows. To
(a) = e'%(2) = (I+ tat a+) (3) go from a point p to a point q, define a curve y(t) con-
necting p and q (a straight line would work, but is not
where terms of the form g? are partial differential oper- necessary). After determining y(t), simply solve:
ators. In order to use Equation (3), composition must
be from left to right, as opposed to right to left for ¥(t) = gi(y(t))v +--+ + gs(r(t))e* (6)
flows, ¢.g., $72 0 of! = e%"1e9"2, where both sides of
this equation mean “flow along g; for time t; and then for the fictitious controls v;. This will involve invert-
flow along gz for time t2.” The relationship between ing a square matrix or determining a pseudo-inverse,
the flow along vector fields sequentially is given by the depending on whether or not there are more 6,’s than
Campbell-Baker-Hausdorff formula [34]. the dimension of the configuration space.
112 W. Goodwine and J. Burdick

To find the actual controls, first determine the Philip of order greater than two are inconvenient to control
Hall basis for the Lie algebra generated by g1,..-,9m; since many motions are needed to effect even a small
and denote by B,, Bo,...,B, a collection of basis el- motion in a higher-order Lie bracket direction. For this
ements such that when they are evaluated as vector reason, and for purposes of the clarity of presentation,
fields, they form a basis space of vector fields. All flows we limit our attention to second order brackets. How-
of Equation (1) can be represented in the form: ever, there is no theoretical limitation on the order of
brackets.
S(t) = elts(t) Bs phs—1(t)Bs-1 at e2(t) Be phi (t)Bi (7)
If the system is nilpotent, this method exactly steers
for some functions hy, h2,..., hs, called the (backward)
the system to the desired final state. Else, the system is
steered to a point that is, at worst, half the distance to
Philip Hall coordinates. Furthermore, S(t) satisfies the
the desired state [18]. The algorithm can be iterated to
formal differential equation:
generate arbitrary precision. This iterated method also
S(t) = S(#)(Byu.
+ +--+ Bsvs); S(0)=1. (8) includes the notion of a “critical” step length. Ref. [18]
estimates the critical step length bound, and shows via
If we define the adjoint mapping: simulations that the actual critical length is typically
larger than the estimated bound.
Ad,-1;3; Be — en Bie tht

3 Stratified Configuration Spaces

then it is straight-forward to show that:
The method reviewed in Section 2.1 can not be di-
Bde) 6828s Byhz = (drs) hs; (9) rectly used for legged or multi-fingered robots because
k=1 their governing equations of motion are not smooth. To
adapt this method (and similar nonholonomic motion
for some polynomials p;,(h). (For a complete deriva- planning methods) to these systems, we use the notion
tion, see Ref. [21]). Equating coefficients of Equa- of a stratified configuration space. While the stratified
tion (8) with the derivative of Equation (7), and using concept is equally applicable to locomotion and multi-
Equation (9), yields differential equations having the fingered manipulation, the language of locomotion is
form used below for simplicity.
h= A(h)v (0) ='0: (10)
Let So denote a robot’s configuration manifold,
These equations specify the evolution of the backward which describes the robot’s position and orientation
Philip Hall coordinates in response to the fictitious in- as well as all of the mechanism’s joint variables. The
puts, which were found via Equation (6). robot’s possible configurations will be subjected to con-
Next one must determine the actual inputs from the straints if one or more of its feet (fingers) are in contact
Philip Hall coordinates. It is easier to determine the with the ground (object). The set of configurations
real inputs using the forward rather than backward corresponding to one contact is generically a codimen-
Philip Hall coordinates. The transformation from the sion one submanifold of So. Let S; C Sp denote the
backward to forward coordinates is an algebraic one codimension one submanifold of So that corresponds
(see [18]). For systems which are nilpotent! of order to all configurations where only the i*” foot contacts
two, or which can be well approximated as nilpotent the terrain. That the {S;} are submanifolds can be
of order two, the transformation between forward and demonstrated by noting that set of points correspond-
backward Philip Hall coordinates can be avoided. In ing to ground contact can be described by the preimage
these cases, the actual controls can be obtained from of a function describing the foot’s height. We generally
the fictitious controls by use of Lie-bracket-like motions assume that S;, is, at least locally, defined by a level
where necessary. In practice, this will often be the case, set of a function ®;(x) : Sy > R.
since physical systems that require Lie bracket motions When both the i” and j* feet are on the ground, the
‘A system of the form Equation (1) is said to be nilpotent corresponding set of states is a codimension 2 subman-
of order k if all the Lie brackets between control vector fields ifold of So that is formed by the intersection of the two
of order greater than k are 0. single contact submanifolds. Denote the intersection of
Motion Planning for Kinematic Stratified Systems
113
M Neither Foot
("Contact
with other submanifolds) removed. By abuse of nota-
tion, we will refer to the submanifolds Oi, ig) OTGhe COL
ee as strata. We will term the highest codimension stra-
tum containing the point « as the bottom stratum, and
Left Foot any other submanifolds containing x as higher strata.
in Contact DN
When making comparisons among different strata, we
Right Foot
will refer to higher codimension (i.e. lower dimen-
in Contact
sional) strata as lower strata, and lower codimension
(i.e. higher dimensional) strata as higher strata.
Whenever an additional foot contacts the ground,
Both Feet the robot is subjected to additional constraints. For
in Contact “point-like” feet, this may be a holonomic constraint;
whereas some contacts are better characterized by non-
Figure 2: Abstract depiction of the stratified structure of holonomic constraints. Regardless of the constraint
a biped robot c-space. type, the system’s equations of motion will change in
a non-smooth manner. Otherwise, the system’s equa-
S; and S;, by S;; = 5; S;. The structure of the con- tions of motion are smooth, though generally different
figuration manifold for a biped is abstractly illustrated in each strata. Hence, the discontinuities are localized
in Figure 2. For systems with larger numbers of legs to regions of transition between strata. Furthermore,
(fingers), further intersections, corresponding to more we assume that on each stratum, S;, our control system
complicated contact states, can be similarly defined in may be subjected to constraints in addition to those
a recursive fashion: Sij, = 5; S5;0 Sk = 539 Sjx, present on So. On any given stratum, the system is
etc. We denote an arbitrary intersection set (or “stra- subjected to at least all the constraints present on all
tum”) by S; = Si,io...i,, [ = {ii2g---in}. We assume the higher strata whose intersection defines that stra-
that S; is a regular submanifold of So. This is gener- tum. Thus, when the system transitions from So to S;,
ically true for rigid body mechanisms. If the strata if the system is going to evolve on the stratum S$; for
Six, Sig,--+, Si, are locally described by the functions some finite time, the system must not only satisfy all
®;,,;,,...,®;,, then S; will be a submanifold of So the constraints that are present on the stratum, but
if the functions ®;,,®;,,...,®;, are functionally inde- also the constraint d®;(x)z = 0.
pendent. If the functions ®; correspond to foot heights, The equations of motion at x € S; are written as
this functional independence will be satisfied.
L= gra(x)ur FA OF,(eyo (11)
We say that the robot c-space is stratified 7. Classi-
cally, a regularly stratified set X isa set X CIR™ decom- where nz depends upon the codimension of S; and the
posed into a finite union of disjoint smooth manifolds, nature of the additional constraints imposed on the
called strata, satisfying the Whitney condition. The system in S;. We assume that the vector fields in the
dimension of the strata varies between zero, which are equations of motion for any given stratum are well de-
isolated point manifolds, and m, which are open sub- fined at all points in that stratum, including points
sets of R™. The Whitney condition requires that the contained in any substrata of that stratum. For exam-
tangent spaces of two neighboring strata “meet nicely,” ple, the vector fields go ;(x) are well defined for z € Sj.
and for our purposes this condition is generically sat- Note, however, that they do not represent the system’s
isfied (see Ref. [12] for details). equations of motion in the substrata, but, nonetheless,
are still well defined as vector fields.
In the classical definition of a stratification [12], stra-
tum 4; consists of the submanifold S; with all lower Figure 3 illustrates, via a graph-like structure, a
dimensional strata (that arise from intersections of 5; four-level stratification, which corresponds to a four—
legged walker. A node corresponds to a stratum, and
2Note that the terms “stratification” and “strata” are the presence of an edge connecting nodes indicates that
also used in other contexts to describe the topology of orbit it is possible to move between the strata that are con-
spaces of Lie group actions, and are a slight generalization
of the notion of a foliation [1]. nected by the edge. The ability to move between two
 W. Goodwine and J. Burdick
114

each i, and the intersection of any number of level sets,

ers), Level 0 Sixignim = ,1(0) 17,10) N---N O10), m <0,
is also a regular submanifold of So. Then So and the
functions ®,, define a stratified configuration space.

For a given strata, S7, the distribution defined by the

span of the control vector fields active on S7 is:
As, = span OLY O

The involutive closure of Ag,, denoted by Ag,, is the

closure of Ag, under Lie bracketing. A basic assump-
tion is that the robot is controllable. The controlla-
bility of a given gait, Equation (12), can be deter-
Level 4
S mined as follows. Let D,; = Ay;,. If Sz,_, C Sz,
1234
then D; =" Diii + Ay: Else, if S;, C Sz,_,, then
Figure 3: Four Level Stratification. D; = (Di-1 NT'S1,) + Ar, In Ref. [8] it s shown that if
dim(Dn) = dim(T;,Sp) the system is gait control-
lable from Zo (i.e., the system can reach open nbhd of
strata depends upon the mechanics of a given problem,
xo in the bottom strata). For a more rigorous discus-
and will generally be obvious from the characteristics of
sion and summary of stratified system controllability,
a given problem. Whether or not edges between nodes
see Refs. [9, 8].
are permissible is considered in more detail in Ref. [8].
We specify a gait as an ordered sequence of strata: 4 Legged Trajectory Generation
Gad Sr Sige 813 Sts =a9;, \3 (12) This section extends the procedure outlined in Sec-
tion 2 to kinematic systems having a stratified c-space.
where n is the number of different contact states in We focus on quasi-static legged locomotion in this sec-
the gait. In this ordered sequence, the first and last tion. However, all of these ideas can be readily ex-
element are identical, indicating that the gait is a closed tended to the finger gaiting problem. Section 6 sketches
loop in the strata graph. For the gait to be meaningful, the extension to hand manipulation.
the system must be able to switch from stratum S7, to Assume that the robot starts at a configuration p
S7,,, for each 7. We further assume that the specified
and seeks to reach a final configuration g. By a con-
gait or gaits satisfy the gait controllability conditions
figuration, we mean the position and orientation of the
of Ref. [8] so that arbitrary trajectories can be tracked.
body, as well as the states of the legs. We assume that
In summary, we assume that the only discontinuities both p and q lie in the same bottom stratum, denoted
present in the equations of motion are due to transi- by Sg. This corresponds to the legged robot starting
tions on and off of the strata. We also make a simi- and stopping with the same set of feet in contact with
lar assumption regarding the control vector fields re- the ground. Eliminating this requirement is a simple
stricted to any stratum, 7.e., the control vector fields extension of the algorithm described below.
restricted to any stratum are smooth away from points The switching behavior associated with stratified
contained in intersections with other strata. When a systems can not be accounted for in the methods of
configuration manifold is consistent with the above de- Section 2.1. However, the method of Section 2.1 can
scription, we will refer to it as a stratified configuration be extended to legged and fingered robotic systems via
space. the notion of a stratified extended system on Sp.

Definition 2 Let So be a manifold, and n functions 4.1 The Stratified Extended System
®; : So H+ R, 7 = 1,...,n be such that the level sets On each strata, only one set of governing equations is
S; = 67'(0) C So are regular submanifolds of So, for in effect. Generally, the equations of motion in the
Motion Planning for Kinematic Stratified Systems
115

The notation “Siz ~— S,” means that the flow takes the
system from S; to Sy2 and “on S$,” means that the flow
lies entirely in S. This sequence of flows is illustrated
in Figure 4. In this sequence, the system first moved off
of the bottom stratum into $1, flowed along the vector
field 91,2, flowed back onto the bottom stratum, off of
the botiom stratum onto S2, along vector field g2,2 and
back to the bottom stratum.
Notice that from the Campbell-Baker—Hausdorff for-
mula (Equation (4)), if the Lie bracket between two vec-
tor fields is zero, then their flows commute. Thus, if

[91,1 91,2]=0 and [921,922] =0, (14)

we can reorder the sequence of flows in Equation (13)
Figure 4: Sequence of Flows. by interchanging the flow along gi, and gi and the
flows along g2,1 and go as follows

bottom strata will be different than those in higher = t

LAN 5 oo 2 o g's
SUP Ri Opes, 1 obf 2 og =a, 1 ops), (20)
strata. Furthermore, it will be typically true that the ee —
goal g can not be reached by remaining in Sg. Hence, interchanged interchanged
some switching amongst the strata will be necessary.
| Pos,2 by 2(x 0); (15)
However, since the bottom strata is defined by the in- ee
On S12
tersection of higher strata, the equations of motion in
the higher strata are valid at points arbitrarily close if ty = t3 and tie = UB
to the bottom strata. As shown below, it is possible
Note that gi,2 and g2,2 are vector fields in the equa-
to consider the vector fields associated with each stra-
tions of motion for strata S, and S2, respectively, but
tum in one common space. In this case, that common
not on stratum Sj2. However, the sequence of flows
space will be the bottom stratum. This concept will
in Equation (13) occurs on different strata, where the
be encapsulated below in the definition of a “stratified
flows are governed by vectors fields associated with each
extended system.” We first introduce some examples
stratum. This flow yields the same net result as the net
to show how we can consider vector fields defined on
flow in Equation (15), where the vector fields are eval-
different strata in a common space. Additional exam-
uated on the bottom stratum, even though they are not
ples that deal with more subtle issues can be found in
part of the equations of motion there. Furthermore, we
Ref. [11].
note that if the vector fields 91,2 and go are tangent
Example 1 Consider the conceptual biped configura- to the substratum So, then the resulting flow given in
tion space as shown in Figure 2. Assume that on stra- Equation (15) will remain in Sj2. In fact, it is implic-
tum S12, the vector field g1,1 moves the system off of itly required in the above argument that at least gi, is
Si2 and onto S$, and correspondingly, g2,1 moves the tangent to S42.
system off of Si2 onto Sp. Also, we consider the vector If the bottom stratum, Sp, is described by the level
fields 91,2 and g2,2, defined on S; and S2 respectively. set of a function, ®g, and if a vector field, 91,2 is not
Consider the following sequence of flows, starting from tangent to Sg, then, (d®g, 91,2) = fi #0. Also, since
the point ro € Si2 the vector field gi, moves the foot out of contact, we
similarly have (d®p, 91,1) = fo #0. Then, the vector
Lf = gs g2,1 ° $52 ° $i, °0 $8 91,1 3
=—— aa field, 91,2 = 91.2'= fou, is tangent to Sp because
Si2e+Sg on
a, eee Sie+Si

aD2 © Psi
1 (x 0). (13) Erect atop acti P(dbp,o1.) =0. (16)
on
ee Beat
 116 W. Goodwine and J. Burdick

Henceforth, we will just assume that the vector field foot is very close to the ground, or very far from the
on the higher stratum is tangent to the lower stratum, ground, but may be dependent upon whether or not a
and note that if it is not tangent, we can modify it to foot is in or out of contact with the ground. When this
be so in the above manner. is so, the Lie bracket of the vector field controlling foot
height with any other vector field is zero, and the de-
The above example shows how one can effectively
coupling requirement is satisfied. Additionally, the tan-
determine the influence of a control that is defined in
gency requirements for canceling the flows associated
a higher stratum on the net evolution of the system in
with raising and lowering the foot will automatically be
the lower stratum. The following example shows how
satisfied.
motions that are analogous to Lie Bracket motions can
~ be realized by controls on different higher strata.
This is arguably a strict assumption. However, for
Example 2 Consider the sequence of flows kinematic, legged robots this assumption will almost
always be satisfied (see Section 5 for an example).
t t t t t t
ay — O54 2 ae ? Gay = ee 3 te = Da 4
Examples 1 and 2 show that in given a stratified
t t t t t t
Ces a ee - Cons a Cain Ssee 2 Dae (xo) system, the vector fields on any stratum (other than
vector fields corresponding to lifting or replacing feet)
The first six flows in this example are the same as in can be considered as part of the equations of motion
Example 1. Following the first siz flows are siz more in the bottom stratum if either certain Lie bracket and
wherein the flows that are entirely on Sj, i.e., the flow tangency conditions are met, or if Assumption 3 is sat-
along 91,2, and entirely on So, i.e., the flow along g22, isfied. If the vector fields are not tangent to the bottom
are in the negative direction. If the Lie brackets are
stratum, they are modified as in Example 1.
zero as in Equation (14), and t; = ti+2, i = 1,4,7, 10
these flows can be rearranged as We have shown above that it is possible to consider
vector fields in higher strata as part of the equations of
t t t t
LF = Pg,. ° Oras © bgo,2 54,2 (0). motion for the system on the bottom stratum. Based
on this observation, we introduce the following.
Now, if ta = ts = tg = Fila;

t t t t
aah Ee v Hite S O23 . Doro (Zo) Definition 4 The extended stratified system on the
2 bottom strata, Sg, is the driftless affine system com-
= Picts as a5 O(t?) (zo), prised of the vector fields on the bottom strata, chosen
where t = tg = ts = tg =t11 <1. Thus, this sequence vector fields from the higher strata, and Lie brackets
provides a net flow in S2 in the direction of the Lie of vector fields from Sg and higher strata. L.e., it is a
bracket between vector fields which are in the equations system taking the form:
of motion on different strata, S; and So.
£ = 064(x)v1 +---bm(x)Um
In Examples 1 and 2, it was required that certain
ta By 1 Ort I bnUn
Lie brackets be zero. While one could simply check —————
that these conditions are met in a given situation, the from higher strata
following assumption will guarantee this condition. ahs bn4i1Un+1 ae ees se DpUp, (17)
—__——— ooo’
Assumption 3 [f it is necessary to lift a foot from the any Lie brackets
ground during a gait cycle, we assume that the robot
can directly control, (via a single control, or a combina- where the {b;,...,6,} span T;So, the inputs v1,...,Un
tion of control inputs), the height of that foot relative to are real, and the inputs vp41,... , Up are fictitious.
the ground. Furthermore, for each stratum comprising
the given gait, we assume that the system’s equations With this definition, we have effectively increased the
of motion are independent of the foot height. I.e., the class of vector fields that we may employ when using
robot’s motion is independent of whether a particular the motion planning algorithm presented in Section 2.
Motion Planning for Kinematic Stratified Systems
147
4.2 The Motion Planning Algorithm the sequence of flows in Equation (7) are arranged by
For motion planning, the method of Section 2 could be order, and, from a gait efficiency point of view, it is
used in conjunction with the stratified extended sys- desirable to have them arranged by strata. It is pos-
tem. The basic idea is to use the stratified extended sible to regroup this sequence of flows by strata if the
system to plan the motion in the bottom stratum in Lie bracket between any vector fields (considered re-
order to obtain the fictitious inputs. We can deter- stricted to the bottom stratum) from different strata
mine the actual inputs by the method in Section 2 are zero. If this is true, Examples 1 and 2 show that it
with the modification that whenever the system must is possible to reorder the flows to obtain the same net
result. Flows corresponding to the same stratum could
flow along a vector field in a higher stratum, it switches
be grouped together. In physical terms, this grouping
to that stratum by lifting the appropriate feet, flowing
will reduce the number of times that a particular foot
along the vector field, and then replacing the appropri-
must make and break contact with the ground. The
ate feet, as in Example 1.
example in Section 5 does not satisfy this assumption.
Specifically, the algorithm to generate trajectories
that move the system from initial configuration p to
4.3 Gait Stability
final configuration q is as follows.
There is not an inherent mechanism in the straight—
1. Construct the extended stratified system, Equation forward application of the method of Section 2 to guar-
(17), on the bottom strata, Sz. antee the stability of the gait. Recall that the method
is based on the selection of a trajectory for the extended
2. Find a nominal trajectory, y(t), that connects p
system, y(t), from which the fictitious inputs are de-
and gq. Given 7(t), solve
termined. It is important to note that the actually
realized trajectory will generally not be y(t). Thus,
Y(t) = bi(x)v1 +--+ + bp(Z) up,
merely picking an initial trajectory y(t) which is al-
for the fictitious inputs, v;. As discussed in Section ways stable is not sufficient. One also must guarantee
4.3, it may be necessary to decompose the entire that the method’s inherent deviations from the initial
trajectory from the initial point to final point into trajectory lie within the stability bounds.
smaller subtrajectories.
Stability considerations can be incorporated into the
3. Solve the stratified extended system for the ficti- method as follows. Assume that there is a means for de-
tious control inputs. Le., solve for the backward termining the stability of the system, such as a scalar—
Philip Hall coordinates by solving the differential valued function of the configuration, U(x). For con-
equations (from Equation (10)). venience, assume that when W(x) < 0, the system is
unstable, when U(x) > 0, the system is stable, and
4, For each path segment in each strata, compute the when W(x) = 0, the system is on the stability bound-
actual controls that steer the system along 7(t). ary. In our analysis, the initial trajectory, y(t), must
This solution might require the transformation of be selected such U(y7(t)) > 0.
the backward Philip Hall coordinates to forward
The overall approach is to, when necessary, take
Philip Hall coordinates.
steps that are “small enough” to ensure that the system
5. Flow along each first order vector field, and ap- remains stable. Since the flow sequences are composed
proximate higher order vector fields as illustrated of small motions and a norm is necessary to measure
in Example 1. In general, it will be necessary to the length of a flow, we will either consider the system
switch strata between some of these flows. locally in R” or equip the configuration manifold with
a metric. Given a desired step along the trajectory,
Before we illustrate this method in Section 5, we con- y(t), t € [0,1], let R = min{||z —cl], ¢ € Y*(0)},
sider the issues of gait efficiency and stability. With re- i.e. the distance from the step’s starting point to the
gard to gait efficiency, note that the straight-forward closest point on the stability boundary. We want to
application of the method of Section 2 may result in an ensure that the system’s trajectory does not intersect
inordinate amount of strata switches. That is because the set Y~1(0). Let 2, and zy denote the starting
 W. Goodwine and J. Burdick
118

and final trajectory points. Without loss of general- bound. Given these two observations, an appropriate
ity, let y(t) = x +t(xy — x5) be a desired straight line step length may often be best determined experimen-
path between the starting and end points. Also, let tally.
A = |\ay —2,||. Recall that the fictitious inputs, v’ These very same observations also apply to obstacle
were determined by solving an equation of the form avoidance. If the robot traverses an environment with
¥(t) = gi(y(t))v' +--+» +gs(7(t))v® for the v'. We have obstacles, we assume that the nominal trajectory is de-
that: ||v*|| < C||+(t)|| = CA, for some constant C. By signed by an holonomic or rigid body planner in such
the method of construction of the real inputs from the a manner as to avoid obstacles. Ensuring that the ac-
fictitious inputs, we have that. ||u*|| <CA1/*, where k tual trajectory also avoids the obstacles, requires that
is the degree of nilpotency of the system, or the degree the nominal trajectory be analogously broken into suf-
of the nilpotent approximation. ficiently small steps to ensure that the actual trajectory
Pick a ball, B, of radius R, and let K be the max- remains sufficiently close to it.
imum norm of all the (first order) vector fields, 9;
for all points in B. Recall that the real inputs, w’ 5 Example
were given by a sequence of inputs which approxi-
mate the flow of the extended system. Denote this We illustrate our approach by generating control in-
sequence by Us, where the superscript indexes the in- puts that will steer the hexapod of Figure 1 to walk
put, and the subscript indexes its position in the se- over flat terrain (see Section 6 for an example involving
quence. The maximum distance that the system can manipulation of a curved object, which is analogous to
possibly flow from the starting point, z,, is given by locomotion over uneven terrain). The key difficulty in
the sum of the distances of the individual flows. Let this example is the fact that the legs are kinematically
Im = MaxX;e(0,1]{||Z(t) — xs ||} denote the point in the insufficient, making sideways motion difficult. Assume
flow that is maximally distant from the starting point that the robot walks with a tripod gait *, alternating
(this is not necessarily the final point, x7). To guaran- movements of legs 1-4-5 with movements of legs 2-3-6.
tee stability, we must show that ||z, — 2|| < R. How- With the tripod gait, this robot has four control inputs.
ever, this distance, ||z,, — x|| is necessarily bounded by The inputs wu; and uz respectively control the forward
the sum of the norms of each individual flow associated
and backward angular leg displacements of legs 1—4—
with one real control input, ui, 4:62;
5 and legs 2-3-6, while inputs ug and wu, respectively
1
control the height of legs 1-4-5 and 2-3-6.
lem — all < aes So| 0 giui dtl) The equations of motion can be written as follows:
tJ

= cos@(a(hi)ui + B(h2)u2)
However, ||u‘|| < CA‘/* and ||g:(z)|| < K Va € B.
ys sin 8(a(hj)uy + B(h2)u2)
Thus,
2m —al|< $)KCAM*, (18) Gr la(hy)uy = IB(h2)ue2

tJ
1 = Ui; oo —— uy
and since A = ||z+ — z||, by choosing the desired final
hy = u3 hs = U4.
point close enough to the starting point, the trajectory
will not intersect the stability boundary. where (z,¥,@) represents the body’s configuration, ¢;
Note that since A is raised to the power of 1/k, if is the front to back angular deflection of the legs, I is
k is large, it may be necessary to make A exceedingly the leg length, and h, is the height of the legs off the
small in order to ensure stability. However, the bound ground. The functions a(h;) and @(h2) are defined by:
expressed in Equation (18) is very conservative since
it sums the length of a bound on each individual flow an ladf Rata 1 ifhy=0
in the series. In actuality, because the largest flows a(n) ={ 4 if h > 0 ata) = {4 if ho > 0
correspond to the Lie brackets of order k, simply sum- Ref. [9] shows that the hexapod is small time locally
ming their component lengths will give a conservative gait controllable when a tripod gait is used.
Motion Planning for Kinematic Stratified Systems
119

where (x, y,0) represents the planar position of a ref-

erence frame attached to the robot’s center. ¢1 is the
angle of legs 1-4-5 and @y is the angle of legs 2-3-6.
The variables u3 and u4 are both 0 since the legs main-
tain ground contact. Let 9i2,1 and gi2.2 represent the
first and second columns in Equation (19)
If legs 1-4-5 are in contact with the ground, but legs
2-3-6 are not in contact, the equations of motion are:

x Case) @) (0)
y sind 0 0 i
0 jal oerg :
il? | hiatal ei Up (20)
Figure 5: Stability Margin for Hexapod Tripod Gait. bo Ot BLT 70 4
he eae ihe |
Since the robot walks in a tripod gait, stability is en-
sured if the robot’s center of mass remains above the where h; is the height of the corresponding set of legs
triangle defined by the tripod of feet which are in con- and ug is constrained to be 0. Label columns one, two
tact with the ground. For the motion of legs 1-4-5, the and three in Equation (20) 91,1, 91,2 and g1,3, respec-
robot’s center of mass must be at least b = - +lsin $1 tively. If legs 2-3-6 are in ground contact and legs
from the front of the robot to ensure stability, where I, 1-4-5 are not, the equations of motion are:
denotes the body’s length. See Figure 5. Alternatively,
if the center of mass is located a distance b from the £ 0 cosé@ O
front of the robot, then stability is ensured during the y 0 sind 0 z
motion if both of these constraints are satisfied 6 Od i
db = 1 0 0 U2 (21)

ope epee (@- #1) dbo

ial

es oie 0
U3

hy 0 8!
ee Mesain (= - 8/1).
where w,4 is constrained to be 0. The columns in Equa-
tion (21) are denoted go1, g2,2, and go.3.
Denote the stratum when all the feet are in contact
(a = B = 1) by Sie, the stratum when tripod one is For motion planning purposes, we must select
in contact (a = 1,0 = 0), by Sj, the stratum when enough vector fields to span the tangent space of the
tripod two is in contact (a = 0,0 = 1), by Sz and the bottom stratum, Sj2. A simple calculation shows that
stratum when no legs are in contact (a = @ = 0), by the set of vector fields,
So. Note that this system satisfies the requirements of
Assumption 3 since, regardless of the values of a and {912,1; 912,25 91,2) 92,1; [912,15 912,2]}
B, the vector fields moving the foot out of contact with
spans T,Sjo for all x € Sig. Note that [g912,1, 912,2] =
the ground are of the form {sz}, and the equations
(—2l sin 6, 21cos0,0,0,0)7. This Lie algebra is not
of motion are independent of the foot heights, h;. nilpotent, and thus the extended system will only be a
The equations of motion in the bottom strata, S12 nilpotent approximation.
(where all the feet maintain ground contact), are:
The stratified extended system is constructed from
z cos@ cosé the extended system that uses the vector fields from
y sin@ sin@ all strata.
Aovlie asic (se ) (19)
Ai i! 0 & = 912,10! + g12,2v" + gi20° + ga1v* + [912,1; 912,210"
(22)
 W. Goodwine and J. Burdick
120

or, in greater detail, coordinates. Instead, we can directly construct a se-

quence of controls to move in the desired direction.
£ cos@é cos@é 0 O —d2lsin#é ve
y sind sind 0 O- 2lcos@ v?
Let o denote concatenation of control inputs. For
0 = I -! 0 0 0 v? example, ui 0 u2 denotes that wu; = 1 for time hy(1)
1 il 0 (Da sell 0 vu followed by ug = 1 for time ho(1). Considering the
dbo 0 1 ih @ 0 y? vector fields on S12, (g12,1; 912,2 and [912,1, 912,2]), the
system needs to flow along the first two vector fields
Let the starting and ending configurations be: for 5 seconds, and construct a piece-wise approxima-
tion to the flow along the third Lie bracket vector field
fUY Wir (x, y,9, 1, $2, hi, he) = (0,0,0,0,0,0,0) :
for 4 seconds. The control sequence to approximately
q = (x,y, 9, 1, 62,21, he) = (1, 1,0, 0, 0, 0, 0)
move the system in the direction of the flow of the Lie
A path that connects these points is y(t) = bracket is:
(t,t, 0,0,0,0,0). Equating +(t) with with the stratified U1 O Ug O —Uy O —UQ (23)
extended system and solving for the fictitious controls where each of the individual control inputs is equal
yields:
to one for /3 seconds (recall Equation (2)). To flow
I(cos @ + sin @) along g11,1, U1 = 1 for 4 seconds. Similarly to flow
I(cos @ + sin @) along gi2,1, U2 = 1 for 4 seconds.
=—]| —I(cosO+sin@) |,
s —I(cos 6 + sin 8) On the higher strata, to flow along gi, ui = —1 for
5 seconds and to flow along g2,1, ui = —1 for 4 seconds.
SS
ow
WN
iss
SS (cos 0 — sin @)
In order to execute these flows, the robot must switch
or, since 9(t) = 0, and if we let | = 1, from the bottom stratum to the higher strata when
executing a control input associated with a fictitious
vi 1 input for a higher strata.
v2 1
Thus, the total control sequence is:
v? =e —l
vt =a
v° i Su 0 Ug O —Uy © —U2)
1
O5Ug t
© 24 © €Ug 0 (—51 U2) © (—eu4)
For a system which is nilpotent of order 2, we have o€ug 0 —(5%1) © (—eus).
from Equation (9) (where the g’s from Equation (22)
are substituted for the B’s in Equation (9) in the order The first four terms in the sequence approximate the
that they appear in Equation (22): Lie bracket motion on the bottom stratum. The -
term denotes the length of time each control input is
hy — jis hy = im “on.” The next two terms are the contribution of the
hg = v, he = v', u, and ue terms individually on the bottom stratum.
hs = v+hyv The next term represents a small flow associated with
removing legs 2-3-6 out of contact with the ground,
which yields: and the following term corresponds to legs 2-3-6 mov-
1 1 ing back to their initial position. Since the legs are not
h
1(1)
1 =
5
oa
ho (1) == 5 in contact with the ground, this motion does not cause
1 1 the body of the robot to move. The next input corre-
sponds to legs 2-3-6 moving back into contact with the
3 ground. The next three inputs correspond to legs 1-4-5
performing an analogous motion.
Figure 6 shows the path of the robot’s center as it fol-
Since the nilpotent approximation is of order two, lows a straight line trajectory, which is broken into four
there is no need to transform to forward Philip Hall equal segments. Due to the nilpotent approximation,
Motion Planning for Kinematic Stratified Systems
121

pem0saGn0s lr Looe 4

Figure 6: Straight Trajectory.

there is some small final error. Better accuracy can be

obtained by use of a higher order nilpotent approxi-
50 100 150 20p| bt il
mation or a second iteration of the algorithm from the
robot’s ending position. Note that the main body axis
is oriented along the z-axis in this example. Since the
legs can not move immediately sideways, the robot’s
motion must include “parallel-parking-like” behavior
to follow this line.
There is no inherent limitation in the method which
requires the trajectory to be broken down into subseg-
ments, however, there are two reasons to do so. First,
since the method is based upon decomposing a desired
trajectory into flows along the Philip Hall basis vector
fields, the final trajectory is only related to the desired
trajectory in that the end points are the same (or ap-
proximately the same for nilpotent approximations). o

“iui
Breaking the path into segments leads to better over-
iI)
all tracking. Second, robot stability requirements may
also demand smaller steps.
The approach is general enough that approximate
400]1BG
tracking of arbitrary trajectories is possible. Figure 7
shows the hexapod following an ellipse while maintain-
ing a constant angular orientation. Figure 8 shows the
results when a smaller step size is used. In the first
Figure 8: Elliptical Trajectory with Smaller Steps.
simulation, the elliptical trajectory is broken into 30
segments. In the second, it is divided into 60 segments.
In this example, part of the trajectory tracking error is Also plotted in these figures is the stability crite-
due to the nilpotent approximation, but another con- rion. Let the body length be 2 units of length and let
tribution to the error is the simplicity of the model. the center of mass be located a distance of 0.75 units
Some directions are more “difficult” for the system to from the front of the robot. Then, the stability crite-
execute than others due to the kinematic limitations rion is ¢; < 0.25 [rad] and ¢2 > —.85 [rad]. In Figures
of the leg design. Because this mechanism can not ex- 7 and 8 the stability limits for ¢ are indicated by the
ecute “crab-like” gaits, its tracking error during side- straight horizontal lines. In the first case, where the
ways motions increases, as this direction corresponds robot takes bigger steps, the stability condition is vio-
to a Lie bracket direction. lated. However, in the second case it is not.
 W. Goodwine and J. Burdick
122

Figure 9: Hexapod leaving footprints.

Figure 9 depicts the footprints left by the hexapod

as it follows a straight line diagonal path while simulta-
neously rotating at a constant rate. The complex pat-
tern of the footfalls suggests that any technique that
is based on foot placement would be very difficult to
apply to this system. Figure 11: (a) Obstacles not avoided; (b)Obstacles
avoided.
Finally, we consider obstacle avoidance. While the
nominal initial trajectory y(t) must @ priori avoid any
obstacles, this constraint alone will not guarantee that the nominal trajectory. A real-world scenario where
the actual motion avoids obstacles. If the trajectory is this might be desirable is a patrol robot that must
divided into sufficiently small segments, as suggested constantly scan in all directions. Figure 11(a) shows
in Section 4.3, then obstacle avoidance can be realized. the path of robot’s center of mass when the trajec-
Figure 10 shows a desired nominal path (indicated by tory is not finely divided enough to satisfy the criteria
a black line) of the hexapod’s body center through a of Section 4.3 (it is subdivided into 100 subtrajecto-
set of obstacles. The walls of the environment are in- ries). Since the path of the center of mass intersects
dicated by dark grey regions. The lighter grey regions the lighter grey regions during portions of its motion,
correspond to locations of the vehicle’s center where the robot would realistically bump into the walls in
some vehicle orientations may cause the hexapod to this example. However, if the nominal trajectory is
intersect the walls (i.e., the grey regions are the pro- sufficiently subdivided (into 300 subtrajectories in this
jected silhouettes of the c-space obstacles). case) to satisfy the requirement of Section 4.3, the ro-
To make the problem more challenging, we also spec- bot avoids the walls, as illustrated in Figure 11(b).
ify that the robot rotates at a uniform rate as it follows
6 Multi-fingered Hand Manipulation

The methodology described above can be almost imme-

diately applied to object manipulation via finger gait-
ing in a multi-fingered hand. The application of this
approach leads to a manipulation planning strategy
that is independent of the geometry of the grasped ob-
ject and independent of the manipulating hand’s mor-
phology. The method is also independent of the type
of contact between the finger and object (e.g., “point
contact with friction,” “soft finger,” etc.) and indepen-
Figure 10: Nominal obstacle avoidance trajectory.
Motion Planning for Kinematic Stratified Systems
123

Since the nominal trajectory stays away from the

fingers’ kinematic singularities, the finger tip velocities
can be considered as system inputs. This input choice
will simplify the computations and make the equations
of motion satisfy Equation (14). One can not generally
choose the inputs in this way, (for example, when the
the finger tips are in rolling contact with the object);
however, the more general cases still fits within the
framework of the stratified motion planning method
outlined in Section 3.
The equations of motion for such a grasped system
are straight-forward, though possibly tedious, to derive
(see [21] for details). The equations of motion on the
bottom stratum are of the form

Figure 12: Four fingers manipulating an object. E = gi(x)ur +--+ + go(x)ug,

and on the higher strata are of the form

dent of the morphology of the manipulating “fingers”
(i.e., independent of the number of joints, etc.).
L=9i(x)uy +--+ + ge(x)U6 + g7(x)uz
Consider the “egg-shaped” object in Figure 12 ane - g9(x)ug,
whose surface is parameterized by
where the first 6 inputs are associated with the fin-
(Le
+e cos U COS U ue (Sr
ger tip velocities for the three fingers contacting the
c(u,v)=| (1+) cosusiny |, 2 Ra :
vée(-t object, and inputs 7 — —9 are the three degrees of free-
5 sin u me
dom for the finger that is not in contact with the ob-
This object is to be manipulated by four, three DOF ject. Note that g7(z) through go(«) will take the form
fingers whose kinematic model is shown in Figure 13. (0,---,1,---,0) since they are the unconstrained finger
A “point contact with friction” model is assumed. tip velocities of the finger which is not contacting the
The stratified c-space will consist of a total of 16 dif- object, and thus they will satisfy Equation 14. There-
ferent strata, corresponding to all the possible combi- fore, they may be incorporated into the equations of
nations of finger contacts. However, as will be clear motion for the bottom stratified extended system.
shortly, the system is manipulable if it is restricted Incorporating these unconstrained finger tip velocity
to only 5 strata: When all four fingers are in con- vector fields for each of the four higher strata gives a
tact plus each of the four cases where only one of stratified extended system of the form:
the fingers is out of contact. Denote these strata as
$1234,
$123, 9124, $134, and S234 where the subscripts a gi(x) Sr oOo ae 96(x) U6
SS
———SSs’!
denote which fingers are in contact with the object. On Si234
+ g7(x)u7 + gs(z)us
————— Se
+ go(x)ug
from S123

+ gie(x)ure + 917(@)ui7 + gis(x)uis,

——————EE___. ae

from S234

where all the vector fields except those on the first line
correspond to free finger tip motion. Tedious calcula-
tions show that {g),...,918} spans the tangent space
Figure 13: Finger kinematics.
 W. Goodwine and J. Burdick
124

to the c-space, so the system is stratified manipulable.

Since no Lie brackets are necessary to make the sys-
tem stratified manipulable, this system is already in
extended form, and the actual control inputs are the
same as the “fictitious” inputs presented in Sections 2
and 3.
Assume that the initial and final configurations are
identical (as illustrated in Figure 12), and that the de-
sired motion is a pure rotation of 27 about the axis
Woe (J, Wet FR): Using exponential EO OHMMELES,
then, the object’s nominal configuration as a function
of time is given by Rodrigues’ formula:

y(t) = e%?™ — 74 sin 2nt + H?(1 —cos2nt),

t € (0, 1).

For the object’s initial and final configuration in Fig-

ure 12, each finger is oriented at an angle of 7/4 relative
to the z— and y-axes. As the object rotates, each fin-
ger’s nominal configuration is such that it contacts the
object along that same axis. This can be determined
by equating the forward kinematics for each finger with
the point on the object’s surface that intersects the re-
spective 7/4 radial from the origin, and then, using
the kinematics of each finger, determine the desired
joint configurations. For this particular example, this
trajectory is difficult to compute analytically, but is
simple to do numerically for each step of the system’s
motion. The desired trajectory is decomposed into 10
subsegments, and a sequence of six “snapshots” from
the manipulation is shown in Figure 14.
Figure 14: Snapshots of computer simulation of object ma-
nipulation via finger repositioning.
7 Conclusions
we believe that our approach provides an evolutionary
Our method provides a general means to generate ra- path for future research and generalizations.
jectories for many types of legged robotic and multi-
Since many interesting robotic systems (such as
fingered systems. The simulations indicate that the bipeds) are not kinematic, an algorithm for solving the
approach is rather simple to apply. The method is in-
trajectory generation problem for such systems is nec-
dependent of the number of legs (fingers) and is not
essary. However, since the state of the art for solving
based on foot (finger) placement principles. For a
the trajectory generation problem for smooth systems
given legged robot mechanism, a specifically tuned leg-
with drift is still in its infancy, it may be difficult to
placement-based algorithm may lead to motions which
make headway along these lines until more complete
use fewer steps or results in less tracking error. How-
results for the smooth case become known.
ever, for the purposes of initial design and evaluation
of a legged mechanism, our approach affords the robot
Acknowledgments
design engineer an automated way to implement a real-
istic trajectory generation scheme for a quasi-static ro- This work was partially supported by a grant from the
bot of nearly arbitrary morphology. More importantly, Office of Naval Research.
Motion Planning for Kinematic Stratified Systems
125
References
[15] Maw Kae Hor. Control and Task Planning of the Four
Finger Manipulator. PhD thesis, NYU, 1987.
[1] R. Abraham, J. E. Marsden, and T. Ratiu. Manifolds,
Tensor Analysis, and Applications. Springer-Verlag, [16] Alberto Isidori. Nonlinear Control Systems. Springer—
second edition, 1988. Verlag, second edition, 1989.

(2——J Matt Berkemeier. Modeling the dynamics of [17] S. Kajita and K. Tani. Study of dynamic biped loco-
quadrupedal running. Int. J. Robotics Research, motion on rugged terrain. In IEEE Int. Conf. on Ro-
16(9):971-985, 1998. botics and Automation, pages 1405-1411, Sacramento,
CA, 1991.
[3] C.H. Chen, K. Mirza, and D.E. Orin. Force control of
planar power grasp in the digits system. In 4th Int. [18] G. Lafferriere and Hector J. Sussmann. A differential
Symp. on Robotics and Manufacturing, pages 189-194, geometric approach to motion planning. In X. Li and
1992. J. F. Canny, editors, Nonholonomic Motion Planning,
pages 235-270. Kluwer, 1993.
[4] I-M Chen and J.W. Burdick. A qualitative test for
n-finger force-closure grasps on planar objects with [19] J.K. Lee and §.M. Song. Path planning and gait of
applications to manipulation and finger gaits. In Proc. walking machine in an obstacle-strewn environment.
IEEE Int. Conf. on Robotics and Automation, pages J. Robotics Systems, 8:801—827, 1991.
814-820, 1993.
[20] D. J. Montana. The kinematics of contact and grasp.
J. J. Collins and Ian Stewart. Hexapodal gaits and Int. J. of Robotics Research, 7(3):17-25, 1988.
coupled nonlinear oscillator models. Biological Cyber-
netics, 68:287—298, 1993. [21] Richard M. Murray, Zexiang Li, and S. Shankar Sas-
try. A Mathematical Introduction to Robotic Manipu-
Ronald S. Fearing and T.O. Binford. Using a cylin- lation. CRC Press, Inc., 1994.
drical tactile sensor for determining curvature. [EEE
Trans. Robotics and Automation, 7(6):806-817, 1991. [22] R.M. Murray and $.S. Sastry. Nonholonomic motion
planning: Steering using sinusoids. IEKEE Trans. Au-
[7] R.S. Fearing. Implementing a force strategy for object tomatic Control, 38:700—-716, 1993.
reorientation. In Proc. IEEE Int. Conf. on Robotics
and Automation, pages 96-102, 1986. [23] T. Okada. Object handling system for manual indus-
try. IEEE Transactions on Systems, Man and Cyber-
[8] B. Goodwine and J.W. Burdick. Controllability of netics, 9(2):79-89, 1979.
kinematic systems on stratified configuration spaces.
IEEE Trans. Automatic Control (to appear), 1999. (24) Domenico Prattichizzo and Antonio Bicchi. Dynamic
analysis of mobility and graspability of general manip-
Bill Goodwine and Joel Burdick. Gait controllability ulation systems. JEEE Transactions on Robotics and
for legged robots. In Proc IEEE Int. Conf on Robotics Automation, 14(2):241-258, April 1998.
and Automation, 1998.
[25] M.H. Raibert. Legged Robots that Balance. MIT Press,
Bill Goodwine and Joel Burdick. Stratified motion 1986.
planning with application to robotic finger gaiting.
Proc. IFAC World Congress, 1999. [26] E. Rimon and J.W. Burdick. Configuration space
analysis of bodies in contact — i. Mechanism and Ma-
J. William Goodwine. Control of Stratified Systems chine Theory, 30(6):897-912, August 1995.
with Robotic Applications. PhD thesis, California In-
stitute of Technology, 1998. [27] E. Rimon and J.W. Burdick. Configuration space
analysis of bodies in contact — ii. Mechanism and Ma-
Stratified Morse Theory. chine Theory, 30(6):913-928, August 1995.
[12] Goresky and Macpherson.
Springer-Verlag, New York, 1980.
[28] K. Salisbury, D. Brock, T. Massie, N. Swarup, and
L. Han, Y.S. Guan, Z.X. Li, Q. Shi, and J.C. Trinkle. C. Zilles. Haptic rendering: Programming touch in-
[13] teration with virtual objects. In Proc. Symp. on In-
Dextrous manipulation with rolling contacts. In Proc.
IEEE Int. Conf. on Robotics and Automation, pages teractive 3D Graphics, pages 123-130, 1995.
992-997, Albuquerque, NM, 1997.
[29] K. Salisbury and C. Tarr. Haptic rendering of surfaces
J. Hong and G. Lafferriere. Fine manipulation with defined by implicit functions. In Proc. ASME Int. Me-
[14] chanical Engineering Congress and Exposition, pages
multifinger hands. In [EEE Int. Conf. on Robotics
and Automation, pages 1568-1573, 1990. 61-67, 1997.
126 W. Goodwine and J. Burdick

[30] Jean-Pierre Serre. Lie Algebras and Lie Groups. [33] J.C. Trinkle and R.P. Paul. Planning for dexterous
Springer-Verlag, 1992. manipulation with sliding contacts. Int. J. of Robotics
: Research, 9(3):24—48, 1990.
[31] K.B. Shimoga. Robot grasp synthesis algorithms: A
~ Lhe f } } =
pean a SR ones Bo eeedebotics He [34] V.S. Varadarajan. Lie Groups, Lie Algebras, and
Their Representations. Springer-Verlag, 1984.
[32] S.M. Song and K.J. Waldron. Machines that walk: the
Adaptive Suspension Vehicle. MIT Press, 1989.
Manipulation of Pose Distributions
Mark Moll, Carnegie Mellon University, Pittsburgh, PA
Michael A. Erdmann, Carnegie Mellon University, Pittsburgh, PA

to the parts orienting device. Assuming the orienting

For assembly tasks parts often have to be oriented strategy succeeds with high probability, on average it
before they can be put in an assembly. The results pre- takes just a few tries to orient the part. An alternative
sented in this paper are a component of the automated view of this type of manipulation is to consider it as
design of parts orienting devices. The focus is on ori- manipulation of the pose distribution. The goal then
enting parts with minimal sensing and manipulation. is to find the pose distribution with minimal entropy,
thereby maximally reducing uncertainty.
We present a new approach to parts orienting through
the manipulation of pose distributions. Through dy-
namic simulation we can determine the pose distrib- 1.1 Example
ution for an object being dropped from an arbitrary
In this paper we will discuss the use of dynamic sim-
height on an arbitrary surface. By varying the drop
ulation for the design of support surfaces that reduce
height and the shape of the support surface we can find
the uncertainty of a part’s resting configuration. As the
the initial conditions that will result in a pose distribu-
support surface is changed, the probability distribution
tion with minimal entropy. We are trying to uniquely
function (pdf) of resting configurations will change as
orient a part with high probability just by varying the
well. The pdf will also vary with the initial drop posi-
initial conditions. We will derive a condition on the
tion above the surface. The following figure and para-
pose and velocity of an object in contact with a sloped
graph illustrate the basic idea:
surface that will allow us to quickly determine the final
resting configuration of the object. This condition can
then be used to quickly compute the pose distribution.
We also present simulation and experimental results
that show how dynamic simulation can be used to find
optimal shapes and drop heights for a given part.
Figure 1: A part with an initially unknown orientation 1s
1 Introduction dropped on a surface.

In our research we are trying to develop strategies to A part with an initially unknown orientation is re-
orient three-dimensional parts with minimal sensing leased from a certain height and relative horizontal po-
and manipulation. That is, we would like to bring sition with respect to the bowl. The only forces acting
a part from an unknown position and orientation to on the part are gravity and friction. We assume the
a known orientation (but possibly unknown position) bowl doesn’t move. We can compute the final resting
with minimal means. In general, it is not possible to configuration for all possible initial orientations. This
orient a part completely without sensors, but it is suffi- will give us the pdf of stable poses. The goal is to find
cient if a particular orienting strategy can bring a part the drop height, relative position and bowl shape that
into one particular orientation with high probability. will maximally reduce uncertainty. In this paper we
The sensing is then reduced to a binary decision: a assume for simplicity that the initially unknown ori-
sensor only has to detect whether the part is in the entation is uniformly random, but our approach also
right orientation or not. If not, the part is fed back works for different prior distributions.
 M. Moll and M. A. Erdmann
128

Stable Poses

i, 1) 7, a (a) CD) Entropy

quasistatic approximation 0.20 0.13 0.16 0.21 0.14 0.16 1.78
dynamic, flat surface, drop height is h = 0 0.18 0.16 0.14 0.34 0.05 0.13 1.66
dynamic, bowl shape is y = 0.242, h = 0.28, 0.24 0.03 0.03 0.50 0.08 0.15 1.35
initial hor. pos. zo = —0.41

Table 1: Probability distribution function of stable poses for two surfaces. The initial velocity is zero and the initial rotation
is uniformly random.

Table 1 shows three different pose distributions. reduce the uncertainty greatly by taking advantage of
Each stable pose corresponds to a set of contact points the dynamics.
(marked by the black dots in the table). For an arbi-
trarily curved support surface the stable poses do not 1.2 Outline
necessarily correspond to edges of the convex hull of
the part. We therefore define a stable pose as a set of In Section 3 we will explain the notion of capture re-
contact points. This means that any two poses with gions and introduce an extension and relaxation of this
the same set of contact points are considered to be the notion in the form of so-called quasi-capture regions.
same as far as the pose distribution is concerned. In our These quasi-capture regions allow for fast computation
example the support surface is a parabola y = ax? with of pose distributions. In Section 4 we will present our
parameter a. Other parameters are the drop height, h, simulation and experimental results. Finally, in Sec-
and the initial horizontal position of the drop location, tion 5 we will discuss the results presented in this pa-
xo. We limit the surface to parabolas for illustrative per. But first we will give an overview of related work
purposes only; in general we would use a larger class in the next section.
of possible shapes (see Section 4.1).
2 Related Work
The first row in the table shows the pdf assuming
quasistatic dynamics. In this case the surface is flat and
2.1 Parts Feeding and Orienting
the part is released in contact with the surface. The
second row shows how the pdf changes if we model the One of the most comprehensive works on the design
dynamics. The initial conditions are the same as for the of parts feeding and assembly design is [12], which de-
quasistatic case, yet the pdf is significantly different. scribes vibratory bowls as well as non-vibratory parts
The third row shows the pdf for the optimized values feeders in detail. The APOS parts feeding system is
for a, h and zo. described by Hitakawa [26]. Berretty et al. [6] present
The objective function over which we optimize is the an algorithm for designing a particular class of gates
entropy of the pose distribution. If p,,... , pn are the in vibratory bowls. Berkowitz and Canny [4, 5] use
probabilities of the n stable poses, then the entropy is dynamic simulation to design a sequence of gates for a
— >, pi log p;. This function has two properties that vibratory bowl. The dynamics are simulated with Mir-
make it a good objective function: it reaches its global tich’s impulse-based dynamic simulator, Impulse [35].
minimum whenever one of the p; is 1, and its maximum Christiansen et al. {16] use genetic algorithms to design
for a uniform distribution. By searching the parameter a near-optimal sequence of gates for a given part. Op-
space we can find the a, h and zp that minimize the timality is defined in terms of throughput. Here, the
entropy. In the third row of the table the pose distribu- behavior of each gate is assumed to be known. So, in
tion is shown with minimal entropy. The table makes a sense [16] is complementary to [5]: the latter focuses
it clear that even with a very simple surface we can on modeling the behavior of gates, the former finds an
optimal sequence of gates given their behavior. Akella
1This is a local minimum found with simulated anneal- et al. [1] introduced a technique for orienting planar
ing and might not be the global minimum. parts on a conveyor belt with a one degree-of-freedom
Manipulation of Pose Distributions
129
(DOF) manipulator. Lynch [32] extended this idea to tact, and the dynamics in the system are dissipative,
3D parts on a conveyor belt with a two DOF manipula- the capture regions will be correct. The capture re-
tor. Wiegley et al. [43] presented a complete algorithm gions will in general not cover the entire configuration
for designing passive fences to orient parts. Here, the space.
initial orientation is unknown.
Goldberg [22] showed that it is possible to orient
2.3 Collision and Contact Analysis
polygonal parts with a frictionless parallel-jaw gripper
without sensors. Marigo et al. [34] showed how to ori-
For rigid body collisions several models have been pro-
ent and position a polyhedral part by rolling it between
posed. Many of these models are either too restric-
the two hands of a parallel-jaw gripper. Grossman and
tive (e.g., Routh’s model [39] constrains the collision
Blasgen [25] developed a manipulator with a tactile
impulse too much) or allow physically impossible col-
sensor to orient parts in a tray. Erdmann and Ma-
lisions (e.g., Whittaker’s model [42] can predict arbi-
son [18] developed a tray-tilting sensorless manipulator
trarily high increases of system kinetic energy). Re-
that can orient planar parts in the presence of friction.
cently, Chatterjee and Ruina [15] proposed a new colli-
If it isn’t possible to bring a part into a unique orienta-
sion rule, which avoids many of these problems. Chat-
tion, the planner would try to minimize the number of
terjee introduced a new collision parameter (besides the
final orientations. In [19] it is shown how (with some
coefficients of friction and restitution): the coefficient
simplifying assumptions) three-dimensional parts can
of tangential restitution. With this extra parameter
be oriented using a tray-tilting manipulator. Zumel
a large part of allowable collision impulses can be ac-
[45] used a variation of the tray tilting idea to orient
counted for, and at the same time this collision rule
planar parts with a pair of moveable palms.
restricts the predicted collision impulse to the allow-
In recent years a lot of work has been done on pro- able part of impulse space. This is the collision rule we
grammable force fields to orient parts [9, 10, 28]. The will use (see [36] for details).
idea is that a kind of force “field” (implemented using,
e.g., MEMS actuator arrays) can be used to push the Instead of having algebraic laws, one could also try
part in a certain orientation. Kavraki [28] presented a to model object interactions during impact. This ap-
vector field that induced two stable configurations for proach is taken, for instance, by Bhatt and Koechling
most parts. Bohringer et al. [9, 10] used Goldberg’s (7, 8], who modeled impacts as a flow problem. While
algorithm [22] to define a sequence of “squeeze fields” this might lead to more accurate predictions, it is obvi-
to orient a part. They also gave an example how pro- ously computationally more expensive. Also, in order
grammable vector fields can be used to simultaneously to get a good approximation of the pdf of resting config-
sort different parts and orient them. urations, this level of accuracy might not be required.
On the other hand, it is also possible to combine the
2.2 Stable Poses effects of multiple collisions that happen almost instan-
taneously. Goyal et al. [23, 24] studied these “clatter-
To compute the stable poses of an object quasistatic ing” motions and derived the equations of motion.
dynamics is often assumed. Furthermore, usually it is
assumed that the part is in contact with a flat sur- Given a collision model and the equations of motion,
face and is initially at rest. Boothroyd et al. [11] were one can simulate the motion of a part. In cases where
among the first to analyze this problem. An O(n?) al- there are a large number of collisions or with contact
gorithm for n-sided polyhedrons, based on Boothroyd modes that change frequently one can simulate the dy-
et al.’s ideas, was implemented by Wiegley et al. [44]. namics using so-called impulse-based simulation [35].
Goldberg et al. [21] improve this method by approx- However, there are limits to what systems one can sim-
imating some of the dynamic effects. Kriegman [30] ulate. Under certain conditions the dynamics become
introduced the notion of a capture region: a region in chaotic [13, 20, 29]. We are mostly interested in sys-
configuration space such that any initial configuration tems that are not chaotic, but where the dynamics can
in that region will converge to one final configuration. not be modeled with a quasistatic approximation. In
Note that his work doesn’t assume quasistatic dynam- Section 4.1 a number of “chaos plots” are shown that
ics; as long as the part is initially at rest and in con- are very similar to the one in [29].
 M. Moll and M. A. Erdmann
130

2.4 Shape Design terms of local state. The closest stable pose can be
defined as follows:
The shape of an object and its environment imposes
constraints on the possible motions of an object. Caine
[14] presents a method to visualize these motion con- Definition 2 We define a stable pose to be a pose
straints, which can be useful in the design phase of such that there is force balance when only gravity and
both part and manipulator. In [31] the mechanics of contact forces are acting on the part. The closest stable
entrapment are analyzed. That is, Krishnasamy dis- pose is the stable pose found by following the gradient
cusses conditions for a part to “get trapped” and “stay of the potential energy function (using, e.g., gradient
trapped” in an extrusion in the context of the APOS descent) from the current pose.
parts feeder. Sanderson [40] presents a method to char-
acterize the uncertainty in position and orientation of We can now define quasi-capture regions:
a part in an assembly system. This method takes into
account the shape of both part and assembly system. Definition 3 A quasi-capture region is the largest
In [33] the optimal manipulator shape and motion are possible region in configuration phase space such that
determined for a particular part. The problem here was (a) all configurations in this region have the same clos-
not to orient the part, but to perform a certain juggler’s est stable pose and (b) no configuration in a quasi-
skill (the “butterfly”). With a suitable parametrization capture region has enough (kinetic and potential) en-
of the shape and motion of the manipulator, a solution ergy to leave this region either with a rolling motion or
was found for a disk-shaped part that satisfied their one collision-free motion.
motion constraints. Although the analysis focuses just
on the juggling task, it shows that one can simulate
Ideally these quasi-capture regions would induce a
and optimize dynamic manipulation tasks using a suit-
partition of configuration phase space, so that for each
able parametrization of manipulator (or surface) shape
point in phase space we would immediately know what
and motion.
its final resting configuration is. Of course, this is not
the case in general, since with a sufficiently large ve-
3 Analytic Results locity an object can reach any stable pose. But if we
restrict the velocity to be small to begin with, then we
3.1 Quasi-Capture Intuition
are able to quickly determine the pose distribution. It
In our efforts to analyze pose distributions in a dynamic has been our experience that without the use of quasi-
environment, we have been working on a generalization capture regions a lot of computation time is spent on
of so-called “capture regions” [30] that we have termed the final part/surface interactions (e.g., clattering mo-
quasi-capture regions. Specifically, for a part in con- tions) before the part reaches a stable pose. In other
tact with a sloped surface, we would like to determine words, with our analytic results it is possible to avoid
whether it is captured, i.e., whether the part will con- computing a potentially large number of collisions.
verge to the closest stable pose. For simplicity, let the In our analysis we have focused on the two dimen-
surface be a tilted plane.
sional case. To illustrate the notion of capture, we will
start with another example. Consider a rod of length
Definition 1 Let a pose be defined as a point in 1 with center of mass at distance R from each vertex.
configuration space such that the part is contact with One can visualize this rod as a disk with radius R and
the surface. uniform mass, but with contact points only at the ends
of the rod (see Figure 2).
We assume that friction is sufficiently high so that a Note that the endpoints of the rod are numbered.
part cannot slide for an infinite amount of time. In gen- We will refer to these endpoints later. Let the “side”
eral capture depends on the whole surface and every- of the rod where the center of mass is above the rod
thing that happens after the current state, but the fric- be the high energy side, and the other side be the low
tion assumption and our definition of pose allow us energy side. We can then define that the rod is “on”
to define quasi-capture (in Section 3.2) of the part in the high energy side if and only if the center of mass is
Manipulation of Pose Distributions
131

plus the maximum gain in kinetic energy has to be less

than the energy required to rotate to the other side.
To guarantee that the rod is indeed captured, we have
to make sure that the maximum gain in kinetic energy
is less than the decrease in kinetic energy due to a col-
lision. There are some additional complicating factors.
Figure 2: A rod with an off-center center of mass. For instance, a change in orientation can increase the
kinetic energy, but to rotate to the other side the rod
has to rotate back, undoing the gain in kinetic energy.
between and above the endpoints of the rod. Suppose
the rod is in contact with a flat, horizontal surface. For
3.2 Quasi-Capture Velocity
the rod to make a transition from one side to the other,
it will have to rotate, either by rolling or by bouncing. What we will prove is a sufficient condition on the pose
At some point during the transition the center of mass and velocity of the rod such that it is quasi-captured.
will pass over one of the endpoints of the rod. Its po- The condition will be of the following form: if the cur-
tential energy at that point will always be greater than rent kinetic energy plus the maximal increase in ki-
or equal to the potential energy at the start of the tran- netic energy is less than some bound, the rod is quasi-
sition. Hence, to make that transition the rod has to captured. This bound depends on the current orienta-
have a minimum amount of kinetic energy. This can tion, the current velocity, the slope of the surface and
be written more formally as the geometry of the rod. Because of the way we have
set up our generalized coordinates (see [36] for details),
$m||v|l? > —mgAh. (1)
the kinetic energy is }m||v||?. In other words, the mass
Figure 3 illustrates this requirement. is just a constant scalar. Without loss of generality we
can assume m = 2. That way the kinetic energy is
simply ||v||?. We will write v for ||v||.
Ah=R(1 + sin@) |
Theorem 4 The rod with a velocity vector of length
v and in contact with the surface is in a quasi-capture
region if the following condition holds:

ip eee (v sin(€+¢)+/v?2 sin? (€+¢)—2gdn cos ¢)

Figure 3: Capture condition for a rod. cos* d

- A es + Re) < —2gR (1+ cos($ + 4)),

For a polygonal object in contact with a surface with
constant slope we will derive in Section 3.2 a lower where € is the direction of the velocity vector that
bound on the norm of the velocity such that for all will result in the largest increase of kinetic energy, 0
velocities below that bound the part will be quasi- is the relative orientation of the contact point, dn =
captured. As we vary the position of the center of R(cos a¥ — sin(@
i + ¢)) and de=
€ = cos($a + 4) _ c0s(a/2)
ae oe

mass with respect to the rod endpoints, the slope of max (tan ¢, 2sin $ sin).
the support surface and the drop height, the bound for
the capture velocity will change. This bound will also Proof: See appendix A. oO
depend on the relative orientation of the contact point
with respect to the center of mass. Note that for ¢ = 0 this bound reduces to v? <
—2gR(1+sin@). In other words, this bound is as tight
For a sloped surface the capture condition is not as as possible when the surface is horizontal.
simple as for the horizontal surface. By bouncing and
rolling down the slope, the rod can increase its kinetic One can compute € numerically, but the appendix
energy. We have derived an upper bound on how far also gives a good analytic approximation. The theo-
the rod can bounce. This gives an upper bound on the rem above gives a sufficient condition on the velocity
increase in kinetic energy. So the quasi-capture condi- and pose of the rod such that it cannot rotate to the
tion can now be stated as: the current kinetic energy other side during one bounce. But suppose there is a
 M. Moll and M. A. Erdmann
132

on David Baraff’s Coriolis simulator [2, 3], which can

simulate the motions of polyhedral rigid bodies. Corio-
lis takes care of the physical modelling. Our simulator
then computes pose distributions for different (parame-
trized) support surfaces and different initial conditions.
Our simulator uses simulated annealing to optimize
over the surface parameters and drop location with re-
spect to the surface. The objective function is to min-
imize the entropy of the pose distribution. Initially
the sampling of orientations of the object is rather
coarse, so that the resulting pose distribution is not
very accurate. But as the simulator is searching, the
simulated annealing algorithm is restarted with an in-
Figure 4: Quasi-capture velocity as a function of the slope
creased sample size and the best current solution as
of the surface and the orientation of the rod
initial guess. This way we can quickly determine the
potentially most interesting parameter values and re-
sequence of bounces, each of them increasing the ki- fine them later. Our implementation is based on the
netic energy. It is possible that the rod satisfies the one given in [37, pp. 444-455].
quasi-capture condition, but is still able to rotate to
Surfaces are parametrized using wavelets [41, 17].
the other side in more than one bounce. Thus, the
Wavelet transforms are similar to the fast Fourier
theorem by itself is not enough to guarantee that the
transform, but unlike the fast Fourier transform ba-
rod will converge to its closest stable orientation. In
sis functions (sines and cosines) wavelet basis functions
the analysis above we have ignored the dissipation of
are localized in space. This localization gives us greater
kinetic energy during collisions. If in the case the cap-
flexibility in modeling different surfaces compared with
ture condition is true the dissipation of kinetic energy
the fast Fourier transform or, say, polynomials. There
is larger than the increase due to the bounce, the rod
are many classes of wavelet basis functions. We are
will indeed be captured after an arbitrary number of
using the Daubechies wavelet filters [17] and in par-
bounces. To make sure this is the case the coefficients
ticular the implementation as given in [37, pp. 591—
of restitution can not be too large.
606]. To reduce an arbitrary surface to a small number
In Figure 4 the quasi-capture velocity is plotted as a of coefficients we first discretize the function describ-
function of the slope of the surface and the orientation ing the surface. We then perform a wavelet transform
of the rod. The slope ranges from 0 to $ and the orien- and keep the largest components (in magnitude) in the
tation ranges from 0 to 27. Note that the orientation transform to represent the surface. When we minimize
of the rod is not the same as the relative orientation the entropy, we optimize over these components. We
of the contact point. However, for each combination can either keep the smaller components of our initial
of @ and @ the relative orientation of the contact point wavelet transform around or set them to zero.
can be easily computed. The other relevant parameter
values for this plot are: R = 1m, g = —9.81m/s? and Development of a second simulator was started, be-
a= 3%. The little bump in the middle corresponds to cause Coriolis had some limitations. In particular, the
the rod being captured on the high-energy side. The collision model could not be changed and we wanted to
bigger bumps on the left and right correspond to being experiment with Chatterjee’s collision model [15]. The
captured on the low-energy side. second simulator also allowed us to optimize for our
specific dynamics model. In our model there is only
one moving object, and the only forces acting on it are
4 Simulation and Experimental Results
gravity and friction. Currently, the simulator only han-
4.1 Dynamic Simulation dles two-dimensional objects, but in the future it might
be extended to handle three dimensions as well. It uses
To numerically compute the pose distribution of parts, the analytic results from the previous section to stop
we have written two dynamic simulators. One is based simulating the motion of the part once it is captured.
Manipulation of Pose Distributions
133

height
Drop
height
Drop

1 2 3 4 5
‘ ; Iniited
nsasen
aa &
AS ! Initial orientation ;

Rio adines ute cS SN ot RN SS

(a) After one bounce ~ (b) After three bounces

= a. 0.7
on
7]
<=
2.0.5 40.6
=
Drop
height “S|
EI0.4 40.5

=
0.3 40.4

i—)ie
C= Optimal drop height |
os Lower bound on prob. of low energy side bound
Lower
of
prob.
low
side
on
energy

“1s
nN
1 6 1 2 3 4
Iniaveto \ Number of bounces
IS : ;
peey atte (c)anhi
(d) Optimal drop height and lower bound on
Afttieo
er ’sfive bounces a probability of ending up on low-energy side

a... on the low energy side a on the low energy side, but not captured ln the high energy side

Figure 5: Quasi-capture regions for the low energy side of the rod on a 7° slope. The rod parameters are a = 1/2,
R = 0.05m, gravity, g, equals —1.20m/s*, the coefficient of friction, 1, is 5 and the restitution parameters are: e = 0.1,
et = —0.2.

Using the simulator we can compute the quasi- Let the optimal drop height be defined as the drop
capture regions for the rod. Figures 5(a)—(c) show the height that maximizes the probability of ending up
quasi-capture regions for the low energy side after one, on the low energy side. Then dropping the rod with
three and five bounces. The dark areas correspond to uniformly random initial orientation from the optimal
initial orientations and initial velocities that result in drop height will reduce uncertainty about its orienta-
the rod being quasi-captured. The zero orientation is tion maximally (unless there exists a drop height that
defined as the orientation where endpoint 1 is to the will result in an even higher probability for the high
right of the center of mass. The triangles below the energy side). In Figures 5(a)—(c) the drop height that
X-axes show the pose of the rod corresponding to the results in the maximum probability of ending up on
orientation at that point of the X-axes. the low energy side is marked by a horizontal line. Af-
 M. Moll and M. A. Erdmann
134

collide at the pins. Experimentally we determined the

pose distribution of the rod for different values for g,
h and @ by determining the final stable pose for 72
equally spaced initial orientations. Our simulation and
experimental results of some tests have been summa-
rized in table 2. The rows marked with an asterisk have
been used to estimate the moment of inertia of the rod
and the coefficients of friction and restitution. The es-
+ optimal
timated values for these parameters are: e = 0.404,
e, = —0.136, p = 0.0376 and p = 4.71. Note that for
a low drop height and a horizontal surface the pdf is
equal to a quasistatic approximation, as one would ex-
+ quasi-
pect. More surprisingly, we see that the probability of
static
ending up on the low-energy side can be changed to ap-
proximately 0.95 by setting g, h and ¢ to appropriate
values. In other words, we can reduce the uncertainty
Table 2: Simulation and experimental results for the rod. almost completely.
Shown are the probabilities of ending up on the low-energy
One can identify several error sources for the differ-
side for different values for g, h and ¢. The drop height is
ences between the simulation and experimental results.
measured from the center of the disk to the surface.
First, there are measurement errors in the experiments:
in some cases slight changes in the initial conditions will
ter each successive bounce this drop height is likely to change the side on which the rod will end up. Second,
be a better approximation of the optimal drop height. since the simulations are run with finite precision, it
In Figure 5(d) the approximate optimal drop height is possible that numerical errors affect the results. Fi-
and lower bound on the probability of ending up on nally, the physical model is not perfect. In particular,
the low-energy side after one through five bounces is the rigid body assumption is just false. The surface on
shown. One thing to note is that both the optimal drop which the rod lands is coated with a thin layer of foam
height and the lower bound on the probability of end- to create a high-damping, rough surface. This is done
ing up on low-energy side rapidly converge. This seems to prevent the rod from colliding with the sides of the
to suggest that after only a small number of bounces air table.
we could make a reasonable estimate of the optimal
drop height and uncertainty reduction. Further study
4.3 3D Results
is needed to find out if this is true in general.
We have not generalized our analytic results to three
4.2 2D Results
dimensions yet, but we can still use our optimization
To verify the simulations we also performed some ex- technique to find a good surface and drop height for a
periments. Our experimental setup was as follows. We given object. For the dynamic simulation we rely now
used an air table to effectively create a two-dimensional on Baraff’s Coriolis simulator. Figure 6(a) shows an
world. By varying the slope of the air table we can vary orange insulator cap? at rest on flat, horizontal surface.
the gravity. At the bottom of the slope is the surface The contact points are marked by the little spheres.
on which the object will be dropped. The angle ¢ of In Figure 6(b) the bowl resulting from the simulated
the surface in the plane defined by the air table can, of annealing search process is shown. The initial shape
course, be varied. is a paraboloid: f(z,y) = (2? + y”)/20. This shape
The rod of the previous section has been imple- is reduced to a triangulation of a 8 x 8 regular grid.
mented as a plastic disk with two metal pins sticking The part is always released on the left-hand side of the
out from the top at an equal distance from the center bowl.
of the disk. When released from the top of the air ta- ae object has been used before as an example in (21,
ble the disk can slide under the surface and will only 30, 38].
Manipulation of Pose Distributions
135

(a) Orange insulator cap on a flat surface (b) ... and on an optimized bowl
Figure 6: Result of optimizing a surface for the orange insulator cap.

Table 3 compares the simulation results with exper-

imental data from [21]. The format is the same as
in table 1, except that the stable poses are now writ-
ten as vectors. These vectors are the outward pointing
normals (w.r.t. the center of mass) of the planes pass-
ing through the contact points. That way, a face with
2Principal
Axis
many vertices in contact with the surface will always
be represented by the same vector, no matter which
subset of the vertices is actually in contact. In the
experimental setup of [21] the part was dropped from
—3+
one conveyer belt onto another. The initial drop height
i erie, ieeal lent ea aa was 12.0 cm. In the experiments the part had an initial
Principal
Axis 1
horizontal velocity of 5.0 cm/s. The second row cor-
Figure 7: Entropy as a function of the two principal axes responds to computing the pose distribution when the
of the searched five-dimensional parameter space. part is dropped from 12.0 cm (but with initial velocity
set to 0). The third row corresponds to a local mini-
We optimized over the four largest wavelet coeffi- mum returned by the simulated annealing algorithm.
cients of the initial shape and the drop height. The
search for the optimal bowl and drop height is visual- 5 Discussion
ized in Figure 7. The five-dimensional parameter space
is projected onto a two-dimensional space using Princi- We have shown a sufficient condition on the posi-
pal Component Analysis [27]. Each point corresponds tion and velocity of the simplest possible “interesting”
to a bowl shape evaluation, i.e., for each point a pose shape (i.e., the rod) that guarantees convergence to the
distribution is computed. The size of each point is nearest stable pose under some assumptions. This con-
proportional to the sample size used to determine the dition gives rise to regions in configuration phase-space,
pose distribution. Computing a pose distribution by where each point within such a region will converge to
taking 600 samples takes about 40 minutes on a 500 the same stable pose. We have coined the term quasi-
MHz Pentium III. The surface in Figure 7 is a cubic capture regions for these regions, since they are very
interpolation between the points. The dark areas cor- similar to Kriegman’s notion of capture regions. In
respond to areas of low entropy. Notice that most of simulations quasi-capture always seems to imply cap-
the points are in or near a dark area. ture, but further research is needed to prove this claim.
 M. Moll and M. A. Erdmann
136

Stable Poses
) (Oy 150) 8,056) (.7,0,—.7) (0,0,—1) | Entropy
(=1,0,0) (0,—1,0
Experimental, flat (1036 trials) 0.271 0.460 0.197 0.050 0.022 1.58
Dynamic simulation, flat surface 0.355 0.207 0.221 0.185 0.019 0.014 1.48
Dynamic simulation, optimal bowl 0.622 0.125 0.154 0.096 0.003 0.000 1.09

Table 3: Probability distribution function of stable poses for two surfaces. The initial velocity is zero and the initial rotation
is uniformly random. The experimental data is taken from [21]. There, (0,—1,0) and (0,1, 0) are counted as one pose.

The quasi-capture regions also apply to general do more experiments to verify our current and future
polygonal shapes. However, we can no longer use the analytic results.
symmetry of the rod. So the quasi-capture expressions
for general polygonal shapes become more complex. Acknowledgments
On the other hand, we might be able to orient pla-
nar parts by using a setup similar to the one described This work was supported in part by the National Sci-
in Section 4 and attaching two pins to the top of the ence Foundation under grant IRI-9503648. The au-
part. Generalizing the quasi-capture regions to three thors are grateful to Al Rizzi, Matt Mason and Devin
dimensions is non-trivial and is an interesting direction Balkcom for their helpful comments.
for future research.
The simulation and experimental results show that References
the simulator is not 100% accurate, but that it is a
useful tool for determining the most promising initial [1] S. Akella, W. H. Huang, K. M. Lynch, and M. T. Ma-
conditions for uncertainty reduction. In other words, son. Sensorless parts orienting with a one-joint ma-
nipulator. In Proc. 1997 IEEE Intl. Conf. on Robotics
the optimum predicted by the simulator will probably
and Automation, 1997.
be near-optimal in the experiments. We can then ex-
perimentally search for the true optimum. [2] David Baraff. Coping with friction for non-penetrating
rigid body simulation. Computer Graphics, 25(4):31-
Another area where quasi-capture regions may be 40, July 1991.
applied is in computer animation. Before a part comes
to rest, there are many interactions between the part [3] David Baraff. Issues in computing contact forces
and the support surface. It turns out that these in- for non-penetrating rigid bodies. Algorithmica, pages
292-352, October 1993.
teractions are computationally very expensive. With
our capture regions we can eliminate the last “clatter- [4] Dina R. Berkowitz and John Canny. Designing parts
ing” motions of the part, since we can predict what feeders using dynamic simulation. In Proc. 1996 IEEE
the final pose will be. For applications where fast an- Intl. Conf. on Robotics and Automation, pages 1127—
1132, April 1996.
imation is more important than physical accuracy, a
pre-computed motion can be substituted for the ac- [5] Dina R. Berkowitz and John Canny. A comparison of
tual motion. real and simulated designs for vibratory parts feeding.
In Proc. 1997 IEEE Intl. Conf. on Robotics and Au-
With future research we hope to improve the con- tomation, pages 2377-2382, Albuquerque, New Mex-
straints on the quasi-capture velocity by taking into ico, 1997.
account more information, such as the direction of the
velocity vector. If improving the quasi-capture bounds [6 —a Robert-Paul Berretty, Ken Goldberg, Lawrence Che-
ung, Mark H. Overmars, Gordon Smith, and A. Frank
is impossible, it might be possible to get better ap-
van der Stappen. Trap design for vibratory bowl feed-
proximations for pose distributions. As noted in Sec- ers. In Proc. 1999 IEEE Intl. Conf. on Robotics and
tion 4.1 it is possible to get a good estimate of the max- Automation, pages 2558-2563, Detroit, MI, May 1999.
imal uncertainty reduction after only a small number
[7] Vivek Bhatt and Jeff Koechling. Partitioning the pa-
of bounces of the rod. So another interesting line of
rameter space according to different behavior during
research would be to find out how accurate these ap- 3d impacts. ASME Journal of Applied Mechanics, 62
proximations are in general. We are also planning to (3):740-746, September 1995.
Manipulation of Pose Distributions
137

[8] Vivek Bhatt and Jeff Koechling. Three dimensional

[20] Rasmus Feldberg, Maciej Szymkat, Carsten Knudsen,
frictional rigid body impact. ASME Journal of Applied and Erik Mosekilde. Iterated-map approach to die
Mechanics, 62(4):893-898, December 1995. tossing. Physical Review A, 42(8):4493-4502, October
1990.
(9 Karl Friedrich Bohringer, Vivek Bhatt, Bruce R. Don-
Tamed
ald, and Kenneth Y. Goldberg. Algorithms for sensor-
less manipulation using a vibrating surface. Algorith-
[21] Ken Goldberg, Brian Mirtich, Yan Zhuang, John
Craig, Brian Carlisle, and John Canny. Part pose sta-
mica, 1997. Accepted for publication. tistics: Estimators and experiments. IEEE Trans. on
Robotics and Automation, 15(5), October 1999.
[10] Karl Friedrich Bohringer, Bruce Randall Donald, and
Noel C. MacDonald. Programmable vector fields for [22] Kenneth Y. Goldberg. Orienting polygonal parts
distributed manipulation, with applications to MEMS without sensors. Algorithmica, 10(3):201-225, August
actuator arrays and vibratory parts feeders. Intl. J. of 1993.
Robotics Research, 18(2):168—200, February 1999.
[23] S. Goyal, J. M. Papadopoulos, and P. A. Sullivan. The
[11] C. Boothroyd, A. H. Redford, C. Poli, and L. E. dynamics of clattering I: Equation of motion and ex-
Murch. Statistical distributions of natural resting as- amples. J. of Dynamic Systems, Measurement, and
pects of parts for automatic handling. Manufacturing Control, 120:83-93, March 1998.
Engineering Transactions, Society of Manufacturing
Automation, 1:93-105, 1972. [24] S. Goyal, J. M. Papadopoulos, and P. A. Sullivan. The
dynamics of clattering II: Global results and shock
[12] Geoffrey Boothroyd, Corrado Poli, and Laurence E. protection. J. of Dynamic Systems, Measurement, and
Murch. Automatic Assembly. Marcel Dekker, Inc., Control, 120:94—102, March 1998.
New York; Basel, 1982.
[25] David D. Grossman and Michael W. Blasgen. Orient-
[13] M. Buhler and D. E. Koditschek. From stable to ing mechanical parts by computer-controlled manipu-
chaotic juggling: Theory, simulation, and experi- lator. IEEE Trans. on Systems, Man, and Cybernetics,
ments. In Proc. 1990 IEEE Intl. Conf. on Robotics 5:561-565, September 1975.
and Automation, pages 1976-1981, 1990.
[26] Hajime Hitakawa. Advanced parts orientation system
[14] Michael E. Caine. The Design of Shape from Motion has wide application. Assembly Automation, 8(3):147-
Constraints. PhD thesis, MIT Artificial Intelligence 150, 1988.
Laboratory, Cambridge, MA, September 1993. Tech-
nical Report 1425. [27] TF lee Jollitte: Principal Components Analysis.
Springer-Verlag, New York, 1986.
[15] Anindya Chatterjee and Andy L. Ruina. A new alge-
braic rigid body collision law based on impulse space [28] Lydia E. Kavraki. Part orientation with program-
considerations. ASME Journal of Applied Mechanics, mable vector fields: Two stable equilibria for most
1998. Accepted for publication. parts. In Proc. 1997 IEEE Intl. Conf. on Robotics
and Automation, pages 2446-2451, Albuquerque, New
Mexico, 1997.
[16] Alan D. Christiansen, Andrea Dunham Edwards, and
Carlos A. Coello Coello. Automated design of part
feeders using a genetic algorithm. In Proc. 1996 IEEE [29] Zhang Kechen. Uniform distribution of initial states:
Intl. Conf. on Robotics and Automation, volume 1, The physical basis of probability. Physical Review A,
pages 846-851, 1996. 41(4):1893-1900, February 1990.

[17] Ingrid Daubechies, editor. Different Perspectives on [30] David J. Kriegman. Let them fall where they may:
Wavelets, volume 47 of Proceedings of Symposia in Capture regions of curved objects and polyhedra. Intl.
Applied Mathematics, 1993. AMS. J. of Robotics Research, 16(4):448-472, August 1997.

[18] Michael A. Erdmann and Matthew T. Mason. An [31] Jayaraman Krishnasamy. Mechanics of Entrapment
exploration of sensorless manipulation. [EEE J. of with Applications to Design of Industrial Part Feeders.
Robotics and Automation, 4(4):369-379, August 1988. PhD thesis, Dept. of Mechanical Engineering, MIT,
May 1996.
[19] Michael A. Erdmann, Matthew T. Mason, and George
Vanétek, Jr. Mechanical parts orienting: The case of [32] Kevin M. Lynch. Toppling manipulation. In Proc.
a polyhedron on a table. Algorithmica, 10:226-247, 1999 IEEE Intl. Conf. on Robotics and Automation,
pages 2551-2557, Detroit, MI, May 1999.
1993.
 M. Moll and M. A. Erdmann
138

[33] Kevin M. Lynch, Naoji Shiroma, Hirohiko Arai, and Appendix A: Proof of Theorem 4
Kazuo Tanie. The roles of shape and motion in dy-
namic manipulation: The butterfly example. In Proc.
1998 IEEE Intl. Conf. on Robotics and Automation, Definition 5 Let a bounce be defined as the flight
1998. path between two impacts.

[34] Alessia Marigo, Yacine Chitour, and Antonio Bicchi.

Manipulation of polyhedral parts by rolling. In Proc. The closest distance between the rod and the slope
1997 IEEE Intl. Conf. on Robotics and Automation, during one bounce can be described by
pages 2992-2997, 1997.
d(t) = $g(cos )t? + (vy cosh + vz sin g)t— de), (2)
[35] Brian Mirtich and John Canny. Impulse-based simu-
lation of rigid bodies. In Proc. 1995 Symposium on where vz and vy are the translational components of
Interactive 3D Graphics, April 1995.
the velocity and dg(z) is a component that depends on
[36] Mark Moll and Michael A. Erdmann. Ma- the orientation. Let the rod be in contact at t = 0.
nipulation of pose distributions. Technical Then d(0) = 0 (and therefore dg(9) = 0). Let t be the
Report CMU-CS-00-111, Dept. of Computer smallest positive solution to d(t) = 0. The change in
Science, Carnegie Mellon University, 2000.
height is then Ah = :gt? + vyt, so that the change in
http://www.cs.cmu.edu/ mmoll/publications.
v2 is Av? = 2gAh = g?t? + 2v,gt. To find the max-
[37] William H. Press, Saul A. Teukolsky, William T. Vet- imum Av” for all velocity vectors of length v we can
terling, and Brian P. Flannery. Numerical Recipes in
parametrize the translational velocity as v; = uvcos€
C: The art of Scientific Computing. Cambridge Uni-
versity Press, second edition, 1992. and vy = vsin€, and maximize over €. This ignores
the rotational component of the velocity, but the fol-
[38] Anil Rao, David Kriegman, and Ken Goldberg. Com- lowing lemma shows that for a certain value of dg/j)
plete algorithms for reorienting polyhedral parts using
a pivoting gripper. In Proc. 1995 IEEE Intl. Conf. on the resulting solution for Av? is an upper bound for
Robotics and Automation, May 1995. the true maximal increase of v?.

[39] Edward John Routh. Dynamics of a System of Rigid

Bodies. MacMillan and Co., London, sixth edition, Definition 6 Let the ideal orientation be defined as
1897. the orientation where the rod is parallel to the surface
[40] Arthur C. Sanderson. Parts entropy methods for ro- and the center of mass is below the rod.
botic assembly system design. In Proc. 1984 IEEE
Intl. Conf. on Robotics and Automation, pages 600- Lemma 7 We can always increase the rod’s kinetic
608, 1984. energy after a bounce by allowing it to rotate around
the center of mass “for free” (i.e., without using en-
[41] G. Strang. Wavelets and dilation equations: A brief
introduction. SIAM Review, 31:613-627, 1989. ergy) to the ideal orientation (ignoring penetrations of
the surface) and then letting it continue to fall while
[42] E. T. Whittaker. A Treatise on the Analytical Dynam- maintaining this orientation. However, if the rod is
ics of Particles and Rigid Bodies. Dover, New York,
fourth edition, 1944.
already in the ideal orientation after the bounce, its ki-
netic energy cannot be increased.
[43] Jeff Wiegley, Ken Goldberg, Mike Peshkin, and Mike
Brokowski. A complete algorithm for designing passive Proof: One can easily verify that rotating around the
fences to orient parts. In Proc. 1996 IEEE Intl. Conf. center of mass to the ideal orientation of the bounce
on Robotics and Automation, apr 1996.
maximizes distance between the rod and the surface.
[44] Jeff Wiegley, Anil Rao, and Ken Goldberg. Computing This distance will always be greater than or equal to
a statistical distribution of stable poses for a polyhe- 0. If we allow the rod to continue to fall until it hits
dron. In 30th Annual Allerton Conf. on Communica- the surface, its kinetic energy will increase. O
tions, Control and Computing, 1992.
From this lemma it follows that by assuming the rod
[45] Nina B. Zumel. A Nonprehensile Method for Reli-
able Parts Orienting. PhD thesis, Robotics Institute, rotates “for free” to the ideal orientation the increase
Carnegie Mellon University, Pittsburgh, PA, January in kinetic energy due to one bounce is an upper bound
1997. on the true increase of kinetic energy. With this lmma
Manipulation of Pose Distributions
139

‘ideal
Asi wo eh
d, = Rcos(Q/2) -Rsin@+0)

Se p Ros)

(a) Change in distance between the center of mass (b) Trajectory of the center of mass during a bounce
and the surface in poses with the initial and ideal
orientation
Figure 8: Increase in kinetic energy when rotating to the ideal orientation

computing the next contact point is a lot easier. Let Proof: We can distinguish several cases: endpoint 1 of
0 be the relative orientation of the contact point at the rod or endpoint 2 can be in contact with the slope,
t = 0. @ = 0 corresponds to the contact point being and the rod can be on the low or high energy side. We
to the right of the center of mass. The signed distance will prove the case where endpoint 1 is in contact and
from the center of mass to the surface at t = 0 is then the rod is on the high energy side. The proof for the
—Rsin(@+ ¢), as shown in Figure 8(a). One can easily other cases is analogous. The case under consideration
verify that in the ideal orientation the relative orienta- is shown in Figure 9(a). To roll counterclockwise over
tion of endpoint 1 is | — $ — @. Let 6 be equal to this to the left side, v? > —2gh1. The distance h is simply
relative orientation. In the pose where the rod is in con- equal to R(1+sin@). If the rod rolls clockwise over to
tact with the surface and has the ideal orientation the two-point contact and continues to roll over endpoint 2,
signed distance from the center of mass to the surface the rod gains kinetic energy because the second contact
is —Rsin(@ + ¢) = —Rcos §. So in total the center of point is lower than the first contact point. This gain is
mass travels at most a distance R(cos $ —sin(#+¢)) in proportional to hg.
the direction normal to the surface during one bounce. One can easily verify that for two-point contact the
Let d, be equal to this distance. To solve for the time relative orientations of contact points 1 and 2 are ao -
of impact we can treat the rod as a point mass centered 5 — 9 and 4 + $ — 9, respectively. The bound for
at the center of mass and replace dys) in Equation 2 rolling over endpoint 2 is therefore
with —d,. Equation 2 is then simply a paraboloid in
t. The distance function now measures the distance Vo 2g(h3 — he)
between the center of mass and the dotted line parallel = — 2gR(1+sin6 — cos($ — ¢) + cos($ + ¢))
to the surface shown in Figure 8(b). This approach is
= — 2gR(1+sin0 — 2sin $ sin).
not limited to the case where our new orientation is the
ideal orientation. Suppose an oracle would tell us that
If the center of mass is to the left of the contact point
the new orientation is 9. Then we can solve for the time
the last term will change sign. We can therefore com-
of impact by substituting R(sin(@+ ¢) —sin(@+ @)) for bine the two bounds (one for rotating clockwise, and
dg) in Equation 2. for rotating counterclockwise) into this bound:
The following lemma gives a bound on the velocity
needed to roll to the other side. v? > min(—2gR(1 + sin 6),
— 2gR(1 + sin@ + 2sign(cos @) sin $ sin @))
Lemma 8 [f the rod is in rolling contact, then to be =—2gR(1+sin §+(sign(cos @)—1) sin - sin ¢). (3)
able to roll to the other side the following condition
has to hold: v2? > —2gR(1 + sin@ + (sign(cos#) — O
1)sin ¢sin¢). We assume 0 << 5.
 M. Moll and M. A. Erdmann
140

sree

ia A

(c) Endpoint 1 in contact, low energy side (d) Endpoint 2 in contact, low energy side

Figure 9: Capture condition for rotation

Theorem 4 The rod with a velocity vector of length The maximum change in v? is then bounded by
v and in contact with the surface is in a quasi-capture
Av? =2gAh< 29(4gt? + v(sin €)t)
region if the following condition holds:
=2veasGsin&(y sin(€ + 9)
v2 Bees se (usin(E+4)+/0? sin®(EFG)—29dn
C056) + 4/v? sin?(€ + ¢) — 2gdn cos d) — 22%. (5)
— 29(-22, + Re) < —29R (1 + cos($ + 4)), cos @

After one bounce the orientation is assumed to be such

where € is the direction of the velocity vector that will that rod is parallel to the surface and the center of
mass is below the rod, as this will result in the largest
result in the largest increase of kinetic energy, dn =
increase in kinetic energy according to lemma 1. This
R(cos $ — sin(@ + 9) and € = cos($ + ¢) — eo +
means that endpoint 1’s relative orientation is equal to
max ee g, 2sin$ sin o).
6=5-— 5-9. Substituting this value in Equation 3 of
lemma 2 gives —2gR(1+sin@). In other words, if the
Proof: The path of the center of mass during a bounce kinetic energy after the bounce is less than —2gR(1 +
that increases the kinetic energy maximally is de- sin) and the rod is in the ideal orientation, the rod
scribed by 3gt? cos $+v(sin € cos d+cos € sin $)t+dn = cannot roll to the other side.
0. The smallest positive solution of this equation is
We can combine the two bounds to obtain a sufficient
f = —v(sin € cos d+cos € sin g)
condition to determine whether the rod can rotate to
gcos @ the other side if its new orientation after one bounce is
Vv? (sin € cos $+cos € sin d)?—2gd,, cos d equal to the ideal orientation. Unfortunately this con-
od gcos @ (4) dition does not imply a similar condition for the general
__—vsin(€+¢)—»/v? sin? (€+¢)—29dn coat case where the new orientation is not necessarily equal
— gcos@ to the ideal orientation.
Manipulation of Pose Distributions
141
Consider the case v = 0*. Substituting this value in The correction € is therefore
Equation 5 and expanding the definition of d, shows
that the maximum increase in kinetic energy is then € = max,(sin6— sin6— (sign(cos @) — 1) sin § sing
_—29dn _ _ 29R(sin(6+$)—sin(6+¢)) 4 BCE Sas
cosdé cos d (6) cos @

Therefore, when v = 0* and the relative orientation By differentiating the expression inside max(-) with re-
of the contact point after the bounce is equal to ideal spect to @ we find that there is a local maximum at
orientation the quasi-capture constraint is 6 = 0. Other local maxima occur when 6 approaches
— 5 from below or =5 from above. The correction ¢€
2gRinte+o)—sin(0+9)
a lah DT —2gR(1+sin6). (7) therefore simplifies S

vie sales) |
That is, if an upper bound on the kinetic energy after € =max (sind sin(O+-o) sind sin 6+2sin &
cos p cos @
one bounce is less than the energy needed to rotate to
the other side, the rod will not be able to rotate to = cos( + ¢) — cos(o/2)
cos d
4 max ae 2sin
$sin @) .
the other side. Now suppose the new orientation is not
equal to the ideal orientation. Then the increase of ki- For uv # 0* the difference between f (6) and f (0) is even
netic energy will be less, but the energy required to roll larger and g(0) does not depend on 2, so the value for €
to the other side will be less, too. Let 6 be the rela- is an upper bound for all v. Combining all the bounds
tive orientation of the contact point after the bounce. we arrive at the desired result. Oo
Equation 7 is of the form f(6) < (6), where f(-) com-
putes the kinetic energy after one bounce for a given Note that for ¢ = 0 this bound reduces to v? <
new orientation and g(-) computes the energy needed —2gR(1+sin@). In other words, this bound is as tight
as possible when the surface is horizontal.
to roll to the other side for a given orientation®. Un-
fortunately, this bound does not imply V6.f (8) < g(@). For an arbitrary d,, it is not possible to compute the
From the ‘oracle argument’ on page 139 it follows that optimal € analytically. Fortunately, we can analytically
f() is indeed an upper bound on the maximum in- solve for € if we assume that the bounce consists of
crease of the kinetic energy. Substituting 9 in lemma 2 pure translation. The resulting € can be used as an
shows that g(@) is a lower bound on the kinetic en- approximation. It can shown that the solution for €
ergy needed to roll to the other side. We would like to can be written as
determine the smallest possible ¢ such that y1—sin d
pe ee cee IT Ee vor
a V2V/1—sin d
f(0)-2gRe<g() = v6.¢(8) < 9(8). Substituting these values in Equation 4, we find that
It is not hard to see € has to be equal to the approximation for the bound for Av? then simpli-
fies to
ax(9(8) ae) f (9) + f(9))/(—2gR).
aN 2 eS ers
2qdn v? sind
parr aUbe 1 — 4dng Frese$)
The difference between f(6) and f(6) is
The relative error in this approximation depends on ¢,
2g R EDEL s dn, v and g and can be computed numerically. Some-
cos @
what surprisingly, the relative error appears to be con-
Similarly, the difference between g(9) and g(6) is stant in v, d, and g. The relative error does vary sig-
nificantly with ¢, but is still fairly small (on the order
—2gR (sind — sin6— (sign(cos 4) — 1) sin $ sin 6)' of LO es

3 Analagous to expression 3, g(6) equals

—2gR (1+ sin@+ (sign(cos @) — 1) sin $ sin 0).
 ;
“tee,

a
~

CO ee
7

hn Swab 09 letys at soagutl

ee ee
gy 6
silvaen Pict
‘Wl La eto oi jalan odeBe
an wut
le oe 4? Fouled > é Nigh
grey aa
ie obs
(v) . (hele #1 Wy |i ia 4.
e - = © ont»

4 an) Kee == ville 2a ois wily vo howod sep

ad eeHO Ayhiteus qpreyte ads cody pael
al©
(+ Ee yr
oY sen op alie on Dae Hise hove oad Bae
Lavlt 2d ‘dg, ” oh
a rut i es, wort OW
wie woPO dy te 1 Sey hy » anypnacepei wl out AO Sadar
ph ape )) Dts Yai Ltave oat Vhs Haig igre
rv adsob
ole pealad
Pl on i ho oe Pe ee. “weal ad Diep olde nhige
AAe WITT yeuened wits india titer Yaka ALT
o % gal

Bion vai ic” «RM mna8 Ga ea

; “tt (-), Sada fdte> (OA aes we
lap e ah 6
0 = 454 wails 9304

ae eh Weare fh usb ct) yang on... aU SING aoly & Tobetge abr a lot oF
ee. Hub Viettidw ne At (jy > (Oy THRE Jind daa Bor deur, ult
a t }
ava pllaniny| Lit
“4
4 Lat Myc Reed qartied HORT omc ages Np Rs : tie
in ‘
mide a qu 2 wil, he
ela “i! murrbeens
i» ix a
— =i4 Go Ditod “a ORS Ge. ial
nites hae Maidaluies :
Vig Cutty als Tiniels Maia atone
mM) aaa H he gers “1% a/toaie ealy 0. yee al A Cy a
: =i #4 inayhdT Ick su on alll SUM WI * aalile tualot
— TS" a dle cl
- 3H
x Sia iSoasua, v/ridorng: oll
> din fry ee te

: mii! Avpmude bd Bey rot. y «

pivenob ri expla cw mil) Mitr) di PN Last nhs i ene ; Hans
aa a i OF(tui! aur ixOn Vs rit i» tears J loupe, Mh 4
7 7 #! mt
— leew sa : ~
ef { "
: -- ee iy ) (jie —s . ache
ae il le
SA
ave | eye )
> 7 ~_ 7
- y?
a apy iayonmidataay a CUO
.
al ho.
Prt
} i
taker il 7
.

: et aeae hinlugebrrgery sae Pi) s.® Diis oh }

kok BS
5
ee
- :
ONE eo iiintige Jun oor
ui lay «

“ ‘og
Bare write GT atin, ob Mh, Wade ~ at (Wiep,bacy q
aa ty) Waice! HRP, et ted te dpa uvrfln «
: = id v \ lhe. >
wi ly lehuis ts
>vis it

Nie 4
7

a
‘ oo
Image Guided Surgery
Eric Grimson, MIT, Cambridge, MA
Michael Leventon, MIT, Cambridge, MA
Liana Lorigo, MIT, Cambridge, MA
Tina Kapur, Visualization Technology Incorporated, Wilmington, MA
Olivier Faugeras, MIT, Cambridge, MA
Ron Kikinis, Brigham and Women’s Hospital, Boston, MA
Renaud Keriven, Cermics, Ecole Nationale des Ponts et Chaussées, Paris, France
Arya Nabavi, Brigham and Women’s Hospital, Boston, MA
Carl-Fredrik Westin, Brigham and Women’s Hospital, Boston, MA

Recent advances in image guided surgery are changing

the manner in which surgeons are able to execute dif-
ficult procedures. By building detailed, patient-specific
models of anatomy, and augmenting those models with
other information, such as functional properties, the
surgeon can better plan her procedure to optimally ex-
tract target tissue while avoiding nearby critical struc-
tures. By registering these models with the actual pa-
tient position in the operating theatre, and by track-
ing surgical instruments relative to the patient and the
registered model, real-time feedback is provided about
the position of the instrument and its relationship to
nearby, hidden tissue.
A central aspect of IGS is creating accurate, detailed, Figure 1: Standard imagery, such as MRI, highlights dif-
patient-specific models from medical imagery. In this ferent tissue responses.
paper, we briefly outline an overall approach to image
guided surgery, and present several examples of current or hidden structures, leading to faster, safer and more
methods for model building. effective minimally invasive procedures.
A standard framework for Image Guided Surgery
1 Introduction consists of the following stages:

Recent developments in computer vision and robotics e A set of medical images (such as MRI or CT) are
are changing the manner in which modern surgery is acquired of the relevant portion of the patient’s
being practiced. Image guided surgical methods are anatomy (Figure 1).
providing a surgeon with the ability to visualize inter- e These images are segmented into distinct anatomi-
nal structures and their geometric relationships, often cal structures, yielding a 3D patient-specific model
in alignment with live imagery or direct views of the (Figure 2).
patient. Such visualization abilities enable a surgeon to
plan minimally invasive procedures. Tracking methods e The model is then registered to the actual posi-
further allow a surgeon to see the actual position of his tion of the patient on the operating table. Surgi-
instruments, and their physical relationship to critical cal instruments are tracked relative to the patient
144 E. Grimson et al.

marks between the scans, we have relied on a regis-

tration method that finds the best alignment between
image volumes, based on the alignment of all the data
in the volumes. In particular, our group has devel-
oped registration methods that maximize the Mutual
Information between data sets, and have demonstrated
that such methods can robustly align a wide range of
medical imagery [29].
Once such augmented models are created, we also
need to register them to the actual position of the pa-
Figure 2: These images are converted to patient-specific tient. To do this, we use a surface matching algorithm.
geometric models of anatomy. They can be used both for 3d data points from the skin surface of the patient are
planning (left) and to support navigation by tracking surgi- acquired using a trackable probe (typical methods in-
cal instruments (right). volve locating synchronized LED’s on the probe in a
set of three cameras and using triangulation to locate
the probe, or using electromagnetic coils tracking small
and the model, allowing the surgeon to effectively
metal balls attached to the probe, and similarly using
execute procedures while avoiding hidden, critical
triangulation to localize the probe). These data points
structures (Figure 2).
are then matched to the skin surface of the recon-
structed patient model to find the best fit, and thus to
In this paper, we briefly outline current approaches
align the model with the actual patient position [13, 14]
to each of these stages, then examine in more detail the
first stage — segmentation of medical scans into patient-
specific models that can be used for surgical planning, 4 Navigation
surgical guidance, and surgical navigation.
Finally, by tracking the position of any surgical instru-
ment in the operating site, one can relate the position
2 Segmentation
of the tip of the instrument to the corresponding point
In the first stage, we need to convert standard medical on the segmented model. By displaying this informa-
imagery into information that more directly reflects pa- tion on a monitor, the surgeon can rapidly navigate to
tient anatomy. As detailed in Section 5, we use a vari- desired target sites while avoid critical structures.
ety of algorithmic methods to label individual elements
of a volumetric scan by tissue type, and to collect those 5 Segmentation
labeled voxels into connected structures. We will dis-
cuss those methods in detail later, for now it suffices The first stage of this pipeline is key. One must be
that patient specific geometric models can be created. able to extract relevant information from the medical
imagery, in order to most effectively present it to the
3 Registration surgeon. This becomes a problem of segmentation —
labeling each voxel of the image with the associated
Given segmented models of the patient (see S ection 5), tissue type, then agglomerating adjacent voxels with
we want to use that information to assist the surgeon. common labels into connected structures. In the fol-
One aspect of this is to allow the surgeon to visualize lowing sections, we describe in more detail three cur-
structures and their relationships, in order to support rent techniques for achieving this stage.
plan reliable procedures. To this end, it is often useful
to augment the structure models created by segment- 6 Statistical Labeling of Images
ing MRI data, with functional and other information
from other imagery (e.g., {MRI, PET, SPECT). This Our first method uses statistical properties of tissue
requires registration of two or more different modali- response in MRI imagery to classify tissue type asso-
ties. Since there is often no set of corresponding land- ciated with each voxel [17, 18, 31]. The basis of the
Image Guided Surgery
145
method is straightforward. Suppose one could iden- ond method [27] adds global shape information into
tify a small set of voxels of each desired tissue type the segmentation task by using an elliptic Fourier de-
in the imagery, for example by using known anatomi- composition of the boundary and placing a Gaussian
cal landmarks. Record the intensity in the MRI data prior on the Fourier coefficients.
at such points. This gives us a set of samples of the
Our approach to object segmentation extends geo-
intensity associated with each tissue type. In princi-
desic active contours [9, 20] by incorporating shape in-
ple, a straightforward application of nearest neighbor
formation into the evolution process. We first compute
labelling can then be used to identify the tissue type
a statistical shape model over a training set of curves.
associated with all other points in the imagery.
To segment a structure from an image, we evolve an
Unfortunately, MRI imagery contains significant active contour both locally, based on image gradients
gain artifacts, which act as a nonlinear multiplier and curvature, and globally to a maximum likelihood
on the observed signal. This causes simple nearest- estimate of shape and pose.
neighbor schemes to fail. As a consequence, our group
has developed [17, 18, 31] methods that automatically
8 Probability Distribution on Shapes
account for the gain artifact, while correctly identifying
the tissue type, using the Expectation/Maximization
To incorporate shape information into the process of
algorithm to iteratively solve for the gain field and the
segmenting an object in an image, we consider a prob-
tissue labeling. The method proceeds by finding the
abilistic approach, and compute a prior on shape vari-
best labeling of the voxels, given the current estimate ation given a set of training instances. To build the
of the gain field, then uses those labels to find the gain shape model, we choose a representation of curves, and
field estimate that maximizes the expected value of the then define a probability density function over the pa-
field. This cycle iterates to convergence and provides rameters of the representation.
accurate estimates of both the gain field and the tissue
labelings. 8.1 Curve Representation
The basic E/M method assumes a stationary prior
on the intensities associated with different tissue types Each curve in the training dataset is embedded as the
(which can be found by measuring relative volumes zero level set of a higher dimensional surface, u, whose
of labeled voxels in training images). Better use of height is sampled at regular intervals (say N¢ samples,
anatomical knowledge can be made, however. In par- where d is the number of dimensions). The embedding
ticular, one can use Markov Random Field methods to function chosen is the signed distance function [26],
impose local continuity on the labeling, and one can use where each sample encodes the distance to the near-
rough knowledge of the distribution of tissue types as est point on the curve, with negative values inside the
a function of distance from known landmarks to create curve (Figure 3). Each such surface (distance map)
non-stationary prior probabilities [17, 18]. Such meth- can be considered a point in a high dimensional space
ods essentially incorporate coarse atlas information to (u € RN . The training set, 7, consists of a set of
guide the segmentation process. surfaces JT= {t,U2,...,Un}. Our goal is to build a
shape model over this distribution of surfaces.

7 Using Anatomical Knowledge

Wl
Because segmentation often requires distinguishing
ambiguous information, having prior information
about the expected shape of a structure can signifi- Cap Ti =)

cantly aid in the segmentation process. For example, Sai

one can find corresponding points across training im-
ages, and use that to construct a statistical model of
shape variation from the point positions. In this case,
the best match of the model to the image is found Figure 3: Level sets of an embedding function u for a
by searching over the model parameters [7]. A sec- closed curve C in R?.
146 E. Grimson et al.

+20

Mode 1

Mode 2

Figure 4: Outlines of the corpus callosum of 6 out of 51

patients in the training set embedded as the zero level set of
a higher dimensional surface, the signed distance function.

Figure 5: The three primary modes of variance of the cor-

The mean surface, p, is computed by taking the
pus callosum training dataset.
mean of the signed distance functions, = ay ty,
The variance in shape is computed using Principal
Component Analysis (PCA). The mean shape, J, is Under the assumption of a Gaussian distribution of
subtracted from each u; to create an mean-offset map, shape represented by a, we can compute the probabil-
u;. Each such map, t;, is placed as a column vector in ity of a certain curve as:
an N¢xn-dimensional matrix M. Using Singular Value
Decomposition (SVD), the covariance matrix MM" is
decomposed as:
P(a) xi
= ty
beet
Gnwe Ea e sane:p(
Ralele5% LEee a) (4)
USUT = MM™ (1)
where U is a matrix whose column vectors represent where }, contains the first k rows and columns of ».
the set of orthogonal modes of shape variation and Figure 4 shows a few of the 51 training curves used
is a diagonal matrix of corresponding singular values. to define the shape models of the corpus callosum. The
An estimate of a novel shape, u, of the same class of original segmentations of the images are shown as red
object can be represented by k principal components curves. The curves are overlaid on the signed-distance
in a k-dimensional vector of coefficients, a. map. Before computing and combining the distance
maps of these training shapes, the curves were aligned
a =U, (uy) (2) using centroids and second moments to approximate
where U;, is a matrix consisting of the first k columns of the correspondence. Figure 5 illustrates zero level sets
U that is used to project a surface into the eigen-space. corresponding to the means and three primary modes
Given the coefficients a, an estimate of the shape u, of variance of the shape distribution of the corpus cal-
namely u, is reconstructed from U, and p. losum. Figure 6 shows the zero level set (as a triangle
surface model) of seven rigidly aligned vertebrae of one
u=U,a+ p (3) patient used as training data. The zero level sets of the
two primary modes are shown as well. Note that for
Note that in general wu will not be a true distance func- both the corpus and the vertebrae, the mean shapes
tion, since convex linear combinations of distance maps and primary modes appear to be reasonable represen-
do not produce distance maps. However, the surfaces tative shapes of the classes of objects being learned. In
generally still have advantageous properties of smooth- the case of the corpus callosum, the first mode seems
ness, local dependence, and zero level sets consistent to capture size, while the second mode roughly cap-
with the combination of original curves. tures the degree of curvature of the corpus. The third
Image Guided Surgery
147

Figure 6: Top: Three-dimensional models of seven thoracic vertebrae (T3-T9) used as training data. Bottom left and
right: Extracted zero level set of first and second largest mode of variation respectively.

mode appears to represent the shifting of the bulk of 9 Shape Priors and Geodesic Active
the corpus from front to back. Contours

8.2 The Correspondence Problem Given a curve representation (the k-dimensional vec-
tor a) and a probability distribution on a, the prior
When measuring shape variance of a certain part of an shape information can be folded into the segmentation
object across a population, it is important to compare process. This section describes adding a term to the
like parts of the object. For example, when looking at level set evolution equation to pull the surface in the di-
variances in the shape of the vertebrae, if two training rection of the maximum likelihood shape and position
examples are misaligned and a process of one is overlap- of the final segmentation.
ping a notch of the other, then the model will not be
9.1 Geodesic Active Contours for Object Seg-
capturing the appropriate anatomical shape variance
mentation
seen across vertebrae.
The snake methodology defines an energy function
One solution to the correspondence problem is to ex-
E(C) over a curve C as the sum of an internal and
plicitly generate all point-wise correspondences to en-
external energy of the curve, and evolves the curve to
sure that comparisons are done consistently, although
minimize the energy [19].
this is dificult to automate and is manually tedious
(see [7]). Another approach to correspondence is to
roughly align the training data before performing the
E(C) =6 fiIc'(lPdq — /IVz(C(q))ldq. 6)
comparison and variance calculation. A rough align- In [9], Caselles, et al. derive the equivalence of geodesic
ment will not match every part of each training in- active contours to the traditional energy-based active
stance perfectly, so one must consider the robustness contours (snakes) framework by first reducing the min-
of the representation to misalignment. imization problem to the following form:
Using the signed distance map as the representation
of shape provides tolerance to slight misalignment of minfig(IVz(C(a))|) {C’(a)| aq (6)
object features, in the attempt to avoid having to solve
the general correspondence problem. In the examples where g is a function of the image gradient (usually of
the form TavTe): Using Euler-Lagrange, the following
presented here, the rough rigid alignment of the train-
ing instances resulted in the model capturing the shape curve evolution equation is derived [9]:
variances inherent in the population due to the depen- OC (t)
Tart = gkN — (Vg: N)N (7)
dence of nearby pixels in the shape representation.
148 E. Grimson et al.

menting the object. The term VJ is the gradient of

the image containing the object to be segmented. By
our definition of shape and pose, the final segmentation
curve is completely determined by a and p. Let wu‘ be
the estimated final curve, which can be computed from
a and p. Therefore, we also have:
Un, = argmax P(u"| u, VI) (10)
us

To compute the maximum likelihood final curve, we

expand the terms from Equation 9 using Bayes’ Rule.

P(u, VI |a,p)P(o,p)
Figure 7: Three steps in the evolution process. The evolv-
P(a,p |u, V1) i ae (11)
|

ing curve is shown in solid blue superimposed on the image P(u|a,p)P(VI |a,p, u)P(a)P(p)
(top row). The curve is matched to the expected curve to ob- P(u, VJ)
tain a PDF over pose (bottom row). The next evolution step
We proceed by defining each term of Equation 11 in
(based on pose and shape) is shown as the dotted blue line.
turn. We discard the normalization term in the denom-
inator as it does not depend on shape or pose. More
where « is the curvature and AN is the unit normal. By details are available in [21].
defining an embedding function wu of the curve C, the The first term in Equation 11 computes the probabil-
update equation for a higher dimensional surface is [9]: ity of a certain evolving curve, u, given the shape and
pose of the final curve, u* (or (a,p)). Notice that this
St = 9 (etn) |Vul + Vu- Vag (8) term does not include any image information whatso-
ever. We model this term as a Laplacian density func-
where c is an image-dependent balloon force added to
tion over V,tsider the volume of the curve wu that lies
force the contour to flow outward [8, 22]. In this level
set framework, the surface, u, evolves at every point
outside the curve wv.
perpendicular to the level sets as a function of the cur-
P(u | a, p) = exp (—Voutside) (12)
vature at that point and the image gradient.
This term assumes that any curve u lying inside u* is
9.2 Estimation of Pose and Shape equally likely. Since the initial curve can be located at
any point inside the object and the curve can evolve
In addition to evolving the level set based on the cur-
along any path, we do not favor any such curve.
vature and the image term, we include a term that
incorporates information about the shape of the object The second term in Equation 11 computes the prob-
being segmented. To add such a global shape force ability of seeing certain image gradients given the cur-
to the evolution, the pose of the evolving curve with rent and final curves. Let h(u*) be the best fit Gaussian
respect to the shape model must be known (see Fig- to the samples (u*, |VJ|). We model the gradient prob-
ure 7). Without an estimate of the pose, the shape ability term as a Laplacian of the goodness of fit of the
model cannot adequately constrain or direct the evolu- Gaussian.
tion. Therefore, at each step of the curve evolution, we P(VI |uu) = exp (—| h(u') — |VJ| |?) (13)
seek to estimate the shape parameters, a, and the rigid
pose parameters, p, of the final curve using a maximum The last two terms in Equation 11 are based on our
likelihood approach. prior models, as described in Section 8. Our shape
(Qn,
>Pu) = argmax P(a,p |u, VI) (9) prior is a Gaussian model over the shape parameters,
ap a, with shape variance Dx.
In this equation, u is the evolving surface at some point
in time, whose zero level set is the curve that is seg- Pla)= espe
af nb dblagls ex (—Saete)
ulEe Ste ad
Image Guided Surgery
149
In our current framework, we seek to segment one ob- timate. Combining these equations yields the final ex-
ject from an image, and do not retain prior information pression for computing the surface at the next step.
on. the likelihood of the object appearing in a certain
location. Thus, we simply assume a uniform distribu- u(t +1) =u(t)-+A, (9(c+) [Vu(t)| + Vu(t)- Vs)
tion over pose parameters, which can include any type
of transformation, depending on application. 295 (we) a2u(t)) (18)
The two parameters 1 and \» are used to balance
P(p) = U(—co,
ov) (15) the influence of the shape model and the gradient-
curvature model. The parameters also determine the
These terms define the maximum likelihood estima-
overall step size of the evolution. The tradeoff between
tor of shape and pose, which estimates the final curve
shape and image depends on how much faith one has
or segmentation of the object. For efficiency, these
in the shape model and the imagery for a given appli-
quantities are computed only in a narrow band around
cation. Currently, we set these parameters empirically
the zero level set of the evolving surface, and the ML
for a particular segmentation task, given the general
pose and shape are re-estimated at each evolution step
image quality and shape properties.
using simple gradient ascent on the log likelihood func-
tion in Equation 11. While each ascent may yield a
10 Results
local maximum, the continuous re-estimation of these
parameters as the surface evolves generally results in We have tested the segmentation algorithm on syn-
convergence on the desired maximum. Next, we in- thetic and real shapes, both in 2D and in 3D. For
corporate this information into the update equation controlled testing, a training set of rhombi of various
commonly used in level set segmentation. sizes and aspect ratios was generated to define a shape
model. Test images were constructed by embedding
9.3 Evolving the Surface the shapes of two random rhombi with the addition of
Gaussian speckle noise and a low frequency diagonal
Initially, the surface, u, is assumed to be defined by bias field.
at least one point that lies inside the object to be seg-
Segmentation experiments were performed on 2D
mented. Given the surface at time t, we seek to com-
slices of MR images of the corpus callosum (Figure
pute an evolution step that brings the curve closer to
8). The corpus callosum training set consisted of 49
the correct final segmentation based on local gradient
examples like those in Figure 4. The segmentations of
and global shape information.
two corpora callosa are shown in Figure 8. Notice that
The level set update expression shown in Equation while the ML shape estimator is initially incorrect, as
8 provides a means of evolving the surface u over time the curve evolves, the pose and shape parameters con-
towards the solution to the original curve-minimization verge on the boundary. The segmentations of the femur
problem stated in Equation 6. Therefore, the shape of slices and the corpora all converged in under a minute
the surface at time t+1 can be computed from u(t) by: on a 550 MHz Pentium III.
The vertebrae example illustrates the extension of
ult +1) = u(t) +1 (e(c+ x) |Vu(t)| + Vu(t) - Vs) (16) the algorithm to 3D datasets. Figure 9 illustrates a few
steps in the segmentation of vertebra T7. The training
where A; is a parameter defining the update step size. set in this case consisted of vertebrae T3-T9, with the
exception of T7. The initial surface was a small sphere
By estimating the final surface u* at a given time
placed in the body of the vertebra. The red contour is
t, (Section 9.2), we can also evolve the surface in the
a slice through the zero level set of the evolving hyper-
direction of the maximum likelihood final surface:
surface. The yellow overlay is the ML pose and shape
u(t +1) = ult) +2 (a(t) - u(t)) (17) estimate. Segmenting the vertebra took approximately
six minutes.

where A2 € [0,1] is the linear coefficient that deter- To validate the segmentation results, we compute
mines how much to trust the maximum likelihood es- the undirected partial Hausdorff distance [15] between
150 E. Grimson et al.

Figure 8: Four steps in the segmentation of two different corpora callosa. The last image in each case shows the final
segmentation in red. The cyan contour is the result of the standard evolution without the shape influence.

Lo OC 4

Figure 9: Karly, middle, and final steps in the segmentation of the vertebra T7. Three orthogonal slices and the 3D
reconstruction are shown for each step. The red contour is a slice through the evolving surface. The yellow overlay is a slice
through the inside of the ML final surface.

the boundary of the computed segmentation and the Corpus 1 Corpus 2. Vertebra
boundary of the manually-segmented ground truth. 1.3 mm 1.5 mm 2.7 mm
The directed partial Hausdorff distance over two point 1.6 mm 2.0 mm 4.4mm
sets A and B is defined as
Table 1: Partial Hausdorff distance between our segmen-
tation and the manually-segmented ground truth.
Srathn x
hx (A,B)
= K" min ||a~— 6]|
11 Evolution of Surfaces

where K is a quantile of the maximum distance. The The previous two methods for segmentation are de-
undirected partial Hausdorff distance is defined as signed to handle a wide range of structures, but work
Hx(A,B) = max(hx(A, B),hx(B,A)). The results best for compact connected anatomical structures.
for the corpora and vertebra shown in Table 1 indicate Segmenting out tubelike structures, such as vessels of-
that virtually all the boundary points lie within one or ten requires more specialized methods, and in this sec-
two voxels of the manual segmentation. tion, we outline our approach to such problems.
Image Guided Surgery
151
Curve-shortening flow is the evolution of a curve over
time to minimize some distance metric. When this initia

distance metric is based on image properties, it can

be used for segmentation. The idea of geodesic ac-
tive contours is to define the metric so that indicators
of the object boundary, such as large intensity gradi-
ents, have a very small “distance” [4, 20, 2]. The min-
imization will attract the curve to such image areas,
thereby segmenting the image, while preserving prop-
erties of the curve such as smoothness and connectiv-
ity. Geodesic active contours can also be viewed as a Figure 10: Simple segmentation example: (a) Evolving
more mathematically sophisticated variant of classical curve. (b) Level set implementation of curve evolution.
snakes [19]. Further, they are implemented with level
set methods [26] which are based on recent results in To minimize this objective function by steepest de-
differential geometry [11, 5]. The method can be ex- scent, consider C’ to be a function of time t as well
tended to evolve surfaces in 3D, for the segmentation as spatial parameter p. The Euler-Lagrange equations
of 3D imagery such as medical datasets [4, 32]. yield the curve evolution equation:
A limitation of the method was that it and its theo- =
retical foundations applied only to hypersurfaces. More C, = 9kN —(Vg-N)N (19)
recent work in differential geometry, however, devel-
oped the equations necessary to evolve arbitrary di- where « is the Euclidean curvature and N is the unit
mensional manifolds in arbitrary dimensional space [1]. inward normal. In the absence of image gradients, this
Subsequent work developed and analyzed a diffusion- equation causes the curve to shrink according to its
generated motion scheme for co-dimension two curves curvature; the presence of image gradients causes the
[25]. We have developed the first application of geo- curve to stop on the object boundary (Figure 10a).
desic active contours in 3D, based on [1]. Our system,
CURVES, evolves 1D curves in 3D images and has been 12.2 Level Set Method for Hypersurfaces
applied to automatic segmentation of blood vessels in
medical images [23]. Level set methods increase the dimensionality of the
problem from the dimensionality of the evolving mani-
12 Background fold to the dimensionality of the embedding space [26].
For the example of planar curves, instead of evolving
Our approach is an extension of geodesic active con- the one-dimensional curve, the method evolves a two-
tours research, also using a level set implementation. dimensional surface. Let u : R? — R be the signed
The extension was enabled by theoretical work on level distance function to curve C' as in Figure 10b; C is
set methods for arbitrary co-dimensional manifolds. thus the zero level-set of u, and u is an implicit repre-
sentation of C’. Let Co be the initial curve. It is shown
12.1 Geodesic Active Contours in [11, 5] that evolving C according to:
The task of finding the curve that best fits the object
C, = BN
boundary is posed [4, 2, 20] as a minimization problem
over all closed planar curves C(p) : [0,1] > R?. The with initial condition C(-,0) = Co(-) for any function
objective function is: B, is equivalent to evolving wu according to:

f g(IVI(C(p))|)|C"(@) lap ut = B|Vul

where I : [0,a] x [0,6] — [0,00) is the image and g : with initial condition u(-,0) = uo(-) and uo(Co) = 0
[0, 00) + Rt is a strictly decreasing function such that in the sense that the zero level set of wu is iden-
g(r) + 0 as r — ov, such as g(|VI|) = Tv: tical to the evolving curve for all time. Choosing
152 E. Grimson et al.

B=gk—(Vg- N ) as in Equation 19 gives the behav-

ior illustrated in Figure 10b according to the update
equation:
Ut = gk|Vul + Vg- Vu.

The extension to surfaces in 3D is straightforward

and is called minimal surfaces [4]. The advantages of
the level set representation are that it is intrinsic (in-
dependent of parameterization) and that it is topolog-
ically flexible since different topologies of C' are repre-
sented by the constant topology of wu. Figure 11: Co-dimension two curve: (a) Tubular isolevel
set. of C. (b) The tangent to C at p, the normal plane,
12.3. Level Set Method for Curves in Higher the external vector d, and its projection onto the normal
Co-dimension plane.

For the task of evolving one-dimensional curves in

That is, this evolution is equivalent to evolving C ac-
three-dimensional space, however, the above level set
cording to C, = KN in the sense that C’ is the zero
relation does not hold. It is applicable only to hyper-
level set of v throughout the evolution. For intuition,
surfaces, that is, surfaces whose co-dimension is one:
let C be some curve in R? and v the distance function
the co-dimension of a manifold is the difference be-
to C. Consider then an isolevel set [, = {z|v(x) = e}
tween the dimension of the evolving space and the di-
of v where € is small and positive, so Tz is a thin tube
mension of the manifold. The examples of a planar
around C (Figure lla). The nonzero eigenvalues of
curve and a 3D surface have co-dimension one, but 1D
Py,V?uPyvy are equal to the principal curvatures of
curves in three dimensions have co-dimension two. In-
this tube. The larger principal curvature depends on €
tuition for why the level set method above no longer
while the smaller is related to the geometry of C. It is
holds is that there is not an “inside” and an “outside”
according to C that we want the evolution to proceed;
to a manifold with co-dimension larger than one, so
thus, the smaller principal curvature is chosen.
one cannot create the embedding surface u in the same
fashion as for planar curves; a distance function must Now assume there is an underlying vector field
be everywhere positive, and is thus singular on the driving the evolution, so the desired evolution equa-
curve itself. Recently, however, more general level set tion is: ,
equations were found for curvature-based evolution [1]. Ct = KN = ld,

It is these equations that motivated the development where II is the projection operator onto the normal
of CURVES, which uses image information to create space of C’ (a vector space of dimension 2) and d is
the auxiliary vector field. a given vector field in R°, (Figure 11). The evolution
Let v : R® — [0,00) be an auxiliary function whose equation for the embedding space becomes [1]:
zero level set is identically C(p) : [0,1] > R°, that is
ve = A(Vv, Vv) + Vo-d.
smooth near C’, and such that Vv is non-zero outside
C. For a nonzero vector q € R”, define:

Sm
13. CURVES
Py =I
lah The evolution equation we use follows directly from an
energy-minimization problem statement.
where J is the identity matrix as the projec-
tor onto the plane normal to q. Further define 13.1 Evolution Equation
A(Vv(z, t), V7u(a, t)) as the smaller nonzero eigenvalue
For 1D structures in 3D images, we wish to minimize:
of Py, V7uPyy. The level set evolution equation is [1]:

Ut = A(Vo(a,t), V7v(z, t)). f a(IVI(C(p)))IC"(p) lap

Image Guided Surgery
153
where C(p) : [0,1] > R® is the 1D curve, I : (0,a] x
[0,6]x [0, c] + [0, 00) is the image, and g : [0,
co) — Rt
is a strictly decreasing function such that g(r) + 0 as
r — oo (analogous to [4]). For our current implemen-
tation, we use g(r) = exp(—r) because it works well in
practice. The Euler-Lagrange equations give:

> = g’ VI
ae
C, = KN — —II(H—),
gal Figure12: To evolve a point on the distance function,
CURVES chooses image information from A instead of B.
where H is the Hessian of the intensity function. The
auxiliary vector field in the above equation is thus:
dot product of the respective gradients of v and I, so
= OR the update equation becomes:
d= —H-—_.,
g |VI|
ff

ve = A(Vv, V2v) + p(Vu- vi)2 Vv- was

so the equation for the embedding space is:
‘go wg eT so
A third aspect of our system is that we do not

t= NVo(a,2),
vy, = A(Vv(z, V0(2,0)+FV0(@,1) HE
t), V20(2,t
é
Lae
VI
z,t)-H——
propagate the image information off the current object
boundary to obtain the values near the boundary, as is
customary in level set methods [4, 2, 20, 12]. Instead,
13.2 Features we use directly the image information at each point in
the image (Figure 12). Our choice has the advantage
Initial experiments required that the evolving volume of enabling attraction to image gradients not on the
be a distance function to the underlying curve; how- current boundary, thereby reducing sensitivity to ini-
ever, it was not clear how to robustly extract the zero tialization, at the expense of requiring more frequent
level set or even evolve those points since the distance reinitializations of the distance function and losing the
function was singular exactly there. For this reason, we equivalence to the Lagrangian method (updating C di-
developed the ¢-Level Set Method which defines a thin rectly instead of v) off the boundary.
tube of radius ¢ around the initial curve, then evolves In short, the CURVES system takes in a 3D im-
that tube instead of the curve. € does not denote a fixed age that contain thin curvilinear structures, such as an
value here, but means only that the evolving shape is MRA image. An initial segmentation estimate is gener-
a “tubular” surface of some unspecified and variable ated by thresholding the image. That estimate is used
nonzero width. Thus, we are now evolving surfaces to generate an initial distance function, which is itera-
similar to [4], but that follow the motion of the under- tively updated according to Equation 20. Convergence
lying curve so they do not regularize against the high is detected automatically when volumetric change in
curvatures found in thin cylindrical structures such as the segmentation is very small over some number of
blood vessels and bronchi. In addition to being more iterations. Further detail was published in [23].
robust, this method better captures the geometry of
such structures, which have nonzero diameter. 14 Results
Next, we modified the update equation for the MRA
segmentation application. To control the trade-off be- We have run CURVES on over 20 medical datasets, pri-
tween fitting the surface to the image data and enforc- marily phase contrast magnetic resonance angiography
(PC-MRA), of various resolutions and scanner types.
ing the smoothness constraint on the surface, we incor-
We provide images of representative segmentations.
porate an image weighting term p which is set by the
user or is pre-set to a default value. Second, because We first show successive boundary estimates in a seg-
vessels in MRA and bronchi in CT appear brighter than mentation of a cerebral MRA image to demonstrate
the background, we weight the image term by the co- the behavior of the algorithm over time, until conver-
sine of the angle between the normal to the surface and gence is reached. We then show CURVES segmenta-
the gradient in the image. This cosine is given by the tions of more cerebral MRA images compared to those
 che
154 E. Grimson et al.

all regions of image “noise” which adjoin vasculature

are incorrectly labeled as vessel and small thin vessels
which may appear broken or disconnected from larger
structures will often be omitted. Thus, our goal is to
reduce user interaction while increasing the ability to
segment thin vessels.
Figure 14 shows CURVES segmentations (red) com-
Figure 13: Surface evolution over time: initialization, fol-
pared to segmentations acquired using the manual pro-
lowed by successive boundary estimates.
cedure just described (blue). The dataset shown here
is PC-MRA acquired on a 1.5T scanner with voxel res-
olution of 1.171875 x 1.171875 x 0.8mm? and a size of
obtained with a manual segmentation technique used 256 x 256 x 84 voxels. The raw dataset is shown in
clinically at our institution. Finally, we illustrate the maximum intensity projection from three orthogonal
advantage of our system compared to co-dimension one viewpoints, as are the two segmentations. Notice that
surface evolution with an experiment involving the seg- CURVES is able to capture more of the thin vessels
mentation of bronchi in a computed tomography (CT) than the manual procedure which is based on simple
image of lung. The co-dimension one result was ob- thresholding.
tained using the mean curvature of the surface as the
regularization force, as in previous level set segmenta-
tion schemes [4]; otherwise, all parameter settings were Acknowledgments
identical to those used in the CURVES experiment.
Support for our research was provided in part by the
14.1 Example Evolution NSF under grant no. IIS-9610249, and in part by NSF
ERC (Johns Hopkins University agreement) 8810-274.
Figure 13 illustrates the behavior of our system over
time on a phase contrast MRA image of cerebral ves-
sels. The initial surface is obtained by thresholding References
the raw dataset, then CURVES evolution produces the
[1] Ambrosio, L. and Soner, H.M.: Level set approach to
subsequent images.
mean curvature flow in arbitrary codimension. Journal
of Differential Geometry 43 (1996) 693-737
14.2 Segmentations of Cerebral Vasculature
[2] Caselles, V., Catte, F., Coll, T. and Dibos, F.: A geo-
One specific practical motivation for our work is the metric model for active contours. Numerische Mathe-
use of surface models of cerebral vasculature as an aid matik 66 (1993) 1-31
in neurosurgical planning and procedure, especially in
the context of the image-guided surgery program at [3] Caselles, V., Morel, J.M., Sapiro, G. and Tannen-
our institution [13]. Currently the vessel models are baum, A.: Introduction to the special issue on partial
differential equations and geometry-driven diffusion in
obtained manually as follows. A neurosurgeon interac-
image processing and analysis. IEEE Transactions on
tively chooses a threshold that is used to binarize the Image Processing 7(3) (1998) 269-273
MRA dataset: all voxels brighter than that threshold
are labeled as vessel, while all others are discarded. [4pear Caselles, V., Kimmel, R. and Sapiro, G.: Geodesic ac-
A “connectivity” program then partitions the set of tive contours. Int’! Journal of Computer Vision 22(1)
(1997) 61-79
labeled voxels into connected components. Each con-
nected component appears in a distinct color on the [5] Chen, Y.G., Giga, Y. and Goto, S.: Uniqueness
user interface. The surgeon looks at individual slices and existence of viscosity solutions of generalized
and clicks on colored regions that correspond to vascu- mean curvature flow equations. Journal of Differential
lature. All connected components so chosen are stored Geometry 33 (1991) 749-786
as the final manual segmentation. The first drawback
[6] 'T. Cootes, C. Taylor, D. Cooper, and J. Graham.
of this method is the expert user-interaction required, Active shape models - their training and application.
the second is that the thresholding step implies that Computer Vision and Image Understanding 1995.
Image Guided Surgery
155

Figure 14: The same MRA dataset is shown from three orthogonal viewpoints. For each viewpoint, the maximum intensity
projection of the raw data is shown, followed by the CURVES segmentation (red), the manual segmentation (blue), and a
combination image showing the differences between the segmentations.

[7] T. Cootes, C. Beeston, G. Edwards, and C. Taylor [14] W.E.L. Grimson, T. Lozano-Pérez, W.M. Wells III,
Unified Framework for Atlas Matching Using Active G.J. Ettinger, S.J. White and R. Kikinis, An Auto-
Appearance Models. Information Processing in Med- matic Registration Method for Frameless Stereotaxy,
ical Imaging 1999. Image Guided Surgery, and Enhanced Reality Visu-
alization, IEEE Trans. Medical Imaging, 15(2), April
[8Dstt
L. Cohen On active contour models and _ balloons. 1996,
pp. 129-140.
CVGIP: Image Understanding, 53(2):211—218, 1991.
[15] D. Huttenlocher, G. Klanderman, W. Rucklidge,
[9] V. Caselles, R. Kimmel, and G. Sapiro. Geodesic ac- “Comparing images using the Hausdorff distance” in
tive contours. Int’l Journal of Computer Vision 22(1) IEEE Trans PAMTI, 15:850-863, 1993.
(1997) 61-79

[10] G. Christensen, R. Rabbitt, and M. Miller. De- [16] M. Jones and T. Poggio. Multidimensional Morphable
Models. Int’l Conf. Computer Vision, 1998.
formable templates using large deformation kinemat-
ics. IEEE Trans. on Image Processing 5(10) (1996)
1435-1447.
[17] T. Kapur, E. Grimson, R. Kikinis, W. Wells, “En-
hanced spatial priors for segmentation of magnetic
resonance imagery”, Medical Image Computation and
[11] Evans, L.C. and Spruck, J.: Motion of level sets by
Computer Assisted Interventions, Boston, October
mean curvature: I. Journal of Differential Geometry
1998.
33 (1991) 635-681

[12] Gomes, J., and Faugeras, O.: Reconciling distance [18] T. Kapur, E. Grimson, W. Wells, R. Kikinis, “Seg-
functions and level sets. mentation of Brain Tissue from Magnetic Resonance
Images”, Medical Image Analysis, Vol. 1, Issue 2, 1997,
[13] Grimson, W.E.L., Ettinger, G.J., Kapur, T., Leven- pp 109-128.
ton, M.E., Wells III, W.M., and Kikinis, R.: Utilizing
Segmented MRI Data in Image-Guided Surgery. Int’! [19] Kass, M., Witkin, A., and Terzopoulos, D.: Snakes:
Journal Pattern Recognition and Artificial Intelligence active contour models. Int’! Journal Computer Vision
(1996) 1(4) (1988) 321-331
 E. Grimson et al.

[20] Kichenassamy, A., Kumar, A., Olver, P., Tannen- aments. UCLA Computational and Applied Mathe-
baum, A. and Yezzi, A.: Gradient flows and geometric matics Report 98-47 (1998)
active contour models. In Proc. IEEE Int’! Conf. Com-
puter Vision (1995) 810-815 J. Sethian. Level Set Methods. Cambridge University
Press (1996)
M. Leventon, E. Grimson, O. Faugeras. Statistical
Shape Influence in Geodesic Active Contours. CVPR L. Staib and J. Duncan. Boundary Finding with Para-
2000 metrically Deformable Models PAMI 14(11), (1992),
1061-1075.
L. Lorigo, O. Faugeras, W.E.L. Grimson, R. Keriven,
R. Kikinis. Segmentation of Bone in Clinical Knee S. Ullman and R. Basri. Recognition by linear combi-
MRI Using Texture-Based Geodesic Active Contours. nations of models. CVPR 1991
In MICCAT, 1998.
P. Viola, W. Wells. IJCV 1997
Lorigo, L. M., Faugeras, O., Grimson, W.E.L.,
Y. Wang and L. Staib. Boundary Finding with Corre-
Keriven, R., Kikinis, R., and Westin, C.-F.: Co-
dimension 2 geodesic active contours for MRA seg- spondence Using Statistical Shape Models CVPR 1998
mentation. Proc. Int’! Conf. Information Procession W.M. Wells, III, W.E.L. Grimson, R. Kikinis, and
Medical Imaging. Lect. Notes Comp. Sci. 1613 (1999) F.A. Jolesz, “Adaptive Segmentation of MRI data”,
126-139 IEEE Trans. Medical Imaging, Vol. 15, No. 4, pp. 429—
A. Pentland and S. Sclaroff. Closed-form solution 442, August 1996.
for physically based shape modeling and recognition.
Zeng, X., Staib, L.H., Schultz, R.T., and Duncan, J.S.:
PAMI 13(7) (1991) 715-729.
Segmentation and measurement of the cortex from 3D
Ruuth, S.J., Merriman, B, Xin, J., and Osher, S.: MR images. Proc. Int’! Conf. Medical Image Com-
el
Diffusion-generated motion by mean curvature for fil- puting Computer-Assisted Intervention. Lect. Notes
Comp. Sci. 1496 (1998) 519-530
Pulling Motion Based Tactile Sensing
Makoto Kaneko, Hiroshima University, Higashi-Hiroshima, Japan
Toshio Tsuji, Hiroshima University, Aigashi-Hiroshima, Japan

An algorithm for detecting the shape of a 2D concave

surface by utilizing a tactile probe is discussed. Pulling
a tactile probe whose tip lies on an object’s surface can
be easily achieved, while pushing it is more difficult due
to stick-slip or blocking up with irregular surface. To
cope with the difficulty of pushing motion on a fric-
tional surface, the proposed sensing algorithm makes
use of the pulling motion of tactile probe from a local
concave point to an outer direction. The algorithm is
Figure 1: Pulling and pushing motions by human hand.
composed of three phases, local concave point search,
tracing motion planning, and infinite loop escape. The
proposed algorithm runs until the tactile probe detects occur for outer and inner directions, respectively, as
every surface which it can reach and touch. We show shown in Figure 2(a). If we can choose the local con-
computer simulations obtained using the proposed algo- cave point as a starting point, however, we can expect
rithm. pulling motions to occur for both directions, as shown
in Figure 2(b).
1 Introduction
In order to utilize this advantage, the algorithm first
This paper focuses on the shape detection of a 2D sur- searches for the local concave point by applying a bi-
face by utilizing a tactile probe which can detect any section method (local concave point search). The tac-
contact point between itself and the environment. One tile probe is then pulled outwards from the local con-
emphasis of our research is to study how the probe cave point while keeping the tip of the probe in contact
motion should be planned for the object including con- with the environment (tracing motion planning). Dur-
cave surface. Suppose that the human moves his or ing the tracing motion, however, the tip of the probe
her fingertip on the table as shown in Figure 1, where may break contact and touch again at a new contact
(a) and (b) denote the pushing and the pulling modes, point due to the existence of another local concave area,
respectively. It should be noted that pushing motion is or the contact point may jump from the tip to an arbi-
not achieved smoothly due to the well-known stick-slip trary point of the probe due to the existence of a local
phenomenon and often blocked by a small irregularity
on the surface, while pulling motion can be achieved
even under significant friction. Considering this fact, Pull
we discuss the pulling motion-based algorithm where Push
no pushing motion occurs. If the environment’s sur-
face is unknown, however, whether the resulting mo-
tion evolves as pulling or pushing strongly depends on
the surface geometry and on the direction of the mo-
tion imparted to the tactile probe. In case that the
sensing motion starts from an arbitrary point on the (a) (b)
surface, pulling and pushing motions are expected to Figure 2: Pulling and pushing motions by a sensor probe.
158 M. Kaneko and T. Tsuji

convex area. When the tactile probe recognizes such objects. Chen, Rink and Zhang [18] introduced an
a particular situation, the sensing system stores each active tactile sensing strategy to obtain local object
contact point before and after jump. For such a non- shape, in which they showed how to find the contact
traced area, the sensing motion is repeated recursively. frame and how to find the local surface parameters in
There might be a failure mode in which the sensing mo- the contact frame. This work might be applied to a
tion results in repeating mode without finding any new concave surface that can be described by the second
contact point. To emerge from such an infinite loop, we order polynomial equation, although their experiment
prepare infinite loop escape by which the tactile probe is limited to a convex object only. In these works, they
can always find a new contact point if it exists. We picked up extremely simple concave objects as test ex-
show that the proposed algorithm can continue to run amples but included no precise discussion on the inher-
until the tactile probe detects every surface that it can ent sensing algorithm for concave objects. This paper
reach and touch. Also, we show computer simulations is an extended version of our former work [19].
obtained using the proposed algorithm.
3 Problem Formulation
2 Related Work
3.1 Preliminary Definitions
There are a number of papers discussing tactile and
Let P(s) (or simply P) be a point on the envi-
haptic perception linked with multi-fingered dexterous
ronment’s surface, where s is the coordinate along
hands [1]-[10]. Caselli et al. [11] proposed an efficient the surface as shown in Figure 3(a). We define
technique for recognizing convex object from tactile
Dist(P(s1), P(s2)), fa and fk, as the distance, the
sensing. ‘They developed internal and external vol-
environment’s contour, and the line segment between
umetric approximation of the unknown objects and
P(s,) and P(s2), respectively. When Ce becomes
exploited an effective feature selection strategy along
with early pruning of incompatible objects to improve known by a probe tracing, we define ane € W,, where
recognition performance. On the other hand, algo- W, is the assembly of the area traced by the probe.
rithms for tactile sensing have also been reported. Gas- If Dist(P(s;), P(s2)) < € exists for a V small ¢ > 0,
ton and Lozano-Perez [12] and Grimson and Lozano- we can regard L,’ as an approximate surface of byFef
Perez [13] discussed object recognition and localization and define C € W2, where W2 is the assembly of
through tactile information under the assumption that approximately detected area by straight-line approxi-
the robot possesses the object models. Cole and Yap mation. When the tactile probe recognizes the partic-
[14] have addressed the “Shape from probing” problem, ular area between P(s;) and P(s2) where it can not
where they discussed how many probes are necessary reach and touch physically, we define the area as the
and sufficient for determining the shape and position non-reachable area and describe by CE2 € W3, where
of a polygon. They showed 3n — 1 probes are neces- Ws3 is the assembly of the wifi pases area verified
sary and 3n are sufficient for any n-gon, where n is the by a probing motion. For every concave object (or en-
number of probes. Most of these works [1]-[9], [11]- vironment), we can make an equivalent convex shape
[14], however, deal with convex objects only and never by connecting common tangential lines. The outside
discuss concave ones.
As far as we know, there are only a few papers Free area: F
[15]-[20] addressing tactile sensing for concave objects.
Russell [15] designed a whisker sensor composed of
Unknown
an insensitive flexible beam, a potentiometer, a re- area
turn spring and counterweight. Assuming that the P(so)

whisker tip always makes contact with an environment,

he showed the output of the sensor when scanning the Xo
concave bowl of a teaspoon. Roberts [17] discussed the (a) (b)
strategy for determining the active sensing motion for
the given set of convex and concave polyhedral model Figure 3: Definition of symbols and problem notation.
Pulling Motion Based Tactile Sensing
159
of the equivalent convex is defined as the free area F
F : Free area
as shown in Figure 3(b) where it is guaranteed that A;B; : Area between A; and B;
there is no object (or environment). We also define G; : Region constructed by two probe poses
Area(G) as the area of G, where G denotes the region when the unknown area A;B; is found
constructed by two probe postures, while the precise Lt : Line segment between A; and B;
definition of G will be given later. VY; : Half plane that we can see in the right hand
side when moving from B; to A;
7; : Semi-infinite region bounded by P;A; and
3.2. Main Assumptions
P;B;, where P; is the joint position

Distributed sensing elements cover the entire tactile

4 Pulling Motion Based Sensing
probe. The probe has negligible thickness and is con-
nected with the end-joint of a robot arm having suffi- 4.1 Outline of the Algorithm
cient degrees of freedom so that the probe can achieve
arbitrary position and pose in a 2D plane. The arm Figure 4 shows an example explaining the sensing al-
is assumed to have a joint torque and a joint position gorithm. The probe is first inserted from an arbitrary
sensors in each joint. Both sensors are indispensable point in the free area F toward the unknown area until
for achieving a compliant motion and determining the the tip makes contact with the environment. By moni-
probe position. The torque sensor is further utilized for toring the torque sensor output, it is checked whether a
confirming whether a clockwise (or counter-clockwise) clockwise (or counter-clockwise) rotation of the probe
rotation of the probe is allowed or not. The probe is is possible or not. After checking such a geometrical
long enough to ensure that the end-joint of the arm condition, the probe is rotated in the direction of rota-
always lies in the free area F, which enables us to ne- tion free until it again makes contact as shown by the
glect any geometrical constraint imposed by the robot dotted line in Figure 4(a), where ¢ is the rotational
arm and to focus on the probe motion. The probe is angle. Choosing the equally divided direction y/2, we
sufficiently stiff to avoid bending. For the sake of sim- again insert the probe until it makes contact with the
plicity, we give the following assumption on the shape environment. By repeating this procedure, the tip can
of environment. Consider a small circle whose center finally reach the local concave point, where the probe
and radius are given by P(s) and ¢/2, respectively, as loses any rotational degree of freedom (local concave
shown in Figure 3(a). We also assume that there al- point search). Then, the probe is moved from the local
ways are only two intersection points between the circle concave point to the outer direction, while maintaining
and the environment for an arbitrary s € [—o0, +00] constant torque control for the last joint, where a clock-
which means that the environment is smooth enough wise torque is applied during the probe motion from Do
in € level and does not include too much irregular sur- to Ag and a counter clockwise torque is imparted dur-
face. This assumption provides a valid reason for ap- ing the probe motion from Do to Bo. Figure 4(b) and
proximating the environment’s contour as the straight- (c) show two examples of tracing motion. By impart-
lined one, namely, C2** = Lt. We also assume that ing the torque depending on the direction of tracing
motion, it is ensured that the probe tip makes contact
there exists only one object (or environment). The
with the environment if the surface is smooth enough
last assumption means that the objects (or environ-
as shown in Figure 4(b) (tracing motion planning).
ment) could be complicated but no island-like object
in addition to the main object. Thus, the tracing motion is executed by a pulling mo-
tion alone. This is the reason why we call the algorithm
the pulling motion based sensing algorithm. If the en-
3.3. Problem Formulation and Nomenclature vironment includes another local concavity as shown in
Figure 4(c), however, the tip will be once away from
Problem formulation : Given lars E€ W and the surface and then make contact with another part of
Ce EW, construct an algorithm such that Gr € the environment due to the constant torque command
W is achieved under the assumption in 3.2, whereW = imparted to the joint. When the probe recognizes such
an additional concave area, we register A; and B, as
160 M. Kaneko and T. Tsuji

Figure 5: Two probe postures when A;B; is detected.

(c) (d)
angles (Figure 5(c) and (d)), and one line segment (Fig-
Figure 4: Outline of the algorithm. ure 6(a)), where the difference between (c) and (d) de-
a new pair indicating the unknown area. The sensing pends upon whether i passes through gl) or gi).
motion is repeated recursively for A,B ,. During both The quadrangle in Figure 5(a) or (b) results in a tri-
local concave point search and tracing motion planning, angle when the line segment A;B; lies on the extended
the sensing motion may result in an infinite loop de- line of one of two probe postures. For a single probe
pending upon the environment’s geometry. In order to detection as shown in Figure 6(a), G; forms either a tri-
emerge from such an infinite loop, we prepare the infi- angle or a line segment (Area(G;) = 0). Area(G;) = 0
nite loop escape, in which the probe is inserted so that it means no more rotational degree of freedom around the
can reach a new contact point between the designated tip. In other words, there exists no other pass except
area. The infinite loop escape is not always necessary the current one resulting in the single probe posture as
but called upon request when the same contact point shown in Figure 6(b). Once Area(G;) = 0 is detected,
is detected repeatedly. the algorithm categorizes the A;B; into never touching
area W3 and leaves from the initial pass planning. In
4.2 Local Concave Point Search this subsection, we temporarily assume Area(G;) 4 0.
4.2.1 Initial Pass Planning

Definition 1 Consider two probe postures when the

unknown area A,B; is found (see Figure 5(a)). G; is
defined as the region constructed by connecting A;, B;
and each joint of the probe. If both A; and B; are de-
tected by one probe posture, G; is generated by rotating
the probe around the tip till it makes contact with the
environment as shown in Figure 6(a).
All possible shapes for G; can be classified into three
groups, one polygon (triangle or quadrangle), two tri- Figure 6: An example of single probe detection.
Pulling Motion Based Tactile Sensing
161
Under such an assumption, there exist only two pat- Theorem 1 A sufficient condition for making the
terns for G; namely, one polygon and two triangles. tip reach Int(A;B;) is to move ie probe along the line
Since G; always links with the free area F under as- passing through one point on iy sGiNV; and one point
sumption in 3.2 (the probe is long enough to ensure
on ie , where if G; OV; is PoriDoend of two triangles,
that the joint never enters within the unknown area),
one point on Le is replaced by the common point for
it is guaranteed that if there is an environment within
both triangles.
G;, its root must be connected with A;B;. If this does
not hold true, the environment in G; must be an island- Proof : ip5/1 Gi expresses the line segment of Le2
like-object. Under the assumption of single object (or within G;. Sanilanlys yA . 1G; V; denotes the line
environment), however, such a situation never appears. segment of L oS within é VY;. Note that there are
In case of Figure 5(c), the probe trajectory should be two possible shapes constructed by G; VV; NF, one is
strictly bounded, because it must be planned to pass convex (Figure 5(a), (b), (d) and Figure 6(b)) and the
through the common point for both triangles to ab- other is concave (Figure 5(c)).
solutely avoid contacting with the environment whose
(i) G; Vi A F is a convex polygon: Assume a
root exists except A;B;. As for the environment clas-
bowstraight line starting from an arbitrary point on
sification, we give the following definitions.
radia G; 1 V; toward ibyte Since G; VV; NF is convex .
Definition 2 For the area A;B;, internal and ez- aaa the half- acta line comes out only from the
ternal environments are defined as follows: line segment A;B; without passing through the ages
Int(A;B;): Internal environment is defined as the one line segments. Therefore, the tip comes out from L?:‘A, OF
whose root comes from the area between A; and B;. 7 due to the existence of Int(A;B;i) before reaching
Ext(A;B;): External environment is defined by the fig*, Once the tip comes out from LB: 4,» the only feasi-
area A;B; except Int(A;B;). iat case is that the tip makes contact with I nt(A;B;).
Let us now define the area V; which also restricts the In any case, the tip finally reaches on Int(A;B;).
initial pass to Int(A;B;). (ii) G; NV; F is a concave polygon: Figure 5(c)
is the only example of this case. Since the polygon
Definition 3 Suppose an arbitrary point Q denoted
is composed of two triangles having the common top
by the vector q with respect to Xo. Let a; and 6; be two
angle, the half-straight line starting from an arbitrary
vectors expressing the positions A; and B; with respect
point on Le AG;NV; toward the common point always
to Xo. Define V; as an assemble of q satisfying V;(q) =
reaches Int(A;B;) without coming out from the other
sgn{(a;—q),(b:—q)} > 0, where sgn(x, y) is given by
line segment forming the polygon. Thus, the theorem
holds true. O
rey The approaching strategy based on Theorem 1 guar-
sgn(z,y) =
je eul w) antees finding a route for reaching a new contact point
within the designated environment’s surface. In the
where, ® denotes a vector product for two vectors x= algorithm, we simply move the pEghe along the poe
(@1, 22)" and y = (yi, 42)". connecting two central points on Le SGiNV: and tk“Ay
Consider two half planes whose boundary line in-
cludes the line segment A;B;. By Definition 3, V; de- 4.2.2 Bisection Method
notes the half plane that we can see in the right-hand
side when moving from B; to A;. Note that GOV: 4 G Once the probe detects an initial contact point, the
for Figure 5(b), while G; NV; = G; for Figure 5(a), (c), bisection method is continuously applied for finding a
(d) and Figure 6(b). Another remark is that G; form- local concave point, while it is not always obtained. In
ing a concave quadrangle (Figure 5(b)) is converted this subsection, after a couple of definitions, we provide
into convex one by G; M V;, while G; having two trian- a theorem ensuring for always making the tip converge
gles (Figure 5(c)) keeps the concave shape even under on Int(A;B;). Suppose that the tip of the probe is
G; A V;. Supposing that the probe is inserted along already in contact with Int(A;B;) by initial pass plan-
its longitudinal direction, we now describe a sufficient ning. Then, bisection method has the following proce-
condition for making the tip reach Int(A;B;). dure:
162 M. Kaneko and T. Tsuji

Figure 8: Relationship between Po ) and 7 Yj.

Figure 7: Definition of a mazimum swing angle is determined such that the probe
never comes out from the line segments PY) A; and
(i) Swing motion: The probe is rotated around the PY) B,. If there is no Ext(A;B;) within Bs Nn V;,
joint until either it makes contact with an envi- the tip always reaches Int(A;B;) through the bisection
ronment or it exceeds a prescribed rotational an- method.
gle.
Proof : By the assumption imparted to the max-
(ii) Dividing: The angular displacement obtained dur- imum swing angle, the probe never comes out from
ing the swing motion is equally divided. Then, the
the line segments pY A; and py )B; as shown in
probe is swung back by the equally divided angle.
Figure 8(a). From Figure 8(b), we can easily show
(itt) Inserting motion: The probe is inserted along the
longitudinal direction till the tip makes contact CONV) De 0 Vids Soret wtecldast fialan
with an environment. which means if there is no Ext(A;B;) within 7? NY,
(iv) (i) through (iii) are repeated until the probe re- (pe NVi(j = 1,2,...,n) cannot include Ezt(A;B;) ei-
sults in one of the following states, (a) The probe ther. Therefore, the tip comes out from i or stops
loses any rotational degree of freedom around the due to the existence of Int(A;B;) before reaching ia
joint, or (b) The tip converges the intersection be- Once the tip comes out from re the only feasible case
tween the environment’s surface and the boundary is that it makes contact with Int(A;B;). In any case,
imparted as a constraint condition. the tip finally reaches on Int(A;B;). 0D
Definition 4 Let the joint position be P;. Define T; In order to utilize Theorem 2, we have to ensure that
as a semi-infinite region sandwiched by two lines P;A;
there is no Ext(A;B;) within iPom V;. In case that
and P;B;.
there exists an external environment within Ti Vi,
7,” is shown by the hatched line in Figure Wee however, the tip does not always reach Int(A;B;)
where the superscript (j) denotes the value after j-th even though the bisection method is executed within
insertion. Now let us consider an extreme case, where
7”) 1 V;. Figure 9(a) may provide a good example.
the initial contact is achieved at the top of the hill
Suppose that the initial contact is achieved at the top of
as shown in Figure 7(b). The bisection method starts
the hill and the bisection method is executed as shown
by swinging the probe in the left direction (or right
in Figure 9(a). At the end of the bisection method,
direction). Since the probe always goes into the safety
the tip will converge to the local concave point D; be-
area F in this particular case, the probe will not make
longing to Ext(A;B;). To exclude the convergence to
contact with the environment any more. Therefore,
such an external environment, we again recall G; where
we need a boundary, such that we can stop the swing
it is guaranteed that there is no Ext(A;B;) in it. For
motion. Te ) provides a reasonable boundary, although example, if the bisection method is planned, such that
it is still not a sufficient boundary for finally making
the tip may move within Te ) AV; 0G; as shown in
the tip converge on Int(A;B;).
Figure 9(b), the convergence on Int(A;B;) is always
Theorem 2 Suppose that an initial contact with guaranteed. Theorem 3 provides a sufficient condition
Int(A;,B;) is already completed. Also suppose that the for making the tip always converge on Int(A;B;).
Pulling Motion Based Tactile Sensing
163

Figure 10: Final states after bisection method: (a) Local

concave point; (b) Intersection between the environment’s
surface and ‘pee (c) Multiple contacts.

Figure 9: An example of environment where there exists Definition 6 Define K, as an assemble of v satis-
Ext(A;B:) within 7 Me.
fying v't* <0, where v and t are vectors expressing
the moving direction of the joint, and the longitudinal
Theorem 3 Suppose that an initial contact with direction of the probe, respectively, and * denotes the -
Int(A;B;) is already completed. A sufficient condi- value just after the bisection method is completed. Also,
tion for making the tip finally converge on Int(A;B;) define Kz as an assemble ofv satisfying sgn(v,t*) < 0.
is to execute bisection method such that the tip may not More simply, K; denotes the half plane excluding t*
come out from the boundary of T) AV;NG; except the when the whole plane is divided at the joint by the line
line segment A;B;. perpendicular to t.
Proof : Since there is no Ext(A;B;) within qe ae Definition 7 Face(right) = ON (or Face(left) =
V;NG;, the tip comes out from ae or stops due to the ON or Face(tip) = ON) means that a part of the right
side (or left side or tip) of the probe makes contact with
existence of Int(A;B;) before reaching Lge: Therefore,
an environment.
by the similar logic that explained in the proof in the-
orem 2, we can show that the tip finally reaches on Based on Definition 5, the three cases (a), (b), and
Int(A;B;) in any case. O (c) in Figure 10 can be classified by (Yi, Y2) = (1,0),
(V1,¥a)= (1,1 or 1), (¥1,¥s) = (2,0), respectively.
For each case, let us now consider a sufficient condi-
4.3 Tracing Motion Planning
tion for realizing the pulling motion based tracing mo-
4.3.1 Realization of the Pulling Motion tion. Let K be the region where the joint can be moved
When the local concave search comes to the end, without generating any pushing motion. In case (a),
there are basically two possible cases as shown in Fig- K = K, (the hatched area in Figure 11(a)). In case
ure 10, namely, the tip finally reaches a local concave (b), if Yo = —1, K = K,MKez (the double hatched area
point (a), and it stops at a non-local concave point in Figure 11(b)) and if Yy= 1, K = KyNKg. Incase (c),
((b) and (c)). In Figure 10(b) and (c), the tip stops if Face(left) =ON, K =K,MKe (the double hatched
due to the boundary constraint (b) and due to multiple area in Figure 11(c)) and if Face(right) = ON, Y2 = 1,
contacts (c), respectively. iS = Ian jee

Theorem 4 A sufficient condition for achieving a

Definition 5 Y, denotes the final contact mode
pulling motion based tracing is to determine the moving
where Y; = 1 means that only the tip makes contact
direction of the joint and the joint torque, such that
with the environment, and Y; = 2 means multiple con-
v; €K and:
tacts at the tip and other points. Y2 denotes the rota-
tional constraint around the joint, where Y2 = 0, 1 and T = T9S8gn(Vj,
t*) (2)
—~1 are full constraint (Figure 10(a)(c)), single con-
straint for the clockwise direction, and single constraint where To(> 0) is the reference torque and the positive
for the counter clockwise direction (Figure 10(b)), re- direction of T is chosen in the clockwise direction, and
spectively. K is given below.
164 M. Kaneko and T. Tsuji

in Figure 12(b). Once the probe is away from the sur-

face, the joint torque sensor can not detect any reaction
torque from the environment, by which the sensing sys-
tem can recognize the case. Now let us classify what
sort of cases appear during a tracing motion and con-
sider how to deal with each case.
Definition 8 Let Prin be the destination point for a
tracing motion. We choose Prin = A; for sgn(v;,t*) >
0 or Prin = Bi for'sgn(v;,t")< 0.
We stop the tracing motion when a part of the probe
reaches Pr;,. During the tracing motion, however,
(A) : If(Yi, 2) there might appear a contact point jump due to the
(B) : If (Yi, Yo) = (1,-1), K= Ky Ko.
surface geometry. For such a contact point jump, we
(erie 7s) =Kb, p= KOK. define two points P;,4 and P2nq as follows.
DPe TY 2¥3)—=(2,0) "and Face(left) =ON, =
Definition 9 Let P,,4 and Pong be the contact points
Ki NK»
just before and after a contact point jump, respectively.
(E): If (¥1,2) = (2,0) and Face(right) =ON, K = In case of a single probe detection, the point closer to
Ki NK. the tip is chosen as Pyst.
Proof : (Omitted) O
All possible cases during a tracing motion can be
In case (A), when v; satisfying K = Ky is cho- classified as follows:
sen, only the right or the left tracing motion will
be achieved. For completing the tracing motion for <Case 1> All contact points are continuously de-
both directions, we further separate K = Ky, into tected by the probe tip for the designated
K =K,iNKz and K = Ki Kp. Then, we execute area.
the right and the left tracing motions by choosing v; <Case 2> At least, one contact point jump appears.
satisfying v; € Ky Kz and v; € K, NK2, respectively. <Case 3> v;!t > 0 is detected.
<Case 4> The joint loses the moving degree of free-
4.3.2 All Possible Cases During Tracing Motion
dom in the direction given by v;.
Tracing motion is continued until the tip success-
All cases can be detected by utilizing the sensors im-
fully traces from the initial point D; to A; (or B;) as
plemented in the robot-probe system. For example,
shown in Figure 12(a). However, the contact between
<Case 1> and <Case 2> are confirmed by the probe
the probe and the environment is not always guaran-
output. <Case 3> is certified by the sensors mounted
teed during the tracing motion. For example, the tip
in each joint of the robot arm. <Case 4> can be con-
may be away from the environment at the top of the
firmed by the torque sensor.
hill during the tracing motion from A; to B;, as shown
Since <Case 1> means that the designated unknown
area Int(A;B;) becomes known, we can simply cat-
egorize such an area into W. For one of the three
other cases, however, we have to memorize a part of
Int(A;B;) as a further non-detected area. Let us now
consider which points we should register for <Case 2>,
<Case 3> and <Case 4>. Just for our convenience,
<Case 2> is further classified into the following three
cases.
<Case 2-1> Ponq exists on Int(A;B;).
<Case 2-2> Ponq exists on Ext(A;B;).
<Case 2-3> Pong does not exist within F.
Figure 12: Realization of tracing motion.
Pulling Motion Based Tactile Sensing
165

Figure 13: P2na detected on Int(A;Bi) or Ext(A;Bi). Figure 14: How to store non-detected area.

<Case 2-1> or <Case 2-2> is distinguished by check- (ii) In case of Prin = Bi:
ing whether Pong € GiU (TiN) or Pong € Gin (TiUV,), (Aj, By) = (Prst, Pona or Pyin).
respectively, as shown in Figure 13(a) and (b). <Case This rule is for keeping the characteristics of V,; de-
2-3> happens when the probe enters F without any fined in 4.2.1.
contact. In <Case 2-1>, the area between P),, and
Pond is registered as a non-detected area and then the 4.4 Infinite Loop Escape
tracing motion is continued for the remaining non-
traced area. In both <Case 2-2> and <Case 2-3>, There might be a particular state in which the tip can
we stop any further tracing motion and register the find the same point repeatedly during the local con-
area between Pj5; and Pyin as a non-detected area. cave point search and the tracing motion planning. In
<Case 3> may occur because the condition realizing a order to avoid such an undesirable mode, we prepare
pulling motion is satisfied only at the starting position. the infinite loop escape, where the probe temporarily
Once v;'t > 0 is detected, we stop the tracing motion searches a new contact point by utilizing the same way
and register the area between P;,; and Pyin as a non- taken in the initial pass planning. Since the initial
detected one, where Pj,; is replaced by the point in pass planning ensures to find a new contact point be-
which v;!t > 0 is detected. <Case 4> may also occur tween A;B;, we can separate the area A;B; into two
depending on the surface geometry, the probe posture new areas A;,,Bj;4, and A;,2Bj42, as shown in Fig-
and the moving direction of the probe. Once the probe ure 15(a). After dividing the area, we leave from the
loses the degree of freedom in the direction v;, we stop infinite loop escape and come back to the normal mode
the tracing motion and register the area between P, st given by local concave point search and tracing mo-
and Prin as a non-detected one, where P< is replaced tion planning. Now, assume that an infinite mode ap-
by the point in which the probe loses the degree of free- pears every time after initial pass planning motion. In
such an extreme case, Aji1Bj41 (or Ai+2Bi+2) is fur-
dom. In case of Figure 11(c), according to Theorem 4,
ther separated into two unknown areas A;+3Bj43 and
the tracing motion is not executed in the direction from
Aj44B;44, and so forth, as shown in Figure 15(a). For
D; to A;. In such a situation, we register (Po) ; Pe)
and (Pe), Pe) as non-detected areas after regarding
(PA2, Pana) = (DisQ) and (PLS2, Phin) = (Q,4i) as
shown in Figure 12(c).
ra
\
A
Ch Ceew
Be

4.3.3 Registering into A;B;

“1 Ais» Bio pa \
—=—— CREW Cf
AWN [Sx ‘ as
The non-detected area, such as (Pist, Pana) has to be ae oii of \ J eo SS O® ...,Cim eW

finally stored into (A;,.B,;) for executing the program

Aj+3Bi43 1-5 AmBm Am+1Bm4t y+) AnBn vat

Dist(A;,B))<¢« => Cie Wo CRmt € W,...,C EW

recursively. In this subsection, we briefly describe the
basic rule to obtain the set of (Aj, B;). (a) (b)
<Rule to determine (A;, B;) > (see Figure 14)
Figure 15: Infinite loop escape.
(i) In case of Prin = A;: (A;, B;) a (Ponaor Pin; Pre):
166 M. Kaneko and T. Tsuji

Local concave point search

Tracing motion planning (a) (b)

Infinite loop? Infinite loop escape

No

j=j~jt+m-1
New non-traced area? Store new
unknown area A;B;
No

Lins Yes
m, : the number of unknown
area detected in i-th roop

Figure 16: Flowchart.

every newly divided area, if Dist(A;,B;) < ¢€ is sat-

isfied, any further separation between A; and B; is
stopped and Gi € W2 is assigned. This implies that
the algorithm brings the environment’s shape in relief
even when an infinite loop occurs continuously. Figure
16 shows the overall flowchart of the algorithm.

Figure 18: Simulation result (example 2).

5 Simulation

Figure 17 and Figure 18 show simulation results, where and the unknown areas, respectively, and the line seg-
the continuous and the dotted lines denote the known ment passing through the joint expresses the moving
direction of the joint, which is determined by the suf-
ficient condition given by Theorem 4. Figure 17 is a
simple example, where the tip can trace every surface
eventually. Also, there is no Ext(A;B;) within 7? NY;
and, therefore, the local concave point can be found by
utilizing either Theorem 2 or Theorem 3. Figure 18
shows an example including a couple of never-touching
areas, where the tip cannot reach and touch. Although
this example includes such a complicated surface, it
can be seen that the proposed algorithm enables the
tip to reach every reachable area step by step and, fi-
nally, it finds the contour of the surface except three
never-touching areas. From these simulation results,
it can be understood that the unknown area gradually
(c) (d) decreases and finally disappears except the particular
area which the probe cannot reach and touch.
Figure 17: Simulation result (example 1).
Pulling Motion Based Tactile Sensing
167
6 Conclusions IEEE Int. Conf. on Robotics and Automation, pp342—
347, 1989.
A tactile sensing algorithm for detecting the concave
surface was discussed. In our next step, we will relax [8] Bajcsy, R.: “What can we learn from one finger ex-
periments?”, Proc. of the 1st Int. Symp on Robotics
this assumption so that the tip can reach much larger
Research, pp509-527, 1984.
area. Also, we will try to extend the pulling-motion-
based tactile sensing into a 3D concave environment. [9] Bicchi, A., Salisbury J. K. and D. J. Brock: Contact
Since tracing every area for a 3D environment is not sensing from force measurements, Int. J. of Robotics
Research, vol.12, no.3, 1993
as feasible as that in a 2D one, the parametrized local
surface function [18] may be a useful tool for expressing [10] Bays, J. S.: Tactile shape sensing via single- and multi-
the surface shape with respect to a local contact frame. fingered hands, Proc. of the IEEE Int. Conf. on Robot-
ics and Automation, PP 290-295, 1989.
We believe, however, that the concept of the pulling-
motion-based tracing should be still included even in [11] Caselli, S., C. Magnanini, F. Zanichelli, and E. Caraffi:
the algorithm for surface sensing of a 3D environment. Efficient exploration and recognition of convex objects
based on haptic perception, Proc. of the IEEE Int.
Conf. on Robotics and Automation, PP 3508-3513,
Acknowledgments 1996.

This work has been supported by CREST of JST [12] Gaston, P. C., and T. Lozano-Perez: Tactile recogni-
tion and localization using object models, The case of
(Japan Science and Technology).
polyhedra on a plane, MIT Artificial Intelligence Lab.
Memo, no.705, 1983.
References
[13] Grimson, W. E. L. and T. Lozano-Perez: Model based
recognition and localization from sparse three dimen-
[1] Dario, P. and G. Buttazzo: An anthropomorphic robot
sional sensory data, AI Memo 738, MIT, AI Laboratory,
finger for investigating artificial tactile perception, Int
Cambridge, MA, 1983.
J. Robotics Research, vol.6, no.3, pp25—48, 1987.
[14] Cole, R., and C. K. Yap: Shape from probing, J. of
N Fearing, R. S. and T. O. Binford: Using a cylindrical
Algorithms 8, pp19-38, 1987.
tactile sensor for determining curvature, Proc. of the
IEEE Int. Conf. on Robotics and Automation, Philadel- [15] Russell, R. A. : Using tactile whiskers to measure sur-
phia, pp765-771, 1988. face contours, Proc. of the 1992 IEEE Int. Conf. on
Robotics and Automation, pp1295—1300, 1992.
[3 ‘otis
Maekawa, H., K. Tanie, K. Komoriya, M. Kaneko, C.
Horiguchi, and T. Sugawara: Development of a finger- [16] Tsujimura, T. and T. Yabuta: Object detection by tac-
shaped tactile sensor and its evaluation by active touch, tile sensing method employing force/moment informa-
Proc. of the IEEE Int. Conf. on Robotics and Automa- tion, IEEE Trans. on Robotics and Automation, vol.5,
tion, Nice, p1327, 1992. no.4, pp444—450, 1988.
[4](ase Salisbury, J. K.: Interpretation of contact geometries [17] Roberts, K. S. : Robot active touch exploration, Proc.
from force measurements, Proc. of the 1st Int. Symp. of the IEEE Int. Conf. on Robotics and Automation,
on Robotics Research, 1984. PP 980-985, 1990.
[5] Brock, D.L. and S.Chiu: Environment perception of [18] Chen, N., R. Rink, and H. Zhang: Local object shape
an articulated robot hand using contact sensors, Proc. from tactile sensing, Proc. of the IEEE Int. Conf. on
of the IEEE Int. Conf. on Robotics and Automation, Robotics and Automation, PP 3496-3501, 1996.
Raleigh, pp89-96, 1987.
[19] Kaneko, M., M. Higashimori, and T, Tsuji: Pulling
[6] Kaneko, M., and K. Honkawa: Compliant motion based motion based tactile sensing for concave surface, Proc.
active sensing by robotic fingers, Preprints of the 4th of the IEEE Int. Conf. on Robotics and Automation,
IFAC Symp. on Robot Control, Capri, pp137-142, pp2477—2484, 1997.
1994.
[20] Boissonnat, J. D. and M. Yvinec: Probing a scene of
[7] Allen, P. and K. S. Roberts: Haptic object recogni- non-convex polyhedra, Algorithmica, vol.8, pp321—342,
tion using a multi-fingered dexterous hand, Proc. of the 1992.
 3 are:Sg dell ae law al Boat ~ "20 —_7 : es
ern
pee bir

Shh > ao pat iw marcitcn

talyl snifonauds veil iaslehche e
dabei
a
tle get,’ iee (Low cer agele tee doe ot bn
mytal dome ‘tao
th (tt Gs) an) Fett oe, .
hijo ont Srutey (2 ert thee ww! A
PEED » a¥innon OF ¢ am qctitnen seat i
A rratmrivers CS, 4 yh ave. vie yuk =e
lecial Lomitenitiog sla ven Cif «ai tnaty ex plete
ce vn
] ee pes (0 aithecan yee Wit lewd felons © vl tare fed) Ce
v2 2 : vei ae
pana bay Srieee ber rote OW Peepeas iw inate
~age 3
guitlling wflt-te oy fr? wile jes! Sei a frroied Bi
ea UP onyebay
ai crn Fadia fhe set Lipo giver be
; Se eras ratneotwas (B® to gales onal ae
wc. errant By bel >
ive eieclan. we ns?
ORY ~
JA staarab
r avaens MN =]

nae es
ret, to Tettuo vd be "dite a
laa staid wose
P a [ragtten viet
a
i.-“ Pa oni ‘ od amd,

i
jit his & 2 owe Ot
_< it are DAP a Mevne: na oe
hewfer Marg wh.mace td Sa
eh yeetiyertsg
fee.i waht ”~ Babipew
eel Tay ange on (Oi aligaatent
cline) jatitbauy yithanites ind
ae RO? beyaes
wie sere
NY av v “hd

ve all ardls Lava hae 1 teenth trol b Pp

te ae Syecdasdcloombots i
“ i abt
a lv wt ee y seatay otnedie'S youd wrmeeted So mibodnyl perc) ad
ie fas SOO addWw pants aut eRe Ete
ern
i st
WE BON Lagi AA Ar iad: ting pj Aibledvi
—
(Ve
) ud nui dé heieee if wind
p-Pagtac wehate hag, t aeentitics' iz> <magitl fhe tragedy) ARs te
ire,
anny spat hi Padhioni gem ge srviaeiaie rab» Jaren
Sk
hai
ate ia ani), : Peete Physi! « fn

PRT UO MRT cit

met af
i *

= =
se ioe ve. 4 a is 4 e
‘dipti se =. HO) Gas Nolo AIS 6 Fed aly iy Pe Le) Ova
eo % mely A rad wiiodal ae
cone
Ay dul 4M. ut at A) URRY A ol mute tae, eee =»
. . ‘eugt ch pater ‘ ®
ne \,
»| » Tale igs
7 reel a de Wee? canad’ b bets ay) .
el liek { Ve Lal Ae fee DT! rt Pee Py
Ine") mins2 ay a 20 AWD gate BAP ad
AP Dicerinaieey, ante "90 argon gels :
oiled
MOE LOC MELE ced unr ae da, beige eid: ‘whi,
A® ie sutton fig) iwih ade
pale, gen hoe -bomdidneH Mh oberee C0 eerie in
rede vr OP
ii
tet
AFD Wobbee arin es, Betas vlttows Dee)
1
Pheu tyemei
yttitnien tins ited) « wr antt
aavataes ten! bse 2 WO a ino) Atel AHO AL Sua? diy) of ant
yar ty a aun Ae Mites
‘ (i Aen seus i pals 1hANYte ol 9 igtliay
Ti | ss ow gh tng —_F va?
sal
ig wis, ity rmuilve Vv haa Ft lage iP iD)
, i ogee Ql ae) |

int : is gust ee oda4

~ ha Lak ! m4 } a Aa ed if stigt ] ye mia

wisly | iheae NerG tien) oncrisi gat: wet

a A
hy een
Compensatory Grasping with the Parallel Jaw Gripper
Tao Zhang, University of California, Berkeley, Berkeley, CA
Gordon Smith, University of California, Berkeley, Berkeley, CA
Ken Goldberg, University of California, Berkeley, Berkeley, CA

For many industrial parts, their resting pose differs

from the orientation desired for assembly. It is pos-
sible in many cases to compensate for this difference
using a parallel-jaw gripper with fixed orientation. The
idea is to arrange contact points on each gripper jaw
so that the part is reoriented as it is grasped. We ana-
lyze the mechanics of compensatory grasping based on
a combination of toppling, jamming, accessibility, and
form closure and describe an algorithm for the design
of such grasps based on the constrained toppling graph.

1 Introduction

Industrial parts on a flat worksurface will naturally

Figure 2: Terminology.
come to rest in one of several stable orientations [1],
but it is often necessary to rotate a part into a differ-
ent (unstable) orientation for assembly [2]. We achieve this using a simple parallel-jaw gripper
with four tips as shown in Figure 2. First, toppling
tip A and pushing tip A’ make contact with the part
and topple it from the initial orientation to the desired
orientation. This process is referred to as constrained
toppling. Then, as soon as the part reaches the desired
orientation, left grasping tip B’ and right grasping tip
B make contact, stop the part’s rotation, and securely
grasp it. This process is simply referred to as grasping.
Note that the pivot point, C, maintains contact with
bottom surface at all times.
These four tips and the parallel jaw gripper are de-
(a) (b)
signed to be easily reconfigurable to handle different
industrial parts, and low in cost, footprint and weight.
Figure 1: A compensatory grasp.

2 Related Work
This paper proposes an inexpensive (minimalist)
method for compensating for these differences in ori- Grasping is a fundamental issue in robotics; [3] pro-
entation by reorienting the part during grasping. As vides a useful review of research on the topic.
illustrated in Figure 1, the part is initially in stable
orientation (a); it then is rotated by the gripper to ori- The two most important classes of grasps are known
entation (b) for assembly onto the peg. We refer to as force closure and form closure. The difference be-
this as a compensatory grasp. tween these two is that the latter is stable regardless
170 T. Zhang, G. Smith, and K. Goldberg

of the external wrench applied to the object. In 1990, in force closure using constrained slip. Rus [13] pro-
Markenscoff et al. [4] proved, by infinitesimal pertur- posed a finger tracking technique to generate rotation
bation analysis, that four hard fingers are necessary of grasped objects with sliding. Hong et al. [14] de-
and sufficient to achieve form closure of a 2D object in veloped a planing algorithm to acquire a desired grasp
the absence of friction. The parallel jaw grippers we by using a finger gait technique, which allows reposi-
propose provide four contact points but the location of tion of fingers while maintaining a grasp. Fearing [15]
tips B and B’ are dependent on the locations of A and considered both sliding and rolling manipulation, and
A’ respectively. developed grasp planning based upon local tactile feed-
Mason [5] was the first to study the role of passive back, geometry, and frictional constraints. Bicchi and
compliance in grasping and manipulation. Brost [6] Sorrentino [16] analyzed the effect of rolling. Compen-
applied Mason’s Rule to analyze the mechanics of the satory grasps combine rolling and sliding. Another ap-
parallel-jaw gripper for polygonal parts moving in the proach was studied by Rao et al. [17]. They proposed
plane. He showed that it is possible to compensate picking up a part using a parallel gripper with a piv-
for errors in part orientation using passive push and oting bearing, allowing the part to pivot under gravity
squeeze mechanics. Our paper considers grasp mechan- to rotate into a new configuration.
ics in the gravitational plane. Our work was motivated by recent research in top-
In 1995, Canny and Goldberg [7] introduced the pling manipulation. Zhang and Gupta [18] studied how
concept of Reduced Intricacy in Sensing and Control parts can be reoriented as they fall down a series of
(RISC) to robotics, which combines simple automation steps. The authors derived the condition for toppling
hardware with appropriate algorithms. The resulting over a step and defined the transition height, which
manipulation systems are inexpensive and reliable. is the minimum step height to topple a part from a
given stable orientation to another. Yu et al. [19] ap-
Trinkle and Paul [8] studied how to orient parts in
plied toppling technique to predict the location of an
the gravitational plane by lifting them off work-surface
object’s COM. The force and position information of
using a gripper. Based upon the geometry of a grasp
the robot finger-tip was provided during Tip (toppling)
and quasi-static analysis, the authors generated lifta-
Operations to estimate the Gravity Equi-Effect Plane
bility regions, which defines the qualitative motion of
containing the COM and the line contact between the
a squeezed object. They predicted the motion by solv-
object and the work-surface. The intersection of three
ing the forward object motion problem that is the dual
such planes at different orientations gave the location of
of a nonlinear program employing Peshkin’s [9] mini-
mum power principle. The pre-liftoff phase analysis of
the COM. Lynch [20] derived sufficient mechanical con-
ditions for toppling parts on a conveyor belt in terms
Trinkle and Paul’s paper is related to our constrained
toppling phase analysis in that we both applied graphic
of constraints on contact friction, location, and motion.
method to analyze the interaction among a planar ob- In [21], we describe the toppling graph to represent the
mechanics and the geometry of toppling manipulation.
ject, a supporting surface, and a gripper in the plane
containing gravity. One important difference is that In this paper, we combine toppling mechanics with an
we focus on the parallel-jaw gripper and consider how analysis of jamming, accessibility and form closure.
jaws can be designed to facilitate grasping using only
translational motion; Trinkle and Paul considered two 3 Problem Definition
or three-point (contact) initial grasps, and employed
both pusher and roll strategy to achieve form-closure Given the planar projection of an n-sided convex poly-
grasps. Abell and Erdmann [10] studied how a planar hedral part P, how can we rotate the part to a de-
polygon can be rotated in a gravitational field while sired orientation and grasp it securely? There are two
stably supported by two frictionless contacts. Zumel
phases involved in this process: constrained toppling
and Erdmann [11][12] analyzed nonprehensile manipu- and grasping. We are given as input: the part’s center
lation using two palms jointed at a central hinge. They
of mass (COM), uncertainty in vertex location, ¢, the
also developed a sensorless approach to orient parts. coefficient of friction between the part and the surface,
Another approach to reorienting parts is dextrous py, and the coefficient of friction between the part and
manipulation, where the part is reoriented as it is held the gripper, jz. Let 6 denote the orientation of the part
Compensatory Grasping with the Parallel Jaw Gripper
171
from the +X direction; initially @ = 0. Let 64 denote
toppling tip
the angle at the desired orientation.
ZL friction cone
During the constrained toppling phase, only gripper constraining
tips A and A’ make contact with the part. We require tip friction
that these two tips cause the part to rotate counter- cone
clockwise without causing C to lose contact with the
surface. From the toppling analysis in section 4 we are
able to show that on a given edge, the rolling condi- dieA in
tions are more easily satisfied if A’ is lower, i.e. da:
is smaller. This will in general produce a finite set of =p OK
bottom surface
possible d4’ candidates for the part. The height of A,
friction cone
da, can then be determined by the graphical analysis
described in section 4.
Figure 3: Notation.
The grasping phase occurs when the part has been
toppled to 6 = 0g. During grasping the grasping grip-
per tips, B and B’, make contact with the part. We re- We consider P as shown in Figure 3. Starting from
quire that the contacts corresponding to A, A’, B, and the pivot, we consider each edge of the part in counter-
B’ create form-closure on the part, even when friction clockwise order, namely e1, €2, ..., €n. The edge e;,
with vertices vu; at (x, 2) and vj41 at (2441, 2:41), is
is disregarded. Since d, and dy: are already known, we
in direction ~; from the +X axis.
must determine dg and dg such that form-closure is
achieved. We additionally require that the tips B and We assume that the part and the gripper are rigid,
B’ do not make contact with the part before 6 = 04. and also that the part’s geometry, the location of the
This requirement is essentially a form of accessibility COM, and the position of the jaws are known exactly.
constraint and further limits the possible values of dz We also assume that the pushing tip and the toppling
and dg. Finally, because A and B are fixed on the tip of the gripper contact the part simultaneously, and
same jaw of the gripper (and likewise A’ and B’) the the motion of the part and is slow enough that we can
geometry of the part along with the relative heights of ignore inertial effects.
A and B will determine the relative x offset between A
and B, which we denote zap.
The final output of the analysis is the height of each 4 Constrained Toppling Analysis
of the four tips, d4, d4’, dg, and dy’, as well as the rel-
ative x offset between tips on each jaw, zap and 2 ,4/p) We divide constrained toppling into a rolling phase and
(see Figure 2). This set of variables determines the a settling phase. Our analysis involves the graphical
gripper design that will rotate the part to the desired construction of a set of functions that represent the me-
orientation. chanics of these phases. In [21], we introduced the top-
pling graph, which includes the radius function, vertex
Figure 3 shows the notation used in our constrained
height functions, constrained rolling height functions,
toppling analysis. The part sits on a flat worksurface
and constrained jamming height functions.
in a stable orientation. The worksurface friction cone
half-angle is a, = tan~+p, and the gripper tip friction The radius function, R(@), is the height of the COM
half-angle is a; = tan~',. The contact point between above the surface as the part is rotated through @ =
the part and the worksurface is called a pivot vertex, 0 > 27. The local minima of the radius function indi-
denoted C in Figure 2, and taken to be at (0,0). The cate the stable orientations of the part [22], while the
COM is a distance p from the origin and angle 7 from local maxima are unstable equilibrium orientations. In
the +X direction at its initial orientation. The pushing this paper we will consider only the range of angles
tip, A’, is a distance d4 from the bottom surface. We corresponding to rotation from one single stable ori-
denote the vector at the left edge of the toppling tip entation to the next. We assume that the part can
friction cone as f; and the right edge as f,. be toppled into that orientation before grasping. This
172 T. Zhang, G. Smith, and K. Goldberg

range consists of the angles 9 = 0 — 0, where 6, is

the angle of the ext stable orientation. Additionally,
represents the unstable equilibrium angle in that range.
The vertex height function, V;(@), gives the height
of vertex i above the surface as the part rotates. Each
vertex of the part has a vertex height function. Using
the vertex height functions we can determine which
edge a gripper tip is in contact with for any @ and
height.
The constrained rolling height function, H;(@), is the
minimum height at @ that the toppling tip, A, in con-
tact with edge e; must be in order to roll the part given
the height of A’. H;(@) is determined on the range 6 =
0 + &. At 6 = @ the part is no longer being rolled,
but it is now settling under its own weight. The con-
strained jamming height function, H;(@), indicates a
range of d, values that may cause jamming during the Figure 4: Rolling conditions.
settling process. H;(@) is determined on the range 6 =
6; => O,,.
applied for the case where the contact switches edges
All of these functions are dependant on 6 and map (see [23] for details).
from part orientation to distance: S! — R*t, where Under the assumption that A’ keeps contact with
S1 is the set of planar orientations. The combination €n, relative motion direction between the part and A’
of these four functions forms the constrained toppling is uncertain, and it depends on an angle, w + 0, where
graph from which we can height of the toppling tip A w denotes the interior angle of a part at C as shown in
required to guarantee toppling. Figure 4. We study the constrained rolling conditions
based upon different w + @ ranges.
4.1 Constrained Rolling Height Function Let w; be the distance along edge e; as shown in
Figure 4. Any point on e; can be expressed as (2; +
w; cosy;, 2; + w;sin Yi).
During rolling, the part rotates about C. Friction be-
tween the part and the bottom surface must not pre- Consider the case where 7 > w+ 0 > 77/2. In sucha
vent C from sliding to the right, and friction between case, rotation causes the contact between the part and
the part and the tips must not prevent the part from A’ to move away from C. To determine the constrained
slipping relative to the tips. Additionally, the system rolling height function, we begin by constructing a tri-
of forces on the part—the contact force at the bottom angle as shown in Figure 4 with vertices Py, P,, and
surface, the contact force at the tips, and the part’s P,. Po is determined by the intersection of the left
weight—must generate a positive moment on the part edge of the bottom friction cone and the left edge of
with respective to C. the pushing tip friction cone, and is at (po, Zpo). Pi is
the intersection of the vertical line through the com and
The constrained rolling height function, H;(@), is the
the left edge of the bottom friction cone, and is located
minimum height at 0 and a given d that A in contact
at (%p1,2p1). P2 is the intersection of the vertical line
with edge e; must be in order to roll a part. This
through the com and the left edge of the constrained
height is determined as a function of 6 based on the tip friction cone, and is located at (Lp2,%p2). From
rolling conditions.
these definitions we have:
As the part rolls, A’ could switch edges if a contact
edge is not long enough. In this paper, we consider Zp = s—tsinas, (1)
the situation that A’ keeps contact with e; during the 2p0 => —8s/[p +t cos ap, (2)
entire rolling phase, and the same methodology can be Tp = Ss, (3)
Compensatory Grasping with the Parallel Jaw Gripper
173

z i= Sis
—, 4
i! Lo ( )
Lp2 = 8; (5)

_ d/tan(@+w)—s

where

Ss = pcos(n+8), (7)
: (Zp2 — Zp1) sin(@ + w + az) (8
sin(O+w+t+ar—ay) )
Rotation angle (8)

Consider a region of the X-Z plane defined by linear

edges. Let a primary region denote a region such that Figure 5: R(@) vs. H3(@), V2(0) and V3(@).
toppling is guaranteed if every force in the toppling
tip friction cone makes a positive moment about every where
point in the primary region. The region is derived using
a graphical method from Mason [24]. Therefore, the 61 = T+ay—%-Yi, (11)
Po P,P» triangle is the primary region in this case. Om = min(m
— yi,7 —w, 4). (12)
For all forces in the toppling tip friction cone to gen-
erate a positive moment about the triangle, the left Thus, the constrained rolling height function within
edge of the friction cone must pass above the triangle; 0<6<4,, is given by:
all other vectors in the friction cone will pass higher.
We find the height sufficient to roll the edge by project- Hy(8) Hz(8)
>Vi(9)
ing lines from Po, Pi, and P» at the angle of the left (0) =| 0 HF) < Vi(6) ve
edge of the pin friction cone, f;, until they intersect the
edge of the part. The intersection with the maximum where
height of those three is the minimum height sufficient
Hj(0) = a; sin + z cos0+ w,sin(O+%,). (14)
to roll the part.
Let w? denote the edge contact on e; where f; passes
Following the same methodology, we find H;(@) un-
exactly through point P2. We can show through geo-
der the condition 6+ w = 7/2 and 7/2 >0+w>0
metric construction that:
(see [23] for details).
w? (8) = (7, :sind 4-2; COs — Z99.— Figure 5 illustrates the functions R(@), H2(@), V2(@)
(x; cos 0 — 2; sinO — &p2) tan(By + 4))/ and V3(@) for the part in Figure 3 with a; = 5°, a, =
10° and d4 = 0.9cm. The kink at 6 = 37° of R(@)
(cos(@ + yy) tan(G; + 8) — sin(@ + y;)). (9)
represents the orientation where é€g is on the bottom
surface. Notice that there are kinks at 6 = 37° in
where 3;, = y; + 1/2 + at.
H2(0), V2(@) and V3(@) too. When @ < 37°, V; is C and
Similarly, the edge contacts for f; passing through tq=4.1lcm, z2=0.0cm, 2 = 56°, 7 = 46°, p=2.2cm,
Po and P, are given by w? and w}. and w = 53°; when 6 > 37°, Ve is C and r2=4.6cm,
The constrained rolling height function, H;(), is Zo=2.4cm, We = 92°, n = 55°, p=2.7cm, and w = 89°.
based on w; which is the maximum of w?, w} and w7 At angle @ any toppling tip at a height, h, such that
in the rolling region 0 < 0 < &. w; can be shown to max(H2(0@),V2(9)) < h < V3(@) will instantaneously
be: rotate the part. The graph indicates that A can roll
the part at any contact on e2 when 0 < @ < 0.
2 0<@ < 4 and yy; <w
w= w? 0<@<0 and yy; > w (10) Given a certain contact edge, a lower A’ results in
i 001 <9 < Om a smaller primary region; thus, a lower A will be able
174 T. Zhang, G. Smith, and K. Goldberg

to topple the part. Therefore, the best location of A’

on a contact edge is ( above the lower vertex, i.e. A’
is as low as possible on that edge. This reduces d,:
candidates to a finite set F ( |F'| =n — 2 in the worst
case) for the part. For each d4, we may employ the
graphical analysis to find the feasible d,.

4.2 Constrained Jamming Height Function

We allow the part continue to rotate after it has

reached 0; if 0g > 6;. We call this process settling,
and intend to determinate whether jamming may occur
in this process. The part may jam while settling due
to the frictional contact at the toppling tip. We will
be conservative and eliminate any toppling tip height
where jamming may occur even though we cannot be
certain it will jam without further information. Note
that we consider the statics (not full dynamics) of the
settling process. Figure 6: Jamming conditions.
We only consider the situation where A’ keeps con-
tact with e; during the entire settling phase. More —d cos Q; Sin ap (20)
4i = ——
SSSSS
general cases can be derived from the same methodol- we sin(@ + w)sin(O + w + az — ap)
ogy. d cos a4 COS Qp
SS 21)
To determine the constrained jamming height func- Se sin(@ + w) sin(@+w +a; — as)’ ey
tion we begin by constructing a primary region as Lp3 — s, (22)

shown in Figure 6. Notice that there is no jamming —s

if the constrained tip contact is left of the part gravity a p3 = =,
ile 23
(23)
line, i.e.,
To guarantee that no jamming occurs, any force in
dtan(@+w) < pcos(0+n). (15)
the toppling tip friction cone must make a positive mo-
Otherwise, the primary region quadrilateral with ver- ment about the critical primary region then the left
tices Po, Pi, Po and P3. Pp is the intersection of edge of the friction cone must make a positive moment.
the vertical line through the part’s com and the right In other words, the left edge of the toppling tip fric-
edge of the constrained tip friction cone, and is lo- tion cone determines the height at which jamming may
cated at (%p0,Zpo). Pi is the constrained tip contact occur.
at (Zp1,Z%pi). P2 is the intersection of the left edge of Similar to the analysis of the toppling height func-
the constrained friction cone and the left edge of the tion, we have w?(9), w}(0) and w?(6) and w;, which
bottom friction cone, located at (p2, Zp2). And P3 is is min(w?, w},w?) at a given @. These functions are
the intersection of the vertical line through the part’s given by:
com and the left edge of the bottom friction cone, lo-
cated at (Xp3, Zp3). w? @< O39 and w; < w — 2a;
w;o)
| @< O32 and w — 2a, < Wiau <w
(ODN om

Ip) = §&, (16) , w? 0 < 635 and w < Wi 24)

dtan(@+w)—s w? O39 <0< 7 = Wk

ae tan(a; — 8 —w)’ ay
where
Lp1 = dtan(9+w), (18)
GS == d, (19) O39 = 1 — a4 — Yi. (25)
Compensatory Grasping with the Parallel Jaw Gripper
175
!
The first two criteria can be described as A must
!
|
1 be above both the rolling height and the jamming
|
I
height on the edge it is in contact with for all 8. Note
|
V2
that when the pin crosses a vertex height function
|
|
l
|
1
| it contacts a new edge and must then be above the
|

i
rolling height and jamming height functions for that
|
| edge. The third criterion is that the pin must not lose
!
|
| contact with the part by passing over it during the
|
| be
Se
er
rolling phase.
fi 20 0. 348 60 80 100 120 140
fe)

Figure 7 demonstrates the constrained toppling

Rotation angle (8) graph of the part shown in Figure 3. From the graph we
can determine a toppling tip at d4, = 2.0cm is capable
Figure 7: Constrained toppling graph. to topple the part to any orientation with 0 < 6 < @,.
Notice that A switches contact edge from é to e; at O..

The constrained jamming height function, J;,(@),

with 0 < 0 < @, is given by:
5 Grasping

oe 4J (9) Hy(0) > Vi()

HO)={ Once the part has been rotated to 0 = 6g, the grasp-
H? (8) < V;(6) cy ing tips, B and B’, must make contact with the part.
Additionally, we require that the combination of the
where contacts corresponding to A, A’, B, and B’ generate
a form-closure grasp on the part. There also exists
J; (9) = 2, sin@ + z;cosO+ wu; sin(6+7%;). (27) an accessibility constraint on the locations of B and
B’ due to the requirement that they not make contact
Therefore, for given @ and d,/, jamming occurs if the with the part until 6 = 0,. Therefore, we divide the
heights of A is lower than J;(9). grasping analysis into two sections corresponding to de-
termining the accessibility constraint and meeting the
4.3 The Constrained Toppling Graph form-closure requirement.
Figure 7 illustrates the entire constrained toppling
5.1 Accessibility
graph that combines the vertex height, constrained
rolling height, and constrained jamming height func- The accessibility constraint will limit the possible
tions to represent the full mechanics of toppling. From heights of the grasping tip, B or B’, for a given height
the constrained toppling graph the necessary toppling of A or A’. In order to determine the accessibility con-
height for A can be determined or shown to be non- straint we must consider the relative motion between
existent. Note that H;(@) must be bounded by the the part and a jaw of the gripper. The rest of the acces-
V,(@) and V;41(0) and is truncated where it intersects sibility discussion will be in a frame of reference fixed
them. to the toppling tip A and will consider a contact B on
For toppling from an initial orientation to the de- edge e;. The accessibility constraint requires that as
sired orientation to be successful, there must exist a the part rotates to the desired final orientation, that
horizontal line from the angle of the initial orientation it is moving out towards the grasping tip and that at
to the angle of the desired orientation at height h that no previous angle has the part been as far out as the
has the following characteristics: fixture tip and made contact.
To illustrate this situation, Figure 8 shows the rota-
1:h > H;(0) for all 0, if Vi(0) < h < Visi (9); tion of a part with respect to the toppling tip. Note
2:h > J;(0) for all 6, if Vi(0) <h < Vi41(4); that at any height within the inaccessible range on
3:h < max(V;(0)) for all i at any 0 < 4. edge €2 indicated in the figure, the location of vertex
v2 would have contacted the grasping tip before the
176 T. Zhang, G. Smith, and K. Goldberg

toppling vertex function gives the location of the vertex with

tip, A respect to the toppling tip as a function of rotation
angle. The vertex function is given by:

inaccessible da — Yi
= 2£;-%,—-—— 28
region
a Ag See alig (28)
& y = Y- (29)
final where (2;,y;) is the location of the vertex in the fixed
orientation
frame of reference, (x;, y;) is the location of the vertex
initial
at the bottom of the edge that the toppling tip con-
orientation tacts, and 7; is the angle of that edge. We must check
what portion of the edge is visible from the +X direc-
tion without being blocked by the combination of the
Figure 8: Rotation of a part relative to the toppling tip A.
vertex function and the initial orientation of the edge.
Note that it is possible that lower edges and vertices
e;, final
may also block part of the final orientation of the edge,
B, orientation
and must be checked as well. This check insures that
vertex the contact point is not blocked by the part during
= SS ~ . function rotation.

Y ier aeVS e,, final The second consideration insures that the edge is
RS orientation moving out to the grasping tip at 0 = 6g. In other
curve separating BSR > words, the relative X displacement between A and B,
proceeding from denoted x4, must be greater than the X location (with
receding of edge )
respect to the toppling tip) of the part at dg for any
0 < 64. We will denote the location of the part at a
e, inital |______1f ORG aS height dg and angle @ as (z.,dg). Note that this is a
orientation ee tat
different physical point at each @.
At some critical height, h,, the derivative of x, with
Figure 9: A portion of the edge in the final orientation
respect to @ is 0. All of the physical points on that
may be blocked in the positive x direction before the part
edge below he will meet the requirement of moving out
reaches the final orientation. Additionally, a curve shows
to the part while those above will be receding. The ac-
the separation between where the part is moving forward and
cessibility constraint for this consideration is therefore,
where it 1s receding.
dp < £, on a given edge.
The relative X displacement of a point of the part at
part reached the desired orientation. Since this would height dg can be shown geometrically to be:
prevent the part from reaching the desired final orien-
tation, these heights are considered inaccessible. fe = 2;-2+————
= (30)
By examining the inaccessible region more closely, as
shown in Figure 9, we can see that there are two factors where (2;,y;) and (z;,y;) are the locations of the ver-
to be considered when determining the accessibility of tex at the bottom of the edge in contact with A and
an edge. The first factor is whether any portion of the the edge in contact with B respectively, and ~; and ~;
edge in the final orientation is blocked from visibility in are the angles of those edges. Therefore the derivative
the positive X direction by the part as it rotates. The of x, with respect to @ is given by:
second factor is what portion of the edge is moving
forward, in the +X direction, at the final orientation. dhe | > 1 dakelg
PT eres ae OS
The first factor is taken into account by calculating a Ae~%j; he - Yj
vertex function for the vertex at the top of edgee;. The
Compensatory Grasping with the Parallel Jaw Gripper
177
and must be 0 at he. Setting (31) equal to 0 and solving
for he yields:

he =(y; —y; — da — 2;a daAT — YiYj A Gej

( 7 tan yj sin?y, tany;

Yi 1 1
a gan Sipe pee 32
se apdAba are 2)

For a given edge, indicated by (x;,y;) and w,, only

heights less than h, can be considered when determin-
ing the height for B in the form closure analysis. A
corresponding procedure is used to determine a range
of possible dg, for a given dy.

5.2 Form-Closure

At the end of the accessibility considerations we know

da and dy, as well as ranges of possible values for dg
and dg. From these ranges we must determine values
such that the four tips generate form-closure on the Figure 10: Compensatory grasp experiment.
part. This is easily done using the method described
in van der Stappen [25]. This method would entail de- LA'B = 2.1cm. Figure 10 illustrates the compensatory
termining the point at which the edge normals through grasp.
A and A’ meet, then selecting the locations of B and
B’ such that the edge normals through B and B’ cre-
ate opposite moments about that point. We omit the 7 Discussion and Future Work
precise details for lack of space.
In industrial practice, gripper jaw geometry is often
custom-designed and machined for each part. De-
6 Physical Experiment sign has been ad-hoc and particularly challenging when
the part’s natural resting pose differs from the desired
We conducted a physical experiment using an grip/insertion pose. In this paper we describe compen-
AdeptOne industrial robot and a parallel jaw gripper satory grasps, a new approach to this problem where
with tips designed using the methodology described in 4 contact points on the jaws guide the part into align-
this paper. The part is a small lever from a standard ment and hold it stably. The next step is to develop
videotape (FUJI serial number: 7410161160). Its pla- more sophisticated jaw shapes based on part trajec-
nar convex hull is shown in Figure 3. The entire com- tory (see Figure 11) and to address shape and posi-
pensatory grasping is restricted in the X-Z plane due to tion uncertainty, friction, and ultimately, 3D geometry.
the mechanical and the geometric property of the part. We are also interested in more efficient algorithms and
As illustrated in Figure 1, the part begins at stable knowing under what conditions a compensatory grasp
orientation (a). Its desired orientation is (b) where exists.
§ = 37°. We choose A and A’ at d4 = 0.9 cm and dy,
= 2.0 cm, respectively. The corresponding friction cone Acknowledgments
half angles are a; = 5° and a» = 10°. When bee 31°;
V, is C and r9=4.1cm, z2=0.0cm, 2 = 56°, 7 = 46°, This paper grew out of some practical experiments with
p=2.2cm, and w = 53°; when @ > 37°, Ve is C and a commercial assembly. We would like to thank Brian
z2=4.6cm, 22=2.4cm, 2 = 92°, 7 = 55°, p=2.7cm, Carlisle and Randy Brost for suggesting such experi-
and w = 89°. The analysis yields the following tip ments, and Kevin Lynch for his elegant toppling analy-
values: dp = 2.7 cm, dg = 3.0 cm, z4g = 0.0 cm, and sis. We would also like to thank RP Berretty and Mark
178 T. Zhang, G. Smith, and K. Goldberg

[9] Michael A. Peshkin and Art C. Sanderson. The mo-

tion of a pushed, sliding workpiece. IEEE Journal of
Robotics and Automation, 4(6), December 1988.

[10] T. Abell and M. Erdmann. Stably supported rota-

tions of a planar polygon with two frictionless con-
tacts. In IEEE/RSJ International Conference on In-
telligent Robots and Systems, 1995.

[11 N. Zumel and M. Erdmann. Nonprehensile two

plam manipulation with non-equilibrium transitions
between stable states. In IEEE International Confer-
ence on Robotics and Automation, 1996.

Figure 11: Gripper Jaws based on compensatory grasp tips

[12] N. Zumel and M. Erdmann. Nonprehensile manipu-
lation for orienting parts in the plane. In [EEE In-
and complement of swept volume. ternational Conference on Robotics and Automation,
1997.

Overmars for their contributions to our thinking about [13] D. Rus. Dextrous rotations of polyhedra. In [EEE In-
the toppling graph. ternational Conference on Robotics and Automation,
May 1992.
This work was supported in part by the National Sci-
ence Foundation under CDA-9726389 and Presidential [14] J. Hong, G. Lafferriere, B. Mishra, and X. Tan. Fine
Faculty Fellow Award IRI-9553197. manipulation with multifinger hands. In International
Conference on Robotics and Automation. IEEE, May
1990.
References
[15] Ronald S. Fearing. Simplified grasping and manipu-
lation with dextrous robot hands. IEEE Journal of
[1] K. Goldberg, B. Mirtich, Y. Zhuang, J. Craig,
Robotics and Automation, RA-2(4), December 1986.
B. Carlisle, and J. Canny. Part pose statistics: Es-
timators and experiments. JEEE Transactions on Ro-
botics and Automation. 15(5), pages 849-857, 1999. [16] A. Bicchi and R. Sorrentino. Dextrous manipulation
through rolling. In International Conference on Ro-
G. Causey and Roger Quinn. Design of a flexible parts botics and Automation. IEEE, 1995.
feeding system. In [EEE International Conference on
Robotics and Automation, 1997. [17] A. Rao, D. Kriegman, and K. Goldberg. Complete
algorithms for feeding polyhedral parts using pivot
Jocelyn Pertin-Troccaz. Grasping: A state of the art. grasps. IFEE Transactions on Robotics and Automa-
In The Robotics Review I, pages 71-98. MIT Press, tion, 12(6):331-42, April 1996.
1989.
[4= Xanthippi Markenscoff, Luqun Ni, and Christos H. Pa-
[18] R. Zhang and K. Gupta. Automatic orienting of poly-
hedral through step devices. In IEEE International
padimitriou. The geometry of grasping. International Conference on Robotics and Automation, 1998.
Journal of Robotics Research, 9(1), February 1990.

[5 Matt Mason and J. Salisbury. Robotic Hands and Me- [19] Y. Yu, K. Fukuda, and S. Tsujio. Estimation of mass
cod
and center of mass of graspless and shape-unknown
chanics of Manipulation. MIT Press, 1985.
object. In IEEE International Conference on Robotics
(6 — Randy C. Brost. Automatic grasp planning in the and Automation, 1999.
presence of uncertainty. The International Journal of
Robotics Research, December 1988. [20] Kevin Lynch. Toppling manipulation. In [EEE In-
ternational Conference on Robotics and Automation,
[7] John Canny and Ken Goldberg. A risc approach to 1999.
sensing and manipulation. Journal of Robotic Systems,
12(6):351-362, June 1995. [21] Tao Zhang, Gordon Smith, Robert-Paul Berretty,
Mark Overmars, and Ken Goldberg. The toppling
J. C. Trinkle and R. P. Paul. Planning for dex- graph: Designing pin sequences for part feeding. In
trous manipulation with sliding contacts. Interna- IEEE International Conference on Robotics and Au-
tional Journal of Robotics Research, June 1990. tomation, 2000.
Compensatory Grasping with the Parallel Jaw Gripper 179

[22] Ken Goldberg. Orienting polygonal parts without sen- [24] Matt Mason. Two graphical methods for planar con-
sors. Algorithmica, 10(2):201-225, August 1993. Spe- tact. problems. In IEEE/RSJ International Workshop
cial Issue on Computational Robotics. on Intelligent Robots and Systems, 1991.

[23] Tao Zhang, Gordon Smith, and Ken Goldberg. Com- [25] F. van der Stappen, C. Wentink, and M. Overmars.
pensatory grasping with the parallel-jaw gripper. Computing form-closure configurations. In [EEE In-
Technical Report, ALPHA Lab, UC Berkeley, 2000. ternational Conference on Robotics and Automation,
1999.
 enc) yevitigk? «athtose sire
hbeibagh

tare
fn wy
| wv. ; PY 2s “Teg :
Y=: Poel Es itty
s<cquistara nit
» io bc Npfeeanit- ie,

— ° eele asin ane

; e a oe | —S> V1 Pe gree
i a i oe ‘ = Ow rea
o o

Pe oot Tak Wire ‘Artin ; Li@ > ign ‘ ‘ : ~ 17 ae’ CV) ro gy

yy) jv * < on Ve
[ ‘ ‘ :
wtlw tac
*
oy thy A A ne

pee tid Pest bite Latkes, & (fare, aoa ee F

rer iL at :07 » TT ify a osoig~ tis ny? oi
: Se Le eae.
—_. —. ane oe

i 7
jw
Ra we
a Peeriteae (PE ry & ‘

W Atlan Ais, (86, om | yt ?

MM te
me Ts
aed Diers Gin. Brees « Op tay i ia

reer ; Ff in ‘ Tee IEA is ae revs five t ' 4

ent we: weed, vs mu, 4)

ator?) P
Perqin. — Ligieabans, ‘i wi avy ar a ~
“aa te Ab rticn ‘ii@Aive } te 4
tom, ' La

7 Piette Markco (oye : a wa ~?

- pen : 2b prexteey, ~ irre ~ 4
ly
ws 2. i = «hf i

a J
hs e » m)"* ] . ' ’ T

; - ,a 2 Paw; a * sf aha —_ ¥

yes a WS pe j a aan

o- a
?- — , Pr a
e

% *
_ ‘
‘=
ee. - on ‘
a. ae

te riruern, Siad
2-1, Ueuihy oe A
Pe dite: Ganel ee
ton (, WEAR), Sone
_ oe
—
Optimal Planning for Coordinated Vehicles
with Bounded Curvature
Antonio Bicchi Centro “E. Piaggio,” University of Pisa, Pisa, Italy
Lucia Pallottino Centro “E. Piaggio,” University of Pisa, Pisa, Italy

In this paper we consider the problem of planning The paper is organized as follows. In Section 2 we
motions of a system of multiple vehicles moving in a describe the problem and introduce some notation. In
plane. Each vehicle is modelled as a kinematic system Section 2.1 a formulation of the problem in a form
with velocity constraints and curvature bounds. Vehi- amenable to application of optimal control theory is
cles can not get closer to each other than a predefined presented. Section 3 is devoted to the study of nec-
safety distance. For such system of multiple vehicles, essary conditions for extremal arcs. Finally, Section 4
we consider the problem of planning optimal paths in describes a numeric algorithm to find solutions, which
the absence of obstacles. The case when a constant applies under some restrictions.
distance between vehicles is enforced (such as when co-
operative manipulation of objects is performed by the 2 Problem Statement
vehicle team) is also considered.
Consider N vehicles in the plane, whose individual con-
1 Introduction figuration is described by q; = (2, yi,0:) € Rx Rx S!,
with (2;,y;) coordinates in a fixed planar reference
In this paper we consider the problem of planning mo- frame (0,z,y) and 0; the heading angle of the vehi-
tions of a system of multiple vehicles moving in a plane. cle with respect to the x axis. Each vehicle is assigned
The motions of each vehicle are subject to some con- a task. In order to compute its task a vehicle starts
straints: The velocity of the center of the vehicle is par- in a configuration g;,, and moves in a final configura-
allel to an axis fixed on the vehicle; the velocity is con- tion gi,g. We call these two particular configurations
stant along such axis; the steering radius is bounded. way-points. The initial way—point time is assigned
Also, a minimum distance between vehicles must be and denoted by T?. Assume vehicles are ordered such
enforced along trajectories. that T? < Tg < --- < Tg. We denote by T? the
time at which the i-th vehicle reaches its goal, and let
The task of each vehicle is to reach a given goal con-
T; os T? —T;. Motions of the i-th vehicle before T?
figuration from a given start configuration. We con-
sider optimal solutions in the sense of minimizing total
and after T? are not of interest.
completion time. The i-th vehicle motion is subject to the constraint
that its transverse velocity is zero, @; sin 8; — y; cos 0; =
The literature on optimal path planning for vehicles
0,i=1,...,N. Equivalently, this motion is described
of this type is very rich. The seminal work of Dubins
by the control system q; = fi (qi, ui,wi). More explic-
[4] and the extension to vehicles that can back-up due
itly:
to Reeds and Shepp [6], solved the single vehicle case
Li U; Cos 8;
by exploiting specialized tools. Later on, Sussmann
Ue) 1 le, ei Oe | (1)
and Tang [7], and Boissonnat et al. [2], reinterpreted
0; Wj
these results as an application of Pontryagin’s mini-
mum principle [5]. Using these tools, Bui et al. [3] where u; and w, are the linear and angular velocity of
performed a complete optimal path synthesis for Du- the i-th vehicle, respectively. All vehicles are subject
bins robots. The minimum principle framework used to the following additional constraints:
in these previous works is also fundamental in the de-
velopments presented here. i) the linear velocity is constant: u; = Wi;
182 A. Bicchi and L. Pallottino

ii) the path curvature is bounded: |w;| < 9;, where define the admissible control sets accord-
0; = FF and R; > 0 denotes the minimum turning ingly. In addition we define the separation vector
radius of the i-th vehicle; D= [Dy2, ee » Din, Das, coe ,Dn-1,N]; and the vector
field f(q,u,w)= col, (fiwi). Finally introduce ma-
iii) the distance between two vehicles must remain trices [; ==eoly.a (Oa; (2 1 1]"), with oj, =1ifi=j,
larger than, or equal to, a given separation limit: else oj;= 0, ad functions 7;(q(t),@) = T: (g(t) — @).
Dij(t) = (x5 (t) —a4(€))?
+(y5(€)—ys(t))? — a3, = 0, Our optimal control problem is then formulated as
at all times ¢ (di, =0,2= 1,45, NY"

Problem 1. Minimize J subject to g = f(qg,u,),

We consider problems in which the goal is to mini-
w EQ, D>0, and to the two sets of N interior-point
mize the total execution time:
constraints:
min 1B
yi(a(t),¢?) =0, t=T?
G = fg, H,u,) t= 1). NN yi(q(t),q7) =0, t = T? (unspecified)
Ee ae ee ae (2)
ll <
D; Ha) 0: Vt, Bad in fos
3 Necessary Conditions
ai(T? ) = Gis; q(T? ) = Gig-
Necessary conditions for Problem 1 can be studied by
If separation constraints are disregarded, the mini- adjoining the cost function with the constraints multi-
mum total time problem is clearly equivalent to N in- plied by unspecified Lagrange covectors. Omitting to
dependent minimum length problems under the above write explicitly the extents of iterative operations when
constraints, i.e. to N classical Dubins’ problems, extending from 1 to N, let:
for which solutions are well known in the literature
([4, 7, 2]). It should be noted that computation of J= VimnaT) —4@)
the Dubins solution for any two given configurations is +3, 77 1(a(T?) — 98) (4)
computationally very efficient. 2 iy Sry beteel Be wie sila8 >
2.1 Formulation as an Optimal Control Prob- with A and v costates of suitable dimension, and with
lem y,=O0if D; > 0,4%;> 0if D; = 0. Let the Hamiltonian
be defined as:
The cost for the total time problem, J = Sp Woy ee
g
Se (ie dt, is not in the standard Bolza form. In or- H=) wit AT ft+v7D (5)
der to use powerful results from optimal control theory,
we rewrite the problem as follows. Let h(t) denote the Substituting Equation 5 in Equation 4, integrating by
Heavyside function, i.e.: parts, and computing the variation of the cost, we get:

b= 3, [P(TF-) — APT) +4 math|dq(T})

A
Omid
nie) = {1s espe, ?
+0; [APLP") — AT (TP) + 1 5s] d(T?)
and define the window function w;(t) = h(t—-T3)—h(t—
T?). Then the minimum total time cost is written as:
+3; |H(E”) - HEP) + 09 336] a9
+S” (7 + aH) dq+ 3H5.0]dt
oo. Ni
Yaoi SSS (3) (6)
(recall that dT = 0). Therefore, we have the following
0 G=1 necessary conditions for an extremal solution:

Using the notation N (vj)

col;_, nen Type a:
= leseee Onn
define the aggregated state q = cole1(@ " con- ce ole:S =: I oe —a © 4
trols u = colj, (a) and w = col}_,(w;), and
Optimal Planning for Coordinated Vehicles with Bounded Curvature
183
= OH In such an interval, the vehicle moves on the straight
Ane Fe hac (10)
route (the supporting line) in the horizontal x, y plane
OH
a5 described in Equation 13. Other extremals of H; oc-
6 = 0 Vdw admis:s. (11) cur at w = +t;/R. The sign of the minimizing yaw
Extremal trajectories for the i-th vehicle will be com- rate w; is opposite to that of A;3. In other words,
prised in general of unconstrained arcs (with Dere0; the supporting line also represent the switching locus
for the yaw rate input. Trajectories corresponding to
Vj # 7) and of constrained arcs, where the constraint
w; = +u;/R correspond to circles of minimum radius
is marginally satisfied (4j : Dj; = 0). We will proceed
the discussion of necessary conditions by distinguishing R followed counterclockwise or clockwise, respectively.
constrained and unconstrained arcs. It is important for our further developments to note
that, along extremal arcs, the costates are completely
3.1 Unconstrained Arcs
determined by boundary configurations up to a multi-
plicative constant p 4 0, which remains undetermined.
Suppose that, for the i-th vehicle, the separation con- For each vehicle, extremal unconstrained arcs are
straints are not active in the interior of an interval concatenations of only two types of elementary arcs:
fee Pee Tic. Dt) oS) 0 line segments of the supporting line (denoted as “S”),
,N, te (12,2). Expanding Equation 10, we get: and circular arcs of minimum radius (denoted by “C”).
The latter type can be further distinguished between
[Jans Aias As3| = [0, 0, Ak,1%; sin 8; — Ajo%;
cos Hj]. (12) “R” clockwise arcs (w; = t;/R), and “L” counterclock-
wise arcs (Ww; = —t;/R). We will use subscripts to de-
The characterization of optimal solutions in the un- note the length of rectilinear segments and the angular
constrained case proceeds along the lines of the clas- span of circular arcs.
sical Dubins solution (see [4, 7, 2]). We report some Switchings of w; among 0, u;/R, and —u;/R can only
results here for reader’s convenience. By integrat- occur when the vehicle center is on the supporting line.
ing Equation 12 one gets A(t? < t < t?)= Xu, As a consequence, all extremal unconstrained paths of
ee te ) — he, and Ag(t? <t-< 2) = each vehicle are written as Cy, Sa,CuzSdp°+* SdnCu n?
Airyi(t)—A2z;i(t) + Az, with constant A;,;, 7 = 1,2,3. with u; = 2kz, k integer, 1 = 2,...,n —1.
In light of these relationships, the conditions in Equa-
In the case of a single vehicle, the discussion of opti-
tions 7 and 8 state that the costate components \j1
mal unconstrained arcs can be further refined by sev-
and Aj2 are piecewise constant, with jumps possibly
eral geometric arguments, for which the reader is re-
at the start and arrival time of the i-th vehicle. The
ferred directly to the literature [4, 7, 2]. Optimal paths
addend in the Hamiltonian relative to the i-th vehicle
necessarily belong to either of two path types in the
can be written as H; = 1+ up; cos(6; — Wi) + Aig Nii,
Dubins’ sufficient family:
where p; = qf At So Nes and ~; = atan2 (Aj2, \i1). From
Pontryagin’s Minimum Principle (PMP), we know that (CaCeCe ’ CySaGu} (14)

H;(t) = const. < 0 along extremal unconstrained arcs with the restriction that:
and, being by assumption the way-points configura-
tions unconstrained, it follows from Equation 9 that b€ (rR, 27R); a,e € [0,5], u,v € [0,27R), d>O0 (15)
H;(t) is also continuous at t = TY.
A complete synthesis of optimal paths for a single
Extremals of H; within the open segment {|w;| < Dubins vehicle is reported in [3]. The length of Du-
u;/R} can only be obtained if: bins paths between two configurations, denoted by
Lp(é?,€7), is then unique and defines a metric on
: — ee Air yi(t) _ ri2i(t) + V3 =)())) (13) IR? x S!. One simply has Lp(-,-) = R(|a| + |b| + lel)
for a C,0pC. path, and Lp(-,-) = R(|u| + |v|) +d for
a CaS aC» pathi
If the condition holds on a time interval of non-zero
measure, then A;,3 = 0 on the interval. This implies In our multivehicle problem, however, other extremal
pitt; sin(O; — Wi)= 0, hence 6;= 7; mod and w; = 0. paths may turn out to be optimal, and therefore have
 A. Bicchi and L. Pallottino
184

to be considered. This may happen for instance for a

path of type CaSpC2krSeCyf iff the corresponding Du-
bins’ path C..$,4¢C (which is shorter), is not collision
free. Arcs of the type Coz, can be interpreted as a cir-
cling maneuver that allows another vehicle to pass by
and avoids collision!. Note explicitly that the length
of two subpaths of type ---Cu,SaCornSeCu,,,°°* and
-++ Cy, SyCokr55Cu,41 °** are equivalent as long as a+
Breage:
By “extremal trajectory” (Dubins’ trajectory, re-
spectively) we indicate henceforth a map Rt 4 R?
defined by (z?(t), yP(t)), denoting the position of the
i-th vehicle at time t along an extremal (Dubins’) path
connecting g# to qf.
Figure 1: Possible constrained arcs for two vehicles with
Remark 1. If a set of non—colliding Dubins’ tra-
the same velocity.
jectories exists, then this is obviously a solution of the
minimum total time problem. More interestingly, if
there is a collision, the optimal solution will contain at When the constraint is active, the two vehicle envelopes
least a constrained arc or at least one wait circle. are in contact, and the relative orientation of the two
vehicles must satisfy Equation 19, which defines (for
8.2 Constrained Arcs given %1, &2) two manifolds of solutions in the space
{(01,
02,¢) € S1 x S* x S1} described as:
Some further manipulation of the cost function is in-
strumental to deal with constrained arcs, i.e. arcs in
which at least two vehicles are exactly at the critical a) 65 @ + arccos (2 cos(¢ — a)): of2)
separation (D;; = 0, 1 # j). To simplify, consider a
constrained arc involving only vehicles 1 and 2. Along b) 6 @ — arccos (2 cos(¢ — a1))pei
a constrained arc, the derivatives of the constraint must 2
vanish:
The two solutions correspond to two different types
Bele ae (“a” and “b”) of relative configurations in contact. For
5 |‘brialhe instance, for: %; = tz, one has:
| (zo — 21)? + (yo -—y1)? — @ Safi
2(t2 — £1)(fo — £1) + 2(yo — 1)(H2 — 1) | a) 63 = 41; (22)
(16) b) Go eran (23)
with d = diz . Let ¢ be the direction of the segment
joining the two vehicles, so that: In case a) the two vehicles have the same direction,
Lo — £1 =dcos¢, while in case b) directions are symmetric with respect
‘ 1 to the segment joining the vehicles (see Figure 3.2).
yo — yi = dsin ¢, ov
The two solutions of Equations 20, 21 coincide for:
From the second Equation in 16, we get:

(xq — 21) (U2 cos Og — Ui cos 6,)+ @ = 0; + arccos (2) ’ (24)

(Yo = Yi) (te sin A = ii sin 61) = (0) (18)
Ui

and, using Equation 17, such a ¢ exists only if = <1. If we find the solution
of Equation 19 in 6¢ and 68, the solutions coincide for:
U1 cos(@ — 61) — U2 cos(d — 62) = 0. (19)
‘This is similar to the current practice in conflict reso-
lution for air traffic control © = 62 + arccos (=) é (25)
U2
Optimal Planning for Coordinated Vehicles with Bounded Curvature
185
@ exists if 2 < 1. Hence, from Equations 24 and
; ae a :
25, @ exists if Uj = Up. In this case the solution of
Equation 19 is:

In order to study constrained arcs of extremal solu-

tions, it is useful to rewrite the cost function given by
Equation 4 as:

J= Pin
+ >0, 72% (a(T?) — a2)
+ 0,72 1s(a(TY) — 4?) ve)
A tae wi +AT(G— f) + uDyodt,

with y > 0 along a constrained arc. The jump condi- Figure 2: A numerically computed solution to a two-
tions at the entry point of a constrained arc, occurring vehicles minimum total time problem. Vehicles are repre-
at time 7, are now: sented as aircraft. Minimum curvature circles are reported
at the start and goal configurations, along with safety discs
of radius d/2 (dashed). The unconstrained Dubins’ paths
= — my |
4
(thin lines) would achieve a cost of 88.75 units, but collide
in this case (collisions are marked by “+” signs). The op-
timal solution consists of two unconstrained arcs for each
vehicle, pieced together with a zero—length constrained arc
of type b. Total cost is 92.25 units.
where H = 3), w; + ATf+v7T Dyp, and:
form:
(a1 — 22) %,cos0, — tig cos Ag
(yi —Yy2) wt sind, — tig sin 2
(XY -2 0 di, sin(d — 61)
Oq : (xo aad £1) U2 COS Og -—- 1 cos 01 A(qe;¢e) |°?47
(y2 = Yi) U2 sin Og — Uy; sin 6;
0 —dt2 sin(d — 62)

A further distinction among constrained arcs of zero where the explicit expression of matrix A(q-,¢c), for
and nonzero length should be done at this point. each contact type, can be easily evaluated in terms of
ai, G1, G5 G5, and is omitted here for space limitations.
3.2.1 Constrained Arcs of Zero Length Non-triviality of costates implies that (q., @-) must sat-
isfy Equation det(A) = 0. A further constraint on
Consider first a constrained arc of zero length occur- contact configurations is implied by the equality of de-
ring at a generic contact configuration. This is com- placement times from start to contact for the vehicles,
pletely described by the configuration of one vehicle which is expressed in terms of Dubins distances as:
(€.g., dc = 1), by the angle ¢. = ¢, and by the con-
tact type. Assume for the moment that there is only Lp(&j, &c)/ta ~ Lp (3, &¢)/ua,
one constrained arc of zero length in the optimal path
between way-points of the two vehicles. Equation 27, where €/, denotes the configuration of vehicle 2 at con-
taking into account that costates of each vehicle are tact. This is uniquely determined for each contact type.
determined (once the way-points and contact configu- If m constrained arcs of zero length are present in an
rations are fixed) up to constants p;(r~), pi(T*), pro- optimal solution, similar conditions apply (with way-
vides a system of 6 equations in 6 unknowns of the points configurations suitably replaced by previous or
186 A. Bicchi and L. Pallottino

successive contact configurations), yielding 2m equa-

tions in 4m unknowns.
yo

3.2.2 Constrained Arcs of Nonzero Length ( Ve E: ae

From this point on, we will make the assumption

that forward velocity of all vehicles are equal to 1
(u; = 1). Consider an interval [T; , T2] during which the —

constraint Dj. = 0.2 A configuration of the two vehi-

cles along such constrained arcs can be completely de-
(¥ ees
1+ ———

scribed by using only four parameters, for instance the 4 3

4
2 1
re 1
0 1 2
1
3
n
4
4
5

configuration (21, y1, 41) of one vehicle and the value of

@. Due to the tangency conditions on the constraint, Figure 3: Extremal constrained arcs of type a consist of
the configuration is described by Equation 17 and one two copies of a Dubins’ path.
of Equations 22 or 23. Moreover, differentiating Equa-
tion 17, one finds:
Let us consider the two types of constrained arcs sep-
to = £, — ddbsing, arately. Notice that two extremal constrained arcs of
, : : 30 different type may be pieced together through a con-
Yo = 91 + d¢cos @, 80)
figuration with 6; = 92 = ¢, which is both of type
and ; a and b. Type a) From Equation 31, ¢(t) = ¢0 =
0
y2(0)—yi (0 hence: .
o= qisin( — ) —sin(4; — ¢)]. (31) arctan a 0)—a
700)?

Differentiating twice D;,2 we obtain:

L1 = £2
Yi = Ye (34)
Wy =—W2
Die(a, WwW, t) ==i() =
4— 4cos(01 — 02) + 2widsin(@1 — g) = 2w2d sin(O2 — dg).
Extremal constrained arcs of type a consist of a Dubins
(32) path for vehicle 1, and of a copy of the same path
translated in the plane by [dcos ¢o, dsin do]? for the
Constrained arcs of nonzero length that are part other vehicle (see Figure 3).
of an optimal solution must themselves satisfy neces-
sary conditions, which can be deduced by rewriting the Type b) In this case, using Equation 32, one ob-
problem in terms of the reduced set of variables. tains ¢ = $(wi + w2) in Equation 33. Introduce
A= (1, A2, A3, A4), and H = 2+A1 cos 6; +2 sin 6,+
A3w1 + A4(w1 + w2)/2. Necessary conditions for opti-
£; = cos 0; mality of solutions of Equation 33 are:
yi; = sin 0;
é,— i; (33)
0 0 -—siné; 0
@ = +[sin(62 — ¢) — sin( — 4)] <a 8 0 0 cosh; O
A=-—A 0 0 0 0 |> (35)
Wh E Q), we € Qs
or 0 0
for i = 1 or i = 2, and for some initial and final spec-
ification of the variables (x;,y;,0;,¢) and of the con- Hence, A;, A2 and A4 are constant. Letting A; = pcosw
strained arc type (a or b). Recall that 6 = 6, (arcs of and A2 = psiny, from Equation 35 we get:
type a), or 02 = 2g — 9, (type b).
3 = psin(O, — wp). (36)
“It should be pointed out that the study of constrained
arcs of nonzero length is also useful to model cooperative
From P.M.P one also gets that, when |w| < ; and
manipulation of object by multiple vehicles, assuming that
each vehicle supports the common load through a hinge |w2| < Qe, it is necessary for an optimal arc that 22
Ow,
—
joint. gH = 0, which implies A3 = \4 = 0. In this case, from
Optimal Planning for Coordinated Vehicles with Bounded Curvature
187

and:

Mi(q) = min{Q.2 — =sin(9, =o ai

we have that along constrained arcs of type b we must

have m1 < w, < M,. Control w, may equal M, if
Oye ee) SOL If pte (ote 4 sin(0; — ). In the latter
case, W2 = —Qa. A similar reasoning applies to vehicle
two, for which we get mz < we < Mo. In conclusion,
along a nonsingular extremal constrained arc of type
b, one of the vehicles moves along a circle of minimum
Figure 4: Singular extremals in a constrained arc of type b. radius, while the other follows a curve such as that
described in Figure 5.
\3 = O0one easily gets 0; = wt, w, = 0. The direction
of the segment joining the two vehicles varies as: 4 Numerical Computation of Solutions
o) = 2 sin(¢ — 6;)

4(0) = do Bn The necessary conditions studied in this paper provide

useful hints in the search for an optimal solution to
Equilibria of Equation 37 at ¢ = 0, and ¢@ = 6; — the problem of planning trajectories of N vehicles in
m@ are respectively unstable and asymptotically stable. a common workspace. Although a complete synthe-
Hence, along a singular constrained arc of type b, one sis has not been obtained so far, we will describe an
vehicle will be moving on a straight line, while the other algorithm that finds efficient solutions to the optimal
will be trailing behind (see Figure 4). planning problem in a reasonably short time.
Extremal constrained arcs may also obtain when a Based on the discussion above, the optimal conflict
control variable is on the border of its domain, e.g. resolution paths for multiple vehicles may include mul-
Wy, = +. In this case the motion of the two vehicles tiple waiting circles and constrained arcs of both zero
result in arcs such as those represented in Figure 5. and nonzero length. The algorithm presented in this
Notice that along such an arc, the steering velocity section was developed to solve air traffic control prob-
of vehicle 2 is uniquely determined by Equation 32. lems [1], and is based on a few simplifying assump-
Hence, since |w2| is bounded by Qe, an arc of maximum tions motivated by the application. Namely, we ass-
curvature for vehicle one will be possible only until the sume henceforth that
limit curvature for vehicle two is reached. In other
words, letting: hi all vehicles have equal geometric characteristics
and equal (constant) speed;
my1(q) = max{—Q2 — =sin(6, — ~),-94},
h2 constrained arcs of nonzero length are not consid-
ered;

h3 multiple zero—length constrained arcs among the

same vehicles are ruled out;

h4 the initial configurations of the vehicles are suffi-

ciently separated.

Assumption h4 implies that for each vehicle, the ini-

tial configuration are collision free and guarantee that
wait circles at the initial configuration are collision
free (this holds for instance if the distance between
the initial position of vehicles i and j is larger than
InR2t + 2R + SH).
Figure 5: An extremal constrained arc of type b.
188 A. Bicchi and L. Pallottino

Consider first the case of two vehicles. If the Du-

bins’ trajectories joining the way-points configurations
do not collide (i.e., D(t) > 0,Vt), this is the optimal
solution. Otherwise we compute the shortest contact—
free solution with wait circles at the initial configura-
tions, and let its length be Ly.
Hence we look for a solution with a concatenation
of two Dubins’ paths and a single constrained zero—
length arc of either type a) or b) for both vehicles.
Such a solution can be obtained by searching over a
2—dimensional submanifold of the contact configura-
tion space (IR? x S! x $'). The optimal solution can
be obtained by using any of several available numeri-
cal constrained optimization routines. Computation is
sped up considerably by using very efficient algorithms
made available for evaluating Dubins’ paths ([3]). The
lenght LD. of such solution is compared with Ly, and
the shorter solution is retained as the two—vehicle op-
timal conflict management path with at most a sin-
gle constrained zero-length arc (OCMP21, for short). Figure 6: Numerically computed solutions to optimal co-
Some examples of OCMP21 solutions are reported in operative conflict resolution for two vehicles. Minimum
Figure 6. curvature circles are reported at the way-point configura-
tions, along with safety discs of radius d/2 (dashed). Opti-
If N vehicles move in a shared workspace, their pos-
mal solutions consist of two unconstrained Dubins’ trajec-
sible conflicts can be managed with the following mul-
tories for each vehicle, pieced together with a zero—length
tilevel policy:
constrained arc.

Level 0 Consider the unconstrained Dubins paths of

all vehicles, which may be regarded as N single— the shortest path obtained at level m — 1, exit.
vehicle, optimal conflict management paths, or Otherwise, continue.
OCMP10. If no collision occurs, the global op-
timum is achieved, and the algorithm stops. Oth- A few three—vehicle conflict resolution trajectories at
erwise compute the shortest contact—free paths different levels are reported in Figure 7.
(with wait circles) and go to next level; When the number of vehicles increases, the number
of optimization problems to be solved grows combina-
N torially. However, in practice, it is hardly to be ex-
Level 1 Consider the M = 2 possible solu-
2 pected that conflicts between more than a few vehicles
tions with a single contact (of either type a or b), at a time have to be managed. It was also observed
between two vehicles, and possibly wait circles for in our simulations that, for vehicle parameters close to
other vehicles, and compute the shortest path in those of the kinematic model of commercial aircraft,
this class. If this is longer than the shortest path solutions including wait circles are very rare.
obtained at level 0, exit. Otherwise, continue;
5 Conclusion

Level m > 2. Consider the M []/";'(M —2*) possible In this paper, we have studied the problem of plan-
solutions involving m zero—length constrained arcs ning trajectories of multiple Dubins’ vehicles in a plane.
between different pairs of vehicles and (possibly) Necessary conditions have been derived for both free
wait circles for other vehicles, and compute the and constrained arcs. An algorithm for numerically
shortest path in this class. If this is longer than finding solutions has been described.
Optimal Planning for Coordinated Vehicles with Bounded Curvature
189

Acknowledgments
Authors wish to thank Gianfranco Parlangeli for help
in an early phase of this work, and Jean-Paul Laumond
for his useful remarks and suggestions on a previous
version of this paper. This work was done with the
support of NATO (CR no. 960750), ASI (ARS-96-170),
and MURST (project RAMSETE).

References

[1] A. Bicchi, A. Marigo, G. Pappas, M. Pardini, G. Par-

langeli, C. Tomlin, and S. S. Sastry. Decentralized air
trafic management systems:performance and fault tol-
erance. In Proc. Int. Works. on Motion Control, pages
279-284. IFAC, 1998.

[2] J.D. Boissonnat, A. Cerezo, and J. Leblond. Shortest

paths of bounded curvature in the plane. In Proc. Int.
Conf. on Robotics and Automation, pages 2315-2320.
IKEE, 1992.
Figure 7: Four cases of three-vehicles conflict resolution.
[3] X.N. Bui, P. Souéres, J-D. Boissonnat, and J-P.
Up left: the conflict is resolved at level 0. Up-right: a level Laumond. Shortest path synthesis for Dubins non—
1 solution. Down left: a level 2 solution whereby the ve- holonomic robots. In Proc. Int. Conf. on Robotics and
hicle starting in the middle contacts first the one arriving Automation, pages 2-7. IEEE, 1994.
on its right, and after the one arriving from left. Down [4] L. E. Dubins. On curves of minimal length with a con-
right: a level 2 resolution that generates a roundabout-like straint on average curvature and with prescribed initial
maneuver. and terminal positions and tangent. American Journal
of Mathematics, 79:497-516, 1957.

[5] L. S. Pontryagin, V.G. Boltianskii, R.V. Gamkrelidze,

Future work on this topic will address the problem
and E.F. Mishenko. The mathematical Theory of Opti-
of finding a complete optimal synthesis at least for the mal Processes. Interscience Publishers, 1962.
simplest cases (N = 2). Further refinement of the al-
gorithm will be sought, that could exploit more of the [6] J. A. Reeds and R. A. Shepp. Optimal paths for a car
that goes both forward and backward. Pacific Journal
rich structure optimal solutions must satisfy. Finally, of Mathematics, 145(2), 1990.
optimal paths of multiple agents at fixed distance will
be studied in more detail, to address such problems as [7] H. J. Sussmann and G. Tang. Shortest paths for the
cooperative manipulation of objects by robotic vehicles Reeds-Shepp car: a worked out example of the use
of geometric techniques in nonlinear optimal control.
and formation flight planning. Technical Report 91-10, SYCON, 1991.
 prereset i iteievy
\ ,
f FAY ta ro gey
i d

rT Aye

7 . ) Tess) heer
’

¢ oT

Soh aH
?

a! passin ‘gh y
ae ee ee

a dine
Sal meet ah al! 6", ~~
oo, » x.

1 Mig! ma ¥ 5
~.

asthin Mrseentcd) Wr gt
wi e

Waar
le lnies! YW aie i]

7.
id Tete asi nd’, fen he i
—) =)
1 fe oe, oh Tyhi
’ ea aon Se ee, e)
Witgemelwrare Walid cod nitoe art
: Wing. ‘we ii c mayan? Yewobh digerahdy 4 van 40 wien es
ee fe
MILD math ii]
i ee EP ee
oan! oat 4 . ae? fC. lfisneceht4,
QU Dts. GAT Ill) gigi b ey iF
> A, - ras S a
vires)
ors
|.
A
i
t
ee yn J
_
Paris i. i Li! enh HT VGei "i. Sree glad
etek hired baer ine Sar
,
ny p ta Wey Ayes ‘, = difwil Ab ah
Lsetenad fist «2 eee MAO Di
™ VWa, A? itt o sires) Wot Vi

ie tan ah Ite #2 Gey. “ ifs

<< =
pe Mae an ah ninetagt F SS nag | peslcte
ny ae) nome lace Set opigath guts | ra) 1
bynd Sea et a <7 wl 0D Need ee aiatinge lamnrtign ae
ij ay Ww sé rita wird (v Wire
oats ' " LA KS hi: Routt ) * Wy
i) i if
Mpaew lest Din Vin, WA) ny ad
ST Races: tity: war Salle oho
ae Wipe hab ovina ain ot SSS Heh Ae roads
Wee us fi |
i ‘ j Py {live doggendnlls Byatt babs outshogs Neg Mag
Mee onal 1) bie nonce | it?
‘_ at
ne smell rage Wie corte iad, Update
AAR due Wwahein a re eds abs) s
ce titiw sitpos ttl agne do
ee le ran mp irik) Imei, © aw vted China
PE) ALY IY? Oa 30 Nradest faninyon’l
> =

®& _— ©

- at

ae

; 2 .
:

- e we

mm.

oe
An Efficient Approximation Algorithm
for Weighted Region Shortest Path Problem
John Reif, Duke University, Durham, NC
Zheng Sun, Duke University, Durham, NC

In this paper we present an approximation algorithm

1 Introduction
for solving the following optimal motion planning prob-
lem: Given a planar space composed of triangular re- 1.1 Definition of the Problem
gions, each of which is associated with a positive unit
weight, and two points s and t, find a path from s to t Robotics motion planning problems are some of the
with the minimum weight, where the weight of a path most fundamental problems in robotics research. An
is defined to be the weighted sum of the lengths of the important category of these problems is to determine
sub-paths within each region. the optimal path (shortest path according to a user-
defined metric on paths) between two points, s and t,
Some prior algorithms (Lanthier et al. [9] and Alek-
in a 2D or 3D space under various conditions. This
sandrov et al. [1]) took a discretization approach by in-
problem has drawn great attention from researchers in
troducing m Steiner points on each edge. A discrete
robotics as well as other areas such as computational
graph is constructed by adding edges connecting Steiner
geometry, geographical information systems (GIS), and
points in the same triangular region and an optimal
graph theory, due to its direct applications. There have
path is computed in the resulting discrete graph us-
been numerous papers on the optimal path problem.
ing Dijkstra’s algorithm. To avoid high time complez-
Interested readers may refer to survey [11] and [12] for
ity, both [9] and [1] use a subgraph of the complete
a comprehensive review.
graph in each triangular region. As a result, in the dis-
crete graph only an approximate optimal path can be In this paper we examine the weighted version of
achieved, whose error is proportional to the weight of this problem in 2D space. More specifically, a weight
the optimal path. This approximate optimal path then function F : R? — Rt is defined on the space. For
is used to approximate the optimal path in the origi- any path p from v1 to v2, the weighted length of p is
nal problem. defined to be f/* F(x)dz, the integral of the weight
function along the path. The objective of the min-
Our algorithm extends these algorithms by dynami-
imum weighted path problem is to find the optimal
cally maintaining O(m) edges in each region to com-
path, i.e., the path with the minimum weighted length,
pute the exact shortest path in the discrete graph
from given source point s and a destination point t.
efficiently. The running time of this algorithm is
O(mnlog mn), where n is the number of triangular re- Figure 1 shows an optimal path in a 2D space with
gions. Our algorithm provides an improvement over four triangular regions. In the figure, the darker a re-
previous algorithms in the case when approximation er- gion is shaded, the more costly it is to move inside the
ror is to be bounded by an additive constant. region. (This designation will be used throughout this
paper.)
Besides (additive) constant error bound, our algo-
rithm also has the following three advantages: (a) tt Observe that the unweighted optimal path problem
can compute exact solutions for a discrete case of this can be considered as a special case of the weighted op-
problem; (b) it can be applied to any discretization; and timal path problem: For any point in the “free space,”
(c) it can be applied to a more general class of problems the weight is defined to be 1; for any point in the “ob-
than the previous algorithms. stacle” (if there is any), the weight is defined to be +o.
192 J. Reif and Z. Sun

Performance Parameters
MOIR
n number of vertices
LN
ottis
Pos halvate
maximum coordinate

the maximum (minimum) weight

€ the user-defined
Paani relative error allowed

1.2 Previous Work

Destination
One of the early algorithms on this problem was given
Source by Mitchell and Papadimitriou [13]. Their algorithm
uses Snell’s Law and “a continuous Dijkstra method”
Figure 1: Optimal path.
to give an optimal-path map for any given source point
s. The time complexity of their algorithm is O(n®M).
Here M is a function of various input parameters in-
Even though currently there is no complexity result
regarding finding the exact solution of the weighted op-
cluding the (relative) error tolerance e.
timal path, it is generally considered to be very hard. Mata and Mitchell [10] presented another approxi-
(See Canny and Reif’s work [5] for the NP-hardness mation algorithm based on discretization, the “path-
proof on the unweighted optimal path problem in 3D net” algorithm. In their scheme, a “pathnet graph”
space.) As a result, most previous research has fo- of size O(nk) is constructed and then an approxi-
cused on approximation algorithms on a special case mate optimal path with relative error of € is com-
of this problem where the planar space can be divided puted, where € = OU as
kO@min
ey The time complex-
into polygons and the weight is uniform within each ity, in terms of €, n and other geometric parameters, is
polygon. In some literature, this problem is named O( ne Winaal Wmin ).
€Omin
“homogeneous-cost-region-path-planning problem.”
Lanthier et al. [9] and Aleksandrov et al. [1] adopted
We define some notations that will be used in the a natural approach to solving this problem. Both algo-
rest of this paper. We use n to denote the number of rithms discretize the polygonal subdivision by placing
vertices of polygonal regions in the plane, and L the m Steiner points along the edges of polygonal regions
length of the longest edge of all polygonal regions. For and then try to compute an optimal path in the result-
our convenience, we assume that all the coordinates of ing discrete graph using Dijkstra’s algorithm. Instead
the vertices are integers, and N is the maximum coordi- of using a complete graph in each region by connecting
nate. Among various weights of all regions, let Wmaz all possible pairs of points (vertices and Steiner points)
be the maximum weight and W,,;, be the minimum in the same region, which is costly in terms of both
weight. time and space complexity, they only selectively add
For any s and t, we use D(s, t) to denote the weighted O(%) ([9] or O(mlogm) ([1]) edges in each region and
length of an optimal path from s to t and use D’(s,t) compute an approximate optimal path with e relative
to denote the weighted length of an approximate opti- error.
mal path found by a given approximate algorithm. We Lanthier et al. [9] used a uniform discretization,
decompose the error of an approximation into two com-
which adds m = O(n?) points on each edge evenly,
ponents: The absolute error 6, which is bounded by a
to compute approximate paths with a LW,,¢2-absolute
constant disregarding the value of D(s,t); and the rel- error and e-relative error. The absolute error of LWmaz
ative error €, which is linear to D(s,t). Therefore, we is resulted from the discretization while the relative er-
can represent the error of an approximation of the opti- ror of € is resulted from using spanner to compute the
mal path from s to t as D’(s,t)— D(s,t) = €-D(s,t)+6. approximate optimal path in the discrete graph. The
We list in the following the performance parameters relative error € is treated as a constant in their complex-
that will be used by various algorithms. ity analysis. Including € also as an input parameter, the
An Efficient Approximation Algorithm for Weighted Region Shortest Path
Problem 193
complexity is ome +n? log n), as in the discrete graph proves Lanthier et al. [9]’s work in terms of both time
there will be O(n*) vertices and DES, edges. complexity (O(n? log n) vs. OM +n?3 log n)) and accu-
Aleksandrov et al. [1] proposed a logarithmic dis- racy (same absolute error but no relative error). It also
cretization that can guarantee a relative error of e. The matches Aleksandrov et al. [1]’s work (O(mnlog(mn))
time complexity of this algorithm is O(mnlog(mn)), time and e-relative error) if logarithmic discretization
where m = O(log;(L/r)). Here r is the minimum is used. The reason our algorithm can not provide im-
distance from any point to the boundary of the re- provement in terms of time complexity over [1] is that
gions adjacent to it, and 6 = 1+ ee Sin Onin, Where during discretization a relative error of € is already in-
Omin is the minimum angle between any two edges. troduced and thus being able to compute the exact op-
If all the points have integer coordinates and N is timal path in the discretized problem does not improve
the maximum coordinate, we have r = 9(1/N) and the complexity result.
9min = 0(1/N?), and thus the time bound is approxi- Although our algorithm is not superior for this par-
mately O( ay Wins log(LN) log( 2A Wan )). ticular problem that can be solved by specialized ap-
In the following table, we list the time complexity proach of [1] due to the nature of discretization, we
and error bound of each of above-mentioned algorithm found our algorithm interesting for the following rea-
for a comparison. sons:
Comparison of Algorithm Performance

Algorithm |Complecty
Mitchell et al. ||O(n® log 24%
| Brror W )
i. It provides an efficient way of computing the opti-
mal path on the discretization graph. We refer to
this problem as Discrete Weighted Shortest Path
O(= + n3 logn) Problem.
LW-absolute
Discrete Weighted Shortest Path
(me t mien Problem A 2D space is composed of n
ee Owe) a
ee ee triangular region, each of which is as-
Aleksandrov O(nmlog(nm)) = sociated to a certain unit weight. On
2
et al. Oa log(LN) each edge e of each region, there are
log( ara) m = f(e) Steiner points. For any given
source point s and destination point f,
For other related work, see [2, 3, 4, 8, 14, 15, 16, 17], find a weighted shortest path that only
or see survey [12]. intersects region boundaries at vertices or
Steiner points.
1.3 Our Result
This is exactly the problem that [9], [1] and our
In this paper we introduce a wavefront-like algorithm
algorithm solve after the discretization step. The
that can compute optimal paths on any discretization
other two algorithm only compute an approximate
(uniform or logarithmic) more effectively than the ones
optimal path with time complexity greater than or
proposed in [9] and [1]. Instead of computing an ap- equal to that of our algorithm.
proximate optimal path in the discretized space with
e-relative error, as the above-mentioned two algorithms ii. It can be applied to different discretizations to
do, our algorithm can give an exact optimal path in less achieve different approximation goals. For exam-
(for the uniform discretization) or equivalent (for the ple, if absolute error is to be limited, we can ap-
logarithmic discretization) time. We achieve so by dy- ply this algorithm on a uniform discretization. If
namically maintaining for each Steiner point a small restricting relative error is the main goal, the log-
set of edges that are to be considered in search for an arithmic discretization then can be used.
optimal path. The total number of edges is O(m) in
each region. iii. It can be applied to a more general class of prob-
For the purpose of solving the original (continuous) lems. As we will show later, our algorithm only
weighted shortest path problem, our algorithm im- assumes the following geometric property:
194 J. Reif and Z. Sun

Inside each region (not including the S. (If otherwise, we can add a constant number of
edges), the weighted length of a path p; edges to construct from S' a new subdivision S’ which
is less than that of po if and only if the has s and t as vertices.) The problem is to find a short-
Euclidean length of p; is less than that est weighted length path (which will be called optimal
of p2- path) from s to t.
Therefore, our algorithm can be applied to opti-
It has been proved (see [13]) that an optimal path
mal path problems on planar regions where weight consists of O(n”) straight line segments. The endpoints
of a path is defined differently but the above- of each segment are on the boundary of a triangu-
mentioned property holds. A good example will lar face.
be computing an optimal path in a space com- Our algorithm is a general optimal path algorithm
posed of triangular regions, each of which has a and can be applied to any discretization that places
uniform flow. Steiner points on edges. Therefore, for now, we use
f(e) to denote the number of Steiner points added
2 Preliminaries along a given edge e. Here f(e) is dependent on e.
It may also depend on n and some other parameters,
2.1 Overview of Our Approach as we shall see later in this paper. We use V to de-
note the set of vertices of S and use V, to denote the
Let S be a polygonal subdivision of a planar space,
set of all Steiner points. In the following discussion, a
and let n be the number of vertices of S. This implies
“point” refers to a point in V UV,.
the number of edges and faces (regions) is also O(n).
Without loss of generality, we assume that each face of We first focus on a subspace P’ of the original path
S is a triangle. A triangulation can be performed in space P. Each path in P’ is composed of a number of
case there are non-triangular faces in the input subdi- line segments, each of whose two endpoints are points
vision. The resulting subdivision S’ after triangulation in V UV,. Further, the two endpoints of such a line
will still have O(n) vertices, edges and faces. segment are in a same face of S. We call such a path
a discrete path.
Each face f is associated with a non-negative real
number wy, the weight of f. For simplicity, we assume We first compute an optimal discrete path p/,,+(s, ¢)
that wy € [1,Wmaz]. For each edge e, the weight we from s to t in P’ and use this path to approximate the
is defined to be min{wy, wy}, where f and f’ are the optimal path popt(s, t) from s to t in the original space.
two faces adjacent to e. Wmaz (Wmin), as we defined Then, we show that D’(s,t), the weighted length of
in Section 1, is the maximum (minimum, respectively) Popt(Sst), gives a good approximation to D(s, t).?
weight of all regions. L is the length of the longest edge Our algorithm is similar to Dijkstra algorithm. How-
among the O(n) edges. ever, by utilizing some geometric properties of this
A line segment 0702 in a triangular region f is called problem, we achieve a time complexity lower than that
edge-crawling if v; and va are on a same edge e of f; of Dijkstra algorithm.
otherwise, it is called face-crossing. Refer to Figure 1,
the third segment of the optimal path is edge-crawling 2.2 Data Structures
and the other three segments are face-crossing. We
Before giving the description of our algorithm, we first
use |0jU2| to denote its Euclidean length and W(v702) present some basic data structures.
to denote its weighted length which is defined to be
|Oy02| - wy. If vive is edge-crawling, W (702) is |U7Vd| - Our algorithm maintains a list QLIST of paths. Each
We. Here we might be different from wy in case the path p’ € QLIST is a candidate optimal path from s to
other face f’ adjacent to e has a smaller weight than some point v € VUV,, and is represented by a quadru-
Piet (@. Goren) Carenne)> bere Uprev 18 the previous point
that of f. For two paths p; and po, we use p; + po to
denote the concatenation of p; and pp and p[v, v2] to “As a matter of fact, in a single run of our algorithm,
denote the part of path p between v; and vo. an optimal discrete path p),:(s,v) is computed for each
vEVUYVs..
Let s and t be the source and the destination, respec- The accuracy of the approximation will depend on the
tively. We can assume that s and ¢ are two vertices of discretization used.
An Efficient Approximation Algorithm for Weighted Region Shortest Path
Problem 195

Discovered Points
7

® Source
Figure 3: Intervals.
Figure 2: Candidate optimal path.

(and the farthest point to v is the other endpoint). This

of v on p’, €prey is an edge that contains @prey aud | is is to guarantee that the distance to v forms a monoton-
the weighted length of p’. Each p’ is “almost optimal” ically increasing or decreasing sequence for points in
in the sense that it is the concatenation of p,4(s, Uprev), Iy,c,e, from left to right along edge e.
the optimal discrete path from s to Pprey, and line seg- From the construction of Iy,¢,e;, it is easy to see that
ment Uprevv- Thus, | = D’(s, Uprey) + W(Uprev®).- at most two intervals are associated with each point
QLIST is sorted in ascending order according to the v € V UY, due to interval splitting. For the sake of
weighted length of candidate optimal paths (i.e., value simplicity, in the following discussion we may assume
of | of the quadruplets). Possible operations on this list that only one interval is associated with each point.
are insertions and removals. We can use an AVL tree Removing this assumption will only increase the time
(see [6]) to implement QLIST so that each operation complexity of our algorithm by a constant factor.
costs only O(log |QLIST]) time. We let Ue ee and Uj ¢.e, denote the left and right
Also, for each edge e = U0; of S, we keep a list endpoint of Iy,c,e,, respectively. As we shall see later,
PLIST. of discovered points on e. A point v is said every point v’ in I, ¢., will be “processed,” meaning
to be discovered when the optimal discrete path from that a candidate optimal path from s to v’ which enters
s to v is determined. The points in this list are or- face f through v will be added to (and later removed
dered according to the distance to v;. Possible oper- from) QLIST. We let 0, .-; be the closest point of
ations on PLIST, include insertions and finding the Iy,c,e, to v that has not yet been processed. Initially it
closest neighbors. Again, using an AVL tree can guar- is either one of the two endpoints of J, ¢,e, whichever
antee that each operation only costs O(log |PLIST,|) = is closer to v. An interval Iy,,e; is said to be active if
O(log f(e)) time. at least one point v* in Iy,e,e, has not been processed.
Let f be a face adjacent to e, and let e; and e2 be the Figure 4 illustrates endpoints and closest un-
other two edges of f. For each e; and each discovered processed point of an interval. In the figure, we use
point v on e that is not an endpoint of e;, we use Ty ,¢,¢; solid circles to denote processed interval points and
to denote the set of points on e; with the following hollow circles to denote unprocessed interval points.
property: for any point v* on e;, v* © Iv.e,e, if and
Let ILIST..., be the list of intervals Iy,¢,e; for all
only if W(vv*) + D’(s,v) < W(v’v*) + D(s, 0") for any
v € PLIST,. The intervals in ILIST...,; are maintained
other discovered point v’ on e. We will show later that
in the order according to the order of v in PLIST..
each such Iy,¢,¢, is an interval of contiguous points on
e€;, as indicated in Figure 3. We also use an AVL tree to implement each ILIST..,
to keep the cost for each operation on ILIST..., to be
For any Iy,c,e,, if the point 6 closest to v in Leanetils O(log |ILIST .,e,|) = O(log f(e)).
not an endpoint of Iy,c,e,, we split I,,c,e; into two inter-
vals IjV,€,€4 and Jj’,.,in such a way that for each inter-
; It is easy to see that, at any time, an edge vv’ is
val, the closest point to v is one of the two endpoints considered to be added into an optimal path only if v’
196 J. Reif and Z. Sun

Figure 4: Endpoints of interval.

is in Iye,e, for some e and e;, and the total amount

of such edges in each face f is bounded by O(m). By
eliminating edges that can not produce optimal paths,
our algorithm is able to compute optimal paths more Figure 5: Insert intervals for a discovered point.
efficiently than Dijkstra’s algorithm.

The main loop consists of two steps: the first step

3 The Algorithm
is to, in case v is a newly discovered point, update the
We now give the formal specification of our algorithm. discovered point list PLIST., and subsequently update
the interval list ILIST.,.., for all appropriate e and e;
Function FindOptimal Discrete (by calling function InsertInterval). The second step
[1] (Initialization) QLIST = {(s,s,0,0)}. For each e, is to propagate the path by one more line segment.
PLIST, = 0. For each edge e and each e; that shares a This is done by calling function Propagate.
face f with e, ILIST..., = 0. For each point v € VUV,, Figure 5 shows a special case of step [2.b.iii]. When
D'(s,v) = +00. the newly discovered point v is an endpoint of an edge
[2] (Main Loop) While QLIST is not empty: (ie. vu € V), we will need to insert new intervals to
edges of all adjacent faces of v.
[a] Remove the first entry of QLIST. Let it To update an interval list ILIST.., when a point v
DEM Us Virals Cpr eh.l): on e€ is discovered, function InsertInterval needs to de-
[b] If D’(s,v) = +00, ie., no other candidate termine vi,,., and Uy,e,e,» the left and right endpoints
optimal path p’ (with weighted length at most of the new interval I,,ce, associated with v, and to
1) has been removed from QLIST and thus v adjust the endpoints of other intervals in ILIST..., if
is not discovered yet: necessary.

fa Set (sya) =" ly and’ set

Function InsertInterval(v, e, e;)
Popt (8; v) a Popt (8, Uprev) oF UprevU:
[ii] Insert v into PLIST, for each e [1] Find the left and right neighbors, 1 and v,, of v in
that contains v. Phd Lies
[iii] For each edge e that contains [2] Initially, v}..., = NULL, v7,.,= NULL.
v, and let f; and fg be the two
[3] Do a binary search on the part of e; to the right
faces adjacent to e. For each f;,
i = 1,2, let e;; and e;,2 be the
of (including) vj, ..., (the right endpoint of the interval
two edges of f; other than e. Call
associated with the left neighbor of v) until we find two
adjacent points 0, and d2 such that, if 6, is in the inter-
InsertInterval(v,e,e;,;) for 1 <i <
val associated with vf and 2 is in the interval associ-
2 and 1< 7 < 2 if v is not an end-
point of e;,;. ated with v3, D'(s,v) +W(vi1) < D'(s, vt) +W(v%* 6)
but D’(s,v)+W(vb2) > D’(s, v3) +W(vdd2). In other
[c] Call Propagate(v, Uprev; Eprev,!). words, to construct a path s to 61, it is advantageous to
An Efficient Approximation Algorithm for Weighted Region Shortest Path Problem
197
extend p%,1(s,v) by adding one more line segment vy
than to extend p7%,,1(s, vj) by adding line segment v* 64;
however, to construct a path from s to é9, extending
Popt(S, V9) is a better idea.
[4] Let uv”...ViE, Eq
=%1.
[5] Similarly, compute v’, . «,.
[6] Insert I,,-,-, into ILIST...,. For each interval IiS oy
entirely covered by Ip.c,e,, set Tyee, = 0.
[WAdd (0, (¢,¢;,, €, D'(s,v)+ W(v, by.¢¢,)) to QLIST.
Figure 6: Two ways of propagating candidate opti-
[8] For any interval I,» ¢.¢, that is partially covered
mal paths.
by Ip,e,e;, adjust its boundary by removing all points
that are covered by Jy,c,e,. If the closest unprocessed
point to v* in the original J,+ ..-, is no longer inside the in the form of p64(8;Uprev) + Uprev¥ has been
interval, add (dy* ¢,e,,*, €, D’(s,v*)+ W(v*,
by« ¢,¢;)) found with a shorter weighted length, where
into QLIST. Uprey 18 also On €prey), return.
Step [7] is very important in function [a.2] Let Unew be the left or right neighbor
InsertInterval. It adds into QLIST a new can- Oi V on €-such that |UgpecUnewlae |Upeeour
didate optimal path which enters face f through v If W(UprevUnew) = W(Uprevd) + W(0tew),°
and leads to the closest point ty,c,e, to v (in interval INSET = “{Unews2.€5, ($0). 4 Wil BUnen,))
Iy,c,e,). Here f is the face adjacent to both e and e;. into QLIST; otherwise insert
When later this path is processed and removed from (neu: Uprevs Eprev» DCS; Uopen) ar
Waren Unes
QLIST, a similar optimal path that connects s to the
subsequent point in interval I,,.-, will be added into [Dili isvonies,cu:
QLIST, as shown in function Propagate. Therefore, [b.1] If there has been another quadruplet
progressively, for each point u € Iy,c,c,, the candidate (UV; Uprevs prev, |) removed from QLIST, re-
optimal path that enters f through v and leads to u turn
will be tested if it is an optimal path.
[b.2] Otherwise, let new be the neighbor of
Similarly, Step [8] is to ensure that, for any active U ON prey that is farther from Uprey than v is.
interval I, ¢.e, whose endpoints are changed because of Insert (Unew, UV;€:, D'(s,v) + W(Utnew)) into
the newly inserted interval, there is a candidate optimal QLIST.
path in QLIST that enters face f through v* and leads
to the closest unprocessed point in Ip ,¢,¢;-
[2] If v is a vertex of the polygonal subdivision:
Function Propagate is used to add candidate opti-
mal paths to QLIST after (v, Uprev, Eprev, !) is removed Let e€1,€2,::: ,e% be the edges adjacent to
from the list. v, and let v; be the closest point € V U V;
that is on e; for 2 = 1,2,---,k. Insert
Function Propagate(v, Uprev; €prev; !) (v;,v, €;, D’(s,v) + W((vv;))) into QLIST for
[1] If v is a Steiner point: each i = 1,2,-:-,k. This is to extend the
candidate optimal path along each e;.
[a] If v is not on prey: Suppose f is the face
that contains both eprey and v, and e is the The two cases in step [a.2] correspond to the two
edge of f that contains v. ways of propagate candidate optimal paths as shown
in Figure 6. On the left, a candidate optimal path to
[a.1] If there has been another quadruplet
(U, Uprevs Eprev, l’) removed from QLIST pre- 3It is possible as the weight of e may be smaller than
viously (i.e., another candidate optimal path the weight of f.
198 J. Reif and Z. Sun

the portion of Dopt (Ss vi) between vj,2 and v;. There-
fore, p21 + U2,101,2 + Pi2 is a path from s to v and
Pi. + U1,1022 + Po2 is a path from s to v2. How-
ever, the total weighted length of the above two paths
is shorter than that of p/,,(s,v1) and pjp(8, v2), as
W (U,10 72) > W (Tri022) + W (e102).
+ W(%3,1032)
Therefore, one of p/,,;(s, 01) and p5pz($, V2) must not be
optimal. A contradiction. O
With exactly the same argument, we can prove the
following lemma:

BY WYP Lemma 2 For any two points v; and v2 in PLIST,

that are not endpoint of e, each point in Ip, e,f 18 to
ne the right (left) of each point in Iy,,c,¢ if and only if v1
is to the right (left, respectively) of v2 one.
Figure 7: Two optimal paths can not intersect.
It directly follows from this lemma that each I,,¢,f
is an interval of contiguous points on e; Ueg. Thus,
Up ent 1S generated by replacing the last line segment a binary search can be used in step [3] of function
vu* of the candidate optimal path to v* by line seg- InsertInterval to find the left and right endpoints of
ment vv»... We call this type of propagation face- the new interval Iy,c.e,-
crossing-propagation. On the right, a candidate opti-
Lemma 3 For-any active Iyc.e,, let 0 € Ive, be
mal path to u*,.,, is generated by adding line segment
the closest point (in terms of Euclidean distance) to
U* Uxon, to the candidate optimal path from s to u*. We
v that has not been processed. Then, there must be
call this type of propagation edge-crawling-propagation.
Edge-crawling-propagation is to take advantage of the
a point 6’ € Iyee, with W(vt’) < W(véd) that satis-
fies at least one of the following two conditions: (i)
smaller weight of the other face.
Quadruplet (6',v, e, D'(s,v) + W(v6’)) € QLIST; (ii)
Quadruplet (6', 6”, e1, D'(s,6”) + W(6"6')) € QLIST
4 Correctness and Error Bound and D'(s, 6") + W(#’6") < D'(s,v) + W(vd). Here 6”
is either the left neighbor or right neighbor of t’ on ey.
In the first subsection we present the correctness proof We call a point that satisfies condition (i) (condition
for our algorithm of finding an optimal discrete path (ii)) a “handle point” of Type A (B, respectively). Ob-
Popt(S,v) for every v € VUV,. Then we show how serve that there might be more than one handle point
an optimal discrete path can provide an approximate for an interval.
solution to the original problem.

4.1 Correctness Proof

[ype B Handle Vv2 Type A Handl.
We first give a lemma that is key to our algorithm. 1

Lemma 1 Any two discrete optimal paths cannot in-

tersect in the interior of any triangular face.

Proof Assume that, as shown in Figure 7, two opti-

mal path pj,:(s, v1) and p5,1(s, V2) intersect at b inside
a triangular face f. For each 7 = 1,2, let U0; be
the segment of pj,.(s,vi) that contains b. Thus, v,;
are points on the boundary of f. Let p; be the por-
Figure 8: Interval handles.
tion of Popt($; Vi) between s and v1 and let pio be
An Efficient Approximation Algorithm for Weighted Region Shortest Path
Problem 199
Proof After an interval Iy,c,e; is first created (and thus become a new Type B handle point, unless DE e€,ey
inserted into ILIST...,) when the optimal path from s 1s not active anymore.
to v is decided, the function Propagate is called which
will add (0,,¢,c;,¥,€, D’(s,v) +W (vdy,c,c,)) into the list (b) Suppose interval I,,c,e; loses all of its handle points
QLIST. Here o,...., is the closest point to v in Tie.€:
as a result of a boundary adjustment. However,
and thus is a handle point. as specified in step [7] of InsertInterval, a new
There are two events that could possibly make ee
quadruplet (6y,c,e,,v,e, D’(s,v) + W(vdy,e.c;)) is
inserted into the list QLIST. Here @,..¢, is the
lose handle points: (a) a quadruplet is removed from
closest point to v in the (adjusted) I,..., and thus
QLIST; (b) The boundary of I,,.,c; is adjusted (as in
is a handle point.
step [7] of function InsertInterval) so that the handle
points are now in another interval [,...,. In the fol-
Therefore, at any time, as long as Iy,e,e; is active,
lowing we prove that in any of the above two cases, at
there is at least one handle point in I, ¢.,. O
least one handle point will be generated unless I,,.¢,
becomes inactive. A Type A handle point ensures that a candidate
optimal path to 6 will be generated and added into
QLIST by face-crossing-propagation, while a Type
(a) Suppose I,,,e, loses a Type A handle point 6’ af-
B handle point ensures an edge-crawling-propagation
ter a quadruplet (6’,v,e, D’(s,v) + W(vd’)) is re-
to 0.
moved from QLIST. The fact that 0’ € Ine,
excludes the possibility that another quadru- With the above lemmas, we are able to prove the
plet (0’,v’,e,l’) removed from QLIST previously. following theorem:
Therefore, step [1.b] of function Propagate is
Theorem 4 Function FindOptimalDiscrete deter-
executed and quadruplet (#/,,,,,v,e,D’(s,v) +
mines an optimal discrete path p’,,.(s,v) for every point
W (vohew)) is inserted into QLIST if W(vti,..,) <
vEVUY,.
W (vt!) + W(0'0!,..,)); otherwise, quadruplet
(OF ews 0, €1, D'(s, 6’) + W(0'o!..,)) is inserted. Proof It suffices to prove that, at each time step [b]
Here ©,,. is the (left or right) neighbor of 6’ on of function F'indOptimal Discrete is executed, the op-
€, such that W(vi!,.,,,) > W(v0"’). timal path pop1(s,U) = Popt(S, Uprev) + Uprevd is deter-
If /,.., is not in I, ¢.,, then t’ must be an end- mined.
point of I,,c,e,. Furthermore, it must be the point Suppose for the sake of contradiction that p* is
of Iy,c,e, that is farthest from v. Therefore, all the an optimal discrete path from s to v with W(p*) <
points in I,..-, must have already been processed. W
(Dope(S,¥))- Let v* be the discovered point on p*
Thus, I,,¢,c, is not active anymore. If ;,.,, is in that is closest to v. v* can not be v as v is not discov-
Iy,c,e;, we Claim that it must be a handle point of ered before quadruplet (v, Uprev, e, f) is removed from
Iy,ec,e;. Observe that, for any unprocessed point QLIST. Let v*,..,next be the successor of v* on p*.
6 € Inee,, 6 # 0’, W(vd) > W(ve’) and thus
W(vd) > W(vé/.,,). Thus, in case quadruplet
(61,ews Vs €, D'(s,v) + W(vb},-.)) is inserted into
QLIST, @/,.,, is a Type A handle point. In case
quadruplet (6/,..,,
8”,€1, D'(s, 0’) + W(0'Phew)) is
inserted into QLIST, we have
D'(s, 6!) +W When)
D'(s,v) + W (vd!) + W(8' Chew)
D'(s,v) +W(vohew)
IA D'(s,v) + W(vd).
Hence, @/,.,, is a Type B handle point.
Similarly, we can prove that if Iy,c,e; loses a Type Figure 9: Proof by contradiction.
B handle point 6’, another point /,.,, € Iv,e,e; will
200 J. Reif and Z. Sun

First assume v*v*,.., is a face-crossing line segment.

Let f* be the face that contains both v* and v7,,4, let
e* be the edge of f* that contains v* and let e*.,, be
the edge that contains u*,,,. Since p* is an optimal
path from s to v, p*[s, v5.,4] is an optimal path from s
-
tO Unext
* - LHUS, Vien,
*
Must be in [ys ex en .-
From Lemma 3 we know that there is at least one
handle point in Iy«,e+,ex,,,- Let vu’ be a handle point
of Type A. (The proof for the case when v’ is a Type
B handle point is similar.) Let vj = v',v3,--: ,u, =
Destination
Unext be the points of Iy*,-*,¢» between v’ and Up cor:
Since D’(s,v*) + W(v*vuz) < D’(s,v*) + W(v*vx ent) < Source

Wp iD (8.tren) + WlUprevd), for t= 1,2,---k, m= 4

(equality holds only when v’ = v;,) quadruplet
Exact Optimal Path
(v*,v*, e*, D'(s,u*) + W(v*v%)) should in turn be in- a

serted into (and then removed from) QLIST before

quadruplet (v, Uprev, €, D'(S, Uprev) + W(Uprevd)) is re- Optimal Path in Discretization
moved, fori = 1,2,--- ,k. Therefore, vj, v5,--- , uz are
already discovered before v is discovered. A contra-
diction to the assumption that v*.,, is not discovered Figure 10: Approximating optimal path by discretization.
before v is discovered.
The case when v*v;,., is an edge-crawling line as compared to [1] where only an approximate optimal
segment can be handled similarly. Therefore, there path is computed, it does not make a difference in this
can not be such an optimal path p* with W(p*) < particular case as a relative error of the same magni-
Popt (8;v), and hence Popt(S; v) = SA Uprev) + Uprev¥ tude has already been introduced in the first phase of
is an optimal path. the algorithm (discretization).
4.2 Approximating the Optimal Path
5 Complexity Analysis
As for each v € V UV, our algorithm uses an opti-
mal path p’,,.(s,v) in the discretized path space P’ to In the following complexity analysis, we assume f(e) =
approximate the optimal path popt(s,v) in the origi- m for all e in the polygonal subdivision S for simplicity.
nal path space P, the accuracy of our approximation is (Thus, the number of Steiner points added to each edge
totally determined by the discretization on which our is the same.)
algorithm is based upon.
To analyze the complexity of our algorithm, we need
If the uniform discretization as in [9] is used, which to examine the number of quadruplets inserted into
puts O(n?) Steiner points on each edge, the absolute QLIST. Observe that, when a quadruplet is removed
error of our approximation is bounded by a constant of from QLIST, there may be two occasions when new
LW max. However, since we calculate the exact optimal quadruplets are inserted into QLIST. One is occurred
path in the discretized space instead of using spanner within function Propagate and the other is occurred
to approximate it (as in [9]), our algorithm eliminates within function InsertInterval.
the extra relative error of € introduced by the usage of
spanner.
Quadruplets added through InsertInterval:
If we adopt the logarithmic discretization proposed For each v € VU V,, when the optimal dis-
by [1] so that m = O(log;(L/r)) Steiner points are crete path pj,.(s,v) is determined, function
placed on each edge, the approximation will have a rel- InsertInterval(v, e,e;) is executed once for each
ative error of «. Although our algorithm is able to com- face-sharing edge pair (e,e;) such that v € e. For
pute the exact optimal path in the discretized space, each edge e, there are at most four face-sharing
An Efficient Approximation Algorithm for Weighted Region Shortest Path Problem
201
edge e; as e is adjacent to two triangular faces. From the above discussion, we can conclude that the
In InsertInterval(v,e,e;), a new interval is total number of quadruplets inserted into QLIST is
added into ILIST..., and a quadruplet (candidate O(nm). The time cost for maintaining QLIST is thus
optimal path) is inserted into QLIST, as indicated O(nmlog(nm)). Also, as there are O(nm) different in-
by line [7] of function InsertInterval. Also, a tervals I, ¢7, and it takes O(log m) time to create such
new quadruplet is inserted into QLIST for each an interval, (observe that a binary search is needed
interval partially covered by the new interval in step [3] of InsertInterval,) the total time cost for
Iy,c,e;, aS indicated by line [8] of InsertInterval. creating these intervals is O(nm log m). Hence, our al-
Since there are at most two intervals partially gorithm takes O(nm log(nm)) time in total.
covered by the new interval, at most three
For the uniform discretization used in [9], we have
quadruplets are inserted into QLIST for each
m = n? and thus the time complexity is O(n? log n).
Iy,e,e,- Thus the total number of quadruplets
For the logarithmic discretization used in [1], the time
added for point v is 12E(v), with E(v) defined as
complexity is O(mnlog(mn)) which matches [1]’s re-
the number of edges that contains v. For v € V,,
sult, here m = O(log;(L/r)).
E(v) = 2. For v € V, E(v) varies but we have
S> E(v) = O(n). Therefore, the total number
vEV
6 Implementation Issues
of quadruplets added in executions of function
We have implemented our algorithm in C++. We used
InsertInterval is ia 12E(v) + S| 6E(v) =
several lists in our algorithm, each of which needs to
vEVs vEV
be able to perform operations such as insertion, dele-
O(|Vs|) + O(n) = O(nm). tion, and find next (or previous) node efficiently. As
Quadruplets added through Propagate: Now we mentioned previously, using AVL tree to implement
calculate the number of new quadruplets inserted the lists may be more efficient than using an ordinary
through executions of function Propagate(v, binary tree. In particular, the discovered point list,
PLIST., for each edge e, might be extremely skewed
Vocceepreegeaeeorseach U.CaVel Vz.ayFirstyawe
if implemented by an ordinary binary tree, as the dis-
assume that v is a Steiner point, ie., vu € V5.
covering of points on an edge e might start at a point
Let e be the edge that contains v and let
in the middle of e and spreads towards both ends of
fi, fo be the two faces adjacent to e. Assume
e. We implemented AVL tree using a C++ template
€i,1,€i,2 are the other two edges of face fi, for
so that it can handle different types of data structure,
me 1o2. 1 Observenthathe,,-c, sisdeitherive,tor
such as discovered point, candidate path and interval.
one of e;;s. There are at most six quadru-
Due to the complexity of AVL tree (i.e., the large con-
plets in the form of (v,*,*,*) whose removal
stant in the analytical complexity), we expect to use
from QLIST can lead to an insertion of a new
large-scale data sets to demonstrate the efficiency of
quadruplet into QLIST. The six possible quadru-
our algorithm.
plets are (v,Vieft,e,D'(s,vieft) + W(Uieft))),
(v, Uright,€,
D’(8, Vieft) + W(Urightd))), and As mentioned earlier, the real strength of our al-
(U, Uprev; €i,5, (8, Uprev) + W (Uprev)) for gorithm is the ability to compute efficiently an exact
i=1,2,j7 =1,2. Here were and vpigne are the left optimal path on a discretization of the original space.
and right neighbors of v on e, respectively. This Both Lanthier’s[9] and Aleksandrov’s[1] are based on
is guaranteed by line [1.a] and [2.a] of function Dijkstra’s algorithm, but they construct a relatively
Propagate. Similarly, for v € V, the number sparse graph in each triangular region and thus only
of quadruplets in the form of (v,*,*,*) whose an approximation can be achieved. To compute an
removal can lead to an insertion of a new quadru- exact optimal path by Dijkstra’s algorithm, a com-
plet is bounded by O(E(v)). Therefore, the total plete graph has to be constructed in each region, which
number of quadruplets added through Propagate means connecting each pair of points by an edge. The
is 5 O(1) + S) O(E(v)) = O(|Vel) + O(n); total number of edges is thus O(m?n) and the total
vEVs MeV
number of points is O(mn). Here again m is the num-
which is again O(nm). ber of Steiner points placed on each edge of the orig-
202 J. Reif and Z. Sun

inal space and n is the number of triangular regions. [2] R. Alexander. Construction of optimal-path maps
Therefore the time complexity of Dijkstra’ algorithm for homogeneous-cost-region path-planning problems.
Ph.D. Thesis, Computer Science, U.S. Naval Postgrad-
is O(m?n + mnlog(mn)) to solve the exact optimal uate School, Monterey, CA, 1989.
path problem, compared to O(mnlog(mn)) of our al-
gorithm. [3oost
R. Alexander and N. Rowe. Geometrical principles
for path planning by optimal-path-map construction
We implemented two versions of Dijkstra’s algo- for linear and polygonal homogeneous-region terrain.
rithm, one based on binary tree and the other based on Technical report, Computer Science, U.S. Naval Post-
Fibonacci heap. The binary tree based Dijkstra’s algo- graduate School, Monterey, CA, 1989.
rithm is the traditional one and has a time complexity
[4— R. Alexander and N. Rowe. Path planning by
of O((|E| + |V|)log|V|). The Fibonacci heap based optimal-path-map construction for homogeneous-cost
one is provided by Fredman and Tarjan [7] and has an two-dimensional regions. In IEEE Int. Conf. Robotics
improved complexity of O(|E|+|V| log |V|). Some pre- and Automation, 1990.
liminary experiments with relatively small data inputs
show that our algorithm performs better than both ver-
[5] J. Canny and J. Reif. New lower bound techniques
for robot motion planning problems. In A. K. Chan-
sions of Dijkstra’s algorithm in finding exact optimal dra, editor, Proceedings of the 28th Annual Sympo-
paths in discretized space. sium on Foundations of Computer Science, pages 49—
60, Los Angeles, CA, Oct. 1987. IEEE Computer So-
For large scale experiments, we plan to use standard ciety Press.
data sets for experiment inputs to make comparison.
If standard data sets are not available, we will try to [6] T. H. Cormen, C. E. Leiserson, and R. L. Rivest. In-
troduction to Algorithms. MIT Press, 1990.
use computer-generated data sets.
M. L. Fredman and R. E. Tarjan. Fibonacci heaps
7 Conclusion and their uses in improved network optimization algo-
rithms. In 25th Annual Symposium on Foundations of
In this paper we present a new approximation algo- Computer Science, pages 338-346, Los Angeles, Ca.,
USA, Oct. 1984. IEEE Computer Society Press.
rithm to solve the weighted shortest path problem.
Compared to some of the previous work, our algorithm [8] M. Kindl, M. Shing, and N. Rowe. A stochastic
provide a more effective way of finding optimal paths in approach to the weighted-region problem, II: Perfor-
the discretized space (resulted from either uniform or mance enhancement techniques and experimental re-
sults. Technical report, Computer Science, U.S. Naval
non-uniform discretization). Our algorithm has the fol- Postgraduate School, Monterey, CA, 1991.
lowing three advantages over previous algorithms: (a)
can compute exact solutions for a discrete case of this [9] M. Lanthier, A. Maheshwari, and J. Sack. Approx-
problem; (b) can be applied to any discretization; and imating weighted shortest paths on polyhedral sur-
faces. In 6th Annual Video Review of Computa-
(c) can be applied to a more generic class of problems. tional Geometry, Proc. 13th ACM Symp. Computa-
We expect to find more applications for our algo- tional Geometry, pages 485-486. ACM Press, 4-6 June
rithm. 1997.

[10] C. Mata and J. Mitchell. A new algorithm for com-

Acknowledgements puting shortest paths in weighted planar subdivisions.
In Proceedings of the 13th International Annual Sym-
This work was supported in part by Grants postum on Computational Geometry (SCG-97), pages
264-273, New York, June4d—6 1997. ACM Press.
NSF/DARPA CCR-9725021, CCR-96-33567, NSF IRI-
9619647, ARO contract DAAH-04-96-1-0448, and [11] J.S. B. Mitchell. Shortest paths and networks. In J. E.
ONR contract N00014-99-1-0406. Goodman and J. O’Rourke, editors, Handbook of Dis-
crete and Computational Geometry, chapter 24, pages
445-466. CRC Press LLC, Boca Raton, FL, 1997.
References
[12] J. S. B. Mitchell. Geometric shortest paths and net-
[1] L. Aleksandrov, M. Lanthier, A. Maheshwari, and J.- work optimization. In J.-R. Sack and J. Urrutia, ed-
R. Sack. An ¢-approximation algorithm for weighted itors, Handbook of Computational Geometry. Elsevier
shortest paths on polyhedral surfaces. Lecture Notes Science Publishers B.V. North-Holland, Amsterdam,
in Computer Science, 1432:11—22, 1998. 1998.
An Efficient Approximation Algorithm for Weighted Region Shortest Path Problem 203

[13] J. S. B. Mitchell and C. H. Papadimitriou. The [16] N. C. Rowe. Roads, rivers, and obstacles: optimal
weighted region problem: Finding shortest paths two-dimensional route planning around linear features
through a weighted planar subdivision. Journal of the for a mobile agent. International Journal of Robotics
ACM, 38(1):18-73, Jan. 1991. Research, Vol 9(6), Dec 1990., pages 67-74, 1990.

[14] N. Papadakis and A. Perakis. Minimal time vessel

[17] N. C. Rowe and R. F. Richbourg. An efficient snell’s
routing in a time-dependent environment. Transporta-
law method for optimal-path planning across multiple
tion Science, 23(4):266—276, 1989. two dimensional, irregular, homogeneous-cost regions.
International Journal Of Robotics Research, Vol 9, No
[15] N. Papadakis and A. Perakis. Deterministic minimal
6, Dec 1990. Page 48-66, 1990.
time vessel routing. Operations Research, 38(3):426—
438, 1990.
 > _
7
“>
“seein iB en1 actor hontgiwd yamine

na
sii+
ahd: ate ey
ad
gael
wht (Albee thao lim
a al ‘wer Exteel, eer eens

valae fry! sat ecient

‘se “p ‘ nite “‘viarrtt Lo aRMAER Y
irpwe - woggaid-#.3

PR».
vite,
bal
is
ié
a’
“fe
om be 1
MARL NS cr] rae 5
akevr’ ;
abc Adana, Mims gpdintat ursBbo Sega
feged (al
4 : xh: FEvRE Y jest ¥ ay: the lied NHOW 6 Te P..
= A Vesta Gey Pins a /
: 0) fa) (wei)oa CMe ig fear reer ;
cleats Aye bn FS De oO >

ti 4 Game
Tr -cWin 6S

eat vo om xo
- Gout = wun AL eliaint * ASP the
Py hiv g ae
i
ee
_ hire
us:
60 Pominditnenas
ou oh 2

-
cgay Meee
G

aoe ome Lagt ah, Axe awe irre

=i im

PDT Chthus, 0. Ledeen aiae ad Te

el aaess } Si7orithey wiTT’ Py a
~~ 14
me
: &. Foeevlow end bg inrsan,: Sai
OO ie TS. ip canton’ sotestyrts
ign
Ya Sth Amat pene nae |
i. "Dy Mower. Feteue S58 M4-.08
* ~) LAP LE Se re pte” Saw w=? 2
ip id <P hive Gar? W i
be = Piglet <g> Ne. Lt
: Mart tlh co «0 ugpeiines
te ‘any = eA 7 Tauber “ew eg
~~ . tee Shite P wise, @ 17a)
‘ae _

bear, aut J

© wor.
vee rn —.

é
ra ‘

~ n
Loward the Regulation and Composition
of Cyclic Behaviors
E. Klavins University of Michigan, Ann Arbor, MI
D.E. Koditschek University of Michigan, Ann Arbor, MI
R. Ghrist Georgia Institute of Technology, Atlanta, GA

Many tasks in robotics and automation require a cyclic For reasons we have discussed elsewhere at length
exchange of energy between a machine and its environ- [7, 21], we construe “task” to mean any behavior that
ment. Since most environments are “underactuated” can be encoded as the limit set of the closed loop dy-
— that is, there are more objects to be manipulated namical system resulting from coupling the robot up to
than actuated degrees of freedom with which to manipu- its environment. By “programming” is meant (at the
late them — the exchange must be punctuated by inter- very least) a means of composing from extant primi-
mittent repeated contacts. In this paper, we develop the tive task behaviors new, more specialized, or elaborate
appropriate theoretical setting for framing these prob- capabilities. A decade’s research by the second author
lems and propose a general method for regulating cou- and colleagues has yielded the beginnings of a compo-
pled cyclic systems. We prove for the first time the sitional methodology for tasks that can be encoded as
local stability of a (slight variant on a) phase regula- point attractors [29, 28, 7, 14, 19]. In the present paper,
tion strategy that we have been using with empirical we take the first steps toward a formal foundation for
success in the lab for more than a decade. We apply tasks that can be encoded as the next simplest class
these methods to three examples: juggling two balls, two of steady state dynamical systems behavior — limit
legged synchronized hopping and two legged running — cycles.
considering for the first time the analogies between jug-
gling and running formally. 1.1 Contributions of the Paper

In this paper we are able to prove for the first time the
1 Introduction partial correctness of a (slight variant on) a phase regu-
lation strategy that we have been using with empirical
success in the lab for more than a decade [5, 31]. The
A robot is a source of programmable work. Robot pro-
object of study is a discrete dynamical control system
gramming problems arise when a mechanism designed
on a co-dimension one subset of the tangent bundle
with certain directly actuated degrees of freedom is re-
over the two-torus, © C T?. The theoretical result
quired to exchange energy with its environment in such
is the proof of local asymptotic stability for a speci-
a fashion that some useful work — its “task,” involving
fied fixed point on this subset, x* € U. To illustrate
the imposition of specified forces over specified motions
the potential implications of the emerging theory, we
— is accomplished. Typically, the environment is not
introduce three example systems that move from jug-
directly actuated and has its own preferred natural dy-
gling toward phase coordination strategies for legged
namics whose otherwise uninfluenced motions would be
machines.
at least indifferent and, possibly, inimical to the task.
The prior century’s end has witnessed the practical tri- We show in our first example that this discrete sys-
umph and emerging formal understanding of programs tem corresponds to the parametrized family of return
for information exchange and manipulation. There maps that arise when a one degree of freedom actuated
does not yet seem to exist a programming paradigm piston strikes two otherwise unactuated one degree of
that can specify similarly goal oriented work exchange freedom balls falling in gravity. This abstraction pre-
at any reasonable level of generality with any reason- sumes sufficient control of the piston to track a suitably
able likelihood of successful implementation (much less, distorted image — a “mirror law” [6] — of the trajec-
of formal verification). tory described by the two balls, along the lines of the
206 E. Klavins, D.E. Koditschek, and R. Ghrist

empirical setup in [5, 30]. Under these assumptions, case of the two-juggle. We suspect, but have not yet
the torus bundle represents the phase coordinate rep- proven, that the coordinated bipedal vertical gait is
resentation of the two degree of freedom hybrid flow once again (essentially globally) asymptotically stable.
formed when the paddle repeatedly but intermittently The final example represents our first and still rather
strikes one or the other ball. The functional freedom tentative efforts to interleave constituent cyclic be-
afforded by the choice of “mirror law” yields the para- haviors that arise in systems possessed of more than
metrization of the available closed loop return maps one degree of freedom. Raibert’s running machines
whose domain, », is now interpreted as the phase con- combined in parallel, for each leg, three independent
dition at which an impact is made. The preliminary and decoupled controllers that operated in three very
analysis presented here develops a set of sufficient con- strongly coupled degrees of freedom, with excellent
ditions on the mirror law that guarantees the local as- empirical success. Moreover, he devised a notion of
ymptotic stability of a limit cycle corresponding to the “virtual leg” that successfully coordinated the relative
the desired two-juggle. We suspect, but have not yet phases of the “vertical” components of the physical legs
proven, that the desired limit cycle is essentially glob- without damaging their other degrees of freedom. In
ally asymptotically stable. this paper, we are content simply to extend our emerg-
We have not yet formalized the notion of behavioral ing notion of “phase” to a pair of two-degree of freedom
composition (as we have begun to do for behaviors en- pogo sticks (the “Spring Loaded Inverted Pendulum”
coded as point attractors [7, 19]) but it represents a [35]) and assume that their individual phase regulation
strong unifying theme throughout the paper. The two- mechanisms are deadbeat. Once again, simulations
juggle mirror law may be seen as a kind of informal suggest that this is the appropriate generalization, but
“interleaving” of two one-juggle functions. However, we remain cautious regarding the larger implications
because we desire a more general compositional notion pending more realistic constituent models.
not tied to the (effective but very costly in sensory
effort) mirror constructions, we next apply our phase 1.2 Motivation and Relation to Existing
regulation method to “interleave” a very different style Literature
of individual controller. Specifically, with the appro-
Coupled oscillators have long been used to model com-
priate notion of phase coordinates described above, we
plex physical and biological settings wherein phase
are now able to consider for the first time the analogies
regulation of cyclic behavior is paramount [15]. The
between juggling and running.
biological reality of neural central pattern generators
Our second example concerns two vertical Raibert (CPGs) — oscillatory signals that arise spontaneously
hoppers [27], each of whose leg springs has an ad- from appropriate intercommunication between neurons
justable stiffness. Now, although both legs are par- — seems by now to have been conclusively demon-
tially actuated, the contact with ground is no longer strated in organisms ranging from insects [26, 12] to
instantaneous and we abandon mirror laws in favor of lampreys [9]. Mathematical models proposed to ex-
a Raibert style energy management strategy coordi- plain the manner in which families of coupled dynam-
nated over repeated intermittent stance modes so as ical systems can stimulate a sustained oscillation and
to nudge the total vertical energy toward that value stably entrain a desired phase relationship have become
which encodes the desired behavior. When the legs are progressively more biologically detailed [8, 13, 16]. But
decoupled from each other, arguments nearly identical the work presented here has relatively little overlap
to those we have developed in past work [20] yield es- with that literature. While we are intrigued by the
sential global asymptotic stability of the two indepen- capabilities of purely “clock driven” systems [36, 32],
dent vertical “gaits”. Note that the reference energy it seems clear that no significant level of autonomy can
is achieved asymptotically, rather than by a deadbeat be developed in the absence of perceptual feedback.
one step correction as was assumed in the first exam- The present investigation cleaves to the opposite (ie.,
ple. Nevertheless, applying the identical phase regula- perceptually driven) end of the sensory spectrum in
tion scheme yields a closed loop system that exhibits adopting the device of a “mirror law” [6] with its com-
in simulation the same striking coordinated behavior mitment to profligate sensory dependence [30]. In this
as we have proven to hold true (at least locally) in the sense, the present work bears a closer relationship to
Toward the Regulation and Composition of Cyclic Behaviors
207
the biological literature concerned with reflex modu- ever, as the degrees of freedom increase, the burdens of
lated phase regulation [11]. high dimensionality make centralized control schemes
Many tasks in robotics and automation entail a prohibitively expensive. There is considerable interest
cyclic exchange of energy between a machine and its in developing cyclic behaviors that are as decoupled
environment. This is evidently the case for legged lo- as possible, promoting decentralized regulation. Our
comotion systems as well as for many less obvious ex- present model for pursuing this desideratum is pro-
amples wherein a mechanism repeatedly changes its vided by our initial work on concurrent composition
local “shape” so as to effect some global “progress” of point attractors [19]. Since our reference flows have
[24]. When viewed from an appropriate geometric per- gradient-like cross-section maps, we harbor some hope
spective, the recourse to repetitive self-motion may be that the connection may be forthcoming.
interpreted as a means of “rectification” — exercising
indirectly the unactuated degrees of freedom through 2 Preliminary Discussion
the influence of the actuated degrees of freedom aris-
ing from an interaction between symmetries and con- We start in Section 2.1 by defining phase coordinates
straints [2]. Because our notion of a task is so com- that enable us to re-cast physical equations involving
pletely bound up with a closed loop dynamical inter- potential and kinetic energy as geometric equations re-
action between the robot and its environment, this lating progress around a circle and its velocity. In the
invaluable geometric control perspective provides no examples at hand, the physical control variables are
solution but merely a complete account of the (open used to adjust the energy of the unactuated degrees of
loop) setting within which our search for stabilizing freedom upon their intermittent contacts with the ac-
feedback controllers can begin. Since the dynamics in tuated components. In phase coordinates, the phase
question are inevitably nonlinear, the relation between velocity of each constituent subsystem is subject to
open loop controllability properties and feedback sta- control at each impact, and effects a corresponding re-
bilizability properties is far from clear. setting of the various relative phases.
In our understanding, the most relevant connection Having arrived at a convenient model space, the
to date remains the nearly two decade old observa- torus, we next examine in Section 2.2 the notion of
tion of Brockett [4], precluding smooth feedback sta- a “reference field” — a family of limit cycle generating
bilization to a point in the face of conditions known vector fields on the k-torus whose return maps on the
[3] to characterize the nonholonomic constraints that (k — 1)-torus admit as a Lyapunov function a “Naviga-
appear in the present underactuated setting [22]. At tion Function” [23] down to the fixed point. The topo-
the very least, this fact necessitates the appearance logically unavoidable repellors can be identified with
of hybrid controllers — feedback laws whose resulting the application as phase pairs that are to be avoided
closed loops make non-smooth transitions triggered by (e.g., when both balls come down at exactly the same
state — in the case of tasks encoded as point attrac- of time). Although our ultimate constructions appear
tors [22]. In the present situation, when tasks are en- as maps of an appropriate cross section (so the topolog-
coded as limit cycles, we are aware of no similar nec- ical constraints appear to lose their force) these toral
essary conditions. Nevertheless, the feedback solutions maps are classical objects and yield very convenient
we construct have a strong hybrid character. Since the and workable targets.
nonholonomic constraints in our setting arise from the
2.1 Controlling Phase
“underactuated” nature of the problem [21], it seems
intuitively clear that the robot’s work on the compo- Let ft: Rx X > X bea flow on X. We are concerned
nents of its environment must be punctuated by inter- with flows that are cyclic in the sense that a global cross
mittent repeated contacts. section can be found. Formally, a global cross section
One last influence on the present work that bears ¥ is a connected submanifold of X which transversally
some comment concerns the possibility of composition. intersects every flowline. For any point « € X, define
Since good regulation mechanisms are hard to find, the time to return of z to be:
there is considerable interest in developing techniques
for composing existing behaviors to get new ones. How- t+ (x) = min{t > 0| f*(x) € 5} (1)
208 E. Klavins, D.E. Koditschek, and R. Ghrist

Of course we really want to control the system so

that the return map P has a stable fixed point at some
z*. Whether or not h~1(0,w) = (0,4*) depends on
the dimension of D. If dim © = 1, as it will be in the
examples we supply, then the preimage of w is indeed
a point.
The main contribution of this paper concerns
the composition or interleaving of two such sys-
tems. That is, we suppose that we have the system
(py iti 09502) 2 with corresponding phase coordi-
Figure 1: The relationship of t~(x) and t* (zx) to a. nates (¢1, 61, b2, b2) € Y*. As before, system 7 may
only be actuated when ¢; = 0. In the examples we
will consider, we suppose that the systems cannot be
and define the time since return of x to be actuated simultaneously. Thus the set of states where
t” (xz) =min{t >0| f(x) € d}. (2) ¢1 = ¢2 = 0 should be repelling. We will design a
controller such that the attracting limit cycle is given
The first return map, P : & >, is the discrete, real by:
valued map given by P(x) = f* (z).
Let s(x) = t*(x) +t7(z). s is the time it takes the G = {(d1,
¢1, 62,42) | br = bo + 5(mod 1)
system starting at the point f*(x) € D to reach Y A ¢,=¢2 =}. (4)
again. Now, define the phase of a point x by:
The constraint ¢; = ¢2 + 4 (mod 1) encodes our desire
(3) to have the pair of phases as far from the situation
¢1 = ¢2 = 0 as possible. In fact, we will consider
Notice that the rate of change of phase, ¢, is equal to the more general case wherein the phase velocities are
1/s. The relationship of these functions to © is shown controlled to («A,«B) for some A,B € Z and scaling
in Figure 1. Therefore, ¢ is constant or piecewise con- factor kK.
stant, changing only when the state passes through ©. To analyze and control such a system, we restrict our
In Section 3, we give a one-dimensional example attention to the sections ©; C Y? and N_ C Y? defined
(Juggling) where h : X — Y, defined by h(z,z) = by ¢1 = 0 and ¢2 = 0 respectively. Suppose that the
(¢,¢), is actually a change of coordinates where Y = flow alternates between the two sections. Let Gt =
S51 x Rt. We use the section © € X defined by z = 0 Ho F‘o H~* be the flow in Y? conjugate to the flow
which corresponds to the set of states where the robot in X? where F = (f, f) and H = (h,h) and 7;(w) =
may contact (and thereby actuate) the system. The min{t > 0| Ho Fo H~1}(w) € ¥3_;}. Start with a
image of this section h(X) will be given by the set point w € by. Let w’ = G™(w) and w” = G™(w’).
C = {(0,¢) | 6 € Rt}. Because we consider intermit- We have w’ € Hy and w” € Dj, so we have defined the
tent control situations, it is only in this section that return map on 4. Now since G is parameterized by
@ may be altered by the control input u. That is, we the control inputs wu; and u2 we get:
change ¢ according to a control policy u to get the re-
RD eae (0, $1, $2, b2) ee
turn map P’ : € > @ given by P’(0,¢) = (0, u(¢))!. ($4, u1,0, d2)

We design the controller so that there is a unique > ww = (0, u1, 5, ua) .
and stable fixed point at some desired phase velocity
Thus, the phase velocity updates wi(w) and u2(w’)
P= Ws
must be found so that (4) is achieved. We do this
‘In Section 3.1, deadbeat control of the phase velocity with two examples in Section 3 and prove the stability
is possible. In Section 3.2, the control of phase velocity is of our method in Section 4.
asymptotically stable. Our analysis in Section 4 depends on
the former. We believe a similar treatment will eventually Notice that a single phase describes a circle $1 and
apply to the latter. two phases describe a torus T? = $1 x $1. In the next
Toward the Regulation and Composition of Cyclic Behaviors
209
section, we define a “reference” vector field on the k- locked attracting periodic orbit, the winding set must
dimensional T* which encodes the ideal behavior of consist of a unique winding vector. In Section 3.1 we
the system as though it were fully actuated. Then, we present a system consisting of a piston which must ver-
show how to use the field to generate velocity updates tically juggle two balls so that the first bounces A times
as above. for every B times the second bounces (See Figure 4),
where A and B are integers: the winding vector is thus
2.2 Construction of a Reference Flow on T* (A,B) (rescaled to unit length). The generalization
of this situation to n juggled items requires a winding
The problem of composing dynamical systems with
vector of integers (Ai, Ao,..., An).
point-goal attractors is relatively straightforward, due
Our goal is to couple systems with unique attrac-
in no small part to the convenient topological fact that
tors satisfying the above restrictions in such a manner
the product of a zero-dimensional set (a point goal)
that the product system remains in the same dynam-
with a zero-dimensional set is again a zero-dimensional
ical class: a single attractor with appropriate winding
set: point-goals are well-behaved with respect to Carte-
vector. In addition, the existence of unstable invariant
sian products. This is not so for the case of systems
sets (in general forced by topological considerations) is
with an attracting periodic orbit. The Cartesian prod-
desirable for setting up “walls” of repulsion in the con-
uct of k such continuous systems gives rise to a flow
figuration space. For example, in juggling it may be
with an attracting k-torus T* = S$! x.--x S!. The
desirable for the configuration wherein both balls hit
desired behavior for a flow on this set is again an at-
the paddle simultaneously to be a repellor. For both
tracting periodic orbit; however, such mode locking can
attractors and repellors, the freedom to “tune” these
occur only if the oscillators are coupled. More unfor-
invariant sets geometrically is a necessity. We thus
tunately, such dynamics arise only through a relatively
turn to a brief exposition of two appropriate classes of
careful tuning of the individual systems and their mu-
reference flows on the k-torus T* which will serve as
tual couplings. Baesens et al. [1] carefully explore the
skeletons for the goal dynamics of the control schemes
intricacies of this problem, illustrating the prevalence
to be constructed.
of complexity in both the dynamics and the bifurcation
structures of flows on the attracting T* in the (osten- The flows we consider on T* will all have global cross
sibly simple) case k = 3. sections © homeomorphic to T*~!. To obtain a unique
attracting periodic orbit for the flow, we specify the
An important measure of complexity for the dynam- appropriate dynamics on the cross-section and accord-
ics of a flow on a torus T’ is the set of winding vectors. ingly suspend to a flow: the flow is then determined
Choose a lift ¢; of the flow ¢; on T* to the univer- by the dynamics of the return map and the desired
sal cover R* of T*. Then, consider for each x € T* winding vector.
with lift z © R* the vector ¢,(Z) normalized to unit
Consider the diffeomorphism which is the time-1
length: denote this by w;(Z). This vector lies in the
map of the gradient field -VV, where:
unit (k —1)-sphere S*—! C R* of directions in R*. De-
fine w(x) C S*-1 to be the set limit points of w;(Z) k-1

as t > oo. This set (independent of the lift 7 in the V:x-R (01,02,--.,Ok—1) + & )- cos(276;).

case of a nonsingular flow). defines the winding vectors i=1

of x. The union of w(x) over all 2 € T* comprises the (5)
Here © & §!x---x S$! is parameterized by k—1 angles
winding set of the flow. Winding vectors/sets are the
6; € [0,1] and «& > 0 is an amplitude which controls the
continuous analogues of the rotation vectors/sets de-
rate of attraction. The dynamics of this return map de-
fined for torus homeomorphisms?: cf. the discussions
couples into the cross-product of the circle maps which
in [1, 25] in the context of coupled oscillators and [34] have the “north pole” (0; = 0 for all i) as a repellor
for a topological generalization to arbitrary spaces.
and the “south pole” (0; = 1/2 for all i) as an attrac-
The systems we consider have specific constraints on tor. It thus follows that (5) has exactly (k—1)-choose-j
the winding vectors. In order to have a single mode- hyperbolic fixed points whose unstable manifold is of
dimension j. There is thus a unique attracting fixed
?More specifically, for those homeomorphisms which are
continuously deformable to the identity map. point, and V defines a navigation function [29] for the
210 E. Klavins, D.E. Koditschek, and R. Ghrist

QY?
o— > 0 «———_0

ae
maximize
minimum
distance

Figure 2: (left) The ideal reference dynamics on a ite

Figure 3: Embedding distinct phases as the “beads on a
cross-section to a flow on T° having a single attracting fixed
circle” problem. The beads must rotate around the circle
point. Here, the 2-torus is represented as a square with op-
while maximally avoiding their neighbors.
posite sides identified. (right) A reference flow on T* with
winding vector (3,2). The repelling orbit passes through
the origin. The appropriate cross-section here is the circle with its last column given by (Ai,..., Ax)-
along the “diagonal” of the square. This construction can be tuned so that the attract-
ing orbit does not pass through the pairwise “obstacle”
Poincaré return map of the flow. See Figure 2(left) for where two phases are identical. However, in the re-
an illustration of the dynamics in the case k = 3. sulting system the obstacles become dynamically neu-
tral — neither attracting nor repelling. In applications
The existence, quantity, and placement of the unsta- where these obstacles are not physically meaningful, we
ble invariant manifolds in the dynamics of (5) are gov- may use this construction. Otherwise we must design a
erned by Morse-theoretic considerations (see [29] for reference field wherein these obstacles are dynamically
applications of Morse theory to the design of naviga- repelling.
tion functions). Of particular interest is the forced ex-
istence of repelling unstable invariant manifolds of all One manner of generalizing (5) to a flow on T* which
dimensions. This is extremely relevant to the control avoids determining a complicated coordinate change
problem in that the “obstacles” in the configuration and which may be suitable in applications where the
space (where the “paddle” is forced to contact several obstacles are important is as follows. We imagine the
distinct elements simultaneously) can be of variable di- phases of the system as coming from k distinct point
mension. The prevalence of unstable manifolds in the on a circle. Each point must rotate around the circle
dynamics of (5) is a valuable resource when one wants with some velocity and the distance between any two
to “tune” the dynamics on the configuration space. consecutive points must be maximized, as in Figure 3.
The potential function:
Consider the problem of designing a vector field on
T* such that all orbits possess a unique winding vector V:T' oR
w which points in the direction (A;,..., Ag) € Z*, as- (b1,-+-,¢e) + > cos (2n[Aid; — A;¢i]) (6)
suming that the A; are all relatively prime. The cross- i<j
sectional dynamics of this system will be conjugate to
(5) for an appropriate cross-section: Namely, the cross- is used to accomplish the latter task. Here the coordi-
section which is the orthogonal complement to 7, where nates ¢; € St = [0,1]/{0 ~ 1} are angular coordinates
T is an integer vector satisfying T - (Ai,..., Ax) = 1, on T*. The function V attains a global maximum along
see [1, App. A]. To obtain a reference flow with the the straight line (mod 1) through the origin tangent to
desired winding vector, we may suspend (5) to a flow the winding vector (Aj,...,A,). The function attains
on T* and then change coordinates so that the attract- its global minimum along a shifted parallel line (mod
ing orbit in the new coordinates has slope (Aj,..., Ax). 1). The addition of a global drift term in the winding
Supposing we start with the slope (0,0,...,1), we de- direction gives a realization of the desired flow.
sire a linear map M on R* (the covering space for T*)
$i = mA; — (VV); (7)
such that M - (0,0,...,1) = (A1,...,A,) and so that
M, when projected onto the torus is a change of coor- See Figure 2 (right) for an illustration of the two-
dinates. This amounts to requiring that M € SL(n,Z) dimensional case which is used in the examples in this
Toward the Regulation and Composition of Cyclic Behaviors
211
With this field the obstacles are indeed repelling, but and the field is:
there are several attracting orbits, one for each of the
(k — 1)! arrangements of k beads on a circular wire.
For applications such as juggling any of these orbits
R(b1, $2)" =k (e )— k2VV(¢1,¢2), (10)
will represent a viable juggling behavior. For other ap-
plications, such as controlling the gait of a legged ma- the two dimensional instantiation of (6) and (7). Here
chine, further tuning to achieve the correct order will Kg is an adjustable gain which controls the rate of con-
vergence to the limit cycle. The lines Ady = Bd, and
be required. In the case of k = 2 that we specifically
Ad¢g2 = Bd, + i are equilibrium orbits. The first is
consider in this paper, the attracting orbit is unique
unstable, the second is stable.
and maximally bounded away from the origin.
3.1 Juggling
3 From Juggling to Running
Consider the system wherein a paddle with position p
In this section we examine in detail the task of regu- controls a single ball with position b to bounce, repeat-
lating the phases of just two cyclic processes. We will edly, to a prespecified apex. We suppose the paddle al-
consider intermittent contact systems. For example, ways strikes the ball at p = b = 0 and instantaneously
a ball being bounced on a controllable paddle is an changes its velocity according to the rule:
intermittent contact system where the phase velocity
corresponds to the energy of the ball between contacts. bnew = —ab+ (1+ a)p (11)
Another type of intermittent contact system is one that
has a stance mode, that is, ¢; is controllable only when where a is the coefficient of restitution in a simple ball
gi € [a,b] C [0,1] for some a and b. A hopping robot, and paddle collision model. We suppose that the ve-
for example, is in its stance mode when it is touching locity of the paddle is unchanged by collisions. Evi-
the ground. Only in stance mode may the controller dently, a paddle velocity of p = (a —1)/(a+ 1)b will
change the energy of the robot. We will not consider
set bnew = —b. Now define n = $b” + bg to be the total
energy of the system. By conservation of energy, 7) = 0
stance systems in general but instead show how to con-
between collisions. Set 7* to be the desired energy
sider certain models of hopping robots as though their
(corresponding to a desired apex). Define a reference
phase velocities were determined by their phase veloc-
trajectory for the paddle to follow by 4 = cb where:
ities at the single point ¢; = a = 0, thereby reducing
the problem to one apparently involving instantaneous
contact. CR
Sail et
We are concerned with regulating the two systems
is constant between collisions. jz is called a mirror law
so that (1) the rate of change of each phase is some
because it defines a distorted “mirror” of the ball’s tra-
desired value (i.e. the first system oscillates A times
jectory. As the ball goes up the paddle goes down and
for every B times the second does) and (2) the phases
vice versa. K is a gain that adjusts how aggressively the
are maximally separated. That is, we require that:
controller minimizes the energy error. The analysis of
this system in [5] proceeds, roughly, as follows. A “re-
Ad» = Bos + 5(mod 1)
(3) =m ey eae
turn map,” mapping the energy just before a collision
to the energy just before the next collision, is derived.
(8) It is shown that the discrete, real valued dynamic sys-
where «1 scales the phase velocities A and B to values
tem that results is globally asymptotically stable by
reasonable for the system.
showing that the map is unimodal [10] with parameter
We construct a reference vector field on T? with this « adjusting the period of the map.
circle as a limit cycle such that (¢1,¢2) = «1(A,B)
along the cycle as described in Section 2. This field 3.1.1 Phase Regulation of Two Balls
encodes the ideal behavior of the system as though it
To control two balls to bounce on the paddle so that
were fully actuated. The potential function is:
one hits exactly when the other is at its highest point
V(¢1,¢2) = — Bor) .
cos(2n[Ads (9) as in Figure 4(a), we will use the reference field (10).
212 E. Klavins, D.E. Koditschek, and R. Ghrist

@ x»
pe
piston

(a)

Figure 4: Models considered in this paper: (a) two balls juggled on a piston; (b) two Raibert style hoppers, hopping out of
phase; (c) two legged spring loaded inverted pendulum (SLIP) model of a biped.

This represents a point of departure from earlier work form (v1, Yi » Y2; yo) ae (yr +yt, V1, Y2 +Yyet, Yy2) between
on juggling two balls where a phase error term was collisions.
combined with two mirror laws somewhat informally. We define reference trajectories jz; and pg based on
We construct a mirror law for each system separately the mirror law idea. Given any pair (@1, ¢2) we define
and then combine the laws into a single mirror law us- a lookahead function Cy : [0,1]? — [0,1]? for ball one
ing an “attention function.” First define the phase of a which gives the phase of ball two at the next ball one
ball according to the discussion in Section 2.1. Suppose collision. Thus:
a ball rebounds from a collision with the paddle with
velocity by. By integrating the dynamics b = —g and Ci (¢1,2) = 1 — $1) + oe.
noting that collisions occur when 6 = 0, we obtain the 1
time since the last impact and the time between im- We desire that after this collision, bean. = uy = Rye
pacts, a computationally effective instance of (1) and C1(¢1, ¢2) (ie. the control input u; follows R). Since
(2), as:
Oi ebay =O ALE rican ls eee reer (since
b; = 0 at the collision), we have, using (11),
(paul and Reiter to (12)
g g oe :
Pi,new ae ae a cr(L+-a)eiby = R1 0 C1(¢1, 2) (14)

respectively. The change of coordinates h : (R* x R) —

(0,0) + S* x Rt from ball coordinates to phase coor- where c; is the coefficient in the mirror law trajectory
dinates is given by h(b, b) = (¢, @) where, following the Lig= ci}. Solving for c, and using the fact that at
recipe (3), we take: by = 0, b} = 2m, gives:

= SN 1 ae g . (15)
nk a ange oe 13 (1+ a)V/2m bs Ri 0 Ci(G1,
G2)
? § 2bo : 2bo ( )
A similar expression for co can be obtained in terms
In this manner, for a two ball system with ball posi- of Ryo Cy. This gives us a mirror law for each ball.
tions b; and b2, we obtain two phases ¢1 and ¢2. Notice However, we have only one paddle so we need an at-
that ¢; € [0,1] and that ¢; is constant between colli- tention function s : [0,1]? — [0,1] and a new reference
sions between ball 2 and the paddle as established in trajectory composed of jz; and pio:
Section 2.1. The velocity bi is reset instantaneously
upon collisions, corresponding to the update rule (11). be = Su, + (1—8)pe.
In the rest of this section we will take advantage of Such a function is fairly easy to define for specific in-
the fact that the flow Gt = H o F' o H™!, described stances of A and B. A more complete treatment of
in Section 2 and instantiated here, has the very simple attention functions can be found in [18].
Toward the Regulation and Composition of Cyclic Behavio
rs 213

Figure 5: (a) The positions of the two balls and the paddle vs. time for A:B = 1:1 starting from a randomly chosen initial
condition. (b) The phase plot $1(t) vs. $2(t) for the same run. Note the limit cycle where $1 = $2 + +. (c) and (d) show
the same information for A:B = 1:2.

We have simulated this system and have found it to as follows:

work as expected. Figures 5(a) and (b) show a run
ie,
of the system with A: B = 1:1 and Figures 5(c) i = —w(1+ 6?)a — wpe
and (d) show a run of the system with A: B = 2:1.
In both cases the paddle regulates the phases to very
—w3(1+ 3)" — 2u2 Got
near the limit cycle within two or three hits of the balls. ie 0 flight
After presenting two more examples of phase regulated if c<OAz<0O compression (16)
if c<0Axz>0 decompression
systems, we give an analysis of this controller for the
1:1 case in Section 4. where w and ( are parameters which determine the
spring stiffness w?(1 + 67) and damping 2wG. During
3.2 Synchronized Hoppers decompression, w2 = wr and (2 = 3/r. We define 7,
the thrust, by tT= «/(1 +2?) where zy is the lowest
In this section we examine a model of a bouncing point point reached by the mass the last time the decom-
mass reminiscent of Raibert’s hopper [27]. A single, pression mode was entered and & is a gain which de-
vertical hopping leg is modeled by a mass m = 1 at- termines the energy of the leg (and which we will use
tached to a massless spring leg. The hybrid dynam- as a control input in the next section). Notice that the
ics has three discrete modes: flight (the leg is not damping during decompression, 2w22 = 2w, is the
touching the ground), compression (the leg has landed same as during compression. The spring stiffness dur-
and is compressing), and decompression (the mass has ing decompression is w3(1 + 33) = w?(r? + 6?) and is
reached its lowest point and is being pushed upward). thus proportional to the magnitude of the thrust. The
These latter modes each have the dynamics of a lin- trajectory in the position/velocity plane of a simula-
ear, damped spring. Flight mode is entered again once tion of this system is shown in Figure 6 (a). That this
the leg has reached its full extension. The equations of is a stable system follows from an argument similar to
motion, borrowed and altered somewhat from [20], are that made in [20] wherein similar systems are shown
214 E. Klavins, D.E. Koditschek, and R. Ghrist

Figure 6: (a) A plot of the velocity vs. the position of a simulated one legged hopper emphasizing the stable limit cycle. (b)
A plot of the position and phase of the hopper vs. time. Between minimal points of the hop, phase velocity 1s constant.

to have global asymptotic stability. Thus, a leg cannot one lowest point to the time of the next. Next, smooth
be controlled to a specified apex height in one bounce this map so that each segment has the same slope.
as could the balls in Section 3.1. First, note that 0g varies between 0 and 94min, Of
To obtain a definition of phase for the leg, we change varies between 0 and 1 and @, varies between 9¢maz
coordinates to real canonical form in the compression and 0. Thus:
and decompression modes. This is demonstrated in
A 6 = a) 1
[20]. We arrive at systems (F.,6.) = (—2wBE-,
—w)
and (Ea,6a) = (—2w@Eqg,—wr) for compression and
decompression respectively. Note that the changes in
the second coordinates of these systems are constant.
is the first transformation.
They can therefore be used as phase variables. In com-
pression, for example, 6. = w and 0, varies between Second, think of each of these phases as a linear
Cana, and 0. Thus: homogeneous transformation Pz : [0,t:] — [0,1/3],
Py : [ti1,t2] > [1/3,2/3] and P. : [te,t3] > [2/3,1]
where t;, t2 and t3 are the durations of the liftoff,
Of= Wil = 0, mas— (17)
touchdown and bottom modes, respectively. We de-
sire the phase to increase from 0 to 1 linearly as time
where t, is the duration of the compression mode. This increases from 0 to t3. The following, final definition
expression is a scalar multiple of the general phase de- of phase for the hopping leg system, has this property:
finition described in Section 2.1. However, to control
two physically unconnected legs to hop in a synchro- HP (6,1) Hz<UKke=0
nized manner as in Figure 4(b), our present construc- 624 HP;*(6;,1) ife>0 (19)
tive approach requires constant phase velocity through-
HP -+(0,. 1\wsil & = OA es Oe
out each cycle of a hop as in Figure 6(b). We must
transform three piecewise linear phases so that each Here H is the transformation (z,1) +> (z/t3,1). This
has the same rate of change, as assumed in the dis- results in a phase definition for a single leg that is
cussion of Section 2.1. We begin with the phases dur- equivalent to t/(t; + t2 + t3) as in (3). Figure 6 (b)
ing compression and decompression, 8, and 64 and the illustrates how @ changes with the position of the mass
phase during flight, 05 = ce in simulation.
The construction of a suitable phase definition from For a system with two physically unconnected legs
64, 9. and @¢ is in two steps. First, apply affine trans- modeled by (16) with states (1,41) and (zo, #2), let
formations so that the decompression phase varies from 6, and 4 be the phases of the legs. Since each leg ac-
0 to 1/3, the flight phase from 1/3 to 2/3 and the com- tuates itself, there is no need for an attention function.
pression phase from 2/3 to 1. This gives a piecewise Once again, we wish to control the legs so that they
linear map from interval of time connecting the time of are hopping to a prespecified height and so that they
Toward the Regulation and Composition of Cyclic Behaviors
215

(a)
Figure 7: (a) The positions of the leg’s centers of mass versus time for A:B =1:1 starting from a randomly chosen initial
condition. (b) The phase plot 01(t) vs. 62(t) for the same run. Note the limit cycle where 6; = 02 + z-

are out of phase: one leg is at its lowest point when again consists of compression and thrust phases. Be-
the other is at its highest. We use a reference field R tween hops, a leg must swing around the mass to ready
wath AB == 431. itself for the next hop. This is called the swing phase.
The task is to construct a controller for the legs that
Recall that the thrust, 7;, supplied by a leg is con-
realizes this gait.
stant through the decompression phase. The gain for
each thrust, «;, controls the phase velocity. A larger This example is different from our previous exam-
thrust gives a smaller phase velocity (because the leg ples in that we start with a specification of the ag-
goes higher and takes longer to return to its lowest gregate behavior of the system and decompose it into
point). Thus, we simply reset the thrust gains, «; to controllers for the individual subsystems. In the jug-
be 1/R; (61, 82) whenever 6; = 0, i = 1,2. gling and hopping examples, the phase regulated sys-
tems are themselves cyclic. For example, in the 1:1
Figures 7(a) and (b) show simulations of the system case, the aggregate phase of a phase regulated system
with A: B =1:1. In all runs with various reasonable at equilibrium is
values of the parameters the legs regulate their phases 1 lige
to very near the limit cycle within two or three stance 5(o1 + G2 — 2) if bi < 2 (20)
agg (hi, $2) = {es + 2 + 4) otherwise .
modes.
In the juggling example, dag, is a measurement of the
3.3. Two Legged SLIP phase of the paddle. In this example, ¢ag, will be
a measurement of the phase of the underlying SLIP
One obvious shortcoming of the synchronized hopping model which the legs will then “service” according to
model of walking is that the two legs are physically the pseudo-inverse of @agg:
unconnected. In this section we examine the Spring
Loaded Inverted Pendulum (SLIP) model [35] of a hop- thao) =(b0+5) (mod 1). (21)
ping leg which we have modified to have two legs as
That is, the phases of the legs are functions of the
shown in Figure 4(c). The SLIP model has a single
phase of the underlying SLIP model. It remains to
point mass (which we assume to be 1 in this paper) in
the plane and a massless, spring loaded leg. When it define phase for the SLIP model.
is not touching the ground, its dynamics are ballistic. As shown in Figure 4(c) the system is described by
When the toe of the leg is touching the ground, the variables x,y, ,, and 62 where (x,y) is the position of
spring of the leg exerts force on the mass along the di- the body and 6; and 6, are the angles of the legs. When
rection of the leg. Our slightly different model consists in stance mode, the position of the body will also be
of a mass with two rotating, massless legs which we call described by the distance r from the body to the toe
the roadrunner. In an alternating gait, the roadrunner (given by (Ztoe, y)) and the angle of the leg touching
uses one leg to hop and then the other. When a leg the ground. When a leg is compressing, 7 < 0, the stift
is touching the ground, it is in stance mode — which ness of the spring is k; and when it is decompressing,
216 E. Klavins, D.E. Koditschek, and R. Ghrist

r > 0, the stiffness is ko. The spring model we will use define the phase of the SLIP model so that it completes
has potential: two cycles between 0 and 1 instead of one cycle so that
(21) makes sense in the present context. We neglect
kof a 1 this detail here.
Now define the position of the legs during their swing
phases as a function of the phase. Let 4.) be an angle
where | is the natural length of the leg and k is the
near the middle of the swing phase, such as 7. We give
current spring stiffness. The dynamics of the system
can be derived from the Hamiltonian as in [35] for the each leg a discrete state s; defined by:
cases with or without gravity in stance. We consider 0 if 6; <G 6; < Dee
only the case without gravity. We have during flight: 54= Tif Giop < 0: — Usa
2 otherwise (leg is touching the ground) .

Pee).
ge
y
=
a)
and é
Ae
=
U2 Thus, a leg is characterized by a sequence such as
(0,1,2,0,1,2...). We define reference maps (functions
(22)

where g is the force of gravity and u; and ug are veloc-

of phase, which is in turn a function of the state of the
ity inputs the legs. During stance, suppose that leg 1
body) Oref,o and O,ef,1 as functions of the leg phase
is touching the ground and leg 2 is not so that 6 = 6.
which give the ideal trajectory of a leg during each of
Then we have:
the discrete states 1 and 2. 0,¢7,9 varies between 4 and

Pee eese 1) (ya)

Mop as @ varies from 0 to 1 so that Bre f,o is equal to 0
0 —2r6/r 05 U2 at liftoff. 0,¢f,1 varies from Oop to O4¢ as @ varies from
0 to its value at touchdown. These may be smoothed
where uz is a control input. The equations for when in various ways to minimize, for example, the velocity
the legs are reversed are similar. We do not consider of the toe relative to the ground at touchdown. There
the case where both legs are touching the ground or is no reference phase during stance because when a leg
when the mass hits the ground—situations we would is in stance it is not actuated. If the discrete states
like to avoid. In [33], the control of the SLIP model is of the legs are initially different, they will alternately
discussed. We do not repeat this discussion here, but service the stance mode of the robot.
simply assume that upon liftoff that 6:4, ky and ke are
given by the controller. 4 Analysis of the Phase Regulation
‘The phase of the virtual leg will once again be com- Algorithm
posed of the phases of flight, compression and decom-
pression. In the rest of the section, variables sub- We have presented three examples of phase regulation
scripted with / represent the state at liftoff and those that differ in several important respects. In the jug-
subscripted with td represent that state a touchdown. gling controller, we are assured that the reference field
As in the previous example, the phase is obtained from can be followed closely because of the deadbeat na-
a piecewise linear transformation on the phases dur- ture of the ball control. That is, within the limits of
ing the various modes. We use the results from [35] the actuator, we can achieve any desired ball energy by
wherein the systems are integrated to obtain the du- striking it with the paddle using (11). Therefore, to an-
rations of the flight, compression and decompression alyze the stability of the control method, we need only
modes, ty, t- and tg respectively. For a given state w consider the system in terms of the phase states and
of the leg, equation (2) defines t~(w) to be the time velocities. We do so in this section. With the synchro-
since the last lift off. Then the phase is: nized hopping example, we do not have deadbeat leg
t- control, but only asymptotic stability. Thus, to ana-
lyze the stability of the hoppers, we would need to take
eRe remmrerer et in to account the rate of convergence of a single leg to
the reference phase velocity dictated by the reference
Notice that the phase varies between 0 and 1. Since field controller. We have not yet performed this analy-
each leg will service every other stance mode, we could sis. However, because of the fast rate of convergence
Toward the Regulation and Composition of Cyclic Behaviors
217
of the single leg controller in practice, the analysis in Since the x and z advance functions are not functions
this section is likely appropriate. The two legged SLIP of y, we can treat y as an output of this system. Thus,
controller, in a sense, needs no further analysis. If we analytically, it will suffice to treat (24) as an iterated
assume that the legs can follow the reference trajectory map of the the variables (z,z) € S! x R* given by
accurately, the model is the same as the original SLIP F(&x, 2) = (@e+41, 241). We have the following fixed
model [35]. point conditions:

4.1 Analysis Proposition 4.1 F(z,z) = (a,z) if and only if

Ja in ell bese a) oa
Consider the phase regulated system (41, $2, $1, ¢2)€
T? x R? where di is constant except for discrete jumps
We omit the proof, which is straightforward algebra
made when ¢; = 0. These jumps are governed by the (note that the values of x are always taken modulo 1
reference field (10). That is, when ¢;= 0, di be- since « € $'). For the reference field we are using, we
comes R(¢1,¢2). Notice that when A: B= 1: 1, have:
then R(0, d2) = R(¢1,0). To simplify notation in this
section, we redefine R : S' — R to be the reference Corollary 4.1 Jf R(¢) = kK; — kgsin(27¢), then the
field restricted to ¢; = 0. Therefore, with A:B = 1:1, only fixed points ofF are (1/2,«1) and (0,K1).
R(g) = K1 — Ke sin(27¢@).
To analyze the dynamics of this system, we consider We wish to show that the first fixed point, (1/2, 1),
the Poincaré sections ©; and Y2 of T? x R? given by is stable, since it corresponds to the situation where
the two subsystems are out of phase and at the desired
¢; = 0 and ¢2 = O respectively. We suppose that
velocity. To do this, we examine the Jacobian. Suppose
adjustments to the phase velocities alternate between
the two phases (i.e. the system is near the limiting
that the fixed point condition we desire is F(1/2,v) =
(1/2,v) where v is the desired phase velocity. Then:
behavior). We construct the return map from ©, into
», as follows. Start with a point w € %}4, integrate the

en a
74 (#-1)m-24+1
me
_{ 7 ~$-Z
0 402
system forward to obtain a point in /2, then integrate
again to get a point in f(w) € ¥1.
A point in ©; has the form w = (0, ¢2, di, d2). This Here, m = &’(1/2) is the slope of R at 1/2. F is stable
maps to the point w’ = (C),0,R(¢2), d2) € Ne where at (1/2, v) if the eigenvalues of the Jacobian lie within
C; is the phase of the first system when the trajectory the unit circle. Values for m and v which guarantee
of the total system first intersects U2. w’ in turn maps this are not difficult to find. For example,
to the point f(w) = (0, C2, R(d1), R(C1)) where C2 is
the phase of the second system when the trajectory Proposition 4.2 [f tite
= 2uv — 2 then the eigenvalues
next intersects ©;. The phases Cy and C2, which can of Ja) F are 0 and +; —1 which implies that (1/2, v)
be obtained via the point-slope formula for a line (in is a stable fixed point of F whenever v > 1.
the ¢1, ¢2 plane), are given by:
Once again, the proof is just a calculation: simplify

Cyr =
PERG amet. yeienh (OY)
hes 2) ey — R(d2) (1—C,) . (23)
(25) using m = 2v — 2 and compute the eigenvalues.
With the reference field we are using, m = 27K. Thus,
for a given value of v, we set R(d) = v — FSsin(27¢).
Let (22, y,2) = ($2, 41,42). Then, expanding f(w), we In practice, it is not difficult to find other parameters
obtain a discrete, real valued map on Ne given by which make F stable. For a given v, we first choose
RPE) m to be quite small and increase it slowly until the
R (1= Lk) controller is aggressive, yet still stable.
c= me) = Lk)
oo R(Lx)
Poe Rae) 5 Conclusion
ag a | BE) — 2y)] In this paper we have taken the first steps toward a for-
(24) mal treatment of phase regulation for underactuated
218 E. Klavins, D.E. Koditschek, and R. Ghrist

environments that must be repeatedly and intermit- [3] A.M. Bloch and N.H. McClamroth. Control of me-
tently contacted by an actuated robot. We have in- chanical systems with classical non-holonomic con-
traints. In Proc. 28th IEEE Conf. on Decision and
troduced a variant of the two-juggle controller [5, 30] Control, pages 201-205, Tampa, FL, Dec 1989.
and, by re-writing the system in phase coordinates, ex-
hibited sufficient conditions for local asymptotic sta- [4] R.W. Brockett. Asymptotic stability and feedback
stabilization. In R.W. Brockett, R.S. Millman, and
bility of a 1:1 mode-locked rhythm. The obvious next
H.J. Sussman, editors, Differential Geometric Control,
step concerns the extent of the domain of attraction. chapter 3, pages 181-191. Birkhauser, 1983.
Here, there is a natural hybrid structure — the order
of “contact events” (i.e., the sequence of balls hit) — [5al M. Buehler, D. E. Koditschek, and P.J. Kindlmann.
Planning and control of robotic juggling and catch-
whose desired sequences might be seen as a pattern to ing tasks. International Journal of Robotics Research,
be regulated against disturbances. Moreover, there is 13(2), April 1994.
a “forbidden” set in phase space — where both balls
[6] M. Biihler and D.E. Koditschek adn P.J. Kindlmann.
must be hit at the same time — that must be shown
A family of control strategies for intermittent dynam-
to be a repeller. We have also suggested the manner ical environments. [EEE Control Systems Magazine,
in which this 1:1 “juggling” framework carries over to 10(2):16—22, 1990.
simple problems in legged locomotion. Because the ef-
[7—w R. Burridge, A. Rizzi, and D.E. Koditschek.Sequen-
fective input enters through an additional dynamical tial composition of dynamically dexterous robot be-
lag in such problems, our present sufficient conditions haviors. International Journal of Robotics Research,
for asymptotic stability will need to be modified in or- 18(6):534-55, 1999.
der to address them. We have not dealt at all with the [8] A. H. Cohen, P. J. Holmes, and R. H. Rand. The na-
problem of regulating more general A:B mode-locking, ture of the coupling between segmental oscillators of
but we believe that similar methods can be used to the lamprey spinal generator for locomotion: A math-
achieve such behaviors. ematical model. J. Math. Biology, 13:345-369, 1982.

Although the applications focus of this paper is lim- [9] Avis H. Cohen, Serge Rossignol, and Sten Grillner
ited to locomotion systems, we are intrigued by the (eds.). Neural Control of Rhythmic Movements in Ver-
tebrates. Wiley Inter-Science, NY, 1988.
prevalence of phase regulation problems in more ab-
stract settings such as factory automation [19, 17] and [10] P. Collet and J.P. Eckmann. Iterated Maps on the
will seek to apply these ideas in that context as well. Interval as Dynamical Systems. Birkhauser, Boston,
1980.

Acknowledgments [11] H. Cruse. What mechanisms coordinate leg movement

in walking arthropods? Trends in Neurosciences,
We thank Bill Rounds for providing many insights con- 13:15-21, 1990.
cerning the compositional semantics of dynamic sys- [12] F. Delcomyn. Neural basis of rhythmic behavior in
tems. This work is supported in part by DARPA/ONR animals. Science, 1980.
under grant N00014-98-1-0747 and in part by the NSF [13] G.B. Ermentrout and N. Kopell. Inhibition-produced
under grant IRI-9510673 at the University of Michi- patterning in chains of coupled nonlinear oscillators.
gan. It is supported in part by the NSF under grant SIAM Journal of Applied Mathematics, 54:478-507,
DMS-9971629 at the Georgia Institute of Technology. 1994.

[14] R. Ghrist and D.E. Koditschek. Safe cooperative ro-

References bot patterns via dynamics on graphs. In Robotics Re-
search, pages 81-92, 1998.
[1] C. Baesens, J. Guckenheimer, S. Kim, and R. MacKay. [15] J. Guckenheimer and P. Holmes. Nonlinear Oscilla-
Three coupled oscillators: Mode-locking, global bifur- tions, Dynamical Systems, and Bifurcations of Vector
cations and toroidal chaos. Physica D, 49(3):387-475, Fields. Springer-Verlag, New York, 1983.
1991.
[16] R. H. Harris-Warrick, F. Nagy, and M. P. Nusbaum.
[2] A.M. Bloch, P.S. Krishnaprasad, J.E. Marsden, and Neuromodulation of stomatogastric networks by iden-
R. Murray. Nonholonomic mechanical systems with tified neurons and transmitters. In Harris- Warrick,
symmetry. Archive for Rational Mechanics and Analy- Marder, Selverston, and Moulins, editors, Dynamic
sis, 136:21—99, 1996. Biological Networks, pages 87-137. MIT Press, 1992.
Toward the Regulation and Composition of Cyclic Behaviors
219

[17] E. Klavins. Automatic compilation of concurrent hy- machine. International Journal of Robotics Research,
brid factories form product assembly specifications. In 3:75-92, 1984.
Hybrid Systems: Computation and Control Workshop,
Third International Workshop, Pittsburgh, PA, 2000. [28] E. Rimon and D.E. Koditschek. The construction of
analytic diffeomorphisms for exact robot navigation
[18] E. Klavins. The construction of attention functions on star worlds. Transactions of the American Mathe-
for phase regulation. Technical report, University of matical Society, 327:71-115, 1991.
Michigan, 2000.
[29] E. Rimon and D.E. Koditschek. Exact robot naviga-
[19] E. Klavins and D.E. Koditschek. A formalism for the tion using artificial potential fields. [EEE Transac-
composition of concurrent robot behaviors. In Pro- tions on Robotics and Automation, 8(5):501-518, Oc-
ceedings of the IEEE Conference on Robotics and Au- tober 1992.
tomation, 2000.
[30] A. Rizzi and D.E. Koditschek. An active visual esti-
[20] D. E. Koditschek and M. Bihler. Analysis of a simpli- mator for dexterous manipulation. IEEE Transactions
fied hopping robot. International Journal of Robotics on Robotics and Automation, 12(5):697-713, October
Research, 10(6):587-605, December 1991. 1996.

[21] D.E. Koditschek. Task encoding: Toward a scientific [31] A.A. Rizzi, L.L. Whitcomb, and D.E. Koditschek. Dis-
paradigm for robot planning and control. Robotics and tributed real-time control of a spatial robot juggler.
Autonomous Systems, 9:5-39, 1992. IEEE Computer, 25(5):12-26, May 1992.

[22] D.E. Koditschek. An approach to autonomous robot [32] U. Saranli, M. Buehler, and D.E. Koditschek. Design,
assembly. Robotica, 12:137—155, 1994. modeling and preliminary control of a compliant hexa-
pod robot. In Proceedings of the IEEE Conference on
[23] D.E. Koditschek and E. Rimon. Robot navigation Robotics and Automation, 2000.
functions on manifolds with boundary. Advances in
Applied Mathematics, 11, 1990. [33] U. Saranli, W.J. Schwind, and D.E. Koditschek. To-
ward the control of a multi-jointed monoped runner.
[24] P.S. Krishnaprasad. Motion, control and geome- In Proc. IEEE Intl. Conf. on Robotics and Automa-
try. In Board of Mathematical Sciences, National Re- tion, pages 2676-2682, 1998.
search Council Motion, Control and Geometry: Pro-
ceedings of a Symposium, pages 52-65. National Acad- [34] S. Schwartzman. Asymptotic cycles. Annals of Math-
emy Press, 1997. ematics, 2(66):270—284, 1957.

[25] R. S. MacKay. Chaos, order, and patterns (lake como, [35] W.J. Schwind and D.E. Koditschek. Approximating
1990). In NATO Adv. Sci. Inst. Ser. B Phys., volume the stance map of a 2 dof monoped runner. Journal
280, pages 35-76, Plenum, New York, 1991. of Nonlinear Science, 2000. ‘To appear.

[26] K. Pearson. The control of walking. Scientific Amer- [36] P. J. Swanson, R. R. Burridge, and D. Koditschek.
ican, 235(6):72-86, December 1973. Global asymptotic stability of a passive juggling strat-
egy: A parts possible feeding strategy. Mathematical
[27] M.H. Raibert, H.B. Brown, and M. Chepponis. Ex- Problems in Engineering, 1(3), 1995.
periments in balance with a 3D one-legged hopping
 Moiretsa
st :
fi.AT
SRSA 4 Uh

Pihad, bg eta raion a5 See

eno tea eet eer hot ..
CMM attirethe 2 oda capita it Tiaest ved,
Tapier &. gaia Tee 19 ani kA eo Mv

ou : “8) AN ciadiclen2,Shella6 Sid marie

aaa 4 tt Ue? ted ieee eae tatid hog wits Jeeta vitae eds
Sa)
ie 1 ON, yore lend anckglSbe phta cadmas pererie res
+
mae mo fh
i e It o>
iD rw Past) 7. (Lia!
PRA AE) HAG? al Alig ecbite obTesahs oho vlan EE fiona i
pith Ay A, WALA 9m eataode Shey Wap hy eekadteen
see Yindeoe ay
wore”
ai, AVE, quite ed'h.(Wiers (AR
pee << 2 rAd ey aig
f Oei.
pe Ae rie? ad)ited 4 tap | sgoitbervwat ian
30" a etiy,; Live a isa rice hyatt
it yehbetyine }eens v. ‘Set“7ee
LS
io
. 6; oa
: Hoapnett fi” siti acd! tt (yeh culled apis Tan ue
4 Mm <A Aone tibadt
Se Gay ecnucliodaa «iy
Bailar al atts pro Ay bebit gad}ins a he meitona

BAL,thsivedn woe, aa, aes Mt rca eg ei “Toei ae

ie adits “ontheEELThr olRiera Se
Tealdineetad LW ide — 3 (82) < _—— a~. ‘ pReegt | 0 pany ¥ ae '

ors "y loti =

Dts ei,
nae i al ge4 — ;
lt err, loi y “has = iran
‘ai
eh mscame Paiva
4
Ne ai a v in?) é yt apenas
ih t's tic Rp! Tere Ra tee
miessere, sw alee ee at a | A aati 4. ii
esee > aM, a ane onip atti
. Shaye
Bae im! = his “% ta ‘
ft
uhveee BR ioT ee
‘a ’ =
ae batt he a tar ‘ MELO Wiel) enaited bos «irre seat)
>
Pan
oe Can
A
eine he oat
= Tay cath Wl amb oane: lode TRAM
By Ripe yr” NEA ; ~~
i, <0 .canintoethnei
i yah ini
er te We like 2 Rh tgpy “rath atlas’,
: ou —_ =
oiled io lean wt
Apts OG ire
¥ dirspity
a-f,
7 wiley ba
Npdedl) j }
Deb Sti: yaeleewnel
bras
any} aitlinne POMS te: at? oy ity ™
ii 1 lays
: is e211 oman wrt A ys balge'aban 2x rd pint) At wey at sedi
“Breterg ht iyey 1) w aj eey
iy ees 2s oa ae 7 “
‘tegh au —
hs cel % a - & Par iind walls | 1...
Sus tric.

Re€aren: es
_

}? - 1) Sh anh i eae

v 4 #

“vs |
at

~ 1*) ».48
A Framework for Steering Dynamic Robotic
Locomotion Systems
James P. Ostrowski, University of Pennsylvania, Philadelphia, PA
Kenneth A. Mclsaac, University of Pennsylvania, Philadelphia, PA

We seek to formulate control and motion planning through control inputs on other parts of the system.
algorithms for a class of dynamic robotic locomotion It is this class of systems in which we are interested,
systems. We consider mechanical systems that involve and in particular to a sub-class that is characterized
some type of interaction with the environment and have by robotic systems that move through their environ-
dynamics that possess rotational and translational sym- ment, or robotic locomotion systems. Some exam-
metries. Research in nonholonomic systems and geo- ples of such systems include flying robots (satellites
metric mechanics has led to a single, simplified frame- with thrusters, blimps, helicopters, etc.), the snake-
work that describes this class of systems, which includes board, bicycles, swimming robots, and even legged sys-
examples such as wheeled mobile robots, bicycles, and tems. Our current work does not include legged robots,
the snakeboard robot; undulatory robotic and biologi- which possess intermittent contacts, though extensions
cal locomotion systems, such as paramecia, inchworms, to such systems are certainly possible [5].
snakes, and eels; and the reorientation of satellites and To study control and motion planning for such sys-
underwater vehicles with attached robotic arms. We tems, we utilize a geometric framework developed in
explore a hybrid systems approach in which small am- previous work [19] to capture the general form of most
plitude, periodic inputs, or gaits, are used to yield sim- locomotion systems. The basic concept is to decom-
plified approximate motions. These motions are then pose the system into a part defining the motion (and
treated as abstract control inputs for a simplified, kine- momentum) in the position of the robot, and a com-
matic representation of the locomotion system. We de- plementary part describing the internal shape of the
scribe the application of such an approach as applied system. Internal shape controls can then be used not
to two examples: the snakeboard robot and an eel-like, only to change the position of the system, but to gen-
underwater robot. erate velocities and hence truly locomote in a dynamic
sense. The current study focuses on introducing meth-
1 Introduction ods for controlling and planning trajectories (steering)
for such dynamic systems.
The field of robotic motion planning has generally fo-
cused on the study of wheeled vehicles, and very of- In this paper, we use perturbation methods to de-
ten this has implied kinematic representations of such rive approximate expressions for the effect of internal
systems [9, 14]. When dynamics are treated, they are shape changes on the motion of the system. By assum-
generally directly actuated, e.g., through wheel torques ing small amplitude inputs we can introduce a scaling
applied to the wheels, and do not involve unactuated parameter into the system. Such perturbation tech-
motions. In such cases, the kinematic representation of niques have proven useful in a variety of control con-
such a system can be proven to be fully adequate [13]. texts (2)12,/18,/22):
More recently, the motion planning field has In order to develop the appropriate motion plans, we
branched out to explore a wider range of topics, in- utilize a framework for abstraction of the system dy-
cluding motion planning for flexible or articulated ob- namics that is used in hybrid systems. We note that
jects, parts assembly, and biological molecules [10]. Re- the motion planning algorithms we are proposing for
searchers have also begun to study underactuated me- dynamic underactuated systems have a natural real-
chanical systems, where some or even most of the dy- ization as hybrid systems, since they involve systems
namics of the system can only be actuated indirectly, with continuous time dynamics for which the controls
222 J. Ostrowski and K. MclIsaac

are generated over discrete intervals (input periods), ODE given by Lagrange’s equations). Thus, Equa-
and with discrete mode-switching. tion 2.1 governs the motion of the system in position
space, based on changes in shape and the momentum
2 Characterization of the Steering in the position (group) directions. Equation 2.3 de-
scribes the evolution of the internal shape of the ro-
Problem
bot in terms of the internal applied forces, 7,. Lastly,
2.1 Background and Problem Formulation
Equation 2.2 is called the generalized momentum
equation, where p is a momentum vector associated
In working with locomotion problems in general, and with the momentum along each of the kinematically
steering problems in particular, there is generally a nat- unconstrained fiber directions. This equation describes
ural decomposition of the state space (say, Q) into a the dynamics of the system, via the momentum, in
position space, represented by a Lie group, G, and a the position, or group, directions. We have included a
shape space, given by a general manifold, M. A more forcing term in this equation, T,, to allow us to model
detailed description of this is given in [19], but some control inputs or external forces, such as those arising
easy examples to visualize the shape variables include from fluid drag models or viscous damping. Notice that
the internal bending of a snake, the rotational position by using the symmetries and writing the equations in
of the wheels on a mobile robot, or the leg positions on a body-fixed frame we can pull the position (group)
a walking robot. variable g out of the equation— this greatly helps to
Denoting the configuration variables by g = (g,r) € simplify the necessary calculations.
G x M = Q, the general formulation of the system dy-
namics is found via Lagrange’s equations, perhaps with 2.2 Analogies and Abstractions to Kinematic
undetermined multipliers representing the constraints. Systems
Another important characteristic of robotic locomotion Our main goal in this paper is to develop a motion plan-
systems, however, is that the motion of such systems ner for dynamic locomotion systems by using equiva-
is generally independent of the actual position of the lent representations, or simplified abstractions, for
system, g. Stated another way, the system’s dynamics the system. This will enable us to compute paths for
are invariant with respect to position. This leads to a
the simplified systems that are then tracked by the full
reduced representation when viewed in the appropriate
dynamic system. Figure 1 shows our general approach
body-fixed frame. Using this invariance we write the
to the system representation at various levels of ab-
(possibly constrained) dynamics in terms of a body ve-
straction. Also see Figure 2 for an overlayed compari-
locity, €= g~1g, and a body momentum, p (see [1, 17] son between the motion plans.
for more formal definitions):
The key idea here is to start with the full dynamics,
f=9 Gg=—A(r)r tT (r)p, (2.1) shown at the lowest level in Figure 1, and build sim-
plified approximations of this system at various levels.
p= alr) +p! B(r)r + p' y(r)p + Tq,
In the example we have shown, the full dynamics are
(2.2) first approximated by an equivalent dynamic system
Vist — 130 (2.3) in which the inputs have been restricted to a class of
simple cyclic inputs. This leads to an “averaged” set
Although the derivation of these equations is quite of dynamics in which we assume direct control over
involved, for our purposes here the basic form of them the momentum terms (the derivation for this is done
is all that is critical (to gain a much better insight into in Section 3 below). The next level of abstraction in-
these equations, the reader is referred to the paper by volves replacing the dynamic model with a kinematic
Bloch et al. [1], and examples from robotics explored approximation (also shown in Section 3). In this paper,
in [8, 19]). Equations 2.1 and 2.3 are the fiber and the kinematic approximation (the second level from
base equations, respectively. They define the body the top) is all that is needed, since we use steering
velocity of the system and the dynamics of the shape algorithms based on a kinematic model of this form.
space, respectively (V represents an affine connec- However, we can consider the use of an even coarser
tion, which we will use to denote the second order approximation to this kinematic model in which all of
A Framework for Steering Dynamic Robotic Locomotion Systems
223

process that works its way down the hierarchy also

Omni-
must occur, in which motion plans are generated with
directional
£ =U —— rae
the simplest level of dynamics at the top, and filtered
down through the hierarchy until plans for the original
kinematic
system can be generated. In the case that the choice
t : =
of abstractions is constructive, the motion plans at one
level will implicitly define motion plans at the next low-
€==-A(r)i Constrained est level. They may also serve to define an initial guess
as to what the motion plans should be. Regardless,
— —

eee kinematic | |: ie this overall scheme will likely require some type of ne-
gotiation between motion planners at the various levels
of abstraction, an issue that we do not address here.
Averaged | There is an obvious compromise here between build-
é=i nae1(u,)pi dynamics
vA ing sufficiently simple higher-level abstractions and los-
ing the fidelity to model the characteristics of the more
Pee Kinematic, Ge complex dynamic system. For example, in an omnidi-
cyclic shape rectional model, right angle (piecewise continuous (C°)
paths) turns are allowed, while this is obviously not
Original feasible at many of the lower levels (which generally re-
t quire at least C1 continuity). On the other hand, deal-
Equations 2.1- ae ing with the full details of the original dynamic model
ms; =a
Complete a is generally not tractable, and even special methods
dynamics such as those based on optimal controls or random-
ized motion planners tend to require a great deal of
machinery and computation to compute plans for even
Figure 1: A hierarchical description of the different mod-
relatively simple systems [21]. In the next section, we
els used.
show a mechanism for approximating a dynamic loco-
motion system with cyclic inputs, and in Section 3.3
Obstacles we show that this leads to a simplified representation
using a kinematic, car-like robot model. Then, in Sec-
tions 4 and 5, we apply these algorithms to the steering
eax:
Goal
of snakeboard and eel robotic systems.
--- Omnidirectional
--- Constrained kinematic

——
Dynamic approximation
Full dynamics
3 Algorithms for Generating Motion
3.1 Cyclic Inputs and Locomotive Gaits
Figure 2: Comparison of the paths planned at different
One of the goals discussed above is to form a simplified
levels.
approximate model of the system. One way in which
we can do this is by working from a set of pre-defined
the directions of motion are directly controlled via the inputs. This technique is commonly used in underac-
inputs. tuated and nonholonomic systems [16, 7, 2, 11], and
Of course, formulating the different levels of abstrac- for a restricted class of inputs is sometimes referred
tion is only half of the work. The reason for abstracting to as “steering using sinusoids.” Since we are investi-
at the various levels is to simplify the motion planning gating locomotion systems, we derive an analogy from
effort at each stage. However, motion plans formulated biological systems where a very common observation
at the highest levels of abstraction may not be imple- is that motions are most often generated by cyclic
mentable at the lower levels (though this is certainly shape changes. The motion takes on a characteristic
a goal of the abstraction). Thus, a motion planning form, called a gait, which we can loosely define as a
224 J. Ostrowski and K. MclIsaac

specified cyclic pattern of internal shape changes (in- ro +eu(t), the momentum to second order in is given
puts) which couple to produce a net motion. For each by:
species, there usually exist at most a handful of gaits,
often tailored for specific needs or environments. For Pa(t) = Poa + €8°;poou'|b
instance, a human will walk or run, depending on the ap. yee
desired speed, but can also hop or skip. ae ((= =ahee 3 poo| ut? dr
(f 0
t
3.2 <A Perturbation Approach to Momentum nbd dt — 06; Oc;,Poou
b i (Ow)
iit +...
+a; |wt?
Generation 0
Motivated by recent work by Leonard, Krishnaprasad, Proof: This result is a straightforward, though exten-
and Bullo [2, 12], we use a perturbation approach to sive, calculation, found by setting p = p® + ep’ + ep? +
analyze the response of dynamic locomotion systems to ..., and Taylor expanding a and ( about ro:
small amplitude cyclic inputs (see Khalil [6] for more
details on the use of perturbation methods). We be- p=ptep't+epr
rt...
gin with the general form of the equations for a dy-
namic nonholonomic system, but with the restriction = 0+ €B(ro)p'u
that the terms in the momentum equation that are 0
a Nigel p> + B(ro)p t+ o(r)it) sy
quadratic in momenta be zero (that is, y = 0 from a (¢ TO
Equation 2.2). We re-write these equations here using
indicial notation, where repeated indices imply sum- Equating terms at each order of € and noting that
mation over that index, since this will be important p(0) = po leads to the result. |
for making sense of the results given below:
Remarks: Proposition 1 provides very interesting in-
Nga) Arr + (1) (r\p,,. ||(3.4) sights into the generation of momentum for this type
Da Oagnyrr +)3,,(r)i
pp. of system. For cyclic inputs, there are only two terms
that lead to net changes in momentum, the terms stem-
It can be shown that in all cases where the generalized ming from a and £, and they are both at order e’.
momentum is one-dimensional the term 7 will be iden- Furthermore, when one uses sinusoids that are driven
tically zero [1]. This is true for many rolling robotic in-phase,! the change in momentum is:
systems including the bicycle. The case in which y is
t
the only nonzero term in the momentum equation is
Apa = Qaij ihut dr.
treated implicitly in [2]. 0

Next, consider inputs of the form r(t) = ro + eu(t),

This directly gives us a mechanism for building up the
where € represents a small-amplitude parameter used
speed (momentum) of the system when starting from
in the perturbation expansion. We will be most, inter-
rest. Note that this also tells us how to stop the system,
ested in the case where u(t) is cyclic, and r(t) follows a
using the same in-phase inputs that are used to start
cyclic loop in the shape space about the base point ro.
the system, but with the sign switched on one of the
We include the possibility of a nonzero initial shape, ro,
inputs, i.e., using a 180° change in phase.
to allow flexibility in the modeling of the inputs. The
time derivative of r is just 7 = eu. A simple perturba- To be a little more explicit about this, let u? =
tion analysis of the momentum equation leads to the a’sin2mwt. This leads to a net change of momen-
following result, which we use to develop motion plans tum after one cycle of Ap = 27?w?a;;a'a!. In gen-
for the snakeboard system below. We remark that the eral, one would like to move from p = pg to p®, us-
form of the eel dynamics is slightly different, and so a ing small magnitude inputs. To achieve this, it may
separate derivation is provided in Section 5. *An important distinction of the eel robot is that the
interaction with the environment implies that zero momen-
Proposition 1 Given a system with initial momen- tum is generated using in-phase inputs, and instead the
tum p(0) = po and inputs r(t) of the form r(t) = gaits require slightly out-of-phase inputs.
A Framework for Steering Dynamic Robotic Locomotion Systems
225
be necessary to break the motion up into several re- approximation must lead to a system that is “steer-
peated cycles, say N cycles. Thus, a suitable method able” in the sense of a wheeled vehicle. We character-
for choosing inputs to control to a desired component, ize as steerable any system for which we can use the
of the momentum, (p%),, is to choose a magnitude of shape variables to generate body velocities (c.f., Equa-
inputs, a‘ and a’, and a number of cycles, N, such that tion 2.1) of the form € = g~!g = (v,0,v/R)", where v
2N12w* aaija'a? = (p4 — po)p. is the forward velocity, and Rmin < R < co is a control
variable representing the turning radius (and Rin a
3.3 Steering for “Car-like” Systems turning radius constraint). This effectively means that
we can create generalized circles, using either the shape
In the previous section, we derived results for control- inputs directly (turning the wheels of the snakeboard),
ling the individual momenta of a dynamic nonholo- or through the momentum of the system (as is done
nomic system. In this section, we explore the impli- with the eel robot). Generalizations to this type of
cation of this for steering, using an analogy to wheeled steering (e.g., to 3D motions) are certainly possible, as
mobile robots. While the results are fairly straight- long as the same type of analogy can be made between
forward applications of the above algorithms to mo- the full dynamic system and its kinematic representa-
tion planning, they are useful because they allow us tion.
to control the robot over a non-local region of the The steering algorithm, then, is as follows:
state-space. In other words, instead of being forced
to patch together purely local results as is generally 1. Momentum generation: As shown above in
done in steering algorithms for nonholonomic systems Proposition 1, momentum can be generated by the
using cyclic inputs, one can determine motion plans simple mechanism of in-phase inputs. For steer-
for more non-local control goals. This has a similar ing in SE(2), the momentum is assumed to be
feel to the steering of a mobile robot or an airplane, one-dimensional. Thus, this step takes the system
where one pieces together very simple trajectories to from (0,0,0,0) to (1,
y1, 1, p“), for some desired
generate global (not STLC) controllability. momentum, p.

First, recall the results of Dubins [4] for kinematic 2. Steering to the desired setup point: Once
mobile robots. It was shown that the optimal path for the desired momentum has been achieved, the sys-
a car steering in SE(2) with limits on the turning ra- tem can be steered along any desired path (by
dius is given by two circular arcs (one arc tangent to assumption). This involves computing the neces-
each of the initial and final headings) connected by a sary arcs and line segments to connect (1, y1, 41)
straight line segment. See Figure 3. In this section to (ta, Yya,9a). In order to setup for the final
we utilize these paths as the backbone for steering dy- step of reducing the momentum, steer the sys-
namic nonholonomic systems. In order to make the tem to the point (rq — 2s, Ya — Ys, 9a — 9s), where
abstraction shown in Figure 1 from the dynamic sys- T;, = (Xs,Ys,9s) is the approximate change in po-
tem to the approximate dynamic system (and hence sition that will result from the stopping motion in
further to the kinematic, car-like system) work, the Step 3. Assuming the appropriate steering can be
executed, the setup point is reached with a mo-
mentum of p?%.
3. Zero the momentum: Execute the opposite ma-
neuver to that chosen in Step 1 in order to reduce
the momentum to zero.

4. Cancel errors in position and momentum:

Since the above analysis is just an approxima-
tion, it is expected that the final state after Step 3
will not be exactly (xa, ya, 9a,0), but instead some
(2,9, 4,p). The final step, then, is to make cor-
Figure 3: Car-like steering using linear and circular arcs. rections to zero the momentum and repeat the
226 J. Ostrowski and K. MclIsaac

process to move to the final state. For systems that the wheels themselves are allowed to spin freely,
such as the snakeboard, it may be possible to use just as with a traditional skateboard.
other gaits to make small corrections in position, Following along the lines of Ostrowski et al. [21], we
as is shown in [18]. use a non-dimensional set of variables, normalized by
the board length and rotor momentum, so that the
This steering algorithm is based solely on the use of fiber equations reduce to the form of Equation 2.1 with:
feedforward terms to control the motion. We explore
the use of feedback terms in discussing steering for the —Jsindcosd 0 aah
eel in Section 5. We also remark that if one is inter- AS 0 0 and 1 = 0 "
ested strictly in configuration controllability (moving J sin? ¢ 0 j
5 tang
between configurations only, without regards to veloc-
ity) [3], then the motion can be achieved using Steps 1
and 2 only. and J a non-dimensional ratio between the rotor iner-
tia and the board inertia. The generalized momentum
4 Steering with a Feedforward equation, Equation 2.2, is given by:
Approach: The Snakeboard Example Des 2cos? f dy —tan ¢ dp.
The Snakeboard is a commercial variant of the skate-
board that allows for independent rotation of the wheel Using this, we can explore the relationship between
trucks. The simplified model of the Snakeboard (re- the integrals of Proposition 1 and the appearance of
ferred to as the snakeboard model) is shown in Fig- cyclic inputs. Using inputs of the form y(t) = > —
ure 4, along with a robotic version that was constructed dy sin2nt and ¢(t) = agsin2at, the a term in the
in order to verify theoretical simulations. We briefly momentum equation leads to changes of momentum
recall the description of the snakeboard as developed equal to:
in [20, 21]. The basic premise is that by properly cou- 1 1
pling the rotating flywheel (modeling the swinging of Ap = aai;(T0) | i ainds fioydr
a human torso) with the turning of the wheels, one 0 0
1
can generate a variety of motions, including a forward
= -2(2n)Pasay | cos? 2n7 dr = —4n agag.
serpentine-like gait reminiscent of a snake. 0
(4.6)
As a mechanical system the snakeboard has a config-
uration manifold given by Q = SE(2)xSxS. As coor-
dinates for @ we shall use (x, y,0,%,¢) where (a, y, 0) A set of simulations for this pair of inputs is pre-
describes the position of the board with respect to a sented in Figure 5. The right-hand plot in this figure
reference frame, w is the angle of the rotor with re- shows the time evolution of the momentum. For each
spect to the board, and ¢ denotes the angle of the back
wheel truck with respect to the board (and the oppo- yx lod
\
Pp

site of the angle of the front wheels, which is assumed 30,00 63,03" 1.00 03,05
05,05 03,05
to move through equal and opposite rotations). Note 25,00 -2.00
3.00
20.00
RO Se
15.00 5.00
-6.00
10.00
-7.00
back wheels *
5.00 8.00
—i\8 = front wheels
-9,00
O00 =v, oe
= a TU a le cca aie cas
x
0,00 00.00 400.00 600,00 0.00 0.50 1.00 '

Figure 5: Forward motion during momentum generation,

Figure 4: The simplified model of the Snakeboard, along and momentum versus time. The approximated value of the
with a demo prototype. momentum ts shown in horizontal lines.
A Framework for Steering Dynamic Robotic Locomotion Systems
227
set of inputs, the value of the momentum after one 5 Adding a Feedback Term:
cycle predicted by Equation 4.6 is shown as a hori-
zontal line. It should be noted that the prediction for
The Snake/Eel Robot
the small inputs works quite well up to an amplitude In [15], we studied anguilliform locomotion using a sim-
(in radians) of approximately ag = ay = 0.3. Larger plified physical model of a snake (we use the term
amplitude inputs (ag = ay = 0.5) are also plotted to “snake” interchangeably with “eel”) to be used as a
show that while some discrepancy does exist between platform to test various locomotive gaits. We model
the predicted and simulated final values of momentum, the snake as a planar, serial chain of identical links with
the error is relatively small (on the order of 15%). mass m and inertia J. We assume full control of the
For the snakeboard example, steering comes internal shape of the snake (the joint angles ¢;) which
from controlling ¢ as it affects the term J-! = allows us to solve the dynamic equations in terms of the
(—J/2,0, Jtan@/2)". To see how the steering algo- unknown configuration variables (x, y, )—the position
rithm could be implemented, we provide in Figure 6 a and orientation of the first link. Since our mechanical
sample plot of motion control for the snakeboard mov- robot (the REEL eel [15]) is composed of four links,
ing from (zx, y,@) = (0,0,0) to (0,-5.0, 7), along with all simulations in this paper have been performed for
a plot of the momentum as a function of time. Notice a four link model of a snake. For simplicity and sym-
that in this plot the system first builds up momentum metry, however, the analytical derivations have been
using a serpentine-like snakeboard “drive” gait in Step performed on a three link model.
1, and then executes a turn, a straight coast, and a The crucial elements in this model are the drag force
final turn in Step 2. The turns are executed at the terms, which generate the locomotion. To simulate the
minimum turning radius, which we have constrained forces in the water, we adopt a simple fluid mechan-
to be @maz = +1.0rad. For this simulation, we exe- ical model. We assume that the Reynolds number is
cute the same set of input motions at the end of the high enough that inertial forces dominate over viscous
trajectory as was used at the start, except that the effects—a reasonable approximation for smooth bodies
wheel angles are turned through opposite rotations of in an inviscid fluid. We also assume that the fluid is
those used originally. These in-phase cyclic gaits used stationary, so the force of the fluid on a given link is
to build up and to reduce momentum at the start and due only to the motion of that link. The pressure dif-
finish of the motion are run for the same length of time. ferential created by an object moving in a fluid causes
As is clear from the the right-hand graph in Figure 6, a drag force opposing the motion. Under the assump-
this results in a near exact cancelation of the momen- tions above, the drag force developed takes the form
tum that was built up during the initial motions. F x pyv?. Here, v is the forward speed of the link
and [ly is a drag coefficient for the water, determined
by the formula zw = pAC/2, where A is the effective
' Cs0-) i WSO)
0.00 - = ee 9 area of the object, p is the density of water, and C is
-0.50 1.00
-1.00 -2.00
a shape coefficient.
-1.50 3.00

-2.00 -4,00
-2.50 5.00
-3.00 6.00
-3.50 7,00
-4,00 8,00
-4.50 9,00
-5.00 10.00 4
0.00 2.00 4,00 0.00 5.00 10.00

Figure 6: Steering using the full algorithm, along with

associated changes in momentum.
Figure 7: (A) Model of snake, (B) Forces and torques on
link i, (C) The REEL eel robot.
228 J. Ostrowski and K. MclIsaac

5.1 Force Approximation and Momentum

Equation

In our simulations, we assume that pressure differen-

tials in the directions parallel to the moving body are
decoupled from pressure differentials perpendicular to
the body, to yield:
J l =
MLS nee eX 9 oO beard}
‘Amplitude
ofgaitwarvatorm (degrees)

Fe = —pysgn(vj) - (oj)? (5.7)

» T
\

\
where v} is the projection of the vector (#;,%;) along
a direction perpendicular to the link. We exclude drag
forces parallel to the link because they were determined rack
Turing
(metres)

in simulation to have negligible effects. For all numeri-

cal simulations, the full forcing term in Equation 5.7 is
used. However, the discontinuity in sgn(v) means that
this expression is not tractable for use in calculations.
Therefore, in our analytical derivations we utilize an Figure 8: Simulation of speed changes per cycle with re-
approximation to this function, which turns out to be spect to amplitude, € (top left), and phase, ¢s (top right).
linear in v: Fapprox = p#u~, where y is defined by a Also shown (bottom) is turning radius versus the gait offset
least squares fit over some small range around v = 0. parameter, Poffs-
We also note that this force model can be interpreted
as a viscous damping model, as might be encountered
the assumption that p(0) = 0, we can determine that
with a snake moving over soft sand.
the momentum at both the zeroth order (p;o) and first
Using this expression for the drag forces and exploit- order (p;1) in € are both zero after any complete cycle
ing the invariance of the system with respect to changes of the gait.
in position and orientation, the momentum equation
becomes: Examination of the second order momentum term
reveals a single term that drives the momentum in the
Bi = 1 Pepi — BYPRR + ig(I* "1 forward direction. For the case of the three link snake
(the simplest configuration capable of locomotion), we
+ (a = AAT), can express the average forward momentum (over one
where A and 7 are transformations related to the drag cycle) with:
forces. This momentum equation differs from the equa-
tion introduced in Section 2.1 because the frictional Ap, = —€’K (go)w sin(¢s), (5.8)
forces must enter the dynamics explicitly, rather than
where the momentum gained over one cycle is propor-
implicitly in the form of constraint forces. A direct
tional to the square of the gait amplitude and the sine
impact of this is that the form of the momentum equa-
tion is no longer quadratic in p and 7, but now also
of the gait phase angle (K is a constant of propor-
contains linear terms. For this reason, the results of tionality that is a function of the robot parameters).
Proposition 1 no longer apply, but similar conclusions Although this is only an approximation, we find that
can still be drawn. the simulation results show a very good agreement with
the approximation, even for fairly large amplitude in-
5.2 Perturbation Analysis puts, as shown in Figure 8. For longer snakes (with
more than three links), the simple sinusoid is replaced
We proceed as before, setting r(t) = ro + eu(t), and by a more complex sum of harmonics of the phase an-
solving for pj = pio + epi + pig +.... A traveling gle. Equation 5.8 provides a theoretical justification
wave of the form r; = ¢; = €sin(wt + i¢,) can be used for the open loop inputs, presented above, for gener-
to drive the snake in the forward and backward direc- ating forward momentum. Intuition suggests that a
tions by appropriately choosing the sign of ¢,. With similar expression exists describing the evolution of p3
A Framework for Steering Dynamic Robotic Locomotion Systems
229
(angular momentum) when unbalanced (turning) gaits Steering controlier tracking anovalpath

are used, by adding an offset, d9 = dog to the joint an-

gles. Initial numerical simulations show that this is the
case, but we are continuing to investigate these issues
further analytically.
(m)
ofposition
ycoordinata

5.3 Controller Development

There are three basic components to the steering algo-

rithm discussed above in Section 3: building up speed,
steering, and stopping. We have just derived an ex-
pression that can be used in driving the system to a Figure 9: Steering around an oval shaped path
steady-state speed. Next, we examine a mechanism for
steering with feedback. Figure 9 shows the path of the eel tracking an
oval shaped path that integrates both circular and
5.3.1 Steering Controller
straight line segments. With our simple kinematic
In our previous work [15], we presented results from tracking (feedback only) approach, (outer path—dot-
open loop control using this model. One of the impor- dashed) there is considerable overshoot where the con-
tant points that can be taken from this work involves troller attempts to transition from circular to straight
the ability to steer. We have examined the addition line motion and vice versa due to accumulated linear
of a biasing term, dogs, to the wave gait, which causes and angular momentum. This overshoot can be elim-
the snake to follow a circular path. There is an inverse inated by the use of a feedforward look-ahead control
relationship between turning radius and gait offset, as (inner path-solid), allowing tracking of path curvature.
shown in Figure 8. The desired path is the dashed curve.

It should also be noted that when performing feed- 5.3.2 Stopping Controller
back on a system using periodic gaits, the control is
based on discrete sampling by averaging the center of The problem of stopping given an initial forward mo-
mass position and velocity over one cycle. This places mentum is considered independently of the problem of
a theoretical (Nyquist) limit on the controller dynam- steering. In general, a stopping controller that drives
ics of Fs /2. In practice, for a 1Hz drive gait, controller the robot’s velocity to zero will result in a repeatable
responses of under five seconds are likely not achiev- motion, possibly including both a translation and a
able. rotation during some stopping time, ts. Unlike the
snakeboard, the first order approximation to stopping
We use a feedforward law based on the (curvature
does not lead to the opposite motion as that found in
of the) path generated by the kinematic motion plan-
starting, largely due to the dissipation found in the eel.
ner. In addition to this, we note that since the steering
is only approximate, the addition of a feedback term Let us define a stopping transformation, T;, for
is desirable. If we define d as the signed, perpendicu- a given initial velocity. Then we can use our steering
lar distance to the desired trajectory (with d negative controller to steer to a configuration that can reach the
when the desired trajectory is to the right of the ac- goal through the transformation T,. Using the stop-
tual trajectory), the motion of the cart is described by ping controller to drive the velocity to zero from that
d = vsin(@q(t) — A(t)) (see Figure 3). Using a pro- point will result in the robot having a final configura-
portional feedback control law for the cart of the form tion at the goal point with zero velocity. The trans-
6(t) = ke(0a(t) — 0(t)) — ka(d), one can show that the formation, T;, depends primarily on the momentum
linearized equations of motion can be easily stabilized of the robot before initiating the stopping gait, which
about a straight path. The characteristic equation for would make the determination of JT, an untractable
the linearized system is by s?+kgs—vkg = 0. The con- problem if it were necessary to consider the entire mo-
dition for stability of this system, therefore is kg > 0 mentum space. However, since we have limited our-
and kg < 0 which matches our intuition about steering. selves to straight line trajectories and circular arcs, we
230 J. Ostrowski and K. MclIsaac

We present results from simulations in which we ap-

plied our motion planning algorithm to two different
types of mobile robots: the snakeboard and the eel. Al-
though these systems differ in the type of interaction
with the environment (non-holonomic versus viscous
drag forces), we show that the same abstract motion
planning algorithm can be used in both cases, with only
slight modifications required to overcome the presence
Figure 10: The motion of the eel tracking from all four of dissipation. Since our algorithms build on results
quadrants, along with a focused plot of the motion from the from kinematic path planners, we believe that it will be
first quadrant natural to extend this work to include obstacle avoid-
ance and other motion planning issues.
need only consider three cases: stopping after straight
line motion and stopping after motion along clockwise Acknowledgments
and counterclockwise circles.
The authors gratefully acknowledge the support of NSF
5.3.3 Path Planning grants IRI-9711834 and IIS-9876301, and ARO grants
Figure 10 shows the results of the proposed motion P-34150-MA-AAS, DAAH04-96-1-0007, and DURIP
planning algorithm for initial conditions in each quad- DAAG55-97-1-0064.
rant, with differing initial orientation. Desired paths
are shown dashed and actual paths solid. Also shown References
in Figure 10 is the boxed area in greater detail for the
initial position in the first quadrant. The robot stops [1] A. M. Bloch, P. S. Krishnaprasad, J. E. Marsden,
within 50cm of the origin in all four cases, with orien- and R. M. Murray. Nonholonomic mechanical systems
with symmetry. Archive for Rational Mechanics and
tation errors of less than 30 degrees. We are currently
Analysis, 136:21-99, December 1996.
exploring “station keeping” gaits to correct these small
errors. [2] F. Bullo, N. E. Leonard, and A. D. Lewis. Control-
lability and motion algorithms for underactuated La-
grangian systems on Lie groups. Submitted to IEEE
6 Conclusions and Future Work Transactions on Automatic Control, February 1998.

This paper establishes easily implemented feedforward [3] F. Bullo and A. D. Lewis. Configuration controllability
for mechanical systems on Lie groups. In Symposium
and feedback control algorithms for mechanical sys-
on Mathematical Theory of Networks and Systems, St.
tems with Lie group symmetries, external forces, and Louis, June 1996.
nonholonomic constraints. We focus our attention on
systems that use small amplitude, periodic shape in- [4] L. E. Dubins. On curves of minimal length with a
puts, or gaits, for momentum generation and steering. constraint on average curvature and with presribed
initial and terminal positions and tangents. American
We have proposed a motion planning algorithm based
Journal of Mathematics, 79:497-516, 1957.
on Dubins’ work on the control of kinematic wheeled
vehicles. The motion planning problem reduces to a [5
J— B. Goodwine and J. W. Burdick. Trajectory genera-
momentum generation phase (with a complementary tion for kinematic legged robots. In Proc. IEEE Int.
momentum dissipation phase) and a steering phase in Conf. Robotics and Automation, pages 2689-2696, Al-
buquerque, NM, April 1997.
which the complex dynamics of the system can be ap-
proximated using a kinematic steering model. We de- (6a= H. Khalil. Nonlinear Systems. Macmillan Publishing
velop these abstract (kinematic) steering models using Co., 1992.
a hierarchical, hybrid-systems approach to the prob-
lem, in which we look for commonalities among a wide
[7<7 P. S. Krishnaprasad and D. P. Tsakiris. Oscillations,
SE(2)-snakes and motion control. In IEEE Conf. De-
range of dynamic systems— in this case, the common- cision and Control, pages 2806-2811, New Orleans,
ality is that the system must be steerable. LA, December 1995.
A Framework for Steering Dynamic Robotic Locomotion Systems
231

[8] P. S. Krishnaprasad and D. P. Tsakiris. G-snakes:

[16] R. M. Murray and S. S. Sastry. Nonholonomic motion
Nonholonomic kinematic chains on Lie groups. In planning: Steering using sinusoids. IEEE Transactions
Proc. 33°? IEEE Conf. Decision and Control, pages on Automatic Control, 38(5):700-716, 1993.
2955-2960, Lake Buena Vista, FL, December 1994.
[17] J. P. Ostrowski. The Mechanics and Control of
[9] J.-C. Latombe. Robot Motion Planning. Kluwer, Undulatory Robotic Locomotion. Ph.D. thesis, Cali-
Boston, 1991. fornia Institute of Technology, Pasadena, CA, 1995.
Available electronically at
[10] J.-C. Latombe. Motion planning: A journey of robots,
http://www.cis.upenn.edu/~jpo/papers.html.
molecules, digital actors, and other artifacts. Inter-
national Journal of Robotics Research, 18(11):1119- [18] J. P. Ostrowski. Steering for a class of dynamic non-
1128, December 1999. holonomic systems. To appear in IEEE Transactions
on Automatic Control, September 1999.
[11] N. E. Leonard. Periodic forcing, dynamics and con-
trol of underactuated spacecraft and underwater vehi- [19] J. P. Ostrowski and J. W. Burdick. The geometric
cles. In Proc. IEEE Conf. Decision and Control, pages mechanics of undulatory robotic locomotion. Jnter-
1131-1136, New Orleans, LA, December 1995. national Journal of Robotics Research, 17(7):683-702,
July 1998.
[12] N. E. Leonard and P. S. Krishnaprasad. Motion con-
trol of drift-free, left-invariant systems on Lie groups. [20] J. P. Ostrowski, A. D. Lewis, R. M. Murray, and
IEEE Trans. on Automatic Control, 40(9):1539-1554, J. W. Burdick. Nonholonomic mechanics and loco-
September 1995. motion: The snakeboard example. In Proc. IEEE
Int. Conf. Robotics & Automation, pages 2391-7, San
[13] Andrew D. Lewis. When is a mechanical control sys- Diego, May 1994.
tem kinematic? In Proc. IEEE Conf. on Decision and
Control, pages 1162-1167, December 1999. [21] J. Ostrowski, J. P. Desai, and V. Kumar. Optimal
gait selection for nonholonomic locomotion systems.
[14] Z. Li and J. F. Canny, editors. Nonholonomic Motion
Planning. Kluwer, 1993.
International Journal of Robotics Research, 19(3):225—
237, March 2000.

[15] K. MclIsaac and J. Ostrowski. A geometric formula- [22] J. Radford and J. Burdick. Local motion planning for
tion of underwater snake-like locomotion: Simulation nonholonomic control systems evolving on principal
and experiments. In JEEE Conf. on Robotics and Au- bundles. Conf. Mathematical Theory of Networks and
tomation, pages 2843-2848, Detroit, May 1999. Systems.
 fy ia
aietee?, wontoerrad, aa v
i

priate Jn jor] _ ns as
i “nha
wei pve
—
paysae
(TA) ud ez“i my att “auf
OW HO grr tmca vcam
ets vette
ity Soy We aghre Ci > oe 8
OnetayWy aT tealFil tea Beet ae} yet The
bye
fleves} EN
VEL pear
a li eae smear
ioe waa ae
ierrghearmy swede eT
J ih te =A wlan peal q gibt: ror. GOO), itieng 4 eect by
_ . re bas ed iyecan ii,“at “Ayot at
ve tL (Ot! anche ypia
Staal oa aN Hosen elbietcus io jay
rm pehzonree. | Dias herere” > ry Mayne G
Anny SA
fone
vert i
— ' , RL, wns 2 sun wat ae TF Sgn

eed ‘ : fOS) ~
ave “wr
at amiol
es d dhe Ca
Me TE
tigre .cap?Apet
bawsite
Pa 2
ap fet ,

ae wpa ;i ste pidtdaliiie ios ar

yy op " is MA, AAS TEALOLO4- LE
PA SA
led ThiPahdie
T n F aiind cage
i bite porwwAL se URE SEAS A al ©
; ; €; [tx eee Ses sled FALE Sh. ney
Ai cat nt m2 caeatbow poten
Sangre baw baclalh 5 mre 1 i] ~ ij Sty.- P =} anew
BF hove ew Th at i ¥ Nicongorvaa ae
~ ‘Mo
; . WH © yy ine eu! ; ates
pg uot innahas bh 3 se bi
MD om rel “Sr i eh i asa vee cealis
sHw inde 7 re) wien Senate
- AE
hats>% Tianhe, OF try
Ey Fasten Ye = Wa LO eye cm Lie geen dele
~ OV bape om Jt Wierhiaiice AOnmiingh ib
ge >". . : s i =. * hy omnr «vin Apared m dele

9 pe av .. “ 60 Mathers
oe idl (Vib bee ;
_ D
Watters 7Tivery af etaiie
herny af
ay
Reena
jOgut A a
: es
oo y :
phd hee’ 2
» 14> * et. » ‘ ‘ . é 7 E - —_—
hic (Map 6 pehy ) _— a 7
onaes - ¥ aan TA et se Cured af
PP it gals BA Pye
iat at ont ae
RNS
Me Acn
ot
wt 6.
presses».
3 ree
| =
ns ;
el peas
7 sto st

7 re
ay 5 i he

<< wee ai ame 8

*
~ > :
A Kinematics-Based Probabilistic Roadmap Method
for Closed Chain Systems
Li Han, Texas A&M University, College Station, TX
Nancy M. Amato, Teras A&M University, College Station, TX

In this paper we consider the motion planning prob-

lem for closed chain systems. We propose an extension
of the PRM methodology which uses the kinematics of
the closed chain system to guide the generation and
connection of closure configurations. In particular, we
break the closed chains into a set of open subchains,
apply standard PRM random sampling techniques and
forward kinematics to one subset of the subchains, and
then use inverse kinematics on the remaining subchains
to enforce the closure constraints. This strategy pre-
serves the PRM sampling philosophy, while addressing Figure 1: The Stanford Assistant Mobile Manipulator
the fact that the probability that a random configura- [12]. (Photo Courtesy of Prof. O. Khatib.)
tion will satisfy the closure constraints is zero, which
has proven problematical in previous attempts to apply ular chains [21], reconfigurable robots [14, 20], and the
the PRM methodology to closed chain systems. closed chain system formed by multiple robots grasping
Another distinguishing feature of our approach is an object [13] (see Figure 1). Closed chains are some-
that we adopt a two-stage strategy, both of which em- times called parallel chains since they can be viewed as
ploy the PRM framework. First, we disregard the en- consisting of two or more serial/open chains that pro-
vironment, fiz the position and orientation of one link vide parallel linkages between two points. While closed
(the “virtual” base) of the system, and construct a kine- chains can offer advantages over open chains in terms
matic roadmap which contains different self-collision- of the rigidity of the mechanism, motion planning and
free closure configurations. Next, we populate the envi- control of closed chains is complicated by the need to
ronment with copies of the kinematic roadmap (nodes maintain the closed chain structure, the so-called clo-
and edges), and then use rigid body planners to connect sure constraint.
configurations of the same closure type. This two-stage In this paper we consider the motion planning prob-
approach enables us to amortize the cost of computing lem for closed chain systems. The motion planning
and connecting closure configurations. problem is to find a collision-free path that takes the
Our results in 3-dimensional workspaces show that closed chain from one configuration to another. A real
good roadmaps for closed chains with many links can world example is to find a collision-free path for a
be constructed in a few seconds as opposed to the sev- multi-fingered robotic hand, to move a grasped part
eral hours required by the previous purely randomized from one station to another for machining. For some
approach. tasks, the robot might need to regrasp the object, i.e.,
to change the grasp points and grasp fingers, so as to
accommodate the workspace limits of the robot or to
1 Introduction
avoid collisions. In general, it is not easy to move and
regrasp the object simultaneously. One approach [16]
Closed chain mechanisms arise in many practical prob-
lems, such as the Stewart Platform [22], closed molec- proposed for this problem is to interleave transit paths,
234 L. Han and N. M. Amato

which only implement the regrasp without moving the The PRM planner for closed chain mechanisms pro-
object, and transform paths, which only transfer the posed in [18] builds a roadmap in the portion of
object using fixed grasps. A manipulation system on a the configuration space that satisfies the closure con-
transform path with a fixed grasp can be viewed as straints. The roadmap vertices are generated by first
a system with closed chains.1 While the transform sampling points from the entire configuration space,
path planning problem has been studied for simple and then performing a randomized gradient descent to
robot manipulation systems, the general problem re- try to transform them into configurations satisfying the
mains open and is one motivation for our work. Other closure constraints. Roadmap vertices are connected
motivational applications of closed chain motion plan- by a randomized gradient descent traversal of the con-
ning include animation, virtual reality, and training. straint surface. When applied to closed chain linkages
composed of line segments in the plane, this approach
Motion planning [15] is a challenging problem which
required several hours of computation to generate a
involves complicated physical constraints and_high-
well connected roadmap. Thus, while this work rep-
dimensional configuration spaces. The fastest existing
resents a crucial first step towards extending the PRM
deterministic planner [6] takes time exponential in the
methodology to this important class of problems, the
number of degrees of freedom of the robot. On the
methods do not yet lead to efficient solutions for many
other hand, a class of randomized planners proposed
practical problems.
during the last decade have successfully solved many
previously unsolved problems. In particular, Proba-
bilistic Roadmap Methods (PRMs) [2, 3, 4, 5, 10, 11, 24] 1.1 Our Approach
have been used successfully in high-dimensional con-
figuration spaces. The general methodology of PRMs In this paper, we propose a new approach for plan-
is to construct a graph (the roadmap) during pre- ning the motion of kinematic chains with closure con-
processing that represents the connectivity of the ro- straints. Like [18], we believe the PRM methodology
bot’s free configuration space (C-space), and then to can be extended to closed chains. However, since the
query the roadmap to find a path for a given mo- probability that a randomly generated node lies on
tion planning task (see Figure 2). Roadmap vertices the constraint surface is zero [18], we believe that the
are (generated from) randomly sampled configurations purely randomized philosophy of the PRM must be aug-
which satisfy feasibility requirements (e.g., collision mented with more deliberate techniques developed in
free), and roadmap edges correspond to connections be- the robotics community to deal with the closure con-
tween “nearby” vertices found with simple local plan- straints. In particular, we advocate the use of kinemat-
ning methods. For the most part, the major successes ics to guide the generation and connection of closure
for PRMs have been limited to rigid bodies or artic- configurations. Briefly, the kinematics of an articulated
ulated objects without closed chains. Recently, some object (such as a robotic finger) describes the relation-
efforts have been made to apply the PRM paradigm to ship between the configuration/motion of the joints of
closed chain systems [18] and to flexible objects [9]. the linkage and the resulting configuration/motion of
the rigid bodies which form the linkage. Our plan-
Goal®~__ ner uses both forward and inverse kinematics to gener-
ate closure configurations. In particular, we break the
closed chain into a set of open subchains, apply stan-
dard PRM random sampling techniques and forward
kinematics to one subset of the subchains, and then
use inverse kinematics on the remaining subchains to
enforce the closure constraints.

Figure 2: A probabilistic roadmap (C-space). Another distinguishing feature of our approach is

that we adopt a two-stage strategy, both of which em-
‘Besides the collision-free and closure constraints, more
manipulation constraints need to be taken into account for ploy the PRM framework. First, we disregard the en-
regrasp planning, which will be addressed in a follow up vironment, fix the position and orientation of one link
paper. of the chain (which can be viewed as a “virtual” base
A Kinematics-Based PRM for Closed Chain Systems
235

sults in 3D workspaces show that good roadmaps for

closed chains with many links can be constructed in a
few seconds as opposed to the several hours required
by the previous purely randomized approach [18].
This paper is outlined as follows. In Section 2, we
describe the related work in more detail. We formally
define the closed chain motion planning problem in Sec-
tion 3. The details of our approach are described in
Sections 4 through 6. We present some experimental
results in Section 7, and some concluding remarks in
Section 8.
For simplicity and clarity, two-dimensional figures
are used throughout this paper to illustrate the closed
chain motion planning problem and our kinematics-
based PRM motion planning approach. It should be
Figure 3: Snapshots of a kinematic roadmap path for a noted that the general discussion and planning frame-
7-link closed chain. work are applicable to both 2D and 3D workspaces.

of the system)? , and construct a roadmap which con- 2 Related Work

tains different self-collision-free closure configurations.
We call this roadmap a kinematic roadmap since it PRMs. As mentioned in Section 1, the success of
deals solely with the robot’s kinematics and utilizes PRMs for rigid body and articulated open chain robots
both forward and inverse kinematics in its construc- [2, 3, 4, 5, 10, 11, 24], has motivated the extension of the
tion. Figure 3 shows snapshots from a path contained PRM strategy to planning for elastic objects (FPRM) [9]
in a kinematic roadmap found by our planner. Next, and closed chains [18]. The major challenge here is in
we populate the environment with copies of (portions finding an effective way to deal with the additional con-
of) the kinematic roadmap. This stage again employs straints imposed on the feasible robot configurations.
the PRM strategy. In particular, we select random con- In particular, while the only constraint on feasible con-
figurations (position and orientation) in the environ- figurations for rigid bodies and open chains is that they
ment for the base in the kinematic roadmap, and re- be collision free (constraints on the joint variables of
tain those portions of the kinematic roadmap that are an open chain linkage can generally be encoded in the
collision-free. Finally, local planning methods for rigid robot’s configuration space), elastic objects can only
bodies are used to connect configurations of the same achieve deformations with (local) minimum elastic en-
ergy and closed chains need to satisfy the closure con-
closure type. Note that this strategy restricts the con-
nection of different closure configurations to the kine- straints.
matic roadmap, i.e., the only edges between different As previously mentioned, while [18] pioneered the
closure types in the main roadmap are copied from the use of PRMs on closed chain systems, the relative inef-
kinematic roadmap. ficiency of the randomized gradient descent technique
used to generate closure configurations from randomly
Our motivation for this two-stage approach is that sampled configurations, and to connect closure config-
it amortizes the cost of computing and connecting the urations, emphasized the need for better techniques.
closed chain configurations. Another benefit of this The fundamental problem is that since the probability
strategy is that we do not waste time trying to connect that a sampled node lies on a constraint surface is zero
unconnectable closure configurations when construct- [18], it is very difficult to find (and connect) configu-
ing the final roadmap. Indeed, our experimental re- rations satisfying the closure constraints using purely
For
randomized techniques. This is what motivates our
2The virtual base can be chosen in other ways.
kinematic roadmap, whose construction employs both
example, in a hand-object manipulation system, we can
choose the object as the virtual base of the system. forward and inverse kinematics. We note that although
236 L. Han and N. M. Amato

it was not used, the possibility of placing pre-computed

closure configurations at different locations in the en-
chain 2
vironment was mentioned in [18]. chain

FPRM [9] deals with the energy requirement for

elastic objects in a manner similar to the kinematic
roadmap we use for closed chains: minimal energy de- 5,ie , 2
formations are computed a priori, disregarding the ob- ; (F)
W
stacles in the environment, and then copies of these
deformations are placed at randomly selected locations
Figure 4: Breaking a Closed Chain into 2 Open Chains.
(positions and orientations) in the environment. How-
ever, a difference between FPRM and our approach is
that we also use the PRM strategy to construct the 3 Problem Formulation
kinematic roadmap and populate kinematic roadmap
edges, which correspond to connections between clo- In this section, we study the configuration spaces of
sure configurations. In contrast, FPRM populates min- multi-link chains and discuss the effect of closure con-
imal energy deformations only and performs node con- straints on these spaces. When a linkage system in-
nection directly in the environment. We note that our volves multiple closed chains, the overall closed chain
two-stage PRM strategy can be applied to flexible ob- constraint is satisfied if and only if each closed chain
ject motion planning. More specifically, we can first constraint is satisfied. Therefore, we discuss in detail
generate a “deformation” roadmap with connections only the case for one closed chain, with the understand-
between minimal energy deformations in a clear envi- ing that the problem involving multiple closed chains
ronment and then populate it to the real environment can be similarly formulated and handled by our plan-
containing obstacles. ner.
We first note that a closed chain system can alter-
natively be viewed as a linkage system consisting of a
Other Methods. A random exploration strategy, collection of open chains, where we “break” each closed
based on the Ariadne’s Clew Algorithm [1], has been chain, and then satisfy the closure constraints, if any,
used to solve point-to-point inverse kinematics prob- by forcing the break points to coincide. For example,
lems for redundant manipulators. Given an initial con- in Figure 4, chain 1 and chain 2 form a closed chain
figuration of a robot, the problem was to find a reach- where the frames FE; and E> attached to the break-
able configuration that corresponds to a desired posi- point (the “end effector”) must coincide to satisfy the
tion and orientation of the robot end-effector and to closure constraints.
find a feasible path connecting the initial and the goal
Consider a closed chain system that can be bro-
configuration. Central to their approach was the con-
ken into k open chains. The configuration of an open
struction of a roadmap® which took into account con-
chain 7 can be specified by its base configuration and
straints due to joint limits, self-collision, and collision
its joint variables. In particular, the configuration
with environment obstacles. A point-to-point inverse
of the base can be specified by the Euclidean (rigid
kinematics problem was then solved by querying the
body) transformation from the world frame Fw to
roadmap.
the body frame Fg;: gus, = (Pwe;, Rwo,) € SE(d)
In [25], the recently proposed Rapidly-exploring Ran- where d € {2,3} is the dimension of the workspace,
dom Trees (RRT) [17] strategy was used to greatly de- Pwo; € R* and Ry», € SO(d) are, respectively, the
crease the computation time required for several of the position and orientation of Fg; relative to Fw, and
examples studied in [18]. SE(d) denotes the special Euclidean group. Denote
by 6; = (Gi1,.-.,Bin;), the vector of joint variables
for chain 7. For a revolute joint, the joint variable is
’While the roadmap in [1] is called a kinematic roadmap,
it is different from our kinematic roadmap which we con- an angle (;; € [0,27), with the angle 27 equated to
struct without any knowledge of the obstacles in the envi- angle 0, which is naturally associated with a unit cir-
ronment. cle in the plane, denoted by $1, and hence we write
A Kinematics-Based PRM for Closed Chain Systems
237
Biz € S'. A prismatic joint is described by a linear standard PRM planners, as we will see, the structure
displacement 3;; € R along a directed axis. In sum- of Cclosure can be utilized to guide the generation and
mary, the configuration space of a multi-link robot can connection of closure configurations.
be represented as:
Finally, for both open and closed chain systems,
feasible configurations should not involve collision be-
C= 1 Gussie emunenGn)|
tween the robot and an obstacle, or self-collision among
Jwb; © SE(d), 3; @ Six RPGS 1,---,k}.(1) the links. We denote by C free the set of robot config-
where r; and p; are the number of revolute joints and urations g € C which do not cause any collision in the
system.
prisinatic joints for link 7, respectively.
Using the notation defined above, the closed-chain
One of the reasons PRMs work well for systems with-
out closed chains is that any configuration q sampled motion planning problem can be defined as follows:
from C is a valid configuration (when collision con- Problem 1 Given a start configuration qo and a goal
straints are ignored). However, this is not true for configuration qi, the objective of the planner is to find
systems involving closed chains where qg must also sat- a path q(t),t € [0,1], such that g(0) = qo, g(1) = u,
isfy the closure constraints. For example, the two end- and Vt € (0, 1], q(t) CC aoaurel Cr rees
effector frames in Figure 4 must coincide:
4 A Kinematics-Based PRM
Gwe, = Gwb,Jbie1 (G1) = Gwb2IJb2e2 (G2) = Qwe2 (2)

where 9, e;(G;),7 = 1, 2, is the forward kinematic trans- In this section, we describe the high-level strategy of
our kinematics-based PRM for closed chain systems.
formation [7, 19] of chain 7 which determines the end-
frame configuration based on joint variables. (Inverse We begin by noting that the closure constraint in
kinematics solves the inverse problem of determining Equation 2 can be reduced to:
proper joint variables to achieve some specified end-
frame configuration.) Jud; (Gbrer (F1) — Gb1b2Gb22 (G2)) = 0 (4)
The closure constraint is often expressed in the form Joye; (1) — Jb1b2Jb2e2 (G2) =0 (5)

f(q) = 0 (e-8., Gwe, — Gwe = 0). When multiple closed where gp,c,,2 = 1,2, are the end-frame configurations
chains exist in a linkage system, each closed chain im- described in the body frame F'g;, and go,», is the trans-
poses one closure constraint; the [th such constraint is formation from the base link of chain 1 to the base link
denoted by f7, and we use f(q) = 0 to denote fi(¢) = 0, of chain 2. If we think of the base link of the first chain
for all 1 < 1 < K, where K is the number of closed as the virtual (mobile) base of the system, its configura-
chain constraints. In general, the valid configurations tion gw, can be interpreted as a rigid body motion on
of a closed chain system lie in the set: the system. In other words, Equation 4 reveals one im-
portant property of closure configurations: Rigid body
Cetosure. = {a\q €C and f(g) ai O} 2 (3)
transformations preserve closure configurations. Equa-
tion 5 further shows that closure constraints can be
Notice that closure constraints such as Equation 2 defined independent of the base configuration.
can be transformed to the zeros of polynomials us-
ing the projective transformation. Then, the valid In the following discussion, we will use gy» to de-
configurations of the system, Cctosure, define a lower- note the configuration of the virtual base of the system.
dimensional algebraic variety embedded in the higher- In addition, we will treat the transformation from the
dimensional configuration space C. This is roughly system base to the base of any chain, say chain 7, as a
virtual link with joint variables being the parameteriza-
analogous to embedding a 2D surface or a 1D curve
tion of the transformation gpp,, €.g., a position vector
in a 3D space. The fact that the volume measure of
a low-dimensional entity in a high-dimensional ambi-
(prismatic joints) py, € R* and an orientation vec-
apes CGD)
ent space is zero is why the probability that a random tor (revolute joints) ay, € S” 2. Thus, every open
configuration q € C will satisfy the closure constraint chain can be viewed to be virtually extended to the sys-
is zero. While closure constraints pose difficulties for tem base. As a result, we can define a joint variable for
238 L. Han and N. M. Amato

the extended chain as 0; = (pop,;, @vv;, Gi), t = 1,---,F. by solving the closure constraints (Equation 7), and
For simplicity, we will call 6; the joint variable of sub- retain the self-collision free closure configurations. For
chain 7, with the understanding that it includes the vir- example, consider the closed chain shown in Figure 4.
tual joint variables, when applicable. Define the joint Suppose we select the joint variables of chain 2, 42, as
variable of the system to be 0 = (6;,---,0,) € S” x R?, the active variables 6,, and randomly generate values
where p and r are the total number of prismatic joints for them. We then use forward kinematics to determine
and revolute joints, respectively. Then the system con- the end-frame configuration gye,, and then use inverse
figuration space can be rewritten as: kinematics to compute joint variables of chain 1 (the
passive variables 6,) which will make goe, coincide with
C={(gu2,)loune SE(d),8dES"xR?}. (6) Jobe, and satisfy the closure constraints.

Since the closure constraint (Equation 2) or its gen- The closure configurations generated will be the ver-
eral form f(q) = 0, in fact, does not depend on the tices of a (small) roadmap which records paths con-
base configuration and only specifies constraints with necting self-collision-free closure configurations (again,
respect to joint variables, we can define the equivalent with no dependence on the base configuration gw»).
constraints of f on the joint variables as: The edges in this roadmap can be generated using
straight-line, or any other simple local planner to con-
f(0) =0 (7) nect the active variables 0, of the two closure configu-
rations, and then computing the corresponding passive
Furthermore, define: variables 9, along the local path. As with any PRM, a
self-collision-free local path is recorded as a roadmap
Culosure = {916 € Ss” x RY, f(0) = o}0 (8) edge. By a slight abuse of terminology, we call such a
roadmap a kinematic roadmap since it reveals the kine-
Now, the subset of C corresponding to closure configu- matic connectivity of the closure structures, and its
rations can be expressed as: construction involves the computation of both forward
and inverse kinematics. Figure 5(a) shows a three-node
Casi’ a {(Su; 9)| Gwe ‘= SE(d), ve (ai aca (9) kinematic roadmap for a 4-link closed chain.

The second observation suggests that we place multi-

The following two observations summarize the above
ple copies of the same closure configuration in the envi-
discussion, and form the basis of our two-stage PRM
ronment. There are two advantages to this approach.
closed chain planner:
First, we quickly populate the environment with clo-
Observation 1 Only the joint variables @ determine sure configurations and amortize the cost of computing
if a configuration q = (gwv,9) is closure or not. The a closure configuration. Second, we can treat configu-
closure constraint defines an algebraic variety (Equa- rations with the same closure structure as rigid body
tion 8) on 8 € S" x RP, which can be parameterized al- configurations, which can then be connected by effi-
most everywhere. In other words, 0 can be partitioned cient rigid body PRM local planning methods.
into 0, and Oy, 0 = 0a X Op, where 6, can be determined
While one could generate a roadmap simply from
from 0, based on the closure constraints (Equation 7).
configurations with one closure structure, the query
Observation 2 A given “closure configuration” 6, process would have to connect any start configuration
can be combined with different base configurations and goal configuration to this closure structure, which
Jwp € SE(d), which corresponds to placing the same might become very hard, or might not be possible at
closure configuration at different locations in the envi- all. Hence, instead of copying only one closure config-
ronment. uration, we copy an entire kinematic roadmap to the
environment and retain all vertices and edges that are
The first observation suggests an efficient way to gen- collision free (see Figure 5(b)). Next, we group con-
erate closure configurations: Ignore the environment figurations by closure structure, and attempt to make
(obstacles) and set a nominal base configuration, ran- (rigid body) connections within each group (see Fig-
domly generate 0, and determine the corresponding 6, ure 5(c)).
A Kinematics-Based PRM for Closed Chain Systems
239

(a) (b) i)
Figure 5: (a) A Kinemattc roadmap (Workspace), (b) copying the kinematic roadmap to different base configurations
(C-space), and (c) connecting configurations with the same closure structure (C-space).

5 The Kinematic Roadmap

This section discusses a PRM planner to construct

a kinematic roadmap. Recall that the kinematic
roadmap encodes the connectivity of the closure con-
figurations 9 € Cgiosure and does not depend on the
environment.

KINEMATIC ROADMAP CONSTRUCTION

1. NODE GENERATION
(find self-collision-free closure configurations)
2. CONNECTION
(connect nodes and save paths with edges)
(repeat as desired)

5.1 Node Generation

The task here is to generate joint variables 6 which sat-

isfy the closure constraints and retain self-collision-free
closure configurations as kinematic roadmap nodes.
Recall that @ can be partitioned into active and pas- Figure 6: Closure configurations for some kinematic
sive variables, 6, and 6,, where the value of #, can chains generated by our planner.
be determined for a given 6, value based on closure
constraints. Op, where | = 1,...,K, is the index of the closed chain.
From an algorithmic point of view, 9, needs to be We also need to ensure that the chain corresponding to
chosen such that for a given value of 6,, the corre- the passive joint variables 0, has a closed-form inverse
sponding value of 0, satisfying the closure constraints, kinematic solution. This can always be done, for exam-
if any, can be computed efficiently. While solutions ple, by choosing any three consecutive joint variables
are not known for inverse kinematics problems for gen- in each closed chain as Op.
eral linkages, the closed-form inverse kinematic solu-
tions for simple chains (such as 4-link chains) and most NODE GENERATION FOR KINEMATIC ROADMAP
industrial robots do exist. Therefore, we are most 1. Randomly generate 0.
2. Use forward kinematics for active chains to
interested in choosing 6, and 9, as consecutive joint compute end-frame configurations gia,! = 1,...,K,
variables. More specifically, for a system involving K at the break point of each closed chain;
closed chains, each closed chain is broken into two open 3. Use inverse kinematics for passive chains to compute
chains: one an “active chain” with joint variables a joint variables 0, to achieve the end-frame
and the other a “passive chain” with joint variables configurations computed in Step 2.
240 L. Han and N. M. Amato

4. Ifasolution is found in Step 3 (closure exists)

5. If closure configuration 0 = (0a, 9p)
is self-collision free
6. retain 0 as a kinematic roadmap node

In Step 3, if multiple solutions exist for the inverse

kinematics problem, we can either keep all solutions
or randomly choose one. Figure 6 shows some closure
configurations generated by our planner.
Finally, we note that when a link or joint is involved
in multiple closed chains, i.e., when the system involves Figure 8: Different joint partition schemes.
common loops, the closed chain constraints need to
be carefully handled to guarantee that different closed
chains will result in the same link configuration or the For the efficiency of the PRM node generation, it is
same joint variable. In particular, we can use the al- important to choose 6, and 6, in a way that the in-
gorithm above to close loops one by one, by choosing verse kinematics for the passive chains, corresponding
joint values to avoid breaking loops while creating other to the @, joint variables, has a closed form solution.
loops. The first loop can choose arbitrary values for its It is also important to maximize the probability that
active joint variables 61. If the loop cannot be closed, a closure configuration can be obtained given a ran-
i.e., there does not exist 6, satisfying the closure con- domly generated 6,. For example, consider the closed
straints, then discard it. Otherwise, continue working chain shown in Figure 8. Two possible selections of 0,
on its neighboring loop, say loop 2. Assume the joints are shown: (i) the two joints marked with black cir-
612 are common for loop 1 and loop 2. Then we will use cles, or (ii) the three joints marked with squares. In
the values of 6)? that have been computed from loop 1 this case, the three joint option would be preferable,
as part of the active joint values 6? of loop 2 (to keep since the intersection of the workspaces of the active
loop 1) and then compute the corresponding Ga In gen- and passive chains is larger. In the following, we make
eral, if a loop shares joint variables with other loops, this argument more precise.
it has to keep the values of the common joint variables Recall that the workspace of an open chain with joint
that have been determined from the closure constraints variables 6 is defined as:
of other loops. Clearly, when a loop has more deter-
mined joint values, it is more constrained, and thus it
W= {Ibe € SE(d)|30 EQ, $.t.9be = Jve(9) } (10)
is more difficult to close the loop. One heuristic for
node generation of common loops is to start from the where Q is the joint space, i.e., the set of all possible
loop with the largest number of common joints. For joint variable values with joint limits taken into ac-
the example shown in Figure 7, it would be better to count, goe is the end-frame configuration with respect
start from the center loop. to the base, and gp-(0) is the forward kinematics of the
open chain.
For one closed chain, we denote the end-frame config-
uration of the active chain by goa, and the workspaces
loop 1 (center)
of the active chain and passive chains by W, and W,,
respectively.

Observation 3 A randomly generated value of 0, can

result in a closure configuration, i.e., there exist passive
joint variable values satisfying the closure constraint,
if and only if the end-frame configuration of the active
chain, goa, falls in the workspace of the passive chain,
Figure 7: A System with Common Loops. 1.€. Oba © Wo:
A Kinematics-Based PRM for Closed Chain Systems
241
Therefore, a rough estimate of the probability that
Finally, we note that increasing the probability that
a randomly generated 0, results in a closure configura- a closure configuration exists will in many cases com-
tions is: plicate the inverse kinematics for the passive chain. For
Volume(W, 1 W,) example, the length heuristic mentioned above might
prob(closure) ~ pa olumel Wane (11) result in longer passive chains with more complicated
inverse kinematics than the 3-joint/4-link chains we
where Volume denotes the volume of the workspace have selected.
measured in SE(d).
5.2 Node Connection
Remark 1 [f both W, and W, can be computed, then
a random closure configuration 0 = (0a,9p) can be ob- An edge between two closure configurations in the kine-
tained by randomly choosing a configuration in WrnWa matic roadmap consists of a sequence of intermediate
and using inverse kinematics of both the active chain closure configurations. Since it is relatively expensive
and the passive chain to compute 6, and §,. This is one to generate closure configurations, the edges of the
of the most effective ways to generate a random closure kinematic roadmap are saved for future use. Node con-
configuration. However, it is not easy to compute the nection in the kinematic roadmap follows the standard
workspaces. We will still use 0, and 0, in the following PRM framework.
discussion with the understanding that the knowledge of
the workspaces, when available, should be exploited to NODE CONNECTION FOR KINEMATIC ROADMAP
improve the effectiveness of node generation. 1. For any two “nearby” closure configurations 6; and 0;
2. Use (simple) local planner to find path from Oj. to
Since multiple joint variable values may result in the Osa: Oa(t),t <€ [0, 1], where 6.(0) = Gia; Oa(1") = Oja
3. For each intermediate point on the path 6.(t)
same end-frame configuration, the above probability
4, If inverse kinematics determines that no 6,(t)
measure is not accurate because it does not take the exists to satisfy the closure constraints
multiplicity into account. Denote by g~!(goa) the in- 5. return no-edge
verse kinematic solutions for one end-frame configu- 6. Choose the closure configuration 0(t) that is
ration of the active joints. Then, the probability of continuous from previous step
4 If @(t) involves self-collision, then return no-edge
obtaining a closure configuration from a randomly gen-
8. endfor
erated 6, is: 9. save the edge (and with it the path 6(t),t € [0, 1)
10. return edge-exist
Volume(g~! (Wy N Wa))
prob(closure) =
Volume(g-!(W,)) (2)
The connection of common loop configurations has
where g-!(W) denotes set of inverse kinematic solu- to be processed similarly as for the node generation
tions for each configuration in W, and where the vol- of common loops. In general, any connection strategy
ume is computed in the active joint space instead of and local planner for rigid body robots or serial chains,
SE(d) as in Equation 11. such as the nearest neighbors connection strategy and
As different joint partition schemes may result in dif- the C-space straight line local planner, can be used
ferent probabilities of successfully generating a closure to choose pairs of closure configurations for potential
configuration, the joint partition scheme should ideally edge generation and to connect the active joint vari-
be chosen based on this probability. However, the com- ables. When the system involves complicated closed
putation of the probability measure (Equation 12) in- chain structures, more sophisticated techniques such as
volves the computation of the workspaces, inverse kine- the Jacobian method or a point-to-point inverse kine-
matic solver [1] can also be used to generate kinematic
matics, and volume integrals, which is probably too
roadmap edges. The distance metric on the set Colosure
complicated to be practical for most linked systems.
Nevertheless, the probability measure provides us with can be, e.g., Euclidean distance (Equation 13) with the
insight that can be used to develop heuristics to guide modification that the distance between two joint angles
is at most 7.
the partition of the joint variables. For example, bal-
ancing the lengths of the active and passive chains is
one possible heuristic.
dist(0, — 02) = ||1 — 94|| (13)
242 L. Han and N. M. Amato

6 Building a Roadmap from for self-collision. Therefore, the reuse of the closure
a Kinematic Roadmap configurations and their connection edges can signifi-
cantly reduce the total number of collision detection
The kinematic roadmap provides us with a set of self- calls, which represent the major computation cost at
collision-free closure configurations 6 € Celosure (Equa- this stage.
tion 8) and connections between them. Therefore, by
randomly generating base configurations gp), we can 6.2 Connecting Same Closure Nodes
“populate” the environment with kinematic roadmap Closed-chain configurations with the same closure con-
nodes and edges—this will only require collision de- figurations can be viewed as configurations of a rigid
tection with environment obstacles since we save the
body. Therefore, we can use rigid body PRM methods
paths associated with the kinematic roadmap edges.
to connect them. More specifically,
Furthermore, roadmap nodes generated from the same
closure configuration can be treated as rigid body con- CONNECTING NODES OF SAME CLOSURE ‘T'YPE
figurations during roadmap connection. 1. For each closure configuration 6 in kinematic roadmap
2. Collect all roadmap nodes with this closure
PROTOTYPE KINEMATICS-BASED PRM configuration in a set
I. POPULATE ENVIRONMENT WITH KINEMATIC ROADMAP a: Use rigid body PRM connection methods to connect
generate random base configurations and retain configurations in the set
collision-free parts of kinematic roadmap in roadmap 4. Add the edges generated in Step 3 to the roadmap
II. ADDITIONAL CONNECTION OF ROADMAP NODES 5. endfor
connect roadmap nodes with the same closure
structure using rigid body planners Figure 9 shows a portion of the roadmap, being pro-
gressively built by connecting configurations with clo-
sure structures C1,C2, and C3, respectively.
6.1 Populating the Environment with
Copies of the Kinematic Roadmap 7 Experimental Results
POPULATING ENV. WITH COPIES OF KINEMATIC ROADMAP 7.1 Implementation Details
1. Choose random vertex @ from the kinematic roadmap
2 Generate random base configuration gw» Our prototype closed chain PRM planner was developed
3 If the configuration (gw»,0) is collision-free
on top of the C++ OBPRM software package developed
4. Retain (gws,9) as a roadmap vertex
5 For each neighbor of 0, say 0, in the kinematic by the robotics group at Texas A&M University [2,
map (repeat with their neighbors as needed) 4|. This strategy was taken because the construction
6 If (gw, ) is collision-free of the kinematic roadmap and the connection of the
re Retain (gus,9) as a roadmap vertex roadmap nodes with the same closure configuration are
8 Retrieve the path 6(t) connecting 6 and 0 both basically simple PRM planners. It turned out to
from the kinematic map
be fairly easy to incorporate the closed-chains into the
9. If (gwo, 0(t)) is collision-free for all intermediate
closure configurations along the path ¥ PRM framework due to the object oriented design of the
10. Add an edge between (gws,9) and (gwo, 0) code. All experimental results reported in this section
(repeat as desired) were performed on an SGI Octane and used the RAPID
[8] package for 3D collision detection.
The generation of the random base configuration in
7.2 Experiments
Step 2 can be implemented with any node generation
strategy developed for rigid body robots such as PRM While our planner can handle complicated three-
[11], PRM with Gaussian filter [5], OBPRM [2], and the dimensional closed chains, the results presented here
medial-axis PRM [23]. We next note that since the are for single loop closed chains in three-dimensional
kinematic roadmap’s nodes and edges are known to environments. In particular, the chains we consider
be self-collision free, the only collision checks needed consist of m identical links, all joints are revolute, and
in this stage are between environment obstacles and we partition the m joint angles into 3 consecutive pas-
the robot, i.e., it is not necessary to check the links sive angles and m — 3 active angles.
A Kinematics-Based PRM for Closed Chain System
s 243

(aJe eee acl pale |

gl g2 g3 | ee a c gl g2 g3
cl Qa \ a \s / cal Re
Nel Aa? AY / i
o : |
| 4elNe wy
13, f |
(\I\ |
C3 A:
. Pas /,
van : ~ cl
@ a SS J

(c)

Kinematic Roadmap Co nstruction

Chain [Generation
||Connection |
Links
6(P
sec |cfg||sec]
24.66
CC]

11(S) |] 1.76

Table 1: Kinematic roadmap construction times (seconds)

Figure 10: The “Walls” Environment.
and statistics for k-link closed chains, k = 6,7,11 and 15.
In the table, planner and spatial chains are labeled respec-
tively with P and S following their link numbers, and cfg results shown in the table are consistent with the analy-
and CC denote the number of nodes and the number of con- sis of Section 5.1. Namely, for both planar and spatial
nected components respectively in the resulting kinematic types of chains, the planner generated more closure
roadmap. Note that the roadmap for a single planar closed nodes for the chains with fewer links, which are also
chain should have two connected components. those for which the partition into the active and pas-
sive chains is most equal, and where their workspaces
overlap the most. In addition, the results for planner
We first study the effectiveness of our method for
chains are better than their spatial counterparts, again
generating the kinematic roadmap, which involves gen-
due to their larger overlapping workspaces. Clearly,
erating and connecting closure configurations in the ab-
the simple PRM kinematic roadmap planner does not
sence of obstacles in the environment. We used the al-
work as well for the 15-link planar chain and 11-link
gorithm presented in Section 5.1 to check if it is feasible spatial chain. We are working on incorporating more
for a randomly generated active joint angle to achieve sophisticated planning methods, such as the Ariadne’s
a closure configuration. As seen in Figures 3 and 6, Clew algorithm [1], into our system which should help
this method was effective in generating and connecting in such situations.
closure configurations.
However, as predicted in Section 5.1, our success in We now analyze the benefit of the kinematic pre-
generating closure configurations is closely tied to the processing. That is, the benefit of using the kinematic
“balance” of the partition of the joints into the active roadmap as opposed to simply generating closure con-
and passive sets. Table 1 shows the kinematic roadmap figurations directly in the environment (as was done in
statistics for closed chains with various numbers of [18]). To study this issue, we compare roadmaps con-
links (all cases contained three passive joint angles). structed with and without the kinematic preprocess-
The nodes were generated from 2000 trials using the ing. We used the environment shown in Figure 10.
algorithm sketched in Section 5.1. The edges were gen- Table 2 shows the statistics for 7 and 9-link chains.
erated by trying to connect each node to its 20 nearest The roadmaps without kinematic preprocessing were
neighbors using a C-space straight line planner. The generated using the PRM planner implemented in the
244 L. Han and N. M. Amato

Roadmap Construction roadmap, and rigid body connections are made be-
Chats |Kinemane Met | Generation | Comertion | tween nodes with the same closure type. Both stages
ec eeree ea employ PRM planners to construct their roadmaps. Our

[re | | |= [525|sss|eaor|
AP) e2o eo 2.44 | 341 || 12.56 ee | preliminary experimental results indicate that the use
of kinematics to guide the generation and connection of

[ ow) | TE es|oefate |
9(P 0.44} 13 3 || 0.99 | 132 9.27 3 |
closure configurations is very beneficial, both reducing

[as PPTL Else] sof) 1] 6

7S 0.31 7 0.09 43 3.93 Fal the computation costs and improving the connectiv-
ity of the resulting roadmap as compared to previous
purely randomized approaches.
Table 2: Roadmap construction times (seconds) and sta- Therefore, we believe that augmenting the ran-
tistics for roadmaps constructed with and without the kine- domized philosophy of PRMs with more deliberate
matic roadmap. In the table, planner and spatial chains techniques developed in the robotics community is
are labeled respectively with P and S following their link a promising direction to pursue for motion planning
numbers, and cfg and CC denote the number of nodes and problems involving additional constraints.
connected components in the roadmap (there are two), re- While our results are promising, there are still many
spectively. issues to be addressed. First, we intend to more fully
exercise our current closed chain PRM planner on more
OBPRM software package. The nodes were generated complex linkages. We anticipate that the computa-
from 4000 attempts, using the generation method out- tional costs will grow, so that additional optimization
lined in Section 5.1, the only difference being that will be required. One area we intend to explore in
now the base configuration was randomly generated this regard is parallelization, which has been shown to
and collision was checked with the obstacles in the be effective for traditional PRMs [3]. We also plan to
environment as well. The edges were generated us- study more complicated manipulation planning prob-
ing the straight line planner to connect each node to lems such as regrasp planning.
its 20 nearest neighbors. The kinematic map nodes
were generated from 400 attempts for planer chains Acknowledgments
and 500 attempts for the spatial 7-link chain, and,
again, the 20 nearest neighbors were checked using the We would like to thank Drs. Steve Wilmarth, Jeff Trin-
straight line planner. Then, five different base configu- kle, and Peter Stiller for valuable discussions. We are
rations were generated for each closure node when pop- also grateful to the robotics group at Texas A&M, es-
ulating the environment with copies of the kinematic pecially Lucia Dale and Guang Song, for their help
roadmap. The final rigid body connections between regarding the OBPRM software package.
configurations with the same closure type also used the This research was supported in part by NSF CA-
straight line planner with the 20 nearest neighbors. As REER Award CCR-9624315, NSF Grants ITS-9619850,
can clearly be seen from the table, the roadmaps con- EIA-9805823, and EIA-9810937, and by the Texas
structed using kinematic preprocessing are superior in Higher Education Coordinating Board under grant
all aspects: faster computation and improved roadmap ARP-036327-017.
quality (fewer connected components).
References
8 Conclusion
[1] J. M. Ahuactzin and K. Gupta. The kinematic
This paper presents a kinematics-based probabilistic roadmap: A motion planning based global approach
for inverse kinematics of redundant robots. JEEE
roadmap planner for closed chains. The two-stage con- Trans. Robot. Automat., RA-15(4):653-670, 1999.
struction of our roadmap first builds a (small) kine-
matic roadmap that deals solely with the robot’s kine- [2] N. M. Amato, O. B. Bayazit, L. K. Dale, C. V. Jones,
and D. Vallejo. OBPRM: An obstacle-based PRM for
matics and utilizes both forward and inverse kinemat-
3D workspaces. In Proc. Int. Workshop on Algorith-
ics in its construction. In the second stage, the en- mic Foundations of Robotics (WAFR), pages 155-168,
vironment is populated with copies of the kinematic 1998.
A Kinematics-Based PRM for Closed Chain Systems
245

[3] N. M. Amato and L. K. Dale. Probabilistic roadmap

trol algorithms. In Proc. Int. Workshop on Algorith-
methods are embarrassingly parallel. In Proc. IEEE
mic Foundations of Robotics (WAFR), pages 375-386,
Int. Conf. Robot. Autom. (ICRA), pages 688-694,
1998.
1999.
[15] J. C. Latombe. Robot Motion Planning. Kluwer Aca-
[4] N. M. Amato and Y. Wu. A randomized roadmap
demic Publishers, Boston, MA, 1991.
method for path and manipulation planning. In Proc.
IEEE Int. Conf. Robot. Autom. (ICRA), pages 113- [16] J.-P. Laumond and M.H. Overmars. Algorithms
120, 1996. for robotic motion and manipulation. In Proc. Int.
Workshop on Algorithmic Foundations of Robotics
[5] V. Boor, M. H. Overmars, and A. F. van der Stap- (WAFR), 1996.
pen. The gaussian sampling strategy for probabilistic
roadmap planners. In Proc. IEEE Int. Conf. Robot. [17] S. M. LaValle and J. J. Kuffner. Randomized kino-
Autom. (ICRA), pages 1018-1023, 1999. dynamic planning. In Proc. IEEE Int. Conf. Robot.
Autom. (ICRA), pages 473-479, 1999.
[6] J. F. Canny. On computability of fine motion plans. In
Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pages [18] S.M. LaValle, J.H. Yakey, and L.E. Kavraki. A prob-
177-182, 1989. abilistic roadmap approach for systems with closed
kinematic chains. In Proc. IEEE Int. Conf. Robot.
[7] John J. Craig. Introduction to Robotics: Mechanics Autom. (ICRA), 1999.
and Control, 2nd Edition. Addison-Wesley Publishing
Company, Reading, MA, 1989. [19] Richard M. Murray, Zexiang Li, and S. Shankar Sas-
try. A Mathematical Introducation to Robotic Manip-
S. Gottschalk, M.C. Lin, and D. Manocha. Obb-tree: ulation. CRC Press, Boca Raton, FL, 1994.
A hierarchical structure for rapid interference detec-
tion. ‘Technical Report TR96-013, University of N. [20] A. Nguyen, L. J. Guibas, and M. Yim. Controlled
Carolina, Chapel Hill, CA, 1996. module density helps reconfiguration planning. In
Proc. Int. Workshop on Algorithmic Foundations of
L. Kavraki, F. Lamiraux, and C. Holleman. Towards Robotics (WAFR), 2000.
planning for elastic objects. In Proc. Int. Workshop on
Algorithmic Foundations of Robotics (WAFR), 1998. [21] A.P. Singh, J.C. Latombe, and D.L. Brutlag. A motion
planning approach to flexiable ligand binding. In 7th
L. Kavraki and J. C. Latombe. Randomized pre- Int. Conf. on Intelligent Systems for Molecular Biology
processing of configuration space for fast path plan- (ISMB), pages 252-261, 1999.
ning. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA),
pages 2138-2145, 1994. [22| D. Stewart. A platform with six degrees of free-
dom. Proc. of the Institute of Mechanical Engineering,
180(part I(5)):171-186, 1954.
[11] L. Kavraki, P. Svestka, J. C. Latombe, and M. Over-
mars. Probabilistic roadmaps for path planning in
high-dimensional configuration spaces. [EEE Trans. [23] S. A. Wilmarth, N. M. Amato, and P. F. Stiller.
MAPRM: A probabilistic roadmap planner with sam-
Robot. Automat., 12(4):566-580, August 1996. pling on the medial axis of the free space. In Proc.
IEEE Int. Conf. Robot. Autom. (ICRA), pages 1024—
[12] O. Khatib. Stanford Robotic Manipulation Group,
1031, 1999.
robotics.Stanford.EDU/groups/manips/projects.

[13] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holm-

[24] S. A. Wilmarth, N. M. Amato, and P. F. Stiller. Mo-
tion planning for a rigid body using random networks
berg, and A. Casal. Vehicle/arm coordination and on the medial axis of the free space. In Proc. ACM
multiple mobile manipulator decentralized coopera- Symp. on Computational Geometry (SoCG), pages
tion. In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), 173-180; 1999.
pages 546-553, 1996.
[25] Jeffery H. Yakey. Randomized path planning for link-
[14] K. Kotay, D. Rus, M. Vona, and C. McGray. The ages with closed kinematic chains. Master’s thesis,
self-reconfiguring robotic molecule: Design and con- Iowa State Univ, Computer Science Dept., 1999.
 ‘vr t Saar

sroliondkiogt easoisamtal

Ww re GF ji; “4 rersTag ee vio y am

‘
pothole cay hyvalshan unaietgao eM
AL My WOR yay aecn rina ¢
any i Vee Ant ene
4 it ontet,
et te SURI Bie, allies —
at. fee Ls ity waar oi hy
a
sd ee Ve vt?1 APR ; ae te =~ re
Ricdas ni WCC IES woolen a
* =. ‘ i
ore -
vanomtian Railee iWag vere ;
Bue Je Balti
- Hen SRI peta
(er 10 Ct net 1 ony taper incr
ABN vs? P a : ip a tan be TUE
Ai s eat-with) allPeon at lupe"ai a, Fe ip)
‘: ; : OS ia rua Oo abn ih a ¥: Ha
PSD eh |) mt rer PUG ie eRe
a es a Oe r
" !
orate Dee
bitin oe "4s
rae ree can’

a
prais “ait mes

1h uySS¥ PIAA
oss J3 Nee* AUG Ty .

a
> ne Se

RH
. se yt
"
ee ede Ol thai
six rab, Be)
Te relies Pacers
iy
DL ayy A al — rH % tie Pi! i
‘ pind ra
ie i 1% a ostz
Gre Viva a Pek! e hol
DRE A rite y In: ee:
eo vaif %. baleaib, ie
a ay mee

mt... ' C atiiar % pee éexvumine ~y

bye 44 i “Mth phy, Dla | |
leaade T)- « et grt
ders ete ;
et og ” a Wit ah Ne Ao pet y Maat 4 a haat ba! ari
Sst igh bral inys wa), de | rr: Rae Hy
oy Cand > oe
4b i leet, vil? Pal
Biff
sig
“| ; SS
a Av i: : eau
ne 1 4 ‘a }3
‘5 ‘ uD
Aes bet
ewregn = ed ish a oye. Mover
|

. — 4, er:
Sen da Ubenid,; 1/7... ria Vaart wd A a1, th
¢ sy a 4
sary Aw it icoeed oN a ve ‘ ie re ees sep
a In
sf é tongs See ee ae ee at & Cite a tat re a rae
nee f re
mWia eh wt a, fa Lt | ee CG ae fete? Gn bewdla sey Poke,
7 7 :
irety wv i ' byi’ an ai ¥ a lis way Wed
7 ' ~~ 4 Sah Ue i : ; ’
: ani wit, er hap Peper dary slater! i les
‘
Wy Weoredgiin OL J wi
so B'S NES Avett: ayite gees 4 ath’ ** he
ONE) Ref D ey Sa He tbh Pea nye spe)
‘ : - wr NW, ise te
- :

;
§
best
e 7

b GD she
Randomized Kinodynamic Motion Planning
with Moving Obstacles
David Hsu, Stanford University, Stanford, CA
Robert Kindel, Stanford University, Stanford, CA
Jean-Claude Latombe, Stanford University, Stanford, CA
Stephen Rock, Stanford University, Stanford, CA

A randomized motion planner is presented for robots free geometric paths [18]. A PRM planner samples the
that must avoid collision with moving obstacles under robot’s configuration space at random and connects the
kinematic and dynamic constraints. This planner sam- generated free samples, called milestones, by simple
ples the robot’s statex time space by picking control in- local paths, typically straight-line segments in config-
puts at random and integrating the equations of motion. uration space. The result is a undirected graph called
The result is a probabilistic roadmap, i.e., a collection a probabilistic roadmap. Multi-query PRM planners
of sampled statex time points, called milestones, con- precompute the roadmap [18], while single-query plan-
nected by short admissible trajectories. The planner ners compute a new roadmap for each query [11]. It
does not precompute the roadmap; instead, for each has been proven that, under reasonable assumptions, a
planning query, it generates a new roadmap to con- small number of milestones picked uniformly at random
nect the input initial and goal statex time points. This are sufficient to capture the free space’s connectivity
paper shows that the probability that the planner fails with high probability [11, 17]. However, with nonholo-
to find a trajectory when one exists quickly goes to 0 nomic and/or dynamic constraints, straight local paths
as the number of milestones grows. The planner has are not feasible. Moreover, allowing obstacles to move
been tested successfully in both simulated and real en- requires indexing milestones by the times when they
vironments. In the latter case, a vision module esti- are attained.
mates the obstacle motions just before planning, and
For each planning query, our algorithm builds a
the planner is then allocated a small amount of time to
new roadmap in the collision-free subset of the robot’s
compute a trajectory. If a change in obstacle motion is
statextime space, where a state typically encodes both
detected while the robot executes the planned trajectory,
the configuration and velocity of the robot (Section 3).
the planner re-computes a trajectory on the fly.
Each milestone is obtained by selecting a control in-
put at random in the set of admissible controls and
1 Introduction integrating the motion induced by this input over a
In its most basic form, motion planning is a purely geo- short duration of time, from a previously-generated
milestone. The local trajectory thus obtained auto-
metric problem: Given the geometry of a robot and sta-
matically satisfies the motion constraints, and if it does
tic obstacles, compute a collision-free path of the robot
not collide with the obstacles, its endpoint is added
between two configurations. This formulation ignores
to the roadmap as a new milestone. This iterative
several key aspects of the real world. Robot motions
process yields a tree-shaped roadmap rooted at the ini-
are subject to kinematic and dynamic constraints that,
tial statextime and oriented along the time axis. It
unlike obstacles, cannot be represented by forbidden
ends when a milestone falls in an “endgame” region
regions in the configuration space. The environment
from which it is known how to reach the goal.
may also contain moving obstacles requiring that com-
puted paths be parametrized by time. In this paper, we In Section 4, we show that the probability that our
consider motion planning problems with both kinody- algorithm fails to find a trajectory when one exists con-
namic constraints and moving obstacles. We propose verges toward 0 (probabilistic completeness), with a
an efficient algorithm for this class of problems. convergence rate that is exponential in the number of
Our work extends the probabilistic roadmap (PRM) generated milestones. We have also tested the algo-
rithm in both simulated and real environments. In
framework originally developed for planning collision-
248 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

simulation (Sections 5 and 6), we have verified that straints. With few exceptions (e.g., [9]), previous work
it can solve tricky problems. In the hardware robot has considered these two types of constraints sepa-
testbed (Section 7), we have checked that the planner rately.
operates properly despite various uncertainties and de- Planning for nonholonomic robots has attracted con-
lays associated with an integrated system. In this test- siderable interest [4, 19, 20, 22, 23, 26, 28]. One ap-
bed, a vision module measures obstacle motions, and proach [19, 20] is to first generate a collision-free path,
the planner has a short, predefined amount of time to ignoring the nonholonomic constraints, and then to
compute a trajectory (real-time planning). The vision break this path into small pieces and replace them
module monitors the obstacles while the robot executes by feasible canonical paths (e.g., Reeds and Shepp
the computed trajectory. If an obstacle deviates from curves [24]). This approach works well for simple ro-
its predicted trajectory, the planner re-computes the bots (e.g., car-like robots), but requires the robots to be
robot’s trajectory on the fly. locally controllable [4, 23]. A related approach [26, 28]
uses a multi-query PRM algorithm that connects mile-
2 Previous Work stones by canonical path segments such as the Reeds
and Shepp curves. Another approach, presented in [4],
Motion planning by random sampling This ap- generates a tree of sampled configurations rooted at the
proach was originally proposed to solve geometric path- initial configuration. At each iteration, a chosen sam-
planning problems for robots with many degrees of free- ple in the tree is expanded into a few new samples, by
dom (dofs) [2, 3, 11, 16, 18]. Sampling replaces the integrating the robot’s equation of motion over a short
prohibitive computation of an explicit representation duration of time with deterministically picked controls.
of the free space by collision checking operations. Pro- A space partitioning scheme limits the density of sam-
posed techniques differ mainly in their sampling strate- ples in any region of the configuration space. This ap-
gies. An important distinction is between multi-query proach has been shown to be also applicable to robots
planners that precompute a roadmap (e.g., [18]) and that are not locally controllable. Our new planner has
single-query planners that don’t (e.g., [11]). Single- many similarities with this approach, but picks controls
query planners build a new roadmap for each query by at random. It is probabilistically complete whether the
constructing trees of sampled milestones from the ini- robot is locally controllable or not.
tial and goal configurations [11, 21]. Our planner falls
Algorithms for dealing with dynamic constraints are
in this second category.
comparable to those developed for nonholonomic con-
It has been shown in [11, 15, 17, 29] that, under straints. In [5, 27] a collision-free path is first com-
some assumptions, a multi-query PRM path planner puted, ignoring the dynamic constraints; a variational
that samples milestones uniformly from the configura- technique then deforms this path into a trajectory that
tion space is probabilistically complete and converges both conforms the dynamic constraints and optimizes
quickly. More formally, the probability that it fails to a criterion such as minimal execution time. No for-
find a path when one exists converges toward 0 ex- mal guarantee of performance has been established for
ponentially with the number of milestones. In [11], these planners. The approach in [7] places a regular
this result is established under the assumption that the grid over the robot’s state space and searches the grid
free space verifies a geometric property called expan- for a trajectory using dynamic programming. It offers
siveness. In this paper, we generalize this property to provable performance guarantees, but is only applica-
statextime space and prove that our new randomized ble to robots with few dofs (typically, 2 or 3), as the
planner for kinodynamic planning is also probabilisti- size of the grid grows exponentially with the number of
cally complete with a convergence rate exponential in dofs. Our planner is related to this second approach,
the number of sampled milestones. No formal guaran- but randomly discretizes the statextime space, instead
tee of performance had previously been established for of placing a regular grid over it. The planner in [21]
single-query planners. resembles ours, but no guarantee of performance has
Kinematic and dynamic constraints Kinodynamic been established for it.
planning refers to problems in which the robot’s mo- Moving obstacles When obstacles are moving, the
tion must satisfy nonholonomic and/or dynamic con- planner must compute a trajectory parametrized by
Randomized Kinodynamic Motion Planning with Moving Obstacles
249
time. This problem has been proven to be compu-
tationally hard, even for robots with few dofs [25].
Heuristic algorithms [8, 10, 14] have been proposed,
but they usually do not consider constraints on the ro-
bot’s motion other than an upper bound on its velocity.
The technique in [14] first ignores the moving obstacles
and computes a collision-free path of the robot among
the static obstacles; it then tunes the robot’s velocity
along this path to avoid colliding with moving obsta-
cles. The resulting planner is clearly incomplete. The
planner in [10] tries to reduce incompleteness by gen-
erating a network of paths. The planner in [9] is a
rare example of planner dealing concurrently with kin-
Figure 1: Model of a car-like robot.
odynamic constraints and moving obstacles. It extends
the approach of [7] to statex time space and thus is also
limited to robots with few dofs. the constraints F;(s,$) = 0 are equivalent to Equa-
tion (1) in which wu is a vector in R™ = R"—*, Recipro-
cally, Equation (1) can be rewritten into k = n—™m in-
3 Planning Framework
dependent equations of the form F;(s, $) = 0. Further-
emore, in Lagrangian mechanics, dynamics equations
Our algorithm builds a probabilistic roadmap in the
are of the form G;(q,q¢,@) = 0, where q, g, and g are
collision-free subset F of the statextime space of the
the robot’s configuration, velocity, and acceleration, re-
robot [9]. The roadmap is computed in the con-
spectively. Defining the robot’s state as s = (q,Q),
nected component of F that contains the robot’s initial
we can rewrite the dynamics equations in the form
statextime point.
F (s,s) = 0, which, as in the nonholonomic case, is
equivalent to Equation (1).
3.1 State-Space Formulation
Robot motions can also be constrained by inequali-
Motion constraints We consider a robot whose mo- ties of the forms F;(q,q) < 0 and Gi(q,4,q¢) < 0. Such
tion is governed by an equation of the form: constraints restrict the sets of admissible states and
controls to subsets of R” and R™.
sh f(s) 4h) (1)
Examples We illustrate these notions with two exam-
where s € S is the robot’s state, § is its derivative ples that will be useful later in the paper:
relative to time, and u € 2 is a control input. The set
Nonholonomic car-like robot. Consider a car A mod-
S and Q) are the robot’s state space and control space, eled as shown in Figure 1. Let (x,y) be the posi-
respectively. Given a state at time ¢t and a control tion of the midpoint R between A’s rear wheels and
function wu: [t,t’/] + 0, where [t, t’] is a time interval, 9 be the orientation of the rear wheels with respect
the solution of Equation (1) is a function s: [t,t/] > S to the x-coordinate axis. We encode A’s configura-
describing the robot’s state over [t,t’]. We assume that tion by (z,y,9) € R°. The nonholonomic constraint
S and 2 are bounded manifolds of dimensions n and m tan @ = y/« is equivalent to the system:
(m <n), respectively. By defining appropriate charts,
we can treat S and 2) as subsets of R” and R”. g=vcos#, y=vsind, 6 = (v/L) tan,
Equation (1) can represent both nonholonomic and
dynamic constraints. A nonholonomic robot is con- which has the same form as Equation (1). This refor-
strained by k independent, non-integrable scalar equa- mulation corresponds to defining the state of A to be
tions of the form F;(q,q) = 0, 1 = 1,2,...,k, where its configuration and choosing the vector (v, @), where
q and g denote the robot’s configuration and velocity, v and ¢ denote the car’s linear velocity and steering
respectively. Let the robot’s state be s = q. It is shown angle, as the input control. Bounds on v and ¢ define
in [4] that, under appropriate mathematical conditions, Q as a subset of R?.
250 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

Point-mass robot. For a point-mass robot A moving in than a milestone lying in an already densely sampled
a plane, we typically want to control the force applied region. This technique avoids oversampling any partic-
to A. So, we define A’s state to be s = (2,y,2,4), ular region of F.
where (z,y) is A’s position. The equations of mo-
Control selection Let Ul; be the set of all piecewise-
tion are:
Lae) 0, constant control functions with at most @ constant
y= Uy/ m,
pieces. Hence, for any u € Us, there exist to < ti <
where m and (uz, uy) denote A’s mass and the force ... <te_1 < tg such that u(t) is a constant c; € 2 over
applied to it. Bounds on the magnitudes of (4, y) and the time interval (t;_1,t;), fori = 1,...,4. We also
(Ue, Uy) define S and 2 as subsets of R* and R?, re- require t; —tj;-1 < Omax, where dmax is a constant. Our
spectively. algorithm picks a control u € Uy, for some prespeci-
Planning query Let S7 denote the statex time space fied £ and dmax, by sampling each constant piece of u
S x [0,+00). Obstacles in the robot’s workspace are independently. For each piece, c; and 6; = t — ti-1
mapped into this space as forbidden regions. The free are selected uniformly at random from 2 and (0, dmax],
space F C ST is the set of all collision-free points (s, t). respectively. The choices of the parameters £ and dmax
A trajectory s: [a,b] > S is admissible if, for all t € will be discussed in Subsection 4.4.
[a, 6], s(t) is an admissible state and (s(t), t) is collision-
free. Endgame connection The above “control-based”
sampling technique does not allow us to reach the goal
A planning query is specified by an initial statex time
(Sg,t,) exactly. We need to “expand” the goal into an
(sp,t») and a goal statextime (s,,t,). A solution to
endgame region that the sampling algorithm will even-
this query is a function wu: [t,, tg] > 2 that produces an
tually attain with high probability. There are several
admissible trajectory from state sp, at time t, to state
ways of creating such an endgame region.
S$, at time t,. In the following, we consider piecewise-
constant functions u(t) only. For some robots, it is possible to analytically com-
pute one or several canonical control functions that ex-
3.2. The Planning Algorithm actly connect two given points while obeying the kino-
Like the planner in [11], our algorithm iteratively builds dynamic constraints. The Reeds and Shepp curves de-
a tree-shaped roadmap T rooted at mp = (85, tp). veloped for nonholonomic car-like robots are an exam-
At each iteration, it picks at random a milestone ple of such functions [24]. If such control functions are
(s,t) from T, a time t’ < t,, and a control function available, one can test if a milestone m belongs to the
u: [t, t'] + Q. The trajectory from (s,t) induced by u engame region by checking that a canonical function
is computed by integrating Equation (1). If this trajec- generates an admissible trajectory from m to (sg, tg).
tory is admissible, its endpoint (s’, t’) is added to T as
A more general technique is to build a secondary tree
a new milestone; an arc is created from (s, t) to (s’,t’),
T’ of milestones rooted at the goal, in the same way as
and u is stored with this arc. The kinodynamic con-
straints are thus naturally enforced in all trajectories
that for the primary tree 7’, except that Equation (1) is
represented in T’. The planner exits with success when integrated backwards in time. The endgame region is
the newly generated milestone lies in an “endgame” then the union of small neighborhoods of the milestones
region that contains (s,, ty). in T’: The planner exits with success when a milestone
m € T falls in the neighborhood of a milestone m’ € T’.
Milestone selection The planner assigns a weight The trajectory following the appropriate arcs of T and
w(m) to each milestone m in T. The weight of m is T’ contains a small gap between m and m’, but this
the number of other milestones contained in a neigh- gap can often be dealt with in practice. For example,
borhood of m. So w(m) represents how densely the beyond m, one can use a PD controller to track the
neighborhood of m has already been sampled. At each trajectory extracted from T’.
iteration, the planner picks an existing milestone m in
T at random with probability 7;(m) inversely propor- Algorithm in pseudo-code The planning algorithm
tional to w(m). Hence, a milestone lying in a sparsely is formalized in the following pseudo-code. We will
sampled region has a greater chance of being selected refer to it as KDP.
Randomized Kinodynamic Motion Planning with Moving Obstacles
251
—————
Algorithm
SS
1 a ee ee eee

1. Initialize T with mp; i <1

2. repeat
Se Pick a milestone m from T with probability
T(m)
4. Pick a control function u from Uy uniformly at
random
5. m’ < PROPAGATE(m,u)
6. if m’ ~ nil then
e Add m' to T;4#i+1 Figure 2: A free space with a narrow passage.
8. Create an arc e from m to m’; store u with e
9. if m’ € ENDGAME then exit with SUCCESS
10. if i= N then exit with FAILURE Let us say that two points in F see each other (or are
mutually visible) if the straight line segment between
them lies entirely in F (no kinodynamic constraint is
In line 5, PROPAGATE(m, u) integrates the equations considered in this example). A classical PRM plan-
of motion from m with control wu. It returns a new ner samples F uniformly at random and connects any
milestone m’ if the computed trajectory is admissible; two milestones that see each other. Let the lookout of
S; be the subset of all points in S; that sees a large
otherwise it returns nil. If there exists no admissible
fraction of S2. If the lookout of S; were large, the plan-
trajectory from ms, = (Sp, to) to (sg,tg), the algorithm
ner would easily pick a milestone in 5S; and another in
cannot detect it. Therefore, in line 10, we bound the
S2 that see each other. However, due to the narrow
total number of milestones by a constant N.
passage between 5S; and 52, S; has a small lookout.
Consequently, it is difficult for the planner to generate
4 Analysis of the Planner a connection between S; and S32. In [11], F is said to
be expansive if every subset S C F has a large lookout.
The experiments that will be described in Sections 5-7
It is shown that in an expansive space, the convergence
demonstrate that KDP provides an effective approach
rate of a classical PRM planner is exponential in the
for solving difficult kinodynamic motion planning prob-
number of milestones.
lems. Nevertheless some important questions cannot
be answered by experiments alone: what is the proba- KDP generates a different kind of roadmap, in
bility ~ that the planner fails to find a trajectory when which trajectories between milestones may neither be
one exists? Does 7 converge toward 0 as the number straight, nor reversible. This leads us to generalize the
of milestones increases? In this section we show that notion of visibility to that of reachability. Given two
the failure probability y decreases exponentially with points (s,t) and (s’,t’) in F C ST, (s’,t’) is said to be
the number of sampled milestones. Hence, with high reachable from (s,t) if there exists a control function
probability, a relatively small number of milestones are u: [t,t’] + Q that induces an admissible trajectory
sufficient to capture the connectivity of the free space from (s,t) to (s’,t’). If (s’,
t’) remains reachable from
and answer the query correctly. Our analysis is based (s,t) by using wu€ Up, a piecewise-constant control
on a generalization of the notion of expansiveness pro- with at most £ segments, then we say that (s’,t’)
posed in [11]. is locally reachable, or ¢-reachable, from (s,t). Let
R(p) and Re(p) denote the set of points reachable
4.1 Expansive StatexTime Space and é-reachable from p, respectively; we call them the
reachability and the ¢-reachability set of p. For any
Expansiveness characterizes the difficulty of finding a
subset S C F, we define:
path between two points in a given space by random
sampling. To be concrete, let us first consider the sim-
ple example in Figure 2. The free space F consists of R(S) = (J R(p) and Re(S) = L Re(p).
two subsets S$; and Sz connected by a narrow passage. pes pes
252 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

We define the lookout of a subset S of F as the sub- Let M = (mo,m1,m2,...) be a sequence of mile-
set of all points in S whose “reachability sets overlap stones generated by KDP with IDEAL-SAMPLE (mo =
significantly with their reachability sets outside S: my), and let M; denote the first 7 milestones in M.
The milestone m, is called a lookout point if it lies in
the G-lookout of Ry(M;_1). Lemma 3 states that the
Definition 1 Let 3 be a constant in (0,1]. The (-
é-reachability set of M spans a large volume if it con-
lookout of S C F is:
tains enough lookout points, and Lemma 4 estimates
B-LOOKOUT(S) = the probability that this happens. Together, they im-
ply that with high probability, the reachability set of
{p€ S| u(Re(p)) \ S) 2 Bu(R(S) \ S)}, a relatively small number of milestones spans a large
where 4(Y )denote the volume of any subset Y C R(S) volume in #.
relative to R(S).
Lemma 3 /f a sequence of milestones M contains k
The free space F is expansive if every subset S C F lookout points, then u(Re(M)) > 1—e-?*.
has a large lookout.
Proof: Let (m;,,mi,,--. ,Mi,) be the subsequence of
lookout points in M. For any i = 1,2,..., we have:
Definition 2 Let a and ( be two constants in (0, 1].
For any p € F, R(p) is (a, 3)-expansive if for every con-
nected subset S C R(p), u(G-LOOKOUT(S)) > ap(S). u(Re(M;)) = w(Re(Mi-1)) + w(Re(m:) fiend
The free space F is (a, 3)-expansive if for every p € F,
R(p) is (a, 3)-expansive. Thus p(Re(M;)) > u(Re(M;)), for any i < j. In par-
ticular:
Think of p in Definition 2 as the initial milestone mp
and S$ as the f-reachability set of a set of milestones u(Re(M)) > w(Re(
Mi,)). (3)
produced by KDP after some iterations. If a and ( are
both reasonably large, then KDP has a good chance to Using (2) with i = 7, in combination with the fact that
sample a new milestone whose f-reachability set adds m,, is a lookout point, we get:
significantly to the size of S. In fact, we show below
that with high probability, the ¢-reachability set of the u(Re(Mi,)) 2 w(Re(Mi,-1)) + B wo& \ Re(Mi,-1))-
milestones sampled by KDP expands quickly to cover
most of R(mp); hence, if the goal (s,,t,) lies in R(ms), Let v; = u(Re(M;)). We observe:
then with high probability, the planner will quickly find
an admissible trajectory. W(X \Re(Mi,-1)) = w(¥) — w(Re(Mi,-1)) = 1-v4,-1.
4.2 Probabilistic Convergence of the Planner Hence, v4, > Vi,-1+ 6 (1—v;,-1), which can be rewrit-
ten as:
Let * = R(mp) be the reachability set of mp. Sup-
pose that ¥ is (a, 3)-expansive. We establish below an
Vin 2 Ving B (1 ea Veen a (1 = BN Gia a ae
upper bound on the number of milestones needed to
guarantee that a milestone lies in the endgame region Since 7, — 1 > ix_1, it follows that.v;,-1 — vi,_, > 0.
E with high probability, if #4 ¥ has non-zero volume Therefore, the previous inequality yields:
relative to Y. For convenience, we scale all volumes so
that u(¥) = 1. Vin 2. Vig gta eee)
Let us assume for now that there is an ideal sam-
pler IDEAL-SAMPLE that picks a point uniformly at Setting wz = v;, leads to the recurrence wy > we_1 +
random from the £-reachability set Ry(M) of any set 2 (1 — wr_1), with the solution:
of milestones M. The procedure IDEAL-SAMPLE re- k—1
places lines 3-5 in KDP. We will discuss how to ap- Wk > (1—B)*wo+B
Y (1-67 =1-(1—8)*(1—w).
proximate IDEAL-SAMPLE in Subsection 4.3.
j=0
Randomized Kinodynamic Motion Planning with Moving Obstacles
253
As wo > 0 and 1— 6 < e7*, we get we > 1—e7**. Proof: Divide M = (mo,m1,mzg,... ,M,) into two
Combined with (3), it yields: subsequences M’ and M” such that M’ contains the
first r’ milestones and M” contains the next r” mile-
w(Re(M)) > 1—e*. stones with r’ +r” =r. By Lemma 4, M’ contains k
Oo lookout points with probability at least 1—k(1 ~a)"/ Ry
If there are k lookout points in M’, then by Lemma 3,
Lemma 4 A sequence of r milestones contains k look- Re(M") has volume at least 1 — g/2, provided that
k > 1/@1n(2/g). As a result, Re(M’) has a non-
out points with probability at least 1 — ke~°l"/* ,
empty intersection J with E of volume at least g/2,
Proof: Let M be the sequence of r milestones and L and so does the é-reachability set of every subsequence
be the event that M contains k lookout points. Assume M; > M’.
that r is an integer multiple of k. We divide M into Since IDEAL-SAMPLE picks a milestone uniformly at
k subsequences of r/k consecutive milestones. Denote random from the ¢-reachability set of the existing mile-
by L; the event that the ith subsequence contains at stones, every milestone m; in M” falls in J with proba-
least one lookout point. Since the probability of bility (g/2)/u(Re(Mi-1)). Since u(Re(Mj_-1)) < 1 for
having k lookout points is greater than the probability all 7, and the milestones are sampled independently,
of every subsequence having at least one lookout point, M” contains a milestone in I with probability at least
we have: Wl Gil) empl caeied
Pr(L) = Pr(Ly (leloe eer lit Le); If M fails to have a milestone in HL, then either the
f-reachability set of M’ does not have a large enough
which implies: intersection J with E (event A), or no milestone of M”
k lands in J (event B). We know that Pr(A) < 7/2 ifr’ >
Toe As De Pri)! (k/a) In(2k/y) and Pr(B) < y/2 if r” > (2/9) In(2/7).
i=0 Choosing r > (k/a)In(2k/y) + (2/9) In(2/7) guaran-
tees that Pr(AUB) < Pr(A)+Pr(B) < ¥. Substituting
Since each milestone picked by IDEAL-SAMPLE has k = (1/() In(2/g) into the inequality bounding r, we
probability a of being a lookout point, the probability get:
Pr(L;) of having no lookout point in the ith subse-
quence is at most (1 — a)"/*. Hence: > InQ/9) ,, 2/9), 24,2
aoe By oN We
Pr(L) =1—Pr(ZL) >1—k(1—a)"/*. O

If r is not an integer multiple of k, we divide M into k— If KDP returns FAILURE, either the query admits no
1 subsequences of length |r/k| and a kth subsequence solution, i.e., (S9,tj) ¢ ¥, or the algorithm has failed
of length r —k|r/k|. We get: to find one. The latter event, which corresponds to re-
turning an incorrect answer to the query, has probabil-
Pe(iy ee Oe
ity less than y. Since the bound in Theorem 5 contains
O only logarithmic terms of 7, the probability of an in-
correct answer converges toward 0 exponentially in the
We are now ready to state our main result which es- number of milestones.
tablishes an upper bound on the number of milestones
The bound given by Theorem 5 also depends on the
needed to guarantee that the planner finds a trajectory
expansiveness parameters a, 3 and the volume g of the
with high probability, if one exists.
endgame region. The greater a, 3, and g, the smaller
Theorem 5 Let g > 0 be the volume of the endgame the bound. In practice, it is often possible to establish
region E in X and y be a constant in (0,1]. A se- a lower bound for g. However, a and # are difficult or
quence M of r milestones contains a milestone in E impossible to estimate, except for trivial cases. This
with probability at least 1—v, if r> (k/a) In(2k/y) + prevents us from determining the parameter N (max-
imal number of milestones) for KDP a priori. This
(2/9) (2/7), where k = (1/() In(2/9).
204 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

is not different from previous analyses of PRM path 4.4 Choice of ¢ and bmax
planners [11, 15, 17, 29].
In theory, the parameter £ must be chosen such that
for any p € R(mpy), Re(p) has the same dimension as
4.3. Approximating IDEAL-SAMPLE R(mp). Otherwise, Re(p) has zero volume relative to
R(mp), and R(mp) cannot be expansive even for ar-
One way of implementing IDEAL-SAMPLE would be bitrarily small values of a and 3. This can only hap-
to use rejection sampling [13]: Generate many samples pen when some dimensions of R(m»,) are not directly
and throw away a fraction of them in the more densely spanned by constant controls in 2. But these dimen-
sampled regions. However, this would lead KDP to sions can then be generated by combining several con-
generate and then discard many potential milestones. trols in 2 using Lie-brackets [4]. The mathematical de-
finition of a Lie bracket can be interpreted as an infin-
KDP seeks to approximate IDEAL-SAMPLE in a itesimal “maneuver” involving two controls. Spanning
more efficient way. Note that every new milestone m’ all the dimensions of R(mp,) may require combining
created in line 5 of Algorithm 1 tends to be relatively more than two controls of 2, by imbricating multiple
close to m, because long trajectories induced by con- Lie brackets. At most n — 2 Lie brackets are needed,
trols picked at random are often in collision. Therefore, where n is the dimension of S. Hence, it is sufficient in
if we selected milestones uniformly in line 3, the result- all cases to choose f= n — 2.
ing distribution would be very uneven; indeed, with
high probability, at line 3, the planner would pick a To simplify the implementation, however, one may
milestone in an already densely sampled region, which choose £ = 1, since a path passing through several con-
would yield a new milestone in that same region in secutive milestones in T corresponds to applying a se-
line 5. The distribution 7;(m) ~ 1/w(m) used at line 3 quence of constant. controls. In general, the larger @,
contributes to the diffusion of milestones over R(mp) the greater a and (, hence the smaller the number of
and avoids oversampling. In general, maintaining the milestones needed according to our analysis, but also
weights w(m) as the roadmap is being built has a much the more costly the generation of each milestone. The
smaller computational cost than performing rejection choice of dmax is somewhat related. A larger dmax re-
sampling. sults in greater a and @, but also leads the planner to
integrate longer trajectories that are more likely to be
There is a slightly greater chance of generating a non-admissible. Experiments show that @ and dmax can
new milestone in an area where the ¢-reachability sets be selected in rather wide intervals without significant
of several milestones already in T overlap. However, impact on the performance of the planner. However, if
milestones in T’ with overlapping ¢-reachability sets are the values for £ dmax are too large, it may be difficult
more likely to be close to one another than milestones to approximate IDEAL-SAMPLE well.
with no such overlapping. Therefore, the use of 1; at
line 3 keeps the problem under control by preventing
it from worsening as the number of milestones grows. 5 Nonholonomic Robots

There is yet another issue to consider. Though line 4 5.1 Robot Description
selects u uniformly at random from Uy, the distribution
of m’ in Re(m) is not uniform in general, because the We implemented KDP for two different robot systems.
mapping from Uz to Re(m) may not be linear. In many One consists of two nonholonomic carts connected by
cases, one may precompute a distribution 7, such that a telescopic link and moving among stationary obsta-
picking wu from Uy with probability 7, (u) yields a uni- cles. The other system is an air-cushioned robot that
form distribution of m’ in R¢(m). In other cases, rejec- is actuated by air thrusters and operates among mov-
tion sampling can be used locally as follows: In line 4, ing obstacles on a flat table. It is subject to strict
pick several control functions u;; in line 5, compute the dynamic constraints. In this section, we discuss the
corresponding m}, throw away some of them to achieve implementation of KDP for the nonholonomic robot.
a uniform distribution among the remaining m{, and In the next two sections, we will do the same for the
pick a remaining mj at random. air-cushioned robot.
Randomized Kinodynamic Motion Planning with Moving Obstacles
255

Figure 3: Two-cart nonholonomic robots.

Wheeled vehicles are a classical example for nonholo- orientation of F;’s velocity relative to the rear wheels).
nomic motion planning. The robot considered here The equations of motion for the system are:
is a new variation on this theme. It consists of two
independently-actuated carts moving on a flat surface Ly = U,COS 0; Xo = U2COS Op
(Figure 3). Each cart obeys a nonholonomic constraint Yi = uj,sing; Yo = uesinds
and has non-zero minimum turning radius. In addition, 0; a (u1/L;) tan oy A => (u2/L2) tan o2

the two carts are connected by a telescopic link whose

length is lower and upper bounded. This system has The control space is restricted by |u;| < U; and |d| <
been inspired by two scenarios. One is the mobile ma- ®;, which bound the carts’ velocities and steering an-
nipulation project in the GRASP Laboratory at the gles.
University of Pennsylvania [6]; the two carts are each
mounted with a manipulator arm and must remain 5.2 Implementation Details
within a certain distance range so that the two arms
can cooperatively manipulate an object (Figure 3(a)). Since all obstacles are stationary, the planner builds a
The manipulation area between the two carts must be roadmap T’ in the robot’s 6-D state space.
clear of obstacles. In the other scenario, two carts pa-
Computing the weights To compute the weight
trolling an indoor environment must remain in a direct w(m) of a milestone m, we define the neighborhood
line of sight of each other (Figure 3(b)), within some of m to be a ball of radius p centered at m. Our imple-
distance range, in order to allow visual contact or sim- mentation uses a naive method that checks every new
ple directional wireless communication. milestone m’ against all the milestones currently in T.
We project the geometry of the carts and the ob- Thus, for every new milestone, updating w takes linear
stacles onto the horizontal plane. For i = 1,2, time in the size of T. More efficient range search tech-
let R; be the midpoint between the rear wheels of niques [1] would improve the planner’s running time
the ith cart and F; be the midpoint between the for problems requiring very large roadmaps.
front wheels. Let L; be the distance between R; Implementing PROPAGATE Given a milestone m
and F;. We define the state of the system by s = and a control function u, PROPAGATE (m, uw) uses the
(11, Y1, 91, 22, Yo, 02), where (x;,4;) are the coordinates Euler method with a fixed step size to integrate the
of R;, and 6; is the orientation of the rear wheels trajectory 7 of the robot. It then discretizes 7 into
of cart i relative to the x-axis (Figure 1). The dis- a sequence of states and returns nil if any of these
tance constraint between the two carts is expressed as states is in collision. For each cart, a 3-D bitmap that
din < \/(@1 — £2)? + (yr — y2)? S dmax- represents the collision-free configurations of the cart
Each cart has two scalar controls: the magnitude is precomputed prior to planning. PROPAGATE then
u; of the velocity of R; and the steering angle ¢; (the takes constant time to check whether a configuration
256 D. Hsu, R. Kindel, J.C. Latombe, and 8. Rock

time (sec
is in collision, or not. In our experiments, we used a
128 x 128 x 64 bitmap. |
std_|
Endgame region We obtain the endgame region by
generating a secondary tree JT” of milestones rooted
at Sg.
roa14.09 3 126,100 |4473
7.42 | 287,800 | 9123 et
5.3. Experimental Results 0.92 | 0.51 56,400 | 1894 20250

We experimented with the planner in many

Table 1: Planning statistics for the nonholonomic robot.
workspaces. Each is a 10 mx10 m square region
with static obstacles. The two carts are identical,
we ran the planner 30 times independently with dif
each represented by a polygon contained in a circle of
ferent random seeds. The results are collected in Ta-
radius 0.4 m. Ly = Lg = 0.5 m. Each cart’s speed
ble 1. Every row of the table corresponds to a par-
ranges from —3 m/s to 3 m/s, and its steering angle
ticular query. Columns 3-7 list the average running
@ varies between —30° and 30° The distance between
time, its standard deviation, the average number of col-
R; and Rp ranges between 1.4 m and 3.3 m.
lision checks, the average number of milestones gener-
Figure 4 shows three computed examples. ated, and the average number of calls to PROPAGATE.
Workspace (a) is a maze; the robot must navi- The running times range from less than a second to a
gate from one side of it to the other. Workspace (b) few seconds. The first query in environment (c) takes
contains two large obstacles separated by a narrow longer because the carts must perform several maneu-
passage. The two carts, which are initially parallel to vers in the hallway before reaching the goal (see Fig-
one another, change formation and proceed in a single ure 4(c)). The planner was written in C++, and the
file through the passage, before becoming parallel running times were collected on a 195 Mhz SGI Indigo2
again. Workspace (c) consists of two rooms cluttered with an R10000 processor.
with obstacles and connected by a hallway. The carts
The standard deviations in Table 1 are larger than
need to move from the bottom one to the the top
we would like. Figure 5 plots a histogram of more than
one. The maximum steering angles and the size of the
100 independent runs for a given query. In most runs,
circular obstacles conspire to increase the number of
planning time is well under the mean or slightly above.
required maneuvers.
This indicates that our planner performs well most of
We have run the planner for several different queries the time. The large deviation is caused by a few runs
in each workspace shown in Figure 4. For every query, that take as long as four times the mean. The long and
Randomized Kinodynamic Motion Planning with Moving Obstacles
257
35

2 3
running time (seconds)

Figure 5: Histogram of planning times for more than 100

runs on a particular query. The average time is 1.4 sec, Figure 6: The air-cushioned robot.
and the four quartiles are 0.6, 1.1, 1.9, and 4.9 sec.
where m is for the robot’s mass, and u and 6 denote
the magnitude and direction of the force generated by
thin tail of the distribution is typical of the tests that the thrusters. We have u/m < 0.025 m/s”, and @ varies
we have performed. freely between 0° and 360°. We also bound the robot’s
velocity by 0.18 m/s.
6 Air-Cushioned Robots
6.2 Implementation Details
6.1 Robot Description
The planner builds a roadmap T in the robot’s 5-D
The second robot system used to evaluate our algo- statextime space, The initial statex time is of the form
rithm was developed in the Stanford Aerospace Ro- (sp,0) and the goal is of the form (sg,t,) where t, can
botics Laboratory for testing space robotic technolo- be any time less than a given tmax. The planner is given
gies. This robot (Figure 6) moves frictionlessly on the obstacle trajectories, and unlike in the experiments
an air bearing on a flat granite table. Air thrusters with the real robot in the next section, planning time
provides omni-directional motion capability, but the is not limited. This is equivalent to assuming that the
thrust available is small compared to the robot’s mass, world is frozen until the planner returns a trajectory.
resulting in tight acceleration limits. We represent the Computing the weights The 3-D configuration-
workspace by a 3 mx4 m rectangle, the robot by a xtime space of the robot is partitioned into an
disc of radius 0.25 m, and the obstacles by discs of 8x11x10 array of identically sized rectangles called
radii between 0.1 and 0.15 cm. Each obstacle moves bins. When a milestone is inserted in 7’, the planner
along a straight path at constant velocity ranging be- adds it to a list of milestones associated with the bin
tween 0 and 0.2 m/s (more complex trajectories will in which it falls. In line 3 of KDP, the planner picks
be considered in Subsection 7.4). In the simulation en- at random a bin containing at least one milestone and
vironment, collisions among obstacles are ignored. So then a milestone from within this bin. Both choices
two obstacles may temporarily overlap without chang- are made uniformly at random. This corresponds to
ing courses. When an obstacle reaches the workspace’s picking a milestone with a probability approximately
boundary, it leaves the workspace and is no longer con- proportional to the inverse of the density of samples
sidered a threat to the robot. in the robot’s configurationxtime space (rather than
We define the robot’s state to be (x,y,z, y), where its statextime space). We did some experiments with
(x,y) are the coordinate of the robot’s center. The higher-dimensional bin arrays, but the results were not
equations of motion are: improved significantly.
Implementing PROPAGATE The simple equations
tial ve.
c= ee cos@ and y= —usin9, of motion make is possible to compute trajectories an-
m m
258 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

T = 22.4 secs T =33.7 secs T = 44.9 secs

T =0.0 secs T=11.2 secs

T =9.0 secs T = 20.0 secs T = 30.0 secs T = 39.2 secs

T = 8.0 secs T = 16.1 secs T = 24.1 secs T =32.1 secs

(c)
Figure 7: Trajectories produced by the planner for the air-cushioned robot.

alytically. Trajectories are discretized and at each dis- 6.3 Experimental Results
cretized statextime the robot disc is checked for col-
lision with each obstacle disc. This naive technique We performed numerous experiments in more than one
works reasonably well when the number of obstacles is hundred simulated environments. In each case, plan-
small. It could easily be improved. ning time was limited to five minutes. For a small
number of queries, the planner failed to return a tra-
jectory, but for none of them were we able to show that
Endgame region When a milestone m is added to T,
an admissible trajectory exists. On the other hand, the
it is checked for a connection to k goal points (sg, t,).
planner successfully solved several queries for which we
Each of the k values of t, is picked uniformly at random
initially thought there were no solution.
in the interval [tmin, tmax], Where tmin is an estimate of
the earliest time when the robot may reach s, given Three trajectories computed by the planner are
its maximal velocity and assuming no obstacles. For shown in Figure 7. For every example, we display
each value of t,, the planner computes the third-order five snapshots labeled by time. The large disc is the
spline connecting m to (s,,t,). It then verifies that robot; the smaller discs are the obstacles. The solid
the spline is collision free and satisfies the velocity and and dotted lines mark the trajectories of the robot and
acceleration bounds. If all the tests succeed, then m lies the obstacles, respectively. For each query, we ran the
in the endgame region. In all the experiments below, planner 100 times independently with different random
k is set to 10. seeds. The planner successfully returned a trajectory
Randomized Kinodynamic Motion Planning with Moving Obstacles
259

[time (Sec) [milestones 7 Experiments with the Real Robot

[mean |_std_[ mean |std
a) || 0.249 | 0.264 | 2008 | 2229 We connected the planner of the previous section to
(b) |} 0.270 | 0.285 | 1946 | 2134
(c) |} 0.002 | 0.005 as the air-cushioned robot of Figure 6 in order to ver-
ify that KDP remains useful in a system integrating
Table 2: Planning statistics for the air-cushioned robot.
control and sensing modules over a distributed archi-
tecture and operating in a physical environment with
in all runs. In Example (a), the duration of the trajec- uncertainties, time delays, and real-time constraints.
tory varied from 40 to 70 sec. Table 2 lists the means
7.1 Testbed Description
and standard deviations of the planning time and num-
ber of milestones for each example. The reported times The robot in Figure 6 moves frictionlessly on an air
are based on a planner written in C and running on a bearing within the limits of a 3 mx4 m table. Obsta-
Pentium-III PC with a 550 Mhz processor and 128M cles, also on air-bearings, translate without friction on
of memory. the table. The positions of the robot and the obstacles
In the first two examples, the moving obstacles cre- are measured at 60 Hz by an overhead vision system
ate narrow passages through which the robot must thanks to LEDs placed on each moving object. The
measurement is accurate to 5 mm. Velocity estimates
pass to reach the goal. Yet planning time remains
are derived from position data.
much under 1 second. The fact that the planner
never failed in 100 runs indicates its reliability. The The robot is untethered. Gas tanks provide com-
configurationxtime space for Example (b) is shown pressed air for both the air-bearing and thrusters. An
in Figure 8. The robot maps to a point (z,y,t), and onboard Motorola ppc2604 computer performs motion
the obstacles to cylinders. The velocity and accelera- control at 60 Hz. The planner runs on on a 333 Mhz
tion constraints force any solution trajectory to pass Sun Sparc 10. The robot communicates with the plan-
through a small gap between cylinders. Example (c) is ner and the vision module over the radio Ethernet.
much simpler. There are two stationary obstacles ob- The obstacles have no thrusters. They are initially
structing the middle of the workspace and three mov- propelled by hand from various locations, and then
ing obstacles. Planning time is well below 0.01 second, move at constant speed until they reach the bound-
with an average of 0.002 second. The number of mile- ary of the table, where they stop due to the lack of air
stones is also small, confirming the result of Theorem 5 bearing.
that in the absence of narrow passages, KDP is very
efficient. As in the experiments on nonholonomic ro- 7.2 System Integration
bot carts, the running time distribution of the planner
Implementing the planner on the hardware testbed
tends to have a long and thin tail.
raises a number of new challenges:
Delays Various computations and data exchanges pro-
duce delays between the instant when the vision mod-
ule measures the trajectories of the robot and the ob-
stacles and the instant when the robot starts executing
the planned trajectory. Ignoring these delays would
lead the robot to begin executing the planned trajec-
tory behind the start time assumed by the planner. It
then may not be able to catch up with the planned tra-
jectory before collision occurs. To deal with this issue,
the planner computes a trajectory assuming that the
robot will start executing it 0.4 second into the future.
It extrapolates the positions of the obstacles accord-
ingly, as well as that of the robot if its initial velocity
Figure 8: Configurationx space for Example (b).
is non-zero. The 0.4 second contains all the delays in
260 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

the system (not just the time needed for planning). It of the robot at all times. A PD-controller with feedfor-
could be further reduced by running the planner on a ward is used to track this trajectory. Maximum track-
machine faster than the Sun Sparc 10 that we are using ing errors are 0.05 m and 0.02 m/s. The size of the
currently. disc modeling the robot is increased by 0.05 m to en-
sure safe collision checking by the planner.
Path optimization Because robot starts executing
the trajectory 0.4 second after planning begins, the
planner exploits any extra time after obtaining a first 7.3. Experimental Results
trajectory to generate additional milestones and keeps
The planner successfully produced complex maneuvers
track of the best trajectory generated. The cost func-
of the robot among static and moving obstacles in var-
tion used to compare trajectories is eer + b)6i,
ious situations, including obstacles moving directly to-
where k is the number of segments in the trajectory, u;
ward the robot, as well as perpendicular to the line
is the magnitude of the force exerted by the thrusters
connecting its initial and goal positions. The tests also
along the ith segment, 6; is the duration of the 7th
demonstrated the ability of the system to wait for an
segment, and b is a constant. This cost combines fuel
opening to occur when confronted with moving obsta-
consumption with execution time. A larger b yields a
cles in the robot’s desired direction of movement and to
faster motion, while a smaller 6 yields less fuel con-
pass through openings that are less than 10 cm larger
sumption. In our experiments, the cost of trajectories
than the robot. In almost every trial, a trajectory was
was reduced, on average, by 14% with this simple im-
computed within the allocated time. Figure 9 shows
provement.
snapshots of the robot during one test.
Sensing errors The obstacle trajectories are assumed
Several constraints limited the complexity of the
to be straight lines at constant velocities. However,
planning problems which we could test. Two are re-
inaccuracy in the measurements by the vision module
lated to the testbed itself: The size of the table relative
and assymmetry in air-bearings cause actual trajecto-
to the robot and the obstacles, and the robot’s small
ries to be slightly different. The planner deals with
acceleration. The other two constraints result from the
these errors by increasing the radius of each moving
design of our system: The requirement that obstacles
obstacle by an amount €Vmaxt, where t denotes time,
move along straight lines and hence do not collide with
Vinax is the measured velocity of the obstacle, and € is
each other, and the relatively high uncertainty on their
a constant.
movements, which forces the planner to grow the obsta-
Safe-mode planning If the planner does not find a cles and thus reduce free space. To eliminate the last
trajectory to the goal within the allocated time, we two constraints, we introduced on-the-fly replanning.
find it useful to compute an escape trajectory. The
endgame region Es. for the escape trajectory consists 7.4 On-the-Fly Replanning
of all the reachable, collision-free states (s.,t.) with
te > Tesc for some Tes-. An escape trajectory corre- Whenever an obstacle leaves the disc in which the plan-
sponds to any acceleration-bounded, collision-free mo- ner believes it lies (because either the error on the pre-
tion in the workspace for a small duration of time. In dicted motion is larger than expected, or the obstacle’s
general, EH... is very large, and so generating an escape direction of motion has changed), the vision module
trajectory often takes little time. To ensure collision- alerts the planner. The planner recomputes a trajec-
free motion beyond T.,., a new escape trajectory must tory on the fly as if it were a normal planning operation
also be computed long before the end of the current within the same time limit, by projecting the state of
escape trajectory so that the robot can escape collision the world 0.4 second into the future. On-the-fly replan-
despite the acceleration constraints. We modified the ning makes it possible to perform much more complex
planner to compute concurrently both a normal and an experiments in the testbed. We show two examples
escape trajectory. In our experiments, the modification below.
slowed down the planner by about 2%.
In the example in Figure 10, eight replanning oper-
Trajectory tracking The trajectory received by the ations are performed over the 75 seconds taken by the
robot specifies the position, velocity, and acceleration robot’s motion. Initially, the robot moves to the left
Randomized Kinodynamic Motion Planning with Movin
g Obstacles 261

T =2.1 secs T = 14.6 secs T = 19.8 secs

15 e AD:

1 1

g 05 @ os e@
Eo 2 0
#05 e 405
1} @ =|
-1.5 -1.5f,
= 0 1 -1 0 1 -1 0 1
x, [meters] x, [meters] x, [meters]

T = 33,2 secs T = 50.2 secs T =75.0 secs

ou
[meters]
X, 1
oS a

5
—1.5
0 1 0 1 10} 1
x, [meters] x, (meters] x, [meters]

Figure 10: Simulation example with replanning.

to reach the goal at the bottom middle. At 14 sec-

onds, the upper-left obstacle changes course, forcing
a replan (snapshot 2). Soon after, the motion of the
upper-right obstacle changes, forcing the robot to re-
verse direction and approach the goal from the other
side of the workspace (snapshot 3). In the remaining
time, new changes in obstacle motion cause the robot
to pause (tight angle in snapshot 5) before a straight
trajectory to the goal is possible (snapshot 6).
The efficacy of the replanning procedure in the test-
bed is demonstrated by the example in Figure 11. In
snapshot 1, the middle obstacle is stationary, while the
two outer obstacles are moving towards the robot. The
robot attempts to move from the back middle to the
front middle of the workspace. In snapshot 2, the ro-
bot dodges the faster-moving obstacle from the left, in
order to let it pass by and then proceeds toward the
goal. In snapshot 3, that obstacle is redirected to block
the trajectory of the robot, causing it to slow down and
stay behind the obstacle to avoid collision. Snapshot
5 shows the leftmost obstacle being redirected again,
this time towards the robot’s goal. The robot follows
this obstacle and move slowly towards the goal. Before
snapshot 7, however, the rightmost obstacle is directed
back towards the robot, forcing the robot to wait and
let it pass (snapshot 8). Finally, in the last snapshot,
the robot attains the goal. The entire motion lasted
40 seconds. Throughout this example, other replan-
Figure 9: Snapshots of the robot executing a trajectory. ning operations were performed due to errors in the
262 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock

Figure 11: An ezample with the real robot using replanning.

measurement of the obstacle trajectories (We set € such The experiments in the hardware robot testbed demon-
that sensing errors caused replanning to occur, on av- strate that the planner remains effective despite vari-
erage, every 5 seconds). However, none resulted in a ous delays and uncertainties inherent to an integrated
major redirection of the robot. system interacting with the physical world. They also
show that the planner can be used in real-time when
8 Conclusion obstacle trajectories are not known in advance.

We have presented a simple, efficient randomized plan- In the future, we plan to apply the planner to en-
ner for kinodynamic motion planning problems in the vironments with more complex geometry. Geometrical
presence of moving obstacles. Under the expansiveness complexity essentially increases the cost of collision-
assumption, we have formally proven that the plan- checking, but hierarchical techniques deal with this is-
ner is probabilistically complete and converges quickly sue well. In [11], a similar, but simpler planner was
when a solution exists. This proof also applies to ro- successfully applied to compute geometric disassembly
bots that are not locally controllable. The planner paths with CAD models having up to 200,000 trian-
was tested successfully with simulated and real robots. gles. Another issue that we would like to investigate is
Randomized Kinodynamic Motion Planning with Moving
Obstacles 263
the reduction of the standard deviation of the planning
time. We suspect that the long thin tail shown in Fig-
[10] K. Fujimura. Time-minimum routes in time-
dependent networks. IEEE Tr. on Robotics and Au-
ure 5 is typical of all PRM planners developed so far. tom., 11(3):343-351, 1995.
But it seems more critical to reduce it for single-query
planners, since such planners are more likely to be used
[11] D. Hsu, J. C. Latombe, and R. Motwani.
Path planning in expansive configuration spaces.
interactively or in real-time than multi-query ones. Int. J. Comp. Geometry and Applications, 9(4-5):495—
512, 1999.
[12] P. Jacobs and J. Canny. Planning smooth paths for
Acknowledgments mobile robots. In Proc. IEEE Int. Conf. on Robotics
and Autom., p. 2-7, 1989.
This work was supported by ARO MURI grant [13] M.H. Kalos and P.A. Whitlock. Monte Carlo Methods,
DAAH04-96-1-007, NASA TRIWG Coop-Agreement Vol. 1, John Wiley & Son, 1986.
NCC2-333, Real-Time Innovations, and the NIST ATP [14] K. Kant and 8S. W. Zucker. Toward efficient trajectory
program. David Hsu has also been the recipient of a planning: The path-velocity decomposition. Int. J. of
Microsoft Graduate Fellowship, and Robert Kindel, the Robotics Research, 5(3):72-89, 1986.
recipient of an NSF Graduate Fellowship. [15] L.E. Kavraki, M. Kolountzakis, and J. C. Latombe.
Analysis of probabilistic roadmaps for path planning.
IEEE Tr. Robotics and Autom., 14(1):166-171, 1998.
References
[16] L.E. Kavraki and J. C. Latombe. Randomized pre-
processing of configuration space for fast path plan-
[1] P. K. Agarwal. Range searching. In Handbook of dis-
ning. Proc. IEEE Int. Conf. on Robotics and Autom.,
crete and computational geometry, J.E. Goodman and
p. 2138-2145, 1994.
J. O’Rourke (eds.), CRC Press, Chap. 31, p. 575-598,
1997.
[17] L.E. Kavraki, J. C. Latombe, R. Motwani, and
[2] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and P. Raghavan. Randomized query processing in robot
D. Vallejo. OBPRM: An obstacle-based PRM for 3D motion planning. J. Computer and System Sciences,
workspaces. In Robotics: The Algorithmic Perspective, 57(1):50-60, 1998.
P.K. Agarwal, L.E. Kavraki, and M.T Mason (eds.),
A K Peters, p. 155-168, 1998. [18] L.E. Kavraki, P. Svestka, J. C. Latombe, and M. Over-
mars. Probabilistic roadmaps for path planning in
[3(atest
J. Barraquand and J. C. Latombe. Robot motion plan- high-dimensional configuration spaces. [EEE Tr. Ro-
ning: a distributed representation approach. Int. J. of botics and Autom., 12(4):566—580, 1996.
Robotics Research, MIT Press, 10(6):628-649, 1991.
[19] J.P. Laumond. Feasible trajectories for mobile ro-
[4] J. Barraquand and J. C. Latombe. Nonholonomic bots with kinematic and environmental constraints.
multibody mobile robots: controllability and motion Proc. Int. Conf. on Intelligent Autonomous Systems,
planning in the presence of obstacles. Algorithmica, p. 346-354, 1986.
10(2-4):121-155, 1993. [20] J.P. Laumond, P.E. Jacobs, M. Taix., and R.M. Mur-
[5] J. E. Bobrow, S. Dubowsky, and J. Gibson. Time- ray. A motion planner for nonholonomic mobile ro-
optimal control of robotic manipulators along specified bots. [EEE Tr. on Robotics and Autom., 10(5):577—
paths. Int. J. of Robotics Research, 4(3):3-17, 1985. 593, 1994.

[6] J.P. Desai and V. Kumar. Motion planning for coop- [21] S. LaValle and J. Kuffner. Randomized kinodynamic
erating mobile manipulators. J. of Robotic Systems, planning. Proc. IEEE Int. Conf. on Robotics and Au-
16(10):557—-579, 1999. tom., p. 473-479, 1999.

[7—a B. Donald, P. Xavier, J. Canny, and J. Reif. Kinody- [22] Z. Li, J. F. Canny, and G. Heinzinger. Robot motion
namic motion planning. J. of the ACM, 40(5):1048- planning with nonholonomic constraints. In Robotics
1066, 1993. Research: The 5th Int. Symp., H. Miura et al. (eds.),
MIT Press, p. 309-316, 1989.
[8] P. Fiorini and Z. Shiller. Time optimal trajec-
tory planning in dynamic environments. Proc. IEEE [23] K. M. Lynch and M. T. Mason. Stable pushing: me-
on Robotics and Autom., p. 1553-1558, chanics, controllability, and planning. Int. J. of Ro-
Int. Conf.
1996. botics Research, 15(6):533-556, 1996.

[9] T. Fraichard. Trajectory planning in a dynamic [24] J.A. Reeds and L.A. Shepp. Optimal paths for a car
Advanced that goes forwards and backwards. Pacific J. of Math-
workspace: a ‘state-time space’ approach.
Robotics, 13(1):75-94, 1999. ematics, 145(2):367-393, 1990.
264 D. Hsu, R. Kindel, J.C. Latombe, and 8. Rock

[25] J. Reif and M. Sharir. Motion planning in the presence presence of obstacles. [IEEE Tr. on Robotics and Au-
of moving obstacles. Proc. IEEE Symp. on Founda- tom., 7(6):785-797, 1991.
tions of Computer Science, p. 144-154, 1985.
[28] P. Svestka and M. H. Overmars. Motion planning for
[26] S. Sekhavat, P. Svestka, J.-P. Laumond, and M. Over- car-like robots using a probabilistic learning approach.
mars. Multi-level path planning for nonholonomic ro- Tech. Rep. RUU-CS-94-33, Dept. of Computer Sci-
bots using semi-holonomic subsystems. Algorithms for ence, Utrecht Univ., The Netherlands, 1994.
Robotic Motion and Manipulation, J.P. Laumond and
M. Overmars (eds.), A.K. Peters, p. 79-96, 1997. [29] P. Svestka and M. Overmars. Probabilistic path plan-
ning: robot motion planning and control. Lec-
[27] Z. Shiller and S$. Dubowsky. On computing the global ture Notes in Control and Information Sciences, 229,
time-optimal motions of robotic manipulators in the Springer, p. 255-304, 1998.
On Random Sampling in Contact Configuration Space
Xuerong Ji, University of North Carolina, Charlotte, NC
Jing Xiao, University of North Carolina, Charlotte, NC

Random sampling strategies play critical roles in ran- indicated by a contact formation (CF) [17] and a rep-
domized motion planners, which are promising and resentative configuration of the CF. Each edge denotes
practical for motion planning problems with many de- the neighboring relationship between the two nodes
grees of freedom (dof). In this paper, we address ran- connected by the edge. With this approach, the prob-
dom sampling in a constrained configuration space — the lem of contact motion planning is effectively simpli-
contact configuration space between two polyhedra, mo- fied as graph search at high-level for state transitions
tivated by the need for generating contact motion plans. and motion planning at low-level within the set of con-
Given a contact formation (CF) between two polyhe- tact configurations constrained by the same contact
dra A and B, our approach is to randomly generate state!. However, even for such reduced-dimension and
configurations of A satisfying the contact constraints reduced-scope motion planning, the dimensionality or
of the CF. Key to the approach is to guarantee that dof can still be quite high for less-constrained contact
sampling happens only in the constrained space to be states, such as those consisting of only a single prin-
efficient, which has not been addressed in the litera- cipal contact (PC) [17] (see Figure 1 for PCs between
ture. We first describe our random sampling strategy two polyhedra). Thus, randomized motion planning is
for configurations constrained by CFs consisting of one desirable for planning contact motions, which requires
or two principal contacts (PCs) and then present im- random sampling of contact configurations.
plementation results. There are promising randomized planners for
collision-free motions, such as those based on proba-
1 Introduction bilistic roadmaps (PRM) [12, 15]. In the PRM model
random sampling consists of generating arbitrary con-
Contact motions are important in automatic assem- figurations of the considered object/robot. The sam-
bly processes, not only because they happen frequently pled configurations may or may not be collision-free.
when clearance between objects is tight, but also be- To make sampling more efficient, several researchers
cause they reduce degrees of freedom (dof), thus re- introduced different methods targeting at producing
duce uncertainties [14, 16]. Contact motion occurs more samples in certain critical areas that tend to
on the boundary of configuration space obstacles (C- be close to C-obstacles and sparse samples in other
obstacles) [13], but computing C-obstacles remains a areas[1, 3, 8]. However, sampling ezactly on the C-
formidable task to date. While there were exact de- obstacle surface, or in the contact configuration space,
has not been addressed in the literature.
scriptions of C-obstacles for polygons [2, 4], there were
only approximations for polyhedra [5, 11]. Contact In this paper, we extends the random sampling strat-
motions, however, require exactness of contact config- egy for a single PC reported in [10] to contact forma-
urations. Hence, some researchers started exploring tions (CFs) of two PCs. Our approach takes advantage
methodologies on contact motion planning without ex- of our work on automatic generation of contact state
plicitly computing C-obstacles (see, for example, [7]). graphs by building sampling on the knowledge of a con-
tact formation and a representative contact configura-
Recently the authors introduced a general divide-
and-merge approach for automatically generating a 1Note that a general contact motion crossing several con-
contact state graph between arbitrary polyhedra (9, tact states consists of segments of motion in each contact
20]. Each node in the graph denotes a contact state, state.
266 X. Ji and J. Xiao

DE? BRP
tion under the contact formation (obtainable from such
agraph). Particularly, given a CF and a seed configura-
tion satisfying the CF, the goal is to randomly sample
configurations satisfying the CF. f-e/e-f f-v/v-f

<p 2p Za
The paper is outlined as follows. in Section 2, we
review the notion of CFs and analyze the dof for each
kind of single-PC or two-PC CF between two arbitrary
polyhedra. In Section 3, we present the random sam- Ng Boa e-v/v-e
pling algorithms. In Section 4, we provide some ex- Neeser
perimental results of the sampling strategy. Section 5
concludes the paper. Figure 1: Principal Contacts (PCs)

2 Contact Formations and Degrees of The two contacting elements of a non-degenerate PC

uniquely determine a plane, which we call a contact
Freedom (dof)
plane (CP). Based on the region of contact on the con-
tact plane, we can further classify the PCs into the
In this section, we first introduce notations related to
following three types:
contact formations used throughout the paper and then
analyze the dof for different types of contact forma- e plane PC: f-f, where the contacting elements in-
tions. Knowledge of the dof is needed for our random tersect at a planar region on the contact plane,
sampling strategy. e line PC: e-f and f-e, where the contacting elements
intersect at a line, called a contact line.
2.1 Notations
e point PC: y-f, f-v, and e-e-c, where the contacting
elements intersect at a point, called a contact point.
Consider two arbitrary polyhedra A and B. Assume
that A is moveable and B is static. The faces, edges, A contact configuration is defined as the configura-
and vertices of each object are the object’s topological tion of A relative to that of B when A and B are in
elements. The boundary elements of a face are its edges contact. Thus, the geometric interpretation of a PC
and vertices, and the boundary elements of an edge are is the region of contact configurations (on the contact
its vertices.
plane) where the PC holds, and that of a CF is the
As defined in [17], a principal contact (PC) between intersection of the regions of contact configurations of
A and B describes a single contact between a pair of the participating PCs.
contacting elements which are not the boundary ele-
ments of other contacting elements. There are 10 types 2.2 Degrees of Freedom
of PCs (Figure 1): face-face (f-f), face-edge(f-e) /edge-
face(e-f), face-vertex (f-v)/vertex-face(v-f), edge-edge- The dof of a PC is expressed by the constraints it im-
cross (e-e-c), edge-edge-touch (e-e-t), edge-vertex (e- poses on contact configurations. To represent such con-
v)/vertex-edge (v-e), and vertex-vertex (v-v), of which tact constraints, for an arbitrary polyhedron P, we at-
e-e-t, e-v/v-e and v-v PCs between convex elements tach coordinate system (or frame) to it. Moreover, for
are degenerate PCs. We denote a PC as u4-ugp, where each element (vertex, edge or face) of the object P, we
ua and ug denote the contacting elements of A and
attach a coordinate system as follows:
B respectively. Given this notion of PCs, an arbitrary e vertex: the coordinate system v has its origin at
contact between A and B can be characterized by the the vertex, and the orientation is the same as that
set of PCs formed, called a contact formation (CF), de- of P.
noted by {PC},...,PC,,}. Since degenerate PCs rarely e edge: the coordinate system e has its origin at one
happen in practice, in this paper, we only consider of its bounding vertices, the direction of +X is
CFs formed by non-degenerate PCs, referred to sim- along the edge pointing to the other bounding ver-
ply as PCs. tex, the direction of +Z is along the outward normal
On Random Sampling in Contact Configuration Space
267

°. Br,
ff
ae Bs:Tye leaps a Y; 0) <l pote(™) ‘Trotz
(a) ATs

where “Ta, “Ta and “Tyra (or 20,8, 80.5 and PT 2)

represent the transformation matrices from the frame
of a vertex, edge and face of A (or B) to the frame of
A (or B) respectively. Trrans(*, *, *) is the 4x4 trans-
lational matrix. Trotz(*), Troty(*), and Trota(*) are
the 4x4 rotational matrix about z, y and x axis re-
spectively, and Tyotzya(*, *, x) is the combination of the
three rotational matrices.

Figure 2: Coordinate systems for an object P and for its

The dof for each type of PC equals to its number of
verter v, edge e and face f
independent variables. Using “t” to indicate transla-
tional dof and “r” to indicate rotational dof, we sum-
marize the dof for single-PC CFs below:
of the edge, defined as the sum of the outward nor-
mals of the faces forming the edge, and the direction e plane PC: dof =3, (2t and Ir)
of +Y is determined by the right-hand rule. e line PC: dof = 4, (2t and 2r)
e face: the coordinate system f has its origin at one of e point PC: dof =5, (2t and 3r)
its bounding vertices, the direction of +Z is defined
as the outward normal of the face, the direction of Since a free-flying polyhedron has 6 dof, it is easy to
+X is along one of the bounding edges of the face, see that a point PC reduces the dof of the object by 1,
and the direction of +Y is determined by the right- a line PC by 2, and a plane PC by 3.
hand rule. The constraint equations for a two-PC CF are the
set of equations for the PCs involved. The dof of a
Figure 2 illustrates the coordinate systems of object
two-PC CF depends not only on the topological types
P and some of its elements.
of the PCs but also on the geometrical relation between
We can present the contact constraint equations for the PCs and their corresponding contact regions. We
each type of PC between A and B in terms of ex- summarize them below:
pressions for contact configurations PT’, (homogeneous CFs where the two contact planes are not par-
transformation matrix) (extending [19]), where for A, allel:
B and their elements, we attach coordinate systems by
the above definitions. In each expression, the under- e two plane PCs {f-f, f-f}:
lined symbols are independent variables, of which the dof = 1, (1t)
Greek symbols are rotational variables in Euler angles. e line PC and plane PC {e-f/f-e, f-f}’:
e v-f:
2, (1t and 1r)
Abs ate Ty é Ttrans €% yy 0) Tr otzyx (a, 2, a) acidta
if Cis 1 CL
; dof =
e f-u: 1, (1t)
eT a = TyB ¥ Trotzyx (a, 0, Y) ; Ttrans (z, Y 0) a Tra otherwise
@ €-€-C.
Without losing generality, we denote PC; to be the
eT, =6 TB > Birana( &450;0) TSO GesD) : plane PC with CP; being the contact plane, PC,
to be the line PC with CP» being the contact plane
and CL» being the contact line. We further denote
CL to be the intersection line of CP; and CP.

e -€. 2The notion “e-f/f-e” means either e-f or f-e. The same
Bia =P T.B a Es (y) VP eee (a) Dinas (gz;Y; 0) ie fA explanation applies to all “/” used in the paper.
268 X. Ji and J. Xiao

Clearly there is one translational dof along CL. In

addition, the only possible kind of rotation main-
taining (or compliant to) the plane PC PC, is along
a normal of CP), and the possible rotation (or com-
bined rotation and translation) maintaining PC» is
either along an axis parallel to C2, or along a nor- at CF 1
(Gi 1 CL
mal of C’P2, or along an axis on the plane pass- CL
ing CL and perpendicular to C'P2, denote it as
(a)
Por,. Since CP; and CP» are not parallel, only
when CL2 1 CL (the first case in the above for- Figure 3: CFs with one plane PC PC, and one line PC
mula), as shown in Figure 3(a), there exists a com- PC2 of non-parallel contact planes: (a) CL2 CL (2 dof),
mon axis to allow rotations or combined rotation (b) CL2> 1 CL (1 dof).
and translation (where the translation depends on
the rotation) to maintain both PCs, i.e., there is a enable CF-compliant motion can be found on
rotational dof (see Figure 3(b)). Otherwise, such as either Por, or Pert,. On Por,, for example, ri
shown in Figure 3(b), there is no rotational dof. and rz can be selected to satisfy r; | CL2 and
e point PC and plane PC {v-f/f-v/e-e-c, f-f}: r2 ||CL2. Thus, there are also 2 rotational dof.
dof = 2, (1t and Ir) Note that for cases (c)-(f), after any CF-compliant
e two line PCs {e-f/f-e, e-f/f-e}: rotation or combined rotation and translation, the
cases become the general case (a), that is, the con-
3, (1t and 2r) dition (CL, 1 CL and CLz2 1 CL) no longer holds.
Note also that in a CF-compliant combined rotation
dof =
2, (1t and Ir) and translation, the translation depends on the ro-
otherwise tation so that there is no additional translational
dof.
In the above, CL; and CL» are the contact lines
e point PC and line PC {v-f/f-v/e-e-c, e-f/f-e}:
of PC; and PC, respectively, CP, and CP» are
the contact planes of PC, and PC, and CL is the
dof = 3, (1t and 2r)
intersecting line of CP; and CP). e two point PCs {v-f/f-v/e-e-c, v-f/f-v/e-e-c}:
Clearly there is one translational dof along the CL. dof = 4, (1t and 3r)
Additionally, there can be one or two rotational dof
depending on the contact geometry. Denote the For CFs where the contact planes are parallel
plane passing CL, and perpendicular to CP, as (see Figure 5):
Pert, and the plane passing CL2 and perpendicular
to CP: as Pcz,. Figure 4 depicts cases of different 4, (2t and 2r)
relations between Poy, and Por,: if two point PCs, else
4, (2t and 2r)
- In (a) and (b), Por, and Por, intersect at line dof =
if contact point(s) /line(s) are collinear
r1, which is the only possible instantaneous ro-
3, (2t and 1r)
tation axis for CF-compliant motion (of com-
otherwise
bined rotation and translation), and thus there
is only one rotational dof.
- In (c) and (d), Pox, and Pox, are coplanar, 3 Random Sampling Strategy
thus any line on the plane can be an instanta-
neous rotation axis for CF-compliant combined As mentioned in Section 1, the authors introduced a
motion, and one can pick a pair of orthogonal general divide-and-merge approach [9, 20] to generate
axes rT; and ro; hence there are 2 rotational dof. contact state graphs automatically. This approach re-
- In (e) and (f), Por, and Pez, are parallel, a duces the contact motion planning problem to a graph
pair of orthogonal rotation axes r; and re that search problem at the high level and the problem of
On Random Sampling in Contact Configuration Space
269

Figure 5: CFs with parallel contact planes: (a) two point

PCs (4 dof), (b) two line PCs with collinear contact lines
(4 dof), (c) one point and one plane PCs (8 dof), (d) two
plane PCs (3 dof)

Figure 4: CFs with two line PCs of non-parallel contact

planes: (a) and (b), CLli,CL2 — 1 CL (2 dof), (c) and

Se
(d), CLi,CL2z | CL and CL; and CL are coplanar (3
dof), (e) and (f), CLIi,CL2 1 CL and CL; and CLz are
not coplanar (3 dof). The instantaneous rotational axes ri
(and r2) are shown. For clarity, only the elements of the (a) (b)
objects in contact are shown. The planes Por, and Por,
Figure 6: Two configurations of the same {e-f, e-f} CF:
are shown in dotted lines.
(a) valid with no local penetration, (b) invalid with local
penetration
planning of contact motions within the same contact
formation at the low level, which we call CF-compliant
motion planning. Our random sampling strategy aims local penetration between an adjacent face of the edge
at CF-compliant motion planning and takes advantage and the face of B in the bottom PC.
of the known information provided by the divide-and- We use two general methods to randomly generate a
merge approach: A CF and a (seed) contact configu- valid configuration:
ration satisfying the CF.
e Direct Calculation: this method first calculates
the valid range for the values of each independent
For each CF, our strategy generates contact config-
variable® and then randomly selects a value within
urations which satisfy the CF and are guaranteed no
the range for the variable. In this way, all sam-
local penetration, that is, no penetration between the
pled configurations are valid ones. For single-PC
two objects through elements ua and up of each PC or
CFs and some two-PC CFs (see Section 3.1 and 3.2
through an element directly connected to ua (or up) for details), it is a good method for sampling since
and up (or ua). We call such configurations valid con-
tact configurations for the CF. Figure 6 shows two con- 3 valid range refers to the range of values for a variable
figurations with the same {e-f, e-f} CF, where (a) is a which satisfy the contact constraints of the CF and do not
valid configuration, and (b) is invalid because it has a cause local penetration between the two objects.
270 X. Ji and J. Xiao

in those cases, the value ranges of all independent

variables can be efficiently calculated.
e Hybrid Method: this method is for sampling re-
garding the other two-PC CFs where the range of
valid values for certain variables are hard or nearly
impossible to calculate. It first uses Direct Calcu-
Figure 7: Example {f-f} CF and the sampling procedure of
lation to obtain valid samples for variables whose
the translational variables: (a) pick p; and pz randomly on
ranges can be efficiently computed. Then, if there
the two contacting faces of A and B respectively, and (b)
are still other variables, it uses the following two-
translate A so that pi and p2 meet.
step procedure to obtain a valid random sample for
each such variable without calculating the range of
valid values with respect to the two-PC CF: domly picks two points on the two contacting elements
Step 1: Use Direct Calculation to randomly find ua and ug of PC; and then translates A until the two
a value of the variable satisfying one PC only. points meet. In this way, the translation always leads
to a valid configuration (i.e., maintaining the PC), and
Step 2: If the value sampled does not result in a
the configurations are evenly sampled due to the uni-
configuration satisfying the other PC as well, sim-
form randomness.
ply discard the value and repeat Step 1, which we
call resampling. Alternatively, a convergent it- The function trans_1PC() is outlined below:
eration strategy can be used to modify the in- func trans_1PC(C;, PC;)
valid value iteratively until a valid value is resulted
begin
(i.e., it leads to a valid configuration satisfying
both PCs). // ua is the contacting element of A in PC
randomly sample a point p; on u4;
3.1 Sampling for Single-PC CFs // up is the contacting element of B in PC,
Given a CF={PC}}, and a valid configuration Csceq
randomly sample a point po on up;
under the CF, the following function randomly gener- C + translate A from C; so that py = po;
ates a new valid configuration under the CF: return C;
func random_sample_1PC(Cseea, PC1) end.
begin
Note that if any of u4 and wz is a vertex, the ran-
// randomly translate A from Cyeeq to get C
domly sampled point on it is the vertex itself; if any of
Ca trons.
1 PO Ceca, 1) them is an edge or a face, the randomly sampled point
// randomly rotate A from C' to get new C is a random point inside the bounded edge or face. It
C + rotate-1PC(C, PC); is easy to pick a point randomly inside a bounded edge
return C; or a bounded convex face: The point is some convex
combination of the boundary vertices *. If the face is
end.
concave, it is first decomposed into several convex parts
The above function calls two subfunctions, which we (e.g., triangles), and then the point is sampled inside
explain in turn now. the convex parts.
Function trans_1PC() is used to translate A ran- Function rotate_1PC() generates a random rotation
domly along the contact plane of PC, to new valid (of A) compliant to the contact constraints of the CF
configurations. Note that finding explicitly the valid to achieve a new valid contact configuration. It pro-
ranges for the translational variables needs to calcu-
late the Minkowski sum of the two contacting elements “The convex combination of vertices PAG 1 25D) 18
yor O% * pi, Where oe, @i = 1. To sample a point inside
of the PC. In this function, we use a simple and effi- the face, we randomly pick up a vertex p; and randomly
cient method to achieve the same sampling effects with- sample a; in [0,1 — s], where s is the sum of a; already
out calculating the Minkowski sum. The function ran- sampled.
On Random Sampling in Contact Configuration Space
271
duces the random or arbitrary rotation through a se- From a given seed configuration Cseea, function
quence of rotations with respect to each independent, random_sample.2PC() randomly generates a valid
rotational variable. For each such variable, it first calls configuration for an arbitrary two-PC CF. Without los-
a function get_azis() to get its axis r and then calls ing generality, we designate PC, to be the PC with
find_angle_range() to determine the valid range of val- fewer dof if PC, and PC2 have different dof. Let CP,
ues for the variable (to satisfy the CF and cause no and C'P2 denote the contact planes of PC, and PC»
local penetration). Next it randomly picks an angle respectively, CL denote the intersecting line of CP;
inside the range and makes a rotation about r by the and CP, if they are not parallel, and cl denote a unit
angle. The function is outlined below. vector along either direction of CL. The function is
func rotate_1PC(C;,, PC,) outlined as follows:
begin func random_sample_-2PC
(Cseea, PC1, PC2)
C+eC,; begin
d < rotational dof of the CF; if (CF has 1 translational dof) then
for | = 1 to d do begin C + trans 2PO(Cseea, PC1, PC2, cl);
r4— getazis(C,1, PC}); else begin
(01,02) < find_angle_-range(C,r, PC,); vo < random unit vector on CP};
0 < randomly sampled angle in (61,62); C ¢ trans_2PC(Cseea, PC1, PC, v0);
C + rotate A by @ about r from C; end;
end; for! =1to3do
return C; if (CF has l-th rotational dof) then
end. C + rotate_ldof(l,C, PC1, PC2);
return C;
The function get_axis() works based on the type of end.
the CF. The axes are determined to facilitate the cal-
culation of valid value ranges for the rotations: the In the above function, independent translational
first axis is along the normal of the contact plane of variables are sampled by Direct Calculation with
the PC, the second axis (if exists) is either along the guaranteed valid values. Function trans_2PC() im-
contact line (for line PCs) or any line on the contact plements a random translation satisfying the two-PC
plane of the PC and passing through the contact point CF. With the axis v as an input, the function starts
(for point PCs), and the third axis (if exists) is de- from a given valid configuration C; and calls a proce-
termined by the right-hand rule from the other two. dure find_trans_range() to calculate the valid range
The angle range for the first rotation is (—7,7], and of translations along v relative to the given configura-
the ranges for the other two (if exist) are returned by tion C;, and then it randomly generates an increment
function find_angle_range(), which uses the algorithm of translation for object A within the valid range and
described in [18] to calculate angle ranges. obtains a new valid configuration, as outlined below.
func trans.2PC(C;, PC;, PC2, v)
begin
3.2 Sampling for Two-PC CFs
// find translational ranges along —v and U
For two-PC CFs with only one translational degree // while maintaining PC; and PC». d,d2 > 0
of freedom, Direct Calculation is sufficient for sam- [—di, do] «+ find_trans_range(C;, ¥, PC, PC2);
pling valid configurations, while for other two-PC CFs, d «+ randomly sampled value in [—d,, da];
the Hybrid Method introduced earlier (in the be- //if d < 0, translate along —v by |d|
ginning of Section 3) is used to sample valid config- C «translate A along U by d from C;;
urations. Nevertheless, we can combine the sampling
return C;
processes for all two-PC CFs in a general function as
described below. end.
 X. Ji and J. Xiao

Figure 8: The calculation of valid range of the translation

variable along CL for a CF with two non-parallel line PCs:
(a) calculate the range for PC; along CL, (b) calculate the
range for PC2 along CL, (c) find intersection of the ranges.

The implementation of function find_trans_range()

involves computing the shortest separation distance be-
tween the two contacting elements of the PC involved, Figure 9: The azes of the three rotational variables for
which can be two faces, a face and an edge, a face various CFs, (a) two point PCs with parallel CPs, (b) two
and a vertex, or two edges, along a given tangential line PCs with parallel CPs and collinear contact lines, (c)
direction of the contact plane. Figure 8 shows how one point and one plane PCs, (d) two lines PCs, (e) one
find_trans_range() works in the case where C'P; and point and one line PCs, (f) two point PCs.
CP, are non-parallel by an example. The function
first projects an arbitrary point of A to CL, denoted
as p, and then calculates the valid ranges to translate PC), or passing through the contact point of PC,
A along el and —cl without breaking PC, and PC2 and parallel to CL (if PC, is a point PC).
respectively. Next it finds the intersection of the two e |= 3: the rotation axis is denoted by Z and de-
ranges and returns it as the final range. In the case of a fined as passing through the two contact points (for
given random unit vector, the function works similarly. two point PCs) or along one contact line (for two
collinear line PCs, or one point and one line PCs
To sample rotational variables, we need to first. deter-
with the point on the line).
mine rotation axes. From analyzing all kinds of two-PC
CFs, we discover the following three general character-
Figure 9 illustrates these three kinds of rotation axes
izations of rotation axes, each corresponding to one ro-
in various examples. Note that in all cases, the rota-
tational variable (if it exists), which are sufficient for all
tion axis Y is actually for a combined rotation and
possible rotations constrained by two-PC CFs. We sim-
translation in order to maintain the CF. As the ro-
ply index them by / = 1, 2,3 in random_sample_2PC():
tation and translation are mutually dependent, there
e [=1: the rotation axis is denoted by X and de- is only one independent variable. We will explain its
fined as passing through one point on PC» (i.e., one sampling later.
point on both contacting elements of PC2) along
Clearly, depending on the rotational dof, not all ro-
the normal of CP}.
tations about these axes are always possible. Table 1
e [=2: the rotation axis is denoted by Y and defined summarizes all kinds of two-PC CFs, their rotational
as along the contact line of PC; (if PC, is a line dof and corresponding rotational axes.
On Random Sampling in Contact Configuration Space
273

if 26 Ces

CP, ||CP. |Fur

two point PCs
collinear contactSu
seagreca
points/lines e

ee
ee
CP, ||CP»

a
[eee Ae ERY oA Cscme] fsreve
3
cy
1 point, 1 line PCs
sonia alas
AE
2 point PCs
SqaEO00Ec
leEels
cele
Table 1: Two-PC CFs, their rotational dof, and corresponding rotational azes

Now we explain the sampling strategies regarding as the independent variable for the combined motion.
rotational variables about X, Y, and Z respectively in It first randomly samples d with a value satisfying PC,
more detail. by Direct Calculation (in a procedure similar to but
simpler than trans_2PC(), since only one PC needs to
Rotation About X, i.e., /] =1: be satisfied). Next it checks whether PC>2 can also be
We use the Hybrid Method introduced earlier. satisfied by a guarded rotation about Y, with the an-
The function rotate_ldof(1,C,PC 1, PC2) first calcu- gle calculated, which depends on the value of d. If so,
lates the angle range about X which satisfies PC2 by the function returns a valid configuration C; otherwise,
calling find_angle_range() (see Section 3.1). It then again, either resampling or convergent iteration on
samples an angle @ inside the range, and next rotates the value d (i.e., d ~ kd, where 0 < k < 1, repeatedly
A about X by @ to get a new configuration C. If C also until d results in a valid configuration) can be used.
satisfies PC}, i.e., forms a valid configuration, it is re- Figure 10 shows how a rotation about Y is sampled
turned; otherwise, either resampling or convergent for a CF with two line PCs of parallel contact lines
iteration can be used (as introduced before Section (and non-parallel contact planes).
3.1). Here convergent iteration is to modify @ by ké,
Figure 11 shows the sampling for a CF with two line
ie., 0 + kO, where 0 < k < 1, repeatedly until @ re-
PCs of non-parallel contact lines and contact planes.
sults in a valid configuration, i.e., a convergence to the
Besides the similar steps as those for the case with
valid value range is achieved.
parallel contact lines as shown in Figure 10, here an
extra step is needed as shown in Figure 11(d) to rotate
Rotation About Y, i.e., | = 2:
about X axis, which is a guarded rotation trying to
Sampling again uses the Hybrid Method. As men- satisfy PC.
tioned earlier, the motion here is a combined transla-
tion and rotation with one independent variable. Al- Rotation About Z, i.e., [E="3:
though the instantaneous rotational axis for the vari- Unlike the two previous cases about X and
able can be found (see Figure 4), it is generally difficult Y, here the rotational variable about Z is sam-
to be used for sampling because the axis is not fixed pled using Direct Calculation. The function
and it is hard to analytically describe the axis’s motion. rotate_ldof(3,C,PC1,PC2) first calculates the ro-
Therefore, we use another strategy to sample this vari- tational angle ranges (011,612) and (021,422) about
able, which is given below. Z for PC, and PCy, respectively by calling
In all cases where this motion is possible (Figure 9 find_angle_range() (see Section 3.1), and then finds
gives some examples), C'P; and C’P2 are not par- the intersection of the ranges as the valid range of the
allel, and they intersect at line CL. The function rotational variable. Finally, an angle @ is sampled ran-
rotate_ldof(2,C,PC,PC2) uses a translational vari- domly inside the range, and A is rotated about Z by 0
able d along an axis U on CP, and perpendicular to CL to generate a valid configuration.
274 X. Ji and J. Xiao

Figure 10: Sampling a combined motion with rotation

about Y for a CF with two parallel line PCs and non-parallel
oe
Figure 11: Sampling a combined motion with rotation
about Y for a CF with two non-parallel line PCs and non-
contact planes: (a) seed configuration, translational range parallel contact planes: (a) sampled distance vector d on
[—di, dz] along ¥, and sampled value d in [—d1, d2]; (b) con- CP;; (b) configuration after translation by d with PC,
figuration after translation along v by d, and the guarded ro- maintained but PC2 broken, and the guarded rotational an-
tation angle 0 about Y; (c) configuration after the guarded gle 0; about Y so that at least one vertex of the contact edge
rotation about Y by @ to form PC2. of PC2 can be on CP2 after the rotation; (c) configuration
after the guarded rotation about Y by 01, with only one ver-
tex v1 of the contact edge of PCz2 on CP2, and guarded
In summary, we have presented a general function rotational angle 02 about X so that the contact edge viv2
random-_sample-2PC() to produce random configura- can be on CP2; (d) configuration after rotation about X
tions satisfying any given two-PC CFs. Note that only by 02, where both PC, and PC2 are satisfied. Note that
in two steps regarding rotations about X and Y (i.e., though the figure shows the two contact lines are coplanar,
| = 1,2), the Hybrid Method is used, and Direct the non-coplanar cases are handled similarly.
Calculation is used in sampling all the other variables
to maximize efficiency.
4 Experimental Results
In the Hybrid Method, the alternative to Direct
Calculation is either resampling or convergent it-
eration. Resampling for a single variable is simply to The random sampling strategy for single-PC and two-
repeat the sampling process for that variable until a PC CFs has been implemented in C. The program runs
value which results in a valid configuration is found or on a SUN Ultral0 workstation. The machine is rated
after some pre-determined number of tries. at 12.1 SPECint95 and 12.9 SPECfp95. The input to
the program is a CF and a valid contact configuration
Convergent iteration, on the other hand, guarantees
satisfying the CF. The output are random configura-
to find a valid value for the variable or makes it con-
tions of the same CF which are guaranteed no local
verge to the valid value range. If there is more than
penetration. We use VRML as the output format.
one connected valid value range, which may happen in
the cases where the two-PC CF has multiple connected Figure 12 shows the sampling results for several
regions of contact configurations, caution is needed to single-PC CFs between a cube A and an L-shape B.
make the convergence to each valid range equally likely Figure 13 shows the sampling results for two-PC CFs
in order to ensure the even distribution of samples. between different shapes of objects. The running time
This, however, can be achieved by always using the (in seconds) for generating 1000 samples of the exam-
newly randomly sampled configuration as the seed con- ples in Figure 12 and Figure 13 are summarized in Ta-
figuration for the next sample. ble 2
On Random Sampling in Contact Configuration Space
275

(a) {f-f}: 3 dof (b) {e-f}: 4 dof

(c) {v-f}: 5 dof (d) {e-e-c}: 5 dof

Figure 12: Examples for single-PC CFs: seed configurations and the results for 1000 samples

The results summarized in Table 2, show clearly the third variable is the fastest because of Direct Cal-
that it takes much shorter time to generate samples culation.
for single-PC CFs. This is because, though usually
single-PC CFs have higher dof, sampling is done by 5 Conclusion
Direct Calculation. For two-PC CFs of the same
objects, usually the higher dof the CF has, the more This paper addresses random sampling of configura-
time is needed to sample the same number of configura- tions constrained by contact, which is not only nec-
tions, although the time also depends on the geometry essary for planning contact motions with certain ran-
of the objects. For the same CF, the running time of domized planners but also useful for planning collision-
the algorithm is nearly proportional to the number of free motions since the sampled contact configurations
samples generated. probabilistically characterize the C-obstacles. An ef-
In the last three rows of Table 2, we show the run- ficient random sampling strategy is implemented for
ning times of the examples using convergent itera- sampling configurations constrained by single-PC or
tion and resampling respectively. It seems that con- two-PC CFs, which satisfy contact constraints of the
vergent iteration runs faster in most cases. Our ex- CF without causing local penetration. The strategy
periments show that for convergent iteration with is characterized by directly computing valid samples
k = 0.5, after at most 13 iterations, a valid configu- wherever possible to maximize efficiency. In the next
ration can be obtained for all the four examples. For step, we intend to apply such a strategy to randomized
resampling, on the other hand, after at most 73 re- contact motion planning.
sampling iterations, a valid configuration can be ob-
tained. Acknowledgments
Our experiments also show that among the three ro-
tational variables, the sampling of the second one about This work has been supported by the National Sci-
Y with dependent translation is the most expensive, ence Foundation under grants IIS-9700412 and CDC-
followed by that of the first variable, and sampling for 9726424. The authors appreciate early inspiration from
276 X. Ji and J. Xiao

(a) {f-f, f-f}: 1 dof (b) {e-f, f-f}: 1 dof

(c) {e-f, f-f}: 2 dof (d) {e-e-c, f-f}: 2 dof

(e) {e-f, e-f}: 2 dof (f)-{e-f, f-e}: 2 dof

(g) {v-f, e-f}: 3 dof (h) {v-f, v-f}: 4 dof

Figure 13: Examples for two-PC CFs: seed configurations and the results for 1000 samples
On Random Sampling in Contact Configuration Space

Method | 3) CrQ 7 [dof

| (EA, Figure 12(6) ef}, Figure 100)
|
Direct_[_{v-t} Figure (6) (
Direct || {£f, Ef}, Figure 13(a)
|
Hybrid|| {e-f, Ef}, Figure 13(c)
|
Hybrid|| {e-f, e-f}, Figure 13(e)
|
Hybrid|| {v-f, e-f}, Figure 13(g)

Table 2: Examples in Figure 12 and Figure 13 and their running times for 1000 samples.
In the last three rows, the two
numbers given for each case under times(s) correspond to the running times using convergent iteration
and resampling
respectively.

discussions with Jean-Paul Laumond, Jean-Claude [10] X. Ji and J. Xiao, “Towards Random Sampling with
latombe, David Hsu. The authors are also grateful to Contact Constraints”, to appear in JCRA’2000.
comments by Matt Mason and Mark Yim.
[11] L. Joskowicz, R. H. Taylor, “Interference-Free Inser-
tion of a Solid Body Into a Cavity: An Algorithm and a
References Medical Application”, Int. J. Robotics Res., 15(3):211-
229, June 1996.
[i] N. Amato, O. Bayazit, L. Dale, C. Jones, D.
Vallejo, “OBPRM: An Obstacle-Based PRM for 3D [12] L.E. Kavraki, P. Svestka, J.C. Latombe, and M. Over-
Workspaces”, Workshop Algor. Found. of Robotics, mars, “Probabilistic Roadmaps for Path Planning in
pp. 155-168, Mar. 1998. High-Dimensional Configuration Spaces”, IEEE Trans.
Robotics & Automation, 12(4):566-580, 1996.
[2] F. Avnaim, J. D. Boissonnat, and B. Faverjon, “A prac-
tical exact motion planning algorithm for polygonal ob- [13] T. Lozano-Pérez, “Spatial Planning: A Configuration
jects amidst polygonal obstacles”, IEEE Int. Conf. Ro- Space Approach”, IEEE Trans. Comput., C-32(2):108-
botics & Automation, pp. 1656-1661, Apr. 1988. 120, 1983.

[3] V. Boor, M. H. Overmars, and A. F. Stappen, “The [14] M. T. Mason, “Compliant Motion”, Robot Motion:
Gaussian Sampling Strategy for Probabilistic Roadmap Planning and Control, MIT Press, 1982.
Planners”, JEEE Int. Conf. Robotics & Automation,
May 1999. [15] C. Nissoux, T. Simeon, and J. P. Laumond, “Visibility
based probabilistic roadmaps”, IEEE Int. Conf. Intell.
[4] R. Brost, “Computing Metric and Topological Proper- Robots and Systems, 1999.
ties of C-space Obstacles”, [EEE Int. Conf. Robotics &
Automation, pp. 170-176, May 1989. [16] D. E. Whitney, “Historical Perspective and State of the
Art in Robot Force Control”, [EEE Int. Conf. Robotics
[5] B. Donald, “A Search Algorithm for Motion Planning & Automation, pp. 262-268, 1985.
with Six Degrees of Freedom”, Artificial Intelligence,
pp. 295-353, 31(3), 1987. [17] J. Xiao, “Automatic Determination of Topological
Contacts in the Presence of Sensing Uncertainties,”
[6] A. Farahat, P. Stiller, and J. Trinkle, “On the Algebraic IEEE Int. Conf. Robotics & Automation, pp. 65-70,
Geometry of Contact Formation Cells for Systems of May 1993.
Polygons”, IEEE Trans. Robotics & Automation, 11(4),
pp. 522-536, Aug. 1995. [18] J. Xiao and L. Zhang, “Computing Rotation Distance
between Contacting Polytopes,” IEEE Int. Conf. Ro-
[7] H. Hirukawa, “On Motion Planning of Polyhedra in botics & Automation, pp. 791-797, Apr. 1996.
Contact”, Workshop Algor. Found. of Robotics, 1996.
[19] J. Xiao and L. Zhang, “Contact Constraint Analy-
[8] D. Hsu, L. Kavraki, J.C. Latombe, R. Motwani, and
sis and Determination of Geometrically Valid Contact
S. Sorkin, “On Finding Narrow Passages with Proba-
Formations from Possible Contact Primitives”, [EEE
bilistic Roadmap Planners”, Workshop Algor. Found.
Trans. Robotics and Automation, 456-466, Jun. 1997.
of Robotics, pp. 141-153, Mar. 1998.
[20] J. Xiao and X. Ji, “A Divide-and-Merge Approach to
[9] X. Jiand J. Xiao, “Automatic Generation of High-Level
Automatic Generation of Contact States and Planning
Contact State Space,” IEEE Int. Conf. Robotics & Au-
tomation, pp. 238-244, May 1999. of Contact Motion”, to appear in ICRA’2000.
 y+‘acttielglh
~ aeutiel) Soule
Binet Ae lat
aay
fa - ~ her eee

> '- Ot ap
/

eo
en eee’ oY
ie is |
e
. ST Eo
a
hea ‘ : nbs Pateviape: 9 Yo “eres? Eph Lee erry Mil Miz :

aT AMOS hE LOT aeestiveret veo

va
. me pe
te! ey ete MMe gy), been eta, erin?

od AMIEL
AL ogee is shi ee A a hie corr ' a ~~. is iT cam
Kid
Raletd” Ioutn Beutler Ny ett
Che? ejay ‘en p
Fave toahghi}Yo cetiareggl ip
ar eee ame Mt ' wh We
OOO BAA th x ede ar eon Faw
doh Low 3 icv WAR ‘oe
OU! veh
Randomized Path Planning for a Rigid Body
Based on Hardware Accelerated Voronoi Sampling
Charles Pisula, University of North Carolina, Chapel Hill, NC
Kenneth Hoff III, University of North Carolina, Chapel Hill, NC
Ming C. Lin, University of North Carolina, Chapel Hill, NC
Dinesh Manocha, University of North Carolina, Chapel Hill, NC

Probabilistic roadmap methods have recently received of digital actors or autonomous agents [16], maintain-
considerable attention as a practical approach for mo- ability studies [4], drug design [7] and robot-assisted
tion planning in complex environments. These algo- medical surgery [22, 24].
rithms sample a number of configurations in the free
This problem has been extensively studied over the
space and build a roadmap. Their performance varies
last three decades. A number of analytical methods
as a function of the sampling strategies and relative
and approximate techniques have been proposed. How-
configurations of the obstacles. To improve the perfor-
ever, due to the computational complexity of complete
mance of the planner through narrow passages in con-
motion planning algorithms, most practical algorithms
figuration space, some researchers have proposed algo-
are based on randomized motion planning algorithms,
rithms for sampling along or near the medial azis of the
such as randomized potential field methods (RPP) or
free space. However, their usage has been limited be-
probablistic roadmap methods (PRMs).
cause of the practical complexity of computing the me-
dial axis or the cost of computing such samples. The simplest PRM algorithms generate a set of con-
figuration in the free space. The planning algorithm
In this paper, we present efficient algorithms for
involves three main steps [26, 14, 15, 21]:
sampling near the medial axis and building roadmap
graphs for a free-flying rigid body. We use a re-
1. Generate a list of samples in the configuration
cent algorithm for fast computation of discrete general-
space. The simplest algorithms use uniform sam-
ized Voronoi diagrams accelerated by graphics hardware
pling techniques.
[10]. We initially compute a bounded error discretized
Voronoi diagram of the obstacles in the workspace and 2. Use collision detection or distance computation al-
use it to generate samples in the free space. We use gorithms to select the samples lying in the free
multi-level connection strategies and local planning al-
space.
gorithms to generate roadmap graphs. We also utilize
the distance information provided by our Voronoi al- 3. Try connecting the samples in the free space by lo-
gorithm for fast proximity queries and sampling the cal planning methods and build a roadmap graph.
configurations. The resulting planner has been applied
to a number of free flying rigid bodies in 2D (with The roadmap graph is used to generate a path between
3-dof) and 3D (with 6-dof) and compared with the
the start and goal configurations. However, the effi-
performance of earlier planners using a uniform sam-
ciency of these planners can degrade in configurations
pling of the configuration space. Its performance varies
containing narrow passages or cluttered environments.
with different environments and we obtain 25% to over
In such cases, a significant fraction of randomly
1000% speed-up. generated configurations are not in the free space.
Moreover, it is hard to generate a sufficient number
1 Introduction of samples in the narrow passages or connect all the
nodes in the free space using local planning methods.
Motion planning is one of the important, classical prob- Many approaches have been proposed in the literature
lems in algorithmic robotics [18]. Besides robotics, it to overcome these problems. These include:
has applications in many areas, including animation
280 C. Pisula, K. Hoff III, M. C. Lin, and D. Manocha

Dilating the Free Space[13]: The main idea is to 1.1 Our Results
dilate the free space by allowing the rigid body to
In this paper, we present improved algorithms for sam-
penetrate the obstacles by a small amount. The areas
near these nodes are resampled to find collision free pling based on medial axis and use them for effi-
ciently constructing the Voronoi roadmap. We make
configurations. However, dilation can alter the topol-
use of bounded error discretized Voronoi diagrams,
ogy of the free space. Furthermore, no good practical
computed using graphics rasterization hardware [10].
(in terms of efficiency and robustness) algorithms are
More specifically, we render distance functions for each
known for penetration depth computation between
primitive of the obstacle and use the frame buffer out-
polyhedral models. Some of the public domain im-
put to identify the Voronoi boundaries upto a given
plementations like I-COLLIDE and V-Clip for convex
resolution. Furthermore, the depth buffer provides
polytopes, based on Lin-Canny distance computation
the distance information which is used to speed up
algorithm, provide only an approximation to the
the proximity queries and sampling the configuration
penetration depth.
space. This computational framework enables insight-
ful analysis of the workspace to identify different types
Sampling near the Obstacle Boundaries [1]: It of narrow passages and efficient generation of sampling
samples the nodes from the contact space, the points with guaranteed distribution on the medial axis.
configurations where the robot just touches one of We use multi-stage connection strategies along with lo-
the obstacles. It works well in many cases, but its cal planning algorithms to build the roadmap graphs.
performance is difficult to analyze. The resulting PRM has been applied to a number of
complex environments composed of 3-dof (in 2D) and
6-dof rigid bodies (in 3D). As compared to uniform
Information about the Environment [12, 21]:
sampling of the configuration space, it can considerably
These algorithms make use of information known
improve the performance of the planner. Our initial ex-
about the environment. These include executing
periments demonstrate 25% to over 1000% speed-up in
random reflections at the C-obstacle surfaces [12]
running times. Since the cost of generating the samples
and adding “geometric” nodes for non-articulated
is relatively small, the planner never underperforms as
robots near the boundaries of the obstacles in the
compared to uniform sampling.
workspace [21].
Some of the main advantages of our approach in-
clude:
Sampling Based on the Medial Axis [27, 26, 9]:
The main idea is to generate nodes that lie on the me-
e Efficiency: The resulting algorithms for gen-
dial axis of the workspace or the free space. Intuitively
erating the bounded error approximation of the
speaking, the medial axis corresponds to a set of points
Voronoi diagram and samples on the medial axis
that are furthest from the obstacle boundaries and have
run relatively fast. For environments composed of
maximum clearance. It has been used for a number of
thousands of polygons, we can generate Voronoi
motion planning algorithms [20, 3, 18, 6, 5]. However,
diagrams at 128 x 128 x 128 resolution at in a few
its practical use has been limited because of the diffi-
seconds or minutes (depending on the size of the
culties in accurately computing the medial axis or gen-
environment) on a SGI workstation. Furthermore,
eralized Voronoi diagrams. Wilmart et al. [27, 26] gen-
the distance buffer helps us speed up the collision
erate random configurations and retract them onto the
queries by a factor of two. Based on our sampling
medial axis without explicitly computing the medial
strategies, a high fraction of the nodes generated
axis. They use a search algorithm based on distance
are in the free space.
to the obstacles for the retraction. They have applied
the resulting algorithm to a few configurations with e Simplicity: The resulting algorithm is relatively
narrow passages and obtained considerable speedups simple to implement and doesn’t suffer from ro-
over uniform sampling. Guibas et al. [9] use point ap- bustness or degeneracies. The basic PRM planner
proximations on the boundary, compute their Voronoi is a rather simple approach and our sampling al-
diagrams to approximate the medial axis and use it for gorithm doesn’t introduce any complications.
motion planning of flexible objects.
Randomized Path Planning for a Rigid Body Based on Hardware Accelerated
Voronoi Sampling 281
e Identification and Characterization of Nar- and orientations. We use (zx, y, z) coordinates to repre-
row Passages: The distance buffer information sent the position and a unit quaternion to represent the
gives us information about narrow passages in the orientation. The (x,y,z) coordinates will be referred
workspace. Using a combination of distance met- to as the translation component of a configuration and
ric for dimension analysis and retraction methods, the quaternion is the rotation component of a config-
it can also be used to identify narrow passages in uration. Furthermore, C can be partitioned into free
many cases. space, F’ and blocked space, B. The blocked space cor-
responds to configurations, where the robot R collides
e Global connectivity information based on
with at least one of the obstacle. The rest of the con-
Voronoi Roadmaps: The connectivity infor-
figuration space corresponds to the free space. In other
mation provided by the discretized generalized
words, F = C’\B. Furthermore, we denote the bound-
Voronoi diagram is used in constructing the
ary of the free space as OF and JO represents the union
roadmap and accelerating performance of the local
of boundaries of all the obstacles.
planner.
2.2 Medial Axis and Voronoi Diagrams
The rest of the paper is organized as follows: In
Section 2, we highlight our notation, provide a brief The medial axis of a solid object provides shape analy-
overview of the medial axis and the fast approximate sis in terms of its boundary elements. It is a skeletal
to compute a discretized approximation using graphics representation that can be formulated as the locus of
hardware. We describe the planner in Section 3 and the center of a maximal sphere as it rolls around the
present the multi-stage connection strategies to build object interior. It is closely related to the Voronoi dia-
the roadmap. Section 4 presents our sampling strate- gram of a solid and for a suitably defined boundary, it
gies based on medial axis of the workspace. can be computed from the Voronoi diagram and vice
versa. The concept of medial axis was first proposed
Section 5 discusses various implementation issues, in- by Blum [2] for biological shape measurement and since
cluding collision culling, computations of Voronoi ver- been used for mesh generation, feature recognition and
tices and graphs, and other data structures. We also molding simulation. Medial axis and Voronoi diagrams
highlight the performance of our planner on different have been long used for robot motion planning [20,
benchmarks. Finally we compare it with related ap- 3, 18, 6]. The medial axis of the free space F, de-
proaches in Section 6. noted by MA(F’) has lower dimension than F' but is
still a complete representation for planning the mo-
2 Background tion. Strictly speaking, MA(F’) is a strong deforma-
tion retract of F, implying that F can be continuously
In this section, we highlight our notation and provide a deformed into MA(F’) and maintain its topology struc-
quick overview of medial axis and fast computation of ture [20, 3, 6, 27].
discretized Voronoi diagrams using graphics hardware. Algorithms to compute Voronoi diagrams and me-
dial axis have been extensively studied in computa-
2.1 Notation and Representation tional geometry and solid modeling. Good theoretical
and practical algorithms are known for point primi-
In this paper, we restrict ourselves to dealing with non-
articulated rigid bodies in 2D or 3D. We use the sym- tives. However, the boundaries of the Voronoi regions
bol W to represent the workspace. We will denote for higher order primitives (e.g., lines, triangles) cor-
the robot as R and the set of obstacles as O. We respond to high-degree algebraic curves and surfaces.
assume that the robot and each obstacle is a closed No good practical algorithms are known for comput-
ing them efficiently and robustly. As a result, their
and bounded set. Our current implementation assumes
applications have been limited.
that each obstacle is represented as a collection of tri-
angles. It is relatively simple to extend them to other Given the practical complexity of computing Voronoi
primitives (e.g., spline models) based on our current diagrams, many researchers have proposed approxi-
Some common approximations in-
algorithmic framework. The configuration space (de- mate approaches.
noted by C) of R is given by the set of all positions clude generating point approximations of the bound-
282 C. Pisula, K. Hoff III, M. C. Lin, and D. Manocha

aries and computing their Voronoi diagrams [23]. How- 3 Path Planning Based on Discretized
ever, it is hard to give any guarantees on the accuracy Voronoi Diagrams
of the resulting Voronoi diagram. Other approaches
use spatial subdivision techniques [25, 19]. While these In this section, we describe our path planning algo-
algorithms can be used to generate bounded error ap- rithm. Initially we present algorithms for computing
proximations of Voronoi diagrams, they can be rather the discretized Voronoi diagrams, followed by use of
slow for large environments. boundary finding algorithms that extract the Voronoi
graph. We use a multi-stage strategy to build the
roadmap. It generates configurations in the free space
2.3. Bounded Error Approximation of Voronoi and connect them using local planning algorithms. The
Diagrams Using Graphics Hardware first stage selects nodes using the medial axis of the
workspace and connects them using simple local plan-
We compute a bounded error approximation of the ning algorithms. Next it attempts to connect different
Voronoi diagram of the obstacles in the workspace, W. components. Finally, it tries to estimate narrow pas-
If any of the obstacles is a closed and bounded solid, sages based on local characteristics of the Voronoi di-
we do not compute the Voronoi diagram inside that agram and generates more samples in those regions.
solid. Each boundary triangle, edge, and vertex of an More details on improved sampling algorithms are
obstacle is treated as a separate site. We compute a given in Section 4.
discrete generalized Voronoi diagram by rendering a
three-dimensional distance mesh for each site. The 3D
3.1 Discretized Voronoi Diagram of the
polygonal distance mesh is a bounded-error approxi-
Workspace
mation of a possibly non-linear distance function over
a plane. Each site is assigned a unique color, and the
Given a bound on the discretization error, the algo-
corresponding distance mesh is rendered in that color
rithm computes the Voronoi diagram of the obstacles
using parallel projection. The graphics system per-
O slice-by-slice (along the z-axis). It renders the dis-
forms a depth test for each pixel in order to resolve the
tance function for each vertex, edge and triangular
visibility of surfaces. The depth buffer keeps a running
face of each obstacle. Each slice is generated using
minimum depth as polygons are rendered. When the
the graphics rasterization hardware. We read back the
minimum depth is updated, the frame buffer is also
color buffer and the depth-buffer for each slice. The
updated with the pixel’s color. Thus, the rasterization
color buffer gives the index of the nearest obstacle to
provides, for each pixel, the identity of the nearest site
each sample point in the slice. The distance buffer
(encoded as a color) and the distance to that site. The
gives the distance to that obstacle. It generates a 3D
error in the mesh is bounded to be smaller than the
voxel grid that corresponds to a uniform sampling of
distance between two pixels, in order to maintain an
the space containing the geometric primitives. This 3D
accurate Voronoi diagram. More details are given in
image gives a volumetric representation of the general-
[10]. This algorithm runs very fast in practice.
ized Voronoi diagram of the primitives. The resolution
of the Voronoi graph can have significant impact on
2.3.1 Motion Planning Using Discretized Voronoi the performance of the planner (as highlighted in Sec-
Diagrams tion 5). In our current implementation, we take uni-
form samples along the z-axis. In theory, it is possible
Many motion planning algorithms have been pro- to vary the step size adaptively using bisection. For
posed based on discretized Voronoi diagrams [25, 10, PRM, we are only interested in computing an accurate
11]. Based on the color and distance buffer informa- approximation of the Voronoi boundaries.
tion, [10, 11] define a potential field and use it to nay-
igate the robot. Since the distance buffer information 3.2 Extracting the Voronoi Graph
is computed at interactive rates, this planner has also
been applied to dynamic 2D environments (3-dof ro- The Voronoi diagram is in the form of two 3D images:
bots). a color image corresponding to the IDs of the closest
Randomized Path Planning for a Rigid Body Based on Hardware Accelerated
Voronoi Sampling 283
site to each sample point and a depth image giving the ples in the contact space. Our roadmap construction
related distances to the closest sites. Since we are us- algorithm proceeds in three stages.
ing a volumetric representation, the actual continuous
boundaries of the Voronoi graph are described implic-
itly as lying between sample points of different colors 1. Preprocessing: As part of a pre-process, we gen-
(sample points that have different closest primitives). erate a number of nodes using points near the
Our goal is to extract the boundary graph struc- medial axis of the workspace. We assign con-
ture in order to bias the randomized motion planning figurations where the position of the robot cor-
sampling. In 3D workspace, the Voronoi graph struc-
responds to a point lying on or near the medial
axis. The orientation is assigned either randomly
ture is composed of Voronoi vertices, edges, and faces.
or based on the local characteristics of the me-
However, for our applications, we only need the ver-
dial axis (see Section 4). Since the points on the
tices and the edges. This will give us a graph struc-
medial axis have the maximal clearance from the
ture forming maximally clear paths from the obsta-
obstacles, these nodes will bias the robot to plan
cles in the workspace. Voronoi vertices are the set
a path near the medial axis. We use an adap-
of points equidistant among four or more primitives,
tive strategy to select the nodes. Initially, we se-
and Voronoi edges are the set of points equidistant
lect nodes corresponding to the vertex locations in
among three primitives. To extract these features we
the Voronoi graph. We use a local planning algo-
use continuation methods, which are very similar to
rithm to check whether we can generate an edge
common iso-surface extraction techniques commonly
of the roadmap between those nodes. If not, we
used in volume rendering. Our goal is to continuously
select a midpoint location along the Voronoi edge
“bracket” the boundary curves in a 2 x 2 x 2 region of
and repeat the process for each Voronoi edge in
sample points and then walk along the boundary one
the roadmap graph. This process is repeated un-
voxel at a time. We only step to the next 2 x 2 x 2
til both nodes are connected. At the end of this
region if it is part of the same boundary. In this man-
phase, the roadmap may consist of one or more
ner, we only touch 3D sample points that are close to
connected components and its topology is similar
the boundary. Since we are effectively growing the en-
to that of the Voronoi graph. The local planning
tire boundary from some starting “seed” point, we are algorithm is similar to the rotate-at-s algorithm
able to form a correctly connected graph structure eas- highlighted in [1].
ily and efficiently. The seed point is found along the
boundaries of the voxel grid since the boundary graph
must typically intersect the bounding volume of the 2. Connecting Multiple Connected Compo-
workspace. nents: If the roadmap has more than one con-
nected component, we try to connect them. This
requires trying different set of nodes between each
3.3. Multi-Stage Roadmap Construction component and trying to find a path using the
local planner. Or we generate more nodes that re-
Given the Voronoi graph, we present a multi-stage al- tain the same positions close to the medial axis of
gorithm to build the roadmaps. The goal is to initially the workspace, but with different orientations for
generate portions of the roadmap using the medial axis the robot. Then, we try to connect these nodes to
and simple local planning algorithms. Next we connect different components of the roadmap.
different roadmaps generated using more sophisticated
techniques. Finally, we make use of the Voronoi graph
and the distance buffer to estimate narrow passages 3. Sampling along Narrow Passages: We esti-
and generate additional samples along these narrow mate narrow passages based on the medial axis
passages and try to connect them with the other sam- and the distance buffer. More details are given in
Section 4. We generate nodes near these narrow
pled nodes. Our overall strategy in using a multi-stage
passages by using a combination of uniform sam-
algorithm is similar to that highlighted in [1]. However,
pling, Gaussian sampling and biased angle sam-
we choose samples based on the medial axis and esti-
pling to connect them with the roadmap.
mate narrow passages, as opposed to generating sam-
284 C. Pisula, K. Hoff III, M. C. Lin, and D. Manocha

4 Sampling Strategies Based on with the distance values of the voxels that represent the
Voronoi Graphs discretized medial axis of the workspace.
Let us assume that the tightest-fitting arbitrarily
In this section, we present strategies to estimate narrow oriented bounding box (OBB) is given for the robot.
passages and improved sampling algorithms based on Moreover, the box’s local coordinate frame and its
the medial axis of the workspace. dimensions with respect to the global coordinate frame
are known. Algorithms based on covariance matrix to
4.1 Estimation of Narrow Passages
approximate the “principle component directions” of
Our algorithm computes a discretized Voronoi diagram a given collection of polygons can be used to compute
of the workspace. With each voxel, we associate a color a tight fitting OBB [8]. Let us further assume that the
that corresponds to an ID of the closest obstacle and a exact dimensions of the “potential” narrow corridors
distance value, which stores the distance to that obsta- are given. Based on this information, we define, the
cle. By comparing this value against the dimensions of following types of narrow passages:
the robot, we can estimate the narrow passage in many
cases. Given a robot R, let roy; be the radius of min-
Type-T Narrow Passages: Robot can move
imum enclosing sphere and r;, be the radius of the through narrow passages by translational motion.
maximum inscribed sphere. If we are given a subset of Rotations are not required for the robot to navigate
voxels on the Voronoi boundary, whose distance value
through the narrow corridors. This occurs when all
is less than rouz, then all the configurations whose cor- the dimensions of the robot’s OBB are equal or slightly
responding positions in workspace (translation compo-
smaller than the dimensions of the narrow corridors.
nents) are close the location of these voxels may contain Images 1-3 are examples of such narrow passages
“narrow passages.” In such cases, it may be difficult to
where merely careful translational movements are
plan the motions of the robot through these narrow
sufficient to navigate the robot through the narrow
passages, even when only translational displacements
corridors.
are required to navigate through the narrow corridors.
Furthermore, if any collection of voxels on the Voronoi
boundary have a distance value less than r;,, then all Type-R Narrow Passages: Rotations are required
configurations whose translation components (or the for the robot to navigate through the corridors. This
corresponding positions in workspace) are close to the occurs often when the largest dimension of the robot’s
location cannot be contained in the free space. OBB exceeds those of the narrow corridors. Images 3
Furthermore, the degrees and types of “tightness” in and 6 highlight examples of two narrow passages that
a cluttered environment are determined by the dimen- require rotations to plan the path for the robot and the
sions of the robot with respect to the dimensions of nar-
piano.
row corridor in the workspace (defined based on some
type of distance metric). Normally, the medial rep- 4.2 Random Sampling Near the Medial Axis
resentation computed is only the generalized Voronoi
Once we can identify different types of narrow passages
diagram or medial axis of the “workspace.” Ideally we
in the configuration space, we can design better sam-
wish to compute the medial axis of the free space and
pling strategies for handling these scenarios.
identify the narrow passages more precisely by con-
sidering the rotational components of robot configura- In general, if the robot encounters a type-T narrow
tions. Unfortunately, no fast and practical algorithm passage, the robot can easily move through the narrow
is known for computing the configuration space. corridors by placing the geometric center of the robot’s
Our algorithm computes an approximate medial axis OBB along the edges of the Voronoi graph. In such
of the workspace. Therefore, the scheme we mention cases taking samples on the medial axis (the first step
to estimate the location of “narrow passages” is only in our multi-level strategy) has considerable benefits.
an approximation technique. In the rest of this sec- For handling type-R narrow passages, sampling
tion, we describe some techniques that perform dimen- along the Voronoi graph with random sampling of an-
sion analysis by comparing the dimensions of the robot gles can lead to poorer performance. Instead, we pro-
Randomized Path Planning for a Rigid Body Based on Hardware Accelerated Voronoi
Sampling 285
pose to sample near the Voronoi graph using a combi- buffer. The performance of the Voronoi algorithm on
nation of the following strategies: different scenarios has been highlighted in Table 1. It
is a linear function of the number of voxels in the grid
e Simple Uniform Sampling: Place more sam- and increases as we subdivide a voxel into 8 sub-voxels.
pling points uniformly inside the narrow passages, We expect this performance can be improved by using
once they have been identified. This technique adaptive grid size selection. We are currently investi-
basically increases the density of sampled nodes gating the implementation issues related to adaptive
inside the narrow passages everywhere. grid size.

e Gaussian distribution: Sample near the medial We perform collision and distance queries between
axis with a Gaussian distribution to randomly po- the robot and the obstacles using PQP [17]. PQP uses
sition the points with higher distribution density a hierarchy of swept sphere volumes and works effi-
near the medial axis while uniformly sampling in ciently on general polygonal models. A typical planner
the angular space. spends a high percentage of its time in performing col-
lision and distance queries. To speed up these queries,
e Angular Bias: Biasing the angular sampling by we enclose the robot R with a bounding sphere. Let
positioning the local coordinate axis along which its radius be rou~. We compare the radius with the dis-
the robot’s OBB has its smallest dimension so that tance value associated with the voxel that contains the
the axis is perpendicular to the tangent direction position of the center of the enclosing sphere. If rox; is
of the Voronoi graph with Gaussian distribution. less than the distance value, it implies that the given
Intuitively, this sampling strategy attempts to ori- configuration lies in the free space and we don’t have
ent the robot to intelligently adapt to the change to perform explicit collision check between the robot
in curvatures of the Voronoi graph, as it moves and all the obstacles. Otherwise, we use PQP routines
through the narrow passages. for exact collision detection. We refer to this acceler-
ation technique as QR collision check for quick rejec-
We have experimented with a combination of these tions. Given the color buffer, we compute the bound-
strategies on several different benchmark environments ary graph of the Voronoi diagram using a marching
and have observed good performance improvement over technique. We next compute an approximation to the
uniform sampling. More details of our implementation medial axis, and identify regions to be considered as
results are given in the next section. narrow passages in the workspace (as described in Sec-
tion 4).
In the roadmap construction phase, the planner at-
5 Implementation and Performance
tempts to build a network of configuration nodes that
can be used for the path planning queries. We use the
We describe the implementation of our planner and its
multi-stage algorithm described in Section 3. We rep-
application to different benchmarks.
resent the roadmap as a graph with multiple connected
components. Given an initial and goal configuration,
5.1 Implementation we combine the roadmap and query phase into one step
so that each query adds new connectivity information
We have implemented a preliminary version of the to the roadmap. The planner builds the roadmap by
planning algorithm for 2D (3-dof) and 3D (6-dof) en- iteratively growing from initial configuration C; and
vironments on an SGI IRIX workstation. The planner the goal configuration C,. We use a growth strategy
proceeds in distinct stages: Pre-processing, roadmap similar to the one highlighted in [15, 13]. To grow a
construction, and path planning query. component c of the roadmap, the algorithm randomly
In the pre-processing phase, the planner computes selects a node N, in c to expand. N, is chosen from
information about the environment. First, it computes the set of previously generated samples in c, giving pri-
the discretized Voronoi diagram. With each voxel of ority to those near the medial axis, and the ones in the
the 3D image, we store information about the closest low density areas. The main goal is to bias the planner
obstacle and the distance computed using the depth towards the nodes whose translational component lies
286 C. Pisula, K. Hoff III, M. C. Lin, and D. Manocha

2180 (Benchmark 6) : :
10900 2.0
Table 1: The cost of computing the discretized Voronoi diagram in seconds as a function of the resolution. The table lists
the number of triangles against Voronoi diagram resolution. All timings are computed on a SGI Infinite Reality (Onyx2).
Each represents a different grid size (16 x 16 x 16 to 512 x 512 x 512).

close to the medial axis and to select new configura- — Scenario (b): Type-R narrow passage, robot
tions in free space. dimension of 4.0 x 40.0.
To expand a node, we generate a number of random — Size of environment : 400 x 400
configurations N, in the neighborhood of N,. using a — Width of narrow passage: 20
Gaussian distribution. The size of the neighborhood — Number of line segments : 158
is defined by the distance to the nearest obstacle and
e Benchmark 2: A 2D environment requiring the
is computed using the distance information associated
with each voxel. After that we check if NV is in the free robot to traverse a long Type-T narrow passage
space by performing a collision query. We try to insert resembling a maze (Image 2).
free configurations into the component c by checking — Size of environment : 400 x 400
for a valid path from N. to Ne using the local plan- — Width of narrow passage: 15
ner. Finally, after each component has completed its
— Number of triangles : 1044
expansion stage, an attempt is made to connect the un-
connected components using local planning algorithms. — Robot dimensions : 10 x 10
This process is repeated until a path has been found e Benchmark 3: A complex 2D environment con-
between the initial and the goal configuration. sisting of chairs, pianos and a music stand, each
projected into the XY plane (Image 3). The robot
5.2 Benchmark Environments (a music stand) must navigate a Type-T narrow
passage.
We have tested the performance of the planner on sev-
eral 2D and 3D benchmarks. We also compared its — Size of environment : 15.5 x 17.5
performance with Stanford PRM developed by David — Number of triangles : 12054
Hsu, J. Latombe et al. [13]. It uses uniform sampling in — Robot dimensions : 1.0 x 1.0
the configuration space to generate the configurations.
e Benchmark 4: A 3D environment with two
We have chosen a suite of environments and scenarios
open areas connected by a single channel (Image
to test the effectiveness of our sampling scheme over ba-
sic uniform sampling. Most of them have either Type-T
4(a-c)). The robot (a block) must move from the
narrow passage or Type-R narrow passage. The set of initial configuration to the goal by traversing a
narrow tunnel.
benchmarks used include:
— Scenario (a): Type-T narrow passage, robot
e Benchmark 1: A 2D environment requiring the dimension of .025 x .025 x .025.
robot to navigate a narrow passage to move from — Scenario (b): Type-T narrow passage, robot
the open area on the left to the open area on the dimension of .08 x .08 x .08.
right (Image l(a-b)). The environment consists — Scenario (c): Type-R narrow passage, robot
of two wide open areas connected by one narrow dimension of .025 x .025 x .125.
channel.
— Size of environment: 1.0 x 1.0 x 1.0
— Scenario (a): Type-T narrow passage, robot — Width of narrow passage: 0.1 x 0.1 x 0.1
dimension of 2.5 x 14.0. — Number of triangles: 28
Randomized Path Planning for a Rigid Body Based on Hardware Accelera
ted Voronoi Sampling 87
i)

C-Space Samples [Free Samples |Pull Coll. Checks |QR Goll, |Connect Comp. Calls
Pi(a)___| Voronoi Based |1.50] 5062 Baldeeeo O ragTTe]leas aol 2655585
1239 ae 21S 0D al Re 36447
30.22 14843 BAgoT | See DOTSCN] La Oa 2360384
20.47 5207 3365 |
=a TY 201654
Table 2: Benchmark 1, environment shown in Image 1. The size of the robot varies so that in
(a) No rotation is required.
(b) Rotation is required.

e Benchmark 5: <A 3D environment similar to e Sampling Type—We have compared our Voronoi
benchmark 4, except that the passage is not a Based sampling to Stanford’s planner that uses
simple straight channel. The channel “spirals” Uniform sampling [13]. The basic structure of the
through space. The tunnel itself provides a nar- implementations are the same. The main differ-
row passage in the workspace. The sharp corners ence is in the sampling strategy.
further complicate planning by causing a Type-R
narrow passage (Image 5). e Time—The running time to service the benchmark
query. The time to compute the Vornoi diagram
— Scenario (a): Type-R narrow passage, robot is not included, but is listed in a separate table.
dimension of .02 x .02 x .125. But it includes the time to build the roadmap.
— Scenario (b): Type-R narrow passage, robot
dimension of .07 x .07 x .07. e C-Space Sample—The total number of random
C-Space configuration generated by the planner.
— Size of environment : 1.0 x 1.0 x 1.0
This number represents all configurations, even
— Width of narrow passage: 0.1 x 0.1 x 0.1 those in contact space, and those that are in free
— Number of triangles : 46 space that fail to connect to a node in the road
map.
e Benchmark 6:

A 3D environment consisting of chairs, a table, e Free Samples—The total number of samples which
and a piano (Image lie in free space and successfully become part of the
roadmap.
6). The goal is to move the piano through the
window. The e Full Coll. Checks—The number of times a full
environment provides Type-R narrow passage, as PQP collision query was performed.
the piano must e QR Coll.—The number of times a full collision
rotate to fit through the window. This benchmark query was avoided, by doing a quick rejection.
has been provided to us by Jean-Paul Laumond at
e Connect Comp. Calls—The number of times the
LAAS, Toulouse.
planner attempted to connect two nodes from dis-
— Size of environment : 6000 x 6000 x 3000 tinct components.
— Dimension of narrow passage: 2000 x 1000 (W
x H) 6 Analysis and Comparison
— Robot dimension (with legs): 1000 x 1210 x
850 Overall the performance of our planning algorithm
varies with the environment. In all our current bench-
— Robot dimension (sans legs): 1000 x 1200 x
marks, it significantly performs uniform sampling in
350
Type-T narrow passages. It also improves the perfor-
mance of the planner when there are Type-R narrow
5.3 Performance Data
passages in the configuration space. In all our bench-
We highlight the performance of the planner on dif mark comparisons, we try to make minimal changes to
ferent benchmarks. The different columns in the table the connection strategies. Based on the medial axis,
used are: we have also proposed a novel multi-stage strategy to
288 C. Pisula, K. Hoff III, M. C. Lin, and D. Manocha

build the roadmap. It leads to considerable improve- have applied it to a number of 2D and 3D benchmarks
ment in the performance. and the preliminary results are promising.

The idea of using the medial axis for sampling is not Next, we would like to investigate the theoretical
novel. Other authors have proposed using medial axis analysis of “Voronoi skeleton” or medial axis for Type-
based sampling [27, 26, 9]. The major benefit in our R narrow passages and develop more rigorous compu-
approach comes from the fact that we have bounded tational techniques using graphics hardware. It will
error approximation of the medial axis computed us- be useful to conduct more extensive performance com-
ing graphics hardware. Wilmarth et al. [27, 26] do not parisons between our approach accelerated by graph-
compute a medial axis. They take random configura- ics hardware against other PRMs based on medial-axis
tions and retract them to the medial using iterative ap- sampling [27, 26, 9]. We would like to further extend
proaches. However, they either cannot guarantee suf- our approach to motion planning with constraints and
ficient number of samples in all the narrow passages, planning of articulated objects or manipulators.
or it will take many more random configurations (fol-
lowed by retraction) to generate sufficient samples in Acknowledgments
some of the challenging scenarios.
We are grateful to David Hsu and Jean-Claude
Guibas et al. [9] take point samples on the boundary Latombe for a version of their planner and many
and compute their Voronoi diagram to estimate sam-
useful discussions related to this problem. We are
ples on the medial axis of the workspace. Again, in
also grateful to Jean-Paul Laumond for providing us
this case, it is difficult to guarantee any bounds on the with the model of Benchmark 6. Supported in part
medial axis or quality of the resulting samples.
by ARO Contract DAAH04-96-1-0257, NSF Career
Overall, having a bounded error approximation of Award CCR-9625217, NSF grants EIA-9806027 and
the medial axis of the workspace, along with the dis- DMI-9900157, ONR Young Investigator Award, DOE
tance information computed using the depth buffer, ASCI Contract and Intel.
helps us in identifying the narrow passages and charac-
terizing them in many cases. Our proposed algorithms References
seem to perform well in all the benchmark cases we
have tried. Clearly, there is no one universal sampling [1] N. Amato, O. Bayazit, L. Dale, C. Jones, and
strategy that will work well in all cases. Therefore, we D. Vallejo. Obprm: An obstacle-base prm for 3d
workspaces. Proceedings of WAFR98, pages 197-204,
are continuing to experiment with various benchmark 1998.
examples to find the worst case scenarios, as well as in-
vestigating a theoretical analysis that relates a medial [2] H. Blum. A transformation for extracting new de-
axis in a workspace to a Voronoi roadmap in the corre- scriptors of shape. In W. Whathen-Dunn, editor,
Models for the Perception of Speech and Visual Form,
sponding configuration space. We believe that a formal pages 362-380. MIT Press, 1967.
analysis and characterization will give us a more rig-
orous method to distinguish between different types of [3pathyJ. Canny and B. R. Donald. Simplified Voronoi di-
narrow passages and enable us to intelligently choose agrams. In Proc. 3rd Annu. ACM Sympos. Comput.
Geom., pages 153-161, 1987.
a combination of sampling strategies for adapting to
different regions of the environment. [4] H. Chang and T. Li. Assembly maintainability study
with motion planning. In Proceedings of Internationa
Conference on Robotics and Automation, 1995.
7 Conclusion
[5is H. Choset. Nonsmooth analysis, convex analysis and
their applications to motion planning. International
We have presented techniques to improve the perfor- Journal of Computational Geometry and Applica-
mance of PRM in configurations with narrow passages. tions, 1997.
We use a bounded error approximation of the general-
[6— H. Choset, I. Konukseven, and A. Rizzi. Sensor based
ized Voronoi diagram to improve the sampling and es-
planning: Using a honing strategy and local map
timate narrow passages in the free space. It is also used method to implement the generalize voronoi graph.
to improve the performance of proximity queries. We SPIE Mobile Robotics, 1997.
Randomized Path Planning for a Rigid Body Based on Hardware Accelera
ted Voronoi Sampling 289

[7] P. W. Finn, L. E. Kavraki, J. C. Latombe, R. Mot-

[17] E. Larsen, S. Gottschalk, M. Lin, and D. Manocha.
wani, C. Shelton, S. Venkatasubramanian, and A. Fast proximity queries with swept sphere volumes.
Yao. Rpid: Randomized pharmacophore identifica- Technical Report TR99-018, Department of Com-
tion for drug design. Proc. of 13th ACM Symp. on puter Science, University of North Carolina, 1999.
Computational Geometry (SoCG’97), 1997. A revised
version of this paper also appeared in Computational
Geometry: Theory and Applications, 10, pp. 263- [18] J. C. Latombe. Robot Motion Planning. Kluwer Aca-
demic Publishers, 1991.
272, 1998.

S. Gottschalk, M. Lin, and D. Manoch. Obb-tree: [19] D. Lavender, A. Bowyer, J. Davenport, A. Wallis,
A hierarchical structure for rapid interference detec- and J. Woodwark. Voronoi diagrams of set-theoretic
tion. In Proc. of ACM Siggraph ’96 pages 171-180, solid models. IEEE Computer Graphics and Applica-
1996. tions, pages 69-77, September 1992.

[9] L. Guibas, C. Holleman, and L. Kavraki. A proba-

[20] C. O’Dunlaing, M. Sharir, and C. K. Yap. Retraction:
bilistic roadmap planner for flexible objects with a
A new approach to motion-planning. In Proc. 15th
workspace medial-axis-based sampling approach. In
Annu. ACM Sympos. Theory Comput., pages 207-
Proc. of IROS, 1999.
220, 1983.
[10] K. Hoff, T. Culver, J. Keyser, M. Lin, and D.
Manocha. Fast computation of generalized voronoi [21] M. H. Overmars and P Svestka. A probabilistic learn-
diagrams using graphics hardware. Proceedings of ing approach to motion planning. In Algorithmic
ACM SIGGRAPH 1999, 1999. Foundations of Robotics, Boston, MA 1995. A. K.
Peters.
[11] K. Hoff. T. Culver, J. Keyser, M. Lin, and D.
Manocha. Interactive motion planning using hard-
ware accelerated comutation of generalized voronoi
[22] A. Schweikard, R. Tombropoulos, L. E. Kavraki, J.
R. Adler, and J. C. Latombe. Treatment planning
diagrams. Techincal report, Department of Com- for a radiosurgical system with general kinematics.
puter Science, University of North Carolina, 1999. IEEE Confrence on Robotics and Automation, 1994.
To appear in INEE Conference on Robotics and Au-
tomation.
[23] D. J. Sheehy, C. G. Armstrong, and D. J. Robinson.
T. Horsch, F. Schwarz, and H. Tolle. Motion planning Computing the medial surface of a solid from a do-
— bopeta
—

for many degrees of freedon-random reflections at c- main delaunay triangulatio. In Chris Hoffman and
space obstacles. Proc. of IEEE International Conf. Jarek Rossignac, editors, Solid Modeling ’95, pages
on Robotics and Automation, pages 3318-3323, 1994. 201-212, May 1995.

D. Hsu, L. Kavraki, J. Latombe, R. Motwani, and

S. Sorkin. On finding narrow passages with proba-
[24] R. Tombropouloas, J. R. Adler, and J. C. Latombe.
Carabeamer. A treatment planner for a robotic ra-
bilistic roadmap planners. Proc. of 8rd Workshop on diosurgical system with general kinematics. Medical
Algoricthmic Foundations of Robotics, 1998. Image Analysis, pages 3(3):1-28, 1999.

[14] L. Kavraki and J. C. Latombe. Randomized pre-

processing of configuration space for fast path plan- [25] J. Vleugels and M. Overmars. Approximating
ning. IEEE Conference on Robotics and Automation, voronoi diagrams of convex sites in any dimension.
International Journal of Computational Geometry
pages 2138-2145, 1994.
and Applications, 8:201-222, 1997.
[15] L. Kavraki, P. Svestka, J. C. Latombe, and M.
Overmars. Probabilistic roadmaps for path planning [26] Steven A. Wilmarth, Nancy M. Amato, and Peter
in high-dimensional configuration spaces. em IEEE F. Stiller. Maprm: Aprobabilisitc roadmap planner
Trans. Robot. Automat., pages 12(4):566-580, 1996. with sampling on the medial axis of the free space.
IEEE Conference on Robotics and Automation, 1999.
[16] Yoshito Koga, Koichi Kondo, James Kuffner, and
Jean-Claude Latombe. Planning motions with in
[27] Steven A. Wilmarth, Nancy M. Amato, and Peter F.
tentions. In Andrew Glassner, editor, Proceedings
Stiller. Motion planning for a rigid body using ran-
of SIGGRAPH ’94 (Orlando, Flordia, July 24—-
dom networks on the medial axis of the free space.
29, 1994), Computer Graphics Proceedings, Annual
Proc. of the 15th Annual ACM Symposium on Com-
Confrence Series, pages 395-408. ACM SIGGRAPH, putational Geometry (SoCG’99), 1999.
ACM Press, July 1994. ISBN 0-89791-667-0.
290 C. Pisula, K. Hoff III, M. C. Lin, and D. Manocha

C-Space Samples Full Coll. Checks [|QR Coll. | Connect Comp. Calls
625.67 50692 3479
1393
meres
86902
|iO
78251
2423896
308528
Voronoi Based | 213.89 10018

Table 3: Benchmark 2, maze environment shown in Image 2. The narrow passage is long with respect to the robot
dimensions.

C-Space Samples Full Coll. Checks [|QR Coll. | Connect Comp. Calls
36631 T7a12 Cr a RN EM 04032
23680 8739 33646 24907 54986
Table 4: Benchmark 3, house environment shown in Image 3. Notice that the Uniform based sampling does not even
complete in the allocated time.

C-Space Samples Full Coll. Checks | QR Coll. | Connect Comp. Calls

1560 714 ie at 375736
eee Biased 3098 2008 T3685
ay |Uniform ~*~ eonno +] ieeiso_ | above «|=D | Oe | meee |
Pg vaio | iai7a.0eF | sis0ia | eas aaa ee reno

Table 5: Benchmark 4, a simple tunnel shown in Image 4. The size of the robot varies so that in (a) The robot is small,
and no rotation is required. (b) Rotation is required. (c) The robot is large, but no rotation is required. Notice that in (c)
the Uniform based sampling does not even complete in the allocated time.

C-Space Samples Full Coll. Checks | QR Coll. | Connect Comp. Calls

296.65 1675 T0985 gered | 0 | 2008713
Voronoi Based
[Ga 5) Unlom — ul loss | vasses | 190357) Jip meal wns Sotelo) maaan |
ULL GN EE GSN Ge

Table 6: Benchmark 5, spiral tunnel shown in Image 5. In (a) and (b) the robot size varies. In (c) we compute the path
with our boundary graph approach. (a) No rotation is required. (b) Rotation is required. (c) The same scenario as in (a)
with the different connection scheme that takes advantage of medial representations. We observe substantial performance
improvement by using a better connection strategy that exploits the Voronoi graph structures.

C-Space Samples Full Coll. Checks

coUniforn | 308.86
| QR Coll. | Connect Comp. Calls
[6]
Voronoi Based | 2983.24 58265
13542
38098
250043
220023
|010821
| apa 048
351247399

Table 7: Benchmark 6, piano environment shown in Image 6. A very challenging Type-R narrow passage still shows 25%
speedup.
 i zed Path Planning Ay d Body Based on Hardware Accelerated Voronoi Sampling
cen for poe a Rigi 291
and
Ran dom
omi

Image 2: The maze, Benchmark 2.

through workspace.
nt, (b) computed voronoi surface, (Cc) extracted core
Image 4: Benchmark 4 showing (a) planning environme

ark 6. Red
5. Image 6: Piano Environment, Benchm
Image 5: A simple tunnel, Benchmark shows the initial position. The Green
position shows an R Narrow Passage.
 ny

fb isirwpuN rigors) sametrayt ay eens

is
a

4
ik lee
g 7 “ad

«sit ‘ “a 7
oa 7
iy ue
‘ of i
4 2.

he
Ps
ess

i—

aim ’
ie! et <oy meh veg hy a of,
eal eel lng,
1 prvadttiay a ow

oo nh emp [Pam 5

F rest Meday ante

,
Rapidly-Exploring Random Trees: Progress and Prospects
Steven M. LaValle, Jowa State University, Ames, IA
James J. Kuffner, Jr., University of Tokyo, Tokyo, Japan

We present our current progress on the design and ical prototypes. Motion planning techniques have al-
analysis of path planning algorithms based on Rapidly- ready been applied to assembly problems in this area
exploring Random Trees (RRTs). The basis for our [9]. As the power and generality of planning techniques
methods is the incremental construction of search trees increase, we expect that more complicated problems
that attempt to rapidly and uniformly explore the state that include differential constraints can be solved, such
space, offering benefits that are similar to those ob- as the evaluation of vehicle performance and safety
tained by other successful randomized planning meth- through dynamical simulation conducted by a virtual
ods. In addition, RRTs are particularly suited for prob- “stunt driver.”
lems that involve differential constraints. Basic the-
As we approach applications of increasing difficulty,
oretical properties of RRT-based planners are estab-
it becomes clear that planning algorithms need to han-
lished. Several planners based on RRTs are discussed
dle problems that involve a wide variety of models, high
and compared. Experimental results are presented for
degrees of freedom, complicated geometric constraints,
planning problems that involve holonomic constraints
and finally, differential constraints. Although exist-
for rigid and articulated bodies, manipulation, nonholo-
ing algorithms address some of these concerns, there
nomic constraints, kinodynamic constraints, kinematic
is relatively little work that addresses all of them si-
closure constraints, and up to twelve degrees of free-
multaneously. This provides the basis for the work
dom. Key open issues and areas of future research are
presented in this paper, which presents randomized,
also discussed.
algorithmic techniques for path planning that are par-
ticularly suited for problems that involve differential
1 Introduction constraints.

Given the vast, growing collection of applications that We present an overview of the progress on our de-
involve the design of motion strategies, the successes of velopment of Rapidly-exploring Random Trees (RRTs)
motion planning algorithms have just begun to scratch [29]. The results and discussion presented here summa-
its surface. The potential for automating motions is rize and extend the work presented in [30, 22]. In [30],
now greater than ever as similar problems continue we presented the first randomized approach to kinody-
to emerge in seemingly disparate areas. The tradi- namic trajectory planning, and applied it to problems
tional needs of roboticists continue to expand in efforts that involve up to twelve-dimensional state spaces with
to automate mobile robots, manipulators, humanoids, nonlinear dynamics and obstacles (for problems with
spacecraft, etc. Researchers in computer graphics and moving obstacles, a similar randomized approach has
virtual reality have increasing interests in automating been proposed more recently, in [20]). In [22], we pre-
the animations of life-like characters or other moving sented and analyzed a holonomic path planner that
bodies. In the growing field of computational biology, gives real-time performance for many challenging prob-
many geometric problems have arisen, such as study- lems. RRTs build on ideas from optimal control theory
ing the configuration spaces of flexible molecules for [5], nonholonomic planning (see [27] for an overview),
protein-ligand docking and drug design. Virtual pro- and randomized path planning [1, 19, 36]. The ba-
totyping is a rapidly-expanding area that allows the sic idea is to use control-theoretic representations, and
evaluation of proposed mechanical designs in simula- incrementally grow a search tree from an initial state
tion, in efforts to avoid the costs of constructing phys- by applying control inputs over short time intervals to
294 S. M. LaValle and J. J. Kuffner

reach new states. Each vertex in the tree represents a choice of a good heuristic potential function, which
state, and each directed edge represents an input that could become a daunting task when confronted with
was applied to reach the new state from a previous obstacles, and differential constraints. In the prob-
state. When a vertex reaches a desired goal region, abilistic roadmap approach [1, 19], a graph is con-
an open-loop trajectory from the initial state is repre- structed in the configuration space by generating ran-
sented. dom configurations and attempting to connect pairs
of nearby configurations with a local planner that will
For problems that involve low degrees of freedom,
connect pairs of configurations. The assumption is that
classical dynamic programming ideas can be employed
the same roadmap will be used for multiple queries. For
to yield numerical optimal control solutions for a broad
planning of holonomic systems or steerable nonholo-
class of problems [5, 24, 28]. Since control theorists
have traditionally preferred feedback solutions, the rep-
nomic systems (see [27] and references therein), the lo-
cal planning step might be efficient; however, in general
resentation takes the form of a mesh over which cost-
to-go values are defined using interpolation, enabling
the connection problem can be as difficult as design-
ing a nonlinear controller, particularly for complicated
inputs to be selected over any portion of the state
space. If open-loop solutions are the only require- nonholonomic and dynamical systems. The probabilis-
ment, then each cell in the mesh could be replaced tic roadmap technique might require the connections
by a vertex that represents a single state within the of thousands of configurations or states to find a so-
cell. In this case, control-theoretic numerical dynamic lution, and if each connection is akin to a nonlinear
programming techniques can often be reduced to the control problem, it seems impractical many problems
construction of a tree grown from an initial state (re- with differential constraints.
ferred to as forward dynamic programming [24]). This
idea has been proposed in path planning literature for 2 Problem Formulation
nonholonomic [4] planning and kinodynamic planning
in [13]. Because these methods are based on dynamic The class of problems considered in this paper can be
programming and systematic exploration of a grid or formulated in terms of six components:
mesh, their application is limited to problems with low
degrees of freedom. 1. State Space: A topological space, X
We would like to borrow some of the ideas from
2. Boundary Values: Zjnjz¢ € X and Xgoai C X
numerical optimal control techniques, while weaken-
ing the requirements enough to obtain methods that 3. Collision Detector: A function, D: X 73
can apply to problems with high degrees of freedom. {true, false}, that determines whether global con-
As is common in most of path planning research, we straints are satisfied from state z. This could be
forego trying to obtain optimal solutions, and attempt a binary-valued or real-valued function.
to find solutions that are “good enough,” as long as
they satisfy all of the constraints. This avoids the use 4. Inputs: A set, U, which specifies the complete set
of dynamic programming and systematic exploration of controls or actions that can affect the state.
of the space; however, a method is needed to guide the
5. Incremental Simulator: Given the current
search in place of dynamic programming.
state, x(t), and inputs applied over a time interval,
Inspired by the success of randomized path planning {u(t’)|t < t’ < t+ At}, compute «(t+ At).
techniques and Monte-Carlo techniques in general for
addressing high-dimensional problems, it is natural to 6. Metric: A real-valued function, p : X x X >
consider adapting existing planning techniques to our (0,00), which specifies the distance between pairs
problems of interest. The primary difficulty with many of points in X.
existing techniques is that, although powerful for stan-
dard path planning, they do not naturally extend to Path planning will generally be viewed as a search
general problems that involve differential constraints. in a state space, X, for a continuous path from an ini-
The randomized potential field method [3], while effi- tial state, init to a goal region Juapal ta) ae Or POal
cient for holonomic planning, depends heavily on the state Lgoai € X. It is assumed that a complicated
Rapidly-Exploring Random Trees
295
set of global constraints is imposed on X , and any so- contexts such as wheeled-robot systems [27], and ma-
lution path must keep the state within this set. A nipulation by pushing [35]. A recent survey appears in
collision detector reports whether a given state, x, sat- [27]. The constraints often appear in the implicit form
isfies the global constraints. We generally use the no- hi(4,q) = 0 for somei from 1 to k < N (N is the dimen-
tation Xf,ee to refer to the set of all states that satisfy sion of C). By the implicit function theorem, the con-
the global constraints. Local, differential constraints straints can also be expressed in control-theoretic form,
are imposed through the definition of a set of inputs q = f(q,u), in which wu is an input chosen from a set of
(or controls) and an incremental simulator. Taken to- inputs U. Using our general notion, x replaces g to ob-
gether, these two components specify possible changes tain ¢ = f(x,u). This form is often referred to as the
in state. The incremental simulator can be consid- state transition equation or equation of motion. Using
ered as the response of a discrete-time system (or a the state transition equation, an incremental simulator
continuous-time system that is approximated in dis- can be constructed by numerical integration (using, for
crete time). Finally, a metric is defined to indicate example Runge-Kutta techniques).
the closeness of pairs of points in the state space. This
metric will be used in Section 3, when the RRT is intro- Kinodynamic’ Path Planning For kinodynamic
duced. It will generally be assumed that a single path planning, constraints on both velocity and acceler-
planning query is presented, as opposed to performing ation exist, yielding implicit equations of the form
precomputation for multiple queries, as in [1, 19]. hi(G,4,¢) = 0 (6, 8, 10, 11, 13, 12, 14, 45, 46]. A
state, c € X, is defined as x = (q,q), forg € C. Us-
Basic (Holonomic) Path Planning Path planning ing the state space representation, this can be simply
can generally be viewed as a search in a configura- written as a set of m implicit equations of the form
tion space, C, in which each q € C specifies the po- Gis) ="0) for =o Lee ald ie eeete
sition and orientation of one or more geometrically- implicit function theorem can again be applied to ob-
complicated bodies in a 2D or 3D world [33, 25]. The tain a state transition equation. The collision detection
path planning task is to compute a continuous path component may also include global constraints on the
from an initial configuration, ginit, to a goal configura- velocity, since ¢ is part of the state vector.
tion, Qgoal- Thus, X = C, init = init, Lgoal = Ygoals
and Xfree = Cfree, which denotes the set of config- Other Problems A variety of other problems fit
urations for which these bodies do not collide with within our problem formulation, and can be ap-
any static obstacles in the world. The obstacles are proached using the techniques in this paper. In gen-
modeled completely in the world, but an explicit rep- eral, any open-loop trajectory design problem can for-
resentation of Xfree is not available. However, using mulated because the models are mostly borrowed from
a collision detection algorithm, a given configuration control theory. For example, the planner might be used
can be tested. (To be more precise, we usually em- to compute a strategy that controls an electrical circuit,
ploy a distance-computation algorithm that indicates or an economic system. In some applications, a state
how close the geometric bodies are to violating the con- transition equation might not be known, but this does
straints in the world. This can be used to ensure that not present a problem. For example, a physical simu-
intermediate configurations are collision free when dis- lator might be developed by engineers for simulating a
crete jumps are made by the incremental simulator.) proposed racing car design. The software might sim-
The set, U, of inputs is the set of all velocities £ such ply accept control inputs at some sampling rate, and
that ||z|| < c for some positive constant c. The incre- produce new states. This could serve directly as the
mental simulator produces a new state by integration incremental simulator for our approach. Other minor
to obtain, fnew = z + uAt, for any given input u € U. variations of the formulation can be considered. Time-
varying problems can be formulated by augmenting the
Nonholonomic Path Planning Nonholonomic
1Tn nonlinear control literature, kinodynamic planning
planning [26] addresses problems that involve nonin- for underactuated systems is encompassed by the definition
tegrable constraints on the state velocities, in addition of nonholonomic planning. Using control-theoretic termi-
to the components that appear in the basic path plan- nology, the task is to design open-loop trajectories for non-
linear systems with drift.
ning problems. These constraints often arise in many
296 S. M. LaValle and J. J. Kuffner

state space with a time. State-dependent inputs sets BUILD-_RRT(zinit)

can also be considered. For example, a robot engaged il T..inht( tence);
Die tori =) etoniado
in a grasping task might have different inputs avail-
able than while navigating. Depending on the state, 3 Lrand < RANDOM-STATE();
4 EXTEND(), trana);
different decisions would have to be made. Problems 5 Return 7
that involve kinematic closure constraints can also be
addressed; an example is shown in Figure 14. EXTEND(7,2)
1 Inear < NEAREST_NEIGHBOR(z, 7);
3 Rapidly-Exploring Random Trees 2 if NEW_STATE(2; tncari@nee) tren) then
3 T .add_vertex(Inew)}
The Rapidly-exploring Random Tree (RRT) was in- 4 Tad
Loge Prnaey tlmetiy tncwds
troduced in [29] as a planning algorithm to quickly 5 it tne =o then
6 Return Reached;
search high-dimensional spaces that have both alge- 7 else
braic constraints (arising from obstacles) and differen- 8 Return Advanced;
tial constraints (arising from nonholonomy and dynam- 9 Return Trapped;
ics). The key idea is to bias the exploration toward un-
explored portions of the space by sampling points in the Figure 1: The basic RRT construction algorithm.
state space, and incrementally “pulling” the search tree
toward them. At least two other randomized path plan-
easily by a simple vector calculation. NEW_STATE
ning techniques have been proposed that generate an
also implicitly uses the collision detection function to
incremental search tree in the configuration space (for
determine whether the new state (and all intermediate
holonomic path planning): The Ariadne’s clew algo-
states) satisfy the global constraints. For many prob-
rithm [37, 36] and the planners in [18, 50]. Intuitively,
lems, this can be performed quickly (“almost constant
these planners attempt to “push” the search tree away
time”) using incremental distance computation algo-
from previously-constructed vertices, contrasting the
rithms [16, 31, 38] by storing the relevant invariants
RRT, which uses the surrounding space to “pull” the
with each of the RRT vertices. If NEW_STATE is suc-
search tree, ultimately leading to uniform coverage of
cessful, the new state and input are represented in 2new
the state space. To the best of our knowledge, a ran-
and Unew, respectively. Three situations can occur:
domized search tree approach has not been proposed
Reached, in which the new vertex reaches the sample x
previously for nonholonomic or kinodynamic planning.
(for the nonholonomic planning case, we might instead
Perhaps the most related approaches are [47, 44], in
have a threshold, ||%new —2|| < € for a small e« > 0); Ad-
which the probabilistic roadmap method is combined
vanced, in which a new vertex Znew # x is added to the
with nonholonomic steering techniques to plan paths
RRT; Trapped, in which NEW_STATE fails to produce
for wheeled mobile robot systems.
a state that lies in Xfree. The left column of Figure
The basic RRT construction algorithm is given in 3 shows an RRT for a holonomic planning problem,
Figure 1. A simple iteration in performed in which constructed in a 2D square space. The right column
each step attempts to extend the RRT by adding a new shows the Voronoi diagram of the RRT vertices; note
vertex that is biased by a randomly-selected state. The that the probability that a vertex is selected for exten-
EXTEND function, illustrated in Figure 2, selects the sion is proportional to the area of its Voronoi region.
nearest vertex already in the RRT to the given sample
state. The “nearest” vertex is chosen according to the
metric, p. The function NEW_STATE makes a motion
toward x by applying an input u € U for some time
increment At. This input can be chosen at random, or
selected by trying all possible inputs and choosing the
one that yields a new state as close as possible to the
sample, x (if U is infinite, then an approximation or
analytical technique can be used). In the case of holo-
nomic planning, the optimal value for wu can be chosen Figure 2: The EXTEND operation.
Rapidly-Exploring Random Trees
297

siderable effort remains to close the gap between our

experimental success, and the analysis that supports
the success.

The limiting distribution of vertices Let D;(z)

denote a random variable whose value is the distance
of x to the closest vertex in G, in which k is the number
of vertices in an RRT. Let d, denote the value of Dx.
Let € denote the incremental distance traveled in the
EXTEND procedure (the RRT step size).
Consider the case of a holonomic planning problem,
in which ¢ = wu (the incremental simulator permits mo-
tion in any direction). The first lemma establishes that
the RRT will (converging in probability) come arbitrar-
ily close to any point in a convex space.

Lemma 1 Suppose X free is a conver, bounded, open,

n-dimensional subset of an n-dimensional state space.
For any « € Xfree and positive constant « > 0,
lim Bldg (a \k< ie) ett:
k—-oo

The proofs for all propositions, except Theorems 5-7,

appear in [22].
The next lemma extends the result from convex
spaces to nonconvex spaces.

Lemma 2 Suppose Xfree 18 a nonconvez, bounded,

open, n-dimensional connected component of an n-
dimensional state space. For any x € Xfree and posi-
tive real number € > 0, then Jim Pidg(i\=<ei=t
Figure 3: An RRT is biased by large Voronoi regions to
rapidly explore, before uniformly covering the space.
For holonomic path planning, this immediately im-
plies the following:
This biases the RRT to rapidly explore. In Section 4 it
is shown that RRTs also arrive at a uniform coverage Theorem 3 Suppose Yinit and Fgoar lie in the
of the space. same connected component of a nonconvex, bounded,
open, n-dimensional connected component of an n-
dimensional state space. The probability that an RRT
4 Analysis of RRTs
constructed from “init will find a path to Xgoa ap-
This section provides some analysis of RRTs, and in- proaches one as the number of RRT vertices approaches
dicates several open problems for future investigation. infinity.
A key result shown so far is that the distribution of
This establishes probabilistic completeness, as consid-
the RRT vertices converges to the sampling distribu- ered in [19], of the basic RRT.
tion, which is usually uniform. This currently has been
shown for holonomic planning in a nonconvex state The next step is to characterize the limiting distribu-
space. We have also verified the results through sim- tion of the RRT vertices. Let X denote a vector-valued
ulations and chi-square tests. We have generally had random variable that represents the sampling process
many experimental successes, indicated in Section 6, used to construct an RRT. This reflects the distribution
that far exceed our current analysis capabilities. Con- of samples that are returned by the RANDOMSTATE
298 S. M. LaValle and J. J. Kuffner

function in the EXTEND algorithm. Usually, X is the next step in the solution trajectory will be con-
characterized by a uniform probability density function structed. This argument is applied inductively from
over X free; however, we will allow X to be character- zx to £,, until the final state Xgoar = Le+1 is reached.
ized by any continuous probability density function. |
Let X; denote a vector-valued random variable that
represents the distribution of the RRT vertices after k Convergence Rate Theorems 6 and 7 express the
iterations. rate of converge of the planner. These results represent
a significant first step towards gaining a complete un-
Theorem 4 X;, converges to X in probability. derstanding of behavior of RRT-based planning algo-
rithms; however, the convergence rate is unfortunately
We now consider the more general case. Suppose expressed in terms of parameters that cannot be easily
that motions obtained from the incremental simulator measured. It remains an open problem to characterize
are locally constrained. For example, they might arise the convergence rate in terms of simple parameters that
by integrating ¢ = f(x, wu) over some time At. Suppose can be computed for a particular problem. This gen-
that the number of inputs to the incremental simula- eral difficulty even exists in the analysis of randomized
tor is finite, At is constant, no two RRT vertices lie path planners for the holonomic path planning problem
within a specified « > 0 of each other according to p, [18, 23].
and that EXTEND chooses the input at random. It
may be possible eventually to remove some of these re- For simplicity, assume that the planner consists of a
strictions; however, we have not yet pursued this route. single RRT. The bidirectional planner is only slightly
Suppose Liniz ANd Fgoq) lie in the same connected com- better in terms of our analysis, and a single RRT is
ponent of a nonconvex, bounded, open, n-dimensional easier to analyze. Furthermore, assume that the step
connected component of an n-dimensional state space. size is large enough so that the planner always attempts
In addition, there exists a sequence of inputs, uj, U2, COCONNCCESL earl OC rand:

++) Up, that when applied to xiniz yield a sequence of Let A = {Ap, Ai,..., Ax} be a sequence of subsets
Biaes Tint: — 20, Lis C2 seed: = Caeai- Al of of X, referred to as an attraction sequence. Let Ap =
these states lie in the same open connected component {init}. The remaining sets must be chosen with the
of X free: following rules. For each A; in A, there exists a basin,
The following establishes the probabilistic complete- B; © X such that the following hold:
ness of the nonholonomic planner.
1. For all « € Aji, y € Aj, and z €\X\ B;, the
Theorem 5 The probability that the RRT initialized metric, p, yields p(x,y) < p(z, z).
at Linit will contain Lgoa1 as a verter approaches one
2. For all x € B;, there exists an m such that the
as the number of vertices approaches infinity.
sequence of inputs {w1, U2,..., Um} selected by the
Overview of Proof: The argument proceeds by EXTEND algorithm will bring the state into A; C
induction on 7. Assume that the RRT contains z; B;.
as a vertex after some finite number of iterations.
Consider the Voronoi diagram associated with the Finally, it is assumed that Ag = Xgoa.
RRT vertices. There exists a positive real number, c;, Each basin B; can intuitively be considered as both
such that p(Vor(a;)) > c, in which Vor(z;) denotes a safety zone that ensures an element of B; will be
the Voronoi region associated with z;. If a random selected by the nearest neighbor query, and a poten-
sample falls within Vor(z;), the vertex will be selected tial well that attracts the state into A;. An attrac-
for extension, and a random input is applied; thus, z; tion sequence should be chosen with each A; as large
has probability (Vor(«;))/w(X free) of being selected. as possible and with k as small as possible. If the
There exists a second positive real number, cz (which space contains narrow corridors, then the attraction
depends on c;), such that the probability that the cor- sequence will be longer and each A; will be smaller.
rect input, u;, is selected is at least co. If both x; and Our analysis indicates that the planning performance
u; have probability of at least co of being selected in will significantly degrade in this case, which is con-
each iteration, then the probability tends to one that sistent with analysis results obtained for randomized
Rapidly-Exploring Random Trees
299
holonomic planners [17]. Note that for kinodynamic Theorem 7 /f an attraction sequence of length k ea-
planning, the choice of metric, p, can also greatly af- ists, for a constant 6 € (0,1), the probability that
fect the attraction sequence, and ultimately the perfor- the RRT finds a path after n iterations is at least
mance of the algorithm. 1 — exp(—npé?/2), in which 6 =1—k/(np).
Using y to represent measure, let p be defined as:
Proof: The random variable C from the proof of The-
DS min{1(Aj)/H(X free) }, orem 6 has a binomial distribution, which enables the
application of a Chernoff-type bound on its tail proba-
which corresponds to a lower bound on the probability bilities. The following theorem [39] is directly applied
that a random state will lie in a particular A;. to establish the theorem. If C is binomially distrib-
The following theorem characterizes the expected uted, 6 € (0, 1], and » = E[C], then P[C < (1—6)p] <
number of iterations required to find a solution. exp(6?/2). i
Theorem 6 If a connection sequence of length k ex-
An RRT in a Large Disc In the limit as the num-
ists, then the expected number of iterations required to
ber of iterations approaches infinity, the RRT becomes
connect Ginit to Qgoat is no more than k/p.
uniformly distributed, but what happens when the
Proof: If an RRT vertex lies in A;_1, and a random RRT is placed in a “large” space? Intuitively, it seems
sample, 2, falls in A;, then the RRT will be connected that the best strategy would be to grow the tree away
to z. This is true because using the first property in from the initial state as quickly as possible. To deter-
the definition of a basin, it follows that one of the ver- mine whether this occurs, we performed many simu-
tices in B; must be selected for extension. Using the lation experiments (each with hundreds of thousands
second property of the basin, inputs will be chosen that of iterations) to characterize how an RRT grows in
ultimately generate a vertex in A;. the limit case of a disc with a radius that approaches
infinity. Consider the case of a 2D state space and
In each iteration, the probability that the random
holonomic planning. Figure 4(a) shows a typical re-
sample lies in A; is at least p; hence, if A;_; contains
sult, in which the RRT has three major branches, each
an RRT vertex, then A; will contain a vertex with prob-
roughly 120 degrees apart. This behavior was repeat-
ability at least p. In the worst-case, the iterations can
edly observed for the 2D case, although the orientation
be considered as Bernoulli trials in which p is the prob-
of the branches is random. In higher dimensions, we
ability of a successful outcome. A path planning prob-
have observed that the RRT makes n + 1 branches in
lem is solved after k successful outcomes are obtained
an n-dimensional space. The branches also have equal
because each success extends the progress of the RRT
separation from each other (they appear to touch the
from A;_1 to A,.
vertices of a regular (n + 1)-simplex, centered at the
Let C1, C2,...,Cn be ii.d. random variables whose origin. This gives experimental evidence that in the
common distribution is the Bernoulli distribution with expected sense, the RRT grows outward from the ori-
parameter p. The random variable C = C;+C2+---+ gin at a rate that is linear in the number of iterations,
C,, denotes the number of successes after n iterations. and decreases moderately with the number of dimen-
Since each C; has the Bernoulli distribution, C’ will sions. It remains an open question to confirm these
have a binomial distribution, observations by proving the number and directions of
these branches.
fal Ag Rea Mier
Relationship to Optimality Another observation
in which k is the number of successes. The expectation that we have made through simulation experiments is
of the binomial distribution is n/p, which also repre- that the paths in a holonomic RRT, while jagged, are
sents an upper bound on the expected probability of not too far from the shortest path (recall Figure 3).
successfully finding a path. | This is not true for paths generated by a simpler tech-
nique, such as Brownian motion. For paths in the
The following theorem establishes that the probabil-
we have performed repeated experiments that
ity of success increases exponentially with the number plane,
compare the distance of randomly-cho sen RRT vertices
of iterations.
300 S. M. LaValle and J. J. Kuffner

RRT-GoalBias, can be obtained by replacing RAN-

DOM-STATE in Figure 2 with a function that tosses
a biased coin to determine what should be returned. If
the coin toss yields “heads”, then Zgoai is returned; oth-
erwise, a random state is returned. Even with a small
probability of returning heads (such as 0.05), RRT-
GoalBias usually converges to the goal much faster
than the basic RRT. If too much bias is introduced;
however, the planner begins to behave like a random-
ized potential field planner that is trapped in a local
minimum. An improvement called RRT-GoalZoom re-
places RANDOM_STATE with a decision, based on a
Figure 4: a) The conver hull of an RRT in an “infinitely”
biased coin toss, that chooses a random sample from ei-
large disc; b) a 2D RRT that was constructed using biased
ther a region around the goal or the whole state space.
sampling.
The size of the region around the goal is controlled by
the closest RRT vertex to the goal at any iteration.
to the root by following the RRT path to the Euclid- The effect is that the focus of samples gradually in-
ean distance to the root. Experiments were performed creases around the goal as the RRT draws nearer. This
in a square region in the plane. The expected ratio planner has performed quite well in practice; however,
of RRT-path distance to Euclidean distance is consis- it is still possible that performance is degraded due
tently between 1.3 and 1.7. It remains an open question to local minima. In general, it seems best to replace
to prove an upper bound on the expected path length, RANDOM-STATE with a sampling scheme that draws
with respect to optimal solutions (e.g., such as a ratio states from a nonuniform probability density function
bound of two). that has a “gradual” bias toward the goal. Figure 4(b)
shows an example of an RRT that was constructed by
5 Designing Path Planners sampling states from a probability density that assigns
equal probability to concentric circular rings. There
Sections 3 and 4 introduced the basic RRT and ana- are still many interesting research issues regarding the
lyzed its exploration properties. Now the focus is on problem of sampling. It might be possible to use some
developing path planners using RRTs. We generally of the sampling methods that were proposed to improve
consider the RRT as a building block that can be used the performance of probabilistic roadmaps [1, 7].
to construct an efficient planner, as opposed to a path
planning algorithm by itself. For example, one might One more issue to consider is the size of the step that
use an RRT to escape local minima in a randomized is used for RRT construction. This could be chosen dy-
potential field path planner [3]. In [48], an RRT was namically during execution on the basis of a distance
used as the local planner for the probabilistic roadmap computation function that is used for collision detec-
planner. We present several alternative RRT-based tion. If the bodies are far from colliding, then larger
planners in this section. The recommended choice de- steps can be taken. Aside from following this idea to
pends on several factors, such as whether differential obtain an incremental step, how far should the new
constraints exist, the type of collision detection algo- state, new appear from Lnear? Should we try to con-
rithm, or the efficiency of nearest neighbor computa- nect Lnear tO Lrana? Instead of attempting to extend
tions. an RRT by an incremental step, EXTEND can be iter-
ated until the random state or an obstacle is reached,
Single-RRT Planners In principle, the basic RRT as shown in the CONNECT algorithm description in
can be used in isolation as a path planner because its Figure 5. CONNECT can replace EXTEND, yield-
vertices will eventually cover a connected component of ing an RRT that grows very quickly, if permitted by
X free, coming arbitrarily close to any specified Zgoat. collision detection constraints and the differential con-
The problem is that without any bias toward the goal, straints. One of the key advantages of the CONNECT
convergence is often slow. An improved planner, called function is that a long path can be constructed with
Rapidly-Exploring Random Trees
301

CONNECT(7, 2) all times until they become connected and a solution

1 repeat
is found. In each iteration, one tree is extended, and
2 S < EXTEND(T7, 2);
an attempt is made to connect the nearest vertex of
3 until not (S = Advanced)
4 Return S; the other tree to the new vertex. Then, the roles are
en ee) eee reversed by swapping the two trees. Growth of two
RRTs was also proposed in [30] for kinodynamic plan-
Figure 5: The CONNECT function.
ning; however, in each iteration both trees were incre-
only a single call to the nearest-neighbor algorithm. mentally extended toward a random state. The current
algorithm attempts to grow the trees into each other
This advantage motivates the choice of a greedier al-
half of the time, which has been found to yield much
gorithm; however, if an efficient nearest-neighbor al-
better performance.
gorithm (e.g., [2]) is used, as opposed to the obvious
linear-time method, then it might make sense to be Several variations of the above planner can also be
less greedy. After performing dozens of experiments considered. Either occurrence of EXTEND may be
on a variety of problems, we have found CONNECT replaced by CONNECT in RRT_BIDIRECTIONAL.
to yield the best performance for holonomic planning Each replacement makes the operation more aggres-
problems, and EXTEND seems to be the best for non- sive. If the EXTEND in Line 4 is replaced with CON-
holonomic problems. One reason for this difference is NECT, then the planner aggressively explores the state
that CONNECT places more faith in the metric, and space, with the same tradeoffs that existed for the
for nonholonomic problems it becomes more challeng- single-RRT planner. If the EXTEND in Line 5 is re-
ing to design good metrics. placed with CONNECT, the planner aggressively at-
tempts to connect the two trees in each iteration. This
Bidirectional Planners Inspired by classical bidi- particular variant was very successful at solving holo-
rectional search techniques [41], it seems reasonable to nomic planning problems. For convenience, we refer to
expect that improved performance can be obtained by this variant as RRT-ExtCon, and the original bidirec-
growing two RRTs, one from 2;,,;, and the other from tional algorithm as RRT-ExtExt. Among the variants
Lgoal; & SOlution is found if the two RRTs meet. For a discussed thus far, we have found RRT-ExtCon to be
simple grid search, it is straightforward to implement a most successful for holonomic planning [22], and RRT-
bidirectional search; however, RRT construction must ExtExt to be best for nonholonomic problems. The
be biased to ensure that the trees meet well before cov- most aggressive planner can be constructed by replac-
ering the entire space, and to allow efficient detection ing EXTEND with CONNECT in both Lines 4 and 5,
of meeting. to yield RRT-ConCon.

Figure 5 shows the RRT_BIDIRECTIONAL algo- Through extensive experimentation over a wide vari-
rithm, which may be compared to the BUILD_RRT al- ety of examples, we have concluded that, when applica-
gorithm of Figure 1. RRT_BIDIRECTIONAL divides ble, the bidirectional approach is much more efficient
the computation time between two processes: 1) ex- than a single RRT approach. One shortcoming of us-
ploring the state space; 2) trying to grow the trees into ing the bidirectional approach for nonholonomic and
each other. Two trees, 7, and 7, are maintained at kinodynamic planning problems is the need to make a
connection between a pair of vertices, one from each
RRT. For a planning problem that involves reaching
RRT_BIDIRECTIONAL(2init, Zgoa1) a goal region from an initial state, no connections are
1 7q-init(2iniz); To-init(tgoat);
2 "for k= 1 to K do necessary using a single-RRT approach. The gaps be-
3 Grand + RANDOM.STATE(); tween the two trajectories can be closed in practice by
4 if not (EXTEND(Ta, Lrana) = Trapped) then applying steering methods [27], if possible, or classical
5 if (EXTEND(7%,fnew) =Reached) then shooting methods, which are often used for numerical
6 Return PATH(7.,, 7s); boundary value problems.
7 SWAP(Ta, Te);
8 Return Failure
Other Approaches If a dual-tree approach offers
advantages over a single tree, then it is natural to ask
Figure 6: A bidirectional RRT-based planner.
302 S. M. LaValle and J. J. Kuffner

whether growing three or more RRTs might be even

better. These additional RRTs could be started at ran-
dom states. Of course, the connection problem will be-
come more difficult for nonholonomic problems. Also,
as more trees are considered, a complicated decision
problem arises. The computation time must be di-
vided between attempting to explore the space and at-
tempting to connect RRTs to each other. It is also not
clear which connections should be attempted. Many
research issues remain in the development of this and
other RRT-based planners.
It is interesting to consider the limiting case in which
a new RRT is started for every random sample, Zrand-
Once the single-vertex RRT is generated, the CON-
NECT function from Figure 5 can be applied to every
other RRT. To improve performance, one might only
consider connections to vertices that are within a fixed
distance of Zang, according to the metric. If a con-
nection succeeds, then the two RRTs are merged into
a single graph. The resulting algorithm simulates the
behavior of the probabilistic roadmap approach to path
planning [19]. Thus, the probabilistic roadmap can be
considered as an extreme version of an RRT-based al- Figure 7: Moving a Piano.
gorithm in which a maximum number of separate RRTs
are constructed and merged.
be based on freely-available collision detection and ef-
ficient nearest neighbor libraries.
6 Implementations and Experiments
In this section, results for four different types of prob- Holonomic planning experiments Through nu-
lems are summarized: 1) holonomic planning, 2) non- merous experiments, we have found RRT-based plan-
holonomic planning, 3) kinodynamic planning, and ners to be very efficient for holonomic planning. Note
4) planning for systems with closed kinematic chains. that our planners attempt to find a solution with-
Presently, we have constructed two planning systems out performing precomputations over the entire state
based on RRTs. One is written in Gnu C++ and space, and are therefore suited for single-query path
LEDA, and experiments were conducted on a 500Mhz planning problems, in contrast to the probabilistic
Pentium III PC running Linux. This implementa- roadmap method. It is easy to construct single-query
tion is very general, allowing many planning variants examples on which an RRT-based planner will be su-
and models to be considered; however, it is limited to perior by terminating before covering the entire state
planar obstacles and robots, and performs naive col- space, and it is easy to construct multiple-query prob-
lision detection. The software can be obtained from lems in which the probabilistic roadmap method will be
http://janowiec.cs.iastate.edu/~lavalle/rrt/. The sec- superior by repeatedly using its precomputed roadmap.
ond implementation is written in SGI C++ and SGI’s Most of the experiments in this section were con-
OpenInventor library, and experiments were conducted ducted on the 200MHz SGI Indigo2. More holonomic
on a 200MHz SGI Indigo2 with 128MB. This imple- planning experiments are presented in [22]. The CON-
mentation considers 3D models, and was particularly NECT function is most effective when one can expect
designed inclusion in a software platform for automat- relatively open spaces for the majority of the planning
ing the motions of digital actors [21]. Currently, we queries. We first performed hundreds of experiments
are constructing a third implementation, which is ex- on over a dozen examples for planning the motions of
pected to be general-purpose, support 3D models, and rigid objects in 2D, resulting in 2D and 3D configu-
Rapidly-Exploring Random Trees
303

Figure 8: Playing a game of virtual chess.

ration spaces. Path smoothing was performed on the

final paths to reduce jaggedness. Figure 7 depicts a
computed solution for a 3D model of a grand piano
moving from one room to another amidst walls and
low obstacles. Several tricky rotations are required of
the piano in order to solve this query.
Figure 8 shows a human character playing chess.
Each of the motions necessary to reach, grasp, and
reposition a game piece on the virtual chess board were
generated using the RRT-ExtCon planner in an aver-
age of 2 seconds on the 200 MHz SGI Inidigo2. The
human arm is modeled as a 7-DOF kinematic chain,
and the entire scene contains over 8,000 triangle primi-
tives. The 3D collision checking software used for these
experiments was the RAPID library based on OBB- Figure 9: A narrow-corridor example.
Trees developed by the University of North Carolina
[32]. The speed of the planner allows for the user to
interact with the character in real-time, and even en-
and the problem is solved in two seconds on the PC
using naive collision checking.
gage in an interactive game of “virtual chess”.
The final holonomic planning example, shown in Fig-
ure 9, was solved using the RRT-ExtExt planner. This Nonholonomic planning experiments Several
problem was presented in [7] as a test challenge for ran- nonholonomic planning examples are shown in Fig-
domized path planners due to the narrow passages that ures 10 and 11. These examples were computed using
exist in the configuration space when the “U”-shaped the RRTExtExt planner, and the average computation
object passes through the center of the world. In the times were less than five seconds on the PC using naive
example shown, the RRT does not explore to much of collision detection. The four examples in Figure 10 in-
the surrounding space (some of this might be due to volve car-like robots that moves at constant speed un-
the lucky placement of the corridor in the center of the der different nonholonomic models. A 2D projection of
world). On average, about 1500 nodes are generated, the RRTs is shown for each case, along with the com-
304 S. M. LaValle and J. J. Kuffner

oS
Ly ie
VR}
A rts
Coxe?

Figure 10: Several car-like robots.

Figure 11: A 7-DOF nonholonomic planning example.

puted path from an initial state to a goal state. The top formed using variants of the RRT_BIDIRECTIONAL
two pictures show paths computed for a 3-DOF model, planner. More experiments were presented in [30].
using the standard kinematics for a car-like robot. Recently, RRTs have been applied to automating the
In the first example, the car is allowed to move in flight of helicopters in complicated 3D simulations that
both forward and reverse. In the second example, the contain obstacles |15].
car can move forward only. In the first example in
Consider the case of a fully-orientable satellite model
the second row in Figure 10, the car is only allowed to
with limited translation. The satellite is assumed to
turn left in varying degrees! The planner is still able to
have momentum wheels that enable it to orient itself
overcome this difficult constraint and bring the robot
along any axis, and a single pair of opposing thruster
to its goal. The final example uses a 4-DOF model,
controls that allow it to translate along the primary
which results in continuous curvature paths [43].
axis of the cylinder. This model has a 12-dimensional
The final nonholonomic planning problem involves state space. The task of the satellite, modeled as a rigid
the 4-DOF car pulling three trailers, resulting in a 7- cylindrical object, is to perform a collision-free dock-
DOF system. The kinematics are given in [40]. The ing maneuver into the cargo bay of the space shuttle
goal is to pull the car with trailers out of one stall, model amidst a cloud of obstacles. Figure 12 shows
and back it into another. The RRTs shown correspond the candidate solution found after 23,800 states were
to one of the best executions; in other iterations the
explored. The total computation time was 8.4 minutes
exploration was much slower due to metric problems. on the SGI.
Kinodynamic planning experiments Several kin- The final result, given in Figure 13, shows a fictitious,
odynamic planning experiments have been performed underactuated spacecraft that must maneuver through
for both non-rotating and rotating rigid objects in 2D two narrow gates and enter a hangar that has a small
and 3D worlds with velocity and acceleration bounds entrance. There are five inputs, each of which applies a
obeying L? norms. For the 2D case, controllability is- thrust impulse. The possible motions are: 1) forward,
sues were studied recently in [34]. All experiments uti- 2) up, 3) down, 4) clockwise roll, 5) counterclockwise
lized a simple weighted L? metric on X, and were per- roll. There are 4000 triangles in the model. Path plan-
Rapidly-Exploring Random Trees
305

ae Ee

e® -°
BS

Figure 12: A 12-DOF kinodynamic planning example.

ning is performed directly in the 12-dimensional state

space. A typical run requires about 12 minutes on an
R12000.

Planning for closed kinematic chains Figure 14

shows a problem that involves a kinematic closure con-
straint that must be maintained in addition to per-
forming holonomic path planning. Many more exam-
ples and experiments are discussed in [49]. In the ini-
tial state, the closure constraint is satisfied. The incre-
mental simulator performs local motions that maintain
with closure constraint within a specified tolerance.

7 Discussion

We have presented a general framework for develop-

ing randomized path planning algorithms based on the
concept of Rapidly-exploring Random Trees (RRTs).
After extensive experimentation, we are satisfied with
the results obtained to date. There is, however, sig-
nificant room for improvement given the complexity of
problems that arise in many applications. To date, we
Figure 13: An underactuated spacecraft that performs
believe we have presented the first randomized path
complicated maneuvers. The state space has twelve dimen-
planning techniques that are particularly designed for
sions, and there are five inputs.
306 S. M. LaValle and J. J. Kuffner

Ideally, p* would make an ideal metric because it in-

ae os
dicates “closeness” as the ability to bring the state
from «x to z’ while incurring little cost. For holonomic
planning, nearby states in terms of a weighted Euclid-
ean metric are easy to reach, but for nonholonomic
problems, it can be difficult to design a good metric.
The ideal metric has appeared in similar contexts as
the nonholonomic metric (see [27]), and the cost-to-
Figure 14: Two manipulators transport a cross-shaped ob-
go function [5]. Of course, computing p* is as diffi-
ject while maintaining kinematic closure.
cult as solving the original planning problem! It is
generally useful, however, to consider p* because the
handling differential constraints (without necessarily performance of RRT-based planners seems to gener-
requiring steering ability). RRTs have also led to very ally degrade as p and p* diverge. An effort to make
efficient planners for single-query holonomic path plan- a crude approximation to p*, even if obstacles are ne-
ning. Several issues and topics are mentioned below, glected, will probably lead to great improvements in
which are under current investigation. performance. In [15], the cost-to-go function from a
hybrid optimal controller was used as the metric in an
Designing Metrics The primary drawback with the RRT to generate efficient plans for a nonlinear model
RRT-based methods is the sensitivity of the perfor- of a helicopter.
mance on the choice of the metric, p. All of the results
presented in Section 6 were obtained by assigning a Efficient Nearest-Neighbors One of the key bot-
simple, weighted Euclidean metric for each model (the tlenecks in construction of RRTs so far has been near-
same metric was used for different collections of obsta- est neighbor computations. To date, we have only
cles). Nevertheless, we observed that the computation implemented the naive approach in which every ver-
time varies dramatically for some problems as the met- tex is compared to the sample state. Fortunately,
ric is varied. This behavior warrants careful investiga- the development of efficient nearest-neighbor for high-
tion into the effects of metrics. This problem might dimensional problems has been a topic of active interest
seem similar to the choice of a potential function for in recent years (e.g., [2]). Techniques exist that can
the randomized potential field planer; however, since compute nearest neighbors (or approximate nearest-
RRTs.eventually perform uniform exploration, the per- neighbors) in near-logarithmic time in the number of
formance degradation is generally not as severe as a vertices, as opposed to the naive method which takes
local minimum problem. Metrics that would fail mis- linear time. Implementation and experimentation with
erably as a potential function could still yield good nearest neighbor techniques is expected to dramatically
performance in an RRT-based planner. improve performance. Three additional concerns must
In general, we can characterize the ideal choice of be addressed: 1) any data structure that is used for
a metric (technically this should be called a pseudo- efficient nearest neighbors must allow incremental in-
metric due to the violation of some metric properties). sertions to be made efficiently due to the incremen-
Consider a cost or loss functional, L, defined as: tal construction of an RRT, and 2) the method must
support whatever metric, p, is chosen, and 3) simple
ile
i= [ I(a(t), u(t))dt + 1j(2(T)). adaptations must be made to account for the topology
of the state space (especially in the case of $1 and P?,
which arise from rotations).
As examples, this could correspond to the distance
traveled, the energy consumed, or the time elapsed dur- Collision Detection For collision detection in our
ing the execution of a trajectory. The optimal cost to previous implementations, we have not yet exploited
go from z to 2’ can be expressed as the fact that RRTs are based on incremental motions.
Given that small changes usually occur between con-
p*(¢,2’)! = min: /a I(a(t), u(t))at + utetry| | figurations, a data structure can be used that dramat-
ically improves the performance of collision detection
Rapidly-Exploring Random Trees
307
and distance computation [16, 31, 38, 42]. The incorpo-
[10] M. Cherif. Kinodynamic motion planning for all-
ration of such approaches into our RRT-based planners terrain wheeled vehicles. In [EEE Int. Conf. Robot.
should cause dramatic performance benefits. & Autom., 1999.

Acknowledgments
[11] C. Connolly, R. Grupen, and K. Souccar. A Hamil-
tonian framework for kinodynamic planning. In Proc.
of the IEEE International Conf. on Robotics and Au-
This work has benefitted greatly from discussions with tomation (ICRA’95), Nagoya, Japan, 1995.
Nancy Amato, Jim Bernard, Francesco Bullo, Peng
B. Donald and P. Xavier. Provably good approxi-
Cheng, Bruce Donald, Yan-Bin Jia, Lydia Kavraki, mation algorithms for optimal kinodynamic planning:
Jean-Claude Latombe, Jean-Paul Laumond, Kevin Robots with decoupled dynamics bounds. Algorith-
Lynch, Ahmad Masoud, and Jeff Yakey. The authors mica, 14(6):443-479, 1995.
thank Valerie Boor for supplying the problem shown
in Figure 9. LaValle is supported in part by NSF CA- [13] B. R. Donald, P. G. Xavier, J. Canny, and J. Reif.
Kinodynamic planning. Journal of the ACM, 40:1048-
REER Award IRI-9875304 and Honda. Kuffner is sup- 66, November 1993.
ported in part by a jointly-funded NSF-JSPS (Japan)
Post-Doctoral Fellowship. [14] Th. Fraichard and C. Laugier. Kinodynamic planning
in a structured and time-varying 2d workspace. In
IEEE Int. Conf. Robot. & Autom., pages 2: 1500-
References 1505, 1992.

[1] N. M. Amato and Y. Wu. A randomized roadmap [15] E. Frazzoli, M. A. Dahleh, and E. Feron. Robust hy-
method for path and manipulation planning. In [EEE brid control for autonomous vehicles motion planning.
Int. Conf. Robot. & Autom., pages 113-120, 1996. Technical Report LIDS-P-2468, Laboratory for Infor-
mation and Decision Systems, Massachusetts Institute
[2jad S. Arya, D. M. Mount, N. S. Netanyahu, R. Silver- of Technology, 1999.
man, and A. Y. Wu. An optimal algorithm for ap-
proximate nearest neighbor searching. Journal of the [16] L. J. Guibas, D. Hsu, and L. Zhang. H-Walk: Hierar-
ACM, 45:891—923, 1998. chical distance computation for moving convex bodies.
In Proc. ACM Symposium on Computational Geome-
[3—
J. Barraquand, B. Langlois, and J. C. Latombe. Nu- try, pages 265-273, 1999.
merical potential field techniques for robot path plan-
ning. IEEE Trans. Syst., Man, Cybern., 22(2):224— [17] D. Hsu, L. E. Kavraki, J.-C. Latombe, R. Motwani,
241, 1992. and S. Sorkin. On finding narrow passages with proba-
bilistic roadmap planners. In et al. P. Agarwal, editor,
[4 J. Barraquand and J.-C. Latombe.
Kecweal
Nonholonomic Robotics: The Algorithmic Perspective, pages 141-154.
multibody mobile robots: Controllability and motion A.K. Peters, Wellesley, MA, 1998.
planning in the presence of obstacles. In [EEE Int.
Conf. Robot. & Autom., pages 2328-2335, 1991. [18] D. Hsu, J.-C. Latombe, and R. Motwani. Path plan-
ning in expansive configuration spaces. Int. J. Com-
[5] R. E. Bellman. Dynamic Programming. Princeton put. Geom. & Appl., 4:495-512, 1999.
University Press, Princeton, NJ, 1957.

[6] J. E. Bobrow, S. Dubowsky, and J. S. Gibson. Time- [19] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H.
Overmars. Probabilistic roadmaps for path planning
optimal control of robotic manipulators along specified
in high-dimensional configuration spaces. IEEE Trans.
paths. Int. J. Robot. Res., 4(3):3-17, 1985.
Robot. & Autom., 12(4):566—-580, June 1996.
[7] V. Boor, N. H. Overmars, and A. F. van der Stap-
pen. The gaussian sampling strategy for probabilistic [20] R. Kindel, D. Hsu, J.-C. Latombe, and S. Rock. Kin-
odynamic motion planning amidst moving obstacles.
roadmap planners. In IEEE Int. Conf. Robot. & Au-
In IEEE Int. Conf. Robot. & Autom., 2000.
tom., pages 1018-1023, 1999.

[8cel
J. Canny, A. Rege, and J. Reif. An exact algorithm [21] J. J. Kuffner. Autonomous Agents for Real-time Ani-
mation. PhD thesis, Stanford University, 1999.
for kinodynamic planning in the plane. Discrete and
Computational Geometry, 6:461—484, 1991.
[22] J. J. Kuffner and S. M. LaValle. RRT-connect: An
[9] H. Chang and T. Y. Li. Assembly maintainability efficient approach to single-query path planning. In
study with motion planning. In IEEE Int. Conf. Ro- Proc. IEEE Int’l Conf. on Robotics and Automation,
bot. € Autom., pages 1012-1019, 1995. 2000.
308 S. M. LaValle and J. J. Kuffner

[23] F. Lamiraux and J.-P. Laumond. On the expected [37] E. Mazer, G. Talbi, J. M. Ahuactzin, and P. Bessiere.
complexity of random path planning. In JEEE Int. The Ariadne’s clew algorithm. In Proc. Int. Conf. of
Conf. Robot. & Autom., pages 3306-3311, 1996. Society of Adaptive Behavior, Honolulu, 1992.

[24] R. E. Larson. A survey of dynamic programming com- [38] B. Mirtich. V-Clip: Fast and robust polyhedral colli-
putational procedures. [EEE Trans. Autom. Control, sion detection. Technical Report TR97-05, Mitsubishi
12(6):767-774, December 1967. Electronics Research Laboratory, 1997.

[25] J.-C. Latombe. Robot Motion Planning. Kluwer Aca- [39] R. Motwani and P. Raghavan. Randomized Algo-
demic Publishers, Boston, MA, 1991. rithms. Cambridge University Press, 1995.

[26] J.-P. Laumond. Finding collision-free smooth trajec- [40] R. M. Murray and S. Sastry. Nonholonomic motion
tories for a non-holonomic mobile robot. In Proc. Int. planning: Steering using sinusoids. Trans. Automatic
Joint Conf. on Artif. Intell., pages 1120-1123, 1987. Control, 38(5):700-716, 1993.

[27] J. P. Laumond, 8S. Sekhavat, and F. Lamiraux. Guide-

[41] I. Pohl. Bi-directional and heuristic search in path
lines in nonholonomic motion planning for mobile problems. Technical report, Stanford Linear Acceler-
robots. In J.-P. Laumond, editor, Robot Motion ator Center, 1969.
Plannning and Control, pages 1-53. Springer-Verlag,
Berlin, 1998. [42] S. Quinlan. Efficient distance computation between
nonconvex objects. In IEEE Int. Conf. Robot. & Au-
[28] S. M. LaValle. Numerical computation of optimal nav- tom., pages 3324-3329, 1994.
igation functions on a simplicial complex. In P. Agar-
wal, L. Kavraki, and M. Mason, editors, Robotics: The [43] A. Scheuer and Ch. Laugier. Planning sub-optimal
Algorithmic Perspective. A K Peters, Wellesley, MA, and continuous-curvature paths for car-like robots. In
1998. IEEE/RSJ Int. Conf. on Intelligent Robots & Systems,
pages 25-31, 1998.
[29] S. M. LaValle. Rapidly-exploring random trees:
A new tool for path planning. TR 98-11,
Computer Science Dept., Iowa State University.
[44] S. Sekhavat, P. Svestka, J.-P. Laumond, and M. H.
Overmars. Multilevel path planning for nonholonomic
<http://janowiec.cs.iastate.edu/papers/rrt.ps>, Oct. robots using semiholonomic subsystems. Int. J. Robot.
1998. Res., 17:840-857, 1998.
[30] 5S. M. LaValle and J. J. Kuffmer. Randomized kinody-
[45] Z. Shiller and S. Dubowsky. On computing time-
namic planning. In Proc. IEEE Int’l Conf. on Robotics optimal motions of robotic manipulators in the pres-
and Automation, pages 473-479, 1999. ence of obstacles. [EEE Trans. on Robotics and Au-
tomation, 7(7), December 1991.
[31] M. C. Lin and J. F. Canny. Efficient algorithms for
incremental distance computation. In [EEE Int. Conf.
Robot. & Autom., 1991.
[46] K. Shin and N. McKay. Open-loop minimum-time con-
trol of mechanical manipulators and its application. In
Proc. of American Control Conf., pages 296-303, San
[32] M. C. Lin, D. Manocha, J. Cohen, and S. Gottschalk.
Diego, CA, 1984.
Collision detection: Algorithms and applications. In
J.-P. Laumond and M. Overmars, editors, Algorithms
for Robotic Motion and Manipulation, pages 129-142. [47] P. Svestka and M. H. Overmars. Coordinated motion
planning for multiple car-like robots using probabilis-
A K Peters, Wellesley, MA, 1997.
tic roadmaps. In IEEE Int. Conf. Robot. & Autom.,
[33] T. Lozano-Pérez. Spatial planning: A configura- pages 1631-1636, 1995.
tion space approach. JEEE Trans. on Comput., C-
32(2):108-120, 1983. [48] D. Vallejo, C. Jones, and N. Amato. An adaptive
framework for ”single shot” motion planning. Texas
[34] K. M. Lynch. Controllability of a planar body with A&M, October 1999.
unilateral thrusters. JEEE Trans. on Automatic Con-
trol, 44(6):1206-1211, 1999. [49] J. H. Yakey. Randomized path planning for linkeages
with closed kinematic chains. Master’s thesis, Iowa
[35] K. M. Lynch and M. T. Mason. Stable pushing: Me- State University, Ames, IA, 1999.
chanics, controllability, and planning. Int. J. Robot.
Res., 15(6):533-556, 1996. [50] Y. Yu and K. Gupta. On sensor-based roadmap: A
framework for motion planning for a manipulator arm
[36] }. Mazer, J. M. Ahuactzin, and P. Bessiére. The Ari- in unknown environments. In IEEE/RSJ Int. Conf. on
adne’s clew algorithm. J. Artificial Intell. Res., 9:295- Intelligent Robots & Systems, pages 1919-1924, 1998.
316, November 1998.
Encoders for Spherical Motion
Using Discrete Optical Sensor
Edward R. Scheinerman, Johns Hopkins University, Baltimore, MD
Gregory S. Chirikjian, Johns Hopkins University, Baltimore, MD
David Stein, Johns Hopkins University, Baltimore, MD

We develop a methodology for absolute encoding of

spherical motion. This is accomplished by painting the
surface of a moving ball in two colors and sensing the
color at a finite set of points. We show how the painting
of a ball in two colors should be performed, and how the
point measurements can resolve an arbitrary rotation to
within a range depending on the number of sensors and
the painting of the ball.

1 Introduction

We consider the following problem: A ball is held in a

housing, but is free to rotate arbitrarily. How can we
determine the orientation of the ball?
We solve this problem by painting the surface of the
ball with two colors (black and white). Fixed point
sensors are located in the housing and we determine the Tey —

orientation based on the feedback from these sensors.

In this paper we present a technique for painting the
surface of the ball, and how to decode the orientation
of the ball based on the sensor readings.
Figure 1: An application of spherical motors in a distrib-
The need to determine the orientation of a ball un-
uted manipulation device.
dergoing spherical motion arises in several scenarios.
One application area is the feedback control of cam-
era pointing devices used in robot vision [2]. An- and implementation of spherical variable-reluctance
other application area is in the design of an optical motors has been studied by Kok-Meng Lee and cowork-
mouse/trackball (see [1, 18] and references therein). ers for a number of years [16, 21, 22, 29]. Toyama
Figure 1 illustrates another application in which an and coworkers have developed spherical ultrasonic mo-
array of spherical motors is used as a distributed ma- tors [25, 26]. Most recently, mathematical issues in
nipulation device. Other mechanisms for distributed the design and commutation of spherical stepper mo-
manipulation have been considered (see, e.g., [9, 17]). tors with full spherical motion has been addressed in
For this application to become a reality, both robust [6]. Techniques from noncommutative harmonic analy-
and inexpensive spherical motors and encoders must sis are useful in this regard (see e.g., [5] for an intro-
become available. duction). Despite the rather large literature on spher-
Spherical motors have been studied for quite some ical motors, the study of optical encoders for spherical
time. The basic operating principles of spherical DC motion appears not to have been approached from a
induction motors are described in [7, 28]. Kaneko et al. mathematical or algorithmic perspective. In this pa-
[14] developed a spherical DC servo motor. The design per we fill this gap by providing a general technique
310 E.R. Scheinerman, G.S. Chirikjian, and D. Stein

termine the angle of the disk using colors and sensors

along the edge of the disk.
Let n be a positive integer. A de Bruijn sequence
is a sequence of 2” zeros and ones arranged so that
each of the 2” possible n-bit binary numbers appears
(cyclically) exactly once in the sequence. For example,
the sequence
OrDeOsOid
Delete doe) 11042

contains all 2+ length-4 binary numbers. The binary

number 0011 appears starting at the third element of
the sequence, and the binary number 0100 appears
starting at the next-to-last element of the sequence
(and wrapping around to the start). By painting the
edge of the disk according to a de Bruijn sequence (as
in the lower portion of Figure 2), all 2” bit patterns
appear exactly once and we can resolve 2” different
rotations of the disk. See [4, 27]. Our purpose is to
present a technique to use optical sensors to establish
the orientation of the ball. We begin by describing a
scheme for painting the surface of the ball. Unfortu-
Figure 2: The angular orientation of a disk can be de- nately, a simple, regular painting of the ball is unac-
termined using optical sensors. Above we see a standard ceptable; such paintings often have symmetries and,
encoding in which the four sensors are positioned radially. consequently, different orientations of the ball produce
Below, we see an encoder based on a de Bruijn sequence; identical sensor readings. On the other hand, a highly
the colors and sensors are placed on the edge of the disk. irregular coloring might be difficult to describe and be
inefficient to handle computationally. We desire a col-
oring that is irregular (highly asymmetrically) but for
for ball painting and sensor placement. This is anal-
which the following problem is efficiently solved: Given
ogous to the optical encoder for axial rotations shown
a point on the surface of the ball, determine the color
in the upper portion of Figure 2. The main difference
of that point.
is that in spherical motion, we cannot take advantage
of a dimension perpendicular to motion. That is, the We call the coloring method we employ a random
painting and sensor measurements must be performed Voronoi coloring. Let m be a positive integer (say,
on the surface of the ball. This is analogous to only cir- between 50 and 100). We choose m anchor points in-
cumferential painting and sensor measurements in the dependently and uniformly at random on the surface
axial case. See Figure 2, bottom. of the sphere; call these points a), a2,... ,Am. (We as-
sume these points lie on a unit sphere, so all a; € R®
2 Voronoi Surface Painting and |/a;|| = 1.) We assign a color to each of these
points; independently, we color point a; either black or
Our method of determining the orientation of a ball white, each with probability $. Let c(a;) be the color
is inspired by the following technique for determining of a;. It is convenient to take c(a;) = 1 if a; is black
the orientation of a disk that is free to rotate about a and c(a;) = —1 if the point is white.
single axis. We paint the disk and locate optical sen- Now let x be an arbitrary point on the surface of the
sors to determine the angle of rotation. For example, sphere. We color x to match the color of its nearest an-
the upper portion of Figure 2 shows a standard optical chor point. That is, choose iso that d(x, a;) < d(x, a;)
encoding. However, for this encoder, the sensors must for all j7. Let c(x) = c(a;). Such a coloring is mildly
be able to see the interior of the disk. An alternative ambiguous. If x is nearest to two (or more) points
method, based on de Bruijn sequences, allows us to de- of different colors, we assign c(x) arbitrarily (e.g., to
Encoders for Spherical Motion
311

3 Sensors

We determine the orientation of the ball by reading

the color at various points on its surface. That is, we
place an array of sensors in the device’s housing. Each
sensor is capable of detecting the color (black or white)
at a point on the surface of the ball. In our laboratory,
we use infrared photosensors that have both an emitter
and a detector in the same package. The sensors use
digital logic. When a black or an absorbent surface it
placed in front of it, the output is in one state. When
a white or reflective surface is placed in front of it the
output switches state. The output of these sensors is
fed into a data acquisition board.
y x The locations of the sensors correspond to points on
the sphere. If we have n sensors, let s1,82,...,S,, be
Figure 3: A random Voronoi painting of a ball.
the points on the sphere (so s; € R® and ||s,|| = 1)
corresponding to the sensors.
Fix a particular orientation of the ball as a home
match the nearest anchor point of lowest index). Such
orientation. Every orientation of the ball can then be
a coloring of the ball is shown in Figure 3.
specified by a rotation matrix A € SO(3).
The anchor points aj,... ,a, partition the surface If the ball has been rotated from the home position
of the sphere into regions based on nearest neighbor. via a rotation A, then the color detected by sensor 7 is
That is, each point is assigned to a region based on c(A™'s;). We define the color vector of a rotation A to
which of the anchor points is nearest to it. The bound- be the following vector (whose entries are +1):
aries of these regions (points that are nearest to two
or more anchor points) are arcs of great circles. Such (A) = [ce(A™s;) rm
c(A? s9) c(A™ sn) deri
a partitioning of the sphere is known as a Voronoi de-
composition. Thus, given random anchor points, we Of course, the color vector depends not only on the ro-
independently color each region of the Voronoi decom- tation A, but also on the painting of the ball c and the
position black or white (each with probability 3): placement of the sensors {s;}. However, only the ori-
entation A changes; the painting and sensor placement
The random Voronoi coloring of the ball achieves the are fixed.
irregularity we desire, but what of the computational
The problem we want to solve is: Given a color vec-
efficiency? That is, given a test point x, how quickly
tor c(A), determine A. The problem does not have
can we determine c(x)? A simple method is to calcu-
a unique solution. Because there are only n sen-
late the m inner products x- a; and then select the
sors, there are (at most) 2” possible color vectors, but
index of the maximum such value. This requires O(m)
there are infinitely many different rotation matrices A.
calculations. Because m is of modest size, the color
Therefore, the problem we actually solve is, given a
of any single point can be found rapidly. However, in
color vector c(A), find a rotation matrix B so that
our method, we repeatedly compute the colors of many
c(A) = c(B), and B is close to A. We can measure
points, and so a more efficient algorithm is needed.
the discrepancy between A and B as an angle. Since
Fortunately, such algorithms already exist [8, 10, 20]. ABT € SO(3), we can consider AB’ as a rotation
One can construct (via a one-time calculation) a data about an axis; the angle of that rotation is our mea-
structure which supports nearest neighbor query in sure of closeness. Thus, the “distance” between A and
time O(logm). That is, given a test point x on the é
B is cos
=i) /(Gaver
( 5
i
yaw
.
surface of the sphere, we can determine which of the
fixed anchor points aj,... ,a@m is closest to x in time How many sensors should we use and where should
O(log m). they be placed? Our experiments indicate that the
312 E.R. Scheinerman, G.S. Chirikjian, and D. Stein

1.5 T rp T = T
number of sensors is more important than their place-
ment. As long as the sensors are reasonably distributed
around the surface of the sphere we achieve nearly iden-
tical performance of our decoding method. The more
sensors we use, the more accurately can we determine A
from c(A). However, the computational burden rises
accordingly. With 50 sensors, we usually find A to
within 2 or 3 degrees. With 200 sensors we typically
find A to within half a degree.

4 Local Solution
0 n
sr
Our problem is: Given c(A), find a B € SO(3) so -60 -40 -20 0 20 60

that c(B) = c(A). To this end, we define a function

f : SO(3) — R by: Figure 4: Plot of f(B) as B varies through a single axis
of rotation. On the horizontal axis, 0° corresponds to the
f(B) = ale(B) — €(4). home orientation. (For this figure, we used a ball with 75
Voronoi regions and 44 sensors.)
The factor 1/,/n is not strictly necessary, but simply
rescales the image of the function so that it always lies be to follow a steepest-descent trajectory. However, to
in [0,2] regardless of the size of n (since the entries of make this precise, we need to be clear on what we mean
c(A) and c(B) are +1, the value of ||c(B) — ¢(A)]|| is by the gradient of f, and then deal with the fact that
at most 2,/n). wherever the gradient is defined, its value is zero.
The problem can then be restated: Find B € SO(8) Because the domain of f is SO(3), we use concepts
so that f(B) = 0. Equivalently, since f is non-negative, from Lie groups/algebras [5, 11, 12, 19] to speak care-
we can think of this problem as searching for a (global) fully of the gradient of f; see [24]. Let X bea 3x 3
minimizer of f. skew-symmetric matrix. We set:
In this section we present an iterative method that
leads to a solution provided we are given an initial Bo
that is close (say, within 10 degrees) to the desired so-
(X®f)(B) ae= (Bet)
=
t=0
lution. This assumption is justifiable for two reasons.
First, as the ball turns in its housing, the sensors con- We can think of X*” as a (right) directional derivative
tinuously track its progress. We can use the previous of f in the direction X.
orientation of the ball as an initial guess for the cur- Define the (right) gradient of f at B to be the vector:
rent orientation. Second, the method described in the
next section can provide us with an initial guess Bo [EP Epona F)
reasonably close to the solution value.
where
We begin by examining a plot of f(B) where B is
restricted to a single axis of rotation; such a plot is Oh AW 0
presented in Figure 4. Ey = TORO" 21 4.
Oe ath 0
Notice that the graph of f is piecewise flat. This
happens because, as the ball rotates, sensors cross the Oh Dal
boundaries between the Voronoi regions and suddenly Japa OPUS Oe ant
change state. This causes a step-jump in the value of f. —-1 0 0

Also notice that within 10° of the actual position, the 0. —10
graph of f decreases (stepwise) to the minimum value. oe ee O=G
Thus, a sensible approach to minimizing f(B) would 0 0 0
Encoders for Spherical Motion
313
The gradient points in the direction of steepest as-
cent, so by following a path in the opposite direction,
we are led to a (local) minimum of the function. How-
ever, for the function we are considering, the gradient
of f at B is 0 for almost all B € $O(3), and is unde-
fined otherwise.
Our solution is to use approximate gradients. We
replace the right derivatives EP f (with j = 1, 2,3) with
finite difference approximations:

iber je f(Be-**s)
2t ;
We begin with a specific step size for t (e.g., t =
0.01). If the approximate gradient we calculate is 0,
50 50
we increase t by a constant factor. We do this until
the approximate gradient is some nonzero vector x.
We then update B by a step of size t in the negative Figure 5: Plot of f(A) as A varies along two orthogonal
gradient direction, i.e., axes.

0 Bei abo
is, we can choose a threshold (say 10°) and find a finite
B+¢ Bexp< -t]}] —-21 0 “x5
subset Z C SO(3) so that for all A € SO(3) there is a
Sig Sahte) 0
B € Z so that the rotational difference between A and
where z; is the finite difference approximation to B is below the threshold.
(E} f)(B). Our method is equally simple. We precompute the
set Z C SO(3). We record in a table the vector c(B)
We then iterate this procedure. At each successive
for all B € Z. Suppose Z = {B, Bo,..., By}. We
step, we decrease t by a constant factor so we do not
create a k x n matrix M whose i, j-entry is c(B/'s;);
overrun the minimum.
i.e., this entry is the color observed by the j*® sensor if
Ideally, we continue this procedure until we reach a the ball is rotated by B; from its home position.
B at which f(B) = 0. Of course, we could get trapped Let A € SO(8) be arbitrary. Then we choose B;
at a local minimum. In this case, if f(B) is fairly small in SO(3) so that c(A) - c(B;) is as large as possible
(say f(B) < 0.2) we are actually quite close to the (best possible match). This can be done by a single
minimum and may stop there. matrix-vector multiply, Mc(A) and finding the coordi-
nate with largest index.
5 Global Solution This calculation is reasonably fast, but the matrix
M is quite large (for 100 sensors and 10000 saved ori-
In the previous section we presented a technique to find
entations we need one million entries). One idea that
a B € SO(3) that solves the equation c(B) = c(A) saves some memory and speeds up the calculation is to
provided we are given a starting Bo sufficiently near A replace M by a reduced-rank approximation.
(within 10°). However, a steepest-descent method fails
Let M = UXV™ be M’s singular value decomposi-
if we start too far from the solution because of the pres-
tion [13]. Here U is a k x k unitary matrix, V is an
ence of many local minima and the highly nonsmooth
n Xn unitary matrix, and » is a k x n diagonal ma-
nature of the function. See Figure 5.
trix whose diagonal entries are real, nonnegative, and
Our strategy is simple. Since SO(3) is compact and in decreasing order. Let r be a modest positive integer
since the descent method just needs a starting value (e.g., r = 10) and let:
that is reasonably close to the correct value, we can
evaluate f(B) over a discrete subset of SO(3). That e be the r x r upper left corner of ©,
314 E.R. Scheinerman, G.S. Chirikjian, and D. Stein

e U be the k x r matrix formed by choosing just the [6] G.S. Chirikjian and D. Stein. Kinematic de-
sign and commutation of a spherical stepper mo-
first r columns of U, and
tor. IEEE/ASME Transactions on Mechatronics,
e V be the nxr matrix formed by choosing just the 4(4):342-353, December 1999.
first r columns of V. K. Davey and G. Vachtsevanos. The analysis of fields
and torques in a spherical induction motor. IEEE
Then M = UXV". This approximate decomposition Trans. Magn., MAG-23, March 1987.
can be computed without finding the full singular value
decomposition. It is a one-time computation that is M. de Berg, M. van Kreveld, M. Overmars, and
reasonably fast. O. Schwarzkopf. Computational Geometry: Algo-
rithms and Applications. Springer-Verlag, 1997.
Notice that these three matrices consume a total of
rk+r?++rn storage. For k = 10000, n = 100 and B.R. Donald, J. Jennings, and D. Rus. Information
r = 10, this is about 100,000 which is significantly less invariants for distributed manipulation. International
than one million. We can then approximate Mc(A) Journal of Robotics Research, 16(5):673-702, October
1997.
by USV'c(A), and select the largest component(s) to
give reasonable starting values to the descent method. H. Edelsbrunner. Algorithms in Combinatorial
Our experience has been that we need to try a few Geomery. Springer-Verlag, 1987.
of the largest starting values of the approximation to
I.M. Gel’fand, R.A. Minlos, and Z.Ya. Shapiro. Repre-
Mc(A) to get a good starting value.
sentations of the rotation and Lorenz groups and their
applications. Macmillan, 1963.
Acknowledgments
S. Helgason. Differential geometry, Lie groups, and
Edward Scheinerman is supported, in part, by an Inter- symmetric spaces. Academic Press, 1978.
disciplinary Grant in the Mathematical Sciences from
R.A. Horn and C.R. Johnson. Matrix Analysis. Cam-
the National Science Foundation, DMS-9971696. Gre- bridge University Press, 1985.
gory Chirikjian and David Stein are supported by
a grant from the Robotics and Human Augmenta- Y. Kaneko, I. Yamada, and K. Itao. A spherical DC
tion program of the National Science Foundation, IS- servo motor with three degrees-of-freedom. ASME
9731720. Dynamic Systems and Controls Division, 11:398—402,
1988.

References [15] K.-M. Lee. Orientation sensing system and method

for a spherical body, #5,319,577. U.S. Patent Office,
[1] X. Arreguit, F.A. van Schaik, F.V. Bauduin, 1994.
M. Bidiville, and E. Raeber. A CMOS motion detector
system for pointing devices. IEEE Journal of Solid- [16] K.-M. Lee and C.-K. Kwan. Design concept develop-
State Circuits, 31(12):1916-1921, December 1996. ment of a spherical stepper for robotic applications.
IEEE Trans. on Robotics and Automation, 7(1):175—
[2] B.B. Bederson, R.S. Wallace, and E.L. Schwartz. A 181, February 1991.
miniature pan-tilt actuator: the spherical pointing
motor. JEEE Trans. on Robotics and Automation,
10(3):298-308, June 1994.
[17] J. Luntz, W. Messner, and H. Choset. Parcel manipu-
lation and dynamics with a distributed actuator array:
[3] K.-F. Bohringer, B.R. Donald, and N.C. MacDonald. The virtual vehicle. In Proc. 1997 [EEE International
Programmable vector fields for distributed manipula- Conference on Robotics and Automation, 1997.
tion, with applications to MEMS actuator arrays and
vibratory parts feeders. International Journal of Ro- [18] R.F. Lyon. The optical mouse and an architectural
botics Research, 18(2):168-200, February 1999. methodology for smart digital sensors. In CMU Conf.
VLSI Systems Computations, pages 1-19, 1981.
[4] J.A. Bondy and U.S.R. Murty. Graph Theory with
Applications. North-Holland, 1976. [19 — W. Miller, Jr. Lie theory and special functions. Aca-
demic Press, 1968.
[5] G.S. Chirikjian and A.B. Kyatkin. Engineering Appli-
cations of Noncommutative Harmonic Analysis. CRC [20] J. O’Rourke. Computational Geometry in C. Cam-
Press, to appear. bridge University Press, second edition, 1998.
Encoders for Spherical Motion 315

[21] J. Pei. Methodology of Design and Analysis of trasonic motor. In Proc. 1995 IEEE Int. Conf. on
Variable-Reluctance Spherical Motors. PhD thesis, Robotics and Automation, pages 2935-2940, Nagoya,
Georgia Institute of Technology, 1990. Japan, 1995.

[22] R.B. Roth and K.-M. Lee. Design optimization of [26] S. Toyama, G. Zhang, and O. Miyoshi. Development
a three degrees-of-freedom variable-reluctance spher- of new generation spherical ultrasonic motor. In Proc.
ical wrist motor. ASME J. Engineering for Industry, 1996 IEEE Int. Conf. on Robotics and Automation,
117:378-388, August 1995. pages 2871-2876, Minneapolis, Minnesota, April 1996.

[23] D. Stein and G.S. Chirikjian. Experiments in the com- [27] D.B. West. Introduction to Graph Theory. Prentice-
mutation and motion planning of a spherical stepper Hall, 1996.
motor. preprint.
[28] F. Williams, Laithwaire, and G.F. Eastham. Develop-
ment and design of spherical induction motors. Proc.
[24] C.J. Taylor and D.J. Kriegman. Minimization on the of the IEEE, pages 471-484, December 1959.
Lie group SO(3) and related manifolds. Technical
Report 9405, Yale Center for Systems Science, April
[29] Z. Zhou and K.-M. Lee. Real-time motion control of
1994. a multi-degree-of-freedom variable reluctance spheri-
cal motor. In Proc. 1996 IEEE Int. Conf. on Robotics
[25] S. Toyama, S. Sugitani, G. Zhang, Y. Miyatani, and and Automation, pages 2859-2864, Minneapolis, Min-
K. Nakamura. Multi degree of freedom spherical ul- nesota, April 1996.
 canal iti eed
' iawiaed tY = 3 twa

ia hae bs hued
ti Taltie
els =) sameny4 [oe
: ag pat: Z ollany wetna
rs
ath go) Pattee foun
moe
arena aS Bet bes)
an wert ddadt

PER hy LiMo. | ,
{ al rear) * atl.ia,. ‘oad pri) iy ba Lil Ani "by
rine hen nuit! nit sca aes
boar :
tyw= el «/ Me ress:1%) tog ila’ sf rote ‘wuts “oe rare’)
fr gt eA ; ih

asecORE Dred iets alicdieatie. t aa ar

oe YEH Pormigous, bew Bip ihe wind , mhy
- e af PRE Hy LA Wein». fi Tarinay ie Renu
Sa cal aca

\ 2 Gps Fi: , Ns inp ;

bey Mathainetirs!. Selene tor
BPotnilatton, LRT, rn
ae Dies’ er i SMared +b
‘Robot: Wai? 1 avtien- Aeiient
Nine al S-ierce rigiinheaint. Ul

we 7

” ’,
ea. ef
6
Te Se iyo
OS «ten Sue ti

eee
i avdyi Af \- w

(af
Tt « )

é
Rei eee wir 29 brtiihen han. > 2
My EE Fiedienom, i > as
_ Filtaybet uh Fite Cio i ; be ‘
- te ft Veen neal
: =
7 e ss
ay i
bea i
' - .
_ = n+ rua hagey, if 7¢ it
SP MATRA Vadis
ri
- ie @cth 4 pd ache
7 a ire) “rT Ls 6 mt ‘
— : ii hs
1 le ‘ ‘\ r

oa I

@ »
:

:
j x
Notes on Visibility Roadmaps and Path Planning
J.-P. Laumond, LAAS-CNRS, Toulouse, France
T. Siméon, LAAS-CNRS, Toulouse, France

This paper overviews the probabilistic roadmap ap- route to face the constraints imposed by such a sce-
proaches to path planning whose surprising practical nario.
performances attract today an.increasing interest. We
This paper does not contain new algorithms, new
first comment on the configuration space topology in-
theorems nor new original results. It constitutes a
duced by the methods used to steer a mechanical sys-
set of remarks and working notes on probabilistic
tem. Topology induces the combinatorial complexity of
roadmaps algorithms introduced at the beginning of
the roadmaps tending to capture both coverage and con-
the ’90s [13, 25, 15] and now attracting numerous re-
nectivity of the collision-free space. Then we introduce
search groups, including ours.
the notion of optimal coverage and we provide a paper
probabilistic scheme in order to compute what we called The first section starts at the beginning: It deals
visibility roadmaps [26]. Reading notes conclude on re- with the controllability of mechanical systems inde-
cent results tending to better understand the behavior pendently from any computational perspective. In the
of these probabilistic path planning algorithms. next section we put emphasis on the choice of the meth-
ods used to steer a mechanical system from a configu-
1 Introduction ration to another one. The set of configurations reach-
able from a given configuration has various shapes de-
The framework of this work lies in the tentative to pending on the considered steering method. Paving
provide path planners working for large classes of me- the space with reachable sets asks for combinatorial
chanical systems. Such a generality is imposed by an topology issues which are discussed. The roadmaps in-
increasing number of path planning applications that duced by such pavements capture both coverage and
extend the robotics area where the research has been connectivity of the space and allow to apply a retrac-
initially conducted [19]. tion approach to path planning. At this stage, we in-
In our case, we are interested in providing CAD sys- troduce the notions of optimal coverage and visibil-
tems with path planning facilities in the context of lo- ity roadmaps. Section 4 addresses the computational
gistics and operation in industrial installations. A typ- point of view: Probabilistic algorithms are today the
ical scenario is the following one. An operator has to only effective way to pave the space with reachable
define a maintenance operation involving the moving sets. After presenting the principle of the probabilistic
of an heavy freight. He has CAD software including roadmap algorithms we show how visibility roadmaps
all the geometric details and facilities to display the may be computed. An interest of the algorithm lies
plant together with catalogue containing lists of avail- in its control. Another one is the small size of the
able tools to perform the maintenance task. He should computed roadmaps. The work related to probabilis-
choose suitable handling devices among cranes, rolling tic roadmap algorithms is reported and commented in
bridges, carts... and validate his choice by simulating Section 5.
the task within the CAD system. As the other facilities
offered by the CAD software, the use of path planners 2 Controllability and CS Topology
should be as easy as possible. The operator does not
care about path planning algorithms. Examples of mechanical systems Consider the
Probabilistic path planners, and among them, prob- mechanical systems displayed in Figure 1 together with
abilistic roadmap algorithms, appeared as a promising examples of collision-free paths. Modeling a mechan-
318 J.-P. Laumond and T. Siméon

ical system in the context of path planning addresses

two issues.
The first one deals with the placement constraints.
We should identify the configuration space C'S of the
system, i.e., a minimal set of parameters locating the
system in its workspace. This is an easy task for the
robot arm, the mobile robot, and the rolling bridge,
all of them being open kinematic chains. The sys-
tem constituted by the two holonomic mobile robots
manipulating an dumbbell is a closed kinematic chain.
The placement parameters of both robots are linked by
equations modeling the grasping of the dumbbell and
giving rise to holonomic constraints. For this special
example it is possible to select five independent para-
meters defining the configuration space properly, the
other placement parameters being deduced by explicit
equations involving only the five independent parame-
ters. For more general closed loop chains, characteriz-
ing C'S properly is not an easy task.
Second, we should consider the kinematic con-
straints. There is no special constraint for the robot
arm, any configuration parameter being a degree of
freedom directly controlled by a motor independently
from any other one. Any path in C'S is admissible.
The same property holds for the coordinated path of
the two holonomic mobile robots. The control of the
rolling bridge may impose special constraints, like mov-
ing a degree of freedom at once. In that case the only
admissible paths in C'S are Manhattan paths. The
motion of the mobile manipulator is submitted to the
constraint of rolling without sliding. This is a nonholo-
nomic constraint that affects the range of admissible
paths, but not the dimension of the reachable configu-
ration space.

Controllability All the previous systems are small-

time controllable: Starting from any configuration, the Figure 1: Examples of mechanical systems and collision-
set of configurations reachable before any given time free paths.
contains a neighborhood of the starting configuration.
Let us translate this property in geometric terms: The
set of configurations reachable by all the admissible specting the kinematic constraints. In other words,
paths not escaping a given neighborhood of the starting there exists an admissible collision-free path between
configuration contains a neighborhood of the starting two configurations if and only if both of them lie in the
configuration (Figure 2). same connected component of the collision-free config-
uration space! C'S free. This means that the knowledge
Small-time controllability plays a central role in ob-
of the connected components of C'S free 18 Sufficient to
stacle avoidance. Indeed, for small-time controllable
systems, any collision-free path in CS may be approx- ‘Configurations touching an obstacle are not considered
imated by a finite sequence of collision-free paths re- as collision-free. C'Sree is an open domain in CS.
Notes on Visibility Roadmaps and Path Planning
319

on the controllability of the considered system. Now,

to solve the problem completely (compute a path) we
have to devise effective ways to steer the system. What
we call a steering method is a procedure computing an
admissible path between any two configurations in the
absence of obstacles?.
Any steering method respecting the kinematic con-
straints of the system is not necessarily a good one with
Figure 2: Small-time controllable systems: The domain respect to obstacle avoidance. For instance, a steering
reachable without escaping a given neighborhood contains a method for a car moving only forward applies also to a
neighborhood of the origin. car moving both forward and backward. Nevertheless,
it is impossible to park a car by using only forward
prove the existence of an admissible collision-free path motions. A steering method induces a topology in CS
between two given configurations. The route is open to that should account for the topology induced by the
devise deterministic and complete algorithms to solve controllability property of the mechanical system. A
the path planning problem. steering method accounting for small-time controllabil-
ity should verify the property illustrated in Figure 2. It
The small-time controllability property does not is said to be stc. Devising stc steering methods is not
hold for any system. A car moving only forward is necessarily a trivial problem (especially for nonholo-
locally controllable (the reachable set from a configu- nomic systems). Moreover the choice is not unique. It
ration contains a neighborhood of the configuration). may affect the combinatorial complexity of the path
It is not small-time controllable and deciding on the planning algorithm.
existence of an admissible collision-free path is still an
This section aims to illustrate the importance of this
open problem for this system.
choice through two examples of stc steering methods
Complexity Small-time controllability allows to for- for a point moving on a plane. Both induce the same
get the kinematic constraints to decide on the existence topology in C'S. We will see that they differ by the
of an admissible path in C'Sree. Nevertheless the kine- combinatorics of the pavements induced on C'S free.
matic constraints affect the combinatorial complexity The first one (Steer;;,) consists in computing a
of the admissible paths. Consider that C'Syfree is re- straight-line segment between two points. The sec-
duced to a long straight-line tube. Moving within the ond one is Steerman. Two points being given,
tube is easy for the robot arm: A single straight-line Steerman computes first an horizontal path from the
path is sufficient. Depending on the orientation of the left point until the right point is reachable by a ver-
tube in C'Sree, the same task will be more difficult for tical path. Both steering methods are symmetric and
the rolling bridge: The number of elementary pieces stc. Figure 3(a) shows the structure of the sets of points
of the Manhattan paths may grow linearly when the reachable with paths of fixed length.
width of the tube decreases. It will be even more diffi- A symmetric stc steering method Steer being given,
cult for a nonholonomic robot: The number of elemen- a configuration is said to be visible from another one
tary pieces of admissible paths may grow exponentially if the path computed between them by Steer lies in
with its degree of nonholonomy when the width of the CSyree. The set of configurations visible from a given
tube decreases (e.g., the number of maneuvers required configuration constitutes its visibility set. The config-
to park a car along a sidewalk varies quadratically with uration is said to be the guard of its visibility set. Fig-
the inverse of the free space size). ure 3(b) shows visible sets of the same configuration, in
the same environment, for both Steer;;, and Steerman-
3 Steering Methods, Visibility, and
Covering CSfree with visibility sets Let us con-
Roadmaps
sider a small-time controllable system together with a
Steering methods The decision part of the path In the path planning literature, authors usually refer to
planning problem (existence of a path) depends only the notion of local methods.
 J.-P. Laumond and T. Siméon

it
320

> P
a: Resehunle sets with paths of fixed length.

bak
b: Visible sets in the presence of an obstacle
(grey domains are not reachable).

Figure 3: Visible sets ofa configuration with Steeriin (left)

and Steetman (right).

> 2
stc steering method. A set of guards is said to be cov-
ering CSfree if any configuration in C'Sfree is visible
from at least one guard.
We first address a question: Is there a finite set of
guards covering CSfree ? The answer to this question
is difficult and there exists today no general result. It
depends on both the shape of C'Syree and the consid-
ered steering method. Figure 4: Finite coverage using Steerji, exists for the cases
a, b, and d. It does not for the case c. Finite coverage using
Let us illustrate the problem via four examples of Steerman exists for the cases e, g, and h. It does not for the
2-dimensional C'Sfree (see Figure 4). First consider case f.
Steer;;,. For the cases of Figures 4(a) and 4(b), we may
provide coverage of C'Sfree, €.g., by putting guards suf-
ficiently close to convex vertices of the obstacles. The reason we say that the set of guards provides optimal
cases 4(c) and 4(d) contain an obstacle whose boundary coverage.
piece is circular and tangent to a straight-line segment:
Clearly, the set of guards providing optimal coverage
There is no way to provide a finite number of guards
is not unique. Taking a combinatorial point of view, a
covering C'Sfree for the case 4(c), while this is possible
second question is: If there exists finite optimal cover-
for the case 4(d) (in this later example it is necessary
age, are all the sets of guards achieving optimal cover-
to put at least one guard on the dashed line).
age necessarily finite? The answer is “no” in general.
Consider the same environments with Steerman. Fi-
The answer is “yes” if we consider Steer);,, and a
nite coverage exists for the cases 4(e), 4(g), and 4(h)
polygonal C'Sfree: The maximal number of guards is
while it does not exist for the case 4f.
bounded by the number of straight-line segments’. The
answer is “no” if we consider a single circular obstacle
Optimal coverage Now, we impose an additional (Figure 5).
constraint. We wish to provide coverage such that any
guard does not see any other one. Withdrawing a single 3Remark: computing the minimal number of guards
guard does not provide coverage anymore. For that constitutes the classical art gallery problem [23].
Notes on Visibility Roadmaps and Path Planning
321

Figure 6: Steerjin: Finite coverage may exist while finite

optimal coverage does not.
Figure 5: Steerjin: Examples
of optimal coverage with 4
and 6 guards. The number of guards achieving optimal cov-
the same component of the graph, and when their visi-
erage is not a constant number. It may be unbounded.
bility sets intersect, we chose a configuration in the in-
tersection set and we add it as a new node to the graph.
Also note that one cannot necessarily extract opti- Such a new configuration does not increase coverage,
mal coverage from any given set of guards providing it just makes the number of connected components of
coverage. More than that, finite coverage may exist the graph decreasing. It is called a connector.
while finite optimal coverage does not. In the example
By this way, it is always possible to build a graph
of Figure 6 finite coverage (with Steer;;,,) necessarily re-
capturing the connectivity of C’Sfree, ie., such that
quires two guards on the dashed lines. It is impossible
there is a one-to-one mapping between the connected
to make one of them not visible from the other one.
components of the graph and the connected compo-
nents of C'Sfree.
Coverage and robustness In several examples
above (Figures 4(d), 4(e), 5, and 6) some guards should Everything is now in place to define a retraction
belong to a domain whose dimension is strictly lower method solving the path planning problem. The re-
than the dimension of C'Sy,ee. In that sense, the cor- traction function applied to a configuration just con-
responding set of guards is not robust: The probability sists in selecting a guard seeing it. This is a well defined
to select such guards randomly is null. function since the set of guards covers CS free. Finally
some starting and goal configurations being given there
Capturing C’S;,e-e connectivity from coverage exists an admissible collision-free path between them if
In this section we show that coverage induces connec- and only if their retractions belong to a same connected
tivity. Let Steer be a symmetric stc steering method in component of the graph. The continuous dimension of
C'Sfree. Consider a given (finite or not) set of guards the path planning problem is then transformed into a
achieving coverage of C'Syree together with a (finite computational one.
length) path. The path is covered. Visibility sets are The graphs above have been introduced in path plan-
open sets. The path being a compact set, there is a ning literature with a computational point of view and
finite number of guards whose visibility sets cover that especially with a probabilistic one. Indeed computing
path’. such roadmaps with complete deterministic algorithms
This property means that it is always possible to is a difficult problem: For given C'Sfree and Steer, we
build a (finite or not) undirected graph accounting for do not know a priori if there exists finite coverage, and
the connectivity of CSfree. Two guards are connected even if there exists, we do not know how to compute one
if they are visible by Steer. At this level there does not of them in a deterministic way. The probabilistic ap-
exist necessarily a one to one mapping between the proaches relax the completeness property and tend to
cover OS free at the best. Experiences show that they
connected components of the graph and the connected
behave well for practical problems. This is the key of
components of CSfree. Nevertheless, it is always pos-
the success of the so-called probabilistic roadmaps.
sible to complete the graph. When two guards belong
to the same connected component of CS¥ree but not to When considering optimal coverage, the roadmap
resulting from the construction above is a bipartite
4This gives rise to a notion of complexity of a path with
graph: The connector nodes are adjacent only to guard
respect to a given set of guards: the complexity of a path is
the minimal number of guards necessary to cover the path. nodes and, from the definition of optimal coverage, a
322 J.-P. Laumond and T. Siméon

guard cannot see any other guard. Such roadmaps have

been recently introduced as visibility roadmaps in [26].
The following section overviews probabilistic algo-
rithms to compute roadmaps.

4 Probabilistic Visibility Roadmaps

Computing probabilistic roadmaps Consider a
small-time controllable mechanical system. Comput- Figure 7: The apparition order of the connectors are num-
ing a probabilistic roadmap for the system requires a bered. The visibility roadmap is not robust when connectors
collision checker and a symmetric stc steering method are not allowed to increase coverage (left): the probability
Steer that accounts for the kinematic constraints of the to find such a roadmap is null. The variant of the visibility
system. roadmap algorithm provides a robust roadmap (right).
The basic version of a probabilistic roadmap algo-
rithm generates configurations randomly. As soon as added as a new connector and the corresponding
a collision-free configuration is found, it is added as connected components are merged by adding the
a new node to the graph. Then the algorithm checks edges between the new connector and the guards
if the current configurations previously generated are seeing it,
visible by Steer from the new configuration. Each time
one of them is visible, a new edge is added; otherwise, 3. otherwise it is visible only by guards belonging to a
the new node creates a new connected component. A same connected component and then it is rejected.
less expensive version consists in restricting the con-
From a practical point of view, we have proposed
nectivity test to connect the new configuration to only
a variant of the algorithm [26] allowing connectors to
one node for each connected component of the current
increase coverage of C'Sfree: Connectors are added to
graph. In that case, the resulting graph is a tree.
the list of the guards. By this way covering CS free
The more time consuming step of the algorithm is is faster. Moreover, an advantage of this variant is to
the call of Steer. Indeed the power of the algorithm increase the robustness of the visibility roadmaps. In
is to avoid the computation of the visibility domains. Figure 7 we show that the visibility roadmap built from
Checking whether a configuration is visible from an- guards achieving optimal coverage is not robust, while
other one is done by checking if the path computed by the roadmap computed by the variant of the algorithm
Steer between them is collision-free. The number of is robust.
calls to Steer then appears as a critical parameter that
measures the combinatorial complexity of the construc- Comparison Figure 8 shows two examples of
tion. roadmaps computed for the same environment (polygo-
nal CS free with Steer;;,) with the same list of 250 ran-
Computing probabilistic visibility roadmaps dom configurations. The first one is a basic roadmap.
The general principle above should be adapted to com- The second one is a visibility roadmap. In both cases
pute visibility roadmaps. We have seen that visibil- the trees cover C'Sfree and capture its connectivity.
ity roadmaps are bipartite graphs with two classes of On this example, the visibility roadmap algorithm ran
nodes: The guards and the connectors. When a new 6 times faster than the basic roadmap algorithm.
collision-free configuration is (randomly) found, three
This simple example suggests a better performance
cases may arise:
of the visibility roadmap algorithm. They both cover
1. either it is not visible from any existing guard: and capture the connectivity of CSfree. The difference
Then it is added as a new guard to the graph (it between the performances is due to the fact that visi-
creates a new connected component), bility roadmaps keep a number of nodes smaller than
basic roadmaps. The number of calls to Steer in the
2. or it is visible by guards belonging to separate con- main loop of the algorithm is then smaller. This is
nected components of the current graph: then it is illustrated on Figure 9.
Notes on Visibility Roadmaps and Path Planning
323

Figure 10: A possible side-effect of the visibility roadmaps.

We have seen that the algorithm consists in reject-

ing the configurations which are reachable only from
nodes belonging to a same connected component of
Figure 8: A basic roadmap and a visibility roadmap.
the roadmap. Therefore, the expansion of the visi-
bility set of a connected component of the visibility
However, we do not succeed in proving formally a
roadmaps may be a priori slower than those of the basic
better behavior. Visibility roadmap algorithms may
roadmaps. However, in addition to the computational
fail (in probability) in capturing the connectivity of
gain discussed above, rejecting those configurations has
CS}ree. Guards may induce artificial narrow passages
an advantage: The number #¢ry of failures in adding a
as shown in Figure 10. In that case, the probability new guard to the visibility roadmap allows to compute
to put a connector belonging to visibility sets of both an estimation of the volume of the free-space remaining
guards is very low. With the same random sampling non covered. We model the percentage of the non cov-
of CSyree, the visibility roadmap will certainly fail in ered free-space as the inverse of the number of failures
capturing the connectivity of the space, while the ba- before adding a new guard. The plain curve in Fig-
sic roadmap will certainly succeed. Nevertheless, this ure 11 shows the evolution of this estimated percent-
behavior may appear as a side-effect of the algorithm, age of the covered free-space for a free-flying body. The
since the probability to fall in this special case is clearly horizontal axis represents the number of guards, while
low (see [24] for details). the vertical one represents (1 — sees For the dotted
Control of the algorithm A critical problem of the curve, the vertical axis represents the real percentages
probabilistic algorithms is first to stop them, and sec- of the covered free-space (the curve is monotonic and
ond to get some information about the quality of the increasing). The estimated percentage converges to the
roadmap in terms of both coverage and connectivity. real one when the number of guards increases.
The structure of the visibility roadmap algorithm al- Guards achieve good coverage when the curve be-
lows to control the quality of the roadmap in term of comes horizontal. Such behavior may be detected
coverage. within the algorithm by looking at the evolution of
the number #try. The algorithm stops when #try
becomes constant (in probability). Of course we may
also bound #try by a huge integer (a detailed version of

i ; the algorithm appears in [26]). In both cases, when the

algorithm stops we get an estimation of the percentage
of the non covered free-space volume.
aS

5 Reading Notes and Comments

Figure 9: Example of CSfree with two connected com- Probabilistic roadmaps were introduced at the begin-
ponents. Any new collision-free configuration should be ning of the ’90s at both Stanford and Utrecht Univer-
checked to know if it belongs to both connected components sity [13, 25, 15]. Ph.D. Theses of L. Kavraki [12] and P.
of CS free. Checking this 1s much more expansive in the Svestka [27] state with details the seminal foundations
case of the basic roadmap (leftt) than in the case of the of these new algorithms together with the first experi-
visibility roadmap (right). mental results which are at the origin of their success.
324 J.-P. Laumond and T. Siméon

portant because it opens the range of applications to

some locally controllable systems. Indeed, the property
holds for a car moving forward which is locally control-
lable but not small-time controllable. Due to the drift
usually present in that kind of systems, the roadmap
can no more be an undirected graph.

Algorithm analysis Capturing the connectivity of

geometric spaces by sampling techniques is related to
the percolation problem is statistical physics [11]. This
problem asks deep and challenging questions in math-
ematics. Analyzing the probabilistic roadmap algo-
8 rithms is a difficult problem. Several authors, mainly
around Latombe’s team, gave pertinent insights on
this issue.
Coverage
percentage
The notion of e-goodness is introduced in [5]. It is
related to the coverage property. CSyfree is e-good if
the volume of the visibility set of any configuration in
So Real coverage
CSfree is greater than some fixed percentage (1 — €) of
Coverage estimated by 1- aS#iry the total volume of C'S;,ee. The authors prove that if
reer! rn == C'S free is €-good the probability that a (basic) roadmap
0” 10"
# guards i does not cover C'Sfree decreases exponentially with the
number of nodes. Moreover the number of nodes in-
Figure 11: Convergence of the visibility roadmap algo- creases moderately when € increases. Notice that the
rithm for a free flying body. notion of e-goodness is not strictly related to the exis-
tence of a finite set of guards achieving coverage. In-
In this section we comment on several issues tending deed, for the cases illustrated in Figures 4(a), 4(b),
to understand their behavior on the basis of recently 4(g) and 4(h), CSfree is €-good, while it is not for the
published papers. cases 4(c), 4(d), 4(e) and 4(f). Finite coverage may
exist for non €-good spaces. Moreover, the number of
Devising adequate steering methods In our pre- guards achieving optimal coverage may be unbounded
sentation, we assumed that the considered steering even for e-good spaces (Figure 5).
methods are both symmetric and stc. The symme-
The notion of expansiveness introduced in [8] deals
try property allows to compute undirected roadmaps,
with connectivity. The proposed model is rather tech-
while the stc property guarantees the convergence of
nical. It deals with the notion of narrow passages
the probabilistic algorithms.
in CSfree and the difficulty to go through them. It
Devising stc steering methods is not necessarily an is shown that for expansive CSf,ee the probability
easy task especially when we consider nonholonomic for a (basic) roadmap not to capture the connectiv-
systems. An overview on this aspect may be found ity of CSyfree decreases exponentially with the number
in [21]. of nodes.
In his PhD thesis, P. Svestka relaxes the stc property. The clearance of a path is also a pertinent factor.
He shows that the steering methods may verify the fol- In [14] a bound on the number of nodes required to
lowing topological property: The set of configurations capture the existence of a path of given clearance is
reachable by paths lying in an arbitrarily constrained provided. This bound depends also on the length of
domain contains a neighborhood in that domain (and the path. In [28] the dependence on the length is re-
not necessarily a neighborhood of the starting config- placed by the dependence on the number of visibility
uration as for the stc property). This property is im- sets required to cover the path. Again, the probability
Notes on Visibility Roadmaps and Path Planning
325
to fail in capturing the existence of a path decreases than a simple collision-checking): In a first step, con-
exponentially with the number of roadmap nodes. figurations inducing small penetrations between bodies
Finally, the dependence on the dimension of C'S is are accepted, then they are progressively moved to fi-
discussed in [10]. nally be collision-free [9].
All these results are based on parameters character- Sampling close to the obstacles is a good strategy
izing the geometry of C'Ss,2¢. The knowledge of these to capture the connectivity of narrow passages. Never-
theless, it may induce undesirable side effects when the
parameters would allow to control the algorithms. Un-
fortunately they are a priori unknown. As commented space is not very cluttered: In the example shown in
Figure 5, sampling close to the obstacle requires more
in [10], “one could be tempted to use Monte Carlo tech-
nique to estimate the values of [such parameters] in a configurations than sampling far from the obstacle.
given free space, and hence obtain an estimate of the Experiences conduced on the visibility roadmaps
number of [nodes] needed to get a roadmap that ade- show that the time gained by constructing small
quately represents [CSfree]. But it seems that a re- roadmaps may be used to concentrate the search on
liable estimation would take at least as much time as regions not connected to the roadmap. In other words,
building the roadmap itself”. An idea could be to try for a given fixed running time, the visibility roadmap
to estimate these parameters during the construction algorithm will explore more space than the basic one.
of the roadmaps. This is the underlying principle to Visibility roadmaps behave rather well to capture nar-
control the visibility roadmap algorithm above. Gener- row passages even if they do not have been devised
alizing it to estimate the e-goodness, the expansiveness to this end. The visibility roadmap algorithm runs
of the spaces or to detect narrow passages seems to be a 20 times faster than the basic roadmap algorithm to
promising issue to be investigated. The main challenge capture the connectivity of CSyree in the example of
is to not degrade the performance of the algorithm with Figure 12 (see [26] for details).
sophisticated computations.
On the choice of steering methods By definition,
On heuristics guiding probabilistic searches stc steering methods verify the topological property il-
The same challenge appears to improve the basic lustrated in Figure 2. This means that any path com-
roadmap algorithm behavior when facing difficult prob- puted with a given method may be approximated by
sub-paths computed with another one. Nevertheless,
lems.
we have seen that the combinatorics of a roadmap de-
Capturing the connectivity of CSree in the presence pends on the considered steering method. Depending
of narrow passages is one of them. In such contexts a on the shape of CSfree a steering method may per-
first improvement has been introduced in [16]: The al- form better than another one. In Figure 13 we can see
gorithm concentrates the search around small isolated that, for the same example of environment, Steerj;n in-
components of the roadmaps. Another idea consists duces a smaller roadmap than Steer,,a,. Moreover,
in sampling C’Sf,ee with configuration close to the ob- the construction of the roadmap is much faster with
stacles. This can be done for free-flying systems by
analyzing the contact configuration space, and by sam-
pling contact subspaces defined by some geometric con-
straints like polyhedron vertex on polyhedron facet [3].
Contact space may be randomly sampled without
using any explicit constraints. In [7] the method gen-
erates a pair of configurations separated by a random
distance. When both configurations are either both
collision-free or both in the collision space, they are
rejected. Otherwise, the collision-free configuration Figure 12: Translating square (grey) with Steerzin: The
is kept. visibility roadmap algorithm (right) performs 20 times bet-
ter than the basic roadmap algorithm (left) in capturing the
Another way to capture narrow passages is to exploit
expensive connectivity of CSfree-
distance computations (an operation more
326 J.-P. Laumond and T. Siméon

Figure 13: Two visibility roadmaps computed with

Steerjin (left) and Steetman (right).

Steer;;,, than with Steerman. However, such a behav-

ior is specific to the example. In the case of the rolling
bridge in Figure 1, our experiments show a performance
gain of 10 when using Steerman instead of Steerjin.
This is a consequence of the special geometric struc-
ture of the workspace to be explored by a rolling bridge.
Steerman is more suitable than Steer;;,, in that case. Figure 14: A real-size problem.

Therefore there is a priori no arguments to conclude

on general results guiding the choice of a suitable steer- data structures®. The crane was added within Move3D.
ing method. This choice should be done after practi- Its workspace a priori covers all the environment. The
cal experiments on special classes of systems and en- model contains around 300.000 polygonal facets. Here
vironments. For instance, in [4] several steering meth- the difficulty is to face the geometrical complexity of
ods working for free-flying rigid bodies are compared the environment in a reasonable time.
from a practical point of view. In particular a steering We are working on new hybrid collision-checkers
method consisting in decoupling translation and rota- combining the techniques developed in [22] and allow-
tion is shown performing well. ing to process polyhedra together with volumic primi-
tives (e.g., spheres, tubes, torus...) as in [6].
6 Current Directions On the other hand we are testing incremental ap-
proaches to roadmap constructions. The principle con-
We began the integration of various probabilistic sists in partitioning the C'S space of the crane into
roadmap algorithms within a same software platform? elementary regions. Then for each corresponding ele-
Move3D. All the examples displayed in Figure 1 have mentary workspace, we apply a filter on the geomet-
been computed within Move3D by using the visibility ric database to select the bodies to be handled by the
roadmap algorithm. The generality constraint intro- collision-checker. Elementary roadmaps are computed
duced as the main motivation of this work has been in each C'S regions and then merged together.
respected.
Another challenging issue is to provide the operator
From a practical point of view, it remains to show
with facilities for combining several handling devices
that such techniques may face real size problems. The
to carry freights. Path planning should be extended to
environment of Figure 14 represents a canonical exam-
handling task planning. This problem is often refer-
ple we are working on. The industrial installation is a
enced as the manipulation planning problem. Its geo-
stabilizer (subset of a plant in chemical industry). The
geometric model was translated from PDMS geometric °PDMS is a product of Cadcentre Ltd, partner of LAAS-
CNRS, EDF and Utrecht University within the MOLOG
°http: //www.laas.fr/~nic/Move3D project.
Notes on Visibility Roadmaps and Path Planning
327
metrical formulation [2] allows to apply probabilistic planners for probabilistic roadmap methods. IEEE In-
approaches. Results already appeared in [18] in the ternational Conference on Robotics and Automation,
domain of digital actors animation and in [1] in the Leuven (Belgium), 1998.
context of manufacturing. [5—s J. Barraquand, L. Kavraki, J.C. Latombe, T.Y. Li, R.
A priori such issues do not ask for new fundamental Motvani and P. Raghavan. A random sampling scheme
for path planning. International Journal of Robotics
research. They require incremental research extending
Research, Vol 16 (6), 1997.
at best the existing state of the art.
From a theoretical point of view, the formal analyses
[6] G. van den Bergen. Efficient Collision Detection
of Complex Deformable Models using AABB Trees.
referenced in Section 5 have opened the route toward a Journal of Graphics Tools, Vol 2 (4), 1997.
better understanding of the behavior and performance
of probabilistic roadmap algorithms. Moreover addi-
[7] V. Boor, M. Overmars and A. Frank van der Stap-
pen. The Gaussian sampling strategy for probabilistic
tional work remains to be done to better control the roadmap planners. JEEE International Conference on
probabilistic roadmap algorithms. A lot of information Robotics and Automation, Detroit (USA), 1999.
is usually lost when running probabilistic roadmap al-
[8] D. Hsu, J.C. Latombe and R. Motwani. Path planning
gorithms. For instance the configurations belonging to in expansive configuration spaces. IEEE International
an obstacle are rejected. Counting them may give an Conference on Robotics and Automation, Albuquerque .
estimation of CSree volume. Memorizing their loca- (USA), 1997.
tion may help in estimating the shape of the obstacles. [9] D. Hsu, L. Kavraki, J.C. Latombe, R. Motwani and
At the same time, it may be possible to estimate on line S. Sorkin. On finding narrow passages with proba-
the expansiveness of a node. The problem is to avoid bilistic roadmap planners. Robotics: The Algorithmic
increasing the cost of the algorithm by expensive oper- Perspective (WAFR98), P. Agarwal et al (Eds), AK
Peters, 1998.
ations. The visibility roadmap algorithm commented
in this paper is an attempt in this research issue. [10] D. Hsu, L. Kavraki, J.C. Latombe and R. Motwani.
Capturing the connectivity of high-dimensional geo-
metric spaces by parallelizable random sampling tech-
Acknowledgments niques. Advances in Randomized Parallel Computing,
P.M. Pardalos and S. Rajasekaran (Eds.), Combinato-
rial Optimization Series, Kluwer Academic Publishers,
We thank Carole Nissoux who has been the kingpin of
Boston, 1999.
Move3D.
This work is supported by the European Esprit [11] G. Grimmet. Percolation. Springer Verlag, 1989.

Project 28226 MOLOG (http://www.laas.fr/molog). [12] L. Kavraki. Random networks in configuration spaces
for fast path planning. PhD Thesis, Dept of Computer
Science, Stanford University, 1995.
References
[13] L. Kavraki and J.C. Latombe. Randomized pre-
processing of configuration space for fast path plan-
[1] J.M. Ahuactzin, K.K. Gupta and E. Mazer. Manipu- ning. [EEE International Conference on Robotics and
lation planning for redundant robots: a practical ap-
Automation, San Diego (USA), 1994.
proach. Practical Motion Planning in Robotics, K.K.
Gupta and A.P. Del Pobil (Eds), J. Wiley, 1998. [14] L. Kavraki, L. Kolountzakis and J.C. Latombe. Analy-
sis of probabilistic roadmaps for path planning. IEEE
[2) R. Alami, J.P. Laumond and T. Siméon. Two manip- International Conference on Robotics and Automa-
ulation planning algorithms. Algorithmic Foundations tion, Minneapolis (USA), 1996.
of Robotics (WAFR94), K. Goldberg et al (Eds), AK
Peters, 1995. [15] L. Kavraki, P. Svestka, J.C. Latombe and M.H. Over-
mars. Probabilistic Roadmaps for Path Planning in
[3] N. Amato, O Bayazit, L. Dale, C. Jones and D. High-Dimensional Configuration Spaces, IEEE Trans-
Vallejo. OBPRM: an obstacle-based PRM for 3D actions on Robotics and Automation, Vol 12 (4), 1996.
workspaces. Robotics: The Algorithmic Perspective
(WAFR98), P. Agarwal and all (Eds), AK Peters 1998. [16] L. Kavraki and J.C. Latombe. Probabilistic roadmaps
for robot path planning. Practical Motion Planning
[4] N. Amato, O Bayazit, L. Dale, C. Jones and D. in Robotics, K. Gupta and A. del Pobil (Eds), Wiley
Vallejo. Choosing good distance metrics and local Press, 1998.
328 J.-P. Laumond and T. Siméon

[17] L. Kavraki, M. Kolountzakis and J.C. Latombe. [23] J. Goodman and J. O’Rourke. Handbook of Discrete
Analysis of Probabilistic Roadmaps for Path Planning, and Computational Geometry. CRC Press, 1997.
IEEE Transactions on Robotics and Automation, Vol
14 (1), 1998. [24] C. Nissoux. Visibilit et mthodes probabilistes pour la
planification de mouvement en robotique. PdD Thesis
[18] Y. Koga, K. Kondo, J. Kuffner and J.C. Latombe. (in french), University Paul Sabatier, Toulouse, 1999.
Planning motion with intentions. ACM SIGGRAPH,
Orlando (USA), 1994.
[25] M. Overmars and P. Svestka. A Probabilistic learning
approach to motion planning. Algorithmic Founda-
[19] J.C. Latombe. Motion planning: a journey of robots,
tions of Robotics (WAFR94), K. Goldberg et al (Eds),
molecules, digital actors, and other artifacts. The In-
ternational Journal of Robotics Research, Vol 18 (11), AK Peters, 1995.
1999.
[26] T. Siméon, J.P. Laumond and C. Nissoux. Visibility-
[20] J.P. Laumond (Ed.). Robot Motion Planning and Con- based probabilistic roadmaps for motion planning.
trol. LNCS 229, Springer Verlag, 1998 (out of print, (Submitted to Advanced Robotics Journal. A short
available freely at http: //www.laas.fr/~jpl1/). version appeared in IEEE IROS, 1999).

[21] J.P. Laumond, F. Lamiraux and S. Sekhavat. Guide-

[27] P. Svestka. Robot motion planning using probabilistic
lines in nonholonomic motion planning. In [20].
road maps. PhD Thesis, Dept of Computer Science,
Utrecht University, 1997.
[22] M. Lin, D. Manocha, J. Cohen and S. Gottschalk.
Collision detection: Algorithms and applications.
Algorithms for Robotic Motion and Manipulation [28] P. Svestka and M. Overmars. Probabilistic path plan-
(WAFR96), JP. Laumond and M. Overmars (Eds), ning. In [20]
AK Peters, 1997.
AutoBalancer: An Online Dynamic Balance
Compensation Scheme for Humanoid Robots
Satoshi Kagami, University of Tokyo, Tokyo, Japan
Fumio Kanehiro, University of Tokyo, Tokyo, Japan
Yukiharu Tamiya, Namco Ltd, Tokyo, Japan
Masayuki Inaba, University of Tokyo, Tokyo, Japan
Hirochika Inoue, University of Tokyo, Tokyo, Japan

Algorithms for maintaining dynamic stability are 1 Introduction

central to legged robot control. Recent advances in com-
Algorithms for maintaining dynamic stability are fun-
puting hardware have enabled increasingly sophisticated
damental to efforts to develop practical humanoid ro-
physically based simulation techniques to be utilized for
bots that can successfully operate in the real world. A
the offline generation of dynamically-stable motions for
humanoid robot may have to perform several concur-
complez robots, such as humanoid robots. However, in
rent tasks, for example: walking, carrying an object
order to design humanoid robots that are reactive and
in its arms, and maintaining visibility of a target. In
robust, a low-level online balancing scheme is required.
order to achieve such tasks, both the desired high-level
This paper presents an online algorithm for auto- behaviors and the low-level dynamic stability should
matically generating dynamically stable compensation be satisfied simultaneously.
motions for humanoid robots. Given an input mo-
Developing techniques for bipedal locomotion has
tion trajectory, the “AutoBalancer” software reactively
been an active area of research for decades. Many ap-
generates a modified dynamically-stable motion for a
proaches have been proposed to bipedal walking pat-
standing humanoid robot. The system consists of two
tern generation [1, 2, 3]. This work can be divided
parts: A planner for state transitions derived from con-
into two major directions: (1) torque-based online tra-
tacts between the robot and the ground, and a dynamic jectory generation methods, and (2) position based of-
balance compensator which formulates and solves the fline trajectory generation methods. The torque-based
balance problem as a constrained, second order nonlin- approach has many advantages, including conceptual
ear programming optimization problem. The balance simplicity and adaptability to rough terrain. However,
compensator can be made to compensate for deviations in order to achieve torque-based control, the dynamic
in the centroid position and tri-azial moments of any equations of the system must be solved. Since this is
standing motion for a humanoid robot, using all joints usually difficult, most existing solutions use model sim-
of the body in real-time. The complezity of the Auto- plification assumptions such as ignoring the weight of
Balancer algorithm is O((p + c)*), where p is number the leg.
of DOFs and c is the number of constraint equations.
Position-based offline trajectory generation has been
We describe results obtained by an experimental im- adopted mostly using ZMP[4] constraints since a hu-
plementation of the AutoBalancer algorithm that has manoid type robot has many degrees of freedom. The
been applied to 16-DOF and 30-DOF humanoid robots, current methods for computing dynamically stable tra-
controlled via a master-slave puppet interface. jectories can be categorized in three ways: (1) heuristic
search such as GA (genetic algorithm), (2) problem op-
timization such as gradient-descent methods, and (3)
solving the problem using a simplified model and it-
eratively calculating solutions to more detailed models
330 S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba, and H. Inoue

ae
wa
(Le
nee
WEAN y.
‘PND.

Standing
Boundary on Both Legs
Standing :
Condition Pstandinig
Motion Name
1 OnLLeg 4LBoundary2Legs 9 RBoundary2RLeg
2LLeg2LBoundary 5Legs2LBoundary 10 RLeg2RBoundary
3 LBoundary2LLeg 6 OnLegs 11 OnRLeg
7 Legs2RBoundary
8 RBoundary2Legs

Standin Standing
ayLeft | Leg
on on Right Leg

Figure 1: Classification of Standing Motion States and their Transitions.

(converging towards the actual model). These schemes 2. The robot tries to follow the input motion while
have been successfully used to generate walking pat- satisfying the dynamic balance constraints.
terns for humanoid type robots [5, 6, 7]. 3. All joints or selected joints can be used in order to
maintain balance.
This paper describes an online dynamic balance sta-
4. No assumptions are made regarding the dynamic
bilization algorithm for humanoid robots that works by
stability of the input motion.
enumerating possible contact state changes, and com-
5. Information regarding the future input motion is
pensating for dynamic instabilities using all joints of
unavailable, aside from the current posture.
the body in real-time. We formulate and solve the bal-
6. The floor and the soles of the robot’s feet are both
ance compensation problem as an optimization prob-
planar.
lem. Our method can be applied to full-body motions
7. Disturbances from the environment are assumed
of humanoid robots that stand on one or two legs. Al-
to be slow.
though our experiments use bipedal humanoid-type ro-
bots, the algorithm can generally be applied to any
AutoBalancer is designed as a two-layered architecture:
type of legged robot.
the Planner manages transitions between states, while
The paper is organized as follows: Section 2 out- the Compensator keeps the robot balanced in the cur-
lines the design of the AutoBalancer algorithm. Sec- rent state.
tion 3 explains the balance compensation method in
detail. Section 4 describes experiments using a 16-DOF 2.1 States and Transitions for a Standing
remote-brained robot and 30-DOF child-size humanoid Humanoid Robot
robot. Section 5 contains some concluding remarks and
discussion. We identify the nature of contacts between the ro-
bot and the ground by enumerating states. The ro-
bot moves by making transitions between these states.
2 Design of AutoBalancer Figure 1 describes the five states and eleven transitions
used by our system. Boundary condition states repre-
“AutoBalancer” makes the following assumptions: sent the special cases when both soles of the feet are in
contact with the ground, but the entire weight of the
1. A filter function is used for maintaining dynamic robot is supported by only a single foot. Transitions
balance in real-time. between the single-leg contact states and the dual-leg
AutoBalancer: An Online Dynamic Balance Compensation Scheme
331
contact state must pass through the boundary condi-
tion states. Additional transitions include: positioning
the robot centroid above a sole, and lifting up or lower-
Ideal
ing a free leg to the ground. AutoBalancer implements
Centroid
balance compensation functions for every state transi-
Projection
tion, and switches between these functions according
to the robot state.
foot x y
2.2 Planner

The planner controls the robot state and transitions 0 dy

according to Figure 1. In this paper, we assume that
direct transitions can only be made between adjacent Figure 2: Limitation of Moment.
states. Therefore we have five states and eleven tran-
sitions (including self-transitions which return to the
current state). Four behaviors are introduced in order e Constrain the moment of inertia M around the
to control a humanoid robot as follows: supporting sole by a predefined tolerance value .
which is calculated from the shape of the sole and
the leg state.
1. Single-leg behavior
2. Dual-leg behavior In this paper, the shape of the sole is assumed to
3. Lifting up a foot behavior be rectangular, and the constraints are defined as in
4. Lowering a foot behavior Figure 2.
The constraints can be formalized by using the cur-
The planner controls the state transition from a rent joint angle @ and the joint angle difference 60 at
given input by using these four behaviors. The plan- each control cycle as follows:
ner manages the state transitions to maintain stability
even when the input state changes very quickly, so it
can cope with highly dynamic situations. The basis
of our dynamic balance compensation method and the
details for each behavior are given in the next section. Oy 1d <a ( )

—dzik < M,(@, 68) < dz 2F, and

3 Dynamic Balance Compensation —pF < M,(0,60)

The axial components of the center of grav-

3.1 Center of Gravity and Moment of Inertia
ity and moment of inertia are denoted by
Constraints
60Cz, 0Cy, Mz,
M,, Mz. Let F be a force from
Basic Control Strategy for Dynamic Balance the ground, and dz1,dz,2,dy,1,dy,2 be the distances
from the edge of the supporting sole along each axis.
The moment of inertia M and center of gravity of Let p be the coefficient of static friction. Then Equa-
the robot C are utilized in order to maintain dynamic tion 1 and 2 constrain the center of gravity to lie along
balance. We introduce the desired point of the pro- the perpendicular line of DPCGG, given that DPCGG
jection of center of gravity to the ground, and name already lies inside the support polygon. Equation 3
it DPCGG. The following schemes are the basic con- and 4 is a ZMP constraint to avoid falling down by
straints for dynamic balance using DPCGG: rotating around the foot edge (Figure 2). Equation 5
is a condition to prevent the foot from sliding around
e Constrain the center of gravity C to lie along the the Z-axis. The following two subsections describe the
perpendicular to the pre-defined DPCGG, inside relationship between these constraints and each joint
the sole which is supporting the body. motion, in order to achieve stable motion.
332 S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba, and H. Inoue

Formulation of Center of Gravity Constraints

Let the joint angle vector of the robot be @. C,

the center of gravity, is a function of @ and it can be
described as follows using a function X which is char-
acterized by the individual robot. Ideal
Centroid
< Projection

C = X(6) (6) virtual link x 3

length : 0
The Jacobian of the center of gravity J is a partial weight : 0
derivative function matrix relating differential changes
of @ (i.e., the vector of rotation angles for all DOFs)
to differential changes in X. Then the velocity of the
center of gravity C and the angular velocity @ can be Figure 3: Virtual Link for Calculating ZMP.
obtained as follows:

can approximate J by setting the robot’s current pos-

C = J(0)8, (7) ture to @ and perturbing each individual joint angle
to measure differential changes in the relative position
3) = PO). (8) of C. Since the calculation of the center of gravity is
O(p), the calculation of the approximate J is O(p”).
Let the change in the joint angles at time t be 60(t), Formulation of Moment of Inertia Constraints
and suppose that each component is small enough. The
differential changes of the center of gravity can be ex- In this section we introduce a system of three virtual
pressed as follows, using Equation 7. links at DPCGG in order to calculate inertial moments.
The axes of the virtual links are parallel to the coordi-
nate system shown in Figure 2. The moments of inertia
6C = J(0)50(t) around DPCGG can be calculated as the joint torque
(9) of the virtual joints which keeps the virtual joint an-
gle constant. In general, the motion equation can be
Assuming that the center of gravity is on the perpen-
represented as follows:
dicular to the DPCGG, then the condition to restrict
its derivatives on the perpendicular line of DPCGG can
be represented as follows from Equation 9.
T = H(0)0+0(6, 6,9), (12)
where T is a vector of p joint torques, H is an inertia
Pp
matrix of size p x p, and b is a vector of size p which
6Cn =)! Jz260;(t) = 0, (10)
ih
combines the centrifugal, coriolis, and gravity forces.
Pp However, we consider only the inertial components of
OC =) 90700;(t) 20) (11) b, and ignore the gravitational component.
4=1 From Equation 12, the relationship between the mo-
ments of inertia around each axis M,, M,, M, and the
where 7 is the joint index, z,y are the motion di- joint angle changes at each control cycle can be calcu-
rections, 00; is the differential angle change for joint lated. If the virtual joint does not move, 6@ can be
1, Jz,i,Jy,¢ are the components of J, dCz,dC, is the obtained as follows:
differential motion of the center of gravity, and p is the
Pp
number of DOFs. Since J is a function of 0, it must
Vee (>Hy i6; + ) (13)
be calculated at every control loop cycle. However, we
t—=1
AutoBalancer: An Online Dynamic Balance Compensation Scheme
333

Using this function, and setting some input compo-

nents to zero [9], H and b can be solved as follows:

Coords 2 H(@)columni = INV(0,O,e;:), (18)

Py2Ft42
b(0, 8, 9) = INV(6,0,O). (19)
Link2
Where e; is a vector whose ith component is 1 and
all other components are 0. Equation 18 is calculated
three times, once for each of the required components
Figure 4: Relative Position and Rotation between Link1 of the virtual linkage. Thus, INV() is applied four
and Link2.
times in calculating H and 6b, and the computation is
O(p).
Pp

M, =- (>:
Hy i6; + ,); (14) Balance Compensation as an Optimization Problem
Gail
Let 60ema be the differential input joint angle vector,
d0ret be the component to approach to the input joint
M, =— (>H.i0; + .) (15)
P -

angle vector, 0@comp be the compensation component

2—
which satisfies the balance constraints. Then 6@ can
be represented as follows:
Since 6 is described as a difference, 6 is approximated
as:
60 = b0cema + OO ret + OO comp- (20)

6;(t) ©
9;(t) — Oi(t — dt) Given 68c¢ma, we can calculate 60,¢¢. Using the con-
bt ; dition of Equation 20, we can obtain constraint equa-
tions with d@comp as a variable. From the right side of
Equation 20, it follows that 6@ becomes similar to the
given input when d@comp is close to zero. Therefore,
By plugging these expressions into Equations 3, 4, we can find a d@comp Which minimizes an evaluation
and 5, we see that the constraint equations for the mo- function as follows:
ments of inertia are linear, just like the constraints on
the position of the center of gravity. H,, b can be calcu-
lated using a fast algorithm for inverse dynamics based
=> Shear) (21)
on the Newton-Euler method|8}.
The inverse dynamics problem consists of comput-
where w; is a positive weight variable. Large values
ing an appropriate set of input torques 7 given a set
of joint angles 6, joint angle velocities 8, and joint an- for w; cause dOcomp,i to be closer to zero, which in
gle accelerations 6. This algorithm solves the inverse turn causes the ith joint angle to more closely match
dynamics problem for a serial-chain manipulator with the given input. The factor w; essentially attempts to
encode the importance of the ith joint angle.
rotational joints, but it can be applied to robots with
a tree-like link structure (see Figure 4). Let the in- The three inequalities of Equations 3, 4, and 5 form
verse dynamics algorithm be denoted by INV(). The a second order programming problem. We can con-
solution torques can be computed as follows: vert each inequality to an equality, and use Lagrange
multipliers to solve this system.
560 can be obtained by solving Equation 21. Thus,
+ = INV(6, 0,8). (17) balance compensation can be solved as a second-order
334 S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba, and H. Inoue

programming problem in every control loop. The com- result in leg collisions are hard to formulate. In our
putational complexity of this problem is O(3” x (p+c)?) current implementation, the user of the system must
where n is the number of inequality constraints, p is take care to detect and prevent possible interference
the number of DOFs, and c is the total number of con- between the legs.
straints. Since Equations 1-5 consist of exactly three
Solving Single-leg Balance Compensation
inequalities, then n is a constant number.
Since all constraints in Equation 1-5, and 26 are
Limiting the Momentum
represented as linear equations in 66, by simply
We would like to impose limits on the moments of in- substituting 6C0,,6Cy,Mz,M,,Mz in Equation 20 and
ertia by considering the accumulated angular momen- OO te ft_crotch-y) 00 right-crotch_y in Equation 26, the opti-
tum of the robot. Consider the x-axis direction. Let mal d0comp can be obtained.
Mz,min; Mzmaz be the maximum and minimum mo-
ment of Equation 3. The limit values for the angular 3.3. Dual-leg Balance Compensation
momentum of the robot Lz min, Lz,max can be defined
In the dual-leg state, both legs cannot move inde-
as follows:
pendently, so a geometrical joint constraint method is
adopted to constrain the relative position and posture
of both feet. We treat the dual-leg state as a special
Lmin =a Mz mazte, (22)
case of a single-leg state using the following assump-
Lig. man = —Mz mints. (23) tions: without loss of generality,

where t, is the given time to stop the motion. The e one leg is designated as the supporting leg, while
angular momentum L, can be calculated as follows, by the other leg is free.
integrating both sides of Equation 12:
e the foot shape of the supporting leg is rectangular
and included in the convex hull of both feet (we
refer to this as the virtual sole, see Figure 5)

e both the relative position and posture are geomet-

rically constrained between the two feet.
Therefore, the angular momentum can be limited by
setting the moment limit values as follows, using dt: e the DPCGG is placed along the line segment con-
necting the DPCGG of each individual foot.
Le mon, Bes DL, ees Ta BE

ot chwavin: ot (25) Formulation of Geometric Joint Constraints

Similarly, moment limit values can be defined for the When both legs are contacting the ground, a closed
y and z axes. loop is formed and all the links are constrained geo-
metrically. In this section, we describe a constraint
3.2 Single-leg Balance Compensation method between two distant links. From the system
Leg Interference
of local coordinates Coords; which is attached to one
link, the posture of the local coordinates Coords2 of
During single-leg motions, the free leg may collide the other link can be represented by a translation vec-
with the supporting leg. To help prevent this, we limit tor p;_,) and a rotation vector rj_,2 (Figure 4).
the y-direction of the crotch joint, so that the angles
For every control cycle, the posture change between
are not changed from the given input:
two links 6p,_,, 6rj-,2 can be represented as a func-
tion of 6@ using the Jacobian K as follows:
OU le feteroteh a hi OOright_enotch = 0. (26)

However, this constraint is not sufficient to guarantee

that no collision will occur, since all the cases that {P12
OT 142
i= K60. (27)
AutoBalancer: An Online Dynamic Balance Compensation Scheme
335

OnLLeg Fix above DPCGG Sole of Equation.1-5

E | O((p + 6)°)
OnRLeg on Supporting Foot Supporting Leg Equation 26
Fix above DPCGG Virtual Sole Equation. Fs1—5
on Virtual Sole
O((p + 11) )
Equation 27
LLeg2LBoundary | Fix above DPCGG Sole of Equation.1—5 | O((p+11)%)
RLeg2RBoundary | on Supporting Foot
(
Supporting Leg Equation 26
Fix above DPCGG Sole of Equation. 1—5
RBoundary2RLeg | on Supporting Foot
-5 | O((p+ 11)")

LegiRBoundary |on One Le ER aaraEWe lier-5

Approach above DPCGG | Virtual Sole

Approach above DPCGG | Virtual Sole

Equation.1—5

Equation.1-5 | O((p+
| O((p + 11)")

RBoundary2Legs | on Virtual Sole

( 11)%)
neuen ae Equation 27

Table 1: Control Condition in Compensator.

Standing on Legs Standing on One Leg

Cx \ erst

ees ) \ ein
niet ate tea)
ere’
ea
— |
“ie Minimum Jerk
ylbe, shasie Ly, Trajectry

foot of free leg

supporting virtual
is restricted sole
polygon
Target Sole Polygon

Figure 5: Standing in the Dual-leg State.

Figure 6: Lowering a free leg to the ground.
An approach similar to the computation of the
balance constraints defined for the single-leg state is desired sole position (DSP). The motion can be calcu-
adopted. Equation 27 gives a linear equation of 60. lated as follows:
The Jacobian K is calculated at every control cycle
using the 3D model of the robot. 1. Plan a trajectory from the current sole position to
the DSP.
Solving Dual-leg Balance Compensation

Since every constraint in Equations 1-5 and 27 is 2. Calculate the free leg position and posture dp,_,9
represented as a linear equation of 66, by assigning and 6rj_52 at every control cycle, and impose geo-
6Cz,6Cy,Mz,M,,Mz to Equation 20 and 6p,_,2, 67152 metrical constraints, using Equation 27.
to Equation 27, the optimal 60-omp can be obtained.
The trajectory of the free leg is calculated using a
3.4 Balance Compensation While Lowering minimum jerk model[10] to generate a smooth motion
the Foot of a Free Leg (Figure 6).

In this section we describe the geometrical constraints 3.5 Balance Compensation While Lifting
used for the state transition from the single leg state a Foot
to the boundary of the dual leg state. The constraints
result in a motion that lowers the foot of the free leg At the start of a lifting motion, the sole of the free leg
to the ground. We name the foot landing position the is attached to the ground. Thus, the robot may cause
 S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba, and H. Inoue

E44 |
44 AON
BA f
0.0 02 04 06 08 1.0 1.2 14 16 1.8
time[sec]

Figure 8: Balance Compensation for Kick Motion.

fe
free leg has little effect upon the balance of the
whole body, it can follow the input very quickly.
Thus, when lifting a foot, the free sole is con-
strained to be parallel to the ground.
Stretching Knee
After some pre-defined time, the system transitions
L__) to a single leg state.

3.6 Complexity of Balance Compensation

Keeping Inclination Sirs eat bi
Table 1 shows the constraint equations for each state.
In each state, the program solves 5 or 11 constraint
Figure 7: Control in lifting a leg. equations including 3 inequalities (Equations 3, 4, and
5). Recall that the computational complexity for cal-
culating J is O(p?), and the complexity for comput-
itself to fall down by inadvertently kicking the ground.
ing H,b are O(p). Our program calculates an LU-
Therefore, we introduce the following constraints along
decomposition with partial pivoting in order to solve
with the single-leg state constraints, in order to not
the second order programming problem. The compu-
touch the ground during a lifting motion (see Figure
tational complexity of this process is O(p + c).
“ae

4 Interactive Humanoid Experiments

a. Supporting Knee Joint
The robot bends the knee of the supporting leg at
Using AutoBalancer
the boundary of the standing state. By augment- 4.1 Humanoid Robot Model
ing the variable w; in Equation 21 corresponding
to the weight of the knee joint temporarily, the A 3D robot model is required to calculate the dy-
knee can directly follow the given motion. namics. We treat the robot as a set of rigid bod-
ies connected by rotational joints. For each link we
b. Free Sole Constraint have the mass, position of the center of gravity, iner-
Since the motion caused by the ankle joint of the tia tensor matrix, and information about the adjacent
AutoBalancer: An Online Dynamic Balance Compensation Scheme
337

motion, the knee joint of the free leg is bent in the

weighted version, and the z-axis moment is compen-
sated by the arm motions.

4.3 Experiment using Humanoid “H5”

The remote-brained robot Akira has only 16DOFs. We
also applied AutoBalancer to the 30DOF humanoid
“H5”. H5 stands 1300mm high, and weighs 33kg. It
has 6 DOFs in each leg, 6 DOFs in each arm, 1 DOF for
each gripper, and 4 DOFs in the head. It has 12 force
sensors at the bottom of each sole which are used to
measure the ZMP. It has a PentiumIII-333MHz PC/AT
clone mounted on the body. It uses a Imsec motor
Figure 9: Direct Operation Using a Puppet.
servo loop, and 20msec Compensator. Figure 11 shows
a high kick motion using H5.
links. This robot model is implemented in our robot
model environment[11] based on Euslisp[12]. In order 5 Concluding Remarks
to achieve real-time motion, the robot model is com-
piled into C code, which is used to control the real This paper describes “AutoBalancer” which reactively
robot. generates the stable motion of a standing humanoid ro-
bot on-line, from given motion patterns. The system
4.2 Remote-Brained 16DOFs Humanoid consists of two parts: (1) a planner for transitions be-
“Akira” and Master-Slave Interface tween states derived from the contacts between the legs
and the ground, and (2) a dynamic balance compen-
We implemented our approach on the remote-brained sator which maintains dynamic stability through solv-
16DOF Humanoid robot “Akira.” We controlled the ing a constrained second order nonlinear programming
robot using a puppet-based master-slave interface (see optimization problem. The dynamic balance compen-
Figure 9). The robot is 320mm tall, and weighs 2.2kg. sator can compensate for the centroid position and the
It has 4 DOFs in each leg, 3 DOFs in each arm, and tri-axial moments of any standing motion, using all
2 DOFs in the neck. The robot has 4 force sensors joints of the body in real-time. The complexity of
at the bottom of each sole which are used to measure AutoBalancer is O((p + c)*), where p is the number
the ZMP. The puppet has the same DOFs and link of DOFs and c is the number of constraint equations.
proportions as “Akira,” and a human can control the Through experiments with real humanoid robots, we
robot intuitively. In order to designate the desired leg have shown that current hardware can treat such a
state, two touch switches are attached to the sole of problem in real-time. The algorithm can potentially be
the puppet. enhanced to control higher DOF humanoids, or other
Figure 10 shows the time series of inputs from the multiple-legged robots. The compensator can also be
puppet, outputs to the robot, and the measured ZMP. applied to stabilize dynamic walking trajectories for
It also describes the state transitions calculated by our humanoid robots.
Planner. There are long time delays from the given Currently, the planner component of AutoBalancer
dual leg state designation input and output of Auto- cannot generate state changes automatically. In ad-
Balancer (about 2 seconds). Therefore, the human op- dition, the time required to achieve the final config-
erator should work within these delays. uration of the input motion may be increased by the
Figure 8 shows a kicking motion input and two types necessity of having to pass through the boundary con-
of balance-compensated motions. Every joint weight is dition states. More research is needed to address these
equal in the middle row, and the crotch joint has 5 limitations. Another problem is that the current algo-
times the weight in the lower row. In both outputs, rithm cannot handle rapid disturbances from the en-
the z-axis moment is compensated by the upper body vironment. Incorporating dynamic state changes in-
338 S. Kagami, F. Kanehiro, Y. Tamiya, M. Inaba, and H. Inoue

Motion

Motion
Output
Input

LBoundary2BLeg
awww mn anna ygh enews ce han ncen Wh wenn smn eeeeseneenen eee eB ea ns ede ohana nt aww on aware ee Oe ESSERE EERE E EEE ERE Teme

Position[mm]
X

| N oO OonBLeg

Change
a
ee
eee
rs
ree
Pee
ee
Input
oO
Standing
Condition
Position[mm]
Y | 10) ° rrr

oO N Ss Oo 00 10 12 14
Time [s]

Figure 10: Compensated Motion and ZMP Tracks.

cluding reaction to sensor input would be a very useful [6] Kazuo HIRAI. Current and Future Perspective of
improvement. Honda Humanoid Robot. In Proc. of 1997 IEEE Intl.
Conf. on Intelligent Robots and Systems (IROS’97),
pages 500-508, 1997.
References ;
[7] K. Nagasaka, M. Inaba, and H. Inoue. Walking Pat-
tern Generation for a Humanoid Robot based on Op-
[1] M. H. Raibert, H. B. Brown, and M. Chepponis. Ex-
timal Gradient Method. In Proc. of 1999 IEEE Int.
periments in Balance with a 3D One-Legged Hopping
Conf. on Systems, Man, and Cybernetics No. VI,
Machine. Robotics Research, 3(2):75-92, 1984.
1999.

[2] Marc H. Haibert: Running with symmetry. Robotics [8] J.Y.S.Luh, M.W.Walker, and R.P.C.Paul. On-Line
Research, 5(4):3-19, 1986. Computational Scheme for Mechanical Manipulators.
ASME Journal of Dynamic Systems, Measurement,
[3] Jessica K. Hodgins and Marc H. Raibert. Biped gym- Control, 102:69-76, 1980.
nastics. Robotics Research, 9(2):115-132, 1990.
[9] M.W.Walker and D.E.Orin. Efficient Dynamic Com-
[4] Vukobratovié, A.A. Frank, and D.Juri¢i¢. On the Sta- puter Simulation of Robotic Mechanisms. ASME
bility of Biped Locomotion. [EEE Trans. on Biomed- Journal of Dynamic Systems, Measurement, Control,
ical Engineering, 17(1):25-36, 1970. 104:205-211, 1982.

ai J. Yamaguchi, A. Takanishi, and I. Kato. Development —s T.Flash and N.Hogan. The coordination of arm
of a Biped Walking Robot Compensating for Three- movements:an experimentally confirmed mathemati-
Axis Moment by Trunk Motion. Journal of the Robot- cal model. The Journal of Neuroscience, 5(7):1688—
ics Society of Japan, 11(4):581-586, 1993. 1703, 1985.
AutoBalancer: An Online Dynamic Balance Compensation Scheme

Figure 11: High Kick Motion using H5

[11] S. Kagami, M. Inaba, and H. Inoue. Design and Devel- [12] Toshihiro Matsui. Multithread Object-Oriented Lan-
opment of Software Platform using Remote-Brained guage Euslisp for Parallel and Asynchronous Program-
Approach. Journal of the Robotics Society of Japan, ming in Robotics. In Workshop on Concurrent Object-
15(4):550-556, 1997. based Systems, IEEE 6th Symposium on Parallel and
Distributed Processing, 1994.
 on ee ; ;

a
ee
oe
<a

aia| PUL CS ee ~~ 4
7

potercal
oe: ule Yor
. x : Rotate s mis NATE tidy
sbomtahe de 81}a loyal Sut pages We
Ab, citi i oehernt
He i
pari oootinlt a
meeiials
\ “ ey i im Won < ~~ . 7

> BSai errant: herente| eae aedori eek lter Ww keer

7 aie ane fe “a Dohtgrti SAM EL
ai ue Age npare eat Pid a * ye ’ mathe _ x sive

_ } ik: Ruel n b , i
; ae ves er, , fe
: Heanaeh «9 4
, i |
‘ >
Fa Un ‘i, _
wy As
ie ; . na
ee mM ee iy
fasrt ne A oe

: ae Y
ovo _
hie y
he
Coupled Oscillators for Legged Robots
Matthew D. Berkemeier, Utah State University, Logan, UT

Biologists have uncovered some of the mechanisms at Oscillator-Oscillator

Coupling
work in the control of legged animals. The animal CPG =-=------=-----------
Oscillator
has received much attention and provides new inspira-
tion for the control of legged robots. This paper presents
the further development of a control method which in- / ‘\ Oscillator-Leg
' Robot Body ‘ Coupling
volves coupled oscillators. In particular, the paper dis-
cusses a pair of coupled oscillators which operate in dis-
Leg-Leg Coupling i, “
crete time and which have been implemented on a pair
Actuator He
of microcontrollers. Coupling between the oscillators
only need occur twice each period (once for each oscil-
Spring
lator), rather than continually. A simulation demon-
strates phaselocking of the two oscillators. Experimen- Uu U

tal results are briefly described.

Figure 1: Biomimetic Legged Robot Control.

1 Introduction
addition, the legs can telescope only. There is no
Legged robots are thought to have great potential for joint to move the leg forward and backward. For-
locomotion over rough terrain. This belief depends ward motions of the robot are not modeled yet.
heavily on observations of legged animals in rugged However, there is good reason to believe that the
outdoor environments. Currently, there are no legged vertical motions of a running system are the most
robots which can compare with humans and other ani- important. In our own analysis the vertical mo-
mals in locomotion in unstructured environments. This tions appear to be relatively unaffected by forward
motions. However, the converse is not true: the
is no surprise, as there are many other problems in
forward motions rely greatly on the vertical dy-
which biological systems excel and engineered systems
namics. For additional evidence of this, see [8].
do not (and vice versa). This paper discusses continu-
The obvious reason for these simplifications is to
ing efforts to build a quadrupedal robot with the ability
make analysis tractable. While it is easy to simu-
to run using several gaits. Simple biological principles
late a four-legged robot with many actuators and
(including central pattern generation) are being used
joints, it is usually not possible to derive analytical
to design and control the robot.
results with such a complicated system.
The current concept for a simple running quadruped
model and mechanical design is shown in Figure 1. The Coupled Oscillator Control Animals are thought
discussion of this model will be divided into three parts. to possess a Central Pattern Generator (CPG)
made up of neurons in the spinal cord. The CPG
Mechanical System This component is analogous to produces patterns for the stimulation of muscles
the animal musculoskeletal system. Rather than in rhythmic activities, such as locomotion. The
start with four legs, the model is restricted to a CPG of (at least) simple animals can produce
vertical plane and has the two front and two rear these patterns even when removed from the an-
legs collapsed into one front and one rear leg. In imal. Thus, it does not depend on feedback from
342 M. Berkemeier

sensory organs in the animal. However, when this a similar result. On the other hand, the pronk’s sta-
feedback is present, the patterns are modulated bility depended both on the inertia and the pronking
to produce signals which are better adapted to height. Oscillator-to-leg coupling was next considered
the environment the animal is operating in. This in [4] and [3]. It was shown that two-way coupling be-
is discussed in [6]. Applied mathematicians have tween an oscillator and a hopping leg provided better
modeled CPGs as coupled oscillators [5, 7]. This performance than one-way coupling from the oscillator
provides a convenient abstraction of the complex to the leg.
system of neurons present in a spinal cord. The [10] present a simple but elegant model for 2 coupled
model in Figure 1 contains two coupled oscillators. oscillators, in which only the phase of the oscillators is
These oscillators are the primary focus of this pa- important. Let 6;, 1 = 1,2, be the phase of oscillator 7.
per. The model is:
Oscillator-to-Leg Coupling The connection be-
tween the animal CPG and muscles has not re- 6, = (Wy + azo sin(O2 = 0) (1)

ceived as much attention as the CPG itself (at 65 = Wot 421 sin(0; — 62). (2)
least by mathematicians). It is clear that there
is coupling in both directions, however. The CPG Oscillator 7 has an intrinsic frequency w;. The parame-
must be able to stimulate the animal’s muscles. ters a;; are constants which control the coupling of the
Similarly, sensory organs in the legs must be able oscillators to one another. If the constants are zero,
to affect the CPG to change its frequency and then the oscillators’ phase increases at the constant
phase. rate w;t. For the case of non-zero coupling constants,
let ¢ = 0, — 02. The dynamics of ¢ (the phase differ-
The approach taken in this paper is different from
ence) are then given by:
that typically found in engineering and control papers.
Rather than attempting to construct an algorithm to
p = wy — we — (12 + G21) sind.
produce specific leg motions and gaits (synthesis), the
preliminary system in Figure 1 has been put together The equilibria are given by:
based on inspiration from biology and intuition. The
intuition comes primarily from rigorous mathematical
results which show that often, when two or more similar pein) (a—=)
a12 + aa1
| (3)
oscillators are coupled together in some manner, they
will synchronize [11]. The legged robot model in Fig- These are 1:1 phase-locked motions, in which the phase
ure 1 consists of several oscillators which are coupled difference ¢(t) is a constant over time. As an example,
(note that the legs are mechanical oscillators). Intu- suppose Ww; = We. Then the two unique equilibria are
ition suggests that for some parameter choices, the os- @ = 0, m. If ay2 + az; > 0, then in-phase oscillations
cillators in the system should synchronize. This behav- (¢@ = 0) are stable, but out-of-phase oscillations (¢ =
ior may correspond to gaits which animals use. While mw) are unstable. The reverse is true for aj2 + ag <
intuition was used to assemble the system in Figure 1, 0. For a more detailed discussion see [10]. They also
careful analysis of the model is being used to determine extend this model to a chain of N coupled oscillators
the resulting behavior of the system. with nearest neighbor coupling.
This model has some features, which are potentially
2 Background
useful in an engineered system (e.g., legged robot).
In previous work, the first and third components of These are:
the model in Figure 1 were considered in detail. The
mechanical system was analyzed in [1] and [2]. Two e Each oscillator can function without the other. If
gaits were considered, the pronk and the bound. Using the coupling mechanism fails for some reason, then
perturbation techniques, it was found that the stabil- an oscillator can still proceed at its intrinsic fre-
ity of the bound depended solely on the dimension- uency. If one oscillator fails, then the other oscil-
less inertia of the robot body. [9] had earlier proved lator can still function at its intrinsic frequency.
Coupled Oscillators for Legged Robots
343
e If the coupling constants are accessible to an Op- It is significant that the intrinsic period, T;, is fixed but
erator (or higher level control system) then there the phase changes from one firing to the next. This is
is an easy way to specify whether in-phase or out- consistent with the model in equations (1, 2). Also, in
of-phase oscillations occur. As discussed above, if previous work [3], the oscillator’s period was adjusted
a2 + agi > 0, then in-phase oscillations are the in response to feedback from a leg sensor. Thus, in the
result, while if aj2 + a2i < 0, then out-of-phase full implementation (Figure 1), the plan is to adjust
oscillations occur. the period, but it will be in response to the leg’s sen-
sor, rather than in response to coupling between the
e The system is very simple. This one system has
oscillators.
two patterns embedded in it. It is not necessary to
have two distinct systems to generate in-phase and The above oscillators would be easy to implement on
out-of-phase oscillations. The sign of the coupling a microcontroller. A timer on a microcontroller could
constants simply determines this. be used to signal (interrupt) when the desired amount
of time had elapsed (T;). When the signal occurred,
e The system is modular. Additional oscillators can the microcontroller would change its state from 0 to 1.
be added to produce a chain [10] or a ring [5]. Next, the form of coupling between the oscillators
It is not necessary to modify the existing set of needs to be determined. There are many different cou-
oscillators when new oscillators are added, except plings which could be used. For example,
to possibly introduce additional coupling.
e When an oscillator fired (made the transition from
While the above model nicely demonstrates coupled os- 0 to 1), it could check the state of the other oscilla-
cillations, it is probably not the best for use in robotics. tor. If the other oscillator was 1 (it had previously
In order to implement it, one would either have to use fired), then the phase should be reduced by some
a numerical integration routine on a microprocessor or fixed amount, to try to get them to fire at the same
build an analog circuit. Suppose microprocessors (or time in the future. If the other oscillator was 0, the
microcontrollers) were used. To obtain the benefits of phase should be increased by some fixed amount.
fault tolerance, modularity, etc., it would make sense e When an oscillator fired, it could immediately no-
to use one microprocessor per oscillator. However, con- tify the other oscillator (i.e., interrupt the other
tinuous coupling between oscillators is required. This microcontroller). The other oscillator would com-
would require a high-speed communications link be- pare the firing time with its own. This could either
tween the two microprocessors. A better approach for be a previous firing, or a firing which was about
robotics would be a modified set of equations which to occur (whichever was closest). The difference
only require infrequent communication. would be considered an error quantity, and it could
advance or retard the phase.
3 Coupled Oscillations from Difference
The first approach would be easier to implement, but
Equations the second has the advantage that there is an amplitude
associated with the error. Performance might be better
Extending the model of [10] to difference equations is
(particularly as in-phase operation was approached), if
not difficult. The following is one possible approach.
the error became smaller, the closer the firings came to
Each oscillator will produce a signal with only 2 dis- being coincident.
tinct values, say 0 and 1. We will only worry about An implementation of the second approach is as fol-
the transition from 0 to 1. This will occur at time lows:
tys, where i = 1,2 (the f is for “fire”). The transition
from 1 to 0 is not important at this point, so long as Oi(j +1) = 61(j ) —tpiG))
+ araltsa(k) (6)
it occurs. The firing time is given by a phase 6; and a Ox(k+1) = 62(k) + aailtpi—(d) to(k)] (7)
constant period T;: For a
where aj and a2; are the coupling constants.
given j, the value of k is chosen to minimize:
t(j) = A1(j) + Tig (4)
tya(k) = 62(k) + Tok. (5) |ti(k) — tro(9)| (8)
344 M. Berkemeier

In general, it is difficult to write down an explicit ex- Finally, the effective period of each oscillator would be:
pression for k as a function of 7 which minimizes (8).
Radtaot T1a21
2 21 + Toa2012 (10)
On the other hand, the implementation of this form of
coupling on a pair of microcontrollers would be rela- a12 + @21
tively easy. Notice that this is just a weighted average of the two
Between the previous and next firing of an oscilla- intrinsic periods.
tor (say, oscillator 1, with previous firing time corre- While the above shows the possibility that phase
sponding to 7 — 1 and next firing time corresponding locked patterns could exist with the discrete time sys-
to 7 + 1), there is a window, during which the oscil- tem, it certainly doesn’t demonstrate that they will
lator can receive notification that the other oscillator exist. In order to give more confidence, we will assume
has fired. When the other oscillator fires, this time is that the value of k which minimizes Equation 8 for a
compared to the current firing time (oscillator 1, firing given j is simply j7. While this will not always be the
time corresponding to j) and the difference produces a case, in Section 4 a specific example is given where this
phase adjustment for the next firing time (oscillator 1, holds.
firing time corresponding to j + 1). It is possible that Let 7(j) = tri(j) — tfo(j) be the difference in firing
no notification of firing is received during this window. times. Then,
For simplicity, this possibility is not considered in the
present paper. Note that the above discussion might AGEe tpi(j +1) — tre(9 + 1)
be easier to understand by referring to Figure 2. = A(j+1)+1) +T — 1G
(j +1)
Although each oscillator has an intrinsic period (Tj,
—T(j + 1)
and T2), the actual period can be different if the phase
is not constant. The effective period of oscillator 1 is = O,(j) + Tij + ailtse(g)
— tri(Z)] +Ti —
given by: 62(j)—Tog — aarltgi (7) — tro(9)] — Te
= tri(j) + aialtre(7) — tri) + Ti —
Temi (J) = tpi(j + 1) — tpi
(7)= A179 +1) — (9) + Th.
tyo(J) — @ailtei(7) — tye(7)]— Tr
Similarly, the effective period of oscillator 2 is given by: = [1 — (a12 Bp a21)|T (j}) +7; — To. (11)

Terro(k) = tro(k +1) — tyo(k) = 02(k +1) — 02(k) + To. Note that although Equation 11 is analogous to Equa-
tion 3, Equation 11 does not have the nonlinear sin()
In [10] the continuous-time oscillators could phase lock term that Equation 3 does. This makes the analy-
with a new frequency which was somewhere between sis simpler, although it also means that only one fixed
the intrinsic frequencies of the 2 oscillators. This is point exists.
also possible with the discrete-time system. Equating
Setting 7(j + 1) =7(j) =7 gives the fixed point:
the two effective periods gives:
T; — T>
ay2[t f2(k) — tei (9)] + Ti = aeilti (7) — tro(k)] + To a2 + @21°
or, which is the same as Equation 9. This fixed point is
T; — To
t ry k eee 9 stable provided that:
fi(j) f2( )= rye ( )

Notice the similarity between this equation and Equa- 0 < ayo + aq] < 2. (12)

tion 3. If this equation held, then the phase changes in

Proposition 1 gives conditions under which equa-
each oscillator would be constant and given by:
tion 11 represents the dynamics of the discrete-time
: ; TT, =T: coupled oscillators (Equations 4—7, subject to k mini-
09+1)-0(7) “= —ay2.—-——* mizing Equation 8). This result is somewhat prelimi-
ai2 + a21
nary, and efforts are still being made to refine it. Be-
Gs(LE d(E)MOes Vag
ag1 a2?
————.. fore discussing the proposition, note that certain self-
a2 + a21
consistency checks are possible. For example, we would
Coupled Oscillators for Legged Robots
345

expect that after the transient had died out, the firing 3. |a12| and |a21| are each sufficiently small,
time difference between the two oscillators would have
to be less than half the effective period of each oscil- then k= 4:
lator. Otherwise, Equation 8 would not be minimized.
Proof: Consider the function:
Using Equations 9 and 10 gives:

[Tia21 + Tea4o| 73(k) = tyi(9) — tyo(k)

Tole emia
9 ans (13)
For a fixed j, this is a strictly decreasing function of
Equation 13 can be viewed as a necessary condition k if the firing times ty2(k) are strictly increasing with
for Equation 11 to represent the dynamics of the cou-
k. Condition 3 is meant to ensure this, although the
pled oscillators. Satisfying this equation doesn’t mean details are not included here. The function |7;(k)| will
that Equation 11 will actually represent the dynamics have a single minimum for each j, which will be given
since it was applied by assuming steady-state condi- by one or possibly two values of k. The following two
tions. Certainly, things could happen during the tran- conditions are equivalent:
sient phase which would invalidate the assumption that
the value of k which minimizes Equation 8 is 7. Also, e k; is the unique value of k which gives a minimum
not satisfying Equation 13 doesn’t mean that the os-
cillators will fail to phase lock. It just means that they
of |7;(F)|.
won’t phase lock according to the dynamics given in © [75 (ks) < l7j(ky + 1)| and |F;(k;)| < |% (kj — 1)|
Equation 11.
Of course, Equation 11 can be solved exactly. The Clearly, then, to show that k; = j, it is sufficient to
solution is show that:

. (ewe fia © léri(7) — teo(9)| < [epi

(¥) — tea + 1)| V9
7(j) = |7(0) - ——> {= ia aaea Say
ai2 + G21 [1—(a12+a21)) Lar + a21
and
Using this result it is again possible look for self-
consistency violations (i.e., cases where Equation 8 is
|t¢1(9) — tea(a)| < Iter (9) — tya(9 — 1)| V2.
not minimized). However, this result allows considera-
tion of the transient portion of the dynamic evolution. Previously, 7(j) was defined to be tyi(j) — tro(J). By
If the inequality in Equation 12 is satisfied, a bound making use of Equations 4-7 the above two inequalities
can be put on the firing time difference: become:
T; — To 2 T, —T>
Ae as ——— 7(0) — ————| Vj. 14 I7(9)| < |(1 — @a1)r(9) — Ta] V5 (15)
a12
+ G21] (0) a32 + Q21 J ia)
Next, we look at the main result. and
Ir(a)| < | — aia)
(¥) + Ti] V3. (16)
Proposition 1 Consider the system (4-7), subject to
choosing k such that (8) is minimized for each j. As- From condition 1,
sume that (12) is satisfied. If:
Tya21 + T2a12 TyiT3
1. The inequality aj2 + G21 ay + G21

|Ty = T>| <a

|T1a21 +5 Trar2| This implies both of the following:

is satisfied, T, a2, + T2a12 T, — TI) =f T, — T» (17)

a42 + G21 ay2 + G21 ai2 + Q21
2. the quantity
J
ignma lySe and
t710) -=ty2(0)Oia ee
7 (8)
Tia. + Toa. = Ti-th (ho
is sufficiently small, and ay2 + G21 ay2 + G21 ay42 + a21
346 M. Berkemeier

Now, consider the sequence given by T(j) as defined

in Equation 11. Since

Ir(i)I = |r) -a2 Aare

Oscillator 1
Ty — T T, - T>
b)
+ a21
+1 j i
by using Equation 14 it is obvious that:
t=20.753 t=21.802 t=22.852
|.0.496__ 0.554
In@)I < |7(0) — A 2) pn i A
T ae T> Ti (a T>

a2 + a21 a12 + 21 a?
Again using Equation 14 it is possible to show that: |j-1 j i
|
dai )r) = To] > Oscillator 2 t=22.298 t=23.348
Te To Uigairt Toai2
20.0 22.0 24.0
aj2 + G21 ai2 + aa1

Jdl-a— ani] 7(0)

|r(0)— — |
T, — To
————| vjh.
Vj. (20)
20 Figure 2: Phase-Locked Oscillations. The difference be-
tween firing times at index j determine the firing times at
By combining Equation 18, 19, and 20 and making index 7 +1. See text for further explanation.
use of condition 2 (define 7(0) = tyi(0) — tro(0)), the
following result is obtained: The effective period of each oscillator at steady state is:
I7(9)| < |. — aa1)7 (9) — Ta| V5. La = 105

One can also show that: from Equation 10. Note that the inequality in Equa-
tion 13 is just barely satisfied:
I7Q)| < | = ai2)rGG)+ T1| V7. |Tya21 + T2a12|
|, — T2| = 0.1 < 0.105 = frame ae
This is done by first showing:
Also, notice that the parameters satisfy Equation 12
\(1 — a12)7G) + Til 2 for the stability of Equation 11. The simulation was
iperigne ral 2ai2)| © performed with Equations 4-7 (not with Equation 11).
aj2 + 421 aj2 + a21 At each step it was verified that the k which minimized

1 ~axa||r(0) -
l-a T(Q)
T, — To
— ————— en
21
Equation 8 was indeed 7.
Figures 2 and 3 show the results. In Figure 2, the
oscillator outputs are shown after the transient behav-
Then, combining Equation 17, 19, and 21 gives the
ior has essentially died out (0.87? = 0.01153). The
result.
oscillators have phase-locked with a firing difference of
These two inequalities are the same as those in Equa- —0.5, which means they are almost exactly out of phase
tion 15 and 16, so this concludes the proof. a (since the effective period of each oscillator is 1.05).
Figure 2 also has labels for different firing times. Os-
4 Simulation Results cillator 1’s firing time at index j + 1 is determined by
the difference in firing times at index j of both oscilla-
In this section a specific example is considered. The tors 1 and 2. The same is true for oscillator 2. Note
parameter values are: that the firing of oscillator 1 at index j + 1 is slightly
farther away from the firing of oscillator 2 at index j
T; = il, T> = ihoil, 6, (0) = 62 (0) = 0, a12 = a21 = Ove
(22.852 — 22.298 = 0.554) than the firing of oscillator 1
The expected fixed point is given by Equation 9 as: at index j (22.298 — 21.802 = 0.496). Thus, oscillator
2 adjusts its firing time at index j + 1 based on oscil-
7 =ty —tpo =—0.5. lator 1’s firing at index j, not 7 +1. The fact that the
Coupled Oscillators for Legged Robots
347

Bite =

‘i
~
oe

1.05 | bs
So Oey
ses
paar
sere
Ja
wy
ys

a
i

1.00 4 :
0.0 10.0° 20.0

Figure 4: A Pair of Coupled Oscillators Implemented on

Figure 3: Convergence of Periods to the Same Effec-
Microcontrollers.
tive Period.

the output state was changed. The external capture

two differences in firing times are so close to each other
ISR compared previous firing times with current and
(0.554 and 0.496) is because the choice of parameters
saved the minimum firing time difference (to a resolu-
just barely satisfied Inequality 13. For better parame-
tion of 1 ms). The timer ISR used the stored difference
ter choices, the decision for oscillator 2 would not be
to modify the count about the nominal 500.
dependent on such a small timing difference.
One evaluation board had a nominal count of 500,
In Figure 3 The effective periods of each oscillator
which gave it a nominal frequency of 1 Hz. The other
are seen to converge to the same value of 1.05. Note
had a nominal frequency of 1.098 Hz. The expected
that the horizontal axis in this graph is the index j, frequency of the coupled pair was therefore 1.047 Hz.
not the time corresponding to the period. Figure 5 shows a frequency of 1.042 Hz for each micro-
controller. It is likely that the difference is simply due
5 Experiments to the limited resolution of the oscilloscope, although
more careful experiments should be performed.
The algorithm was tested on a pair of Motorola 68HC12
The implementation of the algorithm on the micro-
evaluation boards. Figure 4 shows the setup. Note
controllers was very straightforward. This supports the
the pair of wires which provides bidirectional coupling.
earlier argument that the approach advocated here is
There were 3 main parts to the assembly language pro-
more appropriate than, for example, attempting to nu-
gram: merically integrate ordinary differential equations. No
multiplication or division was necessary. Scaling of the
1. Initialization code, followed by an infinite loop,
firing time difference (corresponding to the constants
2. Timer interrupt service routine (ISR), 42, @21) was done by right shifts. Certainly there
would be plenty of processor time remaining to per-
3. External capture ISR. form additional tasks, if desired. Alternatively, one
could easily use a less expensive/less powerful micro-
The evaluation boards used an internal clock speed of controller.
8 MHz. One of the timers was used to interrupt the
processor every 8000 counts, or every 1 ms. Then, in 6 Conclusions
the timer ISR, a RAM variable was incremented until it
reached the maximum count, which was nominally 500 In this paper, inspiration was taken from the coupled
for one board (giving 0.5 s elapsed time). At this point, oscillator model of [10] to develop a new discrete-time
348 M. Berkemeier

References

[1] Matthew D. Berkemeier. Approximate return maps

for quadrupedal running. In Proceedings of the 1997
IFEE International Conference on Robotics and Au-
tomation, 1997.

[2] Matthew D. Berkemeier. Modeling the dynamics of

quadrupedal running. International Journal of Robot-

|
¥ }
Ces ere re
ics Research, 17(9):971—-985, September 1998.

(3—
Matthew D. Berkemeier and Kamal V. Desai. A com-
parison of three approaches for the control of hopping
height in legged robots. International Journal of Ro-
botics Research, 1999. (accepted).

[4Lome
Matthew D. Berkemeier and Kamal V. Desai. Con-
trol of hopping height in legged robots using a neural-
mechanical approach. In Proceedings of the 1999 IEEE
Figure 5: Phase-Locked Oscillations Produced by Two Mi- International Conference on Robotics and Automa-
crocontrollers. tion, pages 1695-1701, 1999.

[5] J. J. Collins and I. N. Stewart. Coupled nonlinear

coupled oscillator model. This new model was more oscillators and the symmetries of animal gaits. Journal
of Nonlinear Science, 3:349-392, 1993.
appropriate for robotics use, as each oscillator could
be easily programmed on a microcontroller. More- [6]eeadlJames Gordon. Spinal mechanisms of motor coordi-
over, communication between the oscillators was only nation. In Eric R. Kandel, James H. Schwartz, and
required twice each period (once for each oscillator), Thomas M Jessel, editors, Principles of Neural Sci-
rather than continually. ence, chapter 38. Elsevier, 3rd edition, 1991.

Preliminary analysis of the discrete model demon- [7]——w N. Kopell and G. B. Ermentrout. Coupled oscillators
strated that it had similarities to the model of [10]. and the design of central pattern generators. Mathe-
matical Biosciences, 90:87—109, 1988.
Each oscillator had an intrinsic period, but under the
right conditions, the coupled pair would phase lock and [8] R. T. M’Closkey and J. W. Burdick. Periodic motions
achieve an effective period which was somewhere be- of a hopping robot with vertical and forward motion.
tween the intrinsic periods of the individual oscillators. International Journal of Robotics Research, 12(3):197—
218, 1993.
A proposition provided precise conditions under which
the oscillators phase-locked in the manner expected. A [9it A. Neishtadt and Z. Li. Stability proof of Raibert’s
simulation demonstrated phase locking for one particu- four-legged hopper in bounding gait. Technical Report
lar set of parameters. Lastly, experiments using a pair 578, New York State University, 1991.
of microcontrollers demonstrated the expected opera- [10] Richard H. Rand, Avis H. Cohen, and Philip J.
tion of the algorithm. Holmes. Systems of coupled oscillators as models of
central pattern generators. In Avis H. Cohen, Serge
The development of this model represents another
Rossignol, and Sten Grillner, editors, Neural Control
step in achieving biologically-inspired control of a of Rhythmic Movements in Vertebrates, chapter 9. Wi-
legged robot. ley, 1988.

[11] Steven H. Strogatz and Ian Stewart. Coupled oscilla-

Acknowledgments tors and biological synchronization. Scientific Ameri-
can, 269(12):102-109, December 1993.
This work was supported by supported by NSF CA-
REER award nos. IIS-9996366, IIS-9733401.
Reliable Mobile Robot Navigation
From Unreliable Visual Cues
Amy J. Briggs, Middlebury College, Middlebury, VT
Daniel Scharstein, Middlebury College, Middlebury, VT
Stephen D. Abbott, Middlebury College, Middlebury, VT

Vision-based mobile robot navigation requires robust 1.1 Navigation Using Expected Shortest
methods for planning and executing tasks due to the Paths
unreliability of visual information. In this paper we
propose a new method for reliable vision-based naviga- Our system uses artificial landmarks as visual cues for
tion in an unmodeled dynamic environment. Artificial navigation in an unknown environment. The robot first
landmarks are used as visual cues for navigation. Our explores the environment to learn the relative locations
system builds a visibility graph among landmark loca- of the landmarks and builds a graph of landmark loca-
tions during an exploration phase and then uses that tions that it subsequently uses for navigation. During
graph for navigation. To deal with temporary occlusion navigation, the robot plans motion paths along edges
of landmarks, long-term changes in the environment, of the landmark visibility graph. If it wants to navigate
and inherent uncertainties in the landmark detection from landmark s to landmark g, it plans and executes
process, we use a probabilistic model of landmark vis- a path starting with a landmark visible from s. As
ibility. Based on the history of previous observations it moves through the environment, it continually up-
made, each visibility edge in the graph is annotated dates the graph with any newly acquired data, such as
with an estimated probability of landmark detection. measured distance between two landmarks, or changes
To solve a navigation task, our algorithm computes the to the landmark visibility information. As is discussed
expected shortest paths between all landmarks and the in Sections 2 and 3, the planner uses estimates of the
specified goal, by solving a special instance of a Markov probabilities of landmark visibility to find paths with
decision process. The paper presents both the proba- expected shortest length by solving a Markov decision
bilistic expected shortest path planner and the landmark process.
design and detection algorithm, which finds landmark
patterns under general affine transformations in real- 1.2 Landmark-based Navigation
time.
Many techniques have been employed for sensor-based
1 Introduction navigation and localization. Industrial mobile robots
Vision-based mobile robot navigation is often planned have traditionally navigated by following painted lines
using landmarks, either artificial or extracted from on the floor or tracking buried wires or infrared bea-
the environment. Such landmarks must be detected cons. The disadvantage of these approaches is that
quickly and as reliably as possible. When landmark de- they require substantial engineering of the environ-
tection is unreliable due to factors such as temporary ment. Recently many researchers have employed land-
occlusion or varying lighting conditions, the planner marks — either artificial or extracted from the envi-
should compute motion paths dynamically to always ronment — to guide the motion of a mobile robot in
find the current best path. This paper makes two ma- indoor environments. The most commonly used ap-
jor contributions: (1) A navigation system that uses proach with artificial landmarks is heuristic: Land-
probability estimates of landmark visibility in order to marks are designed and placed so that landmark de-
compute expected shortest paths between landmarks; tection under normal circumstances is straightforward.
(2) a novel landmark pattern together with a real-time The problem with the heuristic approach is that it only
algorithm for landmark detection that can handle a works under certain restricted conditions that are en-
wide range of affine transformations. forced for the sake of speed: The patterns must be
350 A. Briggs, D. Scharstein, and S. Abbott

viewed from a narrow range of distances and angles Simmons and Koenig [18] to plan navigation strategies
and will not be recognized if partially occluded. in partially observable environments.
We propose a self-similar pattern specifically de- Rather than relying on landmarks extracted from the
signed for the application of mobile robot navigation. environment [4, 5, 11, 13, 17], the approach taken in
The pattern is quickly recognizable under a variety this paper and by a number of other research groups
of viewing conditions, even when partially occluded [1, 6, 10, 15, 19, 20] is to use artificial landmarks that
or mounted at an angle. In contrast to existing ap- can be easily and unobtrusively added to the environ-
proaches that require two-dimensional analysis of an ment. Becker et al. [1] use simple landmarks attached
image, our method finds matches along individual scan- to the ceiling of the environment, and use a recognition
lines, without any preprocessing, making it suitable for algorithm that relies on a fixed distance of the pattern
real-time applications. to the camera. Taylor and Kriegman [20] utilize the
projective invariance of cross-ratios, but their approach
1.3. Related Work cannot handle partial occlusion and requires special-
Traditionally, vision-based robot navigation has pro- ized hardware for real-time performance. Lin and Tum-
ceeded from three-dimensional maps of the environ- mala [10] propose three-dimensional landmarks consist-
ment, constructed, for example, using stereo vision ing of two disks, which can be detected using Hough
techniques [3]. More recently, landmarks have been transforms from a restricted set of viewing angles. Un-
used to navigate without a full environment model. like these approaches, our method uses simple 2D land-
Techniques for mobile robot navigation based on land- marks that can be recognized under a wide range of
marks include those that are primarily reactive [5], affine transformations in real-time without specialized
those planned within a geometric environment map en- hardware.
hanced with perceptual landmarks [9, 11], and those
based on a topological description of landmark loca- 1.4 Outline of the Paper
tions without a global map [8, 13, 17].
Each of the two central themes of the paper — robust
Our navigation system uses artificial landmarks navigation and the design of artificial landmarks — is
placed throughout the environment as visual cues. A discussed in two sections. Section 2 presents our frame-
topological map of current landmark locations is first work for planning based on unreliable sensor data and
constructed during an exploratory phase and then used develops an algorithm for computing expected short-
for navigation without requiring a global geometric est paths. Section 3 discusses how a visibility graph
map. To compensate for occlusion and unreliability can be annotated with estimates of the unreliability
of landmark detection, our navigation algorithm em- of observations. Section 4 then introduces self-similar
ploys probabilistic techniques to construct reliable and functions for the design of an optimally recognizable
efficient motion paths. Several different approaches to intensity pattern. Finally, Section 5 presents an al-
probabilistic path planning have been developed in re- gorithm for finding such patterns in an image under
lated work. Blei and Kaelbling [2] describe Markov de- general affine transformations in real time.
cision processes for finding shortest paths in stochastic
graphs with partially unknown topologies. Their work |
differs from ours in that they assume that an edge is
2 Robust Navigation Using Unreliable
either passable or not, but that the state of each edge Sensors
is only known with a certain probability. Kavraki and
Latombe [7] propose a randomized method for configu- The discussion in this section is based on the following
ration space preprocessing that generates a network of scenario:
collision-free configurations in a known environment. We assume an unknown environment augmented
Overmars and Svestka [12] describe a similar proba- with visual landmarks {L,,L2,...,Ly} that can be
bilistic learning approach that extends to a number of detected by the robot, albeit unreliably. We will use
motion planning problems, including those for free fly- lowercase letters a,b,... when referring to individual
ing planar robots, car-like robots, and robots with high landmarks. The robot navigates the environment by
degrees of freedom. Finally, a Markov model is used by traveling along the edges of the visibility graph defined
Reliable Mobile Robot Navigation From Unreliable Visual Cues
351
by the landmarks. We assume an edge from landmark
a to landmark 6 has associated probability pay € (0, 1]
and length I,, > 0. The probability p,, represents
the likelihood that landmark 6 can be detected from
landmark a. The length J,, can be, for example, the
physical distance between a and b, or the time it takes
the robot to travel from a to b. In this section we in-
vestigate the problem of robot navigation given such
a visibility graph. In Section 3 we discuss how such
a graph can be constructed, and how probability and
length factors can be estimated.
Path planning in visibility graphs typically employs
shortest-path algorithms. Given a directed graph
whose edges have fixed lengths, the shortest path from Iss
a start node s to a goal node g can be computed easily, Figure 1: Path planning example: The robot at landmark
for example using Dijkstra’s algorithm. In our sce- s can currently see landmarks a and b, but not c due to
nario, landmark detection is unreliable, and thus the temporary occlusion.
edges of the graph can only be traversed some of the
time. Therefore, we must change the notion of a short- in the graph has a self-edge n > n with cost In, > 0
est path to that of a path with shortest expected length, and probability pr» = 1 (staying is always an option).
or expected shortest path.
In summary, the robot will make its decision of
2.1 Navigation Using Expected Shortest whether to go to a, to go to b, or to stay at s based on
Paths which of the three sums (Isa + Fag), (Iso + Fog), and
(Iss; +59) is the smallest. If landmark c is permanently
Before explaining how these shortest expected lengths occluded, it may seem that the robot could “get stuck”
can be computed, let us see how the robot can use at s. As will be discussed in Section 3, however, the
them to plan its path. Suppose that the robot at land- current estimate of p,. — the probability that c is vis-
mark s can currently see landmarks a and b, but not ible from s — will decrease after a repeated failure to
landmark c (see Figure 1). Let Ha, denote the ex- detect c. This will in turn increase the expected length
pected length of the shortest path from a to goal g. of the path from s to g, until eventually the expected
The total expected length of the path through a will length of going through a or 6 will be shorter.
be Isa + Fag. Similarly, the total expected length of
the path through 6 will be /,, + E,,. Thus, the smaller 2.2 Deriving the Expected Lengths of the
of those two sums will indicate a candidate shortest Shortest Paths
path. Note that these lengths are independent of the
Given a designated goal g, we will now relate the un-
probabilities p,, and psp, since at the current moment,
known quantities E,, (the expected lengths of the
both a and 6 are visible. The path with overall short-
shortest paths from each node to the goal) in recur-
est expected length, however, may go through neither
sive equations. In the next section, we show that there
a nor b. It is possible that an expected shorter path
is a unique solution to this system of equations if there
to g goes through landmark c, which usually is visible
is a path with non-zero probabilities from each node in
from s, but at the current moment is not (for example,
the graph to goal g. It turns out that this problem is
due to temporary occlusion). This would be reflected
a special instance of a Markov decision process, which
in a low expected length F,. In this case, it would
is discussed in section 2.4.
be better to stay at s and wait for c to become visible,
following relations for the unknowns IS Oe
rather than going to either a or b. To prevent the robot The
from staying at a landmark indefinitely, we associate a motivated by the discussion in the previous section.
non-zero cost with this option (for example, the time To start, the expected length of the shortest path from
it takes to acquire a new image). That is, each node n the goal to itself is:
352 A. Briggs, D. Scharstein, and S. Abbott

+ Pna Prd --- PnzM in( live oP Eag; lan 1 Eng)

Eyg'=0. (1) + Dna Pnb roa min(ln, js Egg,
Dey SO tnn + Eng)

Next, let us consider a node n with only a single outgo- + Dna Dob os? Dag MUN bag + ag) eb Clr bon
ing edge n — a. The expected length E,, of the short- lnn + Eng)
est path from n to g can be expressed as a weighted
+ owe.
sum of two terms that correspond to whether or not a
is visible from n: + Das Dab os Pag Mine FH Bag, tne Eases
Inz a Ezg; lan = Eng).
Eng = (1 — pra) (Inn + Eng) (2)
lan + eg):
if Pna min(Ina =f Bags
Unlike in the case for only a single edge, it is no
The first term represents the case that a is not visible, longer possible to explicitly solve these equations for
which occurs with probability 1 — pn. In this case Eng because of the minimum expressions, which recur-
the only choice is to remain at n and acquire another sively relate the unknown quantities E;,. For example,
image, which incurs cost ly, and results in an expected to determine the value of the minimum expressions in
length of Inn + Eng. The second term represents the Equation 4, we need to know the ordering among Ka,
case that a is visible, which occurs with probability pra. Ky, and K,,, where K; denotes |,; + Eig. While one
In this case the expected length of the shortest path is can still deduce that K,, cannot be the smallest of the
the smaller of Ina + Eag and Inn + Eng, corresponding three, each of the remaining 4 orderings is possible:
to the options of going to a or staying at n. Recall
that we are assuming that the goal is reachable from Kem
Ks =. Be Kat, Mogens (6)
any node, and thus Ey, < oo. Given that the edge RN ale Ke i< 1G ah
nm — ais the only edge leaving n, we know that all
paths from n to g have to go through a, and thus that
Note that if such orderings were known for all nodes,
Eng = Ina + Eag. This allows us to solve Equation 2
Equations 4 and 5 would simplify to linear equations
for Eng, yielding:
involving the unknowns E,,,. The expected lengths of
bes Pna
the shortest paths could then be computed simply by
Baia = lan ri Ina ie Fag: (3) solving a system of N—1 linear equations, where N
Pna
is the total number of landmarks (including the goal).
Now, let us consider a node n with two outgoing The presence of the minima, however, prohibits this
edges n + a and n > b. The relation for the expected approach.
length of the shortest path from n to g can be expressed To summarize, the landmark visibility graph defines
analogously, except that now there are four cases, de- a system of N—1 equations — each of the form of
pending on which of a and 6 are visible. Using the Equation 5 — for the N—1 unknowns En, n ¥ g.
shorthand to denote (1 — p), we obtain the following
We now turn to the question of how this system of
relation for Eng: equations can be solved.

Eng = Dna Pnb (lan + Eng) (4)

+ Dna Prd Min(Ing + Lag) Inn + Eng)
2.3 An Algorithm for Computing the
Expected Shortest Paths
+ Dna Pod Min(Ins + Beg, Inn + Eng)
+ Pna Prd MiN(Ina + Lag, Inp + Evg, Inn + Eng). For notational convenience, let us collect the N —
1 unknowns E,,, n # g, into a vector X =
It is easy to see how the equation generalizes to nodes [v1,%2...,@y_1] € IR, and rewrite Equation 5 in
with more than two outgoing edges. A node n with k vector form:
outgoing edges n > a, n > b, ..., n > z yields an
Ln = fn(X). (7)
equation with 2* terms:
Collecting the individual functions f, : RY7~! > R
Eng = Pna Prd --» Pnz (Inn + Eng) (5) (each of which is a linear combination of minima of
Reliable Mobile Robot Navigation From Unreliable Visual Cues
353
components of X plus constant offsets) into a function 2.4 Relation to Markov Decision Processes
PER ees R es!
The problem of expected shortest paths can be viewed
Be i lone aa ddMex )s (8)
as a special instance of a Markov decision process
we can then rewrite the entire system of equations con-
(MDP). Briefly, a MDP consists of a set of states S,
and a set of allowable actions A, associated with each
cisely:
state s € S. Each action a € A, taken in state s
2S MOG) @ (9)
yields a reward r(s,a), and results in a new (random)
Thus, the desired solution X of this system of equations state s’ according to a transition probability distrib-
is a fixed point of function F. The properties of F are ution p(-|s,a). The objective is to devise a policy
summarized in the following theorem; a proof of the or decision rule d : S + A, that selects a certain
theorem can be found in Appendix A. (fixed) action in each state so as to optimize a func-
Theorem 1 Jf there exists a path to the goal from tion of the total reward. This brief discussion ignores
every node in the graph, and if all edges in the graph many common variations of MDPs, including time-
have non-zero probabilities Dis € (0,1] and positive dependent or discounted rewards, and non-stationary
lengths l;; > O, then F has a unique fixed point X* policies. For a comprehensive introduction to Markov
in RN-}, Moreover, the iterative process: decision processes, see the book by Puterman [14].

X*) — F(X) (10) Our problem of expected shortest paths translates

into a non-discounted negative expected total-reward
converges to this fixed point given the initial value
MDP. This means that each reward is interpreted as
xX) = 0:
cost or penalty, and that the objective is to minimize
Xo tan, Xe") (11) the total expected cost. Upon reaching the goal g, no
k- oo

This theorem translates literally into an efficient al- further cost is incurred. The key insight for relating
gorithm for computing the expected shortest paths Eig. our problem to a MDP is that the states in the MDP
are not simply the nodes in the graph. Rather, each
Starting with an initial estimate X, iterate Equation
state encodes a node together with the set of outgoing
10 until the value converges!. Convergence is geometric
edges currently passable. That is, a node with k out-
(ie., the error decreases exponentially) once the cur-
going edges contributes 2” states. In each state, the
rent value is in the vicinity of the fixed point. In prac-
allowable actions are to traverse any of the passable
tice, usually no more than a few hundred iterations are
(visible) edges, including the self-edge. The cost asso-
necessary for the values to converge to within machine
ciated with an action is the length of the edge traversed.
precision. (It is possible, however, to construct graphs
The transition probabilities are the probabilities asso-
for which convergence is arbitrarily slow.)
ciated with the different states (visibility scenarios) of
Instead of iterating Equation 10, it is also possible to the destination node. For example, if the action is
repeatedly solve the linear system of Equations 9, us- to go to a node x with 2 outgoing edges z — y and
ing the previous solution to hypothesize which terms x — z, the resulting state will be one of the 4 states
are the minima in each equation. Thus, instead of encoding which of the 2 edges will be passable once x
iterating over the values of the expected lengths, we is reached; the corresponding probabilities are Dry Daz,
can iterate over the ordering of the expected lengths Dey Pez) Pay Pzzs and Peay Paz:
of the outgoing paths at each node (as in Equation
6). The iteration terminates when the-solution to the Note that while the corresponding MDP has many
current system of equations yields the same ordering more states than there are nodes or edges, a policy can
as the previous solution. It can be shown that this be specified by ordering the outgoing edges at each
second algorithm (iterating over orderings) converges node (as in Equation 6). Then, for each subset of visi-
much faster than the first (iterating over values), but ble edges, the edge corresponding to the shortest path
each iteration requires solving a system of linear equa- is chosen. Each such ordering can in turn be derived
tions, rather than just evaluating it. from a current estimate of the expected lengths of the
1Tn fact, it can be shown that the process converges for shortest paths from each node to the goal. Thus, while
any initial value xo eur st. the number of states |.$| of the MDP is:
354 A. Briggs, D. Scharstein, and S. Abbott

N yet been traversed, based on the size of the landmark

ISl= 29%, in the image. Such estimates are fairly accurate, and
are immediately available with each new visibility edge;
n=1
they can be replaced with the measured distance once
where od(n) is the out-degree of node n, the entire the edge has been traversed.
MDP can be concisely described with the N — 1 un-
knowns Eng. We now address the less obvious problem of deter-
mining the probabilities p;; (that landmark j is visible
Our algorithms for computing the expected shortest
paths presented in Section 2.3 are variants of two algo-
from landmark i). Since these probabilities are un-
known, the best we can do is to compute estimates
rithms known as value iteration and policy iteration in
the MDP community. An important difference is that pi; for the true pj; as a function g of the history of
in our case both algorithms require the iteration (or observations O,;;:
solution) of only N — 1 equations, rather than of |5|
equations, as discussed in the previous paragraph. Biz = g(Oi;)- (12)

The history of observations of landmark j from land-

3 Building the Visibility Graph mark 7 is:

Now that we are armed with the ability to quickly com- 0;; = (ol? 0 ,..., 08], (13)
pute the expected shortest paths from all nodes to a
given goal node, we can apply the navigation strategy where m, is the total number of observations made
outlined in Section 2.1: At each node along the way,
from landmark i up to this point in time, and each
determine which landmarks are currently visible, and
ove € {0,1} records whether landmark j was visible.
select among those the one that yields the overall ex-
Note that observation histories can have “leading ze-
pected shortest path (which includes the option of stay-
ros”; that is, even if 7 was not visible the first few times
ing at the current position). Repeat the process at each
an observation was made, it is possible to reconstruct
subsequent node, until the goal is reached. If scanning
the complete observation history for 7 by keeping track
for all visible landmarks is significantly more expensive
of all observations ever made at landmark 7.
than just looking for the next landmark on the pre-
dicted shortest path (for example, if scanning requires Let us now turn to the function g: how can we derive
a 360 degree rotation of the robot), the strategy can be probability estimates from observation histories? In
modified as follows: If possible, travel to each landmark the simplest scenario, assuming independent observa-
in the sequence corresponding to the expected short- tions made with a fixed probability p;;, the optimal es-
est path. Only if a landmark in the planned sequence timate p;; is given by a function g that returns the ratio
cannot be detected, scan for all visible landmarks and of detections (“ones”) to the total number of observa-
replan. tions m,;. In reality, however, the observations will nei-
ther be independent, nor will the p;; stay constant over
3.1. Deriving Cost and Probability Estimates time. While some failures to detect a landmark will
have truly random causes (for example, occlusion by a
The remaining problem is how the visibility graph can person walking by), others will be caused by lighting
be constructed and how length (cost) and probability changes throughout the day, or perhaps even by perma-
factors can be estimated and maintained. Let us first nent changes to the environment (most extremely, the
discuss how to estimate the lengths of edges. Clearly, removal or addition of a landmark). Typically, obser-
once a visibility edge has been traversed by the robot, vations closely spaced in time will be highly correlated.
its length [;; is known and can be stored. Note that Therefore, in practice, a more sophisticated estimation
“length” will typically refer to the time it takes to tra- function g should be used. It is also a good idea to
verse the edge. When artificial landmarks with known record a time stamp with each observation, so that the
size are used (such as the ones presented in Section 4), temporal distribution of observations can be taken into
we can also estimate the lengths of edges that have not consideration.
Reliable Mobile Robot Navigation From Unreliable Visual Cues
395
3.2 Exploration and Navigation of a p-similar function is self-similar; that is, it is iden-
tical to itself scaled by p in the z-direction. Note that
In our landmark-based navigation system, the robot
a p-similar function is also p*-similar, for k = 2,3,...
operates in two modes: Exploration and navigation.
Self-similar intensity patterns are attractive for recog-
In exploration mode, the robot explores the environ-
nition since the property of p-similarity is invariant to
ment using a depth-first search among the unvisited
scale, and thus the distance of the pattern to the cam-
landmarks. A visibility edge between two landmarks
era does not matter.
can be traversed by visual servoing, using the real-time
recognition algorithm discussed in Section 5. At every A p-similar pattern can be detected by comparing
newly-visited landmark, the robot scans for all land- the observed intensity pattern to a version of itself
marks visible from this position, records their relative scaled by p. We can accommodate patterns of lim-
angles and estimates of their distances, and starts an ited spatial extent by restricting the comparison to a
observation history for this landmark. As is discussed window of width w. Let dpu(f) be the average ab-
in Section 5.2, the landmarks all have unique barcodes, solute difference between the original and scaled func-
which are used as node labels in the graph. As men- tions over w:
tioned above, in the process of exploring, the robot
replaces distance estimates with more accurate odom- iecoefyeaesl /HEF
W
aap neaemall C1
etry measurements. Also, as landmarks are revisited
during the exploration phase, the observation histories Then f is locally p-similar over w if and only if
are updated and the probability estimates are refined. dy,w(f) = 0. A simple method for detecting a locally
Once part of the environment has been explored, the p-similar pattern in a one-dimensional intensity func-
robot can enter navigation mode, and accept naviga- tion I(x) would then be to minimize the above measure
tion tasks from the user. For a given goal, the ex- over translations [;(7) = I(x + t):
pected shortest paths are computed and used for path
planning as described above. Navigation mode and ex- te ae ale min oh OEE
ploration mode can be interleaved seamlessly; length
and probability factors are continuously updated in The minimal value of 0 is achieved only if I is p-
both modes based on observations made and edges tra- similar over w at translation t. Unfortunately, all con-
versed. In summary, our navigation system is able to stant functions are p-similar for any p. Thus dpw(Jt)
operate robustly in the presence of unreliable sensory would also be minimal in regions of constant inten-
input, and can cope both with the temporary occlu- sity. To exclude locally constant functions, we must
sion of landmarks and with permanent changes to the detect patterns that are self-similar only for scale p
environment, such as the removal and addition of new (and p*,p®,...), and not for other scales. This can
landmarks. be achieved by mazimizing the mismatch at scale pil?
(and p?/?,p°/?,...). Let us assume without loss of
generality that the range of observable intensities is
4 A Self-similar Landmark Pattern
[0,1]. A maximal mismatch for scale ,/p is then given
if |f(z) — f(,/p2)| = 1, or locally, if:
Our vision-based navigation system relies on real-time
detection of landmarks in the environment. We have
designed a self-similar intensity pattern [16] that can dyow(f) == |Sfx foe Were
‘W
mS)
be quickly and reliably detected in images taken by
the robot. In this section we motivate our use of a In this case we say that f is (locally) \/p -antisimilar.
self-similar pattern and describe the pattern we have
developed. We can combine Equations 14 and 15, and revise
our method for detecting self-similar patterns that are
4.1 Self-similar Functions
only p-similar for a given scale p. We will maximize
the match function:
We say a function f : It = R is p-similar for a scale
It) — Gpw\l:)
m(t) = d ypw( (16)
factor p, 0<p<1, if f(x) = f(px) Vx > 0. The graph
356 A. Briggs, D. Scharstein, and S. Abbott

MUO
1
0

|7
yp 1

0 1
0 x
0 p vp 1
Figure 2: The square wave function S(x).

over all translations t. Note that the range of m/(t) is

[—1, 1], since the range of both terms on the right-hand 0| |
=
side of (16) is [0,1]. A locally constant function will 0 p vp 1
yield m(t) = 0. Similarly, a random intensity pattern Figure 3: Self-similar square wave sp(x) for p= + (top);
that is neither p-similar nor ,/p-similar will yield a maximal mismatch at scale p'/? (middle); and match at
response close to zero. A significant positive response scale p (bottom).
is only expected for intensity patterns that are p-similar
but not ,/p-similar.
direction. See Figure 4 for an illustration. If such a
4.2 An Optimally Recognizable Ppattern pattern is then sampled along any non-vertical line, the
resulting intensity function is still p-similar because of
Given the match measure m defined in Equation 16, the scale invariance of self-similarity. This allows us
we now heed to find an intensity function that yields to detect two-dimensional p-similar patterns that have
the optimal response m = 1. That is, we seek a p- undergone an affine transformation by examining iso-
similar, ,/p-antisimilar function s,(x). To derive such lated scanlines.
a function, let us consider the periodic “square-wave”
function S depicted in Figure 2: Formally, let

ste) =|0, «-—|2|< L(x, y) = 8p(z) (19)

1, #-|a2\|> = [2(@@-[2]})], (7)

Hl
dle
wl
be our two-dimensional landmark pattern, where s,(z)
This function has the property that S(a +1) = S(z) is the self-similar square wave function (Equation 18)
and S(«+ 3) =1—S(z), i.e., it is a 1-periodic function
that is similar under a translation of 1 and anti-similar
under a translation of > It is easy to show that we can
transform S into a p-similar, \/p-antisimilar function
Sp by substituting log, x for z:

Sp(x) = S(log, x) = |2(log, x — [log, x|)| (18)

with the desired properties of djw(sp) = 0 and

d/p,w(Sp) = 1 for any w (see Figure 3).

4.3 Two-dimensional Landmarks

We now have all the components for landmark design

and recognition. The key step for moving to two di-
mensions is to use a pattern that is p-similar in one
direction (say, horizontally), and constant in the other Figure 4: Our self-similar landmark pattern with barcode.
Reliable Mobile Robot Navigation From Unreliable Visual Cues
357
for a fixed p. An affine transformation yields:

A(L(z,y)) = L(az+by+c, det+ey+f) = Sp(az+by+c).

Sampled at y = yo, we get s,(ax + (by) + c)) =
Sp(az + t). Thus, the problem of finding an affine
transformation of the two-dimensional pattern L has
been reduced to finding a translation t of the one-
dimensional pattern s,. == =e = = |

marta Serie aera ee eee

|
5 The Landmark Recognition
Algorithm
The idea underlying the recognition algorithm is to find
locations (%m,Ym) in the image at which a scanline is
locally p-similar and ,/p-antisimilar. To do this, we
adopt the matching function m from Equation 16 for
scanlines in an image I(z, y):
1 w

My(z) =

- =f We+6u)—He+pé,y)\as. (20)
1 Ww

The value of m,(x) depends on the intensities along

the line (x,y) to (x + w,y), and is constrained to the
— = ae =: - = a
interval [—1,1]. If the pattern s, is present at (z,y),
then m,(z) = 1. It is easy to see that the pattern
c8p with reduced contrast c < 1 (ie., difference be-
tween maximal and minimal intensities) will only yield
a response m,(xz) = c. Other (non p-similar) intensity
patterns will yield responses close to or below zero. An
algorithm for finding affine transformations of a land-
mark with intensities L(x, y) = sp(x) (for a known p) Figure 5: Various intensity patterns I(x) and match func-
in an image can be formulated as follows: tions my(x) for p = 2 and w = 50. (See Section 5.1 for
details. )
for every k-th scanline y
for all x
compute m,(z) 5.1 Finding Matches Reliably
mark all strong local maxima of m, as matches.
The interesting question is: What constitutes a
This simple algorithm requires only O(nw/k) opera- “strong” local maximum? A simple answer would be to
tions for an n-pixel image. The computation of m,(z) require that a local maximum be greater than a fixed
can be adapted to discrete images by replacing the inte- threshold cmin (corresponding to a minimum contrast).
grals in Equation 20 by summations, and determining More information, however, can be gained from observ-
inter-pixel intensities using linear interpolation. The ing the shape of m,(z) in the vicinity of a local maxi-
two parameters, k, the spacing of scanlines to search, mum. Figure 5 shows several intensity profiles of scan-
and w, the window size, only depend on the smallest lines that were synthesized from continuous functions
expected size of the landmark pattern and can be fixed. using 10-fold oversampling. Below each intensity plot
Typical values for an image of size 640 x 480 are k = 6 is a plot of m,(z) for p = 2 and w = 50. The length of
and w = 40. each scanline is 400. The top two patterns are locally
358 A. Briggs, D. Scharstein, and S. Abbott

2-similar square waves s, with full and half contrast,

respectively. Both patterns result in clear peaks at the
correct location 2, = 350; however, the value of the
maximum m,(%m) is less than expected. The observed
values are 0.66 and 0.33, while the expected values are
1 and 5. The differences are due to sampling and inter-
polation, in particular at locations of discontinuities or
strong change. The bottom two patterns, on the other
hand, are not 2-similar: (c) is a “random” intensity
pattern taken from a real image, and (d) isa 3-similar
square wave. The match function for the random pat-
tern (c) has no strong peaks, but there is a distinctive
local maximum in the match function for (d). It is,
however, much more rounded and not as “sharp” as
the top two peaks.
An experimental study analyzing the shape of m,(z)
for a wide range of parameters has revealed a simple
but effective test for “sharpness”: Check that a peak at
half its height is no wider than a fixed threshold, which
only depends on the window size w. That is, given a
local maximum v = m,(%m) greater than a threshold
Cmin, test whether m,(%m + 62) < v/2, where dz is
approximately w/10.

5.2 Grouping Matches

The actual landmark patterns can be detected in an

image by grouping individual matches found on con-
secutive scanlines. To be able to distinguish different
landmarks, we add a simple binary barcode to the right
side of the self-similar pattern (as shown in Figure 4).
The patterns can then be recognized and identified as
follows: First, the position and orientation of all land-
mark patterns in the image is determined by fitting
a straight line to each set of three or more individual
matches detected on consecutive scanlines. The exact
vertical extent of each pattern is then estimated from
the intensity distribution to the left of this line. Once
the locations of the patterns are known, their barcodes
can be decoded easily. Figure 6 shows a Pioneer 2 mo-
Figure 6: The mobile robot, and two images taken with
bile robot equipped with a camera, and two sample pic-
its camera. All landmarks have been found, and all but one
tures taken by the robot. All landmarks are detected
have been identified by their barcodes.
correctly, and all landmark numbers are decoded ex-
cept for one that was too far away.
0.33 seconds for 640 x 480 images and 0.08 seconds for
5.3 Achieving Real-time Performance 320 x 240 images (using parameters w = 45 and k = 6).
The recognition algorithm as described above is fairly A dramatic speed-up can be achieved by further re-
fast: Typical running times for a straightforward im- stricting the set of pixels for which m,(x) is computed.
plementation on a 450 MHz Pentium II machine are Recall that we are already considering only every k-th
Reliable Mobile Robot Navigation From Unreliable Visual Cues
359
scanline. We can start by looking at only every 2k-th the landmark detection algorithm is publicly available
scanline, and, only if a match is found, look at its neigh- at http: //www.middlebury.edu/~schar/landmark/.
boring scanlines as well. Since isolated matches are dis-
carded anyway in the grouping process, no landmark
Acknowledgements
will be missed. This technique can yield a speed-up of
up to 2 if few or no landmark patterns are present in the We are grateful to Darius Braziunas, Huan Ding, Cris-
image. Another opportunity for restricting the search tian Dima, Cuong Nguyen, Sorin Talamba, and Peter
is to make use of the fact that peaks corresponding Wall for their insights and their assistance in imple-
to matches have a certain minimum width at a given menting the algorithms presented here. Many thanks
height h (e.g., h = Cmin/4). Given a lower bound I for to Tim Huang and Bill Peterson for helpful discussions
this width, it is possible to scan for peaks by looking and literature pointers, and to Leslie Pack Kaelbling
only at every [-th pixel. A conservative bound for | is and the anonymous reviewers for their insightful com-
typically given by 4;, i.e., half the allowable peak width ments.
used in the test for sharpness discussed in Section 5.1.
Support for this work was provided in part by
Typical values for this number are 4 or 5. We have
the National Science Foundation under grant CCR-
found, however, that even with much higher values for
9902032, POWRE grant EIA-9806108, VT-EPSCoR
I (e.g., 10), only very few peaks are overlooked (typi-
grant OSR-9350540, by Middlebury College, and by a
cally those resulting from patterns with low contrast),
grant to Middlebury College from the Howard Hughes
so further speedup can be achieved with minimal im-
Medical Institute.
pact on robustness.
These modifications result in new running times of Appendix A: Proof of Theorem 1
0.027 seconds for 640 x 480 images and 0.012 seconds
for 320 x 240 images, corresponding to frame rates of Proof: The strategy of the proof is to show that each
37 and 83 frames per second, respectively. Thus, the component of xu pees X) ... forms a bounded
implementation runs at video frame rate even for full- increasing sequence, and thus has a limit. Setting
size images, making it the first real-time method for X® = [0,0,...,0], it is straightforward to compute
landmark detection under affine transformations. that the entries of the vector X = F(X) are all
strictly positive. Of particular importance here is the
observation that every component of X® is at least
6 Conclusion
as big as (and in this case strictly greater than) the
corresponding component of x). With this base case
Artificial landmarks that can be unobtrusively added
verified, we can now argue by induction that each in-
to an indoor environment can serve as practical and
dividual component entry of the sequence xO Ce,
inexpensive aids for mobile robot navigation and lo-
x) ... forms a monotone increasing sequence. To
calization. When detected quickly and reliably they
see why, assume that the claim is true for XO; De
can make task execution more robust by reducing un-
.., X“*), and consider x(+1) — F(X)), The n-th
certainty due to control and sensing errors. When de-
component of X*+) is given by fn(X) while the
tection is unreliable, the navigation strategies planned
should be as robust as possible. This paper has n-th component of xX *) is given by fr ext: Using
addressed the robust planning problem by providing the induction hypothesis that every component of x)
two major contributions. First, we have described a is at least as big as that of x it is not hard to see
probabilistic path planner that computes paths of ex- from equation (5) that Fe. aan) = Pia:
pected shortest length, given landmark visibility his- We must now make a case for boundedness of each
tories. Second, we have proposed a novel landmark component sequence. To begin, consider a node n in
pattern together with the first practical method em- the graph containing an edge connected directly to the
ploying full affine invariants for real-time detection of goal g. Looking at equation (5), the component func-
landmarks. The method operates on single scanlines tion for this node would have the form:
without any preprocessing and runs at video frame rate
without specialized hardware. An implementation of Fs (Xoluii Paw 28 Dag oPae omer) (21)
360 A. Briggs, D. Scharstein, and S. Abbott

Heep queachDagie
spre min lng wy, daw cea) A lates ot] Un(£n)| cs un (Xn) ~~ ul?) (an)| +...

Ae ti + ul) (an) — u® en)

He eee
ey ae Dera TOMI Genet:
Lays a) \2n —Un(@n)| (1 +p+B?+...+D*”)
if
ov crests!ial ahoe el eae
Peg << la, Un (ea)! Tp
The letters a,..., z represent nodes that are potentially
visible from node n, so that x, as well as %q,...,%z In other words, the sequence is bounded. Now since
are just some subset of components from the vector for any X = [21,...,%n,---,2N-1] we have Un(%n) 2
x= Dine, tee tN = 4|:
fn(X), it follows that the monotone sequence in the
By choosing a node n that is adjacent to the goal
n-th coordinate of X, x, x@) |.. will also be
g, we ensure that the length of the edge I, g appears bounded, and hence convergent.
as a candidate in the final minimum of equation (21). To explicitly calculate the upper bound, set z, = 0
The length /,,, also appears in several other minima of in equation (23) and solve to get:
equation (21), but the special significance of this last
minimum expression is that our hypothesis of non-zero = P ee
aE = lg
: i Pp Pp
uf) (0) < (p lng sum
probabilities guarantees that Pna.-.Png-+-Pnz > 0.
Now construct the function u,(X) from f,(X) in The observant reader will recognize this as the expected
the following way. Consider each minimum expression length of the shortest path from a node with a single
appearing in fn. If Ing is among the options, replace
outgoing edge to the goal (Equation 3).
the minimum with [,, (regardless of whether or not
it represents the minimum). If l,g does not appear, The preceding argument shows that the sequence
then replace the minimum with the /,, + 2, option x() converges in any component corresponding to a
(present in every minimum expression). In terms of the node that is adjacent to the goal. With this fact es-
underlying graph, this amounts to ignoring all outgoing tablished, we can now repeat the proof for any com-
edges from node n except for Ing and Inn. By replacing ponent of xi*) corresponding to a node adjacent to a
each minimum with a particular candidate, we have node previously handled. More explicitly, assume node
certainly made the value of the function larger, i.e., r has an edge to node n, and assume we have shown
Un(X) > fr(X) VX. Moreover, combining terms, un that the n-th component of xX) is bounded and hence
reduces to a simpler equation for a node with only a converges. The final minimum in the expression for
single outgoing edge, similar to equation (2), and thus f,-(X) will contain, among other options, the expres-
has the more accessible form: sion l,, + Zn. Knowing that l,, + 2p is bounded, by
say b,, we construct ur(X) = u,-(z,) from f,(X) by
replacing each minimum containing l,., +2, with the
upper bound 6, and selecting /,.,+2,. in all other cases.
where png > 0 and thus ppg < 1. For simplicity, we will
The remainder of the argument is the same.
write p instead of png in the following discussion. The
function wn, although technically defined on R‘7!, Given our hypothesis that from every node there ex-
only depends on the n-th coordinate entry z,. Also, ists a path to the goal, this bootstrapping technique
since p < 1, Uy is geometrically contractive in the sense eventually leads to the conclusion that every compo-
that for any two points x, and z/, we have: nent of our sequence X), xo) Xe) As an ine
creasing bounded sequence of real numbers. By the
nen) ie, | =P |fn — isl Monotone Convergence Theorem, we may set X* =
(zj,25,...,£@_,) where each x* is the limit of the n-
What this implies is that iterating a point z, with uy,
th components of (X“*?).
yields a sequence Zn, Un(Ln), u?) (tn); ... where the
distance of the k-th iteration from the starting point This can be summarized with the statement
Ln Must satisfy: lima gga X Ale eX: (Technically this is a coordinate-
wise limit but we certainly get convergence using most
ltn — un? (tn)| (23) any topology on IR% —1.) Now the continuity of the
Reliable Mobile Robot Navigation From Unreliable Visual Cues
361
component functions f, which make up F allow us to [6] M. Kabuka and A. Arenas. Position verification of a
conclude that: mobile robot using standard pattern. JEEE Journal oif
Robotics and Automation, RA-3(6):505-516, Decem-
F(X") = F( lim X®))
= lim F(X) ber 1987.
k-0o
[7] L. Kavraki and J.-C. Latombe. Randomized pre-
lim Xe") — x*. processing of configuration space for fast path plan-
k- oo
ning. In Proceedings of IEEE International Confer-
ence on Robotics and Automation, pages 2138-2145,
Finally, to argue that X* is the unique fixed point of May 1994.
F, let Y* € R™~ also satisfy F(Y*) = Y*. Consider [8= A. Kosaka and J. Pan. Purdue experiments in model-
a particular component function f, of F (see Equa- based vision for hallway navigation. In Proceedings
tion 21) and again pay special attention to the final of the Workshop on Vision for Robots in IROS’95,
minimum term where the variables of all potentially Pittsburgh, PA, pages 87-96, 1995.
visible nodes are included. If X™* is fixed by fp, ie., [9] A. Lazanas and J.-C. Latombe. Landmark-based ro-
fn(X*) = a%, then Inn + 2% cannot be the minimum bot navigation. Algorithmica, 13(5):472-501, May
here since otherwise it would be the minimum through- 1995.
out and we would have f,(X*) = 2* + Inn. (This is [10] C. Lin and R. Tummala. Mobile robot navigation us-
were we need the hypothesis that all edge lengths are ing artificial landmarks. Journal of Robotic Systems, °
strictly positive.) The same observation of course holds 14(2):93-106, 1997.
for Inn + y=. But now, using the fact that the proba-
[11] B. Nickerson, P. Jasiobedzki, D. Wilkes, M. Jenkin,
bility preceding this final minimum is strictly positive, BE. Milios, J. Tsotsos, A. Jepson, and O. N. Bains. The
we can show that: ARK project: Autonomous mobile robots for known
industrial environments. Robotics and Autonomous
|fn(X") — fa(¥")| < lan — yal: Systems, 25:83-104, 1998.

[12] M. H. Overmars and P. Svestka. A probabilistic

Since X* and Y~ are both assumed to be fixed by F, learning approach to motion planning. In Goldberg,
this is only possible if X* = Y~. a Halperin, Latombe, and Wilson, editors, 1994 Work-
shop on the Algorithmic Foundations of Robotics, A.
K. Peters, pages 19-37, 1995.
References
[13] C. Owen and U. Nehmzow. Landmark-based naviga-
tion for a mobile robot. In Proceedings of Simulation
[1] C. Becker, J. Salas, K. Tokusei, and J.-C. Latombe. of Adaptive Behaviour. MIT Press, 1998.
Reliable navigation using landmarks. In Proceedings of
IEEE International Conference on Robotics and Au- [14] M. Puterman. Markov Decision Processes: Discrete
tomation, pages 401—406, June 1995. Stochastic Dynamic Programming. John Wiley &
Sons, New York, NY, 1994.
[2] D. M. Blei and L. P. Kaelbling. Shortest paths in
a dynamic uncertain domain. In Proceedings of the [15] J. Salas, J. L. Gordillo, and C. Tomasi. Visual rou-
IJCAI Workshop on Adaptive Spatial Representations tines for mobile robots: Experimental results. Expert
of Dynamic Environments, 1999. Systems with Applications, 14:187-197, 1998.
[16] D. Scharstein and A. Briggs. Fast recognition of self-
[3] O. Faugeras. Three-Dimensional Computer Vision. similar landmarks. In Workshop on Perception for
MIT Press, Cambridge, MA, 1993. Mobile Agents (in conjunction with IEEE CVPR’99),
pages 74-81, June 1999.
[4] C. Fennema, A. Hanson, E. Riseman, J. R. Beveride,
and R. Kumar. Model-directed mobile robot naviga- [17] R. Sim and G. Dudek. Mobile robot localization from
tion. [EEE Transactions on Systems, Man, and Cy- learned landmarks. In Proceedings of IEEE/RSJ Con-
bernetics, 20(6):1352-1369, 1990. ference on Intelligent Robots and Systems (IROS),
Victoria, BC, October 1998.
[5] D. P. Huttenlocher, M. E. Leventon, and W. J. Ruck-
lidge. Vision-guided navigation by comparing edge im- [18] R. Simmons and S. Koenig. Probabilistic navigation
ages. In Goldberg, Halperin, Latombe, and Wilson, in partially observable environments. In Proceedings
editors, 1994 Workshop on the Algorithmic Founda- of the International Joint Conference on Artificial In-
1995. telligence, pages 1080-1087, 1995.
tions of Robotics, A. K. Peters, pages 85-96,
362 A. Briggs, D. Scharstein, and §. Abbott

[19] K. Tashiro, J. Ota, Y. C. Lin, and T. Arai. Design [20] C. J. Taylor and D. J. Kriegman. Vision-based mo-
of the optimal arrangement of artificial landmarks. In tion planning and exploration algorithms for mobile
Proceedings of IEEE International Conference on Ro- robots. In Goldberg, Halperin, Latombe, and Wilson,
botics and Automation, pages 407-413, June 1995. editors, 1994 Workshop on the Algorithmic Founda-
tions of Robotics, A. K. Peters, pages 69-83, 1995.
Toward Real-Time Path Planning in Changing
Environments

Peter Leven, University of Illinois, Urbana, IL

Seth Hutchinson, University of Illinois, Urbana, IL

We present a new method for generating collision- approach. During a preprocessing stage, the planner
free paths for robots operating in changing environ- generates a set of nodes that correspond to random
ments. Our approach is closely related to recent prob- configurations in the configuration space (hereafter, C-
abilistic roadmap approaches. These planners use pre- space), connects these nodes using a (simple, local)
processing and query stages, and are aimed at plan- path planner to form a roadmap, and, if necessary, uses
ning many times in the same environment. In con- a subsequent sampling stage to enhance the roadmap.
trast, our preprocessing stage creates a representation During the second, on-line stage, planning is reduced
of the configuration space that can be easily modified in to query processing, in which the initial and final con-
real time to account for changes in the environment. figurations are connected to the roadmap, and the aug-
As with previous approaches, we begin by constructing mented roadmap is searched for a feasible path.
a graph that represents a roadmap in the configuration Our new approach is a descendant of the probabilis-
space, but we do not construct this graph for a spe- tic roadmap methods. Our goal, like theirs, is a real-
cific workspace. Instead, we construct the graph for an time planner that uses approximate representations
obstacle-free workspace, and encode the mapping from such as those provided by computer vision or range
workspace cells to nodes and arcs in the graph. When sensors. However, unlike probabilistic roadmap meth-
the environment changes, this mapping is used to make ods, our method is intended for robots that will operate
the appropriate modifications to the graph, and plans in changing environments, and therefore we cannot ex-
can be generated by searching the modified graph. ploit the premise that planning will occur many times
After presenting the approach, we address a number in the same environment.
of performance issues via extensive simulation results Our method begins, as do the probabilistic roadmap
for robots with as many as twenty degrees of freedom. planners, by constructing a roadmap that represents
We evaluate memory requirements, preprocessing time, the C-space. Nodes are generated by a random sam-
and the time to dynamically modify the graph and re- pling scheme, and connections between nodes are gen-
plan, all as a function of the number of degrees of free- erated using a simple, straight-line planner. Unlike
dom of the robot. the probabilistic roadmap planners, we generate a
roadmap that corresponds to an obstacle-free environ-
1 Introduction ment. Then, in a second phase of the preprocess-
ing stage, we generate a representation that encodes
In this paper, we present a new method for generating the mapping from cells in the discretized workspace
collision-free paths for robots operating in changing en- to nodes and arcs in the roadmap. These two phases
vironments. Our work builds on recent methods that are specific to the robot, but are independent of the
environment in which the robot will operate. The
use probabilistic roadmap planners [3, 8, 12, 22, 25].
The idea that the cost of planning will be amortized fact that the preprocessing is completely independent
over many planning episodes provides a justification of the robot’s target environment removes constraints
for spending extensive amounts of time during a pre- on preprocessing time. Indeed, with our approach, it
processing stage, provided the resulting representation is feasible that when a new robot is designed, an ex-
can be used to generate plans very quickly during a tended period of preprocessing could be performed, at
query stage. Thus, these planners use a two-stage the end of which, the robot would essentially be prepro-
364 P. Leven and S. Hutchinson

grammed to construct path plans in any environment [4, 18]. and for manipulation planning (i.e., construct-
that it might encounter. ing a sequence of transfer and transit paths) [1].
In the on-line planning phase, the planner first iden- There have been several previous approaches to path
tifies the cells in the discretized workspace that cor- planning in changing environments. In some cases, off-
respond to obstacles (e.g., by using a range scan- line planners have execution times that make it feasible
ner or stereo vision system), and then uses the en- to directly use them in some kinds of changing environ-
coded mapping to delete the corresponding nodes and ments with no modifications. This is the case, e.g., for
arcs from the roadmap. Planning is then reduced to the Ariadne’s Clew algorithm reported in [4, 18]. The
connecting the initial and final configurations to the Ariadne’s Clew algorithm operates by generating land-
roadmap (again, as is also the case with the probabilis- marks (during an exploration phase) and then connect-
tic roadmap planners), and then searching the roadmap ing them to the existing network (the search phase).
for a path between these newly added nodes. Of course Variations of this algorithm can be obtained by vary-
it is possible to add obstacles to the environment in ing the search phase, and by using different optimiza-
such a way that the roadmap becomes disconnected. tion criteria to select candidate landmarks [1, 19]. The
This is true for any of the probabilistic roadmap plan- idea of incrementally expanding a network for single
ners (once one knows how samples are selected, and query planning has also been used in [9] and [15]. In
how these samples are connected by local planners, it both of these, networks are grown from both the initial
is fairly straightforward to construct environments that and goal configurations until they can be connected.
will thwart them), but, as we will describe in subse- In [9] the notion of expansive C-spaces is used, while
quent sections, there are a number of steps that can be in [15], random trees are used. In [24] an adaptable ap-
taken to cope with this problem, both at runtime and proach that uses multiple local planners is described.
during the preprocessing stages. At runtime, characteristics of the problem are used to
In the on-line planning stage, our method runs in determine which (combination of) local planners will
real time; plans are generated in less than one second. be most effective.
Thus, it is feasible to use the planner even in the case We also note here that in two of these previous ap-
when obstacles are moving in the environment with proaches ((8] and [19]), the idea of somehow represent-
unknown trajectories, provided a sensing system can ing the mapping from the workspace to the C-space
identify in real-time those regions of the workspace that was incorporated. In [8], during the off-line planning
are occupied by the obstacles. stage, the planner is aware of a set of obstacles that
might be present in the environment (in their exper-
2 Related Research iments, a single obstacle was used). The locations of
these obstacles are specified a priori, and at runtime
There has been much work on probabilistic roadmap the robot sensor system determines which, if any, of
planners, including planners for articulated robots the obstacles are present in the environment. Dur-
[12, 22, 8], mobile robots in two dimensional environ- ing the off-line planning stage, the trajectories in the
ments [22], free-flying rigid objects in three dimensions paths tree that cause collision with each of these ob-
[2], and flexible surfaces [7]. There are also versions of stacles is determined. When objects are detected at
these planners that are geared toward single-shot mo-
runtime, the corresponding arcs are deleted from the
tion planning [9, 15, 24]. Some modifications to the
graph. In [19], the paths tree (created by the modified
sampling methods have also been introduced: obstacle
Ariadne’s Clew algorithm) is augmented to generate a
boundary [22, 3] and medial-axis [25].
graph. Then, for each path in the graph, the corre-
The sample configurations generated during the con- sponding workspace cells are identified. Thus, when
struction of a probabilistic roadmap can be thought of a new obstacle is added to the environment, the set
as landmarks in the C-space. A family of approaches of paths that intersect that obstacle can be deleted
based on the Ariadne’s Clew algorithm generate land- from the graph. Because these authors are primarily
marks and search for paths between landmarks in a de- interested in domains for which extensive preprocess-
terministic fashion by solving optimization problems. ing is not viable, they construct relatively small graphs
This method has been used to plan for manipulators (fewer than 100 landmarks, with fewer than 400 cor-
Toward Real-Time Path Planning in Changing Environments
365
responding paths). Therefore, it is fairly easy to add set of tasks that the robot will perform, were available,
obstacles that would disconnect the graph, even though an appropriate importance sampling scheme, or even
these obstacles might not cause the free C-space to be- a deterministic scheme (if the existence of certain ob-
come disconnected. In their experimental evaluation stacles were known in advance) could be used. More
of their planner, a single obstacle was added, and the effective schemes for generating sample configurations
addition of this obstacle resulted in disconnecting the are discussed in Section 6.
graph into five components. In such cases, their algo-
rithm resorts to the Ariadne’s algorithm to reconnect
3.2 Connecting the Nodes
the graph, which can take time long enough to prohibit
the planner from being used in environments with mov- In order to create a graph, pairs of nodes are connected
ing obstacles. by arcs that correspond to trajectories of the arm.
We believe that our planner is the first planner that Like several of the previously mentioned approaches,
constructs a representation that can be used to replan we connect each node to its k-nearest neighbors. In
in real time in changing environments. In the remain- our current implementation, the paths corresponding
der of the paper we describe the specific details of the to arcs are not tested to ensure that no self-collisions
algorithm, give extensive evaluation of our methods via take place along the path.
simulation results, and discuss the current trajectory of The distance metric used for determining the neigh-
our research efforts. bors of a node plays a very important role in the proba-
bilistic roadmap methods [2]. For these planners, good
3 Constructing the Roadmap distance metrics provide a set of neighbors to which the
local planner has a good likelihood of generating a col-
Our construction of a roadmap of the C-space is lision free path. In addition, the distance metric must
very similar to methods used in previous probabilistic be fast to compute, as it will be called many times to
roadmap planers [12]. Nodes are generated by gener- evaluate pairs of nodes (this is particularly true for the
ating sample configurations, and these nodes are then single query planners).
connected to form a graph. We will denote this graph For our application, the swept volume of a path
by G = (Gn; Ga), in which S,, is the set of nodes in the in the workspace connecting two configurations would
graph and JG, is the set of arcs in the graph. be an ideal metric. Unfortunately, as noted by oth-
Since the graph is constructed without the presence ers [2, 13], this metric is very expensive to compute,
of obstacles in the workspace, local planning to con- and therefore, in our current implementation, we have
nect the nodes is trivial. Generating samples is po- opted for approximations that are faster to compute.
tentially more complex than for the traditional proba- In [2], a number of metrics were evaluated for efficiency
bilistic roadmap planners, since we cannot exploit the and effectiveness for the case of a rigid object translat-
geometry of the C-space obstacle region to guide the ing and rotating in a three-dimensional workspace. In
sampling. We now discuss sampling the C-space, and particular, as we will discuss below, we have performed
generating arcs to connect the resulting nodes. extensive experimental evaluation with the following
four distance metrics:
3.1 Generating Sample Configurations
the 2-norm in C-space
Since there are no obstacles to consider, it is fairly easy
to generate samples in the C-space. The only hard con- n 3
straint is that self-collision (i.e., collision between dis- Ds (a,q') = lla’ — all = yo a af
tinct links of the robot) is prohibited. At present, we 4=1

generate configuration nodes by sampling from a uni-

form distribution on the C-space. This approach re- the co-norm in C-space
flects a complete absence of prior knowledge about the
environment in which the robot will ultimately operate. DS.(a,q") = max|qj — a|
If prior knowledge, either about the environment or the
366 P. Leven and S. Hutchinson

the 2-norm in workspace tween the workspace and the C-space. To represent the
workspace, we use a uniform, rectangular decomposi-
tion, which we denote by W. We denote the C-space
Dy (q,4’) = paiacr = nal by @, and define the mapping ¢: W — € as
peA

the co-norm in workspace d(w) = {q| A(q) Nw FO},

DY (aa) = maxlp(4!) - (| in which w is cell of the workspace and A(q) denotes

that subset of W occupied by the robot at configuration
In each of these equations, the robot has n joints, q and q. We note that ¢(w) is exactly the C-space obstacle
q’ are the two configurations corresponding to different region (often denoted by CB) if w is considered as a
nodes in the graph, q; refers to the configuration of the polyhedral obstacle in the workspace. In our approach,
i-th joint, and p(q) refers to the workspace reference we do not explicitly represent the C-space, but instead
point p of the set of reference points of the robot A use the roadmap §. Therefore, we define two additional
at configuration q. Versions of DW and D3” were also mappings, one from the workspace to the nodes in the
used in [13]. graph, and one from the workspace to the arcs in the
graph:
Once a node’s k-nearest neighbors have been identi-
fied, a local planner is used to connect the correspond-
dn(w) = {4 € Gn |A(q) Nw FO},
ing configurations. As a motion planner, this local
planner in the preprocessing phase has very modest
ba(w) = {7 € Ga |A(q) Nw F O for some q € 7}.
requirements. It should be reasonably fast, as that re-
duces the time needed to construct the data structures, 4.1 Computation of ¢, and ¢,
but this is not a primary concern. It must be determin- In terms of the implementation, it is much easier to
istic, i.e., it must always return the same path when compute the inverse maps ¢,' and ¢,'. Therefore,
given the same two nodes as input. This is required we use the inverse maps for the construction of our
since our approach will use the volume swept by the representation of the mapping.
robot when it traverses this path. If the path changed
each time, we would be unable to guarantee that the The construction of the representation for ¢,1 is
path was not blocked during on-line planning. In ad- straightforward. For each q € G, we note the map-
dition, the local planner should consider self-collisions ping from each w € ¢,1(q) to the corresponding q.
of the robot when determining whether two nodes can The set of cells in ¢,1(q) is computed by expanding
be connected (although we have not yet implemented a “seed” cell in a set of shells surrounding the seed.
this feature). The seed for fixed-base articulated robots is the origin
of the robot, which is not stored as part of ¢,. The
We also note here that, during the on-line planning
shell expands in each direction in the workspace until
phase, a second local planner will be used to connect
the collision tester determines that a cell is outside the
the initial and final configurations to the roadmap. In
robot. We use the collision-checking package V-Clip
contrast to the planner used to connect nodes in the
[20] to test for cell intersection with the robot.
graph, this planner does have to consider the obstacles
around the robot. Since this problem is faced by all of The computation of ¢71 is more complex and time-
the probabilistic roadmap planners, we plan to adapt consuming. It involves computing the swept-volume of
existing methods for this problem (e.g., the technique the robot as it traverses a path computed by the lo-
presented in [24] may be used, which selects the lo- cal planner between two configurations. In our imple-
cal planner based on an evaluation of which planner is mentation, cells that are occupied by the robot at the
most likely to succeed). endpoints of the arc ¥ (i.e., the configurations that cor-
respond to the two nodes connected by the arc) are not
4 Workspace to C-space Mapping included in ¢71(7). Computing the swept volume for
a robot trajectory is not a trivial problem. A method
The ability to plan in real time in changing environ- for swept volume computation for three dimensional
ments comes from our encoding of the relationship be- objects that can only translate is presented in [26]. An
Toward Real-Time Path Planning in Changing Environments
367
extension to this method was presented that could also The spatial coherence of CB has been exploited in
accommodate rotation about a single axis, but no re- previous collision checking approaches (e.g., [17, 20]).
sults were given. This approach could be used to sim- In our case, spatial coherence derives from the conti-
plify the computation of ¢7!. nuity of ¢, namely, that small changes to w will cause
only small changes to ¢(w). Because of this, for some
We have developed a method for computing ¢7!(7)
cell, say w* € W, we expect that @(w) will be very
as follows. First, ¢,1 for the two endpoints of y is com-
similar to ¢(w*) for w € n(w*), with n(w*) some ap-
puted. Then, the path corresponding to y is sampled
propriate neighborhood of w*. This spatial coherence
using a recursive bisection method, which proceeds as
presents a situation that is somewhat analogous to the
follows. First, the configuration corresponding to the
situation confronted in video compression: in a stream
midpoint of the segment connecting the two nodes is
of images, there will be only small variations between
computed, and ¢,' for that configuration is computed.
most adjacent images in the sequence. This is one
If this set contains any cells not already in ¢;1 of the
of the premises for many modern video compression
endpoints, these new cells are added to the swept vol-
methods (e.g., MPEG [21]). Unfortunately, we can-
ume, and the path is subdivided again on both sides
not directly apply video compression techniques, since
of the midpoint. This process is repeated until no new
these techniques generally employ lossy compression
cells of W are added.
(i.e., the original image sequence cannot be exactly re-_
constructed), and in our case this could lead to colli-
4.2 Efficient Representations of ¢ sions.
The discussion above suggests the following ap-
Even though computer memories are rapidly growing,
proach: partition W into a set of neighborhoods, and,
it is beneficial to compress the representations of dy,
for each neighborhood (a) choose a representative w*,
and ¢,, provided that this can be done without drasti-
(b) derive a compact representation of ¢(w*) and (c)
cally increasing the computation required to compute
for all w € n(w*), express ¢(w) in terms of ¢(w*). We
plans online. Reducing the size of the representation
postpone the discussion on step (b) to Section 6.3. In
will also enable us to consider larger graphs 9, which
some cases, we may be able to improve upon this by
will increase the efficacy of the online planning.
selecting some other reference set in step (c), and we
From an information theoretic viewpoint, compres- discuss this below.
sion of a data set involves the reduction of redundancy Given the above, we can formulate the corresponding
in that data set. The amount of compression that can optimization problem. For specific choice of neighbor-
be performed is limited by the information content of hood system we have the cost functional:
the data set, which, in turn, is related to the degree
of unexpectedness, or randomness, in the data set [5].
There are three sources of redundancy in the repre-
£(W*,n) =
sentation of ¢, and ¢, that can be exploited: 1) the
spatial coherence of the set ¢(w) in ©, 2) spatial co- S> fcostig(w]+ S> cost[d(w)] }, (1)
w*Eew* wen(w*)
herence of ¢(w) for neighboring w’s in W, and 3) the
representation of the labels of the nodes and arcs in
in which W* is a set of representative cells in W, n(w*)
the graph. The third source provides only limited ef-
is the set of neighbor cells for w*, and cost{d(w)] de-
ficiency increases, as much as a factor of two for our This
notes the cost of encoding the representation.
experiments, depending on the size of the graph.
leads to the the optimization problem:
The spatial coherence of ¢, and @p is based on the
minimize £(W*,n)
following ideas. Since the robot is connected, and since (2)
subject to: U,,«cew= (w*) =W and
w is connected and compact, then ¢(w) is connected
and compact. This follows from the application of well-
n(we) Nn(w7) = 9% F J.
known facts about the C-space obstacles [16], and that
¢(w) is exactly the C-space obstacle region if w is con- This particular formulation of the cost suggests an
sidered as a polyhedral obstacle in the workspace. algorithm that first selects representatives in W and
368 P. Leven and S. Hutchinson

then builds the appropriate neighborhoods. The en- which we have applied the following greedy expansion
coding of the neighborhoods is based on the idea dis- algorithm. The elements of W are placed into a pri-
cussed above that there will be only minor variations ority queue, Q in decreasing order of the size of the
in @ over local neighborhoods of W. Assuming for the representation of ¢(w) (ie., |dn(w)| + |¢a(w)|). Let
moment that the neighborhoods have been determined, w* be the first element in Q. Initialize n(w*) = {w*}.
one way to encode ¢ over a neighborhood 7(w*) is to Then, until the cost of adding an additional neighbor
first determine a representative ¢*(w*) and to then to n(w*) exceeds the cost of directly encoding ¢(w) or
specify ¢(w) relative to ¢*(w*) for each w € n(w*). a size threshold is reached, expand the neighborhood,
This is essentially a differential encoding, and can be deleting the added w from Q. In the hybrid approach,
specified as ¢/(w) = o(w) ® ¢*(w*), where ¢’(w) is each cell w is placed in the neighborhood 7 or m2,
the encoded representation of ¢(w) and © is the set depending on which minimizes the cost of its represen-
symmetric difference operator. tation. This is repeated for the new head of Q until the
neighborhoods form a partition of the workspace.
There are two methods for determining ¢*(w*). The
first of these is to use d(w”) itself, ie., d*(w*) = d(w*). Note that this operation is performed separately for
The second is to compute ¢*(w*) as the set that mini- both ¢, and dg.
mizes:
5 Empirical Evaluation
Ss” |é’(w)|,
wen(w*) For a real-time path planner, we are interested in three
parameters: graph update time, planning time, and
where |-| denotes set cardinality. The first method may data structure size. We are also interested in the
lead to a more efficient representation of ¢*(w*) itself,
preprocessing time, though this is less important. In
while the second minimizes |¢’(w)| for each w € n(w*). this section, we evaluate a preliminary version of the
This is a tradeoff that will have to be evaluated when planner, studying the case of serial-link manipulators
selecting which set to use for ¢*(w%*).
with revolute joints operating in a two-dimensional
There is another possible differential encoding workspace. Note that our implementation is not lim-
scheme that can also be used to take advantage of the ited to revolute joints; prismatic joints can be used
spatial coherence of ¢(w) over W. This approach is as well. We limit the following discussion to revolute
similar to above, except that instead of choosing one joints to simplify the analysis.
g*(w*) over n(w*), the ¢* is chosen individually for
each w € 7(w*) from the set of cells adjacent to w that 5.1 Experimental Set-up
are closer to w*. In this approach, ¢(w*) is encoded by
itself, the cells adjacent to w* are encoded using ¢(w*) The following experimental set-up was used to eval-
as their reference, and for each of the other w € n(w*), uate the approach. The robots tested are all serial-
the neighbor w’ to w that results in the smallest |¢’(w)| link manipulators with two to 20 revolute joints in a
is chosen, with ¢’(w) = ¢(w) @ ¢(w’). two-dimensional workspace. The workspace cells and
the robot are given three-dimensional polyhedral de-
These two approaches may be combined in a hy-
scriptions to allow the use of standard collision check-
brid approach, such that the neighborhood 7(w*) is
ing packages for collision testing. Each cell of the
composed of two parts 71(w*) encoded using the first
workspace is modeled as a unit cube, and each link
scheme above and 72(w*) encoded using the second
of the robot is modeled as a rectanguloid with a width
scheme. The cell w* is encoded as part of 7,(w*), and
of 2.1 units and a height of 1 unit. The total length of
each w € 12(w*) is encoded with respect to the adja-
the robot was fixed at 70 units, with each link length
cent cell that is closest to a member of 7; (w*).
being scaled appropriately. Each joint of the robot was
Still to be addressed is the problem of selecting the given the full revolute range of motion, and the joints
best set of representative cells in the workspace and were allowed to wrap around. In addition, the robot
the problem of choosing the best neighborhoods 7; (w*) was not tested for self-collision, except when the sam-
and 2(w*) for each w* € W* that satisfies (2). This ples of C-space were generated. To evaluate how the
is a difficult combinatoric optimization problem, for data structures and computation times grow with the
Toward Real-Time Path Planning in Changing Environments
369
graphs, we tested graphs with 2048, 8192, and 16384 1000 ag
nodes. eevee

5.2 Results for Serial-link Manipulators in a

2D Workspace

In this section we present some results from using the s

Number
above setup. We start with an analysis of the data TTT

structures that are computed to implement our plan-

hing approach. Each of the four data structures—
graph nodes, graph arcs, ¢,, and ¢a—was computed = 7

and stored separately to simplify analysis. We follow ee ee ge

the analysis with some compression results for ¢, and 2713) 4 606 97) 18) 19) “101 A 4a Hei gas: Ae Hy Be tomeo
Number of joints
¢a, and we conclude with some planning examples.
Figure 1: Number of samples of C-space generated per
5.2.1 C-space Sampling
node accepted.
The C-space for each of the robots was sampled uni-
formly at random, except for the first 129 samples. bytes, where m is the number of nodes and n is the
The first 129 samples were chosen such that the robot number of joints.
covers as much of the workspace as possible. For the
robots tested, this means that 129 uniformly-spaced 5.2.2 Graph Construction
samples were taken of the range of the first joint, and
As described above, arcs in the graph are constructed
the remaining joints were held fixed at a position that
for the k-nearest neighbors of each node. For our eval-
maximized the length of the robot. Of the remaining
uation, we tested the case of k = 5 nearest neighbors.
random samples, samples in which the robot collides
We computed the nearest neighbors using the simple
with itself were rejected.
O(n?) method of calculating the distance between all
It is interesting to note that, when sampling the C- pairs of nodes and keeping the closest five for each
space of the robot uniformly, it became exponentially node. The paths connecting these neighbors are not
more difficult as the number of joints increased to find checked for feasibility, i.e., whether the robot avoids
configurations in which the robot did not collide with self-collision while following the paths.
itself. This is a consequence of a robot consisting of The computation times for computing the graph are
revolute joints operating in the plane: For each set of shown in Figure 2. The increase in computation time
three links, there is a region of the C-space in which with the number of joints reflects the increase in the
the third link is in collision with the first. For each cost of computing the metric. The sharp rise in the
additional joint beyond three, there is a combinatoric computation time after 16 joints seen in the graph for
effect with combinations of links in collision, as well as the D&, and D§ distance metrics is unexpected; this
an interaction among these links that tends to make may be due to a memory effect in the implementation,
the collision regions larger. For a non-planar robot where the amount of memory used for the calculation
operating in 3D, this effect is expected to be drastically crosses some system threshold.
reduced.
What also can be seen in Figure 2 is that there is
The exponential sampling effect can be seen in Fig- not much difference in computation time between the
ure 1, which shows the number of samples of C-space 2-norm and the oo-norm, except that the computations
generated per node accepted. Notice that the y-axis involving the oo-norm tend to take somewhat more
scale of this graph is logarithmic. As expected, the more time to perform (this is likely an artifact of the
number of samples generated per node accepted does processor architecture, in which the cost of a branch is
not change as the desired number of nodes increases. greater than the cost of a floating-point multiplication).
The size of the data structure for the configurations A greater difference can be seen between the metric in
associated with the nodes in the graph is 8mn + 12 C-space and the metric in the workspace, where the
 P. Leven and S. Hutchinson

+1 Bess es 2
4000 2 eee ae 1400 | obee rT oT a eB
Ss
8192 ---»--- 8192 ---x-- bs
900 + 1 6384
1200|
|
«
800

700
1000| - J
él

Paes
2 500 +
g -
-
400 + |
600| 4
300 -

eee
400
x : =
|
200| a 1
“_ acai
at cata oe --- ~ ee 5 -*
yen etka mepesipee eee
10 < ae eve eal
Pe cated
cet 44 12) 4S" 14) 16: 16-17. 16 18 20 (a a a
233° 4 5 6 (7 8) 18°10. 2-8 4 6 & 7 B '9 40 414, 12 10 34 16 16°47. 96. 49/20
Number of joints Number of joints

Dz, metric DZWw metric7

can ee
1900, : | 1900; pera
pe eT a
2048 —+—
8192 ---x--- 8192
| 16384
900
--» |16304
1200
x
4
x
a +
700 1000+

; |
600 +
2 800 x
g
‘> 500 @ «
=
400 +
E600 |
”
ne «
300 Pe 400 + * 4
“
200 x a:
|
ae x pe 200 — * S 4
eer Ee sre ae
SR — a
Sh eee
a eee | tee
a ee a oroe oo
6) 6) 7 18! 4B! AO Ay 129 4S: 14 16 46° A?’ 16° 1a) ‘20
2) 3-4) 2.3, 4.5) 8 7 <8. -B 10 41.42 13) 14. 18) 48°47, 18 19) 20
Number of joints ‘Number
of joints

D5 metric Dz"Ww metric:

Figure 2: Time in seconds to compute the k-nearest neighbors for the different distance metrics.

latter appears to have a more linear increase in compu- head for maintaining the data structure being amor-
tation time as the number of joints increases. Overall, tized over more nodes. The overhead for the data struc-
for robots with more than five joints, the workspace ture depends on the number of cells in the workspace
metric takes more time to compute. that intersect with the robot at any node in the graph.
The size of the graph varies linearly with the num- This overhead does not depend on the number of nodes
ber of nodes, with an average size of 163kB for 2048 in the graph, as long as the graph contains nodes that
nodes, 654kB for 8192 nodes, and 1309kB for 16384 span the reach of the robot in the workspace (i.e., the
nodes. The variance in size from the average was less graph contains nodes that touch all the cells in the
than 15%. workspace that the robot can reach).
The time required per node to compute the node
5.2.3. Computing ¢n
obstacle data structure increases linearly as the number
The overall size of ¢, is the product of the number of of joints increases. This is expected because the size of
nodes and the average number of cells the robot covers the geometric description of the robot used for collision
in the workspace. For a robot with a fixed maximum testing increases with the number of joints.
length, this means that the size of ¢, is largely inde-
pendent of the number of joints. For our experiments, 5.2.4 Computing ¢,
the size of ¢, per node in J, averages around 1000
The size of ¢, for different numbers of nodes is shown
bytes.
in Figure 3. As can be seen in the figure, ¢, can be-
There is a sub-linear increase in the size of ¢,, as the come quite large. This is the reason that the graphs
number of nodes increases that is a result of the over- stop at 13 joints for the 8192 node case, and 8 joints for
Toward Real-Time Path Planning in Changing Environments
371
A Ey Ls T soem) 2)
inf-norm, C-space —+—
"norm, C-space ------
90 + inf-norm, workspai * 2 ————
2-norm, workspace ©
ees

8
(Mb)
Size

cells/arc
Average

4 4 4 — 7 ais
9 10 11) 12.43 =F 15 16 17 18 19 20
Number of joints

2048 nodes —i_t —e 1

10° tile 12) 134)
tL
15:
4
16.
tt
17% 18
1
19°
4
20
Number of joints

250 T = a i
inf-norm, C-space = d ; iS
2-norm, C-space
inf-norm, workspace
rm,
---x---

ce
»--*
—-2
Figure 4: Average of |¢z'(y)| per y for Ga generated from
200 + _ the ey metric.
es & a
iz iad

le
Pa
ore g
“a

=>
150 F
4
ws
x
we g
16384 node cases in order to evaluate the compressibil-
22 Ae fe
F spst
ity of the resulting data files (see Section 5.2.5).
100 + Ox a
we
ae It is also apparent that the D¢ performs the worst
in terms of the size of ¢a. The D§ is the next worst,
fx -

ar oefe ? cs
fe « and the DW and D3” produce the best results. It is
f
0 =
fiseers
aa n 1 4 ot ee? 4 n n 4 1 4 4
expected that the two metrics defined on the workspace
29) 455, 6 er OS) 9) 10) tt
Number of joints
WA AS 14 AS. AG) 17) “18: “fe: 20
would produce better results than the C-space metrics,
8192 nodes since the workspace metrics penalize the motion in the
workspace more than do the C-space metrics. There is
300 T SG C-space T T T T T T T T T 7 no apparent advantage to D3” over DW.
2-norm, C-space ---*---
inf-norm, workspace ---+* ic)

2505
2-norm, rkspace —&
Another interesting point in Figure 3 is the appar-
ro}
ent inflection point at six joints in the graphs for the
200 +
4
, a size of ¢, when computed using the D” and D}” met-
hg
ss
3 5 Ae
f rc}
rics. This inflection point is more obvious in the 8192
=g 150+ / 7x
a and 16384 node graphs. An explanation for this is that,
given the resolution of the workspace that we are using,
the random sampling is better able to “fill” the C-space
with samples for the robot with fewer joints. This re-
sults in the nodes being closer together, and, therefore,
Number of joints
the paths between them do not cover as much of the
16384 nodes
workspace. This effect can also be seen in Figure 4,
which shows the average size of ¢,1(7) per 7 € Ga for
the DY metric (DY produces similar results).
Figure 3: Size of da.
Shown in Figure 5 is the time it takes to compute
da for the DE and D}” metrics (the DE, and the DY
the 16384 case. The limit in this case was the process
produce similar results to the DE and D3” metrics,
size of the program computing ¢,; in order to keep respectively). These times correlate well with the in-
the computation times comparable, the memory size crease in the size of ¢a: The larger da is, the longer
of the program computing ¢a was limited to the physi- it takes to compute. Notice that the graph for the
cal memory available in the computer. More examples computation times for the workspace metric shows an
were computed for the D3” metric for the 8192 and inflection point like the graphs for the size of @a.
 P. Leven and S. Hutchinson

St eg

160000 So io tT
2048 —+— * ie
8192 ---x /
16384 ---* if
140000 }+- sf

: x

120000 + * wf
f x
100000 }- P
3
B ! u as
a el x’
E x
a y;
is Compression
Ratio

60000 + ; 3
f Re
40000 + * ya
x
# ras Pe
zoo |

ee 2 3 4 6
x
6 7
re
B 18
—
40
a
At) s2sid 44 15
oS!
16 i7 16
st
18 (20
0 aa 1 eee eee 1 es et ee | 1 ie
ees) cay i&) GY 7 8) el tO) TH) 12°49. 14" 16°16 “17 “18 19. (20 Number of joints
Number of joints

(b) on
(G .
Ds metric
180000, -— +1 —7 T T T T ee
Oo Te a eT ep 18 T 1 roa T TT T yt
1048 —+— 2048 —+—
8192 -—-*--- | 8192 -
160000 ai Eola x 16384 -*
16 + 4

140000 re =H
oe he 4
14+ er,
120000 + x 4 i Pm
“x
wah Pian 4
2 100000 + Xx
2 * /

= 80000 rae 4

60000 Ratio
Compression

40000

20000

4 4 1 4 n —L a | ee a
ASB 1G PRE | 6 NO) NY 92° 13 4 AS: te 17%. 48. “19/20 2a ree A er BU er 2 SS er onwin dB sn chet ee eet Ba
Number of joints 2 8) 4 i 6S) °F 1B 8 48 th 612 13 AG 1G) 48 t7,, a8)) 1B) ae
Number of joints

DY metric
(b) ¢a
Figure 5: Computation time for ¢a for two metrics.
Figure 6: Compression ratios.

5.2.5 Some Compression Results

tional to the logarithm of |¢,,(w)|. Cells that are white
Shown in Figure 6 are the compression ratios for dy have |@,(w)| = 0. In the figure, the reach of the first
and ¢q that were achieved by differential encoding of link of the robot is readily apparent as a darker disk
the cells of the workspace and by using a more efficient inside a lighter disk. Some variation in darkness can
encoding of the labels used to represent the nodes and be seen in the lighter regions.
arcs in the graph (roughly a factor of two of the com-
pression ratio comes from this change in representa- As the number of joints in the robot increases, the
tion). The compression ratios are relatively flat for dn ring corresponding to the first link becomes smaller,
as the number of joints changes. and the links themselves have a greater tendency to
cluster near the origin due to the uniform random sam-
To gain some insight into the behavior of the com- pling. This clustering also means that clearly-visible
pression ratios achieved for ¢,, a set of images show- rings corresponding to links of the robot other than
ing the distribution of |¢,(w)| over the workspace were the first do not appear. The clustering also has an ef-
generated. Figure 7 shows an example image, showing fect on the compressibility of the ¢,. At two joints,
the two-link robot case with a graph of 2048 nodes. In the distribution of |¢,(w)| is relatively uniform, com-
the figure, the grid lines demark 11x11 collections of pared to robots with more joints. A greater uniformity
workspace cells, and the darkness of a cell is propor- of coverage of the workspace implies greater spatial co-
Toward Real-Time Path Planning in Changing Environments
Sa)

ao { 1 =

2048 node eraph

Figure 7: |¢n(w)| per cell for a two-link robot and a graph
with 2048 nodes.

herence between neighboring cells and that allows for

greater compression. At the 20 joints, the distribution
tends to be highly clustered near the origin, allowing
for higher compression as the cells outside the cluster
near the origin are grouped together in larger groups.
In between the two extremes, the compression is lower.
On the other hand, the compression ratios for ¢g
increase with the number of joints. This is expected
because the average length of the paths corresponding
to arcs in the graph tends to increase with the number “eR eee] |
ete eel
of joints, and that in turn leads to an increase in the size 16384 node graph
of the volume of the workspace swept by the robot as
it traverses the paths. As more cells are affected by the Figure 8: |¢.(w)| per cell for a two-link robot.
longer paths, the spatial coherence of the cells in the
workspace increases. Notice that the inflection point
5.2.6 Some Planning Examples
seen around eight joints in this graph is similar to that
seen in Figure 3. The same effect that minimizes the The current implementation of our path planner
size of @, also reduces spatial coherence, and, therefore, tests the connectivity of the graph between two selected
the achievable compression ratio. nodes in the modified graph; it does not connect arbi-
trary configurations to the graph. In all of the planning
Shown in Figure 8 is the distribution of |¢,(w)| per
runs tested using the uncompressed representations of
cell for the two link robot and the 2048-node and 16384-
gn and dg, it takes less than one second to update
node graphs. These graphs have the same resolution
the node and arc obstacle data structures and to find
as Figure 7. The sparseness of the distribution of the
a path in the modified graph. Extensive tests using
arcs in this case is the reason for the reduction in the
the compressed representations have not yet been per-
compression ratio available using the differential encod-
formed.
ing. Also shown in the figure is the effect of increasing
the size of the graph from 2048 nodes to 16384 nodes. Shown in Figure 9 is a 19-joint robot negotiating be-
As noted earlier, the average distance between nodes tween two obstacles (black objects in the figure). The
decreases, decreasing the average swept volume of the steps shown in the figure correspond to the nodes in the
paths corresponding to the arcs in the graph, and, in graph along the planned path. This plan was generated
this case, dramatically increasing the sparseness of ga. from the graph that used the D3” distance metric with
374 P. Leven and S. Hutchinson

we could attempt to maximize the minimum distance

between samples as in [4]. The quality of the samples
can be further improved by exploiting on the manip-
ulability measure associated with the manipulator Ja-
cobian matrix [27]. The basic idea is the following. In
regions of the C-space where manipulability is high,
the robot has great dexterity, and therefore relatively
fewer samples should be required in these areas. Re-
gions of the C-space where manipulability is low tend
to be near (or to include) singular configurations of the
arm. Near singularities, the range of possible motions
is reduced, and therefore such regions should be sam-
pled more densely.
These criteria can be posed as optimization problems
with deterministic solutions; however, in each case, the
complexity of the resulting problem would prohibit its
direct solution. Therefore, we are currently investigat-
ing important sampling approaches that incorporate
the criteria described above for sampling C-space.
Figure 9: A plan for a 19-joint robot passing through a
(relatively) narrow corridor. The dark blocks are the obsta- 6.2. Graph Enhancement
cles.
In previous probabilistic roadmap planners, if the ini-
tial roadmap was not singly connected, an enhance-
2048 nodes. The fan-like parts of the plan come from ment stage was used to connect its various components.
the set of nodes created to span the workspace of the In our work, since there will be no obstacles in the en-
robot, as described in Section 5.2.1. vironment, the roadmap will always have a single con-
nected component, and therefore the traditional idea
6 Future Directions of enhancement does not apply. However, unlike previ-
ous approaches, we are concerned with maintaining the
Although we have an implemented version of our plan-
connectivity even if arcs or nodes are deleted from the
ner, there remain a number of issues that we are now
roadmap. Therefore, we are investigating two methods
beginning to investigate.
to improve the robustness of roadmap connectivity to
6.1 Generating Sample Configuration
the addition of obstacles to the environment.
In the first method, the enhancement stage will at-
The uniform sampling scheme described in Section 3 tempt to increase the cardinality of the minimum cut
reflects complete ignorance about the environment. If set of §. In particular, after an initial S is computed,
prior knowledge, either about the environment or the minimal cut sets of both edges and vertices will be
set of tasks that the robot will perform, were avail- found, and then the C-space will be sampled in an at-
able, an appropriate importance sampling scheme, or tempt to add paths that would reconnect the graph if
even a deterministic scheme (if the existence of certain these cut edges or vertices were removed. This sam-
obstacles were known in advance) could be used. pling would be driven by constructing the workspace
Even without prior knowledge about the environ- volumes corresponding to the elements of the cut set
ment, there are ways to improve the quality of the set (these are given by ¢,1 and $71). In order to discon-
of sample configurations. We could, for example, at- nect the graph, an obstacle would need to touch each
tempt to maximize the portion of the workspace that is of these volumes. An algorithm such as the one pro-
covered by the sampled configurations (e.g., by gener- posed in [10] could be used to rapidly deduce a minimal
ating configurations for a uniform distribution of end- set of cells in the workspace that satisfy this condition,
effector positions in the workspace). In a similar spirit, and the mapping from workspace to C-space obstacles
Toward Real-Time Path Planning in Changing Environments
375
could then be used to seed an importance sampling are defined by recursive partitions of the image, and in
algorithm that would add nodes to S. run length coding neighborhoods are defined in terms
The second method relies on a quantitative evalua- of individual horizontal rows in the image.
tion of the quality of J. We will investigate a property To compress the representations for dp (w*), we will
that we call e-robustness (inspired by the notion of & make use of the observation that often ¢(w*) is of-
goodness described in [14]). We say that S is e-robust ten fairly symmetric with respect to its center of mass.
if no spherical obstacle in the workspace of radius € (or We plan to investigate using a prespecified graph tra-
less) can cause § to become disconnected. Using this versal algorithm, and recording the depths at which
concept, it will be possible to ensure after the enhance- the graph traversal algorithm exits or reenters p(w*).
ment stage that G can tolerate certain types of obsta- For most problems, breadth first traversal is likely to
cles. In particular, for a specified value of €, § can be be the most effective. For example, if ¢(w*) were a
tested for e-goodness. This test can be performed in hyper-sphere in an n-dimensional C-space, then this
such a way that the specific workspace spheres that vi- representation would require only the storage of a root
olate the e-goodness criterion can be enumerated, and node and a single integer to encode both ¢,(w*) and
these can then be used, as above, to seed an importance ga(w*). Breadth first traversal could then reconstruct
sampling algorithm that will add nodes to S. the exact set of nodes and arcs in S that correspond to
the cell w*.
6.3. Workspace to C-space Mapping This approach is analogous to run length coding. In
run length coding, the lengths of strings of ones are
There are a number of possible improvements that can
stored (for binary images). This approach essentially
be made to reduce the computation time for the map-
works by imposing an ordering on the pixels in the im-
ping and for more efficient representation of the map-
age (raster scan ordering), and then encoding when a
ping.
region of ones is exited or reentered. In our proposed
approach, we will use a breadth first tree traversal to
Improvement in computation time Using colli-
impose an ordering on nodes and arcs in G, and use
sion testing algorithms to compute ¢, and ¢,, while
the analogous idea of encoding tree depths at which
allowing for completely general geometric descriptions
@(w*) is exited or reentered. We note here that ap-
of the robot, is not the most efficient means available
proaches analogous to 2”—trees are not appropriate in
for computing ¢,'(q). An alternative is to consider
our case, because these methods are very sensitive to
3D voxel scan-conversion techniques|[11]. In our case,
small perturbations.
we do not need the full power of these algorithms as
used to generate realistic images of three-dimensional
scenes. We only need the components that compute the
7 Conclusions
occupancy bitmaps for geometric data. This operation We have presented what we believe to be the first plan-
could also take advantage of any hardware accelera- ner that is fully able to plan in real time in changing
tion available for this operation, as was done in [6] to environments. We have presented extensive prelimi-
accelerate the computation of Voronoi diagrams using nary analysis of the planner via planning simulations.
polygon rasterizing hardware. Although these preliminary results are quite promising,
there are a number of open issues that remain, and we
Efficient representations As described above, have outlined these along with our planned approaches.
#(w) is connected and compact. Exploiting the con-
nectedness of regions for the purposes of compression
References
has been a popular idea in the image processing and
computer vision communities for many years [23]. Well [1] J. M. Ahuactzin, K. Gupta, and E. Mazer. Manipu-
known examples of this include using quadtrees and lation planning for redundant robots: A practical ap-
run length coding to compress images. In each case, proach. International Journal of Robotics Research,
17(7):731-747, July 1998.
the compression exploits an efficient representation of
neighborhood structure, and then encodes homoge- [2] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and
neous neighborhoods. For quadtrees, neighborhoods D. Vallejo. Choosing good distance metrics and local
376 P. Leven and S. Hutchinson

planners for probabilistic roadmap methods. In Proc. [14] L. E. Kavraki, M. N. Kolountzakis, and J.-C.
IEEE Conf. Robotics and Automation, pages 630-637, Latombe. Analysis of probabilistic roadmaps for path
1998. planning. In Proc. IEEE Conf. Robotics and Automa-
tion, volume 4, pages 3020-3025, 1996.
[3] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones,
and D. Vallejo. OBPRM: An obstacle-based PRM for [15] J. J. Kuffner, Jr. and S. M. LaValle. RRT-connect:
3D workspaces. In Proceedings of the Workshop on An efficient approach to single-query path planning.
Algorithmic Foundations of Robotics, pages 155-168, In Proc. IEEE Conf. Robotics and Automation, 2000.
1998.
[16] J.-C. Latombe. Robot Motion Planning. Kluwer Aca-
demic Publishers, Boston, 1991.
[4] P. Bessire, J.-M. Ahuactzin, E.-G. Talbi, and
BE. Mazer. The “Ariadne’s Clew” algorithm: Global
planning with local methods. In Proceedings of the
[17] M. Lin and D. Manocha. Efficient contact deter-
mination in dynamic environments. International
Workshop on Algorithmic Foundations of Robotics, Journal of Computational Geometry and Applications,
pages 39-47, 1994. 7(1):123-151, 1997.
[5(aa D. Hankerson, G. A. Harris, and J. Peter D. Johnson. [18] E. Mazer, J. M. Ahuactzin, and P. Bessire. The ari-
Introduction to Information Theory and Data Com- adne’s clew algorithm. Journal of Artificial Intelli-
pression. Discrete Mathematics and its Applications. gence Research, 9:295-316, 1998.
CRC Press, New York, 1998.
[19] A. McLean and I. Mazon. Incremental roadmaps and
[6os K. E. Hoff, T. Culver, J. Keyser, M. Lin, and global path planning in evolving industrial environ-
D. Manocha. Fast computation of generalized voronoi ments. In Proc. IEEE Conf. Robotics and Automation,
diagrams using graphics hardware. In Proceedings of pages 101-107, 1996.
SIGGRAPH 1999 Conference on Computer Graphics,
pages 277-286, 1999. [20] B. Mirtich. V-Clip: Fast and robust polyhedral colli-
sion detection. Technical Report TR97-05, Mitsubishi
[7 =
C. Holleman, L. E. Kavraki, and J. Warren. Planning Electric Research Laboratory, 201 Broadway, Cam-
paths for a flexible surface patch. In Proc. IEEE Conf. bridge, MA 02139, June 1997.
Robotics and Automation, pages 21—26, 1998.
[21] J. L. Mitchell, W. B. Pennebaker, C. E. Fogg, and D. J.
T. Horsch, F. Schwarz, and H. Tolle. Motion planning LeGall, editors. MPEG Video Compression Standard.
with many degrees of freedom — random reflections at Chapman and Hall, New York, 1997.
c-space obstacles. In Proc. IEEE Conf. Robotics and
Automation, pages 3318-3323, 1994. [22] M. H. Overmars and P. Svestka. A probabilistic learn-
ing approach to motion planning. In Proceedings of
the Workshop on Algorithmic Foundations of Robot-
[9] D. Hsu, J.-C. Latombe, and R. Motwani. Path plan-
ics, pages 19-37, 1994.
ning in expansive configuration spaces. In Proc. IEEE
Conf. Robotics and Automation, pages 2719-2726,
1997.
[23] A. Rosenfeld and A. Kak. Digital Picture Processing.
Academic Press, New York, 1982.

[10] Y.-B. Jia and M. Erdmann. Geometric sensing of [24] D. Vallejo, C. Jones, and N. M. Amato. An adaptive
known planar shapes. Technical Report CMU-RI-94- framework for ‘single shot’ motion planning. Tech-
24, CMU, July 1994. nical Report 99-024, Department of Computer Sci-
ence, Texas A&M University, College Station, TX,
[11] A. Kaufman and E. Shimony. 3D scan-conversion al- Oct. 1999.
gorithms for voxel-based graphics. In Proceedings of
the 1986 Workshop on Interactive 3D Graphics, pages [25] S. A. Wilmarth, N. M. Amato, and P. F. Stiller. Mo-
45-75, New York, 1986. ACM. tion planning for a rigid body using random networks
on the medial axis of the free space. In Proceedings of
[12] L. Kavraki and J.-C. Latombe. Randomized pre- ACM Symposium on Computational Geometry, pages
processing of configuration space for fast path plan- 173-180, 1999.
ning. In Proc. IEEE Conf. Robotics and Automation,
volume 3, pages 2138-2145, 1994. [26] P. G. Xavier. Fast swept-volume distance for robust
collision detection. In Proc. IEEE Conf. Robotics and
[13] L. Kavraki, P. Svestka, J.-C. Latombe, and M. Over- Automation, pages 1162-1169, 1997.
mars. Probabilistic roadmaps for path planning in
high-dimensional configuration spaces. IEEE Trans- [27] T. Yoshikawa. Manipulability of robotic mechanisms.
International Journal of Robotics Research, 4(2):3-9,
actions on Robotics and Automation, 12(4):566-580, 1985.
Aug. 1996.
Geometric Construction of Time Optimal
Trajectories for Differential Drive Robots
Devin J. Balkcom, Carnegie Mellon University, Pittsburgh, PA
Matthew T. Mason, Carnegie Mellon University, Pittsburgh, PA

We consider a differential drive mobile robot: Two

unsteered coazial wheels are independently actuated.
Each wheel has bounded velocity, but no bound on
torque or acceleration. Pontryagin’s Maximum Prin-
ciple gives an elegant description of the extremal tra-
jectories, which are a superset of the time optimal tra-
jectories. Further analysis gives an enumeration of the
time optimal trajectories, and methods for identifying
the time optimal trajectories between any two configu-
rations. This paper recapitulates and refines the results
of [1] and [2] and presents a simple graphical technique
for constructing time optimal trajectories.

1 Introduction

This paper addresses the time optimal paths for dif-

ferential drive mobile robots with bounded veloc-
ity. Differential drive (or diff drive) means there are
two unsteered independently actuated coaxial wheels.
Bounded velocity means that each wheel is indepen- Figure 1: The seven simplest optimal trajectory classes.
dently bounded in velocity, but acceleration is not
bounded. Even discontinuities in wheel velocity are
To derive the optimal paths we will use Pontryagin’s
permitted. The environment is planar and free of ob-
maximum principle to obtain a geometric program for
stacles. the extremal trajectories, which are a superset of the
Under these assumptions, we will see that the time optimal trajectories. We then derive some additional
optimal paths are composed of straight lines alternat- necessary conditions, leading to a complete enumera-
ing with turns “in place”, i.e., turns about the center tion of optimal trajectories and a planning algorithm.
of the robot. Optimal paths contain at most three Finally we reformulate the analysis to give a more in-
straights and two turns. There are a number of other tuitive geometric procedure for constructing optimal
restrictions, leading to a set of 40 different combina- trajectories.
tions arranged in 9 different symmetry classes. The
simplest nontrivial motions are turn-straight-turn mo- Previous Work
tions: turn to face the goal (or away from the goal);
roll straight forward (or backward) to the goal; turn This paper expands on the results presented in [1]
to the goal orientation. In some instances the optimal and [2]. Other work on diff drive robots has assumed
path passes through an intermediate “via” point. See bounds on acceleration rather than on velocity; for ex-
Figure 1 for example motions from seven of the nine ample see papers by Reister and Pin [6] and Renaud
classes. and Fourquet [7]. For the bounded acceleration model,
378 D. J. Balkcom and M. T. Mason

gee eg
a |
xa bw VU

NG Pa Ww)
Wy

Figure 2: Notation
|
the time optimal trajectories have been found numer- | ICI
ically, and there is current work to find a closed form
solution. The bounded velocity model is simpler, and
hes}
the structure and cost of the fastest trajectories can be Figure 3: Bounds on (wi, wr)
determined analytically.
Most of the work on time optimal control with
It follows immediately that v(t) and w(t) are mea-
bounded velocity models has focused on steered ve-
surable functions defined on the same interval. Given
hicles rather than diff drives, originating with papers
initial conditions gs = (Zs, Ys,9;) the path of the robot
by Dubins [3] and by Reeds and Shepp [5]. Many of
is given by:
the techniques employed here are an extension of op-
timal control techniques developed for steered vehicles
in [8, 9, 10]. x(t) 24+ f veos() (5)
0

2 Assumptions, Definitions, Notation uit) = wet f vsin(®), (6)

The state of the robot is g = (x,y, 0), where the robot
reference point (x,y) is centered between the wheels,
eit) S10)4|e (7)
0
and the robot direction @ is 0 when the robot is facing
along the z-axis (Figure 2). The robot’s velocity in the We also define rectified path length in E?, the plane of
forward direction is v and its angular velocity is w. The robot positions:

s(t)= fl (8)
t
robot’s width is 2b. The wheel angular velocities are
w, and w,. With suitable choices of units we obtain: 0
i! and rectified arc length in $1, the circle of robot orien-
We 3 wi + wr) (1) tations:

w= Su-w)
il
(2) a(t)= fl (9)
and It follows that 6, z, y, s, and o are continuous,
that their time derivatives exist almost everywhere,
wy = v—bw (3) and that:
Wr = vt bw. (4)
= WwW ae. (10)
The robot is a system with control input w(t) = = vcos(0)i tae: (11)
(wi(t), w,(t)) and output q(t). Admissible controls are = vsin(™) ae. (12)
bounded Lebesgue measurable functions from time in-
terval [0,7] to the closed box W = [-1, 1] x [—1, 1] (see The admissible control region W also provides a con-
Figure 3). venient comparison with previously studied bounded
Geometric Construction of Time Optimal Tra jectories for Differential
Drive Robots 379

Ww Ww where our input is

U v

Equation 13 can be rewritten:

Dubins Reeds and Shepp Convexified
Reeds and Shepp
g = Wifi + wr fy (14)
as
b where f; and f, are the vector fields corresponding to
the left and right wheels:

fl = 3 sin (15)

Steered car Diff drive

ie he ssind |. (16)
Figure 4: Bounded velocity models of mobile robots

velocity models. If we plot W in v-w space, we obtain Vector field f; corresponds to turning about a center
a diamond shape. Steered vehicles are typically mod- located under the right wheel, and f, corresponds to
eled as having a bound on the steering ratio w : v, and turning about a center located under the left wheel.
on the velocity v (Figure 4). Define \ to be an R®-valued function of time called
the adjoint vector:
We also need a notation for trajectories. Later sec-
tions show that extremal trajectories are composed of
x(t)
straight lines and turns about the robot’s center. We A(t)= A2(t)
will represent forward by +}, backward by J), left turn by
A3(t)
4), and right turn by ~~. Thus the trajectory a}
can be read “left forward left”. When necessary, a sub-
script will indicate the distance or angle traveled. We Let H: R®? x SE? x W > R be the Hamiltonian:
will use t and s to represent turns and straights of
A(A,q,wv) —< A, wif + Wy f, >
indeterminate direction.

The maximum principle states that for a control w(t)

3 Pontryagin’s Maximum Principle. to be optimal, it is necessary that there exist a nontriv-
Extremal Controls. ial (not identically zero) adjoint vector A(t) satisfying
the adjoint equation:
The existence of a time optimal trajectory for every
)
pair of start and goal configurations is proven in [1]. A. 93 ed,
dq (17)
This section uses Pontryagin’s Maximum Principle [4]
to derive necessary conditions for time optimal trajec-
while the control w(t) minimizes the Hamiltonian at
tories of the bounded velocity diff drive robot. every t:
The robot system is described by:
H(A,gq,w) = min H(A, q, 2) = do. (18)
(uw; + w,) cos(@)
defoaie Wr) sin(@) (13)
with Ay > 0. Equation 17 is called the adjoint equation
0 55 (Wr aa wr) and Equation 18 is called the minimization equation.
380 D. J. Balkcom and M. T. Mason

For the bounded velocity diff drive, the adjoint equa- We also define a direction for the 7-line consistent with
tion gives: the choice of “left” and “right” for the half planes.
The minimization equation 18 can be rewritten:
r - Aah tae Maes (19)
Og wt a Wr Pr = min al 49 ZrPrs (30)
Z1,2r
0
2 a 0 (20) where ¢, and ¢, are defined to be the two switching
di sin 8 — Ag cos 8 functions:

Fortunately these equations can be integrated to obtain jd = <A, fi> (31)

an expression for the adjoint vector. First we observe Cy g00s 8
that A; and A» are constant and define cy and c2 ac- = C2 -| 5sind (32)
cordingly:
n(x, Y; 0) ~3

A(t) = 4 (21) a — spite + bsin 8, y — bcos é) (33)

AQ (t) = (ON (22)
Gy, tira Tae, fe > (34)
For A3 we have the equation: C1 3cos
= C2 5 sin (35)
tga :“(1 sin @— Az cos 8). (23) n(x, y; 9) Ds
But we can substitute from Equations 1, 11, and 12 to ae sgn — bsin0, y + bcos 8). (36)
obtain:
A3 = C1y — C22, (24) Note that the wheels’ coordinates can be written:

which is integrated to obtain the solution for 3: LI _ x — bsin#@

(YI ) * (y + bcosé ) (37)
A3 = C1y — Cok + ¢3, (25) Le z+ bsin@
where c3 is our third and final integration constant. It (Ur ) begin: ‘k (38)
will be convenient in the rest of the paper to define a
so the switching functions can be written:
function 7 of x and y:

n(x, Y) = Cry — Coe + C3. (26) a = = SpIltes We) (39)

So then the adjoint equation is satisfied by: br ae apn, w) (40)
C1
Now the minimization equation says that if the con-
N= C2 (27)
trols w;,w, are optimal then they minimize the Hamil-
mx, ¥) tonian H = w,¢; + w,¢,. This implies the optimal
for any C1, C2, C3 not all equal to zero. controls can be expressed:
Let the 7-line be the line of points (z,y) satisfying =a if right wheel € right half plane
n(x, y) = 0, and note that (a, y) gives a scaled directed w,4 € [-1,1] if right wheel € 7-line (41)
distance of a point (x,y) from the 7-line. Let the right =-1 if right wheel € left half plane
half plane be the points satisfying:
a | if left wheel € left half plane
n(x, y) > 0, (28) wr 4 €[-1,1] if left wheel € 7-line (42)
if left wheel € right half plane
and let the left half plane be the points satisfying
If c; = co = 0, then the the entire plane is the left half
m(x,y) <0. (29) plane or the right half plane, depending on the sign of
Geometric Construction of Time Optimal Tra. jectories for Differential Drive
Robots 381

—_

Figure 5: Two extremals: zigzag right and tangent CW.

hb? 4
Figure 6: Turn-straight-turn is not always optimal.
Other extremal types are zigzag left, tangent CCW, and
turning in place: CW and CCW. Straight lines are special
cases of zigzags or tangents. straight in the 7-line’s direction until one wheel
crosses. It then turns until the other wheel crosses,
and then goes straight again. There are two non-
cz. (Recall that all three integration constants cannot degenerate patterns: ... Or ... called zigzag
be simultaneously zero.) right (ZR), and ... {tra ... called zigzag left
The location of the 7-line depends on the apparently (ZL). (Recall that forward and backwards actions
arbitrary integration constants. The maximum princi- are denoted by ‘} and J}, and spins in place are
ple does not give the location of the line; it merely says denoted by and 7.)
that if we have an optimal control then the line exists
and the optimal control must conform to the equations Figure 5 shows examples of ZR and TCW.
above. The question that naturally arises is how to
Examining these classes, we see that
locate the line properly, given the start and goal con-
figurations of the robot. There seems to be no direct Theorem 1 For an optimal trajectory,
way of doing so. Rather, we must use other means to
identify the extremal trajectory. t = s(t) + ba(t). (43)
The robot switches only when a wheel touches the
n-line, so the possibilities can be enumerated by con- Proof: Extremal trajectories are composed only of
structing a circle whose diameter is the robot wheel- turns and straight lines. |
base, and considering the different possible relations of
this circle to the 7-line: Note that in [2], we demonstrate that equa-
tion 43 actually holds for any trajectory such that
e CCW and CW: If the robot is in the left half plane and max(|w|, |w,|) = 1 for almost all t; i.e., for trajectories
out of reach of the 7-line, it turns in the counter- in which one control is always saturated. This may
clockwise direction (CCW). CW is similar. provide some intuition for why turns and straights are
faster than curves.
e TCCW and TCW (Tangent CCW and Tangent CW):
The fastest trajectories for a Reeds and Shepp car
If the robot is in the left half plane, but close
enough that a circumscribed circle is tangent to
with zero turning radius are of the form tst. It might
the 7-line, then the robot may either roll straight at first seem that Equation 43 implies the same is true
for the diff drive. Trajectories of type tst certainly
along the line, or it may turn through any positive
minimize translation time; however, they do not not
multiple of 7. TCW is similar.
necessarily minimize rotation time. Figure 3 shows an
e ZRand ZL: If the circumscribed circle crosses the 7- example: The robot is at the origin facing north, and
line, then a zigzag behavior occurs. The robot rolls the goal is some distance to the east, facing north. The
382 D. J. Balkcom and M. T. Mason

Start Goal

Figure 8: Zigzags of three turns are not optimal

Figure 7: There is always a roundtrip of length 27 visiting
0;, 8g, 0, and mr.
Theorem 3 Tangent trajectories containing more
than three actions are not optimal.
robot could follow a ~4) trajectory, with a total turn-
ing angle of 7. However, there is a {}-\) trajectory Proof: An extremal of type TCW or TCCW alternates
that turns through a total angle of less than 7. For a turns and straights. Any full untruncated turn must
wide enough robot (b large enough), the second trajec- be a multiple of 7. If there are four actions, there is at
tory will be optimal. least one untruncated turn of length at least 7, and a
second turn of nonzero length. The rectified arc length
4 Further Conditions for Optimality o would be more than 7, contradicting Theorem 2.

The previous section showed that every time optimal

path must be of type CCW, CW, TCCW, TCW, ZR, or ZL. Necessary conditions on ZR and ZL trajectories
However, the converse is definitely not true—not every
such trajectory is optimal. For example, a robot turn-
Zigzag trajectories are composed of alternating turn
ing in place for several revolutions is not time opti- or straight line actions. Successive turns or straights
mal. To keep the distinction clear, we refer to trajecto- must be in opposite directions, but have the same mag-
ries satisfying Pontryagin’s Maximum Principle as ez- nitude if untruncated. Simple geometry also gives a
tremal, and we note that the time-optimal trajectories relationship between ¢, the angle of each turn, and d,
are a subset of the extremal trajectories. In this sec- the length of each straight. We have:
tion we find additional necessary conditions, ultimately
finding that no time optimal path can have more than i 2btan(S). (44)
three straights and two turns.

Necessary conditions on TCCW and TCW trajecto- Theorem 4 Zigzag subsections containing three turns
ries are not optimal.

Theorem 2 For every time-optimal trajectory o0(T) < Proof: Consider a zigzag subsection with three turns,
TT. and two straights. The straights are the same length,
so the path comprises two legs of an isoceles triangle.
Proof: Consider the fastest tst trajectory between
Construct the circle containing the start, the goal, and
a given start and goal. Obviously the straight action
the via point as in Figure 8. If we perturb the via point
connects the start to the goal. Suppose we orient our
coordinates so the angle from start to goal is 0. The to a nearby point on the same circle the turning time is
robot’s heading during the straight is either 0 or 7. To unchanged, and the translation is decreased. a
plan the turns, we must consider different paths on the
circle from 9; to 0,, passing through either 0 or 7 along Zigzags can also be said to be periodic. Let 7 be the
the way. Note that in every instance there is a round smallest positive time such that:
trip from 8, to 0, 7, 0,, and back to 0s, of length 27
(Figure 7). Hence there is a one-way trip of length 7 O(t + 7)
or less. 5 n(z(t),y(t)) = n(a(t+r7),y(t+7)).
Geometric Construction of Time Optimal Trajectories for Differential Drive
Robots 383

d n-line

6,/2
[\¢
T

Figure 9: Periodicity of a zigzag

Theorem 5 A zigzag trajectory of more than one pe-

riod is not optimal.

Proof: Consider a zigzag of more than one period,

beginning at time 0 and ending at time T > rT. By
theorem 4, the zigzag is not optimal if o(T) > 2¢.
If s(Z’) > 2d, then there are three straights. The first
and last straights are parallel. If we reorder the actions
to perform these consecutively, we get a straight with
length greater than d. Thus we have a path which costs
no more than the original but which is no longer a le-
gitimate zigzag. Since it is not extremal, neither it nor
the original path can be optimal. &

Enumeration

Theorems 3, 4, and 5 allow a finite enumeration of the

T30T2,0T,
structure of optimal trajectories. The structure must
be one of the following, or a subsection of one of the
following:

[Tagent | Ate | AA) am AM

[Tangents | fond | dad | tod | dont | Figure 10: Given an optimal trajectory from “base” with

[ Zieag | PAA [Ponad |tadat |aatat | heading 0; to the origin with heading 0, = 0, transforma-
tions T,,T2, and T3 yield up to seven other optimal trajec-
tories symmetric to the original.
5 Symmetries
tained by applying one or more of three transforma-
Further analysis of the time optimal trajectories is tions defined below. These transforms are isometries;
difficult because of the large number of cases. This if the base trajectory is optimal from the base config-
complexity is reduced using symmetries developed by uration, the transformed trajectories are optimal from
Souéres and Boissonnat [8] and Souéres and Lau- the transformed configurations.
mond [9] for steered cars. Geometrically, the transformations reflect the plane
The symmetries are summarized in Figure 10. Let across the origin or across one of three other lines: the
“base” be any trajectory from g = (x,y, @) to the ori- a-axis, a line Ag at angle (1 +6;)/2, or the line Aj at
gin. Then there are seven symmetric trajectories ob- angle 6, /2.
384 D. J. Balkcom and M. T. Mason

The three transformations are: region; the results then apply to symmetric regions.
In principle, the analysis is completed by the following
| 71: Swap ft and |) Ti: g=(-2,—y,9)
lee
aneversorder’
|Thsinfary) =Rove a)|
steps:

m3: Swap rand o[g=(@,-y,-9)

Ts | 1. For each trajectory type, we identify every feasible
choice of start configuration (x, y,@). This defines
Each transformation is its own inverse, and the three a map from trajectory type to a region of config-
transformations commute. For any given base trajec- uration space.
tory, the transformations yield up to seven different
symmetric trajectories. The result is that all optimal 2. Now we consider a point in configuration space
trajectories fall in one of nine symmetry classes. (x,y,@). If it is in only one region, then the cor-
responding trajectory type is optimal from that
point.

3. When regions overlap, we derive additional nec-

essary conditions for optimality or calculate the
JD. | Ala _| ee actual times for each trajectory type to disam-
[Etat [dat [dat |tad] biguate.
Piel oto jyoto | oto. |
Poa ot ete | tol! tot | | To illustrate this procedure, we present the following
| H.| Aled | afield | tede | data | example (Figure 11). The feasible regions for {/7}>
[2 |tated |betied | tadet [tottat | and «\}-7} overlap. For almost all g, in the overlap,
there are two possible extremals but only one true op-
PB
het ot aehen | timal path. The Ag line is a decision boundary: For q,
to the right of Ag the optimum is 7.1}, and to the
left of Ag the optimum is {}/~4}>. Figure 11 illustra-
tres the proof. First we observe that the alternatives
give equal time on the Ag line, because that line is the
[Baad |tat | dant | teed] axis of reflection for the T, 07> isometry. So both paths
eo) Veta) ees | neies al are optimal on Ag.
[G.{ det | tet | teat | der | Consider a {}4}> path from the start pose shown.
| H. | alent | otal | tole | data | When the path crosses Ag during the 1) action, the re-
[2 Ltadet [otal |tate | betict | maining cost is unchanged if it switches to}. But
We can analyze all types of trajectories by analyzing then the total path would have a structure of {)\\/.4,
just one type from each of the nine classes, and then
and would not be a legitimate extremal. B
applying the transformations 7), 7>,73 to obtain the Similar techniques can be applied to the other re-
other members of the class. gions. The end result is a mapping that defines for each
point in configuration space the set of optimal trajec-
6 Time Optimal Trajectories. tories from that point to the origin. This mapping is
illustrated by showing a slice at 9 = 7/4 (Figure 12).
In this section we identify the time optimal trajectories The mapping from start configuration to optimal tra-
between any given start and goal configuration. We jectory is usually, but not always, unique. At some
introduce a “goal-centric” coordinate system, with the boundaries in the figures there are two distinct. trajec-
origin coincident with the goal position, and the z axis tories that give the same time cost. More interesting
aligned with the goal heading. is the case at 6 = 0 where a continuum of different tra-
The symmetries of the previous section greatly sim- jectories of type A are all optimal, bounded by optimal
plify our analysis. We only need to consider a “base” trajectories of type B.
Geometric Construction of Time Optimal Trajectories for Differential
Drive Robots 385

i,
-1.5

peloton
4 ahr

-1.5 -1 -0.5 0 Oma)

5h il eS
H

ie?

Figure 12: Optimal control for start configuration qs =

(x,y, 4) and goal configuration qg = (0,0,0), with isocost
lines. Coordinates are measured in units of b.

ValueBaseTSTS below calculates the cost of the fastest

trajectory with a structure of m7}.

Procedure ValueBaseTSTS(q = (2, y,9))

arccos(1 — y) — 6/2 —x + v/y(2 — y)
End ValueBaseTSTS

Procedure ValueBaseSTS(q = (2, y,0))

Figure 11: An example of overlapping regions. The path
If y=O then |z|+ 0/2
shown in b) is extremal, but not optimal.
else y(1+cos(6))/sin(@) —2+6/2
End ValueBaseSTS

7 Algorithm for Optimal Control and Procedure ValueBaseTST(q = (z,y,9))

Value Function. r= |\(z,9)lI
¢ = arctan(y,x)
We now present an algorithm to determine the optimal
paths between a given start and goal position, and the
r+ min (|¢| + |¢ — 4],
2 + |¢| — |¢ + 41)
End ValueBaseTST
time cost of those paths. For each optimal path struc-
ture, the necessary conditions yield a region. (Twelve
such regions are shown in Figure 12.) The determines We now can define OptBVDD (optimal bounded ve-
which region(s) the start configuration (2, y, @) falls in, locity diff drive). The function recursively applies sym-
and then calculates the value function for one of the metry transforms until the configuration is in the base
optimal path structures. For example, the function region.
386 D. J. Balkcom and M. T. Mason

The optimal path structure can then be determined

based on the necessary conditions for extremal paths
to be optimal. The value for that path structure is
calculated. The recursion applies the appropriate com-
bination of 7,, 7, and 73 transforms to the base path
structure to determine the actual optimal path struc-
ture.

Procedure OptBVDD(q = (z,y,9))

if 6 € (7,27) then 73 (OptBVDD(T3(q)))

r= ||(z,y)||
¢ = arctan(y, 2)
if ¢ € ((0+7)/2,m)
U((0 — m)/2,0)
then 72 (OptBVDD(T>(q)))
if y <0 then 7; (OptBVDD(T} (q)))

if (<0
return(-)74, ValueBaseTST(q) )
else if y < 1—cos(O) Figure 13: Reachable configurations in normalized time 2.
return (\}~7}, ValueBaseSTS(q) )
else if r>tan(¢/2)
time 2. (x, y, and time are normalized by 6, the width
return(s\)., ValueBaseTST(q) )
of the robot.) Slices of this value function allow the
else
regions in which various extremal paths are optimal to
return(s\)7}, ValueBaseTSTS (q) )
be seen more clearly. For example, Figure 12 shows a
End OptBVDD slice where the angle between the start and goal robot
is fixed at 7.
For the sake of brevity, certain special cases have
been omitted from the pseudocode presented. When- 8 Graphical Method
ever two symmetric regions are adjacent, the fastest
paths for both regions are optimal. For example, if the Graphical construction of the time optimal trajectories
robot starts at (0,1, 7), then both the paths ~1)7~ and is usually straightforward. Figure 14 shows examples
4} are optimal. for each of the nine symmetry classes. The first five
classes are really quite obvious. The last four classes
There are two other cases where multiple paths will are more challenging. In this section we will see a
be optimal. When 6, = 0, there may be a continuum graphical way to identify which class gives the time-
of optimal five action paths, bounded by two different optimal trajectory, and to find the location of a via
four-action paths. When 9, = 7, there will be a con- point if one exists.
tinuum of optimal t,s paths (Class E), bounded by
The main idea is that the primary decision bound-
two-action paths of class C.
aries in the algorithm OptBVDD can be stated as condi-
In all cases, the above algorithm will return a sin- tions comparing the start heading 6,, the goal heading
gle optimal trajectory. Some additional bookkeeping §,, and the direction from start to goal. Based on this
would allow all of the optimal trajectories to be re- observation we can translate OptBVDD to a graphical
turned. procedure.
The level sets of the value function show the reach- It is convenient to choose a different coordinate sys-
able configurations of the robot for some given amount tem, which we will call bisector-centric coordinates. We
of time. Figure 13 shows the shape of this region for place the origin at the midpoint between the start and
Geometric Construction of Time Optimal Tra jectories for Differential Drive
Robots 387

goal, with the positive x axis directed toward the goal.

A) The y axis is then the perpendicular bisector of the
segment between x, and 2g, oriented in the usual way.
For convenience, we define | to be the distance between
the start and goal; i.e, | = 2xz,. We define the range of
B) #, and 6, to be (—z,
7].
trivial
It is also convenient to define the startline and
goalline to be the lines aligned with the robot head-
ing at the start and goal, respectively. We define n to
be the intersection of the startline and goalline, if they
C) intersect.

For the rest of this section, we first walk through the

cases from simplest to most complex. For the zigzags
the most interesting part is finding the location of a via
point, if any, and addressing some of the special cases.
D)
Tangent trajectories. Although the time optimal
trajectories for classes A, B, and C are obvious, we list
tangent them here for completeness:

A If s coincides with g, then turn in place.

B if@;=0, = 0.or 0, = 0, = 7, then roll straight to

the goal.
ote Y
C If @, is 0 or 7, then roll to the goal and turn to the
goal heading. If @, is 0 or 7 then turn to the goal
By 4 SONS
heading and then roll straight to the goal.

Next are the tst tangent and sts tangent, trajecto-

ries.

G) es - \9 D If 6, and 6, are neither 0 nor 7, and have different

signs, a turn from 0, to 0, by the shortest arc will
either pass through 0 or 7. So turn to 0 (7), roll
Ooty y zigzag forward (backward) to the goal, then turn to 64.
The cost of this trajectory is:
H) Ss g
L +2. (45)
ttangent = Omin(|s|+|0|,27—|As|—|Ag|)

E If (@s,0,) = (0,7) or (7,0), then roll partway to-

ward the goal, turn through 7 or —7, then roll the
rest of the way to the goal. This yields a contin-
uum of optimal trajectories of class E, and four
optimal trajectories of class C. The cost of trajec-
tories in this region is also given by Equation 45.
Figure 14: Examples for each of the nine classes.
388 D. J. Balkcom and M. T. Mason

Zigzag trajectories. If the signs of #, and 6, are the

same, but neither is equal to 0 or 7, then the optimal
eoalline startline
trajectory will be a zigzag. The easiest way to approach
zigzags is in terms of their via points. If there is no via,
then we have a simple tst trajectory. If there are one
or two via points, then we have to look at special cases
to determine the number of vias and where they can
occur.

e Every via point is on the startline or on the

Goal goalline;

e If the startline and the goalline intersect to the

right of the y axis, there is at most one via, and it
is on the goalline between g and n, or at n.

e If the startline and the goalline intersect to the left

of the y axis, there is at most one via, and it is on
the startline between s and n, or at n.

e If the startline and the goalline intersect on the y

axis, then there may be two optimal trajectories
Goal
of class H. For one, the via point is on the startline
between s and n. For the other, the via point is
on the goalline between g and n.

e If the startline and the goalline are parallel to the

y axis, there may be up to four optimal trajecto-
ries of class H, and there may be a continuum of
optimal trajectories of class I.

e If the startline and the goalline are parallel to each

other but not the y axis, there may be up to two
viapoints. One is on the goalline, above the x
axis iff the goalline does not intersect y above the
x axis. The other is on the startline, with the
same constraint. This gives up to two optimal
trajectories of class H and a continuum of optimal
Goal
trajectories of class I.

The above case analysis tells where to look for via

points. Next we determine how to find the via points.

8.1 Graphical Construction of Class H Trajec-

tories
Figure 15: Examples of optimal zigzags. If startline and
goalline are symmetric across the y axis, there are at least For any trajectory with a via point we have a simple
two zigzags of equal cost. If startline and goalline are paral- equation for the cost of the straight line actions. The
lel, there are two zigzags of equal cost, and zigzags of class above enumeration shows that there are four line seg-
I may also be optimal. ments where the via point may fall. Let 7 be the mag-
nitude of the internal angle between any line segment
Geometric Construction of Time Optimal Trajectories for Differential Drive
Robots 389
possibly containing a via point and the x axis, and let
0 be the internal angle of the other line segment on
the same side of the x axis. startline
(See figure 15.) Recall
that ¢ is the angle of the turns of the zigzag extremal;
we wish to determine the optimal value for ¢. Some
simple geometry gives us:

ter= 26-547 —m) - ROTA Sd) (46)

Figure 16: Graphical construction of optimal path.
We take the derivative with respect to ¢ and set to
zero. After some simplification, we get the following
to imply that the via point is not between s and n.
condition on ¢:
This occurs when Equation 47 has no solution satisfy-
ing Equations 48 and 49, implying that either a tra-
cos (¢) =1— aenLy (47) jectory of less than four actions is optimal, or the via
2b
point of the four action trajectory will be at n.
@ must be no greater than the turning angle of the
fastest tst trajectory, and cannot be less than the an- 8.2 Construction of Class I Trajectories
gle of the turn if the via point were at n. (Recall that
Equation 47 allows the optimal value of ¢ to be calcu-
the via does not fall past this intersection.)
lated, and this determines the structure of the optimal
@ < min(|6;|
+|0,|,27 —|@s|—|g|) (48) zigzag extremal. The above graphical algorithm allows
this extremal to be constructed. However, if the star-
¢ = x-7-6 (49) tline and the goalline are parallel, there is more than
one subsection of this extremal that connects the start
If Equation 47 has no solution, or requires that @
and goal. In this case, if trajectories of class H are
be larger than the turning angle of the fastest tst tra-
optimal, there will therefore also be a continuum of
jectory, the cost function is monotonically increasing
optimal class I trajectories.
with ¢. In this case it is impossible to save turning
time by using a four action trajectory; a trajectory of
no more than three actions will be optimal. If Equa- 9 Summary and Conclusion.
tion 47 requires that @ < (t—y—6), the cost function is
The bounded velocity model of diff drive robots is
monotonically decreasing with ¢ and the optimal tra-
simple enough that the set of time optimal trajec-
jectory will have a via at n.
tories between any two robot configurations may be
There is a graphical interpretation of Equation 47. found. Pontryagin’s Maximum Principle provides an
For simplicity, we consider the case where n is above elegant geometric program describing extremal trajec-
the z axis and to the left of the y axis, which means we tories. These extremal trajectories are a superset of
should look for a via on the startline between s and g. the optimal trajectories.
Put the robot at the origin, with angle 6,. Roll it We derived conditions necessary for extremal trajec-
forward in a straight line. The right wheel rolls on tories to be optimal. These conditions require that
a straight line, call it lz. Now put the robot at the optimal trajectories fall in one of 40 extremal trajec-
goal, with the right wheel on Jz. That is the config- tory classes. We then applied symmetries developed
uration at which the robot should arrive at the goal. by Souéres and Boissonnat, reducing the set of trajec-
(See Figure 16.) We can determine the location of the tory classes to be analysed to 9. We analysed each of
via point by rolling the robot from this configuration the 9 classes to determine the start and goal configura-
in a straight line until it intersects the startline. tions for which it was optimal. This yields a simple al-
It may be impossible to place the right wheel on gorithm to determine the optimal trajectory structure
lp, or the goal configuration of the robot may seem and cost between any two configurations. The analysis
390 D. J. Balkcom and M. T. Mason

also yields a simple geometric method of determining [5] J. A. Reeds and L. A. Shepp. Optimal paths for a
the optimal path structure. car that goes both forwards and backwards. Pacific
Journal of Mathematics, 145(2):367-393, 1990.

Acknowledgments
[6] D. B. Reister and F. G. Pin. Time-optimal trajecto-
ries for mobile robots with two independently driven
We would like to thank Jean Paul Laumond for guid- wheels. International Journal of Robotics Research,
ance. We would also like to thank Al Rizzi, Howie 13(1):38-54, February 1994.
Choset, Ercan Acar, and the members of the Manipu-
lation Lab for helpful comments. M. Renaud and J.-Y. Fourquet. Minimum time motion
of a mobile robot with two independent acceleration-
driven wheels. In Proceedings of the 1997 IEEE In-
References ternational Conference on Robotics and Automation,
pages 2608-2613, 1997.
[1] D. J. Balkcom and M. T. Mason. Extremal trajecto-
ries for bounded velocity differential drive robots. In
IEEE International Conference on Robotics and Au- [8] P. Souéres and J.-D. Boissonnat. Optimal trajectories
tomation, 2000. for nonholonomic mobile robots. In J.-P. Laumond,
editor, Robot Motion Planning and Control, pages 93—
[2] D. J. Balkcom and M. T. Mason. Time optimal trajec- 170. Springer, 1998.
tories for bounded velocity differential drive robots. In
IEEE International Conference on Robotics and Au-
tomation, 2000. [9] P. Souéres and J.-P. Laumond. Shortest paths synthe-
sis for a car-like robot. [IEEE Transactions on Auto-
matic Control, 41(5):672-688, May 1996.
[3] L. E. Dubins. On curves of minimal length with a
constraint on average curvature and with prescribed
initial and terminal positions and tangents. American [10] H. Sussmann and G. Tang. Shortest paths for the
Journal of Mathematics, 79:497-516, 1957. reeds-shepp car:. a worked out example of the use
of geometric techniques in nonlinear optimal control.
L. S. Pontryagin, V. G. Boltyanskii, R. V. Gamkre- SYCON 91-10, Department of Mathematics, Rutgers
lidze, and E. F. Mishchenko. The Mathematical The- University, New Brunswick, NJ 08903, 1991.
ory of Optimal Processes. John Wiley, 1962.
 GHD
mynke
 na
hal if
 fc
Sie

NEW. DIRECTIONS

oe. 2000 WAFER oo

sel iBRUCE RANDALL DONALD, KEVIN M.-LYNCH;
| and DANIEL A RUS

oe a ee - ee processes that hie :

sensorssand actuators are indispensable for robot navigation and the perception
of the world in
i which they move. Therefore, a deep understanding of how
algorithms work to achieve this control is essential for the development of
ns efficient and yee robots in ina broad fieldofaa

An ile group . oo ecieatiote. fils everytwo years ~

= to document the |progress in.algorithmic foundations of robotics. This
~ volume addresses iin particular the areas of control theory, Sompittioas
and differential geometry in
i rok otics,and applications to core problems 4
; such as
« motion -navigation, sensor-based |

?j

ge -

Cover illustration: —
Leonardo da Vinci mechanical drawing. ANTAL
DONA
QUA

AK Peters, Ltd.