Ohiou1174932976 PDF

: 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 TRODUCTZON
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 ii measured along ii;and
Oi (angle), is the angle between ki-, and ii measured 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 1181).
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,.
: given in Appendix A for the PUMA 560 manipulator,
Considering the equations for T
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
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
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 KIZVEMATICS 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
u = tan-
0
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,.
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

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 aM,. .
ar, = -C-e.e-- 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
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
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) = - ~ ( e ) ~ - l ( e ) ~e),( e )
J-~(O)(V(~,~) (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
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
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.813187 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
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
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
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.
Real vs. Approximate Models

1s
1
QI Real Model
4.8
0
-
01
-1 -- Appmx. Model
U
4
JI
4
4s- I
4- 1
U- ! . .
M I L l O L O I M O I O I a . 7
. . . . .
Time in sec.

(OD
m
Real Model
m
-
m
FO
-- Approx. Model
e2 m
a
0
m
10
~ ~ 1 O L O I o * O T O . 0 . 7
Time in sec.

8
U
I Real Model
LI
U
4 -
a -.
03 u Approx. Model
2
11
1
QI
0
1 - 1 I A 1 I
1 1 1 I Y - - Y I I I
. l J ! ! ! ! ! ! ! !
w a i ~ u a ~ o r ~ a ~
Time in sec.
Real Model
-
--
Approx. Model
OD0.1 0 1 0 1 0 1 0 1 0 ) 0 . 7
Time in sec.
Real Model
-
--
Approx. Model
0 1 ) a 1 0 . 2 a a O A Q I O J a 7
Time in sec.

-8
OIIlOIa
QODPU
Real Model
QODDIL -
86-
QQXDI
-- Approx. Model
Mma
oamrw
ammz
0
QOrmz
Q O D O D L . . . . . . . .
O D ~ 1 0 1 0 1 0 1 0 . 6 4 1 ~ 7
Time in sec.
Figure 4.2 Variation of Joint angles (degrees) for 6, - 0, w.r.t Time

Real Model
-
--
Approx. Model
M Q l M Q I O A P I M Q 7
Time in sec.
Real Model
-
--
Approx. Model
o J t / ! ! ! ! ! ! !
QO 0.q 01 aa OA M oa a7
Time in sec.
Real Model
-
--
Approx. Model
M ~ ~ Q I O S M M O J ~ ~
Time in sec.
I I
Figure 4.3 Variation of Joint velocities ( 8 ' , d e ' J w.r.1 Time
Real Model
-
--
A p p r o ~Model

ODY
om
a Real Modal
-8
OBl8
-
om4 -.
omr Apprax. Model
-
85'
OPll
OPll
OPY
-d 40#
W O . l o l ~ 0 l Q l W O 7
Time in sac.

QmD(p1
onma?
-a Real Model
-8
-0
-
0-2 --
86'm Approx. Model
oomm,
OODDDDl
-
0-
MDDrm
A
- M a f w m 0 4 Q l a a a 7
Time in sec.
Figure 4.4 Variation of Joint Velocities

.'8( a 8 . r . t Time

P
1s
(0 Real Model
s
0
-
4 --
P.E. .lo AFWOz IbfOdd
-16
9
.a
Q
Q
0
4
a o a 1 0 1 0 1 M P I o . w
' I S ~ ~inI sec.

m
m
Y Real Model
e
m -
0 --
ICE.r A p p ~Modal
.
rn
I
m
11
90
s
0
Q D Q l 0 1 I W M P I O S 0 . 7
Time in scc.

7.76
7.7
7 s Real Model
7a
7.66 -
7X
-.
T.E.~: Approx. Model
7 s
7s
7 0
72
7.11
7.1
7B
7
o D a 1 O ) u w
Time in sec.
Figure 4.5 Variation of Potential, Kinetic and Total Energies w.r.t Time
I
I
Figure 4.6 Plot showing Intermediate steps when the PUMA 560 fails under gravity (Real Model).
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

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, toand 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.
I Real vs. Approximate Model
Real Model
Aprx. Model
IFigure 5.1 Torque in N-m (-ve)

An example lot showing the ioint torclues
I
Real vs. Approximate Model
Real Model
I Aprx. Model
Force in N
Figure 5.2 An examvle lot showing the forces
I
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 1991.
[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 1991.
[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 61
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 1991.
[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 1991.
[27l Ganguly, S., Tarn, T.J., and, Bejczy, A.K., "Control of Robots with Discrete
Non-Linear Model: Theory and Experimentation", Proceedings of the 1991IEEE
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
IntergraphIEngineeringModeling 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,
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 1991.
[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,
[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 1991IEEE 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 = J 6 0 ,
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 = J T 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% + s d 4 + 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 = - c l c ~ 3 8+1 s,s&,8, - c,s&.$i, - s,c&.$i, + s&B2 + s , d d 1

512 = - ~1~243423
~ 1 ~ 2 3 4 1 - s1cd44 - c1sdd1 + s , s p , ~ ,-c1cfi,82
J13 = sIsuad1 - c 1 c f i A - 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 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 g l 1,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 < = 4 i + +)
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 d e g ) : - - - - > 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 <<check k31 is same as r > > %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 * k l l -ze*ze),ze );

angles[2][2]= atan2(-kll,k21) - atan2( sqrt(k21*k21 + kl l * k l l -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 < <check k32 is same as r > > %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);
g l l =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,gl 1,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( l h s m t r i x [ 3 ] [ 3 ] / s i n ( a n g l e s [ 5 ] [ l ] ) , l h s m t ~ ] [ 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( - I h s m t r i x [ l ] [ 2 ] , l h s m t ~ l ] );)
else
{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 D E G %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
{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 D E G %f % f
%fin",angles[4][3]*180.0/pi,angles[5][3]*180.0/pi,angles[6][3]*18O.O/pi);
APPENDIX D
p ~ t f ( " t h erespective angles in DEGREES follow\n");

for(i=l;i< =6;i+ +)
p ~ t f (%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
{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 >
/* 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 j a c [ 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 ~ t f ( " j a csubr %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] = s l *s23*a3*thdot[l] - c l * c 2 3 * a 3 * ( t h d o t [ 2 ] + t h d o t [ 3 ] ) - s l * c 2 3 * d 4 * t h d o t [ 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
/* Defining Expressions for the Inertial Constants Appearing in the Equations of Forces of Motion *I
#define i l l (-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 )
#define TINY 1.0e-20;

#define N (6) /***for 6 x 6 "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
+ p o s n [ 3 ] ) * s i n ( p o s n [ 4 ] ) * c o s ( p o s n [ 6 ] ) ) * - 0 . 1 4 3 +
( - s i n ( p o s n [ 2 ] +posn[3])*cos(posn[4])*sin(posn[5]) + c o s ( p o s n [ 2 1 +posn[31)*cos(posn[51))*0.014
-sin(psn[2] +posn[3])*a3 +cos(posn[2] +posn[31)*d4-sin@osn[21)*a2 ) );
printf(" p o n s = %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);
f p ~ t f ( s t d e r r , %s\n"
" ,error-text);
f p ~ t f ( s t d e r r , "..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 i s . , 9 *I
void derivative(x,y ,dydx)

float x,yU,dydxu;
{
void armat();
void brmat();
void CRmat();
void grmat();
int i j ;
+
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<j;i++) {
sum=a[i][i];
for ( k = l ; k < i ; k + +) sum -= a[i]F]*aF]lj];
a[i][i] =sum;
1
big=0.0;
for(i=j;i< =n;i+ +){
sum=a[i][i];
for ( k = l ; k < j ; k + + )
sum -= a[i]F]*aF][i];
a[i][i] =sum;
if ( (dum=w[i]*fabs(sum)) > = 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 "Conservationof Energy
Approach") and find the computer processor time taken for the Approximate Model
#include < stdi0.h >
#include < math. h >
#include <stdlib.h >
#include <time.h >

#define N (6) /***for 6 x 6 "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 i d e n [ 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 . 8 1 * ( 1 7 . 4 0 * ( - s i n @ o s n [ 2 ] ) * 0 . 0 6 8 - c o s ( ~ o s o s n [ 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 i j ;
APPENDIX D
return;
1
void bprnatpp)
float bp[7[16];
{
int i j ;
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 b r l l = %finH ,bp[l][l]);
for(i=l;i<7;i++)
p ~ t f ( " C R%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
int n1,nh;
APPENDIX D
{
void nrerror();
float *v;
v =(float *)malloc((unsigned) (nh-nl+ l)*sizw f(float));
return v-nl;
1
void free-vector(v,nl,nh)
float *v;
int nl,nh;
{
+
free((char*) (v nl));
1
char error-textn;
{
void exit();
fp~tf(stderr,"NumericalRecipies run-time error ...\nW);
f p ~ t f ( s t d e r r ,%s\nW
" ,error-text);
f p ~ t f ( s t d e r r ,...
" 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];
{
int i;
float **m;
+
m =(float **) malloc((unsigned) (nrh-nrl l)*sizeof(float*));
if (!m) nrerror("al1ocation failure 1 in matrix()");
m -=nrl;

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
8
I* This evaluates = 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 [ 6 1 * ~ ~ 4 1 *+~bp[il[7*~[41*~[81
[61 + bp[i1[81*~[41*~[101+ bp[i1[9l*y[41*y[121 + bp[i1[10l*y[61*y[81 +
b~[il[~~1*~[61*~ +[bp[i1[121*~[61*~[121
101 + 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 [ ~ I I ~ I * Y [ ~ I+ * Ybp[il[4l*y[l1*~[91
[~I + 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 + ~ [ i 1;l
/*d~dx~2l=-@~[~l[ll*~[l1*~[31+ b ~ [ 1 1 [ ~ 1 * ~ [ ~ 1 * ~+[ 5 1bp[ll[3I*y[ll*y[7 + bp[11[4I*y[ll*y[91 +
l l lb ~ [ ~ 1 [ 6 1 * ~ [ 3 1+* ~bp[~l[71*~~31*~[7l
b ~ [ l l [ 5 l * ~ [ l l * ~ [+ ~~1 + ~p[11[81*y[31*~[91+ bp[11[91*y[3l*y[lll +
b~[11[101*~[51*~[7 + ~ p ~ 1 1 [ 1 ~ 1 * ~ [ 5 1 *+~bp[11[121*y[51*~[~11
[91 + bp[ll~l31*y[~*y[91+ bp[11[14l*y[7l*y[lll
+ b~[11[1~1*~[9I*y[l~l + C P [ 1 1 [ ~ 1 * ~ ~ l l *+~ [Cl l P ~ ~ I [ ~ I * Y [ ~+I CP[11~3l*y[31*y[31
*~[~I + CP[11[4l*y[41*y[41
+ CP[11[51*~[5I*y[Sl+ CP[11[61*~[61*~[61 + gp[lI 1;
d ~ d x ~ 4 1 = - @ ~ [ 2 1 ~ 1 1 * ~ ~bp[21[2I*~[ll*y[51+
~1*~~31+ 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+ [ ~b~[31~~21*~151*~I111
I*Y~~I + ~p[31~131*~~71*~[91 + bpI31[14I*y[T*y[lll + bpI31[15I*y[91*y[lll
+ c p ~ 3 1 ~ ~ l * y [ ~+ l * CP[31[2l*y[21*y[21
~[ll + 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] +
C P [ ~ I [ ~ I + Y [ ~ ~+* m[41 Y [ ~ I);
d~dx[l0l=-(b~[51[1l*~[l1*~[31 + b p [ ~ 1 I ~ I * y ~ l l * y ~+5 1 bp[51[3l*y[l]*y[71 + bp[5][4]*y[l]*y[9] +
b ~ [ ~ l [ ~ l * ~ [ l l *+~ bp[~1[61*y[31*~[51
[lll + bp[51[7l*y[31*y[71 + bp[51[8l*y[3I*y[91 + bp[5][9]*y[3]*y[ll] +
b ~ [ ~ l [ l o I * ~ [ ~ l+ * ~bp[51[11l*y[51*y[91
[71 + 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 CP~5I[1l*y[lI*y[ll
*~[~ll +
+ CP[51[2l*y[21*y[21 CP[5][31*y[3]*y[3] + CP[5][4]*y[4]*y[4]
+ C P [ 5 1 [ ~ 1 * ~ ~ ~ 1 *+~CP[51[6l*y[61*y[61
[51 + 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 [ ~11I *+Y 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]
+ C P ~ ~ ~ [ ~ I * Y [ ~+I CP[61[61*~[61*y[61
*Y[~I + gp[61 );*I
APPENDIX D 120
int n,*indx;
float **a,*d;
{
int i,irnax j,k;
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;
w[i] = 1 .O/big;
1
for(j=l;j<=nj++) {
for(i=l;i <j;i+ +) {
sum =a[i][i];
for (k= 1;k <i;k+ +) sum -= a[i]F]*a[k][i];
a[i]G] =sum;
1
big=0.0;
sum=a[i][i];
for ( k = l ; k < j ; k + + )
a[i][i] =sum;
if ( (dum=vv[i]*fabs(sum)) > = 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
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 Expressions for the Inertial Constants Appearing in the Equations of Forces of Motion *I
#define i l l (-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)
#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 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 o f 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 i j ;
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 g r [ 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;
}
int nl,nh;
I
void nrerror();
float *v;
+
v = (float *)malloc((unsigned) (nh-nl l)*sizeof(flaat));
return v-nl;
1
float *v;
int n1,nh;
{
free((char*) (v +nl));
1
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
{
int i;
float **m;
m=(float **) malloc((unsigned) (nrh-nrl+ l)*sizmf(float*));

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 */
int i j ;
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 i s . , e' *I
float term1 [7j ,term2[7] ,term3[7];
float torque[q; I* (6x1) Torque Vector *I
APPENDIX D
{
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();

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 i j ;
APPENDIX D
return;
1
void bpmat@p)
float bp[T[16];
{
int i j ;
APPENDIX D
APPENDIX D
for ( i = l ; i < = 6 ; i + + )
r %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]);
{ p ~ t f ( " b %f
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 b r l l = %fln",bp[l][I]);
return;
1
void CPmat(CP)
float CP[7][71;
{
int i j ;
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 [ 6 1 = (0.0);
return;
1
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
int n1,nh;
{
void nrerroro;
float *v;
v = (float *)malloc((unsigned) (nh-nl+ l)*sizeof(float));
return v-nl;
1
float *v;
int n1,nh;
APPENDIX D
char error-textn;
{
void exit();
fprintf(stderr,"NumericalRecipies run-time error ...\nu);
fprintf(stderr," %s\nW ,error-text);
fprintf(stderr," ...now exiting to system ...\nW);
exit(1);
}
int i;
float **m;
m=(float **) malloc((unsigned) (nth-nrl+ l)*sizeof(float*));

m -=nrl;

m[i] = (float *) malloc((unsigned) (nch-ncl+ l)*sizeof(float));
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
/* Defining Expressions for the Inertial Constants Appearing in the Equations of Forces o f Motion */
#define i l l (-0.0142)
APPENDIX D
#define ggl (-37.2)

#define gg2 (-8.44)
#define gg3 (1.02)
#define gg4 (0.249)
#define gg5 (-0.0282 )
int i j ;
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 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 C h a t ( ) ; 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;
a r [ l ] [ S ] = (irnl + i l + i 3 * c c 2 + i 7 * s s 2 3 + i l O * s c 2 3 + i 1 1 * s c 2
+
+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 i j ;
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("from br 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
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
int n1,nh;
{
void nrerroro;
float *v;
v =(float *)malloc((unsigned) (nh-nl+ l)*sizeof(float));
return v-nl;
1
float *v;
int nl,nh;
I
free((char*) (v +nl));
char error-textu;
APPENDIX D
I
void exit();
f p ~ t f ( s t d e r r ,%An"
" ,error-text);
fprintf(stderr," ...now exiting to system.. .\nV);
exit(1);
1
{
int i;
float **m;
m=(float **) malloc((unsigned) (nrh-nrl+ l)*sizeof(float*));

if (!m) nrerror("a1location failure 1 in matrix()");
m -=nrl;

+
m[i] =(float *) malloc((unsigned) (nch-ncl l)*sizeof(float));
m[i] -=ncl;
1
return m;
1
#include < stdi0.h >

#include < math. h >
I* Defining futed link parameters as per Denavit-Hartenberg notation *I

APPENDIX D

#define N (6) /***for 6 x 6 "jactrans" ie to f i d 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 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 ~ t f ( " E r r oopening
r 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 ++ ;)
w h i l e ( f s c a n f ( f p r e a d , " % 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);
j a c t ~ v = m a t r i x ( l , N , l,N);
APPENDIX D
for(i= l;i < =6;i+ +)

p r i n t f ( " m a t r i x j a c t r i n v % 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+ +)
p r i n t f ( " m a t r i x j c i n v r s e % 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
float jacdot[q[q;
{
j a c d o t [ l ] [ l ] = - c l *c23*a3*thdot[l] +
+ s l *s23*a3*(thdot[2] thdot[3]) - c l *s23*d4*thdot[l]
+
-sl *c23*d4*(thdot[2] thdot[3]) +s2*a2*thdot[2] +s 1*(d2 +d3)*thdot[l];
+sl*s2*a2*thdot[l] -cl *c2*a2*thdot[2];
jacdot[l][3] = s l * s 2 3 * a 3 * t h d o t [ l ] - c l *c23*a3*(thdot[2] + t h d o t [ 3 ] ) - s l * c 2 3 * d 4 * t h d o t [ 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
int n,*indx;
float **a,*d;
{
int i,imax j,k;
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< = n j + + )
if ((temp= fabs(a[i][i])) > big) big= temp;
w[i] = 1 .O/big;
}
for(i=l;i<=n;i++) {
for(i=l;i<j;i++) {
sum=a[i][i];
for (k= l ; k < i;k+ +) sum - = a[i]F]*aF]fi];
a[i][i] =sum;
1
big=0.0;
sum=a[i]G];
for ( k = l ; k < j ; k + + )
a[i][i] =sum;
if ( (dum=vv[i]*fabs(sum)) > = big) {
APPENDIX D
big=dum;
imax = i;
1
1
if f j ! = imax) {
for(k=l;k< =n;k+ +) {
dum =a[imax][k];
a[imax][k] =aGl[k];
a f i l k ] =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
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
int n1,nh;
{
void nrerror();
APPENDIX D
float *v;
v=(float *)malloc((unsigned) (nh-nl+ l)*sizeof(float));
return v-nl;
1
void free-vector(v,nl,nh)
float *v;
int n1,nh;
{
+
free((char*) (v nl));
1
char error-textn;
void exit();
f p ~ t f ( s t d e r r"Numerical
, Recipies run-time error ...\nu);
f p ~ t f ( s t d e r r ,%s\nM
" ,error-text);
f p ~ t f ( s t d e r r ,...
" now exiting to system ...\nV);
exit(1);
1
float **rnatrix(nrl,nrh,ncl,nch)
{
int i;
float **mi
m=(float **) malloc((unsigned) (nth-nrl+ l)*sizeof(float*));

m -=nrl;

m[i] =(float *) malloc((unsigned) (nch-ncl+ l)*sizeof(float));
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 */

#define N (6) /***for 6 x 6 "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 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 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();

int k;

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+ +)
p r i n t f ( " m a t r i x j a c t r i n v % 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 + + )
p r i n t f ( " m a t r i x j c i n v r s e % 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 i j ;
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 i j ;
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);
+ s23*c4*c5));
gp[5]= ( -2.8e-02*(~23*~5
APPENDIX D
return;
1
float coeffl41;
{
int i;
coefflo] =tho;
coeffll] =thd0;
coeffl2]=3.0l(tf*tf)*(thf-th0)-2.01tPthdO-1.O/tf*thdf;
1 .O/(tf*tf)*(thdf+thd0);
coeffl3]=-2.0l(tf*tf*tf)*(thf-~hO)+
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] = c l *(-s23*a3 + c23*d4 - s2*a2);
jac[l][3] = c l *(-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 ~ t f ( " j a csubr %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
float jacdot[7[7];
{
jacdot[l][l] = - c l *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];
+ s l *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 ~ t f ( " j a c d o tsubr %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
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< = n i + + )
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<j;i++) {
sum =a[i][i];
for (k=l;k <i;k+ +) sum -= a[i]F]*aF]fi];
a[i][i] =sum;
1
big=0.0;
for(i=j;i < =n;i+ + ) {
sum=a[ilul;
for ( k = I ; k < j ; k + +)
sum -= a[i]F]*a[k][i];
a[ilfil= sum;
if ( (dum =vv[i]*fabs(sum)) > = 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
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
int n1,nh;
{
void nrerror();
float *v;
v=(float *)malloc((unsigned) (nh-nl+ l)*sizeof(flont));
return v-nl;
1
float *v;
int n1,nh;
{
free((char*) (v + nl));
1
char error-textu;
{
void exit();
fprintf(stderr," %s\n",error-text);
" now exiting to system ...\nn);
f p ~ t f ( s t d e r r , ...
exit(1);
1
{
int i;
float **m;
rn =(float **) rnalloc((unsigned) (nrh-nrl+ l)*sizeof(float*));

m -=nrl;

+
m[i] =(float *) malloc((unsigned) (nch-ncl l)*sizeof(float));
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];
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( response = TERMINATE );
endcmd();
begincmd-key ("EMPCyAxR");
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");
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," I D = ", 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);
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");
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( cmd = "xy=3.00,2.00,0.00,front");
endcmdo;
begincmd-key("EMPCmCrAtW);
1l"EMPCmCrAt" "auto link curves"
c i $ p u t ( s t ~ g= "y");
endcmd(),
begincmd-key("EMPS1PrV); 1I"EMPSIPr" "Place solid of projection"

endcmd();

parents = 1,
nb-wanted = 1,
nb-read = &number-found );
write( "link-3 not found.\nW);
else
write( " l i n k no. found = ",number-found," ID= ", link-3, "\nW);
begincmd-key("EMPSlBx2Pn");
ci$put( cmd = "xy=3.5,1.0,5.50,frontU);
endcmd();
gr$last-elements( pobj = & l i 4 a ,

parents = 1,
nb-wanted = 1,
write( "linkpa not found.\nM);
else
APPENDIX D
write( "link-4a no. found = ",number-found," ID= ", link-4a, "\nW);
begincmdkey ("EMPCyAxRW);
ci$put( crnd = "xy =3.00,1 .00,5.75,frontW);
ci$put(string = "1.Ow);
endcmd();
gr$last-elements( pobj = &linkPb,

parents = 1,
nb-wanted = 1,
write( "link-4b not found.\nW);
else
write( " l i n k b 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);
endcmd();

parents = 1,
nb-wanted = 1,
write( " l i n k 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");
endcmd();
gr$last-elements( pobj = &linkpc,

parents = 1,
nb-wanted = 1,
APPENDIX D
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.50,frontV);
endcmd();
gr$last-elements( pobj = &link_6a,

parents = 1,
nb-wanted = 1,
write( "link-6a not found.\nW);
else
write( "link-6a no. found = ",number-found," ID= ", link_6a, "\nu);
begincmd-key ("EMPCy AxR");

endcmd();
gr$last-elements( pobj = &link_6b,

parents = 1,
nb-wanted = 1,
write( "link-6b not found.\nW);
else
write( "link-6b no. found = ",number-found," ID= ", link_6b, "\nu);
return;
1
z e r o p s i t i o n ( ) ; 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");
endcmd();

parents = 1,
nb-wanted = 1 ,
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");
endcmdo;

parents = 1,
nb-wanted = 1,
begincmd-key("GRRtEAbAxU);
ci$put( point = Inkl-out, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
begincmd-key ("GRRtEAbAx");
ci$put( obj = link-4a );
endcmd();
gr$last-elements( pobj = &linkPa,

parents = 1,
nb-wanted = 1,
ci$put( obj = link-4b );
endcrnd();
gr$last-elements( pobj = &link-4b,

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( obj = link-4~);
cisput( point = lnkl-out, window-name

cisput( response = TERMINATE );
-
cisput( point = lnkl-in, window-name = "front");
"front");
endcmd();
gr$last-elements( pobj = &link-4c,

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
nb-read = &nurnbcr-found );
begincmdkey("GRRtEAbAx");
cisput( point = lnkl-in, window-name = "front");
endcmdo;
gr$last-elements( pobj = &link-6a,

parents = 1,
nb-wanted = 1,
ci$put( obj = S i b );
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
I
begincmd-key("GRSAnn);
ci$put( value = theta[2] );
endcmd();
ci$put( point = Ink2-in, window-name = "front");
ci$put( point = lnk2-out, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
begincmd-key("GRRtEAbAxm);
cisput( obj = l i n k );
ci$put( point = lnk2-in, window-name = "front");
cisput( point = lnk2-out, window-name = "front");
endcmd();
gr$last-elements( pobj = & l i n k ,

parents = 1,
nb-wanted = 1,
ci$put( point = Ink2_in, window-name = "front");
ci$put( point = lnk2_out, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
begincmd-key(" GRRtEAbAx");
ci$put( point = lnk2_in, window-name = "front");
ci$put( point = Ink2_out, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
ci$put( obj = l i i c );
endcmd();

parents = 1,
nb-wanted = 1,
begincmd-key("GRRtEAbAxn);
endcmdo;

parents = 1,
nb-wanted = 1,
ci$put( obj = l i n k a );
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
endcmdo;

parents = 1,
nb-wanted = 1,
begincmd-key("GRSAnW);
endcmd();
begincmd-key (" GRRtEAb Ax");

ci$put( point = hk3-out, window-name = "front");
endcmd();
APPENDIX D

parents = 1,
nb-wanted = 1,
begincmd-key("GRRtEAbAxV);
cisput( obj = link-4a );
cisput( point = lnk3_out, window-name = "front");
endcmd();
gr$last-elements( pobj = &linka4a,

parents = 1,
nb-wanted = 1,
begincmd-key("GRRtEAbAxM);
cisput( point = lnk3_in, window-name = "front");
endcmdo;

parents = 1,
nb-wanted = 1,
ci$put( obj = link-4c );
ci$put( point = Ink3-out, window-name = "front");
endcmdo;
gr$last-elements( pobj = &linkpc,

parents = 1,
nb-wanted = 1,
begincrnd-key("GRRtEAbAx");
endcmdo;
APPENDIX D
parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
begincmdkey("GRRtEAbAxV);
cisput( point = Ink3-in, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
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");
endcmd();
endcmd();

parents = 1,
nb-wanted = 1,
&$put( response = TERMINATE );
endcmd();

parents = 1,
nb-wanted = 1,
cisput( obj = link-4c );
cisput( point = Ink4_out, window-name = "front");
gr$last-elements( pobj = &linkW4c,

parents = 1,
nb-wanted = 1 ,
APPENDIX D
endcmd();

parents = 1,
nb-wanted = 1,
begincmd-key (" GRRtEAbAx");

endcmdf);

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
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();
ci$put( point = InW-in, window-name = "front");
ci$put( point = InW-out, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
ci$put( point = 1nW-in, window-name = "front");
ci$put( point = Inks-out, window-name = "front");
endcmd();
gr$last-elements( pobj = &linkP6a,

parents = 1,
nb-wanted = 1,
ci$put( obj = l i i k b );
APPENDIX D
cisput( point = InW-in, window-name = "front");
cisput( point = InW-out, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,

APPENDIX D
write("**** data for link-6 *****","\nn);
I* Performing matrix concatenations for rotation about an arbitrary axis */

APPENDIX D
begincmd-key("GRSAn");
endcmdo;
ci$put( point = Inkbin, window-name = "front");
endcmdo;
APPENDIX D
ci$put( point = Inkbout, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
begincmdkey (" GRRtEAb Ax");

cisput( obj = link-6b );
cisput( point = Ink6-in, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
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 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
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( crnd = "xy =0.0,0.0,-5.00,frontm);
ci$put( crnd = "xy=0.0,0.0,-0.50,frontn);
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");
endcmdo;

parents = 1,
nb-wanted = 1,
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);
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);
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");
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");
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);
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");
endcmd();

parents = 1,
nb-wanted = 1,
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");
endcmd();
APPENDIX D
ll"GRPArR2Pn" "Place Arc by Radius and 2 Points"
cisput( cmd = "xy =2.25,1.25,0.00,fro11:");
endcmd();
begincmdkey("EMPCmCrAtn);
/lUEMPCmCrAt""auto link curves"
c i $ p ~ t ( s t ~=g "y");
cisput( cmd = "xy=3.00,1.25,2.25,front");
cisput( cmd = "xy =3.00,1 .25,2.25,frontU);
endcmd();
begincmdkey("EMPS1Pr"); 1I"EMPSIPr" "Place solid of projection"

ci$put( cmd = "xy =3.75,0.25,O.OO,front");
endcmd();

parents = 1,
nb-wanted = 1,
else
write( "link-3 no. found = ",number-found," ID= ", link-3, "\nH);
begincmd-key("EMPS1Bx2PnM);
endcmd();

parents = 1,
nb-wanted = 1,
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"
cisput( cmd = "xy =3.00,0.58,3.00,front");
ci$put(stMg = " 1.Ow);
endcmdo;

parents = 1,
nb-wanted = 1,
write( "link-4b not found.\nU);
else
write( "link-4b no. found = ",number-found," ID= ", IinkPb, "\nn);
begincmd-key ("EMPCyAxR");
cisput( cmd = "xy=3.00,0.91,3.00,front");
ci$put(string = " 1.00");
endcmdo;

parents = 1,
nb-wanted = 1,
else
write( "link-5 no. found = ",number-found," ID = ", link-5,"\nn);
cisput( cmd = "xy =3.00,0.91,3.00,front");
ci$plIt(st~g= " 1.00");
endcmd();
gr$last-elements( pobj = &link_4c,

parents = 1,
nb-wanted = 1,
APPENDIX D
write( "link-4c not found.\nW);
else
write( "link-4c no. found = ",number-found," ID = ", link_4c, "\nu);
cisput( cmd = "xy =3.00,0.75,3.50,frontW);
ci$put( cmd = "xy =3.00,0.75,3.75,frontW);
endcmd();

parents = 1,
nb-wanted = 1,
write( "linL6a not found.\nn);
else
write( " l i i 6 a no. found = ",number-found," ID = ", lik-6a, "\nW);
&$put( cmd = "xy =3.00,0.75,3.50,front");
endcmd() ;

parents = 1,
nb-wanted = 1,
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
= = NULL )
if( (fptr= fopen( "pieper.dat","rW))
{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();
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( obj = linkZ );
ci$put( point = Inkl-in, window-nnmc = "front");
endcmd();
grslast-elements( pobj = &link3,

parents = 1,
nb-wanted = 1,
endcmdo;

parents = 1,
nb-wanted = 1,
begincmdkey (" GRRtEAbAx");

cisput( point = lnkl-out, window-name = "front");
endcmd();
gr$last-elements( pobj = &linkV4a,

parents = 1,
nb-wanted = 1,
begincmd-key(" GRRtEAbAx");
cisput( obj = link-4b );
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( point = Inkl-out, window-name

-
"front");
endcmd();
&last-elements( pobj = &link_4c,

parents = 1,
nb-wanted = 1,
begincmd-key("GRRtEAbAxN);
endcmd();

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
ci$put( obj = rink-6b );
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
{
ci$put( value = theta121 );
endcmd();
endcmd();

parents = 1,
nb-wanted = 1,
begincmd-key ("GRRtEAbAx");
endcmdo;

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

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( point = hk2-in, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
ci$put( point = lnlc2-in, window-name = "front");
&$put( point = Ink2_out, window-name = "front");
endcmdo;

parents = 1,
nb-wanted = 1,
ci$put( point = Ink2_0ut, window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( point = Ink2_out, window-namt: = "front");
endcmd();

parents = 1 ,
nb-wanted = 1 ,
endcmd();
cisput( obj = link-3 );
endcmd();
APPENDIX D

parents = 1,
nb-wanted = 1,
endcmdo;

parents = 1,
nb-wanted = 1,
ci$put( response = TERMINATE);
endcmd();

parents = 1,
nb-wanted = 1,
ci$put( point = lnlc3-in, window-name = "front");
endcmd();
gr$last-elements( pobj = &linkQc,

parents = 1,
nb-wanted = 1,
cisput( point = lnk3-in, window-name = "front");
ci$put( point = lnH-out, window-name = "front");
endcmd();
APPENDIX D
parents = 1,
nb-wanted = 1,
begincmd-key (" GRRtEAbAxn);

endcmd();

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
APPENDIX D
begincmd-key("GRSAnH);
endcmd();
endcrndo;
gr$last-elements( pobj = &linkP4a,

parents = 1,
nb-wanted = 1,
ci$put( point = lnkqin, window-name = "front");
ci$put( point = lnkqout, window-name = "front");
endcrnd();

parents = 1,
nb-wanted = 1,
cisput( obj = link-4c );
endcmdo;
gr$last-elements( pobj = & l i i c ,

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( point = lnkqin, window-name .= "front");
endcmd();
gr$last-elements( pobj = &linkJ,

parents = 1,
nb-wanted = 1,
begincmd-key("CRRtEAbAx");
endcmd();
gr$last-elements( pobj = &iink_6a,

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
nb-read = &nu~nber-found );
1
APPENDIX D
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();
cisput( point = M - i n , window-name = "front");
endcmd();

parents = 1,
nb-wanted = 1,
ci$put( point = Inks-in, window-name = "front");
ci$put( point = 1nW-out, window-name = "front");
endcmdo;
gr$last-elements( pobj = &linkw6a,

parents = 1,
nb-wanted = 1,
APPENDIX D
ci$put( point = 1nW-in, window-name = "front");
ci$put( point = 1nM-out, window-name = "front");
endcmd();
gr$last-elements( pobj = &iink_6b,

parents = 1,
nb-wanted = 1,

APPENDIX D
write("**** data for link-6 *****","\nW);
I* Performing matrix concatenations for rotation about an arbitrary axis */

APPENDIX D
begincmdkey("GRSAn");
endcmdo;
begincmd-key("GRRtEAbAxfl);
cisput( point = Ink6_in, window-name = "front");
endcmd();
APPENDIX D
endcmd();

parents = 1,
nb-wanted = 1,
endcmd();

parents = 1,
nb-wanted = 1,
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 1192500.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 <Joint 6 Overflow > and
- <Calibration Status > 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
- list of enumerated ioOptionsT. CHAN ranges from 0 to 5 (corresponding
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 o r 6 float values from the robot. The particular
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 */
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. */
+
purnaVarsG.pos[S] = (long)(tempPos - pumaVarsG.oldPos5);
*val = encoderScaleG[S] * pumaVarsG.pos[S];
/* - 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
} /* 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;
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
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 */
va1[5] + = (long)(tempPos - pumaVarsG.oldPos5);
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
return OK;
case POSITION: I* Read a single encoder */
if (chan ! = 5){
*val = (long)inport(TRC-BASE + ENC-COUNT-BASE + 2*chan);
) else {
+
*val = (long)(tempPos - pumaVarsG.oldPos5);
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 */
} 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;
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);
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* R W 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 Ox0010
#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 Ox0001
#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
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[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;
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
scanf(" %dm,&jtNum);
if ((jtNum > = 1) && (jtNum < = 6)){
printf("Enter reference position in radians: ");
scanf(" % f',&posRefG[jtNum-I]);
} else {
p ~ t f ( " J o i n tnumber 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 */
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 ~ t f ( " J o i n tnumber out of range \nM);
) I* endif *I
break;
case 'W': I* Write DAC value */
p ~ t f ( " E n t e joint
r 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;
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
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

T h e E x p l i c i t D y n a m i c M o d e l a n d Inertial Parameter8 of t h e PUMA 600 Arm t
Brian Armstrong, Oussama Khatib, Joel Burdick
Stanford Artificial Intelligence L a b o r a t o y

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 h e d 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 n u l t s 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, m d 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.
m u t s 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 b u n expanded out to present explicit cxprrsi-
The tremendous s i u of an explicit dynamic model is the for each component of the dynamic model. b a u d and B 6 c . 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. P m which were not used hcre.
g r w for 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 t o t d m d 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 k p p o r t f o r h a t author h u been provided by
Hewlett Packard Co., through their Faculty Development Pm-
gram. Partial a ~ p p o r w=
t provided by NSF under contract MEA 2. Derivation of t h e D y m m i c M o d e l
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 ) [ q q l + C(q)[q2]+ 6 ( q ) = r; (1) a11 = It + I 2 cor2(82) + IS~ o a ( B ~ ) c o a+( 88,)~

where A(q) is the n x n kinetic energy matrix; +I, c 0 r ~ ( +
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 t o q u e ;
g(q) is the n-vector of gravity torquca;
q is the n-vector of arcelerationa;
where XI= 4m3 + 4 m 3 + 2 dgdsms + 4 m 2 + Js,, + J,..
r is the gcncrdizrd joint force vector. +Jz=. + J 2 w y + JL.. + JI..; etc.
~ h symbols
c [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, l e d to 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 I 1 arc the input to the RNE, the RNE will effcctivrly carry o11t
the calcr~lationof I t 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*.
4. Formation of the needed partial derivatives, expansion of p'.jk = -(L
2 apt
+
aq,
-- -1aajt
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 w m carried out with a LISP program, n k e d 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 c n c r g 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 m u l t comparable in form
and size to that of A R M [Muny and Neuman 19841. T h m sim-
plifying assumptions were made for this analysis: the rigid body
assumption; link G h a 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 a l l is given by: And equation (10) h o l d 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 t o 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 t o 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 - ~ o r ~ ( 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 T a b l e 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 E d d r Rate of
-
Our mcthod of modcl derivation is able to sinlplify to man- of C.mCsmntiom Comp.uriom
Ikpemdenc Te- of T o r p r
asable for111the con~plexsunl-of-product expressions that arc pro-
duccd by synlbolically carrying out the summatiow of Lagrange's Edmatiom ef the Fdl
hlod.1 E d Iteratiom 78 h 71 hs
cql~atiow. Simplificatiou i s in general a non-detcrn~iuistictask
that Rrows very rapidly with the number of terms in an cqua- Eduatiom of rhr C.=hrmnlim
D+p*nd.ml T e r m om* d v L r ever).
tion; but the procedure prcscnted is deterministic, with a cost low E d u a t i o u of tbt Vrlmly 60 Lm aoOh
that grows most rapidly as p2, where p is the number of sum-of- .nd Arrelrratiom Ikprmdrmt T e r n
product expressions iu the largest individual kinetic energy ma-
trix elemcnt. Our prorcdl~rehm the virtue of producing explicit A final decomposition t o 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 a d C 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 b u n implemented at the Stanford Artificid
the time requircd to dcvclop the full cxplicit modcl.
' Intclligcnce Laboratory [Khatib 19851.
S. Several Advantages O b t a i n e d f r o m Decomposition of
4. T h e Utility of a n Explicit M o d e l for D y n a m i c
t h e Explicit M o d e l
Simulation
The explicit solutiou of PUMA dynamics shows two stmc-
t u r d 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 b e 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 P U h U 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 n u m b a .
the same equation, or less than 0.1% as great as the largest con-
of calculations q u i d to 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 f d
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
T a b l e S. Calculations .It uircd to determine the Kinetic
on configuration, it is possible to decompose the calculation into Energy ~ a t n x % e m e n t s for a P U h U 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
N I Explicit Model 278
560 dynamics that can be achieved with 100,000 floating point
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. M e a s u r e m e n t o f t h e P U M A 680 D y n a m i c 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 t o calculate the elcmen+i of
The advantage of this decomposition applies equally well t o 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 P U M A 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
m d retluction g e m were not removed from the links, so the total
M e a s u r e m e n t of t h e M o t o r a n d D r i v e I n e r t i a
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 l i d s were not directly measured. tributions were found.
The wrist itself ww not disassembled. But the needed parameters M e a s u r e m e n t Tolerance
were estimated using mewurements of the wrist m w and the
external dimensions of the individual l i s . 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-
M e a s u r e m e n t of R o t a t i o n a l I n e r t i a
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 I i i two and t h m *. 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 d u e s 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. T h e M e a s u r e d P U M A 560 P a r a m e t e r s
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 .
T a b l e 4. Link Masses (kilograms; f0.01 + 1%)
F i g u r e 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 t o the axis of rotation, the length of L i 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 Zilies 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.
L i d 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-Y plane is in the center of l i d 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,.,,,
L i d 5: The orihn coiucides with that of frame 4;+ZS is di- were h i n d 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
l i d 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 b e c a m of the larger friction
joints 5 and G are in the zcro position frame G is at that joint. It was necessary t o add positive velocity feedback
aligned with frame 4. (damping factor -0.1)to cause joint one t o oscillate for several
cycles.
Wrist: The dimensions are reported in frame 4. '
T a b l e 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 t o q u e 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 t o q u e resolution can be
Figure 2. The PUMA 5GO in the Zero Position with Attached
lbtained by dividing the reported maximum joint t o q u e by 2048.
Coordinate Rames Shown.
T a b l e 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 f r m e
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 m l x ryi2 term or from the motor and
I drive inertia at joint one because tl~caecontributions an neither
Explicit dynamic models of complex manipulaton arc attain-
t configuration dependent nor appear in any term other tban a l l .
able. The PUMA 560 arm b as complex as any 6 dof arm with a
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 m u m i v e alternative. The availability of M.J. Aldon and A. Licgrois, 'Computational mpects in robot
m e ~ u r e ddynamic 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 d u e parameters. 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,' I E E E Tram. on Syrtemr, Atan a n t Cyber-
-
neticr, vol. SMC-10, no. 11, pp. 730 736, November 1980.
J. Durdick, Private Communication.
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,' I E E E 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.' I E E E
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 D p a m i c Syrtemr:
Meaaurcment, and Control, vol. 104, pp. 205. 211, September,
-
1985, pp. 644 649.
1982.
0.Khstib, 'Dynamic control of manipulaton in operation$ APPENDIX
space,' P a . Sirth IFToMM Congrerr on Theory of Machiner T h e f i l l Expressions for t h e Force. of M o t i o n 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," I E E E Tram. o n 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
"MACSYMA Reference Manual," MIT, inertia or torque, and trigonometric t e r n that are functions of
MIT Mathlab Group,
Cambridge, 1983. the joint angles. We have abbreviated the trigonometric func-
tions by writing S 2 to mean sin(qa) and C 5 t o mean cos(q5).
R. P. Paul, M. Rong and Hong Zhang, 'The dynamics of the W h ~ na trigonometric operation is applied t o 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 C C 2 to
mean cos(q2) cos(q2) and C S 4 to mean cos(q4) * ain(q4). These
final abbreviations, C S 4 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 + m a ) * o r (dr + I
;)
The position of zero joint angles and coodinate frame at- I, = -m, s n y ry, + ( m , + mr + m o ) 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,,)' + I r a
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 A l . +ms riBy + I..6 i
17 = ma r,s1 +
I,., -
Ins m l r,,' + +
2 m, * d l r.,
Table A l . 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 m i rcfer to the second mo-
ment of link i about the z axis of frame i and t h m ~ a s 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'+ - I..~
lzz6 ;
parametcn. Terms of the form r.; arc the offsets t o the center Ill = I.*, - I,", + I.., - I**s i
of gravity of link i in the ith ccoorlnatc frame. In Table A 3 the
values of the model c o n s t a t s 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), -g (m, + ms + m e ) 0s i
0, =
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 q k and 9, to give a contribution to the torque at joint i. Table AS. Computed V d u n 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 m e t e m - s q n d )
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'" l id 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-' f0 . 1 6 ~ 1 0 - ~
1,s = 1.79~10-' f 0 . 1 4 ~ 1 0 - ~ I,, = 1.93x10-' f 0.16x10-'
Part I. Inertlal Conmtantm
11 = + + + +
I,,I m l r y l y + m a * d l 1 ( m l m s ms) *a$' (Gravitational constants have units of newton meten)
+my r,ty + + +
(m, m , m~ + ms) ( d l dalY + gl = -37.2 f 00.5 gy = -8.44 f 0.20
+f,,a + I,,, + 2 r m. d l r,y +
my r,ra +
m t r',. g, r 1.02 i 0.50 g 4 = 2.49~10-' f 0.35~10-'
+2 ma (da + dt r., +I*,, + I,,s + I z a 6 i gs = -2.82~10" f 0.56~10-'
APPENDIX E
Table A4. The rxpressionr giving the elements of the kinetic -

+ I l 6 (C223 C5 S213 C4 S 5 ) + I l l SC23 CC4
enrrgy matrix. - -
+Iyo ( ( I + C C 4 ) SC23 SSS ( I 2 S S 2 3 ) C4 S C S )
(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 ( S S 5 l ( S S 2 3 ( I + C C 4 ) 1) 2 r.SC23 r C4 S C 5 )
+Iyl *SS23*CC4+2*(I~*C2*S23+I~~*c2*C23
+ I l $ ( S S 2 3 CS + SC23 C4 S S )
+ I l 6 C2 (S23 CS + C23 C4 S 5 )
+Ins S4 S!, + IYY ( S C 2 3 CS + CC23 C4 S S ) ) ;
zz 22.7 + 1.38 * CC2 +0.30. SS23 + 7 . 4 4 ~ 1 0 - ~C 2 523 .
a13 = Is C23 + 11s 523 -41s C23 S4 a S 5 + 119 r S23 SC4

+II~*(S~~*C~*S~-CZJ*C~)+I,~*S~~*S~*SS
+Iyo S4 * (S23 l C4 CC5 + C23 S C S ) ;
z -1.34~10-I C23 + -3.97~10-' r S23 .
I l l s = 2 ( l y o (SCS (CC4 ( I CC23) CC23) - -
a 1 4 = 11, * C23 + 11sl S23 C4 S 5 + I t s C2 C4 S5 - -
-SC23 C4 ( 1 2 S S 5 ) ) I I (5.723 * S 5 SC23 * G4 C S ) -
+I2,.C23*C4*S5;
-
+I18 C23 S4 S 5 lyo ( S 2 3 * C4 SCS + C23 S S 5 )
zO. .
-1,s *CZ.(S23.SS-C23*C4.C5)+I,~ . S 4 * C 5
+Il, (CC23 C4 a C5 SC23. S 5 ) ) ; -
-
= I ~ J * S ~ ~ * S ~ * C ~ + I ~ ~ * C ~ * S ~ * C S + I %~ -2.50~10-'
~ * S ~ * S (SS
~2 3 S5 SC23 C4 C 5 )
+Il~*(S23*S5-C23*C4*CS)+I~yrC~*S4*C5; -
2.48~10-' C2 (S23 S5 C23 C 4 . C 5 ) -
zo.
+ 8.Cnx lo-' S4 C5.
ay1 = I,y + 1.1 + Is + 110 S S 4 . S S 5 + 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 S C 5 ; co.

cz - 1 . 2 5 ~ 1 0 - b s 4 S5 .
= 11, S23 + 119 S23 ( I - (2*S S 4 ) )

+2* ( - l l s * C 2 3 * C 4 * S : , + I l 6 * S 2 * C 4 * S S
-
+I10 ( S 2 3 (CC5 CC4 0.5) + C23 C4 S C 5 )
+Ily S23 C4 r S S ) ;
.-
cz l . ~ l x l O - ' r S23 2.50~10-' C23 C4 S 5+
-
2.48~10-' S2 C 4 . SS + 0.30~10-' S23 ( 1 ( 2 S S 4 ) ) .
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 I y y S4 SS) ;
-2.48~10-a C3 S4 8 SS . be45 = b,ar . b4.= 0. bra=O-
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) ;
z -2.50x10-' SS 2.48x10-'+(C3 C4 CS - S3. S5) . (Tbc Abbrrviatcd Exprruions have units of kt-ma.)
by16 =0 - by34 = by, - - - +

baas = 41s . = 0. baa4
cay = +I4 C2 I 8 S23 I D S2 11.7 CW
+Il,.S23*S4.S5+I16*C2*S4.S5
bads = 2. {-Ils S4 C5 - 116S3 S4 C5)
.
-I1, r S4 + I y o S4 ( I - 2 SSS) ;
.. zz 0 .
-
+
+ I I a (C23 C4 SS S2f C5) Ill C23 SC4
+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 ; s0.
ball = 0 . 6x1s = 0 .
bald = ~ * ( - I I , * C Z ~ * C ~ * S ~ + ~ Y ~ * S ~ ~ * C ~ . S S
+Iyo (S23 r (CCS CC4 - 0.5) + C23 C4 SC5))
+I,, 523 + I 1 o S23 ( I ( 2 SS4)) ; -
zz - 2 . 5 0 ~lo-' C23 C4 5 5 + 1.C4xlO-' 523
+ 0.30~10-' S23 ( I - 2 SS4) .
lSls= 2 r (-Ils C23. S 4 CS + I,, S23 S4. (75)
-II, C23. S4
.
+I~o*s4.(C23.(1-2.ss5]-2*S23.C4.SC5);
s - 2 . 5 0 ~ 1 0 -8~C23 S4 CS - 6.42xlO-' C23 S4 . .
cyy =
cy, =
0. cya = 0.5. by,,
-11,*C4*SS-I~.*SS*C4*S5+Ilo*C4*SC5;
ba16 = - h a 6 - b,y~ = 0 . z 0.
b,,, = 2 r {Iyo SC4. SS5 + IYISC4 - I y y
z 0 .
.
S4 S5) ;
1
bays = 2. (-Il, S5 + lye 5 5 4 SC5 + 1 2 1 C4 * C5) ;

rz -2.50~10-' S5 .
bsy6 = 0 . hat4 = ~ J . = -I1,. C4. SS + 1.0 C4. SCS ;
-
Y , c,,
43s = ~ S Y J . ba,, 0 zz -1.25~10-' C4 S 5 .
b,,, = -Il, 2 S4 CS - 11, S4 + Iy, S4 (1 - 2 5.55) ;
s -2.50~10-' * S4 C5 .
b346 = ha6 .~ S J . = bas6 b.12 = - b y 1 4 . . .
- c4s = 0.5 bays c,, = 0 . c,, = 0 .
b41a = - h 1 4
.b,~, = 0.
bils = -llo (S23. C4 ( I - 2 SSS) + 2 C23 SC5)
-Ilr 523 C4 ;
C.6 = 0. csl = -0.5 blIs . csy = -0.5 qls .
z -1j.42~10-' r S23 C4 .
b416 = . b a l ~= -by, b414= 0 .
+
bays = In, S4 I y o S4 (1 2 555) ; -
zz 6.42~10-' S4 .
biy6 = -&is . bas, = 0 .
Table A7. Gravity Terms.
(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 = 4 4 1 s -
bsls = 0 . bs16 = 4156 ~ J Y J = -bays
b~l4= -b,zs - bsys = 0 . bsy6 = -bs6
bsm = bsl, . bsas = 0 . bsa6 = -bas6 -
bs,, = 0 . 8546 = -b4s6 . bss~= 0 .
b61~= b116 . 41s = h a 6 . 4 1 4 = b1.6 -
41s = b156 . be16 = 0 . b6ya = 0 .
4 1 4 = by46 bsrr = bas6 . b6y6 = 0 .

Ohiou1174932976 PDF

Uploaded by

Copyright:

Available Formats

You might also like

Ohiou1174932976 PDF

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Ohiou1174932976 PDF

Uploaded by

Copyright:

Available Formats

: Software for Control and Dynamic Simulation of

UNIMATE PUMA 560 ROBOT,

In Partial Fulfillment of the

I take this opportunity to acknowledge the support of my advisor, Dr. Sunil

Equations of Motion and Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

Conclusions and Future Work ................................ 54

D-10 Computer Program for Graphical Simulation of PUMA 560 ....

devices requiring methods of analysis and design similar to those of industrial

WRIST ROTATION 300'

WRIST BEND 200 '

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

1.2 Literature Review

1.3 Motivation of the Present Study

2.2 Forward Kinematics

Each homogenous transformation c-'is of the form

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

2.3 Denavit- Hartenberg Representation

ai (length), is the distance from iito ii+, measured along xi;

Table (2.1) Link Parameters of the PUMA 560

Figure 3.1 The Standard Frames (Adapted from 118U

form" means a solution based on analytic expressions or on the solution of a polynomial

3.3 Pieper's Solution

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

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

or, using Eq. (3.7) for the g,, we have

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

we have r = k, where r is known. The right-hand side (k,) is a function of 8, only.

Table (3.1) Set of Joint Solutions for PUMA 560

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

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

The elements rij correspond to the R.H.S of Eq. (3.13)

Figure 3.3 Four Solutions of the PUMA 560.

4.2 Lagrangian Formulation of Manipulator Dynamics

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

4.3 Equations of Motion

where T is the nxl vector of actuator torques.

The expanded structure of the terms corresponding to Eq. (4.9) is as follows:

where F is a force-torque vector acting on the end-effector of the robot, and X is an

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)

is arbitrary. Appendix C describes the application of jacobians in the force domain.

4.4 The Real and Approximate Dynamic Models

The key features of the approximate model were:

(i) Its compactness characterized by fewer calculations.

4.5 Dynamic Simulation

integrate the acceleration to compute future positions and velocities 1161.

Real vs. Approximate Models

Real vs. Approximate Models

Real vs. Approximate Models

Real vs. Approximate Models

Real vs. Approximate Models

Real vs. Approximate Models

Figure 4.2 Variation of Joint angles (degrees) for 6, - 0, w.r.t Time

Real vs. Approximate Models

I****thetaa=2.0atan( (d4 +sqrt(d4d4 +

angles[3][1]=atan2(d4,a3)+atan2(sqrt(a3* + d4d4 -kckc),kc);

angles[2][1]= atan2(-kl l,k21) + atan2( sqrt(k21k21 + kl l k l l -ze*ze),ze );

g l l =cos(angles[2][1])( a3cos(angles[3][2]) + d4sin(angles[3][2]) + a2) -sin(angles[2][1])(a3*sin(angles[3][2])