Embed Size (px)
Citation preview
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 UniversityMontreal, Quebec, Canada
November 2014c© Jorge Angeles
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 (Dudita and Diaconescu, 1987):
Different definitions of machine have been given by scholars for more than two millennia, starting withVitruvius 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 exe-cution part (Euler, 1753; Bogolyubov, 1976)
• A machine produces mechanical work, or performs productive operations, actions, or effects (Vit-ruvius, 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; Bo-golyubov, 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, Beitrage zur Geschichte des Maschinenbaues, J. Springer, Berlin.
Bogolyubov, A. N., 1976, Teoriya mekhanismov v istoricheskom razvitii (Theory of Mechanisms andits Historical Development), Nauka, Moscow (in Russian).
Borgnis, G. A., 1818, Traite Complet de Mecanique Appliquee aux Arts. Traite des Compositions desMachines, Paris.
Dudita, Fl. and Diaconescu, D., 1987, Optimizarea Structurala a Mecanismelor (Optimization of Me-chanisms ), in Romanian, Ed. Tehnica (Publishers), Bucharest.
Hachette, 1811, Traite Elementaire des Machines, Paris.
Koenigs, F., 1901, “Etude critique sur la theorie generale des mecanismes,” Comptes Rendus del’Academie des Sciences, Vol. 133.
Leupold, J., 1724, Theatrum Machinarium Generale, Leipzig.
Poncelet, J. V., 1824, Traite de Mecanique Appliquee aux Machines, Liege.
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 prede-termined manner
– an instrument (as a lever) designed to transmit or modify the application of power, force, ormotion
1
– a mechanically, electrically, or electronically operated device for performing a task (a calcu-lating 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 simplifyphysical or intellectual work (Frolov, 1987). Comment: a comprehensive definition, that includescomputers
• Mechanical system that performs a specific task, such as the forming of material, and the trans-ference and transformation of motion and force, Vol. 38, Nos. 7–10 (2003) of Mechanism andMachine 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 ofsome 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 in-formation 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: toovague
• Definitions in Vol. 38, Nos. 7–10 (2003) of Mechanism and Machine Theory on Standardizationof Terminology.
– System of bodies designed to convert motions of, and forces on, one or several bodies intoconstrained motions of, and forces on, other bodies. Comment: English could be terser, butidea 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 HouseCollege Dictionary)
• A combination layout of pieces or elements, assembled with the goal of (producing) an operationas 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 alink or links fixed and by means of which straight or nearly straight lines or other point pathsmay 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) ofMechanism and Machine Theory on Standardization of Terminology). Comment: confuses linkagewith 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 isvery small, it is more suitable to perform a large number of multiplications, say 107, which will requirea 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 amountsto 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 UQAMMcGill and Eastern Quebec for High Performance Computing) supercomputer, AMD Athlon1900+ 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 amountsto 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 verysmall, it is more suitable to perform a bunch of additions, say 107, which should amount to a totaltime 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 to1.25× 10−8 s/add, or 8× 107 add/s.
• The C program below was run for the same purpose on the CLUMEQ supercomputer, AMDAthlon 1900+ cluster:
#include "stdio.h"
#include "math.h"
1See 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 to4× 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 (wTv = vTw = 0). Since we can find two such vectors, let us call themw1 & w2 and, moreover, let us define w1 ⊥ w2, i.e., w
T1 w2 = 0.
We haveT = 1+ uvT
Multiplying both sides by wi, we obtain
Twi = wi + uvTwi︸ ︷︷ ︸
0
= wi, i = 1, 2
Therefore, w1 & w2 are eigenvectors of T, of eigenvalues λ1 = λ2 = 1.
Moreover, let
w3 ≡u
‖u‖Then,
Tw3 = w3 + uvTw3 =u
‖u‖ + uvT u
‖u‖ =u
‖u‖(1 + vTu) ⇒ Tw3 = (1 + vTu)w3
and w3 is the third eigenvector of T, of eigenvalue λ3 = 1 + vTu.
6
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 + vTu = 1 + u · v
2.8 From Q = R1R2, we have
QQT = R1R2(R1R2)T = R1 R2R
T2
︸ ︷︷ ︸
1
RT1 = R1R
T1 = 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− 2ffT ) = 1− 2eeT − 2ffT + 4(eT f)efT
Hence,
vect(Q) = −1
24(eT f)e× f = u sinφ
andtr(Q) = 3− 2− 2 + 4(eT f)2 = 4(eT f)2 − 1 = 1 + 2 cosφ
Therefore,sinφ = ‖vect(Q)‖ = 2|eT f | ‖e× f‖, 0 ≤ φ < π
whence
u = − e× f
‖e× f‖ sign(eT f)
where sign(eT f) is the signum function, which is +1 if eT f > 0, −1 if eT f < 0, and undefined ifeT f = 0. Hence, u is undefined if eT f = 0, which indicates that Q = 1− 2eeT − 2ffT , in which caseQ 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 | ‖e× f‖2(eT f)2 − 1
]
2.9 The product Q = R1R2 is readily computed as
Q = (1− 2eeT )(1− 2ffT ) = 1− 2eeT − 2ffT + 4(eT f)efT
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, thencosφ = 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(efT ) = −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 , whichis the sole condition for Q to be factorable as R1R2.
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)−1b ≡ 1
1 + ‖a‖2 [(1 + ‖a‖2)1−A+A2]b
which, upon expansion, yields
v = b+1
1 + ‖a‖2 (−Ab+A2b)
and, finally,
v =−a× b+ b+ (aTb)a
1 + ‖a‖2
(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.,
− (a× b)Ta
︸ ︷︷ ︸
0
+bTa+ aTb‖a‖2 = 0
which thus reduces to(aTb)(1 + ‖a‖2) = 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
− (a× b)Tb
︸ ︷︷ ︸
0
+‖b‖2 + (aTb)2= 0
which holds if and only if the sum of two positive quantities vanishes. Obviously, this sum cannotvanish, 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)
Multiplying eq.(1) by E, we obtainE2u = λ2u (2)
and multiplying a second time by E,E3u = λ3u (3)
However, from eq.(2.51)E3 = −E (4)
Substituting eq.(4) into eq.(3), we obtain
−Eu = λ3u (5)
Moreover, from eq.(1) we have Eu = λu, and thus eq.(5) reduces to
λ(1 + λ2)u = 0 (6)
8
However, ‖u‖ = 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 threeroots 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 andmutually orthogonal, assume that v and w are two 3-dimensional complex vectors. Moreover, theformer 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∗ = vTr − jvT
c (10)
Moreover, the transpose conjugate p of p, being a scalar, is simply its complex conjugate, which equalsthe 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∗(ETv) ≡ 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 nowto the real and imaginary parts of w, namely, wr and wc, respectively. Substitution of v in terms ofits 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
E2vr = −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 aboveequations leads to
e (eTvr)︸ ︷︷ ︸
0
= 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 eare candidates for the real and complex components of v. A similar reasoning leads to the conclusionthat any two orthonormal vectors lying in the plane normal to e are candidates for the two componentsof w. However, v and w are known to be mutually orthogonal. Upon imposing the condition v∗w = 0,the relation below follows:
vTr wr + vT
c wc + j(vTr wc − vT
c wr) = 0
which readily leads to two real equations, one for the real part, one for its complex counterpart:
vTr wr + vT
c wc = 0, vTr wc − vT
c 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 imaginaryparts of the eigenvector associated with λ2 = j. The same vectors, but with their real and imaginaryroles exchanged, constitute the real and imaginary parts of the eigenvector associated with λ3 = −j.
2.12 From eq.(2.53), we haveQ = 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φ, µ3 = e−jφ
10
2.13 Here we havetr(Q) = −1 = 1 + 2 cosφ
from which,cosφ = −1, i.e., φ = π
Using eq.(2.49), we haveQ+ 1 = 2eeT
which gives
1
3
2 −2 2−2 2 −22 −2 2
= 2
e21 e1e2 e1e3e1e2 e22 e2e3e1e3 e2e3 e23
Thus,
e1 = ±√3
3, e2 = ∓
√3
3, e3 = ±
√3
3
Choosing e1 = +√3/3, we finally obtain
e =
√3
3
1−11
, φ = π
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 Euclideannorm of s, then S = σE, in which E is the cross-product matrix of the unit vector e. Hence, theeigenvalues of S are, from Exercise 2.10, 0, σj, and −σj. Consequently, the eigenvalues of 1+ S are1, 1+σj, and 1−σj, none of which is zero, thereby showing that 1+S is indeed invertible. Multiplyingboth 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 iffthe 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 factoringis impossible in this case.
2.17 From Exercise 2.15, we haveS = (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)
⇒ Eigenvalues of 1+Q: 2, 1 + ejφ, 1 + e−jφ
⇒ Eigenvalues of (1+Q)−1:
1
2,
1
1 + ejφ,
1
1 + e−jφ
Eigenvalues of E: 0, j, −j (see Exercise 2.10)
Eigenvalues of E2: 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)
α+ jβ − γ =1
1 + ejφ(23b)
α− jβ − γ =1
1 + e−jφ(23c)
Substituting α = 1/2 from eq.(23a) into eqs.(23b &c), we obtain, after some simplifications:
β =e−jφ − ejφ
2j(2 + ejφ + e−jφ)=
−2j sinφ2j(2 + 2 cosφ)
= − sinφ
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 (25a)
E4 = (1− eeT ) = −E2 (25b)
12
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
21− sinφ
2(1 + cosφ)E
and
S = (1+Q)−1(1−Q) =
(1
21− sinφ
2(1 + cosφ)E
)
(1−Q)
=1
2
[
1−Q− sinφ
1 + cosφE+
sinφ
1 + cosφEQ
]
Using the representation of Q given in eq.(19), along with relations (25a), we obtain, after somesimplifications,
S =− sinφ
1 + cosφE
But, if we recall the tan-half identities:
cosφ =1− τ21 + τ2
, sinφ =2τ
1 + τ2, τ ≡ tan
(φ
2
)
then, the above expression is equivalent to
S = − tan
(φ
2
)
E
Finally, let us take the vector of the two sides of the above equation
vect(S) = − tan
(φ
2
)
e
which is, using the definition for the Rodriges vector ρ in this Exercise, ρ = tan(φ/2)e, the negativeof 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 aredefined in Fig. 1, one has
i′ = −k, j′ = j, k′ = i
From Definition 2.2.1, then,
Q =
0 0 10 1 0−1 0 0
Hence,tr(Q) = 1 = 1 + 2 cosφ
and thus,
cosφ = 0, φ = ±π2
On the other hand,vect(Q) = [ 0 1 0 ]T = sinφe
If sinφ > 0, then
e = [ 0 1 0 ]T, φ =
π
2The angle between the axis of rotation and AB is thus 0, whereas, the angle between the axis ofrotation and AD is 90. Moreover, the angle between the axis of rotation and AE is 90.
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; inparticular, the first and the third observe form (2.55c), while the second (2.55b), i.e.,
QZ0=
cosφ − sinφ 0sinφ cosφ 00 0 1
, QY1=
cos θ 0 sin θ0 1 0
− sin θ 0 cos θ
, QZ2=
cosψ − sinψ 0sinψ cosψ 00 0 1
Doing the required matrix products, we obtain
Q =
cθcφcψ − sφsψ −cθcφsψ − sφcψ sθcφcθsφcψ + cφsψ −cθsφsψ + cφcψ sθsφ−sθcψ sθsψ cθ
14
X0Y0Z0
QZ0(φ)→ X1Y1Z1
QY1(θ)→ X2Y2Z2
QZ2(ψ)→ X3Y3Z3
Figure 2:
Moreover, from eq.(2.67), we havetr(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 + cosx = 2 cos2(x
2
)
Therefore,
4 cos2(α
2
)
= 4 cos2(θ
2
)
cos2(φ+ ψ
2
)
Thus,
cos(α
2
)
= cos
(φ+ ψ
2
)
cos
(θ
2
)
2.22 We have
Q =
q11 1/2 −2/3q21 q22 3/4q31 q32 q33
If Q represents a rotation matrix, we should have QTQ = 1, and det(Q) should be +1. The foregoingproduct is computed first:
QTQ =
q11 q21 q311/2 q22 q32−2/3 3/4 q33
q11 1/2 −2/3q21 q22 3/4q31 q32 q33
QTQ =
q211 + q221 + q23112q11 + q21q22 + q31q32 − 2
3q11 +34q21 + q31q33
12q11 + q21q22 + q31q32
14 + q222 + q232 − 1
3 + 34q22 + q32q33
− 23q11 +
34q21 + q31q33 − 1
3 + 34q22 + q32q33
49 + 9
16 + q233
15
For the (3, 3) entry of QTQ, we must have
4
9+
9
16+ q233 = 1 or q233 = − 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 notobey a similarity transformation under a change of coordinate frame consists of showing that its traceis not invariant. Indeed, define the coordinate frame A with the origin at point P1, the x-axis directedfrom P1 to P2, the z-axis being normal to the plane of the three points. The vectors pi31 have theA-coordinates displayed below:
[p1]A =
000
, [p2]A =
x200
, [p3]A =
x3y30
,
matrix P thus taking the form
[P]A =
0 x2 x30 0 y30 0 0
.
Hence, tr[P]A = 0. Now, we transform frame A into frame B by rotating it around the z-axis counter-clockwise through an angle θ. The transformation matrix for the said rotation has the form
[Q] =
cos θ − sin θ 0sin θ cos θ 00 0 1
where θ is the angle of rotation. We thus have
[p1]B = [p1]A = 0 ,
[p2]B = [Q]T[p2]A =
cos θ sin θ 0− sin θ cos θ 0
0 0 1
x200
=
x2 cos θ−x2 sin θ
0
,
[p3]B = [Q]T [p3]A =
cos θ sin θ 0− sin θ cos θ 0
0 0 1
x3y30
=
x3 cos θ + y3 sin θ−x3 sin θ + y3 cos θ
0
and
P =
0 x2 cos θ x3 cos θ + y3 sin θ0 −x2 sin θ −x3 sin θ + y3 cos θ0 0 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 linearlydependent, and we can write:
p2 = α2p1, p3 = α3p1.
Therefore, matrix P is of rank one, i.e., it contains only one linearly independent row (or column, forthat 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,
the eigenvalues of P2 beingλ21 > 0, λ22 = λ23 = 0.
Then, we haveq = 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 holdsfor 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 = p1pT1 + p2p
T2 + p3p
T3
Under a rotation Q of the coordinate frame the matrices pipTi for i = 1, 2, 3 change as
[pipTi ]A = [Q]A[pi]B[pi]
TB [Q]TA = [Q]A[pip
Ti ]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 ]BQT ,
PPT thus being frame-invariant.
The singularity of this matrix appears if the origin of the coordinate frame lies in the plane of the threepoints. In this case, if we denote by n the unit normal of this plane, we have
PPTn = p1pT1 n+ p2p
T2 n+ p3p
T3 n = 0
Since n is obviously not zero, matrix PPT is singular, even if the three points are not collinear, andof rank 2.
Let us now assume that three points P1, P2, P3 are collinear and the origin of the frame lies in theline of these points. Then, the position vectors of these points are all linearly dependent, and we canwrite:
p2 = α2p1, p3 = α3p1.
Now, if we let q1 and q2 be any two mutually orthogonal unit vectors lying in a plane normal to p1,we have
PPTqi =[p1 α2p1 α3p1
]
pT1
α2pT1
α3pT1
qi =[p1 α2p1 α3p1
]
000
= 0 , for i = 1, 2 .
Thus, the nullspace of PPT is of dimension 2, and hence, PPT is of rank 1—see Exercise 2.2— therebyproving sufficiency.
17
To prove necessity, let us assume that matrix PPT is singular of rank one. Then, it has two zeroeigenvalues, i.e., λ1 = λ2 = 0, and λ3 6= 0. Let ei31 be the associated mutually orthogonal uniteigenvectors. Then,
(p1pT1 )ei = 0 , for i = 1, 2 ,
i.e.,p1 p
T1 ei︸ ︷︷ ︸
αi
+p2 pT2 ei︸ ︷︷ ︸
βi
+p3 pT3 ei︸ ︷︷ ︸
γi
= 0 , for i = 1, 2 .
Hence, we have
α1p1 + β1p2 + γ1p3 = 0 , (26a)
α2p1 + β2p2 + γ2p3 = 0 . (26b)
Assume now that vectors p1, p2 and p3 are linearly independent. This means that for eq.(26a) to besatisfied all the coefficients α1, β1, and γ1 must be equal to zero. This leads to the following relations:
pT1 e1 = 0 , pT
2 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 saidvectors lie in a plane perpendicular to vector e2. Therefore, vectors p1, p2 and p3 lie in the line ofintersection of these planes, which passes through the origin, thereby proving necessity.
2.26 We have
Q =
−0.5 q12 q13q21 0.25 q23q31 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 haveQ+ 1 = 2eeT
which gives
0.5 q12 q13q12 1.25 q23q13 q23 0.25
= 2
e21 e1e2 e1e3e1e2 e22 e2e3e1e3 e2e3 e23
This implies thate1 = ±0.5, e2 = ±0.7906, e3 = ±0.3535
Therefore,
q12 = 2e1e2 = ±0.7906, q13 = 2e1e3 = ±0.3535, q23 = 2e2e3 = ±0.5589There are thus four possible solutions for matrix Q, namely,
Q1 =
−0.5 0.7906 0.35350.7906 0.25 0.55890.3535 0.5589 −0.75
, Q2 =
−0.5 −0.7906 −0.3535−0.7906 0.25 0.5589−0.3535 0.5589 −0.75
Q3 =
−0.5 −0.7906 0.3535−0.7906 0.25 −0.55890.3535 −0.5589 −0.75
, Q4 =
−0.5 0.7906 −0.35350.7906 0.25 −0.5589−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 holdsfor f1, f2 and f3. Therefore,
f1 = f2 × f3 = e3Moreover, by inspection,
f2 =
√2
2(−e1 + e2), f3 =
√2
2(−e1 − e2)
Using Definition 2.2.1, we obtain
[Q ]F =
0 −√2/2 −
√2/2
0√2/2 −
√2/2
1 0 0
Thus,
vect[Q ]F = e sinφ =1
2
√2/2
−√2/2− 1√2/2
=
√2
4v
where v is given as
[v ]F ≡
1−1−
√2
1
The norm ‖v‖ of v is thus, ‖v‖ =√
5 + 2√2 . Therefore, the unit vector e parallel to the axis of
rotation is given by
[ e ]F =1
√
5 + 2√2
1−1−
√2
1
Moreover, sinφ (≥ 0) is given by
sinφ =
√2
4
√
5 + 2√2 =
√
10 + 4√2
4= 0.9892
which leads toφ = 81.58 or φ = 98.42
19
On the other hand,
cosφ =1
2(tr(Q)− 1) =
1
2
(√2
2− 1
)
= −0.1464
Since cosφ < 0, the angle sought isφ = 98.42
2.29 (a) From the problem statement and Fig. 2.10, we have
[−−→OP ]1 =
1−11
. [−−→OC]1 =
001
, [Q]1 =1
3
1 1−√3 1 +
√3
1 +√3 1 1−
√3
1−√3 1 +
√3 1
We can write[−−→OC]1 = [
−−→OP ]1 + [Q]1[
−−→PC]2
and thus,
[−−→PC]2 = [QT ]1
(
[−−→OC]1 − [
−−→OP ]1
)
=1
3
1 1 +√3 1−
√3
1−√3 1 1 +
√3
1 +√3 1−
√3 1
−110
=
√3
3
11−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 theposition vector of an arbitrary point D on plane ABC. The equation of plane ABC is
[n]T1
(
[−−→OC]1 − [d]1
)
= 0 or
√3
3[ 1 1 1 ]
T
−x1−y11− z1
= 0
which leads tox1 + y1 + z1 − 1 = 0 (27)
The position vector of an arbitrary point R takes the forms
[r]1 = [x1, y1, z1]T , [ρ]2 = [x2, y2, z2]
T
We have[r]1 = [Q]1[ρ]2 + [
−−→OP ]1
or
x1y1z1
=1
3
1 1−√3 1 +
√3
1 +√3 1 1−
√3
1−√3 1 +
√3 1
x2y2z2
+
1−11
and
x1y1z1
=1
3
x2 + (1 −√3)y2 + (1 +
√3)z2 + 3
(1 +√3)x2 + y2 + (1−
√3)z2 − 3
(1−√3)x2 + (1 +
√3)y2 + z2 + 3
(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 [−−→PC]2 into eq.(29), we
obtain √3
3+
√3
3− 2√3
3= 0
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 0-subscripted 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
[Q ]1 =
0 0 −10 −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
2(Q+ 1)
Let ei 31 denote the components of [ e ]1.
Hence,
e21 e1e2 e1e3e1e2 e22 e2e3e1e3 e2e3 e23
=
1/2 0 −1/20 0 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√
1/2. Let us, for example, choose e1 > 0. Then,
[ e ]1 =
√2
2[ 1 0 −1 ]T
2.31 Obviously, the trace is all we know of this matrix. Since the trace of the matrix representation of arotation 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 therange 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 transformationsmapping the cube into the second and third configurations, respectively. Now, clearly,
L′: i→ i′ = −k, j→ j′ = j, k→ k′ = −iL′′: 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 Z axes of an orthogonal coordinateframe. From Definition 2.2.1, then, in the given frame,
[L′ ] =
0 0 −10 1 0−1 0 0
, [L′′ ] =
0 0 11 0 00 1 0
Now it is a simple matter to show that the two foregoing matrices are orthogonal, but the first isimproper, whereas the second, proper. Thus, the second configuration is a reflection, the third beinga 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 theaforementioned coordinate frame be e1, e2 and e3. These are determined from the relation L′e = −e,which thus yields:
−e3 = −e1, e2 = −e2, −e1 = −e3and hence, one possible solution is
[ e ] =
√2
2[ 1 0 1 ]T
the other solution being the negative of the previous one. On the other hand, the linear invariants ofmatrix L′′ are readily computed as follows:
[ e ] sinφ =1
2
111
, cosφ = −1
2
According to our convention, we define sinφ as positive, which thus yields
sinφ = ‖e sinφ‖ =√3
2, [ e ] =
√3
3
111
and hence, φ = 120.
2.33 (a) Simple computations yield
[R1]GT[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 arerotations as well.
22
(b) Further computations lead to:
tr[R1]G = 2, vect[R1]G =
−0.50.5−0.5
, tr[R1]C = 2, vect[R1]C =
0−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 arethe same. In order to conclusively assess whether the two representations correspond to the samerotation, the sine of the angles of rotation has to be computed. Defining the sines as positive, thesecan be obtained as the magnitudes of the two foregoing vectors. The two rotation matrices beingrepresented in different coordinate frames, their vectors have different components. However, notethat
‖vect[R1]G‖ = ‖vect[R1]C‖ = 0.866
Hence, [R1]G and [R1]C represent the same rotation in different coordinate frames.
(c) Furthermore,
tr[R2]G = 1.692, vect[R2]G =
0.6970.558−0.289
, tr[R2]C = 1.692, vect[R2]C =
0.93800
Similarly,‖vect[R2]G‖ = ‖vect[R2]C‖ = 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 thedot product of the two axial vectors in each frame:
[vect[R1]G ]T vect[R2]G = 0.0748
while[vect[R1]C ]
Tvect[R2]C = 0
The foregoing result indicates that, although each pair of matrices represents the same rotationin different frames, the two pairs are not related by the same similarity transformation Q. Thatis, if the data are compatible, we should have
Q[Ri]CQT = [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. Thatis, the data are incompatible. Given the incompatibility of the data, no Q can be found thattransforms 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.,
xyz
=
1−√3√
31 +√3
+1
3
1 1−√3 1 +
√3
1 +√3 1 1−
√3
1−√3 1 +
√3 1
x′
y′
z′
=
1−√3 + x′/3 + (1−
√3)y′/3 + (1 +
√3)z′/3√
3 + (1 +√3)x′/3 + y′/3 + (1 −
√3)z′/3
1 +√3 + (1−
√3)x′/3 + (1 +
√3)y′/3 + z′/3
23
Hence, the equations in G coordinates are:
C : (1−√3 +
1
3x′ +
1−√3
3y′ +
1 +√3
3z′)2
+ (√3 +
1 +√3
3x′ +
1
3y′ +
1−√3
3z′)2 = 4
Π1 : 1 +√3 +
1−√3
3x′ +
1 +√3
3y′ +
1
3z′ = 0
Π2 : 1 +√3 +
1−√3
3x′ +
1 +√3
3y′ +
1
3z′ = 10
2.36 (a) Let us attach a frame X ′Y ′Z ′, labelled F ′, at the center of the ellipse, with the X ′ axis parallelto 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) andX ′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 unitvector normal to the PQR plane, k′, is readily obtained as
[k′ ]F =
√3
3[ 1 1 1 ]
T
Moreover, the X ′ axis is parallel to edge PQ, which gives
[ i′ ]F =
√2
2[−1 1 0 ]
T
Then, the last unit vector j′ is simply given by j′ = k′ × i′. Therefore,
[ j′ ]F =
√6
6[−1 −1 2 ]T
Thus, the rotation matrix Q1 transforming coordinates in frame F ′ into coordinates in F is foundusing Definition 2.2.1, namely,
[Q1 ]F =
√6
6
−√3 −1
√2√
3 −1√2
0 2√2
Now, we will express the unit vectors et, en, and eb, in frame-F coordinates. Here, we can readilywrite,
[ eb ]F = [k′ ]F =
√3
3[ 1 1 1 ]T
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 canthus write et = v/‖v‖. Let p be the position vector of this particle; then,
[p ]F ′ = [x′ y′ z′ ]T = [ a cosϕ b sinϕ 0 ]T
Hence,
[p ]F = [Q1 ]F [p ]F ′
=
√6
6
−√3 −1
√2√
3 −1√2
0 2√2
a cosϕ2a sinϕ/3
0
=
√6
18a
−3√3 cosϕ− 2 sinϕ
3√3 cosϕ− 2 sinϕ
4 sinϕ
24
Now, the velocity of the particle v is given as
[v ]F = [ p ]F
and thus,
[v ]F =
√6
18aϕ
3√3 sinϕ− 2 cosϕ
−3√3 sinϕ− 2 cosϕ4 cosϕ
The norm ‖v‖ of v is given by
‖v‖ =√6
18aϕ
√
(3√3sϕ− 2cϕ)2 + (−3
√3sϕ− 2cϕ)2 + 16c2ϕ
=
√6
18aϕ
√
54 sin2 ϕ+ 24 cos2 ϕ =
√6
18aϕ
√
24 + 30 sin2 ϕ =1
3aϕ
√
4 + 5 sin2 ϕ
Therefore,
[ et ]F =[v ]F‖v‖ =
√6
18aϕ
3
aϕ√
4 + 5 sin2 ϕ
3√3 sinϕ− 2 cosϕ
−3√3 sinϕ− 2 cosϕ4 cosϕ
=
√6
6√
4 + 5 sin2 ϕ
3√3 sinϕ− 2 cosϕ
−3√3 sinϕ− 2 cosϕ4 cosϕ
Now, vector en is simplyen = eb × et
or
[ en ]F =
√6
6√
4 + 5 sin2 ϕ
2√3 cosϕ+ 3 sinϕ
−2√3 cosϕ+ 3 sinϕ−6 sinϕ
The matrix that represents the rotation undergone by the gripper from an orientation in whichvectors et, en, and eb are parallel pairwise to the coordinate axes XY Z, in F coordinates, is thusobtained using Definition 2.2.1, namely,
[Q ]F =
√6
6√
4 + 5s2ϕ
3√3sϕ− 2cϕ 2
√3cϕ+ 3sϕ
√
8 + 10s2ϕ
−3√3sϕ− 2cϕ −2
√3cϕ+ 3sϕ
√
8 + 10s2ϕ
4cϕ −6sϕ√
8 + 10s2ϕ
(b) We know that the rotation matrix is symmetric when the angle of rotation is π. Therefore, wemust have
−3√3 sinϕ− 2 cosϕ = 2
√3 cosϕ+ 3 sinϕ (30)
4 cosϕ =
√
8 + 10 sin2 ϕ (31)
−6 sinϕ =
√
8 + 10 sin2 ϕ (32)
From eq.(30), we havesinϕ
cosϕ≡ tanϕ = −2
3
which leads toϕ = 146.31 or ϕ = 326.31
25
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. Asa verification, substituting ϕ = 326.31 in the rotation matrix, we obtain
[Qπ ]F =
−0.7887 0.2113 0.57740.2113 −0.7887 0.57740.5774 0.5774 0.5774
The trace of Qπ istr[Qπ ]F = −0.7887− 0.7887 + 0.5774 = −1
So,
cosφ =tr[Qπ ]F − 1
2= −1 or φ = π .
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 EHof the cube in Fig. 1a. These vectors are mapped into l, m and n, respectively, in the configurationshown in Fig. 1b. It is apparent that
l = k
m = −√2
2i+
√2
2j
n = −√2
2i−√2
2j
and hence, using Definition 2.2.1,
Q =
0 −√2/2 −
√2/2
0√2/2 −
√2/2
1 0 0
Upon equating the foregoing expression with that displayed in Exercise 2.18, we end up with a matrixequation 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 theunknowns. Obviously, this is not a practical approach to solving the problem. It may be wiser to firstinspect the entries of the two matrices and pick up obvious entries that would yield simple relationsfor 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 double-valuedness of the inverse-trigonometric functions. A more rational approach is described below, usingthe linear invariants of the two foregoing matrices. Indeed, upon equating the vector and the trace ofthese matrices, we obtain a system of four scalar equations, namely,2
sθ(sψ − sφ)sθ(cψ + cφ)
cθ(sφcψ + cφsψ) + sφcψ + cφsψcθ(cφcψ − sφsψ)− sφsψ + cφcψ + cθ
=
√2/2
−√2/2− 1√2/2√2/2
which is algebraically rather simple. Upon simplification, the system reduces to
sθ(sψ − sφ) =√2/2 (33a)
sθ(cψ + cφ) = −(1 +√2/2) (33b)
(1 + cθ)s(ψ + φ) =√2/2 (33c)
(1 + cθ)c(ψ + φ) + cθ =√2/2 (33d)
2Angle φ 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
sσ(1 + cθ) =√2/2 (35a)
cσ(1 + cθ) =√2/2− cθ (35b)
We now solve these equations for cθ:
(35a)2 + (35b)2 ⇒ (1 + cθ)2 = 1−√2cθ + cθ2 ⇒ cθ = 0
thereby ending up with
θ = ±π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
cosσ =
√2
2
which leads toσ = ±π
4⇒ φ+ ψ = ±π
4
Substituting the values of θ and σ found above into eqs.(33a & b) and solving them for ψ and φ, weobtain:
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 unfeasiblesince the substitution of these corresponding values of θ and σ ≡ ψ + φ makes the left-hand sides ofeqs.(33c & d) different from the corresponding right-hand sides.
In summary, then, we have two feasible solutions, namely,
θ =π
2, σ ≡ ψ + φ =
π
4: ψ = π, φ = −3
4π,
θ = −π2, σ ≡ ψ + φ =
π
4: ψ = 0, φ =
π
4
27
2.38 We need first the rotation matrix carrying the triangle from A1, B1, C1 to A0, B0, C0. This matrixcan 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 theattitude A0, B0, C0 from A1, B1, C1 upon bringing edge A1B1 to coincide with edge A0B0, by makingA1 coincide with A0 and B1 with B0, without changing the attitude of A1, B1, C1. In this way, C1 lieson the Y0 axis. Now, we can achieve the desired attitude by rotating triangle A1, B1, C1 about edgeA1B1. By symmetry, line C0C1 intersects line A1B1, as the reader can readily verify upon computingthe distance between the two lines. In this way, then the angle of rotation involved is π, as found whensolving Exercise 2.29, while the axis of rotation is A1B1, the matrix sought then being
Q =
0 0 −10 −1 0−1 0 0
Upon equating this matrix with that displayed in terms of Euler angles in Exercise 2.18, we obtain thematrix equation
cθcφcψ − sφsψ −cθcφsψ − sφcψ sθcφcθsφcψ + cφsψ −cθsφsψ + cφcψ sθsφ−sθcψ sθsψ cθ
=
0 0 −10 −1 0−1 0 0
In view of the simple (symmetric) form of the right-hand side of the above equation, it will be quitesimple to compute the Euler angles by just equating corresponding entries on the two sides. Forstarters, 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 oppositesign of these. We thus have two possible solutions:
First solution : φ, θ, ψ = π, π/2, 0Second 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
Q =
0 0 −1−1 0 00 1 0
(b) From the foregoing representation of Q,
tr(Q) = 0 = 1 + 2 cosφ, i.e., cosφ = −1
2
Moreover,
vect(Q) =1
2[ 1 −1 −1 ]T = (sinφ)e, sinφ > 0,
28
Figure 6:
and hence,
sinφ =
√3
2⇒ e =
√3
3[ 1 −1 −1 ]T , φ = 120
The position vector a of point A, in its original configuration, and the position vector a′ of thesame point in the displaced configuration, denoted by A′, are given by
a = [ 0 0 0 ]T, a′ = a [ 2 1 −1 ]T
Hence, using eq.(3.18),
p0 =(Q− 1)
T(Qa− a′)
2(1− cosφ)=a
3[ 3 2 1 ]
T
Furthermore, the moment n of line L is defined as
n = p0 × e
Thus,
n =
√3
9a
−14−5
Therefore,
κL = [eT nT ]T =[ √
33 −
√33 −
√33 −
√3a9
4√3a9 − 5
√3a9
]T
(c) Since we know the position vector p0 of a point P0 of the screw axis, and the unit vector e parallelto this axis, L has the equations
x− a√3/3
=y − 2a/3
−√3/3
=z − a/3−√3/3
29
i.e.,
x− a = −y + 2
3a = −z + 1
3a
Hence, the intersection of the screw axis with the X-Y plane is (4a/3, a/3, 0), its intersection withthe 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
p1 =
000
, p2 =
1/2√3/20
, p3 =
−1/2√3/20
, p4 =
0√3/3√6/3
Moreover, if f denotes the resultant of the two given forces, then
f1 =
−100
, f2 =
0−√3/3
−√6/3
, f = f1 + f2 =
−1−√3/3
−√6/3
Hence,‖f‖2 = 2
Now, we can readily calculate the resultant moment of the two given forces with respect to P1, namely,
nP1= p2 × f1 + p4 × f2 =
[
0 0√32
]T
where, obviously, the moment of the second force with respect to P1 vanishes because the line of actionof this force passes through P1. From eq.(3.111), one has,
p′′0 =
1
‖f‖2f × (nP1
− f × p1) =1
‖f‖2f × nP1
=1
4[−1
√3 0 ]T
and the vector representing the wrench axis is determined as
e =f
‖f‖ =[
−√22 −
√66 −
√33
]T
The equations of the wrench axis are thus
x+ 1/4
−√2/2
=y −√3/4
−√6/6
=z
−√3/3
(36)
Now let p = [x, y, z]Tbe 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 =[
14
5√3
12
√66
]T
3.3 Three lines are given in terms of their Plucker arrays:
pLi=
[eini
]
, i = 1, 2, 3
The fourth line has the Plucker array
pL =
[en
]
30
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
A ≡
[(p1 − p)× e1]T
[(p2 − p)× e2]T
[(p3 − p)× e3]T
Since ‖e‖ = 1, i.e., e 6= 0, A must be singular, and hence,
det(A) = 0
orf(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 linearlyin each of the three factors involved, f(p) is apparently cubic in p. However, a simple expansion ofthe 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) = pTMp+ gTp+ f0
Now, the 3× 3 matrix M is obtained as
M =1
2
∂2f
∂p2
The above matrix is thus one-half the Hessian matrix of f with respect to p. A straightforwardcalculation of the Hessian leads to
M =1
2
(e1 × e2) [e3 × (p2 − p3)]
T+ [e3 × (p2 − p3)] (e1 × e2)
T
+(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 vectorp 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 showthat this position vector verifies the quadric equation, the conclusion being that line L1 lies in theabove 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 isthe one-sheet hyperboloid of elliptic cross section. This hyperboloid admits, as special cases, the one-sheet hyperboloid of revolution, the cone and the cylinder of elliptical cross sections. Further specialcases 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, wecan obtain the equation of the hyperboloid in its standard form
rT(
1
σ2M
)
r = 1, r = p+ p0 (41)
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)TM(r+ p0) + (2Mp0 + g)T + f0 + gTp0 + pT
0 Mp0 = 0
Thus, in order to obtain the standard form, we must get rid of the linear term in r in the aboveexpression, which can be done by setting
p0 = −1
2M−1g
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
f(r) = rTMr+ f0 −1
4gTMg = 0
which readily leads to the standard form of eq.(41) if we define σ2 as
σ2 =1
4gTMg− f0
Note that σ2 must be positive. Proving that this is the case should be possible with the expressionsfor 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 isthe ‘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 intersectedby line L, which is of the LH regulus.
Furthermore, the standard form can be reduced to canonical form by finding the eigenvalues and theeigenvectors 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 ]
3Sommerville, 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 , ρ =
xyz
, ∆ = diag(λ1, λ2, λ3)
the quadratic equation under study reduces to the canonical form4
x2
a2+y2
b2− z2
c2= 1 (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 positioningtasks of its end-effector, the revolute axes can be regarded as lines L1, L2, L3, that are contained ina one-sheet hyperboloid H. If the operation point of the EE lies in H, then the manipulator is in asingular configuration and the manipulator cannot transmit a velocity to the operation point in thedirection 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 tworepresentations of both n and f are the same; and ii) the angle between them is the same, whenthe foregoing items are computed in either representation. But this is so because the magnitudeof the moment is 5 and the magnitude of the force is 2 in both representations. Moreover, theinner product of the force and the moment in both representations is 0, and hence, the anglebetween the two vectors is 90, regardless of the representation adopted.
4Hurley, 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 Gram-Schmidt orthogonalization procedure, outlined in Appendix B, two orthonormal triads can beobtained. Let
[ e1 ]i ≡[n ]i‖[n ]i‖
, i = 1, 2
and[u′ ]i ≡ [ f ]i − [ fTe1 ]i[ e1 ]i, i = 1, 2
Defining
[ e2 ]i ≡[u′ ]i‖[u′ ]i‖
, i = 1, 2
and[ e3 ]i ≡ [ e1 × e2 ]i, i = 1, 2
we obtain
[ e1 ]1 =
001
, [ e2 ]1 =
010
, [ e3 ]1 =
−100
[ e1 ]2 =
−1/3−2/32/3
, [ e2 ]2 =
−2/32/31/3
, [ e3 ]2 =
−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
‖n‖ , e2 ≡f
‖f‖ , 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, thematrix [Q ]1 transforming frame-2 coordinates into frame-1 coordinates is related to the foregoingmatrices by
[E ]1 = [Q ]1[E ]2
from which one can readily solve for the desired matrix as follows:
[Q ]1 = [E ]1[E ]T2 =1
3
0 0 −10 1 01 0 0
−1 −2 2−2 2 1−2 −1 −2
=1
3
2 1 2−2 2 1−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 tothe screw axis. Then,
n = p× e
Hence,‖n‖ = ‖p‖‖e‖ sin θ = ‖p‖ sin θ
where θ is the angle between p and e. From Fig. 8, we can obtain
d = ‖p‖ sin θ = ‖n‖
34
Figure 8:
(b) Substitute p by p∗ in the expression for n given above, and introduce E = CPM(e), therebyobtaining 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.,
eTp∗ = 0 (44)
We thus have a situation similar to that encountered in eq.(3.61) when attempting an expressionfor p. As in the previous case, eq.(44) is adjoined to eq.(43), thereby obtaining a system of fourequations in three unknowns:
Ap∗ = b, A =
[EeT
]
, b =
[−n0
]
Now, proceeding exactly as when finding p from eq.(3.61), both sides of the foregoing equationare multiplied by AT from the left. The result is readily found to be
p∗ = (ATA)−1
ATb = [ET e ]
[−n0
]
= −ETn = e× n
q.e.d.
(c) We have
[ e ]G =
√2
2[−1 0 1 ]
T, [n ]G = [ 0 −
√2 0 ]
T
Hence,d = ‖n‖ =
√2
and[p∗ ]G = [ e× n ]G = [ 1 0 1 ]
T
35
3.6 (a) Let us attach the unit vectors i, j, and k to axes X , Y , and Z, respectively, of frame F . Once theworkpiece 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,
[ i′ ]F =
√3
3
111
, [ j′ ]F =
−−→C′C × i′
‖−−→C′C × i′‖=
√2
2
−101
,
[k′ ]F = i′ × j′ =
√6
6
1−21
Hence, applying Definition 2.2.1, we obtain
[Q ]F =
√3/3 −
√2/2
√6/6√
3/3 0 −√6/3√
3/3√2/2
√6/6
whence,
[ vect(Q) ]F =1
2
1.5236−0.16911.2844
= sinφ[ e ]F
Upon defining sinφ as positive, we have
sinφ = ‖vect(Q)‖ = 0.999954, i.e., φ = 89.59 or 90.41
and
[ e ]F =[ vect(Q) ]F
sinφ=
0.7618−0.08460.6422
We also havetr(Q) = 0.9856 = 1 + 2 cosφ, i.e., φ = ±90.41
Hence, the angle of rotation is φ = 90.41.
(b) From Theorem 3.2.2, we have that the set of points of G undergoing a displacement of minimummagnitude is located on a line L which is parallel to the axis of the rotation involved. In order tototally specify the screw axis L, we must provide the unit vector e defining its direction, and apoint 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 thesame point in the displaced configuration, denoted by P ′, are given by
[p ]F = [ 2 −1 0.25 ]T, [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 rotationand 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 Bare 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, thelatter can be readily obtained from the normalized cross product of the former. If we denote byc the said cross product, then e = c/‖c‖. Now,
c = nA × nB =
200
⇒ e =
100
(b) Referring to Exercise 3.5(b), we havep∗ = 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
andp∗ = [0, 0, 2]T
(c) The Plucker coordinates of the axis L about the origin are given by
πL =[eT , nT
]T= [1, 0, 0, 0, 2, 0]T
3.8 We want to prove that, for any 3-dimensional vectors ω and p,
ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k factors
×p)) · · ·) = (−1)k(‖ω‖2k1− ‖ω‖2(k−1)ωωT )p
Using Theorem 2.3.4, we haveω × (ω × p) = Ω2p
whereΩ2 = −‖ω‖21+ ωωT
Letp2 ≡ ω × (ω × p) = Ω2p
Thenp4 ≡ ω × (ω × p2) = Ω2p2
Thusp2k ≡ ω × (ω × p2(k−1)) = Ω2p2(k−1)
37
Therefore,
ω × (ω × (ω × (ω × p2(k−1)))) = Ω2Ω2p2(k−1)
ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k−4 factors
×p4))) = Ω2 · · ·Ω2︸ ︷︷ ︸
k−2 factors
p4
ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k−2 factors
×p2))) = Ω2 · · ·Ω2︸ ︷︷ ︸
k−1 factors
p2
ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k factors
×p))) = Ω2 · · ·Ω2︸ ︷︷ ︸
k factors
p
but
Ω2Ω2 =(−‖ω‖21+ ωωT
) (−‖ω‖21+ ωωT
)
= ‖ω‖41− 2‖ω‖2ωωT + ωωTω︸ ︷︷ ︸
‖ω‖2
ωT
= ‖ω‖41− ‖ω‖2ωωT
So,
Ω2Ω2Ω2 =(‖ω‖41− ‖ω‖2ωωT
) (−‖ω‖21+ ωωT
)
= (−1)3‖ω‖61+ 2‖ω‖4ωωT − ‖ω‖2ωωTω︸ ︷︷ ︸
‖ω‖2
ωT
= (−1)3‖ω‖61+ ‖ω‖4ωωT
Therefore,
Ω2Ω2 · · ·Ω2Ω2Ω2︸ ︷︷ ︸
k factors
= (−1)k‖ω‖2k1+ (−1)k+1‖ω‖2(k−1)ωωT
= (−1)k(
‖ω‖2k1− ‖ω‖2(k−1)ωωT)
Finally, we have
ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k factors
×p)) · · ·) = (−1)k(‖ω‖2k1− ‖ω‖2(k−1)ωωT )p (47)
Furthermore, define
a ≡ ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k factors
×p)) · · ·)
b ≡ (−1)k(‖ω‖2k1− ‖ω‖2(k−1)ωωT )p
From, eq.(47), we havea = b
Then, by definition, we have
ω × a = Ωa = Ωb
= (−1)kΩ(‖ω‖2k1− ‖ω‖2(k−1)ωωT )p
= (−1)k(‖ω‖2kΩp− ‖ω‖2(k−1)ΩωωTp)
= (−1)k(‖ω‖2k(ω × p)− ‖ω‖2(k−1) (ω × ω)︸ ︷︷ ︸
0
ωTp)
⇒ ω × a = (−1)k‖ω‖2kω × p
38
Finally,ω × (ω × · · · (ω × (ω︸ ︷︷ ︸
2k+1 factors
×p)) · · ·) = (−1)k(‖ω‖2kω)× p
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φ)E+ (φ sinφ)E2 + (1− cosφ)d
dt(E2)
Ω = QQT
For a “small” angle φ, we havesinφ ≈ φ, cosφ ≈ 1
the foregoing expressions for Q and Q thus becoming
Q ≈ 1+ φE
Q ≈ φE+ φE+ φφE2 (48)
Using the two above expressions to compute Ω for “small” rotations we obtain
Ω ≈ (φE+ φE+ φφE2)(1− φE)
= φE+ φE+ φφE2 − φφE2 − φ2EE− φφ2E3
Upon simplifying the above expression and deleting the higher-order terms, i.e., the terms involvingφφ, φ2 and φφ2, we obtain
Ω ≈ φE+ φE =d
dt((φE)
Finally, upon taking the axial vector of the above expression, we conclude that
ω =d
dt(φe)
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(QQT ). We thus start by computing the product QQT using the expression for Qderived in Exercise 2.18. Here is the result from the corresponding Maple worksheet.
QQT =
0 −φ− ψ cos θ ψ sin θ sinφ+ θ cosφ
φ+ ψ 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
W =∂ω
∂η=
− sinφ 0 sin θ cosφcosφ 0 sin θ sinφ0 1 cos θ
Upon expanding det(W), we obtain det(W) = sin θ.
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∑
i=1
‖p2i ‖ , PPT =
3∑
i=1
pi pTi
Hence,
m [tr(PPT )1−P PT ] = m[ 3∑
i=1
‖p2i ‖ 1−
3∑
i=1
pi pTi
]
=
3∑
i=1
m[‖p2
i ‖1− pi pTi
]
The last expression can be readily identified as a discrete form of definition (3.148) of the mass secondmoment 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 usingeq.(2.39):
m3∑
i=1
[‖p2
i ‖1− pi pTi
]= − m
3∑
i=1
P2i
Now, since the moment of inertia IO is necessarily a positive-definite matrix—see the discussion be-low eq.(3.130)—the sum of the squares of the cross product matrices P2
i is bound to be negative-
semidefinite, a result that the reader is invited to verify. Moreover, the sum∑3
i=1 P2i is negative-
semidefinite 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 Ikn1 identical,then they represent the same transformation, although in different frames.
Here, the two matrices, both symmetric, are
[ I ]A =
1 0 00 2 00 0 3
, [ I ]B =1
3
6 2 22 5 02 0 7
and their traces are
tr[ I ]A = 1 + 2 + 3 = 6
tr[ I ]B =1
3(6 + 5 + 7) = 6
Since the two traces are identical, we need verify now their second moments. We have
[ I2 ]A =
1 0 00 4 00 0 9
, [ I2 ]B =1
9
44 22 2622 29 426 4 53
40
Figure 9:
whence we readily obtain
tr[ I2 ]A = 1 + 4 + 9 = 14
tr[ I2 ]B =1
9(44 + 29 + 53) = 14
Finally, we verify their third moments:
[ I3 ]A =
1 0 00 8 00 0 27
, [ I3 ]B =1
3
40 22 3022 21 830 8 47
whence
tr[ I3 ]A = 1 + 8 + 27 = 36
tr[ I3 ]B =360 + 189 + 423
27= 36
the two matrices thus being related by a similarity transformation and the two measurements areacceptable.
3.15 To share the same principal axes of inertia, a point P and the mass center C of a rigid body must lieon one of the principal axes of inertia, as shown in Fig. 9. More formally, we have, from the Theoremof Parallel Axes,
IP = IC +m(‖p‖21− ppT
)(49)
where p is the vector directed from C to P , and C and P are a distance d apart. If e is an eigenvectorof IP and IC , then p = de, and
IPe = λP e, ICe = λCe
Multiplying eq.(49) by e from the right, we can write
λP e = λCe+m(‖p‖2e− (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 theremaining principal directions, XP and ZP , are parallel, respectively, to XC and ZC , the correspond-ing 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[‖p− c‖21− (p− c)(p − c)T
]
Let e be the eigenvector of IP associated with the smallest principal moment of inertia Is:
IP e = ICe+m‖p− c‖2e−m[(p− c)T e
](p− c)
Now dot-multiply both sides of the above equation by e:
eT IPe ≡ Is = eT ICe+m‖p− c‖2 −m[(p− c)T e
]eT (p− c)
= eT ICe+m‖p− c‖2 −m[(p− c)T e
]2
Thus,
Is = eT ICe+m(p− c)T (p− c)−m[eT (p− c)
]2
The necessary condition for Is to attain a minimum at P is that Is becomes stationary at that point,i.e.,
∂Is∂p
= 0+ 2m
[∂(p− c)
∂p
]T
(p− c) − 2m
[∂(p− c)
∂p
]T
e[(p− c)T e] = 0
whence,∂Is∂p
= 2m[1− eeT ](p− c) = 0
Hence, ∂Is/∂p vanishes if either (p − c) || e or p = c. If (p − c) || e, then P and C share the sameprincipal axes of inertia, as per Exercise 3.15, i.e., Is is the smallest principal moment of inertia ofthe body; else p = c, thereby showing that the centroid is, in fact, a stationary point of the smallestprincipal moment of inertia. To show that this stationary point is a minimum, and not a saddle point ora maximum, the matrix of second partial derivatives, ∂2Is/∂p
2, must be at least positive semidefinite.But this is so because
∂2Is∂p2
= 2m(1− eeT )∂(p− c)
∂p= 2m(1− eeT )1 = 2m(1− eeT )
which can be readily shown to have two identical eigenvalues, λ1 = λ2 = 2m, while the third onevanishes, λ3 = 0, q.e.d.
3.17 We recall below the expressions for M and W:
M =
[IC OO m1
]
and W =
[Ω OO 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 orientationidentical to that of frame F . Thus
[M ]F0=
[[ IC ]F0
OO m1
]
and [W ]F0=
[[Ω ]F0
OO 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,
[ IC ]F0= [ Q ]F0
[ IC ]F [QT ]F0
+ [Q ]F0[IC ]F [Q
T ]F0+ [Q ]F0
[ IC ]F [ QT ]F0
Since frame F is attached to the rigid body, [ IC ]F is constant, and thus [ IC ]F = O. Therefore,
[ IC ]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 [ IC ]F0in the form
[ IC ]F0= [Ω ]F0
[Q ]F0[ IC ]F [Q
T ]F0︸ ︷︷ ︸
[ IC ]F0
+ [Q ]F0[ IC ]F [Q
T ]F0︸ ︷︷ ︸
[ IC ]F0
[ΩT ]F0
and thus,[ IC ]F0
= [Ω ]F0[ IC ]F0
− [ IC ]F0[Ω ]F0
Therefore,
[ M ]F0=
[[ IC ]F0
OO O
]
=
[[ΩIC − ICΩ ]F0
OO O
]
=
[[Ω ]F0
OO O
] [[ IC ]F0
OO m1
]
−[[ IC ]F0
OO m1
] [[Ω ]F0
OO O
]
Finally, we obtainM = WM−MW
For the second part of the problem, we differentiate directly µ with respect to time, thereby obtaining
µ = Mt+ Mt
But we just found thatM = WM−MW
and soµ = Mt+WMt−MWt
Now, the twist of a rigid body lies in the nullspace of its angular-velocity dyad, i.e.
Wt = 0
and hence,µ = Mt+WMt
3.19 The kinetic energy of a rigid body B, expressed as a quadratic form of the twist tA, referred to anarbitrary point A, takes the form below, as derived from eq.(3.145):
T =1
2tTAMAtA (51)
with tA defined as
tA =
[ω
a
]
(52)
43
with a denoting the velocity of A in an inertial frame; now the question is how to define MA. Thistask is eased if the twist-transfer formula of eqs.(3.83a & b) is recalled:
tC = UtA, U ≡[
1 OA−C 1
]
(53)
Upon substitution of the expression for tC as given above into eq.(3.145), the kinetic energy takes theform
T =1
2tTAU
TMUtA (54)
whence MA is the matrix associated with the foregoing quadratic form in tA, namely,
MA = UTMU
Upon expansion,
MA =
[IC +m(A−C)(A−C)T m(A−C)T
m(A−C) m1
]
≡[
IC +m(C−A)(C −A)T m(C−A)m(C−A)T m1
]
(55)thereby deriving eq.(3.148), as readily proven. Indeed, the foregoing expression can be decomposedinto
MA =
[IC OO m1
]
+m
[(C−A)(C−A)T (C−A)
(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)T 1
]
=
[C−A C−A
O 1
]
︸ ︷︷ ︸
P
[(C−A)T O(C−A)T 1
]
︸ ︷︷ ︸
PT
which thus shows that MA is the sum of two terms, the first being positive-definite (p.d.), for its twodiagonal blocks are so, the second being at least positive-semidefinite (p.s.d.). Infact, P is singular, ofrank 5, its null space being spanned by the 6-dimensional vector [ (c− a)T , 0T ]T , with 0 standing forthe 3-dimensional zero vector. Therefore, MA is the sum of one p.d. and one p.s.d. terms, the resultthen 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, wedefine 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 andsin θ1 = 0. Thus,
[ a1 ]1 =
00b1
, [ c ]1 =
xCyCzC
, Q1 =
1 0 00 λ1 −µ1
0 µ1 λ1
with vectors a2, a3, and a4, as well as matrices Q2 and Q3 remaining as in the all-revolute case. Notethat, in our case, b1 = b1(t). Moreover, we recall eq.(4.17):
Q2(b2 +Q3b3 + b4u3) = 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 byQ2; 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 thirdscalar 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 b1and θ3.
Let the Euclidean norm of the left-hand side of eq.(56) be denoted by l, that of its right-hand side byr. We then have
l2 ≡ a22 + b22 + a23 + b23 + b24 + 2bT2 a3 + 2b4b
T2 u3 + 2b3b4λ3
r2 ≡ ‖c‖2 + b21 − 2b1zC
wherebT2 u3 = a2µ3s3 − b2µ2µ3c3 + b2λ2λ3
andbT2 a3 = a2a3c3 + b2µ2a3s3 + b2λ2b3
Upon equating l2 with r2, we obtain
A′c3 +B′s3 − b21 + 2zCb1 + C′ = 0 (57)
whose coefficients do not contain any unknown, i.e.,
A′ = 2(a2a3 − b2µ2µ3b4)
B′ = 2(b2µ2a3 + a2µ3b4)
C′ = a22 + b22 + a23 + b23 + b24 + 2b2λ2b3 + 2b2λ2λ3b4 + 2b3λ3b4 − ‖c‖2
Moreover, the third scalar equation of eq.(56) is similar to that of the all-revolute case, except thatnow, we have c1 = 1 and s1 = 0, which thus yields
F +Hc3 + Is3 + J = 0
whereF + J = λ1b1 + J ′′
with J ′′ defined asJ ′′ ≡ yCµ1 + b2 + λ2b3 + λ2λ3b4 − zCλ1
and hence, the above equation becomes
Hc3 + Is3 + λ1b1 + J ′′ = 0 (58)
Now we can solve for c3 and s3 from eqs.(57) and (58):
[c3s3
]
=1
∆1Mv (59)
where matrix M and vector v are defined as
M ≡[I −B′
−H A′
]
, v ≡[b21 − 2zCb1 − C′
−λ1b1 − J ′′
]
and the scalar ∆1 is given by∆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,
vTMTMv = ∆21 (60)
45
Upon performing the foregoing matrix product, we have
MTM ≡[M11 M12
M12 M22
]
whose entries are given below:
M11 ≡ (b24µ23 + a23)µ
22
M22 ≡ 4[a22(a23 + µ2
3b24) + b22µ
22(µ
23b
24 + a23)]
M12 ≡ 2b2µ22(a
23 + µ2
3b24)
Upon expansion, eq.(60) leads to a quartic equation in b1, namely,
Γ4b41 + Γ3b
31 + Γ2b
21 + Γ1b1 + Γ0 = 0 (61a)
with coefficients Γi 40 defined below:
Γ0 ≡ C′2M11 + 2C′J ′′M12 + J ′′2M22 − (A′I −B′H)2 (61b)
Γ1 ≡ 4zCC′M11 + 2(C′λ1 + 2zCJ
′′)M12 + 2J ′′λ1M22 (61c)
Γ2 ≡ (4z2C − 2C′)M11 − 2(J ′′ − 2zCλ1)M12 + λ21M22 (61d)
Γ3 ≡ −4zCM11 − 2λ1M12 (61e)
Γ4 ≡M11 (61f)
With b1 known, we calculate θ3 from eqs.(59). Once b1 and θ3 are known, all quantities in eq.(56) areknown, 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
Q2u = w (62a)
with u and w defined asu ≡ b2 +Q3b3 + b4u3, w ≡ QT
1 c− b1 (62b)
We now note that the third scalar equation of eq.(62a) does not contain θ2, and hence, does not helpus in our calculation of this unknown. We thus retain only the first two of those three equations, whichare displayed below:
[c2 −λ2s2 µ2s2s2 λ2c2 −µ2c2
]
u1u2u3
=
[w1
w2
]
(63)
and then recast them in a more convenient form:[
u1 −(u2λ2 − u3µ2)u2λ2 − u3µ2 u1
] [c2s2
]
=
[w1
w2
]
(64)
whence,[c2s2
]
=1
∆2
[u1 u2λ2 − u3µ2
−(u2λ2 − u3µ2) u1
] [w1
w2
]
(65)
with ∆2 defined as∆2 = u1u2 + (u2λ2 − u3µ2)
2 (66)
In summary, then, the problem admits up to four solutions for b1, with one unique solution θ3 for eachof these four values of b1, and one unique solution θ2 for each pair of values (b1, θ3). Therefore, a pointC of the three-dimensional space can be reached with the tip of the end link of this manipulator, suchas 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 DHparameters. So, we analyze the manipulator independent from eqs.(4.31 & 32). We define the DHframes as shown in Fig. 10.
Thus,a1 = a3 = b1 = b2 = b3 = 0, a2 = b4 = 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, D = 2a2b4 = 2a2,
E = a22 + b24 − ‖c‖2 = 2a2 − ‖c‖2
Moreover,F = yC , G = −xC , H = I = J = 0
Now, eq.(4.19a) becomes
2a2s3 + 2a2 − ‖c‖2 = 0 ⇒ θ3 = arcsin
(‖c‖2 − 2a2
2a2
)
thus obtaining two supplementary values (they add up to 180) of θ3. Likewise, eq.(4.20a) becomes
yCc1 − xCs1 = 0 ⇒ θ1 = arctan
(yCxC
)
47
Given that arctan is a two-valued function, we thereby obtain two distinct values for θ1 differing in180. 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,
a
[1 + sin θ3 cos θ3− cos θ3 1 + sin θ3
] [cos θ2sin θ2
]
=
[xC cos θ1 + yC sin θ1
zC
]
whence, [cos θ2sin θ2
]
=1
a∆2
[1 + sin θ3 − cos θ3cos θ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
−ητ24 − 2ξτ4 + η = 0 or ητ24 + 2ξτ4 − η = 0
whence,
τ4 =−ξ ±
√
ξ2 + η2
η=−ξ ±
√
1− ζ2η
Furthermore, the two equations for θ5 are obtained by equating the first two components of the thirdcolumns 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 canbe reached with up to two distinct wrist postures. In summary, then, the manipulator can produce anarbitrary pose of its EE with up to eight distinct postures.
4.6 The rotation matrices Qi introduced in Section 4.2 are given by
Qi =
cos θi −λi sin θi µi sin θisin θi λi cos θi −µi cos θi0 µi λi
48
We thus have
qi ≡ vect(Qi) =1
2
µi + µi cos θiµi sin θi
sin θi + λi sin θi
=1
2
µi(1 + cos θi)µi sin θi
sin θi(1 + λi)
but,qi ≡ sinφiei
and thus
sinφi = ‖qi‖ =√
1
4[µ2
i (1 + cos θi)2 + µ2i sin θ
2i + (1 + λi)2 sin θ2i ]
=1
2
√
µ2i (1 + 2 cos θi + cos θ2i + sin θ2i ) + (1 + λi)2 sin θ2i
=1
2
√
2µ2i (1 + cos θi) + (1 + λi)2 sin θ2i (67)
Moreover,tr(Qi) = cos θi + λi cos θi + λi = cos θi(1 + λi) + λi
and
cosφi =tr(Qi)− 1
2=
cos θi(1 + λi) + λi − 1
2(68)
Using eqs.(67) and (68), the angle of rotation φi is uniquely defined considering our convention, where0 ≤ φi ≤ π. Finally, the unit vector ei parallel to the axis of rotation is simply given by
ei =1
sinφiqi, sinφi 6= 0
4.7 For the orientation problem, eq.(4.40) gives two solutions for θ4. These two solutions merge into asingle one when the radical vanishes, and then correspond to a singular posture. We want the solutionsthat lie the farthest apart. Obviously, these solutions will occur when the radical attains its maximumvalue. 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
[e6]4 = [e7]4 = [ ξ η ζ ]T
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
[i7]4 = [ 0 0 1 ]T, and [k7]4 = [ ξ η 0 ]
T
We also have[j7]4 = [k7 × i7]4 = [−η ξ 0 ]
T
49
Using Definition 2.2.1, we obtain
[R]4 =
0 −η ξ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 attainsthe orientation given by R, the three wrist axes are mutually orthogonal, the wrist Jacobian thus beingorthogonal, and hence, isotropic in the sense of Section 5.8.
4.8 Three lines are given in terms of their Plucker arrays:
pLi=
[eini
]
, i = 1, 2, 3
The fourth line has the Plucker array
pL =
[en
]
From Exercise 3.3, we have that the moment of line L about a second line Li vanishes if the two linesintersect. 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 ofFig. 4.15. From Example 4.4.2, we know that this manipulator is displayed in Fig. 4.15 in a singularposture, and thus point C is known to be a point of L. All we need now is determine the direction ofL, given by the unit vector e. Moreover, from Fig. 4.16, the quantities below are readily known in F1:
p1 =
000
, p2 =
−a00
, p3 =
−aa−a
, p = c =
02a−a
e1 =
001
, e2 =
010
, e3 =
100
Thus,
(p1 − p)× e1 =
−2a00
, (p2 − p)× e2 =
−a0−a
, (p3 − p)× e3 =
00a
Equation (70) thus leads to a set of three equations with three unknowns, namely, the three componentsof 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 manip-ulator is determined by the point P and the unit vector e, namely,
p =
02a−a
, e =
010
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 Plucker coordinatesof 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 parameterssummarized in Table. 1.
Figure 11: Kinematic chain of the Fanuc Arc Mate 120iB
Table 1: DH parameters of the Fanuc Arc Mate 120iB
i ai(mm) bi(mm) αi()
1 150 525 902 770 0 03 100 0 904 0 740 905 0 0 906 100 200 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 DHparameters summarized in Table. 2.
51
a1
a2
a3
a4 = 0
a5
a6 = 0
O′1
O′2
O′3
O′4, O
′5
P
O′6
X1
X2
X3
X4
X5
X6
X7
Z1
Z2
Z3
Z4
Z5
Z6, Z7
Figure 12: Kinematic chain of the Motoman-EA1400N
Table 2: DH parameters of the Motoman-EA1400N
i ai(mm) bi(mm) αi()
1 150 450 902 570 0 03 200 0 904 0 640 905 30 0 906 0 -200 0
5 Kinetostatics of Serial Robots
5.1 This solution needs an in-depth revision. The Jacobian matrix will be obtained using the algo-rithm 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 = −1c1 = c2 = c3 = c4 = c5 = c6 = 0
s1 = s3 = s5 = 1, s2 = s4 = s6 = −1Therefore, using eq.(4.1e) we obtain
Q1 = Q3 = Q5 =
0 0 11 0 00 1 0
, Q2 = Q4 = Q6 =
0 0 1−1 0 00 −1 0
52
and, from eq.(4.3b),
[ a1 ]1 = [ a3 ]3 = [ a5 ]5 =
05050
, [ a2 ]2 = [ a4 ]4 =
0−5050
, [ a6 ]6 =
0050
i) Evaluation of submatrix A:
[ e1 ]1 = [ 0 0 1 ]T
P1 = Q1, i.e., [ e2 ]1 = [ 1 0 0 ]T
P2 = P1Q2 =
0 −1 00 0 1−1 0 0
, i.e., [ e3 ]1 =
010
P3 = P2Q3 =
−1 0 00 1 00 0 −1
, i.e., [ e4 ]1 =
00−1
P4 = P3Q4 =
0 0 −1−1 0 00 1 0
, i.e., [ e5 ]1 =
−100
P5 = P4Q5 =
0 −1 00 0 −11 0 0
, i.e., [ e6 ]1 =
0−10
Therefore,
[A ]1 =
0 1 0 0 −1 00 0 1 0 0 −11 0 0 −1 0 0
ii) Evaluation of submatrix B:
We have[ r6 ]6 = [ a6 ]6 = [ 0 0 50 ]T
and[ ri ]i = [ ai ]i +Qi[ ri+1 ]i+1, i = 5, 4, 3, 2, 1
Thus,
[ r5 ]5 =
505050
, [ r4 ]4 =
50−1000
, [ r3 ]3 =
0100−50
,
[ r2 ]2 =
−50−50−50
, [ r1 ]1 =
−5000
Therefore,[ e1 × r1 ]1 = [ 0 −50 0 ]T
and[ ei × ri ]1 = Pi−1[ ei × ri ]i, i = 2, · · · , 6
Thus,
[ e2 × r2 ]1 =
050−50
, [ e3 × r3 ]1 =
00100
, [ e4 × r4 ]1 =
−100500
,
53
[ e5 × r5 ]1 =
05050
, [ e6 × r6 ]1 =
000
Hence,
[B ]1 =
0 0 0 −100 0 0−50 50 0 50 50 00 −50 100 0 50 0
Finally
[J ]1 =
0 1 0 0 −1 00 0 1 0 0 −11 0 0 −1 0 00 0 0 −100 0 0−50 50 0 50 50 00 −50 100 0 50 0
5.2 We have here a decoupled manipulator, and thus,
JT12nw = ∆τw (71a)
JT21f = ∆τ a − JT
11nw (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 allquantities in Frame 1, we have
e1 =
001
, e2 = e3 =
010
, e4 =
001
, e5 =
010
, e6 =
100
r1 = r2 =
101
, r3 =
001
Thus,
e1 × r1 =
010
, e2 × r2 =
10−1
, e3 × r3 =
100
Therefore,
J11 =
0 0 00 1 11 0 0
, J12 =
0 0 10 1 01 0 0
, J21 =
0 1 11 0 00 −1 0
Hence,
J−112 =
0 0 10 1 01 0 0
, J−121 =
0 1 00 0 −11 0 1
From eq.(71a), we have
nw =(JT12
)−1∆τw =
0 0 10 1 01 0 0
010
=
010
54
Figure 13:
and, from eq.(71b),
f =(JT21
)−1 [∆τ a − JT
11nw
]=
0 0 11 0 00 −1 1
021
−
0 0 10 1 00 1 0
010
which yields
f =
0 0 11 0 00 −1 1
010
=
00−1
This last expression represents the weight of the tool, which is directed in the negative Z1 direction,as expected. Hence,
w = 1 N
Now, nw is the resultant moment acting on the EE when the load f is applied at the center C of thewrist. Therefore, defining
r = [−d 0 0 ]T
we havenw = f × r = [ 0 d 0 ]
T
Comparing this expression with eq.(5), we finally obtain
d = 1 m
5.3 We show here two approaches, the first being geometric, and is what we recommend; the second isincluded 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
r1 =
[0
1 +√2
]
, r2 =
[−√2/2
1 +√2/2
]
, r3 =
[−√2/2√2/2
]
55
Figure 14: Manipulator at posture θ1 = θ2 = θ3 = 45
Hence,
Er1 =
[−1−
√2
0
]
, Er2 =
[−1−
√2/2
−√2/2
]
, Er3 =
[−√2/2
−√2/2
]
Therefore,
J =
1 1 1−1−
√2 −1−
√2/2 −
√2/2
0 −√2/2 −
√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 JTw = τ
leads to
1 −1−√2 0
1 −1−√2/2 −
√2/2
1 −√2/2 −
√2/2
nfxfy
=
−√2
−√2
1−√2
We now obtain a reduced system of two equations with two unknowns upon subtracting the first ofthe above three scalar equations from the second and the third, which thus yields
1 −1−√2 0
0√2/2 −
√2/2
0 1 +√2/2 −
√2/2
nfxfy
=
−√2
01
The 2× 2 system is, then,[ √
2/2 −√2/2
1 +√2/2 −
√2/2
] [fxfy
]
=
[01
]
The determinant ∆ of the foregoing matrix turns out to be
∆ =
√2
2=
1√2
56
and hence,[fxfy
]
=√2
[−√2/2
√2/2
−1−√2/2
√2/2
] [01
]
=√2
[√2/2√2/2
]
=
[11
]
N
whence,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 quantitiesbelow must be evaluated:
s3 =
[c123s123
]
=
[−√2/2√2/2
]
s2 =
[c12 + c123s12 + s123
]
=
[0−√2/2
1 +√2/2
]
=
[−√2/2
1 +√2/2
]
s1 =
[c1 + c12 + c123s1 + s12 + s123
]
=
[√2/2 + 0−
√2/2√
2/2 + 1 +√2/2
]
=
[0
1 +√2
]
Furthermore, using eq.(5.64) we have
∆ ≡ −[
−√22 −
√22
][0 −11 0
][
−√22
−1−√22
]
=
√2
2
which is rightfully identical to the value obtained above. Therefore, from eq.(5.72),
f =1
∆[(τ2 − τ1)(s3 − s1)− (τ3 − τ1)(s2 − s1)]
=2√2
(
0(s3 − s1)−[−√2/2
−√2/2
])
=
[11
]
N
Finally,
n = τ1 + sT1 Ef = −√2 + [ 0 1 +
√2 ]
[0 −11 0
] [11
]
n = 1 Nm
thereby confirming the correctness of the results.
5.4 We haveJ12θ = ω
Therefore,
‖ω‖2 = ωTω = θTJT12J12θ
Matrix J12 is given byJ12 = [e4 e5 e6 ]
We have
[e4]4 =
001
, [e5]4 =
sin θ4− cos θ4
0
, [e6]4 =
ξη0
Hence
J12 =
0 sin θ4 ξ0 − cos θ4 η1 0 0
57
Therefore
JT12J12 =
0 0 1sin θ4 − cos θ4 0ξ η 0
0 sin θ4 ξ0 − cos θ4 η1 0 0
=
1 0 00 1 ξ sin θ4 − η cos θ40 ξ sin θ4 − η cos θ4 1
However, since α5 = 90, we must have e5 ⊥ e6, or eT5 e6 = 0. Therefore,
[eT5 e6]4 = ξ sin θ4 − η cos θ4 = 0
Hence, JT12J12 = 1 and ‖θ‖ remains constant when ‖ω‖ 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
c = θ1e1 × r1 + θ2e2 × r2 + θ3e3 × r3 = v
orv = Jθ (72)
whereJ = [ e1 × r1 e2 × r2 e3 × r3 ]
For this problem, all quantities will be expressed in frame 1. Therefore,
e1 =
001
, e2 =
010
, e3 =
100
r1 =
0a0
, r2 =
aa0
, r3 =
a0a
e1 × r1 =
−a00
, e2 × r2 =
00−a
, e3 × r3 =
0−a0
Thus,
J =
−a 0 00 0 −a0 −a 0
= a
−1 0 00 0 −10 −1 0
≡ aQ
where Q is apparently an orthogonal matrix. Now, from eq.(72), we have
θ = J−1v =1
aQTv
Therefore,
‖θ‖2 = θTθ =
1
a2vT(QT)T (
QT)v
Hence,
‖θ‖2 =1
a2vTv =
1
a2‖v‖2
Therefore, if ‖v‖ remains constant, so does ‖θ‖.On the other hand, upon differentiating both sides of eq.(72) with respect to time, we obtain
Jθ + Jθ = v = a
58
or
θ = J−1(
a− Jθ)
=1
aQT (a − Jθ)
whence, it is apparent that ‖θ‖ remains constant under constant a if and only if Jθ is a multiple of a,say Jθ = αa, for a real α. Under these conditions,
θ =1
a(1− α)QTa
and hence,
‖θ‖2 =
(1− αa
)2
‖a‖2 or ‖θ‖ =∣∣∣∣
1− αa
∣∣∣∣‖a‖
which makes it apparent that, under the above condition, ‖θ‖ remains constant for constant ‖a‖. Notethat the same condition implies that
JJ−1v = αa (73)
and hence, α can be obtained from eq.(73) upon premultiplying eq.(73) by aT , thus obtaining
α =aT JJ−1v
‖a‖2 =aT Jθ
‖a‖2 = eTJθ
‖a‖
where e is a unit vector in the direction of the acceleration, i.e., e ≡ a/‖a‖. The geometric interpre-tation of the above result is, then, that α is the projection onto the acceleration a of the quadraticcomponent Jθ normalized with respect to ‖a‖. When this component is normal to a, then the said pro-jection vanishes and the only possibility for ‖θ‖ to remain constant under constant ‖v‖ and constant‖a‖ is Jθ = 0.
5.6 The load f of the joint and torque τ are related by
τ = JT f
Therefore,‖τ‖2 = τTτ = fTJJT f
From Exercise 5.5, we have
J = a
−1 0 00 0 −10 −1 0
and thus
JJT = a2
1 0 00 1 00 0 1
Hence‖τ‖2 = a2fT f = a2‖f‖2
Therefore, as long as ‖f‖ remains constant, ‖τ‖ also remains constant.
5.7 We have here a decoupled manipulator, whose Jacobian matrix is given by
J =
[J11 J12
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
e1 =
001
, e2 =
100
, e3 = e4 =
010
, e5 =
100
, e6 =
001
r1 = r2 =
0b30
, e1 × r1 =
−b300
, e2 × r2 =
00b3
Therefore,
J11 =
0 1 00 0 01 0 0
, J12 =
0 1 01 0 00 0 1
, J21 =
−b3 0 00 0 10 b3 0
Hence,
J =
0 1 0 0 1 00 0 0 1 0 01 0 0 0 0 1−b3 0 0 0 0 00 0 1 0 0 00 b3 0 0 0 0
(b) The twist of the center C of the wrist is given by
tC = Jθ (74)
60
For unit values of all joint rates, we have
tC =
0 1 0 0 1 00 0 0 1 0 01 0 0 0 0 1−b3 0 0 0 0 00 0 1 0 0 00 b3 0 0 0 0
111111
=
212−b31b3
The twist of the EE P , tP , can be obtained using the twist-transfer formula given by eqs.(3.84)and (3.85), as
tP =
[1 O
C−P 1
]
tC
where C and P are the cross-product matrices of the position vectors c and p, respectively. Here,
c = [ 0 b3 0 ]T
p = [ 0 b3 0.5 ]T
Thus
C =
0 0 b30 0 0−b3 0 0
, P =
0 −0.5 b30.5 0 0−b3 0 0
and
C−P =
0 0.5 0−0.5 0 00 0 0
Finally, we obtaintP = [ 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 havetC = Jθ + Jθ
orθ = J−1
(
tC − Jθ)
(75)
wheretC = 0
Thus, eq.(75) reduces toθ = −J−1Jθ (76)
According to eq.(4.94), and considering that we have a prismatic joint, Jθ is given by
Jθ = θ1
[0u1
]
+ θ2
[e2u2
]
+ b3
[0e3
]
+ θ4
[e4u4
]
+ θ5
[e5u5
]
+ θ6
[e6u6
]
(77)
whereei = ωi−1 × ei, i = 2, 3, · · · , 6
andu1 = e1 × r1, u2 = e2 × r2 + e2 × r2
61
noting,
r1 = a1 + a2 + a3
r1 = a1 + a2 + a3
= b3e3 + b3e3
= b3e3 + b3(ω2 × e3)
a1 = 0
a2 = b3e3
a3 = 0
Moreover,r1 = r2 = b3e3 + ω2 × r2
all other ui and ri vectors vanishing. Thus, considering unit joint rates, we have
ω1 = θ1e1 = [ 0 0 1 ]T
ω2 = ω3 = θ1e1 + θ2e2 = [ 1 0 1 ]T
ω4 = [ 1 1 1 ]T
ω5 = [ 2 1 1 ]T
ω6 = ω = [ 2 1 2 ]T
Therefore,
e2 = [ 0 1 0 ]T
e3 = [−1 0 1 ]T
e4 = [−1 0 1 ]T
e5 = [ 0 1 −1 ]T
e6 = [ 1 −2 0 ]T
r1 = r2 = [−b3 1 b3 ]T
r3 = r4 = r5 = r6 = 0
and
u1 = [−1 −b3 0 ]T
u2 = [ 0 −b3 1 ]T
u3 = u4 = u5 = u6 = 0
Using eq.(77) with unit joint rates, we finally obtain
Jθ = [ 0 0 0 −2 −2b3 2 ]T
Let
q ≡ Jθ =
[qa
qw
]
whereqa = [ 0 0 0 ]
T, qw = [−2 −2b3 2 ]
T
Equation (76) can be written asJθ = −Jθ
62
or [J11 J12
J21 0
] [θa
θw
]
= −[qa
qw
]
We thus haveJ21θa = −qw
or
θa = −J−121 qw = −
−1/b3 0 00 0 1/b30 1 0
−2−2b32
=
−2/b3−2/b32b3
andJ11θa + J12θw = −qa = 0
or
θw = −J−112 J11θa = −
0 1 01 0 00 0 1
0 1 00 0 01 0 0
−2/b3−2/b32b3
=
02/b32/b3
Finally,θ = [−2/b3 −2/b3 2b3 0 2/b3 2/b3 ]
T
5.8 (a) For a decoupled manipulator, we have
JT11nw + JT
21f = τ a (78)
JT12nw = τw (79)
where J11 and J21 were obtained in Problem 4.19 as
J11 =
0 1 00 0 01 0 0
, J21 =
−b3 0 00 0 10 b3 0
Moreover, nw is the resultant moment acting on the EE when f is applied at the center of thewrist 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 wesubstitute nw, as given above, into eq.(78), we obtain
JT11(P−C)f + JT
21f = τ a
or[JT
11(P−C) + JT21]f = τ a (80)
Further, we have the numerical values
p− c =
000.5
⇒ P−C =
0 −0.5 00.5 0 00 0 0
Hence,
JT11(P−C) =
0 0 11 0 00 0 0
0 −0.5 00.5 0 00 0 0
=
0 0 00 −0.5 00 0 0
63
Further,
JT11(P−C) + JT
21 =
0 0 00 −0.5 00 0 0
+
−b3 0 00 0 b30 1 0
=
−b3 0 00 −0.5 b30 1 0
(81)
Upon substitution of the foregoing numerical values into eq.(80), we obtain a system of threelinear equations in the three unknown components of f:
−b3 0 00 −0.5 b30 1 0
fxfyfz
=
010050
whence,
fx = 0, fy = 50 N, fz =125
b3N
(b) With f available, nw is readily computed as
nw = (P−C)f =
0 −0.5 00.5 0 00 0 0
050
125/b3
=
−2500
Nm
Now we calculate τw from eq.(79), where we need J12. From Problem 4.19,
J12 =
0 1 01 0 00 0 1
Thus,
τw =
0 1 01 0 00 0 1
−2500
=
0−250
Nm
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
J =
[J11 J12
J21 O
]
withJ11 = [ 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 expressedin F1, we have
e1 =
001
, e2 =
0−10
, e3 =
−100
, e4 =
−100
, e5 =
00−1
, e6 =
0−10
and
r1 = r2 =
−(b3 + b4)00
64
Thus,
e1 × r1 =
0−(b3 + b4)
0
, e2 × r2 =
00
−(b3 + b4)
Therefore,
J11 =
0 0 00 −1 01 0 0
, J12 =
−1 0 00 0 −10 −1 0
, J21 =
0 0 −1−b34 0 00 −b34 0
with b34 defined as b3 + b4, and
J =
0 0 0 −1 0 00 −1 0 0 0 −11 0 0 0 −1 00 0 −1 0 0 0
−(b3 + b4) 0 0 0 0 00 −(b3 + b4) 0 0 0 0
(b) The twist tP of the EE at point P is given. However, we found in (a) the Jacobian for thedecoupled manipulator with its EE twist defined at the center C of the wrist. Thus, the twist atthe center of the wrist, tC , is required. This is obtained using the twist-transfer formula given byeqs.(3.84a & b), as
tC =
[1 0
P−C 1
]
tP
where C and P are the cross-product matrices of the position vectors c and p, respectively. Here,
c = [−(b3 + b4) 0 0 ]T
p = [−(b3 + b4) −b6 0 ]T
Thus
C =
0 0 00 0 (b3 + b4)0 −(b3 + b4) 0
P =
0 0 −b60 0 (b3 + b4)b6 −(b3 + b4) 0
and
P−C =
0 0 −b60 0 0b6 0 0
Therefore,
c = (P−C)ω + p = ω
−b60b6
+ v
111
=
v − b6ωv
v + b6ω
For a decoupled manipulator, we have
J11θa + J12θw = ω
J21θa = c
65
Thus,
θa = J−121 c =
0 −1/(b3 + b4) 00 0 −1/(b3 + b4)−1 0 0
v − b6ωv
v + b6ω
= − 1
b3 + b4
vv + b6ω
(b3 + b4)(v − b6ω)
and
θw = J−112 (ω − J11θa) =
−1 0 00 0 −10 −1 0
ωωω
− 1
b3 + b4
0v + b6ω−v
=−1
b3 + b4
ω(b3 + b4)v + (b3 + b4)ω
−v + (b3 + b4 − b6)ω
(c) We have, for a decoupled manipulator,
JT11nw + JT
21f = τ a
JT12nw = τw
where nw is the resultant moment acting on the EE when f is applied at the center C of the wrist.Therefore,
nw = n+ (p− c)× f = T
111
+
0−b60
× F
111
=
T − b6FT
T + b6F
Thus,
τw = JT12nw =
−1 0 00 0 −10 −1 0
T − b6FT
T + b6F
= −
T − b6FT + b6F
T
and
τ a = JT21f + JT
11nw
=
0 −(b3 + b4) 00 0 −(b3 + b4)−1 0 0
FFF
+
0 0 10 −1 00 0 0
T − b6FT
T + b6F
=
T − (b3 + b4 − b6)F−T − (b3 + b4)F
−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
R =√
b23 + r2, r = a2 +√
a23 + b24
which, for the given numerical values, lead to
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 computationis not as straightforward as that of the maximum reach of the Puma robot, it is possible to find itgraphically, without resorting to specialized methods of nonlinear-equation solving or optimization, asexplained below5
It is apparent that the maximum reach is independent of θ1, for singularities do not depend on thisjoint variable. So, we lock the first joint and, in the posture of Fig. 4.17, rotate the third joint throughone full turn, point C thus describing a circle C of radius a lying in the Y1-Z1 plane, with center atpoint O′
3, of coordinates (0, a, −a). Next, upon performing a full rotation of the second joint, thecircle describes a toroid of axis Z2, the problem now reducing to one of finding the point of the surfaceof 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.
[ c ]2 ≡
x2y2z2
, [ c ]3 ≡
x3y3z3
Therefore, the equation of C in F3 is
C: x23 + y23 = a2 (82)
However, it will be more convenient to calculate the distance of a point on the surface of the toroidfrom 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)
5This solution is reported in Angeles, J., 2003, “The design of a three-axis positioning manipulator for a given reach,”Ingenierıa Mecanica, Vol. 1, No. 2, pp. 35-41.
67
where, either from inspection or from formula (4.3b),
[ a2 ]2 = a
c2s21
Moreover,
Q2 =
c2 0 s2s2 0 −c20 1 0
Therefore,
[ c ]3 =
c2 s2 00 0 1s2 −c2 0
x2 − ac2y2 − as2z2 − a
=
(x2 − ac2)c2 + (y2 − as2)s2z2 − a
(x2 − ac2)s2 − (y2 − as2)c2
=
x3y3z3
(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 theZ1 axis lies on the trace T of the toroid on the X2-Z2 plane, this trace thus being found upon settingy2 = 0 in eqs.(84a & b), thus obtaining, correspondingly,
x22c22 − 2ax2c2 + z22 − 2az2 + a2 = 0 (85a)
andx2s2 − a = 0 (85b)
Now we have the trace T of the toroid on theX2-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. Itwill prove more convenient to work now with nondimensional variables u and v, defined as
u ≡ x2a, v ≡ z2
a(86)
In terms of these new variables, eq.(85a) becomes
u2c22 − 2uc2 + v2 − 2v + 1 = 0 (87a)
Now, c22 in the above equation can be substituted for
c22 ≡ 1− s22 ≡ 1− a2
x22≡ 1− 1
u2
which thus reduces eq.(87a) to a linear equation in c2, namely,
u2 + v2 − 2v − 2uc2 = 0 (87b)
while eq.(85b) reduces tous2 − 1 = 0 (87c)
68
Upon solving for c2 from eq.(87b) and for s2 from eq.(87c), we have
c2 =u2 − 2v + v2
2u, s2 =
1
u, u 6= 0 (88)
Now, upon squaring each side of the two foregoing expressions and adding up these squares, θ2 iseliminated, thus obtaining the trace T sought, namely
T : g(u, v) ≡ (u2 + v2)(u2 + v2 − 4v)− 4(u2 − v2 − 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 Cat 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 jointaxes. 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 itsposition 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
[ e1 ]2 =
010
, [ e2 ]2 =
001
, [ e3 ]2 =
s2−c20
, [ e ]2 =
e10e3
69
T T
S
Figure 18: Contour of the trace T of the toroid and the singularity locus S on the u-v plane
[p1 ]2 =
−a00
, [p2 ]2 =
000
, [p3 ]2 =
ac2as2a
, [p ]2 =
x20z2
[ e2 ]2 =
e10e3
The condition that line L stemming from P intersects all three joint axes is equivalent to the conditionthat 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:
(p1 − p)× e1 =
z20
−(a+ x2)
, (p2 − p)× e2 =
0x20
, (p3 − p)× e3 =
(a− z2)c2(a− z2)s2−a+ x2c2
Therefore, the vanishing of the three moments of L with respect to all three joint axes leads to
z2e1 − (a+ x2)e3 = 0
0 = 0
(a− z2)c2e1 − (a− x2c2)e3 = 0
the second equation indicating that the moment of L with respect to the second joint axis vanishesidentically. The above system of equations is apparently linear homogeneous in the two-dimensionalvector [ 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− x2c2) + (a+ x2)(a− z2)c2 = 0
whencec2 =
z2x2 − z2 + a
or, in terms of the nondimensional variables u and v,
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 ofthe singularity locus S:
v
u− v + 1=u2 + v2 − 2v
2u
If we now clear denominators in the above equation, we obtain a cubic equation in u and v, namely,
S: (u2 + v2 − 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 andthe singularity locus S. The four apparent intersections of these two curves correspond to the fourstationary values of the distance from a point in the trace T to the origin O1 in the u-v plane. Ofthese four intersections, two are local maxima and two local minima. The normality of the radiusvector from O1 to T at the intersection points is to be highlighted. Now it is a simple matter to pickup 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 resortingto 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
rM =√
(2.132241882+ 1)2 + 1.4689899442a = 3.459605564460a
Alternatively, this problem can be solved exactly at the expense of some extra algebra. From thesecond of eqs.(88), we have
u =1
s2(92)
Moreover, from eq.(90), we can write
v =c2(1 + u)
1 + c2=c2(1 + s2)
s2(1 + c2)(93)
Now, from the first of eqs.(88),2uc2 = u2 − 2v + v2
Substituting the expressions for u and v, eqs.(92) and (93), into the above relation, we obtain
2c2s2
=1
s22− 2
c2(1 + s2)
s2(1 + c2)+c22(1 + s2)
2
s22(1 + c2)2
which can be written as
(2c2s2 − 1)(1 + c2)2 = −2c2s2(1 + s2)(1 + c2) + c22(1 + s2)
2
71
Upon expansion, this equation can be recast in the form
4c2s2 − 1 + 4c22s2 + c22s22 + 2c32s2 + 2c2s
22 − 2c2 − 2c22 = 0 (94)
Now, introducing two well-known trigonometric identities, namely,
c2 ≡1− τ221 + τ22
, s2 ≡2τ2
1 + τ22, where τ2 ≡ tan(
θ22)
into eq.(94), we obtain, after some algebra, an octic equation in τ2, i.e.,
τ82 + 4τ72 + 4τ62 + 4τ52 + 10τ42 + 12τ32 − 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, (v)7 = 0.3663245289
(u)8 = 2.132241882, (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 ofrM = 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 ofthe 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 charac-teristic equation, resulting from det(M) = 0, is thus obtained as
(−C2F 2 − 2C2G2 − 2A2H2 − 2H2B2 + 2C2GF + 2CAFH + 4CGBH
−2CFBH) cos2 θ3 +(2A2 + 2B2 − 2D2G2 −D2F 2 + 4 IDGB − 2 IDFB
+ 2 IADF + 2D2GF)sin2 θ3 + (2 ICAF + 4 ICGB + 4CGFD + 4DGBH
−2DFBH + 2ADFH − 2 ICFB − 4CG2D − 4 IHB2 − 2CDF 2 − 4 IA2H)cos θ3 sin θ3
+(−4CG2E − CBF 2 − 2A2FH − 4A2HJ − 2HB2F +ACF 2 − 4HB2J − 2CEF 2
72
+2CGBF − 2CFBJ + 2CAFJ + 4CGBJ + 4CGFE + 2AEFH + 4EGBH
−2EFBH) cos θ3 +(−4DG2E − 2DEF 2 −DBF 2 +ADF 2 − 4 IA2J − 4 IB2J
−2 IEFB − 2 IB2F + 2 IAEF + 2DGBF + 4DGBJ − 2DFBJ + 4DGFE
+2ADFJ +4 IEGB − 2 IA2F)sin θ3 − 2A2FJ − 2GFA2 + 2GFE2 + AEF 2
+ABF 2 + 2AEFJ − 2AGBF − 2EFBJ + 2EGBF + 4EGBJ − 2A2J2
+2G2A2 − E2F 2 − 2G2E2 − 2B2J2 − EBF 2 − 2 JB2F = 0
This equation is obviously quadratic in cos θ3 and sin θ3, as expected. Moreover, using the identities
cos θ3 =1− t231 + t23
, sin θ3 =2t3
1 + t23, t3 = tan
(θ32
)
a quartic equation in t3 is obtained, namely,
a4t43 + a3t
33 + a2t
23 + a1t1 + a0 = 0
where
a0 = − 2A2FJ − 2GFA2 + 2GFE2 +AEF 2 +ABF 2 + 2AEFJ − 2AGBF
−2EFBJ + 2EGBF + 4EGBJ + 2C2GF − 2A2J2 + 2G2A2 − E2F 2 − 4CG2E
−CBF 2 − 2C2G2 − 2A2FH − 4A2HJ − 2HB2F +ACF 2 − 4HB2J − 2CEF 2
−C2F 2 − 2A2H2 − 2H2B2 + 2CAFH + 4CGBH − 2CFBH − 2G2E2 − 2B2J2
−EBF 2 − 2 JB2F + 2CGBF − 2CFBJ + 2CAFJ + 4CGBJ + 4CGFE
+2AEFH + 4EGBH − 2EFBH
a1 = −8 IA2J − 4 IA2F − 4 IEFB + 8 IEGB − 4 IB2F − 8DG2E − 4DEF 2 − 2DBF 2
+2ADF 2 + 4DGBF + 8DGBJ − 4DFBJ + 8DGFE + 4ADFJ − 8 IB2J
−4 ICFB + 4 IACF + 8 ICGB + 4 IAEF + 8CGFD + 8DGBH − 4DFBH
+4ADFH − 8 IHB2 − 8 IA2H − 8CG2D − 4CDF 2
a2 = − 4A2FJ − 4GFA2 + 4GFE2 + 2AEF 2 + 2ABF 2 + 4AEFJ − 4AGBF − 4EFBJ
+4EGBF + 8EGBJ + 8A2 + 8 IADF + 16 IDGB − 4C2GF − 8 IDFB − 4A2J2
+4G2A2 − 2E2F 2 + 8B2 + 8D2GF − 8D2G2 − 4D2F 2 + 2C2F 2 + 4C2G2 + 4A2H2
+4H2B2 − 4CAFH − 8CGBH + 4CFBH − 4G2E2 − 4B2J2 − 2EBF 2 − 4 JB2F
a3 = −8 IA2J − 4 IA2F + 8 IA2H + 8 IHB2 − 4 IEFB + 8 IEGB − 4 IACF
− 8 ICGB − 4 IB2F − 8DG2E − 4DEF 2 − 2DBF 2 + 2ADF 2 + 4DGBF
+8DGBJ − 4DFBJ + 8DGFE + 4ADFJ − 8 IB2J + 4 ICFB + 4 IAEF
− 8CGFD− 8DGBH + 4DFBH − 4ADFH + 8CG2D + 4CDF 2
a4 = −2A2FJ − 2GFA2 + 2GFE2 +AEF 2 +ABF 2 + 2AEFJ − 2AGBF − 2EFBJ
+2EGBF + 4EGBJ + 2C2GF − 2A2J2 + 2G2A2 − E2F 2 + 4CG2E + CBF 2
+2A2FH + 4A2HJ + 2HB2F −ACF 2 + 4HB2J + 2CEF 2 − C2F 2 − 2C2G2
− 2A2H2 − 2H2B2 + 2CAFH + 4CGBH − 2CFBH − 2G2E2 − 2B2J2 − EBF 2
− 2B2J2 − EBF 2 − 2 JB2F − 2CGBF + 2CFBJ − 2CAFJ − 4CGBJ − 4CGFE
− 2AEFH − 4EGBH + 2EFBH
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 figuresabout the same axis. Hence, for α in radian,
q = R2α2R sinα
3α− rb3
2b33
=2
3rR2 − 2
3rb23 =
2
3r(R2 − b23) =
2
3r3
Therefore,
V = 2πq =4πr3
3and hence, the value of the workspace volume of the Puma robot is that of a sphere of radius r =√
R2 − b23.5.14 We have
J = [ e1 e2 e3 ]
which is a square matrix, and hence,
µ ≡√
det(JJT ) = |det(J)|
i.e.,µ = |e2 × e3 · e1| = |e3 × e1 · e2| = |e1 × e2 · e3|
Since ei31 are unit vectors, we have
|e2 × e3 · e1| = ‖e1‖‖e2 × e3‖| cosφ| = ‖e2 × e3‖| cosφ|where φ is the angle between e1 and e2× e3. Furthermore, if α2 denotes the angle between e2 and e3,
‖e2 × e3‖ = ‖e2‖‖e3‖| 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, e3are mutually orthogonal, the maximum value of µ being 1.
74
5.15 We haveJ = [ e4 e5 e6 ]
where, expressing all vectors in F5,
[e4]5 = QT4 [e4]4 , [e5]5 =
001
, [e6]5 = Q5 [e6]6
Thus,
[e4]5 =
0µ4
λ4
, [e5]5 =
001
, [e6]5 =
µ5s5−µ5c5λ5
the Jacobian thus becoming
J ← [J]5 =
0 0 µ5s5µ4 0 −µ5c5λ4 1 λ5
In the sequel, we will need an expression for J−1:
J−1 =1
µ4µ5s5
µ5c5 µ5s5 0−(µ4λ5 + µ5λ4c5) −µ5λ4s5 µ4µ5s5
µ4 0 0
Now, we compute ‖J‖ and ‖J−1‖, using the weighted Frobenius norm
‖J‖F =√
tr(JWJT ), W ≡(1
3
)
1
Hence,
‖J‖F =
√
1
3
√
tr(JJT ), ‖J−1‖F =
√
1
3
√
tr(J−1J−T )
and −T denotes the transpose of the inverse or, equivalently, the inverse of the transpose. We thushave
κ2F =µ24 + µ2
5 + (µ4λ5c5 + µ5λ4)2 + µ2
4s25
3µ24µ
25s
25
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 equationsin three unknowns, derived from
f4(α4, α5, θ5) ≡∂κ2
∂α4= 0, f5(α4, α5, θ5) ≡
∂κ2
∂α5= 0, f6(α4, α5, θ5) ≡
∂κ2
∂θ5= 0
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 = ±π2, α5 = ±π
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 resultthat should have been expected.
75
5.16 First and foremost, we recall the formulas for the determinant of a block matrix (CRC StandardMathematical Tables, 1987) A, given as
A =
[B CD E
]
The formulas are
det(A) = det(E) det(B−CE−1D) = det(B) det(E−DB−1C)
On the other hand, the Jacobian can be written as
J =
[J11 J12
J21 O3×3
]
where O3×3 is the 3× 3 zero matrix. Hence, applying the second of the above formulas,
det
([J11 J12
J21 O3×3
])
= det(J11) det(O3×3 − J21J−111 J12)
and hence,det(J) = −det(J21J12)
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(J21J12)| = | − det(J21J12)|
Therefore,µ = |det(J21)| · |det(J12)|
and hence, µ = µaµw, q.e.d.
5.17 The Jacobian for a 2R planar manipulator is
J = a1
[−s1 − rs12 −rs12c1 + rc12 rc12
]
wheres1 = sin θ1, s12 = sin(θ1 + θ2)
c1 = cos θ1, c12 = cos(θ1 + θ2)
r =a2a1
The inverse of J is, hence,
J−1 =1
a1rs2
[rc2 rs2
−(1 + rc2) −rs2
]
and the condition number is computed using the Frobenius norm:
‖J‖F =√
tr(JWJT ) =a1√1 + 2r2 + 2rc2√
2, W =
(1
2
)
1
Therefore,
‖J−1‖F =√
tr(J−1WJ−T ) =
√1 + 2r2 + 2rc2√
2 a1rs2
76
Then,
κF =1 + 2r2 + 2rc2
2rs2We now solve
κF → minr,θ2
which we do upon zeroing the partial derivatives of κ with respect to r and θ2, namely,
∂κF∂r
= 0 ⇒ 2r2 − 1 = 0 (96)
∂κF∂θ2
= 0 ⇒ 2r + (2r2 + 1)c2 = 0 (97)
From eq.(96), r = ±√2/2, where we reject the negative sign, which has no geometrical meaning. With
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 hollowsphere, its volume VA and reach RA being
VA =4
3πl3[(
√2 + 1)3 − (
√2− 1)3] =
56
3πl3 (98)
RA = (√2 + 1)l (99)
For a similar orthogonal manipulator, with identical link lengths λ, the corresponding volume VB andreach RB are
VB =4
3π(2λ3) =
32
3πλ3
RB = 2λ
Now, since the two manipulators have the same reach,
2λ = (√2 + 1)l ⇒ λ =
√2 + 1
2l ⇒ VA
VB= 0.9949
The Jacobian matrix of the second manipulator takes the form
J = [ e1 × r1 e2 × r2 e3 × r3 ]
where
e1 =
001
, e2 = e3 =
s1−c10
and
r1 = λ
(c2 + c23)c1(c2 + c23)s1s2 + s23
, r2 = r1, r3 = λ
c23c1c23s1s23
.
Since θ1 does not affect the condition number of the Jacobian, we can fix θ1 to any value, and so, weset θ1 = 0, the Jacobian thus becoming
J = λ
0 −(s2 + s23) −s23c2 + c23 0 0
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 onthe Frobenius norm, we can use the Matlab built-in function fminsearch. The results reported byMatlab 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 onlyslightly bigger than that of its isotropic counterpart, for the same reach. The smaller volume of theisotropic 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
2τ1s
′max + (τ2 − τ1)s′max +
1
2(1− τ2)s′max =
1
2(1− τ1 + τ2)s
′max
We need then A = 1, and thus1
2(1− τ1 + τ2)s
′max = 1
from which we obtain
s′max =2
1− τ1 + τ2(100)
78
(b) Using eq.(100), we have
s(τ) =
τ2
τ1(1− τ1 + τ2), 0 ≤ τ ≤ τ1
2τ − τ11− τ1 + τ2
, τ1 ≤ τ ≤ τ2
τ2 + τ22 − 2τ − τ1(τ2 − 1)
(τ2 − 1)(1− τ1 + τ2), τ2 ≤ τ ≤ 1
The plot of s(τ) vs. τ appears in Fig. 21(a). The decomposition of s(τ) into a linear part and a
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.2
0.4
0.6
0.8
1(a) Position profile s(tau)
tau
s(ta
u)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.1
−0.05
0
0.05(b) Periodic part of the position profile
tau
s_p(
tau)
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′N (101b)
s′′1 = s′′N (101c)
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 thedefinitions of eqs.(6.58d–f), we have now the system
As′′ = 6Cs (102)
79
where A and C are (N − 1)× (N − 1) matrices defined as:
A =
2α1,N ′ α1 0 0 · · · αN ′
α1 2α1,2 α2 0 · · · 00 α2 2α2,3 α3 · · · 0...
.... . .
. . .. . .
...0 0 . . . αN ′′′ 2αN ′,′′N ′′ αN ′′
αN ′ 0 0 · · · αN ′′ 2αN,′′N ′
C =
−β1,N ′ β1 0 0 · · · βN ′
β1 −β1,2 β2 0 · · · 00 β2 −β2,3 β3 · · · 0...
.... . .
. . .. . .
...0 0 · · · βN ′′′ −βN ′,′′N ′′ βN ′′
βN ′ 0 0 · · · βN ′′ −βN,′′N ′
ands = [ s1, · · · , sN−1 ]
T, s′′ =
[s′′1 , · · · , s′′N−1
]T
Moreover,
∆xk =1
N − 1, k = 1, · · · , N − 1
Thus, for i, j, k = 1, . . . , N − 1,
αk =1
N − 1, αi,j =
2
N − 1(103)
βk = N − 1, βi,j = 2(N − 1) (104)
and matrices A and C reduce to
A =1
N − 1
4 1 0 0 · · · 11 4 1 0 · · · 00 1 4 1 · · · 0...
.... . .
. . .. . .
...0 0 . . . 1 4 11 0 0 · · · 1 4
C = (N − 1)
−2 1 0 0 · · · 11 −2 1 0 · · · 00 1 −2 1 · · · 0...
.... . .
. . .. . .
...0 0 · · · 1 −2 11 0 0 · · · 1 −2
Now, vector s = [ s1, · · · , sN−1 ]T
is readily known from the N equally spaced points, while thevector s′′ of eq.(102) is obtained as
s′′ = 6A−1Cs
Then, the coefficient Ak, Bk, Ck and Dk of each cubic spline, for k = 1, . . . , N − 1, are obtaineddirectly using eqs.(6.55a–d). TheMatlab code implementing the foregoing calculations is displayedbelow:
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
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.1
−0.05
0
0.05(a) Periodic cubic spline using 8 supporting points
tau
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−10
−5
0
5
10(b) Acceleration profile when using 8 supporting points
tau
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 goodapproximation of the original profile s(τ), while smoothing its acceleration profile. Moreover, themaximum acceleration value, about eight, is only slightly higher than the original accelerationlevel, which is about six, and thus, seems quite reasonable. Also note that, as a matter ofcomparison, 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, asplotted in Fig. 23. As in Problem 6.1, we consider the system of equations of eqs.(6.58a–c). What wewant is to obtain vector s using eq.(6.58a) in the form
s =1
6C−1As′′ (105)
where vector s′′ contains the sampled values of the equally spaced supporting points of the accelerationprofile of Fig. 23. Note that for this profile to be periodic, we must have s′′(0−) = s′′(1+) = 0. Here, wedenote by superscripts (−) and (+) the instant “just before” and “just after,” in order to accommodatethe jump discontinuities. Moreover, in order to determine uniquely the displacement program, we mustgive the initial displacement of s(τ) since the acceleration profile contains no information about it. Wethus set s(0) = 0, and since the acceleration profile s′′ does not contain information on the linear partof 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
C =
−β1,2 β2 0 · · · 0β2 −β2,3 β3 · · · 0...
. . .. . .
. . ....
0 · · · βN ′′′ −βN ′,N ′′ βN ′′
0 0 · · · β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,
αk =1
N − 1, αi,j =
2
N − 1(106)
βk = N − 1, βi,j = 2(N − 1) (107)
matrices A and C reducing to
A =1
N − 1
1 4 1 0 · · · 00 1 4 1 · · · 0...
.... . .
. . .. . .
...0 · · · 1 4 1 00 0 · · · 1 4 1
, C = (N − 1)
−2 1 0 · · · 01 −2 1 · · · 0...
. . .. . .
. . ....
0 · · · 1 −2 10 0 · · · 1 −2
Using eq.(105), s is readily obtained and the coefficients Ak, Bk, Ck and Dk of the cubic spline, fork = 1, . . . , N − 1, are computed directly using eqs.(6.55a–d). The Matlab code implementing theforegoing 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
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−0.1
−0.05
0
0.05(a) Periodic cubic spline using 25 supporting points
tau
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−15
−10
−5
0
5
10(b) Acceleration profile when using 25 supporting points
tau
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 theacceleration profile (τ2 ≤ τ ≤ 1) is not well approximated. Moreover, this number of supporting pointsgives a good approximation of the original profile s(τ), while smoothing its acceleration profile, asexpected.
6.3 Here we want to use cycloidal motions to smooth the joint-rate profile of Fig. 6.7 of the text. Tothis end, we define a segment of a cycloidal-motion function between u = 0 and u = u1 such thats′1(u1) = s′max. We have, from eqs.(6.38a & b) of the text,
s(u) = A(u − 1
2πsin 2πu)
s′(u) = A(1 − cos 2πu), for 0 ≤ u ≤ 1
as depicted in Fig. 25(a). From this figure, it is clear that the slope is horizontal, as required, whenu = 1/2. Since we want to obtain this point at τ = τ1, a change of variable is needed to shrink theplot in the horizontal direction, as shown in Fig. 25(b). This is done by defining u as u = τ/(2τ1), andhence
s(τ) =A
2
[τ
τ1− 1
πsin
(
πτ
τ1
)]
(108)
s′(τ) =A
2τ1
[
1− cos
(
πτ
τ1
)]
(109)
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
2τ1(1− cosπ) =
A
τ1= s′max
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, for τ1 ≤ τ ≤ τ2 (110)
86
and hence,s(τ) = s′maxτ + C1 (111)
where C1 is a constant of integration which is determined by noticing, from eq.(108) that s(τ) = s′max/2,and plugging this value into eq.(111), thus obtaining
C1 = −1
2s′maxτ1
Hence,
s(τ) = s′max
(
τ − 1
2τ1
)
, τ1 ≤ τ ≤ τ2 (112)
Finally, for τ2 ≤ τ ≤ 1, we define the second cycloidal segment (Fig. 26), where s′(τ2) = s′max ands′(1) = 0. Here, we consider the cycloidal function
s(v) = B
(
v − 1
2πsin 2πv
)
(113)
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 andshrink 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 relation1− τ ′ = 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τ22(1− τ2)
, β =1
2(1− τ2)
and hence,
v =1− 2τ2 + τ
2(1− τ2)Therefore,
s′(v) = B
[
1− cos
(
π1− 2τ2 + τ
1− τ2
)]
where B is a constant that can be determined using
s′(τ2) = B(1 − cosπ) = 2B = s′max ⇒ B =s′max
2
Hence
s′(τ) =s′max
2
[
1− cos
(
π1 − 2τ2 + τ
1− τ2
)]
(114)
Now, if we substitute the foregoing expression for B and v into eq.(113), we obtain
s(τ) =s′max
2
(
τ2 −1
2τ1
)
+s′max
2
[
τ − 1− τ2π
sin
(1− 2τ2 + τ
1− τ2
)]
87
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.5
1displacement profile
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.5
1
1.5velocity profile
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1−20
−10
0
10acceleration profile
Figure 27: Cycloidal approximation of the trapezoidal profile of Fig. 13, for τ1 = 0.2, τ2 = 0.9, ands′max = 2/(1− τ1 + τ2)
In summary, we have
s(τ) =
s′max
2
[
τ − τ1π
sin
(
πτ
τ1
)]
, 0 ≤ τ ≤ τ1;
s′max
(
τ − 1
2τ1
)
, τ1 ≤ τ ≤ τ2;
s′max
2
(
τ2 −1
2τ1
)
+s′max
2
[
τ − 1− τ2π
sin
(1− 2τ2 + τ
1− τ2
)]
, τ2 ≤ τ ≤ 1.
s′(τ) =
s′max
2
[
1− cos
(
πτ
τ1
)]
, 0 ≤ τ ≤ τ1;
s′max , τ1 ≤ τ ≤ τ2;s′max
2
[
1− cos
(
π1− 2τ2 + τ
1− τ2
)]
, τ2 ≤ τ ≤ 1.
s′′(τ) =
πs′max
2τ1sin
(
πτ
τ1
)
, 0 ≤ τ ≤ τ1;
0 , τ1 ≤ τ ≤ τ2πs′max
2(1− τ2)sin
(
πτ + 1− 2τ2
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 , p(0) = 0, p(0) = 0
88
Q(0) = QI , ω(0) = 0, ω(0) = 0
p(T ) = pF , p(T ) = pF , p(T ) = 0
Q(T ) = QF , ω(T ) = 0, ω(T ) = 0
In the absence of singularities, these conditions correspond to
θ(0) = θI , θ(0) = 0, θ(0) = 0 (115a)
θ(T ) = θF , θ(T ) = θF , θ(T ) = 0 (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 θFi and θFi , we have, from the conditionsof 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,
aT 2 + bT + c =θFi − θIiT 3
5aT 2 + 4bT + 3c =θFiT 2
20aT 2 + 12bT + 6c = 0
Upon solving the three foregoing equations for the three unknowns, we obtain
a =6
T 5(θFi − θIi )−
3
T 4θFi
b = − 15
T 4(θFi − θIi ) +
7
T 3θFi
c =10
T 3(θFi − θIi )−
4
T 2θFi
and hence, the fifth-degree polynomial sought is
θi(t) = θIi +
[6
T 5(θFi − θIi )−
3
T 4θFi
]
t5 +
[
− 15
T 4(θFi − θIi ) +
7
T 3θFi
]
t4 +
[10
T 3(θFi − θIi )−
4
T 2θFi
]
t3
To obtain the seventh-degree polynomial, we impose the condition that the joint jerk vanishes at thetwo endpoints of the motion, namely,
θ(3)(0) = 0, θ(3)(T ) = 0 (116)
The polynomial has the form
θ(t) = at7 + bt6 + ct5 + dt4 + et3 + ft2 + gt+ h
89
and its three first time-derivatives are given by
θ(t) = 7at6 + 6bt5 + 5ct4 + 4dt3 + 3et2 + 2ft+ 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,
aT 3 + bT 2 + cT + d =θFi − θIiT 4
7aT 3 + 6bT 2 + 5cT + 4d =θFiT 3
42aT 3 + 30bT 2 + 20cT + 12d = 0
210aT 3 + 120bT 2 + 60cT + 24d = 0
Upon solving the four foregoing equations for their four unknowns, we obtain
a = − 20
T 7(θFi − θIi ) +
10
T 6θFi
b =70
T 6(θFi − θIi )−
34
T 5θFi
c = − 84
T 5(θFi − θIi ) +
39
T 4θFi
d =35
T 4(θFi − θIi )−
15
T 3θFi
and hence, the seventh-degree polynomial sought is
θi(t) = θIi +
[
− 20
T 7(θFi − θIi ) +
10
T 6θFi
]
t7 +
[70
T 6(θFi − θIi )−
34
T 5θFi
]
t6
+
[
− 84
T 5(θFi − θIi ) +
39
T 4θFi
]
t5 +
[35
T 4(θFi − θIi )−
15
T 3θFi
]
t4
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 candecompose 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 ,s′
1 = s′
N , s′′
1 = s′′
N . The difference is that here we have N intervals, N + 1 points, and thus, matricesA and C are of N ×N , while
s = [s1, ..., sN ]T , s′′ = [s′′1 , ..., s′′N ]T . (117)
Moreover,
∆xk =1
N, k = 1, ..., N, (118)
and for i, j, k = 1, ..., N ,
αk =1
N, αi,j =
2
N, (119)
90
βk = N, βi,j = 2N. (120)
With the above coefficients we can obtain the A and C matrices that, for periodic splines, are givenin eq.(9.59a & b). Solving for s′′ in terms of s from eq.(6.58a), we obtain
s′′ = 6A−1Cs. (121)
With the foregoing calculations we have all the necessary information (s′′ and s) to find the coefficientsof 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 themaximum absolute value of the error is attained at the midpoint of the interval. Note that a rigorouscalculation of this maximum can be performed with a suitable optimization routine, that would use abisection 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 , isincluded below:
function e = error(N)
% Definitions:
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); % subinterval step vector
for i = 1 : length(t) - 1, % recursion from 1 to N
% 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); % closing the cycle: s(N+1)
sp(N+1) = sp(1); % s’(N+1)
spp(N+1) = spp(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 10 20 30 40 50 60 70 80 90 100
eN 0.0485 0.0124 0.0055 0.0031 0.0020 0.0014 0.0010 0.0008 0.0006 0.0005e′N 1.0171 1.0126 1.0105 1.0100 1.0122 1.0126 1.0123 1.0119 1.0125 1.0126e′′N 36.947 38.855 39.206 39.328 39.384 39.414 39.432 39.444 39.452 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 exhibitinga 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 polynomialand 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 ); % subinterval endpoints
te = ( D2 : Delta : 1 - D2 ); % subinterval midpoints
for i = 1 : length(t) - 1, % 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); % closing the cycle: s
sp(N+1) = sp(1); % s’
spp(N+1) = spp(1); % 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 weinclude the Matlab code that estimates errors and numbers of subintervals required to attain thedesired 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 15 eN 8.7347e-05N 60 e′N 8.9181e-05N 960 e′′N 9.5397e-05
7 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
v, the body moving with an angular velocity ω: t = [ωT ,vT ]T. The 6× 6 angular-velocity dyad
W is also recalled:
W =
[Ω OO O
]
(126)
and hence,
Wt =
[Ωω
0
]
(127)
96
Given that Ω= CPM (ω), Ωω = ω × ω = 0. Therefore, t lies in the null space of W. Now, forthe case of a mechanical system composed of n rigid bodies, let ωi, ti, Ωi and Wi denote theforegoing quantities as pertaining to the ith body.
Further, with the notation introduced in this chapter,
Wt =
W1 O · · · OO W2 · · · O...
.... . .
...O O · · · Wn
t1t2...tn
=
W1t1W2t2
...Wntn
=
00...0
(128)
q.e.d.
(b) The 6n × 6n matrix M for the system introduced in (a) is M = diag(M1,M2, · · · ,Mn), andhence,
M =
M1 O · · · OO M2 · · · O...
.... . .
...O O · · · Mn
(129)
Now, the result of Exercise 3.17 is recalled, as applied to the ith body of the system:
Mi = WiMi −MiWi (130)
When the above expression is substituted into the right-hand side of eq. (129), one obtains two6n× 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 denotesthe momentum screw of the ith rigid body of the system at hand, then the 6n-dimensional systemmomentum screw µ is defined as
µ = Mt (131)
where M is the 6n× 6n system inertia dyad and t is the 6n-dimensional system twist. Therefore,
µ = Mt+Mt (132)
with M having been obtained in (b) above.
Therefore, the above expression becomes
µ = (WM −MW)t+Mt = WMt−MWt+Mt (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 performthe desired maneuver, we must first obtain the joint rates and accelerations. Since the velocity andacceleration of the wrist center C are given, as well as the angular velocity and acceleration of theend-effector, the joint rates and accelerations are readily obtained using the Jacobian matrix. For adecoupled manipulator, we have
J11θa + J12θw = ω (134a)
J21θa = c (134b)
with matrices J11, J12 and J21 given by
J11 = [ e1 e2 e3 ] , J12 = [ e4 e5 e6 ] , J21 = [ e1 × r1 e2 × r2 e3 × r3 ]
97
Figure 28:
and ri is defined as the vector directed from Oi to C. Referring to Fig. 28, and expressing all quantitiesin frame 1, we have
e1 =
001
, e2 = e3 =
010
, e4 =
001
, e5 =
010
, e6 =
100
,
r1 = r2 =
101
, r3 =
001
,
a1 = a3 = a5 = 0, a2 =
100
, a4 =
001
, a6 =
a00
Thus,
e1 × r1 =
010
, e2 × r2 =
10−1
, e3 × r3 =
100
Therefore,
J11 =
0 0 00 1 11 0 0
, J12 =
0 0 10 1 01 0 0
, J21 =
0 1 11 0 00 −1 0
Hence,
J−112 =
0 0 10 1 01 0 0
, J−121 =
0 1 00 0 −11 0 1
From eq.(134b), we have
θa = J−121 c =
0 1 00 0 −11 0 1
101
=
0−12
(135)
98
and, from eq.(134a),
θw = J−112
(
ω − J11θa
)
=
0 0 10 1 01 0 0
010
−
0 0 00 1 11 0 0
0−12
=
0 0 10 1 01 0 0
000
=
000
We therefore have,θ = [ 0 −1 2 0 0 0 ]
Trad/s
Moreover, differentiating eqs.(134a & b), we obtain
J11θa + J11θa + J12θw + J12θw = ω (136a)
J21θa + J21θa = c (136b)
Thereby obtaining
θw = J−112
(
ω − J11θa − J11θa − J12θw
)
(137a)
θa = J−121
(
c− J21θa
)
(137b)
The time-derivatives of the blocks of J are
J11 = [ e1 e2 e3 ] , J12 = [ e4 e5 e6 ] , J21 = [ u1 u2 u3 ]
whereei = ωi−1 × ei, i = 1, 2, · · · , 6
andui = ei × ri + ei × ri, i = 1, 2, 3
Moreover,
r1 = r2 = ω2 × a2 + ω3 × a3
r3 = ω3 × a3
Considering the joint rates obtained above, we have
ω0 = 0
ω1 = θ1e1 = 0
ω2 = θ1e1 + θ2e2 = [ 0 −1 0 ]T
ω3 = ω4 = ω5 = ω6 = θ1e1 + θ2e2 + θ3e3 = [ 0 1 0 ]T
Therefore,
e1 = e2 = e3 = e5 = 0
e4 = [ 1 0 0 ]T
e6 = [ 0 0 −1 ]T
r1 = r2 = [ 1 0 1 ]T
r3 = [ 1 0 0 ]
99
and
u1 = [ 0 1 0 ]T
u2 = [ 1 0 −1 ]T
u3 = [ 0 0 −1 ]T
Thus,
J11 = 0, J12 =
1 0 00 0 00 0 −1
, J21 =
0 1 01 0 00 −1 −1
Therefore, from eq.(137b), we have
θa =
0 1 00 0 −11 0 1
010
−
0 1 01 0 00 −1 −1
0−12
=
0 1 00 0 −11 0 1
111
=
1−12
and from eq.(137a), we have
θw =
0 0 10 1 01 0 0
101
−
0 0 00 1 11 0 0
1−12
=
0 0 10 1 01 0 0
1−10
=
0−11
Therefore,θ = [ 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 theunit 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
P1 =
−1 0 00 0 10 1 0
, P2 =
1 0 00 0 10 −1 0
, P3 =
1 0 00 1 00 0 1
,
P4 =
−1 0 00 0 10 1 0
P5 =
0 0 10 1 0−1 0 0
, P6 =
0 1 01 0 00 0 −1
The position vectors of the mass centers are given as [ρi]i+1, for i = 1, 2, · · · , 6, their correspondingrepresentation 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 coordinatesusing the similarity transformation
[Ii]1 = Pi[Ii]i+1PTi , 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 = c0 = 0
and, in order to incorporate gravity, we consider
c0 = −g = 9.806k
as in eq.(7.44). Therefore, applying Algorithm 7.4.1, we obtain, recursively,
c1 = −0.054k, ω1 = u0 = v1 = c1 = 0, ω1 = k, c1 = 9.806k
c2 = 0.140i, ω2 = −j, u1 = 0, v2 = 0.140k,
c2 = 0.140k, ω2 = −j+ k, c2 = −0.140i+ 0.140j+ 9.946k
c3 = i− 0.197j, ω3 = j, u2 = 0.860k, v3 = 0,
c3 = k, ω3 = j+ k, c3 = −0.803i+ j+ 10.806k
c4 = i− 0.057j, ω4 = j, u3 = v4 = 0,
c4 = k, ω4 = j+ k, c4 = −0.943i+ j+ 10.806k
c5 = i− 0.007j+ k, ω5 = j, u4 = i, v5 = 0,
c5 = i+ k, ω5 = k, c5 = 0.007i+ j+ 9.806k
c6 = i+ 1.019k, ω6 = j, u5 = 0, v6 = 0.019i,
c6 = 1.019i+ k, ω6 = i+ k, c6 = 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, n5 = −0.0717i+ 0.0877k
f4 = −0.9847i+ 2.4483j+ 25.1191k, n4 = −2.1160i+ 0.1347j+ 0.2121k
f3 = −8.0246i+ 11.2153j+ 119.8553k, n3 = −20.7790i+ 0.4356j+ 2.2020k
f2 = −10.2339i+ 13.4247j+ 276.8131k, n2 = −20.7790i− 149.6610j+ 21.8050k
f1 = −10.2339i+ 13.4247j+ 379.9820k, n1 = −20.7790i− 149.6610j+ 22.3141k
Therefore,τ = [ 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 and2, 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 (c2 − c1) = 0 ⇒ (c2 − c1)T c1 − (c2 − c1)
T c2 = 0 (139)
From eqs.(138) and (139), we obtain
[r1 0T
0 (c2 − c1)T
] [
θ1c1
]
+
[−r2 0T
0 −(c2 − c1)T
] [
θ2c2
]
= 0
where 0 is the 2-dimensional zero vector. Hence,
A =
[r1 0T
0 (c2 − c1)T
]
, B =
[−r2 0T
0 −(c2 − c1)T
]
which are both 2× 3 matrices.
(b) With reference to Fig. 29,
ω1 = ψi, ω2v = θv, ‖i‖ = ‖v‖ = 1
The two gears are constrained to move such that 2 rolls onto 1 without slipping, about the commonelement, parallel to the unit vector e, i.e.,
E(ω2 − ω1) = 0, E = CPM(e) (140)
Furthermore, if we let ci =−−→OCi, for i = 1, 2, then
c2 = c1 + b1 + r2 (141)
Upon differentiation of eq.(141), we derive
c2 = c1 + b1 + r2
whereb1 = ω1 × b1, r2 = (ω2 − ω1)× r2
Hence, with B1 and R2 defined as the CPMs of b1 and r2, respectively,
c2 = c1 −B1ω1 +R2(ω1 − ω2)
orc1 −B1ω1 − c2 +R2(ω1 − ω2) = 0 (142)
Combining eqs.(140) and (142), we obtain
[E O
R2 −B1 1
] [ω1
c1
]
+
[−E O−R2 −1
] [ω2
c2
]
= 06
where O and 1 are the 3× 3 zero and identity matrices, respectively. Hence,
A =
[E O
R2 −B1 1
]
, B =
[−E O−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
c2 = θEq+ (φ− θ)Er2 , E ≡[
0 −11 0
]
(143)
Now, body 2 is constrained to move with respect to body 1 in such a way that the relative velocityof point Q in 2 with respect to that of point Q in 1 is tangential to the cam profile, i.e.,
nT (q2 − q1) = 0 (144)
n =q− c1a
(145)
where qi is the velocity vector of point Q in element i, i = 1, 2 and q1 = q2 = q. Substitutingeq.(145) into eq.(144), we obtain
(q− c1)T (q2 − q1) = 0 (146)
Moreover,
q1 = c1 + θE(q− c1), (147a)
q2 = c2 + φE(q− c2) ≡ c2 − φEr2 (147b)
⇒ q2 − q1 = c2 − c1 − φEr2 − θE(q− c1) (147c)
Substituting eq.(147c) into eq.(146), we obtain
(q− c1)T [c2 − c1 − φEr2 − θE(q− c1)] = (q− c1)
T (c2 − c1 − φEr2) = 0 (148)
⇒ (q − c1)T c1 − (q− c1)
T c2 − (q− c1)T φEr2 = 0
103
Figure 30: A cam with a flat-face follower.
⇒[0 (q − c1)
T][
θc1
]
+[−(q− c1)
TEr2 −(q− c1)T][
φc2
]
= 0
Hence,A =
[0 (q− c1)
T], B = −
[(q− c1)
TE(q− c2) (q − c1)T]
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 isI = TTMT, (149)
with the expressions for T and M, and entries of I given in Example 7.6.1. The time derivative of Iis, for the planar case, simply,
I = TTMT+TTMT, (150)
which we can write asI = TTMT+ (TTMT)T . (151)
Now we have already the entries µij of TTMT in Example 7.6.1. Hence, if we denote by Iij the entries
of I, thenIij = µij + µji, (152)
with all µij entries displayed in p. 296. Notice that C = TTMT and, hence,
D ≡ I− 2C = TTMT+TTMT− 2TTMT = TTMT−TTMT
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 Tiof link i is given by
Ti =1
2ωT
i Iiωi
104
where Ii is the moment-of-inertia matrix of link i with respect to O. Since the inertia matricesare 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
Q4 =
cos θ4 0 sin θ4sin θ4 0 − cos θ40 1 0
, Q5 =
cos θ5 0 sin θ5sin θ5 0 − cos θ50 1 0
, Q6 =
cos θ6 − sin θ6 0sin θ6 cos θ6 00 0 1
Thus,[ω4 ]5 = QT
4 θ4[ e4 ]4 = [ 0 θ4 0 ]T
[ω5 ]6 = QT5
(
[ω4 ]5 + θ5[ e5 ]5
)
= [ sin θ5θ4 θ5 − cos θ5θ4 ]T
[ω6 ]7 = QT6
(
[ω5 ]6 + θ6[ e6 ]6
)
=
sin θ5 cos θ6θ4 + sin θ6θ5− sin θ5 sin θ6θ4 + cos θ6θ5
− cos θ5θ4 + θ6
Therefore, the kinetic energy T of the manipulator is
T =1
2
6∑
i=4
[ωTi Iiωi ]i+1
After performing the required calculations, we obtain
T =1
2
[J2 + (K1 + L1c
26 + L2s
26)s
25 + (K3 + L3)c
25
]θ24 +
1
2(K2 + L1s
26 + L2c
26)θ
25 +
1
2L3θ
26
+(L1 − L2)s5s6c6θ4θ5 − L3c5θ4θ6
which can be rewritten as
T =1
2θTI(θ)θ
with the inertia matrix of the manipulator, I(θ), given as
I(θ) =
J2 + (K1 + L1c26 + L2s
26)s
25 + (K3 + L3)c
25 (L1 − L2)s5s6c6 −L3c5
(L1 − L2)s5s6c6 K2 + L1s26 + L2c
26 0
−L3c5 0 L3
(b) Using eq.(7.20), the term of Coriolis and centrifugal forces is given by
h(θ, θ) = I(θ, θ)θ − 1
2
[
∂(Iθ)
∂θ
]T
θ
By direct differentiation of I(θ), we have
I(θ) =
I11 I12 I13I12 I22 I23I13 I23 I33
where
I11 = 2(K1 + L1c26 + L2s
26 −K3 − L3)s5c5θ5
+2(L2 − L1)s6c6s25θ6
I12 = (L1 − L2)[c5s6c6θ5 + s5(c26 − s26)θ6]
I13 = L3s5θ5
I22 = 2(L1 − L2)s6c6θ6
I23 = I33 = 0
105
We also have
∂(Iθ)
∂θ=
0 I ′12 I ′130 I ′22 I ′230 I ′32 I ′33
where
I ′12 = 2(K1 + L1c26 + L2s
26 −K3 − L3)s5c5θ4 + (L1 − L2)c5s6c6θ5 + L3s5θ6
I ′13 = 2(L2 − L1)s6c6s25θ4 + (L1 − L2)s5(c
26 − s26)θ5
I ′22 = (L1 − L2)c5s6c6θ4
I ′23 = 2(L1 − L2)s6c6θ5 + (L1 − L2)s5(c26 − s26)θ4
I ′32 = L3s5θ4
I ′33 = 0
Leth(θ, θ) = [h1 h2 h3 ]
T
Then
h1 = (K1 + L1c26 + L2s
26 −K3 − L3)s5c5θ4θ5 + (L2 − L1)s6c6s
25θ4θ6
+1
2(L1 − L2)c5s6c6θ
25 +
1
2(L1 − L2)s5(c
26 − s26)θ5θ6 +
1
2L3s5θ5θ6
h2 =1
2(L1 − L2)c5s6c6θ4θ5 + (L1 − L2)s6c6θ5θ6 +
1
2(L1 − L2)s5(c
26 − s26)θ4θ6
h3 =1
2L3s5θ4θ5
(c) All we need now to obtain the dynamics model, eq.(7.20), is the term stemming from the potentialV and the vector of nonconservative generalized forces, namely,
∂V
∂θ=
0m6ga sin θ5
0
and
φn =∂ΠA
∂θ− ∂∆
∂θ=
τ4τ5τ6
−
b4θ4 + τC4 sign(θ4)b5θ5 + τC5 sign(θ5)b6θ6 + τC6 sign(θ6)
Therefore, the dynamics model of the wrist is
[J2 + (K1 + L1c
26 + L2s
26)s
25 + (K3 + L3)c
25
]θ4 + (L1 − L2)s5s6c6θ5 − L3c5θ6 +
1
2L3s5θ5θ6
+(K1 + L1c26 + L2s
26 −K3 − L3)s5c5θ4θ5 + (L2 − L1)s6c6s
25θ4θ6 +
1
2(L1 − L2)c5s6c6θ
25
+1
2(L1 − L2)s5(c
26 − s26)θ5θ6 + b4θ4 + τC4 sign(θ4) = τ4
(L1 − L2)s5s6c6θ4 + (K2 + L1s26 + L2c
26)θ5 +
1
2(L1 − L2)c5s6c6θ4θ5 + (L1 − L2)s6c6θ5θ6
+1
2(L1 − L2)s5(c
26 − s26)θ4θ6 +m6gas5 + b5θ5 + τC5 sign(θ5) = τ5
− L3c5θ4 + L3θ6 +1
2L3s5θ4θ5 + b6θ6 + τC6 sign(θ6) = τ6
106
7.7 (a) The kinetic energy of link i can be written as
Ti =1
2tTi Miti
whereti = [ωT
i cTi ]T
and
Mi =
[Ii OO mi1
]
Since the inertia matrices are given in link-fixed coordinates, or [ Ii ]i+1, we need [ ti ]i+1. Byinspection, we have
Q1 =
cos θ1 0 sin θ1sin θ1 0 − cos θ10 1 0
, Q2 =
cos θ2 − sin θ2 0sin θ2 cos θ2 00 0 1
Thus,[ω1 ]2 = QT
1 θ1[ e1 ]1 = [ 0 θ1 0 ]T
[ω2 ]3 = QT2
(
[ω1 ]2 + θ2[ e2 ]2
)
= [ sin θ2θ1 cos θ2θ1 θ2 ]T
Moreover,[ c1 ]2 = QT
1 [ c1 ]1 = [ 0 a b2 ]
T
[ c2 ]3 = QT2 [−−−→O1O2 ]2 + [
−−−→O2C2 ]3
=
a sin θ2a cos θ2
0
+
c/20b
=
c/2 + a sin θ2a cos θ2
b
Therefore,[ c1 ]2 = [ω1 × c1 ]2 = [ b
2 θ1 0 0 ]T
[ c2 ]3 = [ω2 × c2 ]3 = [ cos θ2(bθ1 − aθ2) c2 θ2 − sin θ2(bθ1 − aθ2) − c
2 cos θ2θ1 ]
The total kinetic energy is given by
T =1
2tT1 M1t1 +
1
2tT2 M2t2
with
t1 = [ t1 ]2 =
0θ10
b2 θ100
, t2 = [ t2 ]3 =
sin θ2θ1cos θ2θ1θ2
cos θ2(bθ1 − aθ2)c2 θ2 − sin θ2(bθ1 − aθ2)
− c2 cos θ2θ1
(154)
[M1 ]2 =
I11 I12 I13 0 0 0I12 I22 I23 0 0 0I13 I23 I33 0 0 00 0 0 m1 0 00 0 0 0 m1 00 0 0 0 0 m1
107
[M2 ]3 =
J11 J12 J13 0 0 0J12 J22 J23 0 0 0J13 J23 J33 0 0 00 0 0 m2 0 00 0 0 0 m2 00 0 0 0 0 m2
Finally, performing the required calculations, we obtain
T =1
2
(
I22 +m1b
2
4+ J11 sin
2 θ2 + J22 cos2 θ2 + 2J12 sin θ2 cos θ2 +m2b
2 +m2c
2
4cos2 θ2
)
θ21
+1
2
(
J33 +m2a2 +
m2c2
4+m2ac sin θ2
)
θ22 +
(
J13 sin θ2 + J23 cos θ2 −m2ab−1
2m2bc sin θ2
)
θ1θ2
(b) The kinetic energy T can be written as
T =1
2θTI(θ)θ
By inspection, we obtain
I(θ) =
[Z11 Z12
Z12 Z22
]
with
Z11 = I22 +m1b
2
4+ J11 sin
2 θ2 + J22 cos2 θ2 + 2J12 sin θ2 cos θ2 +m2b
2 +m2c
2
4cos2 θ2
Z12 = J13 sin θ2 + J23 cos θ2 −m2b
(
a+1
2c sin θ2
)
Z22 = m2
(
a2 +c2
4+ ac sin θ2
)
+ J33
(c) We have now
I(θ, θ) =
[Z11 Z12
Z12 Z22
]
with
Z11 =
[
2
(
J11 − J22 −m2c2
4
)
sin θ2 cos θ2 + 2J12(cos2 θ2 − sin2 θ2
)]
θ2
Z12 =
(
J13 cos θ2 − J23 sin θ2 −1
2m2bc cos θ2
)
θ2
Z22 = m2ac cos θ2θ2
(d) We have I = TTMT where
M =
[M1 OO M2
]
, T =
[T1
T2
]
and the twist-shaped matrices, taking into account eqs.(154), have the form
T1 =
0 01 00 0b/2 00 00 0
, T2 =
sin θ2 0cos θ2 00 1
b cos θ2 −a cos θ2−b sin θ2 c/2 + a sin θ2−(c/2) cos θ2 0
108
Then, T1 = O6×2 and
T2 =
θ2 cos θ2 0
−θ2 sin θ2 00 0
−bθ2 sin θ2 aθ2 sin θ2−bθ2 cos θ2 aθ2 cos θ2(c/2)θ2 sin θ2 0
while T =
[O
T2
]
Since matrix M is constant, we have
I = TTMT+TTMT
As the reader can readily verify with the help of computer algebra, by performing these calculationswe 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 denotesthe orientation of the body with respect to a line fixed to the plane of motion, we have
ωi = φin, Ωi = φiN
Likewise, the velocity ci of the mass center can be written as
ci = xili + yimi
where li, mi, n is an orthonormal triad defining a frame F fixed to the body.
Therefore, ωi and ci can be expressed, in terms of the joint rates θj n1 , as
ωi = θ1n+ θ2n+ · · ·+ θin
ci = θ1r1 + θ2r2 + · · ·+ θiri
where all vectors ri n1 ⊥ n, i.e., they lie in the plane of motion. Therefore, Ti has the form
Ti =
[n n · · · nr1 r2 · · · ri
]
Moreover,
Wi = φi
[N OO O
]
Therefore, if N is the cross-product matrix of n, then
WTi Ti = φi
[N OO O
] [n n · · · n 0 . . . 0r1 r2 · · · ri 0 . . . 0
]
= φi
[Nn Nn · · · Nn 0 . . . 00 0 · · · 0 0 . . . 0
]
︸ ︷︷ ︸
n columns
But, by virtue of the definition of N, Nn=0. Therefore, WTi Ti = O6n where O6n is the 6 × n zero
matrix; thus,TT
i Wi = O6n
Finally, we haveTT
i WiMiTi = Onn
109
f6 ← fn6 ← a6 × f + nτ6 ← nT
6 e6For 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 # Mr Ar
2 6 66 6× 5 6× 57 3× 5 2× 5
Total 51 46
7.9 The recursive algorithm is based on the free-body diagram of each link of the manipulator. If weconsider the last link of the manipulator with the force f and moment n applied on the EE, then, fromthe 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 torquerequired 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 performedfor 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 tobalance 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 ofmultiplications 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
τ = JTw
110
The calculation of the Jacobian matrix of a six-axis manipulator requires 202 multiplications and108 additions. Note that this calculation involves vectors that have to be precomputed also with theforegoing algorithm. Hence, for a fair comparison, we have to assume here that the Jacobian matrix isavailable. Under these conditions, then, 36 multiplications and 30 additions are required to computethe foregoing product JTw. Thus, the recursive algorithm requires 15 multiplications and 16 additionsmore 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,
I(θ)θ + I(θ, θ)θ − 1
2
[
∂(Iθ)
∂θ
]T
θ − ∂V
∂θ= φn
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 equationbecomes the term of Coriolis and centrifugal forces sought. Therefore, all we have to do to obtain thisterm using the Newton-Euler algorithm, is to eliminate the θi term, all other statements remainingthe 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 theoutward recursions requires 82n M and 65n A. Since Algorithm 7.4.2 remains exactly the same, andhence, (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 forcesof an n-revolute serial manipulator using the Newton-Euler algorithm.
7.11 (a) The kinetic energy of link i can be written as
Ti =1
2tTi Miti
where
ti = [ωTi cTi ]T and Mi =
[Ii OO mi1
]
Since the inertia matrices are given as [ Ii ]i+1, we need [ ti ]i+1. We have, for the DH parametersof Table 7.5 of the Exercises, λ1 = λ2 = 0, µ1 = µ2 = 1. Therefore, using eq.(4.1d), we obtain
Q1 =
c1 0 s1s1 0 −c10 1 0
, Q2 =
c2 0 s2s2 0 −c20 1 0
, Q3 =
1 0 00 1 00 0 1
Thus,[ω1 ]1 = θ1[ e1 ]1 = [ 0 0 θ1 ]
T
[ω1 ]2 = QT1 [ω1 ]1 =
c1 s1 00 0 1s1 −c1 0
00θ1
=
0θ10
[ω2 ]2 = [ω1 ]2 + θ2[ e2 ]2 =
0θ10
+
00θ2
=
0θ1θ2
[ω2 ]3 = QT2 [ω2 ]2 =
c2 s2 00 0 1s2 −c2 0
0θ1θ2
=
s2θ1θ2−c2θ1
[ω3 ]3 = [ω2 ]3; [ω3 ]4 = QT3 [ω3 ]3 =
s2θ1θ2−c2θ1
111
Moreover,[ c1 ]1 = [ c2 ]2 = 0, [ c3 ]3 = [ 0 0 b3 ]
T
Therefore,
[ c1 ]2 = [ c2 ]3 = 0
[ c3 ]3 = b3[ e3 ]3 + [ω3 × c3 ]3 = [ b3θ2 −b3s2θ1 b3 ]T
[ c3 ]4 = QT3 [ c3 ]3 = [ b3θ2 −b3s2θ1 b3 ]
T
Hence,
T1 =1
2[ 0 θ1 0 0 0 0 ]
A 0 0 0 0 00 A 0 0 0 00 0 A 0 0 00 0 0 m1 0 00 0 0 0 m1 00 0 0 0 0 m1
0θ10000
=1
2Aθ21
Similarly,
T2 =1
2B(θ21 + θ22)
and
T3 =1
2C(θ21 + θ22) +
1
2m3b
23(s
22θ
21 + θ22) +
1
2m3b
23
Thus,
T =1
2[A+B + C +m3b
23s
22]θ
21 +
1
2[B + C +m3b
23]θ
22 +
1
2m3b
23
If we use the Y1Z1 plane as a reference, the potential energy of the system is
V = m3gc3 · (−i)
where i is a unit vector parallel to the X1 axis. We have
[ c3 ]1 = Q1Q2[ c3 ]3 = P2[ c3 ]3 =
c1c2 s1 c1s2s1c2 −c1 s1s2s2 0 −c2
00b3
= b3
c1s2s1s2−c2
Thus,c3 · (−i) = −b3c1s2
andV = −m3gb3c1s2
Hence,
∂T
∂θ1= [A+B + C +m3b
23s
22]θ1
∂T
∂θ2= [B + C +m3b
23]θ2
∂T
∂b3= m3b3
d
dt
(∂T
∂θ1
)
= [A+B + C +m3b23s
22]θ1 + 2m3b3s
22b3θ1 + 2m3b
23s2c2θ1θ2
d
dt
(∂T
∂θ2
)
= [B + C +m3b23]θ2 + 2m3b3b3θ2
112
d
dt
(∂T
∂b3
)
= m3b3
∂T
∂θ1= 0
∂T
∂θ2= m3b
23s2c2θ
21
∂T
∂b3= m3b3
(
s22θ21 + θ22
)
∂V
∂θ1= m3gb3s1s2
∂V
∂θ2= −m3gb3c1c2
∂V
∂b3= −m3gc1s2
Therefore, the Euler-Lagrange equations of the manipulator are
(A+B + C +m3b
23s
22
)θ1 + 2m3b3s
22b3θ1 + 2m3b
23s2c2θ1θ2 +m3gb3s1s2 = τ1
(B + C +m3b
23
)θ2 + 2m3b3b3θ2 −m3b
23s2c2θ
21 −m3gb3c1c2 = τ2
m3b3 −m3b3
(
s22θ21 + θ22
)
−m3gc1s2 = f3
(b) By inspection,
I =
A+B + C +m3b23s
22 0 0
0 B + C +m3b23 0
0 0 m3
Note that the first two diagonal entries of I have units of moment of inertia, while the thirdconsistently has units of mass.
7.12 a) The link is inertially isotropic when its inertia matrix I is isotropic. Then, if we denote the tripleeigenvalue 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)
Line 2 of Algorithm 7.4.2 then changes to
nPn = Inωn − n+ ρn × fPn . (158)
This amounts to (3 M) + 3 A+3 A+ (3 M +6 A), or a total of 6 M and 12 A, which compared with30 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
nPi = Iiωi + ρi × fPi +Qin
Pi+1 + δi × φi+1. (159)
113
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 thegeneral case, line 7 of Table 7.2, gives savings of 24(n− 1) M and 12(n− 1) A. Thus, the total savingsin 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
T ≡
t11 0 · · · 0t21 t22 · · · 0...
.... . .
...tn1 tn2 · · · tnn
where the 3-dimensional vector tij is, for a revolute pair, given by
tij =
[1
Erij
]
, if j ≤ i;[00
]
, otherwise.
For a prismatic pair,
tij =
[0ek
]
, 1 ≤ k ≤ i
with rij defined as
rij ≡
aj + aj+1 + · · ·+ ai−1 + ρi, if j < i;
ρi, if j = i;
0, otherwise.
From eq.(7.108), we have
Mi =
[Ii 0T
0 mi1
]
As in the general case, the 3n× 3n matrix M is given by
M = diag(M1, . . . ,Mn)
We can factor this matrix asM = HTH
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 0T
0 ni1
]
We thus haveMi = HT
i Hi
which makes apparent thatNi =
√
Ii, ni =√mi
114
Algorithm 7.6.1a:
For j = 1 to n step 1 do
rjj ← [ρj ]j+1
pjj ←[
Nj
njErjj
]
j+1
For i = j+ 1 to n step 1 do
if R then
rij ← QTi [ ri−1,j + δi−1 ]i + [ρi ]i+1
pij ←[
Ni
niErij
]
i+1
else
pij ←[
0niej
]
i+1
endif
enddo
enddo
From the foregoing definitions, then, the n×n matrix of generalized inertia I can now be expressed as
I = PTP
where P is defined, in turn, as the 3n× n matrix given below:
P ≡ HT
which can be written as
P =
p11 0 · · · 0p21 p22 · · · 0...
.... . .
...pn1 pn2 · · · pnn
where the 3-dimensional arrays pij are given as
pij ≡ Hitij =
[Ni
niErij
]
if the jth joint is R;
[0
niej
]
if the jth joint is P
Therefore, for a planar manipulator, Algorithm 7.6.1 reduces to Algorithm 7.6.1a. The computationalcost of this algorithm is now evaluated. The first statement of the outermost loop involves no floating-point 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 ofmatrix E given by
E =
[0 −11 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) requiresno 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 transformationbetween two consecutive coordinate frames, from Fi- to Fi+1-coordinates, plus two vector sums, whichconsumes 4(n − i) multiplications and 6(n − i) additions; this loop also consumes one matrix-times-vector multiplication, with E being the said matrix, which involves zero floating-point operations, asmentioned above, and one scalar-times-vector multiplication, which requires 2(n − i) multiplicationsand zero additions. Thus, the total numbers of operations required by this algorithm, for a n-revolutemanipulator, are Mia multiplications and Aia additions, as given below:
Mia = 2n+
n∑
i=1
6(n− i) = n(3n− 1)
Aia =
n∑
i=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,
p1 =
000
, p2 =
1/2√3/20
, p3 =
−1/2√3/20
, p4 =
0√3/3√6/3
and from the data,p1 = [ 0
√3/3
√6/3 ]
T
Now, let us express p2 in the formp2 = [ x2 0 0 ]T
Then, from the relation(p2 − p1) · (p2 − p1) = 0
we obtain x2 = 1. Hence,p2 = [ 1 0 0 ]
T
(b) Now, we must have, for compatibility, with p3 = [x3, y3, z3]T ,
(p3 − p1) · (p3 − p1) = −1
2x3 +
√3
2
(
y3 −√3
3
)
= 0
which leads tox3 =
√3 y3 − 1 (160)
If p3 = 0, we will have x3 = y3 = z3 = 0, which does not agree with eq.(160). Thus, p3 cannotbe zero.
(c) If p3 lies in the P1P2P3 plane, then z3 = 0. Again, we must have
(p3 − p2) · (p3 − p2) = −(x3 − 1) = 0 (161)
From eqs.(160) and (161), we have x3 = 1 and y3 = 2√3/3, i.e.,
p3 = [ 1 2√3/3 0 ]T
116
(d) Let the angular velocity be
ω = [ω1 ω2 ω3 ]T
Then,p2 − p1 = ω × (p2 − p1)
That is
1−√3/3
−√6/3
=1
2
−√3ω3
ω3√3ω1 − ω2
from which we obtain
ω3 = −2√3/3 (162)√
3ω1/2− ω2/2 = −√6/3 (163)
We also havep3 − p1 = ω × (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 p be the position and velocity vectors of a point P on the instantaneous screw axis.We have
p = p1 + ω × (p− p1) =1
3
2√3√
3− 2√3x+ 2
√2 z√
6− 2√2 y
From p× ω = 0, we have
1
3
3− 6x+ 2√6 z
2√3− 10y
−√6 + 2
√6x− 4z
=
000
(165)
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 + y2 + z2 =2
5x2 − 3
2x+
3
8+
3
25
Now, d2 is minimized by setting its derivative with respect to x equal to zero, thereby obtainingx = 3/10. Hence,
p = [ 3/10√3/5 −
√6/10 ]
T(166)
Moreover, we can readily obtain the unit vector e in the direction of the instant screw axis as
e =ω
‖ω‖ =3√5
10[−2√2/3 0 −2
√3/3 ]T (167)
From eqs.(166) and (167), the screw axis is totally determined. Alternatively, we can applyformulas (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 P, and these require, inturn,
c =1
3
111
, c =
111
Therefore,
P =2
3
1 1 −21 −2 11 1 −2
, P = 2
0 1 −10 0 00 −1 1
whence,PTP = 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 showsthat
(p2 − p1)× (p3 − p1) = 4
10−1
6= 0
118
the points thus being noncollinear. Moreover, tr(P) = −2, while
P2 =4
3
0 −1 10 2 −20 −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 torender 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,
[p1]A =
−2/3−2√2/3
0
, [p2]A =
4/3−2√2/3
0
, [p3]A =
−2/34√2/30
,
and
[P]A =
−2/3 4/3 −2/3−2√2/3 −2
√2/3 4
√2/3
0 0 0
⇒ tr([P]A) =−2− 2
√2
3⇒ tr2([P]A) =
12 + 8√2
9
Moreover,
[P2]A =1
9
4− 8√2 −8− 8
√2 4 + 16
√2
8 + 4√2 8− 8
√2 −16 + 4
√2
0 0 0
⇒ tr([P2]A) =12− 16
√2
96= tr2([P]A)
which means that matrix [D]A is invertible. In fact,
[D]A =1
3
−√2 −2 1√2 −1 −2
√2
0 0 −1−√2
whose determinant is readily calculated as
det([D]A) =−2−
√
(2)
96= 0
In order to find ω, then, all we need is the right-hand side of eq.(8.9), which requires calculatingthe given velocities in A. We show in Fig. 33 the relative layout between the original frame Fand the auxiliary frame A.Let iA, jA, kA be the unit vectors parallel to XA, YA, ZA, respectively, while i, j, k are theircounterparts associated with F . From Fig. 33, it is apparent that
[ iA ]F =
0−10
, [ jA ]F =[−−−→P1P3]F
‖[−−−→P1P3 ]F‖
where
[−−−→P1P3 ]F = [p3 − p1 ] =
−20−2
119
Hence,
[ jA ]F =
√2
2
−10−1
⇒ [kA ]F = [ iA × jA ]F =
√2
2
10−1
Now, from Definition 2.2.1, the matrix Q rotating F into A is given by
[Q]F = [ iA jA kA ]F =
0 −√2/2
√2/2
−1 0 00 −
√2/2 −
√2/2
Therefore,[pi]A = [QT ]F [pi]F ⇒ [P]A = [QT ]F [P]F
where
[P]F = 2
0 1 −10 0 00 −1 1
⇒ [P]A = 2√2
0 0 00 0 00 1 −1
Thus,
vect([P]A) =√2
100
and hence, eq.(8.9) leads toω = D−1vect([P]A)
with D−1 calculated, using computer algebra, as
D−1 =2−√2
2
−1−√2 2(1 +
√2) −1− 4
√2
−2−√2 −2−
√2 4−
√2
0 0 −3√2
whence,
[ω]A =2−√2
2
−2−√2
−2− 2√2
0
Therefore,
[ω]F = [Q]F [ω]A =
111
8.3 If the three points are collinear, all three vectors pi − c, for i = 1, 2, 3, are linearly dependent, andmatrix P is of rank 1, its nullspace being a plane passing through the origin and normal to line P1P2P3.Matrix P being of rank 1, its three eigenvalues are π1, 0, 0, with π1 6= 0. Hence, tr2(P) = π2
1 =tr(P2).Notice that the foregoing relation holds regardless of whether the origin is collinear with the three pointsor not.
8.4 From the compatibility condition,PTP+PPT = O
Taking the trace of both sides, we obtain
tr(PTP+PPT ) = 0⇒ tr(PTP) + tr(PPT ) = 0
Now, because tr(AB) = tr(AB) for all square matrices A and B, then
2 tr(PPT ) = 0 =⇒ tr(PPT ) = 0
120
Alternatively, one can write
PPT = [p1 − c p2 − c p3 − c ]
(p1 − c)T
(p2 − c)T
(p3 − c)T
=3∑
1
(pi − c)(pi − c)T
Upon differentiation of both sides with respect to time, we have
PTP+PPT =
3∑
1
[(pi − c)(pi − c)T + (pi − c)(pi − c)T
]
Taking the trace of both sides, while considering tr(AB) = tr(BA),
2 tr(PPT ) = 2
3∑
1
(pi − c)T (pi − c) = 0
and hence, tr(PPT ) = 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 offrame is required. We will consider a frame F1 with its origin at P1 and its Z1-axis perpendicularto the plane defined by P1, P2 and P3. Moreover, its X1-axis is aligned with a vector going fromP2 to P1. In the new frame F1 we have
[p1]1 =
000
, [p2]1 =
−200
, [p3]1 =
0−2√2
0
Moreover,
[c]1 =1
3
3∑
1
[pi]1 =2
3
−1−√2
0
The rotation matrix that brings F1 into an orientation coincident with the original frame F isgiven by
Q =
0 1 0√2/2 0
√2/2√
2/2 0 −√2/2
The accelerations of the three given points in F1 are thus
[pi]1 = Q[pi]F , for i = 1, 2, 3
i.e.,
[p1]1 =
1√20
, [p2]1 =
1√2
2√2
, [p3]1 =
1√2
−2√2
Moreover,
[P]1 =2
3
1 −2 0√2√2 −2
√2
0 0 0
, [P2]1 =4
9
1− 2√2 −2− 2
√2 4√2
2 +√2 2− 2
√2 −4
0 0 0
Furthermore,
tr[P]1 =2
3(1 +
√2), tr2[P]1 =
4
9(3 + 2
√2), tr([P2]1) =
4
9(3 − 4
√2) 6= tr2[P]1
121
From the data,
[c]1 ≡1
3
3∑
1
[pi]1 = [ 1√2 0 ]
T
and thus
[P]1 =
0 0 00 0 00 2
√2 −2
√2
; vect[P]1 =
√200
To be compatible, the motion must satisfy eqs.(8.15) and (8.25), namely,
PTP+PPT = O (168)
PTP+ 2PT P+PT P = O (169)
Equation (168) is obviously satisfied since P = O, and eq.(169) reduces to
PTP+PT P =2
3
0 0 00 0 2
√2
0 0 −2√2
1 −2 0√2√2 −2
√2
0 0 0
+2
3
1√2 0
−2√2 0
0 −2√2 0
0 0 00 0 00 2
√2 −2
√2
=
0 0 00 0 00 0 0
+
0 0 00 0 00 0 0
= O
the motion thus being compatible. The matrix D, defined in eq.(8.10), is then calculated as
[D]1 =1
2
2√2/3 4/3 0
−2√2/3 2/3 4
√2/3
0 0 2(1 +√2)/3
Moreover, since the velocities of the three points vanish, vect(P) = 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 toDω = vect(P) (170)
We thus have
1
2
2√2/3 4/3 0
−2√2/3 2/3 4
√2/3
0 0 2(1 +√2)/3
ω1
ω2
ω3
=
√200
Solving this set of three equations for ω1, ω2 and ω3, we obtain
[ω]1 = [ 1√2 0 ]
T
which can be expressed in the original frame F as
[ω]F = QT [ω]1 = [ 1 1 1 ]T
122
8.9 (a)
R = [p1 − c p2 − c p3 − c ]
(p1 − c)T
(p2 − c)T
(p3 − c)T
=3∑
1
(pi − c)(pi − c)T
and hence,
J =
3∑
1
[‖pi − c‖21− (pi − c)(pi − c)T
]= tr(R)1−R (171)
which is indeed, according to eq.(3.136), the moment of inertia of a set of three punctual unitmasses placed at the three given points, with respect to the centroid C of the given points.
(b) Upon differentiation of eq.(171), we have
J =d
dt(tr(R)1−R) =
d
dt
(tr(PPT )1−PPT
)
J = tr(PPT )1+ tr(PPT )1− PPT −PPT
From eq.(8.16), tr(PPT ) = 0, which implies that tr(PPT ) = 0. Moreover, it is apparent that
P = ΩP, PT = PTΩT = −PTΩ
and hence,J = −ΩPPT +PPTΩ
Therefore,J = RΩ−ΩR (172)
(c) Now, upon differentiation of eq.(172), we have
J =d
dt
(PPTΩ−ΩPPT
)
= PPTΩ+PPTΩ+PPT Ω− ΩPPT −ΩPPT −ΩPPT
= ΩPPTΩ+PPTΩTΩ+PPT Ω− ΩPPT −ΩΩPPT −ΩPPTΩT
= ΩRΩ−RΩ2 +RΩ− ΩR−Ω2R+ΩRΩ
J = RΩ− ΩR−Ω2R−RΩ2 + 2ΩRΩ
8.10 The moment ni of the force f about the three given points Pi31 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∑
1
(ni − n) =
3∑
1
(pi − c)× f (174)
Moreover, eq. (174) can be written in the form M = −Fp where F = CPM(f). Upon application ofTheorem A.1,
−vect(M) = Df =⇒ f = −D−1vect(M)
123
8.11 From Fig. 7, we obtain the points
p1 =
000
, p2 =
1/2√3/20
, p3 =
−1/2√3/20
and thus
c =1
3
3∑
1
pi =
√3
3
010
Furthermore, let
n ≡ 1
3
3∑
1
ni =
√2
12
100
Now, we define
P ≡ [p1 − c p2 − c p3 − c ] , N ≡ [n1 − n n2 − n n3 − n ]
i.e.,
P =1
6
0 3 −3−2√3√3√3
0 0 0
, N =1
6
−2√2√2
√2
0 −√6√6
0√3 −
√3
Note that the nullspaces of P and N are both spanned by [ 1 1 1 ]T, which is a feature of this type
of matrices. As a check, the product NTP must be skew-symmetric:
NTP =
√2
6
0 −1 11 0 −1−1 1 0
This matrix being skew-symmetric, the moments are compatible, which is the static counterpart of thecompatibility 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)
or1
2[tr(P)1−P] f = vect(N) (175)
We have tr(P) =√3/6, and thus,
1
2[tr(P)1−P] =
1
12
√3 −3 3
2√3 0 −
√3
0 0√3
≡ 1
12A
Moreover,∆ ≡ det(A) =
√3[0− 2× (3
√3)] = −18 6= 0
124
A thus being nonsingular. Furthermore,
vect(N) =1
12
√3−√6√
2−√2
; f =
fxfyfz
Hence, from eq.(175), we obtain√3 fx − 3fy + 3fz = −(
√6−√3) (176a)
2√3 fx −
√3 fz =
√2 (176b)√
3 fz = −√2 (176c)
Solving eqs.(176a–c), we obtain
f = −1
3
0√3√6
, with ‖f‖2 = 1
9(3 + 6) = 1
Now, the line sought is the wrench axis. This is a line of direction f passing through a point P0 ofposition vector p0 that verifies
Fp0 = f ×(
pi +f × ni
‖f‖2)
(177a)
fTp0 = 0 (177b)
for some pi and its corresponding ni. If we choose i = 1, the computational effort is reduced. Thus,
Fp0 =f × (f × n1)
‖f‖2
We have
f × n1 =
√2
12
0√6
−√3
, f × (f × n1) =
√2
4
100
Equations (177a & b) can be written asBp0 = d (178)
where
B ≡[FfT
]
, d ≡[f × (f × n1)
0
]
Now, since we are working with infinite precision—no floating-point arithmetic, and hence, the roundofferror is zero—we can safely use the formula from the left Moore-Penrose generalized inverse of AppendixB. With the aid of this inverse, the “least-square approximation” p0 of the overdetermined linear system(178) is computed as
p0 = (BTB)−1BTd
We have,
BTB = [−F f ]
[FfT
]
= −F2
︸︷︷︸
‖f‖21−ffT
+ffT = ‖f‖21 = 1
Hence,
p0 = [−F f ]
[f × (f × n1)
0
]
= −F [f × (f × n1)] + 0 = [f × (f × n1)]× f
p0 =1
12[ 0 2
√3 −
√6 ]
T
125
Finally, the minimum-magnitude moment is obtained as
n0 =ni · f‖f‖2 f , for i = 1, 2 or 3
However, ni · f = 0, for i = 1, 2, 3, and hence,
n0 = 0
8.12 We haveP = ΩP, R = PPT
Multiplying both sides of eq. (8.6) by PT , we obtain
PPT = ΩPPT = ΩR
Recalling Theorem A.1,
vect(PPT ) =1
2Jω
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,
[J]A = tr([p]A[pT ]A)1− [p]A[p
T ]A
≡ tr([Q]A[p]B[pT ]B[Q
T ]A)[Q]A[QT ]A − [Q]A[p]B[p
T ]B[QT ]A
= [Q]A(tr([QT ]A[Q]A[p]B[p
T ]B)1− [p]B[pT ]B)[QT ]A
= [Q]A(tr([p]B[p
T ]B)1− [p]B[pT ]B)[QT ]A
≡ [Q]A[J]B[QT ]A
which shows that J is indeed frame-invariant.
8.13
p1 =
0−3030
, p2 =
03030
, p3 =
000
, ⇒ c =
0020
p1 =
120−60−60
, p2 =
0−6060
, p3 =
000
, ⇒ c =
40−400
⇒ P =
0 0 0−30 30 010 10 −20
, P =
80 −40 −40−20 −20 40−60 60 0
(a)
PT P =
0 −30 100 30 100 0 −20
80 −40 −40−20 −20 40−60 60 0
=
0 12 −12−12 0 1212 −12 0
× 100 =
0 1 −1−1 0 11 −1 0
× 1200 ,
which is apparently skew-symmetric, and hence, estimates are compatible.
126
(b) Dω = vect(p) where
D =1
2[tr(P)1−P] =
1
2
10 0 030 −20 0−10 −10 30
, vect(P) =1
2
202020
Hence,
1
2
1 0 03 −2 0−1 −1 3
=1
2
222
Matrix is lower-triangular. Use forward substitution: ω1 = 2, 3ω1 − 2ω2 = 2 ⇒ −2ω2 =−4 ⇒ ω2 = 2. Then, −ω1 − ω2 + 3ω3 = 2 ⇒ 3ω3 = 6 ⇒ ω3 = 2
⇒ ω =
222
rad/s
9 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 ismapped by H into h′ given by
h′ = (1− 2 f fT /‖f‖2)h =(f · f)h− 2(f · h)f
‖f‖2
Notice that ‖f‖2h′ is the left-hand side of eq.(8.22f). By the same token, we can write, for i′
i′ = (1− 2 ggT /‖g‖2)i = (g · g)i − 2 (g · i)g‖g‖2
Obviously, the right-hand side of eq.(8.22f) is equal to ‖g‖2i′
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 thatwill render the matrix S in an upper triangular form U. That is,
HS = U
Equation (8.51d) can be written as
STHTHx45 = 012 =⇒ (HS)THx45 = 012 (180)
where 012 is the 12-dimensional zero vector. Then, we can write eq.(180) as
UTv = 012
withv = Hx45 (181)
6See Appendix B.
127
Since S is singular, the 12th row ofU is full of zeros, and so is the 12th column ofUT . As a consequence,the nullspace of UT is spanned by the unit vector n = [0T
11 1]T . Consequently, vector v is a multipleof n, i.e., v = αn, where α is a scalar, as yet to be determined, which is done below. From eq.(181),
x45 = HTv = αHTn = α[h12,1 . . . h12,12
]T
where h12,i denotes, as usual, the ith component of the 12th row of H. Upon comparison of the aboveexpression 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 systemof 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−1i 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 thatroot rapidly. Given a tolerance ǫ, the criterion to stop the iteration is
‖xi+1 − xi‖∞ < ǫ
where ‖ · ‖∞ denotes the Chebyshev norm. The Maple code implementing this calculation is given inAppendix 2.
By monitoring the condition number of F based on the Frobenius norm, we observe that the Newton-Raphson method converges faster when the condition number is smaller. The condition number intro-duced 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 themoving platform can be obtained by relating the power generated by the actuators and the powerconsumed by the load. From eq.(9.102a),
b = 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 = bT τ
ΠL = tTw
with b being the vector of actuated joint rates, τ the vector of actuated joint torques, t the twist ofthe moving platform, and w the wrench acting on the moving platform. Then,
bT τ = tTw
tTKTτ = tTw
which is valid for every possible motion, i.e., for every possible twist t, and hence, the above equationleads 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
l, δ ≡ d
l, ρ ≡ r
d
Upon expanding the determinant of Θ and imposing the singularity condition, we have
det(Θ) = ρ[α+ δ sinψ] = 0
Hence,
sinψ = −αδ=⇒ α
δ≤ 1
which yieldsa+ b ≤ d (182)
If the above condition holds, then the angle ψ rendering Θ singular can be evaluated as
ψ = sin−1
(
−a+ b
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 proportionalto the 2× 2 identity matrix. Below we calculate these two matrices:
I. ΘTΘ =
[α1 α3
α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 isotropycondition
ΘΘT =
[σ2 00 σ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 (183a)
g2 ≡[(4α2 − 1) sinψ + 4αδ
]cosψ = 0 (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, fromeq.(183a) we derive
−2(α+ δ)2ρ2 +1
2= 0
and, since α, δ ≥ 0, the foregoing relation implies
α+ δ =1
2ρ
or, in terms of the architecture parameters,
a+ b+ d
l=
d
2r
Substituting this condition into matrix ΘΘT , we obtain
ΘΘT =1
2
[1 00 1
]
Apparently, in this case matrix Θ is isotropic, and has the form
Θ =1
2
[1 1−1 1
]
In case (ii), we have
sinψ =4αδ
1− 4α2(184)
with 1− 4α2 > 4αδ while α < 1/2, and hence,8
cos2 ψ =(1− 4α2)2 − 16α2δ2
(1 − 4α2)2(185)
7The axis of the caster wheel always lags the vertical axis of its bracket.8We 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
ΘΘT =1− 4(α2 − δ2)
1− 4α2
[2α2 00 (1/2)ρ2
]
Apparently, to make this matrix proportional to 2× 2 identical matrix, we need ρ = 2α. Substitutingeqs.(184) and (185) and ρ = 2α into matrix Θ, we obtain
Θ =α
(1 − 4α2)
[ √S + 2δ −
√S + 2δ√
S − 2δ√S + 2δ
]
withS ≡ 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 theform
Θ =1
12
[2√3 + 3 −2
√3 + 3
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.,
U =1
2ρ(α+ δ sinψ)
[ρ[2(α sinψ + δ) + cosψ] 2α cosψ − sinψρ[2(α sinψ + δ)− cosψ] 2α cosψ + sinψ
]
(b) Let
u1 =
2(α sinψ + δ) + cosψ
2(α+ δ sinψ)
2α cosψ − sinψ
2ρ(α+ δ sinψ)
, u2 =
2(α sinψ + δ)− cosψ
2(α+ δ sinψ)
2α cosψ + sinψ
2ρ(α+ δ sinψ)
their gradient with respect to θu being
∇ui =
[∂ui
∂θ3
∂ui
∂ψ
]
, i = 1, 2
where∂u1
∂θ3= 0
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 whosegradient is u1. Likewise,
∂u2
∂θ3= 0
and
∂u2
∂ψ=
1
D
[ρ[2(α2 − δ2) cosψ + (α sinψ + δ)]−α[2(α sinψ + δ)− cosψ]
]
,
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 c are derived in Subsection 10.5.2, and are reproduced below:
ω = − a
3r
3∑
1
θi, c = −2a
3
3∑
1
θifi
Apparently, the first equation leads to the integral
ψ = − a
3r
3∑
1
θi (186)
where ψ is the angular displacement of the platform with respect to a horizontal line fixed to an inertialframe, under the assumption that, when θi = 0, for i = 1, 2, 3, ψ = 0. Now, the second equation iscast in the form
Fθ = − 3
2ac, F ≡ [ f1 f2 f3 ] ≡
[λ1 λ2 λ3µ1 µ2 µ3
]
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 =∂φ1∂θ1
, λ2 =∂φ1∂θ2
where
λ1 = cosψ, λ2 = cos
(
ψ +2π
3
)
= −1
2(cosψ +
√3 sinψ)
Let us assume that the foregoing equations hold, while ψ is given by eq.(186), i.e.,
cos
(θ1 + θ2 + θ3
3ra
)
=∂φ1∂θ1
1
2
[
− cos
(θ1 + θ2 + θ3
3ra
)
+√3 sin
(θ1 + θ2 + θ3
3ra
)]
=∂φ1∂θ2
Let us now take the partial derivative of the first of the foregoing equations with respect to θ2 and ofthe second with respect to θ1:
− a
3rsin
(θ1 + θ2 + θ3
3ra
)
=∂2φ1∂θ1θ2
a
6r
[
sin
(θ1 + θ2 + θ3
3ra
)
+√3 cos
(θ1 + θ2 + θ3
3ra
)]
=∂2φ1∂θ2θ1
For the above assumption to hold, the left-hand sides of the two foregoing equations must be equal,i.e.,
− sin
(θ1 + θ2 + θ3
3ra
)
=1
2
[
sin
(θ1 + θ2 + θ3
3ra
)
+√3 cos
(θ1 + θ2 + θ3
3ra
)]
However, this equation leads to
tan
(θ1 + θ2 + θ3
3ra
)
= −√3
3
and hence,a
3r(θ1 + θ2 + θ3) = −
π
6or
5π
6which contradicts the integral of the first constraint, the conclusion being that the second constraintequation 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 withinthe workspace of the robot. Additionally, two alternatives are possible: either the welding is donefrom 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, withcoordinates in m:
x = 0.3 cosϑ
y = 0.3 sinϑ
z =0.8ϑ
π
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
z =0.8ϑ
π+ 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:
x = −0.3ϑ sinϑy = 0.3ϑ cosϑ
z =0.8ϑ
π
and the corresponding acceleration:
x = −0.3ϑ2 cosϑ− 0.3ϑ sinϑ
y = 0.3ϑ cosϑ− 0.3ϑ2 sinϑ
z =0.8ϑ
π
The constant-speed condition (as in Example 11.3.1) leads to:
x2 + y2 + z2 = v20
where v0 is the constant speed along the helix. Upon substitution of numerical values, the foregoingcondition becomes
0.32ϑ2 +
(0.8
π
)2
ϑ2 = (0.050)2
ϑ = 0.1271
and hence, with c = 0.1271 s−1, we obtain
x = −0.3c sin(ct)y = 0.3c cos(ct)
z =0.8c
π9The center of the helicoidal path is defined as the intersection of its axis with a plane parallel to the X2-Y2 plane and
containing the bottom end of the path.
133
Following Example 11.3.1, since this part is independent of the robot,
et ≡dr
ds≡ r
s=
c
vo
−a sin cta cos ctb
and en = −
cos ctsin ct0
Thus, the binormal vector eb is calculated simply as the cross product of the first two vectors ofthe Frenet-Serret triad:
eb ≡ et × en = − c
vo
−b sin ctb cos ct−a
The orientation matrix Q of the nozzle is given by
Q ≡ [ et en eb ]
Hence,
Q =c
vo
−a sin ct −(vo/c) cos ct b sin cta cos ct −(vo/c) sin ct −b cos ctb 0 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, bothgiven in F2, and cP , the position vector of P in the Frenet-Serret frame.
oH =
−0.500−0.5000.330
, p =
a cosϑa sinϑbϑ
, cP =
0−0.0500.0867
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 inSection 4.4. Once we obtain θ, θ can be found from Section 4.5 as
θ = J−1t
Shown in Figs. 37 and 38 are the plots of the time-histories of the joint rates, as pertaining topaths Γ0 and Γ1, respectively.
(c) The joint accelerations can be found from Section 4.6 as
θ = J−1(t− 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
S(t) =
−0.6 sinϕ − cosϕ 0.8 sinϕ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 of R in F from the relation
A ≡ [ i7 j7 k7 ] =
0.933 0.067 −0.3540.067 0.933 0.3540.354 −0.354 0.866
whence [R]F , defined as [ et en eb ] in F7, is
[R]F = AT =
0.933 0.067 0.3540.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︸ ︷︷ ︸
1
= [S(t)]B [R]F
i.e.,
135
0 5 10 15−200
−150
−100
−50
0 5 10 1580
100
120
140
0 5 10 15−50
0
50
0 5 10 1539
40
41
42
0 5 10 15−80
−75
−70
0 5 10 1550
55
60
65
θ1 θ2
θ3 θ4
θ5 θ6
Figure 35: Joint trajectories vs. time (s) for path Γ0, in degrees
0 5 10 150
50
100
0 5 10 15100
120
140
160
0 5 10 150
20
40
60
0 5 10 15−200
−100
0
0 5 10 15−60
−40
−20
0
0 5 10 15−100
−50
0
50
θ1 θ2
θ3 θ4
θ5 θ6
Figure 36: Joint trajectories vs. time (s) for path Γ1, in degrees.
136
0 5 10 152
4
6
8
0 5 10 152
2.5
3
0 5 10 150
1
2
3
0 5 10 151
1.5
2
0 5 10 154
4.2
4.4
4.6
0 5 10 15−4
−2
0
2
θ1 θ2
θ3 θ4
θ5 θ6
Figure 37: Joint velocities vs. time (s) for path Γ0, in rad/s.
0 5 10 15−10
−5
0
5
0 5 10 150
5
10
0 5 10 15−10
0
10
0 5 10 15−20
0
20
0 5 10 15−10
−5
0
0 5 10 15−20
0
20
40
θ1 θ2
θ3 θ4
θ5 θ6
Figure 38: Joint velocities vs. time (s) for path Γ1, in rad/s.
137
0 5 10 150.04
0.06
0.08
0.1
0 5 10 15−0.5
0
0.5
1
0 5 10 15−0.5
0
0.5
0 5 10 15
0.2
0.4
0 5 10 15−0.1
0
0.1
0 5 10 15
−0.4
−0.2
θ1 θ2
θ3 θ4
θ5 θ6
Figure 39: Joint accelerations vs. time (s) for path Γ0, in rad/s2.
0 5 10 15−2
0
2
0 5 10 15−1
0
1
2
0 5 10 15−3
−2
−1
0
0 5 10 15−20
0
20
0 5 10 15−2
0
2
0 5 10 15−20
0
20
θ1 θ2
θ3 θ4
θ5 θ6
Figure 40: Joint accelerations vs. time (s) for path Γ1, in rad/s2.
138
[Q(t)]B =
−0.6sϕ −cϕ 0.8sϕ0.6cϕ −sϕ −0.8cϕ0.8 0 0.6
0.933 0.067 0.3540.067 0.933 −0.354−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
et =
−0.6 sinϕ0.6 cosϕ
0.8
, en =
− cosϕ− sinϕ
0
, eb =
0.8 sinϕ−0.8 cosϕ
0.6
From eq.(11.5a), we can find the curvature κ:
et = 0.6ϕ
− cosϕ− sinϕ
0
= 0.6ϕen
whence κ = 0.6. Now we determine τ :
eb = 0.8ϕ
sinϕcosϕ0
= −0.8ϕ
− sinϕ− cosϕ
0
ebϕ
= −0.8en = −τen
So, τ = 0.8. Finally,δ = 0.8et + 0.6eb
andδ = 0.8et + 0.6eb = (0.8)(0.6)ϕen − (0.6)(0.8)ϕen = 0
11.3 (a) We have
x = 2t, y = t2, z =t3
3
Then, the position vector r of any point on the curve with its first and second-derivatives are
r =
2tt2
t3/3
, r =
22tt2
, r =
022t
Now, the Frenet-Serret triad is readily evaluated as
et =r
‖r‖ =1
t2 + 2
22tt2
, eb =r× r
‖r× r‖ =1
t2 + 2
t2
−2t2
,
en = eb × et =1
t2 + 22
−2t3 + 4t−t4 + 42t3 + 4t
139
and hence, the orientation matrix Q is given by
Q =[et en eb
]=
1
t2 + 2
2 −2t t2
2t 2− t2 −2tt2 2t 2
Hence,
vec(Q) =1
2
Q(3, 2)−Q(2, 3)Q(1, 3)−Q(3, 1)Q(2, 1)−Q(1, 2)
=1
t2 + 2
101
, cosφ =tr(Q)− 1
2=
2− t2t2 + 2
the unit vector e and angle φ sought then being
e =
√2
2
101
, φ = tan−1
( √2
2− t2
)
(b) The expressions for the curvature and torsion in terms of time are readily evaluated as
κ =‖r× r‖‖r‖3 =
2
t2 + 2, τ =
r× r · r‖r× r‖2 =
2
t2 + 2
Moreover, ω = sδ, and hence, all we need is s and δ, which are readily computed as
s = ‖r‖ =√
4 + 4t2 + t4 = 2 + t2
and
δ = τet + κeb =2
t2 + 2
101
Thus,
ω = sδ = 2
101
Finally, straightforward differentiation of the above expression for ω with respect to time yields
ω = 0
11.7 The path is given as
Γ : r = r
λ+ cosϕsinϕ1/ϕ
=
xyz
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
ϕ 0 90 180 270 360
x 0.45 0.30 0.15 0.30 0.45y 0.00 0.15 0.00 -0.15 0.00z 0.3969 0.4975 0.5809 0.4975 0.3969
Table 7: Spline parameters for N = 5
αk αi,j βk βi,j1 0.2348 0.4627 4.2589 8.51782 0.2279 0.4558 4.3879 8.64683 0.2279 0.4627 4.3879 8.77584 0.2348 0.4696 4.2589 8.6468
Now, we assemble the A and C matrices. Since the path is closed, we have a periodic spline. So, Aand C are given by eqs.(11.59a & b). For N = 5, we obtain
A =
0.9392 0.2348 0 0.23480.2348 0.9254 0.2279 0
0 0.2279 0.9116 0.22790.2348 0 0.2279 0.9254
C =
−8.5178 4.2589 0 4.25894.2589 −8.6468 4.3879 0
0 4.3879 −8.7758 4.38794.2589 0 4.3879 −8.6468
Matrix P can be found using eq.(11.58):
P =
0.45 0 0.39690.30 0.15 0.49750.15 0 0.58090.30 −0.15 0.4975
From eq.(11.60), we obtain P′′ and, with P, we obtain the spline coefficients, ak, bk, ck and dk. Thus,
P′′ = 6A−1CP =
−8.0389 0 5.8816−0.2483 −8.4077 −0.81048.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
et =r′(σk)
‖r′(σk)‖
eb =r′(σk)× r′′(σk)
r′(σk)× r′′(σk)en = eb × 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 asQ = [eb et en ]
In order to obtain the time-histories of the joint angles, we need Q as a function of time. The timerequired to complete this task is found from T = l/vo, where l is the length of the curve. We have
s(σ) =
∫ σfinal
0
‖r′(σ)‖dσ
and hence,
l =
N−1∑
i=1
∫ σi+1
σi
‖p′(σ)‖dσ
Note that ds/dσ = ‖r′(σ)‖, so that we can write
s = σ‖r′(σ)‖ = vo
Finally,
σ =vo
‖r′(σ)‖Unlike Example 11.5.1, r′(σ) is derived from the spline approximation of Γ. Next, we integrate numer-ically the above equation to obtain σ(t), while letting σ(0) = 0, which thus yields Q and p as functionsof time.
Below we compute the pose, twist and twist-rate at each spline supporting point. First, the pose ateach supporting point is nothing but
Q(σk) = [ eb(σk) en(σk) ek(σk) ]
p(σk) = dk
Second, the twist tk = [ωTk vT
k ]T, where ωk is the angular-velocity vector and vk = pk at t = tk.
We have, from eq.(11.12),ωk = sδk
where s = vo and δk is the Darboux vector at the kth supporting point, i.e.,
δk = τk(et)k + κk(eb)k
κk =‖ck × 2bk‖‖ck‖3
τk =ck × 2bk · 6ak‖ck × 2bk‖
Therefore,pk = vo(et)k(σk)
Now, for the twist rate, tk = [ ωTk vT
k ]T , we have
vk = pk = σ22bk
142
0 0.5 1160
180
200
220
0 0.5 160
70
80
90
0 0.5 1250
300
350
0 0.5 10
200
400
0 0.5 140
60
80
100
0 0.5 1160
180
200
θ1 θ2
θ3 θ4
θ5 θ6
Figure 41: Joint trajectories vs. σk/σmax for N = 20, in degrees.
andωk = sδk + sδk, s = vo = 0
Hence,ωk = vo[τk(et)k(σk) + κk(eb)k(σk)]
with
κk =vo(ck × 2bk) · (ck × 6ak)
κk
τk =vo [−2τk(ck × 6ak) · (ck × 2bk)]
κ2k
Finally, θ, θ, and θ at the supporting points are determined using inverse kinematics. Once again, weapply Algorithm 11.5.2 with iterations performed between supporting points. The joint angle, velocityand 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 toSection 11.4 to obtain the A and C matrices. Thus,
θ = 6A−1Cθ
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 isof 6r × n. We can thus express the 6r-dimensional twist vector t of the whole system and its rate ofchange as
t = Tθa, t = Tθa + Tθa (187)
143
0 0.5 1−40
−20
0
20
0 0.5 1−10
0
10
0 0.5 1−20
0
20
0 0.5 1−100
−50
0
0 0.5 1−50
0
50
0 0.5 1−50
0
50
θ1 θ2
θ3 θ4
θ5 θ6
Figure 42: Joint velocities vs. σk/σmax for N = 20, in rad/s.
0 0.5 1−20
0
20
40
0 0.5 1−20
0
20
40
0 0.5 1−20
0
20
40
0 0.5 1−50
0
50
100
0 0.5 1−50
0
50
100
0 0.5 1−100
0
100
θ1 θ2
θ3 θ4
θ5 θ6
Figure 43: Joint accelerations vs. σk/σmax for N = 20, in rad/s2.
144
0 0.5 1−20
0
20
40
0 0.5 10
10
20
30
0 0.5 1−10
0
10
0 0.5 10
50
100
0 0.5 10
50
100
0 0.5 1−50
0
50
∆θ1 ∆θ2
∆θ3 ∆θ4
∆θ5 ∆θ6
Figure 44: Differences in the joint accelerations for N = 5, in rad/s2.
0 0.5 1−5
0
5
10
0 0.5 1−5
0
5
10
0 0.5 1−10
−5
0
5
0 0.5 1−50
0
50
100
0 0.5 1−10
0
10
20
0 0.5 1−20
0
20
40
∆θ1 ∆θ2
∆θ3 ∆θ4
∆θ5 ∆θ6
Figure 45: Differences in the joint accelerations for N = 15, in rad/s2.
145
0 0.5 1−5
0
5
10
0 0.5 1−5
0
5
0 0.5 1−10
−5
0
5
0 0.5 1−20
0
20
40
0 0.5 1−20
0
20
0 0.5 1−20
0
20
40
∆θ1 ∆θ2
∆θ3 ∆θ4
∆θ5 ∆θ6
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 + Tθ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
tTwC = 0 (189)
and, if we replace t in the above equation for its expression in eq.(187),
θT
aTTwC = 0
which holds for any θa, and hence,TTwC = 0 (190)
i.e., wC lies in the nullspace of TT . Thus, upon multiplying both sides of eq.(188) from the left byTT , wC disappears, and we obtain
TTM(Tθa + Tθa) +TTWMTθa = TTwA +TTwG +TTwD
orI(θ)θ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(θ) = TTMT (192a)
C(θ, θa) = TTMT+TTWMT (192b)
τA = TTwA, γ = TTwG, δ = TTwD (192c)
Moreover, T = T(θ), T = T(θ, θa), and W = W(θ, θa). For brevity, we shall omit the arguments inthe derivations below, whenever not essential. Let us now calculate I(θ, θa):
I(θ, θa) = TTMT+TT MT+TTMT (193)
146
Furthermore, we recall the expression for M given in Exercise 3.17 for one single body. For r bodies,M takes on an identical form, except that, now, M, W, and M are 6r × 6r block-diagonal, i.e.,
M = diag(M1, · · · ,Mr), W = diag(W1, · · · ,Wr), M = diag(M1, · · · , Mr)
eq.(193) thus yielding
I(θ, θa) = TTMT+TTMT+TT (WM−MW)T (194)
Upon substituting eq.(192b) into eq.(194), we obtain
I(θ, θa) = C(θ, θa) + TTMT−TTMWT
Hence,
C(θ, θa) = I(θ, θa)− TTMT+TTMWT ≡ 1
2I(θ, θa) +P
where
P ≡ 1
2I(θ, θa)− TTMT+TTMWT
=1
2
[
TTMT+TTMT+TT (WM−MW)T]
− TTMT+TTMWT
=1
2
[
−TTMT+TTMT+TT (WM+MW)T]
thereby obtaining the desired expression for C(θ, θa).
12.2 We haveI = TTMT
Differentiating this equation with respect to time, we obtain
I = TTMT+TT MT+TTMT
Taking into account the result of Exercise 3.17, applicable to one single rigid body, we can readilyextend it to a system of r rigid bodies:
M = WM−MW
and hence,I = TTMT+TTMT+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 ofC(θ, θa), i.e.,
Cs =1
2(C+CT ), Css =
1
2(C−CT )
Now we calculate Cs(θ, θa) using the expression for C(θ, θa) in Exercise 12.1, namely,
Cs =1
2(C+CT )
=1
4[I+TTMT− TTMT+TT (WM+MW)T
+I+ TTMT−TTMT+TT (−WM−MW)T] =1
2I
147
whence,I = C+CT
Therefore,I− 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 inthe from
ω =r
l(θ1 − θ2) = 0
which yieldsθa = Lθ1
where the matrix L isL =
[1 1
]T
Iw and Ip being constant, they are not affected by the type of motion. Ic and Ib are calculatedbelow:
Substituting ψ = π/2 into eq.(8.128b), we derive
θ11 = θ12 =1
2, θ21 = −θ22 = −ρ(α+ δ)
Hence,
Ic =mcr
2
4
[3/2 + ρ2(α+ δ)2 3/2− ρ2(α+ δ)2
3/2− ρ2(α+ δ)2 3/2 + ρ2(α+ δ)2
]
Ib = Ib
[ρ2(α+ δ)2 −ρ2(α+ δ)2
−ρ2(α + δ)2 ρ2(α+ δ)2
]
+1
4mbd
2
[ρ2(α+ δ)2 + ρ2 −ρ2(α+ δ)2 + ρ2
−ρ2(α+ δ)2 + ρ2 ρ2(α + δ)2 + ρ2
]
=
(
Ib +1
4mbd
2
)
ρ2(α + δ)2[
1 −1−1 1
]
+1
4mbd
2ρ2[
1 11 1
]
The inertia matrix under pure translation is, then, a scalar I ′, namely,
I ′ = LT IL = I ′w + I ′c + I ′b + I ′p
where
I ′w ≡ LT IwL = 2(I +mwr2)
I ′c ≡ LT IcL =3mcr
2
2
I ′b ≡ LT IbL = mbr2
I ′p ≡ LT IpL = mpr2
Physical interpretation: I ′w is twice the moment of inertia of wheel w.r.t. a horizontal line parallelto the actuated wheel axis and passing through the contact point; I ′c is moment of inertia of thedisk of mass mc and radius r w.r.t. a horizontal line parallel to the caster wheel axis and passingthrough the contact point; I ′b is the moment of inertia of a particle of mass mb located at thecenter of the caster wheel w.r.t. the contact point; and I ′p is similar to I ′b, with mb replaced bymp.
148
(b) Now, for the case in which the midpoint of segment O1O2 is stationary, the kinematic constraintcan be written as
θ1 + θ2 = 0 =⇒ θa = Uθ1
withU =
[1 −1
]T
Under this maneuver,
ω =2r
lθ1, c = 2a
r
lθ1i
In this case, the platform-bracket ensemble can move as a single rigid body, such as under ma-neuver (a). However, the angle ψ between platform and bracket can also vary, with the midpointof segment O1O2 still remaining stationary. Nevertheless, the only possible motion leaving thatpoint stationary and leading to a generalized inertia matrix that lends itself to a physical inter-pretation, 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,
sinψ = − δα
= − d
a+ b⇒ cosψ =
√
(a+ b)2 − d2a+ b
Therefore,
θ11 =
√
(a+ b)2 − d2l
− d
2(a+ b), θ12 = −
√
(a+ b)2 − d2l
− d
2(a+ b),
θ21 =r
d
√
(a+ b)2 − d22(a+ b)
+r
l, θ22 =
r
d
√
(a+ b)2 − d22(a+ b)
− r
l
Then, we calculate matrices Ic and Ib using computer algebra (the expressions thus obtained arequite cumbersome to display), the generalized inertia matrix in this case becoming the scalar
I ′′ = UT IU = I ′′p + I ′′b + I ′′w + I ′′c
where, after further computer-algebra work,
I ′′p ≡ UT IpU = 4Ip(ρδ)2 + 4λ2mpr
2 = 4(Ip +mpa2)
(2r
l
)2
I ′′b ≡ UT IbU =
Ib +mb
[
(a+ b)2 − 3
4d2](
2r
l
)2
I ′′w ≡ UT IwU = 2[I +mwr
2 + 4(ρδ)2H]= 2
[
I +
(
H +mw
l
4
2)(2r
l
)2]
I ′′c ≡ UT IcU = mc
r2
l26[(a+ b)2 − d2] + r2
=mcr
2
2
[2
l
√
(a+ b)2 − d2]2
+mc
[r2
4+ (a+ b)2 − d2
](2r
l
)2
Physical interpretation:
I ′′p is the moment of inertia of the platform with respect to its instant center of rotation (the
midpoint of the segment O1O2), times the square of 2r/l, which is the ω/θ1 ratio.
149
I ′′b , in turn, is the moment of inertia of the bracket with respect to the same instantant center ofrotation times the same factor, for the angular velocity of the bracket is identical to that of theplatform (ψ = 0).
I ′′w equals twice the term in brackets, which is thus the contribution of each actuated wheel toI ′′w; this has two parts, the first one, I, being the moment of inertia of each wheel with respect toits horizontal axis and, therefore, accounts for the rotation of the wheel about its own axis; thesecond is the contribution of the moment of inertia of the same wheel w.r.t. a vertical line passingthrough the contact point with suitable shifting, to account for the Parallel-Axis Theorem, thisline lying a distance l/2 from the segment O1O2, 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, I ′′c includes the contribution of the moment of inertia of the caster wheel about itshorizontal axis of rotation times the square of (2/l)
√
(a+ b)2 − d2, which is the θ3/θ1 ratio, andthe contribution of the moment of inertia of the caster wheel w.r.t. a vertical line passing throughthe contact point with suitable shifting, to account for the Parallel-Axis Theorem, the distanceinvolved 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 identitymatrix, with a nonzero proportionality factor. Hence, it is apparent that for I(θ) of Subsection 12.5.2to be isotropic, all we need is β = 0, i.e.,
H + 3J = 3mwr2 + 2mpr
2 (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)r
2 > 0
Furthermore, C(σ, θa, θa)θa can be expressed as
C(σ, θa, θa)θa = TTMTθa ≡[
vect(TTMT)]
× θa (199)
where we have exploited the skew-symmetry of TTMT, which is apparent from eq.(12.73), its axial
150
vector being
vect(TTMT) =√3λ2(3mw +mp)r
2ω
111
(200)
or, if we recall eq.(12.59),
vect(TTMT) = 3λ2(3mw +mp)r2(−λ)
(3∑
1
θi
)
︸ ︷︷ ︸√3eT θa
e
where
e ≡√3
3[ 1 1 1 ]T
Thus,C(σ, θa, θa)θa = −3
√3λ3(3mw +mp)r
2(eT θa)e× θa
Therefore, the governing equation (12.76) becomes
[I + 6λ2(3mw +mp)r2]θa − 3
√3λ3(3mw +mp)r
2(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:
A =3√3λ3(3mw +mp)r
2
I + 6λ2(3mw +mp)r2
B =c
I + 6λ2(3mw +mp)r2
C =1
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 θi31 vanishes, i.e., θa is normal to e. This implies that the platform undergoespure translation;
ii) All three wheel rates are identical, i.e., θa is parallel to e. This implies that the platform undergoespure rotation.
Now, in order to gain more insight into the dynamics of the isotropic robot, let us decompose θa intothe three mutually orthogonal directions given by the orthonormal triad ei31, with e1 ≡ e and e2and e3 any two constant unit vectors perpendicular to e and to each other, i.e., let
θa = ϕ1e1 + ϕ2e2 + ϕ3e3; ϕi 6= θi, i = 1, 2, 3
Then,e× θa = e1 × (ϕ1e1 + ϕ2e2 + ϕ3e3) = ϕ2 e1 × e2
︸ ︷︷ ︸
e3
+ϕ3 e1 × e3︸ ︷︷ ︸
−e2
= −ϕ3e2 + ϕ2e3
151
the governing equation (201) thus becoming, in component form
ϕ1 +Bϕ1 = Cτ1(t) (202a)
ϕ2 +Aϕ1ϕ3 +Bϕ2 = Cτ2(t) (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−Btb1 + C
∫ 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) =
∫ t
0
ϕ1(v)dv + a1
where a1 = ϕ1(0) is a second initial condition. Once ϕ1(t) is available, eqs.(202b & c) can be integratednumerically, 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, thatshould ease its control.
12.8 Under pure translation, the kinematic constraint can be written as
ω =a
2r
3∑
1
θi = 0
or, alternatively,
[1 1 1
]
θ1θ2θ3
= 0
Now the robot has 2-dof, and hence θa can be expressed as
θa = Lu, u =[
θ1 θ2]
where, for example,
L =
1 00 1−1 −1
Thus, the generalized inertia matrix under pure translation is
I′ = LT IL =
[1 0 −10 1 −1
]
α β ββ α βα β α
1 00 1−1 −1
= (α− β)[
2 11 2
]
10Kailath, 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
C(σ, θa, θa)θa = TTMTθa ≡[
vect(TTMT)]
× θa (203)
where we have exploited the skew-symmetry of TTMT, which is apparent from eq.(12.73), its axialvector being
vect(TTMT) =√3λ2(3mw +mp)r
2ω
111
(204)
By recalling eq.(12.59),
vect(TTMT) = 3λ2(3mw +mp)r2(−λ)
(3∑
1
θi
)
︸ ︷︷ ︸√3eT θa
e
with
e ≡√3
3[ 1 1 1 ]T
Hence,C(σ, θa, θa)θa = −3
√3λ3(3mw +mp)r
2(eT θa)e× θa
Now, C(σ, θa, θa)θa vanishes under two nontrivial conditions, namely,
i) The mean value of θi31 vanishes, i.e., θa is normal to e. This implies that the platform undergoespure translation;
ii) All three wheel rates are identical, i.e., θa is parallel to e. This implies that the platform undergoespure rotation.
12.10 The characteristic equation of I is
P (λ) = det
λ− α −β −β−β λ− α −β−β −β λ− α
= (λ− α)3 − 3β2(λ− α)− 2β3 = 0
Let us zero P ′(λ):P ′(λ) = 3(λ− α)2 − 3β2 = 0
The two above equations apparently admit the common root (λ − α) = −β, and thus λ = α − β is adouble root11 of P (λ). Let λ1 = λ2 = α− β, which thus yields
tr(I) =
3∑
1
λi
or3α = 2(α− β) + λ3
and hence,λ3 = α+ 2β
11See 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β −β −β−β 2β −β−β −β 2β
ξ3η3ζ3
=
000
Solving this system, we obtain ξ3 = η3 = ζ3 =√3/3 and thus
e3 =
√3
3
111
Therefore, e1 and e2 lie in a plane normal to e3 and can be chosen mutually orthogonal. We can thuslet eT3 e1 = 0 where e1 = [ξ1, η1, ζ1]
T which implies that
ξ1 + η1 + ζ1 = 0 (205a)
Moreover, we haveξ21 + η21 + ζ21 = 1 (205b)
Hence, the three components of e1 are subject to two constraints. Thus, one of these components canbe chosen arbitrarily. Let, for example, ξ1 = 0; then, η1 =
√2/2 and ζ1 = −
√2/2, i.e.,
e1 =
√2
2
01−1
Now, e2 can be obtained as e2 = e3 × e1, which leads to
e2 =
√6
6
−211
Eigenvectors e1 and e2 represent, in the θa space, pure translations of the platform in two orthogonaldirections, for both yield
∑31 θ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,
q =
[θa
t′
]
, ω = σ (206)
From eq.(12.56a),
[J −K ]︸ ︷︷ ︸
A
[θa
t′
]
︸ ︷︷ ︸
q
= 03 ⇒ b = 0 (207)
where J is a constant, but K depends on fi31 and, since this set depends on the orientation of theplatform, we have A = A(σ). On the other hand, the kinetic energy Ti of the ith wheel can beexpressed as
Ti =1
2mw‖ci‖2 +
1
2ωT
i Iiωi, i = 1, 2, 3 (208)
whereci = c+ ωrfi
154
and‖ci‖2 = ‖c‖2 + ω2r2 + 2ωrc · fi
Let Fi be the frame with origin at Ci and unit vectors ei, fi,k. Then,
[ωi]i =
θi0ω
, [Ii]i =
I 0 00 J 00 0 J
and thus,ωT
i Iiωi = Iθ2i + Jω2
Hence,
Ti =1
2mw
(cT c+ ω2r2 + 2ωrcT fi
)+
1
2
(
Iθ2i + Jω2)
, i = 1, 2, 3 (209)
Moreover,
T4 =1
2mpc
T c+1
2Hω2 (210)
Therefore,
T =
4∑
1
Ti =1
2(3mw +mp)c
T c+3
2mwr
2ω2 + 3r(f1 + f2 + f3)T cω
+1
2I(
θ21 + θ22 + θ23
)
+3
2Jω2 +
1
2Hω2
=1
2I(
θ21 + θ22 + θ23
)
+
(3
2mwr
2 +3
2J +
1
2H
)
ω2 +1
2(3mw +mp)(x
2 + y2)
Moreover,V = 0
and hence,L = T (211)
We have
∂L
∂q=
Iθ1Iθ2Iθ3
(3mwr2 + 3J +H)ω
(3mw +mp)x(3mw +mp)y
,d
dt
(∂L
∂q
)
=
Iθ1Iθ2Iθ3
(3mwr2 + 3J +H)ω
(3mw +mp)x(3mw +mp)y
AT =
[JT
−KT
]
=
−a 0 00 −a 00 0 −ar r rξ1 ξ2 ξ3η1 η2 η3
, fi ≡[ξiηi
]
, i = 1, 2, 3, λ ≡
λ1λ2λ3
(212)
ATλ =
−aλ1−aλ2−aλ3
r(λ1 + λ2 + λ3)ξ1λ1 + ξ2λ2 + ξ3λ3η1λ1 + η2λ2 + η3λ3
, φ =
τ1τ2τ3000
(213)
155
∂L
∂q= 0 (214)
whence,
Iθ1 = τ1 − aλ1Iθ2 = τ2 − aλ2Iθ3 = τ3 − aλ3
(3mwr2 + 3J +H)ω = r(λ1 + λ2 + λ3) (215)
(3mw +mp)x = ξ1λ1 + ξ2λ2 + ξ3λ3
(3mw +mp)y = η1λ1 + η2λ2 + η3λ3
Furthermore, let us differentiate eq.(207) with respect to time:
Aq = −Aq (216)
where
A = [03×3 −K ] , K =
0 −ωeT10 −ωeT20 −ωeT3
−Aq =
0T3 0 ωeT1
0T3 0 ωeT2
0T3 0 ωeT3
θa
ωc
= ω
eT1 ceT2 ceT3 c
(217a)
Moreover,
Aq =
−a 0 0 −r −ξ1 −η10 −a 0 −r −ξ2 −η20 0 −a −r −ξ3 −η3
θ1θ2θ3ωxy
=
−aθ1 − rω − ξ1x− η1y−aθ2 − rω − ξ2x− η2y−aθ3 − rω − ξ3x− η3y
(217b)
Substituting eqs.(217a & b) into eq.(216), we obtain
aθ1 + rω − ξ1x− η1y = −ω(η1x− ξ1y)aθ2 + rω − ξ2x− η2y = −ω(η2x− ξ2y) (218)
aθ3 + rω − ξ3x− η3y = −ω(η3x− ξ3y)
Equations (215) and (218) now constitute a system of nine second-order ordinary differential equationsfor the nine state variables, the components of vectors q and λ, thereby completing the mathematicalmodel of the system at hand.
156
Appendix 1
157
158
>>
(2)(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
(4)(4)
>>
>>
(3)(3)
As expected, c is independent of
Now, compute LHS of eq.(4.17)
160
>>
(7)(7)
(6)(6)
(5)(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)
161
(7)(7)
>>
(8)(8)
(9)(9)
>>
>>
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)
As expected, RHS of eq.(4.17) is linear in [
162
>>
(10)(10)
>>
(12)(12)
(15)(15)
>>
(11)(11)
(14)(14)
>>
(13)(13)
>>
Now let's transform, by hand,
163
>>
Now we display the corresponding harmonic functions of the foregoing angles.
164
(16)(16)
(18)(18)
>>
>>
(17)(17)
Now let's substitute
165
>>
>>
(19)(19)
>>
(22)(22)
(20)(20)
>>
(21)(21)
Let P =
gives the harmonic functions of
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.
166
>>
(26)(26)
>>
(24)(24)
>>
>>
(23)(23)
(25)(25)
>>
Now we calculate the RHSs of eqs.(4.28a & b):
Error, (in simplify/table) too many levels of recursion
As Maple apparently cannot simplify r[1], let's do this subtraction manually:
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:
167
>>
>>
(31)(31)
(28)(28)
>>
(30)(30)
(27)(27)
(32)(32)
>>
(29)(29)
>>
>>
The harmonic functions of
which are the two inverse-displacement solutions for
168
(35)(35)
(36)(36)
>>
(34)(34)
(37)(37)
(33)(33)
>>
>>
>>
>>
>>
Let CC_5 & SS_5 denote
thereby obtaining expressions for the harmonic functions of
169
(38)(38)
>>
>>
(40)(40)
(39)(39)
>>
Let CC_6 & SS_6 denote
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] inorder to avoid confusion with scalar b[i]. No confusion with cos(theta[i]) =c[i] in FRMS, because we don’tuse 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 orientationgiven 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 (ft) - 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 (fb) - 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 andPBx45 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 matrixPB 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 matrixR 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 whiley 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)));
DD1 :=
0.0000000001 0.0 3.999999999
0.0 −0.0000000004 −3.9999999990.00000000001 0.0000000002 7.9999999970.00000000002 −0.0000000004 −4.00000000
> H:=delcols(DD1,3..3);
H :=
0.0000000001 0.0
0.0 −0.00000000040.00000000001 0.00000000020.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] inFRMS 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 thisworksheet. The notation c[i] is used instead of b[i] in order to avoid confusion with scalar b[i]. No confusionwith 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 therotation 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 (ft)— 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 (fb)—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 theclosure 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 x12with 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 andPBx45 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 matrixPB 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 matrixR bar, while PuB is the matrix on LHS of eq. (9.37a) Then, derive an 8x6 matrix A from RB, andcompute 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.3701278365If 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 whiley 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 mustbe rank-deficient, and hence, the determinants of the 3x3 submatrices of D1, subD1, subD2, subD3, andsubD4, 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> -3.766717755*cos(theta[4])^3-.6150485995*cos(theta[5])> ^2*sin(theta[5])-2.602128690*cos(theta[5])^3> *cos(theta[4])^3:
183
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 conditionnumber based on the Frobenius norm. This step is not essential in the Newton-Raphson procedure.
> det(FF[i-1]);
−664.9810536The 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.7302667The 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.8689818The 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.4330534The 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.85829260The 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]);
β1 :=(2α2 − 1/2
)(cos (ψ))
2+ 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 (α sin (ψ) + δ)2+ 1/2 (cos (ψ))
2)
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);
gi1 := −2 ρ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])]]));
Ri :=
[1/2 0
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);
R i :=
1/2 0
0 α δ(α+δ)2
+ 1/2 δ2
(α+δ)2+ 1/2 α2
(α+δ)2
=
[1/2 0
0 1/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));
Θ iso :=
[1/2 1/2
−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
cos2 ψ := 1− 16 α2δ2
(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));
Rii :=
2α2(4α2−1−4 δ2)
−1+4α2 0
0 1/2ρ2(4α2−1−4 δ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));
Rii :=
2α2(4α2−1−4 δ2)
−1+4α2 0
0 2α2(4α2−1−4 δ2)
−1+4α2
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
δ := 524
ψ := 1/6 π
Thet iso :=
[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
