ECE 470: Homework #6

Tim Bretl

(Due in class on Tuesday, December 8)

Abstract
We have been talking about motion planning. These are notes to supplement [4,
Chap. 5]. The problems for Homework #6 are interspersed with the text as P1, P2,
etc. There are a total of 13 problems (but the first 7 are easy—if you don’t think so,
come talk to me right away!).

1 Configuration Space
The configuration of a robot is enough to compute the location of every point on the robot.
The configuration space of a robot is the set of all possible configurations. We will denote
the configuration space by Q and arbitrary configurations by things like p, q ∈ Q.
For example, the configuration space of a rigid body that is free to translate but not rotate
in the plane is Q = R2 , and we can describe an arbitrary configuration by q = (x, y) ∈ R2 .
Similarly, the configuration of space of a robot arm with one revolute joint is Q = S 1 , and
we can describe an arbitrary configuration by q = θ1 ∈ S 1 .
Remember that S 1 , the unit circle, is the same as SO(2), the group of all rotations in
the plane (where “same” means they are equivalent representations). So, we can say

S 1 = SO(2).

It is often more convenient (less writing) to use S 1 rather than SO(2) when describing the
configuration space of a robot arm, which usually has more than one joint. For example, in
class we established that a robot arm with n revolute joints and zero prismatic joints has
the configuration space
n
Q = S 1 = S 1 × · · · × S 1,
where we take the product of n different S 1 ’s. But, we could just as well say

Q = (SO(2))n = SO(2) × · · · × SO(2).

As we discussed in class, it is important to note that

S 1 × S 1 6= S 2 .

1
The space S 2 is the unit sphere (the 2-dimensional analog of the unit circle) while S 1 × S 1
is a “donut” or more precisely a torus, which we denote by

T 2 = S1 × S1

The difference between these two things is clear if you think about homotopy. Any path
in S 2 (think: any path on the surface of the earth) can be shrunk to a point by continuous
deformation. A path that goes around the “donut hole” of T 2 cannot be shrunk to a point
without either cutting it in half or leaving the surface of the donut. By convention, we call T 2
the 2-torus, and by extension call
n
T n = S1
the n-torus. So, with this notation,

T 1 = S 1 = SO(2).

The “torus notation” makes it even easier to describe the configuration space of robot arms.
Consider the previous example of an arm with n revolute joints—its configuration space is

Q = T n.

Just as S 1 × S 1 6= S 2 , we have

SO(2) × SO(2) 6= SO(3).

Of course, the spaces S 2 (unit sphere) and SO(3) (group of all rotations in space) are also
not the same. Finally, remember that the configuration space of a rigid body that can rotate
and translate in space is given by

Q = SE(3) = R3 × SO(3).

As practice, please do the following:

P1. Describe the configuration space for a mobile robot that can translate and rotate in
the plane.

P2. Describe the configuration space for the 3-link manipulator in Fig. 3.23 of [4].

P3. Describe the configuration space for the 2-link manipulator in Fig. 3.24 of [4].

P4. Describe the configuration space for the 2-link manipulator in Fig. 3.25 of [4].

P5. Describe the configuration space for the 3-link manipulator in Fig. 3.26 of [4].

P6. Describe the configuration space for a six-link anthropomorphic arm, where the last
three links form a spherical wrist.

P7. Repeat (P6) if the base of the arm is free to translate and rotate in the plane.

2
If these problems seem simple, it’s because they are. If they seem complicated, please reread
this section and also see [4, Chap. 5.1]. Your answer to each problem should be

Q = something

where “something” has the notation we developed in class and summarized above.
As one final piece of notation, we will denote the set of all configurations that do not
place the robot in collision with obstacles by F. (The notation Qfree is used in the book.)
We call this the free space and note that it is a subset of configuration space, so F ⊆ Q.

2 PRM
A path in configuration space is a continuous function

γ : [0, 1] → Q.

We say that a path is collision-free if

γ(t) ∈ F for all t ∈ [0, 1].

Given an initial configuration qinitial and a final configuration qfinal , the problem of path
planning is to find a collision-free path γ such that γ(0) = qinitial and γ(1) = qfinal . In class,
we described a simple path planning algorithm called the probabilistic roadmap method.
This algorithm is one example of a broader class of probabilistic sampling-based algorithms
(see [1, 2] as well as [4, Chap 5.4]). It approximates Q by a graph of vertices and edges
(where “graph” is in used in the sense of computer science, i.e., like a “roadmap” with cities
and roads between them). It builds a list V of vertices and a list E of edges as follows:

• Add qinitial and qfinal to V .

• Repeat n times:

– Sample q ∈ Q uniformly at random.

– If q ∈ F, then add q to V .

