RSD-TR-1-83 A GEOMETRIC APPROACH IN SOLVING THE INVERSE KINEMATICS OF PUM.A ROBOTS C. S. G. Lee M. Ziegler Department of Electrical and Computer Engineering The University of Michigan Ann Arbor, Michigan 48109-1109 December 1983 CENTER FOR ROBOTICS AND INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109

RSD-TR- 1-83 TABLE OF CONTENTS 1. INTRODUCTION............................................................................. 2 2. LINKS, JOINTS, AND COORDINATE TRANSFORMATION......... 3 3. KINEMATIC EQUATIONS FOR MANIPULATORS........................ 6 4. THE INVERSE KINEMATICS SOLUTION OF A PUMA ROBOT ARM 4.1. DEFINITION OF VARIOUS ARM CONFIGURATIONS......... 9 4.2. ARM SOLUTION FOR THE FIRST THREE JOINTS OF A PUMA ROBOT ARM................................................................ 11 4.3. ARM SOLUTION FOR THE LAST THREE JOINTS OF A PUMA ROBOT ARM........................................ 19 5. DECISION EQUATIONS FOR THE ARM CONFIGURATION INDICATORS........................................ 25 6. COMPUTER SIMULATION........................................................ 27 7. CONCLUSION.................................................................................... 27 8. ACKNOWLEDGEMENT.................................................................... 27 9. REFERENCES.................................................................................... 29 10. APPENDIX: COMPUTER PROGRAMS.......................................... 30 11

RSD-TR-1-83 ABSTRACT This paper presents a geometric approach to derive a consistent joint solution of a six-joint PUMA1 robot. The approach calls for the definition of various possible arm configurations based on the link coordinate systems and human arm geometry. These arm configurations are then expressed in an exact mathematical way to allow the construction of arm configuration indicators and their corresponding decision equations. The arm configuration indicators are prespecified by a user for finding the joint solution. These indicators enable one to find a solution from the possible four solutions for the first three joints, and a solution from the possible two solutions for the last three joints. The solution is calculated in two stages. First a position vector pointing from the shoulder to the wrist is derived. This is used to derive the solution of the first three joints by looking at the projection of the position vector onto the x1-yil (i = 1,2,3) plane. The last three joints are solved using the calculated joint solution from the first three joints, the orientation matrices, and the projection of the link coordinate frames onto the xl1-yil (i = 4,5,6) plane. From the geometry, one can easily find the arm solution consistently. Computer simulation study conducted on a VAX11/780 computer demonstrated the validity of the arm solution. This work was supported in part by the Robot Systems Division of the Center for Robotics and Integrated Manufacturing (CRIM) at The University of Michigan, Ann Arbor, MI. Any opinions, findings, and conclusions or recommendations expressed in this article are those of the authors and do not necessarily reflect the views of the funding agency. 1 PUMA is a trademark of Unimation Inc.

RSD-TR- 1-83 1. INTRODUCTION An industrial robot is a general purpose manipulator having several rigid links connected in series by revolute or prismatic joints driven by actuators. One end of the chain is attached to a supporting base while the other end is free and attached with a tool to manipulate objects or perform assembly tasks. The motion of the joints results in relative motion of the links. Since the robot servo system requires the reference inputs to be in joint coordinates and a task is generally stated in terms of the Cartesian coordinate system, controlling the position and orientation of the end-effector of a robot arm to reach its object requires the understanding of the kinematic relationship between these two coordinate systems. The kinematics problem usually consists of two subproblems- the direct and inverse kinematics problems. The direct kinematics problem is to find the position and orientation of the end effector of a manipulator with respect to a reference coordinate system, given the joint variable vector qT = (q, q2,.', qn) of the robot arm and the various geometric link parameters, where superscript T on vectors and matrices denotes a transpose operation, and n is the number of degree-of-freedom. The inverse kinematics problem (or arm solution) is to calculate the joint variable vector q for positioning the end-effector of the robot arm at the desired position with the desired orientation, given the position and. orientation of the end effector with respect to the reference coordinate system and the various geometric link parameters. This paper is concerned with the inverse kinematic analysis of simple manipulators consisting of six rotary joints. In general, the inverse kinematics problem can be solved either by algebraic, iterative, or geometric approach. Several investigators have attempted to solve the problem for the PUMA and Stanford robot arms using the algebraic approach 11]-[61. This approach suffers from the fact that the solution does not give a clear indication on how to select the correct solution from the several possible solutions for a particular arm configuration. The user often needs to rely on his/her intuition to pick the right answer. The iteration solution [7-8] often requires more computations and it does not guarantee convergence to the correct solution, especially in the singular and degenerate cases. Furthermore, there is no indication on how to choose the correct solution for a particular arm configuration. If the manipulator under consideration is simple, that is the geometry of the first three joints has revolute or prismatic pairs and the last three joint axes intersect at a point [1], then the geometric approach presents a better approach for obtaining a closed form solution. This paper presents a geometric approach in solving the inverse kinematics problem of a simple robot arm with rotary joints. The approach calls for the definition of various possible arm configurations based on the link coordinate systems and human arm geometry. These Inverse Kinematics for Puma Robots 2

RSD-TR-1-83 arm configurations are then expressed in an exact mathematical way to allow the construction of three arm configuration indicators (ARM, ELBOW, and WRIST) and their corresponding decision equations. With the assistance of the configuration indicators and the arm geometry, one can easily find the arm solution consistently. The validity of the arm solution was simulated on a VAX11/780 computer. With appropriate modification and adjustment, the user can generalize and extend the method to most present day industrial robots with rotary joints and obtain the arm solution easily. 2. LINKS, JOINTS, AND COORDINATE TRANSFORMATION To describe the translational and rotational relationship between adjacent links, a Denavit-Hartenberg matrix representation [9] for each link is used and shown in Figure 1. From Figure 1, an orthonormal coordinate frame system ( xI,y$,z ) is assigned to link i, where the z; axis passes through the axis of motion of joint i+1, and the xi axis is normal to the zs- axis pointing away from it, while the yi axis completes the right hand rule. With this orthonormal coordinate frame, link i is characterized by two parameters: ai, the common normal distance between the z.I and zi axes and ai, the twist angle measured between the zi_- and zi axes in a plane perpendicular to ai. Joinit i which connects link i-1 to link i is characterized by a distance parameter di measured between the xsl and xi axes and a revolute joint variable 8, which is the joint angle between the normals and measured in a plane normal to the joint axis. If joint i is prismatic, then it is characterized by an angle parameter ej and a joint variable di. Once the link coordinate systems have been established for each link, a homogeneous transformation matrix, A-l', can easily be developed relating the i'h coordinate frame to the i-1th coordinate frame. Using the A-1 matrix, one can relate a point pi at rest in link i and expressed in homogeneous coordinates with respect to the ith coordinate system to the -11th coordinate system established at link (i-1) by: pi- = Ai'l Pi (1) where Pi1 -- (xi-1, yi-, z/1, 1) p; Pi (, Yi, zi, 1)T; cos i - cos aisin; sinaisin8i ai cos:: Al sini cosaicosOi -sinai cos6i aisin for a rotary joint (2) 0 sin ai Cosai di AF1 = O sine cosai dj; for a rotary joint 0 0 0 1 3 Inverse Kinematics for Puma Robots

1RSb-TR-1-83 Jo \ itjoint i Iink i-1 Figur/e 1 Link Joint i-1 YI-1 Figure 1 Link Coordinate System and Its Parameters With the basic rules for establishing an orthonormal coordinate system for each link and the geometric interpretation of the joint and link parameters, a procedure for establishing consistent orthonormal coordinate systems for a robot is outlined in [5]. An example of applying this algorithm to a six-joint PUMA robot arm is given in Figure 2. The six A-1 homogeneous transformation matrices for the PUMA robot shown in Figure 2 are listed in Figure 3. Inverse Kinematics for Puma Robots 4

RSD-TR- 1-83 Zd.Z a2 1 90 -90 d 2 0 0 T 431.8mm!149.09 mm -225to+45 3 90 433.07mm to+170 5_ 0 9_ 0_ ~_ -100 to +100 | 6_____J _ 56.2 mm___6 -266 to +266! x 4 x5 PUreA Robot Link Coordinate Parameters Jo5 Inverse Kinemi aticsi for a d Range, 90 -90 0 0 -160 to +1 60 2 — Oi 0 1431.8 mm ] 149.09 mm -225 to +45 3 90 90 i - 20.32 mmi 0 -45 to +225 4 0 -90 I 0 433.07 mm -110 to +170,, t 0 0 1 3 -4 00 to +1 00 6 O 6 0 o i 56.25 mm -266 to +266 m Figure 2 Link Coordinate Systems For A PUUMA Robot 5 Inverse Kinematics for Puma Reobots

