169

Theoretical Description of Robotic Mechanisms

Kinematics of Common Industrial Robots

John Lloyd and Vincent Hayward 1. Introduction Computer Vision and Robotics Laboratory, McGill Research Centre for Intelligent Machines, McGill University, Montreal, There are two ways to approach the problem of Quebec, Canada inverse robot kinematics: numerically and sym- bolically. Numerical treatments [1] are general, An approach to finding the solution equations for simple manipulators is described which enhances the well known and can be made to yield clean solutions in the method of Paul, Renaud, and Stevenson, by explicitly making presence of singularities, but are computationally use of known decouplings in the manipulator kinematics. This burdensome for real time applications. By con- reduces the set of acceptable equations from which we obtain trast, analytical solutions, optimized for a particu- relationships for the joint variables. For analyzing the Jacobian, such decoupling is also useful since it manifests itself as a lar robot, can be quite rapid. The major drawback block of zeros, which makes inversion much easier. This zero is that working out such a solution may not al- lock can be used to obtain a concise representation for the ways be possible (although it usually is for most forward and inverse Jacobian computations. The decoupling industrial manipulators). In the general case of a also simplifies the calculations sufficiently to allow us to make robot with 6 revolute joints, it has been shown good use of a symbolic algebra program (MACSYMA) in that the solution for the tangent of the half angle obtaining our results. Techniques for using MACSYMA in this way are described. Examples are given for several industrial of each joint is a 32 degree polynomial of the manipulators. coordinates of the last link [12]. Attempts to de- rive an explicit form for this polynomial have been unsuccessful, although it has been shown to be Keywords: Robots, Kinematics, Jacobian matrices, Automated equivalent to both a 16 x 16 determinant equation mathematical derivations.

Vincent Hayward was born in Paris, John Lloyd was born in Victoria, France, on January 5, 1955. He re- Canada, on April 22, 1958. He re- ceived the Dipltme d'Ing~nieur from ceived a B.Sc. in physics from McGill Ecole Nationale Sup~rieure de University in 1980, worked briefly in M~w.anique de Nantes, Nantes, France, the wilds of northern British Col- and the Dipltme de Docteur-Ing~nieur umbia, and returned to McGill and from Universit6 de Paris XI at Orsay received an M.Eng specializing in in computer science, in 1978 and 1981, robotics in 1985. He then worked for a respectively. year designing controllers and build- From December 1981 to December ing laser vision systems for welding 1983, he was at Purdue University, in robots. During the last two years, Mr. the Department of Electrical En- Lloyd has been involved in setting up gineering, the first year as a Visiting robot control environments for pro- Scholar sponsored by CNRS's ARA program (Automatique et jects at RCA/General Electric, NASA, and the Jet Propulsion Robotique Avancte), and the second year as a Visiting Assis- Laboratory. His most recent work has been (with Mike Parker) tant Professor. There, he developed RCCL, a robot control and the development of RCI (Real-time Control Interface), a programming system. He then joined CNRS at the LIMSI package for creating real-time control tasks in a multi-CPU laboratory in Orsay, where he worked as attach6 de Recherche VAX/UNIX environment. His research interests include robot on trajectory planning and spatial reasoning until May 1985. kinematics and control, computer languages and real-time He is now Assistant Professor with the Department of Electri- computing, and the software engineering aspects of sensor- cal Engineering at McGiU University, where he teaches a driven robot and multi-robot systems. He currently lives in course on Artificial Intelligence, and a Research Associate with Montreal and is pursuing a Ph.D. at the McGill University the McCAll Research Center for Intelligent Machines. His Research Center for Intelligent Machines. research interests and publications are in the following areas: robot programming and control, 3-D imaging, computational geometry, spatial reasoning, computational architectures, space North-Holland and remote applications of robotics and telerobotics. Robotics 4 (1988) 169-191 Dr. Hayward is member of IEEE.

0167-8493/88/$3.50 © 1988, Elsevier Science Publishers B.V. (North-Holland) 170 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

