Mock. Mach. Theacy Vol. 28, No. 2, pp. 261-269. 1993 0094-114X/93 $5.00 + 0.00 Printed in Great Britain. All rights tc~-tved Copyright (~ 1993 Pergamon Pre~ Ltd

THE SYNTHESIS OF SMOOTH CARTESIAN TRAJECTORIES FOR PICK-AND-PLACE OPERATIONS

OF SPHERICAL WRISTS

JORGE ANGELES and ZHENG LIU Department of Mechanical Engineering, Robotic Mechanical Systems Laboratory, McGill Research

Centre for Intelligent Machines, McGill University. Montreal, Qu6bec, Canada H3A 2A7

RALPH AKHRAS Canadian Centre for Automation and Robotics in Mining, McGill University, Montreal, Qu6bec,

Canada H3A 2A7

(Received 12 September 1990; received for publication 14 August 1992)

Al~tract--The off-line synthesis of Cartesian trajectories of spherical wrists capable of orienting their end effector arbitrarily in space is the subject of this paper. Thus, serial and parallel, non-redundant and redundant wrists are considered, for the method reported here is architecture independent. Motions undergone by spherical wrists are regarded as points on the surface of the unit sphere centered at the origin of the 4-D space of the variables used to describe the motion. Furthermore, the projection of this sphere onto the 3-D space of the invariant vector of the rotation under study, a solid unit sphere centered at the origin of that 3-D space, yields a straightforward geometric interpretation of this motion. Thus. a rigid-body rotation given as a smooth function of time appears as a smooth curve within the unit sphere of this 3-D space. Moreover. an explicit relation between the time derivative of the said Cartesian vector and the angular-velocity vector yields the kinematic interpretation of the velocity of the point tracing that curve. This concept is applied to the problem of trajectory planning for robotics pick-and-place operations, resorting to a normal trajectory.

I. INTRODUCTION

While the concept of particle motion is amenable to a geometric interpretation, that of rigid-body rotation is more elusive to such an interpretation. This may explain why most trajectory-planning schemes at the Cartesian-coordinate level limit themselves to the planning of the path followed by a point [!-4]. Relevant contributions to the geometry of rigid-body motions have been given in terms of line trajectories [5], the generalization of the well known Frenet-Serret relations of point trajectories--geometric curves--to include those of line trajectories--ruled surfacesmbeing the subject of Ref. [6]. The aforementioned point and line trajectories are generated by the single- degree-of-freedom motion of rigid bodies, the multi-degree-of-freedom case being reported in Ref. [7]. These concepts have been applied to problems of trajectory planning for continuous-path operations [8]. Moreover, in Ref. [9], the interpolation of both translation and rotation of a robotic end effector for continuous-path operations using B6zier curves, is proposed. A simpler method of continuous-path trajectory planning that takes into account both position and orientation of the end effector as well, was reported in Ref. [10]. An alternative procedure of Cartesian trajectory planning that includes both position and orientation is presented in Ref. [i i], where the orientation planning is done via interpolation, while keeping fixed the axis of rotation. However, to the authors' knowledge, the problem of Cartesian-trajectory planning ofpick-and-place operations for spherical wrists has not been discussed in the literature, except for Ref. [12], where trajectory planning for spherical wrists is proposed using parametric cubic spline.

Since point trajectory planning has been discussed extensively in the literature, this paper focuses on the planning of the rigid-body rotations undergone by the end effector of robotic spherical wrists, whether these are serial or parallel, and non-redundant or redundant. Mathematical tools aimed at describing rotations have led to the concepts of quaternions [13], dual numbers [14], screw algebra [15] and spinors [16]. The main problem in describing rigid-body rotations is the fact that the angular-velocity/s not a total derivative, i.e. no vector function exists whose derivative be the

261

262 JO[GE ANGELES eta/.

angular-velocity vector. On the other hand, since the times of the British School of Cayley, between 1846 and 1867[17], it has been known that rigid-body rotations can be represented as proper orthogonal tensors. However, the invariant relations between the time derivative of a proper orthogonal tensor that is a smooth function of time, and the angular-velocity of the motion it represents [18], have not been fully exploited. Basically three concepts have been used in deriving such relations, namely. Euler angles, Euler parameters and Bryant angles[19]. The latter are nothing but a variation of the Euler angles. Similarly, Rodrigues parameters are nothing but the normalized Euler parameters. However, Rodrigues parameters being unbounded quantities, lead to ill-conditioned relations [20]. On the contrary, Euler parameters yield very well-conditioned relations between their time derivatives and the angular-velocity vector [19, 20]. A drawback of Euler parameters, from the computational viewpoint, is that they are related to the rotation tensor via a quadratic relation, and hence, extracting them from the said tensor is not only computation- ally expensive--their extraction involves square-root operations--but also leads to double-sign ambiguities. As a good compromise between the latter feature of Euler parameters and numerical stability, the linear invariants of the rotation tensor were proposed in Ref. [21] for robotics-oriented rotation representation. These invariants are defined as the axial vector [22], or simply the vector, of the rotation tensor, and its trace. Due to the linear relation between these and the rotation tensor, their computation from the tensor components requires only five additions, and neither multipli- cations nor square roots, These features appear particularly attractive in robotics applications requiring real-time implementability. In this paper, the invariant representation of rotations, using the 4-D vectors of either linear invariants or Euler parameters, is used for the synthesis of smooth spherical Cartesian trajectories for pick-and-place operations. Unlike continuous-path operations, which entail a prescribed trajectory, in pick-aod-place operations the motion is specified only at the end points of the trajectory, the problem then requiring the synthesis of a smooth trajectory joining these two points. Obviously, the problem admits infinitely many solutions. The paper shows how to determine one of these trajectories in a systematic and simple fashion.

2. G E O M E T R Y OF RIGID-BODY ROTATIONS

Given a rigid-body rotation, or a rotation, for brevity, represented by a smooth tensor function of time t, Q(t), its linear invariants are defined as its vector, q(t) = veer(Q), and its trace, tr(Q). These can be defined in terms of the unit vector along the axis of the rotation, e(t), and the angle of rotation, ~(t), as follows [21, 22]:

vect(Q) = e sin ~, tr(Q) = 1 + 2 cos ~b. (1)

Alternatively, these can be presented in terms of the components of Q in a given orthogonal coordinate frame, namely, as

q~ = ~,skq*s, tr(Q) --- q., (2)

where q, denotes the ith component of vector q, and qkj denotes the ( k , j ) entry of Q. Moreover, CUk is the Levi-Civi ta tensor defined as + I if its three subscripts, which attain values from 1 to 3, are in cyclic order: - 1 if the said subscripts appear in anticyclic order, and 0 if one subscript is repeated. Moreover, the usual index convention [23] is adopted in equations (2). Because of reasons which will become apparent next, rather than working with the trace itself, the following alternative invariant, which is linear as well, is introduced:

q0 -= ½[tr(Q) - I] = cos 05. (3)

Now the 4-D vector, of linear invariants, 2., is defined as follows:

2. _--[qT, qo]T (4)

and hence, a rotation can be regarded as one point P of the 4-D space to which 2. belongs. Notice, however, that P, of position vector ,1., does not move freely in that space, for it is constrained to

Pick.and-place operations 263

remain on the surface of the unit sphere centered at the orion of that space, i.e.

a result that readily follows from equations (1) and (3). Had the quaternion of Euler parameters, q, been chosen for the representation of rotations, then the following relations would have been derived:

q - - [r T, to] T, (6a)

where

and hence,

r - • s in ~ , r0 -ffi - c o s ~ ( 6 b )

qTq = 1. (6c)

Alternatively, if ~ denotes the unique proper orthogonal square root of tensor Q, then r and ro can be calculated as the linear invariants of ,~Q, i.e. as

r = vect(,v/'Q), ro = ~[tr(,/-Q) - 1]. (6d)

Thus, as a rigid body undergoes an arbitrary smooth motion, P traces a smooth path on the surface of the unit sphere given either by equations (5) or (6c). Now, a geometry of rotations can be established, in which the concepts of distance and angle between two rotations can be defined. Notice, however, that in measuring the distance between two rotations, which in robotics applications is important when defining orientation errors, for example, the said distance will be defined as the arc length of the major circle of the unit sphere (5) or (6c) connecting two points, P: and P2, representing two given rotations or orientations. Indeed, such a major circle is a geodesic of the aformentioned surface.

Next, the direct and inverse relations between the angular-velocity vector of the motion under study and the velocity of point P, ~, are recalled. To this end, the definition of the angular-velocity tensor [24], fL is next given for quick reference, namely,

12 -- QQT (7a)

from which the angular-velocity vector cu is readily defined as

oJ = vect(f~). (7b)

Moreover, the invariant representation of Q in terms of e and O, is given as follows [18]:

Qffi l c o s 0 + ( ! - cos 0)e®e + sin 01 x e, (8)

where ® represents the tensor product [23] of the vectors beside it. Now, oJ is readily derived from equation (8) as follows: first, Q, as given by equation (8), is differentiated with respect to time; next, this derivative is multiplied by QT, va then being obtained as the vector of the arising expression, namely,

os --- sin 0~ + (1 - cos 0)e x ~ + e~. (9)

Next, from the definition of 2., the following is derived:

,~ = [&T sin ~ + #~e'r cos ~, -- ~ sin ~]r. (10)

From equations (9) and (10) one can readily derive the expression given below:

oJ=/I ~lTq q + qx~l - l + q 0 J~-q0 q0q, ( l la)

o r

co ffi L,f (1 lb)

264 JORGE ANGEL~ et al.

where L is the following 3 x 4 matrix:

[ q®q I - l x q - q ] , - - , (l lc) L = 1 l+q0 l+q0

which is the direct relation between the angular velocity and the velocity of point P. The inverse relation cannot be obtained directly from equation (1 lc), however, for matrix L, being of 3 x 4, cannot be interred. Nevertheless, if both sides of equation (5) are differentiated with respect to time, the following is derived:

~T~ =0. (12)

Now, equation (12) is adjoined to equation (llb), which produces a nonsingular system of four equations and four unknowns, namely, the components of ~. The inverse relation sought takes on the form:

,~ = Ato (13a)

where A is the following 4 x 3 matrix:

A =L F~ [tr(Q)_qT! - Q]I " (13b)

Similar relations follow for # and to, namely [18],

to = E#, $ = ~ETto, (14a)

where E - 2[r01 + 1 x r, - r ] . (14b)

Notice that relation (I I b) becomes indefinite if and only if qo = - 1. This is so because of the representation resorted to, namely, based on linear invariants. Indeed, when q0 = - I, ~ = n, and tensor Q becomes symmetric, its vector thus vanishing. The vanishing of veer(Q) implies, in turn, that, for this particular value of ~, this vector provides no information on the direction of the axis of rotation. This singularity presents, in practice, no major problem, for it can be predicted. Once angle q~ is detected to approach n, then, the orientation of the rigid body under study is simply measured from another reference configuration, thereby changing the value of ~. Alternatively, at this singularity, the relation between to and the linear invariants can be obtained by resorting to L'Hospital's rule, which thus produces the following relation [18]:

¢×ti t o = ~ - 4 , f o r d = n , (15)

~0 thereby obtaining the direct and inverse relations sought.

However, for purposes of graphical representation, it is not convenient to work in 4-D spaces, and hence, a 3-D representation of rotations, that is still invariant, is pursued. This is done by simply projecting the unit sphere of equation (5) onto the 3-D subspace ofq. Now, as long as q ~ 0, one point R of this space represents a rotation about an axis parallel to the unit vector e given as the normalized vector q, through an angle that is not unique. This angle is given, in fact, by sin-I(llq]l), which is double-valued. Clearly, the origin represents rotations about any axis of either 0 or n. In view of equation (5), however, point R cannot move freely in the 3-D space under study, for it must obey the following constraint:

Ilqll 2 ~< 1, (16)

which states that R moves within the unit solid sphere centered at the origin of the 3-D space of q. Clearly, the surface of this sphere represents the set of rotations of +n/2 about an axis defined by the line joining the origin with each point of the sphere. Thus, as long as ~ varies inside the open interval (0, n), point R describes a smooth trajectory inside the unit sphere given by equation (16). From equations (13a) and (b), and taking into account equation (5), the following is readily derived:

/I = ~[tr(Q)l - Q]to, (17)

Pick.and-place operations 265

which is the relation between the velocity of R and co. In Ref. [18], the inverse relation of equation (17) is shown to be the following:

2 w = tra(Q) _ 1 [Q tr(Q) + Q~l , (18)

which clearly exists as long as Itr(Q)[ # I, i.e. as long as ~ • + n/2. Thus, relation (17) is invertible only within the unit sphere (16), excluding the origin. At points on the surface of the aforementioned unit sphere, m can be computed recalling equation (I la) and the following:

~iTq = 0, qo = 0, for llqll = I. (19)

Hence, on the surface of the unit sphere consideration, i.e. for values of ~ = ± n/2,

co = ~I + q × ¢ - q0q, (20)

thereby completing all relations needed. In the section below, the problem of trajectory planning is defined for an end effector that is capable of attaining an arbitrary orientation. This problem is then solved in Section 4 using the concept of normal trajectory.

3. THE TRAJECTORY PLANNING OF SPHERICAL MOTIONS

The problem under study arises frequently when planning pick-and-place operations in robotics. In this problem, it is required that the gripper attain a prescribed orientation with a prescribed angular velocity at both the pick and the place configurations. Moreover, the motion undergone by the gripper between these two end positions should be smooth and take place in a given time period T.

Regarding now both Q and co as smooth functions of t, the end conditions of the problem stated above can be written as:

Q(0) = Q,, ¢o(0) = ¢o,, (21a)

Q(T) = Qr, co(T) = co~.. (21b)

It is moreover required that all Q(t), co(t) and &(t) be continuous functions of time in the closed interval [0, T].

The problem can now be readily stated geometrically in the 3-D q-space, namely, as:

Find a smooth trajectory inside the unit sphere centered at the origin that blends smoothly the points I and F, of position vectors q~ and q~, respectively, so that the tangents of the curve at these points be parallel to ¢D and 4~.

In the foregoing problem, the vectors involved are readily derived from the kinematic relations recalled in Section 2, namely,

ql -- vect(QI), qp = vect(Qr), (22a)

~1, = ½[tr(Q,) - Q,]co,, ~i~ = ~[tr(Qr) - Q~]co~-. (22b)

The foregoing trajectory-planning problem is solved in the section below resorting to the concept of normal trajectory.

4. TRAJECTORY PLANNING USING A NORMAL TRAJECTORY

First, the normal trajectory is defined. Following this, it is shown that any trajectory with prescribed end points and tangent directions at the end points can be synthesized by a simple homogeneous deformation of the normal trajectory.

Let two different points, I and F, be given, as well as two directions, defined by the unit vectors t~ and tr (Fig. I). It is desired to join these two points with a smooth curve whose tangent be parallel to the given unit vectors at the given points. Here, the tangent is given a direction identical to that

266 JORGE ANGEI.~ ef 01,

F

Fig. I. Synthesized trajectory in a 3-D view.

of its corresponding unit vector. This direction indicates that of the velocity of the point tracing the desired curve. Moreover, it will be assumed that the two points are a unit distance apart, the vector directed from I to F being denoted by f. Furthermore, it is assumed that tl, tp and f form a dextrous orthonormal basis of the 3-D space. This basis defines, in turn, a coordinate frame ,!, Y, Z with origin at !. Henceforth all vectors and matrices will be represented in this frame. These vectors, then, define a Cartesian coordinate system whose origin can be defined, without loss of generality, as I. The normal trajectory is synthesized as the intersection of the two cylinders of directrices shown in Figs 2 and 3.

l~ojmion on X-Y Plane 0 ~

-0.2

-0.3

.0.4

~- -0.5

-0.6

• 0.7 ~ j ~

41.8,

-0.9

0 0.1 02 0.3 0.4 0.5 0.6 0.7 0.8 0.9 I

X

Fig. 2. Projection of the synthesized trajectory on Xo Y plane.

Pick-and-place operations

1

0.9

0.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

~jmi~ m X-Z

oi, 012 "o13 014 06 01, o18 o; X

Fig. 3. Projection of the synthesized trajectory on X-Z plane.

267

The trajectory can be readily represented with parametric equations in cylindrical coordinates as

0 = ~ t, (23a)

• = a ~ , (23b)

b z = ~[cos( t + 1)~ + 1], (23c)

where t is the normalized time, i.e. 0 ~< t ~< l. The above coordinates are transformed into Cartesian coordinates using the following relations:

x -- • cos 0, y = - r sin 0, z = z, (24)

the normal trajectory being shown in Figs I-4. In general however, point F does not lie a unit distance away from I and the tangent vectors

are not orthogonal to line IF, although point I can always be thought of as being located at the origin of the coordinate system of interest and the unit tangent vector at I can always be defined identical to tl. In this, case, let the final point and the tangent, unit vector at this point be denoted by F' and t~-, respectively. The trajectory joining I ' -= I with F' and having the foregoing unit tangent vectors can be readily obtained via a suitable homogeneous mapping of the normal trajectory. Indeed, let this mapping be given by the constant 3 x 3 matrix M which, according to the rules of matrix representation [25], is given by

M = [t,, t~-, f]. (25)

Now, the desired trajectory is simply determined by mapping the normal trajectory with M. Note that the foregoing construction does not depend on the regularity of M, for this matrix need not be inverted. It can, in fact, become singular, which happens when the three vectors appearing in the right-hand side of equation (25) become linearly dependent, and the mapped trajectory becomes a planar curve. Thus, if p denotes the positive vector of a point on the normal trajectory and r

MMT 211/7.-G

268 JORGE ANGEI,f~ et ai.

Pro;_~_'~ oa Y -7. P ~

0.~

0.6

N 0.5

0.4

0.3

0.2

0.1

-I -0.9 -0.g -0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1

Y Fig. 4. Projection of the synthesized trajectory on Y-Z plane.

the position vector of the corresponding point in the desired trajectory, then the latter is obtained from the former as

r = Mp, (26)

thereby reducing the trajectory-planning problem to a set of matrix-times-vector multiplications. In fact, in the aforementioned coordinate frame, matrix M takes on the form

M = ~ , (27)

?

where 0t, ~ and ? are the direction cosines of the tangent at F', whereas #, o and ¢ are the coordinates of F" in the above-defined coordinate frame. Note that

0c2 +/~2 + y2 = !, (28a)

p 2 + o 2 + x 2 ~ < 1. (28b)

Hence, the trajectory is synthesized, for every sample point, with six multipl ications and four additions. I f N sample points are taken on the trajectory, then the number o f operations becomes 6N mult ipl ication and 4N additions, a quite handleable figure, even if N is relatively large, say 1000.

5. CONCLUSIONS

A method for the trajectory planning of Cartesian trajectories of spherical wrists, capable of orienting an end effector arbitrarily in 3-D space, was introduced in this paper. It was shown that wrist's motion takes place on a spherical surface centered at the origin of a 4-D space. For purposes of visualization in robotics applications, it was shown that the motion can be suitably projected onto a solid unit sphere. Hence, the rotation of a wrist, as the wrist follows a smooth path, appears as a 3-D path described by a point moving inside the unit sphere. In pick-and-place operations, the orientation and the angular velocity of a robot's wrist are prescribed at two finitely-separated configurations, and a smooth trajectory is required that blends both configurations. This trajectory

Pick-and-place operations 269

was synthesized in this paper by suitably mapping a precomputed normal trajectory. The computational complexity of the underlying procedure is reasonably low.

Acknowledfements--The research work reported here was possible under NSERC (Natural Sciences and Engineering Research Council, of Canada) Grant No. A4532, and FCAR (Fonds pour la formation de chercheurs et raide i hi recherche, of Quebec) Grant No. 88-AS-2517.

R E F E R E N C E S

I. R. P. Paul, I£EE Trans. Systems Man Cybern. SMC-9, 702 (1979). 2. C.-S. Lin and P.-R. Chang, I£E£ TranJ. Systems Man Cybern. SMC-13, 1094 (1983). 3. Y. ltoh, M. ldesawa and T. Soma, J. Robot. Systems 2, 191 (1986). 4. M. Brady, Robot Motion (Eds M. Brady et al.). The MIT Press, Cambridge, MA (1983). 5. J. M. McCarthy and B. Roth, ASME JI Mech. Des. 103, 718 (1981). 6. J. M. McCarthy, ASME JI Mech. Tronsmiss. Automata Des. 109, 101 (1987). 7. A. Ghosal and B. Roth, ASMEJI Mech. Transmiss. Autonum Des. 109, i l6 (1987). 8. B. S. Ryuh and G. R. paunock, Trends Developments Mech. Mac& Robo6cs--1988 (Ed. A. Midha), Vol. 3,

DE-Vol. (15-3), pp. 309 (1988). 9. F. L. Litvin and X. C. Gao, Trends Developments Mech. Mach. Robotics--1988(Ed. A. Midha), Vol. 3, DE-Vol. (15-3),

481 (1988). 10. J. Angeles, A. Rojas and C. Lbpez-Caj6n, IEEE J! Robot. Automat. 4, 380 (1988). I i. C. Sawada, H. lshikawa, K. Kawase and M. Takata, Proc. 1989 IEEE Int. Conf. Robot. Automat., Vol. 2, p. 808.

Scottsdale, AZ (1989). 12. J. Angeles and R. Akhras, Proc. 1988 IEEE Int. Conf. Robot. Autonmt., p. 68. Philadelphia, PA (1988). 13. W. R. Hamilton, A Source Book in Mathematics (Ed. D. E. Smith). McGraw-Hill, New York (1886). 14. E. Study, Die Geometric Jet Dynamen. Leipzig (1903). 15. R. Z. Ball, A Treatise on the Theory of Screws. Cambridge Univ. Press, Cambridge (1900). 16. W. pauli, Encyclopedia of Physics (Ed. S. Fh]gge), Vol. I. Springer, Berlin (1958). 17. E. T. Bell, The Development of Mathematics, 2nd edn. McGraw-Hill, New York (1945). 18. J. Angeles, Rational Kinematics. Springer, New York (1988). 19. J. Wittenburg, Dynamics of Systems of Rigid Bodies. B.G. Teubner, Stuttgart (1977). 20. T. R. Kane, P. W. Likins and D. A. Levinson, Spacecraft Dynamics. McGraw-Hill, New York (1983). 21. J. Angeles, Int. J. Robot. Res. 4(2), 21 (1985). 22. E. A. Fox, Mechanics. Harper & Row, New York (1967). 23. J. L. Ericksen, Principles Classical Mech. Field Theory, Encyclopedia of Physics (Ed. S. Fl~gge), Vol. II1/I. Springer,

Berlin (1960). 24. O. Bottema and B. Roth, Theoretical K~ematics. North-Holland, Amsterdam (1979). 25. G. Strang, Linear Algebra and its Applications. Academic Press, New York (1980).

LA S Y N T H E S E D E T R A J E C T O I R E S C A R T E S I E N N E S S O U P L E S P O U R

D E S O P E R A T I O N S D E T R A N S F E R T A U M O Y E N D E P O I G N E T S

S P H E R I Q U E S

R/mmt--Cet article pone sur la synth(~se en diff(~r~ des trajectoires cart~iennes de poignets capables d'orienter leur organe terminal de fafon arbitraire dans respace. La m~thode en question ~tant totalement ind,L'pendante de rarchitecture du robot, elle est applicable aux robots de type s~rie ou parall(~le et aux robots non-redondants ou redondants. Duns cette optique, les mouvements effectu~ par de tels poignets sont consid~r~ comme des points sur la surface de la sph(~re de rayon unit~, qui est centr~e i I'origine de i'espace i quntre dimensions utilis~ pour la description de ces mouvements. En outm, la projection de cette sph(~re dans I'espace tridimensionnel qui est associ~ i rinvariant vectoriel de la rotation en question, i savoir une sph(~re solide centr/~e i rorigine de cet espace tridimensionnel, donne une interp~tation g~m(~trique directe de la rotation qui fait robjet de cette (~tude. Ainsi, uric rotation de corps rigide donn~e comme fonction souple du temps, apparalt comme une courbe souple dens la sph6re i rayon unit~ dudit espace tridimensiounel. En outre, la relation explicite entre la d~riv6e temporelle de rinvariunt vectoriel et le vectenr vitesse angulaire donne une interpretation cin~matique de la vitesse du point trafant la courbe mentionn~ ci-dessus. C.e concept est alors appliqu(~ i la K'solution du probh~me de planification des trajectoires pour les Ol~rations robotiques de transfert au moyen d'une trajectoire dite normale.