RSD-TR-1-83 C1 0 -S0l o C2 - S0 a2C21 c3o S3 a3C3] S1 0 C1, 0 S2 C20 0a2S2 ` S -C3 a3S 0 -1 o 0 0o 1 d2 A2 1 0 0 o 0 Q 1 0 0 0 01 o 0 0 1 c4 0 -s4 o Cs O Ss C6 -S6 o o 3 o -1 o dO A o 1 A 1 6,o o o lA- o o 1 oo 0 0 1 0 0 01 0 0 0 1 where C —=cos,; Si - sin i Figure 3 Coordinate Transformation Matrices For The PUMA in Figure 2 3. KINEMATIC EQUATIONS FOR MANIPUIATORS The homogeneous transformation matrix Toi which specifies the position and orientation of the sih coordinate frame with respect to the base coordinate system is the chain product of successive homogeneous transformation matrices of A/1, expressed as: T ' = Ao A, A I1 " 0 0 1, = or = = ( ) Specifically for i= n, we obtain the T matrix, T = T2, which specifies the position and orientation of the end-point of a manipulator with respect to the base coordinate system. This T matrix is used so frequently in the kinematic analysis of robot arm that it is called the "arm matrix". Consider the T matrix to be of the form: n, 8s az Pz 0n Y0n 1n ] n s 1 0 nszy y a Py 0 00 Inverse Kinematics for Puma Robots 6

RSD-TR- 1-83 where (see Figure 4): n is the normal vector of the hand. Assuming parallel-jaw hand, it is orthogonal to the fingers of the robot arm. is the sliding vector of the hand. It is pointing in the direction of the finger motion as the gripper opens and closes. a is the approach vector of the hand. It is pointing in the direction normal to the palm of the hand. (i.e., normal to the tool mounting plate of the arm.) p is the position vector of the hand. It points from the origin of the base coordinate system to the origin of the hand coordinate system, which is usually located at the center point of the fully closed fingers. The elements of the arm matrix for the PUMA robot arm shown in Figure 2 are found to be Xo Figure 4 Hand Coordinate System and [ n, s, a ] 7 Inverse Kinematics for Puma Robots

RSSDTR-1-83 n. = C1lC23(C4C5C6 - S4S6)- S23Sc6j - S1[s4c5c6 + C4sJl ny = S1[C23(C4C5C6 - S4S6)- S23S5C61 + Cl[S4C5C6 + C4S61 (5) n. = -S23[ C4 C5 C6 - S4S61 - C23S5 C6 s.= C1[-C23(C4C5S6 + S4C6) + S23S5S61 - S1[-S4C5S6 + C4C61 y = sl[-c23(C4C5S6 + S4c6) + 23SS61 + C1[-S4C5S6 + C4C61 (6) = S23(C4C5S6 + S4 C6) + C23s5S6 a,= C1(C23C4S5 + S23C5)- S1S4S5 ay = s, (C23 C4S5 + s23 C5) + c, s,4S (7) a. = -S3C4S5 + C23C5 pz = C1[d6(C23C4s5 + &s3C5) + S23d4 + a3C23 + a2C2] - Sl(4dS45 + d2) Py = S1[dd(C23C4S5 + S23Cs) + S23d4 + a3C23 + a2C2] + C1(d6S4S5 + d2) (8) PZ = d6(C23C5 - S23 C4S5) + C23d4 - a3S23 - a2S& where C; =cos,; i — sin;; Ci=-cos(i; + 0i); Si,-sin(e; + Bi). 4. THE INVERSE KINEMATICS SOLUTION OF A PUMA ROBOT ARM This section presents a geometric approach to derive a consistent joint angle solution of a PUMA robot given the arm matrix as in Eq. 4. Based on the link coordinate systems and human arm geometry, various arm configurations of a PUMA robot can be identified with the assistance of three configuration indicators (ARM, ELBOW and WRIST) - two associated with the solution of the first three joints and the other with the last three joints. For a six-joint PUMA robot arm, there are four possible solutions to the first three joints and for each of these four solutions there are two possible solutions to the last three joints. The first two configuration indicators allow one to determine one solution from the possible four solutions for the first three joints. Similarly, the third indicator selects a solution from the possible two solutions for the last three joints. The arm configuration indicators are prespecified by a user for finding the inverse solution. The solution is calculated in two stages. First a position vector pointing from the shoulder to the wrist is derived. This is used to derive the solution of each joint i (i =1,2,3) of the first three joints by looking at the Inverse Kinematics for Puma Robots 8

RSD-TR- 1-83 projection of the position vector onto the x,4-y-, plane. The last three joints are solved using the calculated joint solution from the first three joints, the orientation submatrices of To' and AA1 (i = 4,5,6), and the projection of the link coordinate frames onto the x,1-y,1 plane. From the geometry, one can easily find the arm solution consistently. As a verification of the joint solution, the arm configuration indicators can be determined from the corresponding decision equations which are functions of the joint angles. 4.1. DEFINITION OF VARIOUS ARM CONFIGURATIONS For the PUMA robot arm shown in Figure 2 (and other rotary robot arms), various arm configurations are defined according to human arm geometry and the link coordinate systems which are established using the algorithm in [5] as: (Figure 5) RIGHT (shoulder) ARM: Positive 02 moves the wrist in the positive zo direction while joint 3 is not activated. LEFT (shoulder) ARM: Positive 82 moves the wrist in the negative z0o direction while joint 3 is not activated. ABOVE ARM (elbow above wrist): Position of the wrist of the RGHLEFT arm with respect to the shoulder coordinate system has positive coordinate value along the y2-axis. BELOW ARM (elbow below wrist): Position of the wrist of the RGLEFT arm with respect to the shoulder coordinate system has i PeOtive} coordinate value along the y2-axis. WRIST DOWN: The a unit vector of the hand coordinate system and the y5 unit vector of the (xs,y5,z5) coordinate system have a positive dot product. WRIST UP: The s unit vector of the hand coordinate system and the y5 unit vector of the (xs,y5,z5) coordinate system have a negative dot product. (Note that the definition of the arm configurations with respect to the link coordinate systems may have to be slightly modified if one uses different link coordinate systems.) With respect to the above definition of various arm configurations, two arm configuration indicators (ARM and ELBOW) are defined for each arm configuration. These two indicators are combined to give one solution out of 9 Inverse Kinematics for Puma Robots

RSD-TR-1-83 LEFT and ABOVE Arm RIGHT and ABOVE Arm LEFT and BELOW Arm RIGHT and BELOW Arm Figure 5. Definition of Various Arm Configurations Inverse Kinematics for Puma Robots 10

RSD-TR- 1-83 the possible four joint solutions for the first three joints. For each of the four arm configurations (Figure 5) defined by these two indicators, the third indicator (WRIST) gives one of the two possible joint solutions for the last three joints. These three indicators can be defined as: ARM +1 RIGHT arm (9) =-1; LEFT arm +1; ABOVE arm (10) ELBOW=11 -; BELOW arm +; WRIST DOWN (11) WRIST- -1; WRIST UP In addition to these indicators, the user can define a "FLIP" toggle as: FL +1; Flip the wrist orientation (12) FI -1; Do not flip the wrist orientation The signed values of these indicators and the toggle are prespecified by a user for finding the inverse kinematics solution. These indicators can also be set from the knowledge of the joint angles of the robot arm using the corresponding decision equations. Ve shall later give the decision equations that determine these indicator values. The decision equations can be used as a verification of the inverse kinematics solution. 4.2. ARM SOLUTION FOR THE FIRST THREE JOINTS OF A PUMA ROBOT ARM From the kinematics diagram of the PUMA robot arm as in Figure 2, we define a position vector p which points from the origin of the shoulder coordinate system (xo yo,zo) to the point where the last three joint axes intersect as (see Figure 4): P = P6- d6S = (ps, py, pz)T (13) which corresponds to the position vector of To4: 11 Inverse Kinematics for Puma Robots

RS D-TR-1-83 [ a2C2 + a3 C23 + d4s23) - d2S P, 4123) 2 SI (14) IP = Si1(a2C + a3C23 + d4S23) + d2C1 lPz] d4C23 - a3s23 - a2S2 Joint One Solution. If we project the position vector p onto the x0-y0 plane as in Figure 6, we obtain the following equations for solving 81: 1L= ---a; 0Rr+ +a (15) r= /ptpy-d2; R= (16) sin — =.; cos= — P- (17) R R sin a= -; coscr= R (18) R R where the superscript L/R on joint angles indicates the LEFT/RIGHT arm configurations. From Eqs. 15-18, we obtain the sine and cosine functions of el for LEFT/RIGHT arm configurations: sin OL = sin ( - a) = sin qcos a - cos Osin a = pyr - pz (19) R2 cos 0L = cos(q — a) = cosocosa + sino sina = Pr + P (20) R2 sin 0R = sin(xr + 4 + a)= -pyr - p.d2 (21) R2 cos OR = cos(?r + ~ + a)= -pr + Pyd2 (22) Inverse Kinematics for Puma Robots 122 Inverse KfCinematics for Puma Robots 12

RSD-TR- 1-83 xo-yo plane Yo ZL (px,py) -A = d2 i<B = r - PA + pC - d= 0, - L, X, LEFT Arm / Yo inner cylinder with radius d2 (Pxy)A = d2 B ~/ I \ // AB = r = P+y -dr /' / I.' + ~ + / = o' B + py /A R IG HT Arm X1 Figure 6. Joint One Solution 13 Inverse Kinematics for Puma Robots

RSD-TR-1-83 Combining Eqs. 19-22 and using the ARM indicator to indicate the LEFT/RIGHT arm configuration, we obtain the sine and cosine functions of el respectively: - ARM. pv/p + p - p-d2 (23) sin l1- 2 2 PZ + Py s - ARM' pr/ppr + -d yd2 (24) pz + Py where positive square root is taken in these equations and ARM is defined as in Eq. 9. In order to evaluate 81 for -7r < 01 < r, an are tangent function, atan2(y ), which returns tan-'(i ) adjusted to the proper quadrant will be used. It is defined as: 00 < 0 < 900; for +x and +y (25) 900< 8 < 180; for -z and +y 0 = atan2() z -1800 < 8 < -900; for -z and -y -900 < 0 <; for +z and -y From Eqs. 23 and 24, and using Eq. 25, e8 is found to be: 81 = atanr2 1= atan2 -; -Ar < 81.<.r cos61 ]- ARM - pzVp +pi -yd2 + pyd2 Joint Two Solution. To find joint 2, we project the position vector p onto the xl-yl plane as shown in Figure 7. From Figure 7, we have four different arm configurations. Each arm configuration corresponds to different values of joint two as: Inverse Kinematics for Puma Robots 14

RSD-TR- 1-83 Arm Configurations 02 ARM ELBOW ARM ELBOW LEFT and ABOVE arm a — B -1 +1 -1 LEFT and BELOW arm a + | -1 -1 +1 RIGHT and ABOVE arm a + 0 +1 +1 +1 RIGHT and BELOW arm a - +1 1 -1 where o~0 < a < 360 and o~0 < 5 < so~. Table 1. Various Arm Configurations for Joint Two From the above table, 02 can be expressed in one equation for different arm and elbow configurations using the ARM and ELBOW indicators as: 02 a + (ARM ELBOW a + K (27) where the combined arm configuration indicator K = ARM * ELBOW will give an appropriate signed value and the "dot" represents a multiplication operation on the indicators. From the arm geometry in Figure 7, we obtain: R= /p p + p- _Y; r= p + py- d (28) Pz Pz (29) sin a= ---- = — -- ARM r ARM p2 +,- (30) Cosa =- R - - + 'py + ' Z a2 + R2- ( 42+ ) p2 +p+ a- y2 + p+2 + a2-) ( 31) 2a2R 2a2pi, o +P +pd 15 Inverse Kinematics for Puma Robots

IRSD-TR-1-83 (PxsPyPz) Z2 -B2 Ep E OA = d2 EF = Px 3 ',AB = a2 EG = py BC = a3 DE Pz?Z1 -'02 CD= d4 AD - R + p= 2 AE = r -- TE- Er-5 /px~'+pY" -d2 y x, 0 G Figure 7. Joint Two Solution sin = v-i - co s2 (32) From Eqs. 27-32, we can find the sine and cosine functions of 02: sin 02 = sin (a + K. B) = sin acosB + (ARM. ELBO W)cosasin B (33) cos 82 = cos(a + K B) = cos cos B - (ARM ELBOW)sin asin~ (34) Inverse Kinematics for Puma Robots 16

RSD-TR- 1-83 From Eqs. 33 and 34, we obtain the solution for 02: 02 = atan2 2 ] -7r < 2; -r (35) cos -02 Joint Three Solution. For joint 3, we project the position vector p onto the x2-y2 plane as shown in Figure 8. From Figure 8, we again have four different arm configurations. Each arm configuration corresponds to different values of joint three as: Arm Configurations (pa2)y 03 ARM ELBOW ARM ELBOW LEFT and ABOVE arm > 0 >- | -1 +1 -1 LEFT and BELOW arm 0 - - - 1 +1 RIGHT and ABOVE arm < 0o - X +1 +1 +1 RIGHT and BELOW arm >0 - _ +1 -1 -1 Table 2. Various Arm Configurations for Joint Three where (p 2)y is the y-component of the position vector from the origin of (x2,y2,z2) to the point where the last three joint axes intersect. From the arm geometry in Figure 8, we obtain the following equations for finding the solution for 03: R= vJp + py + pZ - (36) cos a2 + ( 42 + a32.R; sin b = ARM. ELBOW V/1 - co3 2a2 +(d42 + a32 sI 1 =ARMELBOWV -co 2 (37) sin- -,rd4; Cos a31 (38) 17 Inverse Kinematics for Puma Robots

RSD-TR- 1-83 C AA = a2 X2\ ~~~~~5 ~~ ~ CD =- d4 A, _2xo a2 B B =- -a +d4 = R v p +p2 z d2-2 X3 LEFT and BELOW Arm C d4 A~, _- Dx2 a2 B 33 - - 90~ X3l LEFT and BELOW Arm c3 X2 d= - R 2P3 LEFT and ABOVE Arm D Figure 8. Joint Three Solution Inverse Kinematics for Puma Robots 18

RSD-TR- 1-83 From Table 2, we obtain the equation for 03: 03 = 0- - (39) From Eq. 39, the sine and cosine functions of 03 are, respectively: sinO3 = sin (q - B) - sin ocosfl - coso sin B (40) cos03 = cos(- - B) coskcosB + sinbsin/B (41) From Eqs. 40 and 41, and using Eqs. 36-38, we find the solution for 03: 03 = atan2 cossin 03; I< 3 < ir (42) 4.3. ARM SOLUTION FOR THE LAST THREE JOINTS OF A PUMA ROBOT ARM Knowing the first three joint angles, we can evaluate the T03 matrix which is used extensively to find the solution of the last three joints. The solution of the last three joints of a PUMA robot arm can be found by setting these joints to meet the following criteria: (1) Set joint 4 such that a rotation about joint 5 will align the axis of motion of joint 6 with the given approach vector (a ofT) (2) Set joint 5 to align the axis of motion of joint 6 with the approach vector. (3) Set joint 6 to align the given orientation vector ( or sliding vector or Y6 ) and normal vector. Mathematically the above criteria respectively mean: = (,3 X a) (43) 4= 3; givena = (a,, (43) a z5; given a -(a, ay, a.)T (44) 19 Inverse Kinematics for Puma Robots

RSD-TR-1-83 s = y6; given s = (SZ,sy, s)T and n = (n,,n,,nz)T (45) In Eq. 43, the vector cross product may be taken to be positive or negative. As a result, there are two possible solutions for 04,. If the vector cross product is zero (i.e. z3 is parallel to a), it indicates the degenerate case. This happens when the axes of rotation for joint 4 and joint 6 are parallel. It indicates that at this particular arm configuration, a five-axis robot arm rather than a six-axis one would suffice. Joint Four Solution. Both orientations of the wrist ( UP and DOWN ) are defined by looking at the orientation of the hand coordinate frame (n,s,a) with respect to the (xs,y5,z5) coordinate frame. The sign of the vector cross product in Eq. 43 cannot be determined without referring to the orientation of either the n or s unit vector with respect to the x5 or y5 unit vector, respectively, which have a fixed relation with respect to the X4 unit vector from the assignment of the link coordinate frames. (From Figure 2, we have the Z4 unit vector pointing at the same direction as the y5 unit vector ) We shall start with an assumption that the vector cross product in F.q. 43 has a positive sign. This can be indicated by an orientation indicator n which is defined as: O; if in the degenerate case (46) ~ q s-Y5; if s'ys5 0 n'y5; if -y5 O From Figure 2, y5= Z4 and using Eq. 43, the orientation indicator Q can be rewritten as:; if in the degenerate case ~~(~Z~~3 ~X a) ~(47) (l3 X all; if (3 X a) 0.= ']Za X al; if s'(z3 X a) O (53 x a) 1' '; if '(a X (X&)=-0 If our assumption of the sign of the vector cross product in Eq. 43 is not correct, it will be corrected later using the combination of the WRIST indicator and the orientation indicator Q. The n is used to indicate the initial orientation of the z4 unit vector (positive direction) from the link coordinate systems Inverse Kinematics for Puma Robots 20

RSD-TR- 1-83 assignment, while the WRIST indicator specifies the user's preference of the orientation of the wrist subsystem according to the definition given in Eq. 11. If both the orientation ~i and the WRIST indicators have the same sign, then the assumption of the sign of the vector cross product in Eq. 43 is correct. Various wrist orientations resulting from the combination of the various values of the WRIST and orientation indicators are tabulated in Table 3. Wrist Orientation n2 = 'y5 or n y5 WRIST M = WRIST- sign(f2) DOWN > 0 +1 +1 DOWN < o +1 -1 UP > 0 -1 -1 UP < -1 +1 Table 3. Various Orientations for The Wrist Again looking at the projection of the coordinate frame (x4,y4,N4) on the x3-y3 plane and from the Table 3 and Figure 9, it can be shown that the followings are true (see Figure 9): sin04 = -M'(Z4 x3); cosM4 =- M'(4 'Y3) (48) where x3 and y3 are the z and y column vector of To3 respectively, M-= WRIST-sign(Q), and the sign function is defined as: ign +1;if x> (49) sign~x)~ -1;if x<0 Thus the solution for 84 with the orientation and WRIST indicators is: 94 = atan2 r sin ata2 M.(Cla - SIam) 1< r cos 4 M n ( C1 C23 a,+S1 C23a.-S23 a) -ir 04 () If the degenerate case occurs, any convenient value may be chosen for 04 as long as the orientation of the wrist (UP/DOWN) is satisfied. This can always be ensured by setting 04 equals to the current value of 04. In addition to this, 21 Inverse Kinematics for Puma Robots

RSD-TR-1-83 _ __.._ Z4 sinL44 =-(z4 x3) 1 94 Y3 cos -94 = (Z Y3) 04 x4 x3 Figure 9 Joint Four Solution the user can turn on the FLIP toggle to obtain the other solution of 04, that is 04 = 84 + 1800~. Joint Five Solution. To find 85, we use the criterion that aligns the axis of rotation of joint six with the approach vector ( or a- z5 ). Looking at the projection of the coordinate frame (x5,y5,s6) on the x4-y4 plane, it can be shown that the followings are true (see Figure 10): sin5 - a x4; cos085- -(a y4) (51) where X4 and y4 are the x and y column vector of Too respectively and a is the approach vector. Thus the solution for 85 is: Inverse Kinematics for Puma Robots 22

RSD-TR- 1-83 sin '5 = a x4 COS t5= -(a Y4) K 6 n a x4 Figure 10 Joint Five Solution 05 = atan2.?r <.55 1 5 t,,COS[5 j - ' - 5+-(52) = ata2 (C1 C23 C4-SI S4)as+(SIC23sC4+CS4)ar-C4S23a I ~atan2 ~ ~ c, S23 a+ sI 3.a+G3 a, If es5 O, then the degenerate case occurs. Joint Six Solution. Up to now, we have aligned the axis of joint 6 with the approach vector. Next we need to align the orientation of the gripper to ease picking up the object. The criterion for doing this is to set s = Y6. Looking at the projection of the hand coordinate frame (n,s,a) on the x5-y5 plane, it can be shown that the followings are true (see Figure 11): 23 Inverse Kinematics for Puma Robots

RSD-TR-1-83 sin 6=n y5; cosO6=s-ys (53) where y5 is the y column vector of To5 and n and s are the normal and sliding vectors of T06 respectively. Thus the solution for 86 is: 86 = atan2[ sin6 ] (54) atan2[ (-S1 C4-C1 C23S4)nz+(C1C4-S1 C23S4)ny+(S4S23)n ] (-S1 C4-C1 C23S4),+(C1 C4-' C23S4),y+(S4&S,3)8z If the degenerate case occurs, then (64 + 86) equals to the total angle required to align the sliding vector (s) and the normal vector (n). If the FLIP toggle is on S sin 9n6 ' n \5 6 - Y5 cos t96 = Y5 196 x5 Figure 11 Joint Six Solution Inverse Kinematics for Puma Robots 24

RSD-TR- 1-83 (i.e. FLIP=1), then 04 = 04 + r,0 -- -05, and 6 06 + 7. In summary, there are eight solutions to the inverse kinematics problem of a six-joint PUMA robot arm. There are four solutions for the first three joint solutions - two for the right shoulder arm configuration and two for the left shoulder arm configuration. For each arm configuration, Eqs. 26, 35, 42, 49, 52, and 54 give one set of solution (81,02,03,04,05,06) and (01,02,03,84+r,-65,06+7r) (with the FLIP toggle on) give another set of solution. 5. DECISION EQUATIONS FOR THE ARM CONFIGURATION INDICATORS In the previous section, the arm solution of a PUMA robot arm has been derived. The solution is not unique and depends on the arm configuration indicators specified by the user. These arm configuration indicators (ARM, ELBOW and WRIST) can also be determined from the joint angles. In this section, we shall derive the respective decision equation for each arm configuration indicator. The signed value of the decision equation (positive, zero, or negative) provide an indication of the arm configuration as defined in Eqs. 9-11. For the ARM indicator, following the definition of the RIGHT/LEFT arm, a decision equation for the ARM indicator can be found to be: 0) X p -in c k 1 9(8,p)- Zo' ][Il X p' [[ -'sin1 cos81 0 Ps Py 0 (55) -pysin01 - p.cosel Izli x p' 1 where p' = (p,,p,,)T is the projection of the position vector p (Eq. 14) onto the x0-yO plane, s = (-sinl1,cos0l,O)T from the third column vector of To1, and o = (0,o0, 1) If g(0,p) > 0, then the arm is in the RIGHT arm configuration. If g(o,p) < 0, then the arm is in the LEFT arm configuration. If g({,p) = o, then the criterion for finding the LEFT/RIGHT arm configuration cannot be uniquely determined. The arm is within the inner cylinder of radius d2 in the workspace (see Figure 6). In 25 Inverse Kinematics for Puma Robots

RSD-TR-1-83 this case, it is default to the RIGHT arm (ARM = +1). Since the denominator of the above decision equation is always positive, the determination of the LEFT/RIGHT arm configuration is reduced to checking the sign of the numerator of g(O,p): ARM= sign(g(O,p)) = sign(-pzcosO1 - pysin 81) (56) where the sign function is defined in Eq. 49. Substituting the x and y components of p from Eq. 14, Eq. 56 becomes: ARM = sign(g(o,p)) = sign(g(o)) -= ign(- d4S23 -a3C23 - a2C2) (57) Hence from the decision equation in Eq. 57, one can relate its signed value to the ARM indicator for the RIGHT/LEFT arm configuration as: +1 -> RIGIT arm (58) ARM - sin(- d4S2s-a3C2- a2C2) -= -1 — > LEFT arm For the ELBOW arm indicator, we follow the definition of ABOVE/BELOW arm to formulate the corresponding decision equation. Using (p )y and the ARM indicator in the Table 2, the decision equation for the ELBOW indicator is based on the sign of the y-component of the position vector of A23 A24 and the ARM indicator: +1 -=> ELBOW above wrist f5\ ELBOW = ARM sign( d4C3 - a3S3) - -1 - ELBOW below wrist For the WRIST indicator, we follow the definition of DOWN/UP wrist to obtain a positive dot product of the s and y5 (or s4 ) unit vectors: WRIST=+ 1; if.z=4 <> o (60) WRIST — -1; if s'z4 < 0 = s/gn(s'z4) If as'Z = 0, then the WRIST indicator can be found from: q+1; if n'Z4 > 061 WRIST -1; if n=4 < O ign(ns4) (61) Combining Eqs. 60 and 61, we have Inverse Kinematics for Puma Robots 26

RSD-TR- 1-83 s ign(s"4); if 'Z4 i4 +1; WRISTDOWN (62) WRIST= sign(n s4); if sZ4= - -1; WRIST UP These decision equations provide a verification of the arm solution. We use them to preset the arm configuration in the direct kinematics and then use the arm configuration indicators to find the inverse kinematics solution. (See Figure 12) 6. COMPUTER SIMULATION A computer program was written to verify the validity of the inverse solution of the PUMA robot arm shown in Figure 2. The software initially generates all the locations in the workspace of the robot within the joint angles limits. They are inputed into the direct kinematics routine to obtain the arm matrix T. These joint angles are also used to compute the decision equations to obtain the three arm configuration indicators. These indicators together with the arm matrix T are fed into the inverse solution routine to obtain the joint angle solution which should agree to the joint angles fed into the direct kinematics routine previously. A computer simulation block diagram is shown in Figure 12 and a list of the computer program written in PASCAL is given in the APPENDIX. 7. CONCLUSION The kinematics and inverse kinematics problems of a PUMA robot arm have been discussed. The inverse solution is determined with the assistance of three arm configuration indicators (ARM, ELBOW, and WRIST). There are eight solutions to a six-joint PUMA robot arm- four solutions for the first three joints and for each arm configuration two more solutions for the last three joints. Computer simulation of the direct and inverse kinematics showed that the above derived arm solution is correct. This approach, with appropriate modification and adjustment, can be generalized to other simple industrial robots with rotary joints. 8. ACKNOWLEDGEMENT The authors would like to thank Robert Horner who wrote a "C" program to verify the above direct and inverse kinematics equations together with their corresponding decision equations. The authors also would like to thank Richard Jungclas who wrote the above kinematic equations in PASCAL and verified 27 Inverse Kinematics for Puma Robots

RSD-TR-1-83 POSITION AND OR IENTATION JOINT ANG LES OF THE END-EFFECTOR DIRECT KINEIATICS. n s a p T= 0 0 0 1 E RRORDECISION EQUATIONS ERROR ARM, ELBOW, WRIST INVERSE KINENIATICS Figure 12. Computer Simulation of Joint Solution them by controlling a PUMA robot arm from an IBM PC. Inverse Kinematics for Puma Robots 28

RSD-TR- 1-83 9. REFERENCES 1. D. L. Pieper, "The Kinematics of Manipulators Under Computer Control," Computer Science Department, Stanford University, Artificial Intelligence Project Memo No. 72, October 24, 1968. 2. R. P. Paul, "Modeling, Trajectory Calculation, and Servoing of a Computer Controlled Arm," Stanford Artificial Intelligence Laboratory Memo AIM177, November 1972. 3. R. A. Lewis, "Autonomous Manipulation on a Robot: Summary of Manipulator Software Functions," Technical Memo 33-679, Jet Propulsion Lab, March 15, 1974. 4. R. P. Paul, B. E. Shimano, and G. Mayer, "Kinematic Control Equations for Simple Manipulators," IEEE Transactions of Systems, Man, and Cybernetics, Vol. SMC-11, No. 6, June 1981, pp 449-455. 5. C. S. G. Lee, "Robot Arm Kinematics, Dynamics, and Control," Computer, Vol. 15, No. 12, December 1982, pp 62-80. 6. R. P. Paul, Robot Manipulators: Mathematics, Programming and Control, M.I.T. Press, 1981, pp 50-84. 7. J. J. Uicker, Jr., J. Denavit, and R. S. Hartenberg, "An Iterative Method for the Displacement Analysis of Spatial Mechanisms," Trans. of ASME, Journal of Applied Mechanics, Vol. 31, Series E, 1964, pp. 309-314. 8. V. Milenkovic and B. Huang, "Kinematics of Major Robot Linkages," Proceedings of the 13th International Symposium on Industrial Robots, April 17-19, 1983, Chicago, Illinois, pp 16-31 to 16-47. 9. J. Denavit and R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," Journal of Applied Mechanics, June 1955, pp. 215-221. 29 Inverse Kinematics for Puma Robots

LA im 1 a30U U1U116 04-04-84 15:47:27 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 00 1 {$linesize:132,$pagesize:60} 2 {$title:'Main PUMA Program (main. pas)',$subtitle:'Last change 4-3-84'} 3 {$SPEED, $DEBUG-,$list+,$INDEXCK-,$NILCK-,$RANGECK-,$STACKCK-,$OCODE-} 4 5 { 7 8 University of Michigan 9 College of Engineering 10 Center for Robotics and Integrated Manufacturing 11 Robot Systems Division 12 2510 East Engineering Building 13 Ann Arbor, MI 48109 14 (313) 764-4343 15 16 Copyright 1984, The University of Michigan, All Rights Reserved 17 18 19 Written by: Richard M. Jungclas 20 21 22 23 This system is used for the verification and development of the 24 direct kinematics and the inverse kinematics solutions for a six 25 jointed PUMA robot. The system uses OA Geometric Approach in 26 Solving the Inverse Kinematics of PUMA Robots' developed by 27 C.S.G. Lee and M. Ziegler of the Electrical and Computer Engineering 28 department of the University of Michigan. 29 30 The actual PUMA routines were developed for an offline robot 31 programming project using the IBM PC being developed at the 32 &obot Systems Division of CRIM. As a result these solutions 33 have been extensively tested and compared with the soutions 34 reported by VAL II. We have found the solutions from the IBM PC 35 are within +1- 0.005 of a degree for the angles and within 36 +1- 0.005 of a millimeter for positions reported by VAL II. Generally. 37 the IBM PC gives solutions within +1- 0.001 of a degree or of a 38 millimeter. 39 40 The actual interface given here is a bit simplistic, but serves to 41 illustrates how the PUMA routines are used. While the interface 42 given below does not allow specifying of either the tool to mount 43 transformation or the robot reference to world transformation, the 44 solutions implemented allow for these transformations. The interface 45 assumes an identity transformation for the tool to mount transformation 46 and assumes that the world coordinate frame is a the base of linkO 47 but oriented the same as the 'shoulder" (linki) coordinate frame. 48 49 The interface uses a menu driven by single character inputs. The 50 menu is displayed on the top, left part of the screen. The key 51 used to select the menu item is given in parenthesis. The current 52 system status is diplayed on the top, right portion of the screen. 53 Data, various prompts and error messages are display on the lower Inverse Kinematics for Puma Robots

Main PUMA Program (main.pas) Page 2 Last change 4-3-84 04-04-84 15:47:27 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 54 half of the screen. 55 56 Locations can be specified in either world cartesian, robot cartesian 57 or robot Joint coordinates frames. The default is the world cartesian 58 coordinate frame. The robot cartesian coordinate frame is exactly the 59 as the VAL II "trans' type locations. The Joint coordinate frame is 60 exactly the same as the VAL II 'precision point" locations. Locations 61 reported by all three types. The current type is display in the 62 status area of the menu. 63 64 The interface allows you to assign symbolic names of up to 12 characters 65 to any location. The 'type' of the location is determined by the 66 current type setting at the time the symbol is defined and cannot be 67 changed. There is no method at the moment of preserving the symbolic 68 names between sessions. 69 70 The current PUMA arm configuration is also display in the status area. 71 72 73 74 The main commands are: 75 76 Move Moves the robot to location specified by either a 77 symbolic location or directly from the eyboard. 78 Entries from the keyboard use the 'type" from the 79 current setting. The location in all three types is 80 reported for valid solutions. 81 82 Name Names the current location. The type of the symbolic 83 location is the 'type' from the current setting. 84 85 Robot Config. Starts a submenu allowing changes of all the 86 Robot Configuration settings. 87 88 Where Reports the current location in all three types 89 90 Exit Terminates the program. 91 92 93 94 The Robot Configuration commands are: 96 96 Left/Right Changes the PUMA arm configuration to Left arm or 97 Right arm. 98 99 Above/Below Changes the PUMA arm configuration to elbow Above 100 the wrist or elbow Below the wrist. 101 102 Up/Down Changes the PUMA arm configuration to wrist Up or 103 wrist Down. 104 105 Flip/Noflip Allows the wrist configuration to changed or not. 106

111d.L1u Vi r1_ Uain.pasi Page 3 Last change 4-3-84 04-04-84 15:47:27 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 107 Joint/Cartesian Specifies either Joint or, Cartesian coordinate 108 frames. When cartesain coordinates are specified 109 a World/Robot menu item will appear to allow 110 selection of the type of cartesian coordinate frames. 111 112 Robot/World Specifies the type of cartesian coordinates. Only 113 present if Cartesian coordinate are choosen. 114 115 Trace/Notrace Permits the tracing of valid location in a file 116 named PUMA.DBG on the current directory. 117 118 Debug/Production Permits the tracing of debugging information in 119 a file named PUMA.DBG on the current directory. 120 121 122 123 The system uses a standard device call sercom as a mean of collecting 124 debugging information, data, etc.. By default this the file PUMA.DBG 125 is assigned to this device during initialization. 126 127 } 128 0 {$INCLUDE:'global.inc'} 10 157 {$LIST+) 0 {$INCLUDE:'debug.inc'} 10 23 {$LIST+} 0 ($INCLUDE:'menu.inc'} 10 48 {$LIST+} 132 133 {$title:'Main PUMA Program (main.pas) '$subtitle:'Last change 4-3-84'} 134 135 10 136 program robot~input,output); 137 10 138 uses debug; 10 139 uses globals; 10 140 uses menu-functions; 141 142 143 10 144 var 10 145 last move, Hflag for last move 10 146 invaTid command,!flag for bad command 10 147 leave.pgm: boolean; Hflag for program termination 10 148 commalid, 'char command from user 10 149 spec: char; 150 151 152 10 153 var [public] 10 154 sercom: filet;!Master debugging file 155 156 CONFIG ROBOT

Main PUMA Program (main.pas) Page 4 Last change 4-3-84 04-04-84 15:47:27 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 10 157 procedure initialize; external; Syntab 157 Offset Length Variable - INITIALIZE - 2 2 Return offset, Frame length 10 158 Procedure config_robot; external; Symtab 158 Offset Length Variable - CONFIG ROBOT - 2 2 Return offset, Frame length 10 159 Procedure writeloc(var dev: filel); external; Symtab 159 Offset Length Variable - WRITELOC - 4 2 Return offset, Frame length + 6 2 DEV:File VarP 10 160 Procedure nameloc; external; Symtab 160 Offset Length Variable - NAMELOC - 2 2 Return offset, Frame length 10 161 function move(var tracefil:filel): boolean; external; Symtab 161 Offset Length Variable - MOVE - 4 4 Return offset, Frame length - 2 1 (function return):Boolean + 6 2 TRACEFIL:File VarP MOVE

nain runA rrogram (main.pas) Page 5 Last change 4-3-84 04-04-84 16:47:27 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 162 {$PAGE+} 10 163 begin { MAIN } 164 165 { 166 Here is where we wake up. The first things that have to be done is 167 to initialze the system. 168 } 169 11 170 initialize; 171 172 { 173 This is the root level of the menu. 174 175 leave_pgm is a flag for program termination. When it is set to 176 true, program execution is done. 177 178 Invalid command is a flag that is used within each menu in the 179 system.-When it is true, it indicates that the PREVIOUS command the 180 user typed was invalid. This flag is used in determining whether or 181 not the prompt error should be erased and the menu reprinted. If the 182 last user command was invalid, there is probably an error message in 183 prompt area, meaning the user should be prompted without clear that 184 area first. 185 186 } 187 11 188 last move:= false; 11 189 leave pgm:= false; 11 190 invalid command:= false; 191 11 192 while not leave_pgm do begin 193 12 194 if (not invalid command) 12 195 then begin - 13 196 display menu(menu flag, 'PUMA'); 13 197 menu item('Robot configuration'); 13 198 menu-item ('Move '); 13 199 menu-item('Name this location'); 13 200 menu-item ('Where'); 13 201 menu-item('Exit the program'); 13 202 end - 12 203 else invalid command:= false; 204 12 205 if last move 13 206 then-begin; 13 207 data prompt; / 13 208 writiloc(output); 13 209 last move:= false; 12 210 end; 211 12 212 command prompt; 12 213 write('Enter PUMA command: '); 12 214 repeat until getc(command,spec); ROBOT

Main PUMA Program (main.pas) Page 6 Last change 4-3-84 04-04-84 15:47:27 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 12 215 data prompt; 216 13 217 case command of 218 219 13 220 'm','M': last move: move(sercom);!Move the robot 221 222 13 223 'n','N': nameloc;!Name to current location 224 225 13 226 'r','R': config robot;!Change robot configuration 227 228 14 229 'w','W': begin;!Report robot location 14 230 writeln('Robot location:'); / 14 231 writeloc(output); 13 232 end; 233 234 13 235 'e','E': leave_pgm:= true; IProgram termination 236 237 238 13 239 otherwise begin Invalld commands 14 240 invalid command:= true; 14 241 write('T',command,') is an invalid command') 13 242 end; 243 12 244 end; 245 11 246 end; 247 11 248 cls; 249 00 250 end. Symtab 250 Offset Length Variable 0 666 Return offset, Frame length 0 64 TO:Array Static Extern 0 64 H:Array Static Extern 0 64 HI:Array Static Extern 0 64 TOI:Array Static Extern 28 1 SPEC:Char Static 0 14 CONFIG:Array Static Extern 0 24 THETA:Array Static Extern 30 636 SERCOM:File Static Public 0 8 VERSION:Array Static Extern 0 24 ROB XYZ:Array Static Extern 26 1 COMMAND:Char Static 0 2 FIRST STR:Pointer Static Extern 0 1 MENU FLAG:Boolean Static Extern 20 1 LAST-MOVE:Bool on Stt.i t

A ~ ~ ~ Page 7 Last change 4-3-84 04-04-84 15:147:27 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 24 1 LEAVE PGM:Boolean Static 0 2 MENU CURSOR:Integer Static Extern 0 2 COOR6S TYPE:Integer Static Extern 22 1 INVALIJ3 COMMAND:Boolean Static Errors Warns In Pass One 0 0

Main program routines(routines.pas) Page 1 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 00 1 {$linesize:132,$pagesize:60} 2 {$title:'Main program routines(routines.pas)',$subtitle:'Last change: 4-3-84'} 3 {$SPEED, $DEBUG-,$list+, $INDEXCK-, $NILCK-, $RANGECK-,$STACKCK-,$OCODE-} 4 5 7 8 University of Michigan 9 College of Engineering 10 Center for Robotics and Integrated Manufacturing 11 Robot Systems Division 12 2510 East Engineering Building 13 Ann Arbor, MI 48109 14 (313) 764-4343 15 16 Copyright 1984, The University of Michigan, All Rights Reserved 17 18 19 Written by: Richard M. Jungclas 20 21 22 } 23 24 0 ($INCLUDE: 'global.inc') 10 157 ($LIST+} 0 {$INCLUDE:'debug.inc'} 10 23 {$LIST+} 0 ($INCLUDE:'menu.inc'} 10 48 {$LIST+) 0 {$INCLUDE: 'puma.inc'} 10 35 {$LIST+} 29 30 {$title:'Main program routines(routines.pas),$subtitle:'Last change: 4-3-84') 31 00 32 Module main routines; 33 10 34 uses globals; 10 35 uses debug; 10 36 uses menu functions; 10 37 uses pumaT 38 10 39 var 10 40 sercom [extern]: filel; 41 42 MAIN ROUTINES

Main program routines(routines.pas) Page 2 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 43 {$PAGE+} 10 44 procedure initialize; 45 46 47 PROCEDURE INITIALIZE; 48 49 Purpose: Performs all the initializations necessary for the 50 user to begin running the system. 51 52 53 20 54 var 20 55 i,!loop counters 20 56 J: integer; 20 57 invertible: boolean; 20 58 temp STR: STR_ptr; 59 60 20 61 begin 62 = 21 63 sercom.trap:= true;!Setup standard debugging file / 21 64 assign(sercom, 'PUMA.dbg'); / 21 65 rewrite(sercom); 21 66 if sercom.errs > 0 21 67 then writeln('Unable to open sercom file! Code=', sercom.errs:1); 68 = 21 69 version:= 'AR044.0';!System version 70 71 72 73 Initialize the robot data structures 74 } 21 75 initrobot; 76 = 21 77 config[O]:= 1;!right arm = 21 78 config[l]:= 1;!above elbow = 21 79 config[2] -1;!wrist up = 21 80 config[3]:= -1;!noflip (wrist) = 21 81 config[4]:= 0;!initially valid solution = 21 82 config[5]:= 0;!production = 21 83 config[6] 0; 84 85 { Robot(Shoulder) coords w/ Null Tool = 21 86 xyz[1]:= -20.32; = 21 87 xyz[2] = 149.09; = 21 88 xyz[3]: 921.12; = 21 89 xyz[4]:= 90.0; = 21 90 xyz[5]: -90.0; = 21 91 xyz[6]: 0.0; = 21 92 coords type:= robot type; = 21 93 theta[T] = 0.0; = 21 94 theta[2]:= -90.0; = 21 95 theta[3]: 90.0; INITIALIZE

Main program routines(routines.pas) Page 3 Last change: 4-3-84 04-04-84 JG Ic Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 = 21 96 theta[4] 0.0; - 21 97 theta[5]: 0.0; - 21 98 theta[6] 0.0; 99 1oo 101 Find the initial position of robot 102 21 103 homotran; 21 104 inverse; 105 106 21 107 if not Joint check 21 108 then writiwt('Initial robot configuration bad'); 109 110 - 21 111 firstSTR:= nil;!No symbols to start 112 113 { 114 Predefined symbols 115 } 21 116 new~temp STR); 21 117 temp STRU.symname: 'ready'; 21 118 temp STR@.data: theta; 21 119 temp STR@.ctype joint_type; 21 120 temp STR@.used true; 21 121 temp STR@.next STR: firstSTR; - 21 122 firstSTR timpSTR; 123 124 - 21 125 menuflag:= true;!Full menu to start 126 = 21 127 coordstype:= world type;!default user to world coords 128 10 129 end; Symtab 129 Offset Length Variable - INITIALIZE - 2 10 Return offset, Frame length - 2 2 I:Integer - 4 2 J:Integer - 6 1 INVERTIBLE:Boolean - 8 2 TEMP STR:Pointer INITIALIZE

main program routines(routines.pas) Page 4 Last change: 4-3-84 04-04-84 14: 16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 130 {$PAGE+} 10 131 Procedure config robot; 132 133 134 PROCEDURE CONFIG ROBOT; 135 136 Purpose: Acts as the driver for the robot configuration 137 commands. 138 139 140 20 141 var 20 142 command,!to hold usirlr's command 20 143 spec: char; 20 144 leave,!flag for returning up one level 20 145 invalid-command: boolean;!valid command flag 146 147 20 148 begin 149 { 150 This menu contains the robot configuration programming commands. 151 } 152 21 153 leave: false; 21 154 invalid-command:= false; 155 21 156 while not leave do 21 157 begin 22 158 if not invalid command 22 159 then begin - 160 / 23 161 display menu(menu flag, 'PUMA/config'); 23 162 bmenu item('PUMA ienu'); 23 163 if coffig[0] = 1 23 164 then menu item('Left arm ') 23 165 else menu-itemC'Right arm'); 23 166 if configE1J- 1 23 167 then menu itemC'Below elbow') 23 168 else menu-itemC'Above elbow'); 23 169 if configE2]- 1 23 170 then menu item('Up wrist ') 23 171 else menu-itemC'Down wrist'); 23 172 if configt3V= 1 23 173 then menu itemC'Noflip wrist') 23 174 else menu-itemC'Flip wrist '); 24 175 case coords type of 24 176 world type,robot type: 25 177 Begin; 25 178 menu itemC'Joint coordinates'); 25 179 if coords type = world type 25 180 then gmenu itemC'M'?Robot coordinates') 25 181 else menuTtem('World coordinates'); 24 182 end; CONFIG ROBOT

Main program routines(routines.pas) Page 5 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 24 183 joint_type: 24 184 menu item('Cartesian coordinates'); 24 185 otherwise24 186 23 187 end; 23 188 if config[5] = 0 23 189 then gmenu item('E', 'Debug mode') 23 190 else menu Item('Production mode'); 23 191 if config[6]- 0 23 192 then menu item('Trace moves') 23 193 else gmenu item('O', 'No trace'); 194 23 195 display_status; 22 196 end; 197 22 198 invalid command:= false; 22 199 command-prompt; 22 200 write('Enter robot programming command: '); 22 201 repeat until getc(command,spec); 22 202 dataprompt; 203 23 204 case command of 205 206 23 207 '-': leave:= true;!Back to PUMA menu 208 209 210 = 23 211 'l','L': config[O]:= -1;!Left Arm 212 = 23 213 'r','R': config[O]:= 1;!Right arm 214 215 216 = 23 217 'b','B': config[l]:= -1;!Below elbow 218 = 23 219 'a','A': config[l]:= 1;!Above elbow 220 221 222 = 23 223 'd','D':- config[2]:= 1;!Wrist down 224 = 23 225 'u','U': config[2]:= -1;!Wrist up 226 227 228 = 23 229 'f','F': config[3]:= 1;!Flip of wrist allowed 230 = 23 231 'n','N': config(3]:= -1;!Noflip of wrist allowed 232 233 234 = 23 235 'c','C': coords type:= robot type;!punch robot cartesian coords

ric6 Ia '-'6' V I uu ulines krouuiznes. pas) Page 6 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 236 23 237 J'j,'J": coords type:= joint type; Ipunch joint coords. 238 239 240 241 = 23 242 'i','M': coords type: robot type;!Coordinates 243 = 23 244 'w','W': coords type: world type; 245 246 247 = 23 248 'e','E': configES]:= 1;!Debug Mode 249 = 23 250 'p','P': config[5]:= 0;!Production mode 251 252 253 23 254 't','T': begin ITrace on = 24 255 configL61 1;; 24 256 writeln~sercom); 23 257 end; 258 = 23 259 'o','0': config[61:= 0;!Trace off 260 261 262 23 263 otherwise begin 24 264 write(' ',command,') is an invalid command'); 24 265 invalid-command:= true; 23 266 end; 267 22 268 end; 21 269 end; 10 270 end; Symitab 270 Offset Length Variable - CONFIG ROBOT - 2 10 Return offset, Frime length - 2 1 COMMAND:Char - 4 1 SPEC:Char - 6 1 LEAVE: Boolean - 8 1 INVALIDCOMMAND:Boolean CONFIG ROBOT

Main program routines(routines.pas) Page 7 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 271 {$PAGE+} 20 272 procedure writeloc(var dev: filel); 273 274 275 PROCEDURE WRITELOC(var DEV: filel); 276 277 Purpose: Writes out the robot location in robot coords., 278 world coords. and Joint angles. 279 280 281 20 282 var 20 283 T2, T3, T4, T5, T6: matrix; 284 285 20 286 begin 287 = 21 288 itheta:= theta; 21 289 tmatsCT2, T3, T4, T5, T6); = 21 290 rob_xyz: get xyzoatCT6); 291 292 (add base and hand displacements} = 21 293 tmatrix: mat mult(T6,TO); = 21 294 tmatrix mat-multCH,tmatrix); = 21 295 xyz: get xyz~at tmatrix); 296 21 297 writeln(dev,'world X=',xyz[l]:8:3, Y=',xyzE21:8:3,' Z',xyzE3J:8:3, 21 298 0=',xyz[41:8:3,' A=',xyz(5J:8:3,' T=',xyz[61:8:3); 21 299 writeln(dev,'robot X=',rob xyz lJ:8:3,' Y=',rob xyzE2J:8:3, ' Z=,robxyz(3J:8:3, 21 300 SO=',rob xyz[4]:8:32 A=',rob xyz(5J:8:32 T=',rob xyz[6]:8:3); 21 301 writeln(dev,"joint 1=',theTa{1J:8:3, 2=',thetaT2]:8:3,' 3=',theta[3T:8:3, 21 302 4=',theta[4]:8:3, 5=',theta[5]:8:3,` 6=,theta616:8:3); 303 21 304 writeln(dev); 10 305 end; Symtab 305 Offset Length Variable - WRITELOC 4 386 Return offset, Frame length + 6 2 DEV:File VarP - 64 64 T2:Array - 128 64 T3:Array - 192 64 T4:Array - 256 64 T5:Array - 320 64 T6:Array WRITELOC

