: Software for Control and Dynamic Simulation of UNIMATE PUMA 560 ROBOT,

A Thesis Presented to The Faculty of the College of Engineering and Technology Ohio University

-i .A:~>- I-. '1

.+b. In Partial Fulfillment of the Requirements for the Degree Master of Science

BY Sandeep Anand/- Department of Mechanical Engineering June 1993 Acknowledgements

I take this opportunity to acknowledge the support of my advisor, Dr. Sunil Agrawal, during the course of this research. Working with him has been a delightful, exciting and educating experience for me. A vote of special thanks to the other members on the committee, Dr. Jay Gunasekera and Dr. Larry Snyder for extending their time and perusal on the thesis. I would also like to thank Mr. Ronald B. Buck for his help provided during the course of research. Finally, I would like to deeply thank my family back home for their tremendous support during my stay in the U.S.A.. Table of Contents

Introduction ...... 7 1.1 Background ...... 7 1.2 Literature Review ...... 9 1.3 Motivation of the Present Study ...... 11 1.4 Summary ...... 12

Forward Kinematics ...... 14 2.1 Introduction ...... 14 2.2 Forward Kinematics ...... 14 2.3 Denavit- Hartenberg Representation ...... 17

Inverse Kinematics ...... 21 3.1 Introduction ...... 21 3.2 Solvability ...... 22 3.3 Pieper's Solution ...... 23

Equations of Motion and Dynamics ...... 30 4.1 Introduction ...... 30 4.2 Lagrangian Formulation of Manipulator Dynamics ...... 30 4.3 Equations of Motion ...... 32 4.4 The Real and Approximate Dynamic Models ...... 36 4.5 Dynamic Simulation ...... 37 4.6 Animation ...... 38 4.7 Verification ...... 39 4.8 Results ...... 39 5 Path Planning Schemes ...... 48 5.1 Introduction ...... 48 5.2 Trajectory Interpolation ...... 48 5.3 Joint Space Schemes ...... 49 5.4 Results ...... 51

Conclusions and Future Work ...... 54

References ...... 59

Appendix A ...... 68 A-1 Link Transformations for PUMA 560 ...... 68

Appendix B ...... 71 B-1 Z-Y-Z Euler Angles ...... 71

AppendixC ...... 73 C-1 Jacobians in the Force Domain ...... 73 C-2 The J and J matrices for PUMA 560 ...... 74

Appendix D ...... 79 D-1 Computer Program to Evaluate the Forward Kinematics ...... 79 D-2 Computer Program to Evaluate the Inverse Kinematics Solution ... 81 D-3 Computer Program to Evaluate J and J matrices for PUMA 560 ... 89 D-4 Computer Program to Verify the Equations of Motion and find the computer processor time taken for the Real Model ...... 93 D-5 Computer Program to Verify the Equations of Motion and find the computer processor time taken for the Approximate Model .... 108 D-6 Computer Program to get Joint Motor Torque@) for Controlling the Robot based on the Real Model ...... 122 D-7 Computer Program to get Joint Motor Torque@) for Controlling the Robot based on the Approximate Model ...... D-8 Computer Program to get Forces(s) on the End Effector based on the RealModel ...... D-9 Computer Program to get Forces@)on the End Effector based on the

Approximate Model ...... , ...... D-10 Computer Program for Graphical Simulation of PUMA 560 . . . . D-11 Computer Program for Graphical Simulation of Pieper's Solution . D-12 The main routines for running the PUMA robot with the Trident Robotics and Research, Inc. interface boards...... D-13 The header file containing defined constants for use when using the interface boards from Trident Robotics and Research, Inc. . . . . D-14 An example user-code file for controlling the PUMA robot with the Trident Robotics and Research, Inc. interface boards...... D-15 The main header file for utilizing functions provided for accessing the PUMA robot using the interface boards ...... D-16 File for calibration of the robot using the Trident Robotics cards. .

AppendixE ...... E-1 The Approximate Model ...... I Chapter 1 Introduction

1.1 Background The use of industrial robots became identifiable as unique devices in the 19609s, along with computer aided design (CAD) systems, and computer aided manufacturing (CAM) systems. Presently, the robots characterize the latest trends in automation of the manufacturing process [60]. New disciplines of engineering, such as manufacturing engineering, applications engineering, and knowledge engineering are beginning to deal with the complexity of the field of robotics and the larger area of factory automation. Within a few years, it is possible that robotics engineering will stand on its own as a distinct engineering discipline. A robot can be considered as a computer controlled industrial manipulator, operating under some degree of autonomy. Such devices are extremely complex electro- mechanical systems whose analytic description requires advanced methods which present many challenging and interesting research problems. The official definition of such a robot comes from the Robot Institute of America (RIA): A robot is a reprogrammable multlfinctional manipulator designed to move material, parts, tools, or specialized devices through variable programmed motions for the peformance of a variety of tasks. The key element in this definition is the reprogrammability of robots. It is the computer brain that gives the robot its utility and adaptability. The so-called robotics revolution is, in fact, part of the larger computer revolution. There are many other applications of robots in areas where the use of humans is impractical or undesirable. Among these are undersea and planetary exploration, satellite retrieval and repair, the defusing of explosive devices, and work in radioactive environments. Finally, prostheses, such as artificial limbs, are themselves robotic INTRODUCTION 8 devices requiring methods of analysis and design similar to those of industrial manipulators [19,25]. This thesis will focus mainly on the kinematics, dynamics and control of Unimation PUMA 560 robot. Figure 1.1 shows the PUMA 560, and Figure 1.2 shows its degrees of joint rotation [68]. On the basis of its geometry, the PUMA 560 robot is a six revolute jointed articulated manipulator [52]. IN TROD UCTZON

WAIST 320'

SHOULDER 250

ELBOW 270'

WRIST ROTATION 300'

WRIST BEND 200 '

FLANGE 532'

Figure 1.2 Degrees of Joint Rotation (Adapted from r681).