[2], and a system of eight second degree equations we wish to find, and the hand coordinates and the [131. joint angles we have already solved for: Some of the original work on obtaining inverse A1A2A3A4AsA 6 = T 6 (2) kinematic solutions was done by Pieper, who enu- merated special cases in which a closed form A2A3A4AsA6 = A1-T6 solution is feasible. This includes the case where A3A4AsA6 = A~-lA11T6 any three adjacent joint axes intersect, which is often true of the last three joints (or "wrist") of many industrial robots and defines the class of We present here a variation on this approach. "wrist partitioned" manipulators. The kinematics Assume that there are j adjacent joints whose of these robots have been extensively studied kinematics decouple from those of the N-j sur- [3,4,9]. rounding joints. If the products of the transforma- The best known method for determining a tion matrices of these joints is denoted by C, and dosed kinematic solution is the one described in the matrices for the surrounding joints are given [7,8] which involves systematically exploring vari- by U a and U b, then we have ous kinematic relationships and picking out the UaCUb = T 6 (3) best ones to yield solutions. In this paper, we adapt this technique so that it is more directed by If we can use the decoupling to find an ap- the special geometry of the manipulator in ques- propriate set of equations for the other N-j tion. This limits the search for a workable set of joints, then this equation can be rewritten as as equations and is justifiable since special geometry C = O a- 1T6Ub 1 (4) is required to solve a manipulator anyway. We will where the right hand side is known. Then, if also show how to use the same in determining the necessary, we can resolve C into its component manipulator Jacobian and its inverse. Two special matrices and apply the procedure of (2) to obtain geometries that will be considered in particular are the equations necessary to solve for the remaining the cases where either three adjacent joints axes j angles which comprise C. intersect, or three adjacent axes are parallel. The This approach is useful because it breaks the approach also lends itself to the use of a computer problem cleanly into two smaller problems which algebra program (in particular, MACSYMA *), and are easier to solve, even if it still becomes neces- we will describe how we used this effectively in sary at that point to resort to a numerical tech- obtaining our results. nique to find the remainder of the solution. It is highly applicable to commercial robots which al- most always (intentionally) decouple in some 2. Forward and Inverse Kinematics straightforward w~ty. Conversely, the idea may be used in reverse to provide guidelines for robot 2.1. Finding the Equations design. If C decouples totally, then for an arbitrary The forward kinematics for a manipulator are adjacent transform M, either MC or CM contains always straight-forward to derive: N-j independent components which do not de- T 6 = AIA2...A N (1) pend on C at all. If the operator S groups these where T 6 is the position of link 6 relative to the components into a column vector, then for the robot base, A i is the well known "A" matrix for "MC" case we have link i [6,7], and N is the number of links. UaCUb = T 6 (5) A method for finding the inverse kinematics is UaC = T6Ub- 1 described in [8], where (for a six link arm) we search the following set of equivalent equations S(Ua) = S(T6Ub-1) (6) for simple relationships between the joint angles and, similarly, for the "CM" case: S(Ub) = S(U~ 1T6) (7) * MACSYMA is a large symbolic mathematics program distrib- uted by Symbolics, Inc., Cambridge, Mass. In the manner of (2), we can use these relations to 3". Lloyd, V. Hayward / Kinematics of Common Industrial Robots 171 obtain different sets of equations by taking (5) The rest of the problem can be solved by applying and successively either premultiplying by the in- method (2) to C alone: verse components of Ua, or postmultiplying by the A4AsA 6 = C (15) inverse components of Ub. In formulating C, it may be convenient to allow AsA 6 = A41C (16) the component matrices to differ slightly from the A 6 = A~- 1A41C (17) strict "A" matrix definition. We can do this as follows. An "A" matrix is the product of four If C comprises a set of intermediate links, such as components: A rotation about 0 about the z axis, (for example) 3, 4, and 5, then we obtain a differ- a translation d along the z axis, a translation a ent version of (11)-(13): along the new x axis, and a rotation a about the T61A1A2e4 = A61e4 (18) new z axis: A1A2e4 = T6A6 le4 (19) A = RoTaToR ~ (8) A2e4 = A~- 1T6A61e4 (20) For a joint described by A i, we may migrate some of the constant terms into the adjacent matrices along with equations analogous to (14)-(17) to Ai_l or Ai+ 1, requiring only that A i remains solve for the component of C. dependent on its joint variable. Caution should be Another case of interest is when we have three exercised in doing this since some analysis tech- rotational joints whose axes are parallel to each niques involving "A" matrices do in fact assume other. If we use one of these axes as a reference, the standard definition of (8). then the three joints form a complete two dimen- We shall now study some applications of the sional system with one rotational freedom about approach, beginning with the well studied instance the axis and two translational freedoms per- where three joint axes intersect. This makes it pendicular to it. If we define this axis to be z, then possible to formulate as C for the joints in ques- C can be defined as tion which has no translational component, which [cos(0) -sin(0) 0 Pc,,] implies that c= / sin(O) cos(O) 0 p.i (21) o o o] / ° o o where R is a 3 X 3 rotation matrix. Premultiplying this by any transform X leaves the fourth column where Pox and Pcy are the net displacements, and invariant: 0 is the net rotation. Post multiplication CX of this matrix leaves the 3rd row invariant, which XCe4 = Xe 4 (10) allows us to establish (where e i is defined by el(j) = 0 for j 4: i, ei(i ) = ercx = e~'X (22) 1). In the well known "wrist partioned" case, C If we assume, for example, that C is formed from comprises the last three joints, so that angles 2, 3, and 4 (as is the case for the "elbow" manipulator), then AIAEA3C = T 6 A1CAsA 6 = T 6 Applying (6) and (10) allows us to generate the following equations: and applying (7) and (22) gives the following equations: AIA2A3e4 =Tte 4 (11) e~'A 5 = e3rA11T6A61 (23) A2A3e4 = Ai- aTte4 (12) e3rAsA6 = eTAIlT6 (24) A3e4 -- A21A~- 1T6e4 (13) This provides enough information to determine e &A6X -I = ,gAi (25) A 1, A2, and A 3. C can then be determined from This yields a solution for angles 1, 5, and 6. C is equation (4): then known from (4). C = A31A21AllT6 (14) C = nl 1TtA61A~ 1 (26) 172 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots and given (21), is it easy to solve for the 3 compo- cos 0j. We will also make use of the function nent matrices which comprise C (see the end of atan2(y, x), which takes two arguments, y and x, Section 2.2). proportional, respectively, to the sine and cosine Decouplings similar to the three parallel axis of some angle 0, and returns the value of that case can also occur for combinations of prismatic angle in the range - rr < 0, < = rr. This method of and revolute joints. Prismatic joints are generally performing inverse trigonometry is preferred be- much easier to deal with since they introduce no cause it is numerically well behaved and easily rotational motion. The case where any three adjac- resolves ambiguities about what quadrant the an- ent joints are prismatic is easy to solve, since it gle is in. represents a C in which the rotational component, Once a reasonable set of equations has been if not 0, is at least constant. If Ra, Rc, Rb, and R 6 determined by the methods discussed above, solu- are the rotational submatrices of U a, C, U b and T6, tions are obtained in the same way detailed in [7]: respectively, then R C and R 6 are known and we The individual elements are examined for simple can formulate our base equations from the rela- relationships describing the joint variables. Some tionship of the common forms of these relationships for revolute joints are described here, along with their RaReR b = R 6 (27) solutions. and the component matrices of Ra and R a, which In the most ideal case we will find two equa- together contain only 3 independent variables. tions of the form Lastly, we consider the case where only two as, = k 1 (29) axes intersect. Though this problem is certainly not always tractable, it is common to most mani- ac i = k 2 pulators and hence worth some discussion. As in where k I and k 2 are given or known from a the case where three joints intersect, it is possible previous part of the solution. 0, can then be to construct a C matrix which has only a rota- determined unambiguously from tional component (though using only two compo- nent matrices this time). The difficulty is that the 0/= atan2(k~) (30) resulting equations we can generate for the trans- lational part of the problem will depend on four except when a = 0, which probably indicates a joint variables instead of three. Even if these rela- singularity. Alternatively, we may find an equa- tionships are simple, we will still need an ad- tion ditional equation to solve for the subsystem. This as i 4- bc i = k (31) can be found if the rotational component of C whose solution may be expressed either as [7] contains any constant entries (which is true when any of the component "twist" angles a, are multi- k 0/=atan2( +_~/a2+b2-k2 )-atan2(b) (32) plies of ~r/2). We can then find the necessary equation from or [10], a c = RT~R6R~ (28) Oi=2atan2( a++-v/a2+b2-k2 ) by equating these elements which are constant on k + b (33) the left hand side with their corresponding ele- If such relationships are not explicitly available, ments on the right. This constraint is actually the they can often be found by squaring and combin- one commonly used to solve the elbow manipu- ing neighboring equations. Sometimes we may only lator [7,10], although in that instance it is not have a convenient representation for either the actually needed because of the three parallel axes. sine or the cosine term. This can still be resolved using the above equations; if we have an expres- 2.2. Solving the Equations sion for the cosine, then we can use (33) with a=0and b=ltoget In the rest of this paper, we will use the com- mon kinematician's shorthand where s i ~-sin 0,, 0,= 2 atan2(+_ c------~2!/1)l+c - (34) c i = cos 0 i, sij = sin 0 i + sin 0j, and cij = cos 0 i + Z Lloya~ V. Hayward / Kinematics of Common Industrial Robots 173

Another possibility is that an equation of the form in any treatment of the manipulator Jacobian, (31) is be complemented by an additional equa- where it will manifest itself as zero entries. It tion to give a linear relationship, might even be preferable to work out the Jacobian alsi + big = k 1 (35) before doing the kinematics (in analyzing a manipulator, one usually finds themself doing a2s i + b2c i = k 2 both, anyway) in case a partitioning is revealed which may be solved to give si and c i explicitly. that was not previously noticed. These are then used as arguments to atan20. It is well known that the Jacobian can be We conclude this section with the solution of C greatly simplified if it is expressed in some alter- for the three parallel joint case discussed above. nate frame k, instead of the canonical frame (which For notational convenience, we will assume that is usually in link 6 of the manipulator). This frame the three component joints are 2, 3, and 4. If the k is typically located in an intermediate manipu- translational offset for each link is a~*, we then lator link [11]. Such simplification is of particular have interest for inversion purposes, although it must be remembered that it is then necessary to map C234 -- 5'234 0 Pcx 1 Cartesian forces and velocities to and from frame 5'234 C234 0 Pcy k. 0 0 1 ~] In establishing a simple (easy to invert) version 0 0 0 of the Jacobian, we are interested that it not only be sparse, but also that it be block triangular, if c234 ~s234 0 a2c 2 + a3c23 -4- a4c234 possible. In particular, for wrist partioned mani- = / 5'234 c234 0 a25"2 --[- a35'23 -4- 17/45'234 pulators, where the last three joints do not con- | 0 0 1 0 tribute to translation, the Jacobian is of the form 0 0 1 (36) (J. o) S = ~321 322 (38) The solution to this can be determined with the law of cosines and a little algebra: Lets begin by reiterating the formulae for the column vectors Ji of J expressed in frame k: If the ( :i: ~1 + c2 ) (37) coordinate transform from frame k to the base 03 = 2 atan 2 1+ c 3 frame of axis i (located in link i - 1) is given by the matrix Ki, where a 2 + a32 ~d 2x - d;2 C3 = 2a2a 3 Ki=( Ri0 Pi)= (0 i Oi0 a~0 P~) (39,

dx =Pcx- c234a4 then we have, using a notation similar to that in dy = Pcy - 5'234a4 [14]: j, = (p, x a, / (a3s3dx+(a3c3+a2)dy) ai ] (rotary joint) (40) 02 = atan 2 (a3c 3 + a2)d x + a3s3dy and 04----atan 2( s23-----4) --_ 03 = 02 = ( ai C234 J, 0 ) (prismatic joint) (41) Note that there is a singularity at 03 = 0. (The perhaps more familiar formulae given in [7] are obtained using K71 instead of Ki.) These equations can be used to establish some 3. Forward and Inverse Jacobians characteristics of the simplifying cases considered in section 1. If the three rotational joint axes The decoupling which we have considered in intersect, and we chose our frame k to be coinci- the above section on kinematics is also important dent on this point of intersection, then p,. will be zero for each of the three columns associated with * Because the joints are parallel, d i can be set to 0. the joints in question. The top three elements of