Main program routines(routines.pas) Page 8 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 306 {$PAGE+} 10 307 Procedure nameloc; 308 309 310 PROCEDURE NAMELOC; 311 312 Purpose: Gives the current valid robot location a symbolic 313 name. 314 315 } 316 20 317 var 20 318 command, 20 319 spec: char; 20 320 tempSTR: STR ptr; 20 321 name: name lstr; 20 322 input line: cons5l input lstr; 20 323 temp type: integer; 324 20 325 begin 21 326 writeC'Enter robot location name? '); 21 327 readln~input line); 21 328 trim(input lne); 21 329 name:=inputline; 330 21 331 temp STR:= findSTR~name); 332 21 333 if temp STR <> nil 22 334 then hegin;!Matches existing name 32 335 if temp STR@.used 22 336 then write('Overwrite existing used location? ') 22 337 else write('Overwrite existing unused location? '); 22 338 repeat until getc~command,spec); 22 339 if (command <> 'Y') and (command <> '') 23 340 then begin; 23 341 data prompt; 23 342 writiC'Location not changed!'); 23 343 return; 23 344 end 32 345 else if temp STR@.used 22 346 then-begin 23 347 data prompt; 23 348 writi('Previous references use the redefined location!'); 23 349 end 22 350 else data prompt; 22 351 temptype tempSTR~.ctype;!Used the existing coords type 22 352 end 353 22 354 else begin;!allocate new symbol record 22 355 temp type world type; 23 356 case coordstype of 357 23 358 world-type, robot_type: NAMELOC

