Lecture 2: Motion Planning via Decomposition and Search
– Sweeping Trapezoidal Algorithm
Based on Chapter 2, Bullo and Smith
Vaibhav Srivastava
Department of Electrical & Computer Engineering
Michigan State University
January 11, 2017
ECE 818: Robotics
http://www.egr.msu.edu/∼vaibhav/teaching/robotics.html
1 / 17
Open-loop versus closed-loop planning
• Sensor-based planning is also referred to as closed-loop planning
• Robot actions were functions of the robot sensors
• The loop is: the robot moves, then it senses the environment, and then it decides
how to move again
• Open-loop planning: robots that do not have sensors, but rather have access to a
map of the environment
• Feedback versus Feedforward control
2 / 17
Assumptions on Environment and Robot
Assumptions on Environment
• a workspace W that is a bounded polygon in R2 ;
• a finite set of polygonal obstacles O1 , O2 , . . . , On ;
• Free workspace: Wfree = W \ (O1 ∪ O2 ∪ · · · ∪ On );
• a start point pstart and a goal point pgoal in Wfree ;
• pstart and pgoal are in Wfree
3 / 17
Assumptions on Environment and Robot
Assumptions on Environment
• a workspace W that is a bounded polygon in R2 ;
• a finite set of polygonal obstacles O1 , O2 , . . . , On ;
26
• Free workspace: Wfree = W \ (O1 ∪ O2 ∪ · · · ∪ On );
Chapter 2. Motion Planning via Decomposition
• a start point and a goal point, and
• a start point pstart and a goal point• aprobot
in Wfree
by ;a moving point.
goal described
• pstart and pgoal are in Wfree
As in the previous chapter, we de�ne the free workspace Wfree = W \ (O1 [ O2 [ · · ·
Figure 2.1. We continue to postpone more realistic and complex problems where the
shape, size and orientation.
Assumptions on Robot
pgoal
• robot is a moving point
• robot knows the start and goal locations
• robot knows the workspace and obstacles
pstart
Figure 2.1: An example free workspace with start and goal locations
3 / 17
Our task is to plan the robot motion from the start point to the goal point via
a pr
Polygons
• a polygon is a plane figure composed by a finite chain of segments (called sides or
edges) closing in a loop
• The points where the segments meet are called vertices (or corners)
• Polygons are always assumed to be simple: their boundary does not cross itself
• Programming- wise, it is convenient to represent a polygon by a counter-clockwise
ordered sequence of vertices
4 / 17
Polygons
• a polygon is a plane figure composed by a finite chain of segments (called sides or
edges) closing in a loop
• The points where the segments meet are called vertices (or corners)
• Polygons are always assumed to be simple: their boundary does not cross itself
• Programming- wise, it is convenient to represent a polygon by a counter-clockwise
ordered sequence of vertices
• Complexity of a polygon can be measured by the number of vertices
• Complexity of a workspace can be measured by the total number of vertices in
workspace boundary and obstacle boundaries (# vertices of free space)
4 / 17
Figure 2.2: The function f (n) grows linearly with n. The function g(n) grows with the square of n.
Convex Sets
interior and an exterior angle): a polygonal set is convex if and only if each vertex is convex,
•hasAanset
S is convex if for any two points p and q in S, the entire segment pq is also
i.e., it has an interior angle less than ⇡. A vertex is instead called non-convex if its interior angle is
contained
larger than ⇡. in S
convex set
non-convex set
non-convex set
convex
vertex
non-convex
vertex
Figure 2.3: Examples of convex and non-convex sets. A convex set cannot have any hole. Polygons have convex and
non-convex interior angles.
Planning in non-convex sets via convex decompositions Let us now use the notion of convexity
for planning purposes. If the start point and the goal point belong to the same convex set, then
the segment connecting the two points is an obstacle-free path. If, instead, the free workspace is
not convex, then the following �gure and algorithmic ideas provide a simple e�ective answer.
pgoal
pgoal
5 / 17
Figure 2.2: The function f (n) grows linearly with n. The function g(n) grows with the square of n.
Convex Sets
interior and an exterior angle): a polygonal set is convex if and only if each vertex is convex,
•hasAanset
S is convex if for any two points p and q in S, the entire segment pq is also
i.e., it has an interior angle less than ⇡. A vertex is instead called non-convex if its interior angle is
contained
larger than ⇡. in S
convex set
non-convex set
non-convex set
convex
vertex
non-convex
vertex
Figure 2.3: Examples of convex and non-convex sets. A convex set cannot have any hole. Polygons have convex and
non-convex interior angles.
• A polygonal set is convex if and only if each vertex is convex, i.e., it has an
Planning
non-convex
sets via
interiorinangle
less than
π convex decompositions Let us now use the notion of convexity
for planning purposes. If the start point and the goal point belong to the same convex set, then
segment connecting
two points
is annon-convex
obstacle-free ifpath.
instead,angle
the free
workspace
is π
•theLikewise,
a vertex istheinstead
called
its If,
interior
is larger
than
not convex, then the following �gure and algorithmic ideas provide a simple e�ective answer.
pgoal
pgoal
5 / 17
convex set
non-convex set
vertex
non-convex set
vertex
Figure 2.3: Examples of convex and non-convex sets. A convex set cannot have any hole. Polygons have convex and
non-convex interior angles.
Planning in non-convex sets via convex decompositions
•Planning
in non-convex
sets via convex
decompositions
us now
A robot
can move between
any two
points in a Let
convex
setuse the notion of convexity
for planning purposes. If the start point and the goal point belong to the same convex set, then
•theHow
to leverage
this
find
a obstacle
free pathpath.
in aIf,non-convex
environment?
segment
connecting
theto
two
points
is an obstacle-free
instead, the free
workspace is
not convex, then the following �gure and algorithmic ideas provide a simple e�ective answer.
pgoal
pgoal
pstart
pstart
Figure 2.4: Planning through a non-convex environment by moving from a convex subset to another
6 / 17
Decomposition of a nonconvex set into convex subsets
• decompose Wfree into the union of convex subsets, such as triangles or trapezoids
• triangulation: the decomposition of the polygon into a collection of triangles
• trapezoidation: the decomposition of the polygon into a collection of trapezoids
7 / 17
initialize an empty list T of trapezoids
order all vertices (of the obstacles and of the workspace) horizontally from left to right
3: for each vertex selected in a left-to-right sweeping order :
4:
extend vertical segments upwards and downwards from the vertex until they intersect an obstacle
or the workspace boundary
5:
add to T the new trapezoids, if any, generated by these segment(s)
1:
2:
Decomposition of a nonconvex set into convex subsets
• decompose Wfree into the union of convex subsets, such as triangles or trapezoids
The algorithm isthe
among
a class of algorithms
studied ininto
computational
geometry;
feel free
• triangulation:
decomposition
of the polygon
a collection
of triangles
to inform yourself about this topic at Wikipedia:computational geometry. An execution of the
is illustrated
in decomposition
the Figure 2.5. Note
T3 and
i.e., they
•algorithm
7 are degenerate,
trapezoidation:
the
of that
the trapezoids
polygon into
a Tcollection
of trapezoids
are triangles.
T4
T6
T8
T2
T1
T7
T3
T5
T10
T9
Figure 2.5: A trapezoidation of a workspace into trapezoids T1 , . . . , T10
7 / 17
triangles, and
(ii) the trapezoidation of a polygon is the decomposition of the polygon into a collection of
trapezoidation
trapezoids. (WeThe
allow sweeping
some trapezoids
to have a side of zeroalgorithm
length and therefore be triangles.)
It is easy to see that a polygon can be triangulated in multiple ways (e.g., consider the two possible
diagonals of a square). In what follows we present an algorithm to trapezoidate, i.e., decompose
into trapezoids, a polygon with polygonal holes.
The sweeping trapezoidation algorithm
Input: a polygon possibly with polygonal holes
Output: a set of disjoint trapezoids, whose union equals the polygon
1: initialize an empty list T of trapezoids
2: order all vertices (of the obstacles and of the workspace) horizontally from left to right
3: for each vertex selected in a left-to-right sweeping order :
4:
extend vertical segments upwards and downwards from the vertex until they intersect an obstacle
or the workspace boundary
5:
add to T the new trapezoids, if any, generated by these segment(s)
The algorithm is among a class of algorithms studied in computational geometry; feel free
to inform yourself about this topic at Wikipedia:computational geometry. An execution of the
algorithm is illustrated in the Figure 2.5. Note that trapezoids T3 and T7 are degenerate, i.e., they
are triangles.
8 / 17
Table 2.1: The six vertex types encountered during the trapezoidal decomposition algorithm
The sweeping trapezoidation algorithm: Notation
s1
p6
s7
s8
p4
p5
s6
s9
p3
s5
p7
s10
s4
s3
p2
p8
p1
s2
Figure 2.6: The line segments s1 , . . . , s10 and the obstacle vertices p1 , . . . , p8 required by the algorithm
To execute steps 4: and 5: of the sweeping trapezoidation algorithm, we maintain a list S of
• Let
the workspace boundary be an axis-aligned rectangle and every obstacle vertex
the obstacle segments intersected by the sweeping line L. The obstacle segments are stored in
has
a unique x-coordinate
decreasing order of their y-coordinates at the intersection point with L. A key property of S
is• that
it changes
only when
hits a newboundary
vertex. Thus,
new vertex
is encountered,
each
line segment
si onL obstacle
haswhen
a lefttheendpoint
`i vand
a right
steps
4:
and
5:
update
the
list
of
trapezoids
T
and
the
list
of
obstacle
segments
S.
The details of
endpoint ri
Steps 4: and 5: are as follow, and are illustrated in Figure 2.7 for each of the six vertex types of
• Write
Table
2.1. this segment as si = [`i , ri ]
• Define a sweeping vertical line L moving from left to right
• When L hits an environment vertex, the vertex must connect two segments
9 / 17
sweeping vertical line L moving from left to right. When the sweeping line L hits an environment
vertex, the vertex must connect two segments. This vertex can be categorized into one of six types
(see Figure 2.6) as summarized Classification
in Table 2.1.
of Vertices
Vertex Type
Endpoints at Vertex
Convex/Concave
Example
(i)
(ii)
(iii)
(iv)
(v)
(vi)
both left
both left
both right
both right
one left one right
one left one right
convex
concave
convex
concave
convex
concave
p6 and p8
p3
p2 and p4
p7
p1
p5
Table 2.1: The six vertex types encountered during the trapezoidal decomposition algorithm
s1
p6
s7
s8
p4
p5
s6
s9
p3
s5
p7
s10
s4
s3
p2
p8
p1
s2
10 / 17
Vertex-Type and Update to Algorithm I
• Initialize: S = [s1 , s2 ]; T = {};
11 / 17
2
Vertex-Type and Update to Algorithm I
• Initialize: S = [s1 , s2 ]; T = {};
Chapter 2. Motion Planning via Decomposition and Search
Type (i)
s1
pt
Type (ii)
s1
s6
s7
s8
s3
s9
s10
pb
s2
s2
Update S: [s1 , s2 ] to [s1 , s3 , s10 , s2 ]
Add to T : [pt , `1 , `2 , pb ]
Update segment endpoints: `1 := pt , and `2 := pb
Type (iii)
s1
pt
s6
p4
Update S: [s1 , s6 , s9 , s2 ] to [s1 , s6 , s7 , s8 , s9 , s2 ]
Add to T : None
Update segment endpoints: None
Type (iv)
s1
s8
11 / 17
2
Vertex-Type and Update to Algorithm I
32
2. Planning
Motion Planning
via Decomposition
and Search
• Initialize: S = [s1 , s2 ]; T = {};
ChapterChapter
2. Motion
via Decomposition
and Search
Type (i) Type (i)
s1
s1
pt
pt
Type (ii) Type (ii)
s1
s1
s6
s6
s7
s7
s8
s3
s3
s10
pb
pb
s9
s8
s9
s10
s2
s2
s2
s2
Update
Update S:
[s1 , s6S:
, s9[s, s1 ,2 s] 6to, s[s
, 2s]6 ,tos7[s
, s18, ,ss69, ,ss72, ]s8 , s9 , s2 ]
Update
[s1[s,1s,2s]3to
Update S:
[s1 , s2S:
] to
, s[s
9 ,1s
101, ,ss23] , s10 , s2 ]
Add to TAdd
: None
to T : None
Add to TAdd
: [ptto
, `1T, :`2[p, tp,b`] 1 , `2 , pb ]
segment
endpoints:
None None
Update segment
endpoints:
Update segment
endpoints:
`1 := pt ,`and
pb `2 :=Update
Update segment
endpoints:
, and
pb
2 t:=
1 :=`p
Type (iii)Type (iii)
s1
s 1 pt
pt
s 6 p4
p4
s6
Type (iv) Type (iv)
s1
s1
s8
s
11 / 17
s10
pb
s2
s2
Vertex-Type
and Update
to Algorithm
II
Update S: [s , s , s , s ] to [s , s , s , s , s , s ]
Update S: [s1 , s2 ] to [s1 , s3 , s10 , s2 ]
Add to T : [pt , `1 , `2 , pb ]
Update segment endpoints: `1 := pt , and `2 := pb
Type (iii)
s1
pt
s6
p4
1
6
9
2
1
6
Add to T : None
Update segment endpoints: None
7
8
9
2
Type (iv)
s1
s8
s7
s9
pb
s8
s9
p7
s10
s3
s2
s2
Update S: [s1 , s6 , s7 , s8 , s9 , s2 ] to [s1 , s8 , s9 , s2 ]
Add to T : [pt , `1 , `6 , p4 ], and [p4 , `7 , pb ]
Update segment endpoints: `1 := pt , and `8 := pb
Type (v)
s1
Update S: [s1 , s8 , s9 , s10 , s3 , s2 ] to [s1 , s8 , s3 , s2 ]
Add to T : [p7 , `9 , `10 ]
Update segment endpoints: None
s1
Type (vi)
pt
s7
s8
p5
s7
12 / 17
s10
pb
pb
s10
s2
s2
s2
s2
Vertex-Type
and Update to Algorithm
II
Update S:Update
[s1 , s6S:
, s9[s, 1s,2s] 6to, s[s
, 2s]6 ,tos7[s, s1 8, ,ss69, ,ss72, ]s8 , s9 , s2 ]
Update
[s1, ,ss23] , s10 , s2 ]
9 ,1s
Update S:
[s1 , s2S:
] to[s[s
, s10
1 ,1s,2s]3to
to T : None
Add to TAdd
: None
Add to TAdd
: [ptto
, `1T, `: 2[p
, pt ,b`] 1 , `2 , pb ]
Update segment
endpoints:
segment
endpoints:
None None
Update segment
endpoints:
pt:=
, and
pb
Update segment
endpoints:
`1 := pt ,`and
pb `2 :=Update
1 :=`2
Type (iii)Type (iii)
s 1 pt
s1
pt
s 6 p4
p4
s6
Type (iv)Type (iv)
s1
s1
pb
s9
s8
s9
s9
pb
s9
s8
p7
p7
s10
s10
s3
s2
s2
s8
s8
s7
s7
s3
s2
s2
S:Update
[s1 , s8S:
, s9[s, 1s,10s,8s, 3s,9s, 2s]10to, s[s
, 2s]8 ,tos3[s, s12, ]s8 , s3 , s2 ]
Update S:
[s1 , s6S:
, s7[s, 1s,8s, 6s,9s, 7s,2s] 8to
, s9[s, 1s,2s] 8 , s9 , s2Update
Update
, s[s
]
3 ,1s
9 ,1s,2s]8to
Add to TAdd
: [p7to
, `9T, `: 10
[p]7 , `9 , `10 ]
Add to TAdd
: [ptto
, `1T, `: 6[p
, pt ,4`],1and
] 4 , `7 , pb ]
, `6 , [p
p44],, `and
7 , pb[p
segment
endpoints:
None None
Update segment
endpoints:
Update segment
endpoints:
`1 := pt ,`and
pb `8 :=Update
Update segment
endpoints:
pt:=
, and
pb
1 :=`8
Type (v) Type (v)
s1
s1
s7
s7
s1
s8
Type (vi)Type (vi)
pst1
pt
ps58
s7
p5
s7
12 / 17
9
s10
s3
s2
s2
Vertex-Type
and Update to Algorithm
III
Update S: [s1 , s6 , s7 , s8 , s9 , s2 ] to [s1 , s8 , s9 , s2 ]
Add to T : [pt , `1 , `6 , p4 ], and [p4 , `7 , pb ]
Update segment endpoints: `1 := pt , and `8 := pb
Type (v)
s1
Update S: [s1 , s8 , s9 , s10 , s3 , s2 ] to [s1 , s8 , s3 , s2 ]
Add to T : [p7 , `9 , `10 ]
Update segment endpoints: None
s1
Type (vi)
pt
s7
s8
p5
s7
s4
s3
s2
p1
pb
Update S: [s1 , s7 , s3 , s2 ] to [s1 , s79 , s43 , s2 ]
Add to T : [p1 , `3 , `2 , pb ]
Update segment endpoints: `2 := pb
s3
s2
Update S: [s1 , s8 , s3 , s2 ] to [s1 , s7 , s3 , s2 ]
Add to T : [pt , `1 , `8 , p5 ]
Update segment endpoints: `1 := pt
Figure 2.7: Classi�cation of the six types of vertices. For each vertex type, the �gure illustrates the actions performed
in Step 4: and Step 5: to update list of trapezoids trapezoids T , the segment list S, and the segment endpoints.
13 / 17
s10
9
s3
s3
s2
s2
s2
s2
Update
S:
[s
,
s
,
s
,
s
, s[s
[s1 , s8 , s3 , s2 ]
Update
S:
[s
,
s
,
s
,
s
,
s
,
s
]
to
[s
,
s
,
s
,
s
]
1
8
9
10
3 , s,2s] to
Update S: [s1 , s6 , s71, s86, s97, s28] to9[s12, s8 , s9 ,1s2 ]8 9 2 Update S: [s1 , s8 , s9 , s10 , s3 , s2 ] to
1 8 , s3 , s2 ]
Add
to
T
:
[p
,
`
,
`
]
T
:
[p
,
`
,
`
,
p
],
and
[p
,
`
,
p
]
7
9
10
t
1
6
4
4
7
b
Add
to
T
:
[p
,
`
,
`
]
Add to Add
T : [pto
,
`
,
`
,
p
],
and
[p
,
`
,
p
]
7 9 10
t 1 6 4
4 7 b
Update segment
endpoints:
endpoints:
:= `p8t ,:=
and
pb
segment
endpoints:
None None
Update Update
segmentsegment
endpoints:
`1 := pt`, 1and
pb`8 := Update
Vertex-Type and Update to Algorithm III
Type (v)Type (v)
s1
s1
s7
s3
s3
s2
p1
psb2
s1
Type (vi)Type (vi)
sp1t
pt
s7
s4
p1
pb
s8
sp85
s3
s3
s7
p5
s7
s4
[s31, ,ss27] ,to
s3[s
, s12, ]sto
, s2 ]9 , s3 , s2 ]
Update Update
S: [s1 , sS:
7, s
9 , s[s
3 ,1 s
7
4
T3:, `[p2 1, p, `b3] , `2 , pb ]
Add to Add
T : [pto
1, `
endpoints:
Update Update
segmentsegment
endpoints:
`2 := pb`2 := pb
s2
s2
Update
, s[s
Update S:
[s1 , s8S:
, s[s
, s3[s, 1s,2s] 7 , s3 , s2 ]
3 ,1s,2s]7to
3 ,1s,2s]8to
Add to TAdd
: [ptto
, `1T, :`8[p, tp,5`]1 , `8 , p5 ]
Update segment
endpoints:
Update segment
endpoints:
`1 := pt `1 := pt
Figure 2.7:
Classi�cation
of the six
oftypes
vertices.
For eachFor
vertex
thetype,
�gurethe
illustrates
the actions
Figure
2.7: Classi�cation
oftypes
the six
of vertices.
eachtype,
vertex
�gure illustrates
theperformed
actions performed
in Step in
4: Step
and Step
5: Step
to update
of trapezoids
trapezoids
T , the segment
list S, and
endpoints.
4: and
5: to list
update
list of trapezoids
trapezoids
T , the segment
listthe
S, segment
and the segment
endpoints.
13 / 17
triangles, and
(ii) the trapezoidation of a polygon is the decomposition of the polygon into a collection of
trapezoidation
trapezoids. (WeThe
allow sweeping
some trapezoids
to have a side of zeroalgorithm
length and therefore be triangles.)
It is easy to see that a polygon can be triangulated in multiple ways (e.g., consider the two possible
diagonals of a square). In what follows we present an algorithm to trapezoidate, i.e., decompose
into trapezoids, a polygon with polygonal holes.
The sweeping trapezoidation algorithm
Input: a polygon possibly with polygonal holes
Output: a set of disjoint trapezoids, whose union equals the polygon
1: initialize an empty list T of trapezoids
2: order all vertices (of the obstacles and of the workspace) horizontally from left to right
3: for each vertex selected in a left-to-right sweeping order :
4:
extend vertical segments upwards and downwards from the vertex until they intersect an obstacle
or the workspace boundary
5:
add to T the new trapezoids, if any, generated by these segment(s)
The algorithm is among a class of algorithms studied in computational geometry; feel free
to inform yourself about this topic at Wikipedia:computational geometry. An execution of the
algorithm is illustrated in the Figure 2.5. Note that trapezoids T3 and T7 are degenerate, i.e., they
are triangles.
14 / 17
The sweeping trapezoidation algorithm: Implementation
4.1 determine the type of vertex v
4.2 update S by adding obstacle segments starting at v and removing obstacle
segments ending at v
4.3 use S to extend vertical segments upwards and downwards from v , that is, to find
intersection points pt and pb above and below v (if any)
5.1 add to T zero, one or two new trapezoids depending on vertex type
5.2 update the left endpoints of the obstacle segments in S above and below the
vertex v
15 / 17
Run-time of an algorithm
• the number of computer steps required to execute the code
• assume that addition, multiplication and division of two numbers can be
performed in one computer step
• comparing two numbers requires one computer step
• run-time of an algorithm is a function of the input fed to the algorithm
• Order notation: Given two positive functions f(n) and g(n)
f ∈ O(g )
Planningconstant
via Decomposition
Searchthat f (n) ≤ Kg (n), for all n > n0
if there exists n0Chapter
and2. aMotion
positive
K and
such
• Example: For an input of size
f (n)
g(n)
n
Figure 2.2: The function f (n) grows linearly with n. The function g(n) grows with the square of n.
n, if an algorithm takes
5n2 + 4n + 1 steps, then its
run-time is O(n2 )
16 / 17
Run-time of trapezoidation algorithm
• Consider a free workspace with n vertices
• sorting the vertices from left to right: use well known sorting algorithms, e.g.,
bubble sort: O(n log n)
• Assume S is a sorted array
• Insert/delete from S: scanning the whole array (n entries) for each vertex: O(n2 )
• For a more sophisticated data structure, trapezoidation algorithm can be
implemented in O(n log n) time
17 / 17