• Repeat for each p ∈ V :

– Repeat for each q ∈ V that is close to p, where we require q 6= p:

∗ If (1 − t)p + tq ∈ F for all t ∈ [0, 1], then add (p, q) to E. (Here, we are
checking if the straight-line path between p and q is collision-free.)

• If qinitial and qfinal are connected by a sequence of edges in the graph (V, E), then this
sequence is a path between qinitial and qfinal . Otherwise, we declare that no path exists.

Although simple, this algorithm (and its variants) work remarkably well in practice.

3
2.1 Analysis of PRM
Why does PRM work well? Are we sure that it works at all? First, what we mean by “work”
is that it finds a path with high probability when one exists. (It should be easy to convince
yourself that PRM will never be able to tell you, for sure, when no path exists.) In class, we
computed a bound on the failure to find a path when one exists as follows.
First, consider n samples q1 , . . . , qn ∈ F chosen uniformly at random. (Yes, we mean F, as
discussed in class. It’s for convenience, we could just as well have used Q.) Also, consider m
balls of radius r/2 centered at points xi ∈ F. We denote each ball i by

Br/2 (xi )

and assume that Br/2 (xi ) ⊂ F for all i = 1, . . . , m. Define Ii as the event that no sampled
configuration ends up in ball i. Then, the probability that some ball i ends up containing
no sampled configurations is
Prob (∪m i=1 Ii ) .

Recalling from basic probability that

Prob (A ∪ B) = Prob (A) + Prob (B) − Prob (A ∩ B) ,

it is clear that m
X
Prob (∪m
i=1 Ii ) ≤ Prob (Ii ) .
i=1

Since the configurations are sampled uniformly at random, it should be clear that

Prob (Ii ) = Prob (Ij )

for all i, j ∈ {1, . . . , m}. So, we may as well ignore the center xi of each ball, writing it
simply as Br/2 . So, we have

Prob (∪m
i=1 Ii ) ≤ mProb (I) ,

where I is the event that no sample lands inside a ball of radius r/2 centered at some
arbitrary point, the exact location of which we no longer care about. For each sample k, we
compute

µ(Br/2 )
Prob qk ∈/ Br/2 = 1 − ,
µ(F)
where µ(·) gives the volume of a set. Since the samples are independent and identically
distributed, we apply another fact from basic probability to say that
n
µ(Br/2 ) n
 
n

Y

Prob (I) = Prob ∩k=1 {qk ∈/ Br/2 } = Prob qk ∈/ Br/2 = 1 − .
k=1
µ(F)

In class, we showed that

µ(Br/2 )
= σrd
µ(F)

4
where σ is a constant given by
µ(B1 )
σ=
2d µ(F)
and d is the dimension of configuration space. So, we have

n
Prob (I) = 1 − σrd .

An “elementary” result from calculus is that

n
≤ e−(σr )n .
d
1 − σrd

Putting everything together, we have

−(σrd )n
Prob (∪m
i=1 Ii ) ≤ me .

The point of all of this is the following. Assume that a collision-free path exists from qinitial
to qfinal . Whatever this path is (we don’t know exactly), it will have some minimum clear-
ance r and some length L. We tile this path with
 
L
m = ceil
r/2

balls of radius r/2, spaced along the path no more than r/2 apart. As we discussed in class,
if we are lucky enough to sample at least one configuration in every single one of these balls,
then we are guaranteed to have found a collision-free path. From the analysis given above,
the probability of failure is bounded by
 
L
e−(σr )n .
d
Prob (failure) ≤ ceil
r/2

The important thing to notice about this formula is that the probability of failure decays
exponentially with the number of samples n. This means that PRM is guaranteed to “work”
given a large enough number of samples. (For this reason, we call PRM probabilistically
complete.)

P8. An alternative approach that may at first seem easier is to tile the entire space with
balls, rather than a particular path. Assume that

F = [0, 1]d ∈ Rd ,

i.e., the configuration space is Rd and the free space is a d-dimensional cube. How
many balls of radius r/2 are required to cover the entire free space so that a straight-
line path is guaranteed to exist between each ball and its neighbor? Modify (very, very
slightly) our analysis to compute the resulting bound Prob (failure). Compare your
answer with the bound given above. (Is it much better? Much worse? Why?)

5
 d 1

(x, y)

Figure 1: Figure for Problem (P9).