Main program routines(routines.pas) Page 9 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 23 359 temp_type:= coords_type; 360 23 361 joint type: 23 362 temp_type:= joint_type; 363 22 364 end; 365 22 366 new(temp STR); 22 367 temp STRM.symname: name; 22 368 temp-STR@.ctype:= coords type;!current default setting 22 369 temp-STR@.used:= false; 22 370 temp-STR@.next STR:= first STR; = 22 371 first STR:= timp_STR; 21 372 end; 373 22 374 case temp_type of 375 22 376 world type: 22 377 Begin (add base and hand displacements} = 23 378 xyz:= rob_xyz; 23 379 homotran; = 23 380 tmatrix:= mat mult(tmatrix,TO); = 23 381 tmatrix:= mat-mult(H,tmatrix); 23 382 temp_STR@.data-:= get_xyzoat(tmatrix); 22 383 end; 384 22 385 robot type: 22 386 temp STR@.data:= rob_xyz; 387 22 388 joint type: 22 389 empSTR.data = theta; 390 21 391 end; 392 10 393 end; Symtab 393 Offset Length Variable - NAMELOC - 2 170 Return offset, Frame length - 2 1 COMMAND:Char - 4 1 SPEC:Char - 20 14 NAME:Array - 6 2 TEMP STR:Pointer - 102 82 INPUT LINE:Array - 104 2 TEMP_TYPE:Integer NAMELOC

