MODERN ROBOTICS
MECHANICS, PLANNING, AND CONTROL
Exercise Solutions
September 9, 2017
2
Go to the table of contents.
Contents
Chapter 2 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
5
Chapter 3 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
27
Chapter 4 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
59
Chapter 5 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
73
Chapter 6 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
99
Chapter 7 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
107
Chapter 8 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
121
Chapter 9 Solutions . . . . . . . . . . . . . . . . . . . . . . . . . .
129
Chapter 10 Solutions . . . . . . . . . . . . . . . . . . . . . . . . .
135
Chapter 11 Solutions . . . . . . . . . . . . . . . . . . . . . . . . .
139
Chapter 12 Solutions . . . . . . . . . . . . . . . . . . . . . . . . .
143
Chapter 13 Solutions . . . . . . . . . . . . . . . . . . . . . . . . .
153
3
4
Go to the table of contents.
Chapter 2 Solutions
Exercise 2.1.
The first point placed has n degrees of freedom, the next one has one constraint so n − 1 degrees of freedom,
the next has two constraints, etc. So n + (n − 1) + (n − 2) + . . . 1 = n(n + 1)/2. (Get this by summing the
outermost pair in the sequence, n + 1 = n + 1, then the pair (n − 1) + 2 = n + 1, etc., and observe that
there are n/2 such pairs.) n of these freedoms are the linear freedoms of placing the first point; the other
n(n − 1)/2 are rotational freedoms. After choosing the first point, the next point is on the sphere S n−1 , the
next is on S n−2 , etc., so the topology of the space is Rn × S n−1 × S n−2 × . . . × S 1 .
Exercise 2.2.
(a) The shoulder is a spherical joint (four dof), the elbow has one dof, the wrist has two dof, and between
the elbow and the wrist there is one more dof (rotation of the forearm about the axis of the forearm).
Therefore the arm has seven dof.
(b) Placing the palm at a fixed position and orientation in space puts six constraints on the arm (the six
dof of a rigid body). Keeping the center of the shoulder joint stationary, there is only one dof left: the
arc of a circle on which the tip of the elbow can lie. This is one dof, so the arm must have started with
seven dof before six constraints were placed on it.
Exercise 2.3.
Treat the shoulder as a spherical joint (three dof) between the torso and the upper arm bone (humerus),
and assume the carpal bones just beyond the wrist joint form a rigid body. Then the closed-chain linkage
of the forearm between the humerus and the carpal bones, which includes only the radius and the ulna as
links, must have four dof, since our solution in the previous exercise tells us that the arm has seven dof.
We know that each of the radius and the ulna must have at least one joint at the proximal (closer to the torso)
and distal (closer to the hand) ends of forearm, so there are at least four joints between the humerus and
carpal bones. There could be as many as six: three at the elbow (humeroradial, humeroulnar, and proximal
radioulnar) and three at the wrist (radiocarpal, ulnocarpal, and distal radioulnar). Without knowing more
about the anatomy of the arm, we cannot say for sure.
If we assume the maximum number of joints, six, in the forearm closed chain, then the arm has J = 7 joints
(the three-dof S joint at the shoulder and the six forearm joints mentioned above) and N = 5 links (the
torso “ground,” the humerus, the ulna, the radius, and the carpal bones). By Grübler’s formula,
7 = 6(N − 1 − J) +
7
X
i=1
fi = −18 + freedoms of the six forearm joints.
Therefore the six forearm joints must have a total of 25 freedoms. These joints, averaging more than four
freedoms each, are not standard joints we have studied. They are stabilized by a complex of ligaments joining
the bones.
If we assume the minimum number of joints, four, in the forearm closed chain, then the arm has J = 5 joints
and N = 5 links. By Grübler’s formula,
7 = 6(N − 1 − J) +
5
X
i=1
fi = −6 + 3 + freedoms of the four forearm joints.
Therefore there must be a total of 10 freedoms at the four forearm joints. These could potentially be joints
we have studied, such as two universal joints at the elbow (four dof) and two spherical joints at the wrist
(six dof).
The problem is to show correct general reasoning, not to demonstrate a detailed understanding of arm
anatomy!
Exercise 2.4.
Once the hands firmly grip the steering wheel, each arm has n − 6 dof if the wheel is stationary. The mobility
5
6
Chapter 2. Chapter 2 Solutions
of the wheel adds one dof, though, so the total number of degrees of freedom is 2n − 11.
Exercise 2.5.
X
N
=
6 (links) + 1 (ground) = 7
J
=
5 (R joints) + 2 (S joints) = 7
fi
=
5 × 1 + 2 × 3 = 11.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 5.
Exercise 2.6.
(a) The wheeled mobile base can be regarded as a rolling coin with C-space R2 × T 2 . The C-space of a 6R
robot arm can be written S 1 × S 1 × S 1 × S 1 × S 1 × S 1 = T 6 . The C-space of the wheeled mobile arm
therefore can be written R2 × T 2 × T 6 = R2 × T 8 .
(b) For this problem, the last link of the 6R robot can be regarded as connected to ground by a revolute
joint:
X
N
=
6 (links) + 1 (ground) = 7
J
=
7 (R joints)
fi
=
7 × 1 = 7.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 1.
(c) The second identical 6R robot is grasping the last link (i.e., the refrigerator door) of the original 6R
robot. In this case,
X
N
=
11 (links) + 1 (ground) = 12
J
=
13 (R joints)
fi
=
13 × 1 = 13.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 1.
Exercise 2.7.
(a)
X
N
=
6 (links) + 1 (object) + 1 (ground) = 8
J
=
3 (R joints) + 6 (S joints) = 9
fi
=
3 × 1 + 6 × 3 = 21.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 9.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
7
(b) Consider open chain arms as 7-dof joint connecting object and ground. Then,
X
N
J
= 1 (object) + 1 (ground) = 2
= n (open chain arm)
fi
= n × 7 = 7n.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = (n + 6).
(c) Each of the n 7-dof open chains is replaced by the 6-dof open chains. So,
X
N
=
1 (object) + 1 (ground) = 2
J
=
n (open chain arm)
fi
=
n × 6 = 6n.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 6.
Exercise 2.8.
Set the degrees of freedom of each open chain leg to α. Then
N
=
J
= n (open chain arms)
fi
= n × α = αn.
X
1 (object) + 1 (ground) = 2
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 6 + (α − 6)n = 6.
Therefore, the total degrees of freedom is six regardless of the number of open chain legs.
Exercise 2.9.
(a) Consider the combination of revolute (R) and prismatic (P) joint as a 2-dof cylindrical (C) joint. Then
X
N
=
7 (links) + 1 (ground) = 8
J
=
7 (R joints) + 1 (P joints) + 2 (C joints) = 10
fi
=
7 × 1 + 1 × 1 + 2 × 2 = 12.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 3.
(b) Considering all the R and P joints separately,
X
N
=
13 (links) + 1 (ground) = 14
J
=
16 (R joints) + 2 (P joints) = 18
fi
=
16 × 1 + 2 × 1 = 18.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 3.
Go to the table of contents.
8
Chapter 2. Chapter 2 Solutions
(c) A fork joint is kinematically equivalent to a C joint, so that
X
N
=
7 (links) + 1 (ground) = 8
J
=
6 (R joints) + 2 (P joints) + 1 (C joint) = 9
fi
=
6 × 1 + 2 × 1 + 1 × 2 = 10.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 4.
(d)
X
N
=
5 (links) + 1 (ground) = 6
J
=
6 (R joints) + 1 (P joints) = 7
fi
=
6 × 1 + 1 × 1 = 7.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 1.
(e)
X
N
=
13 (links) + 1 (ground) = 14
J
=
14 (R joints) + 4 (P joints) = 18
fi
=
14 × 1 + 4 × 1 = 18.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 3.
(f)
X
N
=
6 (links) + 1 (ground) = 7
J
=
8 (R joints) + 1 (P joints) = 9
fi
=
8 × 1 + 1 × 1 = 9.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 0.
Exercise 2.10.
(a)
X
N
=
5 (links) + 1 (ground) = 6
J
=
7 (R joints)
fi
=
7 × 1 = 7.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 1.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
9
(b)
X
N
=
5 (links) + 1 (ground) = 6
J
=
2 (R joints) + 4 (P joints) = 6
fi
=
2 × 1 + 4 × 1 = 6.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 3.
(c)
X
N
=
2 (sliding links) + 11 (rod links) + 1 (ground) = 14
J
=
2 (P joints) + 10 (R joints) + 6 (overlapping R joints) = 18
fi
=
2 × 1 + 10 × 1 + 6 × 1 = 18.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 3.
(d)
X
N
=
20 (links) + 1 (ground) = 21
J
=
9 (P joints) + 8 (R joints) + 10 (overlapping R joints) = 27
fi
=
9 × 1 + 8 × 1 + 10 × 1 = 27.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 6.
Exercise 2.11.
(a)
X
N
J
=
=
5 (links) + 1 (ground) = 6
6 (U joints)
fi
=
6 × 2 = 12.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 6.
(b)
X
N
=
6 (rods) + 1 (plate) + 1 (ground) = 8
J
=
3 (P joints) + 3 (U joints) + 3 (S joints) = 9
fi
=
3 × 1 + 3 × 2 + 3 × 3 = 18.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 6.
Go to the table of contents.
10
Chapter 2. Chapter 2 Solutions
(c)
X
N
=
5 (rods) + 1 (plate) + 1 (ground) = 7
J
=
2 (P joints) + 3 (U joints) + 3 (S joints) = 8
fi
=
2 × 1 + 3 × 2 + 3 × 3 = 17.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 5.
(d)
X
N
=
7 (links) + 1 (ground) = 8
J
=
3 (P joints) + 6 (U joints) = 9
fi
=
3 × 1 + 6 × 2 = 15.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 3.
(e)
X
N
=
7 (links) + 1 (ground) = 8
J
=
2 (R joints) + 4 (U joints) + 2 (P joints) = 8
fi
=
2 × 1 + 4 × 2 + 2 × 1 = 12.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 6.
(f)
X
N
=
3 (rods) + 1 (plate) + 1 (ground) = 5
J
=
3 (3 dof joints) + 3 (S joints) = 6
fi
=
3 × 3 + 3 × 3 = 18.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 6.
Exercise 2.12.
(a)
X
N
=
7 (links) + 1 (ground=legs) = 8
J
=
3 (R joints) + 6 (U joints) = 9
fi
=
3 × 1 + 6 × 2 = 15.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 3.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
11
(b)
X
N
=
8 (links) + 1 (ground) = 9
J
=
4 (R joints) + 5 (P joints) = 9
fi
=
4 × 1 + 5 × 1 = 9.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 3.
(c)
X
N
=
12 (links) + 1 (plate) + 1 (ground) = 14
J
=
6 (R joints) + 6 (U joints) + 6 (S joints) = 18
fi
=
6 × 1 + 6 × 2 + 6 × 3 = 36.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 6.
(d) The spatial parallel mechanism consists of four RFRRPR serial subchains, where F is a four-bar
parallelogram linkage. Each serial subchain with RFRRPR joints can be regarded as ground with a
single 6-dof joint:
dof = 1 (four-bar parallelogram linkage) + 1 × 4 (R joints) + 1 (P joint) = 6.
Now apply Grübler’s formula to the 4−RFRRPR mechanism:
X
N
=
1 (plate) + 1 (ground) = 2
J
=
4 (RFRRPR joints)
fi
=
4 × 6 = 24.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 6.
Exercise 2.13.
X
N
=
6 (legs) + 1 (upper platform) + 1 (ground) = 8
J
=
12 (S joints)
fi
=
12 × 3 = 36.
Substituting the above values into the spatial version of Grübler’s formula,
dof = 6(N − 1 − J) +
X
fi = 6.
The upper platform can simultaneously translate and rotate about the vertical axis, and also translate
horizontally.
Exercise 2.14.
Go to the table of contents.
12
Chapter 2. Chapter 2 Solutions
(a)
X
N
=
6 (links) + 1 (upper platform) + 1 (ground) = 8
J
=
3 (P joints) + 6 (U joints) = 9
fi
=
3 × 1 + 6 × 2 = 15.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 3.
(b) If the three P joints are locked, the robot loses three degrees of freedom and thus should become a
structure, but clearly the robot can move.
Exercise 2.15.
(a)
X
N
=
5 (squares) + 1 (ground=square) = 6
J
=
6 (R joints)
fi
=
6 × 1 = 6.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 3.
(b)
X
N
=
5 (squares) + 1 (ground=square) = 6
J
=
6 (R joints)
fi
=
6 × 1 = 6.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 0.
However, under the assumption that all the squares are of the same size, the mechanism can move
with 1 dof. Grübler’s formula is unable to distinguish such cases.
Exercise 2.16.
(a) Since all the links are constrained to move on the surface of a sphere, the planar version (or more
accurately, the two-dimensional version) of Grübler’s formula must be used. In this case,
N
=
3 (links) + 1 (ground) = 4
J
=
4 (R joints)
fi
=
4 × 1 = 4.
dof
=
3(N − 1 − J) +
X
X
fi = 1.
If we had used the spatial (three-dimensional) version of Grübler’s formula, we would obtain the result
that dof = −2, implying the mechanism is incapable of motion.
(b) Since the mechanism has one dof and is constructed of four revolute joints, its C-space is a curve in
the four-dimensional torus T 4 . Depending on the relative lengths of the links, the curve may be closed,
and also have self-intersections.
(c) The workspace is a curve on the sphere as shown in Figure 2.1. At each point on the curve, the
orientation of the frame is fixed.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
13
robot links
robot joints
reference frame work space
Figure 2.1
Exercise 2.17.
(a) The surgical tool can move freely in the base hole, so there’s no joint constraint between the end-effector
and the base. In this case,
X
N
=
3 (3 links for leg A) + 8 (4 links for each leg B and C) + 2 (end-effector and base) = 13
J
=
4 (3 R and 1 P joints for leg A) + 10 (4 R and 1 U joints for each leg B and C) = 14
fi
=
4 (leg A) + 2 × 6 (leg B and C) = 16.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 4.
(b) The constraint that the surgical tool must pass through point A is equivalent to connecting the tool
with a four-dof spherical-prismatic pair: the spherical joint determines the tool orientation, while the
prismatic joint determines the displacement along the tool axial direction. Therefore
X
N
=
3 + 8 + 2 (end-effector and base) = 13
J
=
4 + 10 + 1 (SP joint between end-effector and base) = 15
fi
=
4 + 2 × 6 + 4 = 20.
Go to the table of contents.
14
Chapter 2. Chapter 2 Solutions
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 2.
(c) Since the axes of all the revolute joints pass through point A, all the links are constrained to move on
the two-dimensional sphere and the tool always passes through point A (Refer to exercise 2.16). In
this case, the two-dimensional version of Grübler’s formula must be used:
X
N
=
9 (3 links for each leg) + 2 (end-effector and base) = 11
J
=
12 (4 R joints for each leg)
fi
=
3 × 4 = 12.
Substituting the above values into the planar version of Grübler’s formula,
X
dof = 3(N − 1 − J) +
fi = 6.
Exercise 2.18.
X
N
=
6 (links) + 1 (moving platform) + 1 (ground) = 8
J
=
6 (P joints) + 3 (U joints) = 9
fi
=
6 × 1 + 3 × 2 = 12.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 0.
However, this mechanism can move: if the three P joints on the fixed base move identically, the moving
platform will move vertically. Therefore, it contradicts the fact that the mechanism has zero degrees of
freedom as calculated by Grübler’s formula.
Exercise 2.19.
There are N = 7 links (ground, torso, two upper arms, two lower arms, and one combined hand-object-hand
link) and J = 8 joints (three R, four S, and a joint between the box and the table that has three sliding
freedoms, two translational and one rotational). By Grübler’s formula,
dof = 6(7 − 1 − 8) +
8
X
i=1
fi = −12 + 3(1) + 4(3) + 3 = 6.
Exercise 2.20.
(a) Referring to the Figure 2.2, when the body is fixed (ground) there are four rigid wings, four rigid
legs, four linkages consisting of two links each, and the body (ground), so N = 17. There are four
wing R joints, four leg R joints, four leg S joints, four leg P joints, and four leg U joints, so P
J = 20.
The freedoms of the joints are 1 for the R and P joints, two for the U, three for the U, so i fi =
12(1) + 4(2) + 4(3) = 32. Grübler’s formula gives 6(17 − 1 − 20) + 32 = 8 dof.
(b) Add six dof for the chassis to get 14 dof.
(c) Keeping a foot at a fixed location adds 3 constraints on that foot, or 12 constraints total, so subtract
12 from 14 (the answer to part (b)) to get 2 dof. Alternatively, by Grübler, add four more S joints
at the feet (with 3 dof each) and one more link (ground) compared to the answer in part (a). So
6(18 − 1 − 24) + (32 + 4(3)) = 2 dof. Note that the wings, of course, can still move with 4 dof, so that
means the legs and body only (ignoring the wings) have −2 dof, assuming that none of the constraints
are redundant. This means (1) the body and legs cannot move and (2) we even have two constraints
on where we can position the legs on the ground.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
15
R
S
P
P
U
S
Figure 2.2
Exercise 2.21.
(a) Each of the connections between the body links can be regarded as a three-dof RPR joint. In this case
X
N
=
7 (1 head, 6 body links) + 1 (tail=ground) = 8
J
=
5 (RPR joints between body links) + 2 (R joint between head-body, tail-body) = 7
fi
=
5 × 3 + 2 × 1 = 17.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 17.
(b) Each of the contacts between the body links and the ground can be modelled as a five-dof joint (sliding
along two directions, and rotation about three directions). In this case
X
N
=
8 (1 head, 1 tail, 6 body links) + 1 (ground) = 9
J
=
7 (joints between links) + 6 (contacts between body links and ground) = 13
fi
=
17 + 6 × 5 = 47.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 17.
(c) Only two of the body links are in contact with the ground. In this case
X
N
=
8 (1 head, 1 tail, 6 body links) + 1 (ground) = 9
J
=
7 (joints between links) + 2 (contacts between body links and ground) = 9
fi
=
17 + 2 × 5 = 27.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 21.
Go to the table of contents.
16
Chapter 2. Chapter 2 Solutions
Exercise 2.22.
(a) The palm has six dof and each of the four fingers has four dof, so the hand has 22 dof total. When
one finger is in contact with the table, there is one constraint on its position (the equation describing
the height of the finger above the table being equal to zero), so the hand has 21 dof. If n fingers are
in contact, the hand has 22 − n dof.
(b) 26 − n dof.
(c) Model the finger contacts as spherical joints, so N = 14 (12 finger links, the ellipsoid and the palm
ground) and J = 16, which includes four U joints, eight R joints, and four S joints (at the finger
contacts). By Grübler, dof = 6(14 − 1 − 16) + 4(2) + 8(1) + 4(3) = 10.
(d) This question is to see how the student thinks about rolling constraints. A sphere rolling on a surface
can achieve any configuration in contact with the surface; the rolling, no-slip nonholonomic velocity
constraints do not create any configuration constraint other than that the sphere must remain in
contact with the surface. Therefore each finger contact “joint” has five degrees of freedom. Building
on (c) above, dof = −18 + 4(2) + 8(1) + 4(5) = 28.
Exercise 2.23.
Define the joint variables as shown in Figure 2.3 and let the length of the links be L. The positions of the
(x, y)

(x , y)

(x , y)

Figure 2.3
centers of each link are denoted (x3 , y3 ),(x2 , y2 ), (x1 , y1 ), respectively (starting from the left in Figure 2.3).
Define the eight-dimensional vector x = (θ1 , θ2 , x1 , y1 , x2 , y2 , x3 , y3 ). The constraint equations gi (x) = 0 of
the joints are derived as follows:
g1 (x)
g2 (x)
g3 (x)
g4 (x)
g5 (x)
g6 (x)
= x1 − L cos θ1 + L cos θ2
= y1 − L sin θ1 − L sin θ2
= x2 − L cos θ1 + L/2 cos θ2
= y2 − L sin θ1 − L/2 sin θ2
= x3 − L/2 cos θ1
= y3 − L/2 sin θ1
Additionally, there is one more constraint on the slider g7 (x) = y1 = 0. Therefore, there are 7 constraint
equations and 8 configuration variables in total. The feasible configuration space of the joint variables can
now be determined as
C = {x = (θ1 , θ2 , x1 , y1 , x2 , y2 , x3 , y3 ) |gi (x) = 0 (i = 1, · · · , 7)}.
The configuration space C projected to the space of joint variables (θ1 , θ2 , x1 ) is given by
Cp = {(θ1 , θ2 , x1 ) |θ1 = θ2 , x1 = L(cos θ1 + cos θ2 )}.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
17
Exercise 2.24.
(a) The four-bar linkage is floating in space, so the number of links is 4 since the ground and linkage are
not connected. 4 joints connect the adjacent links of the floating linkage. Finally, m in Grübler’s
formula is set to 6 since it is a spatial mechanism. We therefore have
X
N
=
4 (4 links) + 1 (ground) = 5
J
=
4 (R joints between links)
fi
=
4.
Substituting the above values into the spatial version of Grübler’s formula,
X
dof = 6(N − 1 − J) +
fi = 4.
However, the actual dof of the floating system differs from that predicted by Grübler’s formula: any
single floating system has at least 6 dof. This mismatch will be discussed further in (b).
(b) The degrees of freedom can be calculated by subtracting the number of constraints from the number
of variables. First of all, we already know that there are three coordinates for each point and three
points on each link. Thus, the total number of variables is 36. The planar four-bar linkage has three
types of constraints: rigid body, revolute joint, and planar motion. For rigid body constraints, twelve
constraints should be considered. For link 1,
p
(xA − xB )2 + (yA − yB )2 + (zA − zB )2 = const.
kpA − pB k = const. ⇔
p
kpB − pC k = const. ⇔
(xB − xC )2 + (yB − yC )2 + (zB − zC )2 = const.
p
kpC − pA k = const. ⇔
(xC − xA )2 + (yC − yA )2 + (zC − zA )2 = const.
Similarly, constraints on links 2, 3, and 4 can be expressed as above. The four pairs of points are
connected by a revolute joint: C with D, F with G, I with J, and L with A. These constraints can
be written as follows:
pC = pD ⇔
(xC , yC , zC ) = (xD , yD , zD ) ⇔ xC = xD , yC = yD , zC = zD
pI = pJ ⇔
(xI , yI , zI ) = (xJ , yJ , zJ )
pF = pG ⇔
pL = pA ⇔
(xF , yF , zF ) = (xG , yG , zG )
(xL , yL , zL ) = (xA , yA , zA )
⇔ xF = xG , yF = yG , zF = zG
⇔ xI = xJ , yI = yJ , zI = zJ
⇔ xL = xA , yL = yA , zL = zA .
Finally, only planar motions are admissible: all of the points on the linkage lie on a plane with normal
−→ −→
AJ
vector ~n = AC×
−→ −→ , and
kAC·AJk
−→
AF · ~n
−−→
AB · ~n
−→
AE · ~n
−−→
AH · ~n
−−→
AK · ~n
=
0
=
0
=
0
=
0
= 0
−→
The first constraint AF · ~n = 0 is to prevent folding between planes DF I and ACJ. The total number
of constraints is 29. Therefore, the degrees of freedom of the system is equal to 36 − 29 = 7 dof. The
reason why this differs from the result obtained from Grübler’s formula is that the planar four-bar
linkage floating in 3-D space undergoes 2-D motion. The spatial version of Grübler’s formula should
not be applied to planar motion in 3D space.
Exercise 2.25.
Go to the table of contents.
18
Chapter 2. Chapter 2 Solutions
h
A
a
B
b
c
ŷ
φ
η
a sinθ
C
θ
ψ
x̂
D
g a cosθ
Figure 2.4
(a) Draw a line between joint A and joint D, and set the length of this line as c. In addition, set the angle
between BD and AD as η (see Figure 2.4). The following equation can then be derived:
a2 + g 2 − 2ag cos θ
b2 + c2 − 2bc cos η
=
c2
=
h2 .
From the above, γ 2 and α2 + β 2 can be calculated as follows:
h2 − g 2 − b2 − a2 + 2ag cos θ
2
=
∴ γ2
=
4b2 c2 cos2 η
α2 + β 2
=
∴ α2 + β 2
=
(2gb − 2ab cos θ) + (−2ab sin θ) = 4b2 g 2 + a2 − 2ag cos θ
2
= h2 − b2 − c2
2
γ2
2
= (−2bc cos η)
2
4b2 c2
γ 2 = 4b2 c2 cos2 η ≤ 4b2 c2 = α2 + β 2 .
If the constraint γ 2 ≤ α2 + β 2 is not satisfied, then cos η > 1, which implies that the four-bar linkage
is unable to reach a desired output angle φ. The maximum value ψmax is determined by the structure,
and Figure 2.5 illustrates such a scenario.
ψmax
Figure 2.5
(b) Expressing φ in terms of a, b, g and θ,
φ
=
∴φ
=
−2ab sin θ
β
= tan−1
α
2gb − 2ab cos θ
a sin θ
tan−1 −
.
g − a cos θ
tan−1
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
19
Therefore, φ represents the angle between CD and AD (see Figure 2.4). The relation between φ, ψ
and η can be obtained from
cos(ψ − φ)
ψ−φ
−2bc cos η
γ
p
= − cos η
= √
2
2
4b2 c2
α +β
= ±η
=
∴ ψ = φ ± η.
The two possible values of the output angle ψ represent the elbow-up and elbow-down configurations
with respect to AD (Figure 2.6).
Elbow-up
φ
η
η
Elbow-down
Figure 2.6
(c) By substituting the expressions for a, b, g, h into the equations for α, β, γ, the relation between θ and
ψ can be derived as shown in Figure 2.7 (obtained via MATLAB).
7
elbow-up
elbow-down
6
psi (rad)
5
4
3
2
1
0
0
1
2
3
4
theta (rad)
5
Figure 2.7
(d) Same as 2.25(c) (Figure 2.8).
Go to the table of contents.
6
7
20
Chapter 2. Chapter 2 Solutions
5.5
elbow-up
elbow-down
5
4.5
psi (rad)
4
3.5
3
2.5
2
1.5
1
0
1
2
3
4
theta (rad)
5
6
7
Figure 2.8
(e) Same as 2.25(c) (Figure 2.9).
5
elbow-up
elbow-down
4.5
psi (rad)
4
3.5
3
2.5
2
1.5
0
1
2
3
4
theta (rad)
5
6
7
Figure 2.9
Exercise 2.26.
(a) The configuration space is the space of the variables x1 , y1 , θ1 , x2 , y2 , θ2 where (xi , yi ) denotes the
center of mass of the i-th link and θi denotes the orientation of the link. The constraint equations and
corresponding feasible configuration space are given by
C
= {q = (x1 , y1 , θ1 , x2 , y2 , θ2 ) | gi (q) = 0 (i = 1, · · · , 4),
where
g1 (x)
g2 (x)
= x1 − cos θ1
= y1 − sin θ1
1
cos(θ1 + θ2 ))
2
1
g4 (x) = y2 − (2 sin θ1 + sin(θ1 + θ2 )).
2
g3 (x)
= x2 − (2 cos θ1 +
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
21
(b) The workspace W is the set of all points reachable by the tip as shown in Figure 2.10 (left):
barrier
b
b
a
a
Not Reachable
Figure 2.10
W = {(x, y) |x = 2 cos θ1 + cos(θ1 + θ2 ), y = 2 sin θ1 + sin(θ1 + θ2 ), θi ≤ 2π}.
Corresponding to the given constraint equations, let a = 2, b = 1 in Figure 2.10.
(c) Vertical barrier at x = ±1: exclude the space such that |x| > 1 from the original free C-space, whose
area is (a + b)2 π − (a − b)2 π. Substituting a = 2, b = 1, the area to be excluded is
4
Z a+b p
a−b
(a + b)2 − x2 dx.
R a+b p
Thus, the left area is now ((a + b)2 π − (a − b)2 π) − 4 a−b (a + b)2 − x2 dx = 8.6323. The area
indicated as “Not Reachable” in Figure 2.10 can be calculated as follows:
√
3 π
(triangle − fan shapes) =
− .
2
4
As a result we have
√
8.6323 − 4( 23 − π4 )
restricted area
r=
=
= 0.3306.
original area
8π
Exercise 2.27.
(a) Let the revolute joint variables be θ1 , θ2 , θ3 . Similar to Figure 2.10, the donut-shaped workspace is
now moving along the circle of radius a = 5. We can derive an analytic expression for the coordinates
of the tip (x, y):
x
=
5 cos θ1 + 2 cos(θ1 + θ2 ) + cos(θ1 + θ2 + θ3 )
y
=
5 sin θ1 + 2 sin(θ1 + θ2 ) + sin(θ1 + θ2 + θ3 ).
(2.1)
The admissible region of (x, y) is given in Figure 2.11; it is donut-shaped with radii 2 and 8, with total
area 64π − 4π = 60π.
(b) Setting a = 1, b = 2, c = 5 in Figure 2.11, it can be seen that the lengths are the reverse of (a). Thus,
intuitively one can conjecture that answer is 60π, the same as obtained for (a). To prove this, express
Go to the table of contents.
22
Chapter 2. Chapter 2 Solutions
c
a
b
Figure 2.11
the tip coordinates (x, y) as
x
=
cos θ1 + 2 cos(θ1 + θ2 ) + 5 cos(θ1 + θ2 + θ3 )
y
=
sin θ1 + 2 sin(θ1 + θ2 ) + 5 sin(θ1 + θ2 + θ3 ).
These equations for the tip coordinates can be transformed into the same form as (a):
x =
5 cos α1 + 2 cos(α1 + α2 ) + cos(α1 + α2 + α3 )
y
5 sin α1 + 2 sin(α1 + α2 ) + sin(α1 + α2 + α3 ),
=
(2.2)
where α1 = θ1 + θ2 + θ3 , α2 = θ1 + θ2 , α3 = θ1 . These αi range from 0 to 2π, as do the θi . Since the
equations of the tip coordinates of Equations (2.1) and (2.2) are the same, the area of (b) is also 60π
as conjectured.
(c) For problems like this, it is useful to begin with the simplest case (i.e., the two-link planar open chain)
and to work your way up to higher degrees of freedom. For the 2R planar chain, based on the above
we know that the Cartesian positioning workspace will be an annulus: if the links have length L1 and
L2 and L1 > L2 , then this annulus will have inner radius L1 − L2 and outer radius L1 + L2 , and its
area is given by 4πL1 L2 . Clearly increasing the link lengths does enlarge the Cartesian positioning
workspace area, so the designer’s claim is not entirely incorrect (although one could easily argue that
the shape of the workspace—possessing a large hole close to the base—would not be generally useful).
Now consider the 3R planar open chain. Assume that the length of the last link is L, which is longer
than 5, and the first two links are of lengths 1 and 2 as before. Note that the workspace is independent
of the order of the lengths as previously proven. Thus, the equations for the tip coordinates can be
derived as follows:
x
= L cos α1 + 2 cos(α1 + α2 ) + cos(α1 + α2 + α3 )
y
= L sin α1 + 2 sin(α1 + α2 ) + sin(α1 + α2 + α3 ),
where α1 = θ1 + θ2 + θ3 , α2 = −θ3 , α3 = −θ2 . The workspace is again annular with radii (L − 2 − 1) and
(L + 2 + 1). Further calculation reveals that the area of the workspace is π((L + 3)2 − (L − 3)2 ) = 12Lπ.
This linearity can be preserved for arbitrary lengths a, b, c as indicated in Figure 2.11. Here again the
Cartesian positioning workspace can be enlarged with increased L, so the designer’s claim does not
appear to be incorrect. For planar open chains with increasing degrees of freedom, we can expect the
situation to be more or less the same.
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
23
However, the notion of workspace volume above is based only on the Cartesian positioning workspace.
If one were to take into account the orientation workspace—that is, at any given point in the Cartesian
positioning workspace, what is the range of possible orientations achievable by the tip?—then the
situation becomes different. The analysis is quite involved and we won’t get into it here—some relevant
references are “The workspace of a mechanical manipulator,” A. Kumar and K. Waldron, ASME J.
Mechanical Design, vol. 103, 1981, and ”Optimal kinematic design of 6R manipulators,” B. Paden and
S. Sastry, Int. J. Robotics Research, vol. 7, 1988—but if both the position and orientation workspace
are simultaneously considered in the notion of a robot’s workspace, then it can be shown that varying
the length of the last link does not change this more general notion of a robot’s workspace volume. As
an analogy, for the 3R planar open chain, at each point in the Cartesian positioning workspace there
will be a range of orientations achievable by the tip (some interval on [0, 2π]); imagine integrating this
orientation range over the entire positioning workspace, and using this integral as the total workspace
volume.
Exercise 2.28.
The task space and workspace are not the same concept. The task space is the space of configurations
as specified by the task itself and independent of the robot. On the other hand, the workspace is the
configuration space of the end-effector that the robot can reach, which is primarily determined by the
robot’s structure and independent of the task.
(a) Writing on a blackboard:
Focusing on what is actually written on the board, the task of drawing is determined by the position
of the chalk but not the orientation. Keeping contact with the blackboard, the dimension of the chalk
is two (i.e., (x, y) ∈ R2 ). The task space is therefore R2 .
(b) Twirling a baton:
A rigid body has 6 degrees of freedom. Note however that rotation about the central axis of the baton
does not change its appearance, so that the task space can be considered to be S2 × R3 . Observe that
flipping the baton also does not change the baton’s appearance, so that the orientation could actually
be a half-sphere, or the real projective space RP2 .
Exercise 2.29.
(a) R2 × S 1 .
(b) S 2 × S 1 (the chassis position on the sphere and the chassis heading direction).
(c) R2 × S 1 × T 3 × [a, b] = R2 × T 4 × [a, b].
(d) R3 × S 2 × S 1 × T 6 = R3 × S 2 × T 7 .
Exercise 2.30.
To achieve the desired (x, y) position, change the heading φ to point at the goal location. Then roll there.
Then change the rolling angle θ by driving the coin around a circle so that the contact point with the plane
traces a circle of radius R back to the starting (x, y) position. To change the rolling angle by an amount ∆θ,
R should satisfy R = ∆θr/(2π), where r is the radius of the coin. Once the proper (x, y, θ) is achieved, the
coin can be rotated to the desired heading angle φ.
Exercise 2.31.
(a) Let φ1 be the rotation angle of the left wheel and φ2 be the rotation angle of the right wheel. Let
R1 = r and R2 = r be the steering radii of the left and right wheel respectively. Furthermore, assume
that the distance between the wheels is 2d. Based on this we have the following equations:
φ̇1 = ω1
φ̇2 = ω2
Go to the table of contents.
24
Chapter 2. Chapter 2 Solutions
ẋ = (1/2)(ω1 + ω2 )r cos θ
ẏ = (1/2)(ω1 + ω2 )r sin θ
θ̇ = (r/2d)(ω2 − ω1 )
In vector form we have the equality:




(1/2)r cos θ
(1/2)r cos θ
 (1/2)r sin θ 
 (1/2)r sin θ 







q̇ =  −(r/2d)  ω1 + 
 (r/2d)  ω2




0
1
1
0
(b) The Pfaffian constraints are dependent on the following differential equations:
ẋ − (1/2)φ̇1 r cos θ − (1/2)φ̇2 r cos θ = 0
ẏ − (1/2)φ̇1 r sin θ − (1/2)φ̇2 r sin θ = 0

1 0 0
=⇒ A(q)q̇ = 0 1 0
0 0 1
θ̇ + (r/2d)φ̇1 − (r/2d)φ̇2 = 0
 
ẋ


−(1/2)r cos θ −(1/2)r cos θ 
 ẏ 


−(1/2)r sin θ −(1/2)r sin θ  θ̇ 
=0
φ̇1 
(r/2d)
−(r/2d)
φ̇2
(c) The constraint θ̇ + (r/2d)φ̇1 − (r/2d)φ̇2 = 0 is holonomic (integrable) and the other two constraints
are nonholonomic.
Exercise 2.32.
(a) Transform the constraint into Pfaffian form:
A(q)q̇
where
=0
= 1 + cos q1
A(q)
1 + cos q2
cos q1 + cos q2 + 4
0 .
To check whether A(q) is integrable, assume A(q) = ∂g(q)
∂q :
∂g(q)
=
∂q1
∂g(q)
=
∂q2
∂g(q)
=
∂q3
∂g(q)
=
∂q4
1 + cos q1
→ g(q) = q1 + sin q1 + h(q2 , q3 , q4 )
(2.3)
1 + cos q2
→ g(q) = q1 + sin q1 + q2 + sin q2 + h(q3 , q4 )
(2.4)
cos q1 + cos q2 + 4
→
∂h(q3 , q4 )
= cos q1 + cos q2 + 4
∂q3
0.
(2.5)
(2.6)
A contradiction occurs in Equation (2.5) since h(q3 , q4 ) does not incorporate any q1 or q2 term. Therefore, A(q) is not integrable and the velocity constraint is nonholonomic.
(b) Transform the constraint into Pfaffian form:
A(q)q̇
where
A(q)
=0
− cos q2
=
0
0 sin(q1 + q2 ) − cos(q1 + q2 )
.
0
sin q1
− cos q1
Go to the table of contents.
Chapter 2. Chapter 2 Solutions
25
To check whether A(q) is integrable, assume A(q) = ∂g(q)
∂q :
∂g1 (q)
=
− cos q2
→ g1 (q) = −q1 cos q2 + h(q2 , q3 , q4 )
∂q1
h(q2 , q3 , q4 )
∂g1 (q)
=
0
→
= −q1 sin q2
∂q2
∂q2
∂g1 (q)
=
sin(q1 + q2 )
∂q3
∂g1 (q)
= − cos(q1 + q2 )
∂q4
(2.7)
(2.8)
(2.9)
(2.10)
A contradiction occurs in Equation (2.8) since h(q2 , q3 , q4 ) does not incorporate any q1 terms. Thus,
there exists no admissible g1 (q). To check g2 (q),
∂g2 (q)
=
0
∂q1
∂g2 (q)
=
0
∂q2
∂g2 (q)
=
sin q1
∂q3
∂g2 (q)
= − cos q1
∂q4
→ g2 (q) = h(q2 , q3 , q4 )
(2.11)
→ g2 (q) = h(q3 , q4 )
(2.12)
→
∂h(q3 , q4 )
= sin q1
∂q3
(2.13)
(2.14)
A contradiction occurs in Equation (2.13) since h(q3 , q4 ) does not incorporate any q1 terms. In conclusion, there exists no g(q) satisfying A(q) = ∂g(q)
∂q and thus the given constraint is nonholonomic.
Go to the table of contents.
26
Chapter 2. Chapter 2 Solutions
Go to the table of contents.
Chapter 3 Solutions
Exercise 3.1.
(a) The three frames are:
{'}
{+}
%#"
{-}
$*)
%*
)
$#"
!
*)
$*,
!
*,
%*,
!#"
Figure 3.1
(b) The rotation matrices are:

0
Rsa = 0
1
−1
0
0

0
−1
0

1
Rsb = 0
0
0
0
−1

0
1
0
(c)

1 0
−1
T
Rsb
= Rbs = Rsb
= 0 0
0 1

0
−1
0
(d)

0
T
Rab = Ras Rsb = Rsa
Rsb = −1
0
0
0
−1

1 1
0 0
0 0
0
0
−1
 
0
0
1 = −1
0
0

0
R1 = Rsa R = Rsa Rsb = 0
1
−1
0
0

0
1
−1 0
0
0
0
0
−1
 
0
0 0
1 = 0 1
0
1 0
−1
0
0

0
0
−1
(e)

−1
0
0
R1 corresponds to rotating Rsa by −90◦ about the body-fixed x̂a axis.

1
R2 = RRsa = Rsb Rsa = 0
0
0
0
−1

0
0
−1 0
0
1
−1
0
0
 
0
0
−1 = 1
0
0
R2 corresponds to rotating Rsa by −90◦ about the world-fixed x̂s axis.
(f) ps = Rsb pb = (1, 3, −2)T .
27
−1
0
0

0
0
1
28
Chapter 3. Chapter 3 Solutions
(g)

   
1 0 0 1
1
p0 = Rsb ps = 0 0 1 2 =  3  =⇒ location transformation
0 −1 0 3
−2

   
1 0 0
1
1
T
p00 = Rsb
ps = 0 0 −1 2 = −3 =⇒ coordinate change
0 1 0
3
2
(h)

0
Ras ωs = ωa −1
0
0
0
−1
   
1 3
1
0 2 = −3
0 1
−2
(i)

0
Rsa = 0
1

0
−1
0
−1
0
0
Trace Rsa is equal to 0, so we are in the third condition of the algorithm. Therefore θ = cos−1 (−1/2) =
2π
3 . By definition
√
3
1
T
[ω̂] =
(R̄ − R̄ ) =
(Rsa − Ras )
2 sinθ
3

√ 0 −1 −1
3
1 0 −1
=⇒ [ω̂] =
2
1 1
0
 
√
1
3 
−1 (to make it a unit vector we have to divide the vector by its norm)
=⇒ ω̂ =
3
1
(j)
ω̂θ =
√

 √1 
5
5  √25 
0
0
 0
=⇒ [ω̂] = 
− √25
0
0
√1
5
√2
5
√
− √15 
 , with θ = 5

0
By definition R = e[ω̂]θ = I + sin θ[ω̂] + (1 − cos θ)[ω̂]2 ,

 
 
1 0 0
0
0
0.704
−1.294
0
−0.352 +  0.647
R = 0 1 0 +  0
0 0 1
−0.704 0.352
0
0


−0.2938 0.6469 0.7037
=  0.6469 0.6765 −0.3518
−0.7037 0.3518 −0.6173
0.647
−0.324
0

0

0
−1.6173
Exercise 3.2.
Let point p0 be the new position after rotation, and denote the corresponding rotation matrix as R.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
29
(a) Since the rotation is with respect to the fixed frame,
p0 = Rot(ẑ, −120◦ )Rot(ŷ, 135◦ )Rot(x̂, 30◦ )p


cos(−120◦ ) − sin(−120◦ ) 0
cos 135◦
◦
◦
0
=  sin(−120 ) cos(−120 ) 0  
0
0
1
− sin 135◦


−0.5526
=  0.4571  .
−0.6969
0
1
0

sin 135◦
1
 0
0
cos 135◦
0

0
− sin 30◦  p
cos 30◦
0
cos 30◦
sin 30◦
(b) From (a),

0.3536
R = Rot(ẑ, −120◦ )Rot(ŷ, 135◦ )Rot(x̂, 30◦ ) =  0.6124
−0.7071
0.5732
−0.7392
−0.3536

−0.7392
−0.2803  .
−0.6124
Exercise 3.3.
From Rp = q,
 √
R
2
0
2
√ 
 
√1
−
2
0
0
2
√ 
√

√1
2 
2 2 = 2
√
√2
0
2 − 2
−2
√  √
−1  √2
1
√
− 2
2
1
0
2
√2
√ 
√
2
 0
 =
√1
2
1
2
2


2
√2
2 −1
0
− 2
−2
0
1
1
−1

0

⇔ R= 2
√
2
1
√2
− 22
Exercise 3.4.
Let Rab and Rbc be

r11
Rab =  r21
r31
r12
r22
r32
ŷb


r13
s11
r23  , Rbc =  s21
r33
s31
s12
s22
s32

s13
s23  .
s33
r12
r22
r32

r13
r23 
r33
We then have

x̂b
x̂c
ŷc
ẑb
ẑc
=
x̂a
ŷa
ẑa
=
x̂a
ŷa
ẑa
=
x̂b
ŷb
=
x̂b
ŷb
=
x̂a
ŷa
ẑc
r11
 r21
r31
Rab

s11 s12
ẑb  s21 s22
s31 s32
ẑb Rbc
ẑa Rab Rbc .
Since
x̂c
ŷc
=
x̂a
ŷa
ẑa
we can conclude that
Rac = Rab Rbc .
Go to the table of contents.
Rac ,
− 12

s13
s23 
s33
− 21
1
√2
2
2


.
30
Chapter 3. Chapter 3 Solutions
Exercise
 3.5.

2π
√
 3 √3 
ω̂θ =  − 32π
3 
2π
√
3 3
Exercise 3.6.
R = Rot(x̂, π/2)Rot(ẑ, π) which gives us the matrix:

−1 0
0
R= 0
0 −1

0
−1
0
In order to find R = e[ω̂]θ we have to perform matrix logarithm. The trace of the above matrix is −1, and
thus we are in the first condition. Either the first or the second equation can be used to calculate the unit
vector ω̂ and angle of rotation by definition is θ = π.
 
0
1  
ω̂ = √ −1
2 1
Exercise 3.7.
(a) From tr(R) = −1 = 1 + 2 cos θ it follows that θ = π.
R = I + 2[ω̂]2

1 − 2(ω22 + ω32 )

2ω1 ω2
=
2ω1 ω3


0 0 1
=  0 −1 0  .
1 0 0
ω̂ = ( √12 , 0, √12 )T or (− √12 , 0, − √12 )T .
(b) From the exponential formula

0
sin
θ
 1
e[ω̂]θ = I +
3
−2

2ω1 ω2
2ω1 ω3

1 − 2(ω12 + ω32 )
2ω2 ω3
2
2
2ω2 ω3
1 − 2(ω1 + ω2 )


−1
2
−5
1
−
cos
θ
 4
0 −2  +
9
2
0
2

4
2
−5
2 .
2 −8
Substituting the above equation into v2 = Rv1 yields




−1
2 sin θ − (1 − cosθ)
 1  = 1  − sin θ + 2(1 − cos θ)  ⇔ θ = − π .
3
2
0
−2 sin θ − 2(1 − cos θ)
Exercise 3.8.
(a) Using the fact that ω̂12 + ω̂22 + ω̂32 ,
ω̂12 = r112+1
r11 = 1 − 2(ω̂22 + ω̂32 ) = 1 − 2(1 − ω̂12 ) = 2ω̂12 − 1
2
2
2
2
r22 = 1 − 2(ω̂1 + ω̂3 ) = 1 − 2(1 − ω̂2 ) = 2ω̂2 − 1 ⇒ ω̂22 = r222+1
r33 = 1 − 2(ω̂12 + ω̂22 ) = 1 − 2(1 − ω̂32 ) = 2ω̂32 − 1
ω̂32 = r332+1 .
q
q
q
Hence ω̂1 = ± r112+1 , ω̂2 = ± r222+1 , and ω̂3 = ± r332+1 . Depending on the sign of the off-diagonal
terms in the [ω̂]2 matrix, we can get two combinations of signs of the ω̂ elements. However, the given
solution shows only the magnitude of the elements and is incorrect.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
31
(b)

0
[ω̂](R + I) =  ω̂3
−ω̂2
−ω̂3
0
ω̂1

ω̂2
r11 + 1
−ω̂1   r21
0
r31
r12
r22 + 1
r32