J. Lloyd, K Hayward / Kinematics of Common Industrial Robots 175 then our computations arrange to look something 4. Using MACSYMA to Determine the Kinematics like: The methods described above are easy to im- plement using a computer algebra program. We de = J21 dO1 + J22 dO2 will describe a few details about doing this here, ( Jl~l del ) (52) using MACSYMA as.an illustration. dO = J221(-J21 dO1 + dc2) 4.1. Matrices [ J~ql + J2~q2/ (53) t= 1 Ja~q2 ] Most of the MACSYMA work is done using the matrix routines along with the regular algebra ( j~lT(tl- jTqt) ) (54) facilities. The matrix functions provide for most common matrix operations, including multiplica- q = I J~ 1Tt2 tion (.) and inversion (invO). Column vectors are Likewise, the easiest way to invert the submatrices also represented as matrices. Multiplying a col- Jn and J22 is to solve explicitly for the vector in umn vector v by the inverse of a matrix A 1 would question. If we have the matrix equation My = x, look like where y = (Yl, Y2, Y3) T and x = (xl, x2, X3) T are inv(A1) . v; arbitrary vectors, then we use the notation sol(M) to denote the product M-ax. This is best found in Basic algebraic simplification is performed the usual way by putting the composite matrix using the function ratsimpO. (Mx) into echelon form and then solving for y recursively (dements of the solution vector y may 4.2. Trigonometric Simplification hence be explicitly contained in the expression for sol(M)). The solution to equations (52) and (54) is Although MACSYMAhas rules to handle trigono- completed by finding sol(Jll), sol(J22), sol(J~), metric constructions, we found that it was faster and sol(J2~). (and more compact) to use our own trigonometric As was mentioned a little earlier, if the Jacobian notation and provide the required identities ex- is derived in an intermediate frame k, then we plicitly. The trig expressions contained only sines have to map quantities to and from the frame of and cosines, which were represented using the interest (often the frame in link 6). If the trans- convention: form from frame k to the frame in link 6 is K6, then the matrix D6k which transforms velocities siX = sin(0 x) from e to k, and its inverse, Dke, are given by coX = cos(0x) siXY = sin(0 x + Or) R 6 ] 0 R T ] coXY = cos(0 x + Or') (55) The only trig identities which we found neces- where the notation p x R indicates a matrix whose sary were columns are the cross products of p and each of siX^2 + coX^2 = 1 (57) the columns of R. Similarly, the force transforma- tions F6k and Fk6 are given by siXY^2 + coXY^2 = 1 (58) and

p6XR 6 R 6 (p6 × R6) T R~ sinXY = siXcoY + siYcoX (59) (56) cosXY = ciXcoY- siXsiY (60) Clearly, these transformations are much sim- ~CS~-A allows users to specify a single sub- pler to work with if there is no translation be- stitution with the function ratsubstO, or a list of tween frames k and 6. substitutions with the function lratsubstO. The 176 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

identities (57) and (58) can be wrapped into a FOR i THRU 3 DO function tsimpO, defined as r." addrow(r, [0]), tsimp(x) := lratsubst([tidl, tid2, tid3 .... tid12, r);

tid23 .... ], x) For exchanging rows and columns of the where tidX and tidXY correspond to identities Jacobian, we have defined exrow(m, i, j) and ex- (57) and (58) for the required joint angles. col(m, i, j), which exchange rows (or columns) i Identities (59) and (60) are important whenever and j of m: the axes of two consecutive joints are parallel. For any two joint angles X and Y, we can define a exrow(matrix, i, j) := block( function combineXY 0 which combines angles, and It, t1, a function expandXY 0 which expands them. r: copymatrix(matrix), MACSYMA has some difficulty combining angles t ." r[j], rD'] ." r[i], r[i] : t, which are in the midst of a large expression, r)," although this is usually not a problem since it is excol(matrix, i, j) .'= block( possible to combine the angles before making the Jr, t1, expression complex. For example, if we have the r: transpose(matrix), matrices A1, A2, A3, and A4, then t: r[j], r[j] ." r[i], r[i] ." t, combine23(A 1. A 2. A 3. A 4); transpose (r)); might have some difficulty, whereas Lastly, the functions usolve(m, y, x) and A1. combine23(A2. A3). A4; lsolve(m, y, x) can be used to determine sol(m) for would succeed. It is often useful to assign such a matrix which is either closer to upper triangular, predetermined products to a variable, as in: or closer to lower triangular, respectively. The A12 : combine12(A1. A2); arguments x and y provide symbolic descriptions of the input and output vectors. For analyzing the Jacobian, several support functions were written. These include jacobcolr(m) usolve(m, y, x) := block( and jacobcolp(m), which return a column of the [ue, no1, Jacobian for revolute and prismatic joints, respec- nc : length(m[1]) + 1, tively, given the transform matrix m between the ue : uechelon(m, x), frame of the joint in question and frame k. For col(ue, nc) - (submatrix(ue, nc) historical reasons, our implementation of these - ident(nc - 1)). y)," functions uses the formula given in [7], rather than (40), so m is the inverse of the matrix K i described lsolve(m, y, x) ,= block( in Section 2. [ue, ncl, nc : length (re[l]) + 1, jacobcolr(m) := block([r], ue : lechelon(m, x), r:matrix( [m[2, 1] * re[l, 4] col(ue, nc) - (submatrix(ue, no) -m[1, 11 • m[2, 4]), - ident(nc - 1)).y)," r." addrow(r, [m[2, 21.m[1,41 -- rail, 2] * m[2, 411), uechelon(m, x) .'= echelon(addcol(m, x))," r: addrow(r, [m[2, 31"mH, 41 -- rail, 3] * m[2, 411), lechelon(m, x) := block( FOR i THR U 3 DO Jr, ncl, r: addrow(r, [m[3, ill), nc : length(m[1]) + 1, r); r: echelon(addcol (flipmatrix(m), reverse(x))), addcol (flipmatrix(submatrix(r, no)), jacobcolp(m) := block ([r], reverse(col(r, Nc))))," r: matrix( [m[3, 1]]), r: addrow(r, [m[3, 2]]), flipmatrix(m) r: addrow(r, [m[3, 3]]), := reverse(transpose(reverse(transpose(m)))),. J. Lloyd, V. Hayward/ Kinematicsof CommonIndustrial Robots 177

5. Examples Since this is a wrist partitioned manipulator, we start with equations (11)-(13), which yields several In the following examples, the elements of T 6 equivalently useful relationships. Equation (13) will be denoted in the usual way. evaluates to n X Oxa).x ( d4s3 I ( PyS12+c12Px-axc2 ) ny oy ay py T6= (61) -c3d41 = Pz + d2 (64) nz Oz az 0 0 0 °[13] -oIs2+pxSl2-cl2PYl

while the dements of C will be denoted by The second row gives a single cosine relationship ncx ocx acx Pc~ for 03 . Ocy acy Pcy Squaring and adding rows 1 and 3 gives the Ocz acz Pcz (62) equation 0 0 1 d4s2 32 + d 2 = -2atpyS 1 +p2y+p2_2aaclp~,+a2 (65) 5.1. Example 1: The ETL Robot which then provides a solution for 01. Equation (12) gives us This is a 6 revolute joint robot designed at the Japanese Electrotechnical Laboratory (ETL). (Fig. [e2d4s3+d3s21[pySl+clPx-al ) 1 ), The "A" matrices for this robot are: d4s2s3-c2d3 [ = I clpY-pxSl (66) ¢1 --Sl 0 alc 1 A t ----- S1 Cl 0 a151 (63) 0 0 1 0 Rows 1 and 2 give a set of equations for s 2 and e z 0 0 0 1 of the form sz 0 ) d4s3 d3 c:o k21 ) (67) A2 = 0 -c2 0 1 o -(2 which gives a solution for 02. A solution for 02 0 0 c 3 0 S3 d4s3 ) ® A3 = s 3 0 -- C3 -- c3d4 0 1 0 d 3 A0 0 0 0 1 I Ca 0

A4 = S 4 0 C4 0 -1 0 0 0 0 c5 0 s 5 0 A5 = s 5 0 -c5 0 0 1 0 0 @ 0 0 0 1

C -- S 6

A 6 = $6 C6 0

0 0 1 0 0 °i)0 Fig. 1. The ETL robot. 178 3". Lloyd, V. Hayward / Kinematics of Common Industrial Robots

could also be found by squaring and combining a frame defined by AIA2A 3. Because axes 1 and 2 pair of equations, as was done for 01 . are parallel, columns 1 and 2 can be combined: One we have solved for the first three angles, Ja =Jl -J2 (72) we can solve for the wrist angles using equations The Jacobian relationship then takes the form (14) and (15)-(17). These equations are relatively simple since the wrist contains no offsets and the dr. twist angles are multiples of ~r/2. dy Elements (1.3) and (2.3) of equation (15) give a dz relation for angle 04: d~ C4S do 68, dep We note the singularity when 05 = 0. ' alC3S2 c3d 3 d 4 0 0 0 Elements (1.3) and (2.3) of equation (16) yield a --O1C 2 - d4s 3 0 0 0 0 relation for 05: OIS2S 3 d3s 3 0 0 0 0 (acy, axc4 ) 0 s 3 0 0 - s 4 c 4 s 5 0 0 1 0 c 4 $4s 5 Finally, 06 can be determined from elements 0 - c 3 0 1 0 c 5 (1.1) and (2.1) of equation (17): d01 C6 ( cS(ncyS4"lt-c4Hcx)-BczS5 ) (70) dO2 - dO 1 ($6)= \ C4ncy--ncxS 4 dO3 The complete inverse kinematics for the ETL X dO4 (73) robot can now be summarized: dO5 +_~d4-(pz+d2) dO6 0 3 = 2 atan2 ~-j ~_ ~2---~/4 (71) Permuting columns and rows to optimize the tri- angularity of the diagonal blocks gives us the (2alPy+~4a2p2+4a2p2-k21) following: 01 = 2 atan2 kl 1 + 2alp x kla=p~+pZ+a2_ d4s322 d 2 clz dx 82= 2 atan2( dak2____Al+ d__4sak2_____~2 ) dq~

d4sak21 - d3kE2 dp k21 =pyS1 + Clp x -- a 1 dq~ k22 = C1Py -- pxs1 -- ale 2 -- d4s 3 0 0 0 0 04=atan2(a Yt als2s3 d3s 3 0 0 0 0 ~acx/ alc3s2 c3d 3 d 4 0 0 0 0 s 3 0 c4s 5 - s 4 0 05=atan2( acys4+acxc4)acz 0 0 1 s4s5 c4 0

0 -- C 3 0 C 5 0 1 C4ncy -- ncxS 4 ) 06 = atan2 C5 ( nc -2--y~4+ c4ncx)~ -- glc2S 5 dO,,_ dO 2 -- dO1 where the rotational component of C is defined by eo~ (ncx.acx) (c2c3 s2 c12s × (74) Hcy Ocy acy = c3s12 -- c12 s12s3 ] dO6 ncz ocz acz s 3 0 --C 3 ] dO~ We derive the Jacobian for the ETL robot in the dO4 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots 179

We will now solve for the various blocks of J respectively: using the notation described in section 2. Making the substitution c3d3Y3+d3s3Y2-X2 d4s3 h = ald4S2s 3 - alC2d 3 (75) c3hy3+alc2X2-d4s3x 1 and solving the equation JuY = x yields sol(J r) = _ (79) hs 3 d4x 2 + d3x 1 13 h d4 alc2y 1 + X 1 sol(Jll ) ----- d4s3 (76) C4c5y 3 + $4S5X 2 -- C4X 1 $5 c3d3y 2 + alc3s2y 1 - x 3

sol(J r) = _ Css4y3 -- C4S5X2 -- S4Xl (80) d4 S5 Solving for J22 yields, initially, X3 S4X 2 + C4X 1 The rest of the solution follows from equations $5 (51)-(54). S4S5Yl -- X2 (77) C4 5.2. Example 2: The Elbow Manipulator x 3 -- c5y 1 but this is not quite correct since the c 4 term in The elbow manipulator (Fig. 2) is a common the denominator of the second row cancels out industrial robot in which the axes of joints 2.3, (this can be determined in advance by examining and 4 are parallel. The "A" matrices for this the determinant of J22 and noticing that there are robot, with A 4 and A 5 modified so that the z axis no singularities at c 4 = 0). Substituting for Yl in defined by A 4 is parallel to the z axes of A 2 and the second row gives A3, are: S4X 2 + C4X 1 c 0 s 1 0 sol(J22) = s, (78) 0 -q 0 C4X 2 -- S4X 1 A1 = s1 (81) 0 1 0 0 x 3 - c 5),1 0 0 0 1 Similarly, we can find solutions for jr and ,IT, C2 -s 2 0 a2c2 I ® A 2 = S2 c 2 0 0 0 1 ao) 0 0 0

° t -s 3 0 a 3C3 c 3 0 a3s3 3/ci 0 1 0 0 0 1

C4 -- S4 0 a 4C4 1 c 4 0 A 4 = $4 0 0 1 0 0 0 ai' ) c 5 0 s 5 As = 0 1 0 --S 5 0 c 5 Fig. 2. The elbow manipulator. 0 0 0 180 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

The results for the elbow manipulator can now be summarized: A6= $6 ¢6 0 0 0 1 0 0 0 0 a = 2 atan2 _---p--y (87)

Since this manipulator has three parallel axes, we can first use equations (23)-(25). We find equa- 06 = atan21 °xsa -- cl°y ] tion (24) (presented in column form) to be Clny -- HxS 1 !

05 = atan2 sl( c6nx - °xs6) : Cl(----~C6ny -- OyS6) ] c6S5 {nxSl--Clny 1 axS 1 -- ayC 1 $5S6 =loxs1 C10y I ! C5 laxSl ay¢l I (82) $234 = --¢50zS6 q- azS 5 -[- C5C6H z 0 ~ pxSl C1 py } C234 = nzS 6 + C60z Pcy = Pz The 4th row of this gives a relation for 01, while Pox =pySl + caP~ rows 1 and 2 provide a relation for 06. We note the The remainder of the solution follows from equa- existence of a singularity when 05 = 0. tions (37). Next, we examine equation (23): We can use the same "A" matrices to derive a Jacobian for the manipulator in the frame k de- --S 5 ¢SI ( C6H x -- OxS6 ) -- ¢1(¢6Hy -- OyS6 ) fined by AIA2A3A 4. Since three axes are parallel, 0 sl ( nxS 6 -1- C60x ) -- ¢l ( nyS6 --t- ¢60y) we can simplify the Jacobian by combining col- umns. Specifically, we take ¢5 axS 1 -- ayC 1 0 p~sl - cl py J'~ --J2 -J3 (88) (83) and J~ =J3-J4 (89) This gives an unambiguous solution for 05, since the other two angles have been solved for. The Jacobian relationship is then defined by Having solved for the three angles which sur- dx round C, we now solve for the components of C dy itself (which are of the form given in (21)). Be- ginning begin with the equation do d~ C = A1-1T6A61As I (84) 0 a2s34 a3s 4 0 0 0 \ we find the second row, presented in column = -- a4c234 -- oa3c23 -- a2c 2 a2c3,0 a3c,0 a,0 0 I form, is S234 0 0 0 0 C234 0 0 0 1 234 / --C50zS 6 --1- azS 5 + c5c6n z ' 0 0 0 1 0 c5 J nzS 6 + c60z (85) , dO a , OzS5S 6 -- c6glzS 5 + azC 5 Pcy ] Pz | dO3 - dO2 |

This gives a relation for 0234 and Pcy" Id°,l For p¢~, we see from inspection of the (1.4) ~ dO6 I elements of (84) that Using the substitution

Pc~ =pySa + ClPx (86) h = -a4c234 - a3c23 - a2c 2 (91) J. Lloyd, K Hayward / Kinematics of Common Industrial Robots 181 and rearranging columns to migrate the zero block and into the upper right comer and optimize the trian- x 3 - a4y 3 gularity of the diagonal blocks, we get the rela- a2c34x 2 -- a3c4xl tionship sol(J2~ ) = a2a3s3 (98) 'dz h 0 0 0 0 0 a2s34X 2 -- a3s4x 1 d~b S234 s 5 0 0 0 0 a2a3s3 dp c234 0 1 0 0 0 The rest of the solution follows easily from equa- de 0 c 5 0 0 0 1 tions (51)-(54). dx 0 0 0 a2s34 a3s 4 0

~dy 0 0 0 a2c34 a3c4 a4 6. Summary dO1

dO6 The methods described above can be encapsu- dO5 lated into the following procedures: X (92) dO2 dO3 - dO2 6.1. Working out the Kinematics dO4 - dO 3 1. Enter all the "'A'" matrices, modifying them as The solution for necessary to define a reasonable C matrix. JlaY -- x (93) 2. Build the equations by multiplying the neces- works out to sary matrices and equating elements. There will X 1 typically be two equation sets: a set of column h vector equations in terms of the angles extrin- sol(J11 ) = x2 -- S234Y 1 (94) sic to C, and a set of matrix equations in terms S5 of the angles intrinsic to C. 3. Solve for the angles outside C, first by inspect- X 3 -- C234Y 1 ion, and secondly by combining equal de- Solving for ,I22 initially delds ments. S4X 3 -- C4X 2 -- a4s4x I 4. Solve for the angles of C in terms of C, in the a2s3 same way. The elements of C are known from a2834Y 1 - x 2 (95) step 3. a3s4 x1 6.2. Working out the Jacobian but the s 4 in the denominator of the second row cancels out to yield 1. Establish the frame k in which the I is to be derived. This should be optimized with regard S4X 3 -- C4X 2 -- a4$4x 1 to obtaining a simple Jacobian while not allow- a2s 3 ing the computation of K 6 to become too com- sol(J22 ) = s34x3 -- c34x2 -- a4s34x1 (96) plicated. a3s3 2. Derive J using equations (40) and (41) and the x1 successive Kt found by multiplying "A" Similarly, the solutions for jr and J2~ are, respec- matrices. The functions jacobcolrO and tively, jacobcolpO do this using values of K 71 instead. 3. Rearrange J to simplify the inversion. Permute C234Y 3 + S234Y2 -- X 1 the rows and columns of J to first put the h matrix into a block triangular form, if possible, sol(Jl~) = x__22 (97) and then arrange each of the blocks themselves S5 to be as triangular as possible. Additional sim- X3 plification can be obtained by adding or sub- 182 J. Lloyd, IL. Hayward / Kinematics of Common Industrial Robots

tracting columns which correspond to parallel [13] L.-W. Tsai and A.P. Morgan: "Solving the Kinematics of joints. Most General Six- and Five-degree of Freedom Manipula- 4. Find the inverse relationships. The blocks can tors by Continuation Methods". Journal of Mechanisms, Transmissions, and Automation in Design, June, 1985, pp. be inverted using the functions usolveO and 189-200 (Vol. 197, No. 2). lsolveO, and the rest of the solutions are ob- [14] K.J. Waldron, Shih-Liang Wang, and S.J. Bolin: "A Study tained directly from (51)-(54). of the Jacobian Matrix of Serial Manipulators". Journal of Mechanisms, Transmissions, and Automation in Design, June, 1985, pp. 230-238 (Vol. 197, No. 2). References

[1] Jorge Angeles: "On the Numerical Solution of the Inverse Kinematic Problem". International Journal of Robotics Research, Summer 1985, pp. 21-37 (Vol. 4, No. 2). Appendix A. MACSYMA [2] Duffy and Crane: "A Displacement Analysis of the Gen- eral Spatial 7-link, 7R mechanism". Mechanism and Mac- Sessions hine Theory, 1980, pp. 153-169 (Vol. 15, No. 3). [3] R. Featherstone: "Position and Velocity Transformations between Robot End-effector Coordinates and Joint An- A.1 Derivation of ETL Robot Kinematics gles". International Journal of Robotics Research, Summer 1983, pp. 35-45, (Vol. 2, No. 2). This is an annotated MACSYMAsession used in deriving the [4] John M. HoUerbach and Gideon Sahar: "Wrist Parti- inverse kinematics for the ETL robot. tioned, Inverse Kinematic Accelerations and Manipulator Dynamics". International Journal of Robotics Research, ; The variables al through a6 denote the 6 A matrices for the Winter 1983, pp. 61-76 (Vol. 2, No. 4). ; robot while il through i6 denote their inverses. The link pa- [5] K.H. Hunt: "The Particular or the General? (Some Exam- ; rameters are given by aal, dd2, dd3, and dd4. The sines and ples from Robot Kinematics)". Mechanism and Machine ; cosines are given by sil through si6 and col through co6. sil2 Theory, 1986, pp. 481-487 (Vol. 21, No. 6). ; and co12 are the sine and cosine of angle 1 + angle 2. [6] C.S.G. Lee: "Robot Arm Kinematics, Dynamics, and Control". Computer, December 1982, pp. 61-80 (Vol. 15, ; For later convenience, precombine al. a2 and its inverse: No. 12). (c127) a12 : combine12 (al.a2); [7] Richard Paul: Robot Manipulators: Mathematics, Pro- co12 0 si12 aalcol ] gramming, and Control. The MIT Press, Cambridge, Mas- (d127) sil2 0 - co12 aalsil sachusetts, 1981. 0 1 0 - dd2 [8] Richard Paul, Marc Renaud, and Charles N. Stevenson: 0 0 0 1 "A Systematic Approach for Obtaining the Kinematics of (c128) i12: combine12 (tsimp(invert(a2)). tsimp(invert(al ))); Recursive Manipulators Based on Homogeneous Trans- formations". Robotics Research - The first International co12 si12 0 - aalco2 1 Symposium, Michael Brady and Richard Paul, Editors. (d128) 0 0 1 dd2 [ The MIT Press, Cambridge, Massachusetts, 1984, pp. sil2 - co12 0 - aalsi2 I 707-726. l 0 0 0 [9] Richard P. Paul and Hong Zhang: "Computationally ; Now form the first three equations to find angles 1 through 3. Efficient Kinematics for Manipulators with Spherical ; The vector e4 is used to extract the fourth column of each Wrists Based on the Homogeneous Transform Represen- ; equation. tation". International Journal of Robotics Research, (c129) e4 : transpose (matrix([O, O, O, 1])); Summer 1986, pp. 32-44 (Vol. 5, No. 2). [10] G.R. Pennock and A.T. Yang: "Application of Dual- Number Matrices to the Inverse Kinematics Problem of Robot Manipulators". Journal of Mechanisms, Transmis- sions, and Automation in Design, June, 1985, pp. 201-208 (Vol. 197, No. 2). (c130) eqnl : a12.a3.e4 = t6.e4; [11] Marc Renaud: "Geometric and Kinematic Models of a Robot Manipulator: Calculation of the Jacobian Matrix (d130) dd4sil2si3 + aalsil - col2dd3 = py and Its Inverse". Proceedings of the llth International - co3 dd4- dd2 Symposium on Industrial Robots, Tokyo, Japan, October 1 7-9, 1981, pp. 757-763. (c131) eqn2 : a2.a3.e4 = il. t6.e4; [12] Bernard Roth, J. Rastegar, and V. Scheinman: "On the Design of Computer Controlled Manipulators". On the -co2dd4si3 + dd3si2 - pysil + colpx - aal q Theory and Practice of Robots and Manipulators, First (d131) dd4si2si3 - co2dd3 = colpy - pxsil CISM-IFToMM Symposium, September 1973, pp. 93-113 - co3dd4- dd2 pz (Vol. 1). 1 1 1 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots 183

(c132) eqn3 : a3.e4 = i12.t6.e4; ; Evaluate C in terms of al through a3, and then the find the ; component angles of C. [ dd4si3]1 ].] I pysil2+c°12px-aalc°2 - co3dd4 | = pz + dd2 (c144) c; (d132) dd3 | - aalsi2 + pxsil2 - col2py ncx ocx acx pcx] 1 net ocy acy pcy I (d144) n ocz acz ; Extract solution for angle 3: o o 7J (c133) part(eqn3, 1)[2, 11 = part(eqn3, 2)[2, 1]; (c145) rot(c) ~ rot(a12.a3); (d133) - co3dd4 = pz + dd2 ncx vex acx] [co12co3 sil2 col2si31 ; Extract solution for angle 1; (d145) ncy ocy acy I = I co3si12 - co12 sil2si3 (c134) x :part(eqn3, 2); ncz ocz acz .I L si3 0 - co3 [ pysil2 + col2px - aalco2 1 (c146) rot(a4.a5.a6) = rot(c); I pz + dd2 I co4co5co6 - si4si6 - co4co5si6 - co6si4 co4si5 ] (d134) [-aalsi2 + plsil2-col2py J co4si6 + co5co6si4 co4co6 - co5si4si6 si4si5 I - co6si5 siSsi6 co5 J

(c135) part(eqn3, 1); ncx OCX = [ ncy ocy acx]acy [ dd4si3 ] L ncz ocz acz d (d135) |-co3dd4 | (c147) col(part(%, 1), 3) = col(part( %, 2), 3); [ co4si5 ] [ acx ] (c136) x[1, 11^2 + x[3, 1]'2 (d147) [ si4si5 [ = acy L co5 J [ acz ] = tsimp(expand12(x[1, 112 + x[3, 115)); (c148) rot(a5.a6) = rot(i4.c); (d136) dd42si3: + dd32 co. o6 ...6 = 2aalpysil + pye + px 2 _ 2aalcolpx + aal 2 (d148) I co6si5 - si5si6 ; Extract solution for angle 2 I_ si6 co6 (c137) eqn2; ncysi4 + co4ncx ocysi4 + co4ocx acysi4 + acxco4 ] -- ncz -- ocz [co2dd4si3 + dd3si2 1 [pysil + colpx - aal- co4ncy -- ncxsi4 co4ocy-- ocxsi4 acyco4a'Zacxsi4] l dd4si2si3 - co2dd31 = (d137)[ _co3d~14_dd2 ] [I colpy-pxsilpz (c149) col(part( %, 1), 3) = col(part( %, 2), 3); si' ] [acysi4+acxco4] (c138) x:matrix([dd4 * si3, dd3], [- dde, dd4 * si3]); (d149) - eo51= l - acz I O J L acyco4 - acxsi4 J dd3 (d138) [ dd4si3 dd4si3]J t - dd3 (c15O) rot(a6); (c139) invert(x); [co6-si6 i] dd4si3 dd3 ..o, [,o O6o dd42si32 + dd32 dd42si32+ dd32 (d139) dd3 dd4si3 (c151) col( %, 1) = col(rota5.i4.c),1); dd42si32 + dd32 dd4esi32+ dd3: [co6] [ co5(ncysi4 + co4ncx) -- nczsi5 (d151) (cl40) kl = part(eqn2, 2)[1, 1]; = I co4ncy -- ncxsi4 [ (ncysi4 + co4ncx)si5 + co5ncz (d140) kl = pysil + colpx - aal (c141) k2 = part(eqn2, 2)[2, I]; (d141) k2 = colpy - pxsil (c142) print (matrix([co2], [si2]), A.2 Derivation of Elbow Manipulator Jacobian "'= '" revert(x), matrix([kl], [k2])); This is an annotated MACS','MA session to illustrate the co2 dd42si32+ dd32 dd42si32+ dd3e derivation and manipulation of Jacobians. si2 dd3 dd4si3 k2 ; For the elbow manipulator, the link offsets are given by the J dd42si32+ dd32 dd42si32+ dd32 ; variables aa2, aa3, and aa4. The composite A matrices a23, ; a34, and a234, along with their inverses i23, i34, and i234, ; Derive the solutions for the last three joint angles. For conven- ; are precomputed to help MACSYMA keep the associated angles ," ience, define a macro "rot' which returns the rotational part ; combined ; of a transform, (c143) rot(m) := submatrix(4, m, 4); ; Start by computing and simplifying all six columns of the (d143) rot(m) :ffi submatrix(4, x, 4) jacobian: 184 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

(cl 11) jl : tsimp(jacobcolr(al.a234)); ; Exchange various rows and columns to move the zero block 0 ; into the upper right hand corner and optimize the triangularity 0 ; of the blocks: (d111) - aa4co234 - aa3co23 - aa2co2 (c121) exrow( %, 1, 3)$ si234 (c122) exrow( %, 2, '4)$ co234 0 (c123) exrow( %, 3, 5)$ (cl 12) j2: tsimp(jacobcolr(a2, a3. a4)); (c124) excol(%, 3, 5)$ (aa2co3 + aa3)si4 + aa2co4si3 1 (c125) excol(%, 2, 5)$

- aa2si3si4 + (aa2co3 + aa3)co4 + aa4 (c126) excol( %, 4, 6)$ (dl12) 0 (c127) excol(%, 4, 6)$ 0 ; Now we have the permuted version of the Jacobian. 0 1 (c128) h: - aa4 * co234 - aa3 * co23 - aa2 * co25 (el 13) j3 : tsimp(jacobcolr(a3.a4)); 1 (c12 9) jp : ratsubst( "h, h, d127)," aa3si4 I h 0 0 0 aa3co4 + aa4 si234 si5 0 0 oo]0 0 (dl13) 0 (d129) co234 0 1 0 0 0 0 0 co5 0 0 0 1 0 0 0 0 aa2si34 aa3si4 0 1 0 0 0 aa2co34 aa3co4 aa4 (cl 14) j4: tsimp(jacobcolr(a4)); ," Define the sub-blocks of the matrix: (c130)jll .'submatrix(4, 5, 6,jp, 4, 5, 6)," Vaa4 ° o (d130) si234 si5 co234 0 (c131)j22.'submatrix(1, 2, 3,jp, 1, 2, 3); ; k is the matrix between the frame coinciding with joint 5 [o o,] ; and the Jacobian frame. This is not the identity since the (d131) aa2si34 aa3si4 0 ; z axis of k is not parallel with the axis of joint 5. aa2co234 aa3co4 aa4 (c115)[,o display (k);o (c132)j21[oco, :submatrix(1, 2, 3,jp, 4, 5, 6); k= 0 0 -1 (d132) 0 o 0 1 0 0 0 0 0 0 ," Solve jl l recursively, and simplify." (cll 6) j5 : tsimp(jacobcolr(k)); (c133) tsimp(lsolve(j11, y, x)); xl (d116) 0 h (d133) si234y1 - x2 si5 x3 - co234yl (cl 17) j6: jacobcolr(i5) ; ," Solve j22 recursively, and simplify: (c134) v: tsimp(lsoloe(j22, y, x)); (d117) 0 si4x3 - co4x2 - aa4si4xl S aa2co34si4 - aa2co4si34 (d134) aa2si34y1 - x2 L co5 d aa3si4 ; Simplify the columns by subtraction: xl (c118)j2: combine34(j2 - j3)$ (c135) co34 * si4 - co4 * si34," (c119)jS:j3 - j45 (d135) co24si4 - co4si34 ; Now form the Jacobian: (c136) v : ratsubst(tsimp(expand34( %)), %, v); (c120) j: addcolO'l, j2, j3, j4, jS, j6); 0 aa2si34 aa3si4 0 0 si4x3 - co4x2 - aa4si4xl 0 aa2co34 aa3co4 aa4 0 aa2si3 o --aa4co234 -- aa3co23 -- aa2co2 0 0 0 0 (d136) aa2si34y1 - x2 si234 0 0 0 0 $ aa3si4 co234 0 0 0 1 xl 0 0 0 1 0 co5 J J. Lloya~ V. Hayward / Kinematics of Common Industrial Robots 185

,. Substitute yl into the second row to eliminate si4: is also one of the more complicated industrial arms, since it has (c137) tsimp(ratsubst(v[1, 11, yl, v)); 4 offsets. The modified "A" matrices are defined as follows: si4x3 - co4x2 - aa4si4xl aa2si3 clo sl (d137) si34x3 - co34x2 - aa4si34x1 A1 = s 1 0 -- c 1 aa3si3 0 1 0 xl 0 0 0 ; Solve for jl 1 recursively, and simplify: C2 -- S2 0 a2c 2 I (c138) tsimp(usolve(transpose(j11), y, x))," A2= S200 C200 001 ais2 ) co234y3 + si234y2 - xl h (d138) x2 f c 3 0 -- s 3 a3c 3 -- d4s 3 S 3 0 C 3 a353 + c3d 4 si5 A3= x3 0 -1 0 d 3 ; Solve for j22 recursively, and simplify: 0 0 0 1 (c139) v : tsimp(usolve(transpose(j22), y, x)); C4 x3 - aa4y3 A4 = s4 0 -- C 4 aa3co4y3 - x2 0 1 0 0 0 0 (d139) aa3si4 aa2si34x2 - aa3si4xl ( C5 0 -s 5 O) aa2aa3co34si4 - aa2aa3co4si34 0 c5 i (c140) co34 * si4 - co4 * si34; -1 0 . 0 0 (d140) co34si4 - co4si34 f ¢6 (c141) v: ratsubst(tsimp(expand34( %)), %, v); x3 - aa4y3 A6 = aa3co4y3 - x2 (d141) aa3si4 The inverse kinematics solution is aa2$i34x2 - aa3si4xl aa2aa3si3 (c142) tsimp(ratsubst(v[3, 1], y3, v); x3 - aa4y3 aa2co34x2 - aa3co4xl 02 = atan2 ( 2a2pz + ~4a2(P2k21_2a2kl + k21)1 - k21 ) (d142) aa2aa3si3 aa2si34x2 - aa3si4xl aa2aa3si3 kll = pysl + clpx 2 2 ; Lastly, determine the matrix k6 which maps from k to link 6: k21= t72 + k21 + a2 _ d4 _ a~ (c143) k6 : a5.a6; 83 = atan2( - d4k31 + a3k32 I co5co6 - coSsi6 sis il a3k31 + d4k32 ] (d143) si6 co6 0 k31 = pzS2 + C2kll - a 2 - co6si5 si5si6 co5 0 0 0 k32 = c2p z - klS 2 04 = atan2( - acy I \ - ac~ ] Appendix B. Examples with 05 = atan2(-acys4-acxC4)a.~ Other Robots 06 = atan2 ( - °czs5 S c5 ( °*YS4._...~+ c'°cx ) ) C40cy -- OcxS 4 ] where the rotational components of C can be computed from the first three angles by B.1 The PUMA n cx °cx Cl C23 Probably the most famous of all research robots, the PUMA ncY °cY acy = C23Sl Cl -- $1523 / (Fig. 3) is an anthropomorphic arm with six revohite joints. It n cz Ocz a cz s23 0 c23 ] 186 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

The manipulator Jacobian is derived in the link defined by ® AaA2A 3. This gives a K 6 matrix defined by

C4C5C6 -- S4S 6 -- C4C5S 6 -- C6S 4 -- C4S 5 0 @ C4S6 + C5C6S 4 C4C6 -- C5S4S6 -- S4S 5 0 K6= C6S 5 -- S5S 6 C5 0 0 0 0 1

The corresponding Jacobian relationship is @ ® c/(c23 3h a2s3-4°100 0° 0° -- d3s23 a2c 3 a 3 0 0 0 t.) = . o o o.-.s, do 0 0 -1 0 - c a - s4d 5 d~ c23 0 0 1 0 c 5

d01 dS: Fig. 3. The PUMA manipulator. × de4 dO~ de,

d3s23Y 3 - c23d3y 2 + x 1 where rows 2 and 3 have been combined and h is defined by h a3y 3 -- x 3 h = - d4823 + a3c23 + a2c 2 sol(J T ) = d4 The permuted form of the relationship is a2s3x 3 + d4x 2 a2a3s 3 + a2c3d 4

dx - d3s23 a2c 3 a 3 0 0 0 C45c5y 3 + S4S5X 2 -- C4XI dz s23 0 0 -- C4S 5 S 4 $5 d~b 0 0 - 1 - s4s 5 - c 4 ~ol(J~) = C5s4y 3 -- C4S5X2 -- S4X 1 do C23 0 0 C5 0 S5 ddp X3

dO1 I laa~| B.2 The Stanford Manipulator / ~°6 / The Stanford manipulator (Fig. 4) is a spherical robot with an \ ~04 / offset at the shoulder. The "A" matrices for the robot are: The solution is given by

Xl A 1 = (clSl :1 -"Cl i) h 0 - 0 ( d3das23 - a3c23d3 ) y 1 + d4x 3 + a3x 2 0 0 0 sol(Jll ) = a2a3s 3 + a2c3d a it C2 0 S2 / x 3 -- a2c3y 2 + d3s23Y 1 s 2 0 -- c 2 00 A2= a3 i 0 1 0 d12 0 0 0

t -- $4x2 + C4X1 I sol(J22 ) = Ss lO o S4X 1 -- C4X 2 A3= x 3 -- Csy 1 0 1 d13 0 0 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots 187

The manipulator Jacobian is derived in the link defined by A1A2A 3. This gives a K 6 matrix defined by

C4C5 C6 -- S4S 6 -- C4C5S 6 -- C6S 4 C4S 5 0

K 6 = c436 + c5c6s4 c4c 6 - c5s4s 6 3435 - c6s 5 s5s 6 c 5 ) 0 0 0 The corresponding Jacobian relationship is

_ c2d 2 d3 0 0 0 O ~[dOl~ d~ d3s 2 0 0 0 0 0 ]/d021 dz - d2s 2 0 1 0 0 0 lid03[ dtk - s 2 0 o 0 -s, c,s,//d0,/ dp 0 1 0 0 C4 S4S51/ dO5 ] drk c 2 0 0 1 0 c, ]~d06]

Fig. 4. The Stanford manipulator. The permuted form of the relationship is

dy d3s2 0 0 0 0 O~[dOll dx -- c2d 2 d 3 0 0 0 011d02} dz -- d2s 2 o a o o o//ao, /

/c 0 s4 d~ -- S 2 0 o c,s5 -s, o//a06 / A4 = 0 C4 dp 0 1 0 S4S 5 C4 OlldO, I -1 0 de C2 0 0 c, 0 1]~d04] 0 0 The solution to this is given by

A5 = s5 0 - c 5 X1 0 1 0 d3s2 0 0 0 sol(Jll ) = c2d2y 1 + X 2 /¢ c6 - s 6 0 0 1 d3 A6 = |s 6 c 6 0 0 d2s2y 1 + x 3 0 1 0 0 0 1 S4X 2 + C4X 1 The inverse kinematic solution is sol(J22 ) = ss C4X 2 -- S4X 1 X 3 -- cSy 1

01 = 2 atan 2 d2 + py d2s2y 3 + c2d2y 2 + X 1 d3s 2 ! 2 2 d3 = V(pysl + cip~) + p~ sol(J ) = x2 d3 X3

C4c5y 3 + S4SsX 2 -- C4X1 $5 \acx] sol(Jr) = C5S4y 3 -- C4S5X2 -- $4X1 S5 05 = atan2( acyS4acx+ acxCa ) X3

06= atan2( °czSs--_c,( °cyS, + C_,°cx) ] C40cy -- OcxS 4 ] where the rotational components of C can be computed from B.3 The SCARA, type 1 the first three angles by

The SCARA robots are wrist partioned manipulators where n cy Ocy a cy = the first three joints consist of two rotary joints and one n cz ocz a cz ~ - s 2 0 c 2 prismatic joint, with all the joint axes parallel. 188 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

Io The inverse kinematic solution is 2azpy +-~/4az(P;, + P;,)-k~l 01 = 2 atan 2 ku +2a2Px 7~ kll = p: + p2 - da2 + a22 I d2 =Pz @ J 03 = atall2( pysl + clpx- Clpy - a2 @ o4 = atan2(ac" l I a~x ] Os= atan2( acys4 +cz acxc4

O6=atan2(°¢zss-cs(~°f-ys4+--c4°cx))C40cy-- OcxS 4 O) where the rotational components of C can be computed from the first three angles by

ncy °cy acy = S13 0 -- c13 n cz °cz a cz 0 1 0 The manipulator Jacobian is derived in the link defined by Fig. 5. The SCARA manipulator, type 1. A1A2A 3. This gives a K 6 matrix defined by

C4C5C6 -- $4S 6 -- C4C5S 6 -- C6S 4 C4S s 0

K6 = £4s6 q- c5¢6s 4 c4c 6 -- c5s4s 6 84s 5 0

-- c6$ 5 s5s 6 c 5 0 In the first type of SCARA robot (Fig. 5), the prismatic joint 0 0 0 1 is joint 2. The "A" matrices for this robot are The corresponding Jacobian relationship is 00) dx a2s 3 0 d4 0 0 0 ~ dO 1 dy 0 1 0 0 0 ~ ] dO2 A1 = c I 0 0 0 1 0 dz - a2c 3 0 0 0 0 dO3 - dO1 0 0 1 d~b 0 0 0 0 -- s 4 ¢4s5 dO4 dp 0 0 0 1 0 c 5 [ dO5 dq~ ] dO6 A2 = 0 1 0 l: °°oI where columns 1 and 3 have been combined. The permuted 0 0 I form of the relationsllip is

dy 0 1 0 0 0 0 dO2 A3 = 0 - c 3 - c3d 4 1 0 0 dx a2s 3 0 d 4 0 0 dO3 - dO 1 0 0 1 d~k 0 0 0 c4s5 - s 4 dO6 dp 0 0 1 SaS5 c 4 dO5 ¢ C4 0 -- S 4 0 0 0 C 5 0 dO4 0 C4 0 dck 0 -1 0 0 The solution to this is given by 0 0 1 Xl a2c3

A 5 ~ s5 0 - Cs sol(Jla ) = x 2 0 1 0 a2s3y 1 -- x 3 0 0 0 d4

_s6 o S4X 2 + C4X 1

A6 = c 6 0 sol(Jza ) = s5 0 1 C4X 2 -- S4X 1 0 0 x 3 -- csy 1 J. Lloya~ V. Hayward / Kinematics of Common Industrial Robots 189

a2s3y 3 -- X 1 c 0 -s5 i) O 2C3 A5 = s 5 0 c 5 sol(J~) = X2 0 -1 0 X3 0 0 0 d4

C4Csy 3 + $4$5X2 -- C4X 1 A6 = c 6 0 S5 0 1 sol(J r) = CsS4y 3 -- C4S5X 2 -- S4X 1 0 0 S5 The inverse kinematic solution is X 3 ( 2aaPy±¢4a2(pE + p2)-k2, ) 01 = 2 atan 2 kn +2alpx

B.4 The SCARA, Type 2 kl 1 = py2 +]o 2_ a 2 + a 2