main program routlnes(routines.pas) Page 10 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 394 {$PAGE+} 20 395 function move(var tracefil: filel): boolean; 396 397 { 398 FUNCTION MOVE(var TRACEFIL: filel): boolean; 399 400 Purpose: Moves robot to new location returning true 401 if the location was within robot's workspace. 402 403 } 404 20 405 var 20 406 command,!to hold user's command 20 407 spec: char; 20 408 leave,!flag for returning up one level 20 409 full: boolean;!command menu flag 20 410 x, IGeneralize robot coordinates 20 411 y, 20 412 z, 20 413 o, 20 414 a, 20 415 t: real; 20 416 temp type, 20 417 temp coord type, 20 418 i: integer; 20 419 temp STR: STRptr; 20 420 name: name lstr; 20 421 input-line: cons5_linputlstr; 422 20 423 begin 424 21 425 leave:= false; 21 426 full:= false; = 21 427 menu flag:= true; 21 428 cls;429 21 430 while not leave do 21 431 begin 432 22 433 while not leave do 22 434 begin 23 435 display menu(full,'Move command'); 23 436 bmenu iEem('PUMA menu'); 23 437 menu Item('Direct from Keyboard'); 23 438 menu-item('Named Location'); 439 23 440 leave:= true; 23 441 command prompt; 23 442 wrlte('Enter move input selection: '); 23 443 repeat until getc(command,spec); 23 444 data prompt; 445 24 446 case command of MOVE

Main program routines(routines.pas) Page ii Last change: 4-3-84 04-04-84 14: 16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 447 24 448 '-': return;!Backout 449 25 450 'd','D': begin;!Keyboard selection 451 25 452 input line:= NULL; 25 453 temp type: coords type; 26 454 case coords type of 455 27 456 world type, robot type: begin; 27 457 if coords type = world type 27 458 then whitelnC'World XYZOAT location') 27 459 else writelnC'Robot XYZOAT location'); 27 460 writeC'Enter X position: '); 27 461 readln(x); 27 462 writeC'Enter Y position: '); 27 463 readln~y); 27 464 writeC'Enter Z position: '); 27 465 readln~z); 27 466 writeC'Enter 0- angle: '); 27 467 readln~o); 27 468 writeC'Enter A angle: '); 27 469 readln a); 27 470 writeC'Enter T angle: '); 27 471 readln~t); 472 = 27 473 zyz[l] X; = 27 474 xyz[2J y = 27 475 xyz[3] z; 476 = 27 477 xyz[4] o; = 27 478 xyzI] a; = 27 479 xyz[6] t: 26 480 end; 481 27 482 joint type: begin; 27 483 writelnC Joint position'); 27 484 writeC'Enter J1 angle: / 27 485 readln(ithetal11J) 27 486 write('Enter J2 angle: '); / 27 487 readln ithetaE2J)'; 27 488 writeC'Enter J3 angle: '); / 27 489 readln(itheta(3J); 27 490 writeC'Enter J4 angle: '); / 27 491 readln itheta[4]); 27 492 writeC'Enter J5 angle: '); / 27 493 readln(ithetaE5]); 27 494 writeC'Enter J6 angle: '); / 27 495 readln(itheta[61); 26 496 end; 497 25 498 end; 499

main program routines(routines.pas) Page 12 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 24 500 end; 501 25 502 'n','N': begin; 25 503 writeC'Enter robot location name? ); 25 504 readln(input line); 25 505 trim(input lTne); 25 506 name: =input line; 507 25 508 if (name = NULL) 25 509 then begin 26 510 leave:= false; 26 511 data prompt; 26 512 end 26 513 else begin; 26 514 temp STR:= first STR; 26 515 whili temp STR <>nil and then temp STR@.symname <> name do 26 516 tempSTR:= tempSTR@.nextSTR; 517 26 518 if temp STR <> nil 27 519 then-begin; IMatches existing name 27 520 temp STR@.used: true; 27 521 temp type:= temp STR@.ctype; 28 522 case temp type of 28 523 world-type, robot type: = 28 524 xyz: tempTR@.data; 28 526 joint type: = 28 526 Itheta: tempSTR@.data; 27 527 end; 27 528 end 27 529 else begin; ISymbol not found 27 530 data prompt; 27 531 leave:= false; 27 532 writeln('Location '',name, ' not found'); 26 533 end; 25 534 end; 24 535 end; 536 25 537 otherwise begin; 25 538 writeln '(',command,') is an invalid selection'); 25 539 leave:= false; 24 540 end; 23 541 end; 542 22 543 end; 544 22 545 clear upper; 22 546 data prompt; 22 547 position cursor(15,0); 548 23 549 case temp type of 550 23 551 world type, robottype: 23 552 Begin MOVE