r13
r23  = 0
r33 + 1
r31
r32
r33 + 1
ω̂3
=
=
=
ω̂2
r21
r22 + 1
r23
ω̂3
r31
r32
r33 + 1
=
=
=
ω̂1
r11 + 1
r12
r13
(3.1)
(3.2)
Using Equations (3.1) and (3.2) and the fact that ω̂12 + ω̂22 + ω̂32 = 1,
2 2
r13
r23
· ω̂3 +
· ω̂3 + ω̂32 = 1
r33 + 1
r33 + 1
2
2
2
r13 + r23
+ r33
+ 2r33 + 1
2
ω̂3 ·
=1
(r33 + 1)2
r33 + 1
ω̂32 =
2
2
2
2
since r13
+ r23
+ r33
= 1. In the same way we have
r11 + 1
2
r22 + 1
2
ω̂2 =
.
2
ω̂12 =
It can be verified that the result is the same as (a).
Exercise 3.9.
To multiply two arbitrary 3 × 3 matrices, 27 multiplication and 18 addition operations are required. However
for rotation matrices, once the first and second columns of R1 × R2 — let’s call them u and v — are
obtained, the third column can be obtained simply by taking the cross-product u × v (which requires only 6
multiplications and 3 additions). It is possible to save 3 multiplications and 3 additions with this procedure.
Exercise 3.10.
Since A, R ∈ SO(3) and tr(XY ) = tr(Y X) for any X, Y ∈ R3×3 , the objective function can be equivalently
written as
min tr((A − R)(A − R)| ) ⇒
R∈SO(3)
⇒
R∈SO(3)
R∈SO(3)
min tr(AA| + RR| − 2R| A)
max tr(R| A).
Note that the constant terms in the objective function can be ignored. The optimization can therefore be
formulated as follows:
3
P
max
R∈SO(3)
subject to
i=1
ri| ai
ri| rj
=
r3 − [r1 ]r2
= 0,
1
0
if i = j
otherwise
where ai is the i-th column of A and ri is the i-th column of R.
An alternative solution is also possible based on the singular value decomposition (SVD) of a matrix (consult
Go to the table of contents.
32
Chapter 3. Chapter 3 Solutions
any linear algebra textbook for a discussion of SVD). Letting A = U ΣV | be the SVD of A (and in practice
A may not necessarily be a rotation matrix due to measurement errors) the equivalent problem is
max tr(R| U ΣV | ) = tr(SΣV | ),
R∈SO(3)
where U, V as obtained from the SVD are orthonormal (i.e., U | U = V | V = I) and S = R| U is also
orthonormal. Let si , vi respectively denote the i-th column of U , V , and σi be the (i, i) component of Σ
(where σi ≥ 0). Then
tr(SΣV | ) =
3
X
σi s|i vi .
i=1
Note that s|i vi ≤ 1 and equality is achieved when si = vi . Consequently S = V is the optimal solution,
leading to
S
= R| U = V
R
= U V |.
Exercise 3.11.
(a) Expanding eA eB and eA+B ,
1
1
1
1 2
A + A3 + · · · )(I + B + B 2 + B 3 + · · · )
2!
3!
2!
3!
1
1
1
1
1
1
= I + (A + B) + ( A2 + AB + B 2 ) + ( A3 + A2 B + AB 2 + B 3 ) + · · ·
2!
2!
3!
2!
2!
3!
1
1
= I + (A + B) + (A2 + 2AB + B 2 ) + (A3 + 3A2 B + 3AB 2 + B 3 ) + · · ·
2!
3!
1
1
eA+B = I + (A + B) + (A + B)2 + (A + B)3 + · · · .
2!
3!
eA eB = (I + A +
We can see that eA eB = eA+B holds if
(A + B)2 = A2 + AB + BA + B 2 = A2 + 2AB + B 2
(A + B)3 = A3 + A2 B + ABA + AB 2 + BA2 + BAB + B 2 A + B 3 = A3 + 3A2 B + 3AB 2 + B 3
and so forth. Thus, the necessary condition is AB = BA.
(b) From (a), the necessary condition is [Va ][Vb ] = [Vb ][Va ]. Under this condition,
[ωa ] va
[ωb ] vb
[ωb ] vb
[ωa ] va
[Va ][Vb ] =
=
= [Vb ][Va ]
0
0
0
0
0
0
0
0
[ωa ][ωb ] [ωa ]vb
[ωb ][ωa ] [ωb ]va
=
.
0
0
0
0
That is, the following two equations hold:
[ωa ][ωb ] − [ωb ][ωa ] = 0
[ωa ]vb − [ωb ]va = 0
(3.3)
(3.4)
Equation (3.3) implies that cross product of ωa and ωb is zero, and so two screw rotation axes are
parallel. Let ωb be cωa where c ∈ R is constant. Then, substituting vi = −ωi × qi + hi ωi (i = a, b) and
ωb = cωa into Equation (3.4),
[ωa ]vb − [ωb ]va = ωa × vb − ωb × va
= ωa × vb − cωa × va
= ωa × (−cωa × qb + chb ωa ) − cωa × (−ωa × qa + ha ωa )
= ωa × (−cωa × qb ) − cωa × (−ωa × qa )
=0
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
33
Therefore, ωa × (ωa × qb ) = ωa × (ωa × qa ), and so we can conclude that the two screws are collinear.
Exercise 3.12.
Let A, B ∈ SO(3) have rotation axes and angles such that
A = Rot(ω̂a , α)
B = Rot(ω̂b , β).
If there exists R ∈ SO(3) satisfying AR = RB, then A = RBRT . We get then
e[ω̂a ]α = Re[ω̂b ]β RT
= e[Rω̂b ]β .
R therefore satisfies ω̂a α = Rω̂b β.
(a) If ω̂a = ω̂b = ẑ and α = β, then ẑ = Rẑ. The matrix R is therefore any ẑ-axis rotation.
(b) If ω̂a = ω̂b = ẑ and α 6= β, then two cases arise:
Case 1. |α| =
6 |β|: No solutions.
Case 2. |α| = |β|: From α = −β, we get ẑ = −Rẑ.
R = Rot(ω̂t , θ),
where ω̂t is a unit vector lying on the x-y plane and θ = ±π.
(c) If ω̂a = ω̂b , then the problem can be solved in the same way as (a) and (b).
If ω̂a 6= ω̂b , then we separate this problem into three cases:
Case 1. |α| =
6 |β|: No solutions.
Case 2. α = β: From ω̂a = Rω̂b ,
R = Rot(ω̂t , θ),
where ω̂t is a unit vector with direction ±(ω̂a + ω̂b ), and θ = ±π.
Case 3. α = −β: The direction of ω̂t in R should be ±(ω̂a − ω̂b ).
Exercise 3.13.
(a) There exists two different ways to find the eigenvalues of R.
Method 1. For R = e[ω̂]φ ∈ SO(3), Rω̂ = ω̂. This implies that one of its eigenvectors is ω̂ and its
corresponding eigenvalue is 1. Since the equation det(R − λI) = 0 is a third-order real polynomial, two
roots should be a complex conjugate pair. Observe also that
det(R) = λ1 λ2 λ3 = λ2 λ3 = 1,
from which it follows that
λ2 = cos θ + i sin θ = eiθ
λ3 = cos θ − i sin θ = e−iθ .
Method 2. For R ∈ SO(3), the relation between the eigenvector and eigenvalue of R can be written as
Rx = λx,
(3.5)
where x ∈ R3 is the eigenvector and λ is its corresponding eigenvalue. Multiplying the complex
conjugate transpose of each term in Equation (3.5) to both sides of Equation (3.5),
(Rx)∗ Rx = (λx)∗ λx
⇔ x∗ R∗ Rx = x∗ λ∗ λx
⇔
x∗ x = λ∗ λx∗ x.
We can thus conclude that λ is a complex number with magnitude 1. Hence, the eigenvalues of R can
be written as 1, eiθ , and e−iθ .
Go to the table of contents.
34
Chapter 3. Chapter 3 Solutions
(b) Denote the eigenvector associated with the eigenvalue µ + iν by x + iy for x, y ∈ R3 . The relation can
be written as
R(x + iy) = (µ + iν)(x + iy),
(3.6)
and its complex conjugate can also be written as
R(x + iy) = (µ + iν)(x + iy)
⇔ R(x − iy) = (µ − iν)(x − iy).
(3.7)
From Equations (3.6) and (3.7),
Rx = µx − νy
Ry = νx + µy,
and Rz = z from the eigenvalue whose value is 1. We can thus write


µ ν 0
R = A  −ν µ 0  A−1 ,
0 0 1
where A = x y z . From Equation (3.6),
(xT + iy T )RT R(x + iy) = (µ + iν)2 (xT + iy T )(x + iy)
⇔
(xT + iy T )(x + iy) = (µ2 − ν 2 + 2iµν)(xT + iy T )(x + iy)
⇔ (xT x − y T y + 2i(xT y)) = (µ2 − ν 2 + 2iµν)(xT x − y T y + 2i(xT y)).
Since (µ2 − ν 2 + 2iµν) cannot be 1,
⇔
xT x − y T y = 0
xT y = 0.
which means x and y are orthogonal. From the relation Rz = z, the following holds:
z T RT R(x + iy) = (µ + iν)z T (x + iy)
(z T x + iz T y) = (µ + iν)(z T x + iz T y),
⇔
from which it follows that z T x = 0 and z T y = 0 (since µ+iν 6= 1). x, y, and z are therefore orthogonal.
Taking x, y, and z as unit vectors satisfying det(A) = 1 yields A ∈ SO(3).
Exercise 3.14.
Express the inverse as a quadratic matrix polynomial in [ω], and multiply it with the original polynomial:
(Iθ + (1 − cos θ)[ω] + (θ − sin θ)[ω]2 )(αI + β[ω] + γ[ω]2 ) = I.
Using the identity [ω]3 = −[ω], the above leads to the following set of simultaneous equations:

 θα = 1
(1 − cos θ)α + (sin θ)β − (1 − cos θ)γ = 0

(θ − sin θ)α + (1 − cos θ)β + (sin θ)γ = 0.
Solving the equations,
α=
1
1
1 1
θ
, β = − , γ = − cot .
θ
2
θ 2
2
Exercise 3.15.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
35
(a) As a sequence of rotations about the axes of the fixed frame, rotation matrices should be multiplied
on the left. Beginning with R01 = I,
Step 1. R02 = Rot(x̂0 , α)R01 = Rot(x̂0 , α).
Step 2. R03 = Rot(ŷ0 , β)R02 = Rot(ŷ0 , β)Rot(x̂0 , α).
Step 3. R04 = Rot(ẑ0 , γ)R03 = Rot(ẑ0 , γ)Rot(ŷ0 , β)Rot(x̂0 , α).

− sin γ
cos γ
0
cos γ
R04 =  sin γ
0

0
cos β
0 
0
1
− sin β

0 sin β
1
1
0  0
0 cos β
0

0
− sin α  .
cos α
0
cos α
sin α
(b) As this rotation is about the axes of the moving frame, the rotation matrix should be multiplied on
the right:
R04 = R03 Rot(ẑ3 , γ)
= Rot(ŷ0 , β)Rot(x̂0 , α)Rot(ẑ3 , γ)


cos β 0 sin β
1
0
0
1
0   0 cos α
=
− sin β 0 cos β
0 sin α

0
cos γ
− sin α   sin γ
cos α
0
− sin γ
cos γ
0
(c)
−1
Tca = Tcb Tba = Tcb Tab
= Tcb


=




=

√1
2
0
0
1
0
0
1
2
− √12
− 12
1
2
√1
2
− 12
0
− √12
0
0
√1
2
T
Rab
0
T
−Rab
pab
1
  √1
0
2
 − √1
1 

2
0  0
0
1
0
√1
2
0
√1
2
1
√1
2 − 2
1 − √12
− 21 − √12
0
√1
2
0
√1
2
√1
2
0
0
0
0
1
0



.

1
Exercise 3.16.
(a) The three frames are shown in Figure 3.2.
!#"
%'&
!'
&
$
'&
!'(
%#"
%'(
$
'(
Figure 3.2
Go to the table of contents.
$#"
√1
2
− √12



−1 
1

0
0 .
1
36
Chapter 3. Chapter 3 Solutions
(b)

0
Rsa = 0
1
−1
0
0


0
1
−1 Rsb = 0
0
0

0
0
Tsa = 
1
0
−1
0
0
0
0
−1
0
0


1
3
0
0
T = 
0 sb 0
0
1
T
Rsb
=
0
T
−Rsb
pb
1

0
1
0
0
0
−1
0
0
−1
0

0 0
1 2

0 0
0 1
(c)
Rsb
−1
Tsb
=
0
pb
1
−1

1
0

=
0
0
0
0
1
0

0
0

−2
1
0
−1
0
0
(d)
−1
Tab = Tsa
Tsb

T
Rsa
−1
Tsa =
0

0
−1
Tab = 
0
0
T
pa
−Rsa
1
0
−1
=
0
0

1 0 1

0 3
 0
0 0 0
0 1 0
0
0
−1
0

1 0
0 3

0 0
0 1
0
0
−1
0
0
0
−1
0
 
0
0 0
−1
1 2
=
0 0  0
0
0 1
−1
0
0
0

0
3

−2
1
0
0
−1
0
(e)

0
0
T1 = Tsa T = 
1
0
−1
0
0
0
0
−1
0
0

3 1

0
 0

0 0
1 0
0
0
−1
0
 
0
0 0
0
1 2
=
0 0 1
0 1
0
0
1
0
0
−1
0
0
0

1
0

0
1
this corresponds to a transformation in a body frame

1
0
T2 = T Tsa = 
0
0
0
0
−1
0

0 0 0

1 2
 0

0 0 1
0 1 0
−1
0
0
0
0
−1
0
0
 
3
0
1
0
=
0 0
1
0
−1
0
0
0

0 3
0 2

1 0
0 1
this corresponds to a transformation in s world frame
(f)

1
0
ps = Tsb pb = 
0
0
0
0
−1
0
   
 
0 0 1
1
1





1 2 2  5 

5
=
=⇒
p
=
s
0 0 3 −1
−2
0 1 1
1
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
37
(g)

   
 
1
1 0 0 0 1
1






5
2
0
0
1
2
   =   =⇒  5 
p0 = Tsb ps = 
0 −1 0 0 3 −1
−2
1
0 0 0 1 1

   
 
1
1
1 0 0
0
1






−3
2
0
0
−1
0
−1
   =   =⇒ −3
p00 = Tsb ps = 
0 1 0 −2 3  0 
0
1
1
0 0 0
1
Therefore p0 is a change in location and p00 is change in reference frame.
(h)
Va = [AdTas ]Vs

0
0
−1 0
Tas = 
 0 −1
0
0

0
0
−1 0

 0 −1
=⇒ [AdTas ] = 
 0 −3

0
0
0
0

0
0
−1 0

 0 −1
Va = 
 0 −3

0
0
0
0

1 0
0 3

0 0
0 1
1
0
0
0
0
−3
1
0
0
0
0
−3

0
0

0

1

0
0
   
1
3
0
0 0
 2  −3
0
0 0
   
   
0
0 0
  1  = −2
  

0
0 1 
−1 −9
−1 0 0 −2  1 
−1
−3
0 −1 0
0
0
0
0
−1
0
0
0
0
0
0
−1
−1
0
0

0
−1
0
(i)

0
Rsa = 0
1
Therefore the trace of tr(Rsa ) = 0 which means we are condition (iii) and when we solve for θ we get
θ = cos−1 ((0 − 1)/2) = 2.094
Go to the table of contents.
38
Chapter 3. Chapter 3 Solutions
Then we have

0
[ω] = 1/(2 sin θ)(R − RT ) = 1/ 3 1
1
√
−1
0
1

−1
−1
0
G−1 (θ) = (1/θ)I − (1/2)[ω] + ((1/θ) − (1/2)(cot(θ/2))[ω]2


0.352
0.226
0.352
0.352 0.2257
=  −0.352
−0.2257 −0.352 0.352
v = G−1 (θ)p = [1.0548 − 1.0548 − 0.6772]T

0
−0.5774 −0.5774
0.5774
0
−0.5774
[ω] v
[s] =
=
0.5774 0.5774
0
0 0
0
0
0

1.0548
−1.0548

−0.6772
1
s = [0.5774 − 0.5774 0.5774 1.0548 − 1.0548 − 0.6772]T = ω/||ω||
T
h = ω̂ v/θ̇ = 0.827
v = −ŝθ̇ × q + hŝθ̇ =⇒ q = [−1 1 0]T
(j)
Sθ = [0 1 2 3 0 0]T = [ωθ, vθ]T
√
=⇒ ωθ = [0 1 2]T =⇒ θ = 5
We can find the matrix exponential by:
[ω]θ
e
(Iθ + (1 − cos θ)[ω] + (θ − sin θ)[ω]2 )v
e
=
0
1


0
−0.8944 0.4472
0
0 
[ωs ] =  0.8944
−0.4472
0
0


−0.6173 −0.7037 0.3518
e[ω]θ =  0.7037 −0.2938 0.6469
−0.3518 0.6469 0.6765



−0.6173 −0.7037 0.3518
1.0555

0.7037 −0.2938 0.6469
G(θ)v =  1.9407  e[s]θ = 
−0.3518 0.6469 0.6765
−0.9704
0
0
0
[s]θ

1.0555
1.9407 

−0.9704
1.0000
Exercise 3.17.
(a)

1 0 0
 0 1 0
Tad = 
 0 0 1
0 0 0


−1
0
 1
1 
, T = 
0  cd  0
1
0
Go to the table of contents.
1
0
0
0
0
0
−1
0

0
0 
.
2 
1
=⇒ θ̇ = 1
Chapter 3. Chapter 3 Solutions
39
(b) Tab Tbc Tcd = Tad . Thus Tab = Tad (Tbc Tcd )−1 .

1
 0
Tbc Tcd = 
 0
0

0

1
(Tbc Tcd )−1 = 
 0
0

0
4
 1
0 

0  0
0
1

0
0
0 −4 

−1 2 
0
1
0
1
0
0
1
0
0
0

0 1
−1

1 
 1 0
0  0 0
0 0
1

1 0 0
 0 1 0

Tab = 
0 0 1
0 0 0
1
0
0
0
0
0
1
0
0
0
−1
0
0
0
−1
0
 
0
0
 1
0 
=
2   0
0
1
 
0
0

−4 
= 1
2   0
0
1
1
0
0
0
1
0
0
0
0
0
−1
0
0
0
−1
0

4
0 

2 
1

−1
−3 
.
2 
1
Exercise 3.18.
(a) Tea Tar = Tes Tsr or Tsr = (Tes )−1 Tea Tar . Therefore Trs = (Tar )−1 (Tea )−1 Tes .
(b) Prs = Tre Pes and


1
 1 

Pes = 
 1 
1

−1
Tre = Ter
=
T
Rer
0
T
per
−Rer
1
−1
 0
=
 0
0
0 0
1 0
0 −1
0 0

1
−1 
.
1 
1
Therefore

prs
1
−1
 0
=
 0
0
0 0
1 0
0 −1
0 0

 

1
1
0

 

−1 
 1  =  0 .




1
1
0 
1
1
1
Exercise 3.19.
(a) The transformation from the fixed frame to the satellite can be described as follows: Rotate about
the {0} frame x̂-axis to align the ŷ-axis with the satellite’s ŷ-axis. Then, rotate about the body frame
ŷ-axis to align the x̂-axis with the satellite’s x̂-axis. Finally, translate about the body frame x̂-axis by
Go to the table of contents.
40
Chapter 3. Chapter 3 Solutions
R. Defining θ1 = vR11t , θ2 = vR22t ,
T01 = Rot(x̂, 120◦ )Rot(ŷ, −90◦ + θ1 )Trans(x̂, −R1 )



1 0 0
cos(−90◦ + θ1 ) 0 sin(−90◦ + θ1 ) 0
1
0
0
0
 0 1 0
 0 cos 120◦ − sin 120◦ 0  
0
1
0
0


=
 0 sin 120◦
cos 120◦ 0   − sin(−90◦ + θ1 ) 0 cos(−90◦ + θ1 ) 0   0 0 1
0 0 0
0
0
0
1
0
0
0
1




1 0
0√
0
1 0 0 −R1
sin θ1 0 − cos θ1 0
 0 −1 − 3 0   0
 0 1 0
0 
1
0
0
2



√2
=
 0




3
1
0
0
1
0 
cos
θ
0
sin
θ
0
1
1
−
0
2
2
0
0
0
1
0 0 0
1
0 0
0
1


sin θ1
0
−√cos θ1
−R1 sin θ1
√
√
3

 − 3 cos θ1 − 1 − 3 sin θ1
2
2
2 R1 cos θ1 
√2
=
 − 1 cos θ
3
1
1
− sin θ
R cos θ 
2
1
0
2
0
2
1
0
2
1

−R1
0 

0 
1
1
1
T02 = Rot(x̂, 90◦ )Rot(ŷ, −90◦ + θ2 )Trans(x̂, −R2 )



1 0 0
cos(−90◦ + θ2 ) 0 sin(−90◦ + θ2 ) 0
1
0
0
0
 0 1 0
 0 cos 90◦ − sin 90◦ 0  
0
1
0
0 





=
0 sin 90◦
cos 90◦ 0   − sin(−90◦ + θ2 ) 0 cos(−90◦ + θ2 ) 0   0 0 1
0
0
0
1
0 0 0
0
0
0
1




1 0 0 −R2
sin θ2 0 − cos θ2 0
1 0 0 0
 0 1 0
 0 0 −1 0   0
0 
1
0
0 






=




0 0 1
0 
cos θ2 0
sin θ2
0
0 1 0 0
0 0 0
1
0
0
0
1
0 0 0 1


sin θ2
0 − cos θ2 −R2 sin θ2
 − cos θ2 0 − sin θ2 R2 cos θ2 
.

=

0
1
0
0
0
0
0
1
(b)
−1
T21 = T02
T01
T
T
R01 P01
R02 −R02
P02
=
0
1
0
1


sin θ1
0
sin θ2
− cos θ2 0 R2
√
3
1



0
0
1 0   − 2 cos θ1 −
√2
=
 − cos θ2 − sin θ2 0 0   − 1 cos θ
3
1
2
2
0
0
0 1
0
0


1
a11
θ
a13
a14
2 cos
√ 2
3
 − 1 cos θ1
− 12 sin θ1 12 R1 cos θ1 
,
2
2
=
1


a31
a33
a34
2 sin θ2
0
0
0
1
Go to the table of contents.
−√cos θ1
− 23 sin θ1
− 12 sin θ1
0

−R1 sin θ1
3

2 R1 cos θ1 

1
2 R1 cos θ1
1
√

−R2
0 

0 
1
Chapter 3. Chapter 3 Solutions
41
where
√
3
cos θ1 cos θ2
2√
3
a13 = − cos θ1 sin θ2 +
sin θ1 cos θ2
2√
3
a14 = −R1 sin θ1 sin θ2 −
R1 cos θ1 cos θ2 + R2
2
√
3
a31 = − sin θ1 cos θ2 +
cos θ1 sin θ2
√ 2
3
a33 = cos θ1 cos θ2 +
sin θ1 sin θ2
2√
3
a34 = R1 sin θ1 cos θ2 −
R1 cos θ1 sin θ2 .
2
a11 = sin θ1 sin θ2 +
Exercise 3.20.
Since the two wheels roll the same distance, we can represent the rotation angle of the two wheels with one
variable θ:


0
 I L sin θ 

Tab = 

L cos θ 
0
1


0
 I 2r 

Tbc = 

0 
0 1
Tac = Tab Tbc

 I
=

0

0
L sin θ + 2r 


L cos θ
1
Exercise 3.21.
(a) From the given Tab , qab = (−100, 300, 500)T and

0
Rab =  1
0
−1
0
0

0
0 .
1
Therefore ra = pa − qab , which leads to
T
T
rb = Rab
ra = Rab
(pa − qab ) = (500, −100, −500)T .
(b) From the given pac = pa and Rac = Rot(x, 30◦ ), Tac can be derived as follows:

1
 0
Tac = 
 0
0
0
cos π6
sin π6
0
0
− sin π6
cos π6
0
Go to the table of contents.

0
800 

0 
1
42
Chapter 3. Chapter 3 Solutions
Tbc can then be computed as follows:

0

−1
−1
Tbc = Tab
Tac = 
 0
0
√
3
2
0
1
2
0
− 12
0
√
3
2
0

500
−100 
.
−500 
1
Exercise 3.22.
(a) Since the platform is rising vertically, p01 = (0, 0, vt)| , with the ŷ2 -axis of the laser frame pointing at
the target. Thus R12 = Rot(ẑ, −α)Rot(x̂, −γ), where α and γ satisfy
tan α =
1 − cos θ
Lt
.
, tan γ = p
1 − sin θ
(1 − cos θ)2 + (1 − sin θ)2
The target is rotating about its ẑ-axis, or R03 = Rot(ẑ, π2 + ωt). T01 , T12 , T03 can be therefore be as
follows:


0
 I 0 

T01 = 

vt 
0 1


0
 Rot(ẑ, −α)Rot(x̂, −γ) 0 

T12 = 

0 
0
1


1 − cos ωt
 Rot(ẑ, π + ωt) 1 − sin ωt 
2
.
T03 = 


0
0
1
(b)
T23 = (T02 )−1 T03

−1 

1 − cos ωt
0
 Rot(ẑ, −α)Rot(x̂, −γ) 0   Rot(ẑ, π + ωt) 1 − sin ωt 
2
 

=


vt  
0
0
1
0
1



0
1 − cos ωt
 Rot(x̂, γ)Rot(ẑ, α) vt sin γ   Rot(ẑ, π + ωt) 1 − sin ωt 
2


=


−vt cos γ  
0
0
1
0
1


cos α(1 − cos ωt) − sin α(1 − sin ωt)
 Rot(x̂, γ)Rot(ẑ, π + ωt + α) cos γ(sin α(1 − cos ωt) + cos α(1 − sin ωt)) + vt sin γ 
2
.
=

sin γ(sin α(1 − cos ωt) + cos α(1 − sin ωt)) − vt cos γ 
0
1
Exercise 3.23.
The moving frame {t} can be placed on the table as shown in Figure 3.3, with the origin at the center of
the table and whose orientation is Rot(ẑ, θ) with respect to the {0} frame. Set the {t1 } frame to θ = vr1 t ,
and the {t2 } frame to θ = 45◦ .
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
43
Top View
x̂1 {1}
ŷ2
{2}
ŷ1
x̂2
45◦
θ
ŷ0
x̂0
{t}
{0}
L=1
Figure 3.3
(a) Compute and substitute the rotation matrices Rij and the position vectors pij as follows:

cos θ
 sin θ

T0t = 
0
0
− sin θ
cos θ
0
0


−1 0
0 L

0 L 
 , Tt1 1 =  0 −1
 0
1 H 
0
0
0
0 1


0
0 0

0 R 
 , Tt2 2 =  −1
 0
1 0 
0
0 1

1 0
0
0 0 R − v2 t 
.

0 1
1
0 0
1
Substitute θ = vr1 t for the {t1 } frame and θ = 45◦ for the {t2 } frame into Tot . T01 , T02 can then be
obtained as functions of t:

− cos v21 t
sin v21 t
0 1 − 2 sin v21 t
 − sin v1 t − cos v1 t 0 1 + 2 cos v1 t 
2
2
2 
T01 = T0t1 Tt1 1 = 


0
0
1
2
0
0
0
1
√
√
√
 √2

2
2
0
1
−
2
+
v
t
2
2√
2
2
√
√
√
 − 2
2
0 1 + 2 − 22 v2 t 
.
2
2
T02 = T0t2 Tt2 2 = 
 0

0 1
2
0
0 0
1

(b) Substituting the result obtained from (a),
−1
T12 = T01
T02
−1 R01 p01
R02 p02
=
0
1
0
1
T
T
T
R02 p02
p02 − R01 p01
R01 −R01 p01
R01 R02 R01
=
=
0
1
0
1
0
1
√
√
√
√




2
2
2
− cos v21 t − sin v21 t 0
sin v21 t + cos v21 t
0
1
−
2
+
v t
2√
√2
√2 2
√
 sin v1 t

2
2
− cos v21 t 0 2 − sin v21 t + cos v21 t 
0 1 + 2 − 22 v2 t 
2
 − 2
.
2
=




0
0
1
−2
0
0 1
2
0
0
0
1
0
0 0
1
Exercise 3.24.
Calculate the configuration of the robot at t = 4 sec, (θ1 , θ2 , θ3 ) = (π, π2 , −π). Since θ1 = π and the pitch
Go to the table of contents.
44
Chapter 3. Chapter 3 Solutions
h = 2, we have L1 (t = 4) = 10 + 1 = 11.
T
where
R
p
=
R
0
p
,
1
π
= Rot(ẑ, π)Rot(−ŷ, )Rot(−ẑ, −π),
  2 
−5
5
= Rot(ẑ, π)  2  = −2 .
11
11
To sum up, the transformation matrix of the tip T can be computed as follows:


0 0 1 5
 1 0 0 −2 

T =
 0 1 0 11  .
0 0 0 1
Exercise 3.25.
(a) From AX = XB, we have B = X −1 AX. Substitute the given A, X into B:


0 0 1 −1
 0 1 0 1 

B=
 −1 0 0 −1  .
0 0 0 1
(b) Suppose that both tr(RA ) and tr(RB ) are not equal to −1, so that α and β are uniquely defined.
Rewriting RA RX = RX RA as e[α] RX = RX e[β] and using the property R[w]RT = [Rw] and Re[w] RT ,
T
T
e[α] = RX e[β] RX
= eRX [β]RX = e[RX β] .
This leads to the necessary and sufficient condition.
α = RX β.
(3.8)
Since RX is orthogonal, a solution exists if and only if kαk = kβk. The condition kαk = kβk also holds
when RA and RB have trace −1, since then kαk = kβk = π.
(c) From Equation (3.8) we can construct a set of equations αi = RX βi i = 1, . . . , k. Suppose two pairs of
(Ai , Bi ) are obtained in a physical setting where a solution X ∈ SE(3) is known to exist. If α1 ×α2 6= 0
and β1 × β2 6= 0, a unique solution to Equation (3.8) is
RX = AB −1 ,
where A and B are matrices whose columns are the vectors α1 , α2 , α1 × α2 , and β1 , β2 , β1 × β2 , respectively:
A = α1 , α2 , α1 × α2
B = β1 , β2 , β1 × β2
The translational components of Ai X = XBi can be written
RAi pX + pA = RX pBi + pX
[RAi − I]pX = RX pBi − pAi .
The matrix (RAi − I) has rank two and its null space is spanned by {αi }, so pX cannot be determined
uniquely yet. Satisfying our assumption (i.e. α1 × α2 6= 0), a unique solution for pX can be found by
solving the following augmented matrix equation:
RA1 − I
RX pB1 − pA1
pX =
.
RA2 − I
RX pB2 − pA2
The minimum number of k is 2 for a unique solution X ∈ SE(3) to exist.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
45
Exercise 3.26.
 
0
0
 
1
ŝ

V θ̇ = S =
=
0
−ŝ × q + hŝ
 
3
2
Drawn as Figure 3.4.
!̂
Figure 3.4
Exercise 3.27.
V = [ω, v] = [0 2 2 4 0 0]T
ω = [0 2 2]T v = [4 0 0]T
√
√
ŝ = ω/||ω|| = [0 1/ 2 1/ 2]
√
v = v = [ 2 0 0]T
h = ω̂ T v/θ̇ = 0
q = [0 1 − 1]T
Drawn as Figure 3.5.
Exercise 3.28.

0
Rbs = −1
0
0
0
−1

1
0
0

3
=⇒ Rbs ωˆs = ωˆb −1
−2
Go to the table of contents.

46
Chapter 3. Chapter 3 Solutions
!̂
Figure 3.5
Exercise 3.29.
Given two frames {a} and {b} attached to a rigid body, the representations of a twist with respect to these
two frames are related by
Sa
=
[AdTab ]Sb
(3.9)
wa
=
Rab wb
(3.10)
va
=
[pab ]Rab wb + Rab vb .
(3.11)
Transforming the twists into coordinates of the space frame {s},
Sas
=
[AdTsa ]Sa
(3.12)
=
[AdTsb ]Sb
(3.13)
was
wbs
vas
vbs
=
Rsa wa
(3.14)
=
Rsb wb
(3.15)
=
[psa ]Rsa wa + Rsa va
(3.16)
=
[psb ]Rsb wb + Rsb vb .
(3.17)
Sbs
where (·)sa is (·)a expressed in the {s} frame, and (·)sb is (·)b expressed in the {s} frame. Thus, if we substitute
Equations (3.10) and (3.11) into (3.14) and (3.16),
was
=
Rsa Rab wb = Rsb wb = wbs
vas
=
[psa ]Rsa Rab wb + Rsa ([pab ]Rab wb + Rab vb )
=
[psa + Rsa pab ]Rsb wb + Rsb vb = [psb ]Rsb wb + Rsb vb
=
vbs .
In summary, the twist associated with the motion of a rigid body is always the same when expressed in the
space frame, regardless of where the body frame is attached to the body:
Sas = Sbs .
Exercise 3.30.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
47
(a) First, compute T12 for both cases:
Case 1:

1 0 0
0 1 0
=
0 0 1
0 0 0

−1 0
 0 −1
=
0
0
0
0
T12
Case 2:
T12

0
0

1
1

0 0
0 0
.
1 1
0 1
Cases 1 and 2 can be thought as a screw motion that translates and rotates along the vertical screw
shown in Figure 3.6. Let
ẑ2
ẑ2
x̂2
ŷ2
ŷ2
{2}
{2}
1
1
{0}
x̂2
x̂0
ẑ1
ŷ0
ŷ1
{1}
x̂1
1
ẑ1
ẑ0
ẑ0
{0}
x̂0
ŷ0
x̂1
1
ŷ1
{1}
Figure 3.6
[S]
=
e[S]
=
[ω] v
0 0
[ŵ]θ
e
0
=
[ω̂] v̂
0 0
θ = [Ŝ]θ
G(θ)v̂
.
1
1 T
Case 1: Based on the physical meaning of a screw, we can compute Ŝ as ω̂ = (0, 0, 1)T , v̂ = (v1 , 0, 2nπ
) ,
where v1 = kr × ŵk depends on the location of the axis r. However, the orientation is not changed and
θ = 2nπ. From [S] = [Ŝ]θ, the exponential coordinates can be derived as follows:
ω = (0, 0, 2nπ)| , v = (2nπv1 , 0, 1)| .
Case 2: Similarly, the orientation is rotated about the ẑ axis by a half-circle, thus θ = 2nπ + π.
1
Therefore ω̂ = (0, 0, 1)| and v̂ = (v10 , 0, 2nπ+π
)| , where v10 also depends on the axis. From [S] = [Ŝ]θ,
the exponential coordinates can be derived as follows:
ω = (0, 0, (2n + 1)π)T , v = ((2n + 1)πv10 , 0, 1)T .
(b) Restrict the norm of w as kwk ≤ π. From the answers obtained in (a),
Case 1: To satisfy the restriction, ω = (0, 0, 0)T , otherwise the norm of ω exceeds π. Thus, the joint
can be considered as a prismatic joint. From [S] = [Ŝ]θ, the exponential coordinates can be derived as
follows:
ω = (0, 0, 0)T , v = (0, 0, 1)T .
Go to the table of contents.
48
Chapter 3. Chapter 3 Solutions
Case 2: To satisfy the restriction, θ = ±π. The screw can be computed as ŵ = (0, 0, 1)| and v̂ =
1 |
(±1, 0, ±π
) . From [S] = [Ŝ]θ, the exponential coordinates can be derived as follows:
ω = (0, 0, ±π)T , v = (π, 0, 1)T .
Exercise 3.31.
Since the force is applied at the center of mass there is no torque, therefore the wrench vector in the effector
frame can be written as:
 
0
0
 
0

Fe = 
0
 
0
10
From the example we get the transformation matrix of the object relative to the robot hand to be:


0√
0√
1
−75√
−1/ 2 1/ 2 0 −260/ 2
√
√
√ 
Tce = 
−1/ 2 −1/ 2 0 130/ 2 
0
0
0
1
−1
By defintion Fc = [AdTec ]T Fe , and Tec = Tce
, which gives us the matrices:
√
√


0 −1/√ 2 −1/√2 −65
0 1/ 2 −1/ 2 195 

Tec = 
1
0
0
75 
0
0
0
1




0
0
0
1.0000 195.0000
65.0000
0
 919.2 
−0.7071 0.7071
0
−53.0330 −53.0330 91.9239 





1838.5

−0.7071
−0.7071
0
53.0330
−53.0330
183.8478
T




=⇒ Fc = 
[AdTec ] = 

0
0
0
0
0
1.0000 

 10 


 0 

0
0
0
−0.7071
0.7071
0
0
0
0
−0.7071
−0.7071
0
0
Exercise 3.32.
(a) Let Toa = (Roa , pa ) and Tob = (Rob , pb ). Then
To0 a
=
(Rs Roa , Rs pa + ps )
=
0
(Roa
, p0a ),
=
(Rs Rob , Rs pb + ps )
=
0
(Rob
, p0b ).
and
To0 b
T
We can write Rab = Roa
Rob and pab = pb − pa . Also,
0
Rab
=
0 T 0
(Roa
) Rob
T
= Roa
RsT Rs Rob
T
= Roa
Rob
= Rab ,
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
49
and
p0ab
= p0b − p0a
= Rs (pb − pa )
= Rs pab .
Then from the given distance formula,
dist(To0 a , To0 b ) =
q
θ02 + kp0ab k2 ,
0
where θ0 = cos−1 ((tr(Rab
) − 1)/2).
0
0
(b) Since Rab = Rab , θ = θ. We also have kRs pab k2 = kpab k2 . Therefore
dist(To0 a , To0 b ) = dist(Toa , Tob ),
for all S ∈ SE(3).
Exercise 3.33.
(a) For the matrix exponential, decompose A into P DP −1 by obtaining the eigenvalues and eigenvectors
of A:
det(Is − A) = (s + 2)(s + 1) = 0.
The eigenvalues of A are −1 and −2. Hence,
−1
0
1 1
0
D=
, P =
(eigenvector matrix), P −1 =
0 −2
1 0
1
1
−1
.
The general solution can be written as
x(t) = eAt x(0) = P eDt P −1 x(0)
−t
1 1
e
0
0
=
1 0
0 e−2t
1
−2t
−t
−2t
e
e −e
=
x(0).
0
e−t
1
−1
x(0)
Therefore, limt→∞ x(t) = 0.
(b)
det(Is − A) = (s − 2)2 + 1 = 0.
The eigenvalues of A are 2 + i and 2 − i. Hence,
1 −i
2+i
0
i −i
−1
D=
, P =
(eigenvector matrix), P =
i
0
2−i
1
1
2
The general solution can be written as
x(t) = eAt x(0) = P eDt P −1 x(0)
(2+i)t
1 −i
i −i
e
0
=
1
1
i
0
e(2−i)t 2
cos t − sin t
= e2t
x(0).
sin t
cos t
Therefore, limn→∞ x(t) = ∞ (increasingly larger oscillations).
Go to the table of contents.
1
1
x(0)
1
1
.
50
Chapter 3. Chapter 3 Solutions
Exercise 3.34.
From the linear differential equation ẋ(t) = Ax(t),
 −3e−3t
e−3t


=A
,


9e−3t
−3e−3t

t t 

e
e


=A
.

et
et
From above one can obtain
A=
0
3
1
−2
.
The eigenvalues and eigenvectors of A are
λ1 = 1
v = (1, 1)
⇒ 1
.
λ2 = −3
v2 = (1, −3)
Therefore
t
1
1
e
0
3/4
eAt = P eDt P −1 =
1 −3
0 e−3t
1/4
t
−3t
t
−3t
1
3e + e
e −e
.
=
4 3et − 3e−3t et + 3e−3t
1/4
−1/4
Exercise 3.35.
Set z(t) = e−At x(t) and evaluate ż(t):
= −Ae−At x(t) + e−At ẋ(t)
ż(t)
= −Ae−At x(t) + e−At (Ax(t) + f (t))
= e−At f (t).
∴ ż(t)
Integrate both sides from 0 to t:
Z t
Z t
ż(s) ds =
0
e−As f (s) ds.
0
Express the left side of the equation in terms of x(t):
Z t
ż(s) ds = z(t) − z(0)
0
=
Z t
ż(s) ds =
∴
0
At
Multiply e
e−At x(t) − e−A0 x(0)
e−At x(t) − x(0).
on the left to both sides of the previous equation,
Z t
At
−At
At
e
e
x(t) − x(0) = e
e−As f (s) ds
0
Z t
At
At
e−As f (s) ds
x(t) − e x(0) = e
0
∴ x(t) = eAt x(0) +
Z t
eA(t−s) f (s) ds.
0
Exercise 3.36.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
51
(a) The rotation matrix corresponding to the ZXZ Euler angles can be represented as
R = Rot(ẑ, α)Rot(x̂, β)Rot(ẑ, γ)




cos α − sin α 0
1
0
0
cos γ − sin γ 0
cos γ 0 
=  sin α cos α 0   0 cos β − sin β   sin γ
0
0
1
0 sin β
cos β
0
0
1


cos α cos γ − sin α sin γ cos β − cos α sin γ − sin α cos γ cos β
sin α sin β
=  sin α cos γ + cos α cos β sin γ − sin α sin γ + cos α cos β cos γ − cos α sin β  .
sin β sin γ
sin β cos γ
cos β
√2 2
sin β
−1 ± r13 +r23
, we consider two possible cases:
Since β = tan−1 cos
β = tan
r33
Case 1. 0 ≤ β ≤ π,
q
2 + r 2 , r ),
β = atan2( r13
23 33
α = atan2(r13 , −r23 ),
γ = atan2(r31 , r32 ).
Case 2. −π ≤ β ≤ 0,
q
2 + r 2 , r ),
β = atan2(− r13
23 33
α = atan2(−r13 , r23 ),
γ = atan2(−r31 , −r32 ).
(b) From the result obtained in (a), we can find two sets of Euler angles:
Case 1. 0 ≤ β ≤ π,
π
π
α = π, β = , γ = .
4
4
Case 2. −π ≤ β ≤ 0,
π
3π
α = 0, β = − , γ = − .
4
4
Exercise 3.37.
R is calculated as follows:

cos θ1
R = e[ω̂1 ]θ1 e[ω̂2 ]θ2 =  sin θ1
0
− sin θ1
cos θ1
0
  cos θ
2
0

√ θ2
0   − sin
2
1
√ θ2
− sin
2
sin
√ θ2
2
cos θ2 +1
2
cos θ2 −1
2
sin
√ θ2
2
cos θ2 −1
2
cos θ2 +1
2


.
√ θ2 , cos θ2 −1 , cos θ2 +1 )
Then, the third row of R is the same as the third row of e[ω̂2 ]θ2 . By comparing (− sin
2
2
2
with the third row of the given R, which is ( √12 , 0, √12 ), it can be seen that there is no θ2 which satisfies the
condition. Therefore, the given orientation R is not reachable.
Exercise 3.38.
R = Rot(x̂, φ)Rot(ẑ, θ)


1
0
0
cos θ − sin θ
cos θ
=  0 cos φ − sin φ   sin θ
0 sin φ cos φ
0
0


cos θ
− sin θ
0
=  sin θ cos φ cos θ cos φ − sin φ  .
sin θ sin φ cos θ sin φ cos φ
Go to the table of contents.

0
0 
1
52
Chapter 3. Chapter 3 Solutions
Both φ and θ range in value over a 2π interval.
Exercise 3.39.
(a) R = Rot(ẑ0 , α)Rot(ŷ0 , β)Rot(ω̂, γ), where ω̂ = ( √12 , √12 , 0). Hence,

− sin α
cos α
0
cos α
R =  sin α
0

0
cos β
0 
0
1
− sin β

0 sin β
1
0  Rot(ŵ, γ),
0 cos β
where
Rot(ω̂, γ) = I + sin γ[ω̂] + (1 − cos γ)[ω̂]2

 1
1
√1 sin γ
2 (1 + cos γ)
2 (1 − cos γ)
2


=  12 (1 − cos γ) 12 (1 + cos γ) − √12 sin γ  .
1
1
√ sin γ
cos γ
− √2 sin γ
2
(b) (i) From Figure 3.7, (α, β, γ) = (0, 0, π) or (− π2 , π, 0).
ẑ3
x̂3
ŷ3
ŷ3
ẑ3
x̂3
γ
γ
Figure 3.7
π
(ii) By comparing R03 derived in (a) with e[ω̂] 2 , (α, β, γ) can be calculated. Denoting the (i, j)th
element of R03 by rij ,
2
r11 + r12 = cos α cos β − sin α = − √
5
1
2
r21 + r22 = sin α cos β + cos α = √ +
5 5
1
2
r31 + r32 = − sin β = − √ + .
5 5
(3.18)
(3.19)
(3.20)
From Equation (3.20), β = 2.71◦ or β = 177.29◦ .
Case 1. β = 2.71◦ .
By Equations (3.18) and (3.19), α = 84.23◦ . Substituting β into r31 and r32 , γ = 34.91◦ .
Case 2. β = 177.29◦ .
By Equations (3.18) and (3.19), α = 354◦ . Substituting β into r31 and r32 , γ = 219◦ .
Exercise 3.40.
(a) Compute the unit quaternion q ∈ R4 from the corresponding rotation matrix R ∈ SO(3). Following
the definition and using the Rodrigues formula,
q
R
=
[η, ε| ] = [cos(θ/2), ŵ sin(θ/2)]|
=
[ŵ]
e
(3.21)
2
θ = I + sin θ[ŵ] + (1 − cos θ)[ŵ] ,
(3.22)
where ŵ is the unit vector in the direction of the rotation axis, and θ is the angle of rotation. First
obtain θ and ŵ from Equation (3.22), and compute the quaternion q with θ and ŵ obtained from
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
53
Equation (3.21). Useful formulas for a rotation matrix are
tr(R)
= 2 cos θ + 1
R − R|
2
= sin θ[ŵ]
= 4 cos2 (θ/2) − 2

0
= 2 sin(θ/2) cos(θ/2)  w3
−w2
−w3
0
w1

w2
−w1  .
0
Using these formulas, we can derive the quaternion as follows:
1√
1p
1 + tr(R) =
1 + r11 + r22 + r33
q0 = η =
2 
 
2
q1
r32 − r23
q2  = ε = 1 r13 − r31  .
4q0
q3
r21 − r12
(b) We now compute a rotation matrix from the corresponding quaternion. Following the definitions given
in Equations (3.21) and (3.22),
θ = 2 cos−1 q0
R − R|
.
[ε] =
4q0
We can derive the rotation matrix from the Rodrigues formula of Equation (3.22) as follows:
 2

q0 + q12 − q22 − q32
2(q1 q2 − q0 q3 )
2(q0 q2 + q1 q3 )
q02 − q12 + q22 − q32
2(q2 q3 − q0 q1 ) 
R =  2(q1 q2 + q0 q3 )
2(q1 q3 − q0 q2 )
2(q0 q1 + q2 q3 )
q02 − q12 − q22 + q32
(c) We now derive the formula for the product of the quaternions. Let Rq , Rp ∈ SO(3) denote two rotation
matrices corresponding to unit quaternions q, p, respectively:
 2

q0 + q12 − q22 − q32
2(q1 q2 − q0 q3 )
2(q0 q2 + q1 q3 )
q02 − q12 + q22 − q32
2(q2 q3 − q0 q1 ) 
Rq =  2(q1 q2 + q0 q3 )
2
2(q1 q3 − q0 q2 )
2(q0 q1 + q2 q3 )
q0 − q12 − q22 + q32
 2

p0 + p21 − p22 − p23
2(p1 p2 − p0 p3 )
2(p0 p2 + p1 p3 )
p20 − p21 + p22 − p23
2(p2 p3 − p0 p1 )  .
Rp =  2(p1 p2 + p0 p3 )
2
2(p1 p3 − p0 p2 )
2(p0 p1 + p2 p3 )
p0 − p21 − p22 + p23
Denote the products qp by n and Rq Rp by Rn . After some calculation, n can be obtained as follows:
 


n0
q0 p0 − q1 p1 − q2 p2 − q3 p3
n1 


  = q0 p1 + q1 p0 + q2 p3 − q3 p2  .
n2 
q0 p2 + q2 p0 − q1 p3 + q3 p1 
n3
q0 p3 + q1 p2 − q2 p1 − q3 p0
The product formula for two unit quaternions is therefore given by
ηq ηp − ε|q εp
ηn
qp = n =
=
.
εn
ηq εp + ηp εq + εq × εp
Exercise 3.41.
(a) First, calculate (I + [r])−1 :
(I + [r])−1
=
=

1 + r12
r3 + r1 r2
1
 −r3 + r1 r2
1 + r22
1 + rT r
r2 + r1 r3 −r1 + r2 r3
1
(I + rrT − [r]),
1 + rT r
Go to the table of contents.

−r2 + r1 r3
r1 + r2 r3 
1 + r32
54
Chapter 3. Chapter 3 Solutions
where r = [r1 r2 r3 ]T . Then,
R
(I − [r])2 (I + [r])−2
2
1
2
T
= (I − 2[r] + [r] )
(I + rr − [r])
1 + rT r
2
1
=
(I − 2[r] + [r]2 )(I + rrT − [r])2
1 + rT r
2
1
(I − 2[r] + [r]2 )(I + (rrT )2 + [r]2 + 2rrT − 2[r] − rrT [r] − [r]rrT ).
=
1 + rT r
=
Since rrT [r] = [r]rrT = 0,
R =
1
1 + rT r
2
1
1 + rT r
2
1
1 + rT r
2
=
=
(I − 2[r] + [r]2 )(I + (rrT )2 + [r]2 + 2rrT − 2[r])
(I + 2rrT + (rrT )2 − 4[r] + 6[r]2 − 4[r]3 + [r]4 )
((I + rrT )2 − 4[r] + 6[r]2 − 4[r]3 + [r]4 ).
The following equations can be derived from straightforward calculation: [r]2 = rrT − rT rI, [r]3 =
−rT r[r], [r]4 = −rT r[r]2 . Substituting these into the equations,
2
1
R =
(((1 + rT r)I + [r]2 )2 − 4[r] + 6[r]2 − 4[r]3 + [r]4 )
1 + rT r
2
1
((1 + rT r)2 I + 2(1 + rT r)[r]2 + [r]4 − 4[r] + 6[r]2 − 4[r]3 + [r]4 )
=
1 + rT r
2
1
=
((1 + rT r)2 I − 4[r] + (8 + 2rT r)[r]2 − 4[r]3 + 2[r]4 )
1 + rT r
2
1
=
((1 + rT r)2 I − 4[r] + (8 + 2rT r)[r]2 + 4rT r[r] − 2rT r[r]2 )
1 + rT r
2
1
(−4(1 − rT r)[r] + 8[r]2 )
= I+
1 + rT r
= I −4
1 − rT r
8
[r] +
[r]2 .
T
2
(1 + r r)
(1 + rT r)2
(b) Substituting r = −ŵ tan θ4 into the formula in (a),
R
1 − rT r
8
[r] +
[r]2
(1 + rT r)2
(1 + rT r)2
1 − tan2 (θ/4)
θ
8
θ
I −4
(− tan )[ŵ] +
tan2 [ŵ]2
4
4
(1 + tan2 (θ/4))2
(1 + tan2 (θ/4))2
cos(θ/2)
θ
θ
θ
θ
I +4 2
cos4 tan [ŵ] + 8 cos4 tan2 [ŵ]2
cos (θ/4)
4
4
4
4
2
θ
θ
θ
θ
θ
I + 4 cos cos sin [ŵ] + 8 cos sin
[ŵ]2
2
4
4
4
4
2
θ
θ
1
θ
I + 2 cos sin [ŵ] + 8
sin
[ŵ]2
2
2
2
2
= I −4
=
=
=
=
= I + sin θ[ŵ] + (1 − cos θ)[ŵ]2
= e[ŵ]θ .
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
55
The given r therefore satisfies the formula. This solution is not unique; another solution is given by
r=
ŵ
.
tan(θ/4)
(c) Let the angular velocity in the body-fixed frame be ω. Then ω satisfies the following equation:
[ω] = RT Ṙ,
where [ω] is the skew-symmetric representation of ω. Substituting the R derived in (a),
ṙ =
1
{(1 − rT r)I + 2[r] + 2rrT }ω.
4
For a detailed derivation, see [I.G. Kang and F.C. Park, Cubic spline algorithms for orientation interpolation, Int. J. Numerical Methods in Engineering, vol. 46 (1999): 45-64.].
(d) One of the advantages of the modifed Cayley-Rodrigues parameters is that the singularity at π is
now relocated to 2π; rotations up to 2π are now possible. Referring to the radius π solid ball picture
of SO(3), the modified Cayley-Rodrigues parameters can be obtained by “stretching” the solid ball
of radius 2π (as opposed to π for the standard Cayley-Rodrigues parameters corresponding to the
k = 1 case) to infinity. However, one now loses the one-to-one correspondence between R3 and SO(3)
that exists for the standard Cayley-Rodrigues parameters. Moreover, (i) the formulas for the angular
velocity and acceleration become more complicated, (ii) one cannot obtain r from R by a simple rational
expression as in the case of the standard Cayley-Rodrigues parameters, and (iii) multiplication of two
rotation matrices in the modified parameters does not admit a simple rational expression like the
standard parameters. Going to higher order, for the case k = 4 it can be shown that the corresponding
r is given by
θ
r = ŵ tan .
8
As k increases, one obtains successively closer approximation (up to constant scaling factor) to the
canonical coordinates−the nonlinear warping effect caused by the tangent function becomes less severe.
As expected, however, the formulas are no longer simple rational expressions, but become increasingly
complicated expressions involving transcendental functions.
(e) Given two rotation matrices R1 and R2 , let R3 = R1 R2 , which is also a rotation matrix.
(i) Multiplying two rotation matrices: By simple calculation, it can be seen that 27 multiplications
and 18 additions are needed. Therefore, a total of 45 arithmetic operations are needed. Note that
by calculating the third column of R3 as the cross-product of its first column and second column, 6
operations can be reduced.
(ii) Multiplying two unit quaternions: Let qi = (ηi , εi ) ∈ R4 , (i = 1, 2, 3) denote the unit quaternion
parameters for Ri , (i = 1, 2, 3), where ηi ∈ R, (i = 1, 2, 3) and εi ∈ R3 , (i = 1, 2, 3). Then q3 is
calculated as follows:
q3 = (η1 η2 − εT1 ε2 , η1 ε2 + η2 ε1 + (ε1 × ε2 )).
It can be seen that 16 multiplications and 12 additions are needed. Therefore, a total of 28 arithmetic
operations are needed.
(iii) Multiplying two Cayley-Rodrigues vectors: Let ri =∈ R3 , (i = 1, 2, 3) denote the Cayley-Rodrigues
parameters for Ri , (i = 1, 2, 3). r3 is then calculated as follows:
r3 =
r1 + r2 + (r1 × r2 )
.
1 − r1T r2
It can be seen that 9 multiplications and 15 additions are needed. Therefore, a total of 24 arithmetic
operations are needed.
Go to the table of contents.
56
Chapter 3. Chapter 3 Solutions
Exercise 3.42.
Programming assignment.
Exercise 3.43.
Programming assignment. Could check how close RT R − I is to the zero matrix, and whether det(R) ≈ 1.
Exercise 3.44.
Programming assignment.
Exercise 3.45.
Programming assignment.
Exercise 3.46.
Programming assignment.
Exercise 3.47.
Programming assignment.
Go to the table of contents.
Chapter 3. Chapter 3 Solutions
57
Exercise 3.48.
Programming assignment.
Exercise 3.49.
Programming assignment.
Go to the table of contents.
58
Chapter 3. Chapter 3 Solutions
Go to the table of contents.
Chapter 4 Solutions
Exercise 4.1.
Programming assignment.
Exercise 4.2.
By inspection M can be obtained as

1 0 0
 0 1 0
M =
 0 0 1
0 0 0

0
`1 + `2 
.

`0
1
The screw axes Si = (ωi , vi ) are listed in the following table:
i
1
2
3
4
ωi
(0, 0, 1)
(0, 0, 1)
(0, 0, 1)
(0, 0, 0)
vi
(0, 0, 0)
(`1 , 0, 0)
(`1 + `2 , 0, 0)
(0, 0, 1)
The screw axes Bi = (ωi , vi ) are listed in the following table:
i
1
2
3
4
ωi
(0, 0, 1)
(0, 0, 1)
(0, 0, 1)
(0, 0, 0)
vi
(−`1 − `2 , 0, 0)
(−`2 , 0, 0)
(0, 0, 0)
(0, 0, 1)
The end-effector configuration T ∈ SE(3) can be found, using the FKinSpace and the FKinBody functions,
as


1 0 0 −1
 0 1 0 1 

T =
 0 0 1 2 .
0 0 0 1
Exercise 4.3.
T (θ)
= M e[B1 ]θ1 e[B2 ]θ2 e[B3 ]θ3
The end-effector zero position configuration M is given by

0 0 1
 0 1 0
R p
M=
=
 −1 0 0
0 1
0 0 0

L1
0 
.
−L2 
1
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
1
2
3
ωi
(−1, 0, 0)
(0, −1, 0)
(0, 0, 1)
qi
(0, 0, −L1 )
(−L2 , 0, 0)
(0, 0, 0)
59
vi
(0, L1 , 0)
(0, 0, L2 )
(0, 0, 0)
60
Chapter 4. Chapter 4 Solutions
Exercise 4.4.
T (θ)
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B6 ]θ6
The end-effector zero position configuration M is given by

1 0
 0 1
R p
M=
=
 0 0
0 1
0 0
0
0
1
0

0
L1 + L2 
.

0
1
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
ωi
(0, 0, 1)
(1, 0, 0)
(0, 0, 0)
(0, 1, 0)
(1, 0, 0)
(0, 1, 0)
qi
(0, −L1 − L2 , 0)
(0, −L1 − L2 , 0)
(0, 0, 0)
(0, −L2 , 0)
(0, 0, 0)
vi
(−L1 − L2 , 0, 0)
(0, 0, L1 + L2 )
(0, 1, 0)
(0, 0, 0)
(0, 0, L2 )
(0, 0, 0)
Exercise 4.5.
Screw axes in the body frame for UR5 Bi :
i
1
2
3
4
5
6
ωi
(0, 1, 0)
(0, 0, 1)
(0, 0, 1)
(0, 0, 1)
(0, −1, 0)
(0, 0, 1)
vi
(W1 + W2 , 0, L1 + L2 )
(H2 , −L1 − L2 , 0)
(H2 , −L2 , 0)
(H2 , 0, 0)
(−W2 , 0, 0)
(0, 0, 0)
Exercise 4.6.
Screw axes in the body frame for the WAM arm Si :
i
1
2
3
4
5
6
7
ωi
(0, 1, 0)
(0, 0, 1)
(0, 1, 0)
(0, 0, 1)
(0, 1, 0)
(0, 1, 0)
(0, 1, 0)
vi
(−H1 + H2 , 0, L1 + L2 )
(−L1 − L2 − L3 + W1 + W2 , −L1 − L2 , 0)
(−H1 + H2 , 0, L1 + L2 )
(−L2 − L3 + W1 + W2 , −L1 − L2 + W1 , 0)
(−H1 + H2 , 0, L1 + L2 )
(−H1 + H2 − L3 , 0, L1 + L2 )
(−H1 + H2 , 0, L1 + L2 )
Exercise 4.7.
By inspection M can be obtained as

1 0 0
 0 1 0

M =
0 0 1
0 0 0

0
L1 + L2 + L3 + L4 
.

h
1
The screw axes Si = (ωi , vi ) are listed in the following table:
Go to the table of contents.
Chapter 4. Chapter 4 Solutions
61
i
1
2
3
4
5
6
ωi
(0, 0, 0)
(0, 0, 1)
(-1, 0, 0)
(-1, 0, 0)
(-1, 0, 0)
(0, 1, 0)
vi
(0, 1, 0)
(L1 , 0, 0)
(0, −h, L1 )
(0, −h, L1 + L2 )
(0, −h, L1 + L2 + L3 )
(−h, 0, 1)
The screw axes Bi = (ωi , vi ) are listed in the following table:
i
1
2
3
4
5
6
ωi
(0, 0, 0)
(0, 0, 1)
(-1, 0, 0)
(-1, 0, 0)
(-1, 0, 0)
(0, 1, 0)
vi
(0, 1, 0)
(−L2 − L3 − L4 , 0, 0)
(0, 0, −L2 − L3 − L4 )
(0, 0, −L3 − L4 )
(0, 0, −L4 )
(0, 0, 0)
Exercise 4.8.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 · · · e[S6 ]θ6 M
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B6 ]θ6
The end-effector zero position configuration M is given by

1 0 0
 0 1 0
R p
M=
=
 0 0 1
0 1
0 0 0

L1
L3 + L4 
.
−L5 − L6 
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
ωi
(1, 0, 0)
(0, 0, −1)
(0, 1, 0)
(1, 0, 0)
(0, 0, 0)
(0, 1, 0)
qi
(0, 0, 0)
(L1 , 0, 0)
(L1 , 0, L2 )
(0, L3 , 0)
(L1 , 0, −L5 )
vi
(0, 0, 0)
(0, L1 , 0)
(−L2 , 0, L1 )
(0, 0, −L3 )
(0, 1, 0)
(L5 , 0, L1 )
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
ωi
(1, 0, 0)
(0, 0, −1)
(0, 1, 0)
(1, 0, 0)
(0, 0, 0)
(0, 1, 0)
qi
(0, −L3 − L4 , L5 + L6 )
(0, −L3 − L4 , 0)
(0, 0, L2 + L5 + L6 )
(0, −L4 , L5 + L6 )
(0, 0, L6 )
vi
(0, L5 + L6 , L3 + L4 )
(L3 + L4 , 0, 0)
(−L2 − L5 − L6 , 0, 0)
(0, L5 + L6 , L4 )
(0, 1, 0)
(−L6 , 0, 0)
Exercise 4.9.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 · · · e[S6 ]θ6 M
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B6 ]θ6
Go to the table of contents.
62
Chapter 4. Chapter 4 Solutions
The end-effector zero position configuration M is given by

0 1
 1 0
R p
M=
=
 0 0
0 1
0 0
0
0
−1
0

0
3L 
.
−2L 
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
wi
(0, 0, 1)
(1, 0, 0)
(0, 0, 0)
(0, 0, 0)
(0, 1, 0)
(0, 0, −1)
qi
(0, 0, 0)
(0, 0, −2L)
(0, 0, −L)
(0, 3L, 0)
vi
(0, 0, 0)
(0, −2L, 0)
(0, 1, 0)
(0, 0, 1)
(L, 0, 0)
(−3L, 0, 0)
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
wi
(0, 0, −1)
(0, 1, 0)
(0, 0, 0)
(0, 0, 0)
(1, 0, 0)
(0, 0, 1)
qi
(−3L, 0, 0)
(−3L, 0, 0)
(0, 0, −L)
(0, 0, 0)
vi
(0, −3L, 0)
(0, 0, −3L)
(1, 0, 0)
(0, 0, −1)
(0, −L, 0)
(0, 0, 0)
Exercise 4.10.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 · · · e[S6 ]θ6 M
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B6 ]θ6
The end-effector zero position configuration M is given by
√


1 0 0 (2 + 3)L

 0 1 0
0√
R p

M=
=
 0 0 1 (1 + 3)L  .
0 1
0 0 0
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
wi
(0, 0, 1)
(0, 1, 0)
(0, 1, 0)
(0, 1, 0)
(0, 0, 0)
(0, 0, 1)
qi
(L, 0, 0)
(L,
√ 0, 0)
((1√+ 3)L,√
0, −L)
((2 + 3)L, 0, ( 3 − 1)L)
√((2 + 3)L, 0, 0)
vi
(0, −L, 0)
(0, 0, L)
√
(L,
0,
√ (1 + 3)L)
√
((1 − 3)L, 0, (2 + 3)L)
(0, 0,√1)
(0, −(2 + 3)L, 0)
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
wi
1
2
3
4
5
6
(0, 0, 1)
(0, 1, 0)
(0, 1, 0)
(0, 1, 0)
(0, 0, 0)
(0, 0, 1)
qi
√
(−(1
√ + 3)L, 0, 0)√
(−(1 + 3)L, 0, −(1
√ + 3)L)
(−L, 0, −(2 + 3)L)
(0, 0, −2L)
(0, 0, 0)
vi
√
(0,
√ (1 + 3)L, 0)√
((1 + 3)L,
√ 0, −(1 + 3)L)
((2 + 3)L, 0, −L)
(2L, 0, 0)
(0, 0, 1)
(0, 0, 0)
Go to the table of contents.
Chapter 4. Chapter 4 Solutions
63
Exercise 4.11.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 · · · e[S5 ]θ5 M
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B5 ]θ5
The end-effector zero position configuration M is given by

1
 0
R p
M=
=
 0
0 1
0
0
1
0
0
0
0
1
0

3
0 
.
0 
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
ωi
(0, 0, 1)
(0, 0, 0)
(0, 0, 1)
(0, −1, 0)
1
1
(− √ , 0, √ )
2
2
qi
(0, 0, 0)
(1, 0, 0)
(1, 0, −1)
(1, 0, 0)
vi
(0, 0, 0)
(1, 0, 0)
(0, −1, 0)
(−1, 0, −1)
1
(0, − √ , 0)
2
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
ωi
(0, 0, 1)
(0, 0, 0)
(0, 0, 1)
(0, −1, 0)
1
1
(− √ , 0, √ )
2
2
qi
(−3, 0, 0)
(−2, 0, 0)
(−2, 0, −1)
(−2, 0, 0)
vi
(0, 3, 0)
(1, 0, 0)
(0, 2, 0)
(−1, 0, 2)
√
(0, 2, 0)
Exercise 4.12.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 · · · e[S6 ]θ6 M
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B6 ]θ6
The end-effector zero position configuration M is given by

1 0 0

R p
0 1 0
M=
=
 0 0 1
0 1
0 0 0

L√
(4 +√ 2)L 
.
− 2L 
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
wi
(0, 1, 0)
(0, 0, 1)
(0, 0, 0)
(0, 1, 0)
1
1
(0, √ , − √ )
2
2
(1, 0, 0)
qi
(0, 0, 0)
(0, L, 0)
(0, 0, L)
(0, 4L, L)
√
√
(0, (4 + 2)L, − 2L)
Go to the table of contents.
vi
(0, 0, 0)
(L, 0, 0)
(0, 1, 0)
(−L, 0, 0)
5
(− √ L, 0, 0)
√ 2
√
(0, − 2L, −(4 + 2)L)
64
Chapter 4. Chapter 4 Solutions
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
wi
1
2
3
4
(0, 1, 0)
(0, 0, 1)
(0, 0, 0)
(0, 1, 0)
1
1
(0, √ , − √ )
2
2
(1, 0, 0)
5
6
qi
√
(−L, 0, √2L)
(−L, −(3 + 2)L, 0)
- √
(−L, 0, (1 + 2)L)
(−L, 0, L)
(0, 0, 0)
√
vi
(− 2L,
√ 0, −L)
(−(3 + 2)L, L, 0)
(0,
√ 1, 0)
(−(1 + 2)L, 0, −L)
1
1
1
(− √ L, − √ L, − √ L)
2
2
2
(0, 0, 0)
Setting θ5 = π and all other joint variables to zero, e[S1 ]θ1 = e[S2 ]θ2 = e[S3 ]θ3 = e[S4 ]θ4 = e[S6 ]θ6 = I, while
e[S5 ]θ5 can be expressed by following formula:
[ω]θ
e
Iθ + (1 − cos θ)[ω] + (θ − sin θ)[ω]2 v
[S]θ
e
=
.
0
1
Then e[S5 ]θ5 becomes
−1
 0
[S5 ]θ5
e
=
 0
0
0
0
−1
0
0
−1
0
0

0
5L 
.
5L 
1
−1

0
T06 = e[S5 ]θ5 M = 
 0
0
0
0
−1
0
0
−1
0
0

−L
√
(5 + √2)L 
,
(1 − 2)L 
1

Hence T06 becomes

−1
and T60 is T06
.
Exercise 4.13.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 · · · e[S6 ]θ6 M
= M e[B1 ]θ1 e[B2 ]θ2 · · · e[B6 ]θ6
The end-effector zero position configuration M is given by

−1 0 0
 0 1 0
M =
 0 0 −1
0 0 0

0
4 
.
1 
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
wi
(0, 0, 1)
(1, 0, 0)
(1, 0, 0)
(0, 0, 0)
1
1
(0, √ , √ )
2
2
(0, 0, −1)
qi
(0, 0, 0)
(0, 0, 2)
(0, 1, 2)
(0, 1, 0)
(0, 4, 0)
vi
(0, 0, 0)
(0, 2, 0)
(0, 2, −1)
(0, 1, 0)
1
( √ , 0, 0)
2
(−4, 0, 0)
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
Go to the table of contents.
Chapter 4. Chapter 4 Solutions
65
frame i
1
2
3
4
wi
(0, 0, −1)
(−1, 0, 0)
(−1, 0, 0)
(0, 0, 0)
1
1
(0, √ , − √ )
2
2
(0, 0, 1)
5
6
qi
(0, −4, 0)
(0, −4, −1)
(0, −3, −1)
(0, −1, −1)
(0, 0, 0)
vi
(4, 0, 0)
(0, 1, −4)
(0, 1, −3)
(0, 1, 0)
√
( 2, 0, 0)
(0, 0, 0)
Exercise 4.14.
By inspection M can be obtained as:

−1
 0
M =
 0
0
0
1
0
0

0
0
0 L0 + L2 
.
−1 L1 − L3 
0
1
The screw axes Si = (ωi , vi ) are listed in the following table:
i
1
2
3
ωi
(0, 0, 1)
(0, 0, 0)
(0, 0, −1)
vi
(L0 , 0, 0)
(0, 1, 0)
(−L0 − L2 , 0, h)
where L0 = 4, L1 = 3, L2 = 2, L3 = 1, and h = 0.1.
The screw axes Bi = (ωi , vi ) are listed in the following table:
i
1
2
3
ωi
(0, 0, −1)
(0, 0, 0)
(0, 0, 1)
vi
(L2 , 0, 0)
(0, 1, 0)
(0, 0, −h)
Using FKinSpace and FKinBody should give the following configuration T :


 
0 1 0
−3 − L2
0 1 0
−5
 1 0 0
  1 0 0
L0
4 

 
T =
 0 0 −1 πh + L1 − L3  =  0 0 −1 2.314  .
0 0 0
1
0 0 0
1
Exercise 4.15.
T (θ)
= e[S1 ]θ1 e[S2 ]θ2 e[S3 ]θ3 M
= M e[B1 ]θ1 e[B2 ]θ2 e[B3 ]θ3
The end-effector zero position configuration M is given by

1 0
 0 1
R p
M=
=
 0 0
0 1
0 0
0
0
1
0

0
L2 + L4 
.
−L3 
1
Note that the first joint of the robot is a screw joint with nonzero pitch h. Hence, for frame {1} we should
use v = −ω × q + hω.
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
Go to the table of contents.
66
Chapter 4. Chapter 4 Solutions
frame i
1
2
3
ωi
(0, 0, 1)
(0, 1, 0)
(1, 0, 0)
qi
(0, 0, 0)
(0, 0, 0)
(0, L2 , −L3 )
vi
(0, 0, h)
(0, 0, 0)
(0, −L3 , −L2 )
The values of the screw parameters Bi = (ωi , vi ) are listed in the following table:
frame i
1
2
3
ωi
(0, 0, 1)
(0, 1, 0)
(1, 0, 0)
qi
(0, −L2 − L4 , 0)
(0, 0, L3 )
(0, −L4 , 0)
vi
(−L2 − L4 , 0, h)
(−L3 , 0, 0)
(0, 0, L4 )
Exercise 4.16.
The forward kinematics of a four-dof open chain manipulator in its zero position is written in the following
exponential form:
T04 (θ1 , θ2 , θ3 , θ4 ) = e[A1 ]θ1 e[A2 ]θ2 M e[A3 ]θ3 e[A4 ]θ4 .
Substituting θi = θ0 i + αi (i = 1, · · · , 4) into the above formula, the forward kinematics is of the form:
T04 (θ0 1 , θ0 2 , θ0 3 , θ0 4 )
0
0
0
0
= e[A1 ](θ 1 +α1 ) e[A2 ](θ 2 +α2 ) M e[A3 ](θ 3 +α3 ) e[A4 ](θ 4 +α4 )
0
0
0
0
= e[A1 ]θ 1 e[A1 ]α1 e[A2 ]θ 2 e[A2 ]α2 M e[A3 ]α3 e[A3 ]θ 3 e[A4 ]α4 e[A4 ]θ 4
0
0
0
0
0
0
= e[A1 ]θ 1 e[A 2 ]θ 2 e[A1 ]α1 e[A2 ]α2 M e[A3 ]α3 e[A4 ]α4 e[A 3 ]θ 3 e[A4 ]θ 4 ,
where A0 2 = [Ade[A1 ]α1 ]A2 and A0 3 = [Ade−[A4 ]α4 ]A3 .
∴ [A0 1 ]
=
[A1 ]
[A0 2 ]
= e[A1 ]α1 [A2 ]e−[A1 ]α1
[A0 3 ]
= e−[A4 ]α4 [A3 ]e[A4 ]α4
[A0 4 ]
=
M0
= e[A1 ]α1 e[A2 ]α2 M e[A3 ]α3 e[A4 ]α4
[A4 ]
Exercise 4.17.
(a) As given in the problem, the forward kinematics of the manipulator is expressed as Tb1 b2 = e[S1 ]θ1 e[S2 ]θ2 · · · e[S5 ]θ5 M ,
with the end-effector 1 grasping the tree. Let Si be the screw parameter of the ith joint expressed in
the {b1 } frame, and M ∈ SE(3) be the displacement from the {b1 } frame to the {b2 } frame. M and
Si can be derived as follows:

M=
R
0
p
1
1
 0
=
 0
0
0
0
1
0
0
−1
0
0
Go to the table of contents.

4L
L 

L 
1
Chapter 4. Chapter 4 Solutions
67

S1
:
S2
:
S3
:
S4
:
S5
:





0
L
0
w1 =  0  , q1 =  0  , v1 =  −L 
1
0
0






1
0
0
w2 =  0  , q2 =  L  , v2 =  0 
0
0
−L






0
L
L
w3 =  −1  , q3 =  0  , v3 =  0 
0
L
−L






0
2L
L
w4 =  0  , q4 =  L  , v4 =  −2L 
1
0
0






0
3L
L
w5 =  −1  , q5 =  0  , v5 =  0  .
0
L
−3L
(b) Here end-effector 2 is rigidly grasping a tree; the forward kinematics of the manipulator is expressed
as
Tb2 b1 = e[A5 ]θ5 e[A4 ]θ4 e[A3 ]θ3 N e[A2 ]θ2 e[A1 ]θ1 .
(4.1)
This equation can be modified as follows:
0
0
Tb2 b1 = e[A5 ]θ5 e[A4 ]θ4 e[A3 ]θ3 e[A2 ]θ2 e[A1 ]θ1 N.
A01 − A5 are the screw parameters of the ith joint expressed in the {b2 } frame, and N ∈ SE(3) is the
displacement from the {b2 } frame to the {b1 } frame. Therefore, N and A01 − A5 can be derived as
follows:


1
0 0 −4L
 0
0 1 −L 

N =
 0 −1 0
L 
0
0 0
1

A5
:
A4
:
A3
:
A02
:





0
−L
0
w5 =  0  , q5 =  0  , v5 =  L 
1
0
0
 




0
−2L
0
w4 =  1  , q4 =  0  , v4 =  0 
0
0
−2L






0
−3L
0
w3 =  0  , q3 =  0  , v3 =  3L 
1
0
0






1
0
0
w2 =  0  , q2 =  −L  , v2 =  0 
0
0
L

A01





0
−3L
−L
: w1 =  1  , q1 =  0  , v1 =  0  .
0
L
3L
Now if we move N as below and compare it with Equation (4.1), we get the following:
Tb2 b1 = e[A5 ]θ5 e[A4 ]θ4 e[A3 ]θ3 N eN
−1
[A02 ]N θ2 N −1 [A01 ]N θ1
Go to the table of contents.
e
68
Chapter 4. Chapter 4 Solutions
[A2 ] = N −1 [A02 ]N
[A1 ] = N −1 [A01 ]N






1
0
0
: w2 =  0  , q2 =  L  , v2 =  0 
0
0
−L






0
L
0
: w1 =  0  , q1 =  0  , v1 =  −L  .
1
0
0
A2
A1
Exercise 4.18.
(a) As given in the problem, the forward kinematics of the robot A is expressed as
TAa = e[S1 ]θ1 e[S2 ]θ2 e[S3 ]θ3 e[S4 ]θ4 e[S5 ]θ5 Ma ,
Si is the screw parameter of the ith joint expressed in the {A} frame, and Ma ∈ SE(3) is the displacement from the {A} frame to the {a} frame at the zero position. Therefore, Ma and Si are derived as
follows:


1 0 0 L2
 0 1 0 0 

M =
 0 0 1 L1  .
0 0 0 1



0
0
w1 =  0  , v1 =  0 
1
0






0
0
0
w2 =  0  , q2 =  0  , v2 =  0 
0
0
1






L1
0
0
w3 =  −1  , q3 =  0  , v3 =  0 
L1
0
0




1
0
w4 =  0  , v4 =  0 
0
0






0
0
1
w5 =  0  , q5 =  0  , v5 =  L1  .
L1
0
0

S1
:
S2
:
S3
:
S4
:
S5
:
(b) Because robots A and B are the same robot, TBb can be expressed as
TBb = e[S1 ]φ1 e[S2 ]φ2 e[S3 ]φ3 e[S4 ]φ4 e[S5 ]φ5 Ma ,
where Si and Ma are the same as in (a). The displacement TAB from the {A} frame to the {B} frame,
and the displacement Tab from the {a} frame to the {b} frame, are derived as follows:


−1 0 0 L3
 0 −1 0 L4 
,
TAB = 
 0
0 1 0 
0
0 0 1


−1 0 0 0
 0 −1 0 0 
.
Tab = Tba = 
 0
0 1 0 
0
0 0 1
Go to the table of contents.
Chapter 4. Chapter 4 Solutions
69
Using the relation TAa = TAB TBb Tba ,
TAa
e
[S1 ]θ1 [S2 ]θ2
e
···e
[S5 ]θ5
Ma
e[S1 ]θ1 e[S2 ]θ2 · · · e[S5 ]θ5 Ma
(∵ P eS
e
−[B5 ]φ5
e
[S1 ]θ1 [S2 ]θ2
···e
e
[S5 ]θ5
···e
Ma
· · · e[S5 ]θ5
e
−[B1 ]φ1 [S1 ]θ1
=
TAB TBb Tba
= TAB e[S1 ]φ1 e[S2 ]φ2 · · · e[S5 ]φ5 Ma Tab ,
−1
−1
= eTAB [S1 ]TAB φ1 · · · eTAB [S5 ]TAB φ5 TAB Ma Tba ,
= eP SP
−1
P)
[B1 ]φ1 [B2 ]φ2
= e
e
· · · e[B5 ]φ5 TAB Ma Tba ,
= TAB Ma Tba Ma−1 .
−1
where [Bi ] = TAB [Si ]TAB
or Bi = AdTAB (Ai ), and M = TAB Ma Tba Ma−1 . The Bi are the screw
parameters of robot B as seen from {A}:
 
 
0
0



0
B1 : w1 =
, v1 = 0 
0
1
 




0
L3
L4
B2 : w2 =  0  , q2 =  L4  , v2 =  −L3 
1
0
0
 




0
L3
−L1
B3 : w3 =  1  , q3 =  0  , v3 =  0 
0
L1
L3
 


0
−1
B4 : w4 =  0  , v4 =  0 
0
0






−1
0
0
B5 : w5 =  0  , q5 =  L4  , v5 =  −L1 
0
L1
L4
M
= TAB Ma Tba Ma−1


−1 0 0 L3
1 0 0
 0 −1 0 L4   0 1 0

= 
 0
0 1 0  0 0 1
0
0 0 1
0 0 0


1 0 0 −2L2 + L3
 0 1 0

L4
.
= 
 0 0 1

0
0 0 0
1

L2
−1
 0
0 

L1   0
0
1
0
−1
0
0

1
0 0
 0
0 0 

1 0  0
0 1
0
0
1
0
0
0
0
1
0

−L2
0 

−L1 
1
Exercise 4.19.
(a) To derive the forward kinematics in the given form, it is convenient to use the Denavit-Hartenberg
parameters. However, we are not able to find the Denavit-Hartenberg parameters for the given link
reference frames. Correct link frames for finding the Denavit-Hartenberg parameters are given in
Figure (4.1). Using frame {3} of the third link, the corresponding Denavit-Hartenberg parameters are
as follows:
i
1
2
3
4
5
αi−1
− π2
− π2
− π2
π
2
− π2
ai−1
0
L
0
0
0
di
2L
0
L + θ3
−2L
L
θi
θ1
θ2 − π2
π
2
θ4
θ5 − π2
Go to the table of contents.
.
70
Chapter 4. Chapter 4 Solutions
ẑ1
ŷ1
x̂1
L
{3}
L
x̂3 ŷ
ẑ3
3
θ1
L
θ2
x̂2 ẑ2
ŷ2
or
ŷ3 x̂
3 {3´}
ẑ3
θ3
θ4
L
L
ẑ4
ŷ0
x̂5
x̂4
x̂0
ẑ0
ŷ4
θ5
{s}
ŷ5
ẑ5
{b}
Figure 4.1
Otherwise, using the frame {30 } of the third link, the corresponding Denavit-Hartenberg parameters
for i = 3, 4 transform as follows:
i
3
4
αi−1
− π2
− π2
ai−1
0
0
di
L + θ3
−2L
θi
− π2
θ4 + π
Using the Denavit-Hartenberg parameters derived above,
M2
M3
A2
A3
π
π
Rot x̂, −
· Trans (x̂, L) · Rot ẑ, −
2
2


0 1 0 L
 0 0 1 0 

= 
 1 0 0 0 ,
0 0 0 1
π
π
· Trans (ẑ, L) · Rot ẑ,
= Rot x̂, −
2
2



0 −1 0 0
0
 0


0
1
L
0

 0
= 
 −1 0 0 0  or using the frame {3 },  1
0
0 0 1
0
T
0 0 1 0 0 0
=
,
T
0 0 0 0 0 1
=
.
=
Go to the table of contents.
1
0
0
0
0
1
0
0

0

L 
 ,
0 
1
Chapter 4. Chapter 4 Solutions
71
(b) From the given forward kinematics in (a),
Tsb
= M1 e[A1 ]θ1 M2 e[A2 ]θ2 · · · M5 e[A5 ]θ5
=
M1 e[A1 ]θ1 M1−1 M1 M2 e[A2 ]θ2 M2−1 M1−1
· · · M1 · · · M5 e[A5 ]θ5 M5−1 · · · M1−1 (M1 · · · M5 )
−1
= eM1 [A1 ]M1 θ1 e(M1 M2 )[A2 ](M1 M2 )
= e
[S1 ]θ1 [S2 ]θ2
e
···e
[S5 ]θ5
−1
θ2
M.
· · · e(M1 ···M5 )[A5 ](M1 ···M5 )
−1
θ5
(M1 · · · M5 )
Therefore,
S1
=
S2 =
..
.
AdM1 M2 (A2 ) ,
S5
=
AdM1 ···M5 (A5 ) ,
AdM1 (A1 ) ,
= M1 · · · M 5 .
M
Exercise 4.20.
The end-effector frame {b} as seen from the fixed frame {0} is

0 1
1 0
M =
0 0
0 0
0
0
−1
0

−3L
−L 
.
−2L
1
The values of the screw parameters Si = (ωi , vi ) are listed in the following table:
frame i
1
2
3
4
5
6
ωi
(0, 0, 0)
(0, 0, 1)
(0, −1, 0)
(0, 0, 0)
(1, 0, 0)
(0, 0, 1)
qi
(0, 0, 0)
(−L, 0, L)
(0, 0, L)
(0, 0, 0)
vi
(0, 0, 1)
(0, 0, 0)
(L, 0, L)
(−1, 0, 0)
(0, L, 0)
(0, 0, 0)
Notice that the last row of above table corresponds to the screw parameters as seen from the end-effector
frame {b}. The forward kinematics is written in the following exponential form:
0
T06 = e[S1 ]θ1 · · · e[S5 ]θ5 e[S6 ]θ6 M = e[S1 ]θ1 · · · e[S5 ]θ5 M eM
−1
0
[S6 ]M θ6
= e[S1 ]θ1 · · · e[S5 ]θ5 M e[S6 ]θ6
0
where [S6 ] = M −1 [S6 ]M .
0
[S6 ] = M −1 [S6 ]M can be verified using following matrices:

0
1
[S6 ] = 
0
0
−1
0
0
0

0 0
0 0
,
0 0
0 0

0
−1
0
[S6 ] = 
0
0

1 0
L
0 0 −3L
,
0 0
0 
0 0
0
Go to the table of contents.

0
1
−1
M =
0
0
1
0
0
0
0
0
−1
0

L
3L 
.
−2L
1
72
Chapter 4. Chapter 4 Solutions
Exercise 4.21.
T
=
Rot(x̂, α)Trans(x̂, a)Trans(ẑ, d)Rot(ẑ, φ)


cos φ
− sin φi
0
a
 sin φ cos α cos φ cos α − sin α −d sin α 

= 
 sin φ sin α cos φ sin α cos α
d cos α 
0
0
0
1
(a)

0
 1
T =
 0
0
(b)
1
0
1
0
1
0
0
0

3
0 
 = Ra
1 
0
1
pa
1
Since Ra ∈
/ SO(3), there is no solution for this T .

cos β
 sin β

T =
0
0
sin β
− cos β
0
0
0
0
−1
0

1
0 
 = Rb
0
−2 
1
pb
1
By a correspondence with Equation (4.3),
α
= π
a
=
1
d =
2
φ
= −β
(∵ cos φ = cos β,
sin φ = − sin β).
(c)

0
 0
T =
 1
0
−1
0
0
0
0
−1
0
0

−1
0 
 = Rc
2 
0
1
By a correspondence with Equation (4.3),
cos α
=
0
d cos α
=
2,
from which it follows that there is no solution for this T .
Go to the table of contents.
pc
1
(4.2)
(4.3)
Chapter 5 Solutions
Exercise 5.1.
(a) Given a rolling rate ω = 1 and radius r = 1, v = rω = 1, so that the rotation angle around the −ẑs -axis
is θ = ωt = t. Therefore
Rsb psb
Tsb =
0
1


cos t sin t 0 t + cos t
 − sin t cos t 0 1 − sin t 

= 


0
0
1
0
0
0
0
1
−1
= Ṫsb Tsb

0 1
 −1 0
= 
 0 0
0 0


0
 0 


 −1 
.

Vs (t) = 

 0 
 t 
0
[Vs (t)]
0
0
0
0

0
t 

0 
0
(b) Position of the {b} frame origin: psb = (t + cos t, 1 − sin t, 0)T .
Linear velocity of the {b} frame origin: ṗsb = (1 − sin t, − cos t, 0)T .
Exercise 5.2.
(a) The planar space Jacobian Js (θ) can be computed and written in matrix form as follows:


1 1
1√
1/ 2√  .
Js (θ) =  0 0
0 −1 −1 − 1/ 2
The planar wrench expressed in the space frame, Fs :


0
ms
Fs =
=  5  ∈ R3 .
fs
0
So the set of joint torques τ can be obtained as
√ 


 
1
0
0
0
−5/√2
0√
−1 √   5  =  −5/ 2  .
τ = JsT (θ)Fs =  1
0
1 1/ 2 −1 − 1/ 2
0
(b) Similarly to (a), the set of joint torques τ can be obtained as
√ 


 
1
0
0
0
10 + 5/√ 2
0√
−1 √   0  =  5 + 5/ 2  .
τ = JsT (θ)Fs =  1
5
1 1/ 2 −1 − 1/ 2
5
Exercise 5.3.
73
74
Chapter 5. Chapter 5 Solutions
(a) By inspection M ∈ SE(2) can be obtained as


1 0 L1 + L2 + L3 + L4
.
0
M = 0 1
0 0
1
The screw axes Si = (ωi , vi ) ∈ R3 are listed in the following table:
i
1
2
3
4
ωi
1
1
1
1
vi
(0, 0)
(0, −L1 )
(0, −L1 − L2 )
(0, −L1 − L2 − L3 )
(b) The planar body Jacobian Jb (θ) can be computed by either inspection in the body frame or tranforming
from the planar space Jacobian. The answer can be written in matrix form as follows:


1
1
1
1
L3 s4 + L2 s34 + L1 s234
L3 s4 + L2 s34
L3 s4
0 .
Jb (θ) = 
L4 + L3 c4 + L2 c34 + L1 c234 L4 + L3 c4 + L2 c34 L4 + L3 c4 L4
(c) The space Jacobian Js (θ) in the the configuration θ1 = θ2 = 0, θ3 = π2 , θ4 = − π2 can be computed and
written in matrix form as follows:


0
0
0
0
 0

0
0
0


 1

1
1
1
.
Js (θ) = 
 0

0
0
L3


 0 −L1 −L1 − L2 −L1 − L2 
0
0
0
0
The wrench expressed in the space frame, Fs :

0


 0 
 10 
ms

Fs =
=
 10  .
fs


 10 
0

So the set of joint torques τ can be obtained as


0 0 1
 0 0 1
0 0 1
0 0 1
τ = JsT (θ)Fs = 

0
0
0
L3
0
−L1
−L1 − L2
−L1 − L2

0



0 
1
 0 




0 
1 − L1
  10  = 10 
.





0  10 
1 − L1 − L2


0
10
1 − L1 − L2 + L3
0

(d) Similarly to (a), the set of joint torques τ can be obtained as


0





0 0 1 0
0
0 
1
 0 
 0 0 1 0




−L1
0 
1 + L1
  −10 

.
τ = JsT (θ)Fs = 

 0 0 1 0 −L1 − L2 0   −10  = −10 
1 + L1 + L2




0 0 1 L3 −L1 − L2 0
10
1 + L1 + L2 + L3
0
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
75
(e) Mathematically a singular posture is one in which the Jacobian J(θ) fails to be of maximal rank. In
this case, based on Js (θ), S1 is linearly independent of S2 , S3 and S4 . So at any singularity, S2 , S3
and S4 should be linear dependent, which leads to θ2 = θ3 = θ4 = kπ,k ∈ Z.
Exercise 5.4.
(a) The displacements from the contact frames to the can frame are given by

Tb1 b
=
=
Tb2 b
=
=

1 0 0 0
 0 1 0 L 


 0 0 1 0 
0 0 0 1
Rb1 b pb1 b
0
1


0 −1 0 L
 1 0 0 0 


 0 0 1 0 
0 0 0 1
Rb2 b pb2 b
.
0
1
The total spatial force applied to the can expressed in {b} coordinates is
Fb
=
=
=
=
[AdTb1 b ]T Fb1 + [AdTb2 b ]T Fb2




0
0




 0 
 0  T
T
T
T



R
R
[p
]
Rb1 b RbT1 b [pb1 b ]T 
b2 b
b2 b b2 b
 0 
 0 +
T
T




0
Rb2 b
0
Rb1 b
 fb2 x 
 fb1 x 
 fb2 y 
 fb1 y 
fb2 z
fb1 z


 


0
0
0 1 0 0
0 L
1 0 0 0 0 −L


 0 1 0 0 0 0   0   −1 0 0 0
0
0 
 0 
 


 0 0 1 L 0 0   0   0 0 1 0 −L 0   0 


 




 0 0 0 1 0 0   fb1 x  +  0 0 0 0
1
0 
  fb2 x 
 


 0 0 0 0 1 0   fb1 y   0 0 0 −1 0
0   fb2 y 
fb2 z
fb1 z
0 0 0 0
0
1
0 0 0 0 0 1


−Lfb1 z + Lfb2 z


0


 Lfb1 x − Lfb2 y 

.


fb1 x + fb2 y




fb1 y − fb2 x
fb1 z + fb2 z
(b) From the above it can be seen that the second element of Fb is 0. Therefore, moments about the y-axis
cannot be resisted.
Exercise 5.5.
(a) Since x̂b = (cos θ)x̂s + (sin θ)ŷs and ŷb = (− sin θ)x̂s + (cos θ)ŷs ,
pP = Lx̂s + Lŷs − dŷb = (L + d sin θ)x̂s + (L − d cos θ)ŷs .
(b) ṗP = (dθ̇ cos θ, dθ̇ sin θ, 0)T .
Go to the table of contents.
76
Chapter 5. Chapter 5 Solutions
(c)

− sin θ
cos θ
0
0
cos θ
 sin θ
Tsb = 
 0
0

0 L + d sin θ
0 L − d cos θ 
.

1
0
0
1
(d)

−θ̇ sin θ
 θ̇ cos θ
Ṫsb = 

0
0
−θ̇ cos θ
−θ̇ sin θ
0
0


dθ̇ cos θ
cos θ


−
sin θ
dθ̇ sin θ 
−1
, Tsb
=


0
0
0
0
0
0
0
0
sin θ
cos θ
0
0

0
−L(cos θ + sin θ)
0 −L(cos θ − sin θ) + d 
.

1
0
0
1

0
 0 
dθ̇
 


0 
 ⇒ Vb =  θ̇  = ωb .
 dθ̇ 
vb
0 


 0 
0
0


−θ̇
0
0
0
0

θ̇
−1
[Vb ] = Tsb
Ṫsb = 
 0
0
0
0
0
0

(e)

0
 0 
Lθ̇

  θ̇ 

ωs
−Lθ̇ 


=
⇒
V
=
.
s
 Lθ̇ 
vs
0 


 −Lθ̇ 
0
0


0
 θ̇
−1
[Vs ] = Ṫsb Tsb
=

0
0
−θ̇
0
0
0
0
0
0
0

−1
(f) [Vs ] = Tsb [Vb ]Tsb
.
−1
(g) Since ṗP = ṗsb , Rsb
ṗP = vb .
−1
(h) From Ṙsb Rsb = ωs , −ωs pP + ṗP = vs .
Exercise 5.6.
(a) Given θ1 = t, θ2 = t, θ̇1 = θ̇2 = 1, the problem asks for the linear velocity and angular velocity
expressed in {b} frame coordinates. First find the spatial body velocity Vb as follows:
Vb = Jb (θ)θ̇
Jb (θ) =
Vb1 (θ) Vb2 (θ)
where




sin θ2
0
 cos θ2

 0 






 1 
0




Vb1 (θ) = 
 , Vb2 (θ) =  0 
 −20 cos θ2 


 20 sin θ2 
 10 
−10 cos θ2
0

sin t
 cos t


0
∴ Vb = Jb (θ)θ̇ = 
 −20 cos t

 20 sin t
−10 cos t



0
sin t


0 
cos t
 
 


1  1
1

 = ωb .
=
 −20 cos t 
0 
vb
 1



 20 sin t + 10 
10
0
−10 cos t
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
77
Therefore


sin t
ωb =  cos t 
1


−20 cos t
vb =  20 sin t + 10  .
−10 cos t
(b) The linear velocity of the rider in the fixed frame {s} coordinates is ṗ. Then ṗ = Rsb vb , which expanded
becomes




cos t 0 sin t
cos t − sin t 0
−20 cos t
0
1
0   sin t cos t 0   20 sin t + 10 
ṗ(t) = 
− sin t 0 cos t
0
0
1
−10 cos t


−20 cos t − 20 cos t sin t
.
10 cos t
= 
20 sin t + 10 sin2 t − 10 cos2 t
Exercise 5.7.
(a) The forward kinematics of the RRP robot is expressed as T (θ) = e[S1 ]θ1 e[S2 ]θ2 e[S3 ]θ3 M with the screw
axes in the space frame:



 
0
0
0
ω1 =  0  , q1 =  0  , v1 =  0 
0
0
1

 



0
0
1
ω2 =  0  , q2 =  0  , v2 =  2 
0
2
0




0
0
ω3 =  0  , v3 =  1 
0
0

S1 :
S2 :
S3 :

−1
 0
M =
 0
0

0 0 0
0 1 3 
.
1 0 2 
0 0 1
When θ = (90◦ , 90◦ , 1), the forward kinematics can be evaluated as follows:


0 −1 0 0
1 0
 1 0 0 0  0 0



T (θ) = 
0 0 1 0  0 1
0 0 0 1
0 0


0 1 0 0
 −1 0 0 0 

= 
 0 0 1 6 .
0 0 0 1
0
−1
0
0

0
1

2 
 0
2  0
1
0
0
1
0
0
0
0
1
0

0
−1

1 
 0
0  0
1
0

0 0 0
0 1 3 

1 0 2 
0 0 1
In this configuration, the arm and the end-effector frame are shown in Figure 5.1. In order to obtain
Go to the table of contents.
78
Chapter 5. Chapter 5 Solutions
x̂b
ŷb
ẑb
Q {b}
θ3
3
θ2
θ1
2
ẑs
{s}
ŷs
x̂s
Figure 5.1
the space Jacobian Js (θ) at this configuration,
 
 
 
0
0
0
Vs1 (θ) :
ωs1 =  0  , q1 =  0  , vs1 =  0 
0
0
1






0
0
−2
Vs2 (θ) :
ωs2 =  1  , q2 =  0  , vs2 =  0 
0
2
0




0
0
Vs3 (θ) :
ωs3 =  0  , vs3 =  0 
0
1

0
 0

 1
∴ Js (θ) = 
 0

 0
0
0
1
0
−2
0
0

0
0 

0 
.
0 

0 
1
(b) The forward kinematics of the RRP robot is expressed as T (θ) = M e[B1 ]θ1 e[B2 ]θ2 e[B3 ]θ3 with the screw
axes in the end-effector body frame.






0
0
3
B1 :
ω1 =  1  , q1 =  0  , v1 =  0 
0
−3
0






−1
0
0
B2 :
ω2 =  0  , q2 =  0  , v2 =  3 
0
−3
0




0
0
B3 :
ω3 =  0  , v3 =  0 
0
1
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
79

−1
 0

M =
0
0

0 0 0
0 1 3 
.
1 0 2 
0 0 1
When θ = (90◦ , 90◦ , 1), the forward kinematics can be evaluated as follows:




1
1 0 0 0
0 0 1 3
−1 0 0 0
 0 0 1 3  0 1 0 0  0 0 1 3  0



T (θ) = 
 0 1 0 2   −1 0 0 −3   0 −1 0 −3   0
0
0 0 0 1
0 0 0 1
0 0 0 1


0 1 0 0
 −1 0 0 0 

= 
 0 0 1 6 ,
0 0 0 1
0
1
0
0
0
0
1
0

0
0 

1 
1
which are the same as the results obtained in (a).
In order to obtain the space Jacobian Jb at this configuration,






0
0
0
Vb1 (θ) :
ωb1 =  0  , q1 =  0  , vb1 =  0 
0
0
1

 



0
0
−1
Vb2 (θ) :
ωb2 =  0  , q2 =  0  , vb2 =  3 
0
−3
0




0
0
Vb3 (θ) :
ωb3 =  0  , vb3 =  0 
1
0

0
 0

 1
∴ Jb (θ) = 
 0

 0
0
−1
0
0
0
3
0

0
0 

0 
.
0 

0 
1
Exercise 5.8.
(a) The space Jacobian Js (θ) can be computed and written in matrix form as follows:


0 0
sin θ1
 1 0

0


 0 0

cos θ1

.
Js (θ) = 

 0 0 (2L + θ2 ) cos θ1 
 0 1

0
0 0 −(2L + θ2 ) sin θ1
(b) In the zero position, The body Jacobian Jb (θ) can be computed and written in matrix form as follows:


0 0 0
 1 0 0 


 0 0 1 

Jb (θ) = 
 0 0 −L  .


 0 1 0 
0 0 0
Go to the table of contents.
80
Chapter 5. Chapter 5 Solutions
Suppose an external force f = (fx , fy , fz )T ∈ R3 , which is applied to the {b} frame origin, can be
resisted by the manipulator. The the set of joint torques τ should satisfy:


0


 0  

0
0 1 0 0 0 0 


0  
fy  .
τ = JbT (θ)Fb =  0 0 0 0 1 0  
 fx  =


0 0 1 −L 0 0 
−Lf
x
fy 
fz
In this case, there is zero torques from the manipulator, which means fy = fx = 0. So the external
force can be obtained as f = (0, 0, fz )T , which means force along the ẑ-axis can be resisted by the
manipulator with zero torques.
Exercise 5.9.
We consider only the rotation component; in this case Js (θ) = ṘRT :


0
Vs1 (θ) :
ωs1 =  0 
1
  cos θ 

Vs2 (θ) :
√1
2
√ 1
√1
2
√1
2
 √2θ1 
ωs2 = Rot(ẑ, θ1 )  0  =  sin

2

  cos θ1
1 sin θ2
+ cos θ12cos θ2 − sin θ√
1
2
2


1 sin θ2
ωs3 = Rot(ẑ, θ1 )e[ω̂2 ]θ2  0  =  sin2θ1 + sin θ12cos θ2 + cos θ√

2
1
0
− cos θ2

Vs3 (θ) :
2
2
where
e[ω̂2 ]θ2
=
2
I + sin θ2 [ω̂2 ] + (1 − cos θ2 )[ω̂2 ] .
From above the space Jacobian Js can be expressed as follows:


cos θ1
1 sin θ2
√ θ1
0 cos
+ cos θ12cos θ2 − sin θ√
2
2
2

sin θ1
1 sin θ2 
√ θ1
+ sin θ12cos θ2 + cos θ√
Js (θ) =  0 sin

2
2
2
cos
θ
1
2
√1
1
−
2
2
2
det(Js ) = 0
sin θ2
∴
= 0.
2
Therefore when θ2 is 0, ±π, a singularity arises. Another approach: Intuitively,
when θ2 is 0, three axes lie in the same plane and intersect at a single point, resulting in a singularity.
When θ2 is ±π, two axes are collinear, also resulting in a singularity.
Exercise 5.10.
(a) By Taylor expansion with respect to h,
d A(t)
e
dt
=
=
lim
1
h→0 h
lim
1
h→0 h
(eA(t+h) − eA(t) )
2
(eA(t)+hȦ(t)+O(h ) − eA(t) ).
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
81
The O(h2 ) terms go to zero in the limit, and hence can be ignored.
The above equation then reduces to
d A(t)
e
dt
2
1 A(t)
d
(e
+ h eA(t)+hȦ(t)+O(h ) − eA(t) )
h→0 h
dh
d A(t)+hȦ(t)
= lim
e
.
h→0 dh
=
lim
d (A+B)t
e
|=0 =
Now if A, B are matrices and , t are scalars, it can be shown that d
From this result it follows that
Z 1
d A(t)
e−A(t)s Ȧ(t)eA(t)s ds.
e
= eA(t)
dt
0
R t As A(t−s)
e Be
ds.
0
Therefore
X
−1
Z 1
Ẋ
=
e−A(t)s Ȧ(t)eA(t)s ds.
0
−1
Similarly, ẊX can be derived using same manner.
(b) By using the result of (a) and the relation R [ω] RT = [Rω] for R ∈ SO(3) and ω ∈ R3 ,
[ωb ]
=
=
RT R
Z 1
e−[r(t)]s [ṙ(t)]e[r(t)]s ds
0
=
Z 1h
i
e−[r(t)]s ṙ(t) ds
0
Z 1
=
0
e−[r(t)]s ds · ṙ(t) .
R1
Let 0 e−[r(t)]s ds be A(r). The characteristic polynomial of [r(t)] is s3 + ||r(t)||2 s, and by the CayleyHamilton theorem we have [r(t)]3 = −||r(t)||2 [r(t)]. From this result A(r) can be obtained as follows:
Z 1
A(r) =
e−[r(t)]s ds
0
Z 1
s2
s3
=
I − [r(t)]s + [r(t)]2 − [r(t)]3 + · · · ds
2!
3!
0
2
3
[r(t)] [r(t)]
[r(t)]
= I−
+
−
+ ···
2! 3!
4!
1
||r||2
||r||4
||r||6
1
||r||3
||r||5
||r||7
= I−
−
+
− · · · [r(t)] +
−
+
− · · · [r(t)]2
||r||2
2!
4!
6!
||r||3
3!
5!
7!
1 − cos ||r||
||r|| − sin ||r||
= I−
[r(t)] +
[r(t)]2 .
||r||2
||r||3
Thus we obtain
ωb
= A(r)ṙ
where
A(r)
=
I−
1 − cos ||r||
||r|| − sin ||r||
[r(t)] +
[r(t)]2 .
||r||2
||r||3
(c) Applying the similar method used in (b) to the angular velocity in the space frame, [ωs ] = ṘRT , we
can obtain
ωs
= A(r)ṙ
Go to the table of contents.
82
Chapter 5. Chapter 5 Solutions
where
A(r)
=
I+
||r|| − sin ||r||
1 − cos ||r||
[r(t)] +
[r(t)]2 .
2
||r||
||r||3
Exercise 5.11.
(a)


0
0
0
 0 −1 −1 


 1
0
0 

Jb (0) = 
 −L
0
0 


 2L
0
0 
0 2L
L


10
vb = Rbs vtip =  0  (∵ Rbs = I)
0

vb =

10
 0  =
0
Jb (0)θ̇

−L
0
 2L
0
0 2L
 ˙ 
θ1
0
10 = −Lθ˙1
0   θ˙2  ⇒
0 = 2Lθ˙1
L
θ˙3
Therefore no solution exists.
(b) At the configuration θ = (0◦ , 45◦ , −45◦ ),


0
0
0

0
−1 −1 



1
0
0 


√
Jb (θ) = 
,
2
−L
−
L
0


2
√


 (1 + 22 )L

0
0
√
2
L
0 (1 + 2 )L
∴τ
= JbT (θ)Fb


−10L
√
=  −5 2L  .
0
(c) Jb (θ) is the same as obtained in (b):

Fb
=
∴τ
=
=

10
 0 


 0 


 0 


 0 
0
JbT (θ)Fb


0
 0 .
0
Go to the table of contents.

0
 0 


 0 

Fb = 
 10 


 0 
0

Chapter 5. Chapter 5 Solutions
83
(d)


0
0
0
 0 −1 −1 


 1
0
0 

,
Jb (0) = 
0
0 
 −L

 2L
0
0 
0 2L
L


0
 0 


 0 


Fb = 

 fx 
 0 
0


τ1
 τ2  = JbT (0)Fb
τ3


−Lfx
0 
= 
0
∴ kfx k
10
kτ1 k
≤
.
L
L
=
Exercise 5.12.
(a) Given θ̇ = (0, 0, π2 , L)T ,

0
0
0 0
 0 −1
0 0 


 1
0
1 0 
.

Jb (θ) = 
0 −L 0 

 −L
 L
0
0 1 
0
L
0 0

(b) Given θ̇ = (0, 0, π2 , L)T ,

0
0



 π  

ωb
2


.
Vb = Jb (θ)θ̇ =  πL  =
vb
 − 2 
 L 
0

0
Rsb =  1
0



−L
−1 0
.
0 0  ⇒ ṗ = Rsb vb =  − πL
2
0 1
0
Given θ̇ = (1, 1, 1, 1)T ,


0
 −1 

  2 
ωb


Vb = Jb (θ)θ̇ = 
 = vb .
 −2L 
 L+1 
L

0
Rsb =  1
0



−1 0
−L − 1
0 0  ⇒ ṗ = Rsb vb =  −2L  .
0 1
L
Go to the table of contents.
84
Chapter 5. Chapter 5 Solutions
Exercise 5.13.
(a)

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :
Vs4 (θ) :
Vs5 (θ) :
Vs6 (θ) :





0
0
0
ωs1 =  0  , q1 =  0  , vs1 =  0 
1
0
0






−s1
0
0
ωs2 = e[ẑ]θ1 ω2 =  c1  , q2 =  0  , vs2 =  0 
0
0
0


 


−c1 c2
0
0
ωs3 = e[ẑ]θ1 e[ŷ]θ2 ω3 =  −s1 c2  , q3 =  0  , vs3 =  0 
s2
0
0

 

0
−L(s1 c3 + c1 s2 s3 )
ωs4 = e[ẑ]θ1 e[ŷ]θ2 e−[x̂]θ3 ω4 = ωs3 , q4 = e[ẑ]θ1 e[ŷ]θ2 e−[x̂]θ3  L  =  L(c1 c3 − s1 s2 s3 )  ,
0
−Lc2 s3


2
2
−L(s1 c2 s3 − c1 s2 c3 + s1 s2 s3
vs4 = −ωs4 × q4 =  L(s1 s2 c3 + c1 s22 s3 + c1 c22 s3 ) 
Lc2 c3 (c21 + s21 )


0
ωs5 = ωs4 , q5 = q4 + e[ẑ]θ1 e[ŷ]θ2 e−[x̂]θ3 e−[x̂]θ4  L  , vs5 = −ωs5 × q5
0
 


−s1 c345 − c1 s2 s345
0
ωs6 = e[ẑ]θ1 e[ŷ]θ2 e−[x̂](θ3 +θ4 +θ5 )  1  =  c1 c345 − s1 s2 s345  , q6 = q5 , vs6 = −ωs6 × q6
0
−c2 s345
∴ Js (θ) =
Vs1 (θ) Vs2 (θ) Vs3 (θ) Vs4 (θ) Vs5 (θ) Vs6 (θ)
(b) Case 1. Two collinear revolute joint axes: When θ2 = π2 and 23 π, axis 1 and axis 3 are collinear, and
z-translation becomes impossible.
Case 2. Three parallel coplanar revolute joint axes: When θ4 = 0 and π, axis 3, axis 4 and axis 5 are
coplanar, and y-translation becomes impossible.
Case 3. Four intersecting revolute joint axes: Axis 1, axis 2 and axis 3 always intersect. When axis 6
also intersects these three axes at a single point, z-rotation becomes impossible.
Exercise 5.14.
Suppose that the the prismatic joint axis is coincident with the z-axis of the fixed frame, and the two revolute
joint axes are parallel to the x-axis of the fixed frame as shown in Figure 5.2. Then

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :



0
0
ωs1 =  0  , vs1 =  0 
0
1






1
0
0
ωs2 =  0  , q2 =  L1  , vs2 =  0 
0
0
−L1






1
0
0

0
ωs3 =  0  , q3 =  L1 + L2  , vs3 = 
0
0
−L1 − L2
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
85
Figure 5.2

0
 0

 0
∴ Js (θ) = 
 0

 0
1
1
0
0
0
0
−L1

1

0


0
.

0


0
−L1 − L2
This is a singularity because the rank of Js (θ) is less than 3.
Exercise 5.15.
(a)

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :



0
0
ωs1 =  0  , vs1 =  1 
0
0






0
θ1
0
ωs2 =  0  , q2 =  θ1  , vs2 =  0 
1
0
0





 

0
0
−L
−L cos θ2 − L sin θ2
ωs3 =  0  , q3 =  θ1  + Rot(ẑ, θ2 )  L  =  θ1 − L sin θ2 + L cos θ2  ,
1
0
0
0


θ1 + L(cos θ2 − sin θ2 )
.
L(cos θ2 + sin θ2 )
vs3 = 
0
(b) From (a), the first three columns of the space Jacobian Js (θ) can be expressed as follows:
Js (θ)
=

Vs1 (θ) Vs2 (θ) Vs3 (θ)
0
 0

 0
= 
 0

 1
0
0
0
1
θ1
0
0

0

0


1
.
θ1 + L(cos θ2 − sin θ2 ) 


L(cos θ2 + sin θ2 )
0
Go to the table of contents.
86
Chapter 5. Chapter 5 Solutions
(Vs3 (θ))0
Vs3 (θ) − L(cos θ2 + sin θ2 )Vs1 (θ)


0


0




1


 θ1 + L(cos θ2 − sin θ2 )  .




0
0
=
=
For (Vs3 (θ))0 and Vs2 (θ) to be equal,
θ1
0
∴ θ2
= θ1 + L(cos θ2 − sin θ2 )
cos θ2 − sin θ2
π 5π
=
,
.
4 4
=
(c) At the configuration θ = (0◦ , 0◦ , 0◦ , 90◦ , 0◦ , 0◦ ),

0
 0

 0
Js (θ) = 
 0

 1
0
0
0
1
0
0
0

0
0 

1 
.
L 

L 
0
To obtain the wrench Fs expressed in the space frame {s}, use the displacement Tbs ∈ SE(3) of the
space frame {s} expressed in the body frame {b}:


0 0 −1
0
 0 1 0 −5L 
.
Tbs = 
 1 0 0
L 
0 0 0
1
Then
Fs
AdTT (Fb )

 

 bs
−50L
0
0 0 1 −5L 0
0

 

 0 1 0 −L
0
0 
  0   −10L 





 −1 0 0
0
−L −5L   0   −50L 

= 


=
 0 0 0
0
0
1 
  10   10 





 0 0 0
0 
0
0
1
0
−10
10
0 0 0 −1
0
0
=
∴τ
=
Js (θ)T Fs

=
0
 0
0

=
0
0
0

0
1
1

0
 −50L  .
−40L
0
0
L

−50L
  −10L 

1 0 
 −50L 



0 0 
10 

L 0 
 0 
−10
Exercise 5.16.
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
87
(a)

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :



0
0
ωs1 =  0  , vs1 =  0 
0
1






0
0
0
ωs2 =  0  , q2 =  0  , vs2 =  0 
1
0
0



 

0
0
− sin θ2
ωs3 =  0  , vs3 = eẑθ2  1  =  cos θ2  .
0
0
0
(b)

Vb6 (θ) :
Vb5 (θ) :

 


1
0
0
ωb6 =  0  , q6 =  0  , vb6 =  0 
0
0
0

 





0
0
0
0
ωb5 = ex̂(−θ6 )  0  =  sin θ6  , q5 =  0  , vb5 =  0  .
1
0
0
cos θ6
(c) At the zero position,

0
 0

 0
Js (0) = 
 0

 0
1
0
0
1
0
0
0
0
0
0
0
1
0
0
1
0
0
0
0

0
1
0
0 

1
0 
.
L 0 

0
0 
0 −L
When L = 0, Js (0) becomes singular.
(d) In the zero position, consider the wrench Fs expressed in the space frame {s}:


−100L


0




ms
psb × fb
0

Fs =
=
=


fs
fb
0




0
−100
where




0
0
psb =  L  , fb =  0 
0
−100
∴τ
=
=
J T (0)Fs
s
0 0 0
 0 0 1

 0 0 0

 0 1 0

 0 0 1
1 0 0

 

0 0 1
−100L
−100

  0 
0 0 0 
0

 


  0 
0 1 0 
0

=


  0 .
0 0 0 
0

 

  0 
0
L 0 0 
0 0 −L
−100
0
Exercise 5.17.
Go to the table of contents.
88
Chapter 5. Chapter 5 Solutions
(a) If θ is arbitrary,

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :



0
0
ωs1 =  0  , vs1 =  0 
0
1






1
0
0
ωs2 =  0  , q2 =  0  , vs2 =  θ1 
0
θ1
0

 





0
0
0
L + θ1 sin θ2
 , vs3 = 

L cos θ2
0
ωs3 = e[x̂]θ2  0  =  − sin θ2  , q3 = 
1
cos θ2
θ1 + L sin θ2
0
Therefore, the first three columns of the space Jacobian


0 1
0
 0 0

− sin θ2


 0 0

cos
θ
2


Js (θ) = 

0
0
L
+
θ
sin
θ
1
2 

 0 θ1

0
1 0
0
(b)

0 1
 0 0


 0 0
Js (0) = 
 0 0

 0 0
1 0
∴ Vs
= Js (0)θ̇

0 1
 0 0


 0 0
= 
 0 0

 0 0
1 0
√1
0
1
2
√1
0
0
2
1
0
0
L 0
0
0
0
0
0 −L − √L2

0
0 


0 

0 

1 
0

√
 

0
−1√
+ 2
1


0 
0 
2

 






1
1

0 



=



−1
L

0 





0 √
1  2  
0
1 + (1 − 2)L
0
√1
0
1
2
√1
0
0
2
1
0
0
L 0
0
0
0
0
0 −L − √L2
(c) In the zero position, the space Jacobian Js (θ) is singular, since Vs4 (0) + LVs1 (0) = Vs2 (0).
Exercise 5.18.
(a)

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :

 
0
0
ωs1 =  0  , vs1 =  0 
1
0



  √1


− 2 sin θ1
0
cos θ1 − sin θ1 0
0
1


1
cos θ1 0   √2  =  √2 cos θ1  , vs2 =  0 
ωs2 =  sin θ1
√1
√1
0
0
1
0
2
2


 
1
− √2 sin θ1
0


ωs3 =  0  , vs3 =  √12 cos θ1 
1
√
0
2
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
89
(b)
0
 0

 1

= 
 0

 0
0
0

0
0
√1

0
0
2


√1
0
−1

2
1
√
0
0 −(3 + 2 )L 


√1
√1 )L
0
(1
+
1
0

2
2
1
1
√
√
0
−(2 + 2 )L 0
0
2


 
−1
1

0
 0  


 


1
 1  

=
Js (0) 

0
 −1  


  √1
1
 2   2 − (1 + √2 )L + 2 

√
√1 + (2 + 2 L
0
)
2

Js (0)
Vs
=
0
0
0
0
1
0
0
0
(c) There does not exist a kinematic singularity in the zero position, because rank(Js (0))=6.
Exercise 5.19.
(a)



0
0
ωs1 =  0  , vs1 =  0 
1
0






0
0
1
ωs2 =  0  , q2 =  0  , vs2 =  θ1 
0
θ1
0






θ1 sin θ2 + 1
0
0

 , vs3 = 
0
cos θ2
ωs3 =  − sin θ2  , q3 = 
0
θ1 + sin θ2
cos θ2






0
− cos θ3 − sin θ3
θ1 sin θ2 + cos θ3 − sin θ3
 , vs4 =  cos θ2 (cos θ3 + sin θ3 ) 
cos θ2 (cos θ3 − sin θ3 )
ωs4 =  − sin θ2  , q4 = 
cos θ2
θ1 + sin θ2 (cos θ3 − sin θ3 )
sin θ2 (cos θ3 + sin θ3 )

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :
Vs4 (θ) :
(b) Consider the space Jacobian Js (θ) at the zero position:
Js (0)
=

=
Vs1 (0) Vs2 (0)
0
 0


 0

 0

 0
1
1
0
0
0
0
0
0
0
1
1
0
0
0
0
1
1
1
0
Vs3 (0)
√1
2
− √12
0
0
0
− √12
Vs4 (0)

0
0 


0 
.
0 

1 
0
Vs5 (0) Vs6 (0)
The space Jacobian Js (θ) is singular in the zero position, since Vs4 (0) − Vs6 (0) = Vs3 (0).
Go to the table of contents.
90
Chapter 5. Chapter 5 Solutions
(c) When Fs = (0, 1, −1, 1, 0, 0)T ,
τ(i)
= JsT (0)Fs

0
0
 1
0

 0
0
= 
 0
0
 1
√1
 √
−
2
2
0
0


0
 0 


 0 

=  0 

 1 
 √ 
2
0
0
0
1
1
0
0
0
0
1
1
0
0
0
0
0
1
0
1
0
0
1
1
0
0
0
0
1
1
0
0
0
0
0
1
0
1
1
0
0
0

1
0
0
0


0
 1 


  −1 


 1 



− √12   0 
0
0
When Fs = (1, −1, 0, 1, 0, −1)T ,
τ(ii)
= JsT (0)Fs

0
0
 1
0

 0
0
= 
 0
0
 1
√1
 √
−
2
2
0
0


−1
 1 


 1 

=  1 

 3 
 √ 
2
0

1
  −1 


 0 


 1 



− √12   0 
−1
0
Exercise 5.20.
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
91
(a)

S1 :
S2 :
S3 :
S4 :
S5 :
S6 :





0
0
0
ω1 =  0  , q1 =  0  , v1 =  0 
1
0
0






0
0
L0 − L1
 , v2 = 

0
0
ω2 =  −1  , q2 = 
0
L0 − L1
0




0
0
ω3 =  0  , v3 =  0 
0
1






1
0
0
ω4 =  0  , q4 =  0  , v4 =  L0 
0
L0
0






0
L1
0
ω5 =  0  , q5 =  0  , v5 =  −L1 
1
0
0






0
0
1
 , v6 =  L0 − L1 
0
ω6 =  0  , q6 = 
0
L0 − L1
0

0
 −1
M =
 0
0

1 0 L1 + L2

0 0
0

0 1 L0 − L1 
0 0
1
(b)





0
0
0
ωs1 =  0  , q1 =  0  , vs1 =  0 
0
0
1
 






sin θ1
0
0
(L0 − L1 ) cos θ1
 , vs2 =  (L0 − L1 ) sin θ1 
0
ωs2 = e[ẑ]θ1  −1  =  − cos θ1  , q2 = 
0
0
L0 − L1
0



 

0
0
− cos θ1 sin θ2
ωs3 =  0  , vs3 = e[ẑ]θ1 e[−ŷ]θ2  0  =  − sin θ1 sin θ2 
0
1
cos θ2

Vs1 (θ) :
Vs2 (θ) :
Vs3 (θ) :
(c)

Vs0 1 (θ) :
Vs0 2 (θ) :
Vs0 3 (θ) :





0
0
−L1 − L2

0
ωs0 1 =  0  , q1 =  −L1 − L2  , vs0 1 = 
1
0
0

 





1
cos θ1
0
0

0
ωs0 2 = e[ẑ]θ1  0  =  sin θ1  , q2 =  −L1 − L2  , vs0 2 = 
0
0
0
(L1 + L2 ) cos θ1



 

0
0
sin θ1 sin θ2
ωs0 3 =  0  , vs0 3 = e[ẑ]θ1 e[x̂]θ2  0  =  − cos θ1 sin θ2 
0
1
cos θ2
Go to the table of contents.
92
Chapter 5. Chapter 5 Solutions
(d) Consider the space Jacobian Js (θ) at the zero position:
Js (0)
=
Vs1 (0) Vs2 (0)

0
 0

 1

 0

 0
0
=
0
−1
0
L0 − L1
0
0
Vs3 (0)
0 1
0 0
0 0
0 0
0 L0
1 0
Vs4 (0)
0
0
1
0
−L1
0
Vs5 (0) Vs6 (0)

1

0


0
.

0

L0 − L1 
0
The space Jacobian Js (θ) is singular in the zero position, since Vs1 (0) − Vs5 (0) = Vs4 (0) − Vs6 (0).
Geometrically, this singularity corresponds to four coplanar revolute joint screw axes.
Exercise 5.21.
(a) We can express the forward kinematics of the manipulator in the following form:
T0t = e[S1 ] θ1 e[S2 ] θ2 e[S3 ] θ3 e[S4 ] θ4 e[S5 ] θ5 e[S6 ] θ6 M
where Si (i = 1, 2, ..., 6) is the screw for the ith link relative to frame {0}, and M is the displacement
from frame {0} to frame {t} when the manipulator is in its zero position. We further know that
0
T e[S] T −1 = e[S ] (S 0 = [AdT ]S).
Using the above formula, the forward kinematics can be modified to the following form:
T0t
0
(S 0 6 , [AdM −1 ]S6 )
e[S1 ] θ1 e[S2 ] θ2 e[S3 ] θ3 e[S4 ] θ4 e[S5 ] θ5 M e[S 6 ] θ6
=
0
0
(S 0 5 , [AdM −1 ]S5 )
e[S1 ] θ1 e[S2 ] θ2 e[S3 ] θ3 e[S4 ] θ4 M e[S 5 ] θ5 e[S 6 ] θ6
=
0
0
[S 0 5 ]
[S 0 6 ]
=
e[S1 ] θ1 e[S2 ] θ2 e[S3 ] θ3 e[S4 ] θ4 M0c Mct e[S 5 ] θ5 e[S 6 ] θ6
=
e
[S1 ]
e
[S1 ]
e
[A1 ]
=
=
θ1 e
[S2 ]
θ1 e
[S2 ]
θ1 e
θ2 e
[S3 ]
θ3 M0c e
[S 0 4 ]
[S 0 3 ]
[S 0 4 ]
θ2 M0c e
[A2 ]
θ2 M0c e
θ3 e
[A3 ]
θ4 Mct e
[A4 ]
θ3 e
θ4 Mct e
θ4 Mct e
θ5 e
[S 0 5 ]
[A5 ]
θ5 e
θ5 e
θ6
[S 0 6 ]
[A6 ]
θ6
(M = M0c Mct )
(S 0 4 , [AdM −1 ]S4 )
0c
0
(S 3 , [AdM −1 ]S3 )
0c
θ6
∴ A1 = S1 , A2 = S2 , and Ai = S 0 i (i = 3, 4, 5, 6)
One difficulty with the above is that it is very complex to calculate. Calculating Ai based on the physical
meaning of screws is easier. The adjoint transformation is used to change the relative coordinate frame:
S1 and S2 are screws relative to frame {0}, S 0 3 and S 0 4 are screws relative to frame {c}, and finally
S 0 5 and S 0 6 are screws relative to frame {t}:
S1 :
ω = (0, 0, 1), q = (L, 0, 0), v = (0, −L, 0)
S 03 :
ω = (0, 0, 0), v = (1, 0, 0),
S2 :
0
S4:
0
S5:
0
S6:

1
 0
Also, M0c = 
 0
0
0
1
0
0
0
0
1
0
ω = (0, 1, 0), q = (L, L, −L), v = (L, 0, L)
ω = (1, 0, 0), q = (0, L, 0), v = (0, 0, −L)
ω = (0, 1, 0), q = (−2L, 0, −L), v = (L, 0, −2L)
ω = (0, 0, −1), q = (−L, 0, −L), v = (0, −L, 0)


2L
0
 −1
−L 
, Mct = 
 0
−L 
1
0
0 −1
0 0
1 0
0 0

0
−2L 
.
0 
1
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
93
(b) The space Jacobian when θ2 = 90◦ and all the other joint variables are set to zero is given by


0
0 0
0
1
0
 0
1 0
0
0
0 


 1

0
0
−1
0
−1

.
Js (θ) = 

0
L
0
0
0
2L


 −L 0 0
L −3L L 
0 L −1 0
L
0
Therefore the spatial velocity Vs is
Vs = Js (θ)
1
0
1
0
0
1
T
=
0
0
0
2L 0
−1
T
.
(c) First, intuitively it is clear that if θ2 = 90◦ , the axes of joint 1 and joint 4 are colinear. For six degree
of freedom open chains, two collinear revolute joint axes correspond to a kinematic singularity. We
now show this more rigorously. From part (c), the space Jacobian when θ2 = 90◦ and all the other
joint variables are at zero is


0
0 0
0
1
0
 0
1 0
0
0
0 


 1
0 0 −1
0
−1 
.
Js (θ) = 
 0 L 0
0
0
2L 


 −L 0 0
L −3L L 
0 L −1 0
L
0
From above, if we denote the first column by J1 and the fourth column by J4 , we can write J1 = −J4 .
The rank of JS must therefore be less than six. It follows that this configuration is a kinematic
singularity.
(d) Denote the i-th column of Js as Ji , i = 1, . . . , 6. The spatial forces that the robot is generating are
(−Felbow ) and (−Ftip ), respectively (action and reaction). Joint torques τ5 and τ6 are affected only by
Ftip :
τ5
τ6
= −J5T Ftip =
3L
= −J6T Ftip = −3L
Joint torques τ1 , τ2 , τ3 and τ4 are affected by Felbow and Ftip :
τ1
τ2
τ3
τ4
= −J1T (Felbow + Ftip ) = L
= −J2T (Felbow + Ftip ) = −1 − 2L
= −J3T (Felbow + Ftip ) =
=
−J4T (Felbow + Ftip ) =
1
−L.
Exercise 5.22.
(a) For V2 ,
T02
= e[S1 ]θ1 e[S2 ]θ2 M2
Ṫ02
−1
T02
= [S1 ]e[S1 ]θ1 e[S2 ]θ2 M2 θ̇1 + e[S1 ]θ1 [S2 ]e[S2 ]θ2 M2 θ̇2
= M2−1 e−[S2 ]θ2 e−[S1 ]θ1
[V2 ]
−1
= Ṫ02 T02
=
V2
[S1 ]θ̇1 + e[S1 ]θ1 [S2 ]e−[S1 ]θ1 θ̇2
= S1 θ̇1 + Ade[S1 ]θ1 (S2 )θ̇2
Go to the table of contents.
94
Chapter 5. Chapter 5 Solutions
For V3 ,
T03
Ṫ03
= e[S1 ]θ1 e[S2 ]θ2 e[S3 ]θ3 M3
= [S1 ]e[S1 ]θ1 e[S2 ]θ2 e[S3 ]θ3 M3 θ̇1 + e[S1 ]θ1 [S2 ]e[S2 ]θ2 e[S3 ]θ3 M3 θ̇2 + e[S1 ]θ1 e[S2 ]θ2 [S3 ]e[S3 ]θ3 M3 θ˙3
−1
T03
=
M3−1 e−[S3 ]θ3 e−[S2 ]θ2 e−[S1 ]θ1
[V3 ]
−1
= Ṫ03 T03
[S1 ]θ̇1 + e[S1 ]θ1 [S2 ]e−[S1 ]θ1 θ̇2 + e[S1 ]θ1 e[S2 ]θ2 [S3 ]e−[S2 ]θ2 e−[S1 ]θ1 θ̇3
=
V3
= S1 θ̇1 + Ade[S1 ]θ1 (S2 )θ̇2 + Ade[S1 ]θ1 e[S2 ]θ2 (S3 )θ̇3
(b) Based on the result obtained in (a), we can construct a recursive formula for Vk+1 as follows:
Vk+1
=
=
=
S1 θ̇1 + Ade[S1 ]θ1 (S2 )θ̇2 + · · · + Ade[S1 ]θ1 ···e[Sk ]θk (Sk+1 )θ̇k+1
Vk + Ade[S1 ]θ1 ···e[Sk ]θk (Sk+1 )θ̇k+1
Vs1 (θ) Vs2 (θ) · · · Vs,k+1 (θ) θ̇
Exercise 5.23.
(a) The plot of the arm and its manipulability ellipse is shown in Figure 5.3. Based on the ratio between
Manipulability Ellipsoids
2.5
2
1.5
x2 dot
1
0.5
0
−0.5
−1
−1.5
−2
−1.5
−1
−0.5
0
0.5
x1 dot
1
1.5
2
2.5
Figure 5.3
the longest and shortest semi-axes of the manipulability ellipsoid,
s
p
λmax (A)
λmax (A)
µ1 (A) = p
=
≥ 1,
λmin (A)
λmin (A)
where A = JJ T . When µ1 (A) is low, close to one, then the manipulability ellipsoid is nearly spherical
or isotropic. At the configuration (135◦ , 90◦ ), µ1 (A) = 2.618 comes to be the lowest. So the arm
appears most isotropic at the configuration (135◦ , 90◦ ).
(b) The eccentricity of the ellipse depends only on θ2 . θ1 only effects on the orientation and position of
the ellipse. The effect caused by the first joint is related with the distance between its rotation axis
and the end-effector, whose magnitude is determined by θ2 . This conclusion can also be drawn from
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
95
planar body Jacobian shown below, which doesn’t include θ1 .


1
1
L1 sin θ2
0 .
Jb (θ) = 
L2 + L1 cos θ2 L2
(c) The drawing is shown in Figure 5.4.
2
1.5
1
v2
0.5
0
−0.5
−1
−1
−0.5
0
0.5
v1
1
1.5
2
Figure 5.4
Exercise 5.24.
The plot is shown in Figure 5.5.
Exercise 5.25.
(a) Using the software of the textbook and the kinematics of the 6R UR5, the space Jacobian Js when all
joint angles are π/2 can be obtained as


0
−1
−1
−1
0
0
 0

0
0
0
1
0


 1

0
0
0
0
1
,
Js = 
 0
0
0
0
0.336 −0.297 


 0 −0.089 0.336 0.336
0
0.109 
0
0
0
−0.392 −0.109
0
which can be separated as

−1
0
0
0
0
Jv =  0 −0.089
0
0
0
0.336
0
0
Jω =  0
1
and
(b) Let

−1
0
0

−1 0 0
0 1 0 ,
0 0 1
0
0.336
−0.392

0.336 −0.297
0
0.109  .
−0.109
0


0
0 .
2
3
Aω = Jω JωT =  0
0
0
1
0
Go to the table of contents.
96
Chapter 5. Chapter 5 Solutions
Force Ellipsoids
3
2.5
2
f2
1.5
1
0.5
0
−0.5
−1
−1.5
−2
−1.5
−1
−0.5
0
0.5
1
f1
1.5
2
2.5
3
3.5
4
Figure 5.5
The lengths of the principal semi-axes of the angular-velocity manipulability ellipsoid equal to the
square roots of its eigenvalues. The eigenvalues are 3, 1, and 2, so the lengths are 1.7321, 1, and
1.4142.
The directions of the principal semi-axes are aligned with its eigenvectors, which are (1, 0, 0)T , (0, 1, 0)T ,
and (0, 0, 1)T .
Let

0.2011 −0.0324
Av = Jv JvT =  −0.0324 0.2456
−0.0366 −0.1317

−0.0366
−0.1317  .
0.1655
The lengths of the principal semi-axes of the linear-velocity manipulability ellipsoid equal to the square
roots of its eigenvalues, The eigenvalues are 0.0520, 0.2169, and 0.3434, so the lengths are 0.2280, 0.4657,
and 0.5860.
The directions of the principal semi-axes are aligned with its eigenvectors, which are
(0.3106, 0.5696, 0.7609)T , (0.9500, −0.1596, −0.2683)T , and (−0.0314, 0.8062, −0.5907)T .
(c) The moment ellipsoid can be obtained by stretching the angular-velocity manipulability ellipsoid along
each principal axis i by a factor 1/λi , where λi is its corresponding eigenvalue. So for current moment
ellipsoid, the lengths of the principal semi-axes are 0.5774, 1, and 0.7071. The directions are (1, 0, 0)T ,
(0, 1, 0)T , and (0, 0, 1)T .
The force ellipsoids can be obtained by stretching the linear-velocity manipulability ellipsoid along
each principal axis i by a factor 1/λi , where λi is its corresponding eigenvalue. So for current force
ellipsoid, the lengths of the principal semi-axes are 4.3854, 2.1473, and 1.7066. The directions are
(0.3106, 0.5696, 0.7609)T , (0.9500, −0.1596, −0.2683)T , and (−0.0314, 0.8062, −0.5907)T .
Exercise 5.26.
(a) Using the software of the textbook and the kinematics of the 7R WAM, the space Jacobian Jb when
Go to the table of contents.
Chapter 5. Chapter 5 Solutions
97
all joint angles are π/2 can be obtained as

0
−1

0
0


1
0
Jb = 
 −0.105
0

 −0.889 0.006
0
−0.105
0
1
0
0.006
0
0.889
−1
0
0
0
0.006
0
0
0
1
−0.045
−0.844
0

0
0
1
0 

0
1 
,
0.006 0 

0
0 
0
0
which can be separated as

0
Jω =  0
1
and

−0.105
0
Jv =  −0.889 0.006
0
−0.105
−1
0
0
0 0 −1
1 0 0
0 1 0
0.006
0
0.889

0 0
1 0 ,
0 1
−0.045
−0.844
0
0
0.006
0

0.006 0
0
0 .
0
0
(b) Let

2
Aω = Jω JωT =  0
0
0
2
0

0
0 .
3
The lengths of the principal semi-axes of the angular-velocity manipulability ellipsoid equal to the
square roots of its eigenvalues. The eigenvalues are 2, 2, and 3, so the lengths are 1.4142, 1.4142, and
1.7321.
The directions of the principal semi-axes are aligned with its eigenvectors, which are (1, 0, 0)T , (0, 1, 0)T ,
and (0, 0, 1)T .
Let


0.0131 0.1313
0.0053
Av = Jv JvT =  0.1313 1.5027 −0.0006  .
0.0053 −0.0006 0.8013
The lengths of the principal semi-axes of the linear-velocity manipulability ellipsoid equal to the square
roots of its eigenvalues, The eigenvalues are 0.0016, 0.8014, and 1.5142, so the lengths are 0.0400, 0.8952,
and 1.2305.
The directions of the principal semi-axes are aligned with its eigenvectors, which are
(−0.9962, 0.0872, 0.0067)T , (0.0067, −0.0004, 1.0000)T , and (−0.0872, −0.9962, 0.0002)T .
(c) The moment ellipsoid can be obtained by stretching the angular-velocity manipulability ellipsoid along
each principal axis i by a factor 1/λi , where λi is its corresponding eigenvalue. So for current moment
ellipsoid, the lengths of the principal semi-axes are 0.7071, 0.7071, and 0.5774. The directions are
(1, 0, 0)T , (0, 1, 0)T , and (0, 0, 1)T .
The force ellipsoids can be obtained by stretching the linear-velocity manipulability ellipsoid along
each principal axis i by a factor 1/λi , where λi is its corresponding eigenvalue. So for current force
ellipsoid, the lengths of the principal semi-axes are 25.0246, 1.1171, and 0.8127. The directions are
(−0.9962, 0.0872, 0.0067)T , (0.0067, −0.0004, 1.0000)T , and (−0.0872, −0.9962, 0.0002)T .
Exercise 5.27.
Programming assignment.
Go to the table of contents.
98
Chapter 5. Chapter 5 Solutions
Go to the table of contents.
Chapter 6 Solutions
Exercise 6.1.
Given x, y and θ, the position of the last joint is determined by (x̄, ȳ) = (x − L3 cos θ, y − L3 sin θ). Then
the problem boils down to solving the inverse kinematics θ1 and θ2 for a planar 2R robot with given endeffector position (x̄, ȳ), analytic solutions of which are well described in the text. Finally, θ3 is determined
by θ3 = θ − θ1 − θ2 .
Exercise 6.2.
By placing the base frame at the intersection of first three axes and tool frame at the intersection of the final
two axes, we can think of the given manipulator as an inverted version of the standard elbow manipulator(e.g.,
a PUMA design with zero shoulder offset), whose base frame and tool frame are switched. The standard
elbow manipulator is known to have four inverse kinematic solutions (two corresponding to elbow-up and
elbow-down solutions, and two corresponding to the ZYX Euler angle-type wrist, resulting in four inverse
kinematic solutions.). Suppose the forward kinematics for the standard elbow manipulator is of the form
e[S1 ]θ1 · · · e[S6 ]θ6 = T . It readily follows that e−[S6 ]θ6 · · · e−[S1 ]θ1 = T −1 . Note that the forward kinematics
of the inverse elbow manipulator can be made to assume exactly this form. Therefore the inverse elbow
manipulator also has four inverse kinematic solutions. Intuitively these solutions also correspond to the
elbow-up/down and the ZYX Euler angle solutions.
Exercise 6.3.
√
Given T 0 ∈ SE(3), one can obtain the original end-effector pose T ∈ SE(3) by T = T 0 · Trans(ŷ, (2 − 2)L).
Therefore, we can instead solve for
T = T (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = e[S1 ]θ1 e[S2 ]θ2 e[S3 ]θ3 e[S4 ]θ4 e[S5 ]θ5 e[S6 ]θ6 M,
where the Si are the corresponding joint screws.
It can be easily seen that θ4 , θ5 , θ6 do not change the position of T ; only θ1 , θ2 , θ3 do. Let the position vector
of T be p = (px , py , pz )T . Then by following similar reasoning for solving the analytic inverse kinematics of
6R puma type arm, we have two possible solutions for θ1 :
θ1 = tan−1 (−
py
),
px
and
θ1 = tan−1 (−
py
) + π.
px
(in the latter case the original solution for θ2 is replaced by π − θ2 .) Observe that when px = py = 0, the
arm is in singluarity configuration, resulting in infinitely many possible solutions for θ1 . Now θ2 and θ3 are
determined by solving the inverse kinematics for a planar two-link chain. From the law of cosines,
cos θ3 =
−1
holds, and θ3 = tan
±
√
1−D 2
D
p2x + p2y + p2z − 2L2
, D,
2L2
. Given a solution for θ3 , θ2 can be obtained as
−1
θ2 = tan
pz
q
p2x + p2y
−1
− tan
L sin θ3
.
L + L cos θ3
To sum up, given the position p, we have 4 possible solutions for θ1 , θ2 , θ3 , just like the case for a 6R PUMAtype arm. Now, we are left with determining θ4 , θ5 , θ6 . Having the solutions for θ1 , θ2 , θ3 , forward kinematics
99
100
Chapter 6. Chapter 6 Solutions
can be manipulated into the form
e[S4 ]θ4 e[S5 ]θ5 e[S6 ]θ6 = e−[S3 ]θ3 e−[S2 ]θ2 e−[S1 ]θ1 T M −1
where the right-hand side is given. By following the same reasoning as for the 6R PUMA-type arm, this
problem boils down to
Rot(x̂, θ4 )Rot(ẑ, θ5 )Rot(ŷ, θ6 ) = R,
where R is the SO(3) component of e−[S3 ]θ3 e−[S2 ]θ2 e−[S1 ]θ1 T M −1 . The solutions for θ4 , θ5 , θ6 then correspond
to the XZY Euler angles.
Exercise 6.4.
The forward kinematics results in the following relaton:
p = e[ω1 ]θ1 e[ω2 ]θ2 (0, 1 + θ3 , 0)T ,
(6.1)
√
where ω1 = (0, 0, 1)T and ω2 = (0, 21 , 23 )T .
(a) Taking the norm of both sides of Equation (6.1),
θ3 = kpk − 1 = 7.
Now we are left with an equation of the form p = e[ω2 ]θ2 q, where q = (0, 8, 0)T .
[ω2 ]θ2
vectors p0 and q 0 , depicted
in Figure
Figure 6.1 shows a geometric illustration
√ of pT = e 0 q. Defining
√
0
T
T
6.1, as p = p − (ω2 p)ω2 = (0, 3/4, − 3/4) and q = q − (ω2 q)ω2 = (−3/4, 3/8, − 3/8)T , we can
obtain a solution for θ2 from these two vectors as follows:
θ2 = atan2(ω2T (q 0 × p0 ), q 0 · p0 )
= 60◦ .
Figure 6.1
(b) Figure 6.2 shows a geometric illustration of Equation (6.1) in general.
Since ω1 and ω2 are clearly independent vectors, ω1 , ω2 and ω1 × ω2 constitute basis vectors in R3 .
Therefore we can represent vector r, depicted in Figure 6.2, with the basis vectors, and coefficients
α, β and γ as
r = αω1 + βω2 + γ(ω1 × ω2 ).
(6.2)
Taking the inner product of both sides of Equation (6.2) with ω1 and ω2 , and using the relations
ω2T r = ω2T q and ω1T r = ω1T p, the following two equations in terms of α and β can be obtained:
ω2T q = (ω2T ω1 )α + β
ω1T p = α + (ω1T ω2 )β.
Go to the table of contents.
Chapter 6. Chapter 6 Solutions
101
Figure 6.2
√
Solving for α and β yields α = −4 3 and β = 10. Now, taking the squared norm of Equation (6.2)
and solving for γ, we have
kqk2 − α2 − β 2 − 2αβ(ω1T ω2 )
= 144 = 122
kω1 × ω2 k2
√
(Here we
the fact that krk = kqk). Therefore γ = ±12 and r = −4 3ω1 + 10ω2 ± 12(ω1 × ω2 ) =
√ use
(∓6, 5, 3)T . Now by following the same reasoning used in (a), from q and r we get θ2 , and from r
and p we get θ3 . The two solutions representing elbow up and elbow down configurations are
γ2 =
(θ1 , θ2 , θ3 ) = (0, 60◦ , 7), (atan2(60, −11), −60◦ , 7).
Exercise 6.5.
Among the joint variables, only θ1 and θ3 affect pz :
pz = L + hθ1 − θ3 .
A top-down view of this manipulator is shown in Figure 6.3. The angle β can be found from the law of
(px, py)
β
y
θ2
γ
{s}
θ1
x
Figure 6.3
cosines:
L2 + L2 − 2L2 cos β = p2x + p2y
∴ β = cos−1 (
p2x + p2y − 2L2
).
2L2
Go to the table of contents.
L
102
Chapter 6. Chapter 6 Solutions
From the figure, θ2 = π − β. Also, γ can be found by
q
L2 + p2x + p2y − 2L p2x + p2y cos γ = L2
∴ γ = cos−1 (
p2x + p2y
q
).
2L p2x + p2y
p
p
From Figure 6.3, tan(θ1 + γ) = pxy . Therefore, θ1 = tan−1 pxy − γ. Finally for θ4 , considering the orientation
of the end-effector frame,
Rot(ẑ, θ1 )Rot(ẑ, θ2 )Rot(ẑ, θ4 ) = Rot(ẑ, α)
θ1 + θ2 + θ4 = α.
Therefore
θ4
=
α − θ1 − θ2 = α − tan−1
px
+ γ − π + β.
py
This is an elbow-down solution; the elbow-up solution can be obtained in the same way:
(
p
(tan−1 ppxy − γ, π − β, L + h tan−1 pxy − hγ − pz , α − tan−1 ppxy + γ − π + β)
(θ1 , θ2 , θ3 , θ4 ) =
p
(tan−1 ppxy + γ, π + β, L + h tan−1 pxy + hγ − pz , α − tan−1 ppxy − γ − π − β)
Exercise 6.6.
(a) First, consider the position inverse kinematics.
Figure 6.4 on the left is the robot projected to the plane perpendicular to joint axes 4, 5, and 6. Denote
end-effector
R1
2L+θ
R2 R3
]
2L
ⱜ\
Joint 5(R1)
2L
R
Joint 5’’(R3)
ⱜ[
Joint 4(2L)
Joint 5’(R2)
Figure 6.4
the distance between joint 5 and the end-effector by R:
p
R =
(2L)2 + (2L + θ6 )2 .
Figure 6.4 on the right shows the paths of joint 5 and the end-effector. The radius of the path of joint
5 is 2L. Let the radii of the three paths be R1, R2, and R3, respectively. We can see that three circles
intersect at a point. This means that by changing θ4 , θ5 , and θ6 simultaneously, the end-effector can
be fixed to a stationary point. Therefore, there are an infinite number of position inverse kinematics
solutions.
Now let’s consider the desired end-effector orientation. Varying θ1 , θ2 and θ3 , and preserving the desired
position, the end-effector frame will rotate about the ẑ0 axis. Then varying θ4 , θ5 , θ6 , and preserving
the desired position, the end-effector frame will rotate about an axis on the x̂0 − ŷ0 plane. Therefore,
Go to the table of contents.
Chapter 6. Chapter 6 Solutions
103
two consequent rotations of constant axes determine the end-effector orienation, which means the
feasible orientations of the end-effector are of dimension two. Indeed, the end-effector orientation can
be determined by,
R = e[w1 ](θ1 +θ2 ) e[w4 ](θ4 +θ5 ) R0 = e[ẑ0 ](θ1 +θ2 ) e[x̂0 ](θ4 +θ5 ) R0 .
Therefore, unless the given desired end-effector orientation is a valid orientation, no feasible solution
exists.
√
θ3
= −zA , θ3 can be eaily determined as θ3 = − 2zA .
(b) First determine θ1 , θ2 , and θ3 for point A. Since √
2
From Figure 6.5, D and d are determined as
q
2
D =
x2A + yA
θ3
L + √ = L − zA
2
d =
Using the law of cosines, α and β can be determined as
2L
point A
ⱜY
ⱜX
D
L
2L
45
zW
β
–
ⱜX
point A
xW
d
α
ⱜZ
L
ⱜY
yW
yW
¢W¤
xW
2L
Figure 6.5
α
=
cos−1
β
=
cos−1
2
x2A + yA
+ 4L2 − (L − zA )2
p
2
4L x2A + yA
2
4L2 + (L − zA )2 − x2A − yA
4L(L − zA )
For the elbow-down configuration,
tan−1 (−
θ1
=
θ2
= π−β
θ1
=
θ2
= π+β
xA
)−α
yA
For the elbow-up configuration,
tan−1 (−
xA
)+α
yA
Next we determine θ4 and θ5 for points A and B. Once θ1 , θ2 , and θ3 are obtained, we can easily
derive the position and orientation of joint 4. Consider a new frame positioned at the center of joint
4 with the same z-axis as frame {0} and x-axis in the direction of rotation of joint 4. Then the new
coordinates for point B can be determined from the coordinate change. Finally the problem of solving
(θ4 , θ5 ) boils down to the inverse kinematics problem for a 2R planar robot.
Go to the table of contents.
104
Chapter 6. Chapter 6 Solutions
Exercise 6.7. As shown in Figure 6.6, the slope is mostly horizontal near the initial solution, so the
numerical inverse kinematics method jumps over the closer solution and converges to the farther one. The
xd − f (θ)
slope = − ∂f
∂θ (θ0 )
xd − f (θ0 )
θ0
θ
θ1
∆θ =
∂f
∂θ (θ0 )
−1
(xd − f (θ0 ))
Figure 6.6
basins of attraction for each root depend on the slope of the function xd − f (θ) near them. Steeper slopes
are more likely to converge to the closer solution.
Exercise 6.8.
The gradient is [2x, 0; 0, 2y]. The initial guess and first two iterations are given as: (x1 , y1 ) = (1, 1), (x2 , y2 ) =
(2.5, 5), and (x3 , y3 ) = (2.05, 3.4). The solution converges to (x, y) = (2, 3) eventually, but (x, y) = (−2, −3),
(x, y) = (−2, 3), and (x, y) = (2, −3) are all valid as well. Therefore there are are a total of four solutions.
Exercise 6.9.
Solution converged after three iterations using ω = 0.001 rad (or 0.057◦ ) and v = 10−4 m.
i
0
1
2
3
θi (in degrees)
(0.00, 30.00◦ )
(121.2◦ , 52.82◦ )
(90.00◦ , 128.4◦ )
(90.00◦ , 120.00◦ )
(x, y)
(1.866, 0.500)
(−1.512, 0.960)
(−0.783, 0.378)
(−0.866, 0.5)
Vb = (ωzb , vxb , vyb )
(3.142, 2.145, 3.717)
(0.628, −0.545, 0.594)
(−0.147, 0.000, −0.147)
(0.000, 0.000, 0.000)
kωb k
3.142
0.628
0.147
0.000
kvb k
4.292
0.806
0.147
0.000
Exercise 6.10.
(a) The orientation of the end-effector is determined by Rsb (θ) = e[ẑ]θ1 e[ŷ]θ2 e−[ẑ]θ3 . We define a body twist
−1
ωb for SO(3) as [ωb (θ)] = log(Rsb
(θ)R). This leads to the following inverse kinematics algorithm,
analogous to that of SE(3):
1) Initialization: Given R and inital guess θ0 ∈ R3 . Set i = 0
−1 i
2) Set [ωb ] = log(Rsb
(θ )R). While kωb k > ω for small ω :
• Set θi+1 = θi + Jb† (θi )ωb .
• Increment i.
(b) The forward kinematics is described in (a), and the corresponding body jacobian is computed as
Jb (θ) = e[ẑ]θ3 e−[ŷ]θ2 ẑ e[ẑ]θ3 ŷ ẑ ∈ R3×3 .
With the initial guess θ0 = (0, π/6, 0)T and desired end-effector frame R as defined in the problem, a
single iteration results in θ1 = (23.5505◦ , 1.5720◦ , 23.5505◦ )T .
Exercise 6.11.
Go to the table of contents.
Chapter 6. Chapter 6 Solutions
105
(a) The end-effector position vector p = (x, y, z)T is determined by p = Rot(−ẑ, θ1 )Rot(ω̂2 , θ2 )Rot(−ŷ, θ3 )(0, L, 0)T ,
where each rotaion matrix can be represented in explicit form as

cos θ1
Rot(−ẑ, θ1 ) =  − sin θ1
0

0
0 ,
1
sin θ1
cos θ1
0

cos θ2
 − √1 sin θ
2
Rot(ω̂2 , θ2 ) = I + [ω̂2 ] sin θ2 + [ω̂2 ] (1 − cos θ2 ) = 
2
2
√1 sin θ2
2


cos θ3 0 − sin θ3
.
1
0
Rot(−ŷ, θ3 ) =  0
sin θ3 0 cos θ3
√1 sin θ2
2
1
2 (1 + cos θ2 )
1
2 (1 − cos θ2 )

− √12 sin θ2

1
2 (1 − cos θ2 )  and
1
2 (1 + cos θ2 )
Through straightforward matrix multiplication we get
  √1

L cos θ1 sin θ2 + 21 L sin θ1 (1 + cos θ2 )
x
2
 y  =  − √1 L sin θ1 sin θ2 + 1 L cos θ1 (1 + cos θ2 ) 
2
2
1
z
L(1
−
cos θ2 )
2

Let, f : R2 → R2 be a map defined as
x
y
= f(
θ1
θ2
"
)=
√1 L cos θ1 sin θ2 + 1 L sin θ1 (1 + cos θ2 )
2
2
− √12 L sin θ1 sin θ2 + 12 L cos θ1 (1 + cos θ2 )
#
.
The Jacobian of f with respect to θ = (θ1 , θ2 ) is
∂f
=L
∂θ
"
√1 cos θ1 cos θ2 − 1 sin θ1 sin θ2
− √12 sin θ1 sin θ2 + 12 cos θ1 (1 + cos θ2 )
2
2
1
1
− √2 cos θ1 sin θ2 − 2 sin θ1 (1 + cos θ2 ) − √12 sin θ1 cos θ2 − 21 cos θ1 sin θ2
#
.
The Newton-Rhapson method is applied via the following iterative update rule for some initial value
θ0 :
k k+1 k ∂f −1
x
θ
θ1
θ1
(
− f ( 1k ))
=
+ ∂θ
y
θ2
θ2k
θ2k+1
Note that given only the position information, θ3 can be arbitrary.
(b)

r11
R =  r21
r31
r12
r22
r32

r13
r23  = Rot(−ẑ, θ1 )Rot(ω̂2 , θ2 )Rot(−ŷ, θ3 )
r33
Substituting the explicit forms of the rotation matrices and after straightforward multiplication, we
have
1
(1 − cos θ2 )
2
1
1
r12 = √ cos θ1 sin θ2 + sin θ1 (1 + cos θ2 )
2
2
1
1
r31 = √ cos θ3 sin θ2 − sin θ3 (1 + cos θ2 )
2
2
r32 =
From Equation (6.3),
θ2 = ± cos−1 (1 − r32 ).
Go to the table of contents.
(6.3)
(6.4)
(6.5)
106
Chapter 6. Chapter 6 Solutions
Let ρ sin φ = √12 sin θ2 and ρ cos φ = 12 (1 + cos θ2 ) for some positive scalar ρ and angle φ determined
via
r
1
1
ρ=
sin2 θ2 + (1 + cos θ2 )2
2
4
q
q
q
2 =
2 + r2 =
2 + r2
(6.6)
= 1 − r32
r12
r31
22
33
1
1
φ = atan2( √ sin θ2 , (1 + cos θ2 ))
2
2
2
2
Equation (6.4) then reduces to r12 = ρ sin(θ1 + φ). Also, since ρ2 = r12
+ r22
holds from Equation
(6.6), we have r22 = ρ cos(θ1 + φ). Therefore,
θ1 = −φ + atan2(r12 , r22 ).
Similarly, from Equation (6.5) and (6.6) we have r31 = ρ sin(φ − θ3 ) and r33 = ρ cos(φ − θ3 ). Therefore,
θ3 = φ − atan2(r31 , r33 ).
Exercise 6.12.
θd = (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ) = (8.86937, 44.5978, −77.1251, 82.793, −0.5554, −32.9868)
Exercise 6.13.
θd = (θ1 , θ2 , θ3 , θ4 , θ5 , θ6 , θ7 ) = (−8.88147, 5.8112, −16.5589, −35.8052, −15.28, 21.0232, 3.64464)
Exercise 6.14.
∂g
∂g
∂g
(x∗ )ẋ(0) = 0 implies ẋ(0) ∈ Null( ∂x
(x∗ )) = Range( ∂x
(x∗ )T )⊥ . Therefore
First the condition ∂x
ẋ(0) ⊥ Range(
∂g ∗ T
(x ) )
∂x
∂g
holds. Meanwhile, the second condition implies that ∇f (x∗ ) is perpendicular to ẋ(0). Since Range( ∂x
(x∗ )T )
∂g
∗
∗ T
forms the orthogonal space to arbitrary ẋ(0), we have ∇f (x ) ∈ Range( ∂x (x ) ) which means there exists
∂g
some vector c ∈ Rm such that ∇f (x∗ ) = ∂x
(x∗ )T c. Defining the Lagrange multiplier as λ∗ = −c, we obtain
the first-order necesary condition of the form
∇f (x∗ ) +
Exercise 6.15.
∂g ∗ T ∗
(x ) λ = 0.
∂x
−1
A D
A + EG−1 F
(a) The formula can be proven through direct multiplication of
and
C B
−G−1 F
and checking whether the resulting four sub-matrices equal those of identity matrix.
(b) The first-order necessary condition can be derived as
−EG−1
G−1
Qx + c + AT λ = 0,
where λ ∈ Rm is a Lagrange multiplier. The above together with the constraint Ax = b can be
integrated into a single matrix equation as
∗ x
−c
Q
AT
=
.
λ∗
b
A 0m×m
Computing the matrix inverse using the formula in (a), we have the following closed form solutions for
optimal x∗ and corresponding Lagrange multiplier λ∗ :
∗ −1
x
Q − Q−1 AT (AQ−1 AT )AT Q−1 Q−1 AT (AQ−1 AT )−1
−c
=
λ∗
b
(AQ−1 AT )−1 AT Q−1
(AQ−1 AT )−1
Go to the table of contents.
,
Chapter 7 Solutions
Exercise 7.1.
As shown in in Figure 7.1, let θi1 and θi3 be the revolute joint angles at Ai and
Bi respectively,
and θi2 be
cos θ − sin θ
the prismatic joint value at the rod Ai Bi . Let the rotation matrix R(θ) be
, and di0 be
sin θ
cos θ
A2
B3
y
B2
B1
x
P
b1
P
y
d1
a1
A1
O
A3
x
Figure 7.1
the unit vector of the direction from Ai to Bi when θi1 = 0, which together lead to di = R(θi )di0 .
(a) For the inverse kinematics, θi2 for i = 1, 2, 3 are to be determined given R and p. The position of Bi
is given by θi2 di + ai = Rbi + p. Thus the solution is θi2 = kRbi + p − ai k.
(b) For the forward kinematics, R and p are to be determined given θi2 for i = 1, 2, 3. The number of
unknowns in R and p is equal to 3 (1 for R and 2 for p). R and p can be found from the following
equations:
θi2 = kRbi + p − ai k,
i = 1, 2, 3.
The solution for the forward kinematics can be derived in another way. Note that
k(θ12 d1 + a1 ) − (θ22 d2 + a2 )k − kb1 − b2 k = 0,
k(θ22 d2 + a2 ) − (θ32 d3 + a3 )k − kb2 − b3 k = 0,
k(θ32 d3 + a3 ) − (θ12 d1 + a1 )k − kb3 − b1 k = 0.
di , for i = 1, 2, 3, can be found from above three equations. Let vi denote Rbi + p = θi2 di + ai . Note
that vi is known for i = 1, 2, 3. Then we have
−1
b1 − b2 b2 − b3
R = v1 − v2 v2 − v3
, p = vi − Rbi
(c)

F

Mz
 fx 
fy
T
= G
 τ
(Rb1 )x
0
= 
1
107
−(Rb2 )x
0
−1


(Rb3 )x
τ1
  τ2  .
0
1
τ3
108
Chapter 7. Chapter 7 Solutions
Since GT is singular, this configuration is an end-effector singularity.
The spatial twist of the end-effector frame can be written as V = G1 (θ1 )θ̇1 = G2 (θ2 )θ̇2 = G3 (θ3 )θ̇3 . In
this configuration,






1
0
1
1
0
1
1
0
1
0 θ12  , G2 =  ka2 k 0 ka2 k − θ22  , G3 = 
0
0
θ32  ,
G1 =  0
ka1 k 1 ka1 k
0
−1
0
−ka3 k 1 −ka3 k
which can be rearranged as
G1
0
Gθ̇ =
−G2
−G2


θ̇1
 θ̇2  .
θ̇3
0
G3
Rearrange this equation again into the form
where

1
 0

 ka1 k
Gp = 
 0

 0
0
1
θ12
ka1 k
0
0
0
Ga
Gp
θ̇a
θ̇p
,
−1
−1
−ka2 k −ka2 k + θ22
0
0
−1
−1
−ka2 k −ka2 k + θ22
0
0
0
0
0
1
0
−ka3 k

0

0


0
.

1

θ32 
−ka3 k
Since Gp is singular, this configuration is an actuator singularity.
Exercise 7.2.
cos φ − sin φ
dix
(a) Note that p + Rsb bi = ai + di for i = 1, 2, 3, where Rsb =
. Therefore,
=
sin φ cos φ
diy
px
cos φ − sin φ
bix
aix
+
−
for i = 1, 2, 3.
py
sin φ cos φ
biy
aiy
2
2
Since si 2 = dix + diy , we get
si 2 = (px + bix cos φ − biy sin φ − aix )2 + (py + bix sin φ + biy cos φ − aiy )2 ,
i = 1, 2, 3.
(b) If all the joint variables are fixed, the length of legs 1, 2, 3 shown in in Figure 7.2, become constant.
Then ends of leg 1 and leg 2 meet at two points (right-side and left-side) as they form triangles. If
Figure 7.2
we draw a circle of radius 2 (the distance between the R-joints of moving platform) and find the point
Go to the table of contents.
Chapter 7. Chapter 7 Solutions
109
which satisfies the condition that the distance from the base joint of the 3rd leg is equal to the length
of leg 3 on the circle, we also get two points. Therefore, the maximum possible number of forward
kinematics solution is 2 × 2 = 4.
(c) The planar wrench in the fixed frame {s} is




τ1
ms
−w1 × q1 −w2 × q2 −w3 × q3 
τ2  ,
Fs =  fsx  =
w1
w2
w3
fsy
τ3
1
1
where w1 = (1, 0), w2 = ( √ , √ ), w3 = (1, 0), q1 = (0, 1), q2 = (0, −1), q3 = (0, −1). Therefore
2
2


1
−1 √
1 
 



2
1
−2


 1 √1

1  0  =  0 .
Fs = 


2
0

 −1
1
0 √
0
2
T
T
Since mb = Rsb
[psb ]T fs + Rsb
ms and fb = Rbs fs with Rbs = I, we have Fs = Fb , or equivalently,
(mbz , fbx , fby ) = (−2, 0, 0).
P
(d) Using Grübler’s formula with N = 21, J = 27, and fi = 27, the degrees of freedom of this mechanism
is 3 × (21 − 27 − 1) + 27 = 6.
Exercise 7.3.
cos φ − sin φ
. Using the relationship
sin φ cos φ
between |di |2 and θi , a set of independent equations relating φ, p and joint variables θ1 , θ2 , θ3 can be
derived. From the law of cosines and the above definition,
(a) Define vector di = p + Rbi − ai for i = 1, 2, 3, where R =
|di |2 = 2L2 − 2L2 cos(π − θi ) = 2L2 (1 + cos θi )
= (p + Rbi − ai )T (p + Rbi − ai )
for i = 1, 2, 3, where L is the length of each link. Therefore
(p + Rbi − ai )T (p + Rbi − ai ) = 2L2 (1 + cos θi ),
i = 1, 2, 3.
(b) Given R and p from the above equation, the maximum possible number of solutions for θi is two
for i = 1, 2, 3. This means each joint can have elbow-up and elbow-down solutions. Therefore, the
maximum possible number of inverse kinematic solutions is 2 × 2 × 2 = 8. In contrast, given θ1 , θ2 , θ3 ,
there exist three constraint equations with three unknowns (two for p, one for R). Since |di |2 can
be determined, this problem is similar to that of the 3×RPR planar parallel mechanism in Section
7.1.1. According to this problem, three constraint equations can be reduced to a single sixth-order
polynomial. Thus, the maximum possible number of forward kinematic solutions is six. Further
verification is needed to show that all six mathematical solution are feasible.
Exercise 7.4.
(a) There are six variables (θ1 , θ2 , θ3 , ψ1 , ψ2 , ψ3 ), so six equations are needed. In order to derive the
equations, define the following vectors shown in Figure 7.3:
• a1 is the vector ~a1 expressed in {s} frame coordinates.
• a2 is the vector ~a2 expressed in {s} frame coordinates.
• b1 is the vector ~b1 expressed in {b} frame coordinates.
• b2 is the vector ~b2 expressed in {b} frame coordinates.
Go to the table of contents.
110
Chapter 7. Chapter 7 Solutions
F
y
b
ψ3
D
ψ2
ψ1
x
{b}
a
b
θ3
y
C
θ1
θ2
{s} x
E
a
B
A
Figure 7.3
From the problem description,
a1x
3
a1 =
=
,
a1y
0
b1x
1
b1 =
=
,
b1y
−1
a2
=
b2
=
a2x
a2y
b2x
b2y
=
=
4
3
1
1
,
.
Now, derive the equations for each leg.
1) Leg ABC:
px
a1x
− cos θ1
− sin(θ1 + θ2 )
cos φ − sin φ
b1x
=
+
+
−
py
a1y
sin θ1
cos(θ1 + θ2 )
sin φ cos φ
b1y
φ = θ1 + θ2 + θ3 .
(7.1)
(7.2)
2) Leg DEF:
px
a2x
− cos ψ1
− cos(ψ1 + ψ2 )
cos φ − sin φ
b2x
=
+
+
−
py
a2y
sin ψ1
− sin(ψ1 + ψ2 )
sin φ cos φ
b2
φ = ψ1 + ψ2 + ψ3 .
(7.3)
(7.4)
By solving equations (7.1), (7.2), (7.3), and (7.4), the inverse kinematics solution can be found.
(b) In order to determine whether or not the configuration is an actuator singularity, we need to examine
the determinant of Hp in the equation
q̇a
Ha Hp
= 0.
q̇p
If Hp is invertible, this configuration is not an actuator singularity. In order to find Hp , the Jacobian
of each leg must be determined. From the figure, each leg Jacobian is given by


1
1
1
0
1 
Leg ABC: J1,s =  0
−3 −2 −2
Go to the table of contents.
Chapter 7. Chapter 7 Solutions
111


1
1
3
3 .
−3 −2
1
Leg DEF: J2,s =  3
−4
The kinematic loop constraints can be expressed as T1 (θ) = T2 (ψ). Taking right differentials of both
sides of the equation, we have Ṫ1 T1−1 = Ṫ2 T2−1 . Since Ṫ T −1 = [V ], the identity can also be expressed
in terms of the forward kinematics Jacobian for each chain:
J1,s θ̇ = J2,s ψ̇,
which can also be arranged as
J1,s
−J2,s
θ̇
ψ̇
= 0.
Therefore we get



1
0
−3

θ̇1


 θ̇2 
1
1 −1 −1 −1 

 θ̇ 
0
1 −3 −3 −3   3  = 0.
 ψ̇ 
−2 −2
4
3
2  1 
 ψ̇2 
ψ̇3
The equation above can be rearranged as

Ha
Hp
q̇a
q̇p
Therefore

=
1
0
−3

θ̇1


 ψ̇1 
−1 −1
1
1 −1 

 ψ̇ 
−3 −3
0
1 −3   2  = 0.
 θ̇ 
4
3 −2 −2
2  2 
 θ̇3 
ψ̇3

1
Hp =  0
−2

1 −1
1 −3  .
−2
2
Since det(Hp ) is equal to zero, this configuration is an actuator singularity.
(c) Since the actuator joints now become A, B, and D, the equation in (b) can now be rearranged into
the form


θ̇1



 θ̇2 
1
1 −1
1 −1 −1 

 ψ̇ 
 0
0 −3
1 −3 −3   1  = 0
 θ̇ 
−3 −2
4 −2
3
2  3 
 ψ̇2 
ψ̇3




1
1 −1
1 −1 −1
0 −3  and Hp =  1 −3 −3 . Since Hp is invertible, we have
with Ha =  0
−3 −2
4
−2
3
2
q̇p = −Hp−1 Ha q̇a = Ga q̇a ,
or equivalently,
  3
θ̇3
−2
 ψ̇2  =  1
− 23
ψ̇3

− 32
0
− 12


θ̇1
0
−2   θ̇2  .
1
ψ̇1
Go to the table of contents.
112
Chapter 7. Chapter 7 Solutions
We can find the Jacobian Ja from the equation below:

Vs
=
=


θ̇1
1
J1,s θ̇ = J1,s  θ̇2  = J1,s  0
− 23
θ̇3


 1
θ̇1
− 2 − 12 0
 − 3 − 3 0   θ̇2  .
2
2
0
1 0
ψ̇1
0
1
− 32


θ̇1
0
0   θ̇2 
0
ψ̇1
Therefore, the Jacobian such that Vs = Ja q̇a is
− 12
− 32
1
− 21
Ja =  − 23
0


0
0 .
0
Exercise 7.5.
(a) Using Grübler’s formula for spatial mechanism with N = 8 and J = 9, the degrees of freedom is
M = 6 · (N − 1 − J) + Σfi = 6 · (8 − 1 − 9) + 1 · 6 + 3 · 3 = 3.
(b) As shown in Figure 7.4, let ai and di respectively be the vector ai and di expressd in {s}-frame
coordinates; bi is the vector bi expressed in {b}-frame coordinates. The kinematic constraint equations
s3
b3
b2
w
s2
{b}
u
s
normal
b1
s1
d1
v
p
s1
z
{s}
a1
x
y
Figure 7.4
are then given by
di = Rsb bi + psb − ai ,
i = 1, 2, 3.
(c) Defining three unit vectors u, v and w along the x̂-, ŷ- and ẑ-axis of the moving coordinate frame {b},
Go to the table of contents.
Chapter 7. Chapter 7 Solutions
113
the rotation matrix Rsb is given by


ux vx wx
Rsb =  uy vy wy 
uz vz wz

cos θ cos φ − sin θ cos ψ + cos θ sin φ sin ψ
cos θ cos ψ + sin θ sin φ sin ψ
=  sin θ cos φ
− sin φ
cos φ sin ψ

sin θ sin ψ + cos θ sin φ cos ψ
− cos θ sin ψ + sin θ sin φ cos ψ  .
cos φ cos ψ
Define si = ai + di and b0i = Rsb bi (note that these are expressed in {s}-frame coordinates). Then
i
h
i
h
√
√
a1 = a 0 0 , a2 = − 21 a 23 a 0 , a3 = − 21 a − 23 a 0 ,
psb =
s1 =
a
0
d1
x
y
z
,
h
√
√
− 12 a 23 a d2 , s3 = − 21 a − 23 a
b01 = a − x −y d1 − z ,
h
i
√
b02 = − 21 a − x 23 a − y d2 − z ,
h
i
√
b03 = − 21 a − x − 23 a − y d3 − z .
, s2 =
h
i
d3
i
,
Note that a, x, y, z are known, while d1 , d2 , d3 are unknown. We have
b01 · b02 = kb01 k · kb02 k · cos 120,
b02 · b03 = kb02 k · kb03 k · cos 120,
b01 · b03 = kb01 k · kb03 k · cos 120,
which can be written again as follows:
  0 0


b1 · b2 − kb01 k · kb02 k · cos 120
f1 (d1 , d2 , d3 )
f (d1 , d2 , d3 ) =  f2 (d1 , d2 , d3 )  =  b02 · b03 − kb02 k · kb03 k · cos 120  = 0
f3 (d1 , d2 , d3 )
b01 · b03 − kb01 k · kb03 k · cos 120
(7.5)
Equation (7.5) can be solved numerically for d1 , d2 , d3 using the Newton-Raphson method. We can
also solve for the three unit vectors u, v and w of the moving frame {b}. These unit vectors can be
expressed as follows:
b1
u = ux uy uz =
kb1 k
w = wx wy wz = snormal
v = vx v y vz = w × u
Therefore the Euler angles using Rsb are obtained as
u
θ = tan−1 ( uxy ), φ = sin−1 (−ux ), ψ = tan−1 ( wvzz )
Exercise 7.6.
(a) The following kinematics analysis is described in more detail in J. Kim et al. (2002).
1) Forward kinematics: We use the numerical approach since analytic solutions are not easily derived
for this mechanism. First, the constraint eqatuions are given by


(c1 − c2 )T (c1 − c2 ) − D2
f (θa , θp ) =  (c2 − c3 )T (c2 − c3 ) − D2  = 0,
(7.6)
(c3 − c1 )T (c3 − c1 ) − D2
Go to the table of contents.
114
Chapter 7. Chapter 7 Solutions
where the positions of the spherical joints Ci in fixed frame coordinates are given by


cos θi (rb − L cos θi+6 )
ci =  sin θi (rb − L cos θi+6 )  ,
i = 1, 2, 3.
θi+3 + L sin θi+6
In the equations above, θa = [θ1 , θ2 , θ3 , θ4 , θ5 , θ6 ] represents the active joints, θp = [θ7 , θ8 , θ9 ] the passive
revolute joints, rb the base radius, rp the moving platform radius, L1 = L2 = L3 = L the link
√ length
between the revolute joint and the spherical joint for each leg, and D = D1 = D2 = D3 = 3rp the
distances between the spherical joints.
Second, given θa , determine θp that satisfy Equation (7.6) using the Newton-Raphson method or other
numerical methods. Denote by p ∈ R3 and R ∈ SO(3) the position and orientation of the end-effector
frame, respectively, expressed in the fixed frame coordinates. Then we get p and R as follows:
1
(c1 + c2 + c3 ),
3
R = [Rx Ry Rz ],
1
Rx = (c1 − p),
rp
1
Ry = √
(c2 − c3 ),
3rp
p=
Rz = Rx × Ry
2) Inverse kinematics: In contrast to the forward kinematics, the inverse kinematics problem can be
solved analytically. Given the desired position and orientation of the end-effector frame, p and R, the
positions of the spherical joint centers in the fixed frame are
bi = R b0i + p,
where b0i is the position of Bi in the end-effector frame coordinates for each leg, and given by


rp cos( 32 (i − 1)π)
i = 1, 2, 3.
b0i =  rp sin( 32 (i − 1)π)  ,
0
The joint variable for the circular slide is obtained via the atan2 function:
θi = atan2(cix , ciy ),
i = 1, 2, 3.
Calculating the revolute joint angles for Ci ,
q


Ra − c2ix + c2iy
,
θi+6 = arccos 
L
i = 1, 2, 3.
Two solutions are obtained from this equation; these solutions need to be checked if they are physically
realizable. The prismatic joint values are
θi+3 = ciz − L sin θi+6 ,
i = 1, 2, 3.
At most two solutions are possible depending on the number of solutions for the revolute joint angles.
(b) The Jacobian Ja such that Vs = Ja θ̇a can be obtained by differentiating the kinematic constraint
Equation (7.6).
∂f
∂f
θ̇a +
θ̇p = 0,
∂θa
∂θp
−1
∂f
∂f
θ̇p = −
θ̇a .
∂θp
∂θa
Go to the table of contents.
Chapter 7. Chapter 7 Solutions
115
∂f
is not invertible, in which case the passive joint values cannot
An actuator singularity occurs when ∂θ
p
be obtained from the active joint values. According to J. Kim et al. (2001), this happends when the
tilting angle of the moving platform is approximately 30 degrees.
An end-effector singularity occurs when the end-effector loses one or more degrees of freedom, for
example, the configuration in which the moving platform is parallel with respect to the ground, and
one of the spherical joints Ci is located exactly above the center of the fixed platform. A more detailed
singularity analysis including visualization of some singularity examples can be found in J. Kim et al.
(2001).
(i) J. Kim, K.S. Cho, J.C. Hwang, C.C. Iurascu, and F.C. Park. Eclipse-RP: A new RP machine based
on repeated deposition and machining. Proceedings of the Institution of Mechanical Engineers, Part
K: Journal of Multi-body Dynamics, 216(1), pp.13-20, 2002.
(ii) J. Kim, F.C. Park, S.J. Ryu, J. Kim, J.C. Hwang, C. Park, and C.C. Iurascu. Design and analysis
of a redundantly actuated parallel mechanism for rapid machining. IEEE Transactions on robotics and
automation, 17(4), pp.423-434, 2001.
Exercise 7.7. The Delta robot consists of three kinematic chains connecting the base with the end-effector.
Due to parallelogram four-bar linkage, the end-effector always remains parallel to the base. First derive
the kinematic constraint equations; denote the parameters as shown in Figure 7.5. Three revolute joints
A1 , A2 , A3 are actuated and their joint variables are θ11 , θ12 , θ13 . Define the following vectors:
• psb = (x, y, z)T = p in {s}-frame coordinates.
• ai ∈ R3 = ai in {s}-frame coordinates.
• bi ∈ R3 = bi in {s}-frame coordinates.
• ci ∈ R3 = ci in {s}-frame coordinates.
• di ∈ R3 = di in {b}-frame coordinates.
• Rsb ∈ SO(3) is the orientation of {b} as seen from {s}.
Note that
ai + bi + ci = psb + Rsb di = psb + di ,
for i = 1, 2, 3, where Rsb = I. Since the length of ci is constant, we obtain three kinematic constraint
equations:
kci k2 = L22 = kpsb di − ai − bi k2 ,
i = 1, 2, 3.
(a) Given the position of the end-effector frame {b}, psb = (x, y, z)T , the above kinematic equations can
be written in the form
Pi cos θi + Qi sin θi + Ri = 0,
Defining ti = tan(θi /2) for i = 1, 2, 3,
1 − t2i
2ti
+ Qi
+ Ri = 0,
Pi
1 + t2i
1 + t2i
(Ri − Pi )t2i + 2Qi ti + Ri + Pi = 0,
i = 1, 2, 3.
i = 1, 2, 3,
i = 1, 2, 3.
We obtain two solutions for each leg:
p
Pi2 + Q2i − Ri2
ti =
,
Ri − Pi
θi = 2 tan−1 ti ,
i = 1, 2, 3.
−Qi ±
Therefore eight possible solutions exist.
Go to the table of contents.
i = 1, 2, 3,
116
Chapter 7. Chapter 7 Solutions
Ls
ẑs
ŷs
{s}
A1
x
ˆ1
O
a1
ˆs
x
L1
θ11
b1
B1
p
L2
c1
ẑb
ŷb
{b}
d1
C1
P
Lb
x̂b
Figure 7.5
(b) Although it is generally difficult to solve the forward kinematics for closed chains, the solution for the
Delta robot can be derived comparably easily since the end-effector always remains parallel to the base
and only translational movement is possible (i.e., Rsb = I). Given the joint angles (θ1 , θ2 , θ3 ), the
position of Bi in {s}-frame coordinates is ai + bi . Now consider a set of imaginary spheres with their
centers located at B0i , with the position vectors of B0i expressed in {s} as ai + bi − di for i = 1, 2, 3.
The forward kinematics solution is simply the intersection of these spheres, the raidus of which is L2 .
In most cases, the number of intersection points of three spheres is two. From the configuration of the
Delta robot, the unique solution can be determined in certain cases.
(c) Let us define coordinates frame {i}, composed by x̂i -ŷi -ẑi as shown in Figure 7.6, such that plane x̂s -ŷs
and plane x̂i -ŷi are identical, and axes ẑs and ẑi are also identical. Let φi be the angle measured from
x̂s to x̂i . Note that φi is constant. Let θ1i be the angle measured from x̂i to Ai Bi . Let θ2i be the angle
measured from Ai Bi to Bi Ci . Let θ3i be the angle measured from ŷi to Bi Ci . Since we assume that
the revolute joints at the fixed base are actuated, the vector of actuated joint variables qa is given by
qa = (θ11 , θ12 , θ13 )T .
The Jacobian matrix relates the velocities of actuator joints to the spatial velocity of the end-effector.
We only need to consider the linear velocity of the end-effector frame as the angular velocity is zero
Go to the table of contents.
Chapter 7. Chapter 7 Solutions
117
ẑi
ẑs
ŷs
O
ai A
φi
x
ˆs
x
ˆi
ŷi
Ai
θ1i
i
bi
Bi
Bi
p
θ3i
ci
P
di
θ2i
ci
Ci
Ci
Figure 7.6
for all time, so that we have ṗ = Ja q̇a . First note that
p + di = ai + bi + ci .
(7.7)
Expressing the equation above with respect to x̂i -ŷi -ẑi ,

  √
  √





3
3
x cos φi − y sin φi
cos θ1i
sin θ3i cos(θ2i + θ1i )
3 Lb
6 Ls
 x sin φi − y cos φi  +  0  =  0  + L1 
 + L2 
.
0
cos θ3i
z
− sin θ1i
− sin θ3i sin(θ2i + θ1i )
0
0
If we differentiate the Equation (7.7) with respect to time,
ṗ = ḃi + ċi = ωbi × bi + ωci × ci
since ai , di are constant. ωbi and ωci are the angular velocity of links Ai Bi and Bi Ci , respectively. To
eliminate ωci , take the scalar product with the unit vector ||ccii || :
ci
ci
· ṗ =
· (ωbi × bi )
||ci ||
||ci ||
since ci · (ωci × ci ) = 0. Expressing the vectors in the x̂i -ŷi -ẑi -frame,
ci
· ṗ ={sin θ3i cos(θ2i + θ1i )}{ẋ cos φi − ẏ sin φi } + cos θ3i (ẋ sin φi − ẏ cos φi ) − sin θ3i sin(θ2i + θ1i )ż,
||ci ||
(7.8)
ci
· (ωbi × bi ) = L1 sin θ2i sin θ3i θ̇1i ,
(7.9)
||ci ||
where ωbi = (0, θ̇1i , 0)T . Rearranging Equation (7.8), (7.9),




J1x J1y J1z
ẋ
sin θ21 sin θ31
0
 J2x J2y J2z   ẏ  = L1 
0
sin θ22 sin θ32
J3x J3y J3z
ż
0
0


θ̇11
0
  θ̇12  ,
0
sin θ23 sin θ33
θ̇13
where
Jix = sin θ3i cos(θ2i + θ1i ) cos φi + cos θ3i sin φi ,
Jiy = − sin θ3i cos(θ2i + θ1i ) sin φi − cos θ3i cos φi ,
Jiz = − sin θ3i sin(θ2i + θ1i ).
Go to the table of contents.
118
Chapter 7. Chapter 7 Solutions
Rewriting this equation again as Jp ṗ = Jθ q̇a , the linear velocity of the end-effector is ṗ = Jp−1 Jθ q̇a .
Therefore the Jacobian Ja is given by Ja = Jp−1 Jθ .
(d) The actuator singularities arise when Jp is non-invertible. This happens if
θ3i = 0 or π
for all i
(7.10)
or
θ2i + θ1i = 0 or π
for all i.
(7.11)
If either (7.10) or (7.11) holds, the third column of Jp is equal to zero.
A more detailed derivation of the forward and inverse kinematics, and singularity analysis of the Delta
robot can be found in the following papers:
(i) K. S. Hsu, M. Karkoub, M. C. Tsai, M. G. Her, Modelling and index analysis of a Delta-type mechanism, Proceedings of the Institution of Mechanical Engineers, Part K: Journal of Multi-body Dynamics,
218(3):121-132, Sep 2004.
(ii) M. Lopez, E. Castillo, G. Garcia, A. Bashir. Delta robot: inverse, direct, and intermediate Jacobians, Journal of Mechanical Engineering Science, 220(1):103-109, Jan 2006.
Exercise 7.8. In general, three prismatic joints in each leg of the 3×UPU mechanism are actuated, while
two passive universal joints are attached at the ends of each leg. It is known that the platform of the 3×UPU
mechanism undergoes only translational motion if the universal joints are aligned so that axis 1 is parallel
to the axis 4, and axis 2 is also parallel to axis 3 as shown in the exercise.
Let ẑji denote a unit vector which points along the direction of axis j of leg i, and θji be the corresponding
joint angle. In addition, let ẑP i be a unit vector pointing along the prismatic joint axis of leg i. Let ai ∈ R3
be the vector from the origin of {0} to Ai expressed in coordinates frame {0} and bi ∈ R3 be the vector from
the origin of {1} to Bi expressed in coordinates frame {1}. Let p ∈ R3 be the position vector of the origin of
the frame {1} with respect to the frame {0}. Since no rotation is allowed for the moving platform, R01 = I.
The kinematic constraint equations are
p + bi = ai + di ẑP i ,
i = 1, 2, 3.
(a) Given the leg lengths di for i = 1, 2, 3, the position p of the moving platform needs to be determined.
From the kinematic constraint equations,
d2i = (p + bi − ai )T (p + bi − ai ),
i = 1, 2, 3.
Each equation represents a sphere of radius di whose center is located at (ai − bi ). The solution of
the forward kinematics is the intersection of these three spheres; generally, two solutions exist for this
problem. After some algebraic manipulation, we get two solutions whose x̂ and ŷ coordinate values are
the same, and the ẑ coordinate values are equal and opposite.
(b) Given the position p of the moving platform, the length of each leg di needs to be determined. From
the kinematic constraint equations,
q
i = 1, 2, 3
di = (p + bi − ai )T (p + bi − ai ),
since di should be positive.
(c) The forward kinematics can be derived in another way using the product of exponentials:
Ti (θ1i , θ2i , di , θ3i , θ4i ) = e[S1i ]θ1i e[S2i ]θ2i e[SP i ]di e[S3i ]θ3i e[S4i ]θ4i M,
i = 1, 2, 3,
where Ti , M ∈ SE(3). The kinematic loop constraints can be written as T1 (θ1 ) = T2 (θ2 ) and T2 (θ2 ) =
T3 (θ3 ) where θi ∈ R5 is defined to be (θ1i , θ2i , di , θ3i , θ4i ) for i = 1, 2, 3. Taking right differentials of
these equations, we have Ṫ1 T1−1 = Ṫ2 T2−1 and Ṫ2 T2−1 = Ṫ3 T3−1 . Since Ṫ T −1 = [V], these can also be
expressed in terms of the foward kinematcs Jacobian. Arranging them into a matrix form, we obtain


θ̇1
J1,s (θ1 ) −J2,s (θ2 )
0
 θ̇2  = 0.
0
J2,s (θ2 ) −J3,s (θ3 )
θ̇3
Go to the table of contents.
Chapter 7. Chapter 7 Solutions
119
The equation above can be rearranged into the form
q̇a
Ha Hp
= 0,
q̇p
where qa = (d1 , d2 , d3 ) and qp ∈ R12 is the vector for all revolute (passive) joint angles. If Hp is
invertible, using q̇p = −Hp−1 Ha q̇a , the Jacobian Ja such that Vs = J1,s θ̇1 = Ja q̇a can be obtained.
(d) An actuator singularity occurs when Hp becomes
singular. According to Han et al. (2002), when the
three legs lengths are identical, Ha Hp become rank-deficient, which implies both a configuration
space singularity and an actuator singularity. Experiments with a real prototype, however, show that
even though the three legs are locked, the mechanism still has extra degrees of freedom, which contradict
the singularity analysis results. Han et al. reveal that this gross motion is due to manufacturing
clearances and tolerances in the universal joints. More details can be found in (i) for the kinematics,
and (ii), (iii) for the singularity analysis.
(i) L. W. Tsai. Kinematics of a three-DOF platform with three extensible limbs. Recent advances in
robot kinematics, pages 401-410. Springer Netherlands. 1996
(ii) C. Han, J. Kim, and F. C. Park. Kinematic sensitivity analysis of the 3-UPU parallel mechanism.
Mechanism and Machine Theory, 37(8):787-798, 2002.
(iii) R. Di Gregorio and V. Parenti-Castelli. Mobility analysis of the 3-UPU parallel mechanism
assembled for a pure translational motion. ASME Journal of Mechanical Design, 124(2):259-264,
2002.
Go to the table of contents.
120
Chapter 7. Chapter 7 Solutions
Go to the table of contents.
Chapter 8 Solutions
Exercise 8.1.
First, the inertia matrix of a thin rectangular plate perpendicular to the z-axis can be derived as follows:
1
2
0 0 ,
Ixx =
12 M w
1
M l2 0 ,
Iyy = 0 12
1
M (w2 + l2 ) ,
Izz = 0 0 12
where the reference frame is located at the center of mass, M denotes the mass of the plate, and w and l are
the length of the edges along the x-axis and y-axis, respectively. Similarly, the inertia matrix of a circular
plate can be derived as follows:
1
2
0 0 ,
Ixx =
4Mr
Iyy = 0 14 M r2 0 ,
Izz = 0 0 12 M r2 ,
where the reference frame is located at the center of mass with z-axis perpendicular to the plate, M denotes
the mass of the plate, and r is the radius of the plate.
(a) Rectangular parallelepiped:
Z
Z
1
1
1
2
2
2
2
Ixx =
(w + h )dm =
(w + h ) 1 dm =
(w2 + h2 )M,
12
12
12
where dm is the infinitesimal mass of the thin plate between x and x + dx. By symmetry, it is
1 2
1
straightfoward to derive the remaining inertia components as Iyy = 12
(l + h2 )M, Izz = 12
(w2 + l2 )M .
(b) Circular cylinder:
Following (a), using a thinR circulate plate Rof infinitesimal mass dm between z and z + dz, it is straightfoward to compute Izz = 12 r2 dm = 12 r2 dm = 21 mr2 . To derive Ixx , use a thin rectangular plate of
an infinitesimal mass dmx between x and x + dx as follows:
Z
Z r
p
1
1
Ixx =
(4(r2 − x2 ) + h2 ) dmx =
(4(r2 − x2 ) + h2 )ρ r2 − x2 h dx,
12
−r 6
√
where the infinitesimal mass is dmx = 2ρ r2 − x2 h dx and ρ is the density. Integrating by substituting
√
R
Rr
2
2
2
2
dx = r tan θ and m = dmx = −r 2ρ r2 − x2 h dx, we obtain Ixx = m(3r12+h ) and Iyy = m(3r12+h ) .
(c) Ellipsoid:
Note that the inertia Icm of a sphere about the
through the center of mass is Icm = 25 M R2 where
R axis
2
M is the mass and R is the radius. Since I = r dm, we can compute the inertia terms of an ellipsoid
using the following change of coordinates:
Z
Izz =
x2 + y 2 dm
ellipsoid
Z
b
a
=
( x)2 + ( y)2 dm0
r
sphere r
Z
Z
a 2
b
= ( )
x2 dm0 + ( )2
y 2 dm0
r
r
sphere
sphere
1
=
m(a2 + b2 )
5
whereR dm is an infinitesimal
mass of ellipsoid, dm0 is an infinitesimal mass of sphere, and also the fact
R
1
2
0
that sphere x dm = 2 sphere x2 + y 2 dm0 = 51 mr2 . Similarly, Ixx = 15 m(c2 + b2 ), Iyy = 51 m(a2 + c2 ).
Exercise 8.2.
121
122
Chapter 8. Chapter 8 Solutions
(a) Aligning the z axis along the length of the dumbell gives Icylinder = diag{0.00647, 0.00647, 0.000377}
and Isphere,1 = Isphere,2 = diag{0.126, 0.126, 0.126}.
Using the parallel axis theorem gives Idumbell = diag{2.771, 2.771, 0.252}.
(b) The total mass of the dumbbell is mdumbbell = 64.72 kg. The spatial inertia matrix is Gb = diag{Ib , mI},
so Gb = diag{2.771, 2.771, 0.252, 64.72, 64.72, 64.72}
Exercise 8.3.
(a) The relation between Ga and Gb can be rewritten as
Ia
0
0
mI
Ga
[AdTba ]T Gb [AdTba ]
T
Rba T Rba
[pba ]T
Ib
=
0
0
Rba T
=
0
mI
Rba
[pba ]Rba
0
Rba
.
So we get
T
Ia = Rba T Ib Rba + mRba
[pba ]T [pba ]Rba .
When frame {a} is aligned with frame {b}, Rba = I. Substituting this into the formula yields
Ia = Ib + m[pba ]T [pba ].
Assume pba = q = (qx , qy , qz ), we finally get
Ia
= Ib + m[q]T [q]
 2
qy + qz 2
= Ib + m  −qy qx
−qz qx
= Ib + m(q T qI − qq T ).
−qx qy
qx 2 + qz 2
−qz qy

−qx qz
−qy qz 
qx 2 + qy 2
So Steiner’s theorem is a special case of the given equation and in turn that equation is a generalization
of Steiner’s theorem.
(b) Given the dynamic equations for a single rigid body, we have
Fa
=
=
=
[AdTba ]T Fb
[AdTba ]T Gb V̇b − [AdTba ]T [adVb ]T Gb Vb
[AdTba ]T Gb [AdTba ]V̇a − [AdTba ]T [adVb ]T Gb [AdTba ]Va
= Ga V̇a − [adVa ]T Ga Va
Exercise 8.4.
Figure 8.1 shows the rotational inverted pendulum for arbitrary θ1 and θ2 .
(a) The dynamic equations are derived via the Lagrangian formulation. Assume that frame {b1 } is the
end-effector frame. Then the body Jacobian Jb1 with respect to frame {b1 } is derived as follows:
i
1
wi
(0, 0, 1)
qi
(−L1 , 0, 0)
vi
(0, L1 , 0)
where vi = −wi × qi . Therefore

0
 0 


 1 

Jb1 = 
 0 .


 L1 
0

Go to the table of contents.
Chapter 8. Chapter 8 Solutions
123
ẑs
ŷs
θ1
x̂s {s}
ẑ1
x̂1
ŷ1
{b1 }
θ2
ẑ2
ŷ2
x̂2
{b2 }
Figure 8.1
The spatial velocity of link 1 expressed in frame {b1 } coordinates is



0
0
 0 
 0 




 1 


 θ̇1 =  θ̇1  .
Vb1 = Jb1 θ̇ = 
 0 
 0 




 L1 
 L1 θ̇1 
0
0

Now assume that frame {b2 } is the end-effector frame. Then the body Jacobian Jb2 with respect to
frame {b2 } is derived as follows:
i
1
2
wi
qi
vi
(0, sin θ2 , cos θ2 ) (−L1 , 0, L2 ) (−L2 sin θ2 , L1 cos θ2 , −L1 sin θ2 )
(1, 0, 0)
(0, 0, L2 )
(0, L2 , 0)
where vi = −wi × qi . Therefore

0

sin θ2

 cos θ2
Jb2 = 
 −L2 sin θ2

 L1 cos θ2
−L1 sin θ2

1
0 

0 
.
0 

L2 
0
The spatial velocity of link 2 expressed in frame {b2 } coordinates is

0

sin
θ2

 cos θ2
Vb2 = Jb2 θ̇ = 
 −L2 sin θ2

 L1 cos θ2
−L1 sin θ2



θ̇2
1


θ̇1 sin θ2
0 





θ̇
cos
θ
0 
θ̇


1
2
1

=

.
0 


−L
θ̇
sin
θ
θ̇
2
2 1
2



L2 
 L1 θ̇1 cos θ2 + L2 θ̇2 
0
−L1 θ̇1 sin θ2
Go to the table of contents.
124
Chapter 8. Chapter 8 Solutions
Substituting L1 = L2 = 1,



θ̇2
0


 0 
θ̇1 sin θ2






 θ̇1 
θ̇1 cos θ2




,
V
=
Vb1 = 
.

b
2

 −θ̇1 sin θ2 
 0 


 θ̇1 
 θ̇1 cos θ2 + θ̇2 
0
−θ̇1 sin θ2

The kinetic energy of the system is
K(θ, θ̇)
1 T
1
V Gb Vb + V T Gb Vb
2 b1 1 1 2 b2 2 2
1 T I1
1 T I2
0
V
Vb1 + Vb2
0 m1 I
0
2 b1
2
=
=
0
m2 I
Vb2 ,
where Gbi is the spatial inertia matrix of link i expressed in frame {bi } attached at the center of mass.
Substituting m1 = m2 = 2,
1 T I2 0
1 T I1 0
V
Vb1 + Vb2
Vb2
K(θ, θ̇) =
0 2I
0 2I
2 b1
2
=
3θ̇12 + (θ̇12 + 3θ̇2 + 3θ̇12 sin2 θ2 + 2θ̇1 θ̇2 cos θ2 )
=
4θ̇12 + 3θ̇2 + 3θ̇12 sin2 θ2 + 2θ̇1 θ̇2 cos θ2 .
Let the potential energy of the zero position be zero. The potential energy of the system is given by
P(θ) = m2 gL2 (1 − cos θ2 ).
Substituting m2 = 2, g = 10, and L2 = 1,
P(θ) = 20 − 20 cos θ2 .
The Lagrangian of the system is then
L(θ, θ̇)
= K(θ, θ̇) − P(θ)
=
4θ̇12 + 3θ̇2 + 3θ̇12 sin2 θ2 + 2θ̇1 θ̇2 cos θ2 + 20 cos θ2 − 20.
Therefore, the Euler-Lagrange equations can be written as
τ
=
=
=
d ∂L ∂L
−
dt ∂ θ̇
∂θ
d 8θ̇1 + 6θ̇1 sin2 θ2 + 2θ̇2 cos θ2
0
−
6θ̇12 sin θ2 cos θ2 − 2θ̇1 θ̇2 sin θ2 − 20 sin θ2
6θ̇2 + 2θ̇1 cos θ2
dt
8θ̈1 + 6θ̈1 sin2 θ2 + 12θ̇1 θ̇2 sin θ2 cos θ2 + 2θ̈2 cos θ2 − 2θ̇22 sin θ2
.
6θ̈2 + 2θ̈1 cos θ2 − 6θ̇12 sin θ2 cos θ2 + 20 sin θ2
Substituting θ1 = θ2 = π/4 and θ̇1 = θ̇2 = θ̈1 = θ̈2 = 0,
0√
τ1
=
.
τ2
10 2
(b) Substituting θ1 = θ2 = π/4 to the dynamic equations derived in (a),
√
√
11θ̈1 + 6√
θ̇1 θ̇2 + 2θ̈2 − √2θ̇22
τ =
6θ̈2 + 2θ̈1 − 3θ̇12 + 10 2
√ 11
2
θ̈1
√
=
+ ....
2 6
θ̈2
Go to the table of contents.
Chapter 8. Chapter 8 Solutions
125
Therefore, the mass matrix M (θ) when θ1 = θ2 = π/4 is
M (θ) =
11
√
2
√
2
6
.
The eigenvalues of the mass matrix are λ1 = 5.6277 and λ2 = 11.3723, and the corresponding eigenvectors are v1 = (0.2546, −0.9671)T and v2 = (−0.9671, −0.2546)T . Then the torque ellipsoid for the
mass matrix can be drawn as in Figure 8.2.
τ2
τ1
λ1 v1
λ2 v2
Figure 8.2
Exercise 8.5.
For two arbitrary X, Y ∈ se(3),
[adX (Y )] = [X][Y ] − [Y ][X]
holds. Then applying the same computation rule, we get the following identity
[adVi (adVj (Vk )] = [Vi ][Vj ][Vk ] − [Vi ][Vk ][Vj ] − [Vj ][Vk ][Vi ] + [Vk ][Vj ][Vi ]
(8.1)
for arbitrary given Vi , Vj , Vk ∈ se(3). Summing Equation (8.1) for (i, j, k) = (1, 2, 3), (3, 1, 2), (2, 3, 1) whose
left hand side is exactly that of the Jacobi identity, it is straightforward to show that all the terms in the
right hand side cancel each other and become zero.
Exercise 8.6.
(a) The ith column of the space Jacobian Js (θ) is
Ji = Ade[S1 ]θ1 ···[Si−1 ]θi−1 (Si ),
for i = 2, ..., n, with the first column J1 = S1 . Since Ji depends only on θ1 , · · · , θi−1 , the partial
∂Ji
i
derivative of the space Jacobian ∂J
∂θj becomes zero for i ≤ j. Otherwise, for i > j, ∂θj can be derived
using [·] notation as follows:
∂[Ji ]
∂θj
=
∂ [S1 ]θ1
(e
· · · e[Si−1 ]θi−1 )[Si ]e−[Si−1 ]θi−1 · · · e−[S1 ]θ1
∂θj
∂ −[Si−1 ]θi−1
+ e[S1 ]θ1 · · · e[Si−1 ]θi−1 [Si ]
(e
· · · e−[S1 ]θ1 ).
∂θj
Go to the table of contents.
126
Chapter 8. Chapter 8 Solutions
For the sake of simplicity, let e[S1 ]θ1 · · · e[Si−1 ]θi−1 = Ti−1 and e[S1 ]θ1 · · · e[Sj−1 ]θj−1 = Tj−1 . Then the
first term is
∂ [S1 ]θ1
(e
· · · e[Si−1 ]θi−1 )[Si ]e−[Si−1 ]θi−1 · · · e−[S1 ]θ1
∂θj
−1
= Tj−1 [Sj ]e[Sj ]θj · · · e[Si−1 ]θi−1 [Si ]Ti−1
−1
−1
= Tj−1 [Sj ]Tj−1
Ti−1 [Si ]Ti−1
=
[Jj ][Ji ],
and, in a similar way, the second term is
e[S1 ]θ1 · · · e[Si−1 ]θi−1 [Si ]
=
∂ −[Si−1 ]θi−1
(e
· · · e−[S1 ]θ1 )
∂θj
−1
Ti−1 [Si ]e−[Si−1 ]θi−1 · · · e−[Sj ]θj (−[Sj ])Tj−1
=
−1
−1
Ti−1 [Si ]Ti−1
Tj−1 (−[Sj ])Tj−1
=
−[Ji ][Jj ].
i]
Therefore, ∂[J
∂θj for i > j becomes the Lie bracket of Jj and Ji ,
∂[Ji ]
= [Jj ][Ji ] − [Ji ][Jj ] = [Jj , Ji ],
∂θj
which can be written in vector form as
∂Ji
= adJj (Ji ).
∂θj
(b) The ith column of the body Jacobian Jb (θ) is
Ji = Ade−[Bn ]θn ···−[Bi+1 ]θi+1 (Bi ),
for i = n − 1, ..., 1, with the nth column Jn = Bn . Since Ji depends only on θn , · · · , θi+1 , the partial
∂Ji
i
derivative of the body Jacobian ∂J
∂θj becomes zero for i ≥ j. Otherwise, for i < j, ∂θj can be derived
using [·] notation as follows:
∂[Ji ]
∂θj
=
∂ −[Bn ]θn
(e
· · · e−[Bi+1 ]θi+1 )[Bi ]e[Bi+1 ]θi+1 · · · e[Bn ]θn
∂θj
∂ [Bi+1 ]θi+1
(e
· · · e[Bn ]θn ).
+ e−[Bn ]θn · · · e−[Bi+1 ]θi+1 [Bi ]
∂θj
0
0
For the sake of simplicity, let e−[Bn ]θn · · · e−[Bi+1 ]θi+1 = Ti+1
and e−[Bn ]θn · · · e−[Bj+1 ]θj+1 = Tj+1
. Then
the first term is
∂ −[Bn ]θn
(e
· · · e−[Bi+1 ]θi+1 )[Bi ]e[Bi+1 ]θi+1 · · · e[Bn ]θn
∂θj
0
0
= Tj+1
(−[Bj ])e−[Bj ]θj · · · e−[Bi+1 ]θi+1 [Bi ](Ti+1
)−1
=
0
0
0
0
Tj+1
(−[Bj ])(Tj+1
)−1 Ti+1
[Bi ](Ti+1
)−1
= −[Jj ][Ji ],
and, in a similar way, the second term is
e−[Bn ]θn · · · e−[Bi+1 ]θi+1 [Bi ]
∂ [Bi+1 ]θi+1
(e
· · · e[Bn ]θn )
∂θj
0
0
= Ti+1
[Bi ]e[Bi+1 ]θi+1 · · · e[Bj ]θj [Bj ](Tj+1
)−1
0
0
0
0
= Ti+1
[Bi ](Ti+1
)−1 Tj+1
[Bj ](Tj+1
)−1
=
[Ji ][Jj ].
Go to the table of contents.
Chapter 8. Chapter 8 Solutions
127
i]
Therefore, ∂[J
∂θj for i < j becomes the Lie bracket of Ji and Jj ,
∂[Ji ]
= [Ji ][Jj ] − [Jj ][Ji ] = [Ji , Jj ],
∂θj
which can be written in vector form as
∂Ji
= adJi (Jj ).
∂θj
Exercise 8.7.
From the closed form dynamics formulation derived in chapter 8.4, the mass matrix is expressed as follows:
M (θ) = AT LT (θ)GL(θ)A.
Since A and G are time-invariant, taking the derivative of both sides leads to
d T
d
L (θ)GL(θ)A + AT LT (θ)G L(θ)A.
dt
dt
Ṁ (θ) = AT
The time derivative of L(θ) is required:
d
L(θ)
dt
=
=
=
d −1
(I − W(θ))
dt
d
−1
−1
(I − W(θ)) (I − W(θ))
− (I − W(θ))
dt
d
L(θ)
W(θ) L(θ).
dt
The time derivative of the adjoint is derived in Chapter 8.3.1:
d AdTi,i−1
= − adAi θ̇i AdTi,i−1
dt
d
∴ W(θ) = − adAθ̇ W(θ).
dt
Therefore the time derivative of L(θ) is,
d
L(θ) = −L(θ) adAθ̇ W(θ)L(θ).
dt
Similarly, the time derivative of LT (θ) can be derived as follows:
T
−1
−1
= I − W T (θ)
LT (θ) = (I − W(θ))
d T
L (θ)
dt
=
=
=
=
−1
d
I − W T (θ)
dt
−1
−1 d
T
T
I − W (θ)
I − W T (θ)
− I − W (θ)
dt
T
d T
d
LT (θ)
W (θ) LT (θ) = LT (θ)
W(θ)
LT (θ)
dt
dt
T
−LT (θ)W T (θ) adAθ̇ LT (θ)
Therefore,
T
Ṁ (θ) = − AT LT W T adAθ̇ LT GLA + AT LT GL adAθ̇ WLA .
Go to the table of contents.
128
Chapter 8. Chapter 8 Solutions
Exercise 8.8.
First consider the bold ellipse in the top right plot (force ellipse corresponding to an acceleration circle when
the robot is at (0◦ , 90◦ ). Accelerating the end-effector in the x-direction feels only the mass m2 ; the mass
m1 does not accelerate instantaneously. Hence the required force is just m2 a. Accelerating in the y-direction
feels both masses equally, and the required force is (m1 + m2 )a.
For the bold ellipse in the bottom right plot, accelerating the end-effector at a 60◦ angle feels only m2 ,
as m1 does not accelerate instantaneously. Accelerating at 150◦ requires the force m2 a to accelerate m2 plus
an additional force larger than m1 a, because only some of this additional force accelerates m1 while the rest
acts against the constraint of joint 1. This constraint force occurs because link 1 is not orthogonal to the
direction of endpoint acceleration, as it is in the top right plot for an acceleration in the y-direction.
Exercise 8.9.
dθ̈/dG =
G2 I
2G2 τm Irotor
τm
=0
−
2
(G Irotor + Ilink ) 2
rotor + Ilink
τm Ilink − G2 Irotor
=0
(G2 Irotor + Ilink ) 2
p
Ilink /Irotor = G
Exercise 8.10.
P τ = P (M θ̈ + h)
P τ = P M θ̈ + P h
P (τ − h) = P M θ̈
M −1 P (τ − h) = M −1 P M θ̈
Pθ̈ M −1 (τ − h) = Pθ̈ θ̈, where Pθ̈ = M −1 P M .
Exercise 8.11.
Programming assignment.
Exercise 8.12.
Programming assignment.
Exercise 8.13.
Programming assignment.
Exercise 8.14.
Programming assignment.
Exercise 8.15.
Programming assignment.
Go to the table of contents.
Chapter 9 Solutions
Exercise 9.1.
x = 2(1 − cos 2πs)
y = sin 2πs
Exercise 9.2.
Ẋ = dX
ds ṡ
ẋ = − 12 π(t + 1) sin(2πs)
ẏ = 12 π(t + 1) cos(2πs)
ż = t+1
2
d2 X 2
Ẍ = dX
ds s̈ + ds2 ṡ
1
ẍ = − 4 π π(t + 1)2 cos(2πs) + 2 sin(2πs)
ÿ = − 14 π π(t + 1)2 sin(2πs) − 2 cos(2πs)
z̈ = 21
Exercise 9.3.
Since
−1
X(s) = Xstart exp(log(Xstart
Xend )s),
−1
where s = s(t) and log(Xstart
Xend ) = Sθ(t), So
−1
V = S θ̇(t) = log(Xstart
Xend )ṡ
−1
V̇ = S θ̈(t) = log(Xstart
Xend )s̈
Exercise 9.4.
For time scaled joint trajectories, θ̇ = ṡ(θend − θstart ) and θ̈ = s̈(θend − θstart ). Since θ1 moves a distance
of π and θ2 only moves a distance of π/3, the time scaling is going to be limited by θ1 . Therefore we can
substitute (θend − θstart ) = π leading to θ̇ = ṡπ and θ̈ = s̈π. The joint velocity/acceleration limits are
|θ̇1 |, ≤ 2 rad/s and |θ̈1 | ≤ 0.5 rad/s2 . Therefore ṡ ≤ 2/π and s̈ ≤ 0.5/π.
s(t) = a3 t3 + a2 t2 + a1 t + a0
ṡ(t) = 3a3 t2 + 2a2 t + a1
s̈(t) = 6a3 t + 2a2
a0 → 0, a1 → 0, a2 → T3π2 , a3 → − T2π3
For a cubic trajectory, the max velocity occurs at T /2, and the max acceleration occurs at t = 0 and t = T .
3π
and θ̈max = T6π2 Based on the velocity limit, Tmin = 34 π 2 , and based on the acceleration limit
θ̇max = 2T
√
√
Tmin = 2π 3. Therefore the fastest motion time is the larger of the two T = 2π 3
Exercise 9.5.
s(t) = a5 t5 + a4 t4 + a3 t3 + a2 t2 + a1 t + a0
10
15
6
a0 → 0, a1 → 0, a2 → 0, a3 → 3 , a4 → − 4 , a5 → 5
T
T
T
Exercise 9.6.
3
180t2
60t
In terms of the T , the fifth-order time scaling can be represented as s̈ = 120t
T 5 − T 4 + T 3 . Taking the
√
1
derivative and setting it equal to zero yields tθ̈,max = 6 3T − 3T
Exercise 9.7.
129
130
Chapter 9. Chapter 9 Solutions
7th order.
Exercise 9.8.
The area under a curve in the time vs. velocity domain is the total distance traveled. The maximum time
should be spent traveling at the max velocity v to minimize the travel time. Since the trajectory starts and
begins at rest, applying the maximum acceleration/deceleration a/ − a on either side of the max velocity
v satisfies the endpoint constraints and maximizes the area under the curve. Therefore the trapezoidal
trajectory minimizes the trajectory time T .
Exercise 9.9.
s̈
a
0
T t
-a
Figure 9.1
Exercise 9.10.
A trapezoidal velocity profile has the constraint that the area under the curve ṡ is equal to S(T ) = 1. The
limit case where the trapezoidal profile barely reaches the max velocity v occurs for a triangular profile with
an acceleration to velocity v in time ta = v/a followed by an immediate deceleration to rest in time ta . The
total area of this profile is expressed as b ∗ h/2 = 2ta ∗ v/2 = v 2 /a. As long as v 2 /a ≤ 1, the max velocity
is reached before satisfying the constraint S(T ) = 1, and therefore the max velocity v is reached during the
path.
Exercise 9.11.
A trapezoidal velocity profile has the constraint that the area under the curve ṡ is equal to S(T ) = 1. With
a specified time T and max velocity v, if the max acceleration a is infinite then the area under the curve
simplifies to vT . Since the area under the curve must be equal to 1, and infinite accelerations are impossible,
this leads to the constraint vT > 1. For the limit case of a three-stage trapezoidal motion, the velocity profile
is triangular with an area of vT /2. This area must less than or equal to 1 or else the system would have to
decelerate before reaching the max velocity v. This leads to the constraint that vT /2 ≤ 1, or equivalently
vT ≤ 2.
Exercise 9.12.
A trapezoidal velocity profile has the constraint that the area under the curve ṡ is equal to S(T ) = 1. When
given a and T , for the motion to complete in time the area under the curve must reach 1 in time T . The
maximal area in time T assuming no velocity limit v is constrained by the acceleration a. The maximum
area for time T is a bang-bang triangular profile with maximum acceleration/deceleration a and −a. The
height of this triangle is aT /2, and the base is T , so the total area is aT 2 /4. This must be greater than or
equal to 1 or else the area 1 cannot be achieved in time T . Therefore aT 2 /4 ≥ 1, or equivalently aT 2 ≥ 4.
Exercise 9.13.
0 ≤ t ≤ T2 : s̈(t) = a, ṡ(t) = at, s(t) = 21 at2 .
T
2 <t≤T
2
2
−t)
: s̈(t) = −a, ṡ(t) = a(T − t), s(t) = aT −2a(T
.
4
Exercise 9.14.
Go to the table of contents.
Chapter 9. Chapter 9 Solutions
131
s̈
a
0
T t
-a
Figure 9.2
Exercise 9.15.
A seven-stage S-curve is fully specified by seven variables. The curve must also satisfy the following three
constraints: S(T ) = 1, tJ J = a, and 1/2Jt2J +ata = v. Therefore four of the seven quantities can be specified
independently.
Exercise 9.16.
There are three possible S-curves with fewer than 7 stages. The first occurs if the time tv is zero and
deceleration must occur during the third stage before the max velocity v has been achieved. This corresponds
to 6 total stages. The second occurs if tv and ta are zero, resulting in a 4-stage trajectory which never has
a constant acceleration. The third possibility occurs if the system must decelerate before the constant
acceleration a is achieved. Plots of the three cases are shown in Figure 9.3
ṡ
1
v
e
op
=
slo
a
pe
sl
1
2
ṡ
4
3
2
3
4
−
a
5
6
T
ṡ
2
1
=
T t
t
3
1 2
T
t
Figure 9.3
Exercise 9.17.
Since the S-curve uses a jerk J, an acceleration a, the plot of ṡ(t) should be symmetrical and
t1 = t3 = t5 = t7 = a/J,
t2 = t6 =
v − 21 a2 /J × 2
,
a
T =
7
X
ti ,
i=1
Go to the table of contents.
132
Chapter 9. Chapter 9 Solutions
where ti represents the duration of the ith stage.
So
tv = t4 = T − 2a/J − 2v/a.
Exercise 9.18.
Programming assignment.
Exercise 9.19.
B̈
a0 → Bj , a1 → Ḃj , a2 → 2j ,
2
(B̈j+1 −3B̈j )∆Tj −(12Ḃj +8Ḃj+1 )∆Tj +20(Bj+1 −Bj )
a3 →
,
2∆T3
j
(3B̈j −2B̈j+1 )∆T2j +(16Ḃj +14Ḃj+1 )∆Tj +30(Bj −Bj+1 )
a4 →
,
2∆T4j
a5 →
(B̈j+1 −B̈j )∆T2j −6(Ḃj +Ḃj+1 )∆Tj +12(Bj+1 +Bj )
.
2∆T5j
Exercise 9.20.
ṡ
trapezoidal
time scaling ṡ(s)
0
1
s
Figure 9.4
Exercise 9.21.
A is not possible because ṡ is positive the entire time but s decreases in the middle of the path. B is not
possible because when ṡ = 0, the system must start moving before s changes so the path must initially be
normal to ṡ = 0. C is possible because s is always increasing while ṡ is positive and it is normal to the s
axis when ṡ = 0.
When ṡ = 0 the system must accelerate before s can change, therefore a is the only possible motion cone
which represents the possibility of a positive or negative acceleration.
Exercise 9.22.
(a) The curve integrated forward from (slim , ṡtest ) is following the maximum negative acceleration. Therefore it can never intersect F because they are both following maximum deceleration curves. It will
therefore impact the velocity limit curve or ṡ = 0.
(b) The final time scaling can only touch the velocity limit curve tangentially since any penetration of the
limit curve would cause the robot to be uncontrollable.
(c) For a bang-bang trajectory the robot is always following the max or min acceleration. Before the path
intersects F, it is time optimal for it to be accelerating whenever it will not intersect the velocity limit
curve. Therefore when the path runs tangent to the limit curve it switches to the max acceleration.
Exercise 9.23.
If the initial or final velocities are not zero, the algorithm should set (si , ṡi ) to (0, ṡo ) instead of (0, 0), and
generate F by integrating backwards from (1, ṡf ) instead of (1, 0). The algorithm would also have to check
Go to the table of contents.
Chapter 9. Chapter 9 Solutions
133
whether a feasible trajectory exists because initial and final values of ṡ that are too high could mean that
the path will intersect the infeasible region regardless of the chosen controls.
Exercise 9.24.
If the robot can hold itself at equilibrium at every configuration s along the path, then U (s, 0) ≥ 0 and
L(s, 0) ≤ 0 at those configurations. In general, the motion cone will have an upward vertical ray and a
downward vertical ray. If the robot cannot hold itself at static equilibrium at some s, then, at most, at that
point the robot will only be capable of forward or reverse acceleration along the path, not both. Therefore,
slow motion along the path is not a feasible motion, but solutions may still exist if the robot maintains a
sufficiently positive ṡ at configurations where the robot cannot hold itself statically.
The algorithm should terminate if the integral curve in step 2, which integrates L(s, ṡ) backward from
the final state (1, 0), runs into the s-axis instead of hitting the velocity limit curve or reaching s = 0. No
state above the integral curve can reach the final state, and there is no way to get to this integral curve from
the initial state (0, 0).
The algorithm should also terminate if the forward-integrated curve along U (s, ṡ), in step 3, ever intersects
the s-axis.
Exercise 9.25.
Programming assignment.
Exercise 9.26.
ṡ
ṡ
s
0
s
0
(a)
(b)
Figure 9.5
Go to the table of contents.
134
Chapter 9. Chapter 9 Solutions
Go to the table of contents.
Chapter 10 Solutions
Exercise 10.1.
2π
end
A
B
C
θ2
A
A
start
0
0
2π
θ1
Figure 10.1
Exercise 10.2.
2π
end
Y
A
Y
B
X
X
Z
A
start
0
0
Y
θ2
Z
C
θ1
X
A
2π
Figure 10.2
Exercise 10.3.
Exercise 10.4.
Exercise 10.5.
Programming assignment.
Exercise 10.6.
We can compare all squared distances (ri − bj )T (ri − bj ) and (Ri + Bj )2 and take square root of the minimum
squared distance to calculate the distance.
Exercise 10.7.
Exercise 10.8.
If an edge is “colliding” with an obstacle, then the edge is visiting a “concave” vertex. A path through the
135
136
Chapter 10. Chapter 10 Solutions
2π
Y
end
A
Y
X
B
X
θ2
Z1
Z2
Z1
X
Z2
X
C
A
start
0
0
Y
θ1
X A
2π
Figure 10.3
Figure 10.4
edges connected to a concave vertex is always longer than a path skip the concave vertex. Therefore in a
shortest path there is never edge which does not hit the obstacle tangentially.
Exercise 10.9.
Programming assignment.
Exercise 10.10.
Programming assignment.
Exercise 10.11.
Programming assignment.
Exercise 10.12.
Programming assignment.
Exercise 10.13.
Programming assignment.
Exercise 10.14.
Programming assignment.
Exercise 10.15.
Go to the table of contents.
Chapter 10. Chapter 10 Solutions
137
goal
start
Figure 10.5
Programming assignment.
Exercise 10.16.
Programming assignment.
Exercise 10.17.
Programming assignment.
Exercise 10.18.
Programming assignment.
Go to the table of contents.
138
Chapter 10. Chapter 10 Solutions
Go to the table of contents.
Chapter 11 Solutions
Exercise 11.1.
(a) hybrid motion-force control
(b) motion control
(c) motion control
(d) impedance control
(e) motion control
(f) motion control/impedance control
(g) motion control/impedance control
(h) hybrid motion-force control
(i) hybrid motion-force control
(j) motion control/impedance control
Exercise 11.2.
The 5% settling time is approximately t = 3/(ζωn ), since ln(0.05) = −3.
Exercise 11.3.
As shown in Figure 11.1, the damped natural frequency is ωd = 3.92, the overshoot is 52%, and the 2%
settling time is 4.9 s.
1.0
θe
0.5
1
2
3
4
5
6
t
-0.5
Figure 11.1
Exercise 11.4.
As shown in Figure 11.2, the damped natural frequency is ωd = 9.95.
Exercise 11.5.
(a) θ̈ + 10 sin θ = 0.
(b) Linearization: θ̈ + 10θ = 0. m = 1, b = 0 and√ k = 10. The damping ratio is ζ = 0. The system is
underdamped. The natural frequency is ωn = 10. The time constant is ∞.
(c) Linearization: θ̈ − 10θ = 0, and k = −10.
(d) Kp < −10.
Exercise 11.6.
(a) The damping ratio is ζ = 1.58, the system is overdamped. The time constants are τ1 = 16.7 s and
τ2 = 2.27 s.
(b) ζ = √ b
. When ζ = 1, Kp = 0.15.
2
(k+Kp )m
139
140
Chapter 11. Chapter 11 Solutions
0.10
θe
0.05
1
2
3
4
5
t
-0.05
Figure 11.2
√ d . When ζ = 1, Kd = 3.26.
(c) ζ = 2b+K
km
q
k+Kp
5
(d) ζ = √ b+Kd , and ωn =
m . Since ζ = 1 and 4/(ζωn ) = 0.01, we can solve Kp = 6.4 × 10 and
2
(k+Kp )m
Kd = 107 .
k
xd = 0. The steady-state control force is 0.1 N.
(e) Steady-state error ess = k+K
p
(f) The error dynamics:
Z
mẍe + (b + Kd )ẋe + (k + Kp )xe + Ki xe = kxd .
Take time derivative:
mx(3)
e + (b + Kd )ẍe + (k + Kp )ẋe + Ki xe = 0.
Stable conditions: Kp > −k, Kd > −b and 0 < Ki <
(b+Kd )(k+Kp )
.
m
Exercise 11.7.
Programming assignment.
Exercise 11.8.
Programming assignment.
Exercise 11.9.
Programming assignment.
Exercise 11.10.
Programming assignment.
Exercise 11.11.
Programming assignment.
Exercise 11.12.
In this problem, x is (θe , θ̇e ). If Kp = 0, θ̇e = 0, and θe 6= 0, then V > 0 but V̇ = 0 even though the error is
not zero. A trajectory beginning at this state x = (θe , 0) remains at this state for all time (since there is no
spring trying to eliminate the error), so this state is a member of the set S. Since S contains more than the
origin, the origin is not globally asymptotically stable.
If Kd = 0, then there is no damping to bring the robot to rest. Viscous friction plays the same role
as positive-definite Kd for setpoint control, so a robot subject to viscous friction can still be shown to be
Go to the table of contents.
Chapter 11. Chapter 11 Solutions
141
globally asymptotically stable by the Krasovskii-LaSalle invariance principle.
Exercise 11.13.
J=
−L1 sin θ1 − L2 sin(θ1 − θ2 ) L2 sin(θ1 − θ2 )
−L1 cos θ1 − L2 cos(θ1 − θ2 ) L2 cos(θ1 − θ2 )
.
= J −T M (θ)J −1
Λ(θ)
˙ −1 V,
= J −T b(θ, J −1 V) − Λ(θ)JJ
η(θ, V)
where
M
=
"
b(θ, θ̇)
θ̇
=
I1 + I2 + m2 L1 (L1 + 2r2 cos θ2 ) −I2 − m2 r2 L1 cos θ2
−I2 − m2 r2 L1 cos θ2
I2
m2 r2 L1 sin θ2 θ˙2 (θ˙2 − 2θ˙1 ) − g((m2 L1 + m1 r1 ) cos θ1 + m2 r2 cos(θ1 − θ2 ))
2
m2 r2 (g cos(θ1 − θ2 ) + L1 sin θ2 θ˙1 )
#
= J −1 V
Exercise 11.14.
(a) Space frame s could be chosen as a frame fixed to the human body, and b should be chosen at the
hand.
• natural constraints: zeros motions in five directions (3 linear, 2 rotatory) and zero force in one
rotation.
• artificial constraints: forces in five directions (3 linear, 2 rotatory) and motion in one rotation.
(b) Space frame s could be chosen as a frame fixed to the human body or the table, and b should be fixed
to the screw.
• natural constraints: zeros motions in four directions (2 linear, 2 rotatory) and zero force in two
directions.
• artificial constraints: forces in four directions (2 linear, 2 rotatory) and motions in two directions
with the advance speed and rotating speed are related by a constant factor p.
(c) Space frame s could be chosen fixed to the blackboard, and b should be at the chalk contact point.
• natural constraints: zeros motion in one linear direction and zero force in five directions.
• artificial constraints: force in one direction (linearly push into the board) and motions in five
directions.
Exercise 11.15.
A = [1 − 1].
Exercise 11.16.
F
F
F − AT (θ)(AΛ−1 AT )−1 AΛ−1 F
P (θ)F
P (θ)F
=
=
Λ(θ)V̇ + η(θ, V) + AT (θ)λ
Λ(θ)V̇ + η(θ, V) + AT (θ)(AΛ−1 AT )−1 (AΛ−1 (F − η) − AV̇)
= η(θ, V) − AT (θ)(AΛ−1 AT )−1 AΛ−1 η
+Λ(θ)V̇ − AT (θ)(AΛ−1 AT )−1 A(Λ−1 Λ)V̇
= P (θ)η(θ, V) + P (θ)Λ(θ)V̇
= P (θ)(Λ(θ)V̇ + η(θ, V)).
Exercise 11.17.
Go to the table of contents.
142
Chapter 11. Chapter 11 Solutions
(a) 1/kt . Mechanical dissipation like friction, etc.
(b) May introduce some delay in dynamic response due to the control bandwidth of the torque controller.
Also may require more complex mechanical structure that may reduce the reliability.
Exercise 11.18.
Programming assignment.
Go to the table of contents.
Chapter 12 Solutions
Exercise 12.1.
F T VA = ([pA ]n̂)T ωA + n̂T vA
= −n̂T [pA ]ωA + n̂T vA
= n̂T [ωA ]pA + n̂T vA = n̂T ṗA
Exercise 12.2.
ŷ
+
ŷ
+
x̂
+
x̂
(a)
(b)
Figure 12.1
Exercise 12.3.
−3ωx + ωz + vy ≥ 0.
Exercise 12.4.
p = (0, 0, 0) and n̂ = (0, 0, 1).
(a) vz = 0.
(b) v = 0.
(c) V = 0.
Exercise 12.5.
Form closure subsets: {1, 3, 4, 5} and {1, 2, 3, 5}.
1
3
4
5
2
Figure 12.2
Exercise 12.6.
143
−
144
Chapter 12. Chapter 12 Solutions
+,Sl
+,B
+,Sr
-,B
-,Sl
1
-,Sr
+,R
-
Figure 12.3
Exercise 12.7.
+
BB
BSr
+SrSr/-SlSl
1
SlB RB SrB
−
BB
BSl
BR
BSr
2
Figure 12.4
Exercise 12.8.
−BB
BSr
3 BR
BSl
+SrSr/-SlSl
BSr
SrB
BB
+
RB
2
SlB
Figure 12.5
Exercise 12.9.
Go to the table of contents.
SlB
Chapter 12. Chapter 12 Solutions
145
BSl
+SrSr/-SlSl
+
BB
RB
SrB
1
SlB
SlB
BSl
BB
−
BR
BSr
5
Figure 12.6
Exercise 12.10.
SlSlB
1
BSlB
SlBB
−
SlBSr
BBSr
BBB
BBSl
BBR
BSlSl
2
Figure 12.7
Exercise 12.11.
SlSlB
SlBB
1
SlBSr
−
BBB
BBSl
4
BSlB
BBR
BRB
BSrB
2
BBSl
BSrSl
Figure 12.8
Exercise 12.12.
Go to the table of contents.
146
Chapter 12. Chapter 12 Solutions
BSrB
1
3
SlSrB
−
BBB
BSrSl
BBSl
SlBSl
SlBB
5
Figure 12.9
Exercise 12.13.
1
1
3
3
−
−
4
+
4
+
5
2
5
2
(a)
(b)
Figure 12.10
Exercise 12.14.
1
3
4
−
2
5
Figure 12.11
Exercise 12.15.
(a) The conditions for force closure of Figure 12.12(a) can be arranged into a linear equation of the form
Ax = b:
−1
1
1
1
−1
f1 =
c1 , f2 =
c2 , f 3 =
c3 , f 4 =
c4 , f5 =
c5
−1
−1
1
1
1
Go to the table of contents.
Chapter 12. Chapter 12 Solutions
147


−1
 −1
0

c1



1
1 −1 
fx
 c2 
  fy 
1
1 1 
 c3  =
√

c4 
Mz
− 2 0 0
c5

1
−1
0
Matrix A can be reduced to the following row echelon form via Gauss-Jordan elimination:
f2
f1
f3
f5
f4
(a)
(b)
Figure 12.12

[I
1
S] =  0
0
0
1
0
0
0
1
−1
0
0

0
−1 
0
There does not exist any w ≥ 0 satisfying Sw < 0. Thus, this is not force closure.
(b) In order to make force closure by adding one frictionless point contact, new point contact should be
able to exert positive moment to planar square. Figure 12.12(b) represents all possible locations of
this contact as red line.
Exercise 12.16.
We can express the force applied to the center by a vector sum of c1 , c2 as shown in Figure 12.13:
c3
45◦
45◦
c1
c2
c4
Figure 12.13
Go to the table of contents.
148
Chapter 12. Chapter 12 Solutions
If we place the reference frame at the center, the grasp matrix is a 3 × 4 matrix of the form:



 c


1 1 −1 0  1 
fx
 0 −1 −1 1   c2  =  fy  .
 c3 
0 0
0 0
Mz
c4
Since the four vectors have no z-component (moment term), the convex hull lies on the X-Y plane, and the
grasp is not force-closure. For force closure we need 2 more vectors, one with a positive moment component,
another with a negative moment component:




0
0
 0  and  0  ,
L1
−L2
where L1 , L2 > 0. The additional two frictionless point contacts can be placed between the concave edges,
e.g. one at the upper edge, another at the lower.
Exercise 12.17.
(a) From Nguyen’s theorem, this is a force closure grasp.
(b) From Nguyen’s theorem, this is NOT a force closure grasp.
(c) From Nguyen’s theorem, all positions x that ensure that the grasp is force closure are of the form
0 < x < L.
Exercise 12.18.
The conditions for force closure of Figure 12.14 can be arranged into a linear equation of the form Ax = b:
−µ
µ
0
−1
f1a =
c1 , f1b =
c2 , f2 =
c3 , f 3 =
c4
1
1
−1
0

−µ
 1
0


c1
−1 
c2 

0 

c3 
1
2
c4

µ
0
1 −1
0 − 41


f1
=  f2 
f3
Matrix A can be reduced to the following row-echelon form via Gauss-Jordan elimination:
0.5
c
f2
f3
1
h
f1a
ŷ
α f1b
x̂
0.5
f1
Figure 12.14
Go to the table of contents.
Chapter 12. Chapter 12 Solutions
149

−µ
 1
0


1
−1
0  →  0
1
0
2
µ
0
1 −1
0 − 14
1

S] = 0
0

[I
0
1
0
0
0
1
0
1
0
1 
−1 + 2µ
1 
−1 − 2µ
−2
0
0
1
1 
−1 + 2µ
1 
−1 − 2µ
−2
1
In order to be force closure, all elements of S should be negative. In other words, −1 + 2µ
< 0 and
1
−1 − 2µ < 0.
∴µ>
1
2
Exercise 12.19.
(a) The conditions for force closure of Figure 12.15(a) can be arranged into a linear equation of the form
Ax = b:
0
1
0
−1
0
f1 =
c1 , f2 =
c2 , f3 =
c3 , f4 =
c4 , f5 =
c5
−1
0
−1
0
1


0
 −1
1

c1



0 
fx
 c2 
  fy 
1 
 c3  =

0
c4 
Mz
c5

0 −1
−1 0
−1 1
1
0
−1
Matrix A can be reduced to the following row-echelon form via Gauss-Jordan elimination:
A
Y
A
[
B
X
1
1
x
X
\
B
Z
Z
1
Y
β
x
[
y
y
1
C
(a)
C
(b)
Figure 12.15

[I
1
S] =  0
0
0
1
0
0
0
1
0
−2
−1

−1
0 
0
Since there exists w satisfying Sw < 0, this grasp is force closure.
Go to the table of contents.
150
Chapter 12. Chapter 12 Solutions
(b) The conditions for force closure of Figure 12.15(b) can be arranged into a linear equation of the form
Ax = b:
1
−1
−µ
µ
f1 =
c1 , f2 =
c2 , f3 =
c3 , f4 =
c4
−1
−1
1
1

−1 −µ
−1
1
1
0
1
 −1
−1

c1
µ 
c2 

1 
 c3 
0
c4




f1
=  f2 
f3
Matrix A can be reduced to the following row-echelon form via Gauss-Jordan elimination:




1 0
0
−1
1 −1 −µ µ
 −1 −1
1 1  →  0 1 µ2 − 12 − µ2 − 12 
−1
1
0 0
0 0
1
−1


1 0 0 −1
→  0 1 0 −1 
0 0 1 −1

[I
1
S] =  0
0
0
1
0
0
0
1

−1
−1 
−1
Regardless of µ, this grasp is force closure except when µ = 0. When µ = 0, there are only three
columns in the matrix, so it is impossible to construct a fourth column with all elements negative.
∴0<β≤
π
2
Exercise 12.20.
(a) µ > tan−1 (π/4n).
(b) µ > 0.
Exercise 12.21.
(a) 4 − 3 = 1. The three constraints come from force balance in the normal direction and moment balances
about two horizontal directions.
(b) 4 × 3 − 3 − 3 = 6. Each contact force is three dimensional, and there are three constraints in forces
and three in moments.
Exercise 12.22.
As shown in Figure 12.16, when place the finger on the bottom, then the location of the finger x < 0.
ŷ
+
x̂
x
−
ŷ
x̂
−
d
Figure 12.16
And when the finger is placed on the top of the rod, let x > d can balance the gravity force of the
rod. The magnitude of the second contact normal forces when placed on the top are larger than on the
Go to the table of contents.
Chapter 12. Chapter 12 Solutions
151
bottom since when on the top the normal force need to balance the gravity force plus the normal force of
the stationary finger.
Exercise 12.23.
(a) See Figure 12.17(a).
(b) From Figure 12.17(a), the box will tip right.
(c) If we reduce the support friction a little, as in Figure 12.17(c), the box will slide right.
−
−
+
slide right
−
+
tip right
tip and slide
(a)
(c)
Figure 12.17
Exercise 12.24. The contact wrenches are labelled as in Figure 12.18.
F4
g
F1
F3
F5 F6 F7 F8
F2
Figure 12.18
And we have
F1 = (0, −µ, 1)T
F2 = (0, µ, 1)T
F3 = (y − µxL , −1, −µ)T
F4 = (y + µxL , −1, µ)T
F5 = (xL + µ, −µ, 1)T
F6 = (xL − µ, µ, 1)T
F7 = (xR + µ, −µ, 1)T
F8 = (xR − µ, µ, 1)T
Fg1 = (−m1 gx1 , 0, −m1 g)T
Fg2 = (−m2 gx2 , 0, −m2 g)T
The conditions to stay standing are
k1 F1 + k2 F2 + k3 F3 + k4 F4 + k5 F5 + k6 F6 + k7 F7 + k8 F8 + Fg1 + Fg2 = 0
Go to the table of contents.
152
Chapter 12. Chapter 12 Solutions
where ki ≥ 0. There are 3 equations and 8 unknowns.
Exercise 12.25.
Programming assignment.
Exercise 12.26.
Programming assignment.
Exercise 12.27.
Programming assignment.
Exercise 12.28.
Programming assignment.
Exercise 12.29.
Programming assignment.
Exercise 12.30.
Programming assignment.
Exercise 12.31.
Programming assignment.
Go to the table of contents.
Chapter 13 Solutions
Exercise 13.1.
For robots with greater than three wheels, the constraints from the wheels mean that slipping will occur in
general if each wheel velocity is specified arbitrarily, so it makes sense to start with the chassis velocity then
determine the consistent wheel velocities. This modeling approach does not work if the rank of the matrix
H(0) is not full (rank three). The matrix H(0) depends on the wheel locations and driving directions.
Exercise 13.2.
The kinematic modeling may be unintuitive because it does not consider forces or accelerations. Under
the kinematic velocity modeling, each extra wheel only provides motion constraints (in terms of its max
velocity), not extra forces or torques that increase the acceleration of the robot.
Exercise 13.3.
No it is not possible to drive the wheels so that they skid. The matrix H(0) is invertible, and therefore there
is a 1:1 mapping from the wheel velocities to the motion of the robot.
Exercise 13.4.
Yes it is possible for the wheels to slip. Any choice of u that does not have a valid solution Vb means slipping
is occurring. By choosing u1 = u2 = u3 = 1, we can solve for the unique values of ωbz , νbx , and νby which
are {0, r, 0}. Multiplying H(0)Vb gives u4 = 1. Therefore if u4 6= 1, slipping occurs.
Exercise 13.5.
Assuming
 r = l =√w = 1, √ 
−1 −√ 3 1 −√ 3

 1+ 3 1
√
√3 
H(0) = 
 1 + 3 1 − 3 ,
√
√
3
−1 − 3 1
which is rank three.
Exercise 13.6.
If the three-omniwheel
robot has itswheels replaced with 45◦ mecanum wheels,

1
1
− rdi
ri√
ri √

3
3 
− 1+
H(0) =  − rdi −1+

2ri√
2r√i
1+ 3
−1+ 3
d
− ri − 2ri
2ri
which is rank three, so it is still a properly constructed omnidirectional mobile robot.
Exercise 13.7.
If the triangular, three-omniwheel robot in Exercise 6 has wheel one replaced with a −45◦ mecanum wheel
and wheels
three replacedwith 45◦ mecanum wheels aligned with the x̂b axis,
 two and
d
1
−r
− r1i
ri
 (1+√i3)d

1
1

H(0) = 
2ri√
ri
ri


(−1+ 3)d 1
1
−
2ri
ri
ri
which is rank three. Therefore it is a properly constructed omnidirectional mobile robot.
Exercise 13.8.
Programming assignment.
Exercise 13.9.
153
154
Chapter 13. Chapter 13 Solutions
Programming assignment.
Exercise 13.10.
For the described
robot,

 √
√1
2 − √12
2
√

 2
√1
√1

2
2 ,
H(0) =  √

1
1
√
− √2 
 2
2
√
2 − √12 − √12
which is rank three. Therefore it is a properly constructed omnidirectional mobile robot.
Exercise 13.11.
Programming assignment.
Exercise 13.12.
Programming assignment.
Exercise 13.13.
Programming assignment.
Exercise 13.14.
For the unicycle problem the wheel cannot slip perpendicular to the velocity direction which results in the
Pfaffian constraint,


φ̇
 ẋ 

A(q)q̇ = 0 sin(φ) − cos(φ) 0 
 ẏ  = 0
θ̇
Can also assume that there is no slipping in the x or y directions at the contact between the wheel and the
ground leading to the two constraints: ẋ − r cos φθ̇ = 0, andẏ − rsin φθ̇ = 0
φ̇

0 1 0 −r cos φ 
 ẋ  = 0
which can be expressed as, A(q)q̇ =
0 0 1 −r sin φ  ẏ 
θ̇
Exercise 13.15.
For the differential drive robot, the constraints that there is no slipping

in the x or y direction at each wheel


φ̇
−d cos φ 1 0 −r cos φ
0


 −d sin φ 0 1 −r sin φ
  ẋ 
0



=0
ẏ
are given by: A(q)q̇ = 

d cos φ 1 0
0
−r cos φ  
 θ̇L 
d sin φ 0 1
0
−r cos φ
θ̇R
Exercise 13.16.
For a car we have the Pfaffian constraints for no slipping at the back wheel, and no slipping at the virtual
wheel located between the front two wheels,


φ̇

−l cos ψ sin(φ + ψ) − cos(φ + ψ) 0 
 ẋ  = 0
A(q)q̇ =

0
sin(φ)
− cos(φ)
0
ẏ 
ψ̇
Exercise 13.17.
A space shuttle with two rear thrusters is STLA. A robot arm with no joint limits, but with motors that
Go to the table of contents.
Chapter 13. Chapter 13 Solutions
155
can only move in one direction is also STLA.
Exercise 13.18.
1 ∂gi
q(3) = F−gi (q(2)) = q(2) − gi (q(2)) + 2
gi (q(2)) + O(3 )
2 ∂q
1 ∂gi
1 ∂gj
∂gj
gi (q(0)) + 2
gi (q(0)) + 2
gj (q(0))
= q(0) + gi (q(0)) + gj (q(0)) + 2
∂q
2 ∂q
2 ∂q
1 ∂gi
∂gi
∂gi
− gi (q(0)) − 2
gi (q(0)) − 2
gj (q(0)) + 2
gi (q(0)) + O(3 )
∂q
∂q
2 ∂q
1 ∂gj
∂gi
∂gj
gi (q(0)) + 2
gj (q(0)) − 2
gj (q(0)) + O(3 )
= q(0) + gj (q(0)) + 2
∂q
2 ∂q
∂q
1 ∂gj
q(4) = F−gj (q(3)) = q(3) − gj (q(3)) + 2
gj (q(3)) + O(3 )
2 ∂q
∂gj
1 ∂gj
∂gi
gi (q(0)) + 2
gj (q(0)) − 2
gj (q(0))
= q(0) + gj (q(0)) + 2
∂q
2 ∂q
∂q
∂gj
∂gj
∂gj
1 ∂gj
− gj (q(0)) − 2
gi (q(0)) − 2
gj (q(0)) + 2
gi (q(0)) + 2
gj (q(0)) + O(3 )
∂q
∂q
∂q
2 ∂q
∂gj
∂gi
2
= q(0) + gi (q(0)) −
gj (q(0)) + O(3 )
∂q
∂q
Exercise 13.19.

0
If we draw the body frame {b} as in Figure 13.1, then B = 1
0

1
0.
0
ŷb
φ
{b}
x̂b
(x, y)
Figure 13.1
Exercise 13.20.
[ω1 ]
T
T
We have A1 = (0, cos φ sin φ) and A2 = (1, 0, 0) . So A3 = [A1 , A2 ] =
[v1 ]
0
[ω1 ]


0
ω2
=  sin φ .
v2
− cos φ
Exercise 13.21.
Programming assignment.
Exercise 13.22.
r r
r r
, 2 cos φ, 2r sin φ, 1, 0)T and g2 = ( 2d
, 2 cos φ, 2r sin φ, 0, 1)T .
We have g1 = (− 2d
2
2
r
r
r4
r4
T
Then g3 = [g1 , g2 ] = (0, 2d
sin φ, − 2d
cos φ, 0, 0)T , and g4 = [g1 , g3 ] = (0, − 4d
2 cos φ, − 4d2 sin φ, 0, 0) .
Go to the table of contents.
156
Chapter 13. Chapter 13 Solutions
The Pfaffian constraint is A(q)q̇ = (0, sin φ, cos φ, 0, 0)q̇ = 0.
Exercise 13.23.
Programming assignment.
Exercise 13.24.
Programming assignment.
Exercise 13.25.
Programming assignment.
Exercise 13.26.
Programming assignment.
Exercise 13.27.
Programming assignment.
Exercise 13.28.
Programming assignment.
Exercise 13.29.
Programming assignment.
Exercise 13.30.


 
uL
uL
Ve = Je (θ1 ) uR  = [Jbase Jarm ] uR  ,
θ̇1
θ̇1


r
1
0
0
− 2d
r
Jbase =  0 cos(θ1 + φ) sin(θ1 + φ)   2 cos φ
r
h − sin(θ1 + φ) cos(θ1 + φ)
2 sin φ


r
r
− 2d
2d
r
r
,
=  2 cos θ1
2 cos θ1
r(h−d sin θ1 )
r(h+d sin θ1 )
−
2d
2d
r
2d

r

2 cos φ
r
sin
φ
2
Jarm = (1, 0, 0)T ,
p
2
cos θ1
where h = L21 + x2r + 2xr L1 cos(θ1 ). And det(Je ) = hr 2d
. So Je is not full rank when xr = 0, L1 = 0
or θ1 = π/2 + kπ, k ∈ Z.
Exercise 13.31.
Programming assignment.
Exercise 13.32.
Programming assignment.
Exercise 13.33.
Programming assignment.
Exercise 13.34.
Programming assignment.
Go to the table of contents.