1.2 Literature Review Se-Young Oh and Moon-Jeung Joe have developed a dynamic controller for a six degree of freedom manipulator (tested and simulated on PUMA 560) using backpropagation neural networks [5 11. H. Chuang has examined model uncertainty and effects on compliant motion in design and simulation of the PUMA robot arm [15]. By adding a roll joint to the shoulder of the PUMA geometry, redundancy resolution has been studied to accomplish singularity avoidance in this human arm like manipulator [13]. The PUMA 560 robot manipulator control hardware structures have been sufficiently modified to facilitate manipulator motion control research [26]. A motion planner has been implemented that plans collision free motions for a PUMA 560 manipulator arm among stationary obstacles [29]. A computer simulation using a PUMA INTRODUCTION 10

560 manipulator demonstrates generation of optimal time trajectory for two cooperating robots [14]. A stochastic optimization problem has been formulated to obtain the optimal production speed of robots based on measured geometric and non-geometric errors 1451. Progress in extending "Soar" architecture to tasks that involve interaction with external environments have been reported 1381. For space based robotic applications, a fast computer architecture for the control of robots coupled with closed loop position control and force/torque feedback has been developed [62]. Li and Sankar have presented a scheme for fast inverse dynamic computation in real time robot control [40]. Boudor has developed an exact kinematic model of PUMA 560 [9]. An energy based indirect adaptive controller for robots has also been presented by Serafi and Khalil [24]. The generation of joint space trajectories for robotic manipulators using different curve fitting techniques have been discussed with results applied to PUMA manipulator following circular paths in space [5]. A visual feedback control scheme called image based visual servo has been proposed for manipulators with cameras on their hands (experiments have been validated on PUMA 560) [30]. Dynamic modelling of geared and flexible jointed manipulators (investigated using simulation on PUMA 560) has been presented [47]. The feasibility of applying the principles of direct adaptive control to the industrial trajectory tracking problem was rigorously investigated using PUMA 560 [39]. The trajectory tracking performance of the non-linear feedback controller based on differential geometric control theory was experimentally studied and tested on PUMA 560 arm [66]. A method allowing one to determine the feasibility of displacement trajectories of a class of robot manipulators, namely with a wrist of intersecting axes (with PUMA 560 in particular) has been developed [71]. Singularity redundancy resolution with task priority has been implemented (derivation based on PUMA geometry) using extended Jacobian technique with weighted damped least squares [23]. The discrete time theory that linearizes the nonlinear dynamics of the PUMA 560 robot arm has been systematically developed [27]. A modular hierarchial model targeted for research and development to control robots has also been studied [6]. Li introduced an approach for fast mapping obstacles in the configuration space of the robot to plan collision free paths for industrial robots (Calculation results were proved by graphical simulations of PUMA INTRODUCTION 11

560 arm) [41]. The potential for using robotic techniques in the development of a surgeon robot was investigated in a preliminary feasibility study using a six axis PUMA robot [20]. A systematic procedure was presented to efficiently enumerate all possible singular configurations for robotic manipulators [42]. An active end effector based force control system for robotic deburring was successfully implemented using a PUMA 560 robot [lo].

1.3 Motivation of the Present Study We define an off-line programming (OLP) system as a robot programming language which has been sufficiently extended, generally by means of computer graphics, so that the development of robot programs can take place without access to the robot itself. Off-line programming systems are important both as aids in programming present- day industrial automation as well as platforms for robotics research. Numerous issues must be considered in the design of such systems. In this chapter, a discussion of these issues is presented using the design of PUMA 560. The topics include kinematics, dynamic simulation, path planning of the robot and, spatial representation of solids and their graphical representation. In the development of programming systems for robots, advances in the power of programming techniques seem directly tied to the sophistication of the internal model referenced by the programming language. Early joint space "teach by showing" robot systems employed a limited world model, and there were very limited ways in which the system could aid the programmer in accomplishing a task. Slightly more sophisticated robot controllers included kinematic models so that the system could at least aid the user in moving the joints so as to accomplish Cartesian motions. Robot programming languages (RPLs) evolved which support many different data types and operations which the programmer may use as needed to model attributes of the environment and compute actions of the robot. Some RPLs support world modeling primitives such as affixments, data types for forces and moments, and other features [46]. Although this thesis focuses to some extent on the particular problem of robot INTRODUCTION 12 programming, the notion of an OLP system extends to any programmable device on the factory floor. A common argument raised in their favor is that an OLP system will not tie up production equipment when it needs to be reprogrammed, and hence, automated factories can stay in production mode a greater percentage of the time. They also serve as a natural vehicle to tie computer aided design (CAD) data bases used in the design phase of a product's development to the actual manufacturing of the product. In some applications, this direct use of CAD design data can dramatically reduce the programming time required for the manufacturing machinery. Off-line programming of robots offers other potential benefits which are just beginning to be appreciated by industrial robot users. Some of the problems associated with robot programming are attributed to the fact that an external, physical workcell is manipulated by the robot program. Programming of robots in simulation offers a way of keeping the bulk of the programming work strictly internal to a computer - until the application is nearly complete. Thus, many of the problems peculiar to robot programming tend to diminish.

1.4 Summary Chapter 2 describes the background on representation of coordinate systems and transformation among various coordinate systems. It leads to the forward kinematics problem, which is to determine the position and orientation of the end-effector or tool given the joint variables. Chapter 3 discusses the problem of inverse kinematics, i.e., given the position and orientation of the end-effector, calculate the possible sets of joint angles which attain this given position and orientation. In order to analyze manipulators in motion, the concept of mapping velocities in joint space to velocities in Cartesian space by the Jacobian matrix is presented in Chapter 4. This Chapter also deals with the forces and moments required to cause motion to a manipulator. Here, we develop techniques and algorithms based on Lagrangian dynamics for deriving the equations of motion for PUMA 560. This aspect of developing equations of motion is used to control the motion of the manipulator. To simulate how the manipulator would move under the INTRODUCTION 13 application of a set of actuator torques, the dynamic equations are reformulated so that the acceleration is computed as a function of actuator torques. Chapter 5 is concerned with describing motion of the manipulator in terms of trajectories through space. The conclusions and the scope of future work is presented in Chapter 6. Chapter 2 Forward Kinematics

2.1 Introduction Kinematics is the description of motion without regard to the forces that cause it. It deals with the study of position, velocity, acceleration, and higher derivatives of the position variables. The Unimation PUMA 560 robot is an example of all rotary joint manipulator with 6 dof (i.e., it is a 6R mechanism).

2.2 Forward Kinematics The forward kinematics problem can be stated as follows: Given the joint variables of the robot, determine the position and orientation of the end-effector. Since each joint has a single degree of freedom, the action of each joint can be described by a single number, i.e., the angle of rotation in the case of a revolute joint. The objective of forward kinematic analysis is to determine the cumulative effect of the joint variables. Suppose a robot has n+l links numbered from 0 to n starting from the base of the robot, which is taken as link 0. The joints are numbered from 1 to n, and iiis a unit vector along the axis in space about which the links i-1 and i are connected. The i-th joint variable is denoted by q,. In case of a revolute joint, q, is the angle of rotation, and in the case of a prismatic joint, q, is the joint translation. Next, a coordinate frame is attached rigidly to each link. To be specific, we choose frames 1 through n such that the frame i is rigidly attached to link i. Figure 2.1 illustrates the idea of attaching frames rigidly to links in the case of a PUMA 560 robot. T"' is a homogenous matrix which is defined to transform the coordinates of a point from frame i to frame i-1 [q. The matrix qiilis not constant, but varies as the configuration of the robot is changed. However, the assumption that all joints are either revolute or prismatic means that T,"' FOR WARD KINEMA TICS 15 FOR WARD KINEMATICS is a function of only a single joint variable, namely qi. In other words,

The homogenous matrix that transforms the coordinates of a point from frame i to frame j is denoted by qJ (i > j). Denoting the position and orientation of the end-effector with respect to the inertial or the base frame by a three dimensional vector d,,(' and a 3x3 rotation matrix Rn0, respectively, we define the homogenous matrix

Then the position and orientation of the end-effector in the inertial frame are given by

Each homogenous transformation c-' is of the form

Hence FOR WARD KZNEMA TICS 17

The matrix R/ expresses the orientation of frame i relative to frame j (i > j) and is given by the rotational parts of the TJ-matrices (i > j) as

The vectors d{ (i > j) are given recursively by the formula

2.3 Denavit- Hartenberg Representation A commonly used convention for selecting frames of reference in robotic applications is the Denavit-Hartenberg or D-H convention [21]. In this convention each homogenous transformation ~j-' is represented as a product of "four" basic transformations

where the notation Rot(x, a,,) stands for rotation about iiaxis by (2-1, Trans(x, a,-J is translation along xi axis by a distance a,,, Rot(& BJ stands for r~tationabout iiaxis by B,, and Trans(z, d,) is the translation along iiaxis by a distance di. FORWARD KIMMA TICS

where the four quantities 4, ai, di, ai are the parameters of link i and joint i. Figure 2.2 illustrates the link frames attached so that frame (i) is attached rigidly to link i.

Axis i

L I Figure 2.2 Coordinate Frames Satisfying D-H Assumptions (Adauted from 11811

The various parameters in Eq. (2.10) are given the following names: ai (length), is the distance from iito ii+,measured along xi; ai (twist), is the angle between iiand ii+,measured about i,; di (offset), is the distance from k,, to iimeasured along ii;and Oi (angle), is the angle between ki-, and iimeasured about ii; FORWARD KINEMA TICS 19

In the usual case of a revolute joint, is called the joint variable, and the other three quantities are the fixed link parameters. For the PUMA 560 robot, 18 numbers are required to completely describe the fixed portion of its kinematics. These 18 numbers are in the form of six sets of (a,, ai,di).

Table 2.1 shows the sets in the form of D-H table for PUMA 560:

Table (2.1) Link Parameters of the PUMA 560

Using the above values of the link parameters the individual link transformation matrices c-'are computed. Then, the link transformations can be concatenated (multiplied together) to find the single transformation that relates frame (6) to frame (0):

The transformation given by Eq. (2.11) is a function of all 6 joint variables. From the robot's joint position sensors, the Cartesian position and orientation of the last link may be computed using Eq. (2.11). Appendix A lists the individual link transformations T,? and the final link transformation matrix T: as given by Equations (2.10) and (2.11). FORWARD KINEMA TICS 20

These transformations were derived using the above D-H table for PUMA 560. Appendix D shows the computer program to evaluate the forward kinematics i.e., T: of the PUMA 560. Chapter 3 Inverse Kinematics

3.1 Introduction The Inverse Kinematics problem is contrasted with the forward (direct) kinematics problem as follows: Given a desired position and orientation for the end-effector of the robot, determine a set of joint variables that achieves the desired position and orientation.

Figure 3.1 The Standard Frames (Adapted from 118U

The solution of the problem of finding the required joint angles is achieved by decoupling the inverse kinematics problem into two simpler problems. First, the position of the intersection of the wrist axes (W} is found, and then we find the orientation of the wrist (See Figure 3.1 and Figure 3.2). INVERSE KINEMA TICS 22

I 1 Figure 3.2 Examnle of the assignment of standard frames (Adanted from 11 81).

3.2 Solvability The equations for solving the inverse kinematics of a manipulator are nonlinear. Given the numerical value of the transformation matrix, we find the values 8,, 8,, .. . , 8,. Considering the equations for T: given in Appendix A for the PUMA 560 manipulator, the statement of our problem is: Given the transformation matrix as T:, solve for the six joint angles 8, through 8,. For PUMA 560 with 6 dof, corresponding to equations in Appendix A, we have twelve equations and six unknowns. However, among the nine equations, arising from the rotation part of the transformation matrix T:, only three equations are independent. These added with the three equations from the position part of T: gives six equations with six unknowns. These equations are nonlinear transcendental equations. A manipulator is considered solvable if all the sets of joint variables associated with a given position and orientation can be determined [59]. We restrict our attention to closed form solution methods. In this context "closed INVERSE KINEMA TICS 23 form" means a solution based on analytic expressions or on the solution of a polynomial of degree 4 or less, such that noniterative calculations suffice to arrive at a solution.

3.3 Pieper's Solution Pieper studied manipulators with six degrees of freedom in which three consecutive axes intersect at a point [54,55]. A sufficient condition that a manipulator will have a closed form solution is that three consecutive joint axes intersect at a point. The last three axes i.e., 4, 5, and 6 of PUMA 560 intersect. When the last three axes intersect, the origins of link frames (41, (51, and (6) are located at this point of intersection. This point is given in base coordinates as

Using the fourth column of Eq. (2.10) for i = 4,

where INVERSE KINEMA TICS 24

Using Eq. (2.10) for in Eq. (3.4) yields the following expressions forf; :

fi = a3c3+ d4sa3s3+ a2

f2 = a3ca2s3- d4sa3ca2c3- d4sa2ca3 -d3sa2, (3.5) f3 = a3sag3-d4sa9a2c3 +d,ca2ca3 +d,ca2.

Using Eq. (2.10) for and in Eq. (3.3) we obtain

where

We now write an expression for the magnitude squared of OP,,,,, which is seen from Eq. (3.6) to be

2 2 ' = g: +g2 +g3 or, using Eq. (3.7) for the g,, we have INVERSE KIZVEMA TICS 25

r = f:+$+&+a:+&+2df,+2ul(cfl-s&). (3.9)

We now write this equation, along with the z component equation from Eq. (3.6), as a system of two equations in the form

f = (k1c2+Q2)h1 +k3,

z = (kls2- k2c2)sal+ k,, where

Eq. (3.10) is useful because dependence on 0, has been eliminated, and dependence on 8, takes a simple form. For our case of PUMA 560, since we have chosen the D-H parameters in such a way that a, is zero (see Table (2. I)), therefore after making the substitution

0 u = tan- 3 ' I

we have r = k, where r is known. The right-hand side (k,) is a function of 8, only. This is an equation of degree 2 in u which may be solved for 8,. INVERSE KINEMA TICS 26

Having solved for 03, we may solve Eq. (3.10) for 8,, and Eq. (3.6) for 8,. To complete our solution, we need to solve for 8,, 8,, and 8,. Since these axes intersect, these joint angles affect the orientation of only the last link. We can compute them based only upon the rotation portion of the specified goal, g. Having obtained 9,, 9,, and 9,, we can compute g,the orientation of link frame (3) relative to the base frame. The desired orientation of (6) differs from this orientation by only the action of the last three joints. Since the problem was specified given g,we can compute

For many manipulators, these last three angles can be solved by using the Z-Y-Z Euler angle solution given in Appendix B applied to g. For any manipulator (with intersecting axes 4, 5, and 6), the last three joint angles can be solved as a set of appropriately defined Euler angles. Since there are always two solutions for these last three joints, the total number of solutions for the manipulator will be twice the number found for the first three joints. As shown in Table (3.1) below, corresponding to two values for 8,, (obtained from quadratic equation (3.10) by using substitution of Eq. (3.12)), we have two values of 8,, the equation for later being quadratic again. Using this chain we have corresponding values for 8,, 8,, 8,, and 8,. Thus, we will have four sets of joint solutions for 9,, O2 ..., 8,.

Table (3.1) Set of Joint Solutions for PUMA 560 INVERSE KINEMA TICS 27

Using the Pieper's solution technique the expressions for respective joint angles for PUMA 560 are as follows:

8, = Atan2(Ye, X,) - AtanZ(g,, g,)

8, = Atan2(d4, a3) t ~tanZ(\la:+ df - KC^, KC)

where X,,Y, and Z, are the end effector position coordinates corresponding to the 4th column of the matrix Third, and "KC" is given by

The elements rij correspond to the R.H.S of Eq. (3.13) If 8, = 0.0 then, 8, = 0.0, 8, = 0.0, and 86 = Atan2[-r12, r11l If 6, = T then,

= 7f 6, = 0.0, and 86 = Atan2[r12, -rill The computer program in Appendix D uses the Pieper's solution approach to evaluate the above expressions for the inverse kinematics solution of PUMA 560. This program is easily verified by checking the match of joint angles (O,, 8,, . . ., 8,) input by forward kinematics and the output obtained through inverse kinematics solution. Using IN VERSE KINEMA TICS the set of solutions obtained above a graphic simulation computer program is developed to show the set of 4 possible configurations of the PUMA 560 (Appendix D). Figure 3.3 shows the 4 solutions i.e., arm up, arm down, elbow left, and elbow right configurations of the PUMA 560 using the graphic simulation. INVERSE KINEMATICS 29 -

Figure 3.3 Four Solutions of the PUMA 560. Chapter 4 Equations of Motion and Dynamics

4.1 Introduction In this chapter we consider the equations of motion for the PUMA 560 i.e., the way in which the manipulator moves due to torques applied by the actuators, or from external forces applied to the manipulator. We begin by deriving the so-called Euler-Lagrange equations. In order to determine these equations in a specific situation, one has to form the Lagrangian of the system. The Lagrangian dynamic formulation is an "energy based" approach to dynamics, using the kinetic and potential energy of the system [65]. There are two problems related to the dynamics of the manipulator that we wish to solve. The first problem requires to find the joint torques, T given a trajectory 8, 8 and 9. This formulation of dynamics is useful for controlling the manipulator [4]. The second problem is to calculate how the mechanism will move under application of joint torques. That is, given a torque vector, T, calculate the resulting motion of the manipulator, 8, i and 9. This is useful for simulating the manipulator.

4.2 Lagrangian Formulation of Manipulator Dynamics We start by developing an expression for the kinetic energy of a manipulator. The kinetic energy of the ith link, 4,can be expressed as

where the first term is the translational kinetic energy, and the second term is the rotational kinetic energy. Vci is the linear velocity of the link's center of mass, hiis the angular velocity of the link, and Ii is the inertia dyadic. The total kinetic energy of the manipulator is the sum of kinetic energy of the individual links i.e.,

Since the VCi and 3, in Eq. (4.1) are the functions of 8 and 8, we see that the kinetic energy of a manipulator can be described by a scalar formula as a function of joint position and velocity, ~(8.8). In fact, the kinetic energy of a manipulator is given by

where M(8) is the nxn symmetric and positive definite manipulator mass matrix. An expression of the form of Eq. (4.3) is known as a quadratic form, since when expanded out, the resulting scalar equation is composed solely of terms whose dependence on the 8, is quadratic 1501. Eq. (4.3) can be seen to be analogous to the familiar expression for the kinetic energy of a point mass,

The potential energy of the ith link, U,,can be expressed as

where g is the 3x1 gravity vector, Pa is the vector locating the center of mass of the ith link. The total potential energy stored in the manipulator is the sum of the potential energy in the individual links i.e.,

Since, the Pa in (4.5) are functions of 8, we see that the potential energy of a manipulator can be described by a scalar formula as a function of joint position, U(8). The Lagrangian dynamic formulation provides a means for deriving the equations EQUATIONS OF MOTION AND DYNAMICS 32 of motion from a scalar function called the Lagrangian, which is defined as the difference between the kinetic and potential energy of a mechanical system. In our notation, the Lagrangian of a manipulator is

4.3 Equations of Motion The equations of motion for the manipulator are then given by

where T is the nxl vector of actuator torques. In the case of a manipulator, this equation becomes

The expanded structure of the terms corresponding to Eq. (4.9) is as follows: Corresponding to first term we have,

and EQUATIONS OF MOTION AND DYNAMICS

Also

1 -ar, = -C-e.e--aM,. . au as, 2, ae, 'j ae,

Thus the Euler-Lagrange equations can be written

In the above Eq. (4.13), there are three types of terms. The first involve the second derivative of the generalized coordinates. The second are quadratic terms in the first derivatives of 8, where the coefficients may depend on 8. These are further classified into two types. Terms involving a product of the type I$are called centrifugal, while those involving a product of the type eiej where i # j are called Coriolis terms. The third type of terms are those involving only 8 but not its derivatives. Clearly the latter arise from differentiating the potential energy. It is common to write Eq. (4.13) in joint space as [57]

The fictitious forces acting on the end-effector, F, could in fact be applied by the actuators at the joints using the relationship

where the Jacobian, J(8), is written in the same frame a F, usually the tool frame, {TI. The Jacobian is a multidimensional form of the derivative. In the field of robotics the Jacobians are time-varying linear transformations which relate joint velocities to Cartesian velocities of the tip of the arm [18,64]. Mathematically, the forward kinematic equations define a function between the space of Cartesian positions and orientations and the space of joint positions. The velocity relationships are then determined by the Jacobian of this function. The Jacobian is a matrix valued function and can be thought EQUATIONS OF MOTION AND DYNAMICS 34 of as the vector version of the ordinary derivative of a scalar function. For example

where 19 is the vector of joint angles of the manipulator, and X is a vector of linear and angular velocities of the end-effector. For any given configuration of the manipulator, joint rates are related to velocity of the tip in a linear fashion. This is only an instantaneous relationship. For the PUMA 560, the Jacobian is 6x6, is 6x1, and X is 6x1. This 6x1 Cartesian velocity vector is the 3x1 linear velocity vector and the 3x1 rotational velocity vector stacked together:

The dimension of the Jacobian is defined such that the number of rows equals the number of degrees of freedom in the Cartesian space being considered. The number of columns in a Jacobian is equal to the number of joints of the manipulator. It may be desirable to express the dynamics of a manipulator with respect to Cartesian variables [35]. This entails the formulation of the dynamic equations which relate the acceleration of the end-effector expressed in Cartesian space to artesian forces and moments acting at the end-effector. The general form of the Cartesian state space equation is given as

where F is a force-torque vector acting on the end-effector of the robot, and X is an appropriate Cartesian vector representing position and orientation of the end-effector [36]. Analogous to the joint space quantities, M,(O) is the Cartesian mass matrix, V,(8,0) is a vector of velocity terms in Cartesian space, and G,(O) is a vector of gravity terms in Cartesian space. We can derive the relationship between the term of Eq. (4.14) and those of Eq. (4.18) in the following way. First, we premultiply Eq. (4.14) by the inverse of the EQUATIONS OF MOTION AND DYNAMICS

Jacobian transpose to obtain

We next develop a relationship between the joint space and Cartesian acceleration, starting with the definition of the Jacobian,

and differentiating to obtain

Solving Eq. (4.22) for joint space acceleration leads to

6 = J-1~-J-1 je. (4.23)

Substituting Eq. (4.23) into Eq. (4.20) we have

F = J-~M(B)J-~x- J-~M(B)J-IJ~+ J-~v(B,~)+ J-~G(o), (4.24) from which we derive the expressions for the terms in the Cartesian dynamics as

M,(O) = J -T(~)M(B) J -I(@,

~'(6,e) = J-~(O)(V(~,~)- ~(e)~-l(e)~(e) e), (4.25)

G,(@ = J -T(e)~(6).

The Jacobian appearing in equations (4.25) is written in the same frame as F and X in Eq. (4.18) (certain choices facilitate computation) though, the choice of this frame EQUATIONS OF MOTION AND DYNAMICS 36 is arbitrary. Appendix C describes the application of jacobians in the force domain. Appendices C and D respectively, describe the analytic expressions and algorithms used to obtain the J and J matrix for PUMA 560.

4.4 The Real and Approximate Dynamic Models The algorithms in Appendix D were based on the "Real" model i.e., the model based by actually deriving the equations of motion analytically using the Lagrangian formulation. The "Approximate" model was a simplified model, abbreviated from the full explicit model [3]. The approximate model was an improved dynamic model, in which a PUMA 560 arm was disassembled; the inertial properties of the individual links were measured; and an explicit model incorporating all of the non-zero measured parameters was derived. The procedure used to derive the approximate model entailed simplification of the Kinetic energy, Centrifugal and the Coriolis matrix elements by combining inertia constants that multiply common variable expressions. Also, three simplification assumptions were made for the analysis: The rigid body assumption; link 6 has been assumed to be symmetric, i.e., I,=],; and only the mass moments of inertia are considered, i.e., I,, I, and I,. All the terms of the Kinetic energy, Centrifugal and the Coriolis matrices were retained, but the terms which were less than 1% of the greatest term within the same equation, or less than 0.1 % as great as the largest constant term applicable to the same joint were eliminated. The link parameters required to calculate the elements of the Kinetic energy, Centrifugal and Coriolis matrices and the gravity vector comprise of: the mass, location of the center of gravity and the terms of the inertia dyadic. The wrist, upper arm, and the shoulder of a PUMA 560 were detached in order to measure these parameters (See Figure 1.1). The mass of each component was determined with a beam balance. The center of gravity was located by balancing each link on a knife edge, once orthogonal to each axis. The diagonal terms of the inertia dyadic were measured with a two wire EQUATIONS OF MOTION AND DYNAMICS 37 suspension. For further details on the approximate model see Appendix E.

The key features of the approximate model were:

(i) Its compactness characterized by fewer calculations. (ii) Requirement of less memory storage. (iii) The approximate model was computationally efficient. When the computer program(s) for the verification of both the models were run, it was found that the approximate model took 1.7030297 seconds of the processor time and, the real model took 1.813 187 seconds of the processor time (Appendix D). Thus, the results generated by the approximate model were about 6.08 % faster than the real model. (iv) Efficient formulation since the results were significantly close to the real model. The approximate model would save run time when the robot is being physically moved. This would be possible since the data from the computer could be produced as fast as possible in order to control the actual movement of the robot.

4.5 Dynamic Simulation As mentioned above, a very important use of dynamic equations of motion is in simulation. By reformulating the dynamic equations so that acceleration is computed as a function of actuator torque, it is possible to simulate how a manipulator would move under application of a set of actuator torques. To simulate the motion of PUMA 560, we make use of the model of dynamics developed above. Given the dynamics written in closed form as in Eq. (4.14), simulation requires solving the dynamic equation for acceleration:

We may then apply any of the several known numerical integration techniques to EQUATIONS OF MOTION AND DYNAMICS 38 integrate the acceleration to compute future positions and velocities 1161. Given initial conditions on the motion of the manipulator, usually in the form:

we numerically integrate Eq. (4.26) forward in time steps of size At. For the purpose of performing numerical integration, in our case the fourth order Runge-Kutta method was used [56,12]. For the purpose of illustration, we introduce a simple integration scheme, called Euler integration, which is accomplished as follows: Starting with t = 0, iteratively compute

where for each iteration, Eq. (4.26) is computed to calculate 8. In this way, the position, velocity, and acceleration of the manipulator caused by a certain input torque function can be computed numerically. The time step At is selected such that it is sufficiently small so that breaking continuous time into small increments is a reasonable approximation. Also, At should be sufficiently large so that excessive computer time is not required to compute a simulation.

4.6 Animation A central element in OLP systems is the use of computer graphics to simulate the robot [58]. This requires that the robot be modelled as a three-dimensional object. Intergraph contains a facility to build 3-D CAD models. These models can be stored as wireframe models or solid models. The 3-D solid model for PUMA 560 was created and animated using Intergraph's Engineering Modeling System (EMS) [32,31]. Each simulated entity such as the robot links are represented by an object. The object data EQUATIONS OF MOTION AND DYNAMICS structure contains the model of the entity, and several other attributes (from Kinematics point of view). Each object can be referenced by a label because the object identification numbers are stored as graphic variables. The individual movement of the robot links is achieved using Intergraph's Parametric Programming Language (PPL) feature [33]. PPL acts as the interface between the results of dynamic simulation and its animation. The program (Appendix D) reads the data file containing the time history of joint angles, 8,,8,,. ..8,, (obtained from integration of Eq. (4.26)) and uses this as input to move the joint arms accordingly.

4.7 Verification The computer programs based on the real and approximate model (Appendix D) were tested. One of the criterion used to test the accuracy of the programs was to assume that the PUMA 560 arm was in the "zero" configuration, as shown in Figure (2.1). The arm is then released and falls freely under gravity without the application of any external motor torques. At each successive time step, the value of kinetic energy, potential energy, total energy and the variation in individual joint angles and joint velocities were found. The analysis of output revealed the following results: Starting from zero position the kinetic energy increased gradually and to compensate for the increase in kinetic energy the potential energy decreased, in accordance with the law of conservation of energy. Thus, the total energy of the system remained constant (Figure 4.5). As expected, maximum variation of joint angle(s) was seen in 8,, and minimum in 8, (Figure 4.1 to Figure 4.4).

4.8 Results The objective of the set of computer programs mentioned above is to determine how the mechanism will move under application of a set of joint torques. That is, given a torque vector, 7, calculate the resulting motion of the manipulator, 8, 9 and 8. This is useful for simulating the manipulator. The output of joint angles from both the models was input to graphical simulation algorithm (Appendix D) and the behavior of the EQUATIONS OF MOTION AND DYNAMICS 40 manipulator was analyzed. Figure 4.6 and Figure 4.7 show plots obtained for some intermediate time steps for the real and approximate models respectively, when the algorithms were tested using the "conservation of energy" approach. The above computer programs are modified to find the required vector of joint torques, r given a trajectory, 9, 8 and 8. This formulation of dynamics is useful for the problem of controlling the manipulator. Various joint space schemes are employed to get the desired output. Chapter 5 gives the details on this aspect of trajectory generation. EQUATIONS OF MOTION AND DYNAMICS 4 1

Real vs. Approximate Models 1s 1 QI Real Model 0 4.8 - -1 -- 01 Appmx. Model

U 4 JI 4 4s- I 4- 1 U- ! ...... MILlOLOIMOIOIa.7 Time in sec.

Real vs. Approximate Models (OD m Real Model m FO - -- m Approx. Model e2 m a 0 m 10

~~1OLOIo*OTO.0.7 Time in sec.

Real vs. Approximate Models 8 U I Real Model LI 4 - U a -. 03 u Approx. Model 2 11 1 QI 0 1-1 IA 1 I 11 1 IY--YI I I .lJ!!!!!!!! wai~ua~or~a~ Time in sec. EQUATIONS OF MOTION AND DYNAMICS 42

Real vs. Approximate Models

Real Model - -- Approx. Model

OD0.1 010101010)0.7 Time in sec.

Real vs. Approximate Models

Real Model - -- Approx. Model

01)a10.2aaOAQIOJa7 Time in sec.

Real vs. Approximate Models -8 OIIlOIa QODPU Real Model QODDIL - 86- - - QQXDI Approx. Model Mma oamrw ammz 0 QOrmz QODODL...... OD~10101010.641~7. Time in sec.

Figure 4.2 Variation of Joint angles (degrees) for 6, - 0, w.r.t Time EQUATIONS OF MOTION AND DYNAMICS 43

Real vs. Approximate Models

Real Model - - - Approx. Model

MQlMQIOAPIMQ7 Time in sec.

Real vs. Approximate Models

Real Model - - - Approx. Model

oJt/!!!!!!! QO 0.q 01 aa OA M oa a7 Time in sec.

Real vs. Approximate Models

Real Model - - - Approx. Model

M~~QIOSMMOJ~~ Time in sec.

I I Figure 4.3 Variation of Joint velocities (8',de'J w.r.1 Time EQUATIONS OF MOTION AND DYNAMICS 44

Real vs. Approximate Models

Real Model - -- Appro~Model

Real vs. Approximate Models ODY om a Real Modal -8 OBl8 - om4 -. omr Apprax. Model 85' OPll OPll -OPY

d- 40#d- WO.lol~0lQlWO7 Time in sac.

Real vs. Approximate Models QmD(p1 onma? -a Real Model -8 - 0- 0- 0-2 -- 86'm Approx. Model oomm, OODDDDl 0- MDDrm A- - Mafwm04Qlaaa7 Time in sec.

Figure 4.4 Variation of Joint Velocities (8'. a 8.r.t Time EQUATIONS OF MOTION AND DYNAMICS 45

Real vs. Approximate Models P 1s (0 Real Model s - 0 4 -- P.E. .lo AFWOz IbfOdd -16 9 .a Q Q 0 4 aoa10101MPIo.w 'IS~~Iin sec.

Real vs. Approximate Models m m Y Real Model m e - 0 -- ICE. r App~.Modal rn I m 11 90 s 0 QDQl 01IWMPIOS0.7 Time in scc.

Real vs. Approximate Models 7.76 7.7 7s Real Model 7a 7.66 - 7X -. T.E.~: Approx. Model 7s 7s 70 72 7.11 7.1 7B 7 oDa1O)uw Time in sec.

Figure 4.5 Variation of Potential, Kinetic and Total Energies w.r.t Time EQUATIONS OF MOTION AND DYNAMICS 46

I I Figure 4.6 Plot showing Intermediate steps when the PUMA 560 fails under gravity (Real Model). EQUATIONS OF MOTION AND DYNAMICS 47 Chapter 5 Path Planning Schemes

5.1 Introduction This chapter concerns with the methods of computing a trajectory in joint space which causes a desired motion of a manipulator. Here, trajectory refers to a time history of desired joint position, velocity, and acceleration for each degree of freedom. This problem includes the human interface problem of how we wish to speczfi a trajectory or path through space. In order to make the description of manipulator motion easy for a user of a robot system, the user should not be required to write down complicated functions of space and time to specify the task. Rather, the specification of the trajectories must be done with simple descriptions of the desired motion. For example, the user may just specify the desired goal position and orientation of the end- effector, and let the system decide on the exact path, its duration, the velocity profile, etc..

5.2 Trajectory Interpolation The simplest type of robot motion is the point to point motion. In this motion the robot is commanded to go from an initial configuration to a final configuration without specifying the intermediate path followed by the end-effector. For robots that are commanded to perform a motion using teach pendants, there is no need for calculating the forward or the inverse kinematics. The desired motion is recorded as a set of joint angles (actually as a set of encoder values) and then the robot can be controlled entirely in joint space. For off-line programming, given the desired initial and final positions and orientations of the end-effector, the inverse kinematic solution must be evaluated to find the required initial and final joint variables. PATH PLANNING SCHEMES 49 5.3 Joint Space Schemes We discuss here the problem of generating smooth trajectories in joint space i.e., we consider methods of path generation in which the path shapes (in space and time) are described in terms of functions of joint angles. Each path point is usually specified in terms of a desired position and orientation of the tool frame, (T), relative to the station frame, {S) (See Figure's 3.1 and 3.2). Each of these via points is "converted" into a set of desired joint angles by application of inverse kinematics. Then a smooth function is found for each of the n joints which pass through the via points and end at the goal point. The time required for each segment is the same for each joint so that all joints reach the via point at the same time, thus resulting in the desired Cartesian position of {T) at each via point. Other than specifying the same duration for each joint, the determination of the desired joint angle function for a particular joint does not depend on the other joints. What is required is a function for each joint whose value at to is the initial position of the joint, and whose value at tf is the desired goal position at that joint. There are many smooth functions for eft), which might be used to interpolate the joint value. For the joint space schemes to achieve the desired position and orientation at the via points, we then consider the problem of fitting cubic curves. These cubic curves connect the via points in a smooth way. In making a single smooth motion, at least four constraints on e(t) are evident. Two constraints on the function's value come from the selection of initial and final values:

If desired velocities of the joints at the via points are known, then we can determine cubic polynomials with the velocity constraints as: PATH PLANNING SCHEMES

These four constraints can be satisfied by a polynomial of at least third degree. Since a cubic polynomial has four coefficients, it can be made to satisfy the four constraints given by Eq. (5.1) and Eq. (5.2). These constraints uniquely specify a particular cubic. A cubic has the form

and so the joint velocity and acceleration along this path are clearly

Combining Eq. (5.3) and Eq. (5.4) with the four desired constraints yields four equations in four unknowns:

Solving these equations for the aiwe obtain PATH PLANNING SCHEMES

Using Eq. (5.6) we can calculate the cubic polynomial that connects the initial and final positions. If we have the desired joint velocities at each via point, then we simply apply Eq. (5.6) to each segment to find the required cubics. One of the ways in which desired velocity at each of the via points might be specified is that the user inputs the desired velocity in terms of a Cartesian linear and angular velocity of the tool frame at that instant. The Cartesian desired velocities at the via points are "mapped" to desired joint rates using the inverse Jacobian of the manipulator evaluated at the via point.

5.4 Results We developed algorithm@) corresponding to the real and approximate dynamic models for generating cubic trajectories given arbitrary initial and final conditions (Appendix D):

Given initial and final times, to and tf, respectively, with the set of four initial conditions, (Eq. (5.1) and Eq. (5.2)) the required cubic polynomial 8(t) can be computed from Eq. (5.3). PATH PLANNING SCHEMES 52

A sequence of moves can be planned using equations (5.3) and (5.6) by using the end conditions df, df of the i-th move as initial conditions for the i+ 1-th move. Thus the input of Oft), e(t) and $(') (Eq.(5.3) and Eq. (5.4)) is applied to Eq. (4.14) to get the vector of joint torques T [57]. With a similar analogy, Xft), ~(t),and ~(t)can be applied to the Cartesian state space equation (Eq. (4.18)) to get the force-torque vector F acting on the end-effector of the robot, and X, is an appropriate Cartesian vector representing position and orientation of the end-effector [36]. As an example, Figure 5.1 shows the joint torques (real and approximate model, respectively) for a specific instant at t=3 seconds, with: do = 15' 8, = 75'

8, = 0.0 radlsec and, t, = 3 seconds. Using the above analogy Figure 5.2 shows the forces for the real and approximate model, respectively. PATH PLANNING SCHEMES

I Real vs. Approximate Model

Real Model

Aprx. Model

I Torque in N-m (-ve) I Figure 5.1 An example lot showing the ioint torclues

Real vs. Approximate Model

Real Model

I Aprx. Model

Force in N I Figure 5.2 An examvle lot showing the forces Chapter 6 Conclusions and Future Work

Off-line programming systems are useful in present-day industrial applications and can serve as a basis for continuing robotics research and development. A large motivation in developing OLP systems is to fill the gap between the explicitly programmed systems available today and the task level systems of tomorrow. In the last decade the growth of the industrial robot market has not been nearly as rapid as predicted. One primary reason for this is that robots are still too difficult to use. A great deal of time and expertise is required to install a robot in a particular application and bring the system to production readiness. There are many factors that make robot programming a difficult task. Most of these special problems arise from the fact that a robot manipulator interacts with its physical environment [28]. Even simple programming systems maintain a "world model" of this physical environment in the form of various objects and have "knowledge" about presence and absence of various objects encoded in the program strategies. During development of a robot program it is necessary to keep the internal model maintained by the programming system in correspondence with the actual state of the robot's environment. Interactive debugging of programs with a manipulator requires frequent manual resetting of the state of the robot's environment i.e., parts, tools, etc., must be moved back to their initial locations. Such state resetting becomes especially difficult (and sometimes costly) when the robot performs a irreversible operation on one or more parts (eg., drilling, routing, etc.). Although difficulties exist in maintaining an accurate internal model of the manipulator's environment, there seems no question that great benefits result from doing so. Whole ares of sensor research, perhaps most notably computer vision, focus on developing techniques by which world models may be verified, corrected, or discovered. Clearly, in order to apply any computational consideration algorithm to the robot CONCLUSIONS AND FUTURE WORK 55 command-generation problem, the algorithm needs access to a model of the robot and its surroundings. At the other end of the spectrum, are the so called task level programming (TLP) systems in which the programmer may state high-level goals such as "insert the bolt". Such systems use techniques from artificial intelligence research to automatically generate motion and strategy plans. However, these task level languages do not exist yet, although various pieces of such systems are under development by researchers [44]. Off- line programming systems should serve as the natural growth path from explicit programming systems to task level programming systems. The simplest OLP system is merely a graphical extension to a robot programming language, but from there it can be extended toward a task level programming system. This gradual extension is accomplished by providing automated solutions to various subtasks so as to explore options in the simulated environment. If we take this view, an OLP system serves as an important basis for research and development of task level planning systems. Hence, OLP systems should be a useful tool in research as well as an aid in current industrial practice. Two potential benefits of OLP systems relate directly to the language translation. Most proponents of OLP systems note that one universal interface which enables users to program a variety of robots solves the problems of learning and dealing with several automation languages. The second benefit stems from economic considerations in future scenarios in which hundreds of robots fill factories. The cost associated with a powerful programming environment (such as a language and graphical interface) may prohibit placing this at the site of each robot installation. Rather, it seems to make economic sense to place very simple, but cheap controller with each robot, and have it downloaded from a powerful, "intelligent" OLP system which is located in an office environment. Some of the advanced features which could be integrated into "baseline" OLP systems could be used to accomplish automated planning of an industrial application. Research on the planning of collision-free paths [43, 11, 481 and the planning of time- optimal paths [8, 61, 341 are natural candidates for inclusion in an OLP system. For example, consider the problem of using a six degree of freedom robot for an arc welding CONCLUSZONS AND FUTURE WORK

task whose geometry specifies only five degrees of freedom. Automatic planning of the redundant degree of freedom can be used to avoid collisions and singularities of the robot ~71. In many arc welding situations, details of the process require a certain relationship between the workpiece and the gravity vector to be maintained during the weld. This results in a two or three degree of freedom orienting system on which the part is mounted operating simultaneously with the robot in a coordinated fashion. In such a system there may be nine or more degrees of freedom to coordinate. Such systems are generally programmed today using teach pendant techniques. A planning system that could automatically synthesize the coordinated motions for such a system might be quite valuable [17, 11. In a simulated world in which objects are represented by their surfaces, it is possible to investigate the simulation of manipulator force-control strategies. This task involves the difficult problem of modeling some surface properties and expanding the dynamic simulator to deal with the constraints imposed by various contacting situations. In such an environment it might be possible to assess various force-controlled assembly operations for feasibility 1531. Planning automatic schedules for interacting processes is a difficult problem and an area of research [37, 21. An OLP system would serve as an ideal test bed for such research, and would be immediately enhanced by any useful algorithms in this area. An OLP system might be given some of the capabilities in modelling positioning errors sources and the effect of data from imperfect sensors [63, 221. The world model could be made to include various error bounds and tolerancing information, and the system could assess the likelihood of success of various positioning or assembly tasks. One of the basic tasks which could be extended to the OLP systems is the quick determination of the workcell layout in a simulated world as compared to the actual physical cell. Automatic placement (manipulator(s) collision free i.e., feasible reach to all of the required workpoints) can be computed by direct search, or by heuristic guided search techniques. Other criterion such as measure of manipulability in which the robot could reach in well-conditioned configurations could be applied [17, 491. CONCLUSIONS AND FUTURE WORK 57

Currently, we have TRC004, a PUMA Interface Card, which is a general purpose interface board for servo applications [67]. The TRC004 is ideally suited for high- performance robotic applications including force-controlled manipulators and dextrous hands. It was specifically designed to replace the LSI111 VAL computer and servo cards in the Unimate PUMA 560 manipulator to allow high speed, direct access to joint motor torques and positions [69]. When used as an interface to a Unimate PUMA 560 robotic manipulator, the TRC004, in conjunction with the user's real-time computer, replaces the entire LSI111 VAL computer and the joint servo cards (Figure 6.1). Physically, the CPU, RAM, EPROM, serial controller, interface cards, and joint servos are all removed from the controller backplane. In their place, the TRC004, a single twelve-inch by twelve-inch (approximately) multi-purpose 110 card, is mounted and wired point-to-point to the backplane. The TRC004's encoder inputs are connected to the joint encoders of the PUMA 560 for position acquisition. The analog outputs are connected to the power amplifiers for commanding motor torque. The analog inputs are used to measure the potentiometers during calibration. Finally, the discrete inputs and outputs are connected to various housekeeping functions including enabling arm power, controlling the pneumatic hand valves, and monitoring joint thermal sensors. It depends on the user's desire to replace as much of the functionality of the VAL operating system which includes, but is not limited to: - power-on arm calibration - occasional calibration of the joint potentiometers - low-level joint position control - arm kinematics - trajectory generation - joint limit monitoring The teach-pendant, which is a serial peripheral of the LSI111, becomes non-functional. CONCLUSIONS AND FUTURE WORK

ElSA Bus 486 PC PUMA Robot

'igure 6.1 A block Diagram of the PUMA 560 with New Controller and Sensors.

Appendix D shows a few routines available for running and controlling the PUMA 560 with the TRC004 interface board 1701. References

[I] Ahmad, S., and Luo, S., "Coordinated Motion Control of Multiple Robotic Devices for Welding and Redundancy Coordination through Constrained Optimization in Cartesian Space", Proceedings of the IEEE Conference on Robotics and Automation, Philadelphia, 1988.

[2] Akella, R., and Krogh, B., "Hierarchial Control Structures for Multicell Flexible Assembly System Coordination", IEEE Conference on Robotics and Automation, Raleigh, N.C., April 1987.

[3] Armstrong, B. and Khatib, 0. and Burdick, J., "The Explicit Dynamic Model and Inertial Parameters of the PUMA 560 Arm", IEEE, pp. 510-518, 1986.

[4] Asada, H., and Slotine, J. J., Robot Analysis and Control, Wiley , 1986.

[q Balkan, T., "Joint Space Trajectories for Robotic Manipulators", International Journal of Computer Applications in Technology, vol. 4, no. 3, pp. 148-151, 1991.

[6] Bar-On, D., Gutman, S., and, Israeli, A. "The TRACK - Technion Robot and Controller Kit", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 199 1.

[q Barnett, S., Matrix Methods for Engineers and Scientists, McGraw-Hill Book Company (UK) Limited, 1979.

[S] Bobrow, J., Dubowsky, S., and Gibson, J., "On the Optimal Control of Robotic REFERENCES

Manipulators with Actuator Constraints", Proceedings of the American Control Conference, June 1983.

[9] Bodur, I.N., "Improved Kinematic Model for Robots and Spatial Mechanisms. Part 11. An Exact Kinematic Model of PUMA 560", 3rd ASME Conference on Flexible Assembly Systems presented at the 1991 ASME Design Technical Conferences, Miami, FL, September 1991.

[lo] Bone, G.M., Elbestawi, M.A., Lingarkar, R., and Liu, L. "Force Control for Robotic Deburring " , Journal of Dynamic Systems, Measurement and Control, Transactions of ASME, vol. 113, no. 3, pp. 395-400, September 199 1.

[ll] Brooks, R., "Solving the Find-Path Problem by Good Representation of Free Space", IEEE Transactions on Systems, Man, and Cybernetics, SMC-13, pp. 190- 197, 1983.

[12] Burden, R.L., and Faires, J.D., Numerical Analysis, PWS-KENT Publishing Company, Boston, MA, 1989.

[13] Chiaverini, Stefano, Siciliano, Bruno, Egeland, Olav, "Redundancy Resolution for the Human Arm Like Manipulator", Robotics and Autonomous Systems, vol. 8, no. 3, pp. 239-250, 1991.

[14] Cho, H.C., Jeon, H.T., and, Lee, H.G., "Generation of Optimal Time Trajectory for the Two Cooperating Robot Manipulators", IEEE Conference on systems, Man and Cybernetics, Los Angeles, CA, November 1990.

[IS] Chuang, H., "Model Uncertainty and its Effects on Compliant Motion in Design and Simulation of the PUMA robot arm", Proceedings of the SLAM Regional REFERENCES 6 1

Conference on Geometric Aspects of Industrial Design, Wright-Patterson AFB, OH, April 1990.

[la Conte, S., and DeBoor, C., Elementary Numerical Analysis: An Algorithmic Approach, 2nd Edition, McGraw-Hill, 1972.

[17] Craig, J.J., "Coordinated Motion of Industrial Robots and 2-DOF Orienting Tables", Proceedings of the 17th International Symposium on Industrial Robots, Chicago, April 1987.

[IS] Craig, J. J., Introduction to Robotics, Mechanics and Control, Addison Wesley, Reading, MA, 1989.

[19] Critchlow, A.J., Introduction to Robotics, Macmillan, New York, 1985.

[20] Davies, B.L., Hibberd, R.D., Ng, W .S., Timoney, A. G., and, Wickham, J.E. A., "Development of a Surgeon Robot for Prostatectomies", Proceedings of the Institution of Mechanical Engineers, Pan H: Journal of Engineering in Medicine, vol. 205, no. 1, pp. 35-38, 1991.

[21] Denavit, J., and Hartenberg, R.S., "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices", Journal of Applied Mechanics, pp. 215-221, June 1955.

[22] Durrant-Whyte, H., "Uncertain Geometry in Robotics", IEEE Conference on Robotics and Automation, Raleigh, N. C., April 1987.

[23] Egeland, O., Sagli, J.R., Spangelo, I., and Chiaverini, S., "A Damped Least Squares Solution to Redundancy Resolution", Proceedings of the 1991 IEEE REFERENCES

International Conference on Robotics and Automation, Sacramento, CA, April 1991.

[24] El, S.K., and, Khalil, W., "Energy Based Indirect Adaptive Control of Robots", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 199 1.

[25] Engleberger, J., Robotics in Practice, Kogan Page, London, 1980.

[26] Erlic, Mile, Jones, Kevin, Lu, W. -S . , "Hardware Interface Configuration for Motion Control of the PUMA-560 and the Mitsubishi RM-501 Robots", Proceedings of the 1991 IEEE Pacijic Rim Conference on Communications, Computers and Signal Processing, Victoria, BC , May 199 1.

[27l Ganguly, S., Tarn, T.J., and, Bejczy, A.K., "Control of Robots with Discrete Non-Linear Model: Theory and Experimentation", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 1991.

[28] Goldman, R., Design of an Interactive Manipulator Programming Environment, UMI Research Press, Ann Arbor, Michigan, 1985.

[29] Gupta, K.K., and Guo, Z.P., "A Practical Motion Planner for PUMA 560", Proceedings of the 1991 IEEE Pacijic Rim Conference on Communications, Computers and Signal Processing, Victoria, BC, May 1991.

[30] Hashimoto, K., Kimoto, T., Ebine, T., and, Kimura, H., "Manipulator Control with Image Based Visual Servo", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 1991. REFERENCES 63

IntergraphIEngineering Modeling System (IIEMS), Reference Manual, Huntsville, Alabama, October 1990.

IntergraphIEngineering Modeling System (IIEMS), Operator Training Guide - Basics, Huntsville, Alabama, March 1992.

IntergraphIParametric Programming Language (PPL), User's Guide, Huntsville, Alabama.

Johanni, R., and Pfeiffer, F., "A Concept for Manipulator Trajectory Planning", Proceedings of the IEEE Conference on Robotics and Automation, San Francisco, 1986.

Khatib, O., "Commande Dynamique dans L'Espace Operationnel des Robots Manipulateurs en Presence d 'Obstacles", These de Docteur-Ingenieur. Ecole National Superieure de lYAeronautiqueet de L'Espace (ENSAE), Toulouse.

Khatib, O., "Dynamic Control of manipulators in Operational space", sixth IFTOMM Congress on Theory of Machines and Mechanisms, New Delhi, December 15-20, 1983.

Kusiak, A., and A. Villa, "Architecture of Expert Systems for Scheduling Flexible Manufacturing Systems", IEEE Conference on Robotics and Automation, Raleigh, N.C., April 1987.

Laird, J.E., Yager, E.S., Hucka, M., and Tuck, C.M., "Robo-Soar. An Integration of External Interaction, Planning and Learning Using Soar", Robotics and Autonomous Systems, vol. 8, no. 1-2, pp. 113-129, 1991. REFERENCES 64

[39] Leahy, M.B. Jr., Whalen, P.V., "Direct Adaptive Control for Industrial Manipulators", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 199 1.

[40] Li, C.J., and, Sankar, T.S., "Fast Inverse Dynamics Computation in Real Time Robot Control", 16th Annual Conference of IEEE Industrial Electronics Society, Pacific Grove, CA, November 1990.

[41] Li, W., "Schnelle Abbildung Von Hindernissen in den Konfigurationsraum" ,(Fast Mapping Obstacles in the Configuration Space), Univ des Saarlandes, Saarbruecken, Germany, Robotersysteme vol. 7, no. 3, pp. 148-154, September 1991.

[42] Lipkin, H., and Pohl, E., "Enumeration of Singular Configurations for Robotic Manipulators", Journal of Mechanisms, Transmissions, and Automation in Design, vol. 113, no. 3 pp. 272-279, September 1991.

[43] Lozano-Perez, T., "A Simple Motion Planning Algorithm for General Robot Manipulators", ZEEE Journal of Robotics and Automation, Vol. RA-3, No. 3, June 1987.

[44] Lozano-Perez, T., "Spatial Planning: A Configuration Space Approach", IEEE Transactions on Systems, Man, and Qbernetics, Vol. SMC- 11, 1983.

[45] Moon, K.S., Azadivar, F., and Kim, K., "Stochastic Optimization of the Production Speed of Robots Based on Measured Geometric and Non-Geometric Errors", International Journal of Production Research, vol. 30, no. 1, pp. 49-62, January 1992. REFERENCES 65

[46] Mujtaba, S., and R. Goldman, "4User's Manual", 3rd Edition, Stanford Department of Computer Science, Report No. STAN-CS-81-889, December 1981.

[47l Murphy, S.H., and, Wen, J.T., "Dynamic Modelling of Geared and Flexible Jointed Manipulators", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 1991.

[48] Nakamura, Y., Advanced Robotics, Redundancy and Optimization, Addison- Wesley Publishing Company, Reading, MA, 1991.

[49] Nelson, B., Pedersen, K., and Donath M., "Locating Assembly Tasks in a Manipulator's Workspace", IEEE Journal of Robotics and Automation, Raleigh, N.C., April 1987.

[SO] Noble, B., Applied Linear Algebra, Prentice-Hall, 1969.

[Sl] Oh, Se-Young., and Joe, Moon-Jeung., "Dynamic Control of a Six Degree of Freedom Robot Manipulator Using Neural Networks", International Joint Conference on Neural Networks, Seattle, WA, July 1991.

1521 Parkin, R.E., Applied Robotic Analysis, Prentice Hall, NJ, 1991.

[S3] Peshkin, M., and A. Sanderson, "Planning Robotic Manipulation Strategies for Sliding Objects", IEEE Conference on Robotics and Automation, Raleigh, N.C., April 1987.

[S4] Pieper, D., and B. Roth, "The Kinematics of Manipulators Under Computer Control", Proceedings of the Second International Congress on Theory of Machines and Mechanisms, Vol. 2, pp. 159-169, Zakopane, Poland, 1969. REFERENCES 66

[55] Pieper, D., " The Kinematics of Manipulators Under Computer Control", Unpublished Ph.D. Thesis, Stanford University, 1968.

[56] Press, W.H., Flannery , B. P., Teukolsky , S .A., and Vetterling, W.T., Numerical Recipes in C, The Art of ScientiJic Computing, Cambridge University Press, New York, 1988.

[57] Raibert, M., "Mechanical Arm Control Using a State Space Memory", SME paper MS77-750, 1977.

[58] Rogers, D. F., and J. A. Adams, Mathematical Elements for Computer Graphics, published by McGraw-Hill, New York, 1990.

[59] Roth, B., "Performance Evaluation of Manipulators from a Kinematic Viewpoint", Pe@omance Evaluation of Manipulators, National Bureau of Standards, special publication, 1975.

[60] Roth, R., "Principles of Automation", Future Directions in Manufacturing Technology, Based on the Unilever Research and Engineering Division Symposium held at Port Sunlight, April 1983, Published by Unilever Research, UK.

[61] Shin, K., and N. McKay, "Minimum-Time Control for Manipulators with Geometric Path Constraints", IEEE Transactions on Automatic Control, June 1985.

[62] Smiarowski, A. Jr., and Anderson, J.N., "Fast Computer Architecture for the Control of Robots", Computers and Electrical Engineering, vol. 17, no. 3, pp. 217-235, 1991. REFERENCES 67 [63] Smith, R., M. Self, and P. Cheeseman, "Estimating Uncertain Spatial Relationships in Robotics", IEEE Conference on Robotics and Automation, Raleigh, N.C., April 1987.

[64] Spong, M. W., and M. Vidyasagar, Robot Dynamics and Control, John Wiley & Sons, NY, 1989.

[65] Symon, K.R., Mechanics, 3rd Edition, Addison-Wesley, Reading, MA, 1971.

[66] Tarn, T.J., Ganguly, S., Ramadorai, A.K., Marth, G.T., and Bejczy, A.K., "Experimental Evaluation of the Non-Linear Feedback Robot Controller", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 1991.

[671 Trident Robotics and Research, Inc., TRCW User's Manual, Wexford, Pennsylvania, September 1991.

[68] UNIMATE PUMATMROBOT 5501560 Series Volume 1 - Equipment Manual 398HlA, Unimation Inc., Danbury, CT, May 1982.

[69] UNIMATE PUMATMROBOT 5501560 Series Volume 2 - User's Guide to VALm 398H2A, Version 12.4.6, Unimation Inc., Danbury, CT, May 1982.

[70] Vigilant Technologies, Initial coding by Richard Voyles, 1991.

[71] Zhang, H., "Feasibility Analysis of Displacement Trajectories for Robot Manipulators with a Spherical Wrist", Proceedings of the 1991 IEEE International Conference on Robotics and Automation, Sacramento, CA, April 1991. Appendix A

A-1 Link Transformations for PUMA 560 APPENDIX A

The final transformation matrix T: for PUMA 560 is:

where APPENDIX A Appendix B

B-1 2-Y-2 Euler Angles Starting with a frame coincident with a known frame (A), if a frame (B) is first rotated about 2 by an angle a, then rotated about Y by an angle 0, and then rotated about z by an angle y then the equivalent rotation matrix is given as:

The solution for extracting Z-Y-Z Euler angles from a rotation matrix is stated below: Given

If sin0 # 0, then APPENDIX B

If 0 = 0.0, then a solution may be calculated as

If P = 180.O0,then a solution may be calculated as Appendix C

C-1 Jacobians in the Force Domain There are joint torques that exactly balance forces at hand in the static situation. When forces act on a mechanism, work (in the technical sense) is done if the mechanism has a certain displacement. Work, a scalar quantity, is defined as a force acting through a distance. The principle of virtual work allows us to make certain statements about the static case by allowing the amount of this displacement to go to an infinitesimal. Specifically, we can equate the work done in Cartesian terms with the work done in joint space terms. In the multidimensional case, work is the dot product of a vector force or torque and a vector displacement. Thus we have,

where F is a 6x1 Cartesian force-moment vector acting at the end-effector, 6X is a 6x1 infinitesimal Cartesian displacement of the end-effector, 7 is a 6x1 vector of torques at the joints, 68 is a 6x1 vector of infinitesimal joint displacements. Expression (C-1) can also be written as

The definition of Jacobian is

6X = J60, and so we may write

which must hold for all 68, and so we have

F~J= rT. APPENDIX C

Transposing both sides yields the result

z = JT F.

Thus, the Jacobian transpose maps Cartesian forces acting at the hand into equivalent joint torques. Eq. (C-6) is very interesting relationship in that it allows us to convert a Cartesian quantity into a joint space quantity without calculating any inverse kinematic functions. This is made use of in considering control problems.

C-2 The J and J matrices for PUMA 560

The Jacobian matrix for PUMA 560 is:

where

JII = -s,(%4% + sd4 + cfl3 - c,d,

J,, = c, f'-s2@3 + cd.4 - ?2%.J J,, = c, f-sfi3 + 0 J,, = 0.0 J,, = 0.0 APPENDIX C APPENDIX C

The J matrix for PUMA 560 is:

where

Jll = -clc~381+ s,s&,8, - c,s&.$i, - s,c&.$i, + s&B2 + s,dd1

512 = ~1~2341- ~1~243423 - s1cd44 - c1sdd1 + s,sp,~,-c1cfi,82

J13 = sIsuad1 - c1cfiA - s,c&.$il - c1s&.@, J14 = 0.0 J*, = 0.0 J,, = 0.0 APPENDIX C APPENDIX C Appendix D

D-1 Computer Program to Evaluate the Forward Kinematics, i.e., of the PUMA 560

#include < stdio.h > #include < math.h > #include < stdlib. h > #include < time.h >

I* Defining factorizations for trignometric terms that are functions of joint angles *I

I* Defining fixed link parameters as per Denavit-Hartenberg notation *I

ldefine pi (3.1415927) float theta[q; I* Vector of Joint Angles B,,92, ..., d6 *I tloat xe,ye,ze; I* Desired End-Effector position in Cartesian Coordinates *I int i; float Tdesired[5][5]; I* 4x4 Matrix T,, for End-Effector orientation and position *I

main() { void frwd-matrix(); I* Returns T,, *I APPENDIX D

printf("xe ye ze %f %f %fin",xe,ye,ze); return; 1

void fwd-matrix(Tdesired) float Tdesired[S][S]; {

return; 1 APPENDIX D

D-2 Computer Program to Evaluate the Inverse Kinematics Solution of PUMA 560

#include < stdio. h > #include < math.h > #include < std1ib.h >

I* Defining factorizations for trignometric terms that are functions of joint angles *I

I* Defining fmed link parameters as per Denavit-Hartenberg notation *I

float theta[l; I* Vector of Joint Angles 8,,02, ..., 13, *I float xe,ye,ze; I* Desired End-Effector position in Cartesian Coordinates *I int i j,k; float Tdesired[5][5]; I* 4x4 Matrix T,, for End-Effector orientation and position *I

I* Defining variable expressions and constants in Pieper's Solution *I float kll,k21,k31; float k12,k22,k32; float kc,kcl; float r; float gl1 ,g2; float g12; float g13; float g14; float lhstrans[4][4]; I* 3x3 Matrix for Evaluating (R!)" *I APPENDIX D float Ihsmtrix[4][4]; /* 3x3 Matrix for Evaluating (R$"*T,, for 2-Y-2Euler Angles Solution */ float angIes[7l[S]; I* Array for 4 sets of Joint Solutions using Pieper's Solution approach *I

main()

void fwd-matrix(); /* Returns T,, *I

void zR3TransO; I* Returns (Rg)-'*I

for(i=O;i < =6;i+ +) theta[i] =O.O; theta[l] =O.O*pi/l80.0;theta[2] =30.0*pi/180.0;theta[3] =30.0*pi/l80.0;theta[4] =O.O*pi/l80.0;theta[5] =30.0*pi/180.0; theta[6] =0.0*pi1180.0; printf("the respective angles in DEGREES fed in follow\n"); for(i= 1;i < =6;i+ +) printf("%f\n",theta[i]*180.01pi); for(i=O;i< =4;i+ +) for(j=O;i < =4i+ +) Ihsmtrix[i][i] =0.0;

I****thetaa=2.0*atan( (d4 +sqrt(d4*d4 + a3*a3 - kc*kc)l(a3 + kc)) ); thetab =2.0*atan( (d4-sqrt(d4*d4 + a3*a3 - kc*kc)/(a3 +kc)) );****I

angles[3][1]=atan2(d4,a3)+atan2(sqrt(a3* + d4*d4 -kc*kc),kc); angles[3][2] =atan2(d4,a3)-atan2(sqrt(d*a3 + d4*d4 -kc*kc) ,kc); printf("2 values of theta[3] (in deg):----> angles[3][l] angles[3][2] %f %Anw,angles[3][1]*180.0/pi,angles[3][2]*180.Olpi); APPENDIX D

printf("kll,k2l,k31 < > %f %f %f \nM,kll,k21,k31);

I**** CAN USE THIS CHECK TO FIND SUITABLE thetar31 AND PROCEED TO FIND theta(21 value@)******/

angles[2][1]= atan2(-kl l,k21) + atan2( sqrt(k21*k21 + kl l*kll -ze*ze),ze ); angles[2][2]= atan2(-kll,k21) - atan2( sqrt(k21*k21 + kl l*kll -ze*ze),ze ); printf("2 values of theta[2] in deg (frorn angles[3][2]):-----> angles[2][1] angles[2][2] %f %b",angles[2][1]*180.0/pi,angles[2][2]*180.0/pi);

k32=a3*a3 +d4*d4 +d3*d3 +a2*a2 + 2.0*a2*a3*cos(angles[3][1]) + 2.0*a2*d4*sin(angles[3][1]) + d2*d2 + 2.0*d2*d3; printf("k12,k22,k32 < > %f %f %f \nU,k12,k22,k32);

I**** CAN USE THIS CHECK TO FIND SUITABLE theta[3] AND PROCEED TO FIND theta[2] value(s)******/

angles[2][3]= atan2(-k12,k22) + atan2( sqrt(k22*k22 + k12*k12 -ze*ze),ze ); angles[2][4] = atan2(-k12,k22) - atan2( sqrt(k22*k22 + k12*k12 -ze*ze),ze ); printf("2 values of theta[2] in deg (from angles[3][1]):-> angles[2][3] angles[2][4] %f %~n",angles[2][3]*180.0/pi,angles[2][4]*180.0/pi);

gll =cos(angles[2][1])*( a3*cos(angles[3][2]) + d4*sin(angles[3][2]) + a2) -sin(angles[2][1])*(a3*sin(angles[3][2]) - d4*cos(angles[3][2]) ); g2=d3+d2; printf("gl1 ,g2 %f %flnW,gl1,g2);

printf("angles[l][l] in DEGREES %f\n",angles[l][l]*l80.0/pi); APPENDIX D

angles[l][2] =atan2(gl2*ye-g2*xe,g 12*xe +@*ye);

printf("angles[l][2] in DEGREES %nnn,angles[l][2]*180.0/pi);

angles[l][3]=atan2(g13*yeg?*xe,g13*xe+g2*ye);

printf("angles[l][3] in DEGREES %f\n",angles[l][3]*180.O/pi);

angles[l][4] =atan2(gl4*ye-g2*xe,gl4*xe +g2*ye);

printf("angles[l][4] in DEGREES %f\n",angles[l][4]*180.0/pi);

pTintf("angles[5][1] in DEGREES %f\n",angIes[5][1]*180.0/pi); APPENDIX D

{angles[5][1] = 0.0; angles[4][1] =0.0; angles[6][1] =atan2( -Ihsmtrix[l][2],lhsmtrix[l][l]);} else if(angles[5][1] = =pi) {angles[S][l] =pi; angles[4][1] =0.0; angles[6][1]=atan2( hsmtrix[l][2],-lhsmtriu[l][l]);} else {angles[S][l]=atan2( sqrt( lhsmtrix[2][1]*lhsmtrix[2][1] + lhsmtnx[2][2]*lhsmtrix[2][2] ) , -lhsmtrix[2][3] 9 angles[4][1]=atan2( lhsmtrix[3][3]/sin(angles[5][l]),lhsmt~][l]) ); angles[6][1] =atan2( -lhsmtrix[2][2]lsin(angles[5][l]),lhsmtnx[2][l]lsin(angles[5][l]) );}

printf("angles[4][1],angles[5][1],angles[6][1] in DEG %f %f %fin",angles[4][1]*180.0/pi,angles[51[1]*180.0/pi,angles[6][1]*180.0/pi);

printf("the respective angles in DEGREES follow\nm); for(i=l;i< =6;i+ +) printf(" %finw,theta[i]*180.01pi); 1

printf("angles[5][2] in DEGREES %fin",angles[5][2]*180.0/pi);

if(angles[5][2] < = 1.0e-04) {angles[5][2] =0.0; angles[4][2] =0.0; angIes[6][2] =atan2( -Ihsmtrix[l][2],lhsmt~l] );) else if(angles[5][2] = =pi) {angles[5][2] =pi; angles[4][2] =0.0; angles[q[2]=atan2( lhsmtrix[l][2],-lhsmtrix[l][l] );) else {angles[q[2] =atan2( sqrt( lhsmtrix[2][1]*lhsmtrix[2][1] + Ihsmtrix[2][2]*lhsmtrix[2][2] ) , -lhsmtrix[2][3] APPENDIX D 86

printf("angles[4][2],angles[5][2],angles[6][2] in DEG %f %f %f\n",angles[4][2]*180.0/pi,angles[5][2]*180.0/pi,angIes[6][2)*180.01pi);

printf("the respective angles in DEGREES follow\n"); for(i=l;i< =6;i+ +) p~tf("%f\n",theta[i]*180.0/pi); 1

printf("angles[5][3] in DEGREES %An",angles[5][3]*180.0/pi);

if(angles[5][3] < = 1.0e-04) {angles[Sl[3] =0.0; angles[4][3] =0.0; angles[6][3] =atan2( -lhsrntrix[l][2],lhsmtrix[l][l] );} else if(angles[5][3] = =pi) {angles[5][3] =pi; angles[4][3] =0.0; angles[q[3]=atan2( lhsmtrix[l][2],-lhsmtrix[l][l] );) else {angles[5][3] =atan2( sqrt( Ihsmtrix[2][1]*lhsmtrix[2][1]+ lhsmtrix[2][2]*lhsmtrix[2][2] ) , -lhsmtrix[2][3] angles[4][3]=atan2( Ihsmtrix[3][3]/sin(angles[5][3]),lhsmtrix[l][3]/sin(angles[5][3]) ); angles[6][3]=atan2( -hsmtrix[2][2]lsin(angles[5][3]),lhsmtnx[2][l]/sin(angles[5][3]) );}

printf("angles[4][3],angles[5][3],angles[6][3] in DEG %f %f %fin",angles[4][3]*180.0/pi,angles[5][3]*180.0/pi,angles[6][3]*18O.O/pi); APPENDIX D

p~tf("therespective angles in DEGREES follow\n"); for(i=l;i< =6;i+ +) p~tf("%flnW,theta[i]*180.0/pi); 1

pMtf("angles[S][4] in DEGREES %An" ,angles[5][4]* 180.01pi);

if(angles[5][4] < = 1 .&a) (angles[5][4] =0.0; angles[4][4] =0.0; angles[6][4] =atan2( -lhsmtrix[l][2],lhsmtrix[[l][l]);} else if(angles[5][4] = =pi) {angles[5][4] =pi; angles[4][4] =0.0; angles[6][4]=atan2( lhsmtrix[l][2],-lhsmt~k@][l] );} else {angles[5J[4]=atan2( sqrt( lhsrntrix[2][1]*lhsmtrix[2][1] + lhsmtrix[2][2]*lhsmtrix[2][2) ) , -lhsmtrix[2][3] angles[4][4]=atan2( lhsmtrix[3][3]lsin(angles[5][4]),lhsmt~]) ); angles[6][4] =atan2( -lhsmtrix[2][2]lsin(angles[5][4]),lhsmt~[2][l]/sin(angles[5][4]) );}

printf("angIes[4][4],angles[5][4],angles[6][4] in DEG %f 96f %fln",angles[4][4]*180.0lpi,angles[5][4]*180.0/pi,angles[6][4]*180.0/pi);

printf("the respective angles in DEGREES follow\n"); for(i=l;i< =6;i++) printf("%f\n",theta[i]*l80.O/pi); 1

return; 1 APPENDIX D

void fwd-matrix(Tdesired) float Tdesired[S][S];

return; 1

void zR3Trans(lhstrans) float lhstrans[4][4] ; { lhstrans[l][l] =cl*c23; lhstrans[l][2] =sl*c23; lhstrans[1][3] =-s23;

return; 1 APPENDIX D

D-3 Computer Program to Evaluate J and J matrices for PUMA 560

#include < stdio. h > #include < math.h > #include < std1ib.h >

/* Defining factorizations for trignometric terms that are functions of joint angles */

I* Defining fixed link parameters as per Denavit-Hartenberg notation */

int i;

Iloat theta[q; I* Vector of Joint Angles 8,,8,, ..., 8, *I Iloat jac[l[l; I* (6x6) Jacobian matrix *I tloat thdot[q; /* Vector of d */ Iloat jacdot[T[T; I* (6x6) f matrix */

main() { void jacobiano; I* Returns the (6x6) Jacobian Matrix *I void jacobdoto; I* Returns the (6x6) j Matrix *I

jacobian(jac); for(i=l;i< =6;i+ +) printf("jac main %f %f %f %f %f %fint'jac[i][l] jac[i][2] jac[i][3] jac[i][4] jac[i][5]jac[i][6]); APPENDIX D 90

jacobdot(jacd0t); for(i=l;i< =6;i+ +) printf("jacdot main % f % f % f % f % f %finwjacdot[i][l] jacdot[i][2] jacdot[i][3]jacdot[i][4] jacdot[i][5]jacdot[i][q);

return; 1

void jacobiadjac) float jac[7[7]; { jac[l][l] = -sl*(c23*a3 + s23*d4 + c2*a2) cl*(d2 +d3); jac[l][2]= cl*(-s23*a3 + c23*d4 - s2*a2); jac[l][3]= cl*(-s23*a3 + c23*d4); jac[l][4] = 0.0; jac[1][5J = 0.0; jac[1][6]= 0.0;

jac[S][l]= 0.0; jac[5][2] = cl ; jac[5][3]= cl; jac[5][4]= sl*s23; jac[5][5] = -sl*c23*s4 +cl *c4; jac[5][6] = (sl*c23*c4 +cl*s4)*s5 +sl*s23*c5; APPENDIX D

for(i=l;i< =6;i++) p~tf("jacsubr %f %f %f %f %f %fin"jac[i][l] jac[i][2] jac[i][3] jac[i][4] jac[i][5] ~ac[i][6]);

return; 1

void jacobdot(jacdot) float jacdot[q[q; { jacdot[l][l]= -cl*c23*a3*thdot[l] + sl*s23*a3*(thdot[2]+thdot[3]) -cl*s23*d4*thdot[l] -sl *c23*d4*(thdot[2] +thdot[3]) +s2*a2*thdot[2] +sl*(d2+d3)*thdot[l]; jacdot[l][2]= sl*s23*a3*thdot[l] -cl*c23*a3*(thdot[2]+thdot[3]) -sl*c23*d4*thdot[l] -cl*s23*d4*thdot[l] +sl*s2*a2*thdot[l] -cl*c2*a2*thdot[2]; jacdot[l][3] = sl *s23*a3*thdot[l] -cl*c23*a3*(thdot[2] +thdot[3]) -sl*c23*d4*thdot[l] -cl *s23*d4*(thdot[2]+thdot[3]); jacdot[l][4] = 0.0; jacdot[l][5] = 0.0; jacdot[l][6] = 0.0; APPENDIX D 92

for(i=l;i< =6;i++) printf("jacdot subr %f %f %f %f %f %!In" jacdot[i][l] jacdot[i][2], jacdot[i][3] jacdot[i][4] jacdot[i][5] jacdot[i][6]);

return; 1 APPENDIX D 93

D-4 Computer Program to Verify the Equations of Motion (Using "Conservationof Energy Approach") and find the computer processor time taken for the Real Model

I* Defining factorizations for trignometric terms that are functions of joint angles *I

/* Defining Expressions for the Inertial Constants Appearing in the Equations of Forces of Motion *I

#define ill (-0.0142) #define i12 (-0.0110) #define i13 (-3.79e-03) APPENDIX D

I* Defining Expressions for the Gravitational Constants Appearing in the Equations of Forces of Motion *I

#define ggl (-37.2) #define gg2 (-8.44) #define gg3 (1.02) #define gg4 (0.249) #define gg5 (-0.0282 )

I* Defining fixed link parameters as per Denavit-Hartenberg notation *I

#define TINY 1.0e-20; #define N (6) /***for 6x6 "A" ie to find inv. of "ar" matrix as "yy" matrix *********I float **a,**yy,d,*col; int i ,j,*indx; float theta[q; I* (6x1) Vector of Joint Angles 8,,8,, . . . , 8, */ float ar[q[q,br[7][16],CR[q[q,gr[q;I* A(6x6), B(6x15), and C(6x6) matrices and the g(6x1) vector *I

float **y,*xx=O;

float vel[7j,posn[7l; I* Vector of Joint Velocities and Joint Positions *I

float kecons,pecons,totenrgy; I* The Scalars Kinetic Energy, Potential Energy and the Total Energy *I APPENDIX D

main() { void armat(); I* Returns the expressions giving the elements of the Kinetic Energy matrix *I void brmat(); I* Returns the expressions giving the elements of the Coriolis matrix *I void CRrnat(); I* Returns the expressions giving the elements of the Centrifugal matrix *I void grmato; I* Returns the Gravity terms *I

void rk4(),rkdumb(); I* Runge-KuUa integration *I float vstart[l3]; lI,dydx[l3],yout[l3]; int nvar,nstep; float xl,x2; void (*derivs)(); void derivative(); /* Returns dyldx *I

void ludcmp() ,lubksb(); float **matrix(); float *vector();

int k; float iden[q[q;

clock-t start,end; I* Defining system clock variables *I time-t fust,second; I* Defining system time variables */

start = clock(); I* get the no. of system clock ticks per second *I fmt=time(NULL); I* get the system time *I

derivs=&derivative; I* Passing the address of derivative() as an argument *I

x1=0.0; I* Range *I x2=1.0; nvar= 12; I* Number of variables */ nstep=lO; I* Number of steps *I

I* Defining Boundary Conditions and Initial Conditions *I APPENDIX D

kecons =0.0; pecons=O.O; totenrgy =0.0; APPENDIX D

printf("ke,pe,te before %f %f %An",kecons,pecons,totenrgy); for(i=l;i< =6;i+ +) for(i=I;i<=6;i++) kecons = kecons + O.S*(vel[i]*ar[i]Lj]*velfi]);

printf(" kecons = %finM,kecons); pons=9.81*( 17.40*(-sin@osn[2])*0.068-~os(posn[2])*0.006)+ 4.8*(-cos@osn[2] +posn[3])*-0.07O-sin@osn[2])*a2) + 0.82*(cos@osn[2]+posn[3])*-0.019-sin@osn[2] +posn[3])*a3 + cos@osn[;?]+posn[3])*d4 -sin@osn[2])*a2) + 0.34*(-sin@osn[2]+posn[3])*a3 +cos@osn[2]+posn[3])*d4 -sin@osn[2])*a2)i-O.O9*(-sin(posn[2] +posn[3])*cos@osn[4])*sin(posn[5]) -kos@osn[2] +posn[3])*cos(posn[5])*0.032 -sin@osn[2] +posn[3])*a3 + cos(posn[2] +posn[3])*d4 -sin(posn[2])*a2) ); /I WRIST pecons=9.81*( 17.40*(-sin(posn[2])*0.068-cos(posn[2])*0.006) + 6.04*(

(sin@osn[2] +posn[3])*cos@osn[4])*cos@osn[S])*sin(posn[6]) +cos@osn[21+posn[3l)*sin@osn[5I)*sin@osn[61) +sin@osn[21 +posn[3])*sin(posn[4])*cos(posn[6]) )*-0.143 + (-sin(posn[2] +posn[3])*cos(posn[4])*sin(posn[5]) +cos(posn[21 +posn[31)*cos(posn[51))*0.014 -sin(psn[2] +posn[3])*a3 +cos(posn[2] +posn[31)*d4-sin@osn[21)*a2 ) ); printf(" pons= %AnW,pecons);

end = clock(); I* again get the system clock time after the running of processor *I second=time(NULL); I* again get the system time after the running of processor */

printf("Time elapsed as per system clock was : % f secs.\n" ,(end-start)lCLK-TCK); printf("The elapsed time by difftime is %f secs.\n",difRime(second,fust)); return; }

void armat(ar) float ar[q[q; { int i j; APPENDIX D

return; 1

void brmat(br) float br[7l[16]; { int i j; APPENDIX D APPENDIX D APPENDIX D

for (i=l;i<=6;i+ +) {p~tf("br%f %f %f %f %f %f %f %f\n",br[i][l],br[i][2],br[i][3],br[i][4],br[i][5],br[i][6],br[i][q,br[i][8]); printf(" %f%f %f%f %f %f %~n",br[i][9],br[i][lO],br[i][ll],br[i][l2],br[i][l3],br[i][l4],br[i][l5]);} //pMtf("frombr brll= %f\n",br[l][l]); return; 1 void CRmat(CR) float CR[?[q; { int i J; extern float br[7][16];

IlpMtf("from inside CR brl 1 = %f\n",br[l][l]); APPENDIX D

for(i=l;i<7;i++) printf("CR %f %f %f %f %f %f\nw,CR[i][l],CR[i][2],CR[i][3],CR[i][4],CR[i][5],CR[i][6]); for (i=l;i< =6;i++)

return; 1

void grmat(gr) float gr[7]; { int i; gr[l]= (0.0); gr[2]= (ggl*c2 + gg2*s23 + gg3*s2 + gg4*c?3 + ggS*(s23*c5 + c23*c4*s5) ); gr[3]= (gg2*s23 + gg4*c23 + ggS*(s23*c5 + c23*c4*s5) ); gr[4] = (-ggS*s23*s4*s5 ); gr[5] = (ggS*(c23*sS + s23*c4*c5) ); gr[6] = (0.0);

return; 1

void rk4(y,dydx,n,x,h,yout,derivs) float yn,dydxfl,x,h,youtu; void (*derivs)();

int n; { int i; float xh,hh,h6,*dyrn,*dyt,*yt,*vector(); void free-vector(); APPENDIX D

xh=x+hh; for (i=l;i< =n;i+ +) yt[i]=y[i]+hh*dydx[i]; (*derivs)(xh,yt,dyt); for (i=l;i< =n;i+ +) yt[i]=y[i]+hh*dyt[i]; (*derivs)(xh, yt ,dym); for (i=l;i< =n;i++) { yt[i]=y[i]+h*dym[i]; dym[i] + = dyt[i]; 1 (*derivs)(x+ h,yt,dyt); for (i=l;i< =n;i++) yout[i] =y[i] +h6*(dydx[i] +dyt[i] +2.0*dym[i]); free-vector(yt, 1 ,n); free-vector(dyt,l ,n); free_vector(dym, 1 ,n); 1

float *vector(nl,nh) int nl,nh; { void nremr(); float *v; v=(float *)malloc((unsigned) (nh-nl+ l)*sizeof(float)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free_vector(v,nl,nh) float *v; int n1,nh; { frce((char*) (v + nl)); }

void nrerror(error-text) char error-textu; { void exit(); fp~tf(stderr,"NumericalRecipies run-time error ...\nu); fp~tf(stderr,"%s\n" ,error-text); fp~tf(stderr,"...now exiting to system. ..\nW); exit(1); 1

void rkdumb(vstart,nvar,xl ,x2,nstep,derivs) int nvar,nstep; float vstart0,xl ,x2; void (*derivs)(); { int i,k; float x,h; float *v,*vout,*dv,*vector(),**matrix(); APPENDIX D

void rk4() ,nrerror(), free-vector(); float kemat[T;

v =vector(l ,nvar); vout =vector(l ,nvar); dv =vector(l ,nvar); xx=vector(l ,nvar); y =matrix(l ,nvar, 1,nvar);

for(i=O;i < =7;i+ +) kernat[i] =0.0; for(i= l;i< =nvar;i+ +) { v[i] =vstart[i]; y[il[ll =v[il; 1 xx[l]=xl; x=xl; h =(x2-x1)lnstep; for (k=l:k< =nstep;k+ +) { (*derivs)(x,v,dv); rk4(v,dv,nvar,x,h,vout,derivs); if ((float)(x+h) = = x) nrenor("Step size too small in routine RKDUMB"); x +=h; xxF+l]=x;

for (i=l;i< =nvar;i+ +) { ~[i]=vout[i]; y[i]F+ l]=v[i];

float **matrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch; { int i; float **m;

m=(float **) maUoc((unsigned) (nrh-nrl+ l)*sizeof(float*)); if (!m) nrerror("allocation failure 1 in matrix()"); m -=nrl;

for(i=nrl;i < =nrh;i+ +) { m[i] =(float *) rnalloc((unsigned) (nch-ncl+ l)*sizeof(float)); if (!m[i]) nrerror("al1ocation failure 2 in matrix()"); m[i] -=ncl; 1 return m; APPENDIX D

I* This evaluates e = A-l(7 - ~@)@9)- C@)(02) - g(e)), which is integrated by RK-4 method to yield j o i n t velocities i.e., 9 and joint positions is., 9 *I

void derivative(x,y ,dydx) float x,yU,dydxu; { void armat(); void brmat(); void CRmat(); void grmat();

int ij;

for(i=l j=2;i< =2*N-1 j< =2*N;i+ + j=j+2) dydx[j]=-(br[i][l]*y[2]*y[4] + br[i][2]*y[2]*y[6] + br[i][3]*y[2]*y[8] + br[i][4]*y[2]*y[10] + br[i][S]*y[2]*y[12] + br[i][6]*y[4]*y[6] + br[i][7]*y[4]*y[8] + br[i][8]*y[4]*y[lO] + br[i][9]*y[4]*y[12] + br[i][lO]*y[6]*y[S] + br[i][ll]*y[6]*y[lO] + br[i][12]*y[6]*y[12] + br[i][13]*y[8]*y[10] + br[i][14]*y[8]*y[12] + br[i][l5]*y[lO]*y[12] + CR[i][l]*y[2]*y[2] + CR[i][2]*y[4]*y[4] + CR[i][3]*y[6]*y[6] + CR[i][4]*y[8]*y[S] + CR[i][5]*y[lO]*y[lO] + CR[il[61*~[l21*y[l21 + gr[il 1; //confusion also wrong m2 ylsq. y2sq. for CR dydx[j]=-(br[i][l]*y[l]*y[3! + br[i][2]*y[l]*y[5] + br[il[3I*y[ll*y[T + br[il[4I*y[ll*y[91 + br[il[5l*y[lI*y[lll + br[i1[61*~[31*y[51 + br[i1[7l*y[3l*y[T + br[il[8l*y[3l*y[9] + br[i][9]*y[3]*y[ll] + br[i][lO]*y[S]*y[7] + br[i][ll]*y[S]*y[9] + br[i][l2]*y[5]*y[ll] + br[i][l3]*y[7J*y[9] + br[i][14]*y[7]*y[ll] + br[i][lS]*y[9]*y[ll] + CR[i][l]*y[l]*y[l] + CR[i][2]*y[2]*y[2] + CR[il[31*~[31*~[31+ CR[i1[41*~[41*~[41+ CR[i1[~1*y[~I*y[51+ CR[i1[6l*y[61*y[61 + gr[il 1; 11 printf("dydx-first %f %f %f %f %f %f\n",dydx[2],dydx[4],dydx[6],dydx[8],dydx[l0],dydx[l2]); APPENDIX D

I* for(i= 1;i < =6;i+ +) printf("dydx matrix yy %f %f %f %f %f %nn",yy[i][l],yy[i][2],yy[i][3],yy[i][4],yy[i][S],yy[i][6]); *I

void ludcmp(a,n,indx,d) int n,*indx; float **a,*d; { int i,imax j,k; float big,dum,sum,temp; float *w,*vector(); void nrerror(),free_vector();

w =vector(l ,n); *d = 1 .O; for(i=l;i< =n;i++){ big=0.0; for(i=lG< =n$++) if ((temp = fabs(a[i][i])) > big) big = temp; if (big = = 0.0) nrerror("Singu1ar matrix in routine LUDCMP"); w[i] = 1 .Olbig; 1 forc=l;i<=n;j++) { for(i=l;i = big) { big=dum; imax =i; 1 1 if (i ! = imax) { for(k=l;k< =n;k++) { dum =a[imax]F]; a[imaxlRl =a[ilFI; a[i]F] =durn; 1 *d = -(*d); w[iax]=w[i]; 1 indx[i] =imax; if (a[i][i] = = 0.0) a[i][i]=TINY; if(i ! = n) { durn= l.O/(a[i][i]); APPENDIX D

void lubksb(a,n,indx,b) float **a,bO; int n,*indx; { int i,ii=O,ip J; float sum; for(i=l;i< =n;i++) { ip=indx[i]; sum =b[ip]; b[ip] =b[i]; if (ii) for(i=ii;i < =i-l;i+ +) sum -= a[i]fi]*bfi]; else if (sum) ii=i; b[i] =sum; 1 for(i=n;i > =I$-){ sum=b[i]; for(j=i+l;i< =n;i+ +) sum-= a[i]fi]*bfi]; b[i] =sumla[i][i]; APPENDIX D

D-5 Computer Program to Verify the Equations of Motion (Using "Conservation of Energy Approach") and find the computer processor time taken for the Approximate Model

#include < stdi0.h > #include < math. h > #include #include < time.h >

I* Defining factorizations for trignometric terms that are functions of joint angles *I

I* Defining fixed link parameters as per Denavit-Hartenberg notation *I

#define TINY 1.0e-20; #define N (6) /***for 6x6 "A" ie to find inv. of "ap" matrix as "yy" matrix *********I

float **a,**yy,d,*col; int i j,*indx;

float theta[A; I* (6x1) Vector of Joint Angles 8,,8,, ..., 8, *I float ap[7l[7l,bp[7l[16],CP[7l[l,gp[7]; I* A(6x6), B(6x15), and C(6x6) matrices and the g vector *I

float **y,*xx=O;

float vel[7],posn[7]; I* Vector of Joint Velocities and Joint Positions *I APPENDIX D

float kecons,pecons,totenrgy; I* The Scalars Kinetic Energy, Potential Energy, and the Total Energy *I

main() { void apmat(); I* Returns the expressions giving the elements of the Kinetic Energy matrix *I void bpmat(); I* Returns the expressions giving the elements of the Coriolis matrix *I void CPmat(); I* Returns the expressions giving the elements of the Centrifugal matrix *I void gpmato; I* Returns the Gravity vector *I

void rkl(),rkdumb(); I* Runge-Kutta Integration *I float vstart[l3]; //,dydx[l3],yout[l3]; int nvar,nstep; float x1 ,x2; void (*derivs)(); void derivative(); I* Returns dy/dx *I

void ludcmp(),lubksb(); float **matrix(); float *vector();

int k; float iden[l[l;

clock-t start,end; I* Defining system clock variables *I time-t first,second; I* Defining system time variables *I

start = clock(); I* get the no. of system clock ticks per second *I first=time(NULL); I* get the system time *I

derivs=&derivative; I* Passing the address of deivativeo as an argument *I

x1=0.0; I* Range *I x2=1.0; nvar=12; I* Number of Variables *I nstep= 10; I* Number of Steps *I

/* Defining Boundary Conditions and Initial Conditions *I APPENDIX D APPENDIX D

totenrgy =0.0;

for(i=l;i< =6;i+ +) for(j=l;i<=6;i++) kecons = kecons + O.S*(veI[i]*ap[i]fi]*vel~]);

printf(" kecons = %finN,kecons); pecons =9.81*(17.40*(-sin@osn[2])*0.068-cos(~ososn[2]+posn[3])*-0.070sin(posn[2])*a2) + 0.82*(cos@osn[2]+posn[3])*-0.019 -sin(posn[2] +posn[3])*a3 + cos(posn[2]+posn[3])*d4 -sin@osn[2])*a2) + 0.34*(-sin(posn[2] +posn[3])*a3 +cos(posn[2] +posn[3])*d4 -sin(posn[2])*a2) + 0.09*(-sin(posn[2] +posn[3])*cos(posn[4])*sin(posn[5]) + cos(posn[2] +posn[3])*cos(posn[5])*0.032 -sin@osn[2] +posn[3])*a3 + cos(posn[2] +posn[3])*d4 -sin(posn[2])*a2) ); printf(" pecons = %finN,pecans);

totenrgy =kecons + pecons; printf("totenrgy = %finu,totenrgy);

end = clock(); I* again get the system clock time after the running of processor *I second=time(NULL); /* again get the system time after the running of processor */

printf("Time elapsed as per system clock was : %f secs.\n",(end-start)lCLK-TCK); printf("The elapsed time by diffiime is %f secs.\n",difRime(second,first));

return; 1

void apmat(ap) float apt7)[7); { int ij; APPENDIX D

return; 1

void bprnatpp) float bp[7[16]; { int ij; APPENDIX D APPENDIX D

for (i= l;i< =6;i+ +) {printf("br %f %f %f %f %f %f %f %finw .bp[i][l],bp[i][2],bp[i][3],bp[i][4],bp[i][S],bp[i][6],bp[i][q,bp[i][8]); printf(" %f %f %f %f %f %f %fin",bp[i][9],bp[i][lO],bp[i][ll],bp[i][l2],bp[i][l3],bp[i][l4],bp[i][l5]);} Ilprintf("frorn br brll= %finU,bp[l][l]);

return; 1 void CPrnat(CP) float CP['TJ[T; { APPENDIX D

extern float bp[7][16];

//printf("from inside CR brll = %finH ,bp[l][l]);

for(i=l;i<7;i++) p~tf("CR%f %f %f %f %f %fin",CP[i][l],CP[i][2],CP[i][3],CP[i][4],CP[i][S],CP[i][6]); return; APPENDIX D

void gpmat(gp) float { int i; gp[lI= (0.0); gp[2]= (-37.2*c2 - 8.4*s23 + 1.02*s2 ); gp[3] = (-8.4*s23 + 0.25*c23 ); gp[4]= ( 2.8e-O2*~23*s4*sS ); gp[5] = ( -2.8e-02*(~23*~5+ s23*c4*c5) ); gp[61= (0.0);

return; 1

void rk4(y,dydx,n,x,h,yout,derivs) float yD,dydxu,x,h,yout[l; void (*derivs)();

int n; { int i; float xh,hh,h6,*dym,*dyt,*yt,*vector(); void freevector();

dym=vector(l,n); dyt=vector(l ,n); yt=vector(l ,n); hh=h*O.S; h6=h16.0; xh=x+hh; for (i= l;i< =n;i+ +) yt[i]=y[i]+hh*dydx[i]; (*derivs)(xh,yt,dyt); for (i=l;i< =n;i+ +) yt[i]=y[i]+hh*dyt[i]; (*derivs)(xh,yt,dym); for (i=l;i< =n;i++):{ yt[i] =y[i] + h*dym[i]; dym[i] + = dyt[i]; 1 (*derivs)(x + h,yt,dyt); for (i=l;i< =n;i++) yout[i] =y[i] +h6*(dydx[i] +dyt[i] +2.0*dym[i]); free-vector(yt, 1 ,n); free-vector(dyt,l ,n); free_vector(dym,l ,n); 1

float *vector(nl,nh) int n1,nh; APPENDIX D

{ void nrerror(); float *v; v = (float *)malloc((unsigned) (nh-nl+ l)*sizw f(float)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free-vector(v,nl,nh) float *v; int nl,nh; { free((char*) (v + nl)); 1

void nrerror(error-text) char error-textn; { void exit(); fp~tf(stderr,"NumericalRecipies run-time error .. .\nW); fp~tf(stderr,"%s\nW ,error-text); fp~tf(stderr,"... now exiting to system.. .\nV); exit(1); 1

void rkdumb(vstart,nvar,xl ,x2,nstep,derivs) int nvar,nstep; float vstartn,xl ,a; void (*derivs)(); { int i,k; float x,h; float *v,*vout,*dv,*vector(),**matrix(); void rk4(),nremr(),freeeevector(); float kemat[7]; APPENDIX D

for (k=l;k< =nstep;k++) { (*derivs)(x,v,dv); rk4(v,dv,nvar,x,h,vout,derivs); if ((float)(x+h) = = x) nrerror("Step size too small in routine RKDUMB"); x +=h; xxF+ 1]=x;

for (i=l;i < =nvar;i+ +) { v[i] =vout[i]; y[i]k+l]=v[i];

float **matrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch; { int i; float **m;

m =(float **) malloc((unsigned) (nrh-nrl + l)*sizeof(float*)); if (!m) nrerror("al1ocation failure 1 in matrix()"); m -=nrl;

for(i=nrl;i < =nrh;i+ +) { m[i] =(float *) malloc((unsigned) (nch-ncl+ l)*sizeof(float)); if (!m[i]) nrerror("allocation failure 2 in matrix()"); rn[i] -=ncl; 1 return rn; 1

I* This evaluates 8 = A-'(7 - ~(8)(88) - ~(8)(8') - ge)), which is integrated by RK-4 method to yield j o i n t velocities i.e., 8 and joint positions i.e., 8 *I

void derivative(x,y,dydx) float x,yfl,dydx[; { void apmat(); void bpmat(); void CPmat(); void gpmat();

int i j; APPENDIX D

for(i=lj=2;i< =2*N-1 j< =2*N;i+ + j=j+2) d~dx[il=-(~p~il[ll*y[21*y~41+ bp[i1[21*~[21*~~61 + b~[i1[31*~[21*~[81 + bp[i1[4l*y[2I*y[101 + bp[i1~5l*y[2I*y[121 + b~[~l[61*~~41*~[61+ bp[il[7*~[41*~[81 + bp[i1[81*~[41*~[101+ bp[i1[9l*y[41*y[121 + bp[i1[10l*y[61*y[81 + b~[il[~~1*~[61*~[101+ bp[i1[121*~[61*~[121 + bp~i1[131*y[81*~~101+ bp[i1[14l*y[81*y[121 + bp[il[l5l*y[l01*y[l21 + CP[il[ll*y[21*y[21 + CP[i1[2]*y[41*y[41 + CP[i][3l*y[6]*y[6] + CP[i][4]*y[8]*y[8] + CP[i][5]*y[lO]*y[lO] + CP~il[6l*~[l21*yIl21+ m[il ); //confusion also wrong m2 ylsq. y2sq. for CR dydxfi]=-(bp[i][l]*y[l]*y[3] + bp[i][2]*y[l]*y[5] + ~P[~II~I*Y[~I*Y[~I+ bp[il[4l*y[l1*~[91 + bp[ilI5l*~[ll*~[lll+ bp[i116l*y[31*y[51 + bp[il[7*y[31*y[7 + b~[il[81*~[31*~[91+ bp[il[91*~~3I*y[lll + bp[il[l01*~[51*~[7l+ ~p[il[llI*y[51*y[91+ bp[il[l2l*y[5]*y[l1] + b~[i1[131*~[71*~[91+ bp[i1[14l*y[7I*y~~ll + bp[il[l51*y[91*y[lll + CP[il[ll*y[ll*y~ll+ CP[i1[2I*y[21*y[21 + CP[il[31*~[31*~[31+ CP[i1[41*~[41*~[41+ CP[i1[51*~[51*~[51+ CP[i1[6l*y[61*y[61 + ~[il1; /*d~dx~2l=-@~[~l[ll*~[l1*~[31+ b~[11[~1*~[~1*~[51+ bp[ll[3I*y[ll*y[7 + bp[11[4I*y[ll*y[91 + b~[ll[5l*~[ll*~[lll+ b~[~1[61*~[31*~~~1 + bp[~l[71*~~31*~[7l + ~p[11[81*y[31*~[91 + bp[11[91*y[3l*y[lll + b~[11[101*~[51*~[7+ ~p~11[1~1*~[51*~[91 + bp[11[121*y[51*~[~11 + bp[ll~l31*y[~*y[91+ bp[11[14l*y[7l*y[lll + b~[11[1~1*~[9I*y[l~l+ CP[11[~1*~~ll*~[ll + CP~~I[~I*Y[~I*~[~I + CP[11~3l*y[31*y[31 + CP[11[4l*y[41*y[41 + CP[11[51*~[5I*y[Sl+ CP[11[61*~[61*~[61+ gp[lI 1; d~dx~41=-@~[21~11*~~~1*~~31+bp[21[2I*~[ll*y[51+ bp[21[3I*y[ll*y[~ + bp[21[4I*y[ll*y[91 + bp[21[5I*y[ll*y[111 + b~[21[61*~[31*~[~1+ bp[21~71*~[31*y~71 + bp[21~~1*~[31*~[91 + bp[21[9l*y[3l*y~lll + bp[21[10l*y[5l*y[7l + b~[21[111*~[51*~[91+ bp[21[121*~[51*y[l11 + bp[21[131*y[~*y[91+ bp[21[141*y[7I*y[lll + bp[21[15l*y[91*y[lll + CP~21[~l*y[~l*y[ll+ CP~21[21*~[21*~[21 + CP[2113l*y[31*y[31 + CP~21[4l*y[41*y[41+ CP[2][5l*y[5]*y[5] + CP[21[61*~[6I*y[61 + m[21 ); d~dx[61=-@~[31[11*~[l1*~~31+b~[31[2I*~[ll*yI51 + bp[31[3I*y[ll*y[7 + bp[31[4I*y[ll*y[91 + bp[31[5I*y[ll*y[111 + b~~31[61*~[31*~[51+ bp[31~71*~~31*y[71 + bp[31181*y~31*~[91 + bp[31[9l*y[3l*y[lll + bp[31[10l*y[5l*y[7l + ~P[~I[~~I*Y[~I*Y~~I+ b~[31~~21*~151*~I111 + ~p[31~131*~~71*~[91 + bpI31[14I*y[T*y[lll + bpI31[15I*y[91*y[lll + cp~31~~l*y[~l*~[ll+ CP[31[2l*y[21*y[21 + CPI31[3l*y[31*y[31 + CP[31[4l*y[4I*y[41 + CP[3][5l*y[5]*y[5] + CP[31[61*~[61*~[61+ gpI31 1; d~dx[8l=-(b~[41~1l*~[ll*y~3l+bp[41[2I*y[ll*y~51+ bp[41~3I*y~ll*y[71 + bp[41[4l*y[lI*y[91+ bp[41[5l*y[lI*y[111 + b~[41[63*~[31*~[51+ bp[41[7*~[31*y[71 + bp[41[8l*y[31*y[91 + bp[41[9l*y[3l*y[lll + bp[4][10l*y[5]*y[7l + b~[41[111*~[51*~[91+ bp[41[12l*y[5I*y[lll + bp[41[13l*y[71*y[91 + bp[41[141*y[7*y[l11 + bp[4][15]*y[91*y[l11 + CP[41[~I*y[ll*y[ll + CP[41[2l*y[21*y[21 + CP[41[3l*y[31*y[31 + CP[41[4]*y[4I*y[41 + CP[4][5]*y[S]*y[5] + CP[~I[~I+Y[~~*Y[~I+ m[41 ); d~dx[l0l=-(b~[51[1l*~[l1*~[31 + bp[~1I~I*y~ll*y~51+ bp[51[3l*y[l]*y[71 + bp[5][4]*y[l]*y[9] + b~[~l[~l*~[ll*~[lll+ bp[~1[61*y[31*~[51 + bp[51[7l*y[31*y[71 + bp[51[8l*y[3I*y[91 + bp[5][9]*y[3]*y[ll] + b~[~l[loI*~[~l*~[71+ bp[51[11l*y[51*y[91 + bp[511121*y[5l*y[ll] + bp[51[13]*y[7l*y[91 + bp[5][14]*y[7l*y[ll] + b~[~1[1~1*~[~l*~[~ll+ CP~5I[1l*y[lI*y[ll + CP[51[2l*y[21*y[21 + CP[5][31*y[3]*y[3] + CP[5][4]*y[4]*y[4] + CP[51[~1*~~~1*~[51+ CP[51[6l*y[61*y[61 + gp[51 ); d~d~~l21=-(~~~61[~l*~[l1*~[31+ bp[61[2I*y~ll*y[51 + bp[61[3l*y[ll*y[7l + bp[6][4]*y[l]*y[9] + ~P[~I[~I*Y[~I*Y[~11 + bp[61[6l*yI31*y[51 + bp[61[7l*y[3]*y[7] + bp[6][8]*y[3]*y[9] + bp[6][9]*y[3]*y[l I] + b~[61[101*~[51*~[7+ bp[61[111*y[51*~[91 + bp[61[12l*y[5l*y[lll + bp[6][13]*y[q*y[9] + bp[6][14]*y[~*y[ll] + b~[61[151*~[91*~[lll+ CP[61[1l*~[ll*~[ll + CP[61[21*y[2]*y[21 + CP[6][3]*y[3]*y[3] -+ CP[6][4]*y[4]*y[4] + CP~~~[~I*Y[~I*Y[~I+ CP[61[61*~[61*y[61 + gp[61 );*I APPENDIX D 120

void ludcmp(a,n,indx,d) int n,*indx; float **a,*d; { int i,irnax j,k; float big,dum,sum,temp; float *w,*vector(); void nrerror() ,free-vector();

w=vector(l ,n); *d=1.0; for(i=l;i< =n;i++){ big=0.0; for(i=l;j<=n;i++) if ((temp = fabs(a[i]G])) > big) big= temp; if (big = = 0.0) nrerror("Singu1ar matrix in routine LUDCMP"); w[i] = 1 .O/big; 1 for(j=l;j<=nj++) { for(i=l;i = big) { big=dum; imax = i; 1 1 if (i ! = imax) { for(k=l;k< =n;k+ +) { durn =a[imax][k]; a[imax]F] =ab][k]; APPENDIX D

a[i][k] =durn; 1 *d=-*( dl; w[imax] =w[i]; 1 indx[i] = imax; if (a[i][i] = = 0.0) a[i][i] =TINY; if6 != n) { dum= 1.O/(a[i][i]); for(i=j+l;i < =n;i+ +) a[i][i] *= durn; } 1

void lubksb(a,n,indx,b) float **a,bD; int n,*indx;

int i,ii=O,ip J; float sum;

for(i= 1;i < =n;i+ +) ( ip = indx[i]; sum =blip]; b[ip]=b[i]; if (ii) for(i=ii;i < =i-l;i+ +) sum -= a[i][i]*b[i]; else if (sum) ii=i; b[i] =sum; APPENDIX D 122

D-6 Computer Program to get Joint Motor Torque($ for Controlling the Robot based on the Real Model

#include < stdio .h > #include < math.h > #include < std1ib.h >

I* Defining factorizations for trignometric terms that are functions of joint angles *I

I* Defining Expressions for the Inertial Constants Appearing in the Equations of Forces of Motion *I

#define ill (-0.0142) #define i12 (-0.0110) #define i13 (-3.79e-03) #define i14 ( 1.64e-03) #define il5 (1.25e-03) APPENDIX D

#define iml (1.14) #define im2 (4.71) #define im3 (0.827) #define im4 (0.2000) #define im5 (0.179) #define im6 (0.193)

I* Defining Expressions for the Gravitational Constants Appearing in the Equations of Forces of Motion *I

#define ggl (-37.2) #define gg2 (-8.44) #define gg3 (1.02) #define gg4 (0.249) #define gg5 (-0.0282 )

I* Defining fixed link parameters as per Denavit-Hartenberg notation */

int i j; float theta[T; I* (6x1) Vector of Joint Angles 0,,8,, ..., 8, *I float ar[q[q,br[q[l6],CR[7][7],gr[q;I* A(6x6), B(6x15), and C(6x6) matrices and the g vector *I float coeffl41; I* Vector of Cubic Coefficients *I float tho,thf,thdo,thdf,tf; /* go, e,, eo, B,, t, *I float thdot[q,thdbldt[q; I* (6x1) Vector 8 and (6x1) Vector 8' */ float t; I* Time variable */ float qq[l6],qsquare[7'J; I* (1x15) Vector of velocity products i.e., e*6, and (1x6) Vector of squared velocities i.e., e2 *I float term1 [7],term2[7J,term3[7]; float torque[T; I* (6x1) Torque Vector *I

main0 { void armat(); I* Returns the expressions giving the elements of the Kinetic Energy matrix *I APPENDIX D

void brmat(); I* Returns the expressions giving the elements of the Coriolis matrix *I void CRmat(); I* Returns the expressions giving the elements of the Centrifugal matrix *I void armat(); I* Returns the Gravity vector *I

float **matrix(); float *vector();

void cubic-coeff(); I* Returns the Vector of cubic coefficients *I int k; APPENDIX D

I* Evaluating individual terms of the state space equation i.e., A@)&) + B@)(B~)+ c(B)@) + g@)) = 7 *I

return; 1

void armat(ar) APPENDIX D

float ar[q[q; { int i j;

return; 1

void brmat(br) APPENDIX D

float br[i'l[16]; { int ij; APPENDIX D 128 APPENDIX D

for (i=l;i<=6;i++) {printf("br %f %f %f %f %f %f %f %fin",br[i][l],br[i][2],br[iJ[3],br[i][4],br[i][5],br[i][6],br[i][7,br[i][S]); printf(" %f %f %f %f %f %f %fin",br[i][9],br[i][l0],br[i][ll],br[i][l2],br[i][l3],br[i][l4],br[i][l5]);} Ilprintf("from br brll = %finM ,br[l][l]); return; 1 void CRmat(CR) float CR[A[T; I int i J; extern float br[71[16];

Ilprintf("from inside CR brll = %fin" ,br[l][l]); APPENDIX D

for(i=l;i<7;i+ +) printf("CR %f %f %f %f %f %flnw,CR[i][l],CR[i][2],CR[i][3],CR[i][4],CR[i][5],CR[i][6]); for (i=l;i<=6;i++)

return; 1

void grmat(gr) float gr[l; { int i; gr[l] = (0.0); gr[2]= (ggl*c2 + gg2*s23 + gg3*s2 + gg4*c23 + gg5*(s23*c5 + c23*c4*s5) ); gr[3]= (gg2*s23 + gg4*c23 + gg5*(s23*c5 + c23*c4*s5) ); gr[4] = (-gg5*s23*s4*s5 ); gr[5] = (gg5*(c23*s5 + s23*c4*c5) ); gr[6] = (0.0);

return; 1

void cubic-coeff(coeff) float coeffl41; { int i:

coefflo] =tho; coeffll] =thdO; coeffl'] =3.0/(tPctf)*(thf-tho)-2.0ltprthdO-1 .O/tf*thdf; coeff(3]=-2.O/(tf*tf*tf)*(thf-tho)+ 1.O/(tPctf)*(thdf+thdO); APPENDIX D

return; }

float *vector(nl,nh) int nl,nh; I void nrerror(); float *v; v= (float *)malloc((unsigned) (nh-nl + l)*sizeof(flaat)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free_vector(v,nl,nh) float *v; int n1,nh; { free((char*) (v +nl)); 1

void nrerror(error-text) char error-textu; { void exit(); fprintf(stderr,"Numerical Recipies run-time error ...\n"); fprintf(stderr," %s\nW,error-text); fprintf(stderr," ...now exiting to system ...\nW); exit(1); 1

float **matrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch; { int i; float **m;

m=(float **) malloc((unsigned) (nrh-nrl+ l)*sizmf(float*)); if (!m) nrerror("allocation failure 1 in matrix()"); m -=nrl;

for(i=nrl;i< =nrh;i+ +) { m[i] = (float *) malloc((unsigned) (nch-ncl + I)*sizeo f(float)); if (!m[i]) nrerror("a1location failure 2 in matrix()"); m[i] - =nci; 1 return m; 1 APPENDIX D 132

D-7 Computer Program to get Joint Motor Torque(s) for Controlling the Robot based on the Approximate Model

/* Defining factorizations for trignometric terms that are functions of joint angles */

I* Defining fixed link parameters as per Denavit-Hartenberg notation *I

int ij; float theta[q; /* (6x1) Vector of Joint Angles B,,O,, ..., 8, */ float ap[7[l,bp[l[l61,CP[71[7],gp[7;I* A(6x6), B(6x15), and C(6x6) matrices and the g vector */ float coeffl41; /* Vector of Cubic Coefficients *I float thO,thf,thdO,thdf,tf; I* B,, B,, 8,, O,, I,*/ float thdot[q9thdbldt[7j; I* (6x1) Vector 9 and (6x1) Vector d */ float t; I* Time variable */ float qq[l6],qsquare[i'l; I* (1x15) Vector of velocity products i.e., 8*8, and (1x6) Vector of squared velocities is., e' *I float term1 [7j ,term2[7] ,term3[7]; float torque[q; I* (6x1) Torque Vector *I APPENDIX D

{ void apmat(); I* Returns the expressions giving the elements of the Kinetic Energy matrix *I void bpmat(); I* Returns the expressions giving the elements of the Coriolis matrix *I void CPmat(); I* Returns the expressions giving the elements of the Centrifugal matrix *I void @mat(); I* Returns the Gravity vector *I

float **matrix(); float *vector();

void cubic-coeff(); I* Returns the Vector of cubic coefficients *I int k; APPENDIX D

I* Evaluating individual terms of the state space equation i.e., A(B)(~) + B(B)(B~)+ c@@) + go)) = 7 *I

return; }

void apmat(ap) float ap[T[71; { int ij; APPENDIX D

return; 1

void bpmat@p) float bp[T[16]; { int ij; APPENDIX D APPENDIX D

for (i=l;i< =6;i++) {p~tf("br%f %f %f %f %f %f % f %nn",bp[i][l],bp[i][2],bp[i][3],bp[i][4],bp[i][5],bp[i][6],bp[i][~,bp[i][8]); printf(" %f %f %f %f % f % f %fin",bp[i][9],bp[i][lO] ,bp[il(l 1] ,bp[i][l2],bp[i][l3] ,bp[i][l4],bp[i][l5]);} //printf("from br brll= %fln",bp[l][I]);

return; 1

void CPmat(CP) float CP[7][71; { int ij; APPENDIX D

extem float bp[7j[16];

Ilprintf("from inside CR brll = %t\n",bp[l][l]);

for(i=l;i<7;i+ +) printf("CR %f %f %f %f %f %An",CP[i][l],CP[i][2],CP[i][3IICP[i][4],CP[i][5],CP[i][6]); return; APPENDIX D

void gpmat(gp) float gp[7); { int i; @[I]= (0.0); gp[2]= (-37.2*c2 - 8.4*~23+ 1.02*s2 ); gp[3]= (-8.4*s23 + 0.25*c23 ); gp[4]= ( 2.8e-02*~23*~4*~5); gp[5] = ( -2.8e-O2*(c23*sS + s23*c4*c5) ); a[61= (0.0);

return; 1

void cubic-coeff(coeff) float coeffl4J; { int i:

coefflo] =tho; coeffll] = thd0; coeffl21 =3.0/(tPtt)*(thf-th0)-2.0ItrthdO-1.O/tPLthdf; coeffl3]=-2.0/(tPtPtf)*(thf-thO)+ 1.O/(tPtf)*(thdf+ thd0);

return; 1

float *vector(nl,nh) int n1,nh; { void nrerroro; float *v; v= (float *)malloc((unsigned) (nh-nl+ l)*sizeof(float)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free_vector(v,nl,nh) float *v; int n1,nh; APPENDIX D

void nrerror(error-text) char error-textn; { void exit(); fprintf(stderr,"Numerical Recipies run-time error ...\nu); fprintf(stderr," %s\nW,error-text); fprintf(stderr," ...now exiting to system ...\nW); exit(1); }

float **matrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch;

int i; float **m;

m=(float **) malloc((unsigned) (nth-nrl+ l)*sizeof(float*)); if (!m) nrerror("allocation failure 1 in matrix()"); m -=nrl;

for(i=nrl;i < =nrh;i+ +) { m[i] = (float *) malloc((unsigned) (nch-ncl+ l)*sizeof(float)); if (!m[i]) nrerror("allocation failure 2 in matrix()"); m[i] -=ncl; } return m; 1 APPENDIX D 141

D-8 Computer Program to get Forces(s) on the End Effector based on the Real Model

I* Defining factorizations for trignometric terms that are functions of joint angles *I

/* Defining Expressions for the Inertial Constants Appearing in the Equations of Forces of Motion */

#define ill (-0.0142) APPENDIX D

I* Defining Expressions for the Gravitational Constants Appearing in the Equations of Forces of Motion *I

#define ggl (-37.2) #define gg2 (-8.44) #define gg3 (1.02) #define gg4 (0.249) #define gg5 (-0.0282 )

I* Defining fixed link parameters as per Denavit-Hartenberg notation *I

int ij; float thetam; I* (6x1) Vector of Joint Angles 8,,8,, ..., 0, *I float ar[T[q,br[T[16I9CR[T[T,gr[71;I* A(6x6), B(6x15), and C(6x6) matrices and the g vector *I float coeffl41; I* Vector of Cubic Coefficients *I float thO,thf,thdO,thdf,tf; /* 8,, 6',, e,, e,, t, *I float thdot[q,thdbldt[71; I* (6x1) Vector 8 and (6x1) Vector e' *I float t; I* Time Variable *I APPENDIX D 143 float qq[l6],qsquare[q; I* (1x15) Vector of velocity products i.e., 8*8, and (1x6) Vector of squared velocities i.e., e2 *I float term1 [7],term2[7],term3[7; float torque[q; /* (6x1) Torque Vector */

FILE *fpwrite.; int count;

main() { void amat(); I* Returns the expressions giving the elements of the Kinetic Energy matrix *I void brmat(); I* Returns the expressions giving the elements of the Coriolis matrix *I void Chat(); I* Returns the expressions giving the elements of the Centrifugal matrix *I void grmat(); I* Returns the Gravity Vector */

float **matrix(); float *vector();

void cubic-coeff(); I* Returns the Vector of cubic coefficients *I int k;

I* Evaluating individual terms of the state space equation i.e., A@)&) + B(B)(B~)+ c@)@) + g(B)) = 7 *I APPENDIX D APPENDIX D

if( (fpwrite=fopen("rlrnjfrc5.dat","w")) = = NULL ) {printf("Erroropening output data file rlrnjfrc5.dat\nW); exit(0); 1

return; }

void armat(ar) float ar[7][7]; { int i j;

ar[l][S]= (irnl +il +i3*cc2 +i7*ss23 +ilO*sc23 +i1 1*sc2 +i2O*(ss5*(ss23*(1 +cc4)-1)-2.O*sc23*c4*sc5) + i21*ss23*~~4+2.O*(i5*~2*~23+i12*c2*c23 +i15*(ss23*c5 +sc23*c4*s5)+i 16*c2*(s23*c5 +c23*c4*s5) +il8*s4*s5 +i22*(sc23*c5 +cc23*c4*s5)) ); ar[1][2]= ( i4*s2 + i8*c23 + i9*c2 +i13*sc23 -i15*c23*s4*s5 + i16*s2*s4*s5 + i18*(~23*~4*~5-~23*~5)+ i19*s23*sc4 + i2O*s4*(s23*c4*cc5+c23*sc5) + i22*s23*s4*s5 ); ar[1][3] = (i8*c23 + i13*s23 - i15*c23*s4*s5 + i19*s23*sc4 + i18*(s23*~4*s5-~23*~5)+ i22*s23*s4*s5 + i20*s4*(s23*c4*cc5 +c23*sc5) ); ar[1][4]= (i14*c23 + il5*s23*c4*sS + i16*c2*c4*s5 + ilS*c23*s4*s5 - i20*(s23*c4*sc5 + c23*ss5) + i22*c23*c4*s5 ); ar[1][5] = (iSS*s23*~4*cS+ i16*c2*s4*cS + i 17*s23*s4 + i18*(~23*s5-~23*~4*~5)+ i22*c23*s4*c5 ); ar[S ][6] = (i23*(~23*~5-~23*~4*~5)); APPENDIX D

return; 1

void brmat(br) float br[7][16]; { int ij; APPENDIX D APPENDIX D

for (i=l;i<=6;i+ +) {p~tf("br%f %f %f %f %f %f %f %fin",br[i][l],br[i][2],br[i][3],br[i][4],br[i][5],br[i][6],br[i][~,br[i][8]); printf(" %f %f %f %f %f %f %8onn",br[i][9],br[i][lO~,br[i][ll],br[i][l2],br[i][l3],br[i][l4],br[i][l5]);} //p~tf("frombr brl 1 = %80nnn,br[l][l]); return; 1 void CRrnat(CR) float CR[7[7]; { int ij; extern float br[7[16]; APPENDIX D

//printf("frorn inside CR brll = %fin",br[l][l]);

for(i=l;i<7;i+ +) printf("CR %f %f %f %f %f %f\n",CR[i][l],CR[i][2],CR[i][3],CR[i][4],CR[i][S],CR[i][6]); for (i=l;i< =6;i+ +)

return; 1

void gnnat(gr) APPENDIX D

float gr[7]; { int i; grill= (0.0); gr[2]= (ggl*c2 + gg2*s23 + gg3*s2 + gg4*c23 + ggS*(s23*cS + c23*c4*s5) ); gr[3]= (gg2*s23 + gg4*c23 + ggS*(s23*c5 + c23*c4*sS) ); gr[4] = (-ggS*s23*s4*sS ); gr[S] = (ggS*(c23*s5 + s23*c4*cS) ); gr[6] = (0.0);

return; 1

void cubic-coeff(coeff) float coeffl41; { int i;

coefflo] =tho; coeffll] =thdO; coeffl2]=3.01(tf*tt)*(thf-th0)-2.0ltPthdO-1 .O/tf*thdf; coeffl3]=-2.0l(tf*tf*tf)*(thf-tho) + 1.Ol(tf*tf)*(thdf+thdO);

return; 1

float *vector(nl,nh) int n1,nh; { void nrerroro; float *v; v =(float *)malloc((unsigned) (nh-nl+ l)*sizeof(float)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free_vector(v,nl,nh) float *v; int nl,nh; I free((char*) (v + nl));

void nrerror(error-text) char error-textu; APPENDIX D I void exit(); fp~tf(stderr,"NumericalRecipies run-time error .. .\nW); fp~tf(stderr,"%An" ,error-text); fprintf(stderr," ...now exiting to system.. .\nV); exit(1); 1

float **matrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch; { int i; float **m;

m=(float **) malloc((unsigned) (nrh-nrl+ l)*sizeof(float*)); if (!m) nrerror("a1location failure 1 in matrix()"); m -=nrl;

for(i=nrl;i < =nrh;i+ +) { m[i] =(float *) malloc((unsigned) (nch-ncl + l)*sizeof(float)); if (!m[i]) nrerror("allocation failure 2 in matrix()"); m[i] -=ncl; 1 return m; 1

#include < stdi0.h > #include < math. h > #include < std1ib.h >

I* Defining factorizations for trignometric terms that are functions of joint angles *I

I* Defining futed link parameters as per Denavit-Hartenberg notation *I APPENDIX D

#define TINY 1.0e-20; #define N (6) /***for 6x6 "jactrans" ie to fid inv. of "jactrans" matrix as "jactrinv" matrix *********I

FILE *fpread; float **a,**jactrinv,d,*coI; int *indx; float **jcinvrse;

float theta[q; I* (6x1) Vector of Joint Angles 8,,8,, ..., 0, *I float thdot[q; I* (6x1) Vector 8 *I int i j,k; float jac[q[q; I* (6x6) Jacobian Matrix *I float jacdot[7J[7J; I* (6x6) j matrix *I float ar[q[q,gr[q; I* A(6x6) matrix and the g(6x1) vector *I float term2[l,term3[7]; float jactrans[l[q; I* (6x6) JT *I float iden[q[q; I* (6x6) Identity matrix *I float frctempl [~,frctemp2[7J[7],frctemp3[~[7],frctemp4[~[7],frctemp5[7]; float xdotdot[~,frcterml[7J,frcterm2[7J,frcte~cterm4[7];I* xdotdot: (6x1) Vector of x *I float term23[7); float chkterm2[q,chkterm3[7'j; float force[q; I* (6x1) ForceTorque Vector acting on the End-Effector *I int count:

void ludcrnp(),lubksb();

float **matrix(); float *vector();

void jacobiano; I* Returns the (6x6) Jacobian Matrix *I void jacobdoto; I* Returns the (6x6) j Matrix *I APPENDIX D

thdot[i] =0.0; term2[i] =0.0; term3[i] =0.0; xdotdot[i] =0.0; frcterml [i] =O.O; frcterm2[i] =0.0; frcterm3[i] =0.0; frcterm4[i] =0.0; frctempl [i] =O.O; frctempS[i] =0.0; term23[i] =0.0; chkterm2[i] =0.0; chkterm3[i] =0.0; force[i] =0.0;}

I***theta[i] ,thdot[i] ,term2[i] ,term3[i] ,ar[iJfi],gr[i] will be input externaly from rlmjfr5a.c; xdotdot can be input internally****/

if( (fpread = fopen("rlmjfrcS.dat","r")) = = NULL ) {p~tf("Erroropening input data file rlmjfrcS.dat\n"); exit(0) ; }

while ( fscanf(fpread," % f % f % f % f %finw,&theta[count] ,&thdot[count] ,&term2[count] ,&term3[count] ,&gr[count]) ! =EOF) {p~tf("output-1 %f %f %f %f %f\n",theta[count],thdot[count],term2[count],term3[count],gr[count]); count + + ; )

while (fscanf(fpread,"%f %f %f %f %f %t\n",~r[count][l],&r[count][2],&ar[count][3],&ar[count][4],&ar[count][5],&ar[count][6]) !=EOF) {printf("outputZ % f %f % f % f %f %finv,ar[count][l] ,ar[count][3-],ar[count][3] ,ar[count][4] ,ar[count][5] ,ar[count][6]); count + + ; }******I APPENDIX D 154

I**** BLUFF CHECKER *******I

I* Evaluating the individual terms for the Cartesian state space equation i.e., F = J-' A@) J" x - J-' A@)J-' J 0 + J-' B(6,e) + J-' GO) *I

jacobian(jac); for(i=l;i< =6;i+ +) pMtf("jac main %f %f %f %f %f %An"jac[i][l],jac[i][2] jac[i][3] jac[i][4] jac[i][5] jac[i][6]);

jacobdot(jacd0t); for(i=l;i< =6;i+ +) printf("jacdot main %f %f %f %f %f %fin"jacdot[i][l] jacdot[i][2]jacdot[i][3] jacdot[i][4]jacdot[i][5] ~acdot[i][6]);

I**** ALITER CAN FIND jactrinv by finding jcinvrse and then taking its transpose ******I col=vector(l ,N); a=matrix(l,N,l ,N); jact~v=matrix(l,N,l,N); APPENDIX D

for(i= l;i < =6;i+ +) printf("matrix jactrinv %f %f %f %f %f %fb"jactrinv[i][l] jactrinv[i][2] jactrinv[i][] actrinv[i][4]jactrinv[i][S] jactrinv[i][6]);

ludcmp(a,N ,indx,&d); for(i=l;i<=N$++){ for(i=l;i< =N;i+ +)col[i]=O.O; collj] = 1.o; lubksb(a,N,indx,col); for(i= 1;i < =N;i+ +)jcinvrse[i]lj] =col[i]; 1

for(i=l;i< =6;i+ +) printf("matrix jcinvrse %f %f %f %f %f %f\nvjcinvrse[i][l] jcinvrse[i][2] jcinvrse[i][] jcinvrse[i][4] jcinvrse[i][S] jcinvrse[i][6]); APPENDIX D

for(i= 1;i < =6;i+ +) for(i=l;i<=6;i++) frctempl [i] = frctempl [i] + jcinvrse[i][i]*xdotdot[i];

printf("frctemp1 %f %f %f %f %f %fin" ,frctempl[l] ,frctemp1[2],frctemp1[3],frctemp1[4],frctemp1[S] ,frctemp1[6]);

for(i= 1;i < =6;i+ +) for(i=l;i< =6;i++) frcterml [i] = frcterml [i] + frctemp2[i][i]*frctempl[i]; APPENDIX D

for(i=O;i < =6;i+ +) frcterm3 [i] =chkterm2[i] +chkterm3 [i] ; APPENDIX D

for(i=l;i< =6;i++) printf(" force % f\n" ,force[i]);

return; 1

void jacobian(jac) float jac[q[q; I jac[ll[l] = -sl *(c23*a3 + s23*d4 + c2*a2) -cl *(d2 fd3); jac[ll[2] = cl*(-s23*a3 + c23*d4 - s2*a2); jac[ll[3] = cl*(-s23*a3 + c23*d4); jac[1][4] = 0.0; jac[l J[5] = 0.0; jac[1][6]= 0.0;

jac[4][1]= 0.0; jac[4][2] = -s 1; jac[4][3] = -s 1; jac[4][4]= cl*s23; jac[4][5] = -cl*c23*s4 -sl *c4; jac[4][6] = (cl *c23*c4 -sl *s4)*s5 +cl *s23*c5; APPENDIX D

for(i=l;i< =6;i++) printf("jac subr %f %f %f %f % f %f\n",jac[i][l] ,jac[i][2]jac[i][3]jac[i][4],jac[i][S],jac[i][6]);

return; 1

void jacobdot(jacdot) float jacdot[q[q; { jacdot[l][l] = -cl *c23*a3*thdot[l] + sl*s23*a3*(thdot[2] + thdot[3]) -cl *s23*d4*thdot[l] -sl *c23*d4*(thdot[2] + thdot[3]) +s2*a2*thdot[2] +s 1*(d2 +d3)*thdot[l]; jacdot[l][2]= sl*s23*a3*thdot[l] -cl*c23*a3*(thdot[2]+thdot[3]) -sl*c23*d4*thdot[l] -cl*s23*d4*thdot[l] +sl*s2*a2*thdot[l] -cl *c2*a2*thdot[2]; jacdot[l][3] = sl*s23*a3*thdot[l] -cl *c23*a3*(thdot[2] +thdot[3]) -sl*c23*d4*thdot[l] -cl*s23*d4*(thdot[2] +thdot[3]); jacdot[l][4] = 0.0; jacdot[l][S]= 0.0; jacdot[l][6] = 0.0; APPENDIX D 160

for(i=l;i< =6;i+ +) printf("jacdot subr %f %f %f %f %f %f\nVjacdot[i][l] ,jacdot[i][2],jacdot[i][3],jacdot[i][4] jacdot[i][5] jacdot[i][6]);

return; 1

void ludcmp(a,n,indx,d) int n,*indx; float **a,*d; { int i,imax j,k; float big,dum,sum,temp; float *vv,*vector(); void nrerror() ,free-vector();

w =vector(l ,n); *d=1.0; for(i=l;i< =n;i+ +){ big=0.0; for(j=li< =nj++) if ((temp= fabs(a[i][i])) > big) big= temp; if (big = = 0.0) nrerror("Singu1ar matrix in routine LUDCMP"); w[i] = 1 .O/big; } for(i=l;i<=n;i++) { for(i=l;i = big) { APPENDIX D

big=dum; imax = i; 1 1 if fj != imax) { for(k=l;k< =n;k+ +) { dum =a[imax][k]; a[imax][k] =aGl[k]; afilk]=durn; } *d = -(*d); w [imax] =w fi]; 1 indxu] = imax; if (afilfi] = = 0.0) afi]u] =TINY; iffj != n) { dum= 1.Ol(ac]Gl); for(i=j+ l;i < =n;i+ +) a[i]b] *= durn; 1 1

void lubksb(a,n,indx,b) float **a,bn; int n,*indx; { int i,ii=O,ip j; float sum;

for(i=l;i< =n;i++) { ip=indx[i]; sum=b[ip]; b[ip] =b[i]; if (ii) forfj=ii;i < =i-l;i+ +) sum -= a[i]fi]*bQ]; else if (sum) ii=i; b[i]=sum; 1 for(i=n;i> =l;i-){ sum=b[i]; for(j=i+ l;i < =n;i+ +) sum-= a[i]fi]*bu]; b[i] =sumla[i][i]; 1 1

float *vector(nl,nh) int n1,nh; { void nrerror(); APPENDIX D

float *v; v=(float *)malloc((unsigned) (nh-nl+ l)*sizeof(float)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free-vector(v,nl,nh) float *v; int n1,nh; { free((char*) (v + nl)); 1

void nrerror(error-text) char error-textn;

void exit(); fp~tf(stderr,"Numerical Recipies run-time error ...\nu); fp~tf(stderr,"%s\nM ,error-text); fp~tf(stderr,"... now exiting to system ...\nV); exit(1); 1

float **rnatrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch; { int i; float **mi

m=(float **) malloc((unsigned) (nth-nrl+ l)*sizeof(float*)); if (!m) nrerror("allocation failure 1 in matrix()"); m -=nrl;

for(i=nrl;i < =nrh;i+ +) { m[i] =(float *) malloc((unsigned) (nch-ncl+ l)*sizeof(float)); if (!m[i]) nrerror("al1ocation failure 2 in matrix()"); m[i] -=ncl; 1 return m; 1 APPENDIX D 163

D-9 Computer Program to get Forces(s) on the End Effector based on the Approximate Model

I* Defining factorizations for trignometric terms that are functions of joint angles */

I* Defining fixed link parameters as per Denavit-Hartenberg notation *I

#define TINY 1.0e-20; #define N (6) /***for 6x6 "jactrans" ie to find inv. of "jnctrans" matrix as "jactrinv" matrix *********I

float **a,**jactrinv,d,*col; int *indx; float **jcinvrse; APPENDIX D float theta[T; I* (6x1) Vector of Joint Angles 0,,8,, ..., 8, *I float ap[T[7],bp[71[16l,CP[71[T,gp[7];I* A(6x6), B(6x15), and C(6x6) matrices and the g vector *I float coeffl41; I* Vector of Cubic Coefficients *I float thO,thf,thdO,thdf,tf; I* 8,. Oj, go, Oj, f, *I float thdot[T,thdbldt[T; I* (6x1) Vector 8 and (6x1) Vector e' *I float t; I* Time Variable */ float qq[l6],qsquare[7l; I* (1x15) Vector of velocity products i.e., 0*8, and (1x6) Vector of squared velocities i.e., 8' */ float term1 [7l ,term2[7 ,term3[7; float torque[7]; I* (6x1) Torque Vector *I float jac[7][7; I* (6x6) Jacobian Matrix */ float jacdot[7][T; I* (6x6) j matrix *I float jactrans[T[T; I* (6x6) JT */ float iden[7j[l; I* Identity Matrix *I float frctempl [A ,frctemp2[7][7] ,frctemp3[7][7],frctemp4[7][7],frctemp5[7]; float xdotdot[~,frcterml[7],frcterm2[7],frcterm3[~,frctenn4[7]; I* xdotdot: (6x1) Vector of x *I float term23[7]; float chkterm2[7l ,chkterm3[7l; float force[l; I* (6x1) Force-Torque Vector acting on the End-Effector */

main() { void apmat(); I* Returns the expressions giving the elements of the Kinetic Energy matrix *I void bpmato; I* Returns the expressions giving the elements of the Corioiis matrix */ void CPmat(); I* Returns the expressions giving the elements of the Centrifugal matrix */ void gpmat(); I* Returns the Gravity vector */

void ludcmp(),lubksb();

float **matrix(); float *vector();

void cubic-coeff(); I* Returns the Vector of cubic coefficients *I int k;

void jacobiano; I* Returns the (6x6) Jacobian Matrix *I void jacobdot(); I* Returns the (6x6) 1 Matrix *I APPENDIX D

frcterml[i]=O.O; frcterm2[i] =0.0; frcterm3[i] =0.0; frcterm4[i] = 0.0; frctempl [i] =0.0; frctemp5[i] =0.0; term23[i]=O.O; chkterm2[i] =0.0; chkterm3[i] =0.0; force[i] =0.0;}

I* Evaluating individual terms of the state space equation i.e., A@)($) + ~@)@9)+ c@)@)+ gp)) = 7 */ APPENDIX D

for(i=l;i< =6;i++) for(j=l$< =6;j++) term1 [i] =term1 [i] + ap[i]G]*thdbldt[i];

I**** BLUFF CHECKER *******I

I* Evaluating the individual terms for the Cartesian state space equation i.e., F = J.'A(B) J" x - J-' A(B)J-' J 6' + J-' B(B,O) + J-' G@) *I

jacobian(jac); for(i=l;i< =6;i++) printf("jac main %f %f %f %f %f %fin"jac[i][l] jac[i][2] jac[i][3] jac[i][4],jac[i][S],jac[i][6]); APPENDIX D 167

for(i=l;i< =6;i+ +) printf("jacdot main %f %f %f %f %f %fin"jacdot[i][l]jacdot[i][2] jacdot[i][3] jacdot[i][4] jacdot[i][5] jacdot[i][6]);

I**** ALITER CAN FIND jactrinv by finding jcinvrse and then taking its transpose ******/ col=vector(l ,N); a=matrix(l ,N ,1 ,N); jactrinv =matrix(l ,N, 1,N);

for(i=l;i< =6;i+ +) printf("matrix jactrinv %f %f %f %f %f %finnjactnnv[i][l] ~actrinv[i][2],jactnnv[i][3] ~actrinv[i][4]jactrinv[i][s] jactrinv[i][6]);

col =vector(l ,N); a=matrix(l ,N,1 ,N); jcinvrse=matrix(l ,N,1 ,N); APPENDIX D

for(i=l;i< =6;i++) printf("matrix jcinvrse %f %f %f %f %f %fin"jcinvrse[i][l] jchvrse[i][2] jcinvrse[i][] jcinvrse[i][4]jcinvrse[i][5] ~cinvrse[i][6]);

for(i=l;i< =6;i++) for(i=l;i< =6;i++) frcterml [i] = frcterml [i] + frctemp2[i]b]*frctempl b]; APPENDIX D APPENDIX D

return; 1

void apmat(ap) float ap[cTI[A; I int ij; APPENDIX D

return; 1

void bpmat@p) float bp[7[16]; { int ij; APPENDIX D APPENDIX D

for (i=l;i< =6;i+ +) {printf("br %f %f %f %f %f %f %f %f\n",bp[i][l],bp[i][2],bp[il[3],bp[i][4],bp[i][5],bp[i][6],bp[i][q,bp[i][S]); phtf(" %f %f %f %f %f %f %f\n",bp[i][9],bp[il[lO],bp[i][l l],bp[i][l2],bp[i][l3],bp[i][l4],bp[i][l5]);} //printf("from br brl 1 = %finu,bp[l][l]);

return; 1

void CPmat(CP) float CP[q[q; { int ij; extern float bp[q[16];

lIprintf("from inside CR brl 1 = %f\n",bp[l][l]); APPENDIX D

for(i = 1 ;i < 7;i + +) printf("CR %f %f %f %f %f %finu,CP[i][l],CP[i][7],CP[i][3],CP[i][4],CP[i][5],CP[i][6]); return ;

void gpmat(gp) float gp[7; { int i; (0.0); gp[2]= (-37.2*~2- 8.4*~23 + 1.02*s2 ); gp[3]= (-8.4*~23+ 0.25*c23 ); gp[4] = ( 2.8e-02*~23*~4*~5); gp[5] = ( -2.8e-02*(~23*~5+ s23*c4*c5) ); APPENDIX D

return; 1

void cubic-coeff(coeff) float coeffl41; { int i;

coefflo] =tho; coeffll] = thd0; coeffl2]=3.0l(tf*tf)*(thf-th0)-2.01tPthdO-1.O/tf*thdf; coeffl3]=-2.0l(tf*tf*tf)*(thf-~hO)+1 .O/(tf*tf)*(thdf+ thd0);

return; 1

void jacobian(jac) float jac[7J[7J; { jac[l][l] = -sl*(c23*a3 + s23*d4 + c2*a2) -cl*(d2+d3); jac[l][2] = cl*(-s23*a3 + c23*d4 - s2*a2); jac[l][3] = cl *(-s23*a3 + c23*d4); jac[l][4]= 0.0; jac[l][S]= 0.0; jac[1][6]= 0.0; APPENDIX D

jac[S][l] = 0.0; jac[5][2] = cl ; jac[5][3]= cl; jac[5][4] = sl*s23; jac[5][5] = -sl*c23*s4 +cl*c4; jac[5][6] = (sl *c23*c4 +cl*s4)*s5 +sl*s23*c5;

for(i=l;i< =6;i+ +) p~tf("jacsubr %f %f %f %f %f %finujac[i][l],jac[i][2] jac[i][3] jac[i][4],jac[i][5]jac[i][6]);

return; 1

void jacobdot(jacdot) float jacdot[7[7]; { jacdot[l][l] = -cl *c23*a3*thdot[l] + sl*s23*a3*(thdot[2] +thdot[3]) -cl*s23*d4*thdot[l] -sl*c23*d4*(thdot[2]+thdot[3]) +s2*a2*thdot[2] +sl*(d2+d3)*thdot[l]; jacdot[l][2]= sl*s23*a3*thdot[l] -cl*c23*a3*(thdot[2]+thdot[3]) -sl*c23*d4*thdot[l] -cl*s23*d4*thdot[l] +sl *s2*a2*thdot[l] tl*c2*a2*thdot[2]; printf("BETWEEN % f % f %f %f %f % f\nU,sl*s23*a3*thdot[l], -cl*c23*a3*(thdot[2],thdot[3]), -sl*c23*d4*thdot[l] ,-cl*s23*d4*thdot[l] ,sl*s2*a2*thdot[l], -cl*c2*a2*thdot[2]); printf("BET@ %f %f %f %f %f %f %f %f %fin",a2,a3,d2,d3,d4,cl,sl,c23,s23); jacdot[l][3]= sl*s23*a3*thdot[l] -cl*c23*a3*(thdot[7,]+thdot[3]) -sl*c23*d4*thdot[l] -cl *s23*d4*(thdot[2]+thdot[3]); jacdot[l][4] = 0.0; jacdot[l][5] = 0.0; jacdot[l][6] = 0.0; APPENDIX D

for(i=l;i< =6;i+ +) p~tf("jacdotsubr %f %f %f %f %f %f\nWjacdot[i][l] jacdot[i][2ljacdot[i][3],jacdot[i][4],jacdot[i][S] jacdot[i][6]);

return; 1

void ludcmp(a,n,indx,d) int n,*indx; float **a,*d; I int i,imax j,k; float big ,dum,sum,temp; float *w,*vector(); void nrerror(),free-vector();

w =vector(l ,n); *d=1.0; for(i=l;i< =n;i+ +)I big=0.0; for(j=lG< =ni++) if ((temp= fabs(a[i]b])) > big) big=temp; if (big = = 0.0) nrerror("Singular matrix in routine LUDCMP"); vv[i] = 1 .O/big; APPENDIX D

1 for(j=l;j< =n$++) { for(i=l;i = big) { big=dum; imax = i; 1 1 if (j != imax) { for(k=l;k< =n;k+ +) { dum=a[imax]@c]; a[imax]F] =a[i]F]; a[i]@] =durn;

1 indx[i] = imax; if (a[i][i] = = 0.0) a[i][i]=TINY; if(i ! = n) { dum=l.O/(a~][i]); for(i=j+ 1;i < =n;i+ +) a[i][i] *= dum; 1 1

void lubksb(a,n,indx,b) float **a,bn; int n,*indx; { int i,ii=O,ip j; float sum;

for(i=l;i< =n;i+ +) { ip =indx[i]; sum= b[ip]; b[ip] =b[i]; if (ii) for(j=ii;i < =i-l;j+ +) sum -= a[i]c]*b[i]; else if (sum) ii=i; b[i] = sum; APPENDIX D

float *vector(nl,nh) int n1,nh; { void nrerror(); float *v; v=(float *)malloc((unsigned) (nh-nl+ l)*sizeof(flont)); if (!v) nrerror ("allocation failure in vector()"); return v-nl; 1

void free_vector(v,nl,nh) float *v; int n1,nh; { free((char*) (v + nl)); 1

void nrerror(error-text) char error-textu; { void exit(); fp~tf(stderr,"NumericalRecipies run-time error .. .\nW); fprintf(stderr," %s\n",error-text); fp~tf(stderr,"... now exiting to system ...\nn); exit(1); 1

float **matrix(nrl,nrh,ncl,nch) int nrl,nrh,ncl,nch; { int i; float **m;

rn =(float **) rnalloc((unsigned) (nrh-nrl+ l)*sizeof(float*)); if (!m) nrerror("allocation failure 1 in matrix()"); m -=nrl;

for(i=nrl;i < =nrh;i+ +) { m[i] =(float *) malloc((unsigned) (nch-ncl + l)*sizeof(float)); if (!m[i]) nrerror("al1ocation failure 2 in matrix()"); m[i] -=ncl; 1 return m; 1 APPENDIX D

D-10 Computer Program for Graphical Simulation of PUMA 560

/!-Name : 11 -Originator: sandeep 11 -Originated: (RAP) Thu Sep 24 09:29: 17 1992 11- Product : IJEMS 02.00.01.11 27-Apr-92 I/- Nodename : bear I/- Command : 11- Comments : Source code in Parametric Programming Language @pl)

#include "ciminimum.h" #include "cimacros .h" #include "fi.hn #include "grlastele.h" #include "stdio.h"

FILE *fptr;

I* Defining axis of rotation for each link *I

double Inkl-in[3],lnkl-out[3]; double lnk2-in[3],lnk2-out[3]; double Ink3-in[3],lnk3-out[3]; double lnk4_in[3],lnk4_out[3]; double InW-in[3],lnW-out[3]; double lnk6-in[3],lnk6-out[3]; struct Fl-data-st formData;

I* Defining object variables for respective link id's *I

GRobj link-1 ,link-2,link ,link-4a,link-4b ,link-4c,link-5; GRobj link_6a,link_6b; int number-found; double theta[q;

int i; double ~x,py,pz,qx,qy,qz,cx,cy,cz,d; double temp-in[3],temp-out[3]; double theta-initial[q;

void zerogosition() { APPENDIX D

11"EMPCyAxR" "Place cylinder by axis and radius"

ci$put( cmd = "xy =0.0,0.0,-5.25,frontU); ci$put( crnd = "xy =0.0,0.0,-5.00,front"); ci$put(string = "4.5"); ci$put( response = RESET ); ci$put( crnd = "xy =0.0,0.0,-5.00,frontW); ci$put( cmd = "xy=0.0,0.0,-0.50,front"); ci$put(string = "2.00"); ci$put( response = TERMINATE ); endcmd();

begincmd-key ("EMPCy AxR"); 1I"EMPCyAxR" "Place cylinder by axis and radius" ci$put( crnd = "xy =O,-1.25,O.front"); ci$put( crnd = "xy =0.0,2.00,0.00,front"); ci$put(string = " 1.5"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-1, parents = 1, nb-wanted = 1, nb-rend = &number-found );

if( number-found = = 0 ) write( "link-1 not found.\nH); else write( "link-1 no. found = ",number-found," ID= ", link-1, "\nu);

begincmd-key ("GRPLSgW); ci$put( crnd = "xy = -1.25,2.00,-1 .OO,front"); ci$put( cmd = "xy = 0.00,2.00,-1 .OO,front"); ci$put( cmd = "xy = 3.50,2.00,-0.75,frontn); ci$put( response = TERMINATE ); endcmd();

begincmd-key("GRPArR2Pnt'); /I"GRPArR2PnU "Place Arc by Radius and 2 Points" ci$put(string = " 1.00OW); ci$put( cmd = "xy =3.5,2.00,-0.75,front"); ci$put( crnd = "xy=3.5,2.00,0.00,front"); ci$put( cmd = "xy =3.5,2.00,0.75,front"); ci$put( response = TERMINATE ); endcmd();

begincmd-key("GRPLSgW); cisput( cmd = "xy = 3.50,2.00,0.75,front");

APPENDIX D

IInGRPArR2Pn" "Place Arc by Radius and 2 Points" ci$put(string = "0.75000"); ci$put( cmd = "xy =2.35,2.00,0.00,front"); ci$put( cmd = "xy=3.00,2.00,0.00,front"); ci$put( cmd = "xy=3.75,2.00,0.00,front"); ci$put( response = TERMINATE ); endcmdo;

begincmd-key("EMPCmCrAtW); 1l"EMPCmCrAt" "auto link curves" ci$put(st~g= "y"); ci$put( cmd = "xy =3.00,2.00,5.00,front"); ci$put( cmd = "xy =3.00,2.00,5.00,front"); ci$put( response = RESET ); ci$put( response = TERMINATE ); endcmd(),

begincmd-key("EMPS1PrV); 1I"EMPSIPr" "Place solid of projection" ci$put( cmd = "xy=3.625,2.00,3.50,front"); ci$put( cmd = "xy =3.75,2.00,0.00,front"); ci$put( cmd = "xy=3.75,1.00,0.00,front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-3 not found.\nW); else write( "link no. found = ",number-found," ID= ", link-3, "\nW);

begincmd-key("EMPSlBx2Pn"); ci$put( cmd = "xy=2.5,2.0,5.00,front"); ci$put( cmd = "xy=3.5,1.0,5.50,frontU); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &li4a, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "linkpa not found.\nM); else APPENDIX D

write( "link-4a no. found = ",number-found," ID= ", link-4a, "\nW);

begincmdkey ("EMPCyAxRW); 1I"EMPCyAxR" "Place cylinder by axis and radius" ci$put( crnd = "xy =3.00,1 .00,5.75,frontW); ci$put( crnd = "xy=3.00,1.33,5.75,front"); ci$put(string = "1 .Ow); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkPb, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-4b not found.\nW); else write( "linkb no. found = ",number-found," ID= ", IinkPb, "\nW);

begincmd-key("EMPCyAxR"); ci$put( crnd = "xy =3.00,1 .33,5.75,frontV); ci$put( crnd = "xy =3.00,1 .66,5.75,frontM); ci$put(string = " 1 .OOn); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link not found.\nU); else write( "link-5 no. found = ",number-found," ID= ", link-5,"\nU);

begincmd-key("EMPCykuR"); cisput( crnd = "xy =3.00,1 .66,5.75,frontN); cisput( crnd = "xy=3.00,2.00,5.75,front"); ci$put(string = "1.00"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkpc, parents = 1, nb-wanted = 1, APPENDIX D

nb-read = &number-found );

if( number-found = =: 0 ) write( "link-4c not found.\nM); else write( "link-4c no. found = ",number-found," ID= ", IinkPc, "\nu);

begincmd-key("EMPCyAxR); ci$put( cmd = "xy=3.00,1.50,6.25,front"); ci$put( cmd = "xy =3.00,1 .50,6.50,frontV); ci$put(string = "0.25"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-6a not found.\nW); else write( "link-6a no. found = ",number-found," ID= ", link_6a, "\nu);

begincmd-key ("EMPCyAxR"); ci$put( cmd = "xy=3.00,1.50,6.50,front"); ci$put( cmd = "xy =3.00,1.50,6.75,front"); ci$put(string = "0.50"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-6b not found.\nW); else write( "link-6b no. found = ",number-found," ID= ", link_6b, "\nu);

return; 1

zeropsition(); I* Drawing the zero position configuration *I APPENDIX D

/* Reading the data file containing the set of joint angles i.e., 81, 82, ..., 86 for each time step */

if( (fptr=fopen( "aprx.dat","rU))= = NULL ) {write("error opening file aprx.dat \no); exit;)

while(( fscanf(fptr,"%lf %If %If %If %If %An",&theta[l],&theta[2], &thetn[3],&theta[4],&theta[5],&theta[6]) )! = EOF) { APPENDIX D

I begincmd-key (" GRSAn"); ci$put( value = theta[l] ); endcmdo;

begincmd-key("GRRtEAbAx"); cisput( obj = link-1 ); ci$put( point = Inkl-in, window-name = "front"); cisput( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-1, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key("GRRtEAbAxW); ci$put( obj = link-2 ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-2, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxU); ci$put( obj = link-3 ); ci$put( point = Inkl-in, window-name = "front"); ci$put( point = Inkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key ("GRRtEAbAx"); ci$put( obj = link-4a ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkPa, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = link-4b ); ci$put( point = Inkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcrnd();

gr$last-elements( pobj = &link-4b, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

ci$put( obj = link-4~); cisput( point = lnkl-in, window-name = "front"); cisput( point = lnkl-out, window-name - "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-4c, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-5 ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &nurnbcr-found );

begincmdkey("GRRtEAbAx"); ci$put( obj = link-6a ); cisput( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = Sib); ci$put( point = Inkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

I begincmd-key("GRSAnn); ci$put( value = theta[2] ); endcmd();

begincmd-key("GRRtEAbAxW); ci$put( obj = link-2 ); ci$put( point = Ink2-in, window-name = "front"); ci$put( point = lnk2-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-2, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxm); cisput( obj = link ); ci$put( point = lnk2-in, window-name = "front"); cisput( point = lnk2-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = link-4a ); ci$put( point = Ink2_in, window-name = "front"); ci$put( point = lnk2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-4a, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key(" GRRtEAbAx"); ci$put( obj = link-4b ); ci$put( point = lnk2_in, window-name = "front"); ci$put( point = Ink2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_4b, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey("GRRtEAbAx"); ci$put( obj = liic); ci$put( point = lnk2-in, window-name = "front"); ci$put( point = lnk2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-4c, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxn); ci$put( obj = link-5 ); ci$put( point = lnk2-in, window-name = "front"); ci$put( point = lnk2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxU); ci$put( obj = linka ); ci$put( point = lnk2-in, window-name = "front"); ci$put( point = lnk2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

ci$put( obj = link-6b ); ci$put( point = lnk2-in, window-name = "front"); ci$put( point = lnk2-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRSAnW); ci$put( value = theta[3] ); endcmd();

begincmd-key (" GRRtEAb Ax"); ci$put( obj = link-3 ); ci$put( point = Ink3_in, window-name = "front"); ci$put( point = hk3-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd(); APPENDIX D

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxV); cisput( obj = link-4a ); ci$put( point = lnk3-in, window-name = "front"); cisput( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linka4a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxM); ci$put( obj = link-4b ); cisput( point = lnk3_in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &linkPb, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = link-4c ); ci$put( point = lnk3_in, window-name = "front"); ci$put( point = Ink3-out, window-name = "front"); cisput( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &linkpc, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincrnd-key("GRRtEAbAx"); ci$put( obj = link-5 ); ci$put( point = lnk3_in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); cisput( response = TERMINATE ); endcmdo; APPENDIX D

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxV); ci$put( obj = link-6a ); cisput( point = lnk3_in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey("GRRtEAbAxV); ci$put( obj = link-6b ); cisput( point = Ink3-in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

+cos(theta[l]*pi/l80.0)* 1 .SO;

lnk4-out[2] =-3.00*sin(theta[2]*pi/180.0) +5.50*cos((theta[2] +theta[3])*pi/l80.0);

begincmd-key ("GRSAn"); ci$put( value = theta[4] ); endcmd();

begincmd-key("GRRtEAbAxn); ci$put( obj = link-4a ); ci$put( point = lnk4_in, window-name = "front"); ci$put( point = Ink4-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-4a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-4b ); ci$put( point = lnk4_in, window-name = "front"); cisput( point = lnk4_out, window-name = "front"); &$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_4b, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxM); cisput( obj = link-4c ); cisput( point = lnk4_in, window-name = "front"); cisput( point = Ink4_out, window-name = "front"); ci$put( response = TERMINATE );

gr$last-elements( pobj = &linkW4c, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key("GRRtEAbAxU); ci$put( obj = link-5 ); ci$put( point = lnk4_in, window-name = "front"); ci$put( point = lnk4_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key (" GRRtEAb Ax"); ci$put( obj = link-6a ); ci$put( point = Ink4_in, window-name = "front"); ci$put( point = Ink4_out, window-name = "front"); ci$put( response = TERMINATE ); endcmdf);

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxU); ci$put( obj = link-6b ); ci$put( point = Ink4_in, window-name = "front"); ci$put( point = lnk4_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); 1 APPENDIX D

I* Evaluating the direction cosines *I

write("xoU,temp-in[O], "\nW); write(" yo" ,temp-in[l],"\n"); write("zo" ,temp_in[2], "\nW);

I* Performing matrix concatenations for rotation about an arbitrary axis *I APPENDIX D APPENDIX D

begincmdkey ("GRSAn"); ci$put( value = theta[S] ); endcmd();

begincmd-key("GRRtEAbAx"); ci$put( obj = link-5 ); ci$put( point = InW-in, window-name = "front"); ci$put( point = InW-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxn); ci$put( obj = link-6a ); ci$put( point = 1nW-in, window-name = "front"); ci$put( point = Inks-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkP6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = liikb); APPENDIX D

cisput( point = InW-in, window-name = "front"); cisput( point = InW-out, window-name = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found );

I* Evaluating the direction cosines *I APPENDIX D

write("**** data for link-6 *****","\nn);

I* Performing matrix concatenations for rotation about an arbitrary axis */ APPENDIX D

begincmd-key("GRSAn"); ci$put( value = theta[6] ); endcmdo;

begincmd-key("GRRtEAbAxW); cisput( obj = link-6a ); ci$put( point = Inkbin, window-name = "front"); ci$put( point = lnk6_out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

begincmd-key("GRRtEAbAxW); ci$put( obj = link-6a ); APPENDIX D

ci$put( point = lnk6_in, window-name = "front"); ci$put( point = Inkbout, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey (" GRRtEAbAx"); cisput( obj = link-6b ); cisput( point = Ink6-in, window-name = "front"); ci$put( point = Ink6_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); 1

theta-initialill = theta[l]; theta-initial[',] = theta[',]; theta_initial[3] = theta[3]; theta-initial[4] =theta[4]; theta-initial[S] = theta[S]; theta_initial[6] = theta[6];

i=i+ 1;)llEND WHILE APPENDIX D

D-11 Computer Program for Graphical Simulation of Pieper's Solution

//-Name : /I - Originator: sandeep //-Originated: (RAP) Thu Sep 24 09:29:17 1992 11 -Product : IlEMS 02.00.01.1 1 27-Apr-92 I/-Nodename : bear //-Command : I/- Comments : Source code in Parametric Programming Language @pl)

#include "ciminimum.h" #include "cimacros.hm #include " fi.h" #include "grlaste1e.h" #include "stdi0.h"

FILE *fptr;

I* Defining axis of rotation for each link *I

double Inkl-in[3],lnkl_out[3]; double lnk2-in[3],lnk2-out[3]; double Ink3_in[3] ,lnk3_out[3]; double Ink4_in[3],lnk4_0ut[3]; double lnW-in[3],LnW-out[3]; double Ink6_in[3],lnk6_0ut[3]; struct FI-data-st formData;

I* Defining object variables for respective link id's */

GRobj link-l,link-2.link-3,link-4a,link-4b,link-4c,link-5; GRobj link-6a,link-6b; int number-found; double theta[7'J;

int i; double px,py ,pz,qx,qy,qz,cx,cy ,cz,d; double temp-in[3],temp_out[3]; double theta_initial[7];

void zerogosition() { APPENDIX D

1I"EMPCyAxR" "Place cylinder by axis and radius"

ci$put( crnd = "xy =0.0,0.0,-5.25,front"); ci$put( crnd = "xy =0.0,0.0,-5.00,frontW); ci$put(stMg = "4.5"); ci$put( response = RESET ); ci$put( crnd = "xy =0.0,0.0,-5.00,frontm); ci$put( crnd = "xy=0.0,0.0,-0.50,frontn); ci$put(string = "2.00"); ci$put( response = TERMINATE ); endcmd();

begincmd-key ("EMPCy AxR"); 11"EMPCyAxR" "Place cylinder by axis and radius" cisput( crnd = "xy =0,-1.25,0,front"); cisput( crnd = "xy =0.0,1.25,0.00,front"); ci$put(stMg = "1.5"); cisput( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-1, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-1 not found.\nW); else write( "link-1 no. found = ",number-found," ID= ", link-1, "\nW);

begincmdkey("GRPLSg "); ci$put( crnd = "xy= -1.25,1.25,-1.00,front"); ci$put( crnd = "xy = 0.00,1.25,-1.OO,frontM); ci$put( crnd = "xy= 3.50,1.25,-0.75,frontM); ci$put( response = TERMINATE ); endcmd();

begincmd-key(" GRPArR2PnU); /IwGRPArR2Pn" "Place Arc by Radius and 2 Points" ci$put(stMg = " 1.00OW); ci$put( crnd = "xy=3.5,1.25,-0.75,frontn); ci$put( crnd = "xy=3.5,1.25,0.00,front"); ci$put( crnd = "xy=3.5,1.25,0.75,front"); ci$put( response = TERMINATE ); endcmd();

begincmd-key(" GRPLSg"); ci$put( crnd = "xy= 3.50,1.25,0.75,frontW); APPENDIX D

ci$put( crnd = "xy = 0.00,1.25,1 .OO,frontn); cisput( crnd = "xy= -1.25,1.25,1.OO,front"); ci$put( response = TERMINATE ); endcmd();

begincmd-key("GRPArR2Pn "); ll"GRPArR2Pn" "Place Arc by Radius and 2 Points" ci$put(string = " 1.Won); cisput( crnd = "xy =-1.25,1.25,1 .OO,front"); cisput( crnd = "xy =-1.25,1.25,0.00,front"); cisput( crnd = "xy=-1.25,1.25,-1.00,front"); cisput( response = TERMINATE ); endcmd();

begincmd-key("EMPCmCrAt"); lI"EMPCmCrAtn "auto link curves" ci$put(string = "y "); ci$put( crnd = "xy=-1.00,1.25,-1.OO,frontW); ci$put( crnd = "xy=-1.00,1.25,-1.OO,frontW); ci$put( response = RESET ); ci$put( response = TERMINATE ); endcmd();

begincmdkey("EMPS1PrW); 1I"EMPSIPr" "Place solid of projection" ci$put( crnd = "xy =-1.00,1.25,-1.00,front"); ci$put( crnd = "xy=-1.25,1.25,-1.00,frontW); ci$put( cmd = "xy =-1.25,2.25,-1.00,front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-2, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-2 not found.\nU); else write( "link-2 no. found = ",number-found," ID= ", link-2, "\nu);

begincmd-key("GRPLSg"); ci$put( cmd = "xy = 3.75,1.25,0.00,front"); ci$put( crnd = "xy = 3.50,l .25,2.25,frontW); ci$put( crnd = "xy = 2.50,1 .25,2.25,frontW); ci$put( crnd = "xy= 2.25,1.25,0.00,front"); ci$put( response = TERMINATE ); endcmd(); APPENDIX D

ll"GRPArR2Pn" "Place Arc by Radius and 2 Points" ci$put(string = "0.75000"); cisput( cmd = "xy =2.25,1.25,0.00,fro11:"); ci$put( cmd = "xy =3.00,1.25,0.00,front"); ci$put( cmd = "xy=3.75,1.25,0.00,front"); ci$put( response = TERMINATE ); endcmd();

begincmdkey("EMPCmCrAtn); /lUEMPCmCrAt""auto link curves" ci$p~t(st~g= "y"); cisput( cmd = "xy=3.00,1.25,2.25,front"); cisput( cmd = "xy =3.00,1 .25,2.25,frontU); ci$put( response = RESET ); ci$put( response = TERMINATE ); endcmd();

begincmdkey("EMPS1Pr"); 1I"EMPSIPr" "Place solid of projection" ci$put( cmd = "xy =3.625,1.25,1.125,front"); ci$put( cmd = "xy=3.75,1.25,0.00,front"); ci$put( cmd = "xy =3.75,0.25,O.OO,front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-3 not found.\nW); else write( "link-3 no. found = ",number-found," ID= ", link-3, "\nH);

begincmd-key("EMPS1Bx2PnM); ci$put( cmd = "xy=2.5,1.25,2.25,front"); ci$put( cmd = "xy =3.5,0.25,2.75,front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_4a, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-4a not found.\nW); else APPENDIX D

write( "link-4a no. found = ",number-found," ID= ", link-4a, "\nn);

begincmd-key("EMPCyAxRH); II"EMPCyAxRM"Place cylinder by axis and radius" ci$put( cmd = "xy =3.00,0.25,3.00,front"); cisput( cmd = "xy =3.00,0.58,3.00,front"); ci$put(stMg = " 1.Ow); cisput( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &linkPb, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-4b not found.\nU); else write( "link-4b no. found = ",number-found," ID= ", IinkPb, "\nn);

begincmd-key ("EMPCyAxR"); ci$put( cmd = "xy=3.00,0.58,3.00,front"); cisput( cmd = "xy=3.00,0.91,3.00,front"); ci$put(string = " 1.00"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-5 not found.\nW); else write( "link-5 no. found = ",number-found," ID = ", link-5,"\nn);

begincmd-key("EMPCyAxR"); cisput( cmd = "xy =3.00,0.91,3.00,front"); ci$put( cmd = "xy=3.00,1.25,3.00,front"); ci$plIt(st~g= " 1.00"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_4c, parents = 1, nb-wanted = 1, APPENDIX D

nb-read = &number-found );

if( number-found = = 0 ) write( "link-4c not found.\nW); else write( "link-4c no. found = ",number-found," ID = ", link_4c, "\nu);

begincmd-key("EMPCyAxR"); cisput( cmd = "xy =3.00,0.75,3.50,frontW); ci$put( cmd = "xy =3.00,0.75,3.75,frontW); ci$put(stMg = "0.25"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "linL6a not found.\nn); else write( "lii6a no. found = ",number-found," ID = ", lik-6a, "\nW);

begincmd-key("EMPCyAxR"); &$put( cmd = "xy =3.00,0.75,3.50,front"); ci$put( cmd = "xy=3.00,0.75,4.00,front"); ci$put(stMg = "0.50"); ci$put( response = TERMINATE ); endcmd() ;

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found );

if( number-found = = 0 ) write( "link-6b not found.\nn); else write( "link-6b no. found = ",number-found," ID= ", link_6b, "\nu);

return; 1 main0 { zeroqosition0; /* Drawing the zero position configuration *I APPENDIX D

I* Reading the data file containing a set of 4 set of joint angles i.e., 01, 02, ..., 06 from Pieper's Solution *I

if( (fptr= fopen( "pieper.dat","rW))= = NULL ) {write("erroropening file pieper.dat \n"); exit;}

while(( fscanf(fptr,"%lf %If %If %If %If %fln",&theb[l],&theb[2], &theta[3],&theta[4],&thet3[5],&thrta[6]) )! = EOF) {

Ink2-in [0]= 0.00; lnk2_in[ll= 1.25; lnk2_in[2] = 0.00; APPENDIX D

{ begincmdkey("GRSAnM); ci$put( value = theta[l] ); endcmd();

begincmd-key("GRRtEAbAx"); ci$put( obj = link-1 ); ci$put( point = Inkl-in, window-name = "front"); ci$put( point = Inkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-1, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key("GRRtEAbAxW); ci$put( obj = linkZ ); ci$put( point = Inkl-in, window-nnmc = "front"); ci$put( point = Inkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

grslast-elements( pobj = &link3, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxV); ci$put( obj = link-3 ); ci$put( point = Inkl-in, window-name = "front"); ci$put( point = Inkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey (" GRRtEAbAx"); ci$put( obj = link-4a ); ci$put( point = Inkl-in, window-name = "front"); cisput( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkV4a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key(" GRRtEAbAx"); cisput( obj = link-4b ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkPb, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

ci$put( obj = link-4c ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = Inkl-out, window-name - "front"); ci$put( response = TERMINATE ); endcmd();

&last-elements( pobj = &link_4c, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxN); ci$put( obj = link-5 ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); cisput( obj = link-6a ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey("GRRtEAbAx"); ci$put( obj = rink-6b ); ci$put( point = lnkl-in, window-name = "front"); ci$put( point = lnkl-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

{ begincmd-key("GRSAn"); ci$put( value = theta121 ); endcmd();

begincmd-key("GRRtEAbAx"); ci$put( obj = link-2 ); ci$put( point = lnk2_in, window-name = "front"); ci$put( point = lnk2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-2, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key ("GRRtEAbAx"); ci$put( obj = link-3 ); ci$put( point = lnk2_in, window-name = "front"); ci$put( point = lnk2-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-4a ); ci$put( point = Ink2-in, window-name = "front"); ci$put( point = Ink2-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkPa, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key("GRRtEAbAxW); ci$put( obj = link-4b ); ci$put( point = hk2-in, window-name = "front"); ci$put( point = lnk2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-4b, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxV); ci$put( obj = link-4c ); ci$put( point = Ink2_in, window-name = "front"); ci$put( point = Ink2-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-4c, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-5 ); ci$put( point = lnlc2-in, window-name = "front"); &$put( point = Ink2_out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxU); ci$put( obj = link-6a ); ci$put( point = lnk2_in, window-name = "front"); ci$put( point = Ink2_0ut, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

ci$put( obj = link-6b ); ci$put( point = Ink2_in, window-name = "front"); ci$put( point = Ink2_out, window-namt: = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRSAn"); ci$put( value = theta[3] ); endcmd();

begincmd-key("GRRtEAbAxW); cisput( obj = link-3 ); cisput( point = lnk3_in, window-name = "front"); cisput( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd(); APPENDIX D

gr$last-elements( pobj = &link-3, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey("GRRtEAbAx"); ci$put( obj = link-4a ); ci$put( point = lnk3_in, window-name = "front"); ci$put( point = lnk3-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &link-48, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxU); ci$put( obj = link-4b ); ci$put( point = lnk3_in, window-name = "front"); ci$put( point = Ink3_out, window-name = "front"); ci$put( response = TERMINATE); endcmd();

gr$last-elements( pobj = &link-4b, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = link-4c ); ci$put( point = lnlc3-in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkQc, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-5 ); cisput( point = lnk3-in, window-name = "front"); ci$put( point = lnH-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd(); APPENDIX D

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key (" GRRtEAbAxn); ci$put( obj = link-6a ); ci$put( point = lnk3_in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmdkey("GRRtEAbAx"); ci$put( obj = link-6b ); ci$put( point = Ink3-in, window-name = "front"); ci$put( point = lnk3_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key("GRSAnH); ci$put( value = theta[4] ); endcmd();

begincmd-key("GRRtEAbAxM); ci$put( obj = link-4a ); ci$put( point = lnk4-in, window-name = "front"); ci$put( point = Ink4_out, window-name = "front"); ci$put( response = TERMINATE ); endcrndo;

gr$last-elements( pobj = &linkP4a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-4b ); ci$put( point = lnkqin, window-name = "front"); ci$put( point = lnkqout, window-name = "front"); ci$put( response = TERMINATE ); endcrnd();

gr$last-elements( pobj = &linkPb, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); cisput( obj = link-4c ); ci$put( point = lnk4-in, window-name = "front"); ci$put( point = lnk4_out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &liic, parents = 1, nb-wanted = 1, nb-read = &number-found ); APPENDIX D

begincmd-key("GRRtEAbAxM); ci$put( obj = link-5 ); ci$put( point = lnkqin, window-name .= "front"); ci$put( point = lnk4-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &linkJ, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("CRRtEAbAx"); ci$put( obj = link-6a ); ci$put( point = Ink4_in, window-name = "front"); ci$put( point = lnk4_out, window-name = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &iink_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxV); ci$put( obj = link-6b ); ci$put( point = Ink4_in, window-name = "front"); ci$put( point = lnk4_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-6b, parents = 1, nb-wanted = 1, nb-read = &nu~nber-found ); 1 APPENDIX D

I* Evaluating the direction cosines *I

I* Performing matrix concatenations for rotation about an arbitrary axis *I APPENDIX D

temp-outlo] =cos(theta[4]*pil180.0)*lnk5~out[O] -sin(theta[4]*pi/180,0)*Ink550ut[l]; temp-out[l] =sin(theta[4]*pi/l80.0)*lnW-out[O] +cos(theta[4]*pi/l80.0)*lnk5~out[l]; temp_out[2] =lnW-out[2]*l .O; APPENDIX D

begincmd-key("GRSAnN); ci$put( value = t.heta[S] ); endcmd();

begincmd-key("GRRtEAbAxW); ci$put( obj = link-5 ); cisput( point = M-in, window-name = "front"); ci$put( point = lnk5-out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link-5, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAxW); ci$put( obj = link-6a ); ci$put( point = Inks-in, window-name = "front"); ci$put( point = 1nW-out, window-name = "front"); ci$put( response = TERMINATE ); endcmdo;

gr$last-elements( pobj = &linkw6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = link-6b ); APPENDIX D

ci$put( point = 1nW-in, window-name = "front"); ci$put( point = 1nM-out, window-name = "front"); cisput( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &iink_6b, parents = 1, nb-wanted = 1, nb-read = &number-found );

I* Evaluating the direction cosines *I APPENDIX D

write("**** data for link-6 *****","\nW);

I* Performing matrix concatenations for rotation about an arbitrary axis */ APPENDIX D

begincmdkey("GRSAn"); ci$put( value = theta[6] ); endcmdo;

begincmd-key("GRRtEAbAxfl); ci$put( obj = link-6a ); cisput( point = Ink6_in, window-name = "front"); ci$put( point = lnk6_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

begincmdkey("GRRtEAbAx"); cisput( obj = link-6a ); APPENDIX D

ci$put( point = lnk6_in, window-name = "front"); ci$put( point = lnk6_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6a, parents = 1, nb-wanted = 1, nb-read = &number-found );

begincmd-key("GRRtEAbAx"); ci$put( obj = link-6b ); ci$put( point = Ink6_in, window-name = "front"); ci$put( point = lnk6_out, window-name = "front"); ci$put( response = TERMINATE ); endcmd();

gr$last-elements( pobj = &link_6b, parents = 1, nb-wanted = 1, nb-read = &number-found ); 1

i=i+l;}//END WHILE APPENDIX D 228

D-12 This file contains the main routines for running the PUMA robot with the Trident Robotics and Research, Inc. interface boards.

I* - ROB0T.C - Initial coding by Richard Voyles, Vigilant Technologies. *I

typedef void interrupt (*hndlrgtr)(void);

I* - Input frequency to the Programmable Interval Timer (8253 or - equivalent) on IBM compatible machines. *I #define TIMER-INPUT-FREQ 1 192500.0

I* - This is the external, user-supplied task that is executed every - timer tick. *I extern void periodic-task();

I* -- Global variables (all globals end in "G") *I long clockTicksG; I* number of timer interrupts since init *I float clockPeriodG; I* period, in seconds, of interrupts */ float clockFreqG; I* actual frequency, in hertz *I short installFlagG = 0; I* flag indicating handler installed */ hndlrqtr oldHandlerG; I* function pointer to old timer handler *I PUMAstatusT purnaVarsG; I* structure containing PUMA variables *I

I* - The following values are valid for a PUMA 560 *I short motorScaleG[6] = (400, -250, 400, -700, -600, -600) ; float encoderScaleG[6] = {0.00010035, -0.000073 156, 0.0001 17, -0.000082663, -0.000087376, -0.000081 885) ;

short robot-fltwr(short option, short chan, float *val); short robot-intwr(short option, short chan, short *val); short robot-fltrd(short option, short chan, float *val); short robot-longrd(short option, short chan, long *val);

I* - This function is the interrupt handler for timer 0 (interrupt 8). - Working code can be inserted here or a call to another - subroutine. */ void interrupt timer-handler() { clockTicksG + + ; I* - Working code or function call goes here. *I periodic-task() ; I* - End of working code. */

I* - This line is required to exit properly. *I outportb(0xO20,0x20); I* Send non-specific EOI to 8359A *I 1 APPENDIX D

I* - This function sets the frequency of the timer 0 interrupts. - The float value FREQ is in hertz. The actual interrupt frequency -- in hertz is returned. *I float interrupt-rate(float freq) { int timerCnt;

timerCnt = (int)(TIMER-INPUT-FREQ I freq); outportb(OxO43,0~36); I* init timer 0 for count load *I outportb(Ox04O,(char)timerCnt); I* load timer 0 count LSB *I outportb(Ox04O,(char)(tirnerCnt > > 8)); I* load timer 0 count MSB *I I* - Set the global variables that keep track of interrupt rate. *I clockFreqG = TIMER-INPUT-FREQ I (float)timerCnt; clockPeriodG = (float)timerCnt 1 TIMER-INPUT-FREQ; return clockFreqG; I

I* - This function installs the new timer 0 interrupt handler. It - grabs the old timer handler for restoration upon quitting the - program. *I void install-handler() { if (installFlagG = = 0){ oldHandlerG = getvect(8); I* Save old timer interrupt handler *I instaLlFlagG = 1; I* set flag to indicate handler installed *I ) I* endif *I clockTicksG = 0; setvect(8,timer-handler); I* Install new timer handler *I I

/* - This function provides an orderly shutdown by restoring the original - timer interrupt handler. It should also reset the system time from - the time-of-day clock. *I void shutdown() { pumaVarsG.discrete = pumaVarsG.discrete 6r - POWER-BIT; robot-intwr(STATUS, 0, &pumaVarsG.discrete); interrupt-rate(18.2); setvect(8,oldHandlerG); installFlagG = 0; I

I* - This function initializes the system for robot control. It installs - the interrupt handler and sets the desired interrupt frequency, in - the proper order. It also initializes variables. - It returns the acutal interrupt frequency, in hertz. *I float system-init(float freq) { int i;

I* - First initialize the PUMA structure. *I I* --- Start by resetting everyting but the and - bits and setting the discrete variable. *I APPENDIX D

pumaVarsG.status = inport(TRC-BASE + STATUS-REG); pumaVarsG.discrete = @umaVarsG.status & OxC000) > > 8; outport(TRC-BASE + DISCRETE-REG, pu~naVursG.discrete); pumaVarsG.status = inport(TRC-BASE + STATUS-REG); I* - Now set position variables appropriately *I if (@umaVarsG.status & CALIB-MASK) = = 0){ I* - Arm is NOT calibrated so set everything to zero. *I for (i=O; i<6; i++){ pumaVarsG.pos[i] = OL; I* reset the variable *I robot-intwr(POSITION, i, 0); I* reset the encoder */ } I* endfor *I purnaVarsG.discrete = 0; I* make sure all bits are cleared *I outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); } else { I* - Arm IS calibrated *I pumaVarsG.oldPos5 = 0; I* - This is not quite complete - must deal with bit 17 *I robot~longrd(POSITION6,0,&pumaVarsG.pos[O]); ) I* endif */

I* - These are required for initializing the interrupt handler *I install-handler(); return interrupt-rate(freq); 1

I* - This function writes 1 or 6 float values to the robot. The particular - value is chosen with the OPTION parameter, which is one out of a - list of enumerated ioOptionsT. CHAN ranges froin 0 to 5 (corresponding - to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. *I short robot-fltwr(short option, short chan, float *val) { short intVal, 1;

switch (option) { case TORQUE6: I* Output to all DACs -- val is in N-m *I for (i=O; i<6; i++){ intVal = (int)(val[i] * motorScaleG[i]) + 0x0800; outport(TRC-BASE + DAC-BASE + 2*i, intVal); } I* endfor *I return OK; case TORQUE: I* Output to single DAC -- val is in N-rn *I intVal = (int)(*val * motorScaleG[chan]) + 0x0800; outport(TRC-BASE + DAC-BASE + 2*chan, intVal); return OK; case VOLTAGE: I* Output to single DAC -- val is in volts *I if (*val > = 10.0){ intVal = 0; ) else if (*val < = -9.995){ intVal = OxOFFF; } else { intVal = (int)(*val * DAC-BITS-PER-VOLT) + 0x0800; ) I* endif *I APPENDIX D

outport(TRC-BASE + DAC-BASE + 2*chan, inlVal); return OK; case POSITION6: I* Preset all encoders -- val is in radians *I for (i=O; i <6; i+ +){ intval = (int)(val[i] I encoderScaleG[iJ); outport(TRC-BASE + ENC-LOAD-BASE + 2*i, intVal); } I* endfor *I return OK; case POSITION: I* Preset a single encoder - val is in radians *I intVal = (int)(*val 1 encoderScaleG[chan]); outport(TRC-BASE + ENC-LOAD-BASE + 2*chan, intval); return OK; default: return BAD-OPTION; ) I* endswitch *I 1

I* - This function writes 1 or 6 integer values to the robot. The particular - value is chosen with the OPTION parameter, which is one out of a - list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding - to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. *I short robot-intwr(short option, short chan, short *val) { short i;

switch (option) { case TORQUE6: I* Output to all DACs *I for (i=O; i<6; i++){ outport(TRC-BASE + DAC-BASE + 2*i, val[i]); } I* endfor *I return OK; case TORQUE: I* Output to single DAC *I outport(TRC-BASE + DAC-BASE + 2*chan, *val); return OK; case POSITION6: I* Preset all encoders *I for (i=O; i<6; i++){ outport(TRC-BASE + ENC-LOAD-BASE + 2*i, val[iJ); } I* endfor *I return OK; case POSITION: I* Preset a single encoder *I outport(TRC-BASE + ENC-LOAD-BASE + 2*chan, *val); return OK; case STATUS: I* Write the entire status word *I outport(TRC-BASE + DISCRETE-REG, *val); return OK; default: return BAD-OPTION; ) I* endswitch *I 1

I* - This function reads 1 or 6 float values from the robot. The particular - value is chosen with the OPTION parameter, which is one out of a - list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding APPENDIX D

- to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. */ short robot-fltrd(short option, short chan, float *val) { short tempPos , i;

switch (option) { case POSlTION6: /* Read all encoders -- val is in radians */ for (i=O; i<5; i+ +){ val[i] = encoderScaleG[i] * inport(TRC-BASE + ENC-COUNT-BASE + 2*i); ) /* endfor */ I* Joint 6 must be treated differently because it needs 17 bits. *I tempPos = inport(TRC-BASE + ENC-COUNT-BASE + 10); pumaVarsG.pos[5] + = (long)(tempPos - pumaVarsG.oldPos5); va1[5] = encoderScaleG[5] * pumaVarsG.pos[S]; pumaVarsG.oldPos5 = tempPos; I* - Set the $6 overflow bit appropriately. */ if @umaVarsG.pos[S] < 0){ purnaVarsG.discrete = pumaVarsG.discrete I JT6-OVRFLO-BIT; ) else { pumaVarsG.discrete = pumaVarsG.discrete & (- JT6-OVRFLO-BIT); ) /* endif */ outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); return OK; case POSITION: /* Read a single encoder *I if (chan ! = 5){ *val = encoderScaleG[chan]*inport(TRC-BASE + ENC-COUNT-BASE + 2*chan); } else { I* Joint 6 must be treated differently because it needs 17 bits. */ tempPos = inport(TRC-BASE + ENC-COUNT-BASE + 10); purnaVarsG.pos[S] + = (long)(tempPos - pumaVarsG.oldPos5); *val = encoderScaleG[S] * pumaVarsG.pos[S]; pumaVarsG.oldPos5 = tempPos; /* - Set the $6 overflow bit appropriately. */ if @umaVarsG.pos[S] < 0) { pumaVarsG.discrete = pumaVarsG.discrete f JT6-OVRFLO-BIT; } else { purnaVarsG.discrete = pumaVarsG.discrete & (-JT6-OVRFLO-BIT); } I* endif *I outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); } /* endif */ return OK; case POT: I* Read a single ADC - val is in volts *I outport(TRC-BASE + ADC-MUX-SELECT, chan); /* select the MUX channel */ inport(TRC-BASE + ADC-START); I* trigger the conversion */ I* - Enable the ADC conversion complete bit. */ purnaVarsG.discrete = purnaVarsG.discrete I ADC-MASK-BIT; outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); I* - Wait for the conversion complete flag. */ while ((inport(TRC-BASE + STATUS-REG) & ADC-STAT-MASK) ! = 0); *val = 0.0196 * (inport(TRC-BASE + ADC-VALUE) & OxOOFF); I* - Disable and clear the ADC conversion complete bit. */ pumaVarsG.discrete = pumaVarsG.discrete & ( - ADC-MASK-BIT); APPENDIX D

outport(TRC-BASE + DISCRETE-REG, pu~naVarsG.discrete); return OK; default: return BAD-OPTION; ) I* endswitch */ 1

I* - This function writes 1 or 6 LONG values to the robot. The particular - value is chosen with the OPTION parameter, which is one out of a - list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding - to PUMA joints 1 - 6). It returns one of the enumerated ioErrorsT. *I short robot-longrd(short option, short chan, long *val)

short i, tempPos;

switch (option) { case POSITION6: I* Read all encoders */ for (i=O; i<5; i++){ val[i] = (long)inport(TRC-BASE + ENC-COUNT-BASE + 2*i); } I* endfor */ I* Joint 6 must be treated differently because it needs 17 bits. */ tempPos = inport(TRC-BASE + ENC-COUNT-BASE + 10); va1[5] + = (long)(tempPos - pumaVarsG.oldPos5); pumaVarsG.oldPos5 = tempPos; I* - Set the jt6 overflow bit appropriately. */ if (va1[5] < 0){ pumaVarsG.discrete = pumaVarsG.discrete I JT6-OVRFLO-BIT; } else { pumaVarsG.discrete = pumaVarsG.discrete & (-JT6-OVRFLO-BIT); } I* endif *I outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); return OK; case POSITION: I* Read a single encoder */ if (chan ! = 5){ *val = (long)inport(TRC-BASE + ENC-COUNT-BASE + 2*chan); ) else { I* Joint 6 must be treated differently because it needs 17 bits. */ tempPos = inport(TRC-BASE + ENC-COUNT-BASE + 10); *val + = (long)(tempPos - pumaVarsG.oldPos5); pumaVarsG.oldPos5 = tempPos; I* - Set the jt6 overflow bit appropriately. *I if (*val < 0) { pumaVarsG.discrete = pumaVarsG.discrete I JT6-OVRFLO-BIT; } else { pumaVarsG.discrete = pumaVarsG.discrete & (- JT6-OVRFLO-BIT); } I* endif */ outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); } I* endif */ return OK; case STATUS: I* Read the entire status word into long int */ *val = (long)((unsigned short)inport(TRC-BASE + STATUS-REG)); return OK; APPENDIX D

case POT: I* Read a single ADC *I outport(TRC-BASE + ADC-MUX-SELECT, chan); I* select the MUX channel *I inport(TRC-BASE + ADC-START); I* trigger the conversion *I I* - Enable the ADC conversion complete bit. *I pumaVarsG.discrete = pumaVarsG.discrete I ADC-M ASK-BIT; outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); I* - Wait for the conversion complete flag. *I while ((inport(TRC-BASE + STATUS-REG) & ADC-STAT-MASK) ! = 0); *val = (long)(iport(TRC-BASE + ADC-VALUE) & OxOOFF); I* - Disable and clear the ADC conversion complete bit. *I pumaVarsG.discrete = pumaVarsG.discrete & ( - ADC-MASK-BIT); outport(TRC-BASE + DISCRETE-REG, pumaVarsG.discrete); return OK; default: return BAD-OPTION; } I* endswitch *I 1 APPENDIX D

D-13 This header file contains defined constants for use in both ROB0T.C and ROB0TUSR.C when using the interface boards from Trident Robotics and Research, Inc.

I* - ROB0T.H - Initial coding by Richard Voyles, Vigilant Technologies *I

I* - Enumerated types defining return codes for the I10 routines and - options for the I10 routines. - All typedefs end in "T". *I typedef enum { OK, BAD-OPTION ) ioErrorsT; typedef enum { TORQUE, I* W torque to a single joint motor *I TORQUE6, /* W torques to all joint motors *I VOLTAGE, /* W voltage to a single joint motor *I POSITION, I* R/W position of a single joint *I POSITION6, I* RW position of all joints *I STATUS, I* RW discrete statuslcontrol bits *I POT I* R ADC value *I ) ioOptionsT;

I* - Structure that holds the PUMA information *I typedef struct{ short status; short discrete; long pos[6]; short oldPos5; ) PUMAstatusT;

I* - Base I10 address for the TRC006. *I #define TRC-BASE 0x7000

I* - Address offsets for functional groups of utilities. *I #define ENC-INDEX-BASE OxOOOO #define STATUS-REG OxOOOC #define ENC-COUNT-BASE Ox00 10 #define ADC-VALUE OxOOlC #define ADC-START 0x001 E #define ENC-LOAD-BASE 0x0020 #defme ADC-MUX-SELECT OxOO2C #define DISCRETE-REG Ox002E #define DAC-B ASE 0x0030

I* - Masks for the status register. *I #define CALIB-MASK 0x4000 #defme ADC-STAT-MASK Ox0040

I* - Masks for the discrete register. *I #define POWER-BIT Ox000 1 #define H AND-OPEN-BIT Ox0002 #define HAND-CLOSED-BIT 0x0004 APPENDIX D

#define CALIB-BIT 0x0040 #define JT6-OVRFLO-BIT 0x0080 #define ADC-MASK-BIT 0x4000

I* - Reciprocal of DAC gain (gain is set by resistors) */ #define DAC-BITS-PER-VOLT -204.8 APPENDIX D 237

D-14 This is an example user-code file for controlling the PUMA robot with the Trident Robotics and Research, Inc. interface boards.

I* - ROB0TUSR.C - Initial coding by Richard Voyles, Vigilant Technologies *I

extern int calibrate(int chan);

I* - Global variables *I float KPosG[6], I* position error gain for PD cntrlr *I KVelG[6]; I* velocity error gain for PD cntrlr *I float posRefG[6], I* joint reference positions *I velRefG[6]; I* joint reference velocity *I int cntrlFlagG; I* control startlstop flag *I

I* - Simple PD controller with gravity compensation on joints 2 and 3. *I void pdgControl(float pos[6], float ve1[6], float torque[6]) { float gravity[3];

/* calculate gravity compensation for joints 2 and 3 *I gravity[2] = 0.75 * sin@os[l] - pos[2]); gravity[l] = -2.0 * sin(pos[l]) - gravity[2];

I* - This function must appear and must be named exactly as shown. - It is executed every timer tick. *I void periodic-task()

float posNew[6], *ternphr, velTemp[6], outTorq[6], factor; static float posOId[2][6]; APPENDIX D

I* - WARNING!!! The numeric coprocessor state is NOT saved. If this - interrupt can clobber a floating point calculation, you must -- save state and restore upon exit. */

factor = 2.0 * clockFreqG; if (cntrlFlagG){ robot-fltrd(POSITION6, 0, posNew); /* get current robot pos */ I* calculate velocities *I velTemp[O] = (posNew[O] - posOld[l][O]) * factor; veiTemp[l] = @osNew[l] - posOld[l][l]) * factor; velTemp[2] = @osNew[2] - posOld[l][2]) * factor; velTemp[3] = @osNew[3] - posOld[l][3]) * factor; velTemp[4] = @osNew[4] - posOld[l][4]) * factor; velTemp[5] = @osNew[S] - posOld[l][S]) * factor; pdgControl(posNew, velTemp, outTorq); /* calculate control law *I robot-fltwr(T0RQU E6, 0, outTorq) ; I* write torque values *I } I* endif */

I* - Time shill the position values */ posOld[l][O] = pos01d[0][0]; posOld[O][O] = posNew[O]; posOld[l][l] = posOld[O][l]; posOId[O][l] = posNew[l]; posOld[l][2] = posOld[0][2]; posOId[0][2] = posNew[2]; posOld[l][3] = posOld[O][3]; pos01d[0][3] = posNewU]; posOld[l][4] = posOId[0][4]; posOId[0][4] = posNew[4]; posOld[l][S] = pos01d[0][5]; posOld[Oj[S] = posNew(51; 1 void print-menu() I printf("Menu options - > \nn); if (cntrlFlagG){ printf("\tc\tController toggle off \n"); } else { pMtf("\tc\tController toggle on \nM); } I* endif *I p~tf("\tC\tCalibrate\n"); p~tf("\td\tDisablearm power \nu); p~tf("\tD\tDiscreteoutput \n"); printf("\te\tEnable ann power \nu); printf("\tp\tPosition display \nW); printf("\tP\tPosition reference display \nu); printf("\tq\tQuit \n"); printf("\tr\tReference position \no); printf("\ts\tStatus read \nu); printf("\tw\tWrite a DAC voltage \no); pMtf("\tWtWrite a DAC value in hex \n"); printf("\tz\tZero the DACs \no); APPENDIX D

printf("\tZ\tZero encoder counters \no); printf("\t?\tprint this list \nW); 1 void main (argc, argv, envp) int argc; char *argvO; char *envp; { short keepGoing = 1, jtNum, value, va1[6], ans; long longVal; float actualFreq, volts, posTemp[6];

cntrlFlagG = 0; I* turn off controller *I I* - This call sets up the timer interrupt. *I actualFreq = system-init(300.0); printf("Actua1 interrupt rate is %8.3f Hz \nu,actualFreq);

velRefG[O] = velRefG[l] = velRefG[3] = velRefG(31 = velRefG[4] = velRefG[S] = 0.0; while(keepGoing){ if fibhit()){ ans = getch(); switch (ans) { case 'c': I* Control flag toggle *I if (cntrlFlagG){ cntrlFlagG = 0; } else { cntrlFlagG = 1; } I* endif *I break; case 'C': printf("Enter joint number (1 - 6): "); scanf(" %d",&jtNum); if (QtNum > = 1) && QtNum < = 6)){ calibrateQtNum-1); } else { APPENDIX D

pMtf("Joint number out of range \n"); ) I* endif *I break; case 'd': I* Disable arm power *I pumaVarsG.discrete = pumaVarsG.discrete & - POWER-BIT; robot-intwr(STATUS, 0, &pumaVarsG.discrete); break; case 'D': I* Disable arm power *I p~tf("Discretesare: %04X enter new value in hex: ", pumaVarsG.discrete); scanf(" %xu,&pumaVarsG.discrete); robot-intwr(STATUS, 0, &pumaVarsG.discrete); break; case 'e': I* Enable arm power *I pumaVarsG.discrete = pumaVarsG.discrete I POWER-BIT; robot-intwr(STATUS, 0, &pumaVarsG.discrete); break; case 'p': robot-longrd(POSITION6, 0, &pumaVarsG.pos[O]); printf("Positions: \nu); printf("%61d %61d %61d \n",pumaVarsG.pos[O], pu~naVarsG.pos[l], pumaVarsG.pos[2]); printf(" %6ld %61d %61d \n",pumaVarsG.pos[3], pumaVarsG.pos[4], pumaVarsG.pos[5]); printf("%5.3f %5.3f %5.3f \n",encoderScalcG[O]*pumaVarsG.pos[O], encoderScaleG[l ]*pumaVarsG. pos[l], encoderScaleG[2]*purnaVarsG.pos[2]); pMtf("%S.3f %5.3f %5.3f \n",encoderScaleG[3]*pumaVarsG.pos[3], encoderScaleG[4]*pumaVarsG.pos[4], encoderScaleG[S]*pumaVarsG.pos[5]); break; case 'P': pMtf("Position references: \nW); printf("%5.3f %5.3f %5.3f \n",posRefG[O] ,posRefG[l] ,posRefG[2]); printf("%S.3f %5.3f %5.3f \nn,posRefG[3] ,posRefG[4] ,posRefG[S]); break; case 'q': I* Quit *I keepGoing = 0; break; case 'r': I* Reference position *I printf("Enter joint number (1 - 6): "); scanf(" %dm,&jtNum); if ((jtNum > = 1) && (jtNum < = 6)){ printf("Enter reference position in radians: "); scanf(" % f',&posRefG[jtNum-I]); } else { p~tf("Jointnumber out of range \nV); } I* endif *I break; case 's': robot-longrd(STATUS , 0, &longVal); value = longVal; printf("Status = %04X \nW,value); APPENDIX D

break; case 'w': I* Write DAC value */ printf("Enter joint number (1 - 6): "); scanf(" %dU,&jtNum); if ((jtNum > = 1) 8c& (jtNum < = 6)){ printf("Enter voltage in volts: "); scanf(" % f',&volts); robot-fltwr(VOLTAGE, jtNum-1, &volts); ) else { p~tf("Jointnumber out of range \nM); ) I* endif *I break; case 'W': I* Write DAC value */ p~tf("Enterjoint number (1 - 6): "); scanf(" %dW,&jtNum); if ((jtNum > = 1) 8c& fjtNum < = 6)){ printf("Enter value in hex: "); scanf(" %x",&value); robot-intwr(TORQUE, jtNum-1, &value); ) else { printf("Joint number out of range \nW); ) I* endif */ break; case 'z': I* Zero the DACs */ val[O] = val[l] = va1[2] = va1[3] = va1[4] = va1[5] = 0x0800; robot-intwr(TORQUE6, 0, val); break; case 'Z': I* Zero the encoder counters */ val[O] = val[l] = val[2] = va1[3] = val[4] = va1[5] = 0; robot-intwr(POSITION6, 0, val); break; case '?': print-menu(); break; default: printf("Bad command, try again\nW); print-menu(); ) I* endswitch */ ) I* endif */ ) I* endwhile */

I* - Make sure PUMA arm power is off */ pumaVarsG.discrete = pumaVarsG.discrete & - POWER-BIT; robot-intwr(STATUS, 0, &pumaVarsG.discrete);

I* !!! IMPORTANT !!! - This call is required to restore the system clock */ shutdown(); 1 APPENDIX D 242

D-15 This is the main header file for utilizing functions provided for accessing the PUMA robot using the interface boards from Trident Robotics and Research, Inc.

I* - ROB0TUSR.H - Initial coding by Richard Voyles, Vigilant Technologies *I

I* -- Global variables defincd in ROB0T.C (all globals end in "G") */ extern long clockTicksG; I* number of timer interrupts since init *I extern float clockPeriodG; I* period, in seconds, of interrupts *I extern float clockFreqG; I* actual frequency, in hertz */ extern PUMAstatusT pumaVarsG; I* structure containing PUMA variables */ extern short motorScaleC[6]; extern float encoderScaleG[6];

I* - This function sets up the timer interrupt *I extern float system-init(float freq);

I* - This function re-installs the original timer interrupt handler. *I extern void shutdown();

I* - These functions access the TRC006 */ extern short robot-fltwr(short option, short chan, float *val); extern short robot-intwr(short option, short chan, short *val); extern short robot-fltrd(short option, short chan, float *val); extern short robot-longrd(short option, short chan, long *val);

I* - The following functions are generally not called directly by the - user. The user should place a call to system-init(), which calls - the appropriate routines as necessary. *I

I* - This function sets a new interrupt rate. *I extern float interrupt-rate(float freq);

I* - This function installs a standard timer interrupt handler. *I extern void install-handler(); APPENDIX D

D-16 File for calibration of the robot using the Trident Robotics cards.

/* - CAL1B.C - Initial coding by Richard Voyles, Vigilant Technology *I

I* - These global variables are defined in ROB0TUSR.C *I extern float posRefG[6]; I* joint reference positions *I extern float velRefG[6]; I* joint reference velocity *I

I* - This routine calibrates a single joint, selected by CHAN (0-5). - A joint position controller must be running and it must use - the global variables referenced above. *I int calibrate(int chan) I int keepGoing = 0; short discreteMask = 0x0100, statusMask = 0x0001; long clockNow; float poslnc[6] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001);

if ((chan > = 0) && (chan < =5)){ keepGoing = 1; velRefG[chan] = 0.0; I* Set velocity ref to zero *I discreteMask = discreteMask < < chan; statusMask = statusMask < < chan; I* - Clear the index flag by disabling then enabling *I pumaVarsG.discrete = pumaVarsG.discrete & -discreteMask; robot-intwr(STATUS, 0, &pumaVarsG.discrete); I* disable */ pumaVarsG.discrete = pumaVarsG.discrete I discreteMask; robot-inhvr(STATUS, 0, &pumaVarsG.discrete); I* enable *I I* - Always move the joint toward the zero position. *I if @osRefG[chan] > 0.0){ posInc[chan] = -posInc[chan]; ) I* endif *I clockNow = clockTicksG; } I* endif *I while (keepGoing){ if (clockTicksG ! = clockNow){ I* update posRef each clock tick *I clockNow = clockTicksG; posRefG[chan] + = posInc[chan]; } I* endif *I if ((inport(TRC-BASE + STATUS-REG) & statusMask) = = 0){ I* - An index pulse was found. */ keepGoing = 0;

I* - Grab the index latch and compare it to file data. *I APPENDIX D

} I* endwhile *I I* - Disable the index flag bit for this encoder channel *I pumaVarsG.discrete = pumaVarsG.discrete & - discreteMask; robot-intwr(STATUS, 0, &pumaVarsG.discrete); return 0; 1 Appendix E

El The Approximate Model

The Explicit Dynamic Model and Inertial Parameter8 of the PUMA 600 Arm t

Brian Armstrong, Oussama Khatib, Joel Burdick

Stanford Artificial Intelligence Laboratoy Stanford University size of the models generatcd by these programs varies widely; thew is little consmsus on the question of whether the explicit To provide COSMOS, a dynamic model hed manipulator models can be made sufficiently compact to be used for controL control ryrtem. with an impmved dynomie model, a PUMA 580 Aldon and Liigeois 119841 present an algorithm for obtaining ef. arm war dirarremblad; the inertial propertier of the indimdual ficicnt dynamic models; but none-the-lers momcnd the use of linkr were mcorured; and an ezplicit model incorporating all ofthe recursive algorithms for real time control, claiming that the com. non-zero mearurcd parameterr war derived. The explicit model o/ plete nults ue too complicated for red-time control of robot..' the PUMA arm hor been obtained with o derivation procedure comprised of reveml heurirtic rulcr for rimplijieation. A aim- As we show, explicit dynamic models of manipulaton that pli'cd model. abbreviated from the full ezplicit model with a 1% are more computationally efficient than the alternative recumire rignijicane. criterion, can be cnduatad with 305 calculatioru, one algorithms can be obtained. The computationd cost of the RNE fifth the number required by the recurrive Newton-Euler method. a1:orithm. the full explicit PUMA model, md the explicit PUMA The procedure wed to derive the model ir laid out; the mearured model abbreviated with a 1% significance criterion uc p-tcd inertial parameterr are prerented, and the model ir includcd in an in Table 1. The method presented here for factoring the dpamic' appendix. equations has yielded a dynamic model of the PUMA 560 um. Table 1. Calculations Required to Compute the 1. Introduction Forces of Mot~onby 3 Methoda. The implementation of dynamic control systems for manip ulaton ha, been hampered because the models arc difficult to Method Calculations derive and computationally expcnsivc, snd because the needed Recursive Ncwton-Euler 1560 parameten of the manipulator are generally unavailable. Recur- sive methods for computing the dynamic forces have been avail- Evaluation of the Full able for several yeam [Luh, Walker and Paul 1980a; Hollcrbach Explicit PUMA Model 1165 19801. Several authors have proposed and simulated the use of RNE in control systems [Luh, Walker and Paul 1980b; Kim and Evaluation of the Abbreviated Shin 19851; and [Valavanis, kahy and Sardsi 19851 have used the Explicit PUMA Model 305 RNE to control a PUMA - COO arm. The RNE algorithm has also found use in the computation of forward dynamics for simulation that requires 1165 calculations (i39 multiplications and 426 .d-' [Walker and Orin 1982; Koozekanani et al. 19831. and nominal ditions), 25% fewer than the 1560 calculations required by the 6 trajectory control [Vukobratovit and Kirtanski 19841. The RNE dof RNE. With the application of a 1% sensitivity criterion, h. muts the need for calculation of dynamic forces in these applica- explicit model can be evaluated with one fifth the count of cd;: tions, but does not offer aeveral advantages available provided by culations required by the recursive algorithm. Authermorr, thic an explicit model. The explicit model dlows of the calculation formulation of the explicit model is not optimally compact; decomposition based on a significance criterion or other criteria, torizations that were discovered and emplored during the model and provides a more direct solution for dynamic simulation. derivation have bun expanded out to present explicit cxprrsi- The tremendous siu of an explicit dynamic model is the for each component of the dynamic model. baudand B6c.k greatest barrier to its realization. Comspondingl~.a consider- both report automatic generation of 6 dof manipulator mode! able portion of the effort apent investigating dynamic models for that are more compact than that presented here [Renaud 19% control has been directed toward efficient formulation and aut* Burdick 19851. Their models incorporate nested factorizdi-, matic generation of the manipulator equation. of motion. Pm which were not used hcre. grwfor automatic generation of manipulator dynamics ue re- The count of 1165 calculations for the full PUMA model i~ ported in ILiCgedw et d. 1976; Megahed d Rcn.ud 1982; Cc the total required to erduate the model p-ted in the appmdii sareo, F. Nicolb and S. Nicosia 1984 ; Murray and Neuman 1984; and equation (1) below. This totd md other tot& do Renaud 1984; Aldon and LiCgeoia 1984; Aldon et d. 19853. The not include the calculations required to evaluate the srina and cosines. f Financial kpport forhat author hubeen provided by Hewlett Packard Co., through their Faculty Development Pm- gram. Partial a~pportw= provided by NSF under contract MEA 2. Derivation of the Dymmic Model 80-19628, and by DARPA through the Intelligent Task Automa- The dynamic model used for this andysis follorn from [Lik tion Project, managed by >he Air Force Materials Laboratory, geo;s et d. 1976~. It L: under a subcontract to Honeywell, contract F 33615-82-G5092. APPENDIX E

A(q)q + n(q)[qql + C(q)[q2]+ 6(q) = r; (1) a11 = It + I2 cor2(82) + IS ~oa(B~)coa(8~+ 8,) where A(q) is the n x n kinetic energy matrix; +I, c0r~(8~+ 8,) (3) O(q) is the n x n(n-1)/2 matrix of Coriolis torques; Calculations required: 3 multiplications, 3 additions. C(q) is the n x n matrix of centrifugal toque; is the n-vector of gravity torquca; g(q) where q is the n-vector of arcelerationa; XI= 4m3 + 4m3 + 2 dgdsms + 4m2 + Js,, + J,.. r is the gcncrdizrd joint force vector. +Jz=. + J2wy + JL.. + JI..; etc. ~hcsymbols [qqj and lq2] are notation for the n(-l)/2-vector of Creating It through I,, which are constants of the mccha- ,.,.Iorlty products and the n-vector of squarcd velocities. [qq]and nism, ledto a reduction from 35 to 3 multiplications and fmm j$j arc givrn by: 18 to 3 additions. Computing the ronstant II involves 18 calcu- lations. Since the simple parameten required for the calculatiou of I1 arc the input to the RNE, the RNE will effcctivrly carry o11t the calcr~lationof It on every pms, producing considrral)le un- neressary romprltation. Thirty four lumped constants arc needed The prorctlure used to derive the dynamic niodel entails four by the frtll PUMA model, 8 fewer than the count of 42 simple pa- steps: ramctcn rcqt~irrdto dcscribe the arm. 1. Symbolic Cencration of the kinetic energy matrix and In the third stcp the elements of the Coriolis matrix. b,,, gravity vcctor clrr~ientsby performing the summations of and of the ceutrifugd matrix, cij, are written in terms of the either Lagrange's or thc Gibbs-Alembert formulation. Christoffcl symhols of the fint kind [Corbcn and Stchle 1350; 2. Simplifiration of the kinetic encrgy matrix elements by Libgeois et d. 197G]' giving: coml~iningincrtia constants that multiply common variable cxprrrsioua. 3. Exprcssiou of the Coriolis and centrifugd matrix elcnreuts in trrms of partial derivatives of kinetic energy matrix elemcuts; and rcduction of these expressiom with four where (Qi * 4) is the velocity product in the jqq] vector, and relations that hold on thcse partial derivativd. where I a*. aajt p'.jk = -(L+ - - -1 4. Formation of the needed partial derivatives, expansion of 2 apt aq, aqi the Coriolis and centrifugal matrix elements in terms of the derivatives, and simplification by combining il thc ChristoKcl symbol. incrtia constants ;u in 2. The nuntber of unique non-zero ChristoKel symbols rrqrtired The fint step wm carried out with a LISP program, nked by the PUMA model can be rcduccd from 12G to 33 with four EMDEG,whirh symbolically generates the dynamic model of an equations that hold ou the derivatives of the kinctic cncrg matrix articulated mechanism. EMDEG employs Kane's dynamic for- elc~nents. The first two rqr~ationsarc gcueral, the last two are specific to the PUMA SGO. The equations are: mulation [Kane 19681, and produced a mult comparable in form and size to that of ARM [Muny and Neuman 19841. Thm sim- plifying assumptions were made for this analysis: the rigid body assumption; link G ha been assumed to be symmetric, that is I,, = I,,; and only the masa moments of inertia arc considered. that is I,., I,, and I.. . The original output of EMDEG, includ- ing Coriolis and centrifugd terms, required 15,000 multiplications and 3,500 additions. This stcp might also have been performed with the niomcntum theorem method used in [Izaguim and Paul 19851. The rcduction of Equation (7) arises from the symmetry of Ln the sccond stcp of this procedure, the kinetic energy ma- the kinetic energy matrix. Equation (8) obtaiu~because the ki- trix elements are simplified by combining inertia constants that netic eurrgy inipartcd by the velocity of a joint is iudcpendcnt of multiply common variable expressions. This is the greatest source the configuration of the prior joints. Equation (3) resrtlts from of computational efficiency. Looking to the dynamic model of a 3 dof lnanipulator presented in [MuT and Neuman 19841, we see the symmetry of the sixth and terminal link of the PUMA arm. that the kinctic encrgy matrix element all is given by: And equation (10) hold because the second and third axes of the PUMA arm are parallel. Of the reduction from 126 to 33 unique Christoffcl symbols, GI elit~linatioruare obtained with the general equstiom, 14 more with (3) and a further 12 with (10). Step four requires differentiating the mass matrix elements with respect to the configuration variables. The means to carry out differentiation auto.natirally have been available for some

Calculations required: 37 multiplications, 18 additions. The French authon seem to aasume the use of Cristoffel symbols, while the American authon seem unaware of By com1)iuing inrrtial constants with common variable terms and them. Corben and Stehle, in the 1950 edition of their expandiug ain2(g2)into (1 - ~or~(8~)), equation (2) can be re- text, derive the results required here; but the derivation duced to: is largely omitted from their l9CO edition. APPENDIX E

titllc (LiCgcois et al. 1976; MIT Mathlab Group 19831. Only the calrulatiou of forward dynamics for simulation, where tesselation drrivativcs required after the rirnplification of step 3 need to be is the step size rathcr than servo interval and the cost is run time foreled. Of the 126 derivatives possible when n=6, 46 are required rathcr than boundcd computing power. by the modcl of the PUMA arm. After the nccdrd derivatives Table 2. PUMA 5GO Dynamic Model Evaluation Rate arc for~nrcland expanded into the ChristotTcl symbols, inertial Attainable with l00k FLOPS. ronstrurts that mllltiply common variable cxprcssious are agnin ronil~iucd. - Merkd Rw of Eddr Rate of Our mcthod of modcl derivation is able to sinlplify to man- of C.mCsmntiom Comp.uriom Ikpemdenc Te- asable for111the con~plexsunl-of-product expressions that arc pro- of Torpr duccd by synlbolically carrying out the summatiow of Lagrange's Edmatiom ef the Fdl hlod.1 EdIteratiom 78 h 71 hs cql~atiow. Simplificatiou is in general a non-detcrn~iuistictask that Rrows very rapidly with the number of terms in an cqua- Eduatiom of rhr C.=hrmnlim tion; but the procedure prcscnted is deterministic, with a cost D+p*nd.ml Term om* dvLr ever). low Eduatiou of tbt Vrlmly 60 Lm aoOh that grows most rapidly as p2, where p is the number of sum-of- .nd Arrelrratiom Ikprmdrmt Tern product expressions iu the largest individual kinetic energy ma- trix elemcnt. Our prorcdl~rehm the virtue of producing explicit A final decomposition to be considercd is that for multipro- exprcsaion* for each romponellt of thc dynamic modcl: a rcault cessing, an issue likely to become more important. The recur- that is very uscful for design aualysis and that allows straight sive formulations are well suited to pipeline computation, but forward simplification by application of a scmitivity criterion. poorly suited to multi]~roccsaor computation. For the recursive Stcps 2 through 4 of the above procedure were carried out algorithms. the number of calculations that ran be performed by hand, requiring five wccks of rather tedious labor. To discover cooperating processon is small in rclation to the volume of com- errors, the explicit solution was numerically checked against the munication that is required. Using an explicit model the blocks RNE algori!hm, extcndcd to givc D adC matrix elements in- of parallel computation can be made much larger, and the ratio dividually in a manner similar to that of Walker and Orin. Over of computation to communication comspondingly higher. The a range of configurations, the cxplicit solution of the PUMA dy- decomposition into configuration dependent and velocity or ac- nan~icsagrees exactly with the RNE calculation. It is instructive celeration dependent components is particularly suitable for mul- to observe that the RNE algorithm was coded in 5 hours. 2% of tiprocessing and has bun implemented at the Stanford Artificid the time requircd to dcvclop the full cxplicit modcl. ' Intclligcnce Laboratory [Khatib 19851. S. Several Advantages Obtained from Decomposition of the Explicit Model 4. The Utility of an Explicit Model for Dynamic Simulation The explicit solutiou of PUMA dynamics shows two stmc- turd properties that can be used to advantage: a tremendous Walker and Orin have demonstrated the use of the RNE .I; gorithm in the calculation of forward dynamics for sirnulatic+ range between the largest and the smallest contributing terms By tnkiig advantage of the symmetry of the kinetic energy m- within most equations, and the depend solely upou configurntion trix they have reduced the model order that must be considered of the A, D and C matrix elements. Using the measured PUMA in successive applications of the RNE [Walker and Orin 19821.. parameters an abbreviated dynamic model has been formed. This The RNE algorithm has also been used to compute dynamia modcl is derived from the full PUhU model by eliminating all for simulation in fields outside of robotics (Benati et al. 1989. terms that are less than 1% as great as the greatest term within Koozekanani et al. 19831. Prcamtcd in table 3 arc the numba . the same equation, or less than 0.1% as great as the largest con- of calculations quidto compute the elements of the kineti< stant term applicable to the same joint. All of the elements of the energy matrix using Waker and Orin's method, wing the fd A, B and C matrices are retained: the significance test is applied PUMA model, using the simplified model reported in [ha& on an equation hy equation basis. The reduction in rcqtlired cal- and Paul 19851, and using the abbreviated (1% significance crit+' culatiow achieved via the significance test is roughly a factor of rion) model. The analytic models all show a tremendous advan:, four, as shown in Table 1 above. tage over the RNE algorithm. Observing that the A, B and C matrix elements depend only Table S. Calculations .It uircd to determine the Kinetic on configuration, it is possible to decompose the calculation into Energy ~atnx%ementsfor a PUhU 560 Arm. configuration depcndcnt and velocity or acceleration dependent components. Because configuration changes more slowly than Method Calculations velocity or acccleration, the configuration dependent components may be computed at a slower rate [Khatib 1985; lzaguim and Walker and Orin 2737 Paul 19851. Shown in Table 2 is the evaluation rate of the PUMA NIExplicit Model 560 dynamics that can be achieved with 100,000 floating point 278 operations per second, the approximate speed of a PDP-11. in izaguirre and Paul the first case the entire model is recomputed in each p-; in the Simplified Model 58 second case the A. B and C matrix elements are computed only once for every four iterations of the multiplication by velocitp and Abbreviated Explicit Model 25 acceleration vectors. This partitioning of the dynamic calculation reduces the pace of computing the ronfiguration dependent 5. Measurement of the PUMA 680 Dynamic Paramet- by one third; but increases the pace of computing the velocity and acceleration dependent terms by a factor of two and one half. The link parameten required to calculate the elcmen+i of The advantage of this decomposition applies equally well to the A. B. C and g in equation (1) are mass, location of the APPENDIX E

gavity and the terms of the incrtia dyadic. The wrist, link three and link two of a PUMA 5CO arm were detached in order to mcasure thcse parameters. The mius of each component waa dctcrlnincd with a beam balance; the center of gravity was located where I is the inertia about the axis of rotation; by balancing each link on a knife edge, once orthogonal to each Mg is the weight of the link; axis; and the diagonal terms of the inertia dyadic wrre measured r is the distance from each suspension with a two wire susy~cnsion. wire to the axis of rotation; w is the oscillation frequency in The motor and drive mechanism at each joint contributes to radians per second; th,. iucrtia alorlt that joint an arnollnt eqrral to the inertia of the I is the length of the supporting wires. rotating picccs magnified by the gcar ratio squared. The drives md retluction gem were not removed from the links, so the total Measurement of the Motor and Drive Inertia nlotor and drive contribution at each joint was determined by an idrutificatiou method. This contribution is considered separately A parameter identification method was used to learn the frorn the I.. term of the link itself because the motor and drive total rotational inertia at each joint. This inertia includes the iuertia secn through the reduction gear does not contribute to the eliective motor and drive incrtia and the contribution due to the inertial forces at the other joints in the arm. The moton were mass of the arm. To make this measurcnlcnt our control sys- [eft installed in links two and three when the inertia of these links tem was configured to command a motor torque proportional to werc mcasurcd, so the cffcct of their mass as the supporting links displacement, effecting a torsional spring. Lly mcruuring the pe- move is correctly considered. The gyroscopic forces imparted by riod of oscillation of the resultant mass-sprinr: system, the total the rotating motor rrrmaturw is neglected in the model, but the rotational inertia about each joint was dctermincd. By subtract- data prescnted below include armature inertia and gear ratios, so ing the arm contributions, determined from direct mciuurcments, thrsr forccs can he determined. from the measured total inertia, the motor and drive inertial con- The parameten of the wrist lids were not directly measured. tributions were found. The wrist itself ww not disassembled. But the needed parameters Measurement Tolerance were estimated using mewurements of the wrist mw and the external dimensions of the individual lis. To obtain the inertial A tolerance for each direct measurement was established as terms, the wrist links were modeled as thin shells. the memurement was taken. The tolerance values are derived I from the precision or smallest vaduation of the measuring in- Measurement of Rotational Inertia strument used, or from the repeatability of the measurement it- The two wire suspension shown in Figure 1 war used to mea- self. The tolerances are reported where the data are presented. sure the I.., I,, and I.. parameters of Iiitwo and thm *. The tolerance values assigned to calculated parameten were de- With this arrangement a rotational pendulum is created about termined by RMS combination of the tolerance sssigned to each an axis parallel to and halfway between the suspension wires. direct measurement contributing to the calculation. The inertia Tbc link's center of graviq must lie on this &. The two wire dyadic and center of gravity parameten of link 3 were measured suspension method of memuring the rotational inertia requires with the wrist attached; the dues reported for link 3 alone have knowledge of parameters that arc easily meanucd: the mars been obtained by subtracting the contribution of the wrist from the total of link 3 plus wrist. Tolerance values are reported with the values for link 3 pllu wrist, as these are the original measure- ments.

6. The Measured PUMA 560 Parameters

The mass of links 2 through 6 of the PUMA arm are reported in Table 4; the mass of link 1 in not included because that link w;u not removed from the base. Separately measured mass and incrtia terms are not required for link one because that link rotates only about its own Z axis . Table 4. Link Masses (kilograms; f0.01 + 1%) Figure 1. The two wire suspension wd for Rotational Inertia Measurement. Link Mass Link 2 17.40 of the link, the location of the center of gravity, the distance from Lid 3 4.80 the wire attachment points to the axis of rotation, the length of Li 4* 0.82 the wires, and the period of rotationd millation. The inertia Link 5* 0.34 about each axis is measured by eon6gnring the link to swing Link 6' 0.09 about that axis. Rotational oscillation is started by twisting and releasing the link. If one is careful when releasing the link, it Link 3 with Complete Wrist 6.04 is possible to start fundamental mode oscillation without risi- Detached Wrist 2.24 bly exciting any of the other modes. The relationship betwecn measured properties and rotational inertia is: * Values derived from external dimensions; *25%. The positions of the centers of gravity arc reported in Table 5. This method was suggested by Prof. David Powell. The dintensiom rr. rr. and r. refer to the x, y and z coordinates APPENDIX E

of the center of gravity in the coordinate frame attached to the 'fable 6. Centers of Grarity. (meten f0.003) link. The coordinate frames used am assigned by a modified Denavit-Hartmbrrg method [Craig 851. In this variant of the Druavit-Hartcnbrrg method. frame i is attached to link i, and axis Zi lies aioug the axis of rotation of joint i. The coordinate fra~neattachments are shown in Figure 2; they arc located sr follows: Link 1: Z axis along the axis of rotation, +Z up; +Y1 11 +22. Lid 2: Z axis along the axis of rotation , +Z away from the Ime; X-Y plane in the center of the link. with +X toward link 3. Link 3: 23 11 22; X-Yplane is in the center of lid 3; +Y is away fro111 the wrist. Link 4: The origin is at the intrnection of the axes of joints 4 5 and 6; +Z4 is along the axis of rotation and ' Values derived from external dimensions; f25%. dirrctcd away from link 2; +Y4 11 +Z3 when joint 4 is in the zcro position. The effective torsional spring method of incrtia meiuurcment wa. applird at each joint. The motor and drive inertia, I,.,,, Lid 5: The orihn coiucides with that of frame 4; +ZS is di- were hind by subtracting the inertial contribution due to the rcctccl away from the II~SC;+Y5 is directed toward arm dynamics, known from direct measurements, from the total lid 2 when joint 5 is in the zero position. incrtia measured. The uncertainty in the total inertia measure- Liuk G: The origin coincides with that of frame 4; when ment is somewhat higher at joint one becam of the larger friction joints 5 and G are in the zcro position frame G is at that joint. It was necessary to add positive velocity feedback -0.1) aligned with frame 4. (damping factor to cause joint one to oscillate for several cycles. Wrist: The dimensions are reported in frame 4. ' Table 6. Diagonal Terms of the Inertis Dydics and Effrrtivr Motor Inertia.

The gear ratios, maximum motor torque, and break away torque for each joint of the PUMA is reported in Table 7. The maximum motor torque and break away toque values have been tdrcn from data collected during our motor calibration process. The current amplifiers of the Unimatc controller are driven by 12 bit D/A converters, so the nominal toque resolution can be Figure 2. The PUMA 5GO in the Zero Position with Attached lbtained by dividing the reported maximum joint toque by 2048. Coordinate Rames Shown. Table 7. Motor and Drive Parameten The inertia dyadic and effective motor and drive inertia terms are reported in Table G. For each link, the coordinate frme for the inertla dyadic terms is placed at the center of gravity, parallel to the attached frame used in Table 5. The tolerances assigned to these measurcments are shown in parenthesis. No tol-rance is associatrd with the value of I,. for link one because this value was not directly measurcd; ~t was computed back- I from the measured total joint inertia. It is not important to 7. Conclusion distinguish I.,1 from the ml x ryi2 term or from the motor and arc I drive inertia at joint one because tl~caecontributions an neither Explicit dynamic models of complex manipulaton attain- able. The PUMA 560 arm b as complex as any 6 dof arm with a t configuration dependent nor appear in any term other tban all. The total !ink 1 incrtia measured by the identification method is spherical wrist, yet a deterministic simplification procedure has the sum of I..I and I,.,, in table 6. produced an explicit model that is more economical than the APPENDIX E

RNE algorithm. With the application of a stringent significance ence, San Framisco, June 22 - 24, 1983, pp 491 - 496. criterion, the computational cost of the explicit model is rcduced to one fifth that of the mumive alternative. The availability of M.J. Aldon and A. Licgrois, 'Computational mpects in robot me~ureddynamic parameten provides improved accuracy in the dynamirs motlclling." Advanccd Software in Roboticr ed. A. calculated fox- of motion and simplifies model generation by al- Dmtkine and M. G(.radi&, Elsrvirr Science Publishers, North- lowing one to omit zero dueparameters. Ib automatic model Holland. 1984. pp. 3 - 14. generation becomts available and manufactwm become aware of G. Ccsarco. F. Nicolb hnd S. Nicosia. 'Dymir: A code for gen- the need lor dynamic parameten, we expect to see increuing use erating dynamic model of robots," Proc. 1981 International of explicit models and measured parameten, in the calculation of Conference on Roboticr. Atlauta, Georgia, March 13-15, 1984, dynamics for control. pp. 115 - 120. J.J. Murray and C.P. Nrulnan, 'ARM: au algebraic robot dy- H.C. Corben and P. Stehle, Clarrical Mechanicr; New York: Wi- namic modrling program," Proc. 1981 International Confer- ley, 1950. ence on Rohoticr. Atlanta. Grorgia, March 13.15, 1984. pp. 103 - 114. T.R. Kane, 'Dynamicr;" New York: Holt, Rinehart and Win- ston, Inc., 1W8. M. Renaud, 'An cffirient iterative analytic procedum for obtain- ing a robot mauipulator dynamic model." Roboticr Research. A. LiCgois. W. Khalil. J.M. Dumas, and M. Renaud, 'Math- rd. M. Drady aud R. Paul, MIT Press, Cambridgc, 1984, pp. cmatical and computer models of interconnected mechanical 749 - 764. systcms,"(Scpt.. 1976, Wanaw) Theory and Practice of Robo* and Manipulatorr; New York: Elscvier Scientific PublisLing M. Vukobratcrvit and M Kirtanski. 'A dynamic approach to nom- Co., 1977, pp. 5 - 17. inal trajectory synthesis for redundant manipulaton,' IEEE Traru. on Syrtemr, Man, and Cyberneticr, vol. SMC-14, no 4, M. Drnati, S. Gaglio, P. Mormso, V. Tagliaaeo and R. Zaccaria, pp. 580 - 586. July/Augut 1984. 'Anthropomorphic Robots,' Diologacal Cykrncticr, vol. 38, M.J. Aldon. A. Li&geois, B. Tondu and P. Touron. 'MIRE. A pp. I25 - 140, March 1980. software for computrr-aided design of industrial robots." Proc. J.M. Hollerbach, 'A rccunivc lagrangian formulation of manip CAD, CAM, Roboticr and Automation Con/erence Tucsan, ulator dynamics and a comparative study of dynamics formu- Arirona, February 1985, pp. 1 - 6. lation complrxity,' IEEE Tram. on Syrtemr, Atan ant Cyber- J. Durdick, Private Communication. neticr, vol. SMC-10, no. 11, pp. 730 - 736, November 1980. J.J. Craig, "Introduction to Roloticr: Mechanicr and Control," J.Y.S. Luh, M.W. Walker and R.P.C. Paul, 'On-line coqputa- Reading, M=sachusctts: Addison - Wesley, 1985. tlonal scheme for mechanical manipulaton,' ASME J. oJDy. namic Sydtemr, Mearurcment, and Control, vol. 102, pp. 69 - A. Izaguirre and R.P. Paul, 'Computation of the inertial and 76, June, 1980. gravitational coefficients of the dynamics equatiom for a robot manipulator with a load.' Pmc. 1985 International Confirencc J .Y.S. Luh, M. W. Walker and R.P.C. Paul, 'Resolved-accelera- on Roboticr and Automation, St. Louis, March 25-28, 1985, pp. tion control of mechanical manipulators,' IEEE Tram. on Au. 1024 - 1032. tomatic Control, vol. AC-25,no. 3, pp 468 - 474, June 1980. J. Hollerbach, 'Dynamics,* Robot Afotion ed. M. Drady. 0. Khatib, 'Real-tin~e obstacle avoidance for manipulaton and J. Hollerbach, T. Johnson, T. Lozano-PCm and M. T. Ma- mobile robots, Proc. 1985 International Conference on Roboticr son, MIT Press, Cambridge, 1982, pp. 51 - 53. and Automation, St. Louis, hiarch 25-28, 1985, pp. 500 - 505. S. Megahed and M. Renaud, 'Minimization of the computation B.K. Kim and K.G. Shin, 'Suboptimal control of industrial ma- time necessary for the dynamic control of robot manipula- nipulators with a weighted minimux time-fuel criterion.' IEEE ton." Proc. of the 12th International Symporium on Indurtrial Tramactiow on Automatic Control, vol. AC-30, No I., pp. 1 Robotr / 6th Con/erence on IndurtrialRobot Technology, Park, - 10, January 1985. June 9- 11, 1982,pp4G9-478. 1C.P. Valavanis, M.B. Leahy and G.N. Sardis, 'Real-time evalua- tion of robotic control methods Proc. 1985 International Con- M.W. Walker and D.E. Orin, 'Efficient dynamic computer simu- ference on Roboticr and Automation, St. Louis, March 25-28. lation of robotic mechanisms,' ASME J. of Dpamic Syrtemr: 1985, pp. 644 649. Meaaurcment, and Control, vol. 104, pp. 205. 211, September, - 1982. 0.Khstib, 'Dynamic control of manipulaton in operation$ APPENDIX space,' Pa. Sirth IFToMM Congrerr on Theory of Machiner The fill Expressions for the Force. of Motion of and Mechanismr, New Delhi, December 15-20, 1983. a Puma 660 Arm S. H. Koozekanani, K. Barin, R.B. McGhee, and H.T. Chang, *A recursi~efree-body approach to computer simulation of human In the following tables the exprtssiom lor the elements of postural dynamics," IEEE Tram. on Biomedical Engineering, the A,B and C matrices and the g rector are These vol. BME30, no. 12, pp. 787 - 792, December 1983. cxprcasioru are made in terms of constants which have units of inertia or torque, and trigonometric tern that are functions of MIT Mathlab Group, "MACSYMA Reference Manual," MIT, Cambridge, 1983. the joint angles. We have abbreviated the trigonometric func- tions by writing S2 to mean sin(qa) and C5 to mean cos(q5). R. P. Paul, M. Rong and Hong Zhang, 'The dynamics of the Wh~na trigonometric operation is applied to the sum of several PUMA manipulator," Proc. 1983 American Control Confcr- annles we write C23 to mean cos(qt + q3) and S223 to mean APPENDIX E

sn(q2 + 92 + 93). And whcn a product of several trigonometric oprrations on the same joint variable appears we write CC2 to mean cos(q2) cos(q2) and CS4 to mean cos(q4) * ain(q4). These final abbreviations, CS4 rtc. , are considered to be factorizations; md the cost of romputing these tcrms is included in thr totals I, E ml r,~(dy + r,,) + mr a, r rZl rcportcd a1)ove. +(m, + m, + ms +ma) *or (dr +I); The position of zero joint angles and coodinate frame at- I, = -m, s ny ry, + (m, + mr + mo) as * d, + m, at r,, ; tnrl~mrntsto the PUMA rum are shown in Figure 2 above. The I6 = Iris + m, * r,,' + m, asy + m, (d, + r,,)' + Ira modified Dcnavit-Hartenberg parametcn, awigned according to +m, .a3' + ms d,' +I,.$ + m6 a,' + m6 d, thc mcthod prcsrntcd in [Craig 851, are listed in Table Al. +ms riBy + I..6 i 17 = ma r,s1 + I,., - Ins + ml r,,' + 2 m, *dl r., Table Al. Modifi~dDcnavit - Hartenberg Parameters +(nr, + pn, + mg) (day - as1) + llr,- I*,, + I=.s -IuY~+ m6 r,6' - + I.,6 ;

The rquatious of the PUMA modcl constants arc prcseutcd in Table AZ; these congtants appcar in the dynamic crluations 11s" m6 (dr + da) r,s i of Tables A4 through A7. I..;and mi rcfer to the second mo- ment of link i about the z axis of frame i and th~mas of link Ilo= IIY,- I,,, + I.*s - IVY,+ ms r,e' + I=.~- 1116 i i respectively. The tcrms a; and di are the Denavit-Harterbcrg Ito = I,,, - I,,s - m6 r,6'+ lzz6- I..~ ; parametcn. Terms of the form r.; arc the offsets to the center Ill = I.*, - I,", + I.., - I**s i of gravity of link i in the ith ccoorlnatc frame. In Table A3 the values of the model constats are listed. The terms I,; are the motor and drive train contribution to inertia at joint i. The equations for the elements of the kinetic energy matrix, Part 11. Cravltlond Conatant. A(q), are prcscnted in Table A4. A(q) is symmetric. so only 01 = -~*((m~+m,+m,+m~)*a,+m,*r.~)~ equations for elements on and above the matrix diagonal are pre- sented. The equations for the elements of the Coriolis matrix, B(q), 0, = -g (m, + ms +me) 0s i are presented in Table AS. The Coriolis terms have been left in the form of a three lmensional array, with a convention for the indices that matches that of the Cristoffel symbols. Element Pt' multiples qk and 9, to give a contribution to the torque at joint i. Table AS. Computed Vdun for the Constants Appeuing The Coriolis matrix may also be written as a 6 x 15 array, where in the Equations of Force of Motion. the 15 columns correspond to the 15 possible combinations of (Inertial constants have units of kilogram metem-sqnd) joint velocities. The equations for the elements of the centrifugal matrix, C(q),arc pmented in Table A6. And the equations for the terms of the gravity vector, g(q), are pmented ill Table A7. A load can be represented in this model by attaching it to the 6" link. In the model the 6'" lid is assumed to have a center of gravity on the axis of rotation, and to have I..s = I,,s; these restrictions extend to a load represcnted by changing the 6Ih link parameten. A more general, though computationdly more expensive, method of incorporating a load in the dynamic calculation is presented in [hag-uim and Paul 19851.

Table A2. Expressions for the Constants Appearias in the Equations of Forces of Motion. Iml = 1.14 f 0.27 I = 4.71 f 0.54 , ' 1,s = 8.27~10-' f 0.93x10-' I,, e 2.00xlO-' f 0.16~10-~ 1,s = f f Part I. Inertlal Conmtantm 1.79~10-' 0.14~10-~ I,, = 1.93x10-' 0.16x10-' 11 = I,,I + ml ryly+ma *dl1 + (ml + ms + ms) *a$' (Gravitational constants have units of newton meten) +my r,ty + (m, + m, + m~ + ms) (dl + dalY gl = -37.2 f 00.5 gy = -8.44 f 0.20 +f,,a + I,,, + 2 r m. dl r,y + my r,ra + mt r.,' g, r 1.02 i 0.50 g4 = 2.49~10-' f 0.35~10-' +2 ma (da + dt r., +I*,, + I,,s + Iza6 i gs = -2.82~10" f 0.56~10-' APPENDIX E

Table A4. The rxpressionr giving the elements of the kinetic +Il6 (C223 C5 - S213 C4 S5) + Ill SC23 CC4 enrrgy matrix. +Iyo ((I + CC4) SC23 SSS - (I - 2 SS23) C4 SCS) (The Al>brrviatcd Expmrions have units of kK-my.) +Ily ((1 - 2 SS23) C5 - 2 SC23 C4 5511

nll = Im~+I~+Is*CC2+h*SS23+Ilo*SC23+III~SC2 +Ilo (SS5l (SS23 (I + CC4)- 1) - 2 r.SC23 r C4 SC5) +Iyl *SS23*CC4+2*(I~*C2*S23+I~~*c2*C23 +Il$ (SS23 CS + SC23 C4 SS) +Il6 C2 (S23 CS + C23 C4 S5) +Ins S4 S!, + IYY (SC23 CS + CC23 C4 SS)) ; zz 22.7 + 1.38 * CC2 + 0.30. SS23 + 7.44~10-~C2 523 .

a13 = Is C23 + 11s 523 -41s C23 S4 a S5 + 119 r S23 SC4 +II~*(S~~*C~*S~-CZJ*C~)+I,~*S~~*S~*SS +Iyo S4 * (S23l C4 CC5 + C23 SCS) ; z -1.34~10-I C23 + -3.97~10-' r S23 . Ills = 2 (lyo (SCS (CC4 (I - CC23) - CC23) -SC23 C4 (1 2 SS5)) II (5.723 S5 SC23 G4 CS) a14= 11,* C23 + 11sl S23 C4 S5 + Its C2 C4 S5 - - * - * +I18 C23 S4 S5 - lyo (S23* C4 SCS + C23 SS5) -1,s *CZ.(S23.SS-C23*C4.C5)+I,~ .S4*C5 +I2,.C23*C4*S5; zO. +Il, .(CC23 C4 a C5 - SC23. S5)); = I~J*S~~*S~*C~+I~~*C~*S~*CS+I~~*S~*S~% -2.50~10-' (SS23 S5 - SC23 C4 C5) +Il~*(S23*S5-C23*C4*CS)+I~yrC~*S4*C5; - 2.48~10-' C2 (S23 S5 - C23 C4. C5) + 8.Cnx lo-' S4 C5. zo. ay1 = I,y + 1.1 + Is + 110 SS4. SS5 + Ill sS4 4 +2 (I., * S3 + 11. C3 + 11s r CS +I16*(S3*C5+C3*C4*S5)+Izr*C4*SS); % G.79 + 7.44~10-I S3 .

a,, = -11s S4 S5 + Iyo S4 l SC5 ; co. cz -1.25~10-b s4 S5 .

= 11, S23 + 119 S23 (I - (2*SS4)) +2* (-lls*C23*C4*S:,+Il6 *S2*C4*SS +I10 (S23 (CC5 CC4 - 0.5) + C23 C4 SC5) +Ily S23 C4 r SS) ; cz l.~lxlO-' r S23 - 2.50~10-' C23 C4 S5 + 2.48~10-' S2 .C4. SS + 0.30~10-' S23 (1 - (2 SS4)).

Table AS. The expressions giving the elcments of the Corioh matrix (The Abbreviated Expressions have units of kg-my.) APPENDIX E

= 2*(-116.C3*S4*S5+Ilo*SC4*SS5 b.34 = 4x4 beas = be,, - bas4 = 0 . +Ill .SC4 - Iyy S4 SS) ; be45 = b,ar . b4.= 0. bra=O- = -2.48~10-a C3 S4 8 SS .

bys =2.{-I#~.SS+II.*(C3*C4.C5-S3*SS) Table A6. The expressions for tbr terms of the centrifugd matrix. +bo SS4 SC5 + Irr C4 C5) ; (Tbc Abbrrviatcd Exprruions have units of kt-ma.) z -2.50x10-' SS + 2.48x10-' (C3 C4 CS - S3. S5) . by16 = 0 - by34 = by, - cay = +I4 C2 - I8 S23 - ID S2 + 11.7 CW baas = 41s . baa4 = 0. +Il,.S23*S4.S5+I16*C2*S4.S5 bads = 2. {-Ils S4 C5 - 116 S3 S4 C5) +IIa (C23 C4 SS + S2f C5) + Ill C23 SC4 -I1, r S4 + Iyo .S4 (I - 2 SSS) ; zz 0. +IlO S4 (CW C4 cC5 - S23 .SC5) +I,l*CW.S4*s5; bya6 = Iya C4. S5 ; zz 0 . - s 6.90xlO-' C2 + l.S4~10-' S23 - 2.38~10-' S2. b,,. = Is, .S4. CS ; s 0. ball = 0. 6x1s = 0. bald =~*(-II,*CZ~*C~*S~+~Y~*S~~*C~.SS +Iyo (S23 r (CCS CC4 - 0.5) + C23 C4 SC5)) +I,, 523 + I1o S23 (I - (2 SS4)) ; zz -2.50~lo-' C23 C4 55 + 1.C4xlO-' 523 + 0.30~10-' S23 (I - 2 SS4) . lSls= 2 r (-Ils C23. S4 CS + I,, S23 S4. (75) -II, C23. S4 +I~o*s4.(C23.(1-2.ss5]-2*S23.C4.SC5); cyy = 0. cya = 0.5. by,, . s -2.50~10-~8 C23 S4 CS - 6.42xlO-' .C23 S4 . cy, = -11,*C4*SS-I~.*SS*C4*S5+Ilo*C4*SC5; ba16 = -ha6 - b,y~ = 0 . z 0. b,,, = 2 r {Iyo SC4. SS5 + IYISC4 - Iyy S4 .S5) ; z0. 1 bays = 2. (-Il, S5 + lye 554 SC5 + 121 C4 * C5) ; rz -2.50~10-' S5 . bsy6 = 0 . hat4 = ~JY,. c,, = -I1,. C4. SS + 1.0 C4. SCS ; 43s = ~SYJ. ba,, 0 - zz -1.25~10-' C4 S5 . b,,, = -Il, 2 S4 CS - 11, S4 + Iy, S4 (1 - 2 5.55) ; s -2.50~10-' * S4 C5 . b346 = ha6 ~SJ. = bas6 b.12 = -by14 . . . c4s = 0.5 bays . c,, = 0. c,, = 0. b41a = -h14 - b,~, = 0. C.6 = 0. csl = -0.5 blIs . csy = -0.5 qls . bils = -llo. (S23. C4 (I - 2 SSS) + 2 C23 SC5) -Ilr 523 C4 ; z -1j.42~10-' r S23 C4 .

b416 = . bal~= -by, b414= 0 . bays = In, S4 + Iyo S4 (1- 2 555) ; zz 6.42~10-' S4 . Table A7. Gravity Terms. bas, = 0. biy6 = -&is . (The Abbreviated Expmsions have units of newton-meten.) b4as = bas . b416 = -bad6 - b,is = -Izo 2 SC5 ; % 0 .

b446 = 0 5 bis6 = -Il, SS ; z 0. 41r = -byas - bsls = -baas - bsl4 = 441s - bsls = 0. bs16 = 4156 ~JYJ= -bays b~l4= -b,zs - bsys = 0. bsy6 = -bs6 bsm = bsl, . bsas = 0 . bsa6 = -bas6 - bs,, = 0. 8546 = -b4s6 . bss~= 0 . b61~= b116 . 41s = ha6 . 414= b1.6 - 41s = b156 . be16 = 0. b6ya = 0 . 414 = by46 bsrr = bas6 . b6y6 = 0 .