PUMA robot routines Pg Last change: 4-3-84 rmj 0-48 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 00 1 {$LINESIZE:132 $PAGESIZE:60} 2 {$Title:TPUMA robot routines',$subtitle:'Last change: 4-3-84 rmj'} 3 {$SPEED,$DEBUG-,$LIST+,$INDEXCK-,$NILCK-,$RANGECK-,$STACKCK-,$OCODE-} 4 5 {$MESSAGE:'Enter 1 for debugging information, 0 for normal operation' 6 $INCONST:puma-debug,$INCONST:tmats-debug,$INCONST:homo-debug} 7 8{ 10 11 University of Michigan 12 College of Engineering 13 Center for Robotics and Integrated Manufacturing 14 Robot System Division 15 2514 East Engineering Building 16 Ann Arbor, MI 48109 17 (313) 764-4343 18 19 Copyright 1984, The University of Michigan, All Rights Reserved 20 21 22 Written by: Richard M. Jungclas 23 25} 26 o {$INCLUDE: global.inc'} 10 157 {$LIST+} 0 {$INCLUDE: debug.inc'} 10 23 {$LIST+} 0 {$INCLUDE: puma.inc'} 10 35 {$LIST+} 30 31 {$Title:'PUMA robot routines',$subtitle:2Last change: 4-3-84 rmj'} 32 10 33 implementation of puma; 34 10 35 Uses globals; 10 36 Uses debug; 37 10 38 function a2srqq(consts a,b: real): real; external; Syintab 38 Offset Length Variable - A2SRQQ - 12 2 Return offset, Frame length + 6 4 (function return):Real + 12 4 A:Real Const VarsP + 8 4 B:Real Const VarsP 39 40 10 41 var 10 42 sercom [extern]: filel; 10 43 max-degree: J-matrix;!Joint limits

rur.A roDoo roultnes Page 2 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 10 44 min degree: jmatrix; 10 45 model verts: rmat_ptr; 10 46 A10,!link i to link i-1 transformations 10 47 A21, 10 48 A32, 10 49 A43, 10 50 A54, 10 51 A65: matrix; 52 10 53 const 10 54 D2 = 149.09;!PUMA robot parameters 10 55 A2 = 431.80; 10 56 A3 = -20.32; 10 57 D4 = 433.07; 10 58 D6 = 56.25; 59 { Most of these constants are pre-calculated to maximize numerical 60 accurray, which at best is limited to 7 decimal digits} 10 61 f2 a2 = 863.60;!2.0 * A2 10 62 D2sq = 22227.83;!D2 * D2 10 63 A2sq = 186451.2;!A2 * A2 10 64 A3sq = 412.9024;!A3 * A3 10 65 D4sq = 187549.6;!D4 * D4 10 66 11 = 433.5465;!sqrt( 12 ) 10 67 12 = 187962.5;!14*D4 + A3*A3 10 68 15 = 0.04686926;!abs( A3 ) / 11 10 69 16 = 0.9989010;!D4 / 11 10 70 f2 a2 11 = 374410.7;!2.0 * A2 * 11 10 71 A2-12-= 374413.8;!A2sq + 12 10 72 A2-D4 A3 = -1511.287;!A2sq - D4sq - A3sq 73 10 74 PI = 3.141593; 10 75 twoPI = 6.283185;!2.0 * PI 10 76 f180 pi = 57.29578;!180.0 / PI 10 77 pi 180 = 0.01745329;!PI / 180.0 78 10 79 epsilon = 0.001; 80 A2SRQQ

PUMA robot routines Page 3 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 81 {$PAGE+} 10 82 procedure inverse; 83 84 { 85 PROCEDURE INVERSE; 86 87 Purpose: Calculates the inverse kinematics for the PUMA robot 88 arm. It is given the transformation matrix for the 89 position, and calculates the Joint angles for that 90 position. 91 92 Calling convention: 93 inverse; 94 95 Global Variables: 96 xyz Contains the proposed xyzoat position of 97 the puma arm. 98 tmatrix The homogenous transformation matrix 99 describing the proposed arm position. 100 itheta Returns the inverse solution of the arm 101 (ie. each of the six Joint angles). 102 103 104 } 105 20 106 var 20 107 i: integer;!index 20 108 px,py,pz,!position of the hand 20 109 ax,ay,az,!approach vector components 20 110 sx,sy,sz,!sliding vector components 20 111 nx,ny,nz,!normal vector componets 20 112 sl,s2,s3,s4,s5,s6,s23,!Joint sines and cosines 20 113 cl,c2,c3,c4,c5,c6,c23, 20 114 pxsq,!px * px 20 115 pysq,!py * py 20 116 pzsq,!pz * pz 20 117 rsq,!px*px + py*py + pz*pz - D2*D2 20 118 r,!sqrt( rsq ) 20 119 sal,cal,sbt,cbt,!Misc sine and cosines theta 2 solution 20 120 13,14,!Misc " " theta 3 20 121 omega,!orientation indicator 20 122 z3ax,z3ay,z3az,!z3 cross a components 20 123 kl,k2,k3,k4,!Misc constant terms 20 124 tl,t2,t3,t4: real; 20 125 arm,!ARM is negative(left) 20 126 arm below,!ARM * BELOW is negative 20 127 k: Boolean;!WRIST * sign(Omega) is negative 128 20 129 begin 130 = 21 131 config[4]:= 0;!assume no error 132 133 {

PUMA robot routines Page 4 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 134 First find tO6 matrix, eg T06:= BI * T * HI where B is our TO and T 135 is the "world" transformation! 136 } 21 137 if coords type = world type 22 138 then begin; {Base coords differs from robot coords. = 22 139 tmatrix:= mat mult(HI, tmatrix); = 22 140 tmatrix: mat-mult(tmatrix, TOI); = 22 141 xyz:= get xyzoat(tmatrix); 21 142 end; 143 21 144 nx:= tmatrix(l,1]; 21 145 ny:= tmatrix(1,2]; 21 146 nz:= tmatrix[i,3]; 147 21 148 sx:= tmatrix[2,1]; 21 149 sy:= tmatrix[2,2]; 21 150 sz:= tmatrix[2,3]; 151 21 162 ax:= tmatrix[3,1]; 21 153 ay:= tmatrix[3,2]; 21 154 az:= tmatrix[3,3]; 155 21 156 px:= tmatrix[4, 1] - D6 * ax; 21 157 py:= tmatrix[4,2] - DS * ay; 21 158 pz:= tmatrix[4,3] - D6 * az; 159 21 160 pxsq:= px * px; 21 161 pysq:= py * py; 21 162 pzsq: pz * pz; 163 21 164 k3:= pxsq + pysq;!k3 = px*px + py*py 21 165 kl:= 3 - d2sq; Ikl = px*px + py*py - D2*D2 21 168 if (kl < 0) 21 167 then begin 22 168 kl:= -kl; 22 169 if ki > epsilon 23 170 then begin; 23 171 wrlteln('Warning: Invalid Position (kl)'); = 23 172 config[4]:= 1; 22 173 end; 21 174 end; 21 175 k2:= sqrt(kl);!k2 = sqrt(px*px + py*py - D2*D2) 21 176 rsq:= kl + pzsq;!rsq = px*px + py*py + pz*pz - D2*D2 21 177 if (rsq < O) 21 178 then begin 22 179 rsq:= -rsq; 22 180 if rsq > epsilon 23 181 then begin; 23 182 wrlteln('Warnlng: Invalid Position (rsq)'); = 23 183 config(4]:= 1; 22 184 end; 21 185 end; 21 186 r:= sqrt(rsq); Ir sqrt(px*px + py*py + pz*pz - D2*D2)

PUMA robot routines Page 5 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 187 21 188 arm:= (config[O] = -1);!Set ARM flag 21 189 armbelow:= (arm) xor (config[i] = -1);!Set ARM * BELOW 190 191 { 192 find theta sub 1 193 } 21 194 ti:= py*k2; 21 195 t2:= px*k2; 21 196 if not arm 21 197 then begin 22 198 tl:= -tl; 22 199 t2: -t2; 21 200 end; 21 201 si:= tl - px*d2; 21 202 cl: t2 + py*d2; = 21 203 itheta([]:= a2srqq(sl,cl); 21 204 if abs(k3) < epsilon 22 205 then begin; Idivision by zero (invalid position) 22 206 s1:= 0; 22 207 ci: 0; 22 208 end 22 209 else begin; 22 210 sI:= s / k3; 22 211 cl: cl / k3; 21 212 end; 21 213 if abs(sl*sl + cl*cl - 1.0) > epsilon 21 214 then writeln('Warning: Illegal Position (1) '); 2156 216 find theta sub 2 217 } 21 218 if abs(r) < epsilon 21 219 then ti:= -1.0 Idivision by zero (invalid position) 22 220 else begin; 22 221 sal:-pz / r; Isine alpha 22 222 cal:= k2 / r; Icosine alpha 22 223 if not arm 22 224 then cal:= -cal; 22 225 cbt:= (A2 D4 A3 + rsq) / (f2 a2 * r);!cosine beta 22 226 tl:= 1.0 c5t*cbt; 21 227 end; 21 228 if (ti < 0) 21 229 then begin 22 230 tl:= -tl; 22 231 if ti > epsilon 23 232 then begin; 23 233 writeln('Warning: Invalid Position (2)'); = 23 234 config[4] 1; 22 235 end; 21 236 end; 21 237 sbt:= sqrt(ti); sline beta 21 238 t2:= cal * sbt;

PUMA robot routines Page 6 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 21 240 if arm below 21 241 theii begin 22 242 t2: -t2; 22 243 t3 -t3; 21 244 end; 21 245 s2: sal*cbt + t2; 21 246 c2 cal*cbt - t3; = 21 247 ithetat2]: a2srqq(s2, c2); 21 248 if abs(s2*s2 + c2*c2 - 1.0) > epsilon 22 249 then begin; 22 250 writeln('Warning: Invalid Position (2)'); = 22 251 config(4] 1; 21 252 end; 253 254 255 find theta sub 3 265 } 21 257 13:= CA2 12 - rsq) / f2_A2_11; Icosine phi 21 258 'tI 1.0 — 13*13; 21 259 if (ti < 0) 21 260 then begin 22 261 ti: -tl; 22 262 if ti > epsilon 23 263 then begin; 23 264 writeln('Warning: Invalid Position C3)'); = 23 265 configE4] 1; 22 266 end; 21 267 end; 21 268 14:= sqrt(tl); Isine phi 21 269 if arm below 21 270 theli 14: -14; 21 271 s3: 14*16 - 13*16; 21 272 c3 13*16 + 14*16; = 21 273 itheta[3J: a2srqq s3. c3); 21 274 if abs(s3*s3 + c3*c3 - 1.0) > epsilon 22 275 then begin; 22 276 writelnC'Warning: Invalid Position (3)'); = 22 277 config(4J 1; 21 278 end; 279 280 281 Now for the wrist solution 282 } 21 283 tl:= itheta[21 + itheta[3J; 21 284 while tl < 0.0 do ti:= ti + twoPI;!Needed by PC sinC) and coso) fcns. 21 285 s23 sin~tl); 21 286 c23: cos(tl); 21 287 if abs(s23*s23 + c23*c23 - 1.0) > epsilon 22 288 then begin; 22 289 writeln('Warning: Illegal Position (23)'); = 22 290 config[41 1; 21 291 end; 21 292 t2:= sl * c23;

PUMA robot routines Page 7 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 21 293 t3:= cl * c23; 21 294 omega:= 0.0; 21 295 z3ax:= sl*s23*az - c23*ay;!z3 x a components 21 296 z3ay:= c23*ax - cl*s23*az; 21 297 z3az:= cl*s23*ay - sl*s23*ax; 21 298 if ((z3ax <> 0.0 or (z3ay <> 0.0) or (z3az <> 0.0)) 22 299 then begin; 22 300 omega:= sx*z3ax + sy*z3ay + sz*z3az; Is * (3 x a) 22 301 if abs(omega) < epsilon) 22 302 then omega:= nx*z3ax + ny*z3ay + nz*z3az; In * (z3 x a) 21 303 end; 304 21 305 k:= (config2] = -1) xor ( omega < 0.0);!WRIST * sign(omega) 306 307 21 308 if (not k) and (config(3] = 1) {Necessary to flip wrist!!! 22 309 then begin; = 22 310 config[2]: - config[2]; 22 311 k:= not k; 21 312 end; 313 314 ( 315 find theta sub 4 316 ) 21 317 s4:= cl*ay - sl*ax; 21 318 c4:= t3*ax + t2*ay - s23*az; 21 319 if k 22 320 then begin; 22 321 s4:= -s4; 22 322 c4: -c4; 21 323 end; 21 324 if (abs(s4) < epsilon) and (abs(c4) < epsilon)!Degenerate case = 21 325 then itheta[4]:= theta[4] * pi 180 Itheta 4 already alligned, use current value = 21 326 else itheta[41] a2srqq(s4,c4)Y 327 21 328 tI:= itheta[4]; 21 329 while tl < 0.0 do tl:= tl + twoPI;!Needed by PC sin() and coso) fcns. 21 330 s4:= sin(ti); 21 331 c4:= cos(tl); 332 333 334 find theta sub 5 335 } 21 336 s5 = ax*(t3*c4 - sl*s4) + ay*(t2*c4 + cl*s4) - az*c4*s23; 21 337 c5: ax*cl*s23 + ay*sl*s23 + az*c23; = 21 338 itheta[S]: a2srqq(s5, c5); 339 340 341 find theta sub 6 342 } 21 343 t4:= -sl*c4 - t3*s4; 21 344 t3:= cl*c4 - t2*s4;

PUMA robot routines Page 8 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 21 346 s6 t4*nx + t3*ny + t2*nz; 21 347 c6 t4*sx + t3*sy + t2*sz; = 21 348 itheta[6] a2srqq(s6, c6); 349 350 351 21 352 for i:=1 to 6 do = 21 353 itheta[i] itheta[iJ * fiBOpi; 354 21 355 if puma debug or wrd~config[5J) = 1 then 21 356 for i:1 to 6 do begin 22 357 writeln(sercom,'Joint Di:1. has an angle of ',ithetali]:8:3); 21 358 end; 359 10 360 end; Symtab 360 Offset Length Variable - INVERSE - 2 270 Return offset, Frame length - 2 2 I:Integer - 6 4 PX:Real - 18 4 AX:Real - 126 4 R:Real - 204 1 K: Boolean - 22 4 AY:Real - 26 4 AZ:Real - 42 4 NX:Real - 82 4 Cl:Real - 86 4 C2:Real - 90 4 C3:Real - 94 4 C4:Real - 98 4 C5:Real - 102 4 C6:Real - 146 4 12:Real - 170 4 Ki:Real - 174 4 K2:Real - 178 4 K3:Real - 182 4 K4:Real - 150 4 L4:Real - 46 4 NY:Real - 50 4 NZ:Real - 10 4 PY:Real - 14 4 PZ:Real - 30 4 SX:Real - 54 4 S1:Real - 68 4 S2:Real - 62 4 53:Real - 66 4 S4:Real - 70 4 S5:Real - 74 4 56:Real - 34 4 SY:Real - 38 4 SZ:Real - 78 4 S23:Real - 106 4 C23:Real

PUMA robot routines Page 9 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 - 186 4 T1:Real - 190 4 T2:Real - 194 4 T3:Real - 198 4 T4:Real - 200 1 ARM:Boolean - 122 4 RSQ:Real - 134 4 CAL:Real - 142 4 CBT:Real - 110 4 PXSQ:Real - 130 4 SAL:Real - 138 4 SBT:Real - 114 4 PYSQ:Real - 118 4 PZSQ:Real - 154 4 OMEGA:Real - 158 4 Z3AX:Real - 162 4 Z3AY:Real 166 4 Z3AZ:Real 202 1 ARM BELOW:Boolean INVERSE

ruIA LroDooT routines Page 10 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 361 {$PAGE+} 10 362 procedure homotran; 363 364 365 PROCEDURE HOMOTRAN; 366 367 Purpose: Finds the homogeneous transformation matrix 368 specifying the current position of the end-effector 369 of the robot from the XYZOAT description of the 370 robot location. 371 372 Calling convention: 373 homotran; 374 375 Global variables: 376 xyz xyzoat configuration of robot 377 378 tmatrix returns the homogeneous tranformation of the 379 current position of the end effector of the 380 robot arm. 381 382 383 384 20 385 var 20 386 0, IOAT angles in radians 20 387 a, 20 388 tD 20 389 cosa, 20 390 sinO, 20 391 cosA, 20 392 sinA, 20 393 cosT, 20 394 sinT: real; 395 20 396 begin 21 397 o xyz 4J; 21 398 a xyzESJ; 21 399 t xyz[6J; 400 21 401 cosa: dcos~o); 21 402 cosA: dcos~a); 21 403 cosT: dcos~t); 21 404 sinO: dsin~o); 21 405 sinA:= dsin~a); 21 406 sinT: dsin~t); 407 = 21 408 tmatrixl,1J:1 (cosO * sinT) - (sinO * sinA * cosT); - 21 409 tmatrix[1,2J (cosO * sinA * cosT) + (sinO * sinT); - 21 410 tmatrix(1,3]: - CcosA * cosT); = 21 411 tmatrix[2,1]: (sinO * sinA * sinT) + (cosO * cosT); = 21 412 tmatrix[2,2J:= - (cosO * sinA * sinT) + (sinO * cosT); - 21 413 tmatrix(2,3]:= cosA * sinT; FlnMnTRi AM

PUMA robot routines Page 11 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11. 05/83 - 21 414 tmatrix[3,1] sinO * cosA; - 21 415 tmatrix[3,2]: - (cosO * cosA); = 21 416 tmatrixl3,3]: - sinA; - 21 417 tmatrix[4,1] xyzll]; - 21 418 tmatrixl4,21 xyz[2]; - 21 419 tmatrix(4,3] xyz[3]; 420 21 421 if homo debug or wrd~configl61) = i 21 422 then begin 22 423 writeln~sercom,'tmatrix:'); 22 424 pr mat(tmatrix); 21 425 enU; 426 10 427 end; Symtab 427 Offset Length Variable - HOMOTRAN - 2 42 Return offset, Frame length - 4 4 0:Real - 8 4 A:Real - 12 4 T:Real - 16 4 COSO:Real - 24 4 COSA:Real - 20 4 SINO:Real - 28 4 SINA:Real - 32 4 COST:Real - 36 4 SINT:Real HOMOTRAN