In the second type of scxa_~ robot (Fig. 6), the prismatic joint is joint 3. The "A" matrices for the robot are: \ pysl + clv~ - al }

Cl -- S 1 0 alc 1 d3 = Pz

A 1 = sl c~ 0 als 1 0 0 1 0 ) 04 = atan 2(-a~Y / 0 0 0 1

t C2 -- $2 0 a2c 2 05 = atan 2(-acyS4~-acxC4)acx I A 2 = S2 0 C200 001 a02 I 0 06=atan2(-°czs'~-c--5(°cys4----~+c4°cx))c4ocy- OcxS 4 '1 0 0 0 0 1 0 0 where the rotational components of C can be computed from A3= 0 0 1 d 3 the first three angles by 0 00 1 t C4 0 s a 0 - c4 n cz Ocz a ez, 0 1 0 0 0 The manipulator Jacobian is derived in the link defined by A1A2A 3. This gives a K 6 matrix defined by

® K 6 = c4s6 + c5c654 c4c 6 - c5s4s 6 - s4s 5 c6s 5 -- SsS 6 C5 0 0 0 1 Ther /(als2ooocorresponding Jacobian relationship o iso}/ 1 alc 2 a 2 0 0 0 ~ I dO2- dO21 = 00100 Id031

do o o o o -c, -s,s, | ~os | d~ o 1 o a o ~ / ~ dO6 ] where columns 1 and 2 have been combined. The permuted form of the relationship is

/ /(a12o0a-,_c~ o,, 0 o0 0 0i)O/~'O',-"O',L/ 01 = o o! o o /,~o3 / o o o -c.s .. , , dp 0 0 0 - sass - c4 I dos I Fig. 6. The SCARA manipulator, type 2. dO 0 1 0 c 5 0 ~ dO, ] 190 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots

The solution is given by It is cylindrical, with two prismatic joints, and consequently its kinematics are quite straightforward. x1 The "A" matrices for this robot are: als2 sol(Jll ) = alc2y 1 - x 2 a2 A1 = s 1 c 1 0 x 3 0 0 1 0 0 0 S4X 2 + C4X 1 S5 so1(42) = ( Ol 0) S4X 1 -- C4X 2 A2 0 0 0 x 3 - csy 1 = 1 0 d 2 0 0 1 alc2y 2 -- x 1 als2 ( oo o) 1 0 0 sol(J~) = X2 A3 = 0 1 d 3 a2 0 0 1 X 3 s4 c4c5y 3 + s4s5x 2 -- C4Xl o) A4 = 0 - C 4 0 S5 1 0 0 sol(J~) = C5S4y 3 -- C4S5X 2 -- S4X 1 0 0 1 S5 X3 As = s 5 0 - c 5 0 1 0 0 0 0

