Robotics Research
Ibchnical Report






"^teb,



'g^jA



'orise













An Efficient Motion Planning Algorithm for a Convex
Polygonal Object in 2-Dimensional Polygonal Space.

by

K. Kedem
M. Sharir



Technical Report No. 253

Robotics Report No. 90

October, 1986










5.i
»



r



\



in

I

Pi



ceo

o ^

•i-l 4J r-l

O i-" c

e o o

Jj rH >1

M c «J ■

w

^^ z. -, ^

5; .H -H »

(_) g iw C >

Q) <1) (0 C

D ro r-H O



New York University
^ ?^ It Insttute of Mathematical Sciences

C nputer Science Division

S « < "^ "" 25 1 /\/ rcer Street New York, N. Y 1 00 1 2







^"i^")Â¥



•0.."li



An Efficient Motion Planning Algorithm for a Convex
Polygonal Object in 2-Diniensional Polygonal Space.

by

K. Kedem
M. Sharir



Technical Report No. 253

Robotics Report No. 90

October, 1986



^






V



-V^



.«:'



^



New York University A*^ '^^

Dept. of Computer Science tA^'

Courant Institute of Mathematical Sciences v^ v

251 Mercer Street \^f
New York, New York 10012 ^'



s



Work on this paper has been supported by Office of Naval Research Grant N00014-82-K-
0381, National Science Foundation CER Grant DCR-83-20085, and by grants from the Digi-
tal Equipment Corporation and the IBM Corporation.



An EfTicient Motion Planning Algorithm for a Convex
Polygonal Object in 2-Diniensional Polygonal Space.

K. Kedem^^'> and M. Sharir'-^-'^^

'"Computer Science Department

School of Mathematical Sciences

Tel Aviv University

and

'^'Courant Institute of Mathematical Sciences

New York University



ABSTRACT

We present an efficient algorithm for planning the motion
of a convex polygonal body B in two-dimensional space bounded
by a collection of polygonal obstacles. Our algorithm extends
and combines the techniques of Leven and Sharir and of Sifrony
and Sharir used for the case in which B is a line segment (a
"ladder"). It also makes use of the results of Kedem and Sharir
on the planning of translational motion of B amidst polygonal
obstacles, and of a recent result of Leven and Sharir on the
number of free critical contacts of B with such polygonal obsta-
cles. The algorithm runs in time O {kn\j{kn) log kn), where k is
the number of sides of B, n is the number of obstacle edges, and
\j(<7) is an almost linear function of q yielding the maximal
number of connected portions of q continuous functions which
compose the graph of their lower envelope, where it is assumed
that each pair of these functions intersect in at most s points.



1. Introduction

Let B he a convex polygonal object having k vertices and edges, free to
move (translate and rotate) in an open two-dimensional space V bounded by a
collection of polygonal obstacles ("walls") having altogether n comers. The
problem studied in this paper is to plan automatically a continuous obstacle-
avoiding motion of 5 between any two specified initial and final placements;
cf. Fig. l.L

Work on this paper by the second author has been supported by Office of Naval Research Grant
N00014-82-K-0381, National Science Foundation Grant No. NSF-DCR-83-20085, and by grants from
the Digital Equipment Corporation, and the IBM Corporation.



-2-

This problem has been firs: considered by Schwartz and Sharir [SS], who
present an 0{n^) algorithin for its solution (which applies to non-convex
polygonal moving objects as well). Since then this classical "piano-movers"
problem had been studied exier^ively, and several efficient algorithms had
since been developed for certain special cases of it [BZ], [LSI], [LS2], [KS],
[KLPS], [OY], [OSYl], [0SY2], [SiS].

The algorithm developed in this paper is based on a generalization and
combination of the algorithms developed by Leven and Sharir [LSI] and by
Sifrony and Sharir [SiS] for the case in which B is a line segment. The
Leven-Sharir algorithm partitions the (3-dimensional) space FP of free place-
ments of B (also known as the free configuration space of B) into simple,
openly disjoint and connected cells, and then determines the adjacency
between these cells. This yields aii abstract representation of FP by a connec-
tivity graph whose nodes are ttiese cells and whose edges connect pairs of
adjacent cells. Once the cells containing the specified initial and final place-
ments of B are determined, the motion-planning problem is then reduced to a
simple graph searching.

The second algorithm [SiS] also reduces the problem to a combinatorial
graph searching, but uses a different graph, called the vertex graph, whose
nodes are the comers of FP, and whose edges connect pairs of comers that
are adjacent along edges of FP, or along some additional auxiliary arcs in FP.