ruuivuuI VUV xvuu~rnes Page 12 Last change: 4-3-84 rmJ 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.119 05/83 428 {$PAGE+} 10 429 function jointcheck; 430 431 432 FUNCTION JOINT CHECK: boolean; 433 434 Purpose: Determines if all of the joint angles given are 435 within the PUMA's tolerable limits. It returns true 436 if everything is okay. It returns false if any of the 437 angles are bad. When no errrors are found the 438 position of arm is saved. 439 440 Calling convention: 441 good: joint-check; 442 443 Global Variables: 444 xyz Contains the proposed xyzoat position of 445 the puma arm. 446 tmatrix The homogenous transformation matrix 447 describing the proposed arm position. 448 itheta Contains the inverse solution of the arm 449 Cie. each of the six joint angles). 450 rob xyz Returns last valid xyzoat (robot coords) 451 position of the puma arm. 452 theta Returns last valid inverse solutions of the 453 arm Cie. each of the six Joint angles). 454 455 456 } 457 20 458 var 20 459 i, lincrementor 20 460 error: integer; terror flag 461 20 462 begin 463 21 464 error: 0; 465 21 466 for i:=I to 6 do 21 467 begin 468 469 (Place into range of -360.0 to 360.0) 22 470 if (itheta(iJ > max degree[i)) 22 471 then while itheti[i] > max degree~i] do = 22 472 itheta(iJ itheta[iT - 360.0 473 22 474 else while Citheta~iJ < min~degree~iJ) do = 22 475 itheta~i] itheta[i] + 360.0; 476' 477 (outside legal joint limits) 22 478 if ( CithetasiJ<mindegree~iJ) or C ithetai]>max~degree~i1) ) 22 479 then begin 23 480 if ithetaliJ < 0 'choose closest limit tflTnT CrFCr

PUMA robot routines Page 13 Last change: 4-3-84 rmJ 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 24 481 then begin; 24 482 if abs(itheta[i]-min degree(i) > abs~itheta[i]+360.0-nax_degree~ i]) = 24 483 then itheta~il:=itheta[i] + 360.0; 24 484 end 485 23 486 else if abs(itheta[i]-360.0-min degree[i]) < abs~itheta[i]-max~degree~i]) -23 487 then itheta[i ithetaniJ - 360.0; 488 23 489 writelnC'Joint ',i:1,' at ",itheta[i]:8:3, 23 490 degrees out of '. min degree[iJ:8:3, 23 491 * to ',max degree~i]:8:32 range'); 492 23 493 error:= 1; 494 22 495 end; 496 22 497 if itheta~i] > 180.0 = 22 498 then ithetafiJ:= itheta(i] - 360.0 22 499 else if ithetali] < -180.0 = 22 500 then itheta[iJ: itheta~i] + 360.0; 501 21 502 end; 503 21 504 error error + config[4]; = 21 506 config[4J: error; 506 21 507 if error = 0 21 508 then begin lif no errors = 22 509 rob xyz[1] tmatrix[4,1];!robot coords = 22 510 rob xyz[21 tmatrixE4,2]; = 22 511 rob xyz[3] tmatrixE4,3]; - 22 512 rob xyz 4J xyz[4]; - 22 513 rob xyz[5J xyz(5J; = 22 514 rob xyz6]: xyz[6J; 22 515 for-i:= I to 6 do 'save valid joint angles = 22 516 theta~i] ithetafiJ; 21 517 end; 518 = 21 519 Joint-check:= (error = 0); 520 21 521 return; 10 522 end; Symtab 522 Offset Length Variable - JOINT CHECK - 2 8 Return offset, FFame length - 2 1 (function return):Boolean - 4 2 I:Integer - 6 2 ERROR:Integer JOINTCHECK

PUMA robot routines Page 14 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 523 {$PAGE+} 10 524 procedure robotconfig; 525 626 527 PROCEDURE ROBOT CONFIG; 628 629 Purpose: Displays the configuration of current attempted robot 530 location. This routines expects that the current i 531 valid Joint angle solution is in the array ITHETA. 532 633 634 535 20 536 var 20 537 T2, ITransformations from links back to robot ref 20 538 T3, 20 539 T4, 20 540 TS, 20 541 T6: matrix; 20 542 darm, 20 643 delbow, 20 544 dwrist, 20 546 t23: real; 646 20 547 begin {ROBOTCONFIG} 648 549 650 First compute the necessary transformations 551 } 21 6652 tmats (T2,T3,T4,T6,T6); 653 664 21 556 t23:= itheta[2]+itheta[3]; 21 6656 darm: -D4 * dsin(t23) - A3 * dcos(t23) - A2 * dcos(itheta[2]); 21 657 if darm < -epsilon 21 6558 then write('Left, ') 21 659 else write('Right, '); 660 21 661 delbow:= darm * (-A3*dsin(itheta[3]) + D4*dcos(itheta[3])); 21 562 if (delbow < -epsilon) 21 663 then write(' elow, ') 21 564 else write('Above, '); 566 21 66B dwrist:= t4[3,1]*t6[2,1] + t4[3,2]*t6[2,2] + t4[3,3]*t61 2,3];!S Z4 21 667 if abs(dwrist) < epsilon 21 668 then dwrist:= t4[3,1]*t6[1,11 + t4[3,2]*t6[1,2] + t4[3,3]*t6[1,3];!N * Z4 21 669 if dwrist < -epsilon 21 670 then write('Up, ') 21 671 else write('Down, '); 21 572 if itheta[5] < -epsilon 21 573 then writeln(' lip ' ) 21 574 else writeln('Nof lp '); 575

PUMA robot routines Page 15 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 576 10 577 end; Symtab 677 Offset Length Variable - ROBOT CONFIG - 2 350 Return offset, Frame length - 64 64 T2:Array - 128 64 T3:Array - 192 64 T4:Array - 256 64 T5:Array - 320 64 T6:Array - 324 4 DARM:Real - 336 4 T23:Real - 328 4 DELBOW:Real - 332 4 DWRIST:Real ROBOT CONFIG

PUMA robot routines Page 16 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 578 {$PAGE+} 10 579 procedure tmats; 580 581 582 PROCEDURE TMATS~var T2, T3, T4, T5, T6: matrix); 583 584 Purpose: Finds the transformations from link i coordinate 685 system back to robot reference (base) coordinate 586 system. Note: uses the transformation notation 587 x' = x A (graphics) 588 instead of usual 589 x' = A x (robotics). 590 The result is that matrix A (graphics) is transpose 591 of matrix A (robotics). 592 593 594 20 595 var 20 596 5, Itemporary holds sine and cosine values 20 597 c: real; 598 20 599 begin 600 21 601 s:= dsin(itheta(1]); Ilink 1 to link 0 21 602 c dcos(itheta[i]); - 21 603 A1O[1.1]: c; - 21 604 A1O[1,2] s; - 21 605 A1(3,1]:= -s; - 21 606 A1O[3,2]: c; 607 21 608 s: dsin(itheta(2]); Ilink 2 to link 1 21 609 c dcos(itheta[2J); - 21 610 A21[1,1]: c; - 21 611 A21(1,2]:= - 21 612 A21[2,1]: -s; - 21 613 A21[2,2]: c; - 21 614 A21[4,1J: A2 * c; - 21 615 A21[4,2] A2 * s; 616 21 617 s:= dsin(itheta(3]); Ilink 3 to link 2 21 618 c dcos(itheta[3J); = 21 619 A32E1,1J c; - 21 620 A32[1,2]:s - 21 621 A32E3,1] s; - 21 622 A32[3,2]: -c; - 21 623 A32E4,1]:= A3 * c; - 21 624 A32[4,2] A3 * s; 625 21 626 s:= dsin(itheta(4]); Ilink 4 to link 3 21 627 c dcos(ithetat4]); - 21 628 A43[1,1J:= c; - 21 629 A43E1,2]; - 21 630 A43[3,1J:-s

PUMA robot routines Page 17 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 = 21 631 A43(3,21] c; 632 21 633 s:= dsin(itheta[5]);!link 5 to link 4 21 634 c = dcos(itheta[5]); = 21 635 A54[11] = c; = 21 636 A54[1,2]:= s; = 21 637 A54[3,1] s; = 21 638 A54[3,2]:' -c; 639 21 640 s:= dsin(itheta[6]); Ilink 6 to 5 21 641 c:= dcos(itheta[6]); = 21 642 A65[1,1]:= c; = 21 643 A65[1,2] s; = 21 644 A65[2,1]:= -s; = 21 645 A65[2,2]:= c; 646 647 ( 648 Finally, find transformations from link i back to world. Note: TO 649 is transformation from 'robot world' to 'real world'. 650 } 21 651 T2:= mat mult(A21,AlO); 21 662 T3:= mat mult(A32,T2); 21 653 T4:= mat-mult(A43,T3); 21 654 TB:= mat-mult(AS4,T4); 21 655 T6:= mat-mult(A6b,TS); 656 657 ($IF tmats debug $THEN} 658 wrtteln sercom,'A21'); 669 pr mat(A21); 660 wrTteln(sercom,'A32'); 661 pr mat(A32); 662 writeln(sercom,'A43'); 663 pr mat(A43); 664 writeln(sercom, 'A64'); 666 pr mat(A64); 666 wrTteln(sercom,'A66'); 667 pr mat(A65); 668 wrtteln(sercom, 'TO'); 669 pr mat(TO); 670 wrlteln(sercom,'TI or AIO'); 671 pr mat(A1O); 672 wrTteln(sercom,'T2'); 673 pr mat(T2); 674 wrlteln(sercom,'T3'); 676 pr mat(T3); 676 wrTteln(sercom,'T4'); 677 pr mat(T4); 678 wrTteln(sercom,'TS'); 679 pr mat(TS); 680 wrlteln(sercom,'T6'); 681 prmat(T6); 682 {S$END}

PUMA robot routines Page 18 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 10 684 end; Symtab 684 Offset Length Variable - TMATS - 12 74 Return offset, Frame length + i4 2 T2:Array VarP 4 4 S:Real 8 4 C:Real + 12 2 T3:Array VarP + 10 2 T4:Array VarP + 8 2 TS:Array VarP + 6 2 T6:Array VarP TMATS

PUMA robot routines Page 19 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 685 {$PAGE+} 10 686 procedure mnitrobot; 687 688 689 PROCEDURE INIT ROBOT; 690 691 Purpose: Initializes most of the robot specific data 692 strucutures. This procedure loads the data for the 693 robot figure from the file called *PUMA.DAT' into the 694 grobotu object data structure, However, it should be 695 noted that this object is really a collection of 696 seven objects, one for each link. 697 698 699 20 700 var 20 701 invertible: boolean; 702 703 20 704 begin {ROBOTINIT} 705 = 21 708 tmatrix:= identity matrix; Irobot to world transform matrix 707 708 709 Specify the robot to world transformation. By the correct rotation 710 transformation, table mounts, ceiling mounts and side mounts can be 711 handled as well as translation between the coordinate systems. 712 } = 21 713 TO:= identity matrix; IRobot base to shoulder = 21 714 TO[4,3J:= 669.1; = 21 715 TOI:= invert matrix(TO,invertible); 716 = 21 717 H identity matrix; lRobot to tool transformation (null) = 21 718 HI: identity matrix; 719 720 721 Set the joint limits 722 = 21 723 max degree 1] 160.0; = 21 724 max degreeE2] 45.0; = 21 725 max degreel3J 225.0; = 21 726 max degreeE4] 170.0; = 21 727 max degreeE5J 100.0; = 21 728 max-degree[6] 266.0; 729 = 21 730 min degree(1J -160.0; = 21 731 min degreeE2: -225.0; = 21 732 min degree[3] -45.0; = 21 733 min-degree[41 -110.0; = 21 734 min degree[5 -100.0; = 21 735 min-degree[6J -266.0; 736 7Q7

rurm rouub routines rage 20 Last change: 4-3-84 rmj 04-04-84 12':36':45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 = 21 738 A10:= identity matrix; = 21 739 A10[2,2]:= 0.07 = 21 740 A1012,3]:= -1.0; = 21 741 A10[3,3]:= 0.0; 742 = 21 743 A21:= identity matrix; = 21 744 A21[4,3]: D2; 745 = 21 746 A32:= A10;!Most similar to AlO = 21 747 A32[2,3]:= 1.0; 748 = 21 749 A43:= A10; IMost similar to AlO = 21 750 A43[4,3]:= D4; 751 = 21 752 A54:= A32;!Most similar to A32 753 = 21 754 A65 = A21; IMost similar to A21 = 21 755 A65[4,3]:= D6; 756 757 10 758 end; {ROBOT INIT} Symtab 758 Offset Length Variable - INIT ROBOT - 2 68 Return offset, Frame length - 2 1 INVERTIBLE:Boolean PUMA

PUMA robot routines Page 21 Last change: 4-3-84 rmj 04-04-84 12:36:45 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 759 {$PAGE+} 00 760 end. Symtab 760 Offset Length Variable 0 570 Return offset, Frame length 4 24 XYZ:Array Static Public 0 64 TO:Array Static Extern 0 64 H:Array Static Extern 0 64 HI:Array Static Extern 0 64 TOI:Array Static Extern 186 64 A10:Array Static 250 64 A21:Array Static 314 64 A32:Array Static 378 64 A43:Array Static 442 64 A54:Array Static 506 64 A65:Array Static 0 24 THETA:Array Static Extern 92 24 ITHETA:Array Static Public 0 14 CONFIG:Array Static Extern 0 636 SERCOM:File Static Extern 28 64 TMATRIX:Array Static Public 0 24 ROB XYZ:Array Static Extern 0 8 VERSION:Array Static Extern 0 2 FIRST STR:Pointer Static Extern 0 1 MENU FLAG:Boolean Static Extern 132 24 MAX DEGREE:Array Static 166 24 MIN-DEGREE:Array Static 0 2 COORDS TYPE:Integer Static Extern 0 2 MENU CURSOR:Integer Static Extern 180 6 MODEE VERTS:Pointer Static Errors Warns In Pass One 0 0