B.5 The Microbo ECUREUIL A6 = s6 c6 0 This is a cylindrical robot manufactured by the Swiss 0 0 1 0 0 0 company Microbo (Fig. 7). The inverse kinematic solution is

01 = atan2( py ~Px:I

dE = Pz

® d3=~+P:

04=atan2( acy ) \ acx

@ 05 = ataxl2( acys4)_ +acx acxc4

® where the rotational components of C can be computed from @ the first three angles by

n cx °cx (-)n cy °cy a cy = c 1 0 n cz °cz a cz 0 1 The Jacobian is derived in the frame defined by A 1A 2 A 3A 4, from which K 6 is defined by

K 6 = c6s5 - s5s 6 - c5 S 6 C6 0 Fig. 7. The microbo ECUREUIL. 0 0 0 J. Lloyd, V. Hayward / Kinematics of Common Industrial Robots 191

The resulting Jacobian relationship is The solution is given by S4X2 + C4X 1 d3 sol(Jll ) = S4X1 -- C4X 2 = d3s 4 - c 4 0 0 0 ~ dO 3 X3 S 4 0 0 0 0 S 5 ] d04 l o: o o OlO ,l o, sol(J22 ) S5 d 0 --C 4 0 0 01 __ ~dO6 -- X2 The permuted form of this relationship is c5y I + x 3 d3s4x 2 + c4x 1 c4d 3 s 4 0 0 0 d3 sol(J~) = dy = o o 1 o o I de~ c4d3x 2 - $4x1 d3 d~ s 4 0 0 s 5 0 I de6 x3 dO - c, 0 0 0 1 [ de 5 csy3 + x I dp 0 0 0 - c 5 0 ~ de 4 S5 solOS) = il X2 X3 )