P9. As we discussed in class, a much tighter bound (in general) on the probability of
failure follows from a consideration of visibility. The analysis is more sophisticated,
so we ignored the details—here, we will get some feeling for how “visibility” might be
computed. Consider the configuration space Q = [0, 1] × [0, 1] shown in Fig. 1. The
entire space is free except for a wall of infinitesimal thickness that cuts the space in
half. There is a small opening of height d in the exact middle of this wall. Given
an arbitrary configuration (x1 , y1 ) ∈ F such that x < 1/2, find the probability that a
second configuration (x2 , y2 ) sampled uniformly at random will be visible from (x1 , y1 ).
(Note: you may have to consider several different cases, depending on (x1 , y1 ). Assume
that (0, 0) is at the bottom left-hand corner of Q.)

2.2 Distance metrics

The PRM algorithm depends on identifying pairs of configurations that are “close” to each
other. This requires a distance metric. Distance metrics can be tricky when considering
rotation.
P10. Find a general expression for the distance (in radians) between two configurations
q1 , q2 ∈ S 1 .
There are at least two different ways of solving this problem: one involves parame-
terizing each configuration by an angle and thinking about modulus, and the other
involves parameterizing each configuration by a rotation matrix and thinking about
dot products.

2.3 Collision checking

The PRM algorithm depends on being able to test if q ∈ F. In class, we considered an
example of a 2-link arm moving among circular obstacles, where each link was modeled as

6
a line segment. To check collision, we computed the distance between each link and each
obstacle—if all such distances are positive, the configuration is collision-free. Doing this
requires being able to compute the distance between a line segment and a circular obstacle.

P11. Derive a general expression for the distance between a line segment with endpoints
p1 , p2 ∈ R2 and a ball Br (q) of radius r ≥ 0 centered at q ∈ R2 . (Hint: think about
dot products, and don’t forget to consider special cases.)

The PRM algorithm also depends on being able to test if a straight-line path between p, q ∈ F
is collision-free. An easy way to do this is by interpolating at some fixed resolution between p
and q, and checking each intermediate point. The problem with this approach is that it is
only approximate—it may miss some collisions. Here is an alternative. Let’s say we can
compute the minimum distance η(q) between any point on the robot and any obstacle, and
that we can compute the maximum distance λ(p, q) that any point on the robot will move
when its configuration moves along a straight line from arbitrary configurations p and q.
Then, the following is true [3]:

If λ(p, q) ≤ η(p) + η(q) then the straight-line path from p to q is collision-free. (1)

A recursive algorithm for collision-checking naturally suggests itself. First, check Eq. (1) on p
and q. If it holds, the path between p and q is collision-free. If not, compute the midpoint m
between p and q. Check Eq. (1) on p and m, then on m and q. If it holds in both cases, the
path is collision-free. If not, repeat this process until either (1) we verify that the path is
collision-free, or (2) we find some midpoint m ∈ / F, hence the path is not collision-free.

P12. Assume that the robot is a point that can translate in in the plane and that there
is a single circular obstacle of radius 0.1 located at (0.2, 0.05). Consider the two
configurations p = (0, 0) and q = (1, 0). Note that the straight-line path from p
to q is not collision-free.

• First, implement the “easy” approach to collision-checking, i.e., check N equally-

space configurations on the straight line between p and q. In particular, find the
minimum value of N for which this approach does not miss the collision.
• Now, implement the “exact” approach to collision-checking, given above. Ex-
plicitly compute λ and η at each stage of the recursion and draw a plot of the
midpoints that are computed along [0, 1] during this process. (The last of these
midpoints should be the one that allows you to say that the path is in collision.)

2.4 Local connection strategies

Straight lines are not the only way to connect “close” configurations. Another approach is
based on the use of artificial potential fields, which we will discuss this week.

P13. Do problem 5-12 in your textbook [4]. Assume that the repulsive forces act only at the
four vertices of the robot. Choose a value for ρ0 for both obstacles so that the regions
of influence do not overlap. Use equations (5.5) and (5.6) in the text to construct the

7
 repulsive field Urep and artificial workspace forces Frep . Check your work, because it’s
a little tedious. You will need to find the values of ρ(ai (q)) and ∇ρ(ai (q)) for each
vertex of the robot. Sum the repulsive forces to get the net workspace force. To find
the configuration space forces and torque, follow Example 5.6 in the text.

References
[1] H. Choset, K. Lynch, S. Hutchinson, G. Kanto, W. Burgard, L. Kavraki, and S. Thrun.
Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, 2005.

[2] S. M. LaValle. Planning algorithms. Cambridge University Press, New York, NY, 2006.

[3] F. Schwarzer, M. Saha, and J. C. Latombe. Adaptive dynamic collision checking for
single and multiple articulated robots in complex environments. IEEE Transactions on
Robotics, 21(3):338–353, June 2005.

[4] M. W. Spong, S. Hutchinson, and M. Vidyasagar. Robot Modeling and Control. John
Wiley & Sons, 2006.