04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 00 1 {$linesize:i32,$pagesize:60} 2 {$title:'Global include file (global.inc)'.$subtitle:' '} 3 {$debug-,$list+) 4 6 7 8 University of Michigan 9 College of Engineering 10 Center for Robotics and Integrated Manufacturing 11 Robot Systems Division 12 2510 East Engineering Building 13 Ann Arbor, MI 48109 14 (313) 764-4343 15 16 Copyright 1984, The University of Michigan, All Rights Reserved 17 18 19 Written by: Richard M. Jungclas 20 22 23 0 {$include: 'GLOBAL.INC') I {$LIST+} 2 {$title: 'Global Include file Cglobal.inc) ',$subtitle: 'Last change: 4-3-84'} 3 00 4 interface; 5 { 6 GLOBAL.INC 7 This file contains all declarations of types global to the system. 8 It also includes variables and some matrix manipulation routines 9 used throughout the system. 10 } 11 00 12 unit globalsC 00 13 version, Isystem's version number 14 00 15 r matrix, tGeneric type two-dimensional arrays 00 16 1 matrix, 00 17 Jlmatrix, 00 18 g matrix,!general one dimensional matrix type 00 19 matrix, '4 x 4 homogeneous matrix type 00 20 c matrix.!coordinate matrix type 00 21 rfilat ptr, Itwo super array pointer types 00 22 imat-ptr, 23 00 24 SUCCESS,!return code from serial I/O routines 00 25 NO SUCCESS,!return code from serial I/O routines 00 26 SPECIAL,!return code meaning special key 27 00 28 world type,!world cartesian coordinates type 00 29 robottype,!robot cartesian coordinates type

Global Include file (global.inc) Page 2 Last change: 4-3-84 04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 00 30 jointtype,!Joint coordinates type 31 00 32 filel,!A text file type 00 33 consol input lstr,!lstring type for consol input. 00 34 name 1str,!lstring type for names 00 35 file-lstr,!lstring standard file name type. 36 00 37 STRS,!Symbol Table Record structure 00 38 STR ptr, 'pointer type to symbol table record 00 39 firRtSTR,!Pointer to symbol list 40 00 41 menu cursor,!Menu item cursor 00 42 menu-flag,!Flag to redraw menu 43 00 44 config,!Robot configuration status 00 45 coords type, 'Type of robot coordinates 00 46 TO,!Robot base to world transformation 00 47 TOI,!World to robot base transformation 00 48 H,!Robot to tool transformation 00 49 HI,!Tool to robot transformation 00 60 rob_xyz,!generalized coords Cxyzoat) for robot 51 52 t(Null tool to robot reference) 00 53 theta,!Robot joint angles 54 00 55 find STR,!function to find symbol table entry 00 56 rotateematrix,!function to generate rotation matrix 00 57 dsin, 'sine functiegrees) 00 58 dcos,!cosine function (in degrees) 00 59 mat mult,!matrix manipulation routines 00 60 ideftity matrix, 00 61 invert matrix, 00 62 get xyzoat, 00 63 getc, 00 64 trim); 65 00 66 type 10 67 consol input lstr lstringC8o); 10 68 name l9tr = Tstring(12); 10 69 file-lstr lstringC63); 10 70 fileT = text, 71 10 72 r matrix = super array [1..*,1. of real; 10 73 i-matrix = super array [1..*,1..*J of integer; 10 74 g-matrix = super array [...*] of real; 76 10 76 c matrix = gmatrix(3); 10 77 matrix = r matrix(4,4); 10 78 j-matrix = g-hatrix(6); 79 10 so rmat ptr @r matrix; 10 81 imat-ptr = @i-matrix;

jiuuizi iuuiuuu ilie kgiooai.1fc) Page 3 Last change: 4-3-84 04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 10 83 STRptr = @STRS; 84 85 86 87 Symbol Table Record 88 89 symname: The symbolic name string 90 data: The coordinates of the location 91 ctype: The coordinates type 92 used: A boolean flag which (if true) indicates that the 93 location has been used by the programmer. 94 nextSTR: A link to the next entry in the symbol table (or nil) 95 96 10 97 STRS = record 20 98 symname: name lstr; 20 99 data: matrix; 20 100 ctype: ilteger; 20 101 used: boolean; 20 102 nextSTR: STR_ptr; 10 103 end; 104 105 106 10 107 const 10 108 SUCCESS = 0; (return code from serial I/O routines } 10 109 NO SUCCESS = (; ~ return code from serial I/O routines } 10 110 SPECIAL = 2; (return code meaning special key } 10 111 world type = 0; (type for world cartesian coordinates} 10 112 robot-type 1; (type for robot cartesian coordinates} 10 113 joint-type = 2; (type for joint coordinates) 10 114 var 10 115 version: string(7); (system version number} 118 10 117 first STR: STR_ptr; 118 10 119 menuflag: boolean; 120 10 121 coordstype, (type of robot coordinates 122 0 = world cartesian 123 1 = robot shoulder cartesian 124 2 = joint } 125 10 126 menu cursor: integer; 10 127 config: array[0..6] of integer; 128 (configuration of the robot arm 129 [0] -1=lefty, 1lrighty 130 (1] -lbelow, lzabove 131 [2] -1=up, ldown 132 [3] -l noflip, l flip 133 [4] 1=error, O~valid solution 134 [5] 1=debug, O production 135 616 (not used)

Global Include file (global.inc) Page 4 Last change: 4-3-84 04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11. 05/83 136 10 137 rob xyz, 10 138 theta: J~matrix; 10 139 TO, 10 140 TOI, 10 141 H, 10 142 HI: matrix; 143 144 20 145 function find STR~const name: lstring): STR ptr [pure]; 20 146 function rotate matrix~const x,y,z: real): lilatrix [pure]; 20 147 function dsin(ccnst x: real): real [pure]; 20 148 function dcos const x: real): real [pure]; 20 149 function mat mult~const ml,m2: matrix): matrix [pure]; 20 150 function ideftity matrix: matrix [pure]; 20 151 function invert mitrix~source: matrix; var invertible: boolean): matrix [pure]; 20 152 function get xyioat(const m: matrix): J matrix [pure]; 20 153 function getECvar letter~spec: char): bolean [pure]; 20 154 procedure trim~var s: lstring); 165 10 156 end; 167 {$LIST+} TRIM

Debug Include filel(debug.inc) Page 5 04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 25 {$title:'Debug Include file Cdebug.inc)',$subtitle:' '.$PAGE+} 0 {$include:'DEBUG.INC'} 1 {$LIST+} 2 {$title:'Debug Include file (debug.inc) Last Change: 3-2-84 rmj'} 3 00 4 interface; 6 { 7 DEBUG 8 This interface contains routines to aid in debugging PASCAL programs. 9 } 10 00 11 unit debug~prmat, heap space, writewt, breakpt); 12 10 13 uses globals; 14 15 20 16 procedure pr_matCconst ml: matrix); 10 17 procedure eap space; 20 18 procedure writiwt~const line: consol input lstr); 20 19 procedure breakpt(const line: consol input lstr); 20 21 10 22 end; 23 {$LIST+} BREAKPT

Menu Function Include file (menu.inc) Page 6 04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 27 {$title:'Menu Function Include file (menu.inc)',$subtitle:' ',$PAGE+} 0 {$include:'MENU.INC'} 1 {$LIST+} 2 {$title:'Menu Function include file (menu.inc) Last change: 4-3-84 rmj '} 3 00 4 interface; 5 6 MENU 7 8 This interface contains routines to control the cursor as well as 9 maintain the menu system. 10 } 11 00 12 unit menu functions( 00 13 cIs, 00 14 clear lower, 00 15 clear-upper, 00 16 command prompt, 00 17 data prompt, 00 18 display menu, 00 19 bmenu item, 00 20 gmenu-item, 00 21 menu Item, 00 22 display status, 00 23 highlight, 00 24 position cursor, 00 25 print border); 26 10 27 uses globals; 28 10 29 type 10 30 pitem = lstring(30); 10 31 pkey = lstring(3); 32 10 33 procedure cls; 10 34 procedure clear lower; 10 36 procedure clear upper; 10 38 procedure command prompt; 10 37 procedure data prompt; 20 38 procedure display menu(var full flag: boolean; const level:consol input lstr); 20 39 procedure bmenu item(const item' pltem); - n 20 40 procedure gmenu-item(const key: pkey; const item: pitem); 20 41 procedure menu Ttem(const item: pitem); 10 42 procedure display status; 20 43 procedure highligit(line:consol input lstr); 20 44 procedure position cursor(row,c5lumn:-integer); 10 45 procedure print boFder; 46 10 47 end; ****************************************************************} 48 {$LIST+} PRINT BORDER

rVuZAn flAvuJL IUUL[10UIdS iulUUe lile %puwa.~ncJ Page 7 04-05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 29 {$title:'PUMA Robot Routines Include file (puma.inc)',$subtitle:' ',$PAGE+} 0 {$include: 'PUMA.INC') 1 {$LIST+} 2 ($title:'PUMA Robot Routines Include file Cpuma.inc) Last change: 4-3-84') 3 00 4 Interface; 5 6 PUMA 7 This interface contains PUMA routines 8 9 00 10 unit puma~initrobot, tmats, inverse, homotran, joint-check, robotconfig, xyz, tmatrix, itheta); 11 10 12 uses globals; 13 14 10 15 var 10 16 itheta, {Current joint angles solution for the robot) 10 17 xyz: JImatrix; {Generalized coords (xyzoat) for robot for 18 the last request. Either tool to world or 19 joint 6 to robot reference. Possibly invalid 20 (out of reach) ) 10 21 tmatrix: matrix; {Robot Transformation matrix for the last 22 request. Either tool to world (usually) or 23 joint 6 to robot reference, depending on 24 context. Possibly invalid (out of reach). ) 25 10 26 procedure init robot; 20 27 procedure tmatg(var T2,T3,T4,T5,: matrix); 10 28 procedure inverse; 10 29 procedure homotran; 20 30 function joint check: boolean; 10 31 procedure robot config; 32 33 10 34 end; 36 {$LIST+} DUMMY

Dummy routine to list include files Page 8 04 -05-84 11:05:59 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 31 {$title:'Dummy routine to list include files',$subtitle:',$PAGE+} 10 32 program dummy~input,output); 33 10 34 begin (DUMMY procedure) 35 11 36 writelnC'Dummy procedure'); 37 00 38 end. Symtab 38 Offset Length Variable 0 20 Return offset, Frame length Errors Warns In Pass One 0 0

Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 24 553 if configiS] = 1 and then coords type = world type 24 554 then writeln~sercom, 'World Cartesian coorainates') 24 555 else writeln(sercom, 'Robot Cartesain coordinates'); 24 556 temp coord type:= coordstype; - 24 557 coords type temptype; 24 558 if conligi5] 1 24 559 then writeln~sercom, 'Calling homotran'); 24 560 homotran; 24 561 if config[5] = 1 24 562 then writeln~sercom, 'Calling inverse'); 24 563 inverse; - 24 564 coordstype:= temp coord_type; 23 565 end; 566 23 567 joint type: 23 568 If config(s) =1 23 569 then writeln~sercom, 'Joint coordinates'); 570 22 571 end; 572 22 573 if Joint check 22 574 then Begin - 23 575 move:= true; 23 576 if configE6] = 1 / 23 577 then writeloc~sercom); 23 578 end 23 579 else begin; 23 580 writelnC'Robot can''t reach this locations'); 23 581 leave: false; - 23 582 move: false; 22 583 end; 584 21 585 end; 586 10 587 end; Symtab 587 Offset Length Variable - MOVE 4 140 Return offset, Frame length 2 1 (function return):Boolean + 6 2 TRACEFIL:File VarP - 4 1 COMMAND:Char - 6 1 SPEC:Char - 10 1 FULL:Boolean - 14 4 X:Real - 26 4 0:Real - 30 4 A:Real - 40 2 I:Integer - 34 4 T:Real - 18 4 Y:Real - 22 4 Z:Real - 56 14 NAME:Array - 8 1 LEAVE:Boolean - 42 2 TEMP STR:Pointer MOVE

Main program routines(routines.pas) Page 14 Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler. MS-DOS 8086 Version 3.11, 05/83 - 36 2 TEMP TYPE.Integer - 38 2 TEMP-COORDTYPE:Integer - 138 82 INPUTLINE-:Array MAIN ROUTINES

Last change: 4-3-84 04-04-84 14:16:56 JG IC Line# Microsoft MS-Pascal Compiler, MS-DOS 8086 Version 3.11, 05/83 588 {$PAGE+} 00 589 end. Symtab 589 Offset Length Variable 0 24 Return offset, Frame length 0 8 VERSION:Array Static Extern 0 64 TO:Array Static Extern 0 64 H:Array Static Extern 0 64 HI,Array Static Extern 0 64 TOI:Array Static Extern 0 24 XYZ:Array Static Extern 0 14 CONFIG:Array Static Extern 0 24 THETA:Array Static Extern 0 24 ITHETA:Array Static Extern 0 24 ROB XYZ:Array Static Extern 0 636 SERCOM:File Static Extern 0 64 TMATRIX:Array Static Extern 0 2 FIRST STR:Pointer Static Extern 0 1 MENU FLAG:Boolean Static Extern 0 2 MENU-CURSOR:Integer Static Extern 0 2 C0ORDSTYPE:Integer Static Extern Errors Warns In Pass One 0 0

UNIVERSITY OF MICHIGAN 3 9015 03023 0307