FUNDAMENTALS OF
ROBOTIC MECHANICAL SYSTEMS
Theory, Methods, and Algorithms
Fourth Edition
Solutions to Selected Problems
Jorge Angeles
Department of Mechanical Engineering &
Centre for Intelligent Machines (CIM)
McGill University
Montreal, Quebec, Canada
[email protected]
November 2014
Jorge Angeles
c
2
1
An Overview of Robotic Mechanical Systems
1.1 Machine: A historical account
Here is an account of the definitions of machine, taken from (Dudiţă and Diaconescu, 1987):
Different definitions of machine have been given by scholars for more than two millennia, starting with
Vitruvius in 28 B.C., namely,
• A machine is a combination (system, assemblage) of moving material bodies (Vitruvius, 28 B.C.;
Hachette, 1811; Borgnis, 1818; Beck, 1859; Reuleaux, 1875; Koenigs, 1901)
• A machine is generally composed of three parts: a motor part, a transmission part, and an execution part (Euler, 1753; Bogolyubov, 1976)
• A machine produces mechanical work, or performs productive operations, actions, or effects (Vitruvius, 28 B.C.; Poncelet, 1824; Reuleaux, 1900; Koenigs, 1901; Bogolyubov, 1976)
• A machine transforms or transmits forces (Vitruvius, 28 B.C.; Leupold, 1724; Euler, 1753; Bogolyubov, 1976; Reuleaux, 1900; Koenigs, 1901)
• A machine is characterized by deterministic motions (Hachette, 1811; Leupold, 1724; Reuleaux,
1875; Borgnis, 1818; Reuleaux, 1900)
• A machine is an artifact (Leupold, 1724)
Beck, Th., 1875, Beiträge zur Geschichte des Maschinenbaues, J. Springer, Berlin.
Bogolyubov, A. N., 1976, Teoriya mekhanismov v istoricheskom razvitii (Theory of Mechanisms and
its Historical Development), Nauka, Moscow (in Russian).
Borgnis, G. A., 1818, Traité Complet de Mécanique Appliquée aux Arts. Traité des Compositions des
Machines, Paris.
Dudiţă, Fl. and Diaconescu, D., 1987, Optimizarea Structurală a Mecanismelor (Optimization of Mechanisms ), in Romanian, Ed. Tehnicá (Publishers), Bucharest.
Hachette, 1811, Traité Élémentaire des Machines, Paris.
Koenigs, F., 1901, “Etude critique sur la théorie générale des mécanismes,” Comptes Rendus de
l’Académie des Sciences, Vol. 133.
Leupold, J., 1724, Theatrum Machinarium Generale, Leipzig.
Poncelet, J. V., 1824, Traité de Mécanique Appliquée aux Machines, Liège.
Reuleaux, F., 1875, Theoretische Kinematik, Braunschweig.
Reuleaux, F., 1900, Lehrbuch der Kinematik, Braunschweig.
Vitruvius, P. M., 28 B.C., De Architectura, Libri X.
1.3 Definitions for “machine,” “mechanism,” and “linkage”:
Machine
• Definitions in Merriam Webster’s Collegiate Dictionary (on-line, 2002):
– (archaic): a constructed thing whether material or immaterial.
– an assemblage of parts that transmit forces, motion, and energy one to another in a predetermined manner
– an instrument (as a lever) designed to transmit or modify the application of power, force, or
motion
1
– a mechanically, electrically, or electronically operated device for performing a task (a calculating machine, a card-sorting machine)
Comment: comprehensive definitions when considered as a whole
• An apparatus for transformation of power, materials, and information to substitute or simplify
physical or intellectual work (Frolov, 1987). Comment: a comprehensive definition, that includes
computers
• Mechanical system that performs a specific task, such as the forming of material, and the transference and transformation of motion and force, Vol. 38, Nos. 7–10 (2003) of Mechanism and
Machine Theory on Standardization of Terminology. Comment: leaves computers out
• An apparatus for applying mechanical power, having several parts, each with definite function
(The Concise Oxford Dictionary). Comment: same as above
• An apparatus consisting of interrelated parts with separate functions, used in the performance of
some kind of work (The Random House College Dictionary). Comment: ditto
• Any system in which a specific correspondence exists between an input form of energy or information and the corresponding ones at the output (Loosely translated from Le Petit Robert).
Comment: as comprehensive as Frolov’s
Mechanism
• A piece of machinery (Merriam Webster’s Collegiate Dictionary (on-line, 2002)). Comment: too
vague
• Definitions in Vol. 38, Nos. 7–10 (2003) of Mechanism and Machine Theory on Standardization
of Terminology.
– System of bodies designed to convert motions of, and forces on, one or several bodies into
constrained motions of, and forces on, other bodies. Comment: English could be terser, but
idea is fine.
– Kinematic chain with one of its components (link or joint) connected to the frame. Comment:
confuses mechanism with its representation as a kinematic chain
• Structure, adaptation of parts of machine; system of mutually adapted parts working together
(as) in machine (The Concise Oxford Dictionary)
• An assembly of moving parts performing a complete functional motion (The Random House
College Dictionary)
• A combination layout of pieces or elements, assembled with the goal of (producing) an operation
as a unit (Loosely translated from Le Petit Robert)
Comment: In all above definitions, the concept of goal or task is present
Linkage
• Definitions in Merriam Webster’s Collegiate Dictionary (on-line, 2002):
– a system of links. Comment: concise and comprehensive
– a system of links or bars which are jointed together and more or less constrained by having a
link or links fixed and by means of which straight or nearly straight lines or other point paths
may be traced. Comment: unnecessarily cumbersome and limited to path-generating linkages
• Kinematic chain whose joints are equivalent to lower pairs only (Vol. 38, Nos. 7–10 (2003) of
Mechanism and Machine Theory on Standardization of Terminology). Comment: confuses linkage
with its representation
The Concise Oxford Dictionary of Current English, 1995, Clarendon Press, Oxford
2
Frolov, K. V., (editor), 1987, Teoriya Mechanismov i Mashin (Theory of Mechanisms and Machines),
Vyschaya Shkola, Moscow (in Russian)
Mechanism and Machine Theory on Standardization of Terminology, 2003, Vol. 38, Nos. 7–10
Le Petit Robert, 1994, Dictionnaires Le Robert, Paris
Random House Webster’s College Dictionary, 1997, Random House, New York
Merriam Webster’s Collegiate Dictionary, on-line 2002
1.9 Here we want to estimate the time required to multiply two floating-point numbers. Since this time is
very small, it is more suitable to perform a large number of multiplications, say 107 , which will require
a total time of the order of seconds.
• The Microsoft Visual C++6.0 program below was run on a Pentium IV 2.0:
#include <iostream>
#include <time.h>
int main()
{
clock_t start, end;
start =clock();
float a, i, CLOCKS_PER_mSEC=CLOCKS_PER_SEC/1000;
for(i = 1; i <= 10000000; i++)
a = 5 * 5;
std::cout << ‘‘a= ‘‘ << a << std::endl;
end = clock();
long duration=(long)(end-start)/CLOCKS_PER_mSEC;
std::cout << ‘‘Time: ‘‘ << duration<<’’ms’’<<std::endl;
return 0;
}
A Pentium IV 2.0 processor required 125 ms to perform the 107 multiplications, which amounts
to 1.25 × 10−8 s/mult, or 8 × 107 mult/s.
• The C program below was run for the same purpose on the CLUMEQ (Consortium Laval UQAM
McGill and Eastern Quebec for High Performance Computing) supercomputer, AMD Athlon
1900+ cluster:
# include "stdio.h"
# include "math.h"
# include <sys/time.h>
# include <sys/resource.h>
# define RUSAGE_SELF 0
\* calling process *\
main()
{
int getrusage(int who, struct rusage *rusage);
long diffsec, diffmsec;
float a;
int i;
struct rusage time_begin, time_end;
struct rusage *pb, *pe;
pb=&time_begin;
3
getrusage(RUSAGE_SELF, pb);
for(i=1; i<=100000000; i++)
{
a = 5*5;
}
printf("a=%f \
t\n",a);
pe=&time_end;
getrusage(RUSAGE_SELF,pe);
diffsec=time_end.ru_utime.tv_sec-time_begin.ru_utime.tv_sec;
diffmsec=(time_end.ru_utime.tv_usec-time_begin.ru_utime.tv_usec);
printf("User time used:%d microsec\n",diffmsec);
}
The CLUMEQ supercomputer required 40 ms to perform the 107 multiplications, which amounts
to 4 × 10−9 s/mult, or 2.5 × 108 mult/s.
• On June 20, 2011, Fox News reported that Kei—Japanese for 1016 —was capable of 8 × 1015
flops1 —or eight petaflops—per second.
1.10 Here we want to estimate the time required to add two floating-point numbers. Since this time is very
small, it is more suitable to perform a bunch of additions, say 107 , which should amount to a total
time of the order of seconds.
• The Microsoft Visual C++6.0 program below was run on a Pentium IV 2.0:
#include <iostream>
#include <time.h>
int main()
{
clock_t start, end;
start = clock();
float a, i, CLOCKS_PER_mSEC=CLOCKS_PER_SEC/1000;
for(i=1; i<=10000000; i++)
a = 5+5;
std::cout << "a= "<< a << std::endl;
end = clock();
long duration = (long)(end-start)/CLOCKS_PER_mSEC;
std::cout << "Time: "<< duration<<"ms"<<std::endl;
return 0;
}
A Pentium IV 2.0 processor required 125 ms to perform the 107 additions, which amounts to
1.25 × 10− 8 s/add, or 8 × 107 add/s.
• The C program below was run for the same purpose on the CLUMEQ supercomputer, AMD
Athlon 1900+ cluster:
#include "stdio.h"
#include "math.h"
1 See
the Index for a definition of flop.
4
#include <sys/time.h>
#include <sys/resource.h>
#define RUSAGE_SELF 0
/* calling process */
main()
{
int getrusage(int who, struct rusage *rusage);
long diffsec, diffmsec;
float a;
int i;
struct rusage time_begin, time_end;
struct rusage *pb, *pe;
pb=&time_begin;
getrusage(RUSAGE_SELF, pb);
for(i=1; i<=10000000; i++)
{
a = 5+5;
}
printf("a=%f \t\n",a);
pe=&time_end;
getrusage(RUSAGE_SELF,pe);
diffsec=time_end.ru_utime.tv_sec-time_begin.ru_utime.tv_sec;
diffmsec=time_end.ru_utime.tv_usec-time_begin.ru_utime.tv_usec;
printf("User time used:%d microsec\n",diffmsec);
}
The CLUMEQ supercomputer required 40 ms to perform the 107 additions, which amounts to
4 × 10−9 add/mult, or 2.5 × 108 add/s.
1.11 What we must find here is the largest floating-point number ǫ that, when added to a given number a,
leaves this number unchanged, i.e.,
a+ǫ=a
• The Microsoft Visual C++6.0 program below was run on a Pentium IV 2.0 processor at 550 MHz:
#include <iostream>
//#include <time.h>
int main()
{
long i;
//double a=1;
float a=1;
for(i=1; i<=10000000; i++)
{
a = a/10;
if (a==0)
break;
}
std::cout << "smallest floating-point"<< i-1 << std::endl;
return 0;
}
5
The value of ǫ reported in single-precision arithmetic was 1.0e − 045; in double-precision......
1.0e − 323.
• The program below was run on the CLUMEQ supercomputer, AMD Athlon 1900+ cluster:
#include "stdio.h"
#include "math.h"
#include <sys/resource.h>
main()
{
int i;
float a=1;
for(i=1; i<=10000000; i++)
{
a=a/10;
if (a==0)
break;
}
printf("smallest floating-point=%d \n",i-1);
}
This machine reported exactly the same values as its Pentium counterpart.
2
Mathematical Background
2.6 Let w be perpendicular to v (wT v = vT w = 0). Since we can find two such vectors, let us call them
w1 & w2 and, moreover, let us define w1 ⊥ w2 , i.e., w1T w2 = 0.
We have
T = 1 + uvT
Multiplying both sides by wi , we obtain
Twi = wi + u vT wi = wi ,
| {z }
i = 1, 2
0
Therefore, w1 & w2 are eigenvectors of T, of eigenvalues λ1 = λ2 = 1.
Moreover, let
w3 ≡
Then,
Tw3 = w3 + uvT w3 =
u
kuk
u
u
u
+ uvT
=
(1 + vT u)
kuk
kuk
kuk
and w3 is the third eigenvector of T, of eigenvalue λ3 = 1 + vT u.
6
⇒
Tw3 = (1 + vT u)w3
2.7 The determinant of a matrix is equal to the product of its eigenvalues. Thus
det(1 + uvT ) = det(T) = λ1 λ2 λ3 = 1 + vT u = 1 + u · v
2.8 From Q = R1 R2 , we have
QQT = R1 R2 (R1 R2 )T = R1 R2 RT2 RT1 = R1 RT1 = 1
| {z }
1
and thus Q is orthogonal. Moreover, we have
det(Q) = det(R1 )det(R2 ) = (−1)(−1) = 1
Thus, Q is a rotation.
Let u and φ be the unit vector parallel to the axis of rotation and the angle of rotation, respectively;
then,
Q = (1 − 2eeT )(1 − 2ff T ) = 1 − 2eeT − 2ff T + 4(eT f )ef T
Hence,
1
vect(Q) = − 4(eT f )e × f = u sin φ
2
and
Therefore,
whence
tr(Q) = 3 − 2 − 2 + 4(eT f )2 = 4(eT f )2 − 1 = 1 + 2 cos φ
sin φ = kvect(Q)k = 2|eT f | ke × f k,
u=−
0≤φ<π
e×f
sign(eT f )
ke × f k
where sign(eT f ) is the signum function, which is +1 if eT f > 0, −1 if eT f < 0, and undefined if
eT f = 0. Hence, u is undefined if eT f = 0, which indicates that Q = 1 − 2eeT − 2ff T , in which case
Q is symmetric, as in Exercise 2.13, and
u = e × f,
Hence, if eT f 6= 0,
cos φ = 2(eT f )2 − 1,
⇒
φ=π
φ = tan−1
2|eT f | ke × f k
2(eT f )2 − 1
2.9 The product Q = R1 R2 is readily computed as
Q = (1 − 2eeT )(1 − 2ff T ) = 1 − 2eeT − 2ff T + 4(eT f )ef T
Let φ be the angle of rotation of Q, and hence,
tr(Q) = 3 − 2 − 2 + 4(eT f )2 = 4(eT f )2 − 1 = 1 + 2 cos φ
If eT f 6= 0, then
cos φ = 2(eT f )2 − 1,
Because | cos φ| ≤ 1, we can write the above expression in the form
−1 ≤ 2(eT f )2 − 1 ≤ 1
or 0 ≤ |eT f | ≤ 1.
Since e and f are unit vectors, this condition is always respected for any e or f. However,
vect(Q) = 4(eT f ) vect(ef T ) = −2(eT f ) e × f
where we have recalled relation (2.60). The axis of rotation of Q should thus be parallel to e × f , which
is the sole condition for Q to be factorable as R1 R2 .
7
2.10 (a) With A = CPM(a), the equation can be rewritten as
(1 + A)v = b
which was defined as B in Example 2.3.2. In that example, it was shown that 1+A is always invertible,
and hence, v can be expressed as
v = (1 + A)−1 b ≡
which, upon expansion, yields
v = b+
and, finally,
v=
1
[(1 + kak2 )1 − A + A2 ]b
1 + kak2
1
(−Ab + A2 b)
1 + kak2
−a × b + b + (aT b)a
1 + kak2
(b) For v to be orthogonal to a, one must have vT a = 0, which means that, when “dotted” with a,
the numerator of the above expression must vanish, i.e.,
T
− (a × b) a +bT a + aT bkak2 = 0
| {z }
0
which thus reduces to
(aT b)(1 + kak2 ) = 0
As the second factor above cannot vanish, the condition sought is that a and b be mutually orthogonal.
(c) For v to be orthogonal to b, a similar relation follows upon “dotting” the numerator of v with b,
which leads to
2
T
− (a × b) b +kbk2 + (aT b) = 0
| {z }
0
which holds if and only if the sum of two positive quantities vanishes. Obviously, this sum cannot
vanish, and hence, v cannot be orthogonal to b.
2.11 Let λ be an eigenvalue of E, and u the corresponding unit eigenvector, i.e.,
Eu = λu
(1)
E2 u = λ2 u
(2)
E3 u = λ3 u
(3)
E3 = −E
(4)
−Eu = λ3 u
(5)
Multiplying eq.(1) by E, we obtain
and multiplying a second time by E,
However, from eq.(2.51)
Substituting eq.(4) into eq.(3), we obtain
Moreover, from eq.(1) we have Eu = λu, and thus eq.(5) reduces to
λ(1 + λ2 )u = 0
8
(6)
However, kuk = 1 and hence, u 6= 0. Therefore, we have from eq.(6)
P (λ) ≡ λ(1 + λ2 ) = 0
(7)
Equation (7) is the characteristic equation of E, and P (λ) its characteristic polynomial. The three
roots of eq.(7) are readily obtained as
λ1 = 0,
λ2 = j,
λ3 = −j
The eigenvector u1 corresponding to λ1 is, obviously, e, for Ee = 0.
Now, to prove that the eigenvectors associated with the two complex eigenvalues are complex and
mutually orthogonal, assume that v and w are two 3-dimensional complex vectors. Moreover, the
former is assumed to be the eigenvector of E associated with λ2 = j, the latter with λ3 = −j, i.e.,
Ev = jv,
Ew = −jw
(8)
The scalar product p of v times w is a complex number, given by
p = v∗ w
(9)
where, if, for example, v = vr + jvc , with vr and vc denoting two 3-dimensional real vectors, then,
the transpose conjugate of v is
v∗ = vrT − jvcT
(10)
Moreover, the transpose conjugate p of p, being a scalar, is simply its complex conjugate, which equals
the transpose-conjugate of the product on the right-hand side of eq.(10), namely,
p ≡ (v∗ w)∗ = w∗ v
(11)
Now, multiplying both sides of the second of eqs.(8) by j, we obtain
w = jEw
(12)
Substituting w by the above expression in eq.(10), an alternative expression for p is found:
p = v∗ (jEw) ≡ jv∗ Ew
(13)
Likewise, multiplying both sides of the first of eqs.(8) by j, we obtain
v = −jEv
(14)
which, when substituted into eq.(11), yields an alternative expression for p:
p = w∗ (−jEv) ≡ −jw∗ Ev
(15)
Now, upon taking the complex-conjugate of both sides of eq.(13), one more expression for p is obtained,
namely,
p = −jw∗ (ET v) ≡ jw∗ Ev
(16)
Notice that the two expressions for p, eqs.(15) and (16), differ only in the sign, i.e.,
p = −p
⇒
p = −p
which holds if and only if p = 0, thereby showing that, in fact, the two eigenvectors of E in question,
v and w, are mutually orthogonal. In the next step, these two eigenvectors are found. The real and
9
imaginary parts of v were defined above as vr and vc , respectively. A similar definition is applied now
to the real and imaginary parts of w, namely, wr and wc , respectively. Substitution of v in terms of
its two components, real and imaginary, into the first of eqs.(8) yields
E(vr + jvc ) = jvr − vc
(17)
whence two real equations follow, one for the real, one for the complex part:
Evr = −vc ,
Evc = vr
(18)
Next, upon multiplying both sides of the first of the above equations by E, one obtains
E2 vr = −Evc ≡ −vr
where the second of eqs.(18) has been recalled, the foregoing equation thus becoming
(E2 + 1)vr = 0
and, in light of the expression for the square of a 3 × 3 skew-symmetric matrix, eq.(2.39), the above
equations leads to
e (eT vr ) = 0
| {z }
0
which implies that vr is normal to e. A similar reasoning leads to the same conclusion about vc ,
namely, that vc is also normal to e. Moreover, the first of eqs.(18) leads to
vc = −e × vr
whence vc is also normal to vr . Therefore, any two orthonormal vectors lying in the plane normal to e
are candidates for the real and complex components of v. A similar reasoning leads to the conclusion
that any two orthonormal vectors lying in the plane normal to e are candidates for the two components
of w. However, v and w are known to be mutually orthogonal. Upon imposing the condition v∗ w = 0,
the relation below follows:
vrT wr + vcT wc + j(vrT wc − vcT wr ) = 0
which readily leads to two real equations, one for the real part, one for its complex counterpart:
vrT wr + vcT wc = 0,
vrT wc − vcT wr = 0
The two above equations are verified under the conditions wr = vc and wc = vr , i.e.,
w = vc + jvr
In conclusion: any pair of orthonormal unit vectors normal to e constitute the real and imaginary
parts of the eigenvector associated with λ2 = j. The same vectors, but with their real and imaginary
roles exchanged, constitute the real and imaginary parts of the eigenvector associated with λ3 = −j.
2.12 From eq.(2.53), we have
Q = eEφ = f (E)
where E is the cross-product matrix of the unit vector e with eigenvalues { 0, j, −j }—see Exercise 2.10.
Let λk be an eigenvalue of E and µk be an eigenvalue of Q. Then, by the Cayley-Hamilton Theorem,
we have
µk = eλk φ = f (λk )
Therefore,
µ1 = e0 = 1,
µ2 = ejφ ,
10
µ3 = e−jφ
2.13 Here we have
tr(Q) = −1 = 1 + 2 cos φ
from which,
cos φ = −1,
i.e., φ = π
Using eq.(2.49), we have
Q + 1 = 2eeT
which gives
Thus,

2 −2
1
−2 2
3
2 −2

 2
2
e1
−2  = 2  e1 e2
2
e1 e3
√
3
e1 = ±
,
3
√
3
e2 = ∓
,
3
√
Choosing e1 = + 3/3, we finally obtain


1
3
e=
−1  ,
3
1
√
e1 e2
e22
e2 e3

e1 e3
e2 e3 
e23
√
3
e3 = ±
3
φ=π
2.15 We have, from Cayley’s Theorem,
Q = (1 − S)(1 + S)−1
where 1 + S is invertible. Indeed, if we write vect(S) = s ≡ σe, with σ > 0 denoting the Euclidean
norm of s, then S = σE, in which E is the cross-product matrix of the unit vector e. Hence, the
eigenvalues of S are, from Exercise 2.10, 0, σj, and −σj. Consequently, the eigenvalues of 1 + S are
1, 1 + σj, and 1 − σj, none of which is zero, thereby showing that 1+ S is indeed invertible. Multiplying
both sides of this equation by (1 + S), from the right, we obtain
Q(1 + S) = (1 − S)
i.e.,
(1 + Q)S = 1 − Q
and hence,
S = (1 + Q)−1 (1 − Q) .
For this factoring to be possible, then, matrix 1 + Q should be invertible, i.e., nonsingular. Therefore,
the above-mentioned factoring is not possible if one of the eigenvalues of Q is −1, which happens iff
the angle of rotation is π.
2.16 The given matrix Q is proper orthogonal and symmetric; therefore, Q represents a rotation with φ = π
(see also the solution of Exercise 2.13). As stated in the solution for Exercise 2.15, the Cayley factoring
is impossible in this case.
2.17 From Exercise 2.15, we have
S = (1 + Q)−1 (1 − Q)
where Q can be expressed from eq.(2.54) in the form
⇒
Q = 1 + sin φE + (1 − cos φ)E2
(19)
1 + Q = 21 + sin φE + (1 − cos φ)E2
(20)
11
On the other hand, from the Cayley-Hamilton Theorem we can write
(1 + Q)−1 = α1 + βE + γE2
(21)
where α, β and γ are constants to be determined by imposing
(1 + Q)(1 + Q)−1 = 1
(22)
We can compute coefficients α, β and γ using one of two approaches.
First approach: using eigenvalues
Eigenvalues of Q:
⇒
⇒
{1, ejφ , e−jφ } (see Exercise 2.11)
{2, 1 + ejφ , 1 + e−jφ }
1
1
1
Eigenvalues of (1 + Q)−1 :
,
,
2 1 + ejφ 1 + e−jφ
Eigenvalues of 1 + Q:
Eigenvalues of E:
2
{0, j, −j} (see Exercise 2.10)
Eigenvalues of E : {0, −1, −1} (squares of eigenvalues of E)
Let Σ ≡ α1 + βE + γE2
⇒
Eigenvalues of Σ: {α, α + jβ − γ, α − jβ − γ} (respecting foregoing order)
Equate eigenvalues of Σ with those of (1 + Q)−1 :
α=
1
2
(23a)
1
1 + ejφ
1
α − jβ − γ =
1 + e−jφ
α + jβ − γ =
(23b)
(23c)
Substituting α = 1/2 from eq.(23a) into eqs.(23b &c), we obtain, after some simplifications:
β=
e−jφ − ejφ
−2j sin φ
sin φ
=
=−
,
jφ
−jφ
2j(2 + e + e
)
2j(2 + 2 cos φ)
2(1 + cos φ)
γ=0
(24)
Second approach: using relation (22)
Using eqs.(20) and (21), we derive
(1 + Q)−1 (1 + Q) = α1 + βE + γE2
21 + sin φE + (1 − cos φ)E2
= 2α1 + α sin φE + α(1 − cos φ)E2
+2βE + β sin φE2 + β(1 − cos φ)E3
+2γE2 + γ sin φE3 + γ(1 − cos φ)E4
From eq.(2.51), we have
E3 = −E
E4 = (1 − eeT ) = −E2
12
(25a)
(25b)
Therefore,
(1 + Q)−1 (1 + Q) = 2α1 + [α sin φ + 2β − β(1 − cos φ) − γ sin φ] E
+ [α(1 − cos φ) + β sin φ + 2γ − γ(1 − cos φ)] E2
Since (1 + Q)−1 (1 + Q) = 1, we have
2α = 1
(α − γ) sin φ + β(1 + cos φ) = 0
(α − γ)(1 − cos φ) + β sin φ + 2γ = 0
Solving these three equations for α, β and γ, we obtain
α=
1
,
2
− sin φ
,
2(1 + cos φ)
β=
γ=0
Computation of S
Substitution of foregoing values of α, β and γ into eq.(21) leads to
(1 + Q)−1 =
1
sin φ
1−
E
2
2(1 + cos φ)
and
1
sin φ
S = (1 + Q) (1 − Q) =
1−
E (1 − Q)
2
2(1 + cos φ)
1
sin φ
sin φ
=
1−Q−
E+
EQ
2
1 + cos φ
1 + cos φ
−1
Using the representation of Q given in eq.(19), along with relations (25a), we obtain, after some
simplifications,
− sin φ
S=
E
1 + cos φ
But, if we recall the tan-half identities:
cos φ =
1 − τ2
,
1 + τ2
sin φ =
2τ
,
1 + τ2
S = − tan
φ
E
2
τ ≡ tan
φ
2
then, the above expression is equivalent to
Finally, let us take the vector of the two sides of the above equation
φ
vect(S) = − tan
e
2
which is, using the definition for the Rodriges vector ρ in this Exercise, ρ = tan(φ/2)e, the negative
of this vector, i.e.,
vect(S) = −ρ
13
Figure 1:
2.18 Attaching unit vectors i, j, k to axes X, Y , Z, and i′ , j′ , k′ to axes X ′ , Y ′ , Z ′ , where the axes are
defined in Fig. 1, one has
i′ = −k, j′ = j, k′ = i
From Definition 2.2.1, then,


0 1
1 0
0 0
0
Q= 0
−1
Hence,
tr(Q) = 1 = 1 + 2 cos φ
and thus,
cos φ = 0,
φ=±
On the other hand,
π
2
0 ]T = sin φe
vect(Q) = [ 0 1
If sin φ > 0, then
π
2
The angle between the axis of rotation and AB is thus 0◦ , whereas, the angle between the axis of
rotation and AD is 90◦ . Moreover, the angle between the axis of rotation and AE is 90◦ .
e = [0
1
T
0] ,
φ=
2.19 Using Fig. 2, the rotation matrix Q is given by
Q = QZ0 (φ)QY1 (θ)QZ2 (ψ)
All three rotations involved are about one coordinate axis, and hence, observe a canonical form; in
particular, the first and the third observe form (2.55c), while the second (2.55b), i.e.,
QZ0

cos φ
=  sin φ
0


− sin φ 0
cos θ
cos φ 0  , QY1 =  0
0
1
− sin θ
Doing the required matrix products, we obtain

cθcφcψ − sφsψ
Q =  cθsφcψ + cφsψ
−sθcψ
14
0
1
0


sin θ
cos ψ
0  , QZ2 =  sin ψ
cos θ
0
−cθcφsψ − sφcψ
−cθsφsψ + cφcψ
sθsψ

sθcφ
sθsφ 
cθ
− sin ψ
cos ψ
0

0
0
1
X0 Y0 Z0
QZ0 (φ) → X1 Y1 Z1
QY1 (θ) → X2 Y2 Z2
QZ2 (ψ) → X3 Y3 Z3
Figure 2:
Moreover, from eq.(2.67), we have
tr(Q) = 1 + 2 cos α
Therefore,
1 + 2 cos α = cθcφcψ − sφsψ − cθsφsψ + cφcψ + cθ
= (cφcψ − sφsψ)cθ + (cφcψ − sφsψ) + cθ
= (1 + cos θ) cos(φ + ψ) + cos θ
Adding 1 to each side of the above equation, we obtain
2(1 + cos α) = (1 + cos θ) [1 + cos(φ + ψ)]
Moreover, we have the identity
1 + cos x = 2 cos2
Therefore,
2
4 cos
Thus,
α
cos
2.22 We have
2
α
2
x
2
θ
φ+ψ
2
= 4 cos
cos
2
2
2
= cos

q11
Q =  q21
q31
φ+ψ
2
θ
cos
2

1/2 −2/3
q22
3/4 
q32
q33
If Q represents a rotation matrix, we should have QT Q = 1, and det(Q) should be +1. The foregoing
product is computed first:



q11
q21 q31
q11 1/2 −2/3
QT Q =  1/2 q22 q32   q21 q22
3/4 
−2/3 3/4 q33
q31 q32
q33


1
2
2
2
q11
+ q21
+ q31
− 32 q11 + 34 q21 + q31 q33
2 q11 + q21 q22 + q31 q32
1
2
2
QT Q =  12 q11 + q21 q22 + q31 q32
− 31 + 43 q22 + q32 q33 
4 + q22 + q32
2
3
1
3
4
9
2
− 3 q11 + 4 q21 + q31 q33
− 3 + 4 q22 + q32 q33
9 + 16 + q33
15
For the (3, 3) entry of QT Q, we must have
4
9
2
+
+ q33
=1
9 16
2
or q33
=−
1
144
which is impossible, since all components of Q must be real. So, the given entries are unacceptable.
2.23 Vector pi , i = 1, 2, 3, under the coordinate transformation Q changes as
[pi ]A = [Q]A [pi ]B
where Q denotes the transformation carrying A to B. Hence,
[P]A = [p1 ]A [p2 ]A [p3 ]A = [Q]A [p1 ]B [Q]A [p2 ]B
[Q]A [p3 ]B
= [Q]A [P]B
which is obviously not a similarity transformation. An alternative way of proving that P does not
obey a similarity transformation under a change of coordinate frame consists of showing that its trace
is not invariant. Indeed, define the coordinate frame A with the origin at point P1 , the x-axis directed
from P1 to P2 , the z-axis being normal to the plane of the three points. The vectors {pi }31 have the
A-coordinates displayed below:






x3
0
x2
[p1 ]A =  0  , [p2 ]A =  0  , [p3 ]A =  y3  ,
0
0
0
matrix P thus taking the form

0 x2
[P]A =  0 0
0 0

x3
y3  .
0
Hence, tr[P]A = 0. Now, we transform frame A into frame B by rotating it around the z-axis counterclockwise through an angle θ. The transformation matrix for the said rotation has the form


cos θ − sin θ 0
[Q] =  sin θ cos θ 0 
0
0
1
where θ is the angle of rotation. We thus have
[p1 ]B = [p1 ]A = 0 ,
and

cos θ
T
[p2 ]B = [Q] [p2 ]A =  − sin θ
0

cos θ
[p3 ]B = [Q]T [p3 ]A =  − sin θ
0
sin θ
cos θ
0
sin θ
cos θ
0

0 x2 cos θ
P =  0 −x2 sin θ
0
0

0
0 
1

0
0 
1
 
x2
0 =
0
 
x3
y3  = 
0

x2 cos θ
−x2 sin θ  ,
0

x3 cos θ + y3 sin θ
−x3 sin θ + y3 cos θ 
0

x3 cos θ + y3 sin θ
−x3 sin θ + y3 cos θ  .
0
Therefore tr [P]B = −x2 sin θ which obviously depends on θ, and vanishes only if θ = 0 or π.
16
2.24 If we place the origin on the line defined by the three points, their position vectors are all linearly
dependent, and we can write:
p2 = α2 p1 ,
p3 = α3 p1 .
Therefore, matrix P is of rank one, i.e., it contains only one linearly independent row (or column, for
that matter), and
rank(P) = 1.
Therefore, matrix P has necessarily two zero eigenvalues, the third one being different from zero, i.e.,
λ1 6= 0,
λ2 = λ3 = 0,
λ21 > 0,
λ22 = λ23 = 0.
the eigenvalues of P2 being
Then, we have
q = tr(P2 ) − tr2 (P) = λ21 − (λ1 )2 = 0,
q.e.d.
Note that we did not introduce coordinates in the above proof, which means that the same result holds
for any coordinate frame, as long as its origin is located on the line of the three points.
2.25 From the definition of matrix P we have
PPT = p1 pT1 + p2 pT2 + p3 pT3
Under a rotation Q of the coordinate frame the matrices pi pTi for i = 1, 2, 3 change as
[pi pTi ]A = [Q]A [pi ]B [pi ]TB [Q]TA = [Q]A [pi pTi ]B [Q]TA
which is a similarity transformation. Therefore, matrix PPT also obeys a similarity transformation:
[PPT ]A = [Q]A [PPT ]B [Q]TA = Q[PPT ]B QT ,
PPT thus being frame-invariant.
The singularity of this matrix appears if the origin of the coordinate frame lies in the plane of the three
points. In this case, if we denote by n the unit normal of this plane, we have
PPT n = p1 pT1 n + p2 pT2 n + p3 pT3 n = 0
Since n is obviously not zero, matrix PPT is singular, even if the three points are not collinear, and
of rank 2.
Let us now assume that three points P1 , P2 , P3 are collinear and the origin of the frame lies in the
line of these points. Then, the position vectors of these points are all linearly dependent, and we can
write:
p2 = α2 p1 ,
p3 = α3 p1 .
Now, if we let q1 and q2 be any two mutually orthogonal unit vectors lying in a plane normal to p1 ,
we have




pT1
0
T
PP qi = p1 α2 p1 α3 p1  α2 pT1  qi = p1 α2 p1 α3 p1  0  = 0 , for i = 1, 2 .
0
α3 pT1
Thus, the nullspace of PPT is of dimension 2, and hence, PPT is of rank 1—see Exercise 2.2— thereby
proving sufficiency.
17
To prove necessity, let us assume that matrix PPT is singular of rank one. Then, it has two zero
eigenvalues, i.e., λ1 = λ2 = 0, and λ3 6= 0. Let {ei }31 be the associated mutually orthogonal unit
eigenvectors. Then,
(p1 pT1 )ei = 0 , for i = 1, 2 ,
i.e.,
p1 pT1 ei +p2 pT2 ei +p3 pT3 ei = 0 , for i = 1, 2 .
| {z }
| {z }
| {z }
αi
Hence, we have
γi
βi
α1 p1 + β1 p2 + γ1 p3 = 0 ,
α2 p1 + β2 p2 + γ2 p3 = 0 .
(26a)
(26b)
Assume now that vectors p1 , p2 and p3 are linearly independent. This means that for eq.(26a) to be
satisfied all the coefficients α1 , β1 , and γ1 must be equal to zero. This leads to the following relations:
pT1 e1 = 0 ,
pT2 e1 = 0 ,
pT3 e1 = 0
which means that vectors p1 , p2 and p3 lie in one plane perpendicular to vector e1 and, therefore,
are linearly dependent, which contradicts our assumption. The same argument shows that the said
vectors lie in a plane perpendicular to vector e2 . Therefore, vectors p1 , p2 and p3 lie in the line of
intersection of these planes, which passes through the origin, thereby proving necessity.
2.26 We have


−0.5 q12
q13
Q =  q21 0.25
q23 
q31
q32 −0.75
Here,
tr(Q) = −1 = 1 + 2 cos φ
from which,
cos φ = −1,
i.e., φ = π
Therefore, Q is bound to be symmetric, and we must have
q21 = q12 ,
q31 = q13 ,
q32 = q23
Using eq.(2.49), we have
Q + 1 = 2eeT
which gives

This implies that
Therefore,
0.5
 q12
q13

 2
q12
q13
e1
1.25 q23  = 2  e1 e2
q23 0.25
e1 e3
e1 = ±0.5,
q12 = 2e1 e2 = ±0.7906,
e2 = ±0.7906,
e1 e2
e22
e2 e3

e1 e3
e2 e3 
e23
e3 = ±0.3535
q13 = 2e1 e3 = ±0.3535,
q23 = 2e2 e3 = ±0.5589
There are thus four possible solutions for matrix Q, namely,




−0.5 0.7906 0.3535
−0.5
−0.7906 −0.3535
Q1 =  0.7906 0.25 0.5589  ,
Q2 =  −0.7906
0.25
0.5589 
0.3535 0.5589 −0.75
−0.3535 0.5589
−0.75




−0.5
−0.7906 0.3535
−0.5
0.7906 −0.3535
Q3 =  −0.7906
0.25
−0.5589  , Q4 =  0.7906
0.25
−0.5589 
0.3535 −0.5589 −0.75
−0.3535 −0.5589 −0.75
18
Figure 3:
2.28 e1 , e2 and e3 are unit vectors parallel to the X, Y and Z axes, respectively, as shown in Fig. 3.
Moreover, let f2 be the unit vector directed from E ′ to F ′ , and f3 be the unit vector directed from E ′
to H ′ , as indicated in the same figure. Since e1 , e2 and e3 form an orthogonal triad, the same holds
for f1 , f2 and f3 . Therefore,
f1 = f2 × f3 = e3
Moreover, by inspection,
√
√
2
2
f2 =
(−e1 + e2 ), f3 =
(−e1 − e2 )
2
2
Using Definition 2.2.1, we obtain
√
√


0 −√ 2/2 −√2/2
[ Q ]F =  0
2/2 − 2/2 
1
0
0
Thus,
where v is given as
 √

√
2/2
1 √
2

vect[ Q ]F = e sin φ =
− √2/2 − 1 =
v
2
4
2/2


1√
[ v ]F ≡  −1 − 2 
1
p
√
The norm kvk of v is thus, kvk = 5 + 2 2 . Therefore, the unit vector e parallel to the axis of
rotation is given by


1√
1
[ e ]F = p
√  −1 − 2 
5+2 2
1
Moreover, sin φ (≥ 0) is given by
p
√ q
√
√
2
10 + 4 2
sin φ =
5+2 2=
= 0.9892
4
4
which leads to
φ = 81.58◦
19
or φ = 98.42◦
On the other hand,
1
1
cos φ = (tr(Q) − 1) =
2
2
!
√
2
− 1 = −0.1464
2
Since cos φ < 0, the angle sought is
φ = 98.42◦
2.29 (a) From the problem statement and Fig. 2.10, we have


 

1
0
1√
1
−−→
−−→
[OP ]1 =  −1  . [OC]1 =  0  , [Q]1 =  1 + √3
3
1
1
1− 3
We can write
√
√ 
1 − 3 1 + √3
1√ 1 − 3 
1+ 3
1
−−→
−−→
−−→
[OC]1 = [OP ]1 + [Q]1 [P C]2
and thus,
√
√ 




√
1√ 1 + 3 1 − √3
−1
1
1
3
−−→
−−→
−−→
 1 
[P C]2 = [QT ]1 [OC]1 − [OP ]1 =  1 − √3
1√
1 + 3 1  =
3
3
1+ 3 1− 3
1
0
−2
√
(b) Let [n]1 = ( 3/3)[1, 1, 1]T be the unit normal of plane ABC and [d]1 = [x1 , y1 , z1 ]T be the
position vector of an arbitrary point D on plane ABC. The equation of plane ABC is


√
−x1
3
−
−
→
T
[n]T1 [OC]1 − [d]1 = 0 or
[ 1 1 1 ]  −y1  = 0
3
1 − z1
which leads to
x1 + y1 + z1 − 1 = 0
(27)
The position vector of an arbitrary point R takes the forms
[r]1 = [x1 , y1 , z1 ]T ,
We have
or
and
[ρ]2 = [x2 , y2 , z2 ]T
−−→
[r]1 = [Q]1 [ρ]2 + [OP ]1
√
√   



x1
1√ 1 − 3 1 + √3
x2
1
1
 y1  =  1 + 3
1√
1 − 3   y2  +  −1 
√
3
z1
1− 3 1+ 3
1
z2
1

√
√



x1
x2 + √
(1 − 3)y2 + (1 + √3)z2 + 3
 y1  = 1  (1 + 3)x2 + y2 + (1 − 3)z2 − 3 
√
√
3
(1 − 3)x2 + (1 + 3)y2 + z2 + 3
z1

(28)
Substituting eq.(28) into eq.(27), we obtain the equation of plane ABC in end-effector coordinates,
namely
x2 + y2 + z2 = 0
(29)
−−→
which means that point P , the origin of F2 , lies in the plane. Substituting [P C]2 into eq.(29), we
obtain
√
√
√
3
3 2 3
+
−
=0
3
3
3
thereby verifying the answer to (a).
20
Figure 4:
2.30 Referring to Fig. 4, unit vectors { ii , ji , ki }10 are attached to edges Xi , Yi , and Zi . Now, the 0subscripted unit vectors are expressed as linear combinations of the 1-subscripted ones, i.e.,
i0 = −k1 ,
j0 = −j1 ,
k0 = −i1
Applying Definition 2.2.1, the rotation matrix is given by


0
0 −1
[ Q ]1 =  0 −1 0 
−1 0
0
Since Q is symmetric, its angle of rotation is π, i.e., φ = π. If we recall eq.(2.49), we can readily write
eeT =
1
(Q + 1)
2
Let { ei }31 denote the components of [ e ]1 .
Hence,

e21
 e1 e2
e1 e3
e1 e2
e22
e2 e3
 
e1 e3
1/2 0
e2 e3  =  0
0
e23
−1/2 0

−1/2
0 
1/2
whence
it is apparent that e2 = 0 and e1 and e3 have opposite signs, their absolute values being both
p
1/2. Let us, for example, choose e1 > 0. Then,
√
2
[ e ]1 =
[ 1 0 −1 ]T
2
2.31 Obviously, the trace is all we know of this matrix. Since the trace of the matrix representation of a
rotation equals 1 + 2 cos φ, where φ is the angle of rotation, it is apparent that the trace ranges between
−1 and 3. However, the trace of the proposed matrix is 3 × (−0.866) = −2.60, which lies outside the
range of the trace, and hence the results are wrong, without further investigation.
2.32 Define unit vectors i, j, . . . k′′ , as indicated in Fig. 5. Let L′ and L′′ be the linear transformations
mapping the cube into the second and third configurations, respectively. Now, clearly,
L′ : i → i′ = −k, j → j′ = j, k → k′ = −i
L′′ : i → i′′ = j, j → j′′ = k, k → k′′ = i
21
Figure 5:
where vectors i, j, k are assumed to be parallel to the X, Y and
frame. From Definition 2.2.1, then, in the given frame,



0 0 −1
0
[ L′ ] =  0 1 0  , [ L′′ ] =  1
−1 0 0
0
Z axes of an orthogonal coordinate

0 1
0 0
1 0
Now it is a simple matter to show that the two foregoing matrices are orthogonal, but the first is
improper, whereas the second, proper. Thus, the second configuration is a reflection, the third being
a rotation of the cube given in the first configuration. The plane of reflection of L′ has a unit normal,
e, which is the eigenvector of L′ associated with the eigenvalue −1. Let the components of e in the
aforementioned coordinate frame be e1 , e2 and e3 . These are determined from the relation L′ e = −e,
which thus yields:
−e3 = −e1 , e2 = −e2 , −e1 = −e3
and hence, one possible solution is
√
2
[ 1 0 1 ]T
2
the other solution being the negative of the previous one. On the other hand, the linear invariants of
matrix L′′ are readily computed as follows:
 
1
1
1 
[ e ] sin φ =
1 , cos φ = −
2
2
1
[e] =
According to our convention, we define sin φ as positive, which thus yields
 
√
√
1
3
3 
sin φ = ke sin φk =
, [e] =
1
2
3
1
and hence, φ = 120◦ .
2.33 (a) Simple computations yield
T
[R1 ]G [R1 ]G = 1, and det[R1 ]G = 1
Hence, [R1 ]G is a rotation. Similarly, we can readily show that [R2 ]G , [R1 ]C and [R2 ]C are
rotations as well.
22
(b) Further computations lead to:


−0.5
tr[R1 ]G = 2, vect[R1 ]G =  0.5  ,
−0.5


0
tr[R1 ]C = 2, vect[R1 ]C =  −0.866 
0
Since the camera and the gripper are rigidly coupled, they undergo the same rotation. Actually,
because the first two matrices have identical traces, the cosines of their angles of rotation are
the same. In order to conclusively assess whether the two representations correspond to the same
rotation, the sine of the angles of rotation has to be computed. Defining the sines as positive, these
can be obtained as the magnitudes of the two foregoing vectors. The two rotation matrices being
represented in different coordinate frames, their vectors have different components. However, note
that
kvect[R1 ]G k = kvect[R1 ]C k = 0.866
Hence, [R1 ]G and [R1 ]C represent the same rotation in different coordinate frames.
(c) Furthermore,




0.697
0.938
tr[R2 ]G = 1.692, vect[R2 ]G =  0.558  , tr[R2 ]C = 1.692, vect[R2 ]C =  0 
−0.289
0
Similarly,
kvect[R2 ]G k = kvect[R2 ]C k = 0.938
and so, the two given forms of R2 represent, in fact, the same rotation.
(d) Before we embark on computing the required matrix [Q]G , data compatibility must be verified.
Notice that, if the two pairs of rotations are compatible, the axial vectors of matrices R1 and R2
must make the same angle, whether measured in frame G or in frame C. So, let us compute the
dot product of the two axial vectors in each frame:
[vect[R1 ]G ]T vect[R2 ]G = 0.0748
while
[vect[R1 ]C ]T vect[R2 ]C = 0
The foregoing result indicates that, although each pair of matrices represents the same rotation
in different frames, the two pairs are not related by the same similarity transformation Q. That
is, if the data are compatible, we should have
Q[Ri ]C QT = [Ri ]G , i = 1, 2
where Q stands for either [Q]C or [Q]G , which are identical by virtue of Theorem 2.5.2. That
is, the data are incompatible. Given the incompatibility of the data, no Q can be found that
transforms both [R1 ]C and [R2 ]C into [R1 ]G and [R2 ]G , respectively.
2.35 Set [ p ]B = [ x, y, z ]T and [ p ]G = [ x′ , y ′ , z ′ ]T . We then have,
[p]B = [g]B + [Q]B [p]G
i.e.,
√  ′
√ 
√

  
1√
1 − 3 1 + √3
x
x
1−
√ 3
1
y = 
3√  +  1 + √3
1√
1 − 3   y′ 
3
1+ 3
1− 3 1+ 3
1
z′
z
√
√
√


′
′
1−
/3 + (1 +
3)z ′ /3
√ 3 + x /3
√+ (1′ − 3)y
√
′
=
3√+ (1 + 3)x
(1 − 3)z ′ /3 
√ /3′ + y /3 + √
1 + 3 + (1 − 3)x /3 + (1 + 3)y ′ /3 + z ′ /3
23
Hence, the equations in G coordinates are:
√
√
1 ′ 1− 3 ′ 1+ 3 ′ 2
C : (1 − 3 + x +
y +
z)
3
3
3
√
√
√
1+ 3 ′ 1 ′ 1− 3 ′ 2
+( 3+
x + y +
z) =4
3
√3
√ 3
√
1− 3 ′ 1+ 3 ′ 1 ′
x +
y + z =0
Π1 : 1 + 3 +
3√
3√
3
√
1− 3 ′ 1+ 3 ′ 1 ′
Π2 : 1 + 3 +
x +
y + z = 10
3
3
3
√
2.36 (a) Let us attach a frame X ′ Y ′ Z ′ , labelled F ′ , at the center of the ellipse, with the X ′ axis parallel
to the semiaxis of length a, and with the Y ′ axis parallel to the semiaxis of length b. Moreover,
let us define vectors { i, j, k } and { i′ , j′ , k′ } as the unit vectors parallel to the XY Z (F ) and
X ′ Y ′ Z ′ (F ′ ) axes, respectively. The equation of the ellipse in parametric form is
x′ = a cos ϕ,
y ′ = b sin ϕ,
z′ = 0
Since the vertices of the triangle are located a unit distance away from the origin O, the unit
vector normal to the P QR plane, k′ , is readily obtained as
√
3
T
′
[1 1 1]
[ k ]F =
3
Moreover, the X ′ axis is parallel to edge P Q, which gives
√
2
T
[ i ′ ]F =
[ −1 1 0 ]
2
Then, the last unit vector j′ is simply given by j′ = k′ × i′ . Therefore,
√
6
′
[ j ]F =
[ −1 −1 2 ]T
6
Thus, the rotation matrix Q1 transforming coordinates in frame F ′ into coordinates in F is found
using Definition 2.2.1, namely,
√ 
 √
√
−√ 3 −1 √2
6
[ Q 1 ]F =
3 −1 √2 
6
0
2
2
Now, we will express the unit vectors et , en , and eb , in frame-F coordinates. Here, we can readily
write,
√
3
′
[ e b ]F = [ k ]F =
[ 1 1 1 ]T
3
To obtain the unit vector et , which is tangent to the ellipse, we use the above parametric equations.
Note that vector et is parallel to the velocity v of a particle moving along the ellipse. We can
thus write et = v/kvk. Let p be the position vector of this particle; then,
[ p ]F ′ = [ x′
y′
T
z ′ ] = [ a cos ϕ
b sin ϕ 0 ]
T
Hence,
[ p ]F = [ Q 1 ]F [ p ]F ′
√ 
 √

 √

√
√
− 3 −1 √2
a cos ϕ
−3√ 3 cos ϕ − 2 sin ϕ
6 √
6
=
3 −1 √2   2a sin ϕ/3  =
a  3 3 cos ϕ − 2 sin ϕ 
6
18
2
0
0
2
4 sin ϕ
24
Now, the velocity of the particle v is given as
[ v ]F = [ ṗ ]F
and thus,
 √

√
3 3 sin ϕ − 2 cos ϕ
6  √
[ v ]F =
aϕ̇ −3 3 sin ϕ − 2 cos ϕ 
18
4 cos ϕ
The norm kvk of v is given by
√
q √
√
6
aϕ̇ (3 3sϕ − 2cϕ)2 + (−3 3sϕ − 2cϕ)2 + 16c2 ϕ
kvk =
18
√
√
q
q
q
6
6
1
2
2
aϕ̇ 54 sin ϕ + 24 cos ϕ =
aϕ̇ 24 + 30 sin2 ϕ = aϕ̇ 4 + 5 sin2 ϕ
=
18
18
3
Therefore,
 √

√
3 √3 sin ϕ − 2 cos ϕ
6
3
[ v ]F
 −3 3 sin ϕ − 2 cos ϕ 
=
aϕ̇ p
[ e t ]F =
kvk
18
aϕ̇ 4 + 5 sin2 ϕ
4 cos ϕ

 √
√
3 √3 sin ϕ − 2 cos ϕ
6
 −3 3 sin ϕ − 2 cos ϕ 
= p
6 4 + 5 sin2 ϕ
4 cos ϕ
Now, vector en is simply
en = eb × et
or
√

2 √3 cos ϕ + 3 sin ϕ
 −2 3 cos ϕ + 3 sin ϕ 
[ e n ]F = p
6 4 + 5 sin2 ϕ
−6 sin ϕ
√
6

The matrix that represents the rotation undergone by the gripper from an orientation in which
vectors et , en , and eb are parallel pairwise to the coordinate axes XY Z, in F coordinates, is thus
obtained using Definition 2.2.1, namely,
p
√
 √

√
3 3sϕ − 2cϕ
2 3cϕ + 3sϕ p8 + 10s2 ϕ
√
√
6
2 
 −3 3sϕ − 2cϕ −2 3cϕ + 3sϕ
[ Q ]F = p
p8 + 10s ϕ
6 4 + 5s2 ϕ
4cϕ
−6sϕ
8 + 10s2 ϕ
(b) We know that the rotation matrix is symmetric when the angle of rotation is π. Therefore, we
must have
√
√
−3 3 sin ϕ − 2 cos ϕ = 2 3 cos ϕ + 3 sin ϕ
(30)
q
4 cos ϕ = 8 + 10 sin2 ϕ
(31)
q
−6 sin ϕ = 8 + 10 sin2 ϕ
(32)
From eq.(30), we have
sin ϕ
2
≡ tan ϕ = −
cos ϕ
3
which leads to
ϕ = 146.31◦
25
or ϕ = 326.31◦
if we take positive values of ϕ only. However, if we substitute ϕ = 146.31◦ into eqs.(31 & 32),
these are violated, while they are verified for ϕ = 326.31◦, which is thus the solution sought. As
a verification, substituting ϕ = 326.31◦ in the rotation matrix, we obtain


−0.7887 0.2113 0.5774
[ Qπ ]F =  0.2113 −0.7887 0.5774 
0.5774
0.5774 0.5774
The trace of Qπ is
tr[ Qπ ]F = −0.7887 − 0.7887 + 0.5774 = −1
So,
cos φ =
tr[ Qπ ]F − 1
= −1 or φ = π .
2
2.37 Let i, j and k be unit vectors parallel to axes X, Y and Z, respectively, i.e., to lines EA, EF and EH
of the cube in Fig. 1a. These vectors are mapped into l, m and n, respectively, in the configuration
shown in Fig. 1b. It is apparent that
l=k√
2
m=−
i+
√2
2
n=−
i−
2
√
2
j
√2
2
j
2
and hence, using Definition 2.2.1,
√
√

0 −√ 2/2 −√2/2
2/2 − 2/2 
Q = 0
1
0
0

Upon equating the foregoing expression with that displayed in Exercise 2.18, we end up with a matrix
equation for the unknowns φ, θ and ψ. We thus have nine equations for three unknowns at our disposal.
Out of those nine equations we can pick up 9!/(6!3!) = 84 possible triplets of equations to solve for the
unknowns. Obviously, this is not a practical approach to solving the problem. It may be wiser to first
inspect the entries of the two matrices and pick up obvious entries that would yield simple relations
for those unknowns; the entries in the first column and the third row would be obvious candidates.
However, it turns out that this approach leads to just too many possibilities because of the doublevaluedness of the inverse-trigonometric functions. A more rational approach is described below, using
the linear invariants of the two foregoing matrices. Indeed, upon equating the vector and the trace of
these matrices, we obtain a system of four scalar equations, namely,2

  √

sθ(sψ − sφ)
√ 2/2
sθ(cψ + cφ)

  − √2/2 − 1 

=

cθ(sφcψ + cφsψ) + sφcψ + cφsψ
√2/2
cθ(cφcψ − sφsψ) − sφsψ + cφcψ + cθ
2/2
which is algebraically rather simple. Upon simplification, the system reduces to
√
sθ(sψ − sφ) = 2/2
√
sθ(cψ + cφ) = −(1 + 2/2)
√
(1 + cθ)s(ψ + φ) = 2/2
√
(1 + cθ)c(ψ + φ) + cθ = 2/2
2 Angle
(33a)
(33b)
(33c)
(33d)
φ here is not to be confused with the angle used elsewhere in Chapters 2 and 3 to denote the angle of rotation of Q.
26
If we let
σ ≡ φ + ψ,
(34)
then eqs.(33c & d) become
√
2/2
√
cσ(1 + cθ) = 2/2 − cθ
sσ(1 + cθ) =
(35a)
(35b)
We now solve these equations for cθ:
(35a)2 + (35b)2
⇒
(1 + cθ)2 = 1 −
thereby ending up with
θ=±
√
2cθ + cθ2
⇒
cθ = 0
π
2
Then, using notation (34), we calculate
(33a)2 + (33b)2
⇒
sin2 θ(2 + 2 cos σ) = 2 +
√
2
Upon substitution θ = ±π/2 into the above equation, we derive
√
2
cos σ =
2
which leads to
π
π
⇒ φ+ψ =±
4
4
Substituting the values of θ and σ found above into eqs.(33a & b) and solving them for ψ and φ, we
obtain:
Sol 1:
σ=±
θ = π/2 and φ + ψ = π/4
⇒
ψ1 = π,
φ1 = −
3π
4
Sol 2:
θ = π/2 and φ + ψ = −π/4
⇒
ψ2 =
3π
,
4
φ2 = −π
Sol 3:
θ = −π/2 and φ + ψ = π/4
⇒
ψ3 = 0,
φ3 =
π
4
Sol 4:
θ = −π/2 and φ + ψ = π/4
⇒
π
ψ4 = − ,
4
φ4 = 0
Now, to test the feasibility of these four solutions, we substitute the corresponding values of θ and σ
into the left-hand sides of eqs.(33c & d). The calculations show that solutions 2 and 4 are unfeasible
since the substitution of these corresponding values of θ and σ ≡ ψ + φ makes the left-hand sides of
eqs.(33c & d) different from the corresponding right-hand sides.
In summary, then, we have two feasible solutions, namely,
π
π
3
, σ ≡ ψ + φ = : ψ = π, φ = − π,
2
4
4
π
π
π
θ = − , σ ≡ ψ + φ = : ψ = 0, φ =
2
4
4
θ=
27
2.38 We need first the rotation matrix carrying the triangle from A1 , B1 , C1 to A0 , B0 , C0 . This matrix
can be found using Definition 2.2.1, as when solving Exercise 2.29 above. An alternative approach,
more geometric, is illustrated below:
Making abstraction of the displacement involved, which is not at stake, we can think of achieving the
attitude A0 , B0 , C0 from A1 , B1 , C1 upon bringing edge A1 B1 to coincide with edge A0 B0 , by making
A1 coincide with A0 and B1 with B0 , without changing the attitude of A1 , B1 , C1 . In this way, C1 lies
on the Y0 axis. Now, we can achieve the desired attitude by rotating triangle A1 , B1 , C1 about edge
A1 B1 . By symmetry, line C0 C1 intersects line A1 B1 , as the reader can readily verify upon computing
the distance between the two lines. In this way, then the angle of rotation involved is π, as found when
solving Exercise 2.29, while the axis of rotation is A1 B1 , the matrix sought then being


0
0 −1
Q =  0 −1 0 
−1 0
0
Upon equating this matrix with that displayed in terms of Euler angles in Exercise 2.18, we obtain the
matrix equation

 

cθcφcψ − sφsψ −cθcφsψ − sφcψ sθcφ
0
0 −1
 cθsφcψ + cφsψ −cθsφsψ + cφcψ sθsφ  =  0 −1 0 
−sθcψ
sθsψ
cθ
−1 0
0
In view of the simple (symmetric) form of the right-hand side of the above equation, it will be quite
simple to compute the Euler angles by just equating corresponding entries on the two sides. For
starters, the (3, 3) entry of right-hand side vanishes; now, upon equating the (1, 3), (2, 2) and (3, 1)
entries of these matrices, we obtain, in this order
sθcφ = −1,
cφcψ = −1,
sθcψ = 1
whence all sθ, cφ and cψ are ±1. Moreover, sθ and cψ bear the same signs, but cφ bears the opposite
sign of these. We thus have two possible solutions:
First solution : {φ, θ, ψ} = {π, π/2, 0}
Second solution : {φ, θ, ψ} = {0, 3π/2, π}
3
Fundamentals of Rigid-Body Mechanics
3.1 (a) With reference to Fig. 6, we have
i′ = −j,
j′ = k,
k′ = −i
According to Definition 2.2.1, the rotation matrix is thus


0 0 −1
Q =  −1 0 0 
0 1 0
(b) From the foregoing representation of Q,
tr(Q) = 0 = 1 + 2 cos φ,
Moreover,
vect(Q) =
i.e., cos φ = −
1
T
[ 1 −1 −1 ] = (sin φ)e,
2
28
1
2
sin φ > 0,
Figure 6:
and hence,
√
3
3
sin φ =
⇒ e=
[ 1 −1 −1 ]T , φ = 120◦
2
3
The position vector a of point A, in its original configuration, and the position vector a′ of the
same point in the displaced configuration, denoted by A′ , are given by
√
a = [0 0
T
0] ,
a′ = a [ 2
1 −1 ]
T
Hence, using eq.(3.18),
T
(Q − 1) (Qa − a′ )
a
= [3
2(1 − cos φ)
3
Furthermore, the moment n of line L is defined as
p0 =
2 1]
T
n = p0 × e
Thus,
Therefore,
κ L = [ eT
n T ]T =
√
3
3


√
−1
3 
n=
a 4 
9
−5
−
√
3
3
−
√
3
3
−
√
3a
9
√
4 3a
9
√
3a
9
−5
T
(c) Since we know the position vector p0 of a point P0 of the screw axis, and the unit vector e parallel
to this axis, L has the equations
x−a
y − 2a/3
z − a/3
√
√
=
= √
3/3
− 3/3
− 3/3
29
i.e.,
2
1
x − a = −y + a = −z + a
3
3
Hence, the intersection of the screw axis with the X-Y plane is (4a/3, a/3, 0), its intersection with
the Y -Z plane being (0, 5a/3, 4a/3), while that with the Z-X plane is (5a/3, 0, −a/3).
3.2 We have
 
0
p1 =  0  ,
0

√1/2
p2 =  3/2  ,
0



−1/2
√
p3 =  3/2  ,
0


√0
p4 =  √3/3 
6/3
Moreover, if f denotes the resultant of the two given forces, then






−1
−1
√0
√
f1 =  0  , f2 =  −√3/3  , f = f1 + f2 =  −√3/3 
0
− 6/3
− 6/3
Hence,
2
kf k = 2
Now, we can readily calculate the resultant moment of the two given forces with respect to P1 , namely,
√ T
nP1 = p2 × f1 + p4 × f2 = 0 0 23
where, obviously, the moment of the second force with respect to P1 vanishes because the line of action
of this force passes through P1 . From eq.(3.111), one has,
p′′0 =
1
kf k
2f
× (nP1 − f × p1 ) =
1
2f
kf k
× nP1 =
√
1
[ −1
3
4
0 ]T
and the vector representing the wrench axis is determined as
e=
√
f
= − 22
kf k
−
√
6
6
−
The equations of the wrench axis are thus
√ T
3
3
√
x + 1/4
y − 3/4
z
√
√
=
= √
− 2/2
− 6/6
− 3/3
T
(36)
Now let p = [x, y, z] be the position vector of the point of the wrench axis lying closest to P4 . Since
(p − p4 ) · e = 0, we have
√
√
√
√
−3 2x − 6y − 2 3z + 3 2 = 0
(37)
Solving eqs.(36) and (37) for x, y, and z, we obtain
p=
1
4
√
5 3
12
√
6
6
T
3.3 Three lines are given in terms of their Plücker arrays:
ei
pLi =
, i = 1, 2, 3
ni
The fourth line has the Plücker array
pL =
30
e
n
The condition for intersection of L with Li is that the moment µi of Li w.r.t. L vanish, i.e.,
µi ≡ ni · e = 0,
i = 1, 2, 3
(38)
where ni is the moment of line Li with respect to a point P on line L, i.e.,
ni ≡ (pi − p) × ei
Therefore, eq.(38), for i = 1, 2, 3, leads to a system of 3 linear homogeneous equations in e:
Ae = 0
with

[(p1 − p) × e1 ]
T



A ≡  [(p2 − p) × e2 ]T


T
[(p3 − p) × e3 ]
Since kek = 1, i.e., e 6= 0, A must be singular, and hence,







det(A) = 0
or
f (p) ≡ [(p1 − p) × e1 ] × [(p2 − p) × e2 ] · [(p3 − p) × e3 ] = 0
(39)
which is, apparently, a scalar function f (p) of a vector argument p. Moreover, since p appears linearly
in each of the three factors involved, f (p) is apparently cubic in p. However, a simple expansion of
the double mixed product of eq.(39) shows that f (p) is, in fact, quadratic, i.e.,
f (p) = [(p1 − p) × e1 · e2 ] [p2 × p3 − (p2 − p3 ) × p] · e3
− [(p1 × e1 ) · (p2 − p) − p × e1 · p2 ] e2 × (p3 − p) · e3
Thus, f (p) = 0 represents a quadratic function of the form
f (p) = pT Mp + gT p + f0
Now, the 3 × 3 matrix M is obtained as
M=
1 ∂2f
2 ∂p2
The above matrix is thus one-half the Hessian matrix of f with respect to p. A straightforward
calculation of the Hessian leads to
M=
1
T
(e1 × e2 ) [e3 × (p2 − p3 )] + [e3 × (p2 − p3 )] (e1 × e2 )T
2
+(e3 × e2 ) [e1 × (p1 − p2 )]T + [e1 × (p1 − p2 )] (e3 × e2 )T
(40)
Moreover, g and f0 are given as
g = (e2 × p3 · e3 )(p1 × e1 ) + (e2 × p3 · e3 )e1 × p2 + (p1 × e1 · p2 )e3 × e2
−(p2 × p3 · e3 )e1 × e2 − (p1 × e1 · e2 )e3 × (p2 − p3 )
f0 = (p1 × e1 · e2 )(p2 × p3 · e3 ) − (p1 × e1 · p2 )(e2 × p3 · e3 )
thereby showing that the locus of all lines intersecting a triad of given lines is a quadric. Furthermore,
f (p) represents a one-sheet hyperboloid, as we show below.
31
First, we note that any point P of line Li is a point of the above quadric. Indeed,the position vector
p of P can be expressed as
p = p1 + λe1
where λ is a real variable. Upon substitution of the above expression into f (p), one can readily show
that this position vector verifies the quadric equation, the conclusion being that line L1 lies in the
above quadric. One can prove likewise that lines L2 and L3 also lie in the quadric.
As a consequence, then, the quadric represents a ruled surface. But the only quadratic ruled surface is
the one-sheet hyperboloid of elliptic cross section. This hyperboloid admits, as special cases, the onesheet hyperboloid of revolution, the cone and the cylinder of elliptical cross sections. Further special
cases are the cone, the cylinder of revolution and the plane.
Now, by means of a shift of the origin to a point P0 of position vector p0 , as yet to be determined, we
can obtain the equation of the hyperboloid in its standard form
1
T
M r = 1, r = p + p0
(41)
r
σ2
Below we find expressions for p0 and σ 2 . To do this, we write f (p) in terms of r and p0 :
f (r) ≡ f (r + p0 ) = (r + p0 )T M(r + p0 ) + (2Mp0 + g)T + f0 + gT p0 + pT0 Mp0 = 0
Thus, in order to obtain the standard form, we must get rid of the linear term in r in the above
expression, which can be done by setting
1
p0 = − M−1 g
2
where M−1 exists, unless the three given lines bear special features, like concurrency or parallelism.
Upon choosing p0 as found above, then, the quadratic equation becomes
1
f (r) = rT Mr + f0 − gT Mg = 0
4
which readily leads to the standard form of eq.(41) if we define σ 2 as
σ2 =
1 T
g Mg − f0
4
Note that σ 2 must be positive. Proving that this is the case should be possible with the expressions
for g and f0 found above.
It is noteworthy that the one-sheet hyperboloid has the property3 that it contains two families of lines,
each family being called a ‘regulus’. One of these families is the ‘left-hand (LH) regulus’, the other is
the ‘right-hand (RH) regulus’, as depicted in Fig. 7.
Moreover, each line of the RH regulus intersects all lines of the LH regulus, and the other way around.
Hence, we can think of L1 , L2 and L3 as being three lines of, say, the RH regulus, that are intersected
by line L, which is of the LH regulus.
Furthermore, the standard form can be reduced to canonical form by finding the eigenvalues and the
eigenvectors of M. Let λi , for i = 1, 2, 3, be these eigenvalues, the corresponding eigenvectors being vi .
Since M is symmetric, its three eigenvalues are real and its three eigenvectors are mutually orthogonal.
Now, let
V ≡ [ v1 v2 v3 ]
3 Sommerville, D.M.Y., 1934, Analytical Geometry of Three Dimensions, Cambridge University Press, Cambridge, MA,
pp. 110-120. See also Hurley, J.F., 1981, Multivariable Calculus, Saunders College Publishing, Philadelphia.
32
Figure 7: (a) The right-hand regulus; (b) The left-hand regulus
Upon a change of variable given by
r = Vρ,
M = V∆VT ,
 
x
ρ = y ,
z
∆ = diag(λ1 , λ2 , λ3 )
the quadratic equation under study reduces to the canonical form4
x2
y2
z2
+ 2 − 2 =1
2
a
b
c
(42)
where parameters a2 , b2 and c2 are given by
a2 =
σ2
,
λ1
b2 =
σ2
,
λ2
c2 =
−σ 2
λ3
i.e., M has two positive and one negative eigenvalues.
This result has relevance in robotics because, for a three-revolute manipulator used for positioning
tasks of its end-effector, the revolute axes can be regarded as lines L1 , L2 , L3 , that are contained in
a one-sheet hyperboloid H. If the operation point of the EE lies in H, then the manipulator is in a
singular configuration and the manipulator cannot transmit a velocity to the operation point in the
direction of L, the line intersecting all three lines L1 , L2 and L3 .
3.4 (a) The measurements are acceptable if and only if two conditions hold: i) the magnitudes of the two
representations of both n and f are the same; and ii) the angle between them is the same, when
the foregoing items are computed in either representation. But this is so because the magnitude
of the moment is 5 and the magnitude of the force is 2 in both representations. Moreover, the
inner product of the force and the moment in both representations is 0, and hence, the angle
between the two vectors is 90◦ , regardless of the representation adopted.
4 Hurley,
J.F., 1981, Multivariable Calculus, Saunders College Publishing, Philadelphia.
33
(b) Two different vector quantities are known in each of the given frames. By resorting to the GramSchmidt orthogonalization procedure, outlined in Appendix B, two orthonormal triads can be
obtained. Let
[ n ]i
, i = 1, 2
[ e 1 ]i ≡
k[ n ]i k
and
[ u ′ ]i ≡ [ f ]i − [ f T e 1 ]i [ e 1 ]i ,
Defining
[ e 2 ]i ≡
and
[ u ′ ]i
,
k[ u′ ]i k
[ e 3 ]i ≡ [ e 1 × e 2 ]i ,
we obtain
i = 1, 2
i = 1, 2
i = 1, 2
 
 


0
0
−1
[ e 1 ]1 =  0  , [ e 2 ]1 =  1  , [ e 3 ]1 =  0 
0
0
1






−2/3
−2/3
−1/3
[ e1 ]2 =  −2/3  , [ e2 ]2 =  2/3  , [ e3 ]2 =  −1/3 
−2/3
1/3
2/3
Note that for this particular case, we have [ n ]i ⊥ [ f ]i , for i = 1, 2, as obtained in (a). Therefore,
the triad { e1 , e2 , e3 } can be simply obtained as
e1 ≡
n
,
knk
e2 ≡
f
,
kf k
e3 ≡ e1 × e2
Now, let [ E ]i be the matrix formed by the arrays [ ej ]i , for j = 1, 2, 3, and i = 1, 2. Hence, the
matrix [ Q ]1 transforming frame-2 coordinates into frame-1 coordinates is related to the foregoing
matrices by
[ E ]1 = [ Q ]1 [ E ]2
from which one can readily solve for the desired matrix as follows:





0 0 −1
2
1 2
−1 −2 2
1
1
1  =  −2 2 1 
[ Q ]1 = [ E ]1 [ E ]T2 =  0 1 0   −2 2
3
3
1 0 0
−2 −1 −2
−1 −2 2
3.5 (a) Let p be the position vector of any point P on the screw axis and e be the unit vector parallel to
the screw axis. Then,
n= p×e
Hence,
knk = kpkkek sin θ = kpk sin θ
where θ is the angle between p and e. From Fig. 8, we can obtain
d = kpk sin θ = knk
34
Figure 8:
(b) Substitute p by p∗ in the expression for n given above, and introduce E = CPM(e), thereby
obtaining a system of three equations in the three components of p∗ :
Ep∗ = −n
(43)
However, by its nature, E is singular, of rank 2, and hence, eq.(43) cannot be solved for p∗ .
Nevertheless, for p∗ to be the point of L closest to O, p∗ must be orthogonal to e, i.e.,
e T p∗ = 0
(44)
We thus have a situation similar to that encountered in eq.(3.61) when attempting an expression
for p. As in the previous case, eq.(44) is adjoined to eq.(43), thereby obtaining a system of four
equations in three unknowns:
E
−n
∗
Ap = b, A = T , b =
e
0
Now, proceeding exactly as when finding p from eq.(3.61), both sides of the foregoing equation
are multiplied by AT from the left. The result is readily found to be
−1 T
−n
∗
T
T
p = (A A) A b = [ E
e]
= −ET n = e × n
0
q.e.d.
(c) We have
[ e ]G =
√
2
[ −1 0
2
T
1] ,
Hence,
d = knk =
√
2
and
[ p ∗ ]G = [ e × n ]G = [ 1
35
√
T
− 2 0]
[ n ]G = [ 0
0 1]
T
3.6 (a) Let us attach the unit vectors i, j, and k to axes X, Y , and Z, respectively, of frame F . Once the
workpiece is grasped by the gripper, axis X ′ becomes normal to the triangular faces. Moreover,
Y ′ is perpendicular to both line C ′ C and X ′ . Therefore,
 


√
√
−−′→
1
−1
′
3
C
C
×
i
2
 1  , [ j′ ]F = −−→
 0 ,
[ i ′ ]F =
=
′
′k
3
2
k
C
C
×
i
1
1


√
1
6
′
′
′
[ k ]F = i × j =
−2 
6
1
Hence, applying Definition 2.2.1, we obtain
√
√
√

√3/3 − 2/2
√6/6
[ Q ]F =  √3/3 √ 0
−√ 6/3 
3/3
2/2
6/6
whence,


1.5236
1
[ vect(Q) ]F =
−0.1691  = sin φ[ e ]F
2
1.2844
Upon defining sin φ as positive, we have
sin φ = kvect(Q)k = 0.999954,
and
i.e., φ = 89.59◦ or 90.41◦


0.7618
[ vect(Q) ]F
[ e ]F =
=  −0.0846 
sin φ
0.6422
We also have
tr(Q) = 0.9856 = 1 + 2 cos φ,
Hence, the angle of rotation is φ = 90.41◦ .
i.e., φ = ±90.41◦
(b) From Theorem 3.2.2, we have that the set of points of G undergoing a displacement of minimum
magnitude is located on a line L which is parallel to the axis of the rotation involved. In order to
totally specify the screw axis L, we must provide the unit vector e defining its direction, and a
point P ∗ of L lying the closest to the origin, its position vector being given by eq.(3.18), namely,
p∗ =
(Q − 1)T (Qp − c′ )
2(1 − cos φ)
(45)
The position vector p of point P , in its original configuration, and the position vector p′ of the
same point in the displaced configuration, denoted by P ′ , are given by
[ p ]F = [ 2
T
−1 0.25 ] ,
[ p ′ ]F = [ 0 0
0]
T
Thus, using eq.(45), we have
[ p∗ ]F = [ 0.0179 −0.9684 −0.1488 ]
T
The axis L is thus totally specified by vector p∗ and the vector e that defines the axis of rotation
and that was found in (a), namely,
[ e ]F = [ 0.7618 −0.0846 0.6422 ]T
36
3.7 (a) Let P be a point of the axis sought and p its position vector. If the position vectors of A and B
are denoted by a and b, respectively, then, from eq.(3.25),
nA = (p − a) × e,
nB = (p − b) × e
(46)
From these equations, it is clear that both nA and nB are perpendicular to e, and hence, the
latter can be readily obtained from the normalized cross product of the former. If we denote by
c the said cross product, then e = c/kck. Now,
 
 
2
1
c = nA × nB =  0  ⇒ e =  0 
0
0
(b) Referring to Exercise 3.5(b), we have
p∗ = e × n
where n is the moment of the axis with respect to the origin. Thus,
n= p×e
which can be written using eq.(46) as
n = nA + a × e
Therefore,
n = [0, 2, 0]T
and
p∗ = [0, 0, 2]T
(c) The Plücker coordinates of the axis L about the origin are given by
T
πL = eT , nT = [1, 0, 0, 0, 2, 0]T
3.8 We want to prove that, for any 3-dimensional vectors ω and p,
ω × (ω × · · · (ω × (ω ×p)) · · ·) = (−1)k (kωk2k 1 − kωk2(k−1) ωωT )p
{z
}
|
2k factors
Using Theorem 2.3.4, we have
ω × (ω × p) = Ω2 p
where
Ω2 = −kωk2 1 + ωω T
Let
p2 ≡ ω × (ω × p) = Ω2 p
Then
p4 ≡ ω × (ω × p2 ) = Ω2 p2
Thus
p2k ≡ ω × (ω × p2(k−1) ) = Ω2 p2(k−1)
37
Therefore,
ω × (ω × (ω × (ω × p2(k−1) ))) = Ω2 Ω2 p2(k−1)
2
· · Ω2} p4
ω × (ω × · · · (ω × (ω ×p4 ))) = Ω
| ·{z
|
{z
}
k−2 factors
2k−4 factors
2
ω × (ω × · · · (ω × (ω ×p2 ))) = Ω
· · Ω2} p2
| ·{z
|
{z
}
k−1 factors
2k−2 factors
2
ω × (ω × · · · (ω × (ω ×p))) = Ω
· · Ω}2 p
| ·{z
|
{z
}
k factors
2k factors
but
Ω2 Ω2 = −kωk2 1 + ωω T
−kωk2 1 + ωωT
T
T
= kωk4 1 − 2kωk2 ωω T + ω ω
| {zω} ω
kω k2
= kωk4 1 − kωk2 ωωT
So,
Ω2 Ω2 Ω2 = kωk4 1 − kωk2 ωω T
−kωk2 1 + ωωT
T
T
= (−1)3 kωk6 1 + 2kωk4 ωω T − kωk2 ω ω
| {zω} ω
kω k2
= (−1)3 kωk6 1 + kωk4 ωω T
Therefore,
2 2
Ω
· Ω2 Ω2 Ω2} = (−1)k kωk2k 1 + (−1)k+1 kωk2(k−1) ωω T
| Ω · · {z
k factors
= (−1)k kωk2k 1 − kωk2(k−1) ωωT
Finally, we have
ω × (ω × · · · (ω × (ω ×p)) · · ·) = (−1)k (kωk2k 1 − kωk2(k−1) ωωT )p
{z
}
|
2k factors
Furthermore, define
a ≡ ω × (ω × · · · (ω × (ω ×p)) · · ·)
|
{z
}
2k factors
b ≡ (−1)k (kωk2k 1 − kωk2(k−1) ωω T )p
From, eq.(47), we have
a=b
Then, by definition, we have
ω × a = Ωa = Ωb
= (−1)k Ω(kωk2k 1 − kωk2(k−1) ωω T )p
= (−1)k (kωk2k Ωp − kωk2(k−1) Ωωω T p)
= (−1)k (kωk2k (ω × p) − kωk2(k−1) (ω × ω) ω T p)
| {z }
0
⇒
ω × a = (−1)k kωk2k ω × p
38
(47)
Finally,
ω × (ω × · · · (ω × (ω ×p)) · · ·) = (−1)k (kωk2k ω) × p
|
{z
}
2k+1 factors
3.9 We recall first the expressions for the rotation matrix Q, from which we calculate its time-derivative,
and for the angular-velocity matrix Ω:
Q = 1 + (sin φ)E + (1 − cos φ)E2
Q̇ = (φ̇ cos φ)E + (sin φ)Ė + (φ̇ sin φ)E2 + (1 − cos φ)
d 2
(E )
dt
Ω = Q̇QT
For a “small” angle φ, we have
sin φ ≈ φ,
cos φ ≈ 1
the foregoing expressions for Q and Q̇ thus becoming
Q ≈ 1 + φE
Q̇ ≈ φ̇E + φĖ + φ̇φE2
(48)
Using the two above expressions to compute Ω for “small” rotations we obtain
Ω ≈ (φ̇E + φĖ + φ̇φE2 )(1 − φE)
= φ̇E + φĖ + φ̇φE2 − φ̇φE2 − φ2 ĖE − φ̇φ2 E3
Upon simplifying the above expression and deleting the higher-order terms, i.e., the terms involving
φ̇φ, φ2 and φ̇φ2 , we obtain
d
Ω ≈ φ̇E + φĖ = ((φE)
dt
Finally, upon taking the axial vector of the above expression, we conclude that
ω=
d
(φe)
dt
and hence, the vector whose time-derivative is ω turns out to be φe.
3.10 From the relation between ω and η̇ given in the problem statement,
W=
∂ω
∂ η̇
where ω = vect(Q̇QT ). We thus start by computing the product Q̇QT using the expression for Q
derived in Exercise 2.18. Here is the result from the corresponding Maple worksheet.


0
−φ̇ − ψ̇ cos θ
ψ̇ sin θ sin φ + θ̇ cos φ
Q̇QT = 
φ̇ + ψ̇ cos θ
0
−ψ̇ sin θ cos φ + θ̇ sin φ 
−ψ̇ sin θ sin φ − θ̇ cos φ ψ̇ sin θ cos φ − θ̇ sin φ
0
and hence,


ψ̇ sin θ cos φ − θ̇ sin φ
ω =  ψ̇ sin θ sin φ + θ̇ cos φ 
φ̇ + ψ̇ cos θ
39
Then, matrix W can be readily calculated as

− sin φ
∂ω 
cos φ
W=
=
∂ η̇
0
Upon expanding det(W), we obtain det(W) = sin θ.

0 sin θ cos φ
0 sin θ sin φ 
1
cos θ
This matrix is apparently singular under the condition θ = 0 or θ = π.
3.13 In order to give a mechanical interpretation of the matrix m[tr(PPT )1 − PPT ] we note that:
tr(PPT ) =
3
X
i=1
kp2i k ,
PPT =
3
X
pi pTi
i=1
Hence,
m [tr(PPT )1 − P PT ] = m
3
hX
i=1
kp2i k 1 −
3
X
i=1
3
i X
pi pTi =
m kp2i k1 − pi pTi
i=1
The last expression can be readily identified as a discrete form of definition (3.148) of the mass second
moment of the body with respect to the origin, IO , also called the moment of inertia.
Note that defining Pi as the cross product matrix of pi , we can transform the above expression using
eq.(2.39):
3
3
X
X
2
P2i
kpi k1 − pi pTi = − m
m
i=1
i=1
Now, since the moment of inertia IO is necessarily a positive-definite matrix—see the discussion below eq.(3.130)—the sum of the squares of the cross product matrices P2i is bound to be negativeP3
semidefinite, a result that the reader is invited to verify. Moreover, the sum i=1 P2i is negativesemidefinite if and only if the points pi are collinear.
3.14 The two centroidal inertia matrices are acceptable if we can relate them by a similarity transformation.
It is shown in Section 2.7 that if two symmetric n × n matrices have their n moments {Ik }n1 identical,
then they represent the same transformation, although in different frames.
Here, the two matrices, both symmetric, are


1 0 0
[ I ]A =  0 2 0  ,
0 0 3
and their traces are

6
1
[ I ]B =
2
3
2

2 2
5 0
0 7
tr[ I ]A = 1 + 2 + 3 = 6
1
tr[ I ]B = (6 + 5 + 7) = 6
3
Since the two traces are identical, we

1
[ I2 ]A =  0
0
need verify now their second


44
0 0
1
4 0,
[ I2 ]B =  22
9
0 9
26
40
moments. We have

22 26
29 4 
4 53
Figure 9:
whence we readily obtain
tr[ I2 ]A = 1 + 4 + 9 = 14
1
tr[ I2 ]B = (44 + 29 + 53) = 14
9
Finally, we verify their third moments:


1 0 0
[ I3 ]A =  0 8 0  ,
0 0 27
whence


40 22 30
1
[ I3 ]B =  22 21 8 
3
30 8 47
tr[ I3 ]A = 1 + 8 + 27 = 36
360 + 189 + 423
tr[ I3 ]B =
= 36
27
the two matrices thus being related by a similarity transformation and the two measurements are
acceptable.
3.15 To share the same principal axes of inertia, a point P and the mass center C of a rigid body must lie
on one of the principal axes of inertia, as shown in Fig. 9. More formally, we have, from the Theorem
of Parallel Axes,
(49)
IP = IC + m kpk2 1 − ppT
where p is the vector directed from C to P , and C and P are a distance d apart. If e is an eigenvector
of IP and IC , then p = de, and
IP e = λP e, IC e = λC e
Multiplying eq.(49) by e from the right, we can write
λP e = λC e + m kpk2 e − (pT e)p = λC + md2 e − mdp
Multiplying the above equation by eT from the left, we derive
λP = λC + md2 − md2 = λC
(50)
which shows that the two principal moments of inertia about PC are identical. Furthermore, if the
remaining principal directions, XP and ZP , are parallel, respectively, to XC and ZC , the corresponding principal moments of inertia being labelled µP , νP and µC and νC , respectively, then we have,
41
additionally,
µP = µC + md2
νP = νC + md2
3.16 The inertia matrix at point P is related to that at the centroid C by the Theorem of Parallel Axes,
i.e.,
IP = IC + m kp − ck2 1 − (p − c)(p − c)T
Let e be the eigenvector of IP associated with the smallest principal moment of inertia Is :
IP e = IC e + mkp − ck2 e − m (p − c)T e (p − c)
Now dot-multiply both sides of the above equation by e:
Thus,
eT IP e ≡ Is = eT IC e + mkp − ck2 − m (p − c)T e eT (p − c)
2
= eT IC e + mkp − ck2 − m (p − c)T e
2
Is = eT IC e + m(p − c)T (p − c) − m eT (p − c)
The necessary condition for Is to attain a minimum at P is that Is becomes stationary at that point,
i.e.,
T
T
∂Is
∂(p − c)
∂(p − c)
= 0 + 2m
(p − c) − 2m
e[(p − c)T e] = 0
∂p
∂p
∂p
whence,
∂Is
= 2m[1 − eeT ](p − c) = 0
∂p
Hence, ∂Is /∂p vanishes if either (p − c) || e or p = c. If (p − c) || e, then P and C share the same
principal axes of inertia, as per Exercise 3.15, i.e., Is is the smallest principal moment of inertia of
the body; else p = c, thereby showing that the centroid is, in fact, a stationary point of the smallest
principal moment of inertia. To show that this stationary point is a minimum, and not a saddle point or
a maximum, the matrix of second partial derivatives, ∂ 2 Is /∂p2 , must be at least positive semidefinite.
But this is so because
∂ 2 Is
∂(p − c)
= 2m(1 − eeT )
= 2m(1 − eeT )1 = 2m(1 − eeT )
∂p2
∂p
which can be readily shown to have two identical eigenvalues, λ1 = λ2 = 2m, while the third one
vanishes, λ3 = 0, q.e.d.
3.17 We recall below the expressions for M and W:
IC O
M=
O m1
Ω
and W =
O
O
O
Let us consider a moving frame F attached to the rigid body of interest, and an inertial frame F0 .
These two frames are related by the rotation matrix [ Q ]F0 that carries frame F0 into an orientation
identical to that of frame F . Thus
[ IC ]F0 O
[ Ω ]F 0 O
[ M ]F 0 =
and [ W ]F0 =
O
m1
O
O
42
Now, let us assume that the mass moment of inertia matrix IC is expressed in F , as [ IC ]F . Then,
[ IC ]F0 = [ Q ]F0 [ IC ]F [ QT ]F0
Therefore,
[ İC ]F0 = [ Q̇ ]F0 [ IC ]F [ QT ]F0 + [ Q ]F0 [İC ]F [ QT ]F0 + [ Q ]F0 [ IC ]F [ Q̇T ]F0
Since frame F is attached to the rigid body, [ IC ]F is constant, and thus [ İC ]F = O. Therefore,
[ İC ]F0 = [ Q̇ ]F0 [ IC ]F [ QT ]F0 + [ Q ]F0 [ IC ]F [ Q̇T ]F0
We now recall the identity introduced in eq.(3.102), to express [ İC ]F0 in the form
[ İC ]F0 = [ Ω ]F0 [ Q ]F0 [ IC ]F [ QT ]F0 + [ Q ]F0 [ IC ]F [ QT ]F0 [ ΩT ]F0
|
{z
} |
{z
}
[ IC ]F0
and thus,
[ IC ]F0
[ İC ]F0 = [ Ω ]F0 [ IC ]F0 − [ IC ]F0 [ Ω ]F0
Therefore,
[ Ṁ ]F0
[ ΩIC − IC Ω ]F0 O
[ İC ]F0 O
=
=
O
O
O
O
[ IC ]F0
[ Ω ]F 0 O
[ IC ]F0 O
−
=
O
m1
O
O
O
O
m1
[ Ω ]F 0
O
O
O
Finally, we obtain
Ṁ = WM − MW
For the second part of the problem, we differentiate directly µ with respect to time, thereby obtaining
µ̇ = Mṫ + Ṁt
But we just found that
Ṁ = WM − MW
and so
µ̇ = Mṫ + WMt − MWt
Now, the twist of a rigid body lies in the nullspace of its angular-velocity dyad, i.e.
Wt = 0
and hence,
µ̇ = Mṫ + WMt
3.19 The kinetic energy of a rigid body B, expressed as a quadratic form of the twist tA , referred to an
arbitrary point A, takes the form below, as derived from eq.(3.145):
T =
1 T
t MA tA
2 A
with tA defined as
tA =
43
ω
ȧ
(51)
(52)
with ȧ denoting the velocity of A in an inertial frame; now the question is how to define MA . This
task is eased if the twist-transfer formula of eqs.(3.83a & b) is recalled:
1
O
tC = UtA , U ≡
(53)
A−C 1
Upon substitution of the expression for tC as given above into eq.(3.145), the kinetic energy takes the
form
1
(54)
T = tTA UT MUtA
2
whence MA is the matrix associated with the foregoing quadratic form in tA , namely,
MA = UT MU
Upon expansion,
IC + m(A − C)(A − C)T
MA =
m(A − C)
m(A − C)T
m1
≡
IC + m(C − A)(C − A)T
m(C − A)T
m(C − A)
m1
(55)
thereby deriving eq.(3.148), as readily proven. Indeed, the foregoing expression can be decomposed
into
IC O
(C − A)(C − A)T (C − A)
MA =
+m
O m1
(C − A)T
1
But the matrix in the second term can be readily factored into
(C − A)(C − A)T C − A
C−A C−A
(C − A)T
=
T
(C − A)
1
O
1
(C − A)T
|
{z
}|
{z
P
PT
O
1
}
which thus shows that MA is the sum of two terms, the first being positive-definite (p.d.), for its two
diagonal blocks are so, the second being at least positive-semidefinite (p.s.d.). Infact, P is singular, of
rank 5, its null space being spanned by the 6-dimensional vector [ (c − a)T , 0T ]T , with 0 standing for
the 3-dimensional zero vector. Therefore, MA is the sum of one p.d. and one p.s.d. terms, the result
then being p.d., q.e.d.
4
Geometry of Decoupled Serial Robots
4.2 Since we can place Z1 anywhere, as long as it is parallel to the direction of the prismatic joint, we
define it as intersecting Z2 , so that a1 = 0. Thus, F2 moves under pure translation with respect to F1 .
Moreover, X2 is then defined using the “right-hand” rule. Furthermore, in order to ease the algebra,
we shall define X1 so that X2 moves parallel to it, which yields θ1 = 0, and hence, cos θ1 = 1 and
sin θ1 = 0. Thus,
 




0
xC
1 0
0
[ a1 ]1 =  0  , [ c ]1 =  yC  , Q1 =  0 λ1 −µ1 
zC
0 µ1 λ1
b1
with vectors a2 , a3 , and a4 , as well as matrices Q2 and Q3 remaining as in the all-revolute case. Note
that, in our case, b1 = b1 (t). Moreover, we recall eq.(4.17):
Q2 (b2 + Q3 b3 + b4 u3 ) = QT1 c − b1
(56)
Such as in the all-revolute case, we first note that (i) the left-hand side of eq.(56) appears multiplied by
Q2 ; and (ii) θ2 does not appear in the right-hand side. This implies that (i) if the Euclidean norms of
44
the two sides of that equation are equated, the resulting equation will not contain θ2 ; and (ii) the third
scalar equation of the same eq.(56) is independent of θ2 , by virtue of the structure of the Qi matrices.
Thus, we have two equations free of θ2 , which allows us to calculate the two remaining unknowns b1
and θ3 .
Let the Euclidean norm of the left-hand side of eq.(56) be denoted by l, that of its right-hand side by
r. We then have
l2 ≡ a22 + b22 + a23 + b23 + b24 + 2bT2 a3 + 2b4 bT2 u3 + 2b3 b4 λ3
r2 ≡ kck2 + b21 − 2b1 zC
where
bT2 u3 = a2 µ3 s3 − b2 µ2 µ3 c3 + b2 λ2 λ3
and
bT2 a3 = a2 a3 c3 + b2 µ2 a3 s3 + b2 λ2 b3
Upon equating l2 with r2 , we obtain
A′ c3 + B ′ s3 − b21 + 2zC b1 + C ′ = 0
(57)
whose coefficients do not contain any unknown, i.e.,
A′ = 2(a2 a3 − b2 µ2 µ3 b4 )
B ′ = 2(b2 µ2 a3 + a2 µ3 b4 )
C ′ = a22 + b22 + a23 + b23 + b24 + 2b2 λ2 b3 + 2b2 λ2 λ3 b4 + 2b3 λ3 b4 − kck2
Moreover, the third scalar equation of eq.(56) is similar to that of the all-revolute case, except that
now, we have c1 = 1 and s1 = 0, which thus yields
F + Hc3 + Is3 + J = 0
where
F + J = λ1 b1 + J ′′
with J ′′ defined as
J ′′ ≡ yC µ1 + b2 + λ2 b3 + λ2 λ3 b4 − zC λ1
and hence, the above equation becomes
Hc3 + Is3 + λ1 b1 + J ′′ = 0
(58)
Now we can solve for c3 and s3 from eqs.(57) and (58):
1
c3
Mv
=
s3
∆1
where matrix M and vector v are defined as
I
−B ′
M≡
,
−H A′
and the scalar ∆1 is given by
v≡
b21 − 2zC b1 − C ′
−λ1 b1 − J ′′
(59)
∆1 ≡ det(M) = A′ I − B ′ H
If we now take the Euclidean norm of the two sides of eq(59), we obtain an equation free of θ3 , namely,
vT MT Mv = ∆21
45
(60)
Upon performing the foregoing matrix product, we have
M11 M12
MT M ≡
M12 M22
whose entries are given below:
M11 ≡ (b24 µ23 + a23 )µ22
M22 ≡ 4[a22 (a23 + µ23 b24 ) + b22 µ22 (µ23 b24 + a23 )]
M12 ≡ 2b2 µ22 (a23 + µ23 b24 )
Upon expansion, eq.(60) leads to a quartic equation in b1 , namely,
Γ4 b41 + Γ3 b31 + Γ2 b21 + Γ1 b1 + Γ0 = 0
(61a)
with coefficients { Γi }40 defined below:
2
2
Γ0 ≡ C ′ M11 + 2C ′ J ′′ M12 + J ′′ M22 − (A′ I − B ′ H)2
Γ1 ≡ 4zC C ′ M11 + 2(C ′ λ1 + 2zC J ′′ )M12 + 2J ′′ λ1 M22
2
Γ2 ≡ (4zC
− 2C ′ )M11 − 2(J ′′ − 2zC λ1 )M12 + λ21 M22
Γ3 ≡ −4zC M11 − 2λ1 M12
Γ4 ≡ M11
(61b)
(61c)
(61d)
(61e)
(61f)
With b1 known, we calculate θ3 from eqs.(59). Once b1 and θ3 are known, all quantities in eq.(56) are
known, except for θ2 , appearing in Q2 . The calculation of θ2 from this equation is straightforward.
Indeed, the said equation can be rewritten in the form
Q2 u = w
(62a)
with u and w defined as
u ≡ b2 + Q 3 b3 + b 4 u3 ,
w ≡ QT1 c − b1
(62b)
We now note that the third scalar equation of eq.(62a) does not contain θ2 , and hence, does not help
us in our calculation of this unknown. We thus retain only the first two of those three equations, which
are displayed below:
 
u1
c2 −λ2 s2 µ2 s2  
w1
u2 =
(63)
s2 λ2 c2 −µ2 c2
w2
u3
and then recast them in a more convenient form:
u1
−(u2 λ2 − u3 µ2 )
c2
w1
=
u2 λ2 − u3 µ2
u1
s2
w2
whence,
with ∆2 defined as
c2
s2
=
1
∆2
u1
u2 λ2 − u3 µ2
−(u2 λ2 − u3 µ2 )
u1
∆2 = u1 u2 + (u2 λ2 − u3 µ2 )2
w1
w2
(64)
(65)
(66)
In summary, then, the problem admits up to four solutions for b1 , with one unique solution θ3 for each
of these four values of b1 , and one unique solution θ2 for each pair of values (b1 , θ3 ). Therefore, a point
C of the three-dimensional space can be reached with the tip of the end link of this manipulator, such
as in the all-revolute case, with up to four manipulator postures.
46
Figure 10: Six-revolute orthogonal manipulator
4.4 Manipulator is of an architecture similar to that of Fig. 4.12, but with some special features in its DH
parameters. So, we analyze the manipulator independent from eqs.(4.31 & 32). We define the DH
frames as shown in Fig. 10.
Thus,
a1 = a3 = b1 = b2 = b3 = 0,
a2 = b 4 = a = 1 m
Below we work with a, rather than with its numerical value. Moreover,
α1 = α3 = 90◦ ,
α2 = 0
and hence,
λ1 = λ3 = 0,
λ2 = 1,
µ1 = µ3 = 1,
µ2 = 0
We thus have
A = B = C = 0,
E=
a22
+
b24
D = 2a2 b4 = 2a2 ,
2
− kck = 2a2 − kck2
Moreover,
F = yC ,
G = −xC ,
H=I =J =0
Now, eq.(4.19a) becomes
2a2 s3 + 2a2 − kck2 = 0
⇒ θ3 = arcsin
kck2 − 2a2
2a2
thus obtaining two supplementary values (they add up to 180◦ ) of θ3 . Likewise, eq.(4.20a) becomes
yC
yC c1 − xC s1 = 0 ⇒ θ1 = arctan
xC
47
Given that arctan is a two-valued function, we thereby obtain two distinct values for θ1 differing in
180◦. Now we turn to the calculation of θ2 , which we do from eqs.(4.28a & b). Thus,
A11 = a + a sin θ3 = a(1 + sin θ3 )
A12 = a cos θ3
Therefore,
1 + sin θ3
a
− cos θ3
whence,
cos θ2
sin θ2
1
=
a∆2
cos θ3
1 + sin θ3
1 + sin θ3
cos θ3
cos θ2
sin θ2
xC cos θ1 + yC sin θ1
=
zC
− cos θ3
1 + sin θ3
xC cos θ1 + yC sin θ1
zC
with
∆2 = (1 + sin θ3 )2 + cos2 θ3 = 2(1 + sin θ3 )
We thus have two solutions for θ1 and two for θ3 , each of the latter producing one unique value of θ2 .
The arm thus can reach a point of coordinates (xC , yC , zC ) in up to four distinct postures.
Now we solve the orientation problem. To this end, notice that
α4 = α5 = 90◦ ,
α6 = 0◦
and hence,
λ4 = λ5 = 0,
λ6 = 1,
µ4 = µ5 = 1,
µ6 = 0
The quadratic equation in τ4 , eq.(4.38), thus becomes
−ητ42 − 2ξτ4 + η = 0 or ητ42 + 2ξτ4 − η = 0
whence,
τ4 =
−ξ ±
p
p
−ξ ± 1 − ζ 2
ξ 2 + η2
=
η
η
Furthermore, the two equations for θ5 are obtained by equating the first two components of the third
columns of eq.(4.45), i.e.,
sin θ5 = r13 cos θ4 + r23 sin θ4
cos θ5 = −r33
thus obtaining one single value of θ5 for each of the two values of θ4 . Finally, the equations yielding
θ6 reduce to
cos θ6 = (r11 cos θ4 + r21 sin θ4 ) cos θ5 + r31 sin θ5
sin θ6 = r11 sin θ4 − r21 cos θ4
thereby completing the inverse kinematics of the given manipulator. Note that any EE orientation can
be reached with up to two distinct wrist postures. In summary, then, the manipulator can produce an
arbitrary pose of its EE with up to eight distinct postures.
4.6 The rotation matrices Qi introduced in Section 4.2 are given by


cos θi −λi sin θi µi sin θi
Qi =  sin θi λi cos θi −µi cos θi 
0
µi
λi
48
We thus have
but,




µ + µi cos θi
µi (1 + cos θi )
1 i
1
 =  µi sin θi 
qi ≡ vect(Qi ) =
µi sin θi
2
2
sin θi + λi sin θi
sin θi (1 + λi )
qi ≡ sin φi ei
and thus
1 2
[µ (1 + cos θi )2 + µ2i sin θi2 + (1 + λi )2 sin θi2 ]
4 i
q
1
=
µ2i (1 + 2 cos θi + cos θi2 + sin θi2 ) + (1 + λi )2 sin θi2
2
q
1
=
2µ2i (1 + cos θi ) + (1 + λi )2 sin θi2
2
sin φi = kqi k =
Moreover,
r
(67)
tr(Qi ) = cos θi + λi cos θi + λi = cos θi (1 + λi ) + λi
and
cos θi (1 + λi ) + λi − 1
tr(Qi ) − 1
=
(68)
2
2
Using eqs.(67) and (68), the angle of rotation φi is uniquely defined considering our convention, where
0 ≤ φi ≤ π. Finally, the unit vector ei parallel to the axis of rotation is simply given by
cos φi =
ei =
1
qi ,
sin φi
sin φi 6= 0
4.7 For the orientation problem, eq.(4.40) gives two solutions for θ4 . These two solutions merge into a
single one when the radical vanishes, and then correspond to a singular posture. We want the solutions
that lie the farthest apart. Obviously, these solutions will occur when the radical attains its maximum
value. This radical is given by eq.(4.41b), namely,
f (ξ, η, ζ) ≡ (ξ 2 + η 2 )µ24 − (λ5 − ζλ4 )2
(69)
For the given manipulator, we have
α4 = α5 = 90◦
and thus µ4 = sin 90◦ = 1, λ4 = cos 90◦ = 0, λ5 = cos 90◦ = 0. Therefore, eq.(69) reduces to
f (ξ, η, ζ) = ξ 2 + η 2 = 1 − ζ 2
whose maximum value is thus attained when ζ = 0, or ξ 2 + η 2 = 1.
Moreover, we can attach a frame F7 to the EE so that X7 and Z7 coincide with Z4 and Z6 , respectively.
From eq.(4.35) we have
T
[e6 ]4 = [e7 ]4 = [ ξ η ζ ]
In this problem, we have ζ = 0 and, if i7 , j7 and k7 are unit vectors parallel to the X7 , Y7 and Z7 axes,
we have
T
T
[i7 ]4 = [ 0 0 1 ] , and [k7 ]4 = [ ξ η 0 ]
We also have
[j7 ]4 = [k7 × i7 ]4 = [ −η
49
ξ
0]
T
Using Definition 2.2.1, we obtain

0
[R]4 =  0
1

ξ
η
0
−η
ξ
0
where R is defined in eq.(4.42b) and represents the rotation matrix that brings axes X4 , Y4 , and Z4
into an orientation coincident with axes X7 , Y7 , and Z7 , and is the solution sought. Moreover, ξ and η
are related by ξ 2 + η 2 = 1 when the solutions lie the farthest apart. Notice that, when the EE attains
the orientation given by R, the three wrist axes are mutually orthogonal, the wrist Jacobian thus being
orthogonal, and hence, isotropic in the sense of Section 5.8.
4.8 Three lines are given in terms of their Plücker arrays:
ei
pLi =
, i = 1, 2, 3
ni
The fourth line has the Plücker array
pL =
e
n
From Exercise 3.3, we have that the moment of line L about a second line Li vanishes if the two lines
intersect. This condition can be written as
µ = ni · e = 0
where ni is the moment of line Li with respect to a point P on line L. Therefore, from this equation,
we have
(pi − p) × ei · e = 0
(70)
In this problem, we want to determine the line L that intersects the three axes of the manipulator of
Fig. 4.15. From Example 4.4.2, we know that this manipulator is displayed in Fig. 4.15 in a singular
posture, and thus point C is known to be a point of L. All we need now is determine the direction of
L, given by the unit vector e. Moreover, from Fig. 4.16, the quantities below are readily known in F1 :
 






0
−a
−a
0
p1 =  0  , p2 =  0  , p3 =  a  , p = c =  2a 
0
0
−a
−a
 
 
 
0
0
1
e1 =  0  , e2 =  1  , e3 =  0 
1
0
0
Thus,


−2a
(p1 − p) × e1 =  0  ,
0


−a
(p2 − p) × e2 =  0  ,
−a
 
0
(p3 − p) × e3 =  0 
a
Equation (70) thus leads to a set of three equations with three unknowns, namely, the three components
of e, e1 , e2 , and e3 . Therefore,
−2ae1 = 0
−ae1 − ae3 = 0
ae3 = 0
from which, clearly,
e1 = e3 = 0
50
and thus, the only nontrivial solution is
e = [0
±1 0 ]
T
Therefore, the line passing through the operation point and intersecting the three axes of the manipulator is determined by the point P and the unit vector e, namely,


 
0
0
p =  2a  , e =  1 
−a
0
which means that the line L sought is parallel to L2 and is the common perpendicular of L1 and L3 .
Since parallel lines intersect at infinity, line L intersects all three lines { Li }31 . The Plücker coordinates
of L are readily found to be [0, 1, 0, a, 0, 0]T .
4.9 (a) The kinematic chain of the Fanuc Arc Mate is illustrated in Fig. 11 with the DH parameters
summarized in Table. 1.
E
D
E
D
D
E
2 ϣ
2 ϣ 2 ϣ
D
D
2 ϣ E
2 ϣ
E
D
Figure 11: Kinematic chain of the Fanuc Arc Mate 120iB
Table 1: DH parameters of the Fanuc Arc Mate 120iB
i
1
2
3
4
5
6
ai (mm)
150
770
100
0
0
100
bi (mm)
525
0
0
740
0
200
αi (◦ )
90
0
90
90
90
0
(b) see Maple worksheet in Appendix 1.
4.10 The kinematic chain of the Motoman-EA1400N welding robot is illustrated in Fig. 12 with the DH
parameters summarized in Table. 2.
51
X4
X3
Z3
a3
O3′
Z1
a2
O1′
a1
a4 = 0
X5
Z2
O2′
O4′ , O5′
P
O6′
X2
X1
a5
Z5
Z4
Z6 , Z7
a6 = 0
X7
X6
Figure 12: Kinematic chain of the Motoman-EA1400N
Table 2: DH parameters of the Motoman-EA1400N
i
1
2
3
4
5
6
5
ai (mm)
150
570
200
0
30
0
bi (mm)
450
0
0
640
0
-200
αi (◦ )
90
0
90
90
90
0
Kinetostatics of Serial Robots
5.1 This solution needs an in-depth revision. The Jacobian matrix will be obtained using the algorithm of Section 5.3. First, we note
λi = cos αi ,
µi = sin αi ,
ci = cos θi ,
si = sin θi
Then, using the DH parameters of Table 5.3, we have
λ1 = λ2 = λ3 = λ4 = λ5 = λ6 = 0
µ1 = µ3 = µ5 = 1,
µ2 = µ4 = µ6 = −1
c1 = c2 = c3 = c4 = c5 = c6 = 0
s1 = s3 = s5 = 1,
Therefore, using eq.(4.1e) we obtain

0 0
Q1 = Q3 = Q5 =  1 0
0 1

1
0,
0
52
s2 = s4 = s6 = −1


0
0 1
Q2 = Q4 = Q6 =  −1 0 0 
0 −1 0
and, from eq.(4.3b),


0
[ a1 ]1 = [ a3 ]3 = [ a5 ]5 =  50  ,
50


0
[ a2 ]2 = [ a4 ]4 =  −50  ,
50


0
[ a6 ]6 =  0 
50
i) Evaluation of submatrix A:
[ e 1 ]1 = [ 0 0
1 ]T
T
P1 = Q1 ,

Therefore,
i.e., [ e2 ]1 = [ 1 0 0 ]

 
0 −1 0
0
P2 = P1 Q2 =  0
0 1  , i.e., [ e3 ]1 =  1 
−1 0 0
0




−1 0 0
0
P3 = P2 Q3 =  0 1 0  , i.e., [ e4 ]1 =  0 
0 0 −1
−1




0 0 −1
−1
P4 = P3 Q4 =  −1 0 0  , i.e., [ e5 ]1 =  0 
0 1 0
0




0 −1 0
0
P5 = P4 Q5 =  0 0 −1  , i.e., [ e6 ]1 =  −1 
1 0
0
0

0 1
[ A ]1 =  0 0
1 0
ii) Evaluation of submatrix B:
0 0 −1
1 0
0
0 −1 0
We have
[ r6 ]6 = [ a6 ]6 = [ 0

0
−1 
0
0 50 ]T
and
[ ri ]i = [ ai ]i + Qi [ ri+1 ]i+1 ,
Thus,


50
[ r5 ]5 =  50  ,
50
Therefore,
i = 5, 4, 3, 2, 1




50
0
[ r4 ]4 =  −100  , [ r3 ]3 =  100  ,
0
−50




−50
−50
[ r2 ]2 =  −50  , [ r1 ]1 =  0 
−50
0
[ e1 × r1 ]1 = [ 0 −50 0 ]T
and
[ ei × ri ]1 = Pi−1 [ ei × ri ]i ,
Thus,


0
[ e2 × r2 ]1 =  50  ,
−50

i = 2, · · · , 6

0
[ e3 × r3 ]1 =  0  ,
100
53


−100
[ e4 × r4 ]1 =  50  ,
0


0
[ e5 × r5 ]1 =  50  ,
50
Hence,
 
0
[ e6 × r6 ]1 =  0 
0


0
0
0 −100 0 0
[ B ]1 =  −50 50
0
50
50 0 
0
−50 100
0
50 0
Finally


0
1
0
0
−1 0
0
1
0
0 −1 
 0


0
0
−1
0
0 
 1
[ J ]1 = 

0
0 −100 0
0 
 0


−50 50
0
50
50
0
0
−50 100
0
50
0
5.2 We have here a decoupled manipulator, and thus,
JT12 nw = ∆τ w
JT21 f = ∆τ a − JT11 nw
(71a)
(71b)
with matrices J11 , J12 and J21 given by
J11 = [ e1
e2
e3 ] ,
J12 = [ e4
e5
e6 ] ,
J21 = [ e1 × r1
e2 × r2
e3 × r3 ]
where ri is defined as the vector directed from Oi to C. Referring to Fig. 13, and expressing all
quantities in Frame 1, we have
 
 
 
 
 
0
0
0
0
1
e1 =  0  , e2 = e3 =  1  , e4 =  0  , e5 =  1  , e6 =  0 
1
0
1
0
0
 
 
1
0
r1 = r2 =  0  , r3 =  0 
1
1
Thus,
Therefore,
 
0
e1 × r1 =  1  ,
0
Hence,
0
= 0
1
J11


0 0
1 1,
0 0
J−1
12
From eq.(71a), we have


1
e2 × r2 =  0  ,
−1
J12

0 0
= 0 1
1 0
nw = JT12
−1


0 1
1 0,
0 0

1
0,
0

0
= 0
1
∆τ w
J−1
21

0
= 0
1
54
0
1
0
 
1
e3 × r3 =  0 
0
J21
0 1
= 0 0
1 0

0
= 1
0

1 1
0 0
−1 0

0
−1 
1
   
1
0
0
01 = 1
0
0
0
Figure 13:
and, from eq.(71b),
f = JT21
−1 ∆τ a − JT11 nw
which yields

0
f = 1
0

0
= 1
0
   
0 1
0
0 0
0 0   2  −  0 1
−1 1
1
0 1
  
0
1
0   1 
0
0
  

0 1
0
0
0 01 =  0 
−1 1
0
−1
This last expression represents the weight of the tool, which is directed in the negative Z1 direction,
as expected. Hence,
w=1N
Now, nw is the resultant moment acting on the EE when the load f is applied at the center C of the
wrist. Therefore, defining
T
r = [ −d 0 0 ]
we have
nw = f × r = [ 0
d 0]
T
Comparing this expression with eq.(5), we finally obtain
d=1m
5.3 We show here two approaches, the first being geometric, and is what we recommend; the second is
included for verification purposes, and is based on the formulas developed in the book.
Within the geometric approach, we note that the manipulator is at the posture sketched in Fig. 14,
from which
0√
r1 =
,
1+ 2
√
− √2/2
r2 =
,
1 + 2/2
55
√
−√ 2/2
r3 =
2/2
Figure 14: Manipulator at posture θ1 = θ2 = θ3 = 45◦
Hence,
−1 −
Er1 =
0
Therefore,
√ 2

1√
J =  −1 − 2
0
,
√
−1 −
2/2
√
,
Er2 =
− 2/2

1√
√1

−1 −
√ 2/2 −√2/2 ,
− 2/2
− 2/2
√
−√2/2
Er3 =
− 2/2
√ 
−√2
τ =  − √2 
1− 2

If we let the wrench acting at the EE be w = [ n, fx , fy ]T , then the torque-wrench relation JT w = τ
leads to
√
   √ 

1 −1 −√ 2
n
−√2
√0
 1 −1 − 2/2 − 2/2   fx  =  − 2 
√
√
√
1
− 2/2
− 2/2
fy
1− 2
We now obtain a reduced system of two equations with two unknowns upon subtracting the first of
the above three scalar equations from the second and the third, which thus yields
√

   √ 
1 −1
n
− 2
√− 2
√0
0
  fx  =  0 
2/2
−
2/2
√
√
0 1 + 2/2 − 2/2
fy
1
The 2 × 2 system is, then,
√
√
2/2
−√2/2
fx
0
√
=
1 + 2/2 − 2/2
fy
1
The determinant ∆ of the foregoing matrix turns out to be
√
2
1
∆=
= √
2
2
56
and hence,
fx
fy
whence,
=
√
2
√
√
√
√
− 2/2
0
2/2
1
√
√2/2
= 2 √
=
N
−1 − 2/2
2/2
1
2/2
1
√
√
√
√
n = − 2 + (1 + 2)fx = − 2 + 1 + 2 = 1 Nm
In order to verify the foregoing results, we resort to eq.(5.72). To use this equation, the quantities
below must be evaluated:
√
c123
−√ 2/2
s3 =
=
2/2
s123
√
√
c12 + c123
0 − √2/2
− √2/2
s2 =
=
=
s12 + s123
1 + 2/2
1 + 2/2
√
√
2/2 + 0 − √2/2
0√
c1 + c12 + c123
√
=
s1 =
=
2/2 + 1 + 2/2
1+ 2
s1 + s12 + s123
Furthermore, using eq.(5.64) we have
∆≡−
√
− 22
√
− 22
0
1
−1
0
"
√
− 22
√
−1 − 22
#
=
√
2
2
which is rightfully identical to the value obtained above. Therefore, from eq.(5.72),
1
[(τ2 − τ1 )(s3 − s1 ) − (τ3 − τ1 )(s2 − s1 )]
∆ √
2
−√2/2
1
0(s3 − s1 ) −
= √
=
N
− 2/2
1
2
f=
Finally,
√
√
0 −1
1
n = 1 Nm
n = τ1 + sT1 Ef = − 2 + [ 0 1 + 2 ]
1 0
1
thereby confirming the correctness of the results.
5.4 We have
J12 θ̇ = ω
Therefore,
T
kωk2 = ω T ω = θ̇ JT12 J12 θ̇
Matrix J12 is given by
J12 = [ e4
We have
Hence
 
0
[e4 ]4 =  0  ,
1
e5
e6 ]

sin θ4
[e5 ]4 =  − cos θ4  ,
0
J12


0
= 0
1
57
sin θ4
− cos θ4
0

ξ
η
0
 
ξ
[e6 ]4 =  η 
0
Therefore
JT12 J12

0
=  sin θ4
ξ
0
− cos θ4
η

1
0
sin θ4
0   0 − cos θ4
0
1
0
 
ξ
1
η = 0
0
0
0
1
ξ sin θ4 − η cos θ4
However, since α5 = 90◦ , we must have e5 ⊥ e6 , or eT5 e6 = 0. Therefore,

0
ξ sin θ4 − η cos θ4 
1
[eT5 e6 ]4 = ξ sin θ4 − η cos θ4 = 0
Hence, JT12 J12 = 1 and kθ̇k remains constant when kωk does.
5.5 In this problem, we have a three-axis manipulator where we consider only the positioning problem.
The velocity of point C is given by
ċ = θ̇1 e1 × r1 + θ̇2 e2 × r2 + θ̇3 e3 × r3 = v
or
v = Jθ̇
(72)
where
J = [ e1 × r1
e2 × r2
e3 × r3 ]
For this problem, all quantities will be expressed in frame 1. Therefore,
 
 
 
0
0
1
e1 =  0  , e2 =  1  , e3 =  0 
1
0
0
 
 
 
0
a
a
r1 =  a  , r2 =  a  , r3 =  0 
0
0
a






−a
0
0
e1 × r1 =  0  , e2 × r2 =  0  , e3 × r3 =  −a 
0
−a
0
Thus,

−a
J= 0
0



0
0
−1 0
0
0 −a  = a  0
0 −1  ≡ aQ
−a 0
0 −1 0
where Q is apparently an orthogonal matrix. Now, from eq.(72), we have
θ̇ = J−1 v =
Therefore,
T
kθ̇k2 = θ̇ θ̇ =
Hence,
kθ̇k2 =
1 T
Q v
a
T
1 T
v QT
QT v
2
a
1 T
1
v v = 2 kvk2
a2
a
Therefore, if kvk remains constant, so does kθ̇k.
On the other hand, upon differentiating both sides of eq.(72) with respect to time, we obtain
Jθ̈ + J̇θ̇ = v̇ = a
58
or
1
θ̈ = J−1 a − J̇θ̇ = QT (a − J̇θ̇)
a
whence, it is apparent that kθ̈k remains constant under constant a if and only if J̇θ̇ is a multiple of a,
say J̇θ̇ = αa, for a real α. Under these conditions,
θ̈ =
and hence,
2
kθ̈k =
1−α
a
2
1
(1 − α)QT a
a
kak
2
1 − α
kak
or kθ̈k = a which makes it apparent that, under the above condition, kθ̈k remains constant for constant kak. Note
that the same condition implies that
J̇J−1 v = αa
(73)
and hence, α can be obtained from eq.(73) upon premultiplying eq.(73) by aT , thus obtaining
α=
aT J̇J−1 v
aT J̇θ̇
J̇θ̇
=
= eT
2
2
kak
kak
kak
where e is a unit vector in the direction of the acceleration, i.e., e ≡ a/kak. The geometric interpretation of the above result is, then, that α is the projection onto the acceleration a of the quadratic
component J̇θ̇ normalized with respect to kak. When this component is normal to a, then the said projection vanishes and the only possibility for kθ̈k to remain constant under constant kvk and constant
kak is J̇θ̇ = 0.
5.6 The load f of the joint and torque τ are related by
τ = JT f
Therefore,
kτ k2 = τ T τ = f T JJT f
From Exercise 5.5, we have
and thus
Hence


−1 0
0
J = a 0
0 −1 
0 −1 0

1 0
JJT = a2  0 1
0 0

0
0
1
kτ k2 = a2 f T f = a2 kf k2
Therefore, as long as kf k remains constant, kτ k also remains constant.
5.7 We have here a decoupled manipulator, whose Jacobian matrix is given by
J11 J12
J=
J21 O
59
Figure 15:
(a) In this case, the manipulator has one prismatic joint, its subjacobian matrices J11 , J12 and J21
being given by
J11 = [ e1
e2
0],
J12 = [ e4
e5
e6 ] ,
J21 = [ e1 × r1
e2 × r2
e3 ]
Referring to Fig. 15, and expressing all quantities in frame 1, we have
Therefore,
 
 
 
 
 
0
1
0
1
0
e1 =  0  , e2 =  0  , e3 = e4 =  1  , e5 =  0  , e6 =  0 
1
0
0
0
1
 


 
0
−b3
0
r1 = r2 =  b3  , e1 × r1 =  0  , e2 × r2 =  0 
0
0
b3
J11
Hence,

0
= 0
1

1 0
0 0,
0 0
J12

0
= 1
0

0
 0

 1
J=
 −b3

0
0

1 0
0 0,
0 1
1
0
0
0
0
b3
0
0
0
0
1
0
(b) The twist of the center C of the wrist is given by
tC = Jθ̇
60
0
1
0
0
0
0
J21
1
0
0
0
0
0

−b3
= 0
0
0
0
b3

0
1
0

0
0

1

0

0
0
(74)
For unit values of all joint rates, we have

0
1
0
 0

0
 1
tC = 
 −b3 0

0
0
0
b3
0
0
0
0
1
0
0
1
0
0
0
0
1
0
0
0
0
0
  

0
1
2
01  1 
  

11  2 
  = 

0   1   −b3 
  

0
1
1
0
1
b3
The twist of the EE P , tP , can be obtained using the twist-transfer formula given by eqs.(3.84)
and (3.85), as
1
O
tP =
t
C−P 1 C
where C and P are the cross-product matrices of the position vectors c and p, respectively. Here,
Thus
and

0
C= 0
−b3
T
c = [ 0 b3
0]
p = [ 0 b3
0.5 ]T

0 b3
0 0 ,
0 0

0
P =  0.5
−b3

0
0.5 0
C − P =  −0.5 0 0 
0
0 0
Finally, we obtain


−0.5 b3
0
0
0
0
tP = [ 2
1 2
(0.5 − b3 )
0 b3 ]
T
(c) We want θ̈ such that c̈ = 0 and ω̇ = 0 when
θ̇ = [ 1 1
1 1
1
1 ]T
Differentiating eq.(74), we have
ṫC = Jθ̈ + J̇θ̇
or
where
θ̈ = J−1 ṫC − J̇θ̇
(75)
ṫC = 0
Thus, eq.(75) reduces to
θ̈ = −J−1 J̇θ̇
According to eq.(4.94), and considering that we have a prismatic joint, J̇θ̇ is given by
0
ė
0
ė4
ė5
ė6
J̇θ̇ = θ̇1
+ θ̇2 2 + ḃ3
+ θ̇4
+ θ̇5
+ θ̇6
u̇1
u̇2
ė3
u̇4
u̇5
u̇6
where
ėi = ω i−1 × ei ,
i = 2, 3, · · · , 6
and
u̇1 = e1 × ṙ1 ,
61
u̇2 = ė2 × r2 + e2 × ṙ2
(76)
(77)
noting,
r1 = a1 + a2 + a3
ṙ1 = ȧ1 + ȧ2 + ȧ3
= ḃ3 e3 + b3 ė3
= ḃ3 e3 + b3 (ω 2 × e3 )
a1 = 0
a2 = b3 e3
a3 = 0
Moreover,
ṙ1 = ṙ2 = ḃ3 e3 + ω 2 × r2
all other u̇i and ṙi vectors vanishing. Thus, considering unit joint rates, we have
ω 1 = θ̇1 e1 = [ 0
0 1]
T
ω 2 = ω3 = θ̇1 e1 + θ̇2 e2 = [ 1 0
ω4 = [ 1
1
1]
T
ω5 = [ 2
1
1]
T
ω6 = ω = [ 2 1
2]
1 ]T
T
Therefore,
ė2 = [ 0
1 0]
1]
T
1]
T
1 −1 ]
T
ė3 = [ −1 0
ė4 = [ −1 0
ė5 = [ 0
T
−2 0 ]T
ė6 = [ 1
ṙ1 = ṙ2 = [ −b3 1 b3 ]
ṙ3 = ṙ4 = ṙ5 = ṙ6 = 0
T
and
u̇1 = [ −1 −b3
u̇2 = [ 0
−b3
0]
1]
T
T
u̇3 = u̇4 = u̇5 = u̇6 = 0
Using eq.(77) with unit joint rates, we finally obtain
J̇θ̇ = [ 0 0
Let
0 −2 −2b3
qa
q ≡ J̇θ̇ =
qw
where
qa = [ 0 0
T
0] ,
T
qw = [ −2 −2b3
Equation (76) can be written as
Jθ̈ = −J̇θ̇
62
2]
2]
T
or
J11
J21
J12
0
θ̈a
θ̈w
qa
=−
qw
We thus have
J21 θ̈a = −qw
or
and

−1/b3

θ̈ a = −J−1
q
=
−
0
21 w
0

 

0
0
−2
−2/b3
0 1/b3   −2b3  =  −2/b3 
1
0
2
2b3
J11 θ̈a + J12 θ̈w = −qa = 0
or
Finally,

0

θ̈w = −J−1
J
θ̈
=
−
1
12 11 a
0
θ̈ = [ −2/b3

1 0
0 1
0 00 0
0 1
1 0
−2/b3
2b3
0

 

0
−2/b3
0
0   −2/b3  =  2/b3 
0
2b3
2/b3
2/b3
2/b3 ]
T
5.8 (a) For a decoupled manipulator, we have
JT11 nw + JT21 f = τ a
JT12 nw = τ w
where J11 and J21 were obtained in Problem 4.19 as



0 1 0
−b3
J11 =  0 0 0  , J21 =  0
1 0 0
0
(78)
(79)
0
0
b3

0
1
0
Moreover, nw is the resultant moment acting on the EE when f is applied at the center of the
wrist C. Therefore,
nw = (p − c) × f ≡ (P − C)f
where P and C are the cross-product matrices of vectors p and c, respectively. Thus, if we
substitute nw , as given above, into eq.(78), we obtain
JT11 (P − C)f + JT21 f = τ a
or
[JT11 (P − C) + JT21 ]f = τ a
Further, we have the numerical values


0
p−c =  0 
0.5
Hence,

0
JT11 (P − C) =  1
0
0
0
0
⇒


0 −0.5 0
P − C =  0.5
0
0
0
0
0

 

1
0 −0.5 0
0
0
0
0   0.5
0
0  =  0 −0.5 0 
0
0
0
0
0
0
0
63
(80)
Further,
JT11 (P − C) + JT21

 
0
0
0
−b3
=  0 −0.5 0  +  0
0
0
0
0


−b3
0
0
= 0
−0.5 b3 
0
1
0
0
0
1

0
b3 
0
(81)
Upon substitution of the foregoing numerical values into eq.(80), we obtain a system of three
linear equations in the three unknown components of f:

  

−b3
0
0
fx
0
 0
−0.5 b3   fy  =  100 
0
1
0
fz
50
whence,
fx = 0,
fy = 50 N,
fz =
125
N
b3
(b) With f available, nw is readily computed as


 

0 −0.5 0
0
−25
nw = (P − C)f =  0.5
0
0   50  =  0  Nm
0
0
0
125/b3
0
Now we calculate τ w from eq.(79), where we need

0
J12 =  1
0
Thus,
τw

0 1
= 1 0
0 0
J12 . From Problem 4.19,

1 0
0 0
0 1

 

0
−25
0
0   0  =  −25  Nm
1
0
0
Therefore, the readings of the wrist joints are τ4 = 0, τ5 = −25 Nm, and τ6 = 0.
5.9 (a) Since we have here a decoupled manipulator, its Jacobian matrix at point C can be written as
J11 J12
J=
J21 O
with
J11 = [ e1
e2
0],
J12 = [ e4
e5
e6 ] ,
J21 = [ e1 × r1
e2 × r2
e3 ]
where ri is the vector directed from Oi to the center of the wrist, C. With all quantities expressed
in F1 , we have
 










0
0
−1
−1
0
0
e1 =  0  , e2 =  −1  , e3 =  0  , e4 =  0  , e5 =  0  , e6 =  −1 
1
0
0
0
−1
0
and


−(b3 + b4 )

r1 = r2 = 
0
0
64
Thus,


0
e1 × r1 =  −(b3 + b4 )  ,
0
Therefore,
J11

0
= 0
1

0 0
−1 0  ,
0 0
J12
with b34 defined as b3 + b4 , and


0

e2 × r2 = 
0
−(b3 + b4 )


−1 0
0
= 0
0 −1  ,
0 −1 0
J21

0
=  −b34
0
0
0
−b34

−1
0 
0


0
0
0 −1 0
0
0
−1
0
0
0 −1 



1
0
0
0
−1
0 

J=

0
0
−1 0
0
0 



−(b3 + b4 )
0
0
0
0
0
0
−(b3 + b4 ) 0
0
0
0
(b) The twist
decoupled
the center
eqs.(3.84a
tP of the EE at point P is given. However, we found in (a) the Jacobian for the
manipulator with its EE twist defined at the center C of the wrist. Thus, the twist at
of the wrist, tC , is required. This is obtained using the twist-transfer formula given by
& b), as
1
0
tC =
t
P−C 1 P
where C and P are the cross-product matrices of the position vectors c and p, respectively. Here,
c = [ −(b3 + b4 )
p = [ −(b3 + b4 )
0 0]
−b6
T
0]
T
Thus

and
Therefore,

0
0
0
C = 0
0
(b3 + b4 ) 
0 −(b3 + b4 )
0


0
0
−b6
P= 0
0
(b3 + b4 ) 
b6 −(b3 + b4 )
0

0
P−C = 0
b6
0
0
0

−b6
0 
0


  

−b6
1
v − b6 ω

ċ = (P − C)ω + ṗ = ω  0  + v  1  = 
v
b6
1
v + b6 ω
For a decoupled manipulator, we have
J11 θ̇ a + J12 θ̇w = ω
J21 θ̇a = ċ
65
Thus,



−1/(b3 + b4 )
0
v − b6 ω

0
−1/(b3 + b4 )  
v
0
0
v + b6 ω


v
1


=−
v + b6 ω
b3 + b4
(b3 + b4 )(v − b6 ω)
0

θ̇ a = J−1
ċ
=
0
21
−1
and


  
0
−1 0
0
ω
1
 v + b6 ω 
 0
θ̇w = J−1
0 −1   ω  −
12 (ω − J11 θ̇ a ) =
b3 + b4
−v
0 −1 0
ω


ω(b3 + b4 )
−1 

=
v + (b3 + b4 )ω
b3 + b4
−v + (b3 + b4 − b6 )ω

(c) We have, for a decoupled manipulator,
JT11 nw + JT21 f = τ a
JT12 nw = τ w
where nw is the resultant moment acting on the EE when f is applied at the center C of the wrist.
Therefore,


  
  
1
T − b6 F
1
0

T
nw = n + (p − c) × f = T  1  +  −b6  × F  1  = 
1
T + b6 F
1
0
Thus,
τw
and





−1 0
0
T − b6 F
T − b6 F
 = −  T + b6 F 
= JT12 nw =  0
0 −1  
T
0 −1 0
T + b6 F
T
τ a = JT21 f + JT11 nw

  
0 −(b3 + b4 )
0
F
0
= 0
0
−(b3 + b4 )   F  +  0
−1
0
0
F
0


T − (b3 + b4 − b6 )F
=  −T − (b3 + b4 )F 
−F


0 1
T − b6 F

−1 0  
T
0 0
T + b6 F
whose first two components are torques, its third component being, consistently, a force.
5.10 From Fig. 5.3, it is apparent that the maximum reach R of the Puma robot is
q
q
R = b23 + r2 , r = a2 + a23 + b24
which, for the given numerical values, lead to
q
p
R = b23 + r2 = 0.14902 + 0.86452 = 0.8772 m
66
Now we need the maximum reach rM of the manipulator of Fig. 4.17. Although this computation
is not as straightforward as that of the maximum reach of the Puma robot, it is possible to find it
graphically, without resorting to specialized methods of nonlinear-equation solving or optimization, as
explained below5
It is apparent that the maximum reach is independent of θ1 , for singularities do not depend on this
joint variable. So, we lock the first joint and, in the posture of Fig. 4.17, rotate the third joint through
one full turn, point C thus describing a circle C of radius a lying in the Y1 -Z1 plane, with center at
point O3′ , of coordinates (0, a, −a). Next, upon performing a full rotation of the second joint, the
circle describes a toroid of axis Z2 , the problem now reducing to one of finding the point of the surface
of the toroid lying the farthest from the Z1 axis. Figure 16 includes side views of circle C. Now, let
Figure 16: Side views of circle C: (a) and (b) at the position of Fig. 4.15;
and (c) at an arbitrary position for a given value of θ2 .


x2
[ c ]2 ≡  y 2  ,
z2
Therefore, the equation of C in F3 is
C:


x3
[ c ]3 ≡  y 3 
z3
x23 + y32 = a2
(82)
However, it will be more convenient to calculate the distance of a point on the surface of the toroid
from the Z1 axis in frame F2 , for which reason we need the equation of C in F2 . Note that
[ c ]3 = QT2 ([ c ]2 − [ a2 ]2 )
5 This solution is reported in Angeles, J., 2003, “The design of a three-axis positioning manipulator for a given reach,”
Ingenierı́a Mecánica, Vol. 1, No. 2, pp. 35-41.
67
where, either from inspection or from formula (4.3b),


c2
[ a2 ]2 = a  s2 
1
Moreover,

c2
Q 2 =  s2
0
Therefore,

c2
[ c ]3 =  0
s2
s2
0
−c2
0
0
1

s2
−c2 
0

 
  
0
x2 − ac2
(x2 − ac2 )c2 + (y2 − as2 )s2
x3
 =  y3 
1   y2 − as2  = 
z2 − a
0
z2 − a
(x2 − ac2 )s2 − (y2 − as2 )c2
z3
(83)
Hence, the equation of the circle in F2 is
C:
[(x2 − ac2 )c2 + (y2 − as2 )s2 ]2 + (z2 − a)2 = a2
(84a)
Moreover, from inspection of Fig. 4.17, z3 = a, and hence,
(x2 − ac2 )s2 − (y2 − as2 )c2 = a
(84b)
Now, from the shape of the toroid it is apparent that the point P on its surface lying farthest from the
Z1 axis lies on the trace T of the toroid on the X2 -Z2 plane, this trace thus being found upon setting
y2 = 0 in eqs.(84a & b), thus obtaining, correspondingly,
x22 c22 − 2ax2 c2 + z22 − 2az2 + a2 = 0
(85a)
x2 s2 − a = 0
(85b)
and
Now we have the trace T of the toroid on the X2 -Z2 plane as an implicit function of the form f (x2 , z2 ) =
0 in eqs.(85a & b), but in terms a parameter θ2 that should be eliminated from these equations. It
will prove more convenient to work now with nondimensional variables u and v, defined as
u≡
x2
,
a
v≡
z2
a
(86)
In terms of these new variables, eq.(85a) becomes
u2 c22 − 2uc2 + v 2 − 2v + 1 = 0
(87a)
Now, c22 in the above equation can be substituted for
c22 ≡ 1 − s22 ≡ 1 −
a2
1
≡1− 2
x22
u
which thus reduces eq.(87a) to a linear equation in c2 , namely,
u2 + v 2 − 2v − 2uc2 = 0
(87b)
us2 − 1 = 0
(87c)
while eq.(85b) reduces to
68
Upon solving for c2 from eq.(87b) and for s2 from eq.(87c), we have
c2 =
u2 − 2v + v 2
,
2u
s2 =
1
,
u
u 6= 0
(88)
Now, upon squaring each side of the two foregoing expressions and adding up these squares, θ2 is
eliminated, thus obtaining the trace T sought, namely
T:
g(u, v) ≡ (u2 + v 2 )(u2 + v 2 − 4v) − 4(u2 − v 2 − 1)
(89)
The contour T defined by the foregoing implicit function g(u, v) = 0 is displayed in Fig. 17.
Now, the maximum distance rM of O1 to T can be found by direct measurement on the foregoing plot.
A more precise method of obtaining this distance is outlined below:
Our goal here consists in obtaining the locus S of points in the X2 -Z2 plane, as positions of point C
at which the manipulator becomes singular. To this end, we impose the condition that, when point
T
T
Figure 17: Contour of the trace T of the toroid on the u-v plane
C coincides with any point on S, it is possible to draw a line from C that intersects all three joint
axes. With the usual notation, we denote by Pi the point of the axis of the ith joint lying in the X2 -Z2
plane, while pi denotes its position vector. Moreover, we let P be an arbitrary point on S and p its
position vector, while e denotes the unit vector defining the direction of a line L, lying in the X2 -Z2
plane, that we define below. We thus have
 
 


 
0
0
s2
e1
[ e1 ]2 =  1  , [ e2 ]2 =  0  , [ e3 ]2 =  −c2  , [ e ]2 =  0 
0
1
0
e3
69
S
T
T
Figure 18: Contour of the trace T of the toroid and the singularity locus S on the u-v plane


−a
[ p 1 ]2 =  0  ,
0
 
e1
[ e 2 ]2 =  0 
e3
 
0
[ p 2 ]2 =  0  ,
0


ac2
[ p3 ]2 =  as2  ,
a


x2
[ p ]2 =  0 
z2
The condition that line L stemming from P intersects all three joint axes is equivalent to the condition
that the moment of such a line with respect to each of those axes vanish—see Exercise 3.3 —i.e.,
(pi − p) × ei · e = 0,
for i = 1, 2, 3
For completeness, we report below the foregoing cross products:


 


z2
0
(a − z2 )c2
 , (p2 − p) × e2 =  x2  , (p3 − p) × e3 =  (a − z2 )s2 
(p1 − p) × e1 = 
0
−(a + x2 )
0
−a + x2 c2
Therefore, the vanishing of the three moments of L with respect to all three joint axes leads to
z2 e1 − (a + x2 )e3 = 0
0=0
(a − z2 )c2 e1 − (a − x2 c2 )e3 = 0
the second equation indicating that the moment of L with respect to the second joint axis vanishes
identically. The above system of equations is apparently linear homogeneous in the two-dimensional
vector [ e1 , e3 ]T , which is of unit magnitude, and hence, does not vanish. For this nontrivial solution
70
of the above system of equations to exist, its matrix coefficient should be singular, i.e., the determinant
∆ of that matrix should vanish. We thus have
∆ ≡ −z2 (a − x2 c2 ) + (a + x2 )(a − z2 )c2 = 0
whence
z2
x2 − z2 + a
or, in terms of the nondimensional variables u and v,
c2 =
c2 =
v
u−v+1
(90)
Now, upon equating the right-hand sides of eq.(90) and the first of eqs.(88), we obtain the equation of
the singularity locus S:
v
u2 + v 2 − 2v
=
u−v+1
2u
If we now clear denominators in the above equation, we obtain a cubic equation in u and v, namely,
S:
(u2 + v 2 − 2v − 2)(u − v + 1) − 2uv = 0
(91)
which defines the singularity locus S. Shown in Fig. 18 is the superposition of the trace T and
the singularity locus S. The four apparent intersections of these two curves correspond to the four
stationary values of the distance from a point in the trace T to the origin O1 in the u-v plane. Of
these four intersections, two are local maxima and two local minima. The normality of the radius
vector from O1 to T at the intersection points is to be highlighted. Now it is a simple matter to pick
up the point A lying a maximum distance from O1 . By inspection, the coordinates of this point are,
roughly, u ≈ 2.2, v ≈ 1.5. More precise values of the (u, v) coordinates of A are found by resorting
to a nonlinear-equation solver, with up to ten digits, namely,
u = 2.132241882,
v = 1.468989944
which leads to a maximum reach rM given by
p
rM = (2.132241882 + 1)2 + 1.4689899442 a = 3.459605564460a
Alternatively, this problem can be solved exactly at the expense of some extra algebra. From the
second of eqs.(88), we have
1
u=
(92)
s2
Moreover, from eq.(90), we can write
v=
c2 (1 + u)
c2 (1 + s2 )
=
1 + c2
s2 (1 + c2 )
Now, from the first of eqs.(88),
2uc2 = u2 − 2v + v 2
Substituting the expressions for u and v, eqs.(92) and (93), into the above relation, we obtain
2c2
1
c2 (1 + s2 ) c22 (1 + s2 )2
= 2 −2
+
s2
s2
s2 (1 + c2 ) s22 (1 + c2 )2
which can be written as
(2c2 s2 − 1)(1 + c2 )2 = −2c2 s2 (1 + s2 )(1 + c2 ) + c22 (1 + s2 )2
71
(93)
Upon expansion, this equation can be recast in the form
4c2 s2 − 1 + 4c22 s2 + c22 s22 + 2c32 s2 + 2c2 s22 − 2c2 − 2c22 = 0
(94)
Now, introducing two well-known trigonometric identities, namely,
c2 ≡
1 − τ22
,
1 + τ22
s2 ≡
2τ2
,
1 + τ22
where τ2 ≡ tan(
θ2
)
2
into eq.(94), we obtain, after some algebra, an octic equation in τ2 , i.e.,
τ28 + 4τ27 + 4τ26 + 4τ25 + 10τ24 + 12τ23 − 4τ22 − 20τ2 + 5 = 0
(95)
The eight roots of this equation are
(τ2 )1 = −2.798907440, (τ2 )2 = −1.663251939, (τ2 )3,4 = −1 ± j1.111785941,
(τ2 )5,6 = 0.7071067812 ± j1.383551070, (τ2 )7 = 0.798907440, (τ2 )8 = 0.2490383764
√
where j is the imaginary unit, i.e., j ≡ −1. The four real solutions correspond to the four intersecting
points in Fig. 18, namely,
(θ2 )1 = −140.678184◦,
(θ2 )2 = −117.968752◦,
(θ2 )7 = 77.2432◦,
(θ2 )8 = 27.968752◦
Using eqs.(92) and (93), we obtain
(u)1 = −1.578094854, (v)1 = 1.975316257
(u)2 = −1.13224188, (v)2 = 0.1167964943
(u)7 = 1.025308450,
(u)8 = 2.132241882,
(v)7 = 0.3663245289
(v)8 = 1.468989944
which lead to reach values of
(r)1 = 2.058171027a,
(r)2 = 0.1764350771a,
(r)7 = 2.058171027,
(r)8 = 3.459605564a
for a global maximum reach of
rM = 3.459605564a
which is very close of the approximate value found above graphically.
We can now determine the length a of the manipulator of Fig. 4.15 to attain the maximum reach of
the Puma robot, which was found to be R = 0.8772 m. The value sought is thus obtained from
3.459605564a = 0.8772096591
⇒
a = 0.2535577085 m
5.11 The procedure outlined in the problem statement was followed using computer algebra. The characteristic equation, resulting from det(M) = 0, is thus obtained as
− C 2 F 2 − 2 C 2 G2 − 2 A2 H 2 − 2 H 2 B 2 + 2 C 2 GF + 2 CAF H + 4 CGBH
−2 CF BH) cos2 θ3 + 2 A2 + 2 B 2 − 2 D2 G2 − D2 F 2 + 4 IDGB − 2 IDF B
+ 2 IADF + 2 D2 GF sin2 θ3 + (2 ICAF + 4 ICGB + 4 CGF D + 4 DGBH
−2 DF BH + 2 ADF H − 2 ICF B − 4 CG2 D − 4 IHB 2 − 2 CDF 2 − 4 IA2 H cos θ3 sin θ3
+ −4 CG2 E − CBF 2 − 2 A2 F H − 4 A2 HJ − 2 HB 2 F + ACF 2 − 4 HB 2 J − 2 CEF 2
72
+2 CGBF − 2 CF BJ + 2 CAF J + 4 CGBJ + 4 CGF E + 2 AEF H + 4 EGBH
−2 EF BH) cos θ3 + −4 DG2 E − 2 DEF 2 − DBF 2 + ADF 2 − 4 IA2 J − 4 IB 2 J
−2 IEF B − 2 IB 2 F + 2 IAEF + 2 DGBF + 4 DGBJ − 2 DF BJ + 4 DGF E
+2 ADF J +4 IEGB − 2 IA2 F sin θ3 − 2 A2 F J − 2 GF A2 + 2 GF E 2 + AEF 2
+ABF 2 + 2 AEF J − 2 AGBF − 2 EF BJ + 2 EGBF + 4 EGBJ − 2 A2 J 2
+2 G2 A2 − E 2 F 2 − 2 G2 E 2 − 2 B 2 J 2 − EBF 2 − 2 JB 2 F = 0
This equation is obviously quadratic in cos θ3 and sin θ3 , as expected. Moreover, using the identities
1 − t23
2t3
θ3
cos θ3 =
, sin θ3 =
, t3 = tan
1 + t23
1 + t23
2
a quartic equation in t3 is obtained, namely,
a4 t43 + a3 t33 + a2 t23 + a1 t1 + a0 = 0
where
a0 = − 2 A2 F J − 2 GF A2 + 2 GF E 2 + AEF 2 + ABF 2 + 2 AEF J − 2 AGBF
−2 EF BJ + 2 EGBF + 4 EGBJ + 2 C 2 GF − 2 A2 J 2 + 2 G2 A2 − E 2 F 2 − 4 CG2 E
−CBF 2 − 2 C 2 G2 − 2 A2 F H − 4 A2 HJ − 2 HB 2 F + ACF 2 − 4 HB 2 J − 2 CEF 2
−C 2 F 2 − 2 A2 H 2 − 2 H 2 B 2 + 2 CAF H + 4 CGBH − 2 CF BH − 2 G2 E 2 − 2 B 2 J 2
−EBF 2 − 2 JB 2 F + 2 CGBF − 2 CF BJ + 2 CAF J + 4 CGBJ + 4 CGF E
+2 AEF H + 4 EGBH − 2 EF BH
a1 = −8 IA2 J − 4 IA2 F − 4 IEF B + 8 IEGB − 4 IB 2 F − 8 DG2 E − 4 DEF 2 − 2 DBF 2
+ 2 ADF 2 + 4 DGBF + 8 DGBJ − 4 DF BJ + 8 DGF E + 4 ADF J − 8 IB 2 J
−4 ICF B + 4 IACF + 8 ICGB + 4 IAEF + 8 CGF D + 8 DGBH − 4 DF BH
+4 ADF H − 8 IHB 2 − 8 IA2 H − 8 CG2 D − 4 CDF 2
a2 = − 4 A2 F J − 4 GF A2 + 4 GF E 2 + 2 AEF 2 + 2 ABF 2 + 4 AEF J − 4 AGBF − 4 EF BJ
+ 4 EGBF + 8 EGBJ + 8 A2 + 8 IADF + 16 IDGB − 4 C 2 GF − 8 IDF B − 4 A2 J 2
+4 G2 A2 − 2 E 2 F 2 + 8 B 2 + 8 D2 GF − 8 D2 G2 − 4 D2 F 2 + 2 C 2 F 2 + 4 C 2 G2 + 4 A2 H 2
+4 H 2B 2 − 4 CAF H − 8 CGBH + 4 CF BH − 4 G2 E 2 − 4 B 2 J 2 − 2 EBF 2 − 4 JB 2 F
a3 = −8 IA2 J − 4 IA2 F + 8 IA2 H + 8 IHB 2 − 4 IEF B + 8 IEGB − 4 IACF
− 8 ICGB − 4 IB 2 F − 8 DG2 E − 4 DEF 2 − 2 DBF 2 + 2 ADF 2 + 4 DGBF
+ 8 DGBJ − 4 DF BJ + 8 DGF E + 4 ADF J − 8 IB 2 J + 4 ICF B + 4 IAEF
− 8 CGF D − 8 DGBH + 4 DF BH − 4 ADF H + 8 CG2 D + 4 CDF 2
a4 = −2 A2 F J − 2 GF A2 + 2 GF E 2 + AEF 2 + ABF 2 + 2 AEF J − 2 AGBF − 2 EF BJ
+ 2 EGBF + 4 EGBJ + 2 C 2 GF − 2 A2 J 2 + 2 G2 A2 − E 2 F 2 + 4 CG2 E + CBF 2
+ 2 A2 F H + 4 A2 HJ + 2 HB 2 F − ACF 2 + 4 HB 2 J + 2 CEF 2 − C 2 F 2 − 2 C 2 G2
− 2 A2 H 2 − 2 H 2 B 2 + 2 CAF H + 4 CGBH − 2 CF BH − 2 G2 E 2 − 2 B 2 J 2 − EBF 2
− 2 B 2 J 2 − EBF 2 − 2 JB 2 F − 2 CGBF + 2 CF BJ − 2 CAF J − 4 CGBJ − 4 CGF E
− 2 AEF H − 4 EGBH + 2 EF BH
5.12 Apparently, the workspace is generated by the sector of a circle of radius R and angle 2α = 2 sin−1 (r/R)
from which a triangle of height b3 and base 2r has been removed, upon rotating it about its diameter,
as shown schematically in Fig. 19. Now we use the additivity relation of first moments, i.e., the moment
73
Figure 19: Planar region generating the workspace of the Puma manipulator, decomposed into two parts
of a composed figure about an axis equals the algebraic sum of the moments of the individual figures
about the same axis. Hence, for α in radian,
q = R2 α
2b3
2
2
2
2
2R sin α
− rb3
= rR2 − rb23 = r(R2 − b23 ) = r3
3α
3
3
3
3
3
Therefore,
4πr3
3
and
hence,
the
value
of
the
workspace
volume
of
the
Puma
robot is that of a sphere of radius r =
p
R2 − b23 .
V = 2πq =
5.14 We have
J = [ e1
e2
e3 ]
which is a square matrix, and hence,
µ≡
i.e.,
Since
{ei }31
q
det(JJT ) = |det(J)|
µ = |e2 × e3 · e1 | = |e3 × e1 · e2 | = |e1 × e2 · e3 |
are unit vectors, we have
|e2 × e3 · e1 | = ke1 kke2 × e3 k| cos φ| = ke2 × e3 k| cos φ|
where φ is the angle between e1 and e2 × e3 . Furthermore, if α2 denotes the angle between e2 and e3 ,
ke2 × e3 k = ke2 kke3 k| sin(e2 , e3 )| = | sin α2 |
Then,
µ = | sin α2 cos φ|
which attains a maximum value of 1 when both | sin α2 | = 1 and | cos φ| = 1. The foregoing values of
| sin α2 | and | cos φ| correspond to α2 = ±90◦ and φ = 0◦ . Thus, µ attains its maximum when e1 , e2 , e3
are mutually orthogonal, the maximum value of µ being 1.
74
5.15 We have
J = [ e4
where, expressing all vectors in F5 ,
[e4 ]5 = QT4 [e4 ]4 ,
Thus,


0
[e4 ]5 =  µ4  ,
λ4
the Jacobian thus becoming
e5
e6 ]
 
0
[e5 ]5 =  0  ,
1
 
0
[e5 ]5 =  0  ,
1

[e6 ]5 = Q5 [e6 ]6


µ5 s 5
[e6 ]5 =  −µ5 c5 
λ5

0 µ5 s 5
0 −µ5 c5 
1
λ5
0
J ← [J]5 =  µ4
λ4
In the sequel, we will need an expression for J−1 :

µ5 c 5
µ5 s 5
1
 −(µ4 λ5 + µ5 λ4 c5 ) −µ5 λ4 s5
J−1 =
µ4 µ5 s 5
µ4
0
Now, we compute kJk and kJ−1 k, using the weighted Frobenius norm
q
1
kJkF = tr(JWJT ), W ≡
1
3

0
µ4 µ5 s 5 
0
Hence,
r q
r q
1
1
tr(JJT ),
kJ−1 kF =
tr(J−1 J−T )
3
3
and −T denotes the transpose of the inverse or, equivalently, the inverse of the transpose. We thus
have
µ2 + µ25 + (µ4 λ5 c5 + µ5 λ4 )2 + µ24 s25
κ2F = 4
3µ24 µ25 s25
kJkF =
which is, apparently, a function of α4 , α5 and θ5 only.
The above expression can be minimized over α4 , α5 and θ5 upon solving a system of three equations
in three unknowns, derived from
f4 (α4 , α5 , θ5 ) ≡
∂κ2
= 0,
∂α4
f5 (α4 , α5 , θ5 ) ≡
∂κ2
= 0,
∂α5
f6 (α4 , α5 , θ5 ) ≡
∂κ2
=0
∂θ5
A simpler approach follows, based on the hint, which suggests that J can be rendered isotropic.
For isotropy, the three columns of J must be of identical Euclidean norm and mutually orthogonal.
Orthogonality of the second with the first and the third columns readily leads to
π
π
λ4 = λ5 = 0 ⇒ α4 = ± , α5 = ±
2
2
i.e., the wrist must be orthogonal. Furthermore, orthogonality of the first and third rows leads to
c5 = 0
⇒
θ5 = ±
π
2
and hence, isotropy is reached when three unit vectors e4 , e5 and e6 form an orthogonal triad, a result
that should have been expected.
75
5.16 First and foremost, we recall the formulas for the determinant of a block matrix (CRC Standard
Mathematical Tables, 1987) A, given as
B C
A=
D E
The formulas are
det(A) = det(E) det(B − CE−1 D) = det(B) det(E − DB−1 C)
On the other hand, the Jacobian can be written as
J11
J12
J=
J21 O3×3
where O3×3 is the 3 × 3 zero matrix. Hence, applying the second of the above formulas,
J11
J12
det
= det(J11 ) det(O3×3 − J21 J−1
11 J12 )
J21 O3×3
and hence,
det(J) = −det(J21 J12 )
Now, since J21 and J12 are of 3 × 3, we have
µa = |det(J21 )|,
µw = |det(J12 )|
and
µa µw = |det(J21 )| · |det(J12 )| = |det(J21 J12 )| = | − det(J21 J12 )|
Therefore,
µ = |det(J21 )| · |det(J12 )|
and hence, µ = µa µw , q.e.d.
5.17 The Jacobian for a 2R planar manipulator is
−s1 − rs12
J = a1
c1 + rc12
−rs12
rc12
where
s1 = sin θ1 ,
s12 = sin(θ1 + θ2 )
c1 = cos θ1 ,
c12 = cos(θ1 + θ2 )
a2
r=
a1
The inverse of J is, hence,
J−1 =
1
a1 rs2
rc2
−(1 + rc2 )
rs2
−rs2
and the condition number is computed using the Frobenius norm:
√
q
a1 1 + 2r2 + 2rc2
1
T
√
kJkF = tr(JWJ ) =
, W=
1
2
2
Therefore,
−1
kJ
√
q
1 + 2r2 + 2rc2
−1
−T
√
kF = tr(J WJ ) =
2 a1 rs2
76
Then,
1 + 2r2 + 2rc2
2rs2
κF =
We now solve
κF → min
r,θ2
which we do upon zeroing the partial derivatives of κ with respect to r and θ2 , namely,
∂κF
=0
∂r
∂κF
=0
∂θ2
⇒
2r2 − 1 = 0
(96)
⇒
2r + (2r2 + 1)c2 = 0
(97)
√
From eq.(96), r = ± 2/2, where we reject
√which has no geometrical meaning. With
√ the negative sign,
this value of r, eq.(97) leads to c2 = − 2/2 and s2 = ± 2/2. Hence,
(κF )min = 1
√
which is attained with r = 2/2 and θ2 = ±3π/4.
5.18 Shown in Fig. 19 is a cross-section of the workspace of the manipulator at hand, which is a hollow
sphere, its volume VA and reach RA being
√
√
4
56 3
VA = πl3 [( 2 + 1)3 − ( 2 − 1)3 ] =
πl
(98)
3√
3
RA = ( 2 + 1)l
(99)
For a similar orthogonal manipulator, with identical link lengths λ, the corresponding volume VB and
reach RB are
32 3
4
π(2λ3 ) =
πλ
3
3
RB = 2λ
VB =
Now, since the two manipulators have the same reach,
√
√
2+1
2λ = ( 2 + 1)l
⇒
λ=
l
2
⇒
VA
= 0.9949
VB
The Jacobian matrix of the second manipulator takes the form
J = [ e1 × r1
where
and
e2 × r2
e3 × r3 ]




0
s1
e1 =  0  , e2 = e3 =  −c1 
1
0



c23 c1
(c2 + c23 )c1
r1 = λ  (c2 + c23 )s1  , r2 = r1 , r3 = λ  c23 s1  .
s2 + s23
s23

Since θ1 does not affect the condition number of the Jacobian, we can fix θ1 to any value, and so, we
set θ1 = 0, the Jacobian thus becoming


0
−(s2 + s23 ) −s23
0
0 
J = λ  c2 + c23
0
c2 + c23
c23
77
√
√
Figure 20: A cross-section of the workspace of the isotropic manipulator, with ri = ( 2 − 1)l, r0 = ( 2 + 1)l
In order to determine θ2 and θ3 that minimize the condition number κF (J)of the Jacobian, based on
the Frobenius norm, we can use the Matlab built-in function fminsearch. The results reported by
Matlab are
θ2 = −65.3393◦, θ3 = 130.6760◦
Furthermore, the minimum condition number attained at the optimum posture is
κm = 1.0798
Therefore, the KCI of the manipulator at hand is
KCI = 92.6097%
Apparently, this robot is not far from being kinematically isotropic. However, its workspace is only
slightly bigger than that of its isotropic counterpart, for the same reach. The smaller volume of the
isotropic robot is due to the void in its workspace.
6
Trajectory Planning: Pick-and-Place Operations
6.1 (a) The area A of the trapezoidal profile is
A=
1 ′
1
1
τ1 s
+ (τ2 − τ1 )s′max + (1 − τ2 )s′max = (1 − τ1 + τ2 )s′max
2 max
2
2
We need then A = 1, and thus
1
(1 − τ1 + τ2 )s′max = 1
2
from which we obtain
s′max =
78
2
1 − τ1 + τ2
(100)
(b) Using eq.(100), we have

τ2


,
0 ≤ τ ≤ τ1


τ1 (1 − τ1 + τ2 )



 2τ − τ1
,
τ1 ≤ τ ≤ τ2
s(τ ) =
1 − τ1 + τ2





τ 2 + τ22 − 2τ − τ1 (τ2 − 1)


, τ2 ≤ τ ≤ 1

(τ2 − 1)(1 − τ1 + τ2 )
The plot of s(τ ) vs. τ appears in Fig. 21(a). The decomposition of s(τ ) into a linear part and a
(a) Position profile s(tau)
1
s(tau)
0.8
0.6
0.4
0.2
0
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
tau
(b) Periodic part of the position profile
0.8
0.9
1
0.1
0.2
0.3
0.4
0.8
0.9
1
s_p(tau)
0.05
0
−0.05
−0.1
0
0.5
tau
0.6
0.7
Figure 21:
periodic part, is, then,
sl (τ ) = τ,
sp (τ ) = s(τ ) − τ
with sp (τ ) displayed in Fig. 21(b).
(c) For a periodic cubic spline, we have the conditions
s1 = sN
(101a)
s′1
s′′1
(101b)
(101c)
=
=
s′N
s′′N
As explained in Section 6.6, condition (101c) can be used to eliminate one unknown, namely s′′N ,
while condition (101b) leads to an additional equation given by eq.(6.63). Thus, recalling the
definitions of eqs.(6.58d–f), we have now the system
As′′ = 6Cs
79
(102)
where A and C are (N − 1) × (N − 1) matrices defined as:
 2α
α1
2α1,2
α2
..
.
1,N ′



A=



α1
0
..
.
0
αN ′
 −β ′
1,N
 β1

 0
C=
..


.

0
βN ′
0
α2
2α2,3
..
.
0
0
β1
−β1,2
β2
..
.
0
0
and
0
0
α3
..
.
Thus, for i, j, k = 1, . . . , N − 1,








T
s′′ = s′′1 , · · · , s′′N −1
T
∆xk =
αN ′
0
0
..
.
...
αN ′′′ 2αN ′ ,′′ N ′′
αN ′′
′′
0
···
αN
2αN,′′ N ′
0
0
···
βN ′ 
β2
0
···
0


−β2,3
β3
···
0


..
..
..
..

.
.
.
.


···
βN ′′′ −βN ′ ,′′ N ′′
βN ′′
0
···
βN ′′
−βN,′′ N ′
s = [ s1 , · · · , sN −1 ] ,
Moreover,
···
···
···
..
.
1
,
N −1
k = 1, · · · , N − 1
1
,
N −1
βk = N − 1,
αk =
2
N −1
= 2(N − 1)
αi,j =
βi,j
(103)
(104)
and matrices A and C reduce to

4
1

0
1 
.
A=
.
N −1
.
0
1
4
1
..
.
0
1
4
..
.
0
0
1
..
.

··· 1
··· 0

··· 0
.
..
. .. 

4 1
0 ... 1
0 0 ··· 1 4

−2 1
0
0 ··· 1
 1 −2 1
0 ··· 0 


 0
1 −2 1 · · · 0 

C = (N − 1) 
.
.
.
.
.
.
 ..
..
..
..
..
.. 


 0
0 · · · 1 −2 1 
1
0
0 · · · 1 −2
1

T
Now, vector s = [ s1 , · · · , sN −1 ] is readily known from the N equally spaced points, while the
vector s′′ of eq.(102) is obtained as
s′′ = 6A−1 Cs
Then, the coefficient Ak , Bk , Ck and Dk of each cubic spline, for k = 1, . . . , N − 1, are obtained
directly using eqs.(6.55a–d). The Matlab code implementing the foregoing calculations is displayed
below:
clear
N=10;
80
tau1=0.2;
tau2=0.9;
delta=1/(N-1);
for i=1:N-1,
if i==1
A(i,:)=[4,1,zeros(1,N-4),1]/(N-1);
C(i,:)=[-2,1,zeros(1,N-4),1]*(N-1);
elseif i==N-1
A(i,:)=[1,zeros(1,N-4),1,4]/(N-1);
C(i,:)=[1,zeros(1,N-4),1,-2]*(N-1);
else
A(i,:)=[zeros(1,i-2),1,4,1,zeros(1,N-2-i)]/(N-1);
C(i,:)=[zeros(1,i-2),1,-2,1,zeros(1,N-2-i)]*(N-1);
end
end
t=(0:delta:1-delta);
for i=1:length(t),
if t(i)<=tau1
s(i)=t(i)^2/(tau1*(1-tau1+tau2));
elseif t(i)>=tau2
s(i)=(t(i)^2+tau2^2-2*t(i)-tau1*(tau2-1))/((tau2-1)*(1-tau1+tau2));
else
s(i)=(2*t(i)-tau1)/(1-tau1+tau2);
end
end
sl=t;
sp=s-t;
spp=6*inv(A)*C*sp’;
t(N)=1;
sp(N)=sp(1);
spp(N)=spp(1);
step=delta/10;
for i=1:N-1,
Ak(i)=(spp(i+1)-spp(i))/(6*delta);
Bk(i)=spp(i)/2;
Ck(i)=(sp(i+1)-sp(i))/delta-delta*(spp(i+1)+2*spp(i))/6;
Dk(i)=sp(i);
if i==N-1
tk=((i-1)*delta:step:i*delta);
else
tk=((i-1)*delta:step:i*delta-step);
end
81
(a) Periodic cubic spline using 8 supporting points
0.05
0
−0.05
−0.1
0
0.1
0.2
0.3
0.4
0.5
tau
0.6
0.7
0.8
0.9
1
0.9
1
(b) Acceleration profile when using 8 supporting points
10
5
0
−5
−10
0
0.1
0.2
0.3
0.4
0.5
tau
0.6
0.7
0.8
Figure 22:
sk=Ak(i)*(tk-t(i)).^3+Bk(i)*(tk-t(i)).^2+Ck(i)*(tk-t(i))+Dk(i);
sppk=6*Ak(i)*(tk-t(i))+2*Bk(i);
tt=[tt,tk];
spline=[spline,sk];
splinepp=[splinepp,sppk];
end
The resulting periodic cubic spline and its acceleration profile are displayed in Figs. 22(a) and
(b), respectively, for eight supporting points. This number of supporting points gives a very good
approximation of the original profile s(τ ), while smoothing its acceleration profile. Moreover, the
maximum acceleration value, about eight, is only slightly higher than the original acceleration
level, which is about six, and thus, seems quite reasonable. Also note that, as a matter of
comparison, the maximum acceleration of the cycloidal motion is slightly over six, namely, 2π.
6.2 The acceleration program is obtained by differentiating the trapezoidal joint-rate profile of Fig. 13, as
plotted in Fig. 23. As in Problem 6.1, we consider the system of equations of eqs.(6.58a–c). What we
want is to obtain vector s using eq.(6.58a) in the form
s=
1 −1 ′′
C As
6
(105)
where vector s′′ contains the sampled values of the equally spaced supporting points of the acceleration
profile of Fig. 23. Note that for this profile to be periodic, we must have s′′ (0− ) = s′′ (1+ ) = 0. Here, we
denote by superscripts (−) and (+) the instant “just before” and “just after,” in order to accommodate
the jump discontinuities. Moreover, in order to determine uniquely the displacement program, we must
give the initial displacement of s(τ ) since the acceleration profile contains no information about it. We
thus set s(0) = 0, and since the acceleration profile s′′ does not contain information on the linear part
of s(τ ), what we obtain using eq.(105) is just the periodic part sp (τ ) of s(τ ), and thus, s(1) = 0. Using
82
Figure 23:
these two conditions, namely s(0) = s(1) = 0, vector s can be chosen as
s = [ s2 , · · · , sN −1 ]
T
and matrix C of eq.(6.58c) reduces to
 −β
1,2
 β2
 .
C=
 ..

0
0
β2
−β2,3
..
.
0
β3
..
.
···
···
..
.
0
0
..
.
···
0
βN ′′′
···
−βN ′ ,N ′′
βN ′′
βN ′′
−βN ′′ ,N ′






which is invertible. Moreover, for N equally spaced supporting points, we have
∆xk =
1
,
N −1
k = 1, · · · , N − 1
Thus, for i, j, k = 1, . . . , N − 1,
1
,
N −1
βk = N − 1,
αk =
matrices A and C reducing to

1 4
1
0 1
4
1 
..
..
..

A=
.
.
.
N −1
0 ··· 1
0 0 ···
0
1
..
.
4
1

··· 0
··· 0
.
..
. .. 
,
1 0
4 1
2
N −1
= 2(N − 1)
αi,j =
βi,j
(106)
(107)


−2 1
0 ··· 0
 1 −2 1 · · · 0 
 .
.. 
..
..
..
.
C = (N − 1) 
.
.
.
. 
 .

 0 · · · 1 −2 1 
0
0 · · · 1 −2
Using eq.(105), s is readily obtained and the coefficients Ak , Bk , Ck and Dk of the cubic spline, for
k = 1, . . . , N − 1, are computed directly using eqs.(6.55a–d). The Matlab code implementing the
foregoing calculations is shown below:
clear
N=25;
83
tau1=0.2;
tau2=0.9;
delta=1/(N-1);
for i=1:N-2,
if i==1
A(i,:)=[zeros(1,i-1),1,4,1,zeros(1,N-2-i)]/(N-1);
C(i,:)=[-2,1,zeros(1,N-4)]*(N-1);
elseif i==N-2
A(i,:)=[zeros(1,i-1),1,4,1,zeros(1,N-2-i)]/(N-1);
C(i,:)=[zeros(1,N-4),1,-2]*(N-1);
else
A(i,:)=[zeros(1,i-1),1,4,1,zeros(1,N-2-i)]/(N-1);
C(i,:)=[zeros(1,i-2),1,-2,1,zeros(1,N-3-i)]*(N-1);
end
end
t=(delta:delta:1-delta);
for i=1:length(t),
if t(i)<=tau1
spp(i)=2/(tau1*(1-tau1+tau2));
elseif t(i)>=tau2
spp(i)=2/((tau2-1)*(1-tau1+tau2));
else
spp(i)=0;
end
end
t=[0,t,0];
spp=[0,spp,0];
sp=inv(C)*A*spp’/6;
sp=[0;sp;0];
step=delta/10;
for i=1:N-1,
Ak(i)=(spp(i+1)-spp(i))/(6*delta);
Bk(i)=spp(i)/2;
Ck(i)=(sp(i+1)-sp(i))/delta-delta*(spp(i+1)+2*spp(i))/6;
Dk(i)=sp(i);
if i==N-1
tk=((i-1)*delta:step:i*delta);
else
tk=((i-1)*delta:step:i*delta-step);
end
sk=Ak(i)*(tk-t(i)).^3+Bk(i)*(tk-t(i)).^2+Ck(i)*(tk-t(i))+Dk(i);
sppk=6*Ak(i)*(tk-t(i))+2*Bk(i);
tt=[tt,tk];
spline=[spline,sk];
84
(a) Periodic cubic spline using 25 supporting points
0.05
0
−0.05
−0.1
0
0.1
0.2
0.3
0.4
0.5
tau
0.6
0.7
0.8
0.9
1
0.9
1
(b) Acceleration profile when using 25 supporting points
10
5
0
−5
−10
−15
0
0.1
0.2
0.3
0.4
0.5
tau
0.6
0.7
0.8
Figure 24:
splinepp=[splinepp,sppk];
end
The resulting periodic cubic spline and its acceleration profile are plotted in Figs. 24(a) and (b),
respectively, for 25 supporting points. Below this number of points, the deceleration part of the
acceleration profile (τ2 ≤ τ ≤ 1) is not well approximated. Moreover, this number of supporting points
gives a good approximation of the original profile s(τ ), while smoothing its acceleration profile, as
expected.
6.3 Here we want to use cycloidal motions to smooth the joint-rate profile of Fig. 6.7 of the text. To
this end, we define a segment of a cycloidal-motion function between u = 0 and u = u1 such that
s′1 (u1 ) = s′max . We have, from eqs.(6.38a & b) of the text,
1
sin 2πu)
2π
′
s (u) = A(1 − cos 2πu), for 0 ≤ u ≤ 1
s(u) = A(u −
as depicted in Fig. 25(a). From this figure, it is clear that the slope is horizontal, as required, when
u = 1/2. Since we want to obtain this point at τ = τ1 , a change of variable is needed to shrink the
plot in the horizontal direction, as shown in Fig. 25(b). This is done by defining u as u = τ /(2τ1 ), and
hence
A τ
1
τ
s(τ ) =
− sin π
(108)
2 τ1
π
τ1
A
τ
s′ (τ ) =
1 − cos π
(109)
2τ1
τ1
85
Figure 25: Cycloidal function: (a) its velocity profile; and (b) the same after shrinking in the τ -direction
where A is a constant to be determined. We have
s′ (τ1 ) =
A
A
(1 − cos π) =
= s′max
2τ1
τ1
whence,
A = s′max τ1
Further, from τ1 to τ2 , the velocity is kept constant and equal to s′max . Therefore,
Figure 26: The joint-rate profile
s′ (τ ) = s′max ,
86
for τ1 ≤ τ ≤ τ2
(110)
and hence,
s(τ ) = s′max τ + C1
(111)
where C1 is a constant of integration which is determined by noticing, from eq.(108) that s(τ ) =
and plugging this value into eq.(111), thus obtaining
s′max /2,
1
C1 = − s′max τ1
2
Hence,
1
s(τ ) = s′max τ − τ1 ,
2
τ1 ≤ τ ≤ τ2
(112)
Finally, for τ2 ≤ τ ≤ 1, we define the second cycloidal segment (Fig. 26), where s′ (τ2 ) = s′max and
s′ (1) = 0. Here, we consider the cycloidal function
1
s(v) = B v −
sin 2πv
(113)
2π
s′ (v) = B(1 − cos 2πv), for 0 ≤ v ≤ 1
In this case, we must shift the plot of this equation, which is displayed in Fig. 25(a), to the right and
shrink it such that the slope is horizontal at τ = τ2 . This is done by defining a new variable v as
v ≡ α + βτ
such that v(τ ′ ) = 0 and v(1) = 1 (Fig. 26). We can find τ ′ of Fig. 26 from the obvious relation
1 − τ ′ = 2(1 − τ2 ), which leads to τ ′ = −1 + 2τ2 . Hence,
v(−1 + 2τ2 ) = α + β(−1 + 2τ2 ) = 0 and α + β = 1
Solving the above equations for α and β, we obtain
α=
1 − 2τ2
,
2(1 − τ2 )
and hence,
v=
Therefore,
β=
1
2(1 − τ2 )
1 − 2τ2 + τ
2(1 − τ2 )
1 − 2τ2 + τ
s′ (v) = B 1 − cos π
1 − τ2
where B is a constant that can be determined using
s′ (τ2 ) = B(1 − cos π) = 2B = s′max
Hence
s′ (τ ) =
⇒
B=
s′max
2
s′max
1 − 2τ2 + τ
1 − cos π
2
1 − τ2
Now, if we substitute the foregoing expression for B and v into eq.(113), we obtain
s′
1
s′
1 − τ2
1 − 2τ2 + τ
s(τ ) = max τ2 − τ1 + max τ −
sin
2
2
2
π
1 − τ2
87
(114)
displacement profile
1
0.5
0
0
0.1
0.2
0.3
0.4
0.5
0.6
velocity profile
0.7
0.8
0.9
1
0.1
0.2
0.3
0.4
0.5
0.6
acceleration profile
0.7
0.8
0.9
1
0.1
0.2
0.3
0.4
0.7
0.8
0.9
1
1.5
1
0.5
0
0
10
0
−10
−20
0
0.5
0.6
Figure 27: Cycloidal approximation of the trapezoidal profile of Fig. 13, for τ1 = 0.2, τ2 = 0.9, and
s′max = 2/(1 − τ1 + τ2 )
In summary, we have
 ′
smax
τ1
τ


τ−
sin π
,


2
π
τ1




1
s(τ ) = s′max τ − τ1 ,

2



′

smax
1
s′max
1 − τ2
1 − 2τ2 + τ



τ2 − τ1 +
τ−
sin
,
2
2
2
π
1 − τ2
 ′
τ
smax


1 − cos π
,



2
τ
1

s′ (τ ) = s′max ,



s′
1 − 2τ2 + τ


,
 max 1 − cos π
2
1 − τ2
 ′
τ
πsmax


sin
π
,

 2τ1
τ1

s′′ (τ ) = 0 ,



τ + 1 − 2τ2
πs′max


sin π
,
2(1 − τ2 )
1 − τ2
0 ≤ τ ≤ τ1 ;
τ1 ≤ τ ≤ τ2 ;
τ2 ≤ τ ≤ 1.
0 ≤ τ ≤ τ1 ;
τ1 ≤ τ ≤ τ2 ;
τ2 ≤ τ ≤ 1.
0 ≤ τ ≤ τ1 ;
τ1 ≤ τ ≤ τ 2
τ2 ≤ τ ≤ 1.
The plots of the displacement, velocity and acceleration profiles are displayed in Figs. 27(a), (b) and
(c), respectively, for τ1 = 0.2, τ2 = 0.9 and s′max = 2/(1 − τ1 + τ2 ), as defined in Problem 5.1, item (c).
6.4 From the problem, the set of conditions for the initial and final poses are
p(0) = pI ,
ṗ(0) = 0,
88
p̈(0) = 0
Q(0) = QI ,
ω(0) = 0,
p(T ) = pF ,
Q(T ) = QF ,
ṗ(T ) = ṗF ,
ω(T ) = 0,
ω̇(0) = 0
p̈(T ) = 0
ω̇(T ) = 0
In the absence of singularities, these conditions correspond to
θ(0) = θI ,
θ(T ) = θF ,
θ̇(0) = 0,
θ̇(T ) = θ̇ F ,
θ̈(0) = 0
θ̈(T ) = 0
(115a)
(115b)
First, we consider a fifth-degree polynomial, namely,
θ(t) = at5 + bt4 + ct3 + dt2 + et + f
its first two time-derivatives being given by
θ̇(t) = 5at4 + 4bt3 + 3ct2 + 2dt + e
θ̈(t) = 20at3 + 12bt2 + 6ct + 2d
If θiI is the i-th component of θI , with the same notation for θiF and θ̇iF , we have, from the conditions
of eq.(115a) for joint i,
d = e = 0,
f = θiI
while the conditions of eq.(115b) yield three linear equations in a, b, and c, namely,
θiF − θiI
T3
F
θ̇
5aT 2 + 4bT + 3c = i2
T
20aT 2 + 12bT + 6c = 0
aT 2 + bT + c =
Upon solving the three foregoing equations for the three unknowns, we obtain
6 F
3
(θi − θiI ) − 4 θ̇iF
5
T
T
15 F
7
I
b = − 4 (θi − θi ) + 3 θ̇iF
T
T
10 F
4 F
I
c = 3 (θi − θi ) − 2 θ̇i
T
T
a=
and hence, the fifth-degree polynomial sought is
6 F
3 F 5
15 F
7 F 4
10 F
4 F 3
I
I
I
θi (t) = θiI +
(θ
−
θ
)
−
θ̇
t
+
−
(θ
−
θ
)
+
θ̇
t
+
(θ
−
θ
)
−
θ̇
t
i
i
i
T5 i
T4 i
T4 i
T3 i
T3 i
T2 i
To obtain the seventh-degree polynomial, we impose the condition that the joint jerk vanishes at the
two endpoints of the motion, namely,
θ(3) (0) = 0,
θ(3) (T ) = 0
The polynomial has the form
θ(t) = at7 + bt6 + ct5 + dt4 + et3 + f t2 + gt + h
89
(116)
and its three first time-derivatives are given by
θ̇(t) = 7at6 + 6bt5 + 5ct4 + 4dt3 + 3et2 + 2f t + g
θ̈(t) = 42at5 + 30bt4 + 20ct3 + 12dt2 + 6et + 2f
θ(3) (t) = 210at4 + 120bt3 + 60ct2 + 24dt + 6e
From the conditions of eq.(115a) and the first condition of eq.(116), we have
e = f = g = 0,
h = θiI
while the conditions of eq.(115b) and the second condition of eq.(116) yield four linear equations in a,
b, c, and d, namely,
θiF − θiI
T4
F
θ̇
7aT 3 + 6bT 2 + 5cT + 4d = i3
T
3
2
42aT + 30bT + 20cT + 12d = 0
aT 3 + bT 2 + cT + d =
210aT 3 + 120bT 2 + 60cT + 24d = 0
Upon solving the four foregoing equations for their four unknowns, we obtain
20 F
10
(θ − θiI ) + 6 θ̇iF
T7 i
T
70
34
b = 6 (θiF − θiI ) − 5 θ̇iF
T
T
84
39
c = − 5 (θiF − θiI ) + 4 θ̇iF
T
T
35
15
d = 4 (θiF − θiI ) − 3 θ̇iF
T
T
a=−
and hence, the seventh-degree polynomial sought is
20 F
10 F 7
70 F
34 F 6
I
I
I
θi (t) = θi + − 7 (θi − θi ) + 6 θ̇i t +
(θ − θi ) − 5 θ̇i t
T
T
T6 i
T
84
39
35 F
15 F 4
I
+ − 5 (θiF − θiI ) + 4 θ̇iF t5 +
(θ
−
θ
)
−
θ̇
t
i
T
T
T4 i
T3 i
6.6 In order to have an idea of the magnitude of the error as the number of supporting points grows,
we estimate the maximum error in each subinterval, as described presently. Furthermore, we can
decompose the cycloidal function into a linear and a periodic part; we approximate only the latter,
since the former does not contribute to the approximation error.
For a periodic cubic spline, similar to the solution of Exercise 6.1, we have the conditions, s1 = sN ,
′
′
′′
′′
s1 = sN , s1 = sN . The difference is that here we have N intervals, N + 1 points, and thus, matrices
A and C are of N × N , while
s = [s1 , ..., sN ]T ,
Moreover,
s′′ = [s′′1 , ..., s′′N ]T .
(117)
(118)
∆xk =
1
,
N
k = 1, ..., N,
αk =
1
,
N
αi,j =
and for i, j, k = 1, ..., N ,
90
2
,
N
(119)
βk = N,
βi,j = 2N.
(120)
With the above coefficients we can obtain the A and C matrices that, for periodic splines, are given
in eq.(9.59a & b). Solving for s′′ in terms of s from eq.(6.58a), we obtain
s′′ = 6A−1 Cs.
(121)
With the foregoing calculations we have all the necessary information (s′′ and s) to find the coefficients
of the cubic polynomials appearing in eqs.(6.55a–d).
Now, the error at the ends of each subinterval vanishes. Hence, a plausible assumption is that the
maximum absolute value of the error is attained at the midpoint of the interval. Note that a rigorous
calculation of this maximum can be performed with a suitable optimization routine, that would use a
bisection or golden-section subdivision of the interval.
A Matlab code, implemented as a function error(N) that returns the error eN , for a given N , is
included below:
function e = error(N)
% Definitions:
Delta = 1/N;
D2 = Delta/2;
ak = Delta;
bk = N;
a2ij = 4*Delta;
bij = 2*bk;
pi2 = 2*pi;
%
%
%
%
%
%
%
t = ( 0 : Delta : 1);
for i = 1 : length(t) - 1,
% subinterval step vector
% recursion from 1 to N
Subinterval length
Half of Delta
alpha_k
beta_k
2*alpha_ij
beta_ij
2*pi
% Cycloidal function
cycloida(i) = t(i)- sin(pi2*t(i))/pi2;
cycloidap(i) = 1 - cos(pi2*t(i));
c(i) = cycloida(i) - t(i);
% c(i): the periodic part
cp(i) = cycloidap(i) - 1;
% c’(i): the periodic part
% Building matrices A and C
if i == 1
A(i,:) = [ a2ij, ak, zeros(1,N-3), ak ];
C(i,:) = [ -bij, bk, zeros(1,N-3), bk ];
else
if i == N
A(i,:) = [ ak, zeros(1,N-3), ak, a2ij ];
C(i,:) = [ bk, zeros(1,N-3), bk, -bij ];
else
A(i,:) = [ zeros(1,i-2), ak, a2ij, ak, zeros(1,N-1-i) ];
C(i,:) = [ zeros(1,i-2), bk, -bij, bk, zeros(1,N-1-i) ];
end
end
end
91
% Find s’’ for the supporting points
s = c;
% s(i) = c(i), i=1..N
sp = cp;
% sp(i) = cp(i), i=1..N
spp = 6*inv(A)*C*sp’;
% finding s’’
s(N+1) = s(1);
sp(N+1) = sp(1);
spp(N+1) = spp(1);
% closing the cycle: s(N+1)
% s’(N+1)
% s’’(N+1)
c(N+1) = c(1);
% closing the cycle: c(N+1)
% Construct the spline function and calculate the error
for i = 1 : N,
% Coefficients
Ak(i) = ( spp(i+1) - spp(i) )/(6*Delta);
Bk(i) = spp(i)/2;
Ck(i) = ( s(i+1) - s(i) )/Delta - Delta*( spp(i+1) + 2*spp(i) )/6;
Dk(i) = s(i);
% Spline-function value at the midpoint of the ‘‘i’’-th subinterval
sk = Ak(i)*(D2)^3 + Bk(i)*(D2)^2 + Ck(i)*(D2) + Dk(i);
% Derivative value at the supporting points
skp = Ck(i);
skpp = 2*Bk(i);
sc = [ sc, sk ];
scp = [ scp, skp ];
scpp = [ scpp, skpp ];
% The periodic part of the cycloidal function at the midpoint
cd = - sin ( pi2*(t(i)+D2) )/pi2;
% The derivative values at the supporting points are now
cdp = - cos ( pi2*(t(i)) );
cdpp = - pi2*sin ( pi2*(t(i)) );
cc = [ cc, cd ];
ccp = [ ccp, cd ];
ccpp = [ ccpp, cd ];
end
% Error
ei = sc - cc;
eip = scp - ccp;
eipp = scpp - ccpp;
% Error norm
92
e = norm(ei, inf);
The results are summarized in Table 3. Note that, by increasing the number of subintervals by one
Table 3: Errors in the approximation of a cycloidal function with a periodic spline
N
eN
e′N
e′′N
10
0.0485
1.0171
36.947
20
0.0124
1.0126
38.855
30
0.0055
1.0105
39.206
40
0.0031
1.0100
39.328
50
0.0020
1.0122
39.384
60
0.0014
1.0126
39.414
70
0.0010
1.0123
39.432
80
0.0008
1.0119
39.444
90
0.0006
1.0125
39.452
100
0.0005
1.0126
39.457
order of magnitude, from 10 to 100, eN decreases by two orders of magnitude, from 0.0485 to 0.005.
However, the errors in the first two derivatives remain virtually unchanged, with the error e′′N exhibiting
a slight increment as N increases.
6.8 We consider the 4-5-6-7 polynomial of eq.(6.27), namely,
P4567 (τ ) = −20τ 7 + 70τ 6 − 84τ 5 + 35τ 4 .
(122)
The periodic part, p4567 , for 0 ≤ τ ≤ 1, is
p4567 (τ ) = −20τ 7 + 70τ 6 − 84τ 5 + 35τ 4 − τ,
(123)
its first and second derivatives being:
p′4567 (τ ) = −140τ 6 + 420τ 5 − 420τ 4 + 140τ 3 − 1,
(124)
p′′4567 (τ ) = −840τ 5 + 2100τ 4 − 1680τ 5 + 420τ 3 .
(125)
Following the same procedure as with Exercise 6.6, we find the values of p4567 (τ ) and p′4567 (τ ). Then,
we compute the A and C matrices, s′′ (τi ), coefficients Ak , Bk , Ck , Dk , and, finally, the polynomial
and the spline values for the midpoint of each subinterval, followed by the maximum error value.
A Matlab code, that returns the errors eN , e′N , e′′N , given N , is included below:
function [ e, ep, epp ] = p58(N)
% Definitions for computational efficiency
Delta = 1/N;
% Subinterval length
D2 = Delta/2;
% Half of Delta
ak = Delta;
% alpha_k
bk = N;
% beta_k
a2ij = 4*Delta;
% 2*alpha_ij
bij = 2*bk;
% beta_ij
pi2 = 2*pi;
% 2*pi
t = ( 0 : Delta : 1 );
te = ( D2 : Delta : 1 - D2 );
for i = 1 : length(t) - 1,
% subinterval endpoints
% subinterval midpoints
% recursion from 1 to N
% 4-5-6-7 Polynomial: the periodic part only
po(i) = - 20*t(i)^7 + 70*t(i)^6 - 84*t(i)^5 + 35*t(i)^4 - t(i);
93
pop(i) = - 140*t(i)^6 + 420*t(i)^5 - 420*t(i)^4 + 140*t(i)^3 -1;
popp(i) = - 840*t(i)^5 + 2100*t(i)^4 - 1680*t(i)^3 + 420*t(i)^2;
% Building matrices A and C
if i == 1
A(i,:) = [ a2ij, ak, zeros(1,N-3), ak ];
C(i,:) = [ -bij, bk, zeros(1,N-3), bk ];
else
if i == N
A(i,:) = [ ak, zeros(1,N-3), ak, a2ij ];
C(i,:) = [ bk, zeros(1,N-3), bk, -bij ];
else
A(i,:) = [ zeros(1,i-2), ak, a2ij, ak, zeros(1,N-1-i) ];
C(i,:) = [ zeros(1,i-2), bk, -bij, bk, zeros(1,N-1-i) ];
end
end
end
% Find s’’ for the supporting points
s = po;
% s(i) = c(i)
sp = pop;
% s’(i) = c’(i)
spp = 6*inv(A)*C*sp’;
% finding s’’
s(N+1) = s(1);
sp(N+1) = sp(1);
spp(N+1) = spp(1);
% closing the cycle: s
% s’
% s’’
% Construct the spline function and calculate the error
for i = 1 : N,
% Coefficients
Ak(i) = ( spp(i+1) - spp(i) )/(6*Delta);
Bk(i) = spp(i)/2;
Ck(i) = ( s(i+1) - s(i) )/Delta - Delta*( spp(i+1) + 2*spp(i))/6;
Dk(i) = s(i);
% Spline function at midpoints of a subinterval
sk = Ak(i)*(D2)^3 + Bk(i)*(D2)^2 + Ck(i)*(D2) + Dk(i);
% Spline derivative values at the endpoints
skp = Ck(i);
skpp = 2*Bk(i);
sc = [ sc, sk ];
scp = [ scp, skp ];
scpp = [ scpp, skpp ];
% The periodic part of the 4-5-6-7 polynomial at the midpoints
pom = - 20*te(i)^7 + 70*te(i)^6 - 84*te(i)^5 + 35*te(i)^4 - te(i);
% The periodic part of the 4-5-6-7 polynomial at the endpoints
94
pomp = - 140*t(i)^6 + 420*t(i)^5 - 420*t(i)^4 + 140*t(i)^3 - 1;
pompp = - 840*t(i)^5 + 2100*t(i)^4 - 1680*t(i)^3 + 420*t(i)^2;
poc = [ poc, pom ];
pocp = [ pocp, pomp ];
pocpp = [ pocpp, pompp ];
end;
Note that the approximation error can be estimated using the Matlab spline routines. Below we
include the Matlab code that estimates errors and numbers of subintervals required to attain the
desired accuracy:
function [eN,Ne,epN,Nep,eppN,Nepp] = p58(epsilon)
N = 4; % initial number of subintervals for the iterations
dN = 1; % initial increment for N
e = 2, ep = 1, epp = 1; % initial value for the error
disp(’Function error’);
while (e > epsilon),
% while iteration
N = N + dN; disp(N), disp(e), % increasing the number of subintervals
Delta = 1/N;
% subinterval length
Delta2 = Delta/2;
% half of Delta
te = ( 0 : Delta : 1 );
% subinterval endpoints: N + 1
tm = ( Delta2 : Delta : 1 - Delta2 );
% subinterval midpoints: N
% 4-5-6-7 Polynomial minus the linear part
pm = - 20*tm.^7 + 70*tm.^6 - 84*tm.^5 + 35*tm.^4 - tm; % at midpoints
pe = - 20*te.^7 + 70*te.^6 - 84*te.^5 + 35*te.^4 - te; % at endpoints
sm = ppval(csape(te,pe,[0 0]),tm); % periodic cubic spline at midpoints
ev = sm - pm;
% function error at midpoints
e = norm(ev, inf);
% the maximum midpoint error over all subintervals
end % while
% Documentation
Ne = N, eN = e, % return of N and e
figure(1)
subplot(3,1,1), plot(ev)
dN = dN*5, % increase in the search order for faster search
disp(’First derivative error’);
while (ep > epsilon),
% while iteration
N = N + dN; disp(N), disp(ep), % increasing the number of subintervals
Delta = 1/N;
% subinterval length
Delta2 = Delta/2;
% half of Delta
te = ( 0 : Delta : 1 );
% subinterval endpoints: N + 1
tm = ( Delta2 : Delta : 1 - Delta2 );
% subinterval midpoints: N
% First Derivative of 4-5-6-7 Polynomial minus the linear part
pep = - 140*te.^6 + 420*te.^5 - 420*te.^4 + 140*te.^3 -1; % at endpoints
% 4-5-6-7 Polynomial minus the linear part
95
pe = - 20*te.^7 + 70*te.^6 - 84*te.^5 + 35*te.^4 - te; % at endpoints
sep = ppval(fnder(csape(te,pe,[0 0]),1),te); % p.c.s.’ at endpoints
epv = sep - pep; % first derivative error at endpoints
ep = norm(epv, inf);
% the maximum midpoint error over all subintervals
end % while
% Documentation
Nep = N, epN = ep, % recording N and ep
subplot(3,1,2), plot(epv)
dN = dN*10, % increase in the search order for faster search
disp(’Second derivative error’);
while (epp > epsilon),
% while iteration
N = N + dN; disp(N),disp(epp), % increasing the number of subintervals
Delta = 1/N;
% subinterval length
Delta2 = Delta/2;
% half of Delta
te = ( 0 : Delta : 1 );
% subinterval endpoints: N + 1
tm = ( Delta2 : Delta : 1 - Delta2 );
% subinterval midpoints: N
% First Derivative of 4-5-6-7 Polynomial minus the linear part
pepp = - 840*te.^5 + 2100*te.^4 - 1680*te.^3 + 420*te.^2;
% 4-5-6-7 Polynomial minus the linear part
pe = - 20*te.^7 + 70*te.^6 - 84*te.^5 + 35*te.^4 - te; % at endpoints
sepp = ppval(fnder(csape(te,pe,[0 0]),2),te); % p.c.s.’’ at midpoints
eppv = sepp - pepp; % second derivative error at midpoints
epp = norm(eppv, inf);
% the maximum midpoint error over all subintervals
end % while
% Documentation
Nepp = N, eppN = epp, % recording N and epp
subplot(3,1,3), plot(eppv)
The results obtained for an error less than 0.0001 are given in Table 4.
Table 4: Error values
N
N
N
7
15
60
960
eN
e′N
e′′N
8.7347e-05
8.9181e-05
9.5397e-05
Dynamics of Serial Robotic Manipulators
7.1 (a) We recall the definition of twist t of a rigid body, referred to a point P of the body, of velocity
T
v, the body moving with an angular velocity ω: t = [ ωT , vT ] . The 6 × 6 angular-velocity dyad
W is also recalled:
Ω O
W=
(126)
O O
and hence,
Wt =
96
Ωω
0
(127)
Given that Ω= CPM (ω), Ωω = ω × ω = 0. Therefore, t lies in the null space of W. Now, for
the case of a mechanical system composed of n rigid bodies, let ω i , ti , Ωi and Wi denote the
foregoing quantities as pertaining to the ith body.
Further, with the notation introduced in this chapter,

W1
 O
Wt = 
 ...
O
O
W2
..
.
O
  
  
t1
W1 t1
O
0
O   t2   W2 t2   0 
 . = . =.
.. 
.   ..   ..   .. 
0
tn
Wn tn
· · · Wn
···
···
..
.
(128)
q.e.d.
(b) The 6n × 6n matrix M for the system introduced
hence,

Ṁ1 O
 O Ṁ2
Ṁ = 
..
 ...
.
O
O
in (a) is M = diag(M1 , M2 , · · · , Mn ), and
···
···
..
.

O
O 
.. 
. 
(129)
· · · Ṁn
Now, the result of Exercise 3.17 is recalled, as applied to the ith body of the system:
Ṁi = Wi Mi − Mi Wi
(130)
When the above expression is substituted into the right-hand side of eq. (129), one obtains two
6n × 6n block-diagonal matrices WM and MW, thereby obtaining the desired result.
(c) The 6-dimensional momentum screw µ was defined in eq. (3.142) for one rigid body. If µi denotes
the momentum screw of the ith rigid body of the system at hand, then the 6n-dimensional system
momentum screw µ is defined as
µ = Mt
(131)
where M is the 6n × 6n system inertia dyad and t is the 6n-dimensional system twist. Therefore,
µ̇ = Ṁt + Mṫ
(132)
with Ṁ having been obtained in (b) above.
Therefore, the above expression becomes
µ̇ = (WM − MW)t + Mṫ = WMt − MWt + Mṫ
(133)
However, from (a), Wt = 0, thereby obtaining the intended result.
7.3 In order to apply Algorithms 7.4.1 and 7.4.2 to obtain the necessary torque at each joint to perform
the desired maneuver, we must first obtain the joint rates and accelerations. Since the velocity and
acceleration of the wrist center C are given, as well as the angular velocity and acceleration of the
end-effector, the joint rates and accelerations are readily obtained using the Jacobian matrix. For a
decoupled manipulator, we have
J11 θ̇a + J12 θ̇w = ω
J21 θ̇ a = ċ
(134a)
(134b)
with matrices J11 , J12 and J21 given by
J11 = [ e1
e2
e3 ] ,
J12 = [ e4
e5
97
e6 ] ,
J21 = [ e1 × r1
e2 × r2
e3 × r3 ]
Figure 28:
and ri is defined as the vector directed from Oi to C. Referring to Fig. 28, and expressing all quantities
in frame 1, we have
 
 
 
 
 
0
0
0
1
0
e1 =  0  , e2 = e3 =  1  , e4 =  0  , e5 =  1  , e6 =  0  ,
0
1
0
0
1
 
 
1
0
r1 = r2 =  0  , r3 =  0  ,
1
1
 
 
 
1
0
a
a1 = a3 = a5 = 0, a2 =  0  , a4 =  0  , a6 =  0 
0
1
0
Thus,
Therefore,
 
0
e1 × r1 =  1  ,
0
Hence,
0
= 0
1
J11


0 0
1 1,
0 0
J−1
12
From eq.(134b), we have


1
e2 × r2 =  0  ,
−1
J12

0 0
= 0 1
1 0


0 1
1 0,
0 0

1
0,
0

0
= 0
1

0 1
0 0
θ̇a = J−1
21 ċ =
1 0
98
J−1
21
 
1
e3 × r3 =  0 
0
J21
0 1
= 0 0
1 0

0
= 1
0

1 1
0 0
−1 0

0
−1 
1
  

0
1
0
−1   0  =  −1 
1
1
2
(135)
and, from eq.(134a),
   
0 0 1
0
0






θ̇w = J−1
ω
−
J
θ̇
=
0
1
0
1
−
0
11 a
12
1 0 0
0
1

   
0 0 1
0
0
= 0 1 00 = 0
1 0 0
0
0

We therefore have,
θ̇ = [ 0
−1 2 0
0
0]
T


0 0
0
1 1   −1 
0 0
2
rad/s
Moreover, differentiating eqs.(134a & b), we obtain
J̇11 θ̇a + J11 θ̈ a + J̇12 θ̇w + J12 θ̈w = ω̇
(136a)
J̇21 θ̇a + J21 θ̈a = c̈
(136b)
Thereby obtaining
θ̈w = J−1
12 ω̇ − J11 θ̈ a − J̇11 θ̇ a − J̇12 θ̇ w
θ̇
θ̈a = J−1
c̈
−
J̇
21
a
21
(137a)
(137b)
The time-derivatives of the blocks of J are
J̇11 = [ ė1
ė2
ė3 ] ,
J̇12 = [ ė4
ė5
ė6 ] ,
J̇21 = [ u̇1
u̇2
where
ėi = ω i−1 × ei ,
i = 1, 2, · · · , 6
and
u̇i = ėi × ri + ei × ṙi ,
i = 1, 2, 3
Moreover,
ṙ1 = ṙ2 = ω 2 × a2 + ω3 × a3
ṙ3 = ω 3 × a3
Considering the joint rates obtained above, we have
ω0 = 0
ω 1 = θ̇1 e1 = 0
ω 2 = θ̇1 e1 + θ̇2 e2 = [ 0 −1 0 ]
ω3 = ω 4 = ω5 = ω 6 = θ̇1 e1 + θ̇2 e2 + θ̇3 e3 = [ 0
Therefore,
ė1 = ė2 = ė3 = ė5 = 0
0]
ė6 = [ 0 0
−1 ]
ṙ1 = ṙ2 = [ 1 0
ṙ3 = [ 1 0
99
T
ė4 = [ 1 0
1]
0]
T
T
T
1 0]
T
u̇3 ]
and
0]
u̇2 = [ 1 0
−1 ]T
u̇3 = [ 0 0
Thus,
J̇11 = 0,
J̇12
Therefore, from eq.(137b), we have

0 1
θ̈a =  0 0
1 0

0 1
= 0 0
1 0
and from eq.(137a), we have

Therefore,
0
θ̈w =  0
1

0
= 0
1
T
u̇1 = [ 0 1

1
= 0
0
−1 ]

0 0
0 0 ,
0 −1
T
J̇21

0 1
= 1 0
0 −1

0
0 
−1
   


0
0
0 1
0
0
−1   1  −  1 0
0   −1 
1
0
0 −1 −1
2
  

0
1
1
−1   1  =  −1 
1
1
2
0
1
0
0
1
0
θ̈ = [ 1
   


1
1
0 0 0
1
0   0  −  0 1 1   −1 
0
1
1 0 0
2

 

1
1
0
0   −1  =  −1 
0
0
1
−1 2
0 −1 1 ]T rad/s2
Henceforth, we represent all vectors and matrices in the F1 -frame, while denoting by i, j, and k the
unit vectors parallel to the X1 , Y1 , and Z1 axes, respectively. By inspection, and using Definition 2.2.1,
the rotation matrices Pi transforming coordinates in frame Fi+1 into coordinates in F1 are






−1 0 0
1 0 0
1 0 0
P1 =  0 0 1  , P2 =  0 0 1  , P3 =  0 1 0  ,
0 1 0
0 −1 0
0 0 1






−1 0 0
0 0 1
0 1 0
P4 =  0 0 1  P5 =  0 1 0  , P6 =  1 0 0 
0 1 0
−1 0 0
0 0 −1
The position vectors of the mass centers are given as [ρi ]i+1 , for i = 1, 2, · · · , 6, their corresponding
representation in the F1 -frame are easily obtained by inspection, or using
[ρi ]1 = Pi [ρi ]i+1 ,
i = 1, 2, · · · , 6
Therefore,
ρ1 = −0.054k,
ρ2 = 0.140i,
ρ3 = −0.197j,
ρ4 = −0.057j,
ρ5 = −0.007j,
ρ6 = 0.019k
100
Moreover, the moment-of-inertia matrices are given as [Ii ]i+1 and are transformed in F1 coordinates
using the similarity transformation
[Ii ]1 = Pi [Ii ]i+1 PTi ,
i = 1, 2, · · · , 6
Thus,
I1 = diag(1.6120, 1.6120, 0.5091)
I2 = diag(0.4898, 8.2672, 8.0783)
I3 = diag(3.3768, 0.3009, 3.3768)
I4 = diag(0.1810, 0.1273, 0.1810)
I5 = diag(0.1273, 0.0735, 0.0735)
I6 = diag(0.0071, 0.0071, 0.0141)
Using eq.(7.32a), we have
δ 1 = 0.054k,
δ 2 = 0.860i,
δ 3 = 0.197j,
δ 4 = 0.057j + k,
δ 5 = 0.007j,
δ 6 = ai − 0.019k
Moreover,
δ 0 = ω 0 = ω̇0 = c0 = ċ0 = 0
and, in order to incorporate gravity, we consider
c̈0 = −g = 9.806k
as in eq.(7.44). Therefore, applying Algorithm 7.4.1, we obtain, recursively,
c1 = −0.054k,
ω1 = u0 = v1 = ċ1 = 0,
c2 = 0.140i,
ċ2 = 0.140k,
ω 2 = −j,
ω̇ 2 = −j + k,
c3 = i − 0.197j,
ċ3 = k,
ω 3 = j,
ω̇ 3 = j + k,
u1 = 0,
ω 5 = j,
ċ6 = 1.019i + k,
u3 = v4 = 0,
c̈4 = −0.943i + j + 10.806k
ω̇ 5 = k,
c6 = i + 1.019k,
v3 = 0,
c̈3 = −0.803i + j + 10.806k
c5 = i − 0.007j + k,
ċ5 = i + k,
v2 = 0.140k,
u2 = 0.860k,
ω 4 = j,
ω̇ 4 = j + k,
c̈1 = 9.806k
c̈2 = −0.140i + 0.140j + 9.946k
c4 = i − 0.057j,
ċ4 = k,
ω̇1 = k,
u4 = i,
v5 = 0,
c̈5 = 0.007i + j + 9.806k
ω 6 = j,
u5 = 0,
ω̇ 6 = i + k,
v6 = 0.019i,
c̈6 = 0.981j + 9.787k
and using Algorithm 7.4.2 we obtain, in turn,
f6 = 0.3443j + 3.4352k,
n6 = 0.0006i + 0.0141k
f5 = 0.0074i + 1.3963j + 13.7511k,
f4 = −0.9847i + 2.4483j + 25.1191k,
f3 = −8.0246i + 11.2153j + 119.8553k,
f2 = −10.2339i + 13.4247j + 276.8131k,
f1 = −10.2339i + 13.4247j + 379.9820k,
Therefore,
n5 = −0.0717i + 0.0877k
n4 = −2.1160i + 0.1347j + 0.2121k
n3 = −20.7790i + 0.4356j + 2.2020k
n2 = −20.7790i − 149.6610j + 21.8050k
n1 = −20.7790i − 149.6610j + 22.3141k
τ = [ 22.3141 21.8050 2.2020 0.2121 0.0877 0.0141 ]T Nm
101
7.4 The key idea in solving this problem lies in focusing only on the relative motion between bodies 1 and
2, while making abstraction of their coupling with the frame.
(a) From Fig. 14a,
r1 θ̇1 = r2 θ̇2
or r1 θ̇1 − r2 θ̇2 = 0
(138)
and
(c2 − c1 )T (ċ2 − ċ1 ) = 0
⇒
(c2 − c1 )T ċ1 − (c2 − c1 )T ċ2 = 0
From eqs.(138) and (139), we obtain
r1
0T
−r2
θ̇1
+
0 (c2 − c1 )T
0
ċ1
0T
−(c2 − c1 )T
where 0 is the 2-dimensional zero vector. Hence,
r1
0T
−r2
A=
, B=
0 (c2 − c1 )T
0
θ̇2
ċ2
0T
−(c2 − c1 )T
(139)
=0
which are both 2 × 3 matrices.
(b) With reference to Fig. 29,
ω 1 = ψ̇i,
ω 2 v = θ̇v,
kik = kvk = 1
The two gears are constrained to move such that 2 rolls onto 1 without slipping, about the common
element, parallel to the unit vector e, i.e.,
E(ω 2 − ω 1 ) = 0,
E = CPM(e)
(140)
−−→
Furthermore, if we let ci = OC i , for i = 1, 2, then
c2 = c1 + b1 + r2
(141)
Upon differentiation of eq.(141), we derive
ċ2 = ċ1 + ḃ1 + ṙ2
where
ḃ1 = ω 1 × b1 ,
ṙ2 = (ω 2 − ω1 ) × r2
Hence, with B1 and R2 defined as the CPMs of b1 and r2 , respectively,
ċ2 = ċ1 − B1 ω 1 + R2 (ω 1 − ω 2 )
or
ċ1 − B1 ω 1 − ċ2 + R2 (ω 1 − ω 2 ) = 0
Combining eqs.(140) and (142), we obtain
E
O
ω1
−E
+
R 2 − B1 1
ċ1
−R2
O
−1
ω2
ċ2
(142)
= 06
where O and 1 are the 3 × 3 zero and identity matrices, respectively. Hence,
E
O
−E
O
A=
, B=
R 2 − B1 1
−R2 −1
102
Figure 29: A bevel gear train.
(c) Here, we refer to Fig. 30 where, apparently, ω1 = θ̇, ω2 = φ̇ (not indicated in figure) and
c2 = c1 + (q − c1 ) + r2 = q + r2
Upon differentiation of the above equation, we obtain
ċ2 = θ̇Eq + (φ̇ − θ̇)Er2 ,
E≡
0
1
−1
0
(143)
Now, body 2 is constrained to move with respect to body 1 in such a way that the relative velocity
of point Q in 2 with respect to that of point Q in 1 is tangential to the cam profile, i.e.,
nT (q̇2 − q̇1 ) = 0
(144)
q − c1
(145)
a
where q̇i is the velocity vector of point Q in element i, i = 1, 2 and q1 = q2 = q. Substituting
eq.(145) into eq.(144), we obtain
n=
(q − c1 )T (q̇2 − q̇1 ) = 0
(146)
Moreover,
q̇1 = ċ1 + θ̇E(q − c1 ),
q̇2 = ċ2 + φ̇E(q − c2 ) ≡ ċ2 − φ̇Er2
⇒
q̇2 − q̇1 = ċ2 − ċ1 − φ̇Er2 − θ̇E(q − c1 )
(147a)
(147b)
(147c)
Substituting eq.(147c) into eq.(146), we obtain
(q − c1 )T [ċ2 − ċ1 − φ̇Er2 − θ̇E(q − c1 )] = (q − c1 )T (ċ2 − ċ1 − φ̇Er2 ) = 0
⇒
(q − c1 )T ċ1 − (q − c1 )T ċ2 − (q − c1 )T φ̇Er2 = 0
103
(148)
Figure 30: A cam with a flat-face follower.
⇒
Hence,
0 (q − c1 )T
A=
0
(q − c1 )T
θ̇
ċ1
,
+
φ̇
ċ2
(q − c1 )T E(q − c2 ) (q − c1 )T
−(q − c1 )T Er2
B=−
−(q − c1 )T
=0
i.e., A and B are 1 × 3 matrices, the two bodies being subject to one single scalar constraint.
7.5 The generalized inertia matrix is
I = TT MT,
(149)
with the expressions for T and M, and entries of I given in Example 7.6.1. The time derivative of I
is, for the planar case, simply,
İ = ṪT MT + TT MṪ,
(150)
which we can write as
İ = TT MṪ + (TT MṪ)T .
(151)
Now we have already the entries µij of T MṪ in Example 7.6.1. Hence, if we denote by I˙ij the entries
of İ, then
I˙ij = µij + µji ,
(152)
T
with all µij entries displayed in p. 296. Notice that C = TT MṪ and, hence,
D ≡ İ − 2C = ṪT MT + TT MṪ − 2ṪT MT = ṪT MT − TT MṪ
which is apparently skew-symmetric, its components dij being
dij = µij − µji ,
(153)
q.e.d.
7.6 (a) The point O of concurrency of the three axes is fixed in space. Therefore, the kinetic energy Ti
of link i is given by
1
Ti = ωTi Ii ω i
2
104
where Ii is the moment-of-inertia matrix of link i with respect to O. Since the inertia matrices
are given in link-fixed coordinates, or [ Ii ]i+1 , the same frames must be used for ω i ; we thus need
[ ω i ]i+1 . By inspection, we have






cos θ4 0
sin θ4
cos θ5 0
sin θ5
cos θ6 − sin θ6 0
Q4 =  sin θ4 0 − cos θ4  , Q5 =  sin θ5 0 − cos θ5  , Q6 =  sin θ6
cos θ6 0 
0
1
0
0
1
0
0
0
1
Thus,
T
[ ω 4 ]5 = QT4 θ̇4 [ e4 ]4 = [ 0 θ̇4 0 ]
T
[ ω5 ]6 = QT5 [ ω 4 ]5 + θ̇5 [ e5 ]5 = [ sin θ5 θ̇4 θ̇5 − cos θ5 θ̇4 ]


sin θ5 cos θ6 θ̇4 + sin θ6 θ̇5
[ ω 6 ]7 = QT6 [ ω 5 ]6 + θ̇6 [ e6 ]6 =  − sin θ5 sin θ6 θ̇4 + cos θ6 θ̇5 
− cos θ5 θ̇4 + θ̇6
Therefore, the kinetic energy T of the manipulator is
6
1X T
T =
[ ω Ii ω i ]i+1
2 i=4 i
After performing the required calculations, we obtain
T =
1
1
1
J2 + (K1 + L1 c26 + L2 s26 )s25 + (K3 + L3 )c25 θ̇42 + (K2 + L1 s26 + L2 c26 )θ̇52 + L3 θ̇62
2
2
2
+(L1 − L2 )s5 s6 c6 θ̇4 θ̇5 − L3 c5 θ̇4 θ̇6
which can be rewritten as
1 T
θ̇ I(θ)θ̇
2
with the inertia matrix of the manipulator, I(θ), given as

J2 + (K1 + L1 c26 + L2 s26 )s25 + (K3 + L3 )c25

I(θ) =
(L1 − L2 )s5 s6 c6
−L3 c5
T =
(L1 − L2 )s5 s6 c6
K2 + L1 s26 + L2 c26
0
(b) Using eq.(7.20), the term of Coriolis and centrifugal forces is given by
"
#T
1 ∂(Iθ̇)
h(θ, θ̇) = İ(θ, θ̇)θ̇ −
θ̇
2
∂θ
By direct differentiation of I(θ), we have

where
I˙11
İ(θ) =  I˙12
I˙13
I˙12
I˙22
I˙23

I˙13
I˙23 
I˙33
I˙11 = 2(K1 + L1 c26 + L2 s26 − K3 − L3 )s5 c5 θ̇5
+2(L2 − L1 )s6 c6 s25 θ̇6
I˙12 = (L1 − L2 )[c5 s6 c6 θ̇5 + s5 (c26 − s26 )θ̇6 ]
I˙13 = L3 s5 θ̇5
I˙22 = 2(L1 − L2 )s6 c6 θ̇6
I˙23 = I˙33 = 0
105

−L3 c5
0 
L3
We also have

0
∂(Iθ̇) 
= 0
∂θ
0
where
′
I12
′
I22
′
I32

′
I13
′ 
I23
′
I33
′
I12
= 2(K1 + L1 c26 + L2 s26 − K3 − L3 )s5 c5 θ̇4 + (L1 − L2 )c5 s6 c6 θ̇5 + L3 s5 θ̇6
′
I13
= 2(L2 − L1 )s6 c6 s25 θ̇4 + (L1 − L2 )s5 (c26 − s26 )θ̇5
′
I22
= (L1 − L2 )c5 s6 c6 θ̇4
′
I23 = 2(L1 − L2 )s6 c6 θ̇5 + (L1 − L2 )s5 (c26 − s26 )θ̇4
′
I32
= L3 s5 θ̇4
′
I33 = 0
Let
h(θ, θ̇) = [ h1
h2
h3 ]
T
Then
h1 = (K1 + L1 c26 + L2 s26 − K3 − L3 )s5 c5 θ̇4 θ̇5 + (L2 − L1 )s6 c6 s25 θ̇4 θ̇6
1
1
1
+ (L1 − L2 )c5 s6 c6 θ̇52 + (L1 − L2 )s5 (c26 − s26 )θ̇5 θ̇6 + L3 s5 θ̇5 θ̇6
2
2
2
1
1
h2 = (L1 − L2 )c5 s6 c6 θ̇4 θ̇5 + (L1 − L2 )s6 c6 θ̇5 θ̇6 + (L1 − L2 )s5 (c26 − s26 )θ̇4 θ̇6
2
2
1
h3 = L3 s5 θ̇4 θ̇5
2
(c) All we need now to obtain the dynamics model, eq.(7.20), is the term stemming from the potential
V and the vector of nonconservative generalized forces, namely,


0
∂V
=  m6 ga sin θ5 
∂θ
0
and
  

τ
b θ̇ + τ4C sign(θ̇4 )
∂∆  4   4 4
∂ΠA
−
= τ5 − b5 θ̇5 + τ5C sign(θ̇5 ) 
φn =
∂ θ̇
∂ θ̇
τ
b θ̇ + τ C sign(θ̇ )
6
6 6
6
6
Therefore, the dynamics model of the wrist is
1
J2 + (K1 + L1 c26 + L2 s26 )s25 + (K3 + L3 )c25 θ̈4 + (L1 − L2 )s5 s6 c6 θ̈5 − L3 c5 θ̈6 + L3 s5 θ̇5 θ̇6
2
1
+(K1 + L1 c26 + L2 s26 − K3 − L3 )s5 c5 θ̇4 θ̇5 + (L2 − L1 )s6 c6 s25 θ̇4 θ̇6 + (L1 − L2 )c5 s6 c6 θ̇52
2
1
+ (L1 − L2 )s5 (c26 − s26 )θ̇5 θ̇6 + b4 θ̇4 + τ4C sign(θ̇4 ) = τ4
2
1
(L1 − L2 )s5 s6 c6 θ̈4 + (K2 + L1 s26 + L2 c26 )θ̈5 + (L1 − L2 )c5 s6 c6 θ̇4 θ̇5 + (L1 − L2 )s6 c6 θ̇5 θ̇6
2
1
+ (L1 − L2 )s5 (c26 − s26 )θ̇4 θ̇6 + m6 gas5 + b5 θ̇5 + τ5C sign(θ̇5 ) = τ5
2
1
− L3 c5 θ̈4 + L3 θ̈6 + L3 s5 θ̇4 θ̇5 + b6 θ̇6 + τ6C sign(θ̇6 ) = τ6
2
106
7.7 (a) The kinetic energy of link i can be written as
1 T
t Mi ti
2 i
Ti =
where
ti = [ ω Ti
and
ċTi ]
I
Mi = i
O
T
O
mi 1
Since the inertia matrices are given in link-fixed coordinates, or [ Ii ]i+1 ,
inspection, we have



cos θ1 0
sin θ1
cos θ2 − sin θ2
Q1 =  sin θ1 0 − cos θ1  , Q2 =  sin θ2
cos θ2
0
1
0
0
0
Thus,
Moreover,
[ ω 1 ]2 = QT1 θ̇1 [ e1 ]1 = [ 0 θ̇1 0 ]T
[ ω 2 ]3 = QT2 [ ω 1 ]2 + θ̇2 [ e2 ]2 = [ sin θ2 θ̇1 cos θ2 θ̇1
[ c1 ]2 = QT1 [ c1 ]1 = [ 0
Therefore,
we need [ ti ]i+1 . By

0
0
1
θ̇2 ]
T
b T
2]
a
−−−→
−−−→
[ c2 ]3 = QT2 [ O1 O2 ]2 + [ O2 C2 ]3

 
 

a sin θ2
c/2
c/2 + a sin θ2
=  a cos θ2  +  0  =  a cos θ2 
0
b
b
[ ċ1 ]2 = [ ω 1 × c1 ]2 = [ 2b θ̇1
[ ċ2 ]3 = [ ω 2 × c2 ]3 = [ cos θ2 (bθ̇1 − aθ̇2 )
c
2 θ̇2
0
0 ]T
− sin θ2 (bθ̇1 − aθ̇2 )
− 2c cos θ2 θ̇1 ]
The total kinetic energy is given by
T =
with

0
 θ̇1 


 0 
t1 = [ t1 ]2 =  b  ,
 2 θ̇1 


0
0

1 T
1
t M1 t1 + tT2 M2 t2
2 1
2


t2 = [ t2 ]3
I11
I
 12

I
[ M1 ]2 =  13
 0

0
0
I12
I22
I23
0
0
0
107
I13
I23
I33
0
0
0

sin θ2 θ̇1


cos θ2 θ̇1




θ̇2


=

 c cos θ2 (bθ̇1 − aθ̇2 )

 2 θ̇2 − sin θ2 (bθ̇1 − aθ̇2 ) 
− 2c cos θ2 θ̇1

0
0
0
0
0
0 

0
0
0 

m1 0
0 

0 m1 0
0
0 m1
(154)


J11 J12 J13 0
0
0
0
0 
 J12 J22 J23 0


0
0 
 J13 J23 J33 0
[ M 2 ]3 = 

0
0 m2 0
0 
 0


0
0
0
0 m2 0
0
0
0
0
0 m2
Finally, performing the required calculations, we obtain
1
m1 b 2
m2 c2
2
2
2
2
T =
I22 +
+ J11 sin θ2 + J22 cos θ2 + 2J12 sin θ2 cos θ2 + m2 b +
cos θ2 θ̇12
2
4
4
1
m2 c2
1
J33 + m2 a2 +
+ m2 ac sin θ2 θ̇22 + J13 sin θ2 + J23 cos θ2 − m2 ab − m2 bc sin θ2 θ̇1 θ̇2
+
2
4
2
(b) The kinetic energy T can be written as
T =
By inspection, we obtain
1 T
θ̇ I(θ)θ̇
2
Z11
I(θ) =
Z12
with
Z12
Z22
m1 b 2
m2 c2
+ J11 sin2 θ2 + J22 cos2 θ2 + 2J12 sin θ2 cos θ2 + m2 b2 +
cos2 θ2
4
4
1
= J13 sin θ2 + J23 cos θ2 − m2 b a + c sin θ2
2
2
c
= m2 a 2 +
+ ac sin θ2 + J33
4
Z11 = I22 +
Z12
Z22
(c) We have now
İ(θ, θ̇) =
with
Ż11
Ż12
Ż12
Ż22
c2
Ż11 = 2 J11 − J22 − m2
sin θ2 cos θ2 + 2J12 cos2 θ2 − sin2 θ2 θ̇2
4
1
Ż12 = J13 cos θ2 − J23 sin θ2 − m2 bc cos θ2 θ̇2
2
Ż22 = m2 ac cos θ2 θ̇2
(d) We have I = TT MT where
M=
and the twist-shaped matrices,

0
 1

 0
T1 = 
 b/2

 0
0
M1
O
O
M2
,
T=
T1
T2
taking into account eqs.(154), have the form


sin θ2
0
0

0 
cos θ2
0



0 
0
1
 , T2 = 

b cos θ2
−a cos θ2
0 



 −b sin θ2
0
c/2 + a sin θ2
0
−(c/2) cos θ2
0
108








Then, Ṫ1 = O6×2 and




Ṫ2 = 



θ̇2 cos θ2
−θ̇2 sin θ2
0
−bθ̇2 sin θ2
−bθ̇2 cos θ2
(c/2)θ̇2 sin θ2
0
0
0
aθ̇2 sin θ2
aθ̇2 cos θ2
0








while
Ṫ =
O
Ṫ2
Since matrix M is constant, we have
İ = ṪT MT + TT MṪ
As the reader can readily verify with the help of computer algebra, by performing these calculations
we derive the result identical to (c).
7.8 Let n be the unit normal of the plane of motion and N be its cross-product matrix. Then, if φi denotes
the orientation of the body with respect to a line fixed to the plane of motion, we have
ωi = φ̇i n,
Ωi = φ̇i N
Likewise, the velocity ċi of the mass center can be written as
ċi = ẋi li + ẏi mi
where {li , mi , n} is an orthonormal triad defining a frame F fixed to the body.
Therefore, ω i and ċi can be expressed, in terms of the joint rates { θ̇j }n1 , as
ω i = θ̇1 n + θ̇2 n + · · · + θ̇i n
ċi = θ̇1 r1 + θ̇2 r2 + · · · + θ̇i ri
where all vectors { ri }n1 ⊥ n, i.e., they lie in the plane of motion. Therefore, Ti has the form
n n ··· n
Ti =
r1 r2 · · · ri
Moreover,
Wi = φ̇i
N
O
O
O
Therefore, if N is the cross-product matrix of n, then
N O
n n ··· n 0
WiT Ti = φ̇i
O O
r1 r2 · · · ri 0
Nn Nn · · · Nn 0 . . .
= φ̇i
0
0 ··· 0 0 ...
|
{z
n columns
... 0
... 0
0
0
}
But, by virtue of the definition of N, Nn=0. Therefore, WiT Ti = O6n where O6n is the 6 × n zero
matrix; thus,
TTi Wi = O6n
Finally, we have
TTi Wi Mi Ti = Onn
109
f6
n6
τ6
For
← f
← a6 × f + n
← nT6 e6
i = 5 till 1 step −1 do
fi ← fi+1
ni ← ai × fi+1 + ni+1
τi ← nTi ei
enddo
Figure 31:
Table 5:
Row #
2
6
7
Total
Mr
6
6×5
3×5
51
Ar
6
6×5
2×5
46
7.9 The recursive algorithm is based on the free-body diagram of each link of the manipulator. If we
consider the last link of the manipulator with the force f and moment n applied on the EE, then, from
the equilibrium equations of the EE, we have
f6 = f
n6 = a6 × f + n
where a6 is the vector directed from O6 to the operation point P of the EE, as defined in eq.(4.3a).
Clearly, this force and moment represent the constraint wrench acting at the sixth joint. The torque
required at this joint to counteract this moment is simply given by
τ6 = nT6 e6
where e6 is a unit vector directed along the axis of the joint. Now, the same analysis can be performed
for the fifth link, namely,
f5 = f6
n5 = a5 × f6 + n6
τ5 = nT5 e6
Thus, applying the same analysis recursively to all manipulator links, all joint torques required to
balance a wrench w acting at the EE are obtained, the underlying algorithm being given in Fig. 31.
A summary of the computational costs of this algorithm is given in Table 5. The total numbers of
multiplications Mr and additions Ar required by the algorithm are
Mr = 51,
Ar = 46
Another way to compute these joint torques is by the use of the transpose Jacobian, as
τ = JT w
110
The calculation of the Jacobian matrix of a six-axis manipulator requires 202 multiplications and
108 additions. Note that this calculation involves vectors that have to be precomputed also with the
foregoing algorithm. Hence, for a fair comparison, we have to assume here that the Jacobian matrix is
available. Under these conditions, then, 36 multiplications and 30 additions are required to compute
the foregoing product JT w. Thus, the recursive algorithm requires 15 multiplications and 16 additions
more than the Jacobian-based calculation.
7.10 The equations of motion of a n revolute serial manipulator can take the form of eq.(7.20), namely,
"
#T
1 ∂(Iθ̇)
∂V
I(θ)θ̈ + İ(θ, θ̇)θ̇ −
θ̇ −
= φn
2
∂θ
∂θ
For the Newton-Euler algorithm, gravity is not explicitly considered. Therefore, we can remove the
∂V /∂θ term from the previous equation. Moreover, taking θ̈ = 0, the left-hand side of this equation
becomes the term of Coriolis and centrifugal forces sought. Therefore, all we have to do to obtain this
term using the Newton-Euler algorithm, is to eliminate the θ̈i term, all other statements remaining
the same. For Algorithm 7.4.1, the term θ̈i appears only in the 10th line. By eliminating this term,
we save 1 addition, which means that n additions are saved for the whole manipulator, while the
outward recursions requires 82n M and 65n A. Since Algorithm 7.4.2 remains exactly the same, and
hence, (55n − 22) M and (44n − 14) A are required for the inward recursions. Therefore, a total of
(137n − 22) M and (109n − 14) A are required to compute the term of Coriolis and centrifugal forces
of an n-revolute serial manipulator using the Newton-Euler algorithm.
7.11 (a) The kinetic energy of link i can be written as
Ti =
where
ti =
[ ω Ti
ċTi ]T
1 T
t Mi ti
2 i
I
and Mi = i
O
O
mi 1
Since the inertia matrices are given as [ Ii ]i+1 , we need [ ti ]i+1 . We have, for the DH parameters
of Table 7.5 of the Exercises, λ1 = λ2 = 0, µ1 = µ2 = 1. Therefore, using eq.(4.1d), we obtain






c1 0 s1
c2 0 s2
1 0 0
Q1 =  s1 0 −c1  , Q2 =  s2 0 −c2  , Q3 =  0 1 0 
0 1
0
0 1
0
0 0 1
Thus,
T
[ ω 1 ]1 = θ̇1 [ e1 ]1 = [ 0 0 θ̇1 ]

   
c1 s1 0
0
0
[ ω1 ]2 = QT1 [ ω 1 ]1 =  0
0
1   0  =  θ̇1 
s1 −c1 0
θ̇1
0
     
0
0
0
[ ω2 ]2 = [ ω 1 ]2 + θ̇2 [ e2 ]2 =  θ̇1  +  0  =  θ̇1 
0
θ̇2
θ̇2


  
c2 s2 0
0
s2 θ̇1
[ ω2 ]3 = QT2 [ ω 2 ]2 =  0
0
1   θ̇1  =  θ̇2 
−c2 θ̇1
s2 −c2 0
θ̇2


s2 θ̇1
[ ω 3 ]3 = [ ω2 ]3 ; [ ω3 ]4 = QT3 [ ω 3 ]3 =  θ̇2 
−c2 θ̇1
111
Moreover,
[ c1 ]1 = [ c2 ]2 = 0,
[ c3 ]3 = [ 0 0
b3 ]
T
Therefore,
[ ċ1 ]2 = [ ċ2 ]3 = 0
[ ċ3 ]3 = ḃ3 [ e3 ]3 + [ ω3 × c3 ]3 = [ b3 θ̇2
[ ċ3 ]4 = QT3 [ ċ3 ]3 = [ b3 θ̇2
Hence,
T1 =
1
[ 0 θ̇1
2
0
Similarly,
and
T3 =
0 0

−b3 s2 θ̇1
A
0

0
0]
0

0
0
0 0
0
A 0
0
0 A 0
0 0 m1
0 0
0
0 0
0
T2 =
1
B(θ̇12 + θ̇22 )
2
−b3 s2 θ̇1
ḃ3 ]
ḃ3 ]T
T
0
0
0
0
m1
0
 
0
0
0   θ̇1 
 
0  0  1 2
   = Aθ̇
0  0  2 1
 
0
0
m1
0
1
1
1
C(θ̇12 + θ̇22 ) + m3 b23 (s22 θ̇12 + θ̇22 ) + m3 ḃ23
2
2
2
Thus,
1
1
1
[A + B + C + m3 b23 s22 ]θ̇12 + [B + C + m3 b23 ]θ̇22 + m3 ḃ23
2
2
2
If we use the Y1 Z1 plane as a reference, the potential energy of the system is
T =
V = m3 gc3 · (−i)
where i is a unit vector parallel to the X1 axis. We have

c1 c2 s1
[ c3 ]1 = Q1 Q2 [ c3 ]3 = P2 [ c3 ]3 =  s1 c2 −c1
s2
0
Thus,


 
0
c1 s2
c1 s2
s1 s2   0  = b 3  s1 s2 
b3
−c2
−c2
c3 · (−i) = −b3 c1 s2
and
V = −m3 gb3 c1 s2
Hence,
∂T
= [A + B + C + m3 b23 s22 ]θ̇1
∂ θ̇1
∂T
= [B + C + m3 b23 ]θ̇2
∂ θ̇2
∂T
= m3 ḃ3
∂ ḃ3
d ∂T
= [A + B + C + m3 b23 s22 ]θ̈1 + 2m3 b3 s22 ḃ3 θ̇1 + 2m3 b23 s2 c2 θ̇1 θ̇2
dt ∂ θ̇1
d ∂T
= [B + C + m3 b23 ]θ̈2 + 2m3 b3 ḃ3 θ̇2
dt ∂ θ̇2
112
d
dt
∂T
∂ ḃ3
∂T
∂θ1
∂T
∂θ2
∂T
∂b3
∂V
∂θ1
∂V
∂θ2
∂V
∂b3
= m3 b̈3
=0
= m3 b23 s2 c2 θ̇12
= m3 b3 s22 θ̇12 + θ̇22
= m3 gb3 s1 s2
= −m3 gb3 c1 c2
= −m3 gc1 s2
Therefore, the Euler-Lagrange equations of the manipulator are
A + B + C + m3 b23 s22 θ̈1 + 2m3 b3 s22 ḃ3 θ̇1 + 2m3 b23 s2 c2 θ̇1 θ̇2 + m3 gb3 s1 s2 = τ1
B + C + m3 b23 θ̈2 + 2m3 b3 ḃ3 θ̇2 − m3 b23 s2 c2 θ̇12 − m3 gb3 c1 c2 = τ2
m3 b̈3 − m3 b3 s22 θ̇12 + θ̇22 − m3 gc1 s2 = f3
(b) By inspection,

A + B + C + m3 b23 s22

I=
0
0
0
B + C + m3 b23
0

0
0 
m3
Note that the first two diagonal entries of I have units of moment of inertia, while the third
consistently has units of mass.
7.12 a) The link is inertially isotropic when its inertia matrix I is isotropic. Then, if we denote the triple
eigenvalue of I with I, we can write
I = I 1.
(155)
with 1 denoting the 3 × 3 identity matrix. Therefore, for an arbitrary three-dimensional vector x,
Ix = Ix,
(156)
and hence, x is an eigenvector of I of eigenvalue I, q.e.d.
b) If all the links are inertially isotropic, then we have,
Ii = Ii 1.
(157)
P
nP
n = In ω̇ n − n + ρn × fn .
(158)
Line 2 of Algorithm 7.4.2 then changes to
This amounts to (3 M ) + 3 A + 3 A + (3 M + 6 A), or a total of 6 M and 12 A, which compared with
30 M and 27 A for the general case, line 2 of Table 7.2, gives savings of 24 M and 25 A.
Line 10 of Algorithm 7.4.2, in turn, changes to
P
P
nP
i = Ii ω̇ i + ρi × fi + Qi ni+1 + δ i × φi+1 .
113
(159)
This amounts to (3 M ) + 3 A + (3 M + 6 A) + 3 A + (8 M + 4 A) + 3 A + (3 M + 6 A) per revolute,
or a total of 17(n − 1) M and 25(n − 1) A, which compared with 44(n − 1) M and 37(n − 1) A for the
general case, line 7 of Table 7.2, gives savings of 24(n − 1) M and 12(n − 1) A. Thus, the total savings
in multiplications, denoted Ms , and additions, denoted As , are given below:
Ms = 24n,
As = 12n + 13
7.13 For a planar manipulator, its 3n × n natural orthogonal complement T is given by


t11
0 ··· 0
 t21 t22 · · · 0 
T≡
..
.. 
..
 ...
.
.
. 
tn1
tn2
· · · tnn
where the 3-dimensional vector tij is, for a revolute pair, given by

1



 Erij , if j ≤ i;
tij = 0


,
otherwise.


0
For a prismatic pair,
tij =
with rij defined as
From eq.(7.108), we have
0
,
ek
1≤k≤i


 aj + aj+1 + · · · + ai−1 + ρi , if j < i;
if j = i;
rij ≡ ρi ,

 0,
otherwise.
I
Mi = i
0
0T
mi 1
As in the general case, the 3n × 3n matrix M is given by
M = diag(M1 , . . . , Mn )
We can factor this matrix as
M = HT H
which turns out to be straightforward due to its block-diagonal structure. In fact, H is given by
H = diag(H1 , . . . , Hn )
each 3 × 3 block Hi being given, in turn, as
Hi =
Ni
0
0T
ni 1
We thus have
Mi = HTi Hi
which makes apparent that
Ni =
p
Ii ,
114
ni =
√
mi
Algorithm 7.6.1a:
For j = 1 to n step 1 do
rjj ← [ρj ]j+1 Nj
pjj ←
nj Erjj j+1
For i = j + 1 to n step 1 do
if R then
T
rij ← Q
i [ ri−1,j + δ i−1 ]i + [ ρi ]i+1
Ni
pij ←
ni Erij i+1
else
0
pij ←
ni ej i+1
endif
enddo
enddo
From the foregoing definitions, then, the n × n matrix of generalized inertia I can now be expressed as
I = PT P
where P is defined, in turn, as the 3n × n matrix given below:
P ≡ HT
which can be written as

p11
 p21
P=
 ..
.
pn1

0
0 
.. 

.
0
p22
..
.
···
···
..
.
pn2
· · · pnn
where the 3-dimensional arrays pij are given as
 Ni



 ni Erij
pij ≡ Hi tij =


0


ni e j
if the jth joint is R;
if the jth joint is P
Therefore, for a planar manipulator, Algorithm 7.6.1 reduces to Algorithm 7.6.1a. The computational
cost of this algorithm is now evaluated. The first statement of the outermost loop involves no floatingpoint operations; the second statement involves (a) one multiplication of a matrix by a vector, and
(b) one multiplication of a scalar by a vector. Of the last two items, (a) is nothing but the product of
matrix E given by
0 −1
E=
1 0
by vector rjj . This product involves zero floating-point operations, for the result is, simply, [ Erjj ]j+1 =
[ −y, x, 0 ]T , with x and y denoting the Xj+1 and Yj+1 components of rjj . Hence, item (a) requires
no floating-point operations, while item (b) requires 2n multiplications and zero additions.
115
The innermost do-loop, as pertaining to revolute manipulators, involves one coordinate transformation
between two consecutive coordinate frames, from Fi - to Fi+1 -coordinates, plus two vector sums, which
consumes 4(n − i) multiplications and 6(n − i) additions; this loop also consumes one matrix-timesvector multiplication, with E being the said matrix, which involves zero floating-point operations, as
mentioned above, and one scalar-times-vector multiplication, which requires 2(n − i) multiplications
and zero additions. Thus, the total numbers of operations required by this algorithm, for a n-revolute
manipulator, are Mia multiplications and Aia additions, as given below:
Mia = 2n +
n
X
i=1
Aia =
n
X
i=1
6(n − i) = n(3n − 1)
6(n − i) = 3n(n − 1)
the presence of prismatic pairs reducing the above figures.
8
Special Topics on Rigid-Body Kinematics
8.1 (a) We have, from Fig. 7,
 
0
p1 =  0  ,
0
and from the data,

√1/2
p2 =  3/2  ,
0


−1/2
√
p3 =  3/2  ,
0

ṗ1 = [ 0
√
3/3
Now, let us express ṗ2 in the form
ṗ2 = [ ẋ2
0


√0
p4 =  √3/3 
6/3
√
T
6/3 ]
0 ]T
Then, from the relation
(ṗ2 − ṗ1 ) · (p2 − p1 ) = 0
we obtain ẋ2 = 1. Hence,
ṗ2 = [ 1
0 0]
T
(b) Now, we must have, for compatibility, with ṗ3 = [ẋ3 , ẏ3 , ż3 ]T ,
√
3
1
(ṗ3 − ṗ1 ) · (p3 − p1 ) = − ẋ3 +
2
2
which leads to
ẋ3 =
√ !
3
ẏ3 −
=0
3
√
3 ẏ3 − 1
(160)
If ṗ3 = 0, we will have ẋ3 = ẏ3 = ż3 = 0, which does not agree with eq.(160). Thus, ṗ3 cannot
be zero.
(c) If ṗ3 lies in the P1 P2 P3 plane, then ż3 = 0. Again, we must have
(ṗ3 − ṗ2 ) · (p3 − p2 ) = −(ẋ3 − 1) = 0
√
From eqs.(160) and (161), we have ẋ3 = 1 and ẏ3 = 2 3/3, i.e.,
√
ṗ3 = [ 1 2 3/3 0 ]T
116
(161)
(d) Let the angular velocity be
ω = [ ω1
ω2
ω3 ]
T
Then,
ṗ2 − ṗ1 = ω × (p2 − p1 )
That is
from which we obtain
√



− 3 ω3
√1
 − 3/3  = 1 

ω3
√
2 √
− 6/3
3 ω1 − ω2

√
ω3 = −2 3/3
√
√
3 ω1 /2 − ω2 /2 = − 6/3
(162)
(163)
We also have
ṗ3 − ṗ1 = ω × (p3 − p1 )
which yields
√
√
3 ω1 /2 + ω2 /2 = − 6/3
(164)
The solution of eqs.(162)–(164) is thus
√
√
ω = [ −2 2/3 0 −2 3/3 ]T
Alternatively, we can apply eq.(8.9) to compute ω.
(e) Let p and ṗ be the position and velocity vectors of a point P on the instantaneous screw axis.
We have
√


√2 3
√
1 √
ṗ = ṗ1 + ω × (p − p1 ) =
3 −√2 3 x√+ 2 2 z 
3
6 − 2 2y
From ṗ × ω = 0, we have
√
  
0
3 −√
6x + 2 6 z
1
 = 0
3
−
10y
(165)
2
√
√
3
− 6 + 2 6 x − 4z
0
√
√
Solving eq.(165) for y and z, we obtain y = 3/5 and z = 6 (2x − 1)/4. Thus, the distance
between P and the origin is

d2 = x2 + y 2 + z 2 =
2 2 3
3
3
x − x+ +
5
2
8 25
Now, d2 is minimized by setting its derivative with respect to x equal to zero, thereby obtaining
x = 3/10. Hence,
√
√
T
p = [ 3/10
3/5 − 6/10 ]
(166)
Moreover, we can readily obtain the unit vector e in the direction of the instant screw axis as
√
√
√
ω
3 5
e=
=
[ −2 2/3 0 −2 3/3 ]T
(167)
kωk
10
From eqs.(166) and (167), the screw axis is totally determined. Alternatively, we can apply
formulas (3.72) to find the above value of p.
117
Figure 32: Auxiliary coordinate frame.
Figure 33: Relative layout of the A and F frames.
8.2 (a) We first verify compatibility, which requires computing matrices P and Ṗ, and these require, in
turn,
 
 
1
1
1 
c=
1 , ċ =  1 
3
1
1
Therefore,
whence,


1 1 −2
2
P =  1 −2 1  ,
3
1 1 −2

0
Ṗ = 2  0
0

1 −1
0
0 
−1 1
ṖT P = O
which is apparently skew-symmetric, the motion thus being possible.
(b) To compute ω, we must verify first whether the points are collinear. A quick calculation shows
that


1
(p2 − p1 ) × (p3 − p1 ) = 4  0  6= 0
−1
118
the points thus being noncollinear. Moreover, tr(P) = −2, while


0 −1 1
4
P2 =  0 2 −2 
3
0 −1 1
and hence, tr2 (P) = tr(P2 ), even though the points are noncollinear. Thus, we cannot find ω
from eq.(8.9), for matrix D is singular. However, since P is not frame-invariant, it is possible to
render D invertible upon a change of frame. Thus, let A be an auxiliary frame {XA , YA , ZA },
with origin at C and axes XA and YA defined as in Fig. 32. Hence,






4/3
−2/3
−2/3
√
√
√
[p1 ]A =  −2 2/3  , [p2 ]A =  −2 2/3  , [p3 ]A =  4 2/3  ,
0
0
0
and


−2/3
4/3
−2/3
√
√
√
[P]A =  −2 2/3 −2 2/3 4 2/3 
0
0
0
√
√
12 + 8 2
−2 − 2 2
2
⇒ tr([P]A ) =
⇒ tr ([P]A ) =
3
9
Moreover,
√
√
√ 

√
4 − 8√2 −8 − 8√ 2 4 + 16 √2
1
12 − 16 2
2
2


[P ]A =
8 + 4 2 8 − 8 2 −16 + 4 2 ⇒ tr([P ]A ) =
6= tr2 ([P]A )
9
9
0
0
0
which means that matrix [D]A is invertible. In fact,
 √

−√ 2 −2
1√
1
[D]A =  2 −1
−2 √
2 
3
0
0 −1 − 2
whose determinant is readily calculated as
p
−2 − (2)
det([D]A ) =
6 0
=
9
In order to find ω, then, all we need is the right-hand side of eq.(8.9), which requires calculating
the given velocities in A. We show in Fig. 33 the relative layout between the original frame F
and the auxiliary frame A.
Let iA , jA , kA be the unit vectors parallel to XA , YA , ZA , respectively, while i, j, k are their
counterparts associated with F . From Fig. 33, it is apparent that


−−−→
0
[P1 P3 ]F
[ iA ]F =  −1  , [ jA ]F = −−−→
k[ P1 P3 ]F k
0
where


−2
−−−→
[ P1 P3 ]F = [ p3 − p1 ] =  0 
−2
119
Hence,




√
√
−1
1
2
2
 0 
[ jA ]F =
0  ⇒ [ kA ]F = [ iA × jA ]F =
2
2
−1
−1
Now, from Definition 2.2.1, the matrix Q rotating F into A is given by
√
√


0 − 2/2
2/2

[Q]F = [ iA jA kA ]F =  −1
√0
√0
0 − 2/2 − 2/2
Therefore,
[ṗi ]A = [QT ]F [ṗi ]F ⇒ [Ṗ]A = [QT ]F [Ṗ]F
where
Thus,



0 1 −1
√ 0
[Ṗ]F = 2  0 0
0  ⇒ [Ṗ]A = 2 2  0
0
0 −1 1
0
0
1

0
0 
−1
 
√ 1
vect([Ṗ]A ) = 2  0 
0
and hence, eq.(8.9) leads to
ω = D−1 vect([Ṗ]A )
with D−1 calculated, using computer algebra, as
√
√
√ 

√
4 2
−1 − √2 2(1 + √2) −1 − √
2− 2
D−1 =
−2 − 2 −2 − 2
4 −√ 2 
2
0
0
−3 2
whence,
√ 

√
−2 − √2
2− 2
[ω]A =
−2 − 2 2 
2
0
Therefore,
 
1
[ω]F = [Q]F [ω]A =  1 
1
8.3 If the three points are collinear, all three vectors pi − c, for i = 1, 2, 3, are linearly dependent, and
matrix P is of rank 1, its nullspace being a plane passing through the origin and normal to line P1 P2 P3 .
Matrix P being of rank 1, its three eigenvalues are {π1 , 0, 0}, with π1 6= 0. Hence, tr2 (P) = π12 =tr(P2 ).
Notice that the foregoing relation holds regardless of whether the origin is collinear with the three points
or not.
8.4 From the compatibility condition,
ṖT P + PṖT = O
Taking the trace of both sides, we obtain
tr(ṖT P + PṖT ) = 0 ⇒ tr(ṖT P) + tr(PṖT ) = 0
Now, because tr(AB) = tr(AB) for all square matrices A and B, then
2 tr(PṖT ) = 0 =⇒ tr(PṖT ) = 0
120
Alternatively, one can write


3
(p1 − c)T
X
PPT = [ p1 − c p2 − c p3 − c ]  (p2 − c)T  =
(pi − c)(pi − c)T
T
1
(p3 − c)
Upon differentiation of both sides with respect to time, we have
ṖT P + PṖT =
3
X
(ṗi − ċ)(pi − c)T + (pi − c)(ṗi − ċ)T
1
Taking the trace of both sides, while considering tr(AB) = tr(BA),
2 tr(PṖT ) = 2
3
X
1
(ṗi − ċ)T (pi − c) = 0
and hence, tr(PṖT ) = 0.
8.7 (a) The data satisfy the relation tr2 (P) = tr(P2 ) with P as defined in eq.(8.4), and hence, a change of
frame is required. We will consider a frame F1 with its origin at P1 and its Z1 -axis perpendicular
to the plane defined by P1 , P2 and P3 . Moreover, its X1 -axis is aligned with a vector going from
P2 to P1 . In the new frame F1 we have
 




0
−2
0√
[p1 ]1 =  0  , [p2 ]1 =  0  , [p3 ]1 =  −2 2 
0
0
0
Moreover,
[c]1 =
3
1X
3
1

−1
√
2
[pi ]1 =  − 2 
3
0

The rotation matrix that brings F1 into an orientation coincident with the original frame F is
given by


1 √0
√0

Q =  √2/2 0
√2/2
2/2 0 − 2/2
The accelerations of the three given points in F1 are thus
[p̈i ]1 = Q[p̈i ]F ,
i.e.,


√1
[p̈1 ]1 =  2  ,
0
Moreover,

1
2 √
[P]1 =
2
3
0
−2
√
2
0
Furthermore,
tr[P]1 =
√
2
(1 + 2),
3
for i = 1, 2, 3


√1
[p̈2 ]1 =  √2  ,
2 2

√1
[p̈3 ]1 =  √
2 
−2 2
√

1 − 2√ 2
4
[P2 ]1 =  2 + 2
9
0

0√
−2 2  ,
0
tr2 [P]1 =

√
4
(3 + 2 2),
9
121
tr([P2 ]1 ) =
√
√ 
−2 − 2√ 2 4 2
2−2 2
−4 
0
0
√
4
(3 − 4 2) 6= tr2 [P]1
9
From the data,
3
[c̈]1 ≡
and thus

0
[P̈]1 =  0
0
1X
[p̈i ]1 = [ 1
3 1
0
0
√
2 2
√
2
0]
T
√ 
2
vect[P̈]1 =  0 
0

0
0√  ;
−2 2
To be compatible, the motion must satisfy eqs.(8.15) and (8.25), namely,
ṖT P + PṖT = O
T
T
T
P̈ P + 2Ṗ Ṗ + P P̈ = O
(168)
(169)
Equation (168) is obviously satisfied since Ṗ = O, and eq.(169) reduces to



0 0
0
1 √
−2
0√
√
√
2
P̈T P + PT P̈ =  0 0 2 √2   2
2 −2 2 
3
0 0 −2 2
0
0
0
√



1
2
0
0
0
0
√
2
2
00
0
0√ 
+  −2
√
√
3
0 −2 2 0
0 2 2 −2 2

 

0 0 0
0 0 0
= 0 0 0 + 0 0 0 = O
0 0 0
0 0 0
the motion thus being compatible. The matrix D, defined in eq.(8.10), is then calculated as
 √

0
2 √2/3 4/3
√
1

[D]1 =  −2 2/3 2/3
4 2/3
√
2
0
0 2(1 + 2)/3
Moreover, since the velocities of the three points vanish, vect(Ṗ) = 0, and eq.(8.9) reduces to
Dω = 0
Since det(D) 6= 0, the solution of the foregoing system of equations is ω = 0.
(b) Equation (8.23) reduces to
Dω̇ = vect(P̈)
We thus have
 √
2 2/3
1 √
−2 2/3
2
0
(170)
  √ 
4/3
ω̇1
2
√0
  ω̇2  =  0 
2/3
4 2/3
√
0 2(1 + 2)/3
ω̇3
0
Solving this set of three equations for ω̇1 , ω̇2 and ω̇3 , we obtain
√
T
[ω̇]1 = [ 1
2 0]
which can be expressed in the original frame F as
[ω̇]F = QT [ω̇]1 = [ 1 1
122
1 ]T
8.9 (a)
R = [ p1 − c
and hence,
J=


3
(p1 − c)T
X
p2 − c p3 − c ]  (p2 − c)T  =
(pi − c)(pi − c)T
T
1
(p3 − c)
3
X
kpi − ck2 1 − (pi − c)(pi − c)T = tr(R)1 − R
(171)
1
which is indeed, according to eq.(3.136), the moment of inertia of a set of three punctual unit
masses placed at the three given points, with respect to the centroid C of the given points.
(b) Upon differentiation of eq.(171), we have
d
d
(tr(R)1 − R) =
tr(PPT )1 − PPT
dt
dt
J̇ = tr(ṖPT )1 + tr(PṖT )1 − ṖPT − PṖT
J̇ =
From eq.(8.16), tr(PṖT ) = 0, which implies that tr(ṖPT ) = 0. Moreover, it is apparent that
Ṗ = ΩP,
ṖT = PT ΩT = −PT Ω
and hence,
J̇ = −ΩPPT + PPT Ω
Therefore,
J̇ = RΩ − ΩR
(172)
(c) Now, upon differentiation of eq.(172), we have
d
PPT Ω − ΩPPT
dt
= ṖPT Ω + PṖT Ω + PPT Ω̇ − Ω̇PPT − ΩṖPT − ΩPṖT
J̈ =
= ΩPPT Ω + PPT ΩT Ω + PPT Ω̇ − Ω̇PPT − ΩΩPPT − ΩPPT ΩT
= ΩRΩ − RΩ2 + RΩ̇ − Ω̇R − Ω2 R + ΩRΩ
J̈ = RΩ̇ − Ω̇R − Ω2 R − RΩ2 + 2ΩRΩ
8.10 The moment ni of the force f about the three given points {Pi }31 can be expressed as
ni = n + (pi − c) × f =⇒ ni − n = (pi − c) × f
(173)
The moment about the centroid C can be determined from eq. (173)
3
X
1
(ni − n) =
3
X
1
(pi − c) × f
(174)
Moreover, eq. (174) can be written in the form M = −Fp where F = CPM(f ). Upon application of
Theorem A.1,
−vect(M) = Df =⇒ f = −D−1 vect(M)
123
8.11 From Fig. 7, we obtain the points
 
0
p1 =  0  ,
0
and thus

√1/2
p2 =  3/2  ,
0


−1/2
√
p3 =  3/2 
0
 
√
3
0
1X
3 
c=
pi =
1
3 1
3
0
Furthermore, let
 
√
3
1
2 
1X
ni =
0
n≡
3 1
12
0
Now, we define
P ≡ [ p1 − c p2 − c
i.e.,

p3 − c ] ,


0
3 √
−3
1 √ √
P=
−2 3
3
3,
6
0
0
0
N ≡ [ n1 − n
n2 − n
 √
−2 2
1
N=
0
6
0
√
√2
−√ 6
3
T
n3 − n ]
√ 
√2

√6
− 3
Note that the nullspaces of P and N are both spanned by [ 1 1 1 ] , which is a feature of this type
of matrices. As a check, the product NT P must be skew-symmetric:


√
0 −1 1
2
NT P =
1
0 −1 
6
−1 1
0
This matrix being skew-symmetric, the moments are compatible, which is the static counterpart of the
compatibility equation (8.15). Furthermore,
ni − n = (pi − p) × f ≡ F(p − pi )
where F is the cross-product matrix of f, and thus
FP = N
whence,
vect(FP) = vect(N)
or
√
We have tr(P) = 3/6, and thus,
Moreover,
1
[tr(P)1 − P] f = vect(N)
2
 √

3 −3
3
√
1
1  √
1
[tr(P)1 − P] =
2 3 0 −√ 3  ≡
A
2
12
12
0
0
3
∆ ≡ det(A) =
√
√
3[0 − 2 × (3 3)] = −18 6= 0
124
(175)
A thus being nonsingular. Furthermore,
√ 
√
3√
− 6
1 
;
vect(N) =
√2
12
− 2


fx
f =  fy 
fz
Hence, from eq.(175), we obtain
√
√
√
3 fx − 3fy + 3fz = −( 6 − 3)
√
√
√
2 3 fx − 3 fz = 2
√
√
3 fz = − 2
(176a)
(176b)
(176c)
Solving eqs.(176a–c), we obtain


0
1 √ 
f =−
3 ,
3 √
6
with
kf k2 =
1
(3 + 6) = 1
9
Now, the line sought is the wrench axis. This is a line of direction f passing through a point P0 of
position vector p0 that verifies
f × ni
Fp0 = f × pi +
(177a)
kf k2
f T p0 = 0
(177b)
for some pi and its corresponding ni . If we choose i = 1, the computational effort is reduced. Thus,
Fp0 =
We have
f × (f × n1 )
kf k2


√
0
2 √ 
f × n1 =
√6 ,
12
− 3
 
√
1
2 
f × (f × n1 ) =
0
4
0
Equations (177a & b) can be written as
Bp0 = d
where
F
B≡ T
f
(178)
−F2
|{z}
+ff T = kf k2 1 = 1
f × (f × n1 )
d≡
0
,
Now, since we are working with infinite precision—no floating-point arithmetic, and hence, the roundoff
error is zero—we can safely use the formula from the left Moore-Penrose generalized inverse of Appendix
B. With the aid of this inverse, the “least-square approximation” p0 of the overdetermined linear system
(178) is computed as
p0 = (BT B)−1 BT d
We have,
BT B = [ −F f ]
F
fT
=
Hence,
f × (f × n1 )
= −F [f × (f × n1 )] + 0 = [f × (f × n1 )] × f
0
√
√ T
1
p0 =
[0 2 3 − 6]
12
p0 = [ −F f ]
kf k2 1−ff T
125
Finally, the minimum-magnitude moment is obtained as
n0 =
ni · f
f,
kf k2
for i = 1, 2 or 3
However, ni · f = 0, for i = 1, 2, 3, and hence,
n0 = 0
8.12 We have
Ṗ = ΩP,
R = PPT
Multiplying both sides of eq. (8.6) by PT , we obtain
ṖPT = ΩPPT = ΩR
Recalling Theorem A.1,
1
Jω
2
Now, to show that matrix J is frame-invariant, we introduce two different frames, labeled A and B,
in which the representation of J is [J]A and [J]B , respectively. From the definition of p, moreover,
[p]A = [Q]A [p]A . Additionally, let [Q]A denote the rotation of frame A into frame B. Thus,
vect(ṖPT ) =
[J]A = tr([p]A [pT ]A )1 − [p]A [pT ]A
≡ tr([Q]A [p]B [pT ]B [QT ]A )[Q]A [QT ]A − [Q]A [p]B [pT ]B [QT ]A
= [Q]A tr([QT ]A [Q]A [p]B [pT ]B )1 − [p]B [pT ]B [QT ]A
= [Q]A tr([p]B [pT ]B )1 − [p]B [pT ]B [QT ]A
≡ [Q]A [J]B [QT ]A
which shows that J is indeed frame-invariant.
8.13


0
p1 =  −30  ,
30


120
ṗ1 =  −60  ,
−60

(a)






0
0
0
p2 =  30  , p3 =  0  , ⇒ c =  0 
30
0
20






0
0
40
ṗ2 =  −60  , ṗ3 =  0  , ⇒ ċ =  −40 
60
0
0



0
0
0
80 −40 −40
0  , Ṗ =  −20 −20 40 
⇒ P =  −30 30
10 10 −20
−60 60
0

 

−30 10
80 −40 −40
30
10   −20 −20 40 
0
−20
−60 60
0



0
12 −12
0
1 −1
0
12  × 100 =  −1 0
1  × 1200 ,
=  −12
12 −12
0
1 −1 0
0
PT Ṗ =  0
0

which is apparently skew-symmetric, and hence, estimates are compatible.
126
(b) Dω = vect(ṗ) where
Hence,


10
0
0
1
1
30 −20 0  ,
D = [tr(P)1 − P] =
2
2
−10 −10 30


20
1
20 
vect(Ṗ) =
2
20




1
0 0
2
1
1
3 −2 0  =  2 
2
2
−1 −1 3
2
Matrix is lower-triangular. Use forward substitution: ω1 = 2, 3ω1 − 2ω2 = 2
−4 ⇒ ω2 = 2. Then, −ω1 − ω2 + 3ω3 = 2 ⇒ 3ω3 = 6 ⇒ ω3 = 2


2
⇒ ω =  2  rad/s
2
9
⇒
−2ω2 =
Geometry of General Serial Robots
9.1 A reflection H onto a plane Π of a unit normal n can be expressed as
H = 1 − 2nnT
(179)
Now, if the reflection plane is normal to f, which is not necessarily of unit magnitude, then vector h is
mapped by H into h′ given by
(f · f )h − 2(f · h)f
kf k2
h′ = (1 − 2 f f T /kf k2 )h =
Notice that kf k2 h′ is the left-hand side of eq.(8.22f). By the same token, we can write, for i′
i′ = (1 − 2 g gT /kgk2 )i =
(g · g)i − 2 (g · i)g
kgk2
Obviously, the right-hand side of eq.(8.22f) is equal to kgk2 i′
9.3 See Maple worksheet in Appendix 1.
9.5 Assume that we have an orthogonal matrix H chosen as a product of 12 Householder reflections6 that
will render the matrix S in an upper triangular form U. That is,
HS = U
Equation (8.51d) can be written as
ST HT Hx̃45 = 012 =⇒ (HS)T Hx̃45 = 012
(180)
where 012 is the 12-dimensional zero vector. Then, we can write eq.(180) as
UT v = 012
with
v = H x̃45
6 See
Appendix B.
127
(181)
Since S is singular, the 12th row of U is full of zeros, and so is the 12th column of UT . As a consequence,
the nullspace of UT is spanned by the unit vector n = [0T11 1]T . Consequently, vector v is a multiple
of n, i.e., v = αn, where α is a scalar, as yet to be determined, which is done below. From eq.(181),
T
x̃45 = HT v = αHT n = α h12,1 . . . h12,12
where h12,i denotes, as usual, the ith component of the 12th row of H. Upon comparison of the above
expression of x45 with its definition in eq.(8.27a), we find α as
αh12,12 = 1 ⇒ α =
1
h12,12
9.7 We have four equations according to eq.(9.70a). By selecting any two of them, we end up with a system
of two nonlinear equations f in two unknowns x. The Jacobian matrix F is given by
F≡
∂f
∂x
According to the Newton-Raphson method, we have
xi+1 = xi − F−1
i fi ,
i = 0, 1, 2, ...
where Fi and fi represent F and f evaluated at xi , respectively.
With an initial guess x0 close enough to a root, the Newton-Raphson method may converge to that
root rapidly. Given a tolerance ǫ, the criterion to stop the iteration is
kxi+1 − xi k∞ < ǫ
where k · k∞ denotes the Chebyshev norm. The Maple code implementing this calculation is given in
Appendix 2.
By monitoring the condition number of F based on the Frobenius norm, we observe that the NewtonRaphson method converges faster when the condition number is smaller. The condition number introduced in Section 5.8, with the Frobenius-norm condition number discussed in eqs.(5.79)–(5.82).
10
Kinematics of Complex Robotic Mechanical Systems
10.1 For the parallel manipulator of Fig. 9.7, the matrix mapping joint forces into wrenches acting on the
moving platform can be obtained by relating the power generated by the actuators and the power
consumed by the load. From eq.(9.102a),
ḃ = Kt
where K is the Jacobian of the manipulator given in eq.(9.102b). Under static, conservative conditions,
the power delivered by the actuators equals that developed by the load, i.e., Πa = ΠL , where
Πa = ḃT τ
ΠL = tT w
with ḃ being the vector of actuated joint rates, τ the vector of actuated joint torques, t the twist of
the moving platform, and w the wrench acting on the moving platform. Then,
ḃT τ = tT w
T
t KT τ = tT w
which is valid for every possible motion, i.e., for every possible twist t, and hence, the above equation
leads to
w = KT τ
128
10.8 Matrix Θ is given as
α cos ψ + (1/2) sin ψ
−α cos ψ + (1/2) sin ψ
Θ=
ρ [−α sin ψ + (1/2) cos ψ − δ] ρ [α sin ψ + (1/2) cos ψ + δ]
with the definitions below:
a+b
d
r
, δ≡ , ρ≡
l
l
d
Upon expanding the determinant of Θ and imposing the singularity condition, we have
α≡
det(Θ) = ρ[α + δ sin ψ] = 0
Hence,
sin ψ = −
α
α
=⇒ ≤ 1
δ
δ
which yields
a+b≤d
(182)
If the above condition holds, then the angle ψ rendering Θ singular can be evaluated as
a+b
ψ = sin−1 −
d
However, notice that many a caster wheel exhibits the value d = r; most commonly, one has has d < r.
In the extreme case d = r, condition (182) implies that a + b is smaller than the radius of the wheels,
a rather unlikely design!
10.9 To determine the conditions for matrix Θ to be isotropic, any of ΘT Θ and ΘΘT must be proportional
to the 2 × 2 identity matrix. Below we calculate these two matrices:
α1 α3
I.
ΘT Θ =
α3 α2
where
α1 = (1 − ρ2 )[(α2 − 1/4) cos ψ + α sin ψ] cos ψ − δρ2 (cos ψ − 2α sin ψ)
+ ρ2 (α2 + δ 2 ) + 1/4
α2 = (1 − ρ2 )[(α2 − 1/4) cos ψ − α sin ψ] cos ψ + δρ2 (cos ψ + 2α sin ψ)
+ ρ2 (α2 + δ 2 ) + 1/4
α3 = (α2 + 1/4)(ρ2 − 1) cos2 ψ − 2αδρ2 sin ψ − ρ2 (α2 + δ 2 ) + 1/4
and
II.
T
ΘΘ =
β1
β3
β3
β2
where
β1 = (2α2 − 1/2) cos2 ψ + 1/2
β2 = ρ2 [2(α sin ψ + δ)2 + (1/2) cos2 ψ]
β3 = −(1/2)ρ(4α2 sin ψ + 4αδ − sin ψ) cos ψ .
129
Apparently, matrix ΘΘT is simpler, and hence, we work with this matrix. Upon imposing the isotropy
condition
2
σ
0
T
ΘΘ =
0 σ2
we have g1 ≡ β1 − β2 = 0 while β1 > 0 and g2 ≡ β3 = 0, which yield the corresponding conditions:
g1 ≡ (1 + ρ2 )(2α2 − 1/2) cos2 ψ − 4αδρ2 sin ψ − 2(α2 + δ 2 )ρ2 + 1/2 = 0
g2 ≡ (4α2 − 1) sin ψ + 4αδ cos ψ = 0
(183a)
(183b)
with α, δ, and ρ defined as in the book:
α≡
a+b
,
l
δ≡
d
,
l
ρ≡
r
.
d
From eq.(183b) we have two cases:
(i) cos ψ = 0 and
(ii) cos ψ 6= 0, which requires that (4α2 − 1) sin ψ + 4αδ = 0
In case (i), sin ψ = ±1. However, for stable equilibrium we must have7 sin ψ = +1, which leads to
ψ = π/2, i.e., the posture adopted by the robot when travelling on a straight course. Then, from
eq.(183a) we derive
1
−2(α + δ)2 ρ2 + = 0
2
and, since α, δ ≥ 0, the foregoing relation implies
α+δ =
1
2ρ
or, in terms of the architecture parameters,
a+b+d
d
=
l
2r
Substituting this condition into matrix ΘΘT , we obtain
1 1 0
T
ΘΘ =
2 0 1
Apparently, in this case matrix Θ is isotropic, and has the form
1
1 1
Θ=
2 −1 1
In case (ii), we have
4αδ
1 − 4α2
(184)
(1 − 4α2 )2 − 16α2 δ 2
(1 − 4α2 )2
(185)
sin ψ =
with 1 − 4α2 > 4αδ while α < 1/2, and hence,8
cos2 ψ =
7 The
8 We
axis of the caster wheel always lags the vertical axis of its bracket.
do not consider the case |1 − 4α2 | = 4αδ because this takes us back to case (i).
130
Then, upon substituting the above expressions into matrix ΘΘT , we obtain
1 − 4(α2 − δ 2 ) 2α2
0
T
ΘΘ =
0
(1/2)ρ2
1 − 4α2
Apparently, to make this matrix proportional to 2 × 2 identical matrix, we need ρ = 2α. Substituting
eqs.(184) and (185) and ρ = 2α into matrix Θ, we obtain
√
√
α
S + 2δ −√ S + 2δ
√
Θ=
(1 − 4α2 )
S − 2δ
S + 2δ
with
S ≡ 16α2 (α2 − δ 2 ) − 8α2 + 1
For example, if α = 1/3, ρ = 2α = 2/3 and δ = (1 − 4α2 )/8αδ = 5/24, then ψ = π/6 and Θ has the
form
√
√
1
2√3 + 3 −2√ 3 + 3
Θ=
12 2 3 − 3 2 3 + 3
A Maple worksheet supporting the calculations involved is included in Appendix 3.
10.12 (a) Upon inversion of eq.(8.128a), we obtain,
θ̇a = Uθ̇ u
with U = Θ−1 , i.e.,
1
ρ[2(α sin ψ + δ) + cos ψ] 2α cos ψ − sin ψ
U=
2ρ(α + δ sin ψ) ρ[2(α sin ψ + δ) − cos ψ] 2α cos ψ + sin ψ
(b) Let



2(α sin ψ + δ) + cos ψ
2(α sin ψ + δ) − cos ψ



2(α
+
δ
sin
ψ)
2(α + δ sin ψ)
 , u2 = 
u1 = 



2α cos ψ − sin ψ
2α cos ψ + sin ψ
2ρ(α + δ sin ψ)
2ρ(α + δ sin ψ)
their gradient with respect to θu being
∂ui ∂ui
, i = 1, 2
∇ui =
∂θ3 ∂ψ
where




∂u1
=0
∂θ3
and
∂u1
1
=
∂ψ
D
ρ[2(α2 + δ 2 ) cos ψ − (α sin ψ + δ)]
−α[2(α sin ψ + δ) + cos ψ]
D ≡ 2ρ(α + δ sin ψ)2
whence it is apparent that ∇u1 is not symmetric, and hence, no function U1 (θ3 , ψ) exists whose
gradient is u1 . Likewise,
∂u2
=0
∂θ3
and
∂u2
1 ρ[2(α2 − δ 2 ) cos ψ + (α sin ψ + δ)]
=
,
−α[2(α sin ψ + δ) − cos ψ]
∂ψ
D
Apparently, neither ∇u2 is symmetric, the conclusion being that none of the two constraints in
θ̇a = Uθ̇ u is holonomic.
131
10.13 Expressions for ω and ċ are derived in Subsection 10.5.2, and are reproduced below:
ω=−
3
a X
θ̇i ,
3r 1
3
ċ = −
2a X
θ̇i fi
3 1
Apparently, the first equation leads to the integral
3
ψ=−
a X
θi
3r 1
(186)
where ψ is the angular displacement of the platform with respect to a horizontal line fixed to an inertial
frame, under the assumption that, when θi = 0, for i = 1, 2, 3, ψ = 0. Now, the second equation is
cast in the form
3
λ1 λ2 λ3
Fθ̇ = − ċ, F ≡ [ f1 f2 f3 ] ≡
µ1 µ2 µ3
2a
Thus, the second constraint equation is integrable iff there is a 2-dimensional vector function φ(θ) =
[ φ1 , φ2 ]T such that
∂φ
F=
∂θ
For example, we should have, for the (1, 1) and (1, 2) entries of F,
λ1 =
where
λ1 = cos ψ,
∂φ1
,
∂θ1
λ2 =
∂φ1
∂θ2
√
2π
1
λ2 = cos ψ +
= − (cos ψ + 3 sin ψ)
3
2
Let us assume that the foregoing equations hold, while ψ is given by eq.(186), i.e.,
θ1 + θ2 + θ3
∂φ1
cos
a =
3r
∂θ1
√
1
θ1 + θ2 + θ3
θ1 + θ2 + θ3
∂φ1
− cos
a + 3 sin
a
=
2
3r
3r
∂θ2
Let us now take the partial derivative of the first of the foregoing equations with respect to θ2 and of
the second with respect to θ1 :
a
θ1 + θ2 + θ3
∂ 2 φ1
− sin
a =
3r
3r
∂θ1 θ2
√
θ1 + θ2 + θ3
a
θ1 + θ2 + θ3
∂ 2 φ1
sin
a + 3 cos
a
=
6r
3r
3r
∂θ2 θ1
For the above assumption to hold, the left-hand sides of the two foregoing equations must be equal,
i.e.,
√
θ1 + θ2 + θ3
1
θ1 + θ2 + θ3
θ1 + θ2 + θ3
− sin
a =
sin
a + 3 cos
a
3r
2
3r
3r
However, this equation leads to
√
θ1 + θ2 + θ3
3
tan
a =−
3r
3
and hence,
a
π
5π
(θ1 + θ2 + θ3 ) = −
or
3r
6
6
which contradicts the integral of the first constraint, the conclusion being that the second constraint
equation is not integrable. This result is a consequence of the nonholonomy of the system at hand.
132
11
Trajectory Planning: Continuous-Path Operations
11.1 (a) Many solutions are possible for the location of the robot base. Obviously, the path must lie within
the workspace of the robot. Additionally, two alternatives are possible: either the welding is done
from outside or from within the helix. The workspace of the PUMA 560 is displayed in Fig. 34,
along with a projection of the path. For an interior operation, we could have a path Γ0 , with
coordinates in m:
x = 0.3 cos ϑ
y = 0.3 sin ϑ
0.8ϑ
z=
π
Here, the center9 OH of the helicoidal path has F2 coordinates (0, 0, 0). For an exterior operation,
we have a path Γ1 , with coordinates in m as well:
x = 0.3 cos ϑ − 0.5
y = 0.3 sin ϑ − 0.5
0.8ϑ
z=
+ 0.33
π
with the center of the helicoidal path located at a point of F2 coordinates (−0.5, −0.5, 0.33) m.
Now, let us determine the Frenet-Serret vectors, which are independent of where the center OH
is located. First, we determine the velocity along the helix:
ẋ = −0.3ϑ̇ sin ϑ
ẏ = 0.3ϑ̇ cos ϑ
ż =
0.8ϑ̇
π
and the corresponding acceleration:
ẍ = −0.3ϑ̇2 cos ϑ − 0.3ϑ̈ sin ϑ
ÿ = 0.3ϑ̈ cos ϑ − 0.3ϑ̇2 sin ϑ
0.8ϑ̈
π
The constant-speed condition (as in Example 11.3.1) leads to:
z̈ =
ẋ2 + ẏ 2 + ż 2 = v02
where v0 is the constant speed along the helix. Upon substitution of numerical values, the foregoing
condition becomes
2
0.8
0.32 ϑ̇2 +
ϑ̇2 = (0.050)2
π
ϑ̇ = 0.1271
and hence, with c = 0.1271 s−1 , we obtain
ẋ = −0.3c sin(ct)
ẏ = 0.3c cos(ct)
0.8c
ż =
π
9 The center of the helicoidal path is defined as the intersection of its axis with a plane parallel to the X -Y plane and
2 2
containing the bottom end of the path.
133
Following Example 11.3.1, since this part is independent of the robot,




−a sin ct
cos ct
dr
ṙ
c 
et ≡
≡ =
a cos ct  and en = −  sin ct 
ds
ṡ
vo
b
0
Thus, the binormal vector eb is calculated simply as the cross product of the first two vectors of
the Frenet-Serret triad:


−b sin ct
c 
b cos ct 
eb ≡ et × en = −
vo
−a
The orientation matrix Q of the nozzle is given by
Q ≡ [ et
Hence,

−a sin ct
c 
a cos ct
Q=
vo
b
en
eb ]
−(vo /c) cos ct
−(vo /c) sin ct
0

b sin ct
−b cos ct 
a
Now, the center of the wrist is located, with respect to the base, by the following vector:
c = oH + p + QcP
with oH denoting the position vector of OH , p that of an arbitrary point P on the helix, both
given in F2 , and cP , the position vector of P in the Frenet-Serret frame.






0
−0.500
a cos ϑ
oH =  −0.500  , p =  a sin ϑ  , cP =  −0.050 
0.0867
0.330
bϑ
The joint trajectories appear in Figs. 35 and 36 as pertaining to paths Γ0 and Γ1 , respectively.
(b) Now, once c is available, we proceed with the inverse kinematics of the PUMA 560 as done in
Section 4.4. Once we obtain θ, θ̇ can be found from Section 4.5 as
θ̇ = J−1 t
Shown in Figs. 37 and 38 are the plots of the time-histories of the joint rates, as pertaining to
paths Γ0 and Γ1 , respectively.
(c) The joint accelerations can be found from Section 4.6 as
θ̈ = J−1 (ṫ − J̇θ̇)
Plotted in Figs. 39 and 40 are the joint-acceleration time-histories for paths Γ0 and Γ1 , respectively.
11.2 (a) Matrix representation of S(t) in B. We have
et = −0.6 sin ϕio + 0.6 cos ϕjo + 0.8ko
en = − cos ϕio − sin ϕjo
eb = 0.8 sin ϕio − 0.8 cos ϕjo + 0.6ko
and hence, S(t) = [ et
en
eb ] expressed in {io , jo , ko } is found from Definition 2.2.1 as


−0.6 sin ϕ − cos ϕ 0.8 sin ϕ
S(t) =  0.6 cos ϕ − sin ϕ −0.8 cos ϕ 
0.8
0
0.6
134
Figure 34: Top view of the workspace of the PUMA 560
(b) Now we determine likewise the matrix representation

0.933
A ≡ [ i7 j7 k7 ] =  0.067
0.354
whence [R]F , defined as [ et
en
of R in F from the relation

0.067 −0.354
0.933
0.354 
−0.354 0.866
eb ] in F7 , is


0.933 0.067 0.354
[R]F = AT =  0.067 0.933 −0.354 
−0.354 0.354 0.866
(c) We have, from a result displayed in eqs.(4.8a) and (4.9a),
[Q(t)]B = [R]B [S(t)]B = [S(t)]B [R]F [S(t)]TB [S(t)]B = [S(t)]B [R]F
|
{z
}
1
i.e.,
135
θ1
−50
140
−100
θ2 120
−150
100
−200
0
5
10
80
0
15
50
θ3
5
10
15
5
10
15
5
10
15
42
41
θ4
0
40
−50
0
5
10
39
0
15
−70
65
60
θ5
−75
−80
0
θ6
5
10
55
50
0
15
Figure 35: Joint trajectories vs. time (s) for path Γ0 , in degrees
100
θ1
160
θ2
50
140
120
0
0
5
10
15
100
0
60
5
10
15
5
10
15
5
10
15
0
40
θ3
θ4−100
20
0
0
5
10
15
0
θ5
−200
0
50
−20
0
−40
θ6 −50
−60
0
5
10
15
−100
0
Figure 36: Joint trajectories vs. time (s) for path Γ1 , in degrees.
136
8
θ̇1
3
6
θ̇2 2.5
4
2
0
5
10
2
0
15
3
5
10
15
5
10
15
5
10
15
2
2
θ̇3
θ̇4 1.5
1
0
0
5
10
1
0
15
4.6
2
4.4
θ̇5
0
θ̇6
4.2
4
0
5
10
−2
−4
0
15
Figure 37: Joint velocities vs. time (s) for path Γ0 , in rad/s.
5
θ̇1
10
0
θ̇2
5
−5
−10
0
5
10
0
0
15
10
θ̇3
θ̇4
0
−10
0
5
10
15
5
10
15
5
10
15
20
5
10
0
−20
0
15
0
40
20
θ̇5
−5
−10
0
θ̇6
5
10
15
0
−20
0
Figure 38: Joint velocities vs. time (s) for path Γ1 , in rad/s.
137
0.1
1
0.08
θ̈1
0.5
θ̈2
0.06
0.04
0
5
10
15
0
−0.5
0
0.5
5
10
15
5
10
15
5
10
15
0.4
0
θ̈3
θ̈4
−0.5
0
5
10
0.2
15
0.1
0
−0.2
0
−0.4
θ̈6
θ̈5
−0.1
0
5
10
15
0
Figure 39: Joint accelerations vs. time (s) for path Γ0 , in rad/s2.
2
2
1
0
θ̈1
θ̈2
−2
0
5
10
0
−1
0
15
0
5
10
15
5
10
15
5
10
15
20
−1
0
θ̈3
−2
−3
0
θ̈4
5
10
−20
0
15
2
θ̈5
20
0
−2
0
θ̈6
5
10
15
0
−20
0
Figure 40: Joint accelerations vs. time (s) for path Γ1 , in rad/s2.
138



−0.6sϕ −cϕ 0.8sϕ
0.933 0.067 0.354
[Q(t)]B =  0.6cϕ −sϕ −0.8cϕ   0.067 0.933 −0.354 
0.8
0
0.6
−0.354 0.354 0.866


−0.843sϕ − 0.067cϕ 0.243sϕ − 0.933cϕ
0.4804sϕ + 0.354cϕ
=  0.843cϕ − 0.067sϕ −0.243cϕ − 0.933sϕ −0.4804cϕ + 0.354sϕ 
0.534
0.266
0.8028
where sϕ = sin ϕ and cϕ = cos ϕ.
(d) The Darboux vector δ is given as
δ = τ et + κeb
In the base frame B, we have


−0.6 sin ϕ
et =  0.6 cos ϕ  ,
0.8


− cos ϕ
en =  − sin ϕ  ,
0


0.8 sin ϕ
eb =  −0.8 cos ϕ 
0.6
From eq.(11.5a), we can find the curvature κ:


− cos ϕ
ėt = 0.6ϕ̇  − sin ϕ  = 0.6ϕ̇en
0
whence κ = 0.6. Now we determine τ :




sin ϕ
− sin ϕ
ėb = 0.8ϕ̇  cos ϕ  = −0.8ϕ̇  − cos ϕ 
0
0
ėb
= −0.8en = −τ en
ϕ̇
So, τ = 0.8. Finally,
δ = 0.8et + 0.6eb
and
δ̇ = 0.8ėt + 0.6ėb = (0.8)(0.6)ϕ̇en − (0.6)(0.8)ϕ̇en = 0
11.3 (a) We have
t3
3
Then, the position vector r of any point on the curve with its first and second-derivatives are






2t
2
0
r =  t2  , ṙ =  2t  , r̈ =  2 
t3 /3
t2
2t
x = 2t, y = t2 , z =
Now, the Frenet-Serret triad is readily evaluated as


 2 
2
t
1 
ṙ
×
r̈
1
ṙ
 −2t  ,
2t  , eb =
= 2
= 2
et =
kṙk
t +2
kṙ × r̈k
t +2
t2
2


−2t3 + 4t
1
 −t4 + 4 
en = eb × et =
t2 + 2 2
2t3 + 4t
139
and hence, the orientation matrix Q is given by
Q=
et
en
eb
Hence,

2
1 
2t
= 2
t +2
t2
−2t
2 − t2
2t


 
Q(3, 2) − Q(2, 3)
1
1
1  
vec(Q) =  Q(1, 3) − Q(3, 1)  = 2
0 ,
2
t +2
Q(2, 1) − Q(1, 2)
1
the unit vector e and angle φ sought then being


√
1
2
0  , φ = tan−1
e=
2
1

t2
−2t 
2
cos φ =
tr(Q) − 1
2 − t2
= 2
2
t +2
√ !
2
2 − t2
(b) The expressions for the curvature and torsion in terms of time are readily evaluated as
κ=
kṙ × r̈k
2
ṙ × r̈ · r̈
2
= 2
, τ=
= 2
kṙk3
t +2
kṙ × r̈k2
t +2
Moreover, ω = ṡδ, and hence, all we need is ṡ and δ, which are readily computed as
p
ṡ = kṙk = 4 + 4t2 + t4 = 2 + t2
and
Thus,


1
2 
0 
δ = τ et + κeb = 2
t +2
1


1
ω = ṡδ = 2  0 
1
Finally, straightforward differentiation of the above expression for ω with respect to time yields
ω̇ = 0
11.7 The path is given as

  
λ + cos ϕ
x
Γ : r = r  sin ϕ  =  y 
1/ϕ̂
z
with ϕ̂ defined in Example 11.3.2. To obtain the spline approximation of Γ, we make use of eqs.(11.47d & e).
For N = 5, we have the supporting points displayed in Table 6. Hence,
∆σ1 = 0.2348
∆σ2 = 0.2279
∆σ3 = 0.2279
∆σ4 = 0.2348
while αk , αi,j , βk , and βi,j are displayed in Table 7.
140
Table 6: Supporting points for N = 5
ϕ
x
y
z
0◦
0.45
0.00
0.3969
90◦
0.30
0.15
0.4975
180◦
0.15
0.00
0.5809
270◦
0.30
-0.15
0.4975
360◦
0.45
0.00
0.3969
Table 7: Spline parameters for N = 5
1
2
3
4
αk
0.2348
0.2279
0.2279
0.2348
αi,j
0.4627
0.4558
0.4627
0.4696
βk
4.2589
4.3879
4.3879
4.2589
βi,j
8.5178
8.6468
8.7758
8.6468
Now, we assemble the A and C matrices. Since the path is closed, we have a periodic spline. So, A
and C are given by eqs.(11.59a & b). For N = 5, we obtain


0.9392 0.2348
0
0.2348
0 
 0.2348 0.9254 0.2279
A=

0
0.2279 0.9116 0.2279
0.2348
0
0.2279 0.9254


−8.5178 4.2589
0
4.2589
0
 4.2589 −8.6468 4.3879

C=

0
4.3879 −8.7758 4.3879
4.2589
0
4.3879 −8.6468
Matrix P can be found using eq.(11.58):

0.45
0
 0.30 0.15
P=
0.15
0
0.30 −0.15

0.3969
0.4975 

0.5809
0.4975
From eq.(11.60), we obtain P′′ and, with P, we obtain the spline coefficients, ak , bk , ck and dk . Thus,

−8.0389
0
5.8816
 −0.2483 −8.4077 −0.8104 
P′′ = 6A−1 CP = 

8.7839
0
−4.4127
−0.2483 8.4077 −0.8104

With P′′ and P, we obtain the spline parameters from eq.(11.53a). The Frenet-Serret vectors, eb , eb ,
et at each supporting point are
r′ (σk )
kr′ (σk )k
r′ (σk ) × r′′ (σk )
eb = ′
r (σk ) × r′′ (σk )
en = eb × et
et =
141
where
r(σ) = ak (σ − σk )3 + bk (σ − σk )2 + ck (σ − σk ) + dk
r′ (σ) = 3ak (σ − σk )2 + 2bk (σ − σk ) + ck
r′′ (σ) = 6ak (σ − σk ) + 2bk
r′′′ (σ) = 6ak
The orientation matrix is given as
Q = [ eb
et
en ]
In order to obtain the time-histories of the joint angles, we need Q as a function of time. The time
required to complete this task is found from T = l/vo , where l is the length of the curve. We have
Z σfinal
s(σ) =
kr′ (σ)kdσ
0
and hence,
l=
N
−1 Z σi+1
X
i=1
′
σi
kp′ (σ)kdσ
Note that ds/dσ = kr (σ)k, so that we can write
ṡ = σ̇kr′ (σ)k = vo
Finally,
σ̇ =
vo
kr′ (σ)k
Unlike Example 11.5.1, r′ (σ) is derived from the spline approximation of Γ. Next, we integrate numerically the above equation to obtain σ(t), while letting σ(0) = 0, which thus yields Q and p as functions
of time.
Below we compute the pose, twist and twist-rate at each spline supporting point. First, the pose at
each supporting point is nothing but
Q(σk ) = [ eb (σk ) en (σk )
p(σk ) = dk
Second, the twist tk = [ ω Tk
We have, from eq.(11.12),
ek (σk ) ]
T
vkT ] , where ωk is the angular-velocity vector and vk = ṗk at t = tk .
ωk = ṡδ k
where ṡ = vo and δ k is the Darboux vector at the kth supporting point, i.e.,
δ k = τk (et )k + κk (eb )k
kck × 2bk k
κk =
kck k3
ck × 2bk · 6ak
τk =
kck × 2bk k
Therefore,
ṗk = vo (et )k (σk )
Now, for the twist rate, ṫk = [ ω̇ Tk
v̇kT ]T , we have
v̇k = p̈k = σ̇ 2 2bk
142
220
θ1
90
200
θ2
180
160
0
θ3
80
70
0.5
1
60
0
350
400
300
θ4 200
250
0
0.5
0
0
1
100
0.5
1
0.5
1
0.5
1
200
80
θ5
θ6 180
60
40
0
0.5
1
160
0
Figure 41: Joint trajectories vs. σk /σmax for N = 20, in degrees.
and
ω̇ k = s̈δ k + ṡδ̇ k ,
s̈ = v̇o = 0
Hence,
ω̇ k = vo [τ̇k (et )k (σk ) + κ̇k (eb )k (σk )]
with
vo (ck × 2bk ) · (ck × 6ak )
κk
vo [−2τk (ck × 6ak ) · (ck × 2bk )]
τ˙k =
κ2k
κ̇k =
Finally, θ, θ̇, and θ̈ at the supporting points are determined using inverse kinematics. Once again, we
apply Algorithm 11.5.2 with iterations performed between supporting points. The joint angle, velocity
and acceleration trajectories are plotted in Figs. 41, 42 and 43, respectively, for N = 20.
Now, for comparison, we fit a periodic cubic spline using θ at the supporting points. We refer to
Section 11.4 to obtain the A and C matrices. Thus,
θ̈ = 6A−1 Cθ
The differences in joint accelerations for N = 5, 15, 20 are plotted in Figs. 44, 45 and 46, respectively.
12
Dynamics of Complex Robotic Mechanical Systems
12.1 The system mass and angular velocity matrices M and W are, in this case, of 6r × 6r. Moreover, T is
of 6r × n. We can thus express the 6r-dimensional twist vector t of the whole system and its rate of
change as
t = Tθ̇a ,
ṫ = Tθ̈ a + Ṫθ̇a
(187)
143
20
θ̇1
10
0
θ̇2
0
−20
−40
0
θ̇3
0.5
−10
0
1
20
0
0
θ̇4 −50
−20
0
0.5
1
−100
0
50
θ̇5
1
0.5
1
0.5
1
50
0
−50
0
0.5
0
θ̇6
0.5
−50
0
1
Figure 42: Joint velocities vs. σk /σmax for N = 20, in rad/s.
40
θ̈1
40
20
θ̈2
20
0
−20
0
0
0.5
−20
0
1
40
20
θ̈3
1
0.5
1
0.5
1
50
θ̈4
0
−20
0
0.5
100
0.5
0
−50
0
1
100
100
50
θ̈5
θ̈6
0
−50
0
0.5
1
0
−100
0
Figure 43: Joint accelerations vs. σk /σmax for N = 20, in rad/s2.
144
40
∆θ̈1
30
20
∆θ̈2
0
−20
0
10
0.5
∆θ̈4
0.5
0
0
0.5
1
0.5
1
50
50
0
0
1
50
1
100
∆θ̈5
0.5
100
0
−10
0
0
0
1
10
∆θ̈3
20
0
∆θ̈6
0.5
1
−50
0
Figure 44: Differences in the joint accelerations for N = 5, in rad/s2.
10
∆θ̈1
10
5
5
∆θ̈2
0
−5
0
0
0.5
−5
0
1
5
0
∆θ̈3
0.5
1
1
0.5
1
40
10
20
∆θ̈6
0
−10
0
0.5
0
−50
0
20
∆θ̈5
1
50
∆θ̈4
−5
−10
0
0.5
100
0.5
1
0
−20
0
Figure 45: Differences in the joint accelerations for N = 15, in rad/s2.
145
10
∆θ̈1
5
5
∆θ̈2
0
0
−5
0
0.5
−5
0
1
5
0
∆θ̈3
1
0.5
1
0.5
1
20
∆θ̈4
−5
−10
0
0.5
40
0.5
0
−20
0
1
20
40
20
∆θ̈5
0
−20
0
∆θ̈6
0.5
0
−20
0
1
Figure 46: Differences in the joint accelerations for N = 20, in rad/s2.
and hence, the 6r Newton-Euler equations of the whole multibody system takes the form
M(Tθ̈ a + Ṫθ̇a ) + WMTθ̇a = wA + wG + wC + wD
(188)
with the usual definitions for wA , wG , wC , and wD . Now, t and wC are reciprocal, in the sense that
tT wC = 0
(189)
and, if we replace t in the above equation for its expression in eq.(187),
T
θ̇ a TT wC = 0
which holds for any θ̇ a , and hence,
TT wC = 0
(190)
i.e., wC lies in the nullspace of TT . Thus, upon multiplying both sides of eq.(188) from the left by
TT , wC disappears, and we obtain
TT M(Tθ̈a + Ṫθ̇a ) + TT WMTθ̇a = TT wA + TT wG + TT wD
or
I(θ)θ̈ a + C(θ, θ̇a )θ̇ a = τ A + γ + δ
(191)
with I(θ) and C(θ, θ̇a ) defined as in the holonomic case, except that now we distinguish between θ
and θa . Thus,
I(θ) = TT MT
C(θ, θ̇a ) = TT MṪ + TT WMT
τ A = TT wA ,
γ = TT wG ,
(192a)
(192b)
δ = TT wD
(192c)
Moreover, T = T(θ), Ṫ = Ṫ(θ, θ̇a ), and W = W(θ, θ̇a ). For brevity, we shall omit the arguments in
the derivations below, whenever not essential. Let us now calculate İ(θ, θ̇a ):
İ(θ, θ̇a ) = ṪT MT + TT ṀT + TT MṪ
146
(193)
Furthermore, we recall the expression for Ṁ given in Exercise 3.17 for one single body. For r bodies,
Ṁ takes on an identical form, except that, now, M, W, and Ṁ are 6r × 6r block-diagonal, i.e.,
M = diag(M1 , · · · , Mr ),
W = diag(W1 , · · · , Wr ),
Ṁ = diag(Ṁ1 , · · · , Ṁr )
eq.(193) thus yielding
İ(θ, θ̇a ) = ṪT MT + TT MṪ + TT (WM − MW)T
(194)
Upon substituting eq.(192b) into eq.(194), we obtain
İ(θ, θ̇a ) = C(θ, θ̇a ) + ṪT MT − TT MWT
Hence,
C(θ, θ̇a ) = İ(θ, θ̇a ) − ṪT MT + TT MWT ≡
1
İ(θ, θ̇a ) + P
2
where
1
İ(θ, θ̇a ) − ṪT MT + TT MWT
2
i
1h T
=
Ṫ MT + TT MṪ + TT (WM − MW)T − ṪT MT + TT MWT
2
i
1h
=
−ṪT MT + TT MṪ + TT (WM + MW)T
2
P≡
thereby obtaining the desired expression for C(θ, θ̇ a ).
12.2 We have
I = TT MT
Differentiating this equation with respect to time, we obtain
İ = ṪT MT + TT ṀT + TT MṪ
Taking into account the result of Exercise 3.17, applicable to one single rigid body, we can readily
extend it to a system of r rigid bodies:
Ṁ = WM − MW
and hence,
İ = ṪT MT + TT MṪ + TT (WM − MW)T
(195)
On the other hand, the Cartesian decomposition of C(θ, θ̇a )—see Section 2.3.3—leads to
C(θ, θ̇ a ) = Cs (θ, θ̇a ) + Css (θ, θ̇a )
(196)
where Cs (θ, θ̇a ) is the symmetric component and Css (θ, θ̇a ) is the skew-symmetric component of
C(θ, θ̇a ), i.e.,
1
1
Cs = (C + CT ), Css = (C − CT )
2
2
Now we calculate Cs (θ, θ̇a ) using the expression for C(θ, θ̇a ) in Exercise 12.1, namely,
1
(C + CT )
2
1
= [İ + TT MṪ − ṪT MT + TT (WM + MW)T
4
Cs =
+İ + ṪT MT − TT MṪ + TT (−WM − MW)T] =
147
1
İ
2
whence,
İ = C + CT
Therefore,
İ − 2C = C + CT − 2C = CT − C = −2Css
which is obviously skew-symmetric.
12.3 (a) Because the whole robot undergoes pure translation, the kinematic constraint can be written in
the from
r
ω = (θ̇1 − θ̇2 ) = 0
l
which yields
θ̇a = Lθ̇1
where the matrix L is
L=
1
1
T
Iw and Ip being constant, they are not affected by the type of motion. Ic and Ib are calculated
below:
Substituting ψ = π/2 into eq.(8.128b), we derive
θ11 = θ12 =
1
,
2
θ21 = −θ22 = −ρ(α + δ)
Hence,
mc r2 3/2 + ρ2 (α + δ)2 3/2 − ρ2 (α + δ)2
3/2 − ρ2 (α + δ)2 3/2 + ρ2 (α + δ)2
4
2
2
1
ρ (α + δ)2 −ρ2 (α + δ)2
ρ (α + δ)2 + ρ2
2
Ib = Ib
+
m
d
b
2
2
2
2
−ρ (α + δ)
ρ (α + δ)
−ρ2 (α + δ)2 + ρ2
4
1
1
1 −1
1 1
= Ib + mb d2 ρ2 (α + δ)2
+ mb d2 ρ2
−1 1
1 1
4
4
Ic =
−ρ2 (α + δ)2 + ρ2
ρ2 (α + δ)2 + ρ2
The inertia matrix under pure translation is, then, a scalar I ′ , namely,
I ′ = LT IL = Iw′ + Ic′ + Ib′ + Ip′
where
Iw′ ≡ LT Iw L = 2(I + mw r2 )
3mc r2
Ic′ ≡ LT Ic L =
2
Ib′ ≡ LT Ib L = mb r2
Ip′ ≡ LT Ip L = mp r2
Physical interpretation: Iw′ is twice the moment of inertia of wheel w.r.t. a horizontal line parallel
to the actuated wheel axis and passing through the contact point; Ic′ is moment of inertia of the
disk of mass mc and radius r w.r.t. a horizontal line parallel to the caster wheel axis and passing
through the contact point; Ib′ is the moment of inertia of a particle of mass mb located at the
center of the caster wheel w.r.t. the contact point; and Ip′ is similar to Ib′ , with mb replaced by
mp .
148
(b) Now, for the case in which the midpoint of segment O1 O2 is stationary, the kinematic constraint
can be written as
θ̇1 + θ̇2 = 0 =⇒ θ̇a = Uθ̇1
with
U=
Under this maneuver,
1 −1
T
2r
r
θ̇1 , ċ = 2a θ̇1 i
l
l
In this case, the platform-bracket ensemble can move as a single rigid body, such as under maneuver (a). However, the angle ψ between platform and bracket can also vary, with the midpoint
of segment O1 O2 still remaining stationary. Nevertheless, the only possible motion leaving that
point stationary and leading to a generalized inertia matrix that lends itself to a physical interpretation, is the first one, i.e., that under which angle ψ remains constant, and hence, ψ̇ = 0.
Substituting the foregoing value into eq.(8.126b), we obtain an equation for ψ, namely,
ω=
α sin ψ + δ = 0
whence,
d
δ
sin ψ = − = −
α
a+b
⇒
p
(a + b)2 − d2
cos ψ =
a+b
Therefore,
θ11
θ21
p
p
(a + b)2 − d2
(a + b)2 − d2
d
d
=
−
, θ12 = −
−
,
l
2(a + b)
l
2(a + b)
p
p
r (a + b)2 − d2
r
r (a + b)2 − d2
r
=
θ 22 =
+ ,
−
d
2(a + b)
l
d
2(a + b)
l
Then, we calculate matrices Ic and Ib using computer algebra (the expressions thus obtained are
quite cumbersome to display), the generalized inertia matrix in this case becoming the scalar
I ′′ = UT IU = Ip′′ + Ib′′ + Iw′′ + Ic′′
where, after further computer-algebra work,
Ip′′
T
2
2
2
2
≡ U Ip U = 4Ip (ρδ) + 4λ mp r = 4(Ip + mp a )
2r
l
2
2
3
2r
Ib + mb (a + b)2 − d2
4
l
"
2 #
2r
l2
′′
T
2
2
Iw ≡ U Iw U = 2 I + mw r + 4(ρδ) H = 2 I + H + mw
4
l
Ib′′ ≡ UT Ib U =
r2
Ic′′ ≡ UT Ic U = mc 2 {6[(a + b)2 − d2 ] + r2 }
l
2
2
2
mc r 2 2 p
r
2r
2
2
2
2
=
(a + b) − d
+ mc
+ (a + b) − d
2
l
4
l
Physical interpretation:
Ip′′ is the moment of inertia of the platform with respect to its instant center of rotation (the
midpoint of the segment O1 O2 ), times the square of 2r/l, which is the ω/θ̇1 ratio.
149
Ib′′ , in turn, is the moment of inertia of the bracket with respect to the same instantant center of
rotation times the same factor, for the angular velocity of the bracket is identical to that of the
platform (ψ̇ = 0).
Iw′′ equals twice the term in brackets, which is thus the contribution of each actuated wheel to
Iw′′ ; this has two parts, the first one, I, being the moment of inertia of each wheel with respect to
its horizontal axis and, therefore, accounts for the rotation of the wheel about its own axis; the
second is the contribution of the moment of inertia of the same wheel w.r.t. a vertical line passing
through the contact point with suitable shifting, to account for the Parallel-Axis Theorem, this
line lying a distance l/2 from the segment O1 O2 , times the square of 2r/l, which is the ratio ω/θ̇1 ,
for ω is the vertical component of the angular velocity of the actuated wheels.
Likewise, Ic′′ includes the contribution of the moment
p of inertia of the caster wheel about its
horizontal axis of rotation times the square of (2/l) (a + b)2 − d2 , which is the θ̇3 /θ̇1 ratio, and
the contribution of the moment of inertia of the caster wheel w.r.t. a vertical line passing through
the contact point with suitable shifting, to account for the Parallel-Axis Theorem, the distance
involved being that between C ′ and O3 .
Figure 47:
12.6 The condition for a symmetric matrix to be isotropic is that the matrix be proportional to the identity
matrix, with a nonzero proportionality factor. Hence, it is apparent that for I(θ) of Subsection 12.5.2
to be isotropic, all we need is β = 0, i.e.,
H + 3J = 3mw r2 + 2mp r2
(197)
which yields an equality constraint at the design stage among the set of parameters H, J, mw , mp ,
and r. If I(θ) is isotropic, then we have
I(θ) = α1
(198)
where 1 is the 3 × 3 identity matrix. Now, under condition (197), α becomes
α = I + λ2 (3mw + 2mp + 15mw + 4mp )r2 = I + 6λ2 (3mw + mp )r2 > 0
Furthermore, C(σ, θ a , θ̇a )θ̇a can be expressed as
h
i
C(σ, θ a , θ̇ a )θ̇a = TT MṪθ̇a ≡ vect(TT MṪ) × θ̇a
(199)
where we have exploited the skew-symmetry of TT MṪ, which is apparent from eq.(12.73), its axial
150
vector being
√
vect(TT MṪ) =
or, if we recall eq.(12.59),
 
1
3λ2 (3mw + mp )r2 ω  1 
1
2
T
2
vect(T MṪ) = 3λ (3mw + mp )r (−λ)
3
X
θ̇i
1
where
e≡
Thus,
√
3
[1 1
3
(200)
!
e
| {z }
√ T
3e θ̇ a
1 ]T
√
C(σ, θ a , θ̇a )θ̇a = −3 3λ3 (3mw + mp )r2 (eT θ̇ a )e × θ̇ a
Therefore, the governing equation (12.76) becomes
√
[I + 6λ2 (3mw + mp )r2 ]θ̈a − 3 3λ3 (3mw + mp )r2 (eT θ̇a )e × θ̇ a = τ − cθ̇a
which can be further expressed as a quadratic second-order ODE, namely,
θ̈ a − A(eT θ̇a )e × θ̇a + B θ̇a = Cτ (t)
(201)
with A, B, and C defined as the constant scalars given below:
√
3 3λ3 (3mw + mp )r2
A=
I + 6λ2 (3mw + mp )r2
c
B=
2
I + 6λ (3mw + mp )r2
1
C=
I + 6λ2 (3mw + mp )r2
Moreover, the quadratic term of eq.(201) vanishes under two conditions (besides the trivial one θ̇a = 0):
i) The mean value of {θ̇i }31 vanishes, i.e., θ̇ a is normal to e. This implies that the platform undergoes
pure translation;
ii) All three wheel rates are identical, i.e., θ̇a is parallel to e. This implies that the platform undergoes
pure rotation.
Now, in order to gain more insight into the dynamics of the isotropic robot, let us decompose θ̇a into
the three mutually orthogonal directions given by the orthonormal triad {ei }31 , with e1 ≡ e and e2
and e3 any two constant unit vectors perpendicular to e and to each other, i.e., let
θ̇a = ϕ̇1 e1 + ϕ̇2 e2 + ϕ̇3 e3 ;
ϕ̇i 6= θ̇i ,
i = 1, 2, 3
Then,
e × θ̇a = e1 × (ϕ̇1 e1 + ϕ̇2 e2 + ϕ̇3 e3 ) = ϕ̇2 e1 × e2 +ϕ̇3 e1 × e3 = −ϕ̇3 e2 + ϕ̇2 e3
| {z }
| {z }
e3
151
−e2
the governing equation (201) thus becoming, in component form
ϕ̈1 + B ϕ̇1 = Cτ1 (t)
ϕ̈2 + Aϕ̇1 ϕ̇3 + B ϕ̇2 = Cτ2 (t)
(202a)
(202b)
ϕ̈3 − Aϕ̇1 ϕ̇2 + B ϕ̇3 = Cτ3 (t)
(202c)
Since eq.(202a) is linear with constant coefficients, its integral is readily computed as10
ϕ̇1 = e−Bt b1 + C
Z
t
0
e−Bu τ1 (t − u)du
in which b1 = ϕ̇1 (0) is an initial condition, and u is a dummy variable of integration. Thus,
ϕ1 (t) =
Z
t
ϕ̇1 (v)dv + a1
0
where a1 = ϕ1 (0) is a second initial condition. Once ϕ1 (t) is available, eqs.(202b & c) can be integrated
numerically, with initial conditions
ϕi (0) = ai ,
ϕ̇i (0) = bi ,
i = 2, 3
Moreover, eqs.(202b & c) now become linear, although with a time-varying coefficient, which is Aϕ̇1 (t).
Apparently, the mathematical model of an isotropic robot leads itself to interesting simplifications, that
should ease its control.
12.8 Under pure translation, the kinematic constraint can be written as
ω=
3
a X
θ̇i = 0
2r 1
or, alternatively,
1
1
1


θ̇1
 θ̇2  = 0
θ̇3
Now the robot has 2-dof, and hence θ̇a can be expressed as
θ̇a = Lu, u = θ̇1 θ̇2
where, for example,


1
0
1 
L= 0
−1 −1
Thus, the generalized inertia matrix under pure translation is



α β β
1
0
1 0 −1 
2 1
β α β  0
1  = (α − β)
I′ = LT IL =
0 1 −1
1 2
α β α
−1 −1
10 Kailath,
T., 1980, Linear Systems, Prentice-Hall Inc., Englewood Cliffs, NJ.
152
12.9 The matrix C(σ, θ a , θ̇a )θ̇ a of Coriolis and centrifugal forces can be expressed as
h
i
C(σ, θ a , θ̇ a )θ̇a = TT MṪθ̇a ≡ vect(TT MṪ) × θ̇a
(203)
where we have exploited the skew-symmetry of TT MṪ, which is apparent from eq.(12.73), its axial
vector being
 
1
√
vect(TT MṪ) = 3λ2 (3mw + mp )r2 ω  1 
(204)
1
By recalling eq.(12.59),
vect(TT MṪ) = 3λ2 (3mw + mp )r2 (−λ)
3
X
1
with
e≡
Hence,
θ̇i
!
e
| {z }
√ T
3e θ̇ a
√
3
[1 1
3
1 ]T
√
C(σ, θ a , θ̇a )θ̇a = −3 3λ3 (3mw + mp )r2 (eT θ̇ a )e × θ̇ a
Now, C(σ, θ a , θ̇a )θ̇ a vanishes under two nontrivial conditions, namely,
i) The mean value of {θ̇i }31 vanishes, i.e., θ̇ a is normal to e. This implies that the platform undergoes
pure translation;
ii) All three wheel rates are identical, i.e., θ̇a is parallel to e. This implies that the platform undergoes
pure rotation.
12.10 The characteristic equation of I is

λ−α
−β
P (λ) = det  −β
λ−α
−β
−β
Let us zero P ′ (λ):

−β
−β  = (λ − α)3 − 3β 2 (λ − α) − 2β 3 = 0
λ−α
P ′ (λ) = 3(λ − α)2 − 3β 2 = 0
The two above equations apparently admit the common root (λ − α) = −β, and thus λ = α − β is a
double root11 of P (λ). Let λ1 = λ2 = α − β, which thus yields
tr(I) =
3
X
λi
1
or
3α = 2(α − β) + λ3
and hence,
λ3 = α + 2β
11 See
reasoning behind eqs.(5.37a & b)!
153
Now, let e3 be the eigenvector of λ3 . If we denote its components by ξ3 , η3 , ζ3 , then

   
2β −β −β
ξ3
0
 −β 2β −β   η3  =  0 
−β −β 2β
ζ3
0
√
Solving this system, we obtain ξ3 = η3 = ζ3 = 3/3 and thus
 
√
1
3 
e3 =
1
3
1
Therefore, e1 and e2 lie in a plane normal to e3 and can be chosen mutually orthogonal. We can thus
let eT3 e1 = 0 where e1 = [ξ1 , η1 , ζ1 ]T which implies that
ξ1 + η1 + ζ1 = 0
(205a)
ξ12 + η12 + ζ12 = 1
(205b)
Moreover, we have
Hence, the three components of e1 are subject to two constraints.
Thus, one√of these components can
√
be chosen arbitrarily. Let, for example, ξ1 = 0; then, η1 = 2/2 and ζ1 = − 2/2, i.e.,


√
0
2
e1 =
1 
2
−1
Now, e2 can be obtained as e2 = e3 × e1 , which leads to


√
−2
6
e2 =
1 
6
1
Eigenvectors e1 and e2 represent,
in the θ̇ a space, pure translations of the platform in two orthogonal
P3
directions, for both yield 1 θ̇i = 0. Vector e3 , in turn, represents, in the same space, a pure rotation
of the platform.
12.11 Let q = [θ1 , θ2 , θ3 , σ, x, y]T where σ is the angle of rotation of the platform and (x, y) are the
Cartesian coordinates of its mass center in an inertial frame. Therefore,
θ̇ a
q̇ =
,
ω = σ̇
(206)
t′
From eq.(12.56a),
[ J −K ]
| {z }
A
θ̇a
= 03
t′
| {z }
⇒
b=0
(207)
q̇
where J is a constant, but K depends on {fi }31 and, since this set depends on the orientation of the
platform, we have A = A(σ). On the other hand, the kinetic energy Ti of the ith wheel can be
expressed as
1
1
Ti = mw kċi k2 + ωTi Ii ω i , i = 1, 2, 3
(208)
2
2
where
ċi = ċ + ωrfi
154
and
kċi k2 = kċk2 + ω 2 r2 + 2ωrċ · fi
Let Fi be the frame with origin at Ci and unit vectors {ei , fi , k}.
 

θ̇i
I 0
[ωi ]i =  0  ,
[Ii ]i =  0 J
ω
0 0
and thus,
Then,

0
0
J
ω Ti Ii ω i = I θ̇i2 + Jω 2
Hence,
Ti =
Moreover,
1 2
1
mw ċT ċ + ω 2 r2 + 2ωrċT fi +
I θ̇i + Jω 2 ,
2
2
T4 =
i = 1, 2, 3
1
1
mp ċT ċ + Hω 2
2
2
(209)
(210)
Therefore,
T =
4
X
1
3
1
(3mw + mp )ċT ċ + mw r2 ω 2 + 3r(f1 + f2 + f3 )T ċω
2
2
1
3
1
+ I θ̇12 + θ̇22 + θ̇32 + Jω 2 + Hω 2
2
2
2
3
1 2
3
1
1
2
2
2
= I θ̇1 + θ̇2 + θ̇3 +
mw r + J + H ω 2 + (3mw + mp )(ẋ2 + ẏ 2 )
2
2
2
2
2
Ti =
Moreover,
V =0
and hence,
L=T
We have


I θ̇1
I θ̇2



∂L 
I θ̇3


=
,
2
∂ q̇  (3mw r + 3J + H)ω 


(3mw + mp )ẋ
(3mw + mp )ẏ

−a 0

T  0 −a
J
0
 0
T
A =
=
−KT
r
 r

ξ1 ξ2
η1 η2

(211)


I θ̈1
I θ̈2




d ∂L
I θ̈3


=

2
 (3mw r + 3J + H)ω̇ 
dt ∂ q̇


(3mw + mp )ẍ
(3mw + mp )ÿ

0
0 

−a 
,
r 

ξ3
η3
fi ≡
ξi
, i = 1, 2, 3,
ηi

−aλ1
−aλ2




−aλ3


T
A λ=
,
 r(λ1 + λ2 + λ3 ) 


ξ1 λ1 + ξ2 λ2 + ξ3 λ3
η1 λ1 + η2 λ2 + η3 λ3
155


τ1
 τ2 
 
τ 
φ= 3
0
 
0
0


λ1
λ ≡  λ2 
λ3
(212)
(213)
∂L
=0
∂q
(214)
whence,
I θ̈1 = τ1 − aλ1
I θ̈2 = τ2 − aλ2
I θ̈3 = τ3 − aλ3
(3mw r + 3J + H)ω̇ = r(λ1 + λ2 + λ3 )
2
(215)
(3mw + mp )ẍ = ξ1 λ1 + ξ2 λ2 + ξ3 λ3
(3mw + mp )ÿ = η1 λ1 + η2 λ2 + η3 λ3
Furthermore, let us differentiate eq.(207) with respect to time:
Aq̈ = −Ȧq̇
where
Ȧ = [ 03×3

0T3

−Ȧq̇ = 0T3
0T3
Moreover,

−a 0
Aq̈ =  0 −a
0
0
0 −r
0 −r
−a −r
−ξ1
−ξ2
−ξ3
(216)


0 −ωeT1
K̇ =  0 −ωeT2 
0 −ωeT3
−K̇ ] ,
 
 T 
θ̇a
0 ωeT1
e1 ċ
0 ωeT2   ω  = ω  eT2 ċ 
0 ωeT3
ċ
eT3 ċ
(217a)

θ̈1

  θ̈  
−aθ̈1 − rω̇ − ξ1 ẍ − η1 ÿ
−η1  2 
θ̈


−η2   3  =  −aθ̈2 − rω̇ − ξ2 ẍ − η2 ÿ 
 ω̇ 
−aθ̈3 − rω̇ − ξ3 ẍ − η3 ÿ
−η3  
ẍ
ÿ
(217b)

Substituting eqs.(217a & b) into eq.(216), we obtain
aθ̈1 + rω̇ − ξ1 ẍ − η1 ÿ = −ω(η1 ẋ − ξ1 ẏ)
aθ̈2 + rω̇ − ξ2 ẍ − η2 ÿ = −ω(η2 ẋ − ξ2 ẏ)
(218)
aθ̈3 + rω̇ − ξ3 ẍ − η3 ÿ = −ω(η3 ẋ − ξ3 ẏ)
Equations (215) and (218) now constitute a system of nine second-order ordinary differential equations
for the nine state variables, the components of vectors q̈ and λ, thereby completing the mathematical
model of the system at hand.
156
Appendix 1
157
158
(2)
> p:= Vector([x, y, z]); Rot:= Matrix([[Q[1,1], Q[1,2], Q[1,3]], [Q[2,1], Q[2,2],
Q[2,3]], [Q[3,1], Q[3,2], Q[3,3]]]); QQ6T:= MatrixMatrixMultiply[R](Rot,
Transpose(Q[6]));prod:= MatrixVectorMultiply[R](QQ6T,q[6]); prod:= map(evalf,
prod); prod:= simplify(prod); c:= VectorAdd(p, - prod);# implementing c, as
given by eq.(4.18b)
159
(3)
As expected, c is independent of
Now, compute LHS of eq.(4.17)
>
(4)
>
160
(5)
> lh:= VectorAdd(bb[2], MatrixVectorMultiply(Q[3],bb[3])); lh:= VectorAdd(lh,
b4u3); lh:= MatrixVectorMultiply(Q[2],lh); lh:= map(simplify, lh);#implementing
LHS of eq.(4.16)
(6)
>
(7)
161
(7)
As expected, LHS of eq.(4.17) is linear in [
> rh:= MatrixVectorMultiply(Transpose(Q[1]), c); rh:= VectorAdd(rh, - bb[1]);
rh:= map(simplify, rh);#implementing RHS of eq.(4.17)
(8)
>
(9)
As expected, RHS of eq.(4.17) is linear in [
>
162
(10)
Now let's transform, by hand,
>
(11)
>
(12)
>
(13)
>
(14)
>
(15)
163
Now we display the corresponding harmonic functions of the foregoing angles.
>
164
(16)
Now let's substitute
>
(17)
>
(18)
165
Let P =
>
(19)
>
(20)
gives the harmonic functions of
>
(21)
Extract coefficients A_11, A_12, A_21 & A_22 from eq_3 & eq_4, with A_11 & A_12 denoting the coefficients of
. We'll have to check later that A_21 = -A_12 & A_22 =
A_11. This is done by hand, as the Maple command "coeff" works only for polynomial expressions.
>
(22)
166
Now we calculate the RHSs of eqs.(4.28a & b):
>
Error, (in simplify/table) too many levels of recursion
(23)
As Maple apparently cannot simplify r[1], let's do this subtraction manually:
>
(24)
>
(25)
>
Error, (in simplify/table) too many levels of recursion
As Maple appears to be incapable of handling the symbolic solution of the above system, let's help Maple:
>
(26)
167
The harmonic functions of
>
(27)
>
(28)
>
(29)
>
(30)
>
(31)
which are the two inverse-displacement solutions for
>
(32)
168
>
(33)
>
(34)
>
(35)
Let CC_5 & SS_5 denote
>
(36)
>
(37)
thereby obtaining expressions for the harmonic functions of
>
169
(38)
>
(39)
Let CC_6 & SS_6 denote
>
(40)
>
thereby completing the inverse kinematics of the Fanuc Arc Mate 120iB.
170
Appendix 2
restart:
with(linalg):
Warning, the protected names norm and trace have been redefined and unprotected
> a[1]:=50: a[2]:=50: a[3]:=50:
a[4]:=50: a[5]:=50: a[6]:=50:
>
b[1]:=50: b[2]:=50: b[3]:=50: b[4]:=50: b[5]:=50: b[6]:=50:
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
alpha[1]:=Pi/2:
alpha[2]:=-Pi/2: alpha[3]:=Pi/2:
alpha[4]:=-Pi/2: alpha[5]:=Pi/2: alpha[6]:=-Pi/2:
L:=50: #The charateristic length
#Data normalization: All the lengths divided by charateristic
length:
a[1]:=a[1]/L: a[2]:=a[2]/L: a[3]:=a[3]/L: a[4]:=a[4]/L: a[5]:=a[5]/L: a[6]:=a[6]/L:
b[1]:=b[1]/L: b[2]:=b[2]/L: b[3]:=b[3]/L: b[4]:=b[4]/L: b[5]:=b[5]/L: b[6]:=b[6]/L:
for i from 1 to 6 do
q[i]:=vector([a[i]*cos(theta[i]),a[i]*sin(theta[i]),b[i]]);
Q[i]:= matrix([
[cos(theta[i]),-cos(alpha[i])*sin(theta[i]),
sin(alpha[i])*sin(theta[i])],
[sin(theta[i]),cos(alpha[i])*cos(theta[i]),
-sin(alpha[i])*cos(theta[i])],
[0,sin(alpha[i]),cos(alpha[i])]]) od:
Factor matrix Q i as Q=Z iX i, where X i and Z i are two reflections, as per eq. (4.2b):
> for i from 1 to 6 do
> X[i]:=matrix([[1,0,0],
> [0,-cos(alpha[i]),sin(alpha[i])],
> [0,sin(alpha[i]), cos(alpha[i])]]);
> Z[i]:=matrix([[cos(theta[i]),sin(theta[i]),0],
>
[sin(theta[i]),-cos(theta[i]), 0],
>
[0,0,1]]) od:
Vectors c[i], of eq. (4.3e), and γ[i], of eq.(9.9), are given below. The notation c[i] is used instead of b[i] in
order to avoid confusion with scalar b[i]. No confusion with cos(theta[i]) =c[i] in FRMS, because we don’t
use this shorthand notation here. The notation “gama” stands for the Greek letter γ defined in eq.(9.9),
but because Maple doesn’t allow the user to define a variable as γ, the Maple file uses “gama” instead.
> for i from 1 to 6 do
> c[i]:=vector([a[i],b[i]*sin(alpha[i]),b[i]*cos(alpha[i])]);
> gama[i]:=vector([a[i],0,b[i]])
> od:
>
The (dimensionless) position of the EE operation point P, of position vector p, and the EE orientation
given by the rotation matrix Rot:
> x:=0: y:=-50: z:=50: p:=vector([x/L,y/L,z/L]):
> #Data normalized by division by L
> Rot:=matrix([[0,-1,0],[0,0,-1],[1,0,0]]):
Derivation of the Fundamental Closure Equations
> rho:=map(simplify,evalm(p-Rot&*c[6])): #rho given in line after eq.(9.21b)
o6 - eq. (9.6), sigma6 - eq.(9.20c), u5 - eq. (9.7), f - eq. (9.22c), g - eq. (9.22d), h - eq. (9.22a),
i - eq. (9.22b) (both h and i are unit vectors):
171
>
>
>
>
>
>
>
>
>
>
o6:=row(Q[6],3):
sigma6:=multiply(Rot,o6): #sigma6 is sigma_[6] in FRMS
i:=multiply(transpose(Q[2]),transpose(Q[1]),sigma6):
u5:=col(Q[5],3):
f:=map(simplify,evalm(multiply(Q[3],(c[3]+multiply(Q[4],c[4])
+multiply(Q[4],Q[5],c[5]))))):
g:=evalm(multiply(transpose(Q[2]),(multiply(transpose(Q[1]),rho))
-c[1])-c[2]):
h:=multiply(Q[3],Q[4],u5):
The Bivariate-Equation Approach with Raghavan-Roth Procedure
f -tilde (f t) - eq.(9.29a), h-tilde (ht) - eq.(9.29b), r-tilde (rt) - eq.(9.29c), n-tilde (nt) - eq.(9.29d)
> ft:=map(simplify,multiply(Z[3],f)):
> ht:=map(simplify,multiply(transpose(Q[1]),rho)-c[1]):
> rt:=multiply(X[3],Q[4],u5):
> nt:=multiply(transpose(Q[1]),sigma6):
Derive Raghavan and Roth’s eqs. (9.34a–f):
f bar (f b) - eq. (9.33a), g bar (gb) - eq. (9.33b), h bar (hb) - eq. (9.33c), i bar (ib)- eq. (9.33d)
LHS and RHS of eq.(9.34c)
> dfb:=simplify(multiply(fb,fb),trig):
> dgb:=simplify(multiply(gb,gb),trig):
LHS and RHS of eq.(9.34d)
> dfhb:=simplify(multiply(fb,hb),trig):
> dgib:=simplify(multiply(gb,ib),trig):
LHS and RHS of eq.(9.34e)
> cfhb:=map(simplify,crossprod(fb,hb)):
> cgib:=map(simplify,crossprod(gb,ib)):
LHS and RHS of eq.(9.34f)
Start by deriving matrices P bar and R bar, eq.(9.35), using eqs.(9.34a–f)
Define vector x45, eqs.(9.28a) as a 9-dimensional vector
> x45:=vector(
> [sin(theta[4])*sin(theta[5]),sin(theta[4])*cos(theta[5]),
> cos(theta[4])*sin(theta[5]),cos(theta[4])*cos(theta[5]),
> sin(theta[4]),cos(theta[4]),sin(theta[5]),cos(theta[5]),1]):
Then, obtain matrices RBex and PB pr as the coefficients of vectors x12ex and x45 in RBx12ex and
PBx45 pr, respectively
172
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
RBex:=matrix(14,9):
for i from 1 to 14 do
RBex[i,1]:=coeff(coeff(RBx12ex[i],sin(theta[1])),sin(theta[2])):
RBex[i,2]:=coeff(coeff(RBx12ex[i],sin(theta[1])),cos(theta[2])):
RBex[i,3]:=coeff(coeff(RBx12ex[i],cos(theta[1])),sin(theta[2])):
RBex[i,4]:=coeff(coeff(RBx12ex[i],cos(theta[1])),cos(theta[2])):
RBex[i,5]:=coeff(coeff(coeff(coeff(RBx12ex[i],sin(theta[1])),
cos(theta[1]),0),sin(theta[2]),0),cos(theta[2]),0):
RBex[i,6]:=coeff(coeff(coeff(coeff(RBx12ex[i],cos(theta[1])),
sin(theta[1]),0),sin(theta[2]),0),cos(theta[2]),0):
RBex[i,7]:=coeff(coeff(coeff(coeff(RBx12ex[i],sin(theta[2])),
cos(theta[1]),0),sin(theta[1]),0),cos(theta[2]),0):
RBex[i,8]:=coeff(coeff(coeff(coeff(RBx12ex[i],cos(theta[2])),
sin(theta[2]),0),sin(theta[1]),0),cos(theta[1]),0):
RBex[i,9]:=coeff(coeff(coeff(coeff(RBx12ex[i],sin(theta[1]),0),
cos(theta[1]),0),sin(theta[2]),0),cos(theta[2]),0):
od:
RBex:=evalm(RBex):
PB_pr:=matrix(14,9):
for i from 1 to 14 do
PB_pr[i,1]:=coeff(coeff(PBx45_pr[i],sin(theta[4])),sin(theta[5])):
PB_pr[i,2]:=coeff(coeff(PBx45_pr[i],sin(theta[4])),cos(theta[5])):
PB_pr[i,3]:=coeff(coeff(PBx45_pr[i],cos(theta[4])),sin(theta[5])):
PB_pr[i,4]:=coeff(coeff(PBx45_pr[i],cos(theta[4])),cos(theta[5])):
PB_pr[i,5]:=coeff(coeff(coeff(coeff(PBx45_pr[i],sin(theta[4])),
cos(theta[4]),0),sin(theta[5]),0),cos(theta[5]),0):
PB_pr[i,6]:=coeff(coeff(coeff(coeff(PBx45_pr[i],cos(theta[4])),
sin(theta[4]),0),sin(theta[5]),0),cos(theta[5]),0):
PB_pr[i,7]:=coeff(coeff(coeff(coeff(PBx45_pr[i],sin(theta[5])),
cos(theta[4]),0),sin(theta[4]),0),cos(theta[5]),0):
PB_pr[i,8]:=coeff(coeff(coeff(coeff(PBx45_pr[i],cos(theta[5])),
sin(theta[5]),0),sin(theta[4]),0),cos(theta[4]),0):
PB_pr[i,9]:=coeff(coeff(coeff(coeff(PBx45_pr[i],sin(theta[4]),0),
cos(theta[4]),0),sin(theta[5]),0),cos(theta[5]),0):od:
PB_pr:=evalm(PB_pr):
Verify derivation of matrices RBex and PB pr
> checkRBex:=evalm(multiply(RBex,x12ex)-RBx12ex);
> checkPB_pr:=map(simplify,evalm(multiply(PB_pr,x45)-PBx45_pr)):
Now, derive the 14x8 matrix RB from matrix of RBx12ex with last column deleted:
> RB:=delcols(RBex,9..9):
> #RB is R_bar in the RHS of eq.(9.35)
> nine:=col(RBex,9):
PB is the 14x9 submatrix of matrix of PBx45 pr, with its last column being the last column of matrix
PB pr minus the last column of matrix of RBx12ex (vector nine)
> PB:=matrix(14,9):
> for i from 1 to 14 do
> PB[i,1]:=PB_pr[i,1]: PB[i,2]:=PB_pr[i,2]: PB[i,3]:=PB_pr[i,3]:
> PB[i,4]:=PB_pr[i,4]: PB[i,5]:=PB_pr[i,5]: PB[i,6]:=PB_pr[i,6]:
> PB[i,7]:=PB_pr[i,7]: PB[i,8]:=PB_pr[i,8]:
> PB[i,9]:=PB_pr[i,9]-nine[i]:
> od:
Partition eq.(9.35) into two groups:
173
>
>
>
>
>
C:=delcols(delcols(matrix([row(RB,3),row(RB,6),row(RB,7),row(RB,8),
row(RB,11),row(RB,14)]),1..4),3..4):
PuB:=matrix([row(PB,3),row(PB,6),row(PB,7),row(PB,8),row(PB,11),
row(PB,14)]):
where C is a 6x2 constant matrix that contains the nonzero entries in rows 3,6,7,8,11, and 14 of matrix
R bar, while PuB is the matrix on LHS of eq. (9.37a)
To derive matrices Cu and Cl as in eqs. (9.39a & b), we partition eq. (9.37a)
Below are all possible cases of such partitioning.
>
# k:=1:
> m:=2:
# 12
> # k:=1:
m:=3:
# 13
>
k:=1:
m:=4:
# 14
> # k:=1:
m:=5:
# 15
> # k:=1:
m:=6:
# 16
> # k:=2:
m:=3:
# 23
> # k:=2:
m:=4:
# 24
> # k:=2:
m:=5:
# 25
> # k:=2:
m:=6:
# 26
> # k:=3:
m:=4:
# 34
> # k:=3:
m:=5:
# 35
> # k:=3:
m:=6:
# 36
> # k:=4:
m:=5:
# 45
> # k:=4:
m:=6:
# 46
> # k:=5:
m:=6:
# 56
> Cu:=matrix(2,2):
> Cu[1,1]:=C[k,1]:
> Cu[1,2]:=C[k,2]:
> Cu[2,1]:=C[m,1]:
> Cu[2,2]:=C[m,2]:
> Cl:=matrix(4,2):
> n:=1: for i from 1 to 6 do
> if((i=k) or (i=m))
> then
> else
> Cl[n,1]:=C[i,1]:
> Cl[n,2]:=C[i,2]:
>
n:=n+1:
> fi:
> od:
In the above partitioning, the equations must be grouped such that Cu be a nonsingular matrix. Hence,
check its determinant:
> evalf(det(Cu));
2.0
If this determinant is zero, choose other values of k and m.
Derive vector d, LHS of eq. (9.37a), and du, dl of eqs. (9.39a & b)
174
d:=map(simplify,multiply(PuB,x45)):
du:=vector(2):
du[1]:=d[k]: du[2]:=d[m]:
dl:=vector(4):
n:=1: for i from 1 to 6 do
if((i=k) or (i=m))
then
else
dl[n]:=d[i]:
n
:=n+1:
fi:
od:
Then, obtain vector gamma4x45, eq. (9.40a),
> gamma4x45:=map(simplify,evalm(multiply(Cl,inverse(Cu),du)-dl)):
>
>
>
>
>
>
>
>
>
>
>
>
Finally, derive the 4x3 matrix D1 of eq. (9.40b). Entries of matrix D1 are bilinear in x 4 and x 5 while
y 3=[c3,s3,1]ˆT
> D1:=matrix(4,3):
> for i from 1 to 4 do
> D1[i,1]:=coeff(gamma4x45[i],cos(theta[3])):
> D1[i,2]:=coeff(gamma4x45[i],sin(theta[3])):
> D1[i,3]:=simplify(gamma4x45[i]-D1[i,1]*cos(theta[3])
> -D1[i,2]*sin(theta[3])):
> od:
> DD1:=evalf(subs(theta[4]=Pi/2,theta[5]=Pi/2,evalm(D1)));


0.0000000001
0.0
3.999999999



0.0
−0.0000000004 −3.999999999 
DD1 := 

 0.00000000001 0.0000000002
7.999999997 
0.00000000002 −0.0000000004 −4.00000000
> H:=delcols(DD1,3..3);


0.0000000001
0.0



0.0
−0.0000000004 
H := 

 0.00000000001 0.0000000002 
0.00000000002 −0.0000000004
175
Appendix 3
For starters, we generate the four nonlinear equations of interest using the Maple code below:
restart: with(linalg):
The DH parameters of the Fanuc Arc Mate, as per Table 5.2, are reproduced below. Vectors a[i] in
FRMS are labelled here q[i], to avoid confusion with scalars a[i].
> a[1]:=200: a[2]:=600: a[3]:=130:
> a[4]:=0:
a[5]:=0:
a[6]:=0:
> b[1]:=810: b[2]:=0:
b[3]:=-30:
b[4]:=550: b[5]:=100: b[6]:=100:
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
alpha[1]:=Pi/2:
alpha[2]:=0:
alpha[3]:=Pi/2:
alpha[4]:=Pi/2:
alpha[5]:=Pi/2: alpha[6]:=0:
L:=35123/100: # the CHARACTERISTIC
LENGTH found MD-05-1171 paper
for i from 1 to 6 do
a[i]:=a[i]/L: b[i]:=b[i]/L: od:
for i from 1 to 6 do
q[i]:=vector([a[i]*cos(theta[i]),a[i]*sin(theta[i]),b[i]]):
Q[i]:= matrix([
[cos(theta[i]),-cos(alpha[i])*sin(theta[i]),
sin(alpha[i])*sin(theta[i])],
[sin(theta[i]),cos(alpha[i])*cos(theta[i]),
-sin(alpha[i])*cos(theta[i])],
[0,sin(alpha[i]),cos(alpha[i])]]) od:
Factor matrix Q i as Q=Z iX i, where X i and Z i are two reflections, as per eq. (4.2b):
> for i from 1 to 6 do
> X[i]:=matrix([[1,0,0],
> [0,-cos(alpha[i]),sin(alpha[i])],
> [0,sin(alpha[i]), cos(alpha[i])]]):
> Z[i]:=matrix([[cos(theta[i]),sin(theta[i]),0],
>
[sin(theta[i]),-cos(theta[i]), 0],
>
[0,0,1]]) od:
Vectors c[i], of eq.(4.3e), and γ[i], of eq.(9.9), are given below. We use “gama” to represent γ in this
worksheet. The notation c[i] is used instead of b[i] in order to avoid confusion with scalar b[i]. No confusion
with cos(theta[i]) =c[i] in FRMS, because we don’t use this shorthand notation here.
> for i from 1 to 6 do
> c[i]:=vector([a[i],b[i]*sin(alpha[i]),b[i]*cos(alpha[i])]):
> gama[i]:=vector([a[i],0,b[i]]) od:
The position of the EE operation point P, of position vector p, and the EE orientation given by the
rotation matrix Rot:
> x:=130/L: y:=850/L: z:=1540/L:
> p:=vector([x,y,z]):
> Rot:=matrix([[0,1,0],[0,0,1],[1,0,0]]):
Derivation of the Fundamental Closure Equations
> rho:=map(simplify,evalm(p-Rot&*c[6])):
> #p.328, next line after eq.(9.18b)
Computing o6 - eq.(9.6), sigma, which is σ[6] in the book - eq.(9.20a), u5 - eq.(9.7), f - eq.(9.22c),
g - eq.(9.22d), h - eq.(9.22a), i - eq.(9.22b) (both h and i are unit vectors):
176
>
>
>
>
>
>
>
>
>
>
o6:=row(Q[6],3): sigma:=multiply(Rot,o6):
i:=multiply(transpose(Q[2]),transpose(Q[1]),sigma):
u5:=col(Q[5],3):
f:=map(simplify,evalm(multiply(Q[3],(c[3]+multiply(Q[4],c[4])
+multiply(Q[4],Q[5],c[5]))))):
g:=evalm(multiply(transpose(Q[2]),
(multiply(transpose(Q[1]),rho)-c[1]))-c[2]):
h:=multiply(Q[3],Q[4],u5):
The Bivariate-Equation Approach with the Raghavan-Roth Procedure follows:
f -tilde (f t)— eq.(9.29a), h-tilde (ht)–eq.(9.29b), r-tilde (rt)–eq.(9.29c), n-tilde (nt)–eq.(9.29d)
> ft:=map(simplify,multiply(Z[3],f)):
> ht:=map(simplify,multiply(transpose(Q[1]),rho)-c[1]):
> rt:=multiply(X[3],Q[4],u5):
> nt:=multiply(transpose(Q[1]),sigma):
Derive Raghavan and Roth’s eqs.(9.34a–9.34f):
f bar (f b)—eq.(9.33a), g bar (gb)— eq.(9.33b), h bar (hb)—eq.(9.33c), i bar (ib)—eq.(9.33d)
LHS and RHS of eq.(9.34c)
> dfb:=simplify(multiply(fb,fb),trig):
> dgb:=simplify(multiply(gb,gb),trig):
LHS and RHS of eq.(9.34d)
> dfhb:=simplify(multiply(fb,hb),trig):
> dgib:=simplify(multiply(gb,ib),trig):
LHS and RHS of eq.(9.34e)
> cfhb:=map(simplify,crossprod(fb,hb)):
> cgib:=map(simplify,crossprod(gb,ib)):
LHS and RHS of eq.(9.34f)
> reffhb:=map(simplify,evalm(dfb*hb-2*dfhb*fb)):
> #the reflection of the h_bar wrs to the plane
> #with the unit normal f_bar
>
>
>
>
refgib:=map(simplify,evalm(dgb*ib-2*dgib*gb)):
#the reflection of i_bar wrs to the plane with
#the unit normal g_bar
Derive the contour equations by eliminating three of the remaining five unknown joint angles from the
closure equations.
Start by deriving matrices P bar and R bar, eq.(9.35), using eqs.(9.34a)–(9.34f)
Define vectors x12 and x45, eqs.(9.28) as 8- and 9-dimensional vectors, correspondingly
> x12:=vector(
> [sin(theta[1])*sin(theta[2]),sin(theta[1])*cos(theta[2]),
> cos(theta[1])*sin(theta[2]),cos(theta[1])*cos(theta[2]),
> sin(theta[1]),cos(theta[1]),sin(theta[2]),cos(theta[2])]):
>
>
>
>
>
x45:=vector(
[sin(theta[4])*sin(theta[5]),sin(theta[4])*cos(theta[5]),
cos(theta[4])*sin(theta[5]),cos(theta[4])*cos(theta[5]),
sin(theta[4]),cos(theta[4]),sin(theta[5]),cos(theta[5]),1]):
and an auxiliary vector x12ex (not in the book), which is a 9-dimensional array defined as vector x12
with an added 9th component, equal to 1
177
x12ex:=vector(
[sin(theta[1])*sin(theta[2]),sin(theta[1])*cos(theta[2]),
cos(theta[1])*sin(theta[2]),cos(theta[1])*cos(theta[2]),
sin(theta[1]),cos(theta[1]),sin(theta[2]),cos(theta[2]),1]):
and obtain vectors
> RBx12ex:=vector([gb[1],gb[2],gb[3],ib[1],ib[2],ib[3],dgb,dgib,
> cgib[1],cgib[2],cgib[3],refgib[1],refgib[2],refgib[3]]):
>
>
>
>
>
>
>
PBx45_pr:=vector([fb[1],fb[2],fb[3],hb[1],hb[2],hb[3],dfb,dfhb,
cfhb[1],cfhb[2],cfhb[3],reffhb[1],reffhb[2],reffhb[3]]):
Then, obtain matrices RBex and PB pr as the coefficients of vectors x12ex and x45 in RBx12ex and
PBx45 pr, respectively
> RBex:=matrix(14,9):
> for i from 1 to 14 do
> RBex[i,1]:=coeff(coeff(RBx12ex[i],sin(theta[1])),sin(theta[2])):
> RBex[i,2]:=coeff(coeff(RBx12ex[i],sin(theta[1])),cos(theta[2])):
> RBex[i,3]:=coeff(coeff(RBx12ex[i],cos(theta[1])),sin(theta[2])):
> RBex[i,4]:=coeff(coeff(RBx12ex[i],cos(theta[1])),cos(theta[2])):
> RBex[i,5]:=coeff(coeff(coeff(coeff(RBx12ex[i],sin(theta[1])),
> cos(theta[1]),0),sin(theta[2]),0),cos(theta[2]),0):
> RBex[i,6]:=coeff(coeff(coeff(coeff(RBx12ex[i],cos(theta[1])),
> sin(theta[1]),0),sin(theta[2]),0),cos(theta[2]),0):
> RBex[i,7]:=coeff(coeff(coeff(coeff(RBx12ex[i],sin(theta[2])),
> cos(theta[1]),0),sin(theta[1]),0),cos(theta[2]),0):
> RBex[i,8]:=coeff(coeff(coeff(coeff(RBx12ex[i],cos(theta[2])),
> sin(theta[2]),0),sin(theta[1]),0),cos(theta[1]),0):
> RBex[i,9]:=coeff(coeff(coeff(coeff(RBx12ex[i],sin(theta[1]),0),
> cos(theta[1]),0),sin(theta[2]),0),cos(theta[2]),0):
> od:
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
RBex:=evalm(RBex):
PB_pr:=matrix(14,9):
for i from 1 to 14 do
PB_pr[i,1]:=coeff(coeff(PBx45_pr[i],sin(theta[4])),sin(theta[5])):
PB_pr[i,2]:=coeff(coeff(PBx45_pr[i],sin(theta[4])),cos(theta[5])):
PB_pr[i,3]:=coeff(coeff(PBx45_pr[i],cos(theta[4])),sin(theta[5])):
PB_pr[i,4]:=coeff(coeff(PBx45_pr[i],cos(theta[4])),cos(theta[5])):
PB_pr[i,5]:=coeff(coeff(coeff(coeff(PBx45_pr[i],sin(theta[4])),
cos(theta[4]),0),sin(theta[5]),0),cos(theta[5]),0):
PB_pr[i,6]:=coeff(coeff(coeff(coeff(PBx45_pr[i],cos(theta[4])),
sin(theta[4]),0),sin(theta[5]),0),cos(theta[5]),0):
PB_pr[i,7]:=coeff(coeff(coeff(coeff(PBx45_pr[i],sin(theta[5])),
cos(theta[4]),0),sin(theta[4]),0),cos(theta[5]),0):
PB_pr[i,8]:=coeff(coeff(coeff(coeff(PBx45_pr[i],cos(theta[5])),
sin(theta[5]),0),sin(theta[4]),0),cos(theta[4]),0):
PB_pr[i,9]:=coeff(coeff(coeff(coeff(PBx45_pr[i],sin(theta[4]),0),
cos(theta[4]),0),sin(theta[5]),0),cos(theta[5]),0):od:
PB_pr:=evalm(PB_pr):
Verify derivation of matrices RBex and PB pr
checkRBex := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
checkPB pr := [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
Now, derive the 14x8 matrix RB from matrix of RBx12ex with last column deleted:
178
RB:=delcols(RBex,9..9):
#RB is R_bar in the RHS of eq.(9.35)
> nine:=col(RBex,9):
PB is the 14x9 submatrix of matrix of PBx45 pr, with its last column being the last column of matrix
PB pr minus the last column of matrix of RBx12ex (vector nine)
> PB:=matrix(14,9):
> for i from 1 to 14 do
> PB[i,1]:=PB_pr[i,1]: PB[i,2]:=PB_pr[i,2]:
> PB[i,3]:=PB_pr[i,3]: PB[i,4]:=PB_pr[i,4]: PB[i,5]:=PB_pr[i,5]:
> PB[i,6]:=PB_pr[i,6]: PB[i,7]:=PB_pr[i,7]: PB[i,8]:=PB_pr[i,8]:
> PB[i,9]:=PB_pr[i,9]-nine[i]:
> od:
and vector x12t, x12-tilde, of eq.(9.38):
> x12t:=vector([sin(theta[1])*sin(theta[2]),
> sin(theta[1])*cos(theta[2]),cos(theta[1])*sin(theta[2]),
> cos(theta[1])*cos(theta[2]),sin(theta[2]),cos(theta[2])]):
>
>
Partition eq.(9.35) into two groups by deriving:
> C:=delcols(delcols(matrix([row(RB,3),row(RB,6),row(RB,7),row(RB,8),
> row(RB,11),row(RB,14)]),1..4),3..4):
>
>
>
PuB:=matrix([row(PB,3),row(PB,6),row(PB,7),row(PB,8),row(PB,11),
row(PB,14)]):
where C is a 6x2 constant matrix that contains the nonzero entries in rows 3,6,7,8,11, and 14 of matrix
R bar, while PuB is the matrix on LHS of eq. (9.37a) Then, derive an 8x6 matrix A from RB, and
compute PlB as the matrix on LHS of eq. (9.37b)
> A:=delcols(matrix([row(RB,1),row(RB,2),row(RB,4),row(RB,5),row(RB,9),
> row(RB,10),row(RB,12),row(RB,13)]),5..6):
> PlB:=matrix([row(PB,1),row(PB,2),row(PB,4),row(PB,5),row(PB,9),
> row(PB,10),row(PB,12),row(PB,13)]):
To derive matrices Cu and Cl as in eqs. (9.39a & b), we partition eq.(9.37a)
Below are all possible cases of such partitioning.
179
k:=1:
m:=2:
# 12
# k:=1:
m:=3:
# 13
# k:=1:
m:=4:
# 14
# k:=1:
m:=5:
# 15
# k:=1:
m:=6:
# 16
# k:=2:
m:=3:
# 23
# k:=2:
m:=4:
# 24
# k:=2:
m:=5:
# 25
# k:=2:
m:=6:
# 26
# k:=3:
m:=4:
# 34
# k:=3:
m:=5:
# 35
# k:=3:
m:=6:
# 36
# k:=4:
m:=5:
# 45
# k:=4:
m:=6:
# 46
# k:=5:
m:=6:
# 56
Cu:=matrix(2,2):
Cu[1,1]:=C[k,1]:
Cu[1,2]:=C[k,2]:
Cu[2,1]:=C[m,1]:
Cu[2,2]:=C[m,2]:
Cl:=matrix(4,2):
n:=1: for i from 1 to 6 do
if((i=k) or (i=m))
then
else
Cl[n,1]:=C[i,1]:
Cl[n,2]:=C[i,2]:
n:=n+1:
fi:
od:
In the above partitioning, the equations must be grouped such that Cu be a nonsingular matrix. Hence,
check its determinant:
> evalf(det(Cu));
−0.3701278365
If this determinant is zero, choose other values of k and m.
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
Derive vector d, LHS of eq. (9.37a), and du, dl of eqs. (9.39a & b)
> d:=map(simplify,multiply(PuB,x45)):
> du:=vector(2):
du[1]:=d[k]: du[2]:=d[m]:
> dl:=vector(4):
> n:=1: for i from 1 to 6 do
> if((i=k) or (i=m))
> then
> else
> dl[n]:=d[i]:
>
n
> :=n+1:
> fi:
> od:
Then, obtain vector gamma4x45, eq. (9.40a),
> gamma4x45:=map(simplify,evalm(multiply(Cl,inverse(Cu),du)-dl)):
Finally, derive the 4x3 matrix D1 of eq. (9.40b). Entries of matrix D1 are bilinear in x 4 and x 5 while
y 3=[c3,s3,1]ˆT
180
D1:=matrix(4,3):
for i from 1 to 4 do
D1[i,1]:=coeff(gamma4x45[i],cos(theta[3])):
D1[i,2]:=coeff(gamma4x45[i],sin(theta[3])):
D1[i,3]:=simplify(gamma4x45[i]-D1[i,1]*cos(theta[3])
-D1[i,2]*sin(theta[3])):
od:
Derive four 3x3 submatrices of D1
> subD1:=matrix(3,3): subD2:=matrix(3,3):
> subD3:=matrix(3,3): subD4:=matrix(3,3):
> for i from 1 to 3 do
for j from 1 to 3 do
> subD1[i,j]:=D1[i,j]:
> od:
> od:
> m:=1: n:=1: for i from 1 to
> 4 do
if (i=3)
> then
> else
>
for j from 1 to 3 do
> subD2[m,n]:=D1[i,j]:
> n:=n+1:
> od:
> m:=m+1:
> fi:
> n:=1:
> od:
> m:=1: n:=1: for i from 1 to
> 4 do
if (i=2)
> then
> else
>
for j from 1 to 3 do
> subD3[m,n]:=D1[i,j]:
> n:=n+1:
> od:
> m:=m+1:
> fi:
> n:=1:
> od:
> m:=1: n:=1: for i from 2 to
> 4 do
for j from 1 to
> 3 do
> subD4[m,n]:=D1[i,j]:
> n:=n+1:
> od:
> n:=1:
> m:=m+1:
> od:
It is clear that the trivial solution of y 3 is not admissible, since y 3 cannot be zero. Hence, D1 must
be rank-deficient, and hence, the determinants of the 3x3 submatrices of D1, subD1, subD2, subD3, and
subD4, must vanish, which leads to four equations:
> eq1:=evalf(simplify(det(subD1),trig));
> eq2:=evalf(simplify(det(subD2),trig));
> eq3:=evalf(simplify(det(subD3),trig));
> eq4:=evalf(simplify(det(subD4),trig));
Next, with the numerical data of Example 9.7.1, we produce the four equations in θ4 and θ5 .
>
>
>
>
>
>
>
181
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
eq1:=
19.92539102*cos(theta[5])*sin(theta[5])-.7970156408*sin(theta[4])
*cos(theta[5])+5.317076946*cos(theta[4])-0.5459011238e-1*cos(theta[4])
*sin(theta[5])-19.35110304*cos(theta[5])+42.17268149*sin(theta[4])
*cos(theta[4])^2*sin(theta[5])*cos(theta[5])^2
+24.87562241*cos(theta[4])^2*cos(theta[5])^2
+10.91802248*cos(theta[4])^3*cos(theta[5])^2
-.5495404647*cos(theta[5])*cos(theta[4])+3.329996855*cos(theta[4])
*sin(theta[5])*sin(theta[4])-24.87562241*cos(theta[5])^2
-.6150485995*sin(theta[4])*cos(theta[5])^2+15.55818203
*cos(theta[4])^2*sin(theta[5])-3.340914878*cos(theta[5])
*sin(theta[4])*sin(theta[5])-24.40250810*cos(theta[4])^2
+2.656718803*sin(theta[4])*cos(theta[5])*cos(theta[4])-41.44481332
*sin(theta[4])*cos(theta[4])^2*sin(theta[5])-2.602128690
*cos(theta[4])*sin(theta[5])*sin(theta[4])*cos(theta[5])-10.91802248
*cos(theta[5])^2*cos(theta[4])-42.17268149*cos(theta[5])
^2*sin(theta[4])*sin(theta[5])+116.5018506*cos(theta[4])
*sin(theta[4])-116.5018506*cos(theta[4])*sin(theta[4])*cos(theta[5])
^2-19.92539102*cos(theta[5])*sin(theta[5])*cos(theta[4])
^2-10.91802248*cos(theta[4])^3:
eq2:=
30.35647305*cos(theta[5])+128.4312321*cos(theta[4])
*sin(theta[5])-10.25808563*cos(theta[4])*sin(theta[4])+23.35113312
*sin(theta[4])*cos(theta[5])-2.424638423*cos(theta[5])*sin(theta[4])
*sin(theta[5])-1.367744751*cos(theta[4])^2*sin(theta[5])
-.3232851230*cos(theta[5])*cos(theta[4])-129.7989769*cos(theta[4])
^3*sin(theta[5])+31.21856671*cos(theta[4])^2
*cos(theta[5])^3-24.01428209*sin(theta[4])*cos(theta[5])
^3+34.19361878*cos(theta[4])^2*sin(theta[4])
+8.082128076*cos(theta[4])*sin(theta[5])*sin(theta[4])*cos(theta[5])
-.2486808639*sin(theta[4])*cos(theta[5])*cos(theta[4])+10.25808563
*cos(theta[4])*sin(theta[4])*cos(theta[5])^2+1.865106479
*cos(theta[5])*sin(theta[5])*cos(theta[4])^2-30.67975818
*cos(theta[5])*cos(theta[4])^2-132.0785515*cos(theta[4])
*sin(theta[5])*cos(theta[5])^2+24.01428209*sin(theta[4])
*cos(theta[5])^3*cos(theta[4])^2-34.19361878
*sin(theta[4])*cos(theta[4])^2*cos(theta[5])^2
-31.21856671*cos(theta[5])^3+132.0785515*cos(theta[4])
^3*sin(theta[5])*cos(theta[5])^2+6.217021597
*cos(theta[5])*cos(theta[4])*sin(theta[5])-23.59981398*sin(theta[4])
*cos(theta[5])*cos(theta[4])^2-6.217021597*cos(theta[4])
^3*sin(theta[5])*cos(theta[5])-1.865106479*cos(theta[5])
*sin(theta[5]):
182
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
eq3 := -.9567996237*cos(theta[4])-131.7369469*cos(theta[5])
+7.204284626*sin(theta[4])*cos(theta[4])^2*sin(theta[5])
*cos(theta[5])^2+4.249458602*cos(theta[4])^2
*cos(theta[5])^2+30.34714752*cos(theta[4])*sin(theta[5])
-14.16486201*cos(theta[4])^3*cos(theta[5])^2
+17.47716677*cos(theta[4])*sin(theta[4])-.1361527730*sin(theta[4])
*cos(theta[5])+1.481723481*sin(theta[4])*cos(theta[5])*sin(theta[5])
*cos(theta[4])^2+9.687363052*cos(theta[5])*sin(theta[4])
*sin(theta[5])+23.91999059*cos(theta[4])*sin(theta[5])*sin(theta[4])
-.1050676650*sin(theta[4])*cos(theta[5])^2+2.365576718
*cos(theta[4])^2*sin(theta[5])+12.29291680*cos(theta[5])
*cos(theta[4])-4.249458602*cos(theta[5])^2-39.53901395
*cos(theta[4])^3*sin(theta[5])-132.0785515*cos(theta[4])
^2*cos(theta[5])^3-58.25722257*cos(theta[4])
^2*sin(theta[4])-32.73572722*cos(theta[4])*sin(theta[5])
*sin(theta[4])*cos(theta[5])+.9076851531*sin(theta[4])*cos(theta[5])
*cos(theta[4])-9.224816645*sin(theta[4])*cos(theta[4])^2
*sin(theta[5])
-7.204284626*cos(theta[5])^2*sin(theta[4])*sin(theta[5])
-17.12694122*cos(theta[4])*sin(theta[4])*cos(theta[5])^2
-3.403819324*cos(theta[5])*sin(theta[5])*cos(theta[4])^2
+130.1119003*cos(theta[5])*cos(theta[4])^2+14.16486201
*cos(theta[5])^2*cos(theta[4])-31.21856671*cos(theta[4])
*sin(theta[5])*cos(theta[5])^2+58.25722257*sin(theta[4])
*cos(theta[4])^2*cos(theta[5])^2+132.0785515
*cos(theta[5])^3+31.21856671*cos(theta[4])^3
*sin(theta[5])*cos(theta[5])^2-11.34606441*cos(theta[5])
*cos(theta[4])*sin(theta[5])-1.512808589*sin(theta[4])*cos(theta[5])
*cos(theta[4])^2+11.34606441*cos(theta[4])^3
*sin(theta[5])*cos(theta[5])+3.403819324*cos(theta[5])*sin(theta[5])
-.9793052419*cos(theta[4])^2+13.89545774*cos(theta[4])^3:
eq4 := 3.329996855*cos(theta[4])-3.340914878*cos(theta[5])
+28.15102915*sin(theta[4])*cos(theta[4])^2*sin(theta[5])
*cos(theta[5])^2+1.164589064*cos(theta[4])^2
*cos(theta[5])^2+116.5018506*cos(theta[4])*sin(theta[5])
+3.766717755*cos(theta[4])^3*cos(theta[5])^2
-0.5459011238e-1*cos(theta[4])*sin(theta[4])+19.92539102*sin(theta[4])
*cos(theta[5])-19.35110304*cos(theta[5])*sin(theta[4])*sin(theta[5])
+5.317076946*cos(theta[4])*sin(theta[5])*sin(theta[4])+.6150485995
*cos(theta[4])^2*sin(theta[5])*cos(theta[5])^2
-2.602128690*cos(theta[5])*cos(theta[4])+2.602128690*cos(theta[4])
^3*cos(theta[5])+2.602128690*cos(theta[5])^3
*cos(theta[4])-1.164589064*cos(theta[5])^2-116.5018506
*cos(theta[4])^3*sin(theta[5])-3.340914878*cos(theta[4])
^2*cos(theta[5])^3-19.92539102*sin(theta[4])
*cos(theta[5])^3+15.55818203*cos(theta[4])^2
*sin(theta[4])-.5495404647*cos(theta[4])*sin(theta[5])*sin(theta[4])
*cos(theta[5])-27.67791485*sin(theta[4])*cos(theta[4])^2
*sin(theta[5])-28.15102915*cos(theta[5])^2*sin(theta[4])
*sin(theta[5])+0.5459011238e-1*cos(theta[4])*sin(theta[4])*cos(theta[5])
^2+.7970156408*cos(theta[5])*sin(theta[5])*cos(theta[4])
^2+3.340914878*cos(theta[5])*cos(theta[4])^2
-3.766717755*cos(theta[5])^2*cos(theta[4])-116.5018506
*cos(theta[4])*sin(theta[5])*cos(theta[5])^2+19.92539102
*sin(theta[4])*cos(theta[5])^3*cos(theta[4])^2
-15.55818203*sin(theta[4])*cos(theta[4])^2*cos(theta[5])
^2+3.340914878*cos(theta[5])^3+116.5018506
*cos(theta[4])^3*sin(theta[5])*cos(theta[5])^2
+2.656718803*cos(theta[5])*cos(theta[4])*sin(theta[5])-19.92539102
*sin(theta[4])*cos(theta[5])*cos(theta[4])^2-2.656718803
*cos(theta[4])^3*sin(theta[5])*cos(theta[5])-.7970156408
*cos(theta[5])*sin(theta[5])-.4367208991*cos(theta[4])^2
183
-3.766717755*cos(theta[4])^3-.6150485995*cos(theta[5])
^2*sin(theta[5])-2.602128690*cos(theta[5])^3
*cos(theta[4])^3:
Definition of the tolerance
> epsilon:=10^(-6):
Max number of iterations allowed
> N:=20:
Initial guess
> x[0]:=Vector([0.26, 4.47]):
#Taken from the rough estimates
1. Consider eq1 and eq2. Notice that the Maple command ”fsolve” can solve the problem at hand,
however, it does not provide information of the number of iterations.
> F:=Matrix(2,2,
> [[diff(eq1,theta[4]),diff(eq1,theta[5])],
> [diff(eq2,theta[4]),diff(eq2,theta[5])]]):
>
>
>
>
>
>
>
>
>
>
>
>
f:=Vector(2,[eq1,eq2]):
for i from 0 to N do
FF[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(F)));
ff[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(f)));
tt:=linsolve(FF[i],-ff[i]);
x[i+1]:=evalm(x[i]+tt);
nx[i+1]:=norm(tt,infinity);
#print(i,tt,x[i+1],nx[i+1]);
if nx[i+1]<epsilon then break; fi;
od:
The Determinant of F is computed below, to decide whether F can be inverted, to compute its condition
number based on the Frobenius norm. This step is not essential in the Newton-Raphson procedure.
> det(FF[i-1]);
−664.9810536
The Frobenius Norm of F
> F_f:=sqrt(trace(evalm(FF[i-1] &*
> transpose(FF[i-1])))/2):
> FI_f:=sqrt(trace(evalm(inverse(FF[i-1]) &*
> transpose(inverse(FF[i-1]))))/2):
> k_f:=F_f*FI_f; i;
k f := 18.35941761
3
Procedure converged in 3 iterations
2. Consider eq1 and eq3
> F:=Matrix(2,2,
> [[diff(eq1,theta[4]),diff(eq1,theta[5])],
> [diff(eq3,theta[4]),diff(eq3,theta[5])]]):
>
>
>
>
>
>
>
>
>
>
>
>
f:=Vector(2,[eq1,eq3]):
for i from 0 to N do
FF[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(F)));
ff[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(f)));
tt:=linsolve(FF[i],-ff[i]);
x[i+1]:=evalm(x[i]+tt);
nx[i+1]:=norm(tt,infinity);
#print(i,tt,x[i+1],nx[i+1]);
if nx[i+1]<epsilon then break; fi;
od:
The Determinant of F
184
>
det(FF[i-1]);
−23.7302667
The Frobenius Norm of F
> F_f:=sqrt(trace(evalm(FF[i-1] &*
> transpose(FF[i-1])))/2):
> FI_f:=sqrt(trace(evalm(inverse(FF[i-1]) &*
> transpose(inverse(FF[i-1]))))/2):
> k_f:=F_f*FI_f; i;
k f := 545.4068014
6
Procedure converged in 6 iterations
3. Consider eq1 and eq4
> F:=Matrix(2,2,[
> [diff(eq1,theta[4]),diff(eq1,theta[5])],
> [diff(eq4,theta[4]),diff(eq4,theta[5])]]):
>
>
>
>
>
>
>
>
>
>
>
>
f:=Vector(2,[eq1,eq4]):
for i from 0 to N do
FF[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(F)));
ff[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(f)));
tt:=linsolve(FF[i],-ff[i]);
x[i+1]:=evalm(x[i]+tt);
nx[i+1]:=norm(tt,infinity);
# print(i,tt,x[i+1],nx[i+1]);
if nx[i+1]<epsilon then break; fi;
od:
The Determinant of F
> det(FF[i-1]);
197.5540528
The Frobenius Norm of F
> F_f:=sqrt(trace(evalm(FF[i-1] &*
> transpose(FF[i-1])))/2):
> FI_f:=sqrt(trace(evalm(inverse(FF[i-1]) &*
> transpose(inverse(FF[i-1]))))/2):
> k_f:=F_f*FI_f; i;
k f := 58.54693579
3
Procedure converged in 3 iterations
4. Consider eq2 and eq3
185
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
F:=Matrix(2,2,[
[diff(eq2,theta[4]),diff(eq2,theta[5])],
[diff(eq3,theta[4]),diff(eq3,theta[5])]]):
f:=Vector(2,[eq2,eq3]):
for i from 0 to N do
FF[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(F)));
ff[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(f)));
tt:=linsolve(FF[i],-ff[i]);
x[i+1]:=evalm(x[i]+tt);
nx[i+1]:=norm(tt, infinity);
# print(i,tt,x[i+1],nx[i+1]);
if nx[i+1]<epsilon then break; fi;
od:
The Determinant of F
> det(FF[i-1]);
−251.8689818
The Frobenius Norm of F
> F_f:=sqrt(trace(evalm(FF[i-1] &*
> transpose(FF[i-1])))/2):
> FI_f:=sqrt(trace(evalm(inverse(FF[i-1]) &*
> transpose(inverse(FF[i-1]))))/2):
> k_f:=F_f*FI_f; i;
k f := 10.60876987
2
Procedure converged in 2 iterations
5. Consider eq2 and eq4
> F:=Matrix(2,2,[
> [diff(eq2,theta[4]),diff(eq2,theta[5])],
> [diff(eq4,theta[4]),diff(eq4,theta[5])]]):
> f:=Vector(2,[eq2,eq4]):
> for i from 0 to N do
>
FF[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(F)));
>
ff[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(f)));
>
tt:=linsolve(FF[i],-ff[i]);
>
x[i+1]:=evalm(x[i]+tt);
>
nx[i+1]:=norm(tt,infinity);
> # print(i,tt,x[i+1],nx[i+1]);
>
if nx[i+1]<epsilon then break; fi;
> od:
The Determinant of F
> det(FF[i-1]);
−170.4330534
The Frobenius Norm of F
> F_f:=sqrt(trace(evalm(FF[i-1] &*
> transpose(FF[i-1])))/2):
> FI_f:=sqrt(trace(evalm(inverse(FF[i-1]) &*
> transpose(inverse(FF[i-1]))))/2):
> k_f:=F_f*FI_f; i;
k f := 7.582670607
2
Procedure converged in 2 iterations
6. Consider eq3 and eq4
186
>
>
>
>
>
>
>
>
>
>
>
>
>
>
>
F:=Matrix(2,2,[
[diff(eq3,theta[4]),diff(eq3,theta[5])],
[diff(eq4,theta[4]),diff(eq4,theta[5])]]):
f:=Vector(2,[eq3,eq4]):
for i from 0 to N do
FF[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(F)));
ff[i]:=evalf(subs(theta[4]=x[i][1],theta[5]=x[i][2],evalm(f)));
tt:=linsolve(FF[i],-ff[i]);
x[i+1]:=evalm(x[i]+tt);
nx[i+1]:=norm(tt, infinity);
# print(i,tt,x[i+1],nx[i+1]);
if nx[i+1]<epsilon then break; fi;
od:
The Determinant of F
> det(FF[i-1]);
−80.85829260
The Frobenius Norm of F
> F_f:=sqrt(trace(evalm(FF[i-1] &*
> transpose(FF[i-1])))/2):
> FI_f:=sqrt(trace(evalm(inverse(FF[i-1]) &*
> transpose(inverse(FF[i-1]))))/2):
> k_f:=F_f*FI_f; i;
k f := 25.03698706
3
Procedure converged in 3 iterations
187
Appendix 4
>
restart:
with(LinearAlgebra):
Matrix Θ
Θ=
α cos ψ + 1/2 sin ψ
−α cos ψ + 1/2 sin ψ
ρ(−α sin ψ + 1/2 cos ψ − δ) ρ(α sin ψ + 1/2 cos ψ + δ)
Compute product L= ΘˆT*Θ, simplify it by hand and check the simplifications
>
>
>
>
alpha1:=(1-rho^2)*((alpha^2-1/4)*cos(psi)+alpha*sin(psi))*cos(psi)
-delta*rho^2*(cos(psi)-2*alpha*sin(psi))+rho^2*(alpha^2+delta^2)+1/4;
factor(alpha1-L[1,1]);
α1 := 1 − ρ2
α2 − 1/4 cos (ψ) + α sin (ψ) cos (ψ) − δ ρ2 (cos (ψ) − 2 α sin (ψ)) + ρ2 α2 + δ 2 + 1/4
>
>
>
>
0
alpha2:=(1-rho^2)*((alpha^2-1/4)*cos(psi)-alpha*sin(psi))*cos(psi)
+delta*rho^2*(cos(psi)+2*alpha*sin(psi))+rho^2*(alpha^2+delta^2)+1/4;
factor(alpha2-L[2,2]);
α2 := 1 − ρ2
α2 − 1/4 cos (ψ) − α sin (ψ) cos (ψ) + δ ρ2 (cos (ψ) + 2 α sin (ψ)) + ρ2 α2 + δ 2 + 1/4
>
>
>
>
0
alpha3:=(alpha^2+1/4)*(rho^2-1)*cos(psi)^2-2*alpha*delta*rho^2*sin(psi)
-rho^2*(alpha^2+delta^2)+1/4;
factor(alpha3-L[1,2]); factor(alpha3-L[2,1]);
α3 := α2 + 1/4 ρ2 − 1 (cos (ψ))2 − 2 ρ2 α sin (ψ) δ − ρ2 α2 + δ 2 + 1/4
0
0
Computing the product R=ΘΘˆT
> R:=map(simplify,Multiply(Theta,Transpose(Theta)),trig);
We simplify R by hand and check the simplifications:
> beta1:=(2*alpha^2-1/2)*cos(psi)^2+1/2; factor(beta1-R[1,1]);
2
β1 := 2 α2 − 1/2 (cos (ψ)) + 1/2
>
>
>
>
0
beta2:=rho^2*(2*(alpha*sin(psi)+delta)^2+(1/2)*cos(psi)^2);
factor(simplify(beta2-R[2,2],trig));
2
2
β2 := ρ2 2 (α sin (ψ) + δ) + 1/2 (cos (ψ))
0
beta3:=-(1/2)*rho*(4*alpha^2*sin(psi)+4*alpha*delta-sin(psi))*cos(psi);
factor(beta3-R[1,2]); factor(beta3-R[2,1]);
β3 := −1/2 ρ cos (ψ) 4 α2 sin (ψ) + 4 α δ − sin (ψ)
0
0
Apparently, matrix R is simpler than matrix L. Hence, we work with matrix R.
g1 := β1 − β2 = 2 α2 + 2 ρ2 α2 − 1/2 − 1/2 ρ2 cos2 ψ − 4 ρ2 α δ sin ψ + 1/2 − 2 ρ2 α2 − 2 ρ2 δ 2
= (1 + ρ2 )(2a2 − 1) cos2 ψ − 4aδρ2 sin ψ − 2(a2 + δ 2 )ρ2 + 1/2
188
Case (i): psi=Pi/2
> with(student):
> gi[1]:=completesquare(collect(eval(simplify(subs(psi=Pi/2,g1))),rho),alpha);
2
>
>
gi 1 := −2 ρ2 (α + δ) + 1/2
Ri:=map(eval,Matrix([[subs(psi=Pi/2,R[1,1]),subs(psi=Pi/2,R[1,2])],[subs(psi=Pi/2,
R[2,1]),subs(psi=Pi/2,R[2,2])]]));
"
#
1/2
0
Ri :=
0 4 ρ2 α δ + 2 ρ2 δ 2 + 2 ρ2 α2
In this case matrix R can be rendered proportional to the 2x2 identity matrix if 2ρˆ2(α + δ)ˆ2 = 1/2,
which yields ρ = 1/[2(α + δ)]:
> R_i:=subs(rho=1/2/(alpha+delta),Ri);


"
#
1/2
0
1/2 0
=
R i := 
αδ
δ2
α2
0
0
1/2
2 + 1/2
2 + 1/2
2
(α+δ)
(α+δ)
(α+δ)
√
thereby deriving an isotropic Θ with two identical singular values of 2/2, namely,
> Theta_i:=Matrix(map(factor,subs(rho=1/2/(alpha+delta),psi=Pi/2,Theta))):
> Theta_iso:=Matrix(map(factor,Theta_i));
"
#
1/2 1/2
Θ iso :=
−1/2 1/2
Hence, the conditions in case (i) are: ψ = π/2 and ρ = 1/[2(α + δ)]
Case (ii): ψ 6= π/2 but can be calculated from eq.(4b)
> sin(psi):=4*alpha*delta/(1-4*alpha^2); #eq.(4b)
> cos(psi)^2:=1-sin(psi)^2;
αδ
sin ψ := 4 1−4
α2
2 2
α δ
cos2 ψ := 1 − 16 (1−4
α2 )2
Hence: 4αδ < 1 − 4α2 or 0 < δ < (1 − 4α2 )/(4α). Moreover, for δ to be positive, α < 1/2
> Rii:=map(simplify,subs(sin(psi)=sin_psi,cos(psi)^2=cos_sqr_psi,R));


α2 (4 α2 −1−4 δ 2 )
0
−1+4 α2
 2

Rii := 

2
ρ (4 α2 −1−4 δ 2 )
0
1/2
−1+4 α2
To render this matrix proportional to the 2 x 2 identity matrix, the condition ρ = 2α should be imposed,
which yields
> Rii:=map(simplify,subs(rho=2*alpha,Rii));


α2 (4 α2 −1−4 δ 2 )
2
0
2
−1+4 α


Rii := 

α2 (4 α2 −1−4 δ 2 )
0
2
2
−1+4 α
p
thereby deriving an isotropic Θ with two identical singular values of α 2(4α2 − 4δ 2 − 1)/(4α2 − 1)
189
>
>
>
>
Example
alpha:=1/3; rho:=2*alpha; delta:=(1-4*alpha^2)/(8*alpha);
psi:=arcsin(4*alpha*delta/(1-4*alpha^2)); #in rad
R_id=R; Thet_iso:=Theta;
α := 1/3
ρ := 2/3
δ :=
Thet iso :=
>
"
5
24
ψ := 1/6 π
√
√
#
1/6 3 + 1/4 −1/6 3 + 1/4
√
√
−1/4 + 1/6 3 1/6 3 + 1/4
190
http://www.springer.com/978-3-319-01850-8