Our algorithm constructs an "intermediate" kind of graph, which we call
an edge graph; its nodes are edges of the boundary of FP and its edges con-
nect pairs of adjacent FP-edges (in a sense to be defined more precisely
below). Our algorithm begins (as in [LSI]) by restricting the motion of B to
be purely translational at some fixed orientation 9 . This motion has only two
degrees of freedom, so that it is easier to calculate its associated restricted 7-
D space FPq of free placements (a task which has already been carried out in
[KS]; cf. also [BZ], [KLPS], [LS2]), and to represent it as the union of 2-D
polygonal regions having simple shape. Each such region can be given a
discrete combinatorial labeling that does not depend continuously on 6.
Roughly speaking, we represent FP^ by a graph VG^, whose nodes are the
comers of FPq, and whose edges connect pairs of adjacent comers. The
nodes of VG^ are given discrete combinatorial labels (that do not depend
continuously on 6).

Next we observe that this combinatorial description of FPq will not
change as 9 varies slightly, unless 9 is one of finitely many critical orienta-
tions, at which some critical condition, which affects the combinatorial stmc-
ture of FPe , occurs.



-3-

As it turns out, the most complex of these criticaJ orientations are those
at which the object B makes three simultaneous contacts with the obstacles,
without penetrating into any obstacle. If fl is a line segment (a "ladder"),
then it is shown in [LSI] that the total munber of such critical placements of
B is 0{n^), which consequently leads to an O(n^logn) algorithm for the
desired motion planning. If fi is a convex it-gon which is free only to translate
in V but not to rotate, then the motion planning problem becomes simpler
and can be accomplished in time 0{kn log kn) [LS2], [KS], [KLPS], [BZ].
This follows from the property, which is proved in [KS] and will be used
below, that the number of placements of B (all having the same given orien-
tation) at which it simultaneously touches two obstacles, without penetrating
mto any obstacle, is 0{n) (provided that B and the obstacles are in "general
position"; cf. [KS] and below). If B is also allowed to rotate, then the
corresponding critical orientations are much harder to analyze. Since each
contact of B with the walls is a contact of either a corner of B with a wall
edge or an edge of B with a wall comer, a crude and straightforward upper
bound on the number of these critical placements of triple contact of B is
0{{kny). Moreover, if B is nonconvex, then there are cases where the
number of these critical placements of B is indeed Cl{{kny). However, a
recent result of Leven and Sharir [LS3] shows that if B is convex, then the
number of these critical orientations is only 0{kn\j{kn)), where \j{q) is the
maximal number of connected portions of the graphs of q continuous func-
tions which compose the graph of their lower envelope, where it is assumed
that each pair of these functions intersect in at most s points. It is shown in
[Sz] that Xj(n) = 0{n \og*n) (where \og*n is the length of the smallest

exponential tower 2^ exceeding or equal to n). A better asymptotic

bound is given in [HS] for the case s = 3 and in [Sh] for larger values of s.
These better bounds are roughly of the form 0(na(n)'^^'^^"^' )), where a(n)
is the functional inverse of Ackermann's function, and is extremely slowly
growing. In short, for a fixed s, Xj(rz) is nearly linear in n, although, as is
shown in [HS], [Sh2] one has X,(n) = a(na\-^'-'^'>'^i{n)), so that X,(n) is
super linear in n for j>3.

Using these bounds together with the techniques of [LSI], [SiS], [KLPS]
and [KS], we next extend each node in VG^, by varying 0, into a node which
represents an edge of FP, and then construct an edge graph EG which
represents adjacency of such edges along the boundary of FP. All this finally
yields a motion planning algorithm for a convex polygonal object B which
nms in time 0{kn\j{kn) log kn). This algorithm is being implemented on an
IBM robot RS/2, and its experimental results will be described in a forthcom-
ing paper.



-4-

The paper is organized as follows. In section 2 we describe the recursive
discrete representation of FF by its associated edge-graph EG, as outlined
above. In section 3 we discuss ihe algorithmic details involved in the con-
struction of EG, and in its use foi 3:ctual motion planning.

2. Discrete Recursive Representation of the Free Configuration Space.

Let B be a bounded convex ^-sided polygonal body, whose interior is
nonempty. B is free to move (translate and rotate) in a bounded two dimen-
sional open region V having a polygonal boundary with n corners altogether.
We assume that this boundary can be partitioned into a collection of pairwise
disjoint simple closed polygonal curves (which we call "walls"); the "wall
region" V can then be partitioned into a collection of convex polygonal
regions having pairwise disjoint (and nonempty) interiors. Note that these
assumptions exclude degenerate configurations in which a wall is just an iso-
lated comer or an isolated segment, or in which a wall comer is adjacent to
more than two wall edges.

Let P be an arbitrary fixed reference point in the interior of B, and let Q
be an arbitrary comer of B. Each placement Z of fi in the plane can be
represented by the parameters (AT, 9), where X is the position of P, and
where is the orientation of the vector PQ.

As in [SS], we define a free placement (X,Q) of B to be a placement at
which B is fully contained within V; a semi free placement {X, 9) of B is
defined to be a placement at which B may touch some walls, but not
penetrate into the interior of the wall region V . The set FP of all free place-
ments of 5 is an open three dimensional manifold, and the set SFP of all semi
free placements is closed.

As in [LS3], we assume that the moving object B and the obstacles are in
general position. Roughly speaking, this means that (i) there does not exist a
placement of B in which it meets four independent constraints involving con-
tacts with obstacles; and (ii) there do not exist two placements of B with the
same orientation such that B meets at each of them three independent con-
straints involving contacts with obstacles (cf. [LS3] for more detail). We also
assume for simplicity that no wall edge is horizontal; this can always be
enforced by an appropriate rotation of V.

Since the motion of B has three degrees of freedom, we first analyze, as
in [LSI], only purely translational motion of B (involving just two degrees of
freedom), and only then treat the case of general motion of B, including rota-
tion. This will enable us to obtain recursively a combinatorial representation
of each cross-section of FP at a fixed 9, from which we will then construct a
certain discrete graph which represents the entire space FP in a "connectivity



preserving" manner, and which allows us to reduce the motion planning prob-
lem to a discrete problem of path searching through that graph. (Our
method can be regarded as a hybrid of the two techniques presented in [LSI]
and [SiS], in a sense that will become clear later on. An attempt at direct
generalization of the technique of [LSI] has led to certain technical difficul-
ties that we have not been able to overcome)

2.1. The case of translatlonal motion of B.

Definition 2.1: (a) ([LS3]) A (potential) contact pair O is a pair (W,S) such
that either W is a wall edge and 5 is a corner of 5, or W is a wall comer and 5
is a side of S. In the first case we call the pair a contact pair of type I and in
the second case a contact pair of type n.

(b) A contact pair of type III is a pair O = {W,S) where W is a wall comer
and 5 is a comer of B.

(c) An actual obstacle contact (i.e. a contact of B with an obstacle) is said to
involve the contact pair O = {W,S) if this contact is of a point on 5 against a
point of W, and furthermore if this contact is locally free. i.e. the inner angle
of 5 at 5 lies entirely on the exterior side of W if 5 is a comer of B, and the
entire angle within the wall region V at W lies exterior to 5 if W is a wall
comer.

(d) (cf . [KS]) Let A be one of the convex polygonal obstacles into which V is
decomposed. The expanded obstacle A^ associated with A for a given orienta-
tion 9 of fi is the pointwise vector difference A - Bq, where B e is the stan-
dard placement of the moving object B in which P lies at the origin, rotated
by 9. Ae is also a convex polygonal region whose sides are vector (Min-
kowski) differences of the form W - S where {W,S) is an contact pair of
type I or n, and whose vertices have a similar representation for contact pairs
(W,5) of type HI.

It follows from the results of [KS] that the restricted free configuration
space FPq, which is the space of all free placements of B having orientation
9 , can be represented as the complement of the union

K, = ^U^ (AOe* = U^ (Ai - B,)

where A 1, ... ,A^ are the convex polygonal regions into which V is decom-
posed. See Fig. 2.1 for an illustration of K^ and of FP^. The boundary of
ATe (and also of FP^) thus consists of a collection of polygonal curves having
finitely many comers. An edge on the boundary of Kq is a connected portion
of an edge of an expanded obstacle induced by a type I or a type II contact
pair, and each vertex of K^ is either a (convex) comer of an expanded



-6-

obstade induced by a type III (X)ntact pair, or a (non-convex) intersection
point of two sides of different expanded obstacles, each induced by a contact
pair of type I or of type 11.

The non-convex corners of the boundary of ATe are the locations of the
reference point P at semi free placc;nents in which B (at orientation 6) makes
two distinct obstacle contacts simultaneously (in each of which either a comer
of B touches a wall edge or a side of B touches a wall comer).

It is shown in [KS] (cf. also [KLPS], [LS2]) that, for each fixed orienta-
tion 9 of 5, the number of non-convex comers oi K^ is only 0{n) (actually it
is only 0{m)). An algorithm for the calculation of FP^, which makes use of
this property, is presented in [KS]; its time complexity is 0(kn log^kn),
which has later been improved to 0{kn log kn) in [LS2], using a different
approach involving generalized Voronoi diagrams (and also in a recent paper
[BZ] that uses a line-triangle representation of the moving object).

The first step of our algorithm is to use the results of [KS] or of [LS2] to
obtain a decomposition of FP^ into connected components, and to represent
each such component Q by a. discrete connected graph whose nodes are the
comers of Q, each of which is given a discrete combinatorial labeling that
does not depend continuously on 9. (This differs from the approach used in
[LSI], namely to partition FP^ into trapezoidal cells, and to establish adja-
cency of these cells in FPq, obtaining this way a connectivity graph CG^
whose connected components correspond in a 1-1 manner to the connected
components of FP^. Although this approach can be used in our case as well,
it creates technical difficulties when we attempt to extend these cells into 3-D
cells as we add the degree of freedom of rotation. We therefore prefer to use
the approach mentioned above, which is m_ore similar to that of [SiS].)

More precisely, let Cq = Co(9) be the collection of all convex and non-
convex comers of K^ (and also of FPe). Define a vertically maximal corner
of FPq to be a convex comer m of A^e which has the largest >'-coordinate
among all points of AT e in a sufficiently small neighborhood of u. For each
such vertically maximal comer u, we introduce an auxiliary corner u * , which
is the unique point on the boundary of FPo that lies directly above u and is
such that the open segment uu* is wholly contained in FPq. (Note that since
V is assumed to be bounded, FP^ is also bounded, and u * thus always exists
and is well-defined.) Let C* = C*(9) denote the set of all such auxiliary
comers.

Next define a vertex graph VG^, whose set of nodes is
C = C(9) = Co U C*, and whose edges either connect pairs of adjacent
comers (including auxihary ones) along edges of FPq, or connect vertically
maximal comers u to their associated auxiliary comers u* € C*.



-7-

For certain critical values of 0, the boundary of FP^ may contain a
comer u for which the intersection of FP^ with any sufficiently small neigh-
borhood of u is disconnected; this would be the case if at orientation 6 a
comer of B touches a wall comer, while B also makes another contact with
the walls, which corresponds to a convex comer of K^ also lying on another
edge of Kq. (Note that more degenerate doubk- contacts, in which, say, two
convex corners of K^ touch one another, are ruled out by our assumptions
that the obstacles are in general position; cf. also [LS3].) In these cases we
split u into two distinct comers, one for each connected component of
FPq D N, for an arbitrarily small circular neighborhood N of u, and then con-
nect each of these split comers u' to its two adjacent corners along the two
rays bounding the component Q' of FPq associated with u' (cf. Fig. 2.2).
The split comer u' will be regarded as lying only on the boundary of Q' , and
not on the boundary of the other components of FPq which lie near u. (Note
that, assuming general position of the obstacles, such a comer u cannot be
vertically maximal.)

In the following subsection we will consider general motion of B includ-
ing rotation. It is important for analysis of such general motion that VGq
does not depend continuously on 6, but rather remains invariant except at
certain finitely many critical orientations, to be defined below. For this reason
we assign discrete combinatorial labelings to each node (and thus also to each
edge) of VGe in the following simple manner: Each convex comer of A^e is
labeled by the type III contact pair that induces it; each non-convex corner is
labeled by the two contact pairs (of type I or II) that induce it; and each auxi-
liary comer w * is labeled as an auxiliary comer associated with the type III
contact pair that induces the vertically maximal comer u to which u*
corresponds. It is easily checked that these labelings uniquely define the
corresponding corners of FPq (at a given orientation 9). This labeling scheme
turns VGe into a discrete structure, clearly not depending continuously on 9.

Theorem 2.1: Two corners «, v € C belong to the same connected com-
ponent of VGq if and only if they lie on the boundary of the same connected
component of FPq.

Proof: If the edge («,v) belongs to VGe then by definition u and v lie on the
(boundary of the) same connected component of FPq. Thus the "only if" part
of the Theorem follows by transitive closure. As to the "if" part, it is clearly
true in the case in which u and v lie on the same connected component of the
boundary of a connected component Q of FP^. U Q has more than one boun-
dary component, then we let A„ (resp. Ay) denote the set of all nodes in VGe
reachable from u (resp. from v) by a path in VGe- We claim that both A„ and
Ay contain corners lying on the (unique) exterior boundary E of Q, so that



-8-

the preceding argument implies that A„ = Ay, or, in other words, u and v lie
in the same cx)nnected comporent of VGq. Indeed, suppose that A„ contains
no comer of E. Let z € A„ be the corner with highest y coordinate. It is then
easy to show that z is a ver*ic2lly maximal corner. (This is because z lies on
an interior boundary of Q, and if the upward-directed vertical ray from z did
not contain free positions sufficiently near z, then it would have to intersect
the boundary component containing z at a point having larger y-coordinate
than z, contradicting our choice of z. This argument is easy in 2-space; a more
general argument for higher-dimensional space is given in [SiS, Section 3].)
But then VGq contains the edge {z,z*), and z* has larger y-coordinate than z,
a contradiction which completes the proof of the Theorem. â–¡

2.2. The case of a general moilow of B.

We now turn to the general case in which B can both translate and
rotate. To do so we first consider how the combinatorial characterization of
FPq, provided by the graph VGq, changes as 9 varies.

Definition 2.3: An orientation 6 of fi is called a critical orientation, if one of
the following conditions occurs:

(i) There exists a semi-free placement of B at orientation 6 at which either it
makes simultaneously three distinct obstacle contacts involving contact
pairs of types I or 11, or it makes simultaneously two obstacle contacts,
one involving a contact pair of type III and another involving a contact
pair of type I or II. In other words, either three edges of expanded obs-
tacles meet at the same non-convex corner of K^, or a. convex comer of
ATe meets another edge of Kq (see Fig. 2.3(a,b)).

(ii) There exists a vertically maximal convex comer w of ^e whose associated
auxiliary corner u* coincides with a (convex or non-convex) comer of
K, (see Fig. 2.3(c)).

(iii) (a) Two adjacent edges of K^ become collinear (Fig. 2.3(d)), or

(b) An edge of Kq becomes horizontal (see Fig. 2.3(e)).

Lemma 2,5: The vertex graph VGq does not change as 8 varies in a suffi-
ciently small neighborhood of any non-critical orientation. Furthermore, for
each such sufficiently small neighborhood, the connected component Q{A,Q)
of FPq, corresponding to a fixed connected component A of VG^, varies con-
tinuously (in the Hausdorff topology of sets) with 9 .

Proof: Filial observe that as long as condition (iii) does not arise, each edge
or a convex corner of an expanded obstacle, corresponding to some contact
pair O, continues to appear on the boundary of that obstacle, and varies con-
tinuously with 9. Moreover, the intersection between any two such edges



-9-

(belonging to different expanded obstacles) is transversal, unless either these
edges become collinear, or they meet at an endpoint of one of them. Hence
as long as conditions (i) and (iii) do not arise, each such intersection is
transversal, and thus also varies continuously with G. Also, as long as no
three edges of expanded obstacles meet at the same non-convex comer of ATe ,
and no convex comer of Kq meets an edge of that set, it follows that as we
vary 9, each non-convex comer of K^, formed by intersection of edges
corresponding to two contact pairs Oi, O2, will continue to appear on the
boundary of K^, continue to be induced by the same two contact pairs, and
vary continuously with 0. It follows that as 6 varies slightly, no connected
subcomponent of FPq (or even no component of the boundary of FPq)
shrinks to a point and disappears, no such component newly appears, no two
such components merge into a single component, nor does such a component
split into two subcomponents. Furthermore, each connected component of
the boundary of FPq retains the same combinatorial representation as a circu-
lar sequence of convex and non-convex comers induced by the same combina-
tions of obstacle contacts, and each of these comers varies continuously with
9. This also implies that each connected component of FPq varies continu-
ously with 0.

To complete the proof of the Lemma, let u be a vertically maximal
comer of Kq. First note that as long as we do not cross any critical orienta-
tion, u remains vertically maximal as 9 varies. Also, as long as condition (ii)
does not arise, the auxiliary comer u * associated with u remains on the same
edge of ATe between the same two vertices of VG^, and varies there continu-
ously with e. All these observations clearly imply that VGq remains constant
as 9 varies through non-critical orientations. â–¡

Critical orientations of type (i) were analyzed in [LS3], where it was
shown that there are at most O (knX(,{kf^)) such orientations. Section 3
presents an algorithm for the calculation of these orientations, which runs in