UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Closed loop end piece control of a servo controlled manipulator Boucher, Daniel Charles 1985

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata

Download

Media
831-UBC_1985_A7 B67.pdf [ 11.28MB ]
Metadata
JSON: 831-1.0096322.json
JSON-LD: 831-1.0096322-ld.json
RDF/XML (Pretty): 831-1.0096322-rdf.xml
RDF/JSON: 831-1.0096322-rdf.json
Turtle: 831-1.0096322-turtle.txt
N-Triples: 831-1.0096322-rdf-ntriples.txt
Original Record: 831-1.0096322-source.json
Full Text
831-1.0096322-fulltext.txt
Citation
831-1.0096322.ris

Full Text

CLOSED LOOP END PIECE CONTROL OF A SERVO CONTROLLED MANIPULATOR By DANIEL CHARLES BOUCHER B.A.Sc, The University of Bri t i s h Columbia, 1980 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES Department of Mechanical Engineering We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA June 1985 © Daniel Charles Boucher, 1985 In presenting t h i s thesis i n p a r t i a l f u l f i l m e n t of the requirements for an advanced degree at the University of B r i t i s h Columbia, I agree that the Library s h a l l make i t f r e e l y available for reference and study. I further agree that permission for extensive copying of t h i s thesis for scholarly purposes may be granted by the head of my department or by h i s or her representatives. I t i s understood that copying or publication of t h i s thesis for f i n a n c i a l gain s h a l l not be allowed without my written permission. Department of Mechanical Engineering The University of B r i t i s h Columbia 1956 Main Mall Vancouver, Canada V6T 1Y3 Date 19 June 1985 DE-6 ( W a i n ABSTRACT T h i s t h e s i s d i s c u s s e s the p o s s i b i l i t y of a t t a i n i n g c l o s e d l o o p end p i e c e c o n t r o l o f a s e r v o c o n t r o l l e d m a n i p u l a t o r by e x t e r n a l l y m o d i f y i n g t h e independent c o n t r o l l o o p s e t p o i n t of each r o b o t l i n k . T h i s p r o c e d u r e l e a v e s t h e e x i s t i n g s e r v o c o n t r o l l o o p s i n t a c t and thus has the b e n e f i t of a c q u i r i n g the advantages of an advanced c o n t r o l s t r a t e g y w h i l e r e t a i n i n g t h e i n h e r e n t r e l i a b i l i t y and s t a b i l i t y o f t h e e x i s t i n g c o n t r o l l o o p s . A l g o r i t h m s a r e proposed f o r compensating f o r known d i s t u r b a n c e t o r q u e s , e l i m i n a t i n g end p i e c e p o s i t i o n e r r o r s , and a p p l y i n g s p e c i f i e d f o r c e s t o the s u r r o u n d i n g s w i t h t h e end p i e c e . These a l g o r i t h m s a r e demon-s t r a t e d f o r the t h r e e degree of freedom case u s i n g a s i m u l a t i o n package ROBSIM.FOR wh i c h s i m u l a t e s t h e r e s p o n s e o f a s e r v o c o n t r o l l e d m a n i p u l a t o r to s e t p o i n t i n p u t s . To compensate f o r known d i s t u r b a n c e t o r q u e s t h e t o r q u e s g e n e r a t e d by the robot c o n t r o l system a r e assumed t o be d i r e c t l y p r o p o r t i o n a l to the changes i n s e t p o i n t s . These approximate r e l a t i o n s a r e used t o e s t i m a t e the adjustment i n s e t p o i n t s n e c e s s a r y t o gen e r a t e the r e q u i r e d a d d i t i o n a l j o i n t t o r q u e s . S i m u l a t i o n r e s u l t s show t h a t t h i s method i s e f f e c t i v e i n compensating f o r g r a v i t y d i s t u r b a n c e t o r q u e s . The J a c o b i a n r e l a t e s changes i n p o s i t i o n o f t h e end p i e c e t o changes i n j o i n t a n g l e s . U s i n g the J a c o b i a n combined w i t h a w e i g h t i n g f a c t o r to p r e v e n t o v e r s h o o t an a l g o r i t h m i s d e v e l o p e d which d r i v e s end p i e c e p o s i t i o n e r r o r s to z e r o . - i i -The transpose of the Jacobian i s used to relate forces exerted on the surroundings by the end piece to generated joint torques. The required torques are created by adjusting the set points. Simulation results indicate that this method can be used to exert specified forces. Force control i s more d i f f i c u l t to achieve than position control because the robot interacts physically with i t s surroundings. The ab i l i t y of the robot to exert forces depends strongly on the robot's configuration. A l l work in this thesis can be generated using the single program ROBSIM.FOR. - i i i -TABLE OF CONTENTS Page ABSTRACT i i LIST OF TABLES v i i LIST OF FIGURES v i i i LIST OF SYMBOLS x ACKNOWLEDGEMENT x i i i 1. INTRODUCTION 1 1.1 Review of Literature 2 1.2 Purpose and Scope of This Work 4 2. PROBLEM FORMULATION AND SOLUTION GUIDELINES 7 2.1 Basic Problem 7 2.2 Guidelines for Developing Simulation of Servo Controlled Robot 7 2.3 Guidelines for Supervisory Controller 9 3. DEVELOPMENT OF THE SIMULATION OF A SERVO CONTROLLED ROBOT 14 3.1 Kinematics 14 3.1.1 Review of the Homogeneous Transform 15 3.1.2 Defining a Link 18 3.1.3 Frame Assignment 20 3.1.4 A ± Transform Matrices Defining the Robot 23 - iv -Page 3.2 Dynamics 125 3.2.1 Kinetic Energy of Robot Structure 27 3.2.2 Potential Energy of Robot Structure 29 3.2.3 Lagrangian and Resulting Dynamics Equations 30 3.2.4 Evaluation of the Dynamics Terms 31 3.3 Actuator Characteristics 35 3.4 Link Control Law 37 3.5 Computer Implementation 42 3.5.1 User Input 44 3.5.2 Assigning Values to Robot Parameters 45 3.5.3 Solving the Equations of Motion 47 3.5.4 Monitoring Results 47 3.5.5 Output of Results 51 3.6 Results of Simulation for Nominal Motion 52 3.7 Conclusions 62 4. SUPERVISORY CONTROLLER 63 4.1 Basic Structure of Supervisory Controller 63 4.2 Gravity Compensation 65 4.3 Position Controller 73 4.3.1 Jacobian Development 73 4.3.2 Results and Implementation 77 4.3.2.1 Firs t Algorithm 77 4.3.2.2 Second Algorithm 82 4.3.2.3 Third Algorithm 87 - v -Page 4.3.3 Conclusions 99 4.4 Force Controller 99 4.4.1 Relating Applied Forces to Joint Torques 100 4.4.2 Modifying Simulation to Include External Forces, a Newton-Euler Approach 101 4.4.3 Results and Implementation 113 4.4.3.1 First Algorithm 113 4.4.3.2 Second Algorithm 115 4.4.3.3 Third Algorithm 119 4.4.4 Conclusions 126 5. CONCLUSIONS 127 5.1 Summary of Work and Results 127 5.2 Suggestions for Further Work 128 REFERENCES 130 APPENDICES 133 Appendix A - R0BSIM.F0R Program Listing 134 Appendix B - C0MMAND.COM and SETUP.DAT Program Listings 193 Appendix C - SH0RTEN.DAT Program Listing 195 - v i -LIST OF TABLES Page 3.1 Estimating Actuator Inertias 46 3.2 Robot Moving to Nominal Configuration 53 4.1 Homogeneous Transforms Tg and AjA^AgA^A^Ag 75 4.2 Position Supervisory Controller Simulation 95 4.3 Force Supervisory Controller Simulation 123 - v i i -LIST OF FIGURES Page 2.1 Robot and Geometric Model 10 2.2 Robot System Schematic 11 3.1 Defining One Frame With Respect to Another 16 3.2 Link Parameters and Frames for a Revolute Joint 19 3.3 Link Coordinate Frames and Link Parameters 22 3.4 Relating Quantities Between Coordinate Frames 26 3.5 Actuator (Armature Controlled D.C. Motor) 36 3.6 Control Servo for Each Link 38 3.7 Simplified Control Block Diagram for Each Link 40 3.8 Defining a Stopped Condition 49 3.9 A Problem With Numerical Accuracy 50 3.10 Simulation Run With No Gravity or Dynamic Coupling 55 3.11 Simulation Run With No Gravity, No Dynamic Coupling, and Reduced Damping 56 3.12 Simulation Run With No Gravity and Inertial Coupling Only .... 58 3.13 Simulation Run With No Gravity But Including Coriolis and Centripetal Effects 59 3.14 Simulation Run Including A l l Effects 61 4.1 Supervisory Controller Structure 64 4.2 Second Link Gravity Compensation, Update Frequency 5 Hz 69 4.3 Third Link Gravity Compensation, Update Frequency 5 Hz 70 - v i i i -Page 4.4 Second Link Gravity Compensation, Update Frequency 20 Hz 71 4.5 Third Link Gravity Compensation, Update Frequency 20 Hz 72 4.6 Simulation Run Including A l l Effects and Gravity Compensation 79 4.7 Fi r s t Position Controller Algorithm, Joint Angle Response .... 80 4.8 First Position Controller Algorithm, Position Error Behaviour 81 4.9 Second Position Controller Algorithm, Joint Angle Response ... 85 4.10 Second Position Controller Algorithm, Position Error Behaviour 86 4.11 Third Position Controller, Joint Angle Response, Example 1 ... 88 4.12 Third Position Controller, Position Error Behaviour, Example 1 89 4.13 Third Position Controller, Joint Angle Response, Example 2 ... 90 4.14 Third Position Controller, Position Error Behaviour, Example 2 91 4.15 Third Position Controller, Joint Angle Response, STOP = 0.1°/sec 93 4.16 Third Position Controller, Position Error Behaviour, STOP = 0.1°/sec 94 4.17 Robot Link Newton-Euler Vectors 104 4.18 Second Force Controller Algorithm, Joint Angle Response 117 4.19 Second Force Controller Algorithm, Applied Forces 118 4.20 Third Force Controller Algorithm, Joint Angle Response 121 4.21 Third Force Controller Algorithm, Applied Forces 122 - ix -LIST OF SYMBOLS Symbols used in Newton-Euler formulation listed and defined on page (103) ja third basis vector of frame defined with respect to world coordinates a^ length of link i homogeneous transform describing frame F^ with respect to frame F C actuator motor damping coefficient d^ distance of link i f_ force vector applied to surroundings f component of f_ in world coordinate x_ direction f component of £_ i n world coordinate y_ direction f component of t_ in world coordinate direction F world coordinate frame o F. coordinate frame of link i x £ gravity vector G disturbance torque due to gravity ±_ unit vector in world coordinate JC direction I inertia of link I inertia of actuator motor m j_ unit vector i n world coordinate direction J Jacobian pseudo inertia matrix of link i J t unit vector i n world coordinate z_ direction Kfi proportional gain for link control loop K c actuator motor back emf constant emf - x -K actuator gain m K t actuator motor torque constant K v velocity feedback gain for link control loop L armature winding inductance m^  mass of link i ri f i r s t basis vector of frame defined with respect to world coordinates N gear ratio link:motor o_ second basis of frame defined with respect to world coordinates jp the location of frame origin defined with respect to world coordinates P x component of £ i n world coordinate direction Py component of £ i n world coordinate y_ direction P z component of j> i n world coordiante z_ direction jr*| a vector quantity of link i defined with respect to F.. r. location of center of mass of link i defined with respect to F — i r o R armature winding resistance R^  3x3 rotation matrix defining orientation of frame F^ with respect to frame Fj s Laplace variable homogeneous transform describing frame F^ with respect to world coordinate frame F Q W weighting factor 3c world coordinate x_ axis x^ x axis of frame F^ £ world coordinate axis Z.-L X. a x * s °f frame F^ z world coordinate z axis z. z axis of frame F. —1 — 1 - x i -a user specified parameter used to determine weighting factor twist of link i K damping ratio 0 joint angle 6, desired joint angle (nominal setpoint) d 0^ link i joint angle x torque torque exerted on link i u) natural frequency - x i i -ACKNOWLEGEMENT I would like to thank Professor Dale Cherchas for his help and advice throughout my work. Some funding for the work was obtained from NSERC Grant No. A4682. - x i i i -1. INTRODUCTION 1. The industrial robot i s currently playing an important role in the continuously evolving manufacturing environment. Modern factories strive to incorporate and u t i l i z e the a b i l i t i e s of manipulators to improve quality and reduce cost. Programmable manipulators form the basis of new warehousing techniques involving automated storage and retrieval. They are also integral to the development and successful implementation of flexible manufacturing c e l l s . Robots are also expanding their applications in hazardous or remote working environments. Despite the major effects the robot i s having on the industrial sector the capabilities of the current industrial robot are s t i l l rather limited. Although robots are task programmable most operate i n an open loop fashion with respect to tool or gripper locations. Some robots provide for input from external sensors but most of these applications are rudimentary, often limited to simple contact or co l l i s i o n detection. Thus most robots used i n industrial applications are effectively blind. An additional limitation is that in order to take advantage of the robot's v e r s a t i l i t y a significant amount of human interaction i s needed, particularly when setting up a new task. This includes the procedure of teaching the robot which involves the human operator physically moving the robot through the desired motions. If tolerances are close or the task complex the robot may be off line for an inordinate amount of time and the performance upon playing back the recorded motion is often suboptimal (Lambeth 1982). As a result robots are usually found i n highly structured work 2. environments similar to those associated with mass assembly. It is not surprising then to find that a large amount of research i s being directed towards expanding the capabilities of manipulators to increase their applicability in the work place and also to more ful l y u t i l i z e their inherent f l e x i b i l i t y . Central to this theme i s research being done to examine methods of improving the positional accuracy of the robot gripper or tool. 1.1 Review of Literature The location of the gripper and the planned interaction of the robot with i t s surroundings is determined by the response of the control servo-mechanisms . To expand the capabilities of manipulators i n these areas many researchers have concentrated their efforts towards designing and imple-menting improved control strategies. Developing a successful control strategy consists basically of two steps. The f i r s t i s to derive a dynamics model of the physical robot arm. This i s perhaps the most tedious task. The second step i s to design a control strategy in accordance with the characteristics of the dynamics model, which when applied to the robot w i l l result i n desired responses to specified inputs. Current industrial robots use conventional position control servo-mechanisms to individually move each link to a specified angle. The cumu-lative motion of a l l links moves the gripper or tool to the desired location. Most research work concerns or focuses on replacing the existing control system. Several alternative algorithms have been proposed. These 3. include the resolved motion rate control (Whitney, 1969), inverse problem techiques (Paul, 1972), computed torque techniques (Markiewicz, 1973; Lee, 1982), near minimum time control (Kahn and Roth, 1971), the linearization of the dynamics about a trajectory (Popov, 1974), decoupling techniques (Freund, 1977; Roessler, 1980) and various adaptive control strategies (Dubrowsky and Des Forges, 1979; Koivo and Gvo, 1981; Balestrino et a l . , 1981; Lee and Chung, 1982). The above schemes a l l u t i l i z e only kinematic information in their control laws, i.e.; positions, velocities and accelerations. However many tasks such as assembly are more naturally described i n terms of applied forces or torques. Mason (1979) has formalized an algorithm for describing the force and position aspects of a task using a Task Coordinate Frame. The a b i l i t y to measure and control contact forces at the robot end piece would greatly extend the capabilities of the industrial robot. Paul and Shimano (1976) have proposed a technique of controlling applied forces by matching a manipulator joint to the specified force or torque. The joint i s then torque-servoed rather than position-servoed. More recently Raibert (1981) has described a control architecture for attaining hybrid position/ force control. His work suggests that the actuator control signal to each robot link be a function of both force and position errors. Each link simultaneously contributes to attaining the desired position and force. Raibert does not prescribe particular feedback control laws for the regulation of errors. Rather his work suggests a control architecture within which such laws can be applied. 4. 1.2 Purpose and Scope of This Work As previously mentioned most robotic research i s focussed on replacing the conventional control loops with conceptually more elegant and elaborate control algorithms. The potential of these algorithms i s usually verified in a simulation using a dynamics model which i s greatly simplified. While a l l published algorithms demonstrate impressive po s s i b i l i t i e s a l l have so far encountered serious drawbacks when implementation on a real robot has been attempted. The result i s that a l l commercial robots u t i l i z e conventional servomechanisms. This i s not to suggest that current control systems are adequate. The equations of motion of a six link manipulator are comprised of six, highly coupled, nonlinear, second order differential equations. Current practise is to control each link with an individual servomechanism. These controllers are designed neglecting a l l coupling and external effects. Thus there is no doubt that a more advanced control strategy is required (Vukobratovic and Stokic, 1983). This situation i s not unique to robotics. Many control problems faced in the process industries are highly coupled (Harrison, 1978). Although multivariable controller design techniques have been available for some time (MacFarlane, 1975; Asher et a l . , 1976) investigators have identified only thirty applications i n the process industry of multivariable control (Rijnsdorp and Seborg, 1976). In practise controllers which use propor-tional, integral and derivative elements, i.e.; PID controllers, are the accepted standard. In his paper of 1982 Dr. D. Cherchas describes a method of implement-ing multivariable control by modifying the independent control loop set points i n a coordinated fashion. This has the advantage of retaining the desirable characteristics of the PI or PID controllers while acquiring the 5. benefits of an advanced control strategy for a complex control problem. This thesis examines the possibility of using this type of method to supplement or improve the existing control system of a servo controlled robot. In particular this work uses a geometric model of a six degree of freedom revolute manipulator as described by Lee (1982), and whose geometric parameters are based on the Unimation PUMA 560. Using this model as a basis we would like to work towards implementing, in a simulation, the position and force control concepts as described by Raibert. Whereas Raibert was concerned with replacing the existing robot control servos, this work investigates the possibility of attaining a similar effect u t i l i z i n g external manipulation of the set points of the robot's existing control loops. That i s , can we use a Supervisory Controller to modify the individual servomechanism inputs in accordance to an algorithm which w i l l improve the coordinated response of the robot? Investigating such a system presents advantages in the stages of development, implementation, and operation. The most attractive feature i s that this improves the operational capabilities of the robot while leaving the existing robot control loops intact. Thus the s t a b i l i t y and r e l i a b i l i t y of the robot is maintained. Of course stability of the super-visory controller algorithm must be ensured. Also i f for some reason the advanced control strategy is removed, the robot's performance w i l l degrade only to i t s original capabilities. Since only control inputs to the individual loops are adjusted no hardware modifications on the existing robot are required to implement an advanced control strategy. So this method could feasibly be used to control the PUMA 560 robot in the UBC Mechanical Engineering Department. Finally, as a l l advanced control 6. algorithm processing would be done in a d i g i t a l computer, testing and modi-fication of the algorithms in the development stage could be accomplished simply by changing the programming. The work in this thesis concerns the development stage and includes the creation of a program which simulates the response of a servo controlled robot to set point inputs. This simulation i s used to develop and demonstrate, for three degrees of freedom, two supervisory controller algorithms. One algorithm improves position control, the second causes the application of a desired force. The remainder of this report discusses the work done to develop the simulation and the supervisory controller algorithms. 7. 2. PROBLEM FORMULATION AND SOLUTION GUIDELINES 2.1 Basic Problem The basic question this work attempts to answer can be stated as follows: Is i t possible to effect small motion accommodation of the robot end piece, and apply specified forces to the surroundings with the end piece, using an external supervisory controller which modifies the existing control loop set points in accordance to some overall control strategy? To obtain a quantitative answer to this question a computer simulation is used. A simulation provides the most insight and supplies the required analytical f l e x i b i l i t y . To implement this approach two basic tasks have to be completed. First a simulation of a servo controlled robot must be developed. Secondly, the supervisory controller algorithms must be designed and demonstrated using the simulation. 2.2 Guidelines for Developing Simulation of Servo Controlled Robot The f i r s t task is to develop a program which simulates the response of a servo controlled robot to conventional set point inputs. Such a model should include the robot arm dynamics, actuator characteristics and the link control laws found in current industrial practise. It should be noted that a l l investigators whose work was reviewed simplified the equations governing the behaviour of the robot. The usual simplification is to ignore Coriolis and centripetal effects using the rationale that these become important only for high angular velocities. Inertial coupling between the more weakly coupled outer links is also usually neglected as the actuator inertias of these joints are assumed to dominate any coupling effects. In addition to neglecting various dynamic 8. terms the physical description of the robot i t s e l f i s generally simplified. Inevitably the inertia matrix of each link i s assumed to be diagonal, i.e. a l l cross products of inertia are assumed zero. Even i f the links could be assumed to be symmetrical there can s t i l l be cross products of inertia because the link frame of reference i s not necessarily located on an axis of symmetry. Also investigators tend to work with the robot equations of motion alone so that their control strategies manipulate the torque supplied to each link directly. Since torque can not be specified independently but must be generated by some type of actuator the dynamics of the actuator are largely ignored. Since these researchers are, at this time, trying only to verify the possibilities of new control algorithms most of the work and development centres on the control strategies. So overlooking the tedious dynamics work and actuator characteristics is justifiable. With these points i n mind the robot simulation i s designed as a stand alone program. That is the simulation of a servo controlled industrial robot provides the Mechanical Engineering Department with a useful research and development tool. Also i n view of the fact that robot angular velocities and accelera-tions are continuously increasing as the technology develops a l l the dynamics terms are evaluated. In addition, to maximize the future usefulness of the simulation, a l l physical parameters of the robot are specified in an i n i t i a l i z i n g subroutine. Any assumptions are made by assignment during execution of the program and not during the derivation of the dynamics model. Finally to keep the simulation as flexible as possible to future uses the program is designed in a modular fashion. This means that the robot 9 . dynamics equations can be easily separated from the actuator characteristics and PI control loops. 2.3 Guidelines for Supervisory Controller It i s helpful at this point to consider two robots as shown i n Figure 2.1. A real robot with mass, gear backlash, etc. and a geometric model with massless rig i d links. To simplify the discussion i t i s assumed that joints 4, 5 and 6 of the six link robot are locked. Given a location i n space within the working envelope of the robot we can use the geometric model to find an exact set of angles 8j, 9 2 and 0 3 which w i l l move point A on the end piece to the desired location. We would then like to drive the real robot until i t obtains a configuration equal to that of the geometric model, and this i s what i s done i n a conventional robot. As shown schematically i n Figure 2.2 these desired angles are input as set points to the robot link PD control loops. The robot responds by moving the end piece to some location. Using the geometric model this location corresponds to an actual set of angles which define the resulting configuration. The overall system i s open loop i n the sense that no information about the actual end piece location is used in control of the robot. As a result of disturbance torques from gravity loads, gear backlash, and other errors the discrepancy in computed and actual locations can be excessive, which would clearly make this arrangement unsuitable for obtain-ing desired positions. To overcome this d i f f i c u l t y industrial robots are taught by a human operator. To teach a robot the robot i s put in what i s called free mode and 10. Figure 2.1. Robot and Geometric Model 11. D e s i r e d end p i e c e l o c a t i o n + g e o m e t r i c model y i e l d s : A c t u a l end p i e c e l o c a t i o n + g e o m e t r i c model y i e l d s : a ^ d e s i r e d ROBOT DYNAMICS ^ a c t u a l AND i n p u t s e t p o i n t s CONTROL SYSTEM r e s u l t i n g c o n f i g u r a t i o n "OPEN LOOP W.R.T. END PIECE LOCATION" F i g u r e 2.2 Robot System Schematic 12. led through the task positions. The operator either by grasping the end piece physically or by pendant control moves the robot to the desired loca-tion. The set point values from the encoders on each link are recorded into memory and these set points are played back during performance of the task replicating the desired motion. In the teaching mode the operator can be viewed as closing the loop, providing in effect closed loop end piece control. If the robot end piece i s not at the location desired the set points are adjusted by the operator moving the end piece until the robot has reached the desired configuration. In this work the supervisory controller i s designed from the standpoint of f u l f i l l i n g the role that the human operator holds during the teaching mode. The supervisory controller must attempt to attain, using position and force information sent from the end piece, closed loop end piece control of forces applied to the surroundings and local corrections in position. Work has been done to automate teaching (Seltzer, 1979). However these techniques involve using the commands available to the robot, for example MOVE X, and do not directly manipulate the set points of the individual control loops. The above controller differs from Raibert's work i n that his approach in specifying position and force control is open loop with respect to end piece position. His control architecture i s designed to provide manipulators with the ab i l i t y to move globally along a force trajectory i n the same manner as they now move along a position trajectory. In incorporating the supervisory controller algorithm into the simula-tion i t i s desirable that the program structure be made to emulate the physical arrangement of an actual robot and it s components. Again this 13. leads to a modular approach with individual subroutines simulating position and force sensors and a supervisory controller subroutine external to the robot model. Finally i n designing a supervisory controller consideration should be given not only to the kinematics of the manipulator but also i t s dynamics. The background material and jus t i f i c a t i o n for this work has now been introduced. The remainder of this thesis describes in detail the actual work and i t s results. 14. 3. DEVELOPMENT OF THE SIMULATION FOR A SERVO CONTROLLED INDUSTRIAL ROBOT To build a simulation of a servo controlled robot one must consider the physical laws which define and govern the behaviour of a real manipulator. These include: 1) kinematics; 2) dynamics; 3) actuator characteristics; and, 4) control loop structures. The information accumulated from the above must then be combined such that i t satisfies the requirements of the computer routines used to create the simulation. The behaviour of numerical calculations, their errors and results must be examined. Finally i t i s necessary to verify the simulation to ensure that i t yields results consistent with those of the real system. This chapter discusses these topics. Since much of the work that follows builds on the kinematics of the manipulator we w i l l f i r s t develop the required kinematic material. 3.1 Kinematics The kinematics of a manipulator provides a geometric description of the robot's configuration and motion with respect to a fixed world coordinate system. The torques or forces required to i n i t i a t e the motion are not considered. In manipulator applications i t i s necessary to describe the position and orientation of1 the robot end piece with respect to a fixed world 15. coordinate frame. To do this we attach a right handed orthonormal coordinate frame Fg to the end piece. Defining the configuration of the robot i s then equivalent to determining the position and orientation of Fg with respect to the fixed world coordinate frame F Q. A useful mathematical device for expressing the relationship between different coordinate frames is the homogeneous transform. 3.1.1 Review of the Homogeneous Transform Consider the two frames F Q and Fg shown in Figure 3.1. We wish to define the position and orientation of frame Fg with respect to F Q. From the diagram we see that Fg i s both translated and rotated with respect to 0 Fg. The homogeneous transform Ag is a 4x4 matrix which describes Fg with respect to F Q. The structure of the homogeneous transform i s unit vectors in displacement of direction of origin of Fg 3 6 Z 6 £6 component in ; Ho 3x3 [ Px Lo Rotation py Lo Matrix ; P z r~ direction 0 0 0 [ l Referring to the diagram we see that the X g axis points i n the ^.Q T direction. Thus the f i r s t column in the 3x3 rotation matrix is [0 1 0] . 16. Figure 3.1. Defining One Frame With Respect to Another. 17. The y_g axis points i n the negative 3CQ direction and implies that the second T column i s [-1 0 0] . The axis points in the same direction as the _ZQ .T axis so the third column i s [0 0 1] F 6 with respect to F Q is then The homogeneous transform describing As 0 -1 0 -2 1 0 0 4 0 0 1 3 0 0 0 1 (3.2) Homogeneous transforms have the following useful properties: • If we have a third frame F 7 which i s defined with respect to Fg by 6 the homogeneous transform A 7 the description of the frame F ? with respect 0 to F Q (A^) can be obtained by multiplying the transforms as follows: 0 0 6 —7 ~ h& —7 (3.3) • The inverse of the homogeneous transform AT defining frame F^ with respect to F . exists and is equal to A . the homogeneous transform defining J J 0 6 frame F. with respect to F^ ,. obtain Ag as follows So i f we have available A 7 and A 7 we can 0 Ae A.7 0 6 6 Ag A 7 (Aj) = . A. 0 6 - i -7 (A7) 1 0 7 h As (3.4) For a revolute robot position and orientation of the gripper is a function of the angular displacements of the robot links. To move from \ 18. angular to fixed world coordinates we f i x to each link a coordinate refer-ence frame. We then use homogeneous transforms to move from one frame to the other. Before we can assign frames i t i s necessary that we define geometrically what we have called a link. 3.1.2 Defining a Link Our six degree of freedom revolute manipulator consists of six rig i d links each of which rotates about one of six joint axes. In general each of the links i s defined between adjacent joint axes except of course the gripper link which is attached to only one joint axis. Two adjacent joint axes w i l l move through space fixed relative to each other. This relation is defined by specifying the length of the common normal between axes and the angle between the axes i n the plane perpendicu-lar to the common normal. These parameteres are known as "a" the length of the link and a the twist of the link respectively. Three adjacent joint axes defining two links w i l l share the interme-diate joint axis. Each of the two links w i l l have a common normal inter-secting this shared axis. The distance between the intersections i s known as d the distance between links. The angle between the common normals measured i n a plane perpendicular to the shared joint axis i s called 6 the angle between links. These parameters are shown in Figure 3.2. A six degree of freedom manipulator having six links w i l l have a set of parameters a^, ct^, d^ and 6^ associated with each link. A l l these para-meters taken together determine the complete configuration of the robot. For our revolute manipulator a^, ct^  and d^ are fixed while 9^, may vary. Having geometrically defined a link we are now ready to assign a suit-able coordinate frame to each of the robot links. Figure 3 . 2 . Link Parameters and Frames for a Revolute Joint. 20. 3.1.3 Frame Assignment In addition to the frame attached to each link we also have the fixed world coordinate frame. These seven frames F Q through Fg are assigned in accordance to the algorithm developed for linkages by Denavit and Hartenberg in 1955. The fixed world coordinate frame F Q i s positioned with the z^ axis lying along the axis of the f i r s t joint. To position the remaining six til frames we place the origin of the i frame at the intersection of the th common normal a^ and the (i+1) joint axis. th Note: • i f the joint axes intersect we place the origin of the i frame at the point of intersection. • i f the joint axes are parallel we locate the origin so as to make d zero for the next link whose coordinate origin i s defined. t h We then align the z_ axis so that the (i+1) link rotates about i t . The x_^  axis i s aligned with the common normal consistent with the fact that the x^ axis i s normal to the z.^ _-^  axis. Note: • i f a_^ =0 (joint axes intersect) the J C ^ axis is parallel to the vector cross product of and z_ . With the manipulator i n the zero position the positive sense of rota-tion for revolute joints can be decided and direction of the z_ axes deter-mined. The origins of the world coordinate frame and the f i r s t link frame are chosen to be coincident. As there is no seventh joint the sixth link frame can be placed where convenient as long as the Xg axis i s normal to the axis. Using this procedure we arrive at a series of frames located on the robot as shown in Figure 3.3. If we move sequentially through these frames recalling the definition of a link we can determine the values of the fixed parameters a^, ct^  and d^. Recall that for a revolute manipulator 6^  is variable. We can then establish the relationship between successive frames by noting that the position and orientation of the i 1 " * 1 frame i s defined by taking the ( i - l ) t n frame and: 1) rotating i t by 0^  about the axis. This defines the 1st inter-mediate frame. 2) then displacing this intermediate frame a distance d^ along the z_ ^ axis (2nd intermediate frame) 3) then displacing the 2nd intermediate frame a distance a^ along i t s own x_ axis (3rd intermediate frame) 4) followed by rotating the 3rd intermediate frame by ct^  about i t s own J C axis. The position and orientation of the i ^ frame with respect to the th (i-1) frame can then be described by a homogeneous transform which i t s e l f is a product of four homogeneous transforms each one describing an intermediate frame in terms of the frame before i t . The homogeneous transform defining the i t n frame with respect to the (i - l ) * " * 1 frame i s A 1 " 1 —l cos9 sin6 0 0 -sinScosa cosOcosa sincx 0 sin9sina -cos0sina cosa 0 acos9 asin9 d 1 (3.5) 22. Note: o r i g i n s of frames FQ and a r e c o i n c i d e n t as a r e o r i g i n s of F„ and F „ . O r i g i n s o f F, and F,. a r e a l s o c o i n c i d e n t . F i g u r e 3.3. L i n k C o o r d i n a t e Frames and L i n k Parameters. 23. 3.1.4 A. Transform Matrices Defining the Robot — i We now use the Denairt-Hartenberg transform and the link parameters of our manipulator to evaluate the six transformation matrices A^ ^ relating successive coordinate frames. For our manipulator a 2 > d 2, d^ and dg have been l e f t as variables so although they are constant for our robot they can be changed i f desired to create different robots. Using the parameters shown in Figure 3.3 we can then develop the following A^ ^ matrices. It i s conventional to omit the superscript of the A matrices with the understanding that i t i s one less than the subscript. Also cos6^ and sin0^ are denoted by and S^respect-ively. Thus 0 - s l 0~ _C2 "S2 0 a 2C 2 Ai = s i 0 0 -1 c l 0 0 0 A2 = s 2 0 C2 0 0 1 a2S2 d2 0 0 0 1 0 0 0 1 •mm " C 3 0 S 3 0 c- 0 0 A3 - S 3 0 0 1 " C 3 0 0 0 A* = \ 0 0 -1 0 0 \ (3.6) 0 0 0 1 0 0 0 1 0 S5 0 " S6 0 0 As = S5 0 0 1 - C5 0 0 0 As = S6 0 C6 0 0 1 0 d6 0 0 0 1 0 0 0 1 In working with these transforms i t is often necessary to define a frame F^ with respect to the world coordinate frame F Q. The resulting transform i s designated by T_^  and can be found by noting that —1^2 24. (3.7) The product of a l l six matrices w i l l be the matrix Tg = AjA^AjA^AgAg (3.8) The Tg matrix is generally denoted as n o a X X X n o a T = y y y — 6 n o a z z z 0 0 0 where the matrix elements are functions of the individual angular displace-ments 8^. The matrix Tg defines the position and orientation of the frame Fg attached to the end piece with respect to the world coordinate frame V In trajectory planning Tg i s given a set of values corresponding to the desired end piece position and orientation. The above matrix equation i s used to develop a set of equations which w i l l find the joint angles required to locate the end piece. The work i n this thesis does not include any trajectory planning as this can be done off line. Also the robot during motion responds to the set points input to the link control servos. These set points represent angular values. A detailed derivation of the joint angle solution can be found i n Lee's paper (1982). We now have the necessary material to develop the dynamics equations of our manipulator. = [n, o, a, j>] (3.9) 3.2 Dynamics In the previous section we described how the homogeneous transform can be used to define one coordinate frame with respect to another. The same transform can be used to redefine points i n space. For example consider the point in space p shown in Figure 3.4. This location can be defined with respect to the frame Fg by the vector _r 6. This same point i n space i s defined with respect to frame F Q by the vector 0 r L = Ag £ 6 (3.10) T where r 6 = [r r r 11 . The fourth term is known as a scale factor x6 ?6 z6 and is set to 1 for robotic work. If we are dealing not with a point i n space but with the vector quantity r 6 we are interested only in the length of r 6 and i t s direction. For example r^ could represent a velocity. This quantity can be expressed with respect to the F Q frame by using only the rotational part of the homogeneous transform, would now be a conventional three component vector and rg = Rg r 6 (3.11) We w i l l have occasion to make use of both results in the work that follows. To obtain a closed form set of dynamics equations for our robot we use the Lagrange-Euler method. This formulation w i l l yield a set of six highly coupled, nonlinear, second order differential equations. Each equation w i l l give the net torque which must be supplied to a link to result in the Figure 3.4. Relating Quantities Between Coordinate Frames. motion defined by a set of angular positions, velocities and accelerations. In deriving the equations of motion we use the matrices developed in the previous section to compute the kinetic and potential energies of the manipulator. We then form the Lagrangian and differentiate i t to obtain the dynamics equations. 3.2.1 Kinetic Energy of Robot Structure Consider a point on link i located by the vector r 1 which is defined with respect to F^. Its position i n world coordinates i s £° = L± L where ^ = A ± (3.12) If a vector is expressed in world coordinates i t is conventional to omit the 0 superscript so r = T_ r 1 Its velocity is then d£ i 3T The velocity squared is T dr_ 2 dr_ dr_ d_r dr_ <d7> = a t * dT - T r a c e < d t d t ) ( 3 ' 1 4 ) The kinetic energy of a particle of mass dm on link i at position r_ is T dr dr dK± = -| Trace (-^  ) dm (3.15) The kinetic energy of link i is then K. = / dK. 1 link i 1 28. (3.16) Substituting in (3.13) and recalling that (AB) T = B TA T we have , i i 8T . .T 8T_.T K. = Y ^ace [ E I ^ ( / r r 1 dm) j ± 6 i j (3.17) j=l k=l j link i k J The integral in the above equation i s known as the pseudo inertia matrix and is given by i i J L L dm link i (3.18) / link i i i X X i i x y i i x z i X i i x y i i y y i i y z i y i i x z i i y z i i z z i z i X i y i z 1 dm (3.19) Recalling that the moments of inertia, cross products of inertia, and f i r s t moments of a body are defined as xx I yy i zz I xy I xz I yz mx my mz / (y 2 / (x 2 / (x 2 J xy dm / xz dm / yz dm / x dm / y dm / z dm + z 2) dm + z 2) dm + y 2) dm (3.20) 29. we can write J_^  as 2<- Iix* + Iiy y + Il«> "ixy ixz mix1 ixy 2 ^ I I x x " I i y y + I i z z ^ ixz iyz "iyz V i 2 ( I i x x + I i y y ~ I i z z ) ni, z. i x m. x. i x m i y i m. z. i x (3.21) The total kinetic energy of the manipulator is then the sum of the kinetic energies of the links 6 i i 3T 9J_ K = 2 .\ T r a c e 1= * a e T ^ L a e — 8 j S J j=l j=l k=l j k J (3.22) 3.2.2 Potential Energy of Robot Structure In this derivation we express the acceleration due to gravity as a vector g whose direction is specified with respect to the world coordinate frame. The position of the center of mass of link i with respect to the world coordinate frame i s r. = T J rt — l — i — i (3.22) The potential energy of link i i s then expressed as P i = -m (3.24) or P, T - i ~ mi£ 4i i where g = [g x g y g z 0] (3.25) The total potential energy of the manipulator i s then 30. 6 = - E i=l T - i (3.26) 3.2.3 Lagragian and Resulting Dynamics Equations The Lagragian L = K-P i s then 6 i i 9T 3T 6 i=l j=l k=l j k J i=l We now obtain the dynamics equations by applying the Euler Lagrange equation ^ 3L__3L ( 3 > 2 g )  a t 36 ± 39 ± where is the generalized torque. Performing the operations we arrive at the equations of motion. See Paul (1981) for a complete development of this work. 6 ^ 6 6 x = E D 9 . + E E D. .. 9.9 + G. i=l,2,...6 (3.29) 1 • i i j J - i i - i ijk i k i ' ' 2=1 J J j=l k=l J 6 3T 3T T where D = E T r a c e ( _ £ j _P_) ( 3 . 3 0 ) p=maximum j i of i , j 6 32T 3T T D i j k = z , T r a c e ^ w ^ r ^ w - ^ <3-31> p=maximum j k l of i , j , k 6 9 T^ G± - = " »p £ asf IJ (3-32) p=i i 31. represents the effective inertia at joint i represents the coupling inertia between joints i and j represents centripetal forces at joint i due to velocity at joint j represents Coriolis forces at joint i due to velocities at joints i and k, and represents gravity loading at joint i It now remains to find these coefficients as functions of the robot parameters. 3.2.4 Evaluation of the Dynamics Terms In order to evaluate the terms extensive use was made of the UBC Computer Centre's REDUCE package. This program is used to manipulate v a r i -ables rather than numbers. The capabilities of the program are limited to rather simple manipulations so the problem has to be structured carefully. However the routine i s well suited for manipulating the large groups of variables encountered in the derivation of the dynamics terms. Considerable work was done In this thesis to develop the dynamics terms. This section briefly describes the procedures used. A l l computer runs and intermediate results have been saved on magnetic tape. Using these results as a basis the development of the dynamics equations for any other robot i n the future could be performed much more quickly. First for il l u s t r a t i o n let us expand the equation of motion for the f i r s t link. It can be shown that D, . = D # J and D. = DJt .. With these i j J i i j k i k j simplifications we then have where D i i G. 1 32, T i = D i i'8i + D 1 2 e 2 + D 1 3 e 3 + D^e , + D 1 5 e 5 + D 1 6 0 6 + D m e * + 2D 1 1 2e 1e 2 + 2D 1 1 3e 1e 3 + 2D l l t te 1e l t + 2D 1 1 5e 1e 5 + 2D 1 1 6e 1e 6 + D 1 2 2 0 2 + 2 D 1 2 3 0 2 0 3 + 2 D 1 2 t t e 2 e [ t + 2 D 1 2 5 e 2 e 5 + 2 D 1 2 6 e 2 e 6 + D 1 3 3 0 3 + 2 D 1 3 L F 8 3 8 L F + 2 D 1 3 5 0 3 0 5 + 2D 1 3 6e 3e 6 + D l t t l X + 2 D 1 I t 50,0 5 + 2 D l l t 6 e l t e 6 + D 1 5 5 § 5 + D 1 5 6 6 5 0 6 + D 1 6 6 9 6 + G i Now let us expand 9 i l 8 l l T 9l2 9l2T D l l = T r a c e Ue, - 1 30, ) + T r a c e ~* 30, ^  •301 30x J ,-301 30x aT_3 3T_3T ST^ ST^ 1 + Trace ( ~ ^ ~ ) + Trace ( ~ ^ ~ ) (3.34) + Trace *5 ^ ) + T r a c e i ^ ) To evaluate the complete set of dynamics terms f i r s t a l l the required matrices are developed. Then each trace term i s computed. Finally the trace terms are assembled under the appropriate dynamics term. The formulation process i s based on the six link coordinate transforms Aj through Ag and the six pseudo inertia matrices through Jg. We then use the A matrices to form the six matrices; Jj = Aj = func (0j) J_2 = = func (0j,0 2) (3.35) Ifi = A ^ A ^ A ^ A E = func (0 1,0 2,0 3, 0lt,05,0g) 33. It i s then necessary to calculate 9 T j 3T_2 9T_3 9 T g aBj ' 9 9 1 » 30J 9 9 x 3l2 9 9 2 : 9 l 6 9 9 6 9 T j d2"^ from TTT— we derive 9 9 1 ae^Sj ' g e ^ e , a " 9 9 ^ 9 3 (3.36) 3T-2 9 2 I s a 2 l 2 from gg- we derive yg-g-g- and j ^ ^ - (3.37) 9 X 3 92T_3 9 2 T 3 d2T^ from -rT-s— we derive ~ Q , » Q Q and A l l differentiation was done using REDUCE f a c i l i t i e s to ensure the correctness of the results. _1 To calculate the gravity terms we require six 4x1 vectors through _6 £g which define the mass center of each link. The last thing to be speci-fied is the gravity vector i t s e l f . At this point we now have developed 95 4x4 matrices and 7 4x1 vectors. A l l elements of a l l matrices are variables, usually each element i s a function of several variables. A l l matrices and vectors are stored i n f i l e s and the line number ranges defining each matrix or vector i s recorded. Now we are ready to evaluate the dynamics terms. Each coefficient in the dynamics equations consists of a number of trace terms. Each trace term i s found by multiplying three matrices. None of the trace terms are 34. repeated. There are 322 trace terms and 21 gravity terms. The amount of execution time required that the trace terms be evaluated in small bunches in batch runs. The nature of the problem also required that the trace terms be evaluated out of sequence. Therefore the 322 trace terms had to be f i l e d and their line ranges recorded. The individual trace terms were then assembled into groups corresponding to their appropriate dynamics coefficient. From the expanded formula for Dj ^  we can see that this coefficient i s composed of six trace terms. In addition each trace term is a function of one of the link pseudo inertia matrices JL . Thus each trace term represents the contribution of one link to the dynamics coefficient. To maintain this distinction i n the results each trace term i s given a label. So i f at some later date these individual trace terms are to be examined they are readily available. In the program results we then write Dll = D11A + D11B + D11C + D11D + DUE + D11F (3.38) where D11A through DllF correspond to the six trace terms. A l l of the dynamics equations have now been derived. There are two sets of these, one is in REDUCE format so that further work can be done on them i f desired. For example, in developing and debugging the simulation a simplified set of equations was used. The second set is in Fortran format and i s the basis of the f u l l simulation. The equations occupy about 400 kB of f i l e space. There are no simplifications i n the derivations. For example a l l link mass center locations and inertias are l e f t as variables to be assigned during the simulation. 35. 3.3 Actuator Characteristics In the previous section the equations of motion were developed which govern the response of the robot link structures to the net applied torques. We will'now examine the characteristics of the actuators used to supply these torques. For our robot model we w i l l use conventional, armature controlled, D.C. motor, positioning systems. A typical arrangement for any one of the links i s shown in Figure 3.5. In this diagram V i s the control voltage applied to the motor which produces the torque used to overcome damping and accelerate the motor armature and link. The following relations define the behaviour of the actuator. Kirchhoff's voltage law: V = Ri + L 4^ + K * u dt emr D.C. motor relation: x = K i m t * C 3 3 9 } Motor dynamics: T _ = K i - Cu> - I u> J net t m Gear pair: x =Nx „ , o) = N8 r net where x is the torque from the motor available to do work defined with net respect to the motor shaft, x i s the torque available to accelerate the robot link and is equivalent to the torque required by the dynamics equa-tions of the previous section. Combining the above relations we can write V = RI + L4^ + K we dt emf (3.40) x = N K i - N 2 C e - N 2 I e t m 36. V = applied armature voltage (volts) R = armature winding resistance (ohms) L = armature winding inductance (henrys) i = armature winding current (amps) i j = f i e l d current (amps) Vf = f i e l d voltage (volts) Vjj = back emf (volts) u> = angular velocity of motor shaft (rads/sec) . T M = gross torque delivered by motor (Nm) I m = motor armature inertia referred to motor shaft (kgm2) N = gear ratio LINK:MOTOR C = viscous f r i c t i o n coefficient for motor (Nm/rad/sec) T = torque delivered to link (Nm) I = inertia of link referred to link shaft (kgm2) K e m £ = back emf constant (volts/rad/sec) K t = motor torque constant (Nm/amp) Figure 3.5 Actuator (Armature Controlled D.C. Motor) 37. which describe the dynamics of this type of actuator. Recall that since our robot has six links we w i l l require six sets of these relations. Note that for a real actuator of this type there w i l l be a saturation voltage corresponding to the maximum torque the motor is able to produce. To incorporate this effect in the model would imply that the relation between V and x is a discontinuous function. The presence of such a function i n the model complicates the execution of the simulation. Thus the saturation effect is not included in the model and we assume that the robot operates within the torque limitations of the actuator. 3.4 Link Control Law We now have actuators that generate torques in response to an applied voltage and a robot which wi l l respond to the torques produced. Our f i n a l task i n modelling a servo controlled robot i s to describe the relations which control the voltage applied to the actuator. This is done using a PD control law as shown in Figure 3.6 for each link. Referring to the figure we see that the actual angular position of the link i s compared to the desired angle and this error signal through the proportional gain K g provides the control effort. The observed angular velocity i s fed back through the velocity gain to increase damping. The control voltage applied to the armature of the D.C. motor can then be written as V = K e (9 d-9) - 6 (3.41) For our six link manipulator we w i l l require six of these relations. Since a robot must not overshoot the desired position control gains K d e s i r e d j o i n t angle (nominal set point) Q voltage applied to actuator observed j o i n t angular v e l o c i t y observed j o i n t angle O + K e / * V ROBOT LINK AND ACTUATOR DYNAMICS 1 s K v Figure 3.6. Control Servo f o r Each L i n k . 00 39. and must be chosen for each link to yield a c r i t i c a l l y or overdamped response. They must also be chosen so that structural natural frequencies are not excited. Finally the torque output limitations of the actuator motor should be considered i n choosing appropriate gains. To choose gains i t is conventional practise to neglect a l l coupling and gravity effects i n the robot link dynamics model. The actuator i s replaced by a constant proportional gain K which for our model implies m neglecting armature inductance and back emf. Supplied torque is then directly proportional to applied voltage by the relation x = K V where K = NK /R (3.42) gross m m t The control loop of each link can then be represented by the block diagram i n Figure 3.7. Referring to this diagram F i s the viscous damping coefficient and from previous equations equals N2C. If back emf is not to be ignored i t s effects can be included by incorporating i t into the damping term. F would now equal (N2C + NK K ,.). I is the effective link inertia m emf and equals (D..+ N 2I ). i i m Using standard block reduction techniques we can write a K K 6 _ e m .„ ... Q - 7 = (3.43) s 2I + s(F + K K ) + K K v m em which corresponds to a linear second order system with general transfer function desired , joint angle (nominal set point) 9 d + voltage applied to actuator o— - o observed joint angular velocity 1 s observed j oint angle 9 Figure 3 . 7 . Simplified Control Block Diagram for Each Link. s 2 + 2£a> s + a) 2 n n (3.44) By comparison of terms 0 ) ^ = undamped natural frequency of system / K K (3.45) £ = damping ratio F + K K v m (3.46) 2/IK K e m For no overshoot the damping ratio must be greater than or equal to one. Since I i s configuration and load dependent I corresponds to the max worst case. Thus we can write F + K K > 2 /I K K (3.47) v m max e m To prevent the servo system from exciting any natural structural frequencies we ensure that ui < .5 u s (3.48) n min where w . corresponds to the lowest natural structural frequency, min Recalling that w . = 2rrf . we can write ° min min 42. J E M 'K K I min again 1 = 1 represents the worst case so max K K < T T 2 f 2 . I (3.50) e m min max < .5 (2irf ) (3.49) To avoid saturating the actuator we recall that the gross torque sup-plied by the motor referred to the motor shaft i s x^. We can then write NT = K K ( 6 , - 6 ) - K K 0 m m e d m v K K T H U S' -TP ( e H _ 9 ) m a Y < T m < 3* 5 1> N d max m max Note that in practise occasional saturation of the actuator may be an intended operating condition. When using the above relations to choose gains we find that equations (3.47) and (3.51) govern selection. The gains so chosen are well below the limits sets by equation (3.50). 3.5 Computer Implementation We have derived the equations defining the various operational characteristics of a servo controlled robot. It is now necessary to incor-porate them into a computer program which w i l l simulate the response of the robot to set point inputs. This is done in the program R0BSIM.F0R (Robot Simulation in Fortran Language) list e d i n Appendix A. 43. The program performs the following five basic functions: 1. accepts user inputs 2. assigns values to robot parameters 3. numerically solves dynamics and control equations 4. monitors results of numerical processes for errors or a stopped condition 5. outputs dynamic response of the robot The program was written to accommodate the simulation of a six degree of freedom revolute manipulator. However, in view of the complexity of the equations of motion some preliminary work was done at this time on the development of the supervisory controller. This included deriving the complete Jacobian and a substantial part of an algorithm for obtaining the inverse Jacobian. It became clear that to design, develop, and simulate supervisory controller algorithms encompassing a l l six degrees of freedom would present a considerable amount of work beyond what was already done. In addition, the added complexity obscures visualization of the robot motion hampering the development of algorithms. At this point i t was decided to simulate the response of the robot assuming that joints 4, 5 and 6 are locked. The supervisory controller i s then designed to eliminate postion errors and apply desired forces. The portions of the program which simulate the response of a servo controlled robot to set point inputs w i l l , with minimal modification, handle a l l six joints (see Section 3.5.3). A l l subroutines are written with an index which is incremented from 1 to the number of links not locked. This quantity M i s specified i n the f i r s t lines of the main pro-gram and in this work equals 3. A l l array sizes are large enough to handle six links. Assignment statements have been used for a l l robot parameters, 44. however the symbols NA (not applicable) follow those parameters which are not used. The program i s completely documented and comments are used to c l a r i f y a l l variables and steps which may be of interest to the user. 3.5.1 User Input ROBSIM.FOR i s designed to be used as a research tool or instructive aid. The program currently resides in the DEC VAX 11/750 computer in the UBC Mechanical Engineering Department. Written i n standard fixed format IBM Fortran the program does not use any of the special capabilities offered by the DEC Fortran language. To operate the simulation the user need only type RUN ROBSIM. The program displays information on the screen and prompts the user for answers to various questions. In the event that a numerical value i s required a typical value is displayed. The program allows the user to impose a r t i f i -c i a l conditions on the response of the robot, i.e.; the robot may be run neglecting any combination of dynamics terms including i n e r t i a l coupling, gravity, Coriolis and centripetal effects, armature inductances, back, emfs, and damping. The user i s also free to change i n i t i a l conditions and control gain values. A summary of the user's choices i s supplied i n the header of the output. If Coriolis and centripetal effects are not ignored, due to the length of the execution time (approximately 20 minutes), the simulation should be run in batch mode. Two f i l e s have been created to f a c i l i t a t e this and are listed in Appendix B. The f i r s t C0MMAND.COM contains a set of operating system commands. The second SETUP.DAT contains the set of answers which the user would make to the questions asked by the program. To run a batch 45. job the user simply types SUBMIT COMMAND. The simulation w i l l then run taking answers from the f i l e SETUP.DAT. 3.5.2 Assigning Values to Robot Parameters A l l robot parameters are assigned values i n the subroutine ROBPAR. Choosing values for most of these parameters involved a significant amount of approximation. Geometric parameters used i n the kinematics of a manipu-lator such as the PUMA 560 are well known. However physical parameters such as inertias and locations of mass centers are unknown even to the manufacturer. In addition the manufacturer does not supply, whether avail-able or not, such things are motor armature inductances, torque capacities, gear ratios and details of control loops. It i s not within the scope of this work to develop an accurate model of the PUMA 560, rather the purpose is to build a simulation of a general six degree of freedom revolute manipulator whose equations of motion are as complete as possible. For this work i t is sufficient to specify our own robot. The geometric link parameters and total mass of the robot are based on the PUMA 560. The remaining physical parameters are estimated. The total mass i s proportioned between links and each link modelled as a cy l i n -drical tube to compute inertias and locate mass centers. Values for a typical D.C. armature controlled motor are taken from an example in "Modelling, Analysis and Control of Dynamic Systems", by W.J. Palm. The parameters of the Stanford manipulator have been computed by Bejczy (1974) and are reprinted in Paul's book (1981). The f i r s t two links of the Stanford manipulator are revolute and in the same arrangement as our robot. Their masses and actuator inertias (N 2I ) are shown in Table 3.1. Using 4 6 . the assumption that actuator inertia i s proportional to link mass our robot is assigned the actuator inertias shown in the lower half of Table 3.1. These values are used to calculate the gear ratios through the relation I = N 2I . actuator m Table 3.1. Estimating Actuator Inertias Stanford Manipulator Link No. 1 2 Joint Type revolute revolute Link Mass (Kg) 9.3 5.3 Actuator inertias (Kgm2) 0.95 2.20 Robot Model Link No. 1 2 3 Joint Type revolute revolute revolute Link Mass (Kg) 22 16 12 Actuator inertias (Kgm2) 2.4 6.0 4.0 Having given the robot physical parameters the methods of Section 3.4 are used to approximate the control gains. Simulation runs are then used to adjust the gains to yield a slightly overdamped 99% response time of about 2 seconds. This is comparable to the performance of current industrial robots. Again, a l l parameters are defined and assigned values i n the sub-routine ROBPAR and i t is a simple matter to change them for any robot with a similar geometry. 47. 3.5.3 Solving the Equations of Motion The equations of motion are solved numerically using the VAX routine DERKF (Differential Equation solver package using a Runge-Kutta Fifth order method). DERKF requires that the equations be expressed as a set of f i r s t order differential equations and this i s done in subroutine FUNC. Joints 4, 5 and 6 of our manipulator are locked. As a result only those equations di, d i 0 d i 3 for 0j, © 2, 0 3, 9j, 02, 03, ^ , ^  and ^  are included. Also sub-routines that FUNC calls contain only those dynamics terms dJJ> ^ i j k a n c* ^ i that remain after noting that 0^  = 0g = 06 = 0^  = 0 5 = 0 6 = 0. These sim-plifications s t i l l leave a program which is over 3000 lines long. For a six link simulation the subroutine FUNC must be modified to include the additional equations and the f i l e s containing the complete DJ ., and G. terms must be called. These modifications are the only ones i j k i necessary to simulate the response of six links to set point inputs. 3.5.4 Monitoring Results There are two subroutines whose function is to monitor the results of the numerical calculations. The f i r s t subroutine STATUS checks the performance of the DERKF pack-age using the value of the parameter IDID returned by DERKF. STATUS w i l l terminate the task i f serious errors are encountered i n the routine DERKF. This may happen i f the user keys in nonsensical data. If the task is interrupted by an error which i s recoverable a warning message i s printed and the task is continued. For example a warning w i l l occur i f the user has specified a time step size TSTEP which i s too small for efficient use of DERKF. The second subroutine STOPT monitors the values of angular velocities and accelerations to determine when the robot has stopped. This proved to 48. be a nontrivial problem. In a numerical procedure we can not simply wait for the velocity to become equal to zero because this w i l l never happen,' i.e.; even 1 0 - 2 0 i s greater than zero. Also the angular velocity of a link may be either positive or negative as the link comes to rest. So subroutine STOPT waits for the magnitude of the velocity to f a l l below the value of STOP which is specified interactively by the user. To differenti-ate between the case when the link i s starting from rest and when i t i s coming to rest STOPT also monitors the sign of the acceleraton. A robot link i s defined as stopped when the magnitude of i t s angular velocity i s less than STOP and the direction of the angular acceleration is opposite that of the angular velocity. This condition i s depicted i n Figure 3.8. Numerical inaccuracies result in chatter about the time axis even when the link has satisfied the stopped condition. For example, the angular velocity of the f i r s t link as a function of time may be as shown in the top diagram of Figure 3.9. At time tj the f i r s t link i s i n a stopped condi-tion. Time t 2 shows where the numerical accuracy of the computer has been exceeded with both the angular velocity and acceleration of the f i r s t link small and positive. If the response of the second link i s as shown in the bottom diagram of Figure 3.9 a total stopped condition by monitoring both links at successive time intervals i s never attained. The reason for this i s that at time t 3 when the second link has come to rest STOPT interprets the f i r s t link as accelerating from a rest condition. Subroutine STOPT thus monitors each link separately and when a l l have stopped the robot i s in a stopped condition. This works because the angular velocities of each link are well behaved and do not exceed the value of STOP once having fallen below i t . Figure 3.8. Defining a Stopped Condition. Figure 3.9. A Problem With Numerical Accuracy. When implementing the force controller the links are constrained so a response finished condition is given by monitoring the rate of change of current, 4~> f° r each link actuator. These rates are however not as well dt behaved as the angular velocities. So i n order to guarantee a stopped condition the rates of current change for a l l link actuators have to be approximately zero. A zero value i s again defined by a small number STABLE whose value is set interactively by the user. 3.5.5 Output of Results Normally ROBSIM.FOR runs i n step mode outputting data only at specific times. This time interval i s set interactively by the user. The output i s arranged in a tabular fashion stored i n the f i l e corresponding to logical unit 9. The f i r s t page of output summarizes choices made, and values assigned by the user. If graphical results are wished ROBSIM.FOR w i l l run i n intermediate mode. In this mode data at a l l time intervals used internally by the differential equation solver package DERKF is made available. This data i s placed in a holding f i l e corresponding to logical unit 8. The tabular results are unaffected by this process and can be used as a reference to help isolate sections of the data of particular interest. The program SHORTEN.DAT li s t e d i n Appendix C can then be used to reduce the number of data points to a managable level. This shortened set of data, stored in logical unit 7, can then be manipulated and plotted using standard routines. Results in this thesis were obtained using the routines TELLABANK and TELLAGRAF on the UBC MTS system. 5 2 . 3.6 Results of Simulation for Nominal Motion This section examines the results of some actual runs simulating the response of a servo controlled robot to set point inputs. We w i l l examine the effects of various dynamics terms and in doing so attempt to verify the robot model. The i n i t i a l conditions defining the state of the robot are set inter-actively by the user. For a l l runs described in this thesis the robot i s at rest i n the 0^02=03=0 configuration at the start of the simulation corresponding to time equals zero. For the f i r s t run the disturbance torques introduced by gravity and a l l dynamic coupling effects are ignored. The response of each link should then be reduced to a linear second order system as described i n Section 3.4. Recalling that this simplified system was used to determine the control gains we should expect the robot link responses to be overdamped. This is verified by the results shown in Table 3.2 and the corresponding graph, Figure 3.10, which illustrates the response of robot links one, two, and three to step inputs 10, 30 and 60 degrees respectively. Since gravity i s ignored there are no steady state disturbance torques so we find that the robot links converge to the angles equivalent to the supplied step inputs. Table 3.2 represents the typical output from the simulation pack-age R0BSIM.F0R. Damping i n the robot system comes from three sources; the velocity feedback in the PD control loops, the back emf of the actuators and the viscous damping terms. Let us reduce damping by setting the back emf and viscous damping terms to zero and repeat the above motion. These results, again neglecting gravity and dynamic coupling, are shown i n Figure 3.11. As expected the response time of the robot has been reduced. Table 3.2. Robot Moving to Nominal Configuration. SIMULATION TRIAL NUMBER=150 HAVE CORIOLIS AND CENTRIPETAL EFFECTS BEEN IGNORED? Y HAVE GRAVITY EFFECTS BEEN IGNORED? Y HAVE INERTIAL COUPLING EFFECTS BEEN IGNORED? Y ARE ARMATURE INDUCTANCES ASSUMED NEGLIGIBLE? Y ARE BACK EMF EFFECTS ASSUMED NEGLIGIBLE? N ARE MOTOR DAMPING EFFECTS IGNORED? N IS SUPERVISORY CONTROLLER BYPASSED? N IS GRAVITY COMPENSATION PACKAGE ACTIVATED? N ANGULAR VELOCITY BELOW WHICH LINK IS CONSIDERED STATIONARY IS 0.0030 DEG/SEC ACCEPTABLE POSITION ERROR IS 0.0500 MILLIMETERS POSITION CONVERGENCE FACTOR ALPHA=0.500000 LINK 1 : ACTUATOR GAIN(KM)= 1 PROPORTIONAL GAIN(KE)= 10.000 30.OOO 13.000 0.000 DEGREES 0.000 DEGREES PER SECOND 10.000 DEGREES VELOCITY FEEDBACK GAIN(KV)= INITIAL JOINT ANGLE= INITIAL JOINT VELOCITY= FINAL DESIRED JOINT ANGLE-LINK 2 : ACTUATOR GAIN(KM)= 1 PROPORTIONAL GAIN(KE)= 10.000 25.000 14.500 0.000 DEGREES 0.000 DEGREES PER SECOND 30.000 DEGREES VELOCITY FEEDBACK GAIN(KV)= INITIAL JOINT ANGLE-INITIAL JOINT VELOCITY' FINAL DESIRED JOINT ANGLE= LINK 3: ACTUATOR GAIN(KM)= 1 PROPORTIONAL GAIN(KE)= 10.000 20.000 6.800 0.000 DEGREES 0.000 DEGREES PER SECOND 60.000 DEGREES VELOCITY FEEDBACK GAIN(KV)= INITIAL JOINT ANGLE = INITIAL JOINT VELOCITY' FINAL DESIRED JOINT ANGLE' Table 3.2. (Continued). TIME THETA1 (SEC) (DEG) 0 .OOOO 0 .OOOO 0 .2000 2 . 3840 0 .4000 4 . 7940 0 .6000 6 .4858 0 .8000 7 .6401 1 .0000 8 .4191 1 .2000 8 .9419 1 .4000 9 .2918 1 .6000 9 .5259 1 .8000 9 .6825 2. .OOOO 9 . 7873 2 .2000 9 .8575 2 . 4000 9 .9045 2 .6000 9 .9360 2. .8000 9 .9571 3 .OOOO 9 .9712 3 . 2 OOO 9 .9807 3. . 4000 9 .9871 3 .6000 9. .9913 3 .8000 9. 9942 4, .0000 9 .9961 4. , 2 OOO 9. ,9974 4. .4000 9. .9982 4 . ,6000 9. .9988 4. ,8000 9. 9992 5. OOOO 9. ,9995 5. 2000 9. 9996 5. ,4000 9. 9998 5 . 6000 9. 9998 5. 8OO0 9. 9999 6. OOOO 9. 9999 6. ,2000 10. OOOO 6. 4000 10. OOOO 6. 6000 10. OOOO *********STOPPED AT* THETA2 THETA3 S (DEG) (DEG) (I 0 .OOOO 0 .0000 10 5 .2617 14 . 7586 10 1 1 .3698 29 . 2750 10 16 .0942 39 .2106 10 19 .6436 45 .9353 10 22 . 2953 50 .4848 10 24 .2717 53 .5627 10 25 . 7428 55 .6450 10 26 .8369 57 .0537 10 27 .6502 58 .0068 10 28 .2546 58 .6515 10 28 .7036 59 .0877 10 29 .0372 59 . 3828 10 29 .2849 59 .5825 10 29 .4690 59 .7175 10 29 .6056 59 .8089 10 29 .7071 59 .8707 10 29 .7825 59 .9125 10 29 .8385 59 .9408 10 29. .8800 59 .9600 10 29 .9109 59 .9729 10 29. .9338 59 9817 10 29 .9509 59. 9876 10 29 .9635 59. 9916 10 29 9729 59 9943 10 29 9799 59. 9962 10 29. ,9851 59 ,9974 10 29. 9889 59. 9982 10 29. 9918 59. 9988 10 29. ,9939 59. 9992 10 29. ,9955 59. 9995 10 29. .9966 59. 9996 10 29. 9975 59. 9997 10 29. 9981 59. 9998 ********** :TPTI SETPT2 SETPT3 )EG) (DEG) (DEG) OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .OOOO OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30, .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30 .0000 60 .0000 OOOO 30. OOOO 60, .0000 OOOO 30. OOOO 60, .0000 OOOO 30. OOOO 60. .0000 OOOO 30, OOOO 60, OOOO OOOO 30. OOOO 60, OOOO OOOO 30. OOOO 60. OOOO OOOO 30. OOOO 60. OOOO OOOO 30. OOOO 60. OOOO OOOO 30. OOOO 60. OOOO OOOO 30. OOOO 60. OOOO OOOO 30. OOOO 60. OOOO OOOO 30. OOOO 60. OOOO ROBOT GRIPPER LOCATION: (IN MILLIMETERS) PX 823.564 PY 297.023 PZ -215.971 REQUIRED POSITION: PXD= 823.564 PYD= 297.023 PZD=-215.971 ***START WITH ADJUSTED SET POINT**** 823.564 297.023 -215.971 END PIECE LOCATION WITHIN REQUIRED TOLERANCES SET POINT 1= 10.OOOO SET POINT 2= 30.0000 SET POINT 3= 60.0000 ANGULAR RESPONSE OF ROBOT LINKS Robot moving to nominal configuration (10,30,60) degrees. 70-1 1 Legend A THETA1 X THETA2 • THETA3 H SETPT1 ffi SETPT2 X SETPT3 0 1 2 3 4 5 6 7 TIME IN SECONDS Figure 3.10. Simulation Run With no Gravity or Dynamic Coupling. ANGULAR RESPONSE OF ROBOT LINKS Robot moving to nominal configuration (10,30,60) degrees. 70 i i Legend A THETA1 X THETA2 • THETA3 B SETPT1 H SETPT2 X SETPT3 TIME IN SECONDS Figure 3.11. Simulation Run With no Gravity, No Dynamic Coupling, and Reduced Damping. 5 7 . Now let us replace the damping and include i n e r t i a l coupling. S t i l l neglecting gravity, Coriolis and centripetal effects we w i l l consider robot link motion to the configuration &1 = 0°, 9 2 = 0° and 9 3 = 90°. Since the i n i t i a l configuration is G ^ S ^ e ^ O 0 any deviation in the position of the f i r s t two links from zero degrees w i l l be a result of the motion of the third link. Referring to Figure 3.3 showing the robot with 8 J = 90°, 9 2 = 0° and 9 3 = 90° we would expect that the i n i t i a l acceleration of the third link as i t rotates from a vertical to horizontal configuration w i l l tend to make 01 move i n the positive direction. As the third link rotates downward i t should also tend to l i f t the end of the second link rotating i t in the negative direction. As the third link approaches i t s desired angle the angular acceleration w i l l be reversed. We would then expect opposite responses from links one and two from those above. Finally the PD control loops of links one and two w i l l tend to drive any deflection to zero. This i s exactly what we see when we examine Figure 3.12. In the next run the motion to 9^ — 0, 9 2 — 0 and 0 3 ~ 90° is repeated neglecting gravity and i n e r t i a l coupling but including Coriolis and centri-petal effects. We would expect the angular velocity of the third link to generate a centripetal force which w i l l cause the f i r s t link to rotate in the negative direction and the second link to rotate upwards in the nega-tive direction. Since the velocity of the third link does not change sign during the motion the deflections should reach a maximum and then be driven to zero by the action of the PD control loops alone. This response i s shown in Figure 3.13. ANGULAR RESPONSE OF ROBOTS FIRST AND SECOND LINKS Robot moving to nominal configuration (0,0,90) degrees. Legend A THETA1 X THETA2 TIME IN SECONDS Figure 3.12. Simulation Run With no Gravity and Inertial Coupling Only. ANGULAR RESPONSE OF ROBOT'S FIRST AND SECOND LINKS Robot moving to nominal configuration (0,0,90) degrees. Legend A THETA1 X THETA2 1 2 3 4 5 TIME IN SECONDS Figure 3.13. Simulation Run With no Gravity But Including Coriolis and Centripetal Effects. 60. Finally let us consider the effects of the gravity disturbance torques. Since the f i r s t joint axis i s vertical we need only consider the motion of links 2 and 3. For the motion to an intended configuration of 62= 10° and 63 = 65° we see from Figure 3.14 that the actual angles to which the manipulator moves are 37.1° and 76.6° respectively. This represents a steady state gravity droop of 27.1° for link 2 and 11.6° for link 3 for this run. Although the values may seem high the response i s what we would expect. More w i l l be said about gravity droop in Section ANGULAR RESPONSE OF ROBOT LINKS Robot moving to nominal configuration (0,10,65) degrees. LxJ L d UJ _ l o z < 2 3 TIME IN SECONDS Figure 3.14. Simulation Run Including A l l Effects. Legend A THETA2 X THETA3 • SETPT2 H SETPT3 62. 3.7 Conclusions We have now completed the work which discusses and examines the struc-ture of a servo controlled robot. To obtain this information the manipula-tor was considered as a set of six links. The response of each link was then evaluated as the cumulative effects of three component systems; the link structure dynamics, the link actuator characteristics, and the link control law. The resulting equations define the response of the robot. These equations are incorporated into the computer program ROBSIM.FOR which simulates the response of a servo controlled manipulator to set point inputs. Some runs of the simulation package were performed for the three degree of freedom case and we found that results reflected expected trends. Having developed a simulation of a servo controlled manipulator we can now turn to the task of designing a superivsory controller. However, before leaving this section, some areas for improvement i n the simulation should be noted. The most important would be the determina-tion of the physical parameters of an existing manipulator for use in the simulation. Thus Coriolis, centripetal, and in e r t i a l dynamic terms would be those of a real system. Also the governing equations could be expanded to include such effects as Coulomb fr i c t i o n and actuator saturation to improve the realism of the simulation. 63. 4. SUPERVISORY CONTROLLER This chapter develops the algorithms used in the supervisory controller and examines the results obtained when these algorithms are implemented in the simulation. External modification of the set points, input to the individual link control servos, is used f i r s t to compensate for gravity disturbance torques. As we saw in the last section the gravity torque effects are large. The magnitude of these torques can be computed easily implying that real time feedforward compensation may be possible. In addition to this we include in the controller the capability of eliminating position errors in the robot end piece location. After examining some results we leave position control and create a controller which w i l l attempt to apply a desired force on the surroundings with the end piece. In this work no attempt is made to combine position and force control. 4.1 Basic Structure of Supervisory Controller The general structure of the supervisory controller i s shown in Figure 4.1. External sensors are used to provide end piece position or force errors expressed i n world coordinates x, y_ and jz. This i s combined with other information, in particular the robot's observed and desired configur-ation, to create an adjustment i n set points which w i l l reduce the errors to zero. Effects which can be foreseen such as gravity can be calculated and compensated for i n a feedforward manner. The supervisory controller is external to the robot and i t s control servos. The program simulation maintains this structure with the super-Geometric model y i e l d s : - d e s i r e d a d j u s t e d s e t p o i n t s Geometric model y i e l d s : O + ROBOT, SERVO, AND SENSORS - a c t u a l -^observed x e , e V z p o s i t i o n o r f o r c e e r r o r s de SUPERVISORY CONTROLLER F i g u r e 4.1. S u p e r v i s o r y C o n t r o l l e r S t r u c t u r e . 65. visory controller subroutine SUPCON entirely self contained and called by the main program, i.e., the simulated robot, in only one place. When the program i s run the user i s asked to input values for some operating parameters. One of these i s the value of TSTEP. This value i s used by the differential equation solver package DERKF to control the output of solutions. For example in most runs TSTEP = 0.2 seconds, i n which case DERKF w i l l compute and have available the value of a l l state variables and their derivatives at times t = 0, 0.2, 0.4, 0.6,...seconds. These values are available for use by the supervisory controller at these times. The effect is that the supervisory controller i s able to modify the set points only every 0.2 seconds. Thus the robot responds i n a continuous manner to step changes in set points. Changing the value of TSTEP changes the duration of the step change and hence the frequency of input from the supervisory controller. The computation time required by the supervisory controller i s assumed negligible compared to the value of TSTEP used. Position and force sensors are simulated by subroutines SENSOR and CENSOR respectively. In both cases the actual value as determined by the simulation is compared to a desired value to generate the position or force error. Again this information i s only evaluated at TSTEP intervals. At the start of a corrective run the user is asked by the program to provide the desired change i n position or applied force. This describes the overall structure of the supervisory controller. We now turn to the details. 4.2 Gravity Compensation We have seen that for our robot the disturbance torques due to gravity result in large steady state differences between the desired joint angle 66. and the actual joint angle obtained. The severity of the problem for our manipulator may be exaggerated compared to that of an actual industrial robot but the problem s t i l l exists. This can be explained by the fact that a robot link's ability to resist an external torque is dependent on the stiffness of the control servo. When a link i s subjected to a disturbance torque the link w i l l move changing the value of the observed joint angle. This w i l l change the control voltage, via the PD control loop, changing the actuator torque produced. The link w i l l continue to move until at steady state the disturbance torque is exactly balanced by the change i n actuator torque. For a particular link recall that G defines the torque due to gravity. Recall also that at steady state the torque produced by the actuator is given by ss K K (6 . e m desired 8 ) ss (4.1) K K 8 e m error This balances the gravity torque so G = K K 8 ss em error The servo stiffness of the link is then defined as G s s = K K (4.2) 6 error 67. For a given gravity disturbance torque the error can be reduced by increasing the control gain Kg. However, the error is proportional to the magnitude of the gravity disturbance torque which i s configuration and load dependent. For the above reasons i t i s very desirable to compensate for gravity effects. This is done by the subroutine COMGRA called by SUPCON. The equations giving the totally general gravity torques were simplified using the assigned robot parameters and the UBC REDUCE package. These simplified equations for Gj, G 2 and G 3 are contained i n COMGRA. Let us now consider one of the links. By neglecting armature inductance we can write the control torque applied to the link as x = K K (9-6) - K (K +NK ,) 9 - N2C9 - N 2I 8 (4.3) e m d m v emf m where KtN Km = ~ Let the supervisory controller adjust the desired set point by A9 and let the corresponding change i n control torque be AT. Then T + AT = K K (9^6-9) - K (K +NK ^)9 - N2C6 - N 2I 9 (4.4) e m d m v emf m which implies that AT = K K A9 (4.5) em Recalling that the link equation of motion has the form 68. T . 1 = D i i i 6, + (4.6) and that at steady state Z torques = 0, we see that the torque from gravity acting on the link i s To compensate for gravity the actuator must provide a torque to balance this, i.e., -(-G^) = G^. The required adjust-ment in set point for the link is then Since the gravity torques are configuration dependent the supervisory controller calculates these torques and updates the adjustment i n set points every TSTEP seconds. To evaluate the performance of this algorithm we f i r s t make a run with no gravity effects to the nominal configuration 9^0°, 92=10° and 93=65°. Since G± = 0 we need not consider the motion of the f i r s t link and we set this joint angle to zero. The robot motion is then repeated using the above algorithm with an update every .2 seconds (5 per s e c ) . As shown in Figures 4.2 and 4.3 we find that the effects of the gravity disturbance torques are reduced dramatically. Recall that the steady state droop for this configuration with no gravity compensation was 27.1° for link 2 and 11.6° for link 3 (Figure 3.14). For an update frequency of 20 Hz (TSTEP = .05 sec) Figures 4.4 and 4.5 show that the effects of gravity are almost eliminated. Regardless of the update frequency the steady state errors are reduced to zero. A9 = G (4.7) K K e m ANGULAR RESPONSE OF ROBOT'S SECOND LINK Robot moving to nominal configuration (0,10,65) degrees. 20-1 1 to Ld Ld QtL O LxJ a CO Ld _J o z < -10 H -20 H -30 Legend A THETA2(N0 GRAVITY) X THETA2 • SETPT2 1 2 3 4 5 6 TIME IN SECONDS Figure 4.2. Second Link Gravity Compensation, Update Frequency 5 Hz. ANGULAR RESPONSE OF ROBOTS THIRD LINK Robot moving to nominal configuration (0,10,60) degrees. t o O Ld _ J o < Legend A THETA3(N0 GRAVITY) X THETA3 B SETPT3 2 3 TIME IN SECONDS o Figure 4 . 3 . Third Link Gravity Compensation, Update Frequency 5 Hz. ANGULAR RESPONSE OF ROBOTS SECOND LINK Robot moving to nominal configuration (0,10,65) degrees. t o Ld Ld G£ CO Ld —I O z < 20-1 -10 H Legend A THETA2(NO GRAVITY) X THETA2 • SETPT2 2 3 4 TIME IN SECONDS Figure 4.4. Second Link Gravity Compensation, Update Frequency 20 Hz, ANGULAR RESPONSE OF ROBOTS THIRD LINK Robot moving to nominal configuration (0,10,60) degrees. 80 i • Figure 4.5. Third Link Gravity Compensation, Update Frequency 20 Hz. 73. 4.3 Position Controller This section covers the development of a supervisory controller which wi l l drive robot end piece position errors to zero. The robot end piece must not overshoot the desired position as this can result i n undesirable collisions with the surroundings. The controller must move the end piece cautiously towards the desired position. The basic procedure i s to move the manipulator end piece to the approximate location by supplying a nominal group of set points. After the robot has stopped the error i n end piece position is measured. The supervisory controller computes the change in joint angles necessary to move the end piece a fraction of the required distance. The set points are adjusted and the robot responds. When the robot has stopped moving the error i n position is again measured. New set points are then computed and applied. The process is repeated until the position errors are eliminated. A l l three successful algorithms employ the Jacobian for determining the required joint angle changes. 4.3.1 Jacobian Development Both the position and force supervisory controllers lean heavily upon the availability of the Jacobian and i t s inverse. The Jacobian relates differential motions of the end piece frame Fg to differential changes i n the joint angles. In this three degree of freedom simulation we w i l l be prescribing desired changes in the location of Fg AX, AY and AZ as measured in the world coordinate system. Using dx to denote a 3x1 vector describing a differential translation we can write dx = J d9 (4.8) 74. where dj) is a vector of differential changes in the angles of the f i r s t three links and J_ i s the Jacobian. There are several ways of computing a Jacobian (Paul, 1981). The most direct in our case is to work with the position of the end piece directly. Recall that the position and orienta-tion of the end piece frame Fg defined with respect to the world coordinate frame F Q i s given by Tg, where Tg is the homogeneous transform whose columns are commonly denoted by the vectors n, o_, a_ and Recall also that Tg = A 1 A^AjA^Ac^Ag, the product of the individual link transforms. Both these matrices have been computed with REDUCE and are shown in Table 4.1. Since (Px> Py> P z) Is the location of the origin of F g defined with respect to F Q we equate elements in the fourth columns. Noting that joints 4, 5 and 6 are locked with 0lf=65=0g=O the results are the equations for the location of the end piece as a function of the three unlocked joint angles P x " C l < d 6 S 2 3 + d4 S23 + a 2 C 2 ) " d 2 S l P y = S l < d 6 S 2 3 + d- S23 + a2 C2> + d 2 C l (4.9) = dcC 6^23 + d. ,C •4°23 where S 2 3 = sin(0 2+0 3) and C 2 3 = cos(0 2+0 3) We now form the differentials of these equations noting that dsin0 = cos0d0, dcos0 = -sin0d0, d(0 2+0 3) = d02+d03, and letting d 5 = d^+dg. We then have 7 5 . Table 4.1. Homogeneous Transforms T^ and A^A^A^A^A^A^ T 6 ( 1 . 1 ) m N X $ T 6 ( 1 . 2 ) K 0 X $ T 6 ( 1 , 3 ) S A X $ T 6 ( 1 . 4 ) E P X $ T 6 ( 2 . 1) S N Y $ T 6 ( 2 . 2 ) • 0 V $ T 6 ( 2 . 3 ) B A Y $ T G ( 2 , 4 ) « P Y $ T 6 ( 3 . 1 ) N Z $ T G ( 3 . 2 ) B 0 Z $ T 6 ( 3 . 3 ) B A Z $ T 6 ( 3 , 4 ) V P Z $ T 6 ( 4 . 1) B 0 $ T 6 ( 4 , 2 ) B 0 $ T G ( 4 . 3 ) B 0 $ T 6 ( 4 . 4 ) B 1$ A 0 6 ( 1 , 1 ) : » - C 1 * S 6 * C 2 3 * S 4 + C 1 * C G * C 2 3 * C 4 * C 5 - C 1 * C 6 * S 2 3 * S 5 - S 1 * S 6 * C 4 - S 1 * C G * S 4 * C 5 $ A 0 6 ( 1 . 2 ) :«= - C 1 * S 6 * C 2 3 * C 4 * C 5 + C 1 * S 6 * S 2 3 * S 5 - C 1 * C 6 * C 2 3 * S 4 + S 1 * S 6 * S 4 * C 5 - S 1 * C 6 * C 4 $ A 0 6 ( 1 , 3 ) : * C 1 * C 2 3 * C 4 * S 5 + C 1 * S 2 3 * C 5 - S 1 * S 4 * S 5 $ A 0 6 ( 1 , 4 ) : « A 2 * C 1 * C 2 + C 1 * D 4 * S 2 3 + C 1 * C 2 3 * C 4 * S 5 * D 6 + C 1 * S 2 3 * C 5 * D 6 - S 1 * 0 2 - S 1 * S 4 * S 5 * D 6 $ A 0 € ( 2 , 1 ) : » C 1 * S 6 * C 4 + C 1 » C 6 * S 4 * C 5 - S 1 * S 6 * C 2 3 * S 4 + S 1 * C 6 * C 2 3 * C 4 * C 5 - S1 * C 6 * S 2 3 * S 5 $ A 0 6 ( 2 , 2 ) := - C 1 * S 6 * S 4 * C 5 + C 1 * C 6 * C 4 - S 1 * S 6 * C 2 3 * C 4 * C 5 + S 1 * S 6 * S 2 3 * S 5 - S 1 * C 6 * C 2 3 * S 4 $ A 0 6 ( 2 . 3 ) : « C 1 * S 4 * S 5 + S 1 * C 2 3 * C 4 » S 5 + S 1 * S 2 3 * C 5 $ A 0 G ( 2 . 4 ) : - A 2 * S 1 * C 2 + C 1 * D 2 + C 1 * S 4 * S 5 » D 6 + S 1 * D 4 * S 2 3 + S 1 * C 2 3 * C 4 * S 5 * D 6 + S 1 * S 2 3 * C 5 * D G $ A 0 6 ( 3 . 1 ) A 0 6 ( 3 . 2 ) A 0 6 ( 3 . 3 ) A O € ( 3 . 4 ) A 0 6 ( 4 , 1 ) A 0 6 ( 4 . 2 ) A 0 6 ( 4 . 3 ) A 0 6 ( 4 , 4 ) » S G * S 2 3 * S 4 - C S * C 2 3 * S 5 - C 6 * S 2 3 * C 4 * C 5 $ « S 6 * C 2 3 * S 5 + S G * S 2 3 * C 4 * C 5 + C G * S 2 3 * S 4 $ - C 2 3 * C 5 - S 2 3 * C 4 * S 5 $ » - A 2 * S 2 + 0 4 * C 2 3 + C 2 3 * C 5 * D 6 - S 2 3 * C 4 * S 5 * D 6 $ - 0 $ = 0 $ - 0 $ - 1$ 76. dp x = C 1d(d 5S 2 3+a 2C 2) + dC 1(d 5S 2 3+a 2C 2) - d 2dS 1 = C 1(d 5C 2 3(d6 2+d8 3) + a 2(-S 2d6 2)) - S 1d6 1(d 5S 2 3+a 2C 2) - d 2C 1d8 1 = {-S 1(d 5S 2 3+a 2C 2) - d 2C 1}d8 1 + {c i(d 5C 2 3-a 2S 2)}d8 2 + {d 5C 1C 2 3}d8 3 (4.10) dp y = S 1(d 5C 2 3(d8 2+d6 3) - a 2S 2d9 2) + C 1d8 1(d 5S 2 3+a 2C 2) - d 2S 1d8 1 = {c i(d 5S 2 3+a 2C 2) - d 2S 1}d8 1 + {s i(d 5C 2 3-a 2S 2)}d8 2 + {d 5 S lC 2 3}d8 3 (4.11) dp z = d 5dC 2 3-a 2dS 2 = -d 5S 2 3(d0 2+d8 3) - a 2C 2d8 2 = {0}d81 + {-d 5S 2 3-a 2C 2}d8 2 + {-d 5S 2 3}d8 3 (4.12) These three equations can be written as dp X c i p z ( d 5 c l S 2 3 ) - -d9 : = P x s i P z ( d 5 s 1 c 2 3 ) d8 2 - d P z . 0 (~d5S23""a2C2) ( - d 5 s 2 3 ) d8 3 which corresponds to dx = Jd8_. Thus we now have the Jacobian. To calculate the inverse of the Jacobian the simulation uses Cramer's rule. The inefficiency of this method is acceptable for a 3x3 Jacobian but computation of the inverse for the f u l l 6x6 Jacobian i s a computational bottleneck. Paul (1981) describes a method of calculating the results that would be given by the inverse without actually using the inverse directly, thereby reducing computation time substantially. 4.3.2 Results and Implementation 4.3.2.1 Fi r s t Algorithm When the robot has stopped the position sensor measures the error i n position; AX, AY and AZ. The supervisory controller then applies a weight-ing factor of the form (1 - a N) where 0 < a < 1 (4.14) N equals 1 on the f i r s t correction attempt and is incremented upon each additional evaluation of the desired change in position dX, dY and dZ. So i f we have for example an a of .5 the sequence of desired changes in the direction would be: dXx = 0.5 AXj dX2 = 0.75 AX2 dX3 = 0.875 AX3 where AXj, AX2 and AX3 are the errors measured in the JC direction when the robot has stopped for the 1st, 2nd and 3rd times. If the algorithm works position errors w i l l be decreasing while the weighting factor increases. To calculate the required joint angle changes the Jacobian relation d8_ = J_-1dx is used. Recall that once these adjustments i n set point are applied no further action is taken until the robot has ceased moving. At this time new position errors are measured and the weighting factor updated. During the corrective procedure there are no adjustments in set points to compen-sate for gravity. When running the simulation the user i s asked to provide a value for ALPHA. The smaller a is the faster the weighting factor converges to 1 at 78. which point the required correction i s attempted i n one change of set points. Despite the simplicity of this algorithm we find that end piece position errors are driven to zero. However, two limitations of this algorithm may cause undesirable over-shoot. The f i r s t is that the value of the weighting factor may converge to unity too quickly at which point the robot no longer creeps towards the desired position. The second problem arises i f the errors are large, i.e. despite a conservative value of a the manipulator may be asked to move too great a distance in one motion, in which case the Jacobian relation breaks down. This situation i s shown graphically in Figures 4.6, 4.7 and 4.8. Figure 4.6 shows the response of the robot links moving to a nominal configuration defined by Q1 = 10°, 6 2 = 30°, and 63 = 60°. At this point the position error of the end piece is measured as 150, 15, and -75 m i l l i -meters i n the world coordinate 2£,y_, and z^  directions respectively. The adjustment of set points and the response of the robot links towards eliminating the end piece position error i s shown i n Figure 4.7. The behaviour of the position error i s shown in Figure 4.8, where we see that although the position error i s reduced to zero there i s an excessive over-shoot in the z_ direction of more than 40 mm. The shortcomings of this f i r s t algorithm arise from the fact that the weighting factor does not take into account the magnitude of the position errors nor their ratios. A more intelligent weighting factor i s developed in the next section. ANGULAR RESPONSE OF ROBOT LINKS Robot moving to nominal configuration (10,30,60) degrees. t o o Ld O to Ld _J o < -10-3 4 TIME IN SECONDS Legend A THETA1 X THETA2 • THETA3 H SETPT1 H SETPT2 X SETPT3 VO Figure 4.6. Simulation Run Including A l l Effects and Gravity Compensation. ANGULAR RESPONSE OF ROBOT LINKS Robot nominal config. (10,30,60)deg., position error (I50,15,-75)mm. 70 i — • — , 10 15 20 TIME IN SECONDS r 25 30 Legend A THETA1 X THETA2 • THETA3 H SETPT1 ffi SETPT2 X SETPT3 00 o Figure 4.7. First Position Controller Algorithm, Joint Angle Response. RESPONSE OF ROBOT END PIECE Robot nominal config. (10,30,60)deg., position error (150,15,-75)mm. 150-1 A 1 -50 H Legend A XERROR X YERROR • ZERROR 00 TIME IN SECONDS Figure 4.8. First Position Controller Algorithm, Position Error Behaviour. 82. 4.3.2.2 Second Algorithm In this algorithm the weighting factor i s chosen i n consideration of the magnitude of the largest position error AX, AY or AZ. The computation of the weighting factor i s done i n subroutine AJUSTP (adjust position) which is called by SUPCON each time the robot has come to rest. The magnitude of a position error w i l l generally be greater than zero and less than some maximum determined by the geometric constraints of the robot. This set of position errors for which the robot can correct i s divided into three ranges by two parameters assigned values in AJUSTP. The f i r s t is CLOSE and in the simulation i s given a value of 20 mm. The second i s BALLPK and is assigned a value of 40 mm. The weighting factor chosen depends on in which range the maximum position error l i e s . The weighting factors W and their range of application are shown below BALLPK CLOSE 40mm 20mm 0 w , BALLPK (1-ct) ! W = (1-a) ! W = ( l - a N ) ^~ BIGEST : : where BIGEST denotes the magnitude of the largest position error AX, AY or AZ and a is again the user specified parameter. This algorithm uses the same technique to move the weighting factor to unity as the f i r s t algorithm only after a l l position errors are less than 20 mm. For a l l errors greater than 20 mm the desired change in positon i s < 40(l-a) mm. Note that for CLOSE this algorithm to work properly a > otherwise the range between o A L L r K CLOSE and BALLPK may be jumped over. For the simulations in this work this implies that a > 0.5. 83. The behaviour of this algorithm i s best described by example. For simplicity let us just consider error in one direction AX and assume that the desired change i n position dX i s attained exactly for each correction interval, i.e., between robot stops. The behaviour of the position error, weighting factor, and desired correction i s shown below for two different values of a. a = .5 correction AX W dX (=WAX) interval (mm) (mm) 1 70 40(.5)/70 = .29 20 2 50 40(.5)/50 = .4 20 3 30 1-.5 = .5 15 4 15 1-.51 = .5 7.5 5 7.5 1-.52 = .75 5.6 6 1.9 1-.53 = .88 1.6 a = .6 correction AX W dX interval 1 70 40(.4)/70 = .23 16 2 54 40(.4)/54 = .30 16 3 38 1-.6 .4 15.2 4 22.8 1-.6 .4 9.1 5 13.7 1-.61 .4 5.5 6 8.2 1-.62 .64 5.3 7 3.0 1-.63 .78 2.3 * • * • • From the above we see that increasing a decreases the rate of convergence of the end piece to the desired position. 84 . The results of a simulation run with this algorithm are shown i n Figures 4.9 and 4.10. Again a nominal configuration corresponding to 6^10°, 9 2=30°, and 03=6O° i s used. The i n i t i a l end piece position error is specified as 50 mm in the x_ direction, 5 mm in the y^  direction, and -25 mm in the z_ direction. Figure 4.9 shows that the robot joint angles move smoothly in one direction towards their f i n a l values. However, despite the fact that the position error i s eliminated Figure 4.10 shows that there i s an overshoot of about 10 mm in the z_ direction. This could probably be reduced or eliminated by increasing the value of ALPHA and, i f necessary, decreasing values assigned to CLOSE and BALLPK. However in this algorithm no attempt was made to compensate for gravity during the corrective motions. The supervisory controller only compensated for gravity during the global motion of the manipulator. So instead of altering the above parameters we w i l l move on to the third and fin a l algorithm used for eliminating end piece postion errors. ANGULAR RESPONSE OF ROBOT LINKS Robot nominal config. (I0,30,60)deg., position error (50,5,-25)mm. CO Ul LJ P to UJ - J o z < 10 15 20 25 TIME IN SECONDS 30 35 Legend A THETA1 X THETA2 • THETA3 B SETPT1 ffi SETPT2 X SETPT3 00 Cn Figure 4.9. Second Position Controller Algorithm, Joint Angle Response. RESPONSE OF ROBOT END PIECE Robot nominal conf ig. (10,30,60)deg., position error (50,5,-25)mm. -20 H -40 Legend A XERROR X YERROR • ZERROR 15 20 25 TiME IN SECONDS 00 0>i Figure 4.10. Second Position Controller Algorithm, Position Error Behaviour. 87. 4.3.2.3 Third Algorithm This algorithm i s identical to the second with the exception that, i n addition to adjusting set points to correct for position errors each time the robot stops, the set points are adjusted every TSTEP seconds to compen-sate for gravity. TSTEP is set equal to 0.2 seconds unless otherwise noted. Running this algorithm for the case with the robot i n position 8^10°, 62=30° and 83=60° and an i n i t i a l position error of 50, 5, and -25 m i l l i -meters i n the and z_ directions respectively, we obtain the results shown in Figures 4.11 and 4.12. From these figures we see that the over-shoot in the z_ direction has been eliminated. We also note that the joint angles s t i l l proceed in one direction towards their f i n a l values. For the same i n i t i a l configuration but with an i n i t i a l position error of 150 mm in the x_ direction, 15 mm in the y direction, and -75 mm i n the z direction we again find a gradual response with no overshoot. These results are shown in Figures 4.13 and 4.14. ANGULAR RESPONSE OF ROBOT LINKS Robot nominal config. (I0,30,60)deg., position error (50,5,-25)mm. 70-1 I t o Ql O UJ O t o U i —I o < 10 15 20 25 TIME IN SECONDS 30 35 Legend A THETA1 X THETA2 • THETA3 H SETPT1 ffi SETPT2 X SETPT3 00 00 Figure 4.11. Third Position Controller, Joint Angle Response, Example 1. RESPONSE OF ROBOT END PIECE Robot nominal config. (I0,30,60)deg., position error (50,5,-25)mm. 01 o 01 CO o Q_ L d y Q_ Q _ Legend A XERROR X YERROR • ZERROR 20 25 TIME IN SECONDS Figure 4.12. Third Position Controller, Position Error Behaviour, Example 1. 00 ANGULAR RESPONSE OF ROBOT LINKS Robot nominal config. (10,30,60)deg., position error (150,15,-75)mm. to LxJ LxJ on o t o L d —I o 2 < 70^ 20 30 40 TIME IN SECONDS 50 60 Legend A THETA1 X THETA2 • THETA3 H SETPT1 H SETPT2 X SETPT3 O Figure 4.13. Third Position Controller, Joint Angle Response, Example 2. RESPONSE OF ROBOT END PIECE Robot nominal config. (10,30,60)deg., position error (I50,15,-75)mm. Legend A XERROR X YERROR • ZERROR TIME IN SECONDS Figure 4.14. Third Position Controller, Position Error Behaviour, Example 2. 92. Although this algorithm works well the convergence time demonstrated to this point is approximately 30 seconds. The majority of this time i s spent waiting for the robot to come to rest before any change in set points is made. Recall that a stopped condition requires that the link angular velocities be less than the value of STOP. STOP i s user specified and to this point has been given a value of .003 degrees/second. To speed up the correction process a simulation was run with a value of STOP equal to 0.1 degrees/second. The correction algorithm works as before but the robot i s no longer stationary while the changes in set points are made. The results for the i n i t i a l configuration of 6^10°, 6 2=30°, and 93=60° with i n i t i a l position errors 50.22, 5.02, and -24.06 mm i n the j£,y_, and z_ directions are shown in Figures 4.15 and 4.16. Actual numerical values are shown in Table 4.2. This table represents typical output from a run with ROBSIM.FOR using the position controller. Note that the errors have been modified to yield the same desired f i n a l end piece location. The algorithm s t i l l works and convergence time has been reduced by about 50% to 17 sec. However, the table shows that the end piece overshoots the desired position by .101 mm in the x_ direction and .019 mm in the y_ direction. Although small, these overshoots indicate the destabilizing effect that adjusting set points while the robot is s t i l l moving has on the correction process. Recalling that the allowed tolerance i s .05 mm this performance may be acceptable. These results imply that the value of STOP could be adjusted during the corrective motion to speed up response and prevent overshoot. ANGULAR RESPONSE OF ROBOT UNKS Robot nominal config. (10,30,60)deg., position error (50.22,5.02,-24.06)mm. 70 Legend A THETA1 X THETA2 • THETA3 H SETPT1 ffi SETPT2 X SETPT3 0 5 10 15 20 TIME IN SECONDS Figure 4.15. Third Position Controller, Joint Angle Response, STOP = 0.1°/sec. RESPONSE OF ROBOT END PIECE Robot nominal config. (10,30,60)deg., position error (50.22,5.02,-24.06)mm. 6O-1 Ql o Ql Ql U l CO o Q_ UJ a CL O Z U l 40 H -40 H Legend A XERROR X YERROR • ZERROR 8 10 12 TIME IN SECONDS T 18 Figure 4.16. Third Position Controller, Position Error Behaviour, STOP = 0.1°/sec. Table 4.2. Position Supervisory Controller Simulation. SIMULATION TRIAL NUMBER=140 HAVE CORIOLIS AND CENTRIPETAL EFFECTS BEEN IGNORED? N HAVE GRAVITY EFFECTS BEEN IGNORED? N HAVE INERTIAL COUPLING EFFECTS BEEN IGNORED? N ARE ARMATURE INDUCTANCES ASSUMED NEGLIGIBLE? N ARE BACK EMF EFFECTS ASSUMED NEGLIGIBLE? N ARE MOTOR DAMPING EFFECTS IGNORED? N IS SUPERVISORY CONTROLLER BYPASSED? N IS GRAVITY COMPENSATION PACKAGE ACTIVATED? Y ANGULAR VELOCITY BELOW WHICH LINK IS CONSIDERED STATIONARY IS 0.1000 DEG/SEC ACCEPTABLE POSITION ERROR IS 0.0500 MILLIMETERS POSITION CONVERGENCE FACTOR ALPHA'O.50O00O LINK 1: ACTUATOR GAIN(KM)= 10.000 PROPORTIONAL GAIN(KE)= 30.000 VELOCITY FEEDBACK GAIN(KV)= 13.000 INITIAL JOINT ANGLE= 0.000 DEGREES INITIAL JOINT VELOCITY' 0.000 DEGREES PER SECOND FINAL DESIRED JOINT ANGLE' 10.000 DEGREES LINK ACTUATOR GAIN(KM)= 10.000 PROPORTIONAL GAIN(KE)= 25.OOO VELOCITY FEEDBACK GAIN(KV)= 14.500 INITIAL JOINT ANGLE' 0.000 DEGREES INITIAL JOINT VELOCITY' 0.000 DEGREES PER SECOND FINAL DESIRED JOINT ANGLE' 30.000 DEGREES LINK 3: ACTUATOR GAIN(KM)= 10 PROPORTIONAL GAIN(KE)= VELOCITY FEEDBACK GAIN(KV)= INITIAL JOINT ANGLE' INITIAL JOINT VELOCITY' FINAL DESIRED JOINT ANGLE' 000 20.000 6.800 0.000 DEGREES 0.000 DEGREES PER SECOND 60.000 DEGREES v o L n Table 4.2. (Continued). TIME THETA1 THETA2 THETA3 SETPT1 (SEC) (DEG) (DEG) (DEG) (DEG) 0 .0000 0 .OOOO 0 .0000 0 .0000 10 .0000 0 . 2000 2 .6781 4. . 7202 14. .8398 10, .0000 O .4000 4. .7403 1 1 . . 1918 30 .1512 10, .0000 O .6000 6 . 2372 16 .3712 40 .5264 10. .0000 0 .8000 7 . 3732 20 . 1495 47 .2713 10, .0000 1 .0000 8 .2057 22 .8535 51 .6462 10, .0000 1 , .2000 8 .7926 24 . 7906 54. .4995 10, .0000 1 .4000 9 . 1956 26 . 1865 56. .3710 10, .0000 1 .6000 9 .4679 27 . 1985 57. .6037 10. .0000 1 .8000 9 .6500 27 .9360 58 .4178 10, .0000 2 .0000 9 .7709 28 .4756 58, .9562 10. OOOO 2 .2000 9 .8508 28 .8719 59 .3124 10. OOOO 2 .4000 9 .9033 29 . 1637 59, .5480 10. .0000 2 .6000 9 .9378 29 . 3790 59, , 7037 10. .0000 2. ,8000 9 ,9602 29 .5383 59. ,8064 10. OOOO 3 OOOO 9 .9748 29. .6563 59 ,8740 10. OOOO 3. .2000 9 .9842 29. .7439 59. 9185 10. OOOO 3. .4000 9 .9902 29 .8090 59. 9476 10. OOOO 3 . 6000 9 .9940 29 .8574 59. 9665 10. OOOO 3. ,8000 9. .9965 29. .8935 59. ,9789 10. OOOO 4 .0000 9. .9980 29. .9204 59. 9868 10. OOOO 4 . 2000 9 .9989 29 .9404 59. 9919 *********STOPPED AT************ * ROBOT GRIPPER LOCATION: (IN MILLIMETERS) REQUIRED POSITION: PXD= 773.564 PYD= 292.024 PZD=-190.973 ***START WITH ADJUSTED SET POINT**** 4 , 2000 9. 9989 29. .9404 59, ,9919 10 .0998 4 .4000 9. .9799 31 , .0001 57, .2969 10 .0998 4 .6000 10. ,0229 31 .9817 54, .8583 10 .0998 4 .8000 10. .0560 32 .7331 53. . 1884 10 .0998 5, .0000 10. 0770 33 .3152 52. 0518 10 .0998 5 . 20OO 10. 0899 33, .7656 51 . 2785 10 .0998 5, .4000 10. ,0975 34, .1131 50. .7520 10 .0998 5 .6000 10. . 1016 34 . 3803 50. .3934 10 .0998 5 .8000 10. 1037 34 . 5854 50. 1487 lO .0998 6 .0000 10. , 1044 34, .7425 49. ,9816 10, .0998 6 . 200O 10. , 1045 34 , .8625 49. ,8673 10, .0998 6 .4000 10. . 1042 34, .9540 49, ,7890 10 .0998 6 6 OOO 10. 1037 35 .0238 49. , 7352 10 .0998 6, .8000 10. 1031 35. ,0768 49. 6983 10. .0998 SETPT2 SETPT3 (DEG) (DEG) 7 .6612 60 .0000 4, . 3449 55 . 7599 1 , , 3933 51 .6341 0, ,0796 49 . 3908 -0 , 3268 48 .3060 -0. .3485 47 .7959 -0. ,2355 47 .5559 -0, ,0924 47 .4419 0. .041 1 47 . 3873 0. . 1530 47 . 3609 0. ,2422 47 . 3480 0. ,3112 47 , .3416 0. ,3639 47, .3384 0. .4036 47 . 3369 0. 4336 47 , . 3361 0. 4560 47 , .3357 0. 4727 47 , 3355 0. 4852 47 , 3354 0. 4946 47 , .3353 0. 5016 47 . , 3353 0. 5068 47. ,3353 PX PY PZ 823 . 784 297 .045 -215 .034 5, .7540 36 .9500 823. . 784 297 .045 -215. ,034 5 .9679 36 .9556 819 .680 296 .032 -207 . 981 6 . 1788 36 .9693 815 . 132 295 .882 -201 .880 6 . 3458 36 .9821 811 , .488 295 .738 -198 .850 6 .4770 36 .9914 808. ,671 295, .554 -197. .816 6 . 5788 36 .9974 806. 531 295 . 367 - 197 , ,903 6, .6572 37 .0008 804 . ,920 295, . 193 -198, ,557 6 .7173 37 .0027 803, ,712 295, .040 -199, .447 6. . 7632 37 .0035 802. 810 294 , 910 -200. 386 6 , . 7982 37 .0037 802. 135 294 .801 -201 . ,274 6 . .8248 37 .0036 801 . ,630 294 , .712 -202 , 065 6 .8450 37 .0033 801 , 254 294 , 641 -202 , . 743 6 .8603 37 .0030 800. .971 294 , . 583 -203. .310 6 , .8720 37 .0026 800. 760 294 .537 -203. , 774 Table 4.2. (Continued)... 7 .0000 10. , 1026 35 . 1 172 49 .6728 10 .0998 7 .2000 10. 1021 35 . 1478 49 .6552 10 .0998 7 .4000 10. . 1017 35 . 1710 49 .6430 * * * * * * * * S T O P P E D A T * * * * * * * * * * * * 7 . 4000 10. . 1017 35 . 1710 49 .6430 10, . 1789 7 .6000 10. 0944 35 .7002 48 .2781 10, , 1789 7 .8000 10. 1252 36 . 1860 47 .0354 10, . 1789 8 .0000 10. 1476 36 .5582 46 . 1852 10, . 1789 8 .2000 10. , 1617 36 .8474 45 .6075 10, . 1789 8 .4000 10. . 1704 37 .0716 45 .2152 10, . 1789 8 .6000 10. 1756 37 . 2449 44 . 9485 10, . 1789 8 .8000 10. 1785 37 . 3785 44 .7670 10, , 1789 9 .0000 10. . 1801 37 .4811 44 .6434 10, . 1789 9 . 20O0 10. 1808 37 .5598 44 .5590 10. , 1789 9 .4000 10. 1810 37 .6200 44 .5014 10. 1789 9 . 6OO0 10. 1810 37 .6660 44 .4619 10, 1789 9 .8000 10. . 1808 37 .7010 44 .4349 10, , 1789 10 .0000 10. . 1805 37 .7277 44 .4163 10. , 1789 10 .2000 10. 1803 37 .7481 44 .4035 ******** STOPPED A T * * * * * * * * * * * * 10 .2000 10. 1803 37 . 7481 44 .4035 10. 2206 10 . 4000 10. 1786 37 .9717 43 .8309 10. 2206 10 .6000 10. 1939 38 . 1749 43, .3081 10. 2206 10 .8000 10. 2049 38 .3306 42, .9506 10. 2206 11 .0000 10. 2117 38 .4516 42, .7080 10. 2206 11 .2000 10. 2160 38 .5455 42, .5433 10. 2206 11 .4000 10. 2185 38. .6182 42, ,4314 10. 2206 11 .6000 10. 2200 38. .6742 42, .3553 10. 2206 11 .8000 10. 2208 38 .7173 42. , 3035 10. 2206 12 OOOO 10. 2212 38 .7503 42. 2682 10. 2206 12 .2000 10. 2214 38 .7756 42 , 2440 10. 2206 12 4O00 10. 2214 38 . 7949 42. , 2275 ******** STOPPED A T * * * * * * * * * * * * 12 .4000 10. 2214 38. .7949 42. ,2275 10. 2530 12 .6000 10. 2209 38. .9539 41 . ,8203 10. 2530 12 .8000 10. 2326 39. .0973 41 . .4488 10. 2530 13 OOOO 10. 2409 39. . 2073 41 . 1948 10. 2530 13 . 2000 10. 2461 39. . 2928 41 . 0224 10. 2530 13 .4000 10. 2493 39. ,3592 40. ,9055 10. 2530 13 .6000 10. 2513 39. ,4105 40. 8261 10. 2530 13 .8000 10. 2524 39. ,4501 40. 7721 10. 2530 14 OOOO 10. 2531 39. ,4806 40. 7354 10. 2530 14. . 2000 10. 2534 39. ,5040 40. 7104 10. 2530 14 .4000 10. 2535 39. 5219 40. 6932 ******** STOPPED A T * * * * * * * * * * * * 14 .4000 10. 2535 39. 5219 40. 6932 10. 2630 14 .6000 10. 2537 39. ,5734 40. 5747 10. 2630 14 . .8000 10. 2572 39. 6183 40. 4682 10. 2630 6 .8808 37 .0023 800 .602 294 .501 -204 . 149 6 .8874 37 .0021 800 .484 294 .472 -204 .449 9 .4610 31 .7615 800 .395 294 .450 -204 .686 9 .5950 31 .7795 797 .470 293 .822 -200 .845 9 . 721 1 31 .7982 794 .488 293 . 745 -197 .396 9 .8175 31 .8111 792 .244 293 .675 -195 .612 9 .8915 31 .8193 790 . 577 293 .585 -194 .921 9 .9480 31 .8242 789 .345 293 .491 -194 .851 9 .9910 31 .8270 788 .436 293 .404 -195 . 104 10 .0238 31 .8285 787 . 765 293 .327 -195 .500 10 .0486 31 .8291 787 .270 293 . 261 -195 .937 10 .0674 31 .8293 786 .904 293 . 205 -196 . 360 10 .0817 31 .8292 786 .634 293 . 160 -196 .741 10 .0925 31 .8290 786 .433 293 . 123 -197 .071 10 . 1006 31 .8287 786 . 284 293 .094 -197 .348 10 . 1068 31 .8285 786 . 173 293 .071 -197 .576 1 1 . 1559 29 .6448 786 .091 293 .052 -197 . 760 1 1 , .2180 29 .6556 784 .672 292 .773 -196 . 145 1 1 , .2751 29 .6658 783 .274 292 .745 -194 .655 1 1 .3180 29 .6725 782 .250 292 .720 -193 .876 1 1 , ,3506 29 .6766 781 .501 292 .686 -193 , .566 1 1 , .3754 29 .6790 780, .954 292 .649 -193 , .525 1 1 , ,3941 29, .6804 780 .553 292 .614 -193 , .623 1 1 , ,4083 29, .681 1 780. .260 292 .582 -193 , , 784 1 1 , ,4190 29, .6814 780. .045 292 , .555 -193 . .963 1 1 . .4272 29 .6815 779, .886 292 , .533 - 194 , 138 1 1 . ,4333 29, .6814 779, . 769 292 , ,514 -194 . , 297 12. , 1607 28. . 1460 779. ,683 292 . ,499 -194 . ,434 12 . 2065 28. , 1547 778 . 616 292 . ,299 -193 . 277 12 . 2483 28. , 1629 777. ,575 292 . ,281 - 192 . 199 12 . 2796 28. 1682 776 . 818 292 . 265 - 191 . 630 12 . 3032 28. 1714 776 . 268 292 . 24 1 -191 . 399 12 . 321 1 28. 1733 775. 868 292 , 215 -191 . 362 12 . 3347 28 . 1743 775. 576 292 . 190 -191 . 425 12. 3449 28. 1749 775. 363 292 . 168 -191 . 536 12 . 3527 28. 1751 775 . 207 292. 149 -191 . 660 12 . 3585 28. 1751 775. 093 292 . 133 -191 . 782 12 . 5516 27. 7600 775. 009 292 . 120 -191 . 893 12 . 5665 27 . 7626 774 . 669 292 . 061 -191 . 630 12 . 5795 27 . 7649 774 . 350 292 . 054 -191 . 372 Table 4.2. (Continued)... 15.0000 10.2596 39.6526 40. . 3954 10. , 2630 12 .5892 27, , 7664 774 . 120 292 .047 - 191 . 248 15.2000 10.2611 39.6792 40, , 3460 10. . 2630 12 .5965 27, .7672 773 .953 292 .039 -191 .211 15.4000 10.2620 39.6998 40. ,3124 10, , 2630 12 .6020 27, . 7677 773 .832 292 .030 -191 .222 15.6000 10.2626 39.7157 40, .2896 *********STOPPED AT************ * 15.6000 10.2626 39.7157 40. .2896 10. 2652 12 .6255 27 , 7036 773 .743 292 .022 -191 .257 15.8000 10.2632 39.7322 40 ,2578 10. .2652 12 .6301 27 , . 7042 773 .640 292 .01 1 -191 .224 16.OOOO 10.2640 39.7450 40. , 2318 10. .2652 12 .6337 27 , 7047 773 .556 292 .008 -191 . 187 16.2000 10.2646 39.7548 40, .2141 *********STOPPED AT************ * 16.2000 10.2646 39.7548 40. 2141 10. 2656 12, ,6185 27. .71 18 773 .495 292 .005 -191 . 177 16.4000 10.2651 39.7593 40. ,2041 *********STOPPED AT************ * 16.4000 10.2651 39.7593 40 .2041 10. .2654 12 .5972 27. .7287 773 .463 292 .008 -191 . 157 16.6000 10.2655 39.7576 40. .2020 10. , 2654 12 .5969 27 . 7288 773 .463 292 .014 -191 .115 16.8000 10.2655 39.7550 40. ,2011 *********STQppgp AT************ * 16.8000 10.2655 39.7550 40. 2011 10. 2649 12, .5779 27 . ,7487 773, .471 292 .015 -191 .071 17.OOOO 10.2655 39.7495 40. 2058 10. 2649 12 .5766 27. ,7487 773 .495 292 .019 -191 .033 17.2000 10.2653 39.7444 40. ,2101 *********5T0Pp£Q AT************ * 773 .519 292 .020 -190 .997 END P I E C E LOCATION WITHIN REQUIRED TOLERANCES SET POINT 1= 10.2649 SET POINT 2= 12.5766 SET POINT 3= 27.7487 NUMBER OF POINTS EVALUATED 8786 00 99. 4.3.3 Conclusions At this point we have shown that i t i s possible to eliminate end piece position errors by adjusting the set points of the robot link control loops i n a coordinated manner. The development of an algorithm u t i l i z i n g the robot simulation has been illustrated. Much work could be done to further improve this algorithm. For instance the algorithm could monitor the improvement in measured error and i n so doing develop a cause and effect relation between set point adjust-ment and end piece position changes. This provides a method of checking and improving the elements of the Jacobian which are effected by errors in the measurement of angles (encoder errors). For this work we have assumed that measurement errors are a second order effect and thus can be neglected. 4.4 Force Controller In many situations the task the robot is to perform is more easily described by specifying an applied force rather than a positional change. For example in a grinding or deburring operation the tool may be required to push against the work piece with a force of 10 Newtons. In our three degree of freedom simulation we w i l l examine the a b i l i t y of the robot to exert a desired force on i t s surroundings with the end piece. The force w i l l be specified as a 3x1 vector [f f f ] defined with x y z respect to the world coordinate system. To simulate this new situation two tasks must be completed. The f i r s t task is to design an algorithm to use in our supervisory controller. The second task i s to include i n the robot dynamics model the generation and effects of the forces that w i l l be applied to the end piece by the surroundings. 100. 4.4.1 Relating Applied Forces to Joint Torques From the success in compensating for gravity effects we know i t i s possible to create desired joint torques by modifying the set points of the link PD control loops. We now need to relate changes in joint torques to the forces applied to the surroundings by the gripper. To do this we w i l l use the method of virtual work. Consider the robot i n static equilibrium with i t s surroundings and with joints 4, 5 and 6 locked. Neglecting gravity loading the torques exerted by the actuators T, , x 0 and t. w i l l result i n forces f , f and f i z. i x y z exerted on the surroundings. Now we consider a differential imaginary (virtual) displacement of joint angles 69^ 692 and <593 which w i l l corres-pond to a virtual displacement of the end piece 6X, SY and 6Z. The virtual work done on the robot by the joint torques i s The virtual work done on the robot by external forces applied to the end 6 W1 " t T l T2 T 3 l T = T 68 (4.15) piece i s 6X (4.16) The virtual displacements are small and in the limit do not change the energy of the system. Thus fiWj + 6W2 = 0 T T69 - f T6x = 0 101. (4.17) (4.18) Recall that 6x = J60 where J is the Jacobian. We can then write T T x 69 - f J69 and fi n a l l y T ,TT T = f J x = J T f (4.19) Given an applied force on the surroundings the above equation speci-fies the joint torques which must be supplied to maintain equilibrium. If the manipulator is free to move in the direction of the applied force the specified joint torques w i l l result in the application of the desired force. These torques are in addition to any others required to hold position, i.e. against gravity. This relation forms the basis of the supervisory controller algorithm. Before discussing the algorithm in detail we w i l l consider how applied forces can be included i n the simulation. 4.4.2 Modifying Simulation to Include External Forces, a Newton-Euler  Approach In order to examine the application of desired forces we need to have a model of the surroundings which w i l l generate the external forces applied to the end piece. To do this we consider the fact that i n the application of a force neither the robot nor the surroundings are r i g i d . Both the robot and contact surface w i l l deflect i n the presence of an applied force 102. u n t i l the potential energy in the deflected members balances that of the robot control servos. To model this interaction we w i l l assume that the robot i s rig i d and the surroundings deflect under the force applied by the end piece. Also the reactive force generated by the surroundings is linearly proportional to the deflection. In other words, the robot grlpper is assumed to be attached to three rigidly mounted springs. One in each of the i£, y_ and ^ directions. The stiffness of each spring represents the stiffness of the surroundings in that direction. The force applied to the end piece i s then F = -K 6X , F = -K <5Y and F = -K 6Z (4.20) x x y y z z where 6X, 6Y and <5Z are the changes in gripper position. The negative sign indicates that the forces on the gripper act against the direction of the gripper's change in position. The simulation program asks the user i f the force controller i s to be used. If so the user is asked to input values of K , K and K . x y z These external forces cause disturbance torques which must be included i n the dynamics model. To find these contributions we use the Newton-Euler recursive dynamics equations. The derivation i s based on the kinematic relations of the moving links defined with respect to world coordinates. These are combined in the general vector equations of motion dzr_ (4.21) L i = ™A — ~ and N . = - r - U ^ , ) j = 1,2, 6 dt J J - J (4.22) 103. where referring to Figure 4.17. F_ = net force vector on link j through centre of mass m. = mass of link j J v_ = position vector of center of mass of link j N. = net moment vector about center of mass of link i - J J I. = inertia matrix of link j about center of mass - J 00j = angular velocity of link j coordinate frame v = velocity of origin of link j coordinate frame _f = force vector exerted on link j by link j-1 rij = moment vector exerted on link j by link j-1 L_j = vector from origin of link j-1 coordinate frame to origin of link j coordinate frame S^. = vector from origin of link j coordinate frame to center of mass of link j 6^  = rate of joint angle change for link j x.,y.,z. = the axes of coordinate frame for link j - J ^ J J = scalar input torque exerted on link j Note: a l l the above vectors (and inertia matrix) are defined with respect to the world coordinate frame. Using the theorem of Coriolis we can write co ... =. u>. + 9 ... z . (4.23) -J+l - J J+l - J -J+l - J J+l - J - J J+l - J -v ' 104. ttl S p a t i a l diagram of j l i n k Y i "3 '*0 ^ 0 F r e e body diagram of .th J l i n k ^j+1 gure 4.17. Robot L i n k Newton-Euler V e c t o r s . v = v + oi x L - j "j+1 -j+1 105. (4.25) v.,1 = v. + (*)..- xL.,_ + to.,, x (a) x L . ) (4.26) —J+1 - J -J+1 -J+1 -J+1 -J+1 -J+1 The above equations can be solved recursively from j = 0 to 6. To include the effects of gravity OJ^  = ®Q = V_Q = 0_ and v Q = - j j . Using the vector equations of motion the following equations can be obtained. See Luh et a l . (1980) for a more detailed derivation. n.= n . , . + L . x f + ( L . + S . ) x F . + N. (4.27) - J "J+1 — J "J+1 " J - J " J " J i j - + li <<-28> F. = (v. + a), x S. + to. x (oi. x S.)) (4.29) - J j - J - J — J - J — J - J N. = I.u). + oi. x (I.cu.) (4.30) - J -J-J - J -J-J x . = n. • z. . (4.31) J - J -J-1 These equations can be solved recursively from j = 6 to 0. A l l vectors i n the above derivation are expressed with respect to the world coordinate frame F . The inertia matrix I . is found also with 0 - J respect to F Q and is computationally messy. This problem is solved by using the rotational part of the homogeneous transform to relate the vectors to their respective link coordinate frames. It is necessary then only to find I_j, the inertia matrix of link j defined with respect to the 106. coordinate frame F . which is fixed on the link. Thus I"! is the usual inertia matrix. For this section of work we desire only the contributions to the dynamics equations of the robot caused by an external arbitrary load on the end piece. To determine these components we can use the Newton-Euler formulations and apply them to the robot assuming the links have no mass. This implies that a l l inertia terms are zero. The vector equations then become w - co .+ 6 z . (4.32) - J+l - J J+l - J a) ., = co .+ 6 .,, z. + (o. x 9 ... z. (4.33) - J+l - J J+l - J - J J+l - J Vj+i = Vj + x L . + 1 + x ( £ i j + i x L. + i) (4.35) |\ =0 (4.36) N. =0 (4.37) -J -n. - n t t 1 + L. x f (4.39) - J "J+l " J "J+l From the geometric definition of a link we can write 107. 0 0 0 L. = a.x. + d.z. , (4.41) -J J-J J-J-1 Recalling that 0 -j 0 0 3~1 0 R. x. = x, and R. 1 z. . = z. . (4.42) —J — J - j -J-1 -J-1 -J-1 then 0 0 J 0 J ~ l L. = a. R. x. + d. R. . z . . (4.43) -J J - J - J J -J-1 -J-1 This simplified set of equations provides a method of determining the disturbance torques caused by an arbitrary load on the end piece. Such a condition arises when the robot gripper i s carrying a heavy tool which is applying a force to the work piece. To compute the additional torques required to carry the tool and apply the force we f i r s t attach a coordinate frame, F 7, to the mass centre of the tool, oriented to simplify the calculation of the tool's inertia matrix. F 7 w i l l be fixed relative to the end piece coordinate frame Fg and the 6 homogeneous transform Ay can be calculated. The forward recursive equa-tions can then be used to find ay 7 , j £ , v 7 and _v7» Having these vectors we then find f_7 and n_7, the force and moment exerted on the end piece, by applying EF = ma and EM = H (H = angular momentum) (4.44) 108. about the mass center of the tool. The backward set of recursive equations can then be evaluated to determine the joint torques required. If in evaluating the equations 9.., 9^  and 9^  are l e f t as variables a closed form set of equations results. These formulations provide a method for determining the effects on joint torques caused by an arbitrary load. These effects could then be compensated for by adjusting set points i n a manner analogous to that used for gravity disturbance torques. For the simulations i n this work there i s no carried mass only the external force f_ applied to the end piece at the origin of F g. Thus the forward recursive equations are not required. To solve the backward recursive equations the rotational transform relating each link coordinate frame to world coordinates i s required. Recalling that joints 4, 5 and 6 are locked we can write *3 * 5 c l 0 - s l 1 C2 "S2 0 = s l 0 c l *2 " s 2 C2 0 0 -1 0 _ 0 0 1 C 3 0 S 3 3 1 0 0 S 3 0 ~ C 3 0 0 1 0 1 0 0 -1 0 1 0 0 1 0 0 5 0 0 -1 Is - 0 1 0 0 1 0 0 0 1 (4.45) Now we obtain 109. 0 0 1 C 1 C 2 " C 1 S 2 = Ri £ 2 S 1 C 2 _ S 1 S 2 C l (4.46) - s 2 - c 2 0 0 0 2 C 1 C 2 3 " S l C 1 S 2 3 *3 - *2 *3 = S 1 C 2 3 C l S 1 S 2 3 (4.47) " S 2 3 0 C 2 3 0 0 3 C 1 C 2 3 " C 1 S 2 3 - S l " = h = S 1 C 2 3 ~ S 1 S 2 3 C l (4.48) ~ S23 ~ C23 0 0 0 C 1 C 2 3 C 1 S 2 3 = 1% = S 1 C 2 3 C l S 1 S 2 3 (4.49) " S23 0 C 2 3 0 0 5 0 Re = *s *6 = Is (4.50) 0 0 J 0 J - 1 We also require L. = a.R. x. + d.R. , z. , - J J-J - J J - J - l -J-1 where x . - J 1 0 0 and j-1 0 0 1 (4.51) Thus 0 ki = 9-0 2 a 2 1 ^ X 2 0 1 C 1 C 2 S 1 C 2 " S o - s , a 2 C l C 2 " d 2 S l a 2 S l C 2 + d 2 C l - a 2 S 2 (4.52) 110. = 0 (4.53) 0 3 di+!*3il3 di+ Cl S23 d 4 S l S 2 3 d4 C23 (4.54) = 0 0 5 d6^5l5 d 6 C l S 2 3 d 6 S l S 2 3 d6 C23 (4.55) (4.56) Now we can evaluate the backward recursive equations. Recall that i f no superscript i s written i t can be taken as 0. f • = (4.57) n . = n + L . x f - J -J+1 - J -J+1 (4.58) 0 J " 1 n . , z . , = n . » R . 1 z . 1 - J -J-1 " J -J-l-J-1 (4.59) First we define f j = force on end piece = -(force on surroundings) = -f (4.60) n_7 = torque on end piece = 0_ (4.61) 111. Now for j = 6 (4.62) u - - i £s " £ + £6 x ("I) " ( - d 6 S l S 2 3 f z + d 6 C 2 3 f y ) - + ( d 6 C l S 2 3 f z " d 6 C 2 3 f x U + ( - d 6 C l S 2 3 f y + d 6 S l S 2 3 f x ) k (4.63) we are only interested in torques for links 1, 2 and 3. For j =5 Is = "I (4.64) £ 5 " £ 6 + 1 (4.65) For j = 4 " "I (4-66) £f = £ 5 + £tf x ("£) (4.67) D4 Note rig = n_6 and = - j — which implies that (4.68) 6 £, = (d,+d 6 ) [ ( - s 1 s 2 3f z + c 2 3 f ) i + ( C j S ^ f - c 2 3 f ) j + ( - C l S 2 3 f y + S l S 2 3 f x > ^ < 4* 6 9> 112. For j = £ 3 3 -f £4 + £ T0 - n c 0 2 (4.70) (4.71) o r T 3 = £ 3 ' 1 where q = = ( W < - C l C 2 3 f x - S l C 23 f v + S 2 3 f z ) (4.72) For j =2 12 = "I £2 = £3 + Ii2 x (4.73) (4.74) x (-f) = [ ( ^ S ^ - d 2 C l> f z " a 2 S 2 f y ] i + [(a^C;, - d 2 S l ) f z + a 2 S 2 f x U + [ ( - 8 2 0 ^ 2 " d 2 S l ) f y + <a2SlC2 + D 2 C l > £ k (4.75) 0 1 T 2 = £2 * (4.76) £2 * £ = n, • q + L, x (-f) • q = T3 + £2 * ("£) ' £ = + a 0C,S 0f + a~S,S0f + a„C„f 3 ' a2^1u2 l2""2J 113. Let d 5 = d^+dg then T2 = " C l ( d 5 C 2 3 " a 2 S 2 ) f x " S l ( d 5 C 2 3 ~ a 2 S 2 > f y + ( d 5 S 2 3 + a 2 C 2 > f j (4.77) For j =1 - f l l n, = T, = 0 0 £1 • io£o RQ i s the identity matrix (4.78) (4.79) (4.80) = Hi " £ 0 T j = [s i(d 5S 23+a 2C 1)+d2C 1]f x + [-C 1(d 5S 23+a 2C 2) + d 2 S j f y (4.81) The equations for T 1, T 2 and x 3 are equivalent to those found using virtual work and the Jacobian. However the above method can be expanded to include an arbitrary load. 4.4.3 Results and Implementation 4.4.3.1 Firs t Algorithm The f i r s t algorithm i s designed along the lines of the position controller. The robot end piece is moved to some nominal location. In this position the end piece i s assumed to be i n contact with i t s surroundings but is exerting no force. The supervisory controller is then given a set of desired forces to apply to the surroundings and the relation T T_ = J_ f_ is used to calculate the additional joint torques required. These are then created by adjusting the set points i n an analogous fashion as was 114. done to compensate for gravity. After the robot has finished responding to these changes in set point the actual force applied to the surroundings i s measured. The required change in force i s calculated and the additional T joint torques needed to create this change computed using T_ = J_ f_. The set points are adjusted accordingly and applied. The process i s continued until the desired force is applied to the surroundings. Unfortunately this algorithm does not work. For a contact configuration corresponding to 61 = 10°, 8 2 = 10° and 93 = 65° and a desired force o f f = 1 N, f = 2 N and f = 0 N after 200 seconds the x y z actual force applied was f = 0.1 N, f = 1.7 N and f = -0.1 N. The x y z reason this algorithm does not work can be explained as follows. The torques required are generated by adjusting the set points by an amount A0. If the robot and surroundings were perfectly rig i d this steady state error would persist generating the required joint torques and hence the desired applied forces. However the robot end piece although constrained can move. Given an i n i t i a l difference between adjusted set point and observed angle ((0^ + A0) - 9) for any link the control system of the robot w i l l cause the end piece to move in a direction which reduces the magnitude of this effect. The joint angle moves towards (0^ + A0) by an amount 60 so that at steady state the joint torque is T = K K [(0, + A0) - (0 + 60)1 (4.82) ss m e 1 d J x ' which w i l l always be less than the torque required T = K K [(8. + A8) - 81 (4.83) req m e 1 d J v ' Thus the applied forces are always less than those desired. 115. 4.4.3.2 Second Algorithm The second algorithm attempts to maintain the difference between the set points and the observed angles. Recall that the f i r s t algorithm adjusted the set points each time the robot response was complete using T T_ = J _ f_. In addition to this the second algorithm updates the set points every TSTEP seconds (usually 0.2 seconds) by adding to them the amount that the corresponding angle has changed, i.e.; A9 = A6 + 69 . This has - t 2 - t x — t 2 " t l the effect of maintaining the required difference between set points and the observed angles. The simulation results using this algorithm are much improved. For an i n i t i a l contact configuration of Q-^ = 10°, &2 = 10° and 63 = 65° the robot created the desired forces f = 1 N, f = 2 N and f = 0 N, within 24 x y z seconds. The results of a t r i a l with an i n i t i a l contact configuration of 9,=10°, 99=30° and 6o=60° and desired exerted forces f =10 N, f =20 N and l » i i x y f =0 N are shown in Figures 4.18 and 4.19. For this example the applied forces were created within a tolerance of 1 N i n 34 seconds. However the applied forces tend to creep towards their desired values. This i s caused by the nature of the updating when the robot approaches a stopped condition. As the robot begins to stop the changes in observed angles at each 0.2 second interval decrease. These changes are added to the set points every 0.2 seconds and although the changes are small the robot responds to them. The result i s that the robot creeps as i t nears the desired configuration. Figure 4.19 also illustrates that the robot's a b i l i t y to apply force with the end piece i s very strongly affected by the robot's configuration. In particular i t s deflected configuration. Referring to the results of Figure 4.19 we see that the robot configuration is such that applying a force in the y direction is very effective. 116. However, the application of this force causes a deflection which changes the configuration and hampers application of the required force in the x direction. ANGULAR RESPONSE OF ROBOT LINKS Robot nominal config. (10,30,60)deg., desired exerted force (10,20,0)N. 80-1 60-5! 4 0 - I 20-« r • - X - A 10 20 30 TIME IN SECONDS r 40 50 Legend A THETA1 X THETA2 • THETA3 H SETPT1 H SETPT2 X SETPT3 Figure 4.18. Second Force Controller Algorithm, Joint Angle Response. FORCES EXERTED BY ROBOT END PIECE Robot nominal config. (10,30,60)deg., desired exerted force (10,20,0)N. 25 i , 10 z LxJ LxJ O OH O on LxJ X LxJ 20-15-10-5 -0 -•54 5 Legend A XFORCE X YFORCE • ZFORCE H FXDESRD H FYDESRD X FZDESRD 15 20 25 30 TIME IN SECONDS 35 40 45 Figure 4.19. Second Force Controller Algorithm, Applied Forces. 119. 4.4.3.3 Third Algorithm Before designing a third algorithm a few assumptions are made about the nature of force applications. For most tasks a nominal applied force i s sufficient. Accuracy requirements are much less stringent than i n posi-tion control. Also a reasonable overshoot of an applied force i s usually acceptable. Finally the object on which the force i s applied may i t s e l f move resulting in significant changes in the robot configuration. This w i l l effect both the Jacobian relation and the gravity torques. With these points in mind the fi n a l force controller is designed and implemented. This controller measures the desired change i n force every TSTEP seconds. It then evaluates the Jacobian and uses the relation T _x = J_ f_ to compute the required joint torque additions. The corresponding adjustments in set points are calculated and applied. This process i s repeated every TSTEP (usually 0.2) seconds. In addition the set points are also adjusted every TSTEP seconds to compensate for gravity. We should realize that the robot may not be finished responding to one change i n set points when the next set are made. However, we may be w i l l -ing to accept some i n i t i a l overshoot and oscillation i f the response of the robot can be sped up. As a protection against overshoot the algorithm freezes the set points when the exerted forces are within the allowed tolerance. The robot i s then allowed to come to rest at which time the applied forces are examined and further corrective action taken i f necessary. Also because the Jacobian i s calculated and used every TSTEP seconds this algorithm may be able to react fast enough to apply desired forces while the work piece is moving, although this was not verified by simula-tion. When this algorithm is used in a simulation with the i n i t i a l 120. contact configuration of the robot defined by 9j = 10°, 9 2 = 10° and 93 = 65° the robot applies the desired forces f = 1 N, f = 2 N and f = 0 N x y z within 11.8 seconds using a tolerance of 0.2 N. Furthermore the forces behave well and do not overshoot the desired values. Figures 4.20 and 4.21 show results using an i n i t i a l configuration of 9^10°, 9 2=30°, and 63=60° and a desired force of f x = 10 N, f = 20 N and f = 0 N. The robot attains these forces within a required tolerance of 1 z N i n 16 seconds. For another configuration of 91 = 45°, 92 = -20° and 9 3 = 15° and a desired force o f f = f = f = 10 N the response is complete x y z within a tolerance of 1 N in 3.0 seconds as shown in Table 4.3. This table represents typical output from R0BSIM.F0R when the force controller i s activated. ANGULAR RESPONSE OF ROBOT UNKS Robot nominal config. (10,30,60)deg., desired exerted force (10,20,0)N. 80 i I 60^ f 4 0 -I/ - 2 0 j JLS-I 5 10 15 20 TIME IN SECONDS i 25 -11 30 Legend A THETA1 X THETA2 • THETA3 B SETPT1 H SETPT2 X SETPT3 Figure 4.20. Third Force Controller Algorithm, Joint Angle Response. FORCES EXERTED BY ROBOT END PIECE Robot nominal config. (I0,30,60)deg., desired exerted force (10,20,0)N. Table 4.3. Force Supervisory Controller Simulation. SIMULATION TRIAL NUMBER=190 HAVE CORIOLIS AND CENTRIPETAL EFFECTS BEEN IGNORED? N HAVE GRAVITY EFFECTS BEEN IGNORED? N HAVE INERTIAL COUPLING EFFECTS BEEN IGNORED? N ARE ARMATURE INDUCTANCES ASSUMED NEGLIGIBLE? N ARE BACK EMF EFFECTS ASSUMED NEGLIGIBLE? N ARE MOTOR DAMPING EFFECTS IGNORED? N IS SUPERVISORY CONTROLLER BYPASSED? N IS GRAVITY COMPENSATION PACKAGE ACTIVATED? Y ANGULAR VELOCITY BELOW WHICH LINK IS CONSIDERED STATIONARY IS 0.0030 DEG/SEC CONSTRAINED LINK RESPONSE CONSIDERED COMPLETE WHEN DI/DT FALLS BELOW 0.000100 AMPS/SEC ENVIRONMENTAL STIFFNESSES: ACCEPTABLE FORCE ERROR IS FORCE CONVERGENCE FACTOR=0 KX= 1000.0000 N/M KY= 1000.0000 N/M KZ= 1000.0000 N/M 1.0000 NEWTONS .000000 LINK 1: ACTUATOR GAIN(KM)= 10.000 PROPORTIONAL GAIN(KE)= 30.000 VELOCITY FEEDBACK GAIN(KV)= 13.000 INITIAL JOINT ANGLE= O.OOO DEGREES INITIAL JOINT VELOCITY= 0.000 DEGREES PER SECOND FINAL DESIRED JOINT ANGLE= 45.000 DEGREES LINK 2: ACTUATOR GAIN(KM)= 10.000 PROPORTIONAL GAIN(KE)= 25.000 VELOCITY FEEDBACK GAIN(KV)= . 14.500 INITIAL JOINT ANGLE= 0.000 DEGREES INITIAL JOINT VELOCITY= 0.000 DEGREES PER SECOND FINAL DESIRED JOINT ANGLE= -20.000 DEGREES LINK 3: ACTUATOR GAIN(KM)= 10.000 PROPORTIONAL GAIN(KE)= 20.000 VELOCITY FEEDBACK GAIN(KV)= 6.800 INITIAL JOINT ANGLE= 0.000 DEGREES INITIAL JOINT VELOCITY= 0.000 DEGREES PER SECOND FINAL DESIRED JOINT ANGLE= 15.000 DEGREES CO Table 4.3. (Continued). TIME THETA1 THETA2 THETA3 SETPT1 SETPT2 SETPT3 (SEC) (DEG) (DEG) (DEG) (DEG) (DEG) (DEG) 0 .OOOO 0 .0000 0 .0000 0 .0000 45 .0000 -42 .3388 15 .0000 0 .2000 10 . 7533 -3 .2626 4 .4531 45 .0000 -42 .5131 14 .7369 0 .4000 21 .4323 -7 .2409 7 .9545 45 .0000 -42 .2868 14 .8423 0 .6000 28 .8440 -IO .4217 10 . 1685 45 .0000 -41 ,9255 15 .0560 0 .8000 33 .9292 -12 .8661 1 1 .6331 45 .0000 -41 , 5599 15 ;2725 1 OOOO 37 .4134 -14 .7136 12 .6277 45 .0000 -41 . ,2374 15 .4610 1 , .2000 39 .8003 -16 .0955 13 .3153 45 .0000 -40, .9717 15 .6143 1 . .4000 41 .4359 -17 . 1224 13 .7970 45 .0000 -40, ,7610 15 . 7346 1 .6000 42 .5569 -17 .8820 14 . 1376 45 .0000 -40, ,5979 15 .8271 1 .8000 43, .3254 -18 .4423 14 .3801 45 .0000 -40, ,4738 15 .8972 2 .OOOO 43 .8523 - 18 .8548 14 .5535 45 .0000 -40. ,3802 15 .9499 2 .2000 44 .2136 -19 . 1583 14 .6779 45 .OOOO -40, ,3101 15 .9893 2 .4000 44 .4613 -19 .3813 14 . 7674 45 .0000 -40. 2578 16 .0188 2. .6000 44 .6310 -19 .5452 14 , .8318 45 .0000 -40. 2190 16, .0407 2 .8000 44 . 7474 -19 .6656 14, .8783 45 .0000 -40. 1902 16 .0570 3 .0000 44, ,8271 -19 .7541 14 , .91 19 45 OOOO -40. 1689 16, .0691 3. .2000 44, .8817 -19 .8192 14 .9362 45 .0000 -40. 1532 16, .0780 3. .4000 44. 9192 -19 .8670 14 , .9537 45 .0000 -40. 1415 16 .0847 3 .6000 44, ,9447 -19 .9021 14 .9664 45 .0000 -40. 1329 16 .0896 3 .8000 44 . 9623 -19 .9280 14 9756 45 OOOO -40. 1265 16 .0933 4 OOOO 44. .9742 -19 .9470 14 9823 45 .0000 -40. 1218 16 .0960 4 . 2000 44 . 9824 -19 ,9610 14. 9871 45. .0000 -40. 1 183 16. ,0980 4. 4000 44. 9880 -19, .9713 14. ,9907 45. .0000 -40. 1 158 16. ,0995 4. 6000 44. 9918 -19, ,9789 14 . 9932 45 .0000 -40. 1139 16. 1006 4 . 8000 44. 9945 -19. 9844 14 . 9951 45. OOOO -40. 1 125 16 . 1015 5. OOOO 44. 9962 -19, .9885 14 . 9964 45. OOOO -40. 11 15 16. , 1021 5. ,2000 44. 9974 -19. .9916 14 . 9974 45 , OOOO -40. 1 107 16. 1025 5. 4000 44. 9983 -19, ,9938 14. 9981 45. OOOO -40. 1101 16 . 1029 5. 6000 44. 9988 -19, .9954 14. 9986 45 , OOOO -40. 1097 16 . 1031 5. .8000 44. 9992 -19. 9966 14. 9990 45. OOOO -40. 1094 16. 1033 6. OOOO 44 . 9995 -19. 9975 14 . 9993 45. OOOO -40. 1092 16 . 1034 6. 2000 44 . 9996 -19. 9982 14 . 9995 *********ST0PPED AT************ ROBOT GRIPPER LOCATION: (IN MILLIMETERS) PX 151.243 PY 362.665 PZ 634.382 REQUIRED APPLIED FORCES: FX= 10.000 FY= 10.000 FZ= 10.000 **START WITH ADJUSTED SET POINT AND*** 6.2000 44.9996 -19.9982 14.9995 44.5962 -38.8857 18.1970 151.243 362.665 634.382 6.4000 44.9325 -19.8576 15.4440 44.3809 -38.6481 19.3868 155.444 366.254 633.795 6.6000 44.8498 -19.7749 16.0053 44.3735 -39.3015 19.7252 160.004 370.040 633.600 6.8000 44.8191 -19.9379 16.3987 44.4274 -39.9785 19.9107 161.298 371.043 634.881 FX(N) 0.OOOO 4.2003 8 . 7612 10.0547 FY(N) 0.OOOO 3.5890 7.3754 8.3780 FZ(N) O.OOOO -0.5873 -0.7818 0.4988 Table 4.3. (Continued)... 7 OOOO 44 8275 -20 2407 16 6945 44 4523 -40 2817 7 2000 44 8418 -20 5216 17 0100 44 4567 -40 3506 7 4000 44 8519 -20 7275 17 3479 44 4721 -40 3963 7 6000 44 8650 -20 8843 17 6525 44 5045 -40 4685 7 8000 44 8859 -21 0175 17 8914 44 5409 -40 5227 8 OOOO 44 9120 -21 1285 18 0693 44 5409 -40 5227 8 2000 44 9308 -21 2082 18 1899 44 5409 -40 5227 8 4000 44 9420 -21 2614 18 2706 44 5409 -40 5227 8 6000 44 9487 -21 2969 18 3247 44 5409 -40 5227 8 8000 44 9528 -21 3205 18 3608 44 5409 -40 5227 9 OOOO 44 9552 -21 3364 18 3848 44 5409 -40 5227 9 2000 44 9567 -21 3470 18 4008 44 5409 -40 5227 9 40O0 44 9577 -21 3541 18 4115 44 5409 -40 5227 9 6000 44 9582 -21 3589 18 4186 44 5409 -40 5227 9 8000 44 9586 -21 3620 18 4234 44 5409 -40 5227 10 OOOO 44 9588 -21 3642 18 4266 44 5409 -40 5227 10 2 OOO 44 9590 -21 3656 18 4287 44 5409 -40 5227 10 40O0 44 9590 -21 3665 18 4301 44 5409 -40 5227 10 6000 44 9591 -21 3672 18 4310 44 5409 -40 5227 10 8000 44 9591 -21 3676 18 4317 44 5409 -40 5227 1 1 OOOO 44 9591 -21 3679 18 4321 44 5409 -40 5227 1 1 2000 44 9592 -21 3681 18 4324 44 5409 -40 5227 11 4000 44 9592 -21 3682 18 4326 44 5409 -40 5227 1 1 6000 44 9592 -21 3683 18 4327 44 5409 -40 5227 1 1 8000 44 9592 -21 3683 18 4328 *********STQPP£Q AT************ * END PIECE LOCATION WITHIN REQUIRED TOLERANCES SET POINT 1= 44.5409 SET POINT 2= -40.5227 SET POINT 3 20 2512 160 644 370 472 637 20 6320 160 238 370 199 639 20 9002 160 581 370 634 640 21 0418 161 091 371 262 641 21 1 168 161 338 371 703 642 21 1 168 161 360 371 966 643 21 1 168 161 331 372 1 1 1 644 21 1 168 161 320 372 205 644 21 1 168 161 320 372 268 644 21 1 168 161 323 372 308 644 21 1 168 161 325 372 333 645 21 1 168 161 328 372 350 645 21 1 168 161 329 372 360 645 21 1 168 161 331 372 367 645 21 1 168 161 332 372 371 645 21 1 168 161 332 372 374 645 21 1 168 161 333 372 375 645 21 .1 168 161 333 372 377 645 21 1 168 161 333 372 377 645 21 1 168 161 334 372 378 645 21 1 168 161 334 372 378 645 21 1 168 161 334 372 378 645 21 1 168 161 334 372 379 645 21 1 168 161 334 372 379 645 21 . 1 168 022 9 4012 7 8067 2 6398 025 8 9948 7 5340 4 6428 546 9 3378 7 9689 6 1638 723 9 8478 8 5975 7 341 1 71 1 10 0951 9 0376 8 3293 523 10 1 166 9 3006 9 1408 102 10 0875 9 4461 9 7198 488 10 0768 9 5402 10 1064 745 10 0770 9 6027 10 3635 917 10 0796 9 6429 10 5352 032 10 0821 9 6684 10 6500 109 10 0844 9 6846 10 7270 160 10 0861 9 6949 10 7785 195 10 0874 9 7016 10 8129 218 10 0884 9 7058 10 8359 233 10 0891 9 7087 10 8513 244 10 0896 9 7105 10 8616 250 10 0901 9 7117 10 8685 255 10 0903 9 7124 10 8731 258 10 0905 9 7130 10 8762 260 10 0906 9 7133 10 8783 262 10 0907 9 7135 10 8796 262 10 0908 9 7136 10 8805 263 10 0908 9 7137 10 881 1 10 0908 9 7137 10 8815 NUMBER OF POINTS EVALUATED 6069 126. 4.4.4 Conclusions We have now shown that the robot end piece can be made to exert forces on the surroundings u t i l i z i n g a supervisory controller which modifies set points i n a coordinated manner. Three different algorithms have been designed and their performance evaluated using the robot simulation. From these results we have found that force control i s more d i f f i c u l t to obtain than position control since the robot is interacting physically with i t s surroundings. When the robot i s i n contact the forces generated cause changes i n configuration and a successful force control algorithm must constantly adjust for these changes. Fortunately the requirements for an applied force are usually much less stringent than a position error correction. The configuration of the robot also greatly affects i t s a b i l i t y to apply forces. This was shown in Figure 4.20 where the force in the y_ direction was applied almost immediately whereas the force i n the JC direc-tion took several seconds to materialize. This suggests that the force controller could be improved by incor-porating into i t some mechanism for measuring the a b i l i t y of the robot to exert the desired forces. The set points could then be altered to improve the robustness of the response. Also i f some means was provided for estimating the environmental stiffnesses the deflections could be estimated and appropriate compensation made. 5. CONCLUSIONS 127. 5.1 Summary of Work and Results The work described i n this thesis can be separated into two main areas. One area of work deals with the development of a computer program which simulates the response of a servo controlled manipulator to set point inputs. To accomplish this task the complete set of dynamics equations for a six degree of freedom revolute manipulator have been derived using the Lagrange-Euler approach and the UBC Computing Center's REDUCE package. The procedures and results from this work can be readily applied to any other manipulator type. The dynamics of a D.C. armature controlled motor have been examined and these actuator characteristics incorporated in the model. Also the structure of a PD control loop to accomplish angular positioning of each link has been discussed and these relations included in the simula-tion. Finally the effects of external forces applied to the robot end piece have been derived using a Newton-Euler recursive approach and the appropriate terms added to the model. The performance of the model has been demonstrated for the three degree of freedom case and only minor modifications need to be made to handle a l l six links. Suggestions for improvement in the simulation have been made. The second area of work concerns the design of supervisory control algorithms to improve the response of the robot. These algorithms manipu-late the PD control loop set point of each robot link. In this work i t has been shown that this type of approach can be used to compensate for gravity disturbance torques, eliminate end piece position errors, and apply 128. specified forces to the surroundings with the end piece. In addition a method has been suggested using the Newton-Euler dynamics equations for compensating for the disturbance torques caused by an arbitrary load. A number of algorithms haye been suggested and the process of testing and improving them using the simulation as a development tool has been illustrated. Suggestions for improving these algorithms further have been made. 5.2 Suggestions for Further Work The work i n this thesis i s the f i r s t of i t s type i n the UBC Mechanical Engineering Department. Thus work can be continued along two courses. One course of further work i s to use the servo controlled robot simu-lation on i t s own. The simulation can be used to examine various dynamic effects; their relations between each other and the physical parameters of the links. The model can also be modified to test different link control strate-gies by simply replacing the present PD control loop equations. In addition to these applications the model could be used as a basis for such tasks as computer graphics analysis and off line trajectory planning. A second course along which work may be pursued i s to improve and expand the capabilities of the discussed supervisory controller algorithms. An immediate goal could be to reduce response time. The obvious extension of this work i s to combine the position and force control algorithms to provide simultaneous position/force trajectory control. Another is to include a l l six degrees of freedom. 129. Using ROBSIM.FOR as a development tool work can proceed towards any structure of supervisory controller desired. The results could then be used as a basis for implementation and testing on the UBC Mechanical Engineering Department's Unimation PUMA 560 robot. 130. REFERENCES 1. Asher, R.B., Andrisani, D., Dorato, D. "Bibliography on Adaptive Control Systems", Proc. IEEE, Vol. 64, 1976, pp. 1226-1240. 2. Balestrino, A., De Maria, G., Sciavicco, L. "An Adaptive Model Following Control for Robotic Manipulators", Transactions of the ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 105, September 1983, pp. 143-151. 3. Bejczy, A.K. "Robot Arm Dynamics and Control", NASA Jet Propulsion Lab. Tech. Memo. 33-669, February 1974. 4. Cherchas, D. "Achieving Multivariable Control by Independent Control Loop Set Point Adjustment", Proc. 1982 Canadian Conference on Industrial Computer Systems, May 1982, pp. 17.1-17.2. 5. Denavit, J., Hartenberg, R.S. "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices", ASCE Journal of Applied Mechanics, Vol. 22, No. 1, 1955, pp. 215-221. 6. Dubowsky, S., DesForges, D.T. "The Application of Model Referenced Adaptive Control to Robotic Manipulators", Transactions of the ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 101, September 1979, pp. 193-200. 7. Freund, E. "A Nonlinear Control Concept for Computer Controlled Manipulators", Proc. 7th International Symp. on Industrial Robots, Tokyo, 1977. 8. Harrison, T.J. "Minicomputers in Industrial Control", Instrument Society of America, 1978. 9. Kahn, M.E., Roth, B., "The Near-Minimum-Time Control of Open-Loop Articulated Kinematic Chains", Trans. ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 93, September 1971, pp. 164-172. 10. Koivo, A.J., Guo, T.H. "Control of Robotic Manipulator with Adaptive Controller", Proc. 20th IEEE Conf. on Decision and Control, December 1981, pp. 271-276. 11. Lambeth, D.M. "An Approach to Tactile Feedback Programming for Robotic D r i l l i n g " , Proc. Robot VI Conf., Detroit, 1982. 12. Lee, C.S.G., "Robot Arm Kinematics, Dynamics, and Control", Computer, December 1982, pp. 62-80. 13. Lee, C.S.G., Chung, M.J. "An Adaptive Control Strategy for Computer-Based Manipulators", Proc. 21st IEEE Conf. on Decision and Control, December 1982, pp. 95-100. 131. 14. Luh, J.Y.S., Walker, M.W., Paul, R.P. "On-Line Computational Scheme for Mechanical Manipulators", Trans. ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 120, June 1980, pp. 69-76. 15. MacFarlane, A.G.J. "Relationships Between Recent Developments i n Linear Control Theory and Classical Design Techniques", Measurement and Control, Vol. 8, 179-375, 1975. 16. Markiewicz, B.R. "Analysis of the Computed Torque Drive Method and Comparison With Conventional Position Servo for a Computer-Controlled Manipulator", NASA Jet Propulsion Lab. Tech. Memo. 33-601, March 1973. 17. Mason, M.T. "Compliance and Force Control for Computer Controller Manipulators", MIT A r t i f i c i a l Intelligence Lab. Memo. TR-515, Apr i l 1979. 18. Palm, W.J. Modeling, Analysis and Control of Dynamic Systems, John Wiley & Sons, 1983. 19. Paul, R.P. Robot Manipulators: Mathematics, Programming, and Control, MIT Press, 1981. 20. Paul, R.P. "Modelling, Trajectory Calculation and Servoing of a Computer Controlled Arm", Stanford A r t i f i c i a l Intelligence Laboratory Memo. AM-177, 1972. 21. Paul, R.P., Shimano, B.E. "Compliance and Control", The 1976 Joint Automatic Control Conf., San Francisco, 1976, pp. 694-699. 22. Popov, E.P. et a l . "Synthesis of Control System of Robots Using Dynamic Models of Manipulation Mechanisms", Proc. 6th IFAC Symp. on Automatic Control in Space, 1974. 23. Raibert, M.H., Craig, J.J. "Hybrid Position/Force Control of Manipulators", Trans. ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 102, 1981, pp. 126-133. 24. Rijnsdorp, J.E., Seborg, D.E. "A Survey of Experimental Applications of Multivariable Control to Process Control Problems", AICHE Symposium Series, Vol. 72, 159, 1976, pp. 112-123. 25. Roessler, J. "A Decentralized Hierarchical Control Concept for Large-Scale Systems", Preprint 2nd Symp. on Large-Scale System Theory Application, ed. A. T i t l i and M. Singh, Oxford: Pergamon Press, 1980, pp. 171-180. 26. Seltzer, D.S. "Use of Sensory Information for Improved Robot Learning", Society of Manufacturing Engineers, Technical Paper, MS79-799. Dearborn, MI, 1979. 27. Whitney, D.E. "Resolved Motion Rate Control of Manipulators and Human Prostheses", IEEE Trans, on Man-Machine Systems, MMS-10, 1969, pp. 47-53. 132. Vukobratovic, M., Stokic, D. "Is Dynamic Control Needed i n Robotic Systems, and i f so, to What Extent?", the International Journal of Robotics Research, Vol. 2, No. 2, Summer 1983, pp. 18-34. 133. APPENDICES APPENDIX A ROBSIM.FOR PROGRAM LISTING 135. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 1 ***************************************************************** PROGRAM NAME: ROBSIM.FOR ROBOT SIMULATION PACKAGE IN FORTRAN LANGUAGE REVISION DATE: 14 MAY 1985 AUTHOR: DAN BOUCHER DATE WRITTEN: 6 FEBRUARY 1985 PURPOSE: THIS PROGRAM SIMULATES THE RESPONSE OF A MANIPULATOR CONTROLLED BY A CONVENTIONAL SERVO SYSTEM,IE, A PRESENT DAY INDUSTRIAL ROBOT. IT ALSO CONTAINS A SUPERVISORY CONTROLLER SUBROUTINE WHICH PROVIDES CLOSED LOOP END PIECE CONTROL BY MODIFYING THE INDEPENDENT PI CONTROL LOOP SET POINTS. DESCRIPTION: GIVEN DESIRED ANGULAR FUNCTIONS FOR EACH LINK THE PROGRAM COMPUTES THE OBSERVED RESPONSE OF EACH LINK. SYSTEM BLOCK DIAGRAM: DESIRED CONTROL SERVO PLANT OBSERVED ANGULAR INPUT ANGULAR POSITION TORQUE POSITION LINK"I" ********* ********* ********* ***** LINK"I" + * * + * * * ROBOT * * 1 * **********>0**** KE " I " +*>0**** KM"I" ***** LINK ******** - *********** a- * * a- * * * n j » * * * 5 * * (MAY BE A * ********* * ********* ********* * ***** * FUNCTION * * * * OF TIME) * * * * * * * * * * * * * * * * * * * * * * * * ***** j<V I I J I I * * * * * * * * * * * * * * * * * * * * * * * ********* * * * ******************************************************** EXTERNAL SUBROUTINES USED: DERKF(A O.D.E. SOLVER) C******************.*************************************************** EXTERNAL FUNC INTEGER TRIAL INTEGER YES/'Y'/.NO/'N'/.ANSWER REAL KX.KY.KZ REAL KM.KE.KV,L,M1,M2,M3,M4,M5,M6,IA1,IA2,IAS,IA4,IA5,IA6, 11XX,I1YY,I1ZZ,11XY,I 1XZ,I 1YZ,I2XX,I2YY,I2ZZ,I2XY,I2XZ,I2YZ. I3XX,I3YY,I3ZZ,I3XY,I3XZ,I3YZ,I4XX,I4YY.I4ZZ,I4XY,I4XZ,I4YZ, I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ DIMENSION Y(18),YPRIME(18),YDOT(18),INFO(15),RWORK(160), IWORK(40),IPAR(10),GEARA(6),R(6),L(6),EMFK(6),TORQK(6), F(6),KM(6),KE(6),KV(6),THETA(6),ANGVL(6),THETD(6),THETDA(6), ISLNK(6) COMMON/BLOCK 1/F,GEARA,R,L,EMFK,TORQK,KM,KE,KV. IA1,IA2,IA3,IA4,IA5,IA6 C0MM0N/BL0CK2/ 1 C 2 C 3 C 4 C 5 C 6 C 7 C 8 C 9 C 10 c 11 c 12 c 13 c 14 c 15 c 16 c 17 c 18 c 19 c 20 c 21 c 22 c 23 c 24 c 25 c 26 c 27 c 28 c 29 c 30 c 31 c 32 c 33 c 34 c 35 c 36 c 37 c 38. c 39 c 40 c 41 c 42 C" 43 c 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 136. L i s t i n g of ROBSIM.FOR a t 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 2 59 I1XX,I1YY,I1ZZ,I1XY,11XZ,11YZ,12XX,I2YY,I2ZZ,I2XY,I2XZ,I2YZ, 60 I3XX,I3YY,I3ZZ,I3XY.I3XZ,I3YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ.I4YZ, 61 I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ 62 C0MM0N/BL0CK3/D2,A2,D4,D6,M1,M2,M3,M4,M5,M6,GX.GY,GZ, 63 X1,X2,X3,X4,X5,X6,Y1,Y2,Y3,Y4,Y5,Y6.Z1,Z2,Z3,Z4.Z5.Z6 64 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4,C4.S5,C5,S6,C6,RAD 65 COMMON/BLOCKS/THETDA 66 C0MM0N/BL0CK6/KX,KY,KZ,XSTART,YSTART,ZSTART,FX,FY,FZ 67 C0MM0N/BL0CK7/PXD,PYD,PZD 68 C0MM0N/BL0CK8/LINE 69 C 70 C DEFINE ALL NECESSARY PARAMETERS TO MAKE A ROBOT 71 C 72 CALL ROBPAR(RAD) 73 C 74 C AT THIS POINT WE SPECIFY THAT ONLY FIRST THREE LINKS ARE FREE 75 M=3 76 C WE ALSO WANT TO USE THIS NUMBER IN SUBROUTINE "FUNC" 77 IPAR(5)=M 78 C 79 C ENTER TRIAL NUMBER OF THIS RUN FROM TERMINAL 80 C 81 WRITE(6.2) 82 2 F0RMAT(T1,'0','ENTER TRIAL NUMBER (INTEGER) 001 TO 999') 83 READ(5,3 )TRIAL 84 3 FORMAT(13) 85 C 86 WRITE(6,4) 87 4 FORMATCODO YOU WISH TO PLOT RESULTS?') 88 READ(5,5)ANSWER 89 5 FORMAT(A 1 ) 90 C IGRAPH IS A FLAG WHICH INDICATES IF PLOT DATA IS DESIRED 91 IGRAPH=0 92 IF (ANSWER .EQ. YES) IGRAPH=1 93 C 94 c SUBROUTINE TO ALLOW INTERACTIVE GAIN SETTING 95 c 96 WRITE(6,6) 97 6 FORMAT('OCONTROL GAINS HAVE BEEN ASSIGNED TO YIELD A STEP INPUT' 98 ./' RESPONSE TIME OF APPROX 4 SEC. DO YOU WISH TO CHANGE GAINS?') 99 READ(5,8)ANSWER 100 8 FORMAT(A 1) 101 IF (ANSWER .EQ. NO) GO TO 10 102 C 103 CALL GAINS(KE.KV.M) 104 c 105 10 WRITEO, 1 1 )TRI AL 106 11 F0RMAT('1'/'0'/'0'/'0'/'0'/'0'/,11X, 'SIMULATION TRIAL NUMBER= ' ,13) 107 c 108 IF (IGRAPH .EQ. 0) GO TO 13 109 WRITE(8,12)TRIAL 1 10 12 FORMAT(' '13) 1 1 1 C 112 13 WRITE(6,14) 113 14 FORMAT('000 YOU WISH TO IGNORE CORIOLIS AND CENTRIPETAL EFFECTS?') 114 READ(5,16)ANSWER 115 16 FORMAT(A1) 1 16 WRITEO, 18)ANSWER 137. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 3 117 18 FORMAT('0'5X'HAVE CORIOLIS AND CENTRIPETAL EFFECTS BEEN IGNORED? 118 'AD 119 C 120 IPAR( 1 )=0 121 IF (ANSWER .EQ. YES) IPAR(1)=1 122 C 123 WRITE(6,20) 124 20 FORMAT('ODO YOU WISH TO IGNORE GRAVITY EFFECTS?') 125 READ(5,22)ANSWER 126 22 F0RMAT(A 1) 127 WRITE(9,24)ANSWER 128 24 FORMAT(' '5X'HAVE GRAVITY EFFECTS BEEN IGNORED? 'A1) 129 C 130 IPAR(2)=0 131 IF (ANSWER .EQ. YES) IPAR(2)=1 132 C 133 WRITE(6,26) 134 26 FORMAT('ODO YOU WISH TO IGNORE INERTIAL COUPLING?') 135 READ(5,28)ANSWER 136 28 FORMAT(A 1 ) 137 WRITE(9,30)ANSWER 138 30 F0RMAT(6X'HAVE INERTIAL COUPLING EFFECTS BEEN IGNORED? 'A1) 139 C 140 IPAR(3)=0 141 IF (ANSWER .EQ. YES) IPAR(3)=1 142 C 143 WRITE(6,32) 144 32 F0RMAT('OPROGRAM HAS ASSIGNED TYPICAL ARMATURE INDUCTANCES.' 145 ./' DO YOU WISH TO IGNORE INDUCTANCE EFFECTS?') 146 READ(5,34)ANSWER 147 34 FORMAT(A 1) 148 WRITE(9,36)ANSWER 149 36 FORMAT('0'5X'ARE ARMATURE INDUCTANCES ASSUMED NEGLIGIBLE? 'A1) 150 C 151 IL=1 152 IF (ANSWER .EQ. NO) GO TO 38 153 IL=0 154 DO 37 1=1,6 155 L(I)=0.0 156 37 CONTINUE 157 C 158 38 WRITE(6,40) 159 40 FORMAT('OPROGRAM HAS ASSIGNED TYPICAL MOTOR BACK EMF CONSTANTS.' 160 ./' DO YOU WISH TO IGNORE BACK EMF EFFECTS?') 161 READ(5,42)ANSWER 162 42 FORMAT(A 1 ) 163 WRITE(9,44)ANSWER 164 44 FORMAT(6X'ARE BACK EMF EFFECTS ASSUMED NEGLIGIBLE? 'A1) 165 C 166 IF (ANSWER .EQ. NO) GO TO 46 167 DO 45 1=1,6 168 EMFK(I)=0.O 169 45 CONTINUE 170 C 171 46 WRITE(6,48) 172 48 FORMAT('OPROGRAM HAS ASSIGNED MOTOR DAMPING COEFFICIENTS.' 173 ./' DO YOU WISH TO NEGLECT DAMPING EFFECTS?') 174 READ(5.50)ANSWER 138. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 4 175 50 FORMAT(A 1) 176 WRITE(9,52)ANSWER 177 52 FORMAT(6X ' ARE MOTOR DAMPING EFFECTS IGNORED? 'A1) 178 C 179 IF (ANSWER .EQ. NO) GO TO 54 180 DO 53 1=1.6 181 F(I)=0.0 182 53 CONTINUE 183 C 184 54 WRITE(6,56) 185 56 FORMATCODO YOU WISH TO RUN ROBOT WITHOUT SUPERVISORY 186 . CONTROLLER?') 187 READ(5,58)ANSWER 188 58 FORMAT(A 1 ) 189 WRITE(9,60)ANSWER 190 60 FORMAT('0'5X'IS SUPERVISORY CONTROLLER BYPASSED? 'A1) 191 C 192 ICON=0 193 IF (ANSWER .EQ. YES) IC0N=1 194 C 195 C ENTER FROM TERMINAL INITIAL ROBOT CONFIGURATION DEFINED BY THE JOINT 196 C ANGLE IN DEGREES AND THE JOINT ANGULAR VELOCITY IN DEGREES PER 197 C SECOND. RECALL THAT JOINTS 4,5 AND 6 ARE LOCKED 198 C 199 DO 61 I = 1 ,M 200 THETA(I)=0.0 201 ANGVL(I)=0.0 202 61 CONTINUE 203 C 204 WRITE(6.62) 205 62 FORMAT( 'ODO YOU WISH ROBOT TO START AT ZERO POSITION WITH 206 . ZERO VELOCITY?') 207 READ(5,64)ANSWER 208 64 FORMAT(A 1) 209 IF (ANSWER .EQ. YES) GO TO 66 210 C 211 CALL CONFIG(THETA,ANGVL,M) 212 C 213 C ENTER FROM TERMINAL DESIRED JOINT ANGLES (STEP INPUT) 214 C 215 66 CALL ANGREQ(THETD.M) 216 C 217 C NOW WE SET PARAMETERS FOR FIRST ORDER DIFFERENTIAL EQU. SOLVER 218 C SUBROUTINE DERKF 219 C 220 C FIRST CHANGE INITIAL CONDITIONS TO RADIANS AND RAD/SEC 221 DO 67 1=1,M 222 Y(I)=ANGVL(I)*RAD 223 Y(I+M)=THETA(I)*RAD 224 C INITIAL MOTOR CURRENTS 225 Y(I+2*M)=0.0 226 C INITIALIZE ADJUSTED THETA WHICH ON FIRST CALL TO 227 C DERKF WILL EQUAL THE DESIRED THETA 228 THETDA(I)=THETD(I) 229 67 CONTINUE 230 C 231 C NUMBER OF F.O.D.E. TO BE SOLVED? 232 NEQ=3*M 139. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 5 233 C 234 C IS THIS 1ST CALL TD DERKF? 0=YES 1=ND 235 INFO(1)=0 236 C ARE ERROR TOLERANCES SCALARS? 0=YES 1=N0 237 INF0(2)=0 238 C SOLUTION DESIRED AT TOUT ONLY? 239 INF0(3)=0 240 IF (IGRAPH .EQ. 1) INF0(3)=1 241 C RELATIVE ERROR TOLERANCE 242 RT0L=1.0E-04 243 C ABSOLUTE ERROR TOLERANCE 244 AT0L=1.OE-05 245 C LRW .GE. 33+7*NEQ 246 LRW=160 247 c LIW .GE. 33 248 LIW=40 249 c TIME CORRESPONDING TO INITIAL ROBOT CONFIGURATION 250 T=0.0 251 c TO HAVE DERKF PRINT INITIAL CONDITIONS SET FIRST TOUT=TIN,IE; 252 TOUT=0.0 253 c SET COUNTER WHICH WILL RECORD NUMBER OF POINTS EVALUATED 254 IPAR(4)=0 255 c 256 c ENTER STEP TIME FROM TERMINAL 257 WRITE(6,70) 258 70 F0RMAT(T1,'0','ENTER STEP TIME DESIRED (SEC) USED IN DERKF') 259 READ(5,80)TSTEP 260 80 F0RMAT(F15.0) 261 C 262 C ENTER SIMULATION END TIME FROM TERMINAL 263 WRITE(6,90) 264 90 F0RMAT(T1,'0','ENTER SIMULATION END TIME (SEC) USED IN DERKF') 265 READ(5,100)TFINAL 266 100 F0RMAT(F15.0) 267 C 268 IF( ICON .EQ. 1) GO TO 190 269 C 270 C ENTER OTHER CONTROLLER PARAMETERS FROM TERMINAL 271 C 272 WRITE(6,102) 273 102 FORMAT('ODO YOU WISH TO USE GRAVITY COMPENSATION PACKAGE?') 274 READ(5,104)ANSWER 275 104 FORMAT(A1) 276 WRITE(9,106)ANSWER 277 106 FORMAT('0'5X'IS GRAVITY COMPENSATION PACKAGE ACTIVATED? 'A1) 278 C 279 IGRAV=0 280 IF(ANSWER .EQ. YES) IGRAV=1 281 C 282 WRITE(6,110) 283 1 10 FORMAT('OENTER VALUE FOR "STOP" APPROX 0.003 DEG/SEC. LINKS' 284 ./' WITH ANGULAR VELOCITIES BELOW THIS ARE CONSIDERED STOPPED') 285 READ(5,12O)ST0P 286 120 FORMAT(F15.0) 287 WRITEO, 122)ST0P 288 122 F0RMAT('0'5X'ANGULAR VELOCITY BELOW WHICH LINK IS CONSIDERED' 289 ./20X'STATIONARY IS'F8.4' DEG/SEC') 290 C 140. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CC1d=DACO Page 6 291 WRITE(6,124) 292 124 FORMATCOIS SUPERVISORY CONTROLLER CORRECTING FOR POSITION ERRORS 293 .?') 294 READ(5,126)ANSWER 295 126 FORMAT(AI) 296 C EXTERNAL FORCES ARE NOT APPLIED UNTIL ROBOT REACHES FIRST STOP 297 C IPAR(6) MUST BE 1 UNTIL THEN WHEN IT IS CHANGED BY SUPCON 298 IPOS=0 299 IPAR(6)=1 300 IF(ANSWER .EQ. NO) GO TO 163 301 IP0S=1 302 C 303 WRITE(6,130) 304 130 FORMAT('OENTER VALUE FOR "TOL" APPROX .025 MILLIMETERS.' 305 ./' POSITION ERRORS LESS THAN THIS ARE CONSIDERED ZERO') 306 READ(5,140)T0L 307 140 F0RMAT(F15.0) 308 WRITEO. 142)T0L 309 142 FORMAT(6X'ACCEPTABLE POSITION ERROR IS'F8.4' MILLIMETERS') 310 C 311 WRITEO, 150) 312 150 FORMAT('OENTER VALUE FOR "ALPHA" BETWEEN 0 AND 1. THIS VALUE IS' 313 ./' USED TO FRACTION POSITION CHANGES AS DX=DPX*(1-ALPHA**N)' 314 ./' WHERE N IS INCREMENTED. SMALL ALPHA CORRESPONDS TO RAPID' 315 ./' CONVERGENCE') 316 READ(5,160)ALPHA 317 160 F0RMAT(F15.0) 318 WRITEO, 162)ALPHA 319 162 FORMATOX'POSITION CONVERGENCE FACTOR ALPHA='F8.6) 320 C 321 GO TO 190 322 C 323 163 WRITEO, 164) 324 164 FORMAT('OENTER VALUE FOR "STABLE" APPROX .0001 AMPS/SEC. WHEN' 325 ./' DI/DT FALLS BELOW THIS THE RESPONSE OF THE CONSTRAINED END' 326 ./' PIECE TO SET POINT CHANGES IS CONSIDERED COMPLETE') 327 READ(5, 166JSTABLE 328 166 F0RMAT(F15.0) 329 WRITEO, 168)STABLE 330 168 FORMATOX'CONSTRAINED LINK RESPONSE CONSIDERED COMPLETE WHEN' 331 ./20X'DI/DT FALLS BEL0WF10.6' AMPS/SEC) 332 C 333 WRITEO, 172) 334 172 FORMAT('OENTER VALUES FOR ENVIRONMENTAL STIFFNESSES KX.KY AND KZ' 335 ./' TO BE USED TO SIMULATE EXTERNAL FORCES APPLIED TO END PIECE' 336 ./' REASONABLE VALUES ARE APPROX 1000. NEWTONS/METER') 337 READO, 174)KX,KY,KZ 338 174 FORMAT(F15.0/F15.0/F15.0) 339 WRITEO. 176 )KX,KY,KZ 340 176 FORMAT('0'5X'ENVIRONMENTAL STIFFNESSES: KX='F15,4' N/M' 341 ./T35'KY='F15.4' N/M'/T35'KZ='F15.4' N/M') 342 C 343 WRITEO. 178) 344 178 FORMAT('OENTER VALUE FOR "FTOL" APPROX 0.5 NEWTONS.' 345 ./' FORCE ERRORS LESS THAN THIS ARE CONSIDERED ZERO') 346 READO. 180)FT0L 347 180 F0RMAT(F15.O) 348 WRITEO, 182)FT0L 141. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 7 349 18-2 F0RMAT(6X'ACCEPTABLE FORCE ERROR IS'F8.4' NEWTONS') 350 C 351 WRITE(6,184) 352 184 F0RMAT('OENTER VALUE FOR "ALFA" BETWEEN 0 AND 1.' 353 ./' SMALL ALFA CORRESPONDS TO RAPID CONVERGENCE') 354 READ(5,186)ALFA 355 186 F0RMAT(F15.0) 356 WRITE(9,188)ALFA 357 188 F0RMAT(6X'FORCE CONVERGENCE FACTOR='F8.6) 358 C 359 190 DO 250 1=1.M 360 WRITE(9.200)I,KM(I),KE(I),KV(I),THETA(I),ANGVL(I),THETD(I) 361 200 F0RMAT(T1,'O',' LINK',13,':',/.10X,'ACTUATOR GAIN(KM)=',F14.3, 362 ./.10X,'PROPORTIONAL GAIN(KE)=',F14.3,/.10X,'VELOCITY 363 . FEEDBACK GAIN(KV)=',F14.3,/,10X,'INITIAL JOINT ANGLE=', 364 .F14.3,' DEGREES',/,10X.'INITIAL JOINT VELOCITY''.F14.3, 365 .' DEGREES PER SECOND'./.10X.'FINAL DESIRED JOINT ANGLE=', 366 .F14.3.' DEGREES') 367 250 CONTINUE 368 C 369 C PRINT APPLICABLE HEADINGS IN OUTPUT DATA FILE 370 WRITE(9,30O) 371 300 FORMAT('1'/'0'/'0'/'0'/'0'/,4X'TIME'5X'THETA1 '4X'THETA2' 372 4X'THETA3') 373 C 374 C COUNTER LINE IS USED TO CONTROL TABULAR OUTPUT 375 LINE=11 376 C 377 IF(ICON .EQ. 1) GO TO 320 378 C IC0N=1 MEANS THAT SUPERVISORY CONTROLLER IS NOT USED 379 WRITE(9,310) 380 310 FORMAT('+'T45'SETPT1'4X'SETPT2'4X'SETPT3' 381 ./4X'(SEC)'6(5X'(DEG)')) 382 GO TO 340 383 C 384 320 WRITE(9,330) 385 330 F0RMAT(4X'(SEC)'3(5X'(DEG)')) 386 340 CONTINUE 387 C 388 C RUN THE THING AND PRINT RESULTS 389 C 390 400 CONTINUE 391 410 CALL DERKF(FUNC,NEQ,T.Y,TOUT,INFO.RTOL.ATOL.IDID.RWORK, 392 LRW,IWORK,LIW,RPAR,IPAR) 393 C 394 C CHANGING BACK TO DEGREES 395 DO 450 1=1,M 396 THETA(I)=Y(I+M)/RAD 397 450 CONTINUE 398 C 399 IF (IGRAPH .EQ. O) GO TO 470 400 C 401 C COMPUTE POSITION FOR GRAPH RESULTS 402 Q=(D4+D6)*S23+A2*C2 403 PX=(C1*Q-D2*S1)*1000.0 404 PY=(S1*Q+D2*C1)*1000.0 405 PZ=((D4+D6)*C23-A2*S2)*1COO.O 406 C 142. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 8 407 C COPY RESULTS INTO A DATA FILE FOR GRAPHING 408 ERRORX=PX-PXD 409 ERRORY=PY-PYD 410 ERRORZ=PZ-PZD 411 WRITE(8,4G0)T,THETA(1),THETA(2),THETA(3),THETDA(1),THETDA(2), 412 THETDA(3),ERR0RX,ERRORY,ERRORZ,FX,FY,FZ 413 480 FORMAT(' 'F9.4,6F10.4,3F10.3,3F10.4) 414 C 415 C CHECK STATUS OF DERKF 416 470 CALL STATUS( IDID , INFO( 1 ), &4 10,8.9999 ) 417 C 418 IF(LINE .LT. 58) GO TO 490 419 WRITE(9,480) 420 480 FORMAT('1'/'O'/'O'/'O'/'O') 421 LINE=10 422 490 CONTINUE 423 C 424 WRITE(9,500)TOUT,(THETA(I),1=1,M) 425 500 FORMAT(T1,' ',F9.4,6F10.4) 426 C 427 LINE=LINE+1 428 C CHECK TO SEE IF ROBOT IS TO RUN WITHOUT SUPERVISORY CONTROL 429 IF (ICON .EQ. 1) GO TO 650 430 C 431 DO 600 1=1,NEQ 432 YDOT(I)=RWORK(20+I) 433 600 CONTINUE 434 C 435 C INVOKE OPERATION OF SUPERVISORY CONTROLLER 436 C 437 CALL SUPCON(M,THETA,KM,KE,KV,Y,YDOT,STOP,TOL,FTOL, 438 IGRAV,ALPHA,ALFA,RAD,D2,A2,D4.D6,INFO(1),TOUT, 439 NEQ,STABLE,IPAR(6),IPOS.IL,&4000,S9999) 440 C 441 650 T0UT=T0UT+TSTEP 442 IF(TOUT .LE. TFINAL) GO TO 400 443 GO TO 4200 444 C 445 4000 WRITE(9,4100)THETDA(1),THETDA(2),THETDA( 3) 446 4100 FORMAT('OEND PIECE LOCATION WITHIN REQUIRED TOLERANCES' 447 ./' SET POINT 1='F10.4,2X'SET POINT 2='F10.4,2X, 448 .' SET POINT 3='F10.4) 449 C 450 4200 WRITE(9,4300)IPAR(4) 451 4300 FORMAT('ONUMBER OF POINTS EVALUATED'I 10) 452 C 453 9999 STOP 454 END 455 C 456 C 4 5 7 c**********»*******************************«************ 458 C 459 C 460 SUBROUTINE FUNC(T,Y,YPRIME,RPAR,IPAR) 461 C 462 REAL KM,KE,KV,L,M1,M2,M3,M4,M5,M6,IA 1,IA2,I A3,IA4,IA5,IA6 463 DIMENSION Y(18).YPRIME(18),IPAR(10),GEARA(6),R(6),L(6),EMFK(6), 464 TORQK(6),F(6),KM(6 ) ,KE(6),KV(6),THETDA(6),V(6),CURNT(6) , 143. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 9 465 EXT0RQ(6) 466 COMMON/BLOCK 1/F,GEARA,R,L,EMFK,TOROK,KM,KE,KV, 467 IA1 ,IA2,IA3,IA4,IA5,IA6 468 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 469 C0MM0N/BL0CK5/THETDA 470 M=IPAR(5) 471 C 472 C FIRST WE CALCULATE THE SERVO TORQUE (CONTROL TORQUE INPUT TO EACH LINK) 473 C 474 C DESIRED JOINT ANGLES AT TIME "T" MUST BE EVALUATED 475 C IF WE WERE NOT USING INTERACTIVE ANGLE INPUT DESIRED THETA FUNCTIONS 476 C WOULD BE IN A SUBROUTINE CALLED FROM THIS LOCATION 477 C 478 C CHANGING TO RADIANS 479 C 480 DO 60 I=1,M 481 V(I)=KE(I)*((THETDA(I)*RAD)-Y(I+M))-KV(I)*Y(I) 482 J=I+2*M 483 IF(L( I ) .EQ. O.O) GO TO 50 484 CURNT(I)=Y(J) 485 YPRIME(J)=(-EMFK(I)*GEARA(I)*Y(I)-R(I)*Y(J)+V(I))/L(I) 486 GO TO 60 487 50 CURNT(I) = (V(I)-EMFK(I )*GEARA(I)*Y(I))/R(I) 488 YPRIME(J)=0.0 489 60 CONTINUE 490 C 491 C NOW WE EVALUATE THE ROBOT DYNAMICS 492 C 493 S1=SIN(Y(4)) 494 C1=C0S(Y(4) ) 495 , S2=SIN(Y(5)) 496 C2=C0S(Y(5)) 497 S3=SIN(Y(6)) 498 C3=C0S(Y(6)) 499 S23=SIN(Y(5)+Y(6)) 500 C23=C0S(Y(5) + Y(6) ) 501 S4=0.0 502 C4=1.0 503 S5=0.0 504 C5=1.0 505 S6=0.0 506 C6=1.0 507 C 508 CALL INERTL(D11,D12,D13,D22,D23,D33) 509 C 510 C RECALL THAT IF IPAR(3)=0 WE WISH TO INCLUDE INERTIAL COUPLING 51 1 IF (IPAR(3) EQ. 0) GO TO 70 512 D12=0.0 513 D13=0.0 514 D23=0.0 515 C 516 70 G1=0.0 517 G2=0.0 518 G3=0.0 519 C RECALL THAT IF IPAR(2)=1 WE IGNORE GRAVITY TERMS 520 IF (IPAR(2) .EQ. 1) GO TO 80 521 C 522 CALL GRAV(G1,G2,G3) 144. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 fo r CC1d=DAC0 Page 10 523 C 524 80 D112=0.0 525 0113=0.0 526 D123=0.0 527 D223=0.0 528 D122=0.0 529 D211=0.0 530 D311=0.0 531 D322=0.0 532 C IF IPAR(1)=1 WE IGNORE CORIOLIS AND CENTRIPETAL EFFECTS 533 IF (IPAR(1 ) .EQ. 1) GO TO 90 534 C 535 CALL C0RI0L(D112,D113,0123,D223) 536 C 537 CALL CENTRI(D122,D211,D311,D322) 538 C 539 90 DO 100 1=1,M 540 EXTORQ(I)=0.0 541 100 CONTINUE 542 C IF IPAR(6)=1 THERE ARE NO EXTERNAL FORCES APPLIED TO END PIECE 543 C CAUSING ADDITIONAL TORQUES IN DYNAMICS 544 IF(IPAR(6) .EQ. 1) GO TO 110 545 C 546 CALL FORCE(M,EXTORQ) 547 C 548 1 10 B1 = -F(1 )*Y(1)-2.*D1 12*Y(1 )*Y(2)-2.*D 1 13*Y(1)*Y(3)-D122 549 .*Y(2)**2-D123*Y(3)*(2.*Y(2)+Y(3))-G1-EXTORQ(1) 550 .+TORQK(1)*GEARA(1)*CURNT(1 ) 551 C 552 B2=-F(2)*Y(2)-D211*Y(1)**2-D223*Y(3)*(2.*Y(2)+Y(3))-G2 553 .-EXTORQ(2)+TORQK(2)*GEARA(2)*CURNT(2) 554 C 555 B3 = -F(3)*Y(3)-D31 1*Y(1 ) **2-D322*Y(2)**2-G3 556 .-EXTORQ(3) + TORQK(3)*GEARA(3 ) *CURNT(3 ) 557 C 558 D11S=D11+IA1 559 D22S=D22+IA2 560 D33S=D33+IA3 561 C 562 A=D11S*D22S*D33S+2.*D12*D13*023-D11S*D23**2-D22S*D13**2 563 .-D33S*D12**2 564 C 565 W1=B1*D22S*D33S+B2*D23*D13+B3*D12*D23-B3*D22S*D13-B2*D12*D33S 566 .-B1*D23**2 567 C 568 W2=D11S*B2*D33S+D12*B3*D13+D13*B1*D23-D13*B2*D13-D12*B1*D33S 569 .-D11S*B3*D23 570 C 571 W3=D11S*D22S*B3+D12*D23*B1+D13*D12*B2-D13*D22S*B1-D12*D12*B3 572 .-D11S*D23*B2 573 C 574 YPRIME(1 )=W1/A 575 YPRIME(2)=W2/A 576 YPRIME(3)=W3/A 577 YPRIME(4)=Y( 1 ) 578 YPRIME(5)=Y(2) 579 YPRIME(6)=Y(3) 580 c 145. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 11 581 C INCREMENT COUNTER 582 IPAR(4)=IPAR(4)+1 583 C 584 RETURN 585 END 586 C 588 C 589 SUBROUTINE FORCE(M,EXTORO) 590 C 591 C THIS SUBROUTINE ADJUSTS DYNAMICS TO INCLUDE THE APPLICATION OF 592 C AN EXTERNAL FORCE ACTING ON THE GRIPPER 593 C 594 REAL KX.KY.KZ 595 DIMENSION EXTORO(M) 596 COMMON/BLOCKS/D2,A2,D4,D6,M1,M2,M3,M4.M5,M6,GX,GY,GZ, 597 X1,X2,X3,X4,X5,X6,Y1 ,Y2,Y3,Y4,Y5,Y6.Z1,Z2,Z3,Z4,Z5,Z6 598 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 599 C0MM0N/BL0CK6/KX,KY,KZ,XSTART,YSTART,ZSTART,FX,FY,FZ 600 C 601 C FIRST CALCULATE NEW END PIECE LOCATION 602 C 603 0=(D4+D6)*S23+A2*C2 604 X=C1*0-D2*S1 605 Y=S1*0+D2*C1 606 Z=(D4+D6)*C23-A2*S2 607 C 608 C NOW CALCULATE DISTANCE IT HAS MOVED FROM FIRST STOP POSITION 609 C AND THE FORCES END PIECE IS APPLYING TO SURROUNDINGS 610 C 611 FX=KX*(X-XSTART) 612 FY=KY*(Y-YSTART) 613 FZ=KZ*(Z-ZSTART) 614 C 615 C CALCULATE ADDITIONS TO JOINT LOAD TORQUES CORRESPONDING TO 616 C FORCES APPLIED TO SURROUNDINGS BY GRIPPER 617 C 618 EXTORQ(1)=-Y*FX+X*FY 619 C 620 EXT0RQ(2)=C1*Z*FX+S1*Z*FY-Q*FZ 621 C 622 EXT0RQ(3)=(D4+D6)*(C1*C23*FX+S1*C23*FY-S23*FZ) 623 C 624 RETURN 625 END 626 C 628 C 629 SUBROUTINE ROBPAR(RAD) 630 C 631 C ALL PHYSICAL ROBOT PARAMETERS ARE INITIALIZED IN THIS SUBROUTINE 632 C 633 REAL NA 634 REAL KM,KE.KV,L,M1,M2,M3,M4,M5,M6,1A1,1A2,1A3,1A4,1A5,1A6, 635 I1XX,I1YY,I1ZZ,I 1XY,I 1XZ,I 1YZ,12XX,12YY,12ZZ,12XY,I2XZ,I2YZ, 636 I3XX,I3YY.I3ZZ,I3XY.I3XZ,I3YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ,I4YZ, 637 I5XX,I5YY,I5ZZ,I5XY,I5XZ.I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ 638 DIMENSION GEARA(6),R(6 ) ,L(6),EMFK(6),TORQK(6),F(6),KM(6), 146. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 12 639 KE(6),KV(6) 640 COMMON/BLOCK 1/F,GEARA,R,L,EMFK,TOROK,KM,KE,KV, 641 IA1 , IA2,IA3, IA4,IA5,IA6 642 C0MMON/BL0CK2/ 643 I1XX,I1YY,I1ZZ,11XY , I 1XZ,I 1YZ,I2XX,12YY,I2ZZ,I2XY,I2XZ,I2YZ, 644 I3XX,I3YY.I3ZZ,I3XY,I3XZ,I3YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ,I4YZ, 645 . I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ 646 COMMON/BLOCK3/D2,A2,D4,D6,M1,M2,M3,M4.M5,M6,GX,GY,GZ, 647 X1,X2,X3,X4,X5,X6,Y1 ,Y2.Y3.Y4,Y5,Y6.Z1,Z2,Z3,Z4,Z5,Z6 648 C 649 C DEFINE A SYMBOL "NA" TO MEAN "NOT APPLICABLE" 650 NA=0.0 651 C 652 C SPECIFY REQUIRED CONSTANTS, INCLUDING GRAVITY VECTOR (M/S**2) 653 C 654 PI=3.141593 655 RAD=PI/180.0 656 GX=0.0 657 GY=0.0 658 GZ=-9.81 659 C 660 C NOW WE SET PHYSICAL PARAMETERS OF ROBOT 661 C 662 C FIRST THE LENGTH OF. EACH LINK IN METERS 663 C 664 D2=0.1495 665 A2=0.432 666 04=0.432 667 06=0.0565 668 C 669 C SET THE MASS OF EACH LINK IN KG AND THE LOCATION OF THE CENTER 670 C OF GRAVITY OF EACH LINK IN METERS DEFINED W.R.T. THAT LINK'S 671 C COORDINATE FRAME 672 C 673 M1=22.0 674 X1=0.0 675 Y1=0.0 676 Z1=0.13 677 C 678 M2=16.0 679 X2=-0.27 680 Y2=0.0 681 Z2=0.12 682 C 683 M3=12.0 684 X3=0.0 685 Y3=0.0 686 Z3=0.21 687 C 688 M4=3.0 689 X4=0.0 690 Y4=0.08 691 Z4=0.0 692 C 693 M5=1.0 694 X5=0.0 695 Y5=0.0 696 Z5=0.05 147. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 13 697 C 698 M6=1.0 699 X6=0.0 700 Y6=0.0 701 Z6=-0.04 702 C 703 C NOW WE SET THE PRODUCTS OF INERTIA FOR EACH LNK AGAIN DEFINED WRT 704 C THAT LINK'S COORDINATE FRAME IN KG*(METERS)**2 705 C 706 I1XX=0.606 707 I1YY=0.606 708 I1ZZ=0.22 709 I1XY=0.0 710 I1XZ=0.0 711 I1YZ=0.0 712 C 713 I2XX=0.31 714 I2YY=1.825 715 I2ZZ=1.56 716 I2XY=0.0 717 I2XZ=0.5184 718 I2YZ=0.0 719 C 720 I3XX=0.755 721 I3YY=0.755 722 I3ZZ=0.03 723 I3XY=0.0 724 I3XZ=0.0 725 I3YZ=0.0 726 C 727 I4XX=0.028 728 I4YY=0.0048 729 I4ZZ=0.028 730 I4XY=0.0 731 I4XZ=0.0 732 I4YZ=0.0 733 C 734 I5XX=0.00378 735 I5YY=0.00378 736 I5ZZ=0.0009 737 I5XY=0.0 738 I5XZ=0.0 i 739 I5YZ=0.0 740 C 741 I6XX=0.0026 742 I6YY=0.0026 743 I6ZZ=0.0O09 744 I6XY=0.0 745 I6XZ=0.0 746 I6YZ=0.0 747 C 748 C SET THE ACTUATOR "MOTOR" PARAMETERS 749 C 750 C SET GEAR RATIOS- LINK:MOTOR V 751 GEARA(1)=22.608 752 GEARA(2)=22.608 753 GEARA(3)=22.608 754 GEARA(4)=NA L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 1 755 GEARA(5)=NA 756 GEARA(6)=NA 757 C 758 C SET ARMATURE RESISTANCES IN OHMS 759 R(1)=0:43 760 R(2)=0.43 761 R(3)=0.43 762 R(4)=NA 763 R(5)=NA 764 R(6)=NA 765 C 766 C SET ARMATURE INDUCTANCES IN HENRYS 767 L(1)=.0021 768 L(2)=-0021 769 L(3)=.0021 770 L(4)=NA 771 L(5)=NA 772 L(6)=NA 773 C 774 C MOTOR BACK EMF CONSTANTS IN VOLTS/RAD/SEC 775 EMFK(1)=0.199 776 EMFK(2)=0.199 777 EMFK(3)=0.199 778 EMFK(4)=NA 779 EMFK(5)=NA 780 EMFK(6)=NA 781 C 782 c MOTOR TORQUE CONSTANTS IN NEWTON METERS/AMP 783 TORQK(1)=0.1902 784 T0RQK(2)=0.1902 785 T0RQK(3)=0.1902 786 TORQK(4)=NA 787 TORQK(5)=NA 788 T0RQK(6)=NA 789 c 790 c MOTOR ARMATURE INERTIAS IN KG*(METERS)**2 791 ARMI1=0.0046956 792 ARMI2=0.011739 793 ARMI3=0.0078261 794 ARMI4=NA 795 ARMI5=NA 796 ARMI6=NA 797 c 798 c MOTOR DAMPING COEFFICIENTS IN NM/RAD/SEC 799 0AMP1=O.OOO5 800 DAMP2=0.0005 801 DAMP3=0.0005 802 DAMP4=NA 803 DAMP5=NA 804 DAMP6=NA 805 c 806 c NOW WE MUST CALCULATE THE ACTUATOR INERTIAS FOR EACH LINK 807 c REFERED TO JOINT ANGLE RATES AGAIN IN KG*(METERS)**2 808 c 809 IA 1=ARMI1 *GEARA(1)**2 810 IA2=ARMI2*GEARA(2)**2 811 IA3=ARMI3*GEARA(3)**2 812 IA4=NA 149. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 15 813 IA5=NA 814 IA6=NA 815 C 816 C NOW WE SET THE VISCOUS DAMPING COEFFICIENT FOR EACH LINK 817 C REFERED TO JOINT ANGLE RATES IN N*M/RADS/SEC 818 C 819 F(1)=DAMP1*GEARA(1)**2 820 F(2)=DAMP2*GEARA(2)**2 821 F(3)=DAMP3*GEARA(3)**2 822 F(4)=NA 823 F(5)=NA 824 F(6)=NA 825 C 826 C NOW WE SET THE LINK SERVO CONTROL GAINS 827 C NOTE THAT KM IS AN APPROXIMATION ,IE, THE ACTUATOR GAIN 828 C IS NOT NECESSARILY A CONSTANT 829 C 830 KM(1)=TORQK(1)*GEARA(1)/R(1) 831 KE(1)=30.0 832 KV(1)=13.0 833 C 834 KM(2)=T0RQK(2)*GEARA(2)/R(2) 835 KE(2)=25.0 836 KV(2)=14.5 837 C 838 KM(3)=T0RQK(3)*GEARA(3)/R(3) 839 KE(3)=20.0 840 KV(3)=6.8 841 C 842 KM(4)=NA 843 KE(4)=NA 844 KV(4)=NA 845 C 846 KM(5)=NA 847 KE(5)=NA 848 KV(5)=NA 849 C 850 KM(6)=NA 851 KE(6)=NA 852 KV(6)=NA 853 C 854 WRITE(6,1) 855 1 F0RMAT(T1,'1','HELLO. YOU WILL BE ASKED TO ENTER VARIOUS' 856 ./' QUANTITIES. AS WE ARE USING FIXED FORMAT IT IS NECESSARY' 857 ./' THAT NUMBERS BE ENTERED PROPERLY. PLEASE ENTER ALL NUMBERS' 858 ./' BEGINNING IN THE FIRST COLUMN, ONE NUMBER PER ROW, AND REAL' 859 ./' NUMBERS EXPRESSED WITH NO EXPONENT. ALL REQUIRED NUMBERS' 860 ./' ARE REAL UNLESS OTHERWISE SPECIFIED. NOTE: COMPUTER USES' 861 ./' SEVEN SIGNIFICANT DIGITS.' 862 ./'OYOU WILL ALSO BE ASKED SOME QUESTIONS ABOUT HOW YOU WISH' 863 ./' THE PROGRAM TO PERFORM. PLEASE ENTER YES OR NO (Y OR N' 864 ./' ARE SUFFICIENT RESPONSES). THANKYOU AND GOODLUCK.' 865 ./'OPLEASE NOTE THAT TABULAR OUTPUT IS SENT TO LOGICAL UNIT 9' 866 ./' WHICH ON THE VAX SYSTEM IS FILE "F0R009.DAT".' 867 ./'OIF IN ADDITION GRAPHS ARE DESIRED, DATA SUITABLE FOR' 868 ./' PLOTTING IS SENT TO LOGICAL UNIT 8 WHICH ON THE VAX' 869 ./' SYSTEM IS FILE "F0R008.DAT".') 870 C 150. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DAC0 Page 16 871 RETURN 872 END 873 C 874 C********************************************************** 875 c 876 SUBROUTINE GAINS(KE,KV,M) 877 c 878 C THIS SUBROUTINE ALLOWS THE USER TO SET CONTROL GAINS 879 C 880 REAL KE,KV 881 DIMENSION KE(M),KV(M) 882 DO 30 I=1,M 883 WRITE(6,10)I 884 10 F0RMAT(T1,'0','ENTER KE AND KV GAINS FOR LINK: '13) 885 READ(5.20)KE(I),KV(I) 886 20 FORMAT(F15.0./.F15.0) 887 30 CONTINUE 888 RETURN 889 END 890 C 891 892 C 893 SUBROUTINE CONFIG(THETA,ANGVL,M) 894 c 895 C THIS SUBROUTINE ALLOWS US TO INPUT ROBOT INITIAL CONDITIONS 896 C 897 DIMENSION THETA(M),ANGVL(M) 898 DO 40 1=1,M 899 WRITE(6,10)1 900 10 F0RMAT(T1,'0','ENTER INITIAL ANGLE IN DEGREES FOR LINK: ',13) 901 READ(5,20)THETA(I) 902 20 F0RMAT(F15.0) 903 WRITE(6,30)I 904 30 F0RMAT(T1,'0','ENTER INITIAL ANGULAR VELOCITY IN DEGREES PER 905 .SECOND FOR LINK: ',13) 906 READ(5,20)ANGVL(I) 907 40 CONTINUE 908 RETURN 909 END 910 C 911 Q * * * * * * i t * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 912 c 913 SUBROUTINE ANGREO(THETD,M) 914 c 915 c SUBROUTINE ANGREO INPUTS THE DESIRED JOINT ANGLES. 916 c FOR THIS SIMULATION WE CONSIDER ONLY STEP INPUTS 917 c AND WE WISH TO ENTER THESE FROM A TERMINAL 918 c 919 DIMENSION THETD(M) 920 DO 30 I=1,M 921 WRITE(6,10)1 922 10 F0RMAT(T1,'OENTER DESIRED JOINT ANGLE IN DEGREES FOR LINK: '.13) 923 READ(5,20)THETD(I) 924 20 F0RMAT(F15.0) 925 30 CONTINUE 926 RETURN 927 END 928 c 151. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 fo r CCid=DACO Page 17 929 c * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 930 C 931 SUBROUTINE STATUS(IDID,INF01,*,*) 932 C 933 C THIS SUBROUTINE CHECKS HOW DERKF IS DOING? 934 C 935 IF(IDID .EQ. 1) RETURN 1 936 IF(IDID .EQ. 2) RETURN 937 IF((IDID .NE. -4).AND.(IDID .NE. -5)) GO TO 50 938 IF(IDID .EQ. -5) GO TO 20 939 WRITEO, 10) 940 10 FORMAT(' *WARNING*PROBLEM APPEARS STIFF*') 941 GO TO 40 942 20 WRITEO,30) 943 30 FORMAT(' *WARNING*STEP SIZE IS TOO SMALL*') 944 40 INF01=1 945 RETURN 1 946 50 WRITEO,60)IDID 947 60 FORMAT('OTASK INTERRUPTED: IDID= '15) 948 RETURN 2 949 END 950 C 95 1 c * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 952 C 953 SUBROUTINE SUPC0N(M,THETA,KM,KE,KV,Y,YDOT,STOP,TOL. 954 FTOL.IGRAV.ALPHA,ALFA,RAD,D2,A2,D4,D6,INF01, 955 TOUT,NEQ,STABLE,IPAR6,IPOS,IL,*,*) 956 C 957 C THIS SUBROUTINE AND THOSE THAT IT CALLS COMPRISE THE SUPERVISORY 958 C CONTROLLER 959 C 960 INTEGER YES 961 REAL KM,KE,KV 962 DIMENSION THETDA( 6 ) , GDELTA (6 ) , PDELTA (6 ) , FDELTA (6 )', 963 THETA(M),Y(NEQ),YDOT(NEQ),KM(M),KE(M),KV(M) 964 C0MM0N/BL0CK5/THETDA 965 C0MM0N/BL0CK6/KX,KY,KZ,XSTART,YSTART,ZSTART,FX,FY,FZ 966 C0MM0N/BL0CK8/LINE 967 C 968 C INITIALIZE SOME PARAMETERS REQUIRED BY SUPERVISORY CONTROLLER 969 DATA GDELTA/6*0.0/,N/1/,IFIRST/1/,YES/1/,NO/0/ 970 C 971 C FIRST CALCULATE SET POINT ADJUSTMENT REQUIRED TO COMPENSATE FOR 972 C GRAVITY TORQUE DISTURBANCES "GDELTA(I)" 973 C 974 IF(IGRAV .EQ. NO) GO TO 4 975 C 976 CALL COMGRA(M,GDELTA,KM,KE) 977 INF01=0 978 C 979 C THIS SECTION WILL PERFORM THE ADJUSTMENT OF THE JOINT ANGLE 980 C SET POINTS BASED ON PROXIMITY SENSOR INFORMATION 981 C 982 C BEFORE ADJUSTING SET POINTS WE NEED TO KNOW IF ROBOT HAS STOPPED? 983 C 984 4 CALL STOPT(I SIT,M,Y,YDOT,STOP,NEQ,STABLE,IPAR6.IL,RAD,TOUT) 985 C 986 C HAS ROBOT STOPPED? 152. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CC1d=DAC0 Page 18 987 IFUSIT .EQ. YES) GO TO 18 988 C 989 IF(IFIRST .EQ. NO) GO TO 7 990 DO 6 1 = 1 ,M 991 THETDA(I)=THETDA(I)+GDELTA(I) 992 6 CONTINUE 993 GO TO 400 994 C 995 7 IF(IPOS .EQ. NO) GO TO 12 996 CALL P0SITN(D2,A2,D4.D6,PX,PY,PZ) 997 C 998 C IF WE GOT HERE ROBOT IS RUNNING POSITION CORRECTION ALGORITHM. 999 C ONLY INPUT PDELTA ON RETURN FROM POSCON. IT IS CARRIED 1000 C IN ADJUSTED THETA ON SUBSEQUENT UPDATES. 1001 DO 8 1 = 1 ,M 1002 PDELTA(I)=0.0 1003 8 CONTINUE 1004 C 1005 9 DO 10 1=1,M 1006 THETDA(I) = THETDA(I)+GDELTA(I)+PDELTA( I ) 1007 10 CONTINUE 1008 GO TO 400 1009 C 1010 12 CALL FORCON(M,THETA,KM,KE,KV,IFIRST,FTOL.N, 1011 GDELTA.ISIT, ALFA, FDELTA , D2 , A2 , D4 , D6 ,8.999) 1012 C 1013 15 DO 16 I = 1 , M 1014 THETDA(I)=THETDA(I)+GDELTA(I) + FDELTA( I ) 1015 16 CONTINUE 1016 INF01=0 1017 GO TO 400 1018 C 1019 C IF WE GOT TO HERE ROBOT IS STATIONARY 1020 18 WRITE(9,20) 1021 20 FORMAT(T1,' ',' *********STOPPED AT************'/' *') 1022 C 1023 LINE=LINE+2 1024 C HAS ANY CORRECTIVE SET POINT BEEN APPLIED YET? IF NOT WE PRINT 1025 C ROBOT GRIPPER POSITION HEADINGS 1026 IF(IFIRST .EQ. NO) GO TO 40 1027 C 1028 WRITE(9,30) 1029 30 FORMAT('0'2X'R0B0T GRIPPER LOCATION:'T75'PX'8X'PY'8X, 1030 .'PZ'/.5X'(IN MILLIMETERS)') 1031 C 1032 LINE=LINE+10 1033 C DETERMINE WHICH CONTROL MODE, FORCE OR POSITION? 1034 C IF IPOS=YES WE ARE RUNNING POSITION ERROR ALGORITHM 1035 40 IF(IPOS .EQ. YES) GO TO 50 1036 C 1037 C SET FLAG TO TELL SIMULATION "FUNC" TO RUN IN FORCE MODE 1038 IPAR6=0 1039 C 1040 CALL F0RC0N(M,THETA,KM,KE.KV,IFIRST,FTOL,N, 1041 GDELTA.ISIT,ALFA,FDELTA,D2,A2,D4,D6,8.999) 1042 WRITE(9.42)TOUT,THETA(1).THETA(2),THETA(3) 1043 42 FORMAT('+'F9.4,6F10.4) 1044 GO TO 15 153. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25. 1985 for CCid=DACO Page 19 1045 C 1046 C 1047 50 CALL POSCON(M,PDELTA,INF01,IFIRST,TOL,N,ALPHA, 1048 D2,A2,D4.D6,&999,&1000) 1049 WRITE(9,52)TOUT,THETA(1).THETA(2),THETA(3) 1050 52 FORMAT('+'F9.4,6F10.4) 1051 GO TO 9 1052 C 1053 C 1054 400 WRITE(9,500)THETDA(1),THETDA(2),THETDA( 3) 1055 500 FORMAT('+'T41,3F10.4) 1056 RETURN 1057 C 1058 999 RETURN 1 1059 1000 RETURN 2 1060 END 1061 C 1063 C 1064 SUBROUTINE COMGRA(M.GDELTA,KM,KE) 1065 C 1066 C THIS SUBROUTINE CALCULATES ADJUSTMENT IN SET POINTS NECESSARY 1067 C TO COMPENSATE FOR GRAVITY DISTURBANCE TORUES 1068 C 1069 REAL KM.KE.M1,M2.M3,M4,M5,M6 1070 DIMENSION G(6),GOLD(6),GNEW(6),GDELTA(M),KM(M),KE(M) 1071 DATA GOLD/6+0/ 1072 C 1073 C0MM0N/BL0CK3/D2.A2,D4,D6,M1,M2,M3,M4,M5,M6.GX,GY,GZ, 1074 X1,X2,X3,X4,X5.X6,Y1 ,Y2,Y3,Y4,Y5,Y6.Z1,Z2,Z3,Z4,Z5,Z6 1075 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3.S23,C23,S4,C4,S5,C5,S6,C6,RAD 1076 C 1077 G( 1 )=0. 1078 C 1079 G(2)=GZ*(A2*M6*C2+A2*M5*C2+A2*M4*C2+A2*M3*C2+A2*C2*M2+X2*C2*M2 1080 . +Z6*M6*S23+D6*M6*S23+M6*D4*S23+Z5*M5*S23+M5*D4*S23+D4*M4*S23 1081 . -Y4*M4*S23+S23*Z3*M3) 1082 C 1083 G(3)=GZ*S23*(Z6*M6+D6*M6+M6*D4+Z5*M5+M5*D4+D4*M4-Y4*M4+Z3*M3) 1084 C 1085 DO 20 1=1,M 1086 GNEW(I)=G(I)/(KM(I)*KE(I)*RAD) 1087 GDELTA(I)=GNEW(I)-GOLD(I) 1088 GOLD(I)=GNEW(I) 1089 20 CONTINUE 1090 C 1091 RETURN 1092 END 1093 C 1094 *************** 1095 C 1096 SUBROUTINE STOPT(I SIT,M,Y,YDOT,STOP,NEO.STABLE, 1097 IPAR6,IL,RAD,TOUT) 1098 C 1099 C THIS SUBROUTINE MONITORS NUMERICAL PROCESSES TO DETERMINE 1100 C WHEN THE ROBOT HAS FINISHED RESPONDING TO THE CHANGE IN 1101 C SET POINTS 1 102 C 154. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 20 1103 INTEGER YES 1104 DIMENSION VEL(6),ISLNK(6),Y(M),YD0T(NE0) 1105 DATA ISLNK/6*0/,YES/1/,NO/0/ 1 106 C 1 107 ISIT=N0 1 108 IF(T0UT .EQ. 0.0) RETURN 1 109 C 1 1 10 ISIT=YES 1111 C 1112 C FIRST CHECK IF WE ARE MONITORING VELOCITIES (POSITION ALGORITHM) 1113 C OR CURRENTS (FORCE ALGORITHM). WE MUST ALSO MONITOR VELOCITIES 1 1 14 C IF INDUCTANCES ARE ZERO AS DI/DT HAS NO MEANING 1 1 15 IF ((IPAR6 .EQ. 1).OR.(IL .EQ. 0)) GO TO 10 1 1 16 C 1117 C CURRENT IS MONITORED. DUE TO FLUCTUATIONS ROBOT HAS STOPPED 1 1 18 C ONLY WHEN ALL CURRENT RATES ARE BELOW "STABLE" 1 1 19 DO 4 1=1,M 1 120 IF(ABS(YDOT(I+2*M)).LT.STABLE) ISLNK(I)=YES 1121 ISIT=ISIT*ISLNK(I) 1 122 C RESET ISLNK FLAG FOR NEXT CALL TO STOPT 1 123 ISLNK(I)=N0 1 124 4 CONTINUE 1 125 RETURN 1 126 C 1 127 C VELOCITY AND SIGN OF ACCELERATION ARE MONITORED. IF VELOCITY 1 128 C IS ALLOWED TO GO TOO SMALL THE SIGN OF ACCEL. BECOMES 1 129 C UNDEPENDABLE. THUS WE MONITOR EACH LINK SEPARATELY 1 130 C SET INDIVIDUAL "LINK STOPPED?" FLAGS 1131 10 DO 30 1=1,M 1 132 IF(ISLNK(I ) .EQ. YES) GO TO 20 1 133 C 1 134 VEL(I)=(-Y(I)/RAD)*SIGN(1.O.YDOT(I)) 1 135 IF ( ( VEL( I ) . GE .O.O) .AND. (VEL(I) . LT . STOP ) ) ISLNK( I.) = YES 1 136 c 1 137 20 ISIT=ISIT*ISLNK(I) 1 138 30 CONTINUE 1 139 C 1 140 IF(ISIT .EQ. NO) RETURN 1 141 C 1 142 C IN THIS CASE ROBOT HAS STOPPED AND ISLNK FLAGS MUST BE 1 143 C RESET FOR NEXT CALL TO STOPT 1 144 DO 60 1=1,M 1 145 ISLNK(I)=NO 1 146 60 CONTINUE 1 147 RETURN 1 148 END 1 149 C 1 150 1 151 c 1 152 SUBROUTINE P0SITN(D2,A2,04,D6,PX,PY,PZ) 1 153 c 1 154 C THIS SUBROUTINE DETERMINES THE LOCATION OF THE ROBOT END PIECE 1155 C 1 156 C0MM0N/BL0CK4/S1,C1,S2,C2.S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 1 157 Q=(D4+D6)*S23+A2*C2 1 158 PX=(C1*Q-D2*S1)*1000.0 1 159 PY=(S1*Q+D2*C1)*1000.0 1 160 PZ=((D4+D6)*C23-A2*S2)*1000.0 155. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CC1d=DAC0 Page 21 1161 WRITEO, 10)PX,PY,PZ 1162 10 FORMAT(T1,'+',T71,3F10.3) 1163 RETURN 1164 END 1165 C 1166 £**************************************** 1167 C 1168 SUBROUTINE POSCON(M,PDELTA,INF01,IFIRST,TOL,N,ALPHA, 1169- D2,A2,D4,D6,*,*) 1 170 C 1171 C THIS SUBROUTINE IS THE POSITION SUPERVISORY CONTROLLER 1172 C 1173 DIMENSION PDELTA(M) 1174 C 1175 C WHERE DOES ROBOT THINK IT IS? 1 176 C 1177 CALL PQSITN(D2,A2,D4.D6,PX,PY,PZ) 1178 C 1179 C COMPUTE ERRORS IN POSITION AS DETERMINED FROM PROXIMITY SENSORS 1 180 C 1181 CALL SENSOR(IFIRST,PX,PY,PZ,DPX,DPY,DPZ) 1182 C 1183 C DPX,DPY AND DPZ ARE REQUIRED WORLD COORD. CORRECTIONS IN MILLIMETERS 1184 C WE USE THESE AS A BASIS TO FIND CHANGES IN THE ANGLE SET POINTS WHICH 1185 C WILL HOPEFULLY MOVE THE ROBOT SO AS TO REDUCE AND ELIMINATE POSITION 1186 C ERRORS DPX,DPY AND DPZ. FIRST WE CHECK IF ROBOT HAS REDUCED ERRORS 1187 C TO A REASONABLE LIMIT? 1 188 C 1189 IF((ABS(DPX) .LT.TOL).AND.(ABS(DPY).LT.TOL).AND. 1190 (ABS(DPZ).LT.TOL)) RETURN 1 1191 C 1192 C ERRORS ARE NOT WITHIN TOLERANCE THEREFORE CALCULATE ADJUSTMENT 1193 C IN SET POINT 1194 C 1195 CALL AJUSTP(M,PDELTA,DPX,DPY,DPZ,PX.PY.PZ, 1196 D2,A2,D4,D6,N,ALPHA,& 1000) 1197 INF01=0 1198 C 1199 RETURN 1200 C 1201 1000 RETURN 2 1202 C 1203 END 1204 C 1206 C 1207 SUBROUTINE SENSOR(IFIRST,PX,PY,PZ,DPX,DPY,DPZ ) 1208 C 1209 C THIS SUBROUTINE SIMULATES A POSITION SENSOR 1210 C 1211 C0MM0N/BL0CK7/PXD,PYD,PZD 1212 C 1213 C IS THIS THE FIRST CALL TO SENSOR? N0=0 YES=1 1214 IF(IFIRST .EQ. O) GO TO 70 1215 WRITEO,20) 1216 20 F0RMAT(T1,'0','THE FOLLOWING IS THE CURRENT ESTIMATED POSITION' 1217 ./.' OF THE MANIPULATOR IN WORLD COORDINATES CALCULATED FROM THE' 1218 ./' STEADY STATE JOINT ANGLES. VALUES ARE IN MILLIMETERS') 156. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 22 1219 WRITE(6,30)PX,PY,PZ 1220 30 FORMAT(T1,'0'5X'PX='F10.3,5X'PY=/F10.3,5X'PZ=/F10.3) 1221 WRITE(6,40) 1222 40 F0RMAT('OPLEASE INPUT REQUIRED CORRECTIONS IN WORLD COORDINATES' 1223 ./' WHICH WILL MOVE END PIECE TO DESIRED LOCATION' 1224 ./' DPX IN MILLIMETERS' 1225 ./' DPY IN MILLIMETERS' 1226 ./' DPZ IN MILLIMETERS') 1227 READ(5,50)DPX,DPY,DPZ 1228 50 F0RMAT(F15.0/F15.0/F15.0) 1229 C 1230 C COMPUTE ACTUAL DESIRED END PIECE LOCATION 1231 PXD=PX+DPX 1232 PYD=PY+DPY 1233 PZD=PZ+DPZ 1234 C 1235 WRITE(9,60)PXD,PYD,PZD,PX,PY,PZ 1236 60 FORMAT('0'2X'REQUIRED POSITION:'/' PXD='F8.3,2X'PYD='F8.3,2X 1237 .'PZD='F8.3,/,'0 ***START WITH ADJUSTED SET POINT****' 1238 ./' 'T71.3F10.3) 1239 IFIRST=0 1240 RETURN 1241 C 1242 70 DPX=PXD-PX 1243 DPY=PYD-PY 1244 DPZ=PZD-PZ 1245 C 1246 RETURN 1247 END 1248 C 1249 Q********************************************************* 1250 C 1251 SUBROUTINE AJUSTP(M,PDELTA,DPX,DPY,DPZ,PX,PY,PZ, 1252 D2,A2,04,06,N,ALPHA,*) 1253 C 1254 C THIS SUBROUTINE CALCULATES THE ADJUSTMENT IN SET POINTS USED IN 1255 C THE POSITION SUPERVISORY CONTROLLER USING THE INVERSE JACOBIAN 1256 C COMBINED WITH A WEIGHTING FACTOR 1257 C 1258 DIMENSION PDELTA(M) 1259 C0MM0N/BL0CK4/S1,C1 ,S2,C2,S3,C3,S23,C23,S4,C4,S5;C5,S6,C6,RAD 1260 C 1261 IF (N .NE. 1) GO TO 20 1262 X=ABS(DPX) 1263 Y=ABS(DPY) 1264 Z=ABS(DPZ) 1265 C 1266 C FIND BIGEST ERROR 1267 BIGEST=AMAX1(X,Y,Z) 1268 C 1269 CL0SE=20. 1270 C 1271 IF(BIGEST .LT. CLOSE) GO TO 20 1272 WEIGHT=(1.0-ALPHA) 1273 BALLPK=2.0*CL0SE 1274 IF(BIGEST .LT. BALLPK) GO TO 30 1275 WEIGHT=(BALLPK*WEIGHT)/BIGEST 1276 GO TO 30 157. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCld=DACO Page 23 1277 C 1278 20 WEIGHT=(1.0-ALPHA**N) 1279 N=N+1 1280 C 1281 30 DX=WEIGHT*DPX 1282 DY=WEIGHT*DPY 1283 DZ=WEIGHT*DPZ 1284 C 1285 C CALCULATE THE DELTA THETA TO BRING ABOUT DX,DY,AND DZ 1286 C 1287 A11=-PY 1288 A12=C1*PZ 1289 A13=(D4+D6)*C1*C23*1000.0 1290 A21=PX 1291 A22=S1*PZ 1292 A23=(D4+D6)*S1*C23*10O0.0 1293 A31=0.0 1294 A32=(-(D4+D6)*S23-A2*C2)*1000.0 1295 A33=-(D4+D6)*S23*1000.0 1296 C 1297 W1=DX*A22*A33+DY*A32*A13+DZ*A12*A23-DZ*A22*A13-DY*A12*A33 1298 -DX*A32*A23 1299 C 1300 W2=A11*DY*A33+A21*DZ*A13+A31*DX*A23-A31*DY*A13-A21*DX*A33 1301 -A11*DZ*A23 1302 C 1303 W3 = A1 1*A22*DZ+A21*A32*DX+A31*A12*DY-A31*A22*DX-A21*A12*DZ 1304 -A11*A32*DY 1305 C 1306 DETA = A1 1*A22*A33+A21*A32*A13+A31*A12*A23-A31*A22*A13 1307 -A21*A12*A33-A11*A32*A23 1308 C 1309 IF(ABS(DETA) .LT. 0.0OO01) GO TO 50 1310 C 1311 PDELTA(1)=(W1/DETA)/RAD 1312 PDELTA(2)=(W2/DETA)/RAD 1313 PDELTA(3)=(W3/DETA)/RAD 1314 C 1315 RETURN 1316 C 1317 50 WRITE(9,60) 1318 60 F0RMAT(T1,'0','JACOBIAN UNDEFINED THIS CONFIGURATION') 1319 RETURN 1 1320 END 1321 C 1322 c*************************************************** 1323 c 1324 SUBROUTINE FORCON(M,THETA,KM,KE,KV,I FIRST,FTOL, 1325 N.GDELTA,I SIT,ALFA,FDELTA,D2,A2,D4,D6.*) 1326 c 1327 C THIS SUBROUTINE IS THE FORCE SUPERVISORY CONTROLLER 1328 C 1329 INTEGER YES 1330 REAL KM,KE,KV,KX,KY,KZ 1331 DIMENSION THETA(M),KM(M),KE(M),KV(M).FDELTA(M), 1332 GDELTA(M) 1333 C0MM0N/BL0CK6/KX,KY,KZ,XSTART,YSTART.ZSTART,FX,FY.FZ 1334 DATA YES/1/,NO/0/ 158. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25. 1985 for CCid=DACO Page 24 1335 C 1336 C IS THIS THE FIRST CALL TO FORCE CONTROLLER? IF SO WE MUST INPUT 1337 C DESIRED APPLIED FORCES AND INITIALIZE END PIECE POSITION 1338 IF(IFIRST .EQ. NO) GO TO 40 1339 C 1340 CALL P0SITN(D2,A2.D4,D6,XSTART,YSTART,ZSTART) 1341 C RECALL POSITN EVALUATES LOCATION IN MM WHILE WE REQUIRE METERS 1342 XSTART=XSTART/1000. 1343 YSTART=YSTART/1000. 1344 ZSTART=ZSTART/1000. 1345 C INITIALIZE FORCES APPLIED BY END PIECE 1346 FX=0.0 1347 FY=0.0 1348 FZ=0.0 1349 C 1350 40 CALL CENSOR(IFIRST,FX,FY,FZ,DFX,DFY,DFZ) 1351 C 1352 WRITEO,50)FX,FY,FZ 1353 50 FORMAT('+'T101,3F10.4) 1354 C 1355 C DFX.DFY AND DFZ ARE REQUIRED CORRECTIONS IN FORCE THAT THE 1356 C ROBOT MUST TRY TO ACHIEVE 1357 C FIRST CHECK IF ROBOT HAS REDUCED FORCE ERRORS TO RESONABLE LIMIT? 1358 C 1359 ICLOSE=NO 1360 C 1361 IF((ABS(DFX).LT.FTOL).AND.(ABS(DFY).LT.FTOL).AND. 1362 . (ABS(DFZ).LT.FTOL)) ICLOSE=YES 1363 C 1364 IF((ISIT.EQ.YES).AND.(ICLOSE.EQ.YES)) RETURN 1 1365 C 1366 CALL P0SITN(D2,A2.D4,D6,PX,PY,PZ) 1367 C 1368 IF(ICLOSE .EQ. NO) GO TO 70 1369 C 1370 C AT THIS POINT ROBOT IS CLOSE BUT STILL- MOVING THUS FREEZE SET 1371 C POINT TO STOP ROBOT 1372 DO 60 1=1.M 1373 FDELTA(I)=0.O 1374 GDELTA(I)=0.0 1375 60 CONTINUE 1376 GO TO 80 1377 C 1378 C FORCE ERRORS ARE NOT WITHIN TOLERANCES THEREFORE ADJUST SET POINT 1379 C THAT IS COMPUTE FDELTA 1380 70 CALL AJUSTF(M,KM,KE,KV,DFX,DFY,DFZ,PX,PY,PZ,D2,A2,D4,D6, 1381 N,ALFA,FDELTA) 1382 80 CONTINUE 1383 C 1384 RETURN 1385 END 1386 C 1387 Q************************************************************ 1388 C 1389 SUBROUTINE CENSOR(IFIRST.FX,FY,FZ,DFX,DFY,DFZ) 1390 C 1391 C THIS SUBROUTINE SIMULATES A FORCE SENSOR 1392 C 159. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 25 1393 C IS THIS FIRST CALL TO CENSOR? N0=0 YES=1 1394 IF(IFIRST .EQ. 0) GO TO 60 1395 C 1396 WRITE(6,20) 1397 20 FORMAT('OPLEASE ENTER DESIRED FORCES ROBOT END PIECE IS TO APPLY 1398 ./' TO SURROUNDINGS: FXD IN NEWTONS' 1399 ./' FYD IN NEWTONS' 1400 ./' FZD IN NEWTONS') 1401 READ(5,30)FXD,FYD,FZD 1402 30 F0RMAT(F15.0/F15.0/F15.0) 1403 WRITE(9,40)FXD,FYD,FZD 1404 40 FORMATCO REQUIRED APPLIED FORCES:'/' FX='F8.3,2X'FY='F8.3,2X, 1405 .'FZ='F8.3.T105,'FX(N)'5X'FY(N)'5X'FZ(N)' 1406 ./'0 **START WITH ADJUSTED SET POINT AND***'/' *') 1407 IFIRST=0 1408 C 1409 60 DFX=FXD-FX 1410 DFY=FYD-FY 1411 DFZ=FZD-FZ 1412 C 1413 RETURN .1414 END 1415 C 1416 1417 C 1418 SUBROUTINE AJUSTF(M,KM,KE.KV,DFX,DFY,DFZ,PX,PY,PZ, 1419 D2,A2,D4,DG,N,ALFA,FDELTA) 1420 c 1421 c • THIS SUBROUTINE CALCULATES THE ADJUSTMENT IN SET POINTS REQUIRED 1422 c BY THE FORCE SUPERVISORY CONTROLLER USING THE TRANSPOSE OF THE 1423 c JACOBIAN. NOTE THAT A WEIGHTING FACTOR CAN BE USED IF DESIRED 1424 c ALTHOUGH RESPONSE IS QUICKEST IF ALFA=0 1425 c 1426 REAL KM,KE,KV 1427 DIMENSION TORQ(6),KM(M),KE(M),KV(M),FDELTA(M) 1428 C0MM0N/BL0CK4/S1,C1,S2,C2.S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 1429 c 1430 WEIGHT=(1.0-ALFA**N) 1431 N=N+1 1432 c 1433 DFXD=WEIGHT*DFX 1434 DFYD=WEIGHT*DFY 1435 DFZD=WEIGHT*DFZ 1436 c 1437 C CALCULATE JOINT TORQUE CHANGES REQUIRED USING TRANSPOSE OF JACOBIAN 1438 C 1439 TORQ(1)=(-PY*DFXD+PX*DFYD)/1000. 1440 T0RQ(2)=C1*(PZ/1000. )*DFXD+S1 *(PZ/lOOO. ) *DF YD 1441 .-((D4+D6)*S23+A2*C2)*DFZD 1442 T0RQ(3)=(D4+D6)*(C1*C23*DFXD+S1*C23*DFYD-S23*DFZD) 1443 C 1444 DO 30 1=1,M 1445 FDELTA(I )=TORQ(I)/(KM(I)*KE(I)*RAD) 1446 30 CONTINUE 1447 C 1448 RETURN 1449 END 1450 C 160. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25. 1985 f o r CC1d=DAC0 Page 26 1451 <;*.*****************»****************************»****•******** 1452 C 1453 C THE FOLLOWING SUBROUTINES INERTL,GRAV,CORIOL, AND CENTRI 1454 C EVALUATE THE DYNAMICS TERMS USED FOR A THREE LINK 1455 C SIMULATION. IF FURTHER WORK IS TO BE DONE ON THIS PROGRAM 1456 C THE REMAINDER OF THIS PROGRAM SHOULD BE PUT IN ITS OWN 1457 C FILE AND COMPILED SEPARATELY. IT CAN THEN BE LINKED TO 1458 C THE PREVIOUS SECTION WHICH WOULD BE IN ITS OWN FILE. THIS 1459 C AVOIDS HAVING TO COMPILE THE DYNAMICS TERMS EACH TIME A 1460 C CHANGE IN THE SIMULATION PROGRAM IS DESIRED. 1461 C 1462 c***************************************** 1463 C 1464 SUBROUTINE INERTL(D11,D12,D13,022,D23,D33) 1465 REAL M1,M2,M3,M4,M5,M6, 1466 I1XX,I1YY,I1ZZ,I1XY,I1XZ,I1YZ,I2XX,I2YY,I2ZZ,I2XY,I2XZ,I2YZ, 1467 . I3XX,I3YY,I3ZZ.I3XY,I3XZ,I3YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ,I4YZ, 1468 I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ 1469 C0MM0N/BL0CK2/ 1470 I1XX,11YY,I1ZZ,11XY,11XZ,I1YZ,12XX,12YY,12ZZ,12XY,12XZ,12YZ, 1471 I3XX,I3YY,I3ZZ,I3XY,I3XZ,I3YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ,I4YZ, 1472 I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY.I6XZ,I6YZ 1473 COMM0N/BLOCK3/D2,A2,D4,D6,M1,M2,M3,M4,M5,M6,GX,GY,GZ, 1474 . X1,X2,X3,X4,X5,X6,Y1,Y2,Y3,Y4,Y5,Y6,Z1,Z2,Z3,Z4,Z5,Z6 1475 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4.C4,S5,C5,S6,C6.RAD 1476 C 1477 D11A=I1YY 1478 C 1479 D11B=A2**2*M2*C2**2+2.*A2*X2*M2*C2**2-2.*A2*Y2*M2*S2*C2+2.*Z2*M2* 1480 . D2+M2*D2**2-2.* 12XY*S2*C2-I2XX*C2**2+12XX + I2YY*C2**2 1481 C 1482 011C=A2**2*M3*C2**2+2.*A2*Z3*C3*M3*S2*C2+2.*A2*Z3*S3*M3*C2**2+2.* 1483 . A2*C3*M3*X3*C2**2-2.*A2*S3*M3*X3*S2*C2+2.*Y3*M3*D2+4.*C3*S3*I3XZ 1484 . *C2**2-2.*C3*S3*I3XZ+2.*C3*S3*13XX*S2*C2-2.*C3*S3* 13ZZ*S2*C2-4.* 1485 . S3**2*I3XZ*S2*C2+2.*S3**2*I3XX*C2**2-S3**2*I3XX-2.*S3**2*I3ZZ*C2 1486 . **2+S3**2*I3ZZ+M3*D2**2+2.* 13XZ*S2*C2-13XX*C2**2+13XX + I3ZZ*C2**2 1487 C 1488 ANSI = -2.*I4XZ*C2**2*S4*C4+2.*I4XZ*S4*C4-2.* 14XY*S2*C2*C4-14XX*C2 1489 . **2*C4**2+I4XX*C4**2+I4YY*C2**2+I4ZZ*C2**2*C4**2-I4ZZ*C2**2-I4ZZ 1490 . *C4**2+I4ZZ 1491 D11D=A2**2*M4*C2**2-2.*A2*C3*Z4*M4*C2**2*S4-2.*A2*C3*Y4*M4*S2*C2+ 1492 . 2.*A2*C3*M4*X4*C2**2*C4+2.*A2*C3*M4*S2*D4*C2+2.*A2*S3*Z4*M4*S2*C2 1493 . *S4-2.*A2*S3*Y4*M4*C2**2-2.*A2*S3*M4*X4*S2*C2*C4+2.*A2*S3*M4*D4* 1494 . C2**2-4.*C3*S3*Z4*M4*D4*C2**2*S4+2.*C3*S3*Z4*M4*D4*S4-4.*C3*S3* 1495 . Y4*M4*S2*D4*C2+4.*C3*S3*I4YZ*C2**2*S4-2.*C3*S3*I4YZ*S4+4.*C3*S3* 1496 . M4*X4*D4*C2**2*C4-2.*C3*S3*M4*X4*D4*C4+2.*C3*S3*M4*S2*D4**2*C2+ 1497 . 4.*C3*S3*I4XZ*S2*C2*S4*C4-4.*C3*S3*I4XY*C2**2*C4+2.*C3*S3*I4XY*C4 1498 . +2.*C3*S3*I4XX*S2*C2*C4**2-2.*C3*S3*I4YY*S2*C2-2.*C3*S3*I4ZZ*S2* 1499 . C2*C4**2+2.*C3*S3*I4ZZ*S2*C2+4.*S3**2*Z4*M4*S2*D4*C2*S4-4. *S3**2 1500 . «Y4*M4*D4*C2**2+2.*S3**2*Y4*M4*D4-4.*S3**2*14YZ*S2*C2*S4-4.*S3** 1501 . 2*M4*X4*S2*D4*C2*C4+2.*S3**2*M4*D4**2*C2**2-S3**2*M4*D4**2+4.*S3 1502 . **2*I4XZ*C2**2*S4*C4-2.*S3**2*I4XZ*S4*C4+4.*S3**2*I4XY*S2*C2*C4+ 1503 . 2.*S3**2*I4XX*C2**2*C4**2-S3**2*I4XX*C4**2-2.*S3**2*I4YY*C2**2+S3 1504 . **2*I4YY-2.*S3**2*I4ZZ*C2**2*C4**2+2.*S3**2*I4ZZ*C2**2+S3**2* 1505 . I4ZZ*C4**2-S3**2*I4ZZ+2.*Z4*M4*D2*C4-2.*Z4*M4*S2*D4*C2*S4+2.*Y4* 1506 . M4*D4*C2**2-2.*Y4*M4*04+2.*I4YZ*S2*C2*S4+2.*M4*X4*D2*S4+2.*M4*X4 1507 . *S2*D4*C2*C4+M4*D2**2-M4*D4**2*C2**2+M4*D4**2+ANS1 1508 C 161. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 27 1509 ANS2=2.*S5*C5*C2**2*I5XZ*C4**2+2.*S5*C5*C2**2*I5XZ-2.*S5*C5*I5XZ* 1510 . C4**2+2.*S5*Z5*D2*M5*S4+2.*S5*Z5*S2*D4*C2*M5*C4+2.*S5*S2*C2*I5XY 1511 . *S4+2.*S5*D4*C2**2*M5*X5-2.*S5*D4*M5*X5-2.*S5*C2**2*I5YZ*S4*C4+ 1512 . 2.*S5*I5YZ*S4*C4+4 . *C5**2*S2*C2*I5XZ*C4-C5**2*C2**2*I5XX*C4**2-C5 1513 . **2*C2**2*I5XX+C5**2*C2**2*I5ZZ*C4**2+C5**2*C2**2*I5ZZ+C5**2* 1514 . I5XX*C4**2-C5**2*I5ZZ*C4**2-2 . *C5*Z5*D4*C2**2*M5+2.*C5*Z5*D4*M5+ 1515 . 2.*C5*D2*M5*X5*S4+2.*C5*S2*D4*C2*M5*X5*C4-2.*C5*S2*C2*I5YZ*S4-2.* 1516 . C5*C2**2*I5XY*S4*C4+2.*C5*I5XY*S4*C4+D2**2*M5+2.*D2*Y5*M5*C4-2.* 1517 . S2*D4*C2*Y5*M5*S4-2.*S2*C2*I5XZ*C4-D4**2*C2**2*M5+D4**2*M5+C2**2 1518 . *I5XX+C2**2*I5YY*C4**2-C2**2*I5YY-C2**2*I5ZZ*C4**2-I5YY*C4**2+ 1519 . I5YY+I5ZZ*C4**2 1520 ANS1=4.*S3**2*S5*C5*S2*C2*I5ZZ*C4-4.*S3**2*S5*C5*C2**2*I5XZ*C4**2 1521 . -4.*S3**2*S5*C5*C2**2*I5XZ+2.*S3**2*S5*C5*I5XZ*C4**2+2.*S3**2*S5 1522 . *C5*I5XZ-4.*S3**2*S5*Z5*S2*D4*C2*M5*C4-4.*S3**2*S5*S2*C2*I5XY*S4 1523 . -4.*S3**2*S5*D4*C2**2*M5*X5+2.*S3**2*S5*D4*M5*X5+4.*S3**2*S5*C2 1524 . **2*I5YZ*S4*C4-2.*S3**2*S5*I5YZ*S4*C4-8.*S3**2*C5**2*S2*C2*I5XZ* 1525 . C4+2.*S3**2*C5**2*C2**2*I5XX*C4**2+2.*S3**2*C5**2*C2**2*I5XX-2.* 1526 . S3**2*C5**2*C2**2*I5ZZ*C4**2-2.*S3**2*C5**2*C2**2*I5ZZ-S3**2*C5 1527 . **2*I5XX*C4**2-S3**2*C5**2*I5XX+S3**2*C5**2*I5ZZ*C4**2+S3**2*C5 1528 . **2*I5ZZ+4.*S3**2*C5*Z5*D4*C2**2*M5-2.*S3**2*C5*Z5*D4*M5-4.*S3** 1529 . 2*C5*S2*D4*C2*M5*X5*C4+4.*S3**2*C5*S2*C2*I5YZ*S4+4.*S3**2*C5*C2 1530 . **2*I5XY*S4*C4-2.*S3**2*C5*I5XY*S4*C4+4.*S3**2*S2*D4*C2*Y5*M5*S4 1531 . +4.*S3**2*S2*C2*I5XZ*C4+2.*S3**2*D4**2*C2**2*M5-S3**2*D4**2*M5-1532 . 2.*S3**2*C2**2*I5XX-2.*S3**2*C2**2*I5YY*C4**2+2.*S3**2*C2* *2*15YY 1533 . +2.*S3**2*C2**2*I5ZZ*C4**2+S3**2*I5XX+S3**2*I5YY*C4**2-S3**2* 1534 . I5YY-S3**2*I5ZZ*C4**2+2.*S5*C5*S2*C2*15XX*C4-2.*S5*C5*S2*C2* 15ZZ 1535 . *C4+ANS2 1536 D11E=A2**2*C2**2*M5+2.*A2*C3*S5*Z5*C2**2*M5*C4-2.*A2*C3*S5*S2*C2* 1537 . M5*X5+2.*A2*C3*C5*Z5*S2*C2*M5+2.*A2*C3*C5*C2**2*M5*X5*C4+2 . *A2* 1538 . C3*S2*D4*C2*M5-2.*A2*C3*C2**2*Y5*M5*S4-2.*A2*S3*S5*Z5*S2*C2*M5* 1539 . C4-2.*A2*S3*S5*C2**2*M5*X5+2.*A2*S3*C5*Z5*C2**2*M5-2.*A2*S3*C5* 1540 . S2*C2*M5*X5*C4+2.*A2*S3*S2*C2*Y5*M5*S4+2.*A2*S3*D4*C2**2*M5-4.* 1541 . C3*S3*S5*C5*S2*C2*I5XZ*C4**2-4.*C3*S3*S5*C5*S2*C2*I5XZ+4.*C3*S3* 1542 . S5*C5*C2**2*I5XX*C4-4.*C3*S3*S5*C5*C2**2*I5ZZ*C4-2.*C3*S3*S5*C5* 1543 . I5XX*C4+2 . *C3*S3*S5*C5*I5ZZ*C4+4.*C3*S3*S5*Z5*D4*C2**2*M5*C4-2 . * 1544 . C3*S3*S5*Z5*D4*M5*C4-4.*C3*S3*S5*S2*D4*C2*M5*X5+4.*C3*S3*S5*S2* 1545 . C2*I5YZ*S4*C4+4.*C3*S3*S5*C2**2*I5XY*S4-2.*C3*S3*S5*I5XY*S4+2.* 1546 . C3*S3*C5**2*S2*C2*I5XX*C4**2+2.*C3*S3*C5**2*S2*C2*I5XX-2.*C3*S3* 1547 . C5**2*S2*C2*I5ZZ*C4**2-2.*C3*S3*C5**2*S2*C2*I5ZZ+8.*C3*S3*C5**2* 1548 . C2**2*I5XZ*C4-4.*C3*S3*C5**2* 15XZ*C4 + 4.*C3*S3*C5*Z5*S2*D4*C2*M5+ 1549 . 4.*C3*S3*C5*S2*C2*I5XY*S4*C4+4.*C3*S3*C5*D4*C2**2*M5*X5*C4-2.*C3* 1550 . S3*C5*04*M5*X5*C4-4.*C3*S3*C5*C2**2*I5YZ*S4+2.*C3*S3*C5*I5YZ*S4+ 1551 . 2.*C3*S3*S2*04**2*C2*M5-2.*C3*S3*S2*C2*I5XX-2.*C3*S3*S2*C2*I5YY* 1552 . C4**2+2.*C3*S3*S2*C2*I5YY+2.*C3*S3*S2*C2*I5ZZ*C4**2-4.*C3*S3*D4* 1553 . C2**2*Y5*M5*S4+2.*C3*S3*D4*Y5*M5*S4-4.*C3*S3*C2**2*I5XZ*C4+2.*C3 1554 . *S3*I5XZ*C4-4.*S3**2*S5*C5*S2*C2*I5XX*C4+ANS1 1555 C 1556 ANS9=C2**2*I6XX-C2**2*I6YY-C4**2*I6YY+C4**2*I6ZZ+I6YY 1557 ANS8=4.*S3**2*S5*C5*S2*C2*C4*I6ZZ-4.*S3**2*S5*S2*D4*C2*C4*Z6*M6-1558 . 4.*S3**2*S5*S2*C2*S4*I6XY + 2.*S3**2*C5**2*C2* *2*C4* *2*16XX-2 . *S3** 1559 . 2*C5**2*C2**2*C4**2*I6ZZ+2.*S3**2*C5**2*C2**2*I6XX-2.*S3**2*C5** 1560 . 2*C2**2*I6ZZ-S3**2*C5**2*C4**2*I6XX+S3**2*C5**2*C4**2*I6ZZ-S3**2 1561' . *C5**2*I6XX+S3**2*C5**2*I6ZZ+4.*S3**2*C5*D4*C2**2*Z6*M6-2.*S3**2 1562 . *C5*D4*Z6*M6+4.*S3**2*C5*C2**2*S4*C4*I6XY-2.*S3**2*C5*S4*C4*I6XY 1563 . +2.*S3**2*D4**2*C2**2*M6-S3**2*D4**2*M6-2.*S3**2*C2**2*C4**2* 1564 . I6YY+2.*S3**2*C2**2*C4**2*I6ZZ-2 . *S3**2*C2**2*I6XX+2.*S3**2*C2** 1565 . 2*I6YY+S3**2*C4**2*I6YY-S3**2*C4**2*I6ZZ+S3**2*I6XX-S3**2*I6YY+ 1566 . 2.*S5*C5*S2*C2*C4*I6XX-2.*S5*C5*S2*C2*C4*I6ZZ+2.*S5*D2*S4*Z6*M6+ 162. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 28 1567 . 2.*S5*S2*D4*C2*C4*Z6*M6+2.*S5*S2*C2*S4*I6XY-C5**2*C2**2*C4**2* 1568 . I6XX+C5**2*C2**2*C4**2*I6ZZ-C5**2*C2**2*I6XX+C5**2*C2**2*I6ZZ+C5 1569 . **2*C4**2*I6XX-C5**2*C4**2*I6ZZ-2.*C5*D4*C2**2*Z6*M6+2.*C5*D4*Z6 1570 . *M6-2.*C5*C2**2*S4*C4*I6XY+2.*C5*S4*C4*I6XY+D2**2*M6-D4**2*C2**2 1571 . *M6+D4**2*M6+C2**2*C4**2*I6YY-C2**2*C4**2*I6ZZ+ANS9 1572 ANS7=-2.*S6*S3**2*S5*C5*C4**2*I6YZ-2.*S6*S3**2*S5*C5*I6YZ+4.*S6* 1573 . S3**2*S5*D4*C2**2*Y6*M6-2.*S6*S3**2*S5*D4*Y6*M6+4.*S6*S3**2*S5* 1574 . C2**2*S4*C4*I6XZ-2.*S6*S3**2*S5*S4*C4*I6XZ+8.*S6*S3**2*C5**2*S2* 1575 . C2*C4*I6YZ+4 . *S6*S3**2*C5*S2*D4*C2*C4*Y6*M6+4.*S6*S3**2*C5*S2*C2 1576 . *S4*I6XZ+4.*S6*S3**2*S2*D4*C2*S4*M6*X6-4.*S6*S3**2*S2*C2*C4*I6YZ 1577 . -2.*S6*S5*C5*C2**2*C4**2*I6YZ-2.*S6*S5*C5*C2**2*I6YZ+2.*S6*S5*C5 1578 . *C4**2*I6YZ-2.*S6*S5*D4*C2**2*Y6*M6+2.*S6*S5*D4*Y6*M6-2.*S6*S5* 1579 . C2**2*S4*C4*I6XZ+2.*S6*S5*S4*C4*I6XZ-4.*S6*C5**2*S2*C2*C4*I6YZ-1580 . 2.*S6*C5*D2*S4*Y6*M6-2.*S6*C5*S2*D4*C2*C4*Y6*M6-2.*S6*C5*S2*C2*S4 1581 . *I6XZ+2.*S6*D2*C4*M6*X6-2.*S6*S2*D4*C2*S4*M6*X6+2.*S6*S2*C2*C4* 1582 . I6YZ+4.*C3*S3*S5*C5*C2**2*C4*I6XX-4.*C3*S3*S5*C5*C2**2*C4* 16ZZ-1583 . 2.*C3*S3*S5*C5*C4*I6XX+2.*C3*S3*S5*C5*C4*I6ZZ+4.*C3*S3*S5*D4*C2** 1584 . 2*C4*Z6*M6-2.*C3*S3*S5*D4*C4*Z6*M6+4.*C3*S3*S5*Q2**2*S4*I6XY-2 . * 1585 . C3*S3*S5*S4*I6XY+2.*C3*S3*C5**2*S2*C2*C4**2*I6XX-2.*C3*S3*C5**2* 1586 . S2*C2*C4**2*I6ZZ+2.*C3*S3*C5**2*S2*C2*I6XX-2.*C3*S3*C5**2*S2*C2* 1587 . I6ZZ+4.*C3*S3*C5*S2*D4*C2*Z6*M6+4.*C3*S3*C5*S2*C2*S4*C4*I6XY+2.* 1588 . C3*S3*S2*D4**2*C2*M6-2.*C3*S3*S2*C2*C4**2*I6YY+2.*C3*S3*S2*C2*C4 1589 . **2*I6ZZ-2.*C3*S3*S2*C2*I6XX+2.*C3*S3*S2*C2*I6YY-4.*S3**2*S5*C5* 1590 . S2*C2*C4*I6XX+ANS8 1591 ANS6=-4.*S6**2*S3**2*C2**2*I6YY+S6**2*S3**2*C4**2*I6XX-S6**2*S3** 1592 . 2*C4**2*I6YY-2.*S6**2*S3**2*I6XX+2.*S6**2*S3**2*I6YY-2.*S6**2*S5 1593 . *C5*S2*C2*C4*I6XX+2.*S6**2*S5*C5*S2*C2*C4*I6YY-4.*S6**2*S5*S2*C2 1594 . *S4*I6XY+S6**2*C5**2*C2**2*C4**2*I6XX-S6**2*C5**2*C2**2*C4**2* 1595 . I6YY+S6**2*C5**2*C2**2*I6XX-S6**2*C5**2*C2**2*I6YY-S6**2*C5**2* 1596 . C4**2*I6XX+S6**2*C5**2*C4**2*I6YY+4.*S6**2*C5*C2**2*S4*C4* 16XY-1597 . 4.*S6**2*C5*S4*C4*I6XY+S6**2*C2**2*C4**2*I6XX-S6**2*C2**2*C4**2* 1598 . I6YY-2.*S6**2*C2**2*I6XX+2.*S6**2*C2**2*I6YY-S6**2*C4**2*I6XX+S6 1599 . **2*C4**2*I6YY+S6**2*I6XX-S6**2*I6YY+4.*S6*C3*S3*S5*C5*S2*C2*C4 1600 . **2*I6YZ+4.*S6*C3*S3*S5*C5*S2*C2*I6YZ+4.*S6*C3*S3*S5*S2*D4*C2*Y6 1601 . *M6+4.*S6*C3*S3*S5*S2*C2*S4*C4*I6XZ-8.*S6*C3*S3*C5**2*C2**2*C4* 1602 . I6YZ+4.*S6*C3*S3*C5**2*C4*I6YZ-4.*S6*C3*S3*C5*D4*C2**2*C4*Y6*M6+ 1603 . 2.*S6*C3*S3*C5*D4*C4*Y6*M6-4.*S6*C3*S3*C5*C2**2*S4*I6XZ+2.*S6*C3* 1604 . S3*C5*S4*I6XZ-4.*S6*C3*S3*D4*C2**2*S4*M6*X6+2.*S6*C3*S3*D4*S4*M6 1605 . *X6+4.*S6*C3*S3*C2**2*C4*I6YZ-2.*S6*C3*S3*C4*I6YZ+4.*S6*S3**2*S5 1606 . *C5*C2**2*C4**2*I6YZ+4.*S6*S3**2*S5*C5*C2**2*I6YZ+ANS7 1607 ANS5=-2.*C6*S2*C2*C4*I6XZ-4.*S6**2*C3*S3*S5*C5*C2**2*C4*I6XX+4.* 1608 . S6**2*C3*S3*S5*C5*C2**2*C4*I6YY+2.*S6**2*C3*S3*S5*C5*C4*I6XX-2.* 1609 . S6**2*C3*S3*S5*C5*C4*I6YY-8.*S6**2*C3*S3*S5*C2**2*S4*I6XY+4.*S6 1610 . **2*C3*S3*S5*S4*I6XY-2.*S6**2*C3*S3*C5**2*S2*C2*C4**2*I6XX+2.*S6 1611 . **2*C3*S3*C5**2*S2*C2*C4**2*I6YY-2.*S6**2*C3*S3*C5**2*S2*C2* 16XX 1612 . +2.*S6**2*C3*S3*C5**2*S2*C2*I6YY-8.*S6**2*C3*S3*C5*S2*C2*S4*C4* 1613 . I6XY-2.*S6**2*C3*S3*S2*C2*C4**2*I6XX+2.*S6**2*C3*S3*S2*C2*C4**2* 1614 . I6YY+4.*S6**2*C3*S3*S2*C2*I6XX-4.*S6**2*C3*S3*S2*C2*I6YY+4.*S6** 1615 . 2*S3**2*S5*C5*S2*C2*C4*I6XX-4.*S6**2*S3**2*S5*C5*S2*C2*C4*I6YY+ 1616 . 8.*S6**2*S3**2*S5*S2*C2*S4*I6XY-2.*S6**2*S3**2*C5**2*C2**2*C4**2* 1617 . I6XX+2.*S6**2*S3**2*C5**2*C2**2*C4**2*I6YY-2.*S6**2*S3**2*C5**2* 1618 . C2**2*I6XX+2. *S6**2*S3**2*C5**2*C2**2*I6YY+S6**2*S3**2*C5**2*C4 1619 . **2*I6XX-S6**2*S3**2*C5**2*C4**2*I6YY+S6**2*S3**2*C5**2*I6XX-S6 1620 . **2*S3**2*C5**2*I6YY-8.*S6**2*S3**2*C5*C2**2*S4*C4*I6XY+4.*S6**2 1621 . *S3**2*C5*S4*C4*I6XY-2.*S6**2*S3**2*C2**2*C4**2*I6XX+2.*S6**2*S3 1622 . **2*C2**2*C4**2*I6YY+4.*S6**2*S3**2*C2**2*I6XX+ANS6 1623 ANS4 = -2 . *C6*S6*C2**2*C4**2*I6XY+4.*C6*S6*C2**2*I6XY + 2.*C6*S6*C4** 1624 . 2*I6XY-2.*C6*S6*I6XY-4.*C6*C3*S3*S5*C5*S2*C2*C4**2*I6XZ-4.*C6*C3 163. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 29 1625 . *S3*S5*C5*S2*C2*I6XZ-4.*C6*C3*S3*S5*S2*D4*C2*M6*X6+4.*C6*C3*S3* 1626 . S5*S2*C2*S4*C4*I6YZ+8.*C6*C3*S3*C5**2*C2**2*C4*I6^Z-4.*C6*C3*S3* 1627 . C5**2*C4*I6XZ+4.*C6*C3*S3*C5*D4*C2**2*C4*M6*X6-2.*C6*C3*S3*C5*D4 1628 . *C4*M6*X6-4.*C6*C3*S3*C5*C2**2*S4*I6YZ+2.*C6*C3*S3*C5*S4*I6YZ-4. 1629 . *C6*C3*S3*D4*C2**2*S4*Y6*M6+2.*C6*C3*S3*04*S4*Y6*M6-4.*C6*C3*S3* 1630 . C2**2*C4*I6XZ+2.*C6*C3*S3*C4*I6XZ-4.*C6*S3**2*S5*C5*C2**2*C4**2* 1631 . I6XZ-4.*C6*S3**2*S5*C5*C2**2*I6XZ+2, *C6*S3**2*S5*C5*C4**2* 16XZ+ 1632 . 2.*C6*S3**2*S5*C5*I6XZ-4.*C6*S3**2*S5*D4*C2**2*M6*X6+2.*C6*S3**2* 1633 . S5*D4*M6*X6+4.*C6*S3**2*S5*C2**2*S4*C4*I6YZ-2.*C6*S3**2*S5*S4*C4 1634 . *I6YZ-8.*C6*S3**2*C5**2*S2*C2*C4*I6XZ-4.*C6*S3**2*C5*S2*D4*C2*C4 1635 . *M6*X6+4.*C6*S3**2*C5*S2*C2*S4*I6YZ+4.*C6*S3**2*S2*D4*C2*S4*Y6* 1636 . M6+4.*C6*S3**2*S2*C2*C4*I6XZ+2.*C6*S5*C5*C2**2*C4**2*I6XZ+2.*C6* 1637 . S5*C5*C2**2*I6XZ-2.*C6*S5*C5*C4**2*I6XZ+2.*C6*S5*D4*C2**2*M6*X6-1638 . 2.*C6*S5*D4*M6*X6-2.*C6*S5*C2**2*S4*C4*I6YZ+2.*C6*S5*S4*C4*I6YZ+ 1639 . 4.*C6*C5**2*S2*C2*C4*I6XZ+2.*C6*C5*D2*S4*M6*X6+2.*C6*C5*S2*D4*C2* 1640 . C4*M6*X6-2.*C6*C5*S2*C2*S4*I6YZ+2.*C6*D2*C4*Y6*M6-2.*C6*S2*D4*C2 1641 . *S4*Y6*M6+ANS5 1642 ANS3=-2.*D6*C4**2*Z6*M6+2.*D6*Z6*M6+8.*C6*S6*C3*S3*S5*C5*C2**2*C4 1643 . *I6XY-4.*C6*S6*C3*S3*S5*C5*C4*I6XY-4.*C6*S6*C3*S3*S5*C2**2*S4* 1644 . I6XX+4.*C6*S6*C3*S3*S5*C2**2*S4*I6YY+2.*C6*S6*C3*S3*S5*S4*I6XX-1645 . 2.*C6*S6*C3*S3*S5*S4*I6YY+4.*C6*S6*C3*S3*C5**2*S2*C2*C4**2*I6XY+ 1646 . 4.*C6*S6*C3*S3*C5**2*S2*C2*I6XY-4.*C6*S6*C3*S3*C5*S2*C2*S4*C4* 1647 . I6XX+4.*C6*S6*C3*S3*C5*S2*C2*S4*C4*I6YY+4.*C6*S6*C3*S3*S2*C2*C4 1648 . **2*I6XY-8.*C6*S6*C3*S3*S2*C2*I6XY-8.*C6*S6*S3**2*S5*C5*S2*C2*C4 1649 . *I6XY+4.*C6*S6*S3**2*S5*S2*C2*S4*I6XX-4.*C6*S6*S3**2*S5*S2*C2*S4 1650 . *I6YY+4.*C6*S6*S3**2*C5**2*C2**2*C4**2*I6XY+4.*C6*S6*S3**2*C5**2 1651 . *C2**2*I6XY-2.*C6*S6*S3**2*C5**2*C4**2*I6XY-2.*C6*S6*S3**2*C5**2 1652 . *I6XY-4.*C6*S6*S3**2*C5*C2**2*S4*C4*I6XX+4.*C6*S6*S3**2*C5*C2**2 1653 . *S4*C4*I6YY+2.*C6*S6*S3**2*C5*S4*C4-*I6XX-2.*C6*S6*S3**2*C5*S4*C4 1654 . *I6YY+4.*C6*S6*S3**2*C2**2*C4**2*I6XY-8.*C6*S6*S3**2*C2**2*I6XY-1655 . 2.*C6*S6*S3**2*C4**2*I6XY+4.*C6*S6*S3**2*I6XY+4.*C6*S6*S5*C5*S2* 1656 . C2*C4*I6XY-2.*C6*S6*S5*S2*C2*S4* 16XX+2.*C6*S6*S5*S2*C2*S4* IGYY-1657 . 2.*C6*S6*C5**2*C2**2*C4**2*I6XY-2.*C6*S6*C5**2*C2**2*I6XY+2.*C6* 1658 . S6*C5**2*C4**2*I6XY+2.*C6*S6*C5*C2**2*S4*C4* 16XX-2.*C6*S6*C5*C2 1659 . **2*S4*C4*I6YY-2.*C6*S6*C5*S4*C4*I6XX+2 ..*C6*S6*C5*S4*C4* 16YY + 1660 . ANS4 1661 . ANS2=-2.*D6*S6*S3**2*S5*C5*Y6*M6+4.*D6*S6*S3**2*S5*C2**2*S4*C4*M6 1662 . *X6-2.*D6*S6*S3**2*S5*S4*C4*M6*X6+8.*D6*S6*S3**2*C5**2*S2*C2*C4* 1663 . Y6*M6+4.*D6*S6*S3**2*C5*S2*C2*S4*M6*X6-4.*D6*S6*S3**2*S2*C2*C4* 1664 . Y6*M6-2.*D6*S6*S5*C5*C2**2*C4**2*Y6*M6-2.*D6*S6*S5*C5*C2**2*Y6* 1665 . M6+2.*D6*S6*S5*C5*C4**2*Y6*M6-2.*D6*S6*S5*C2**2*S4*C4*M6*X6+2 . * 1666 . 06*S6*S5*S4*C4*M6*X6-4.*D6*S6*C5**2*S2*C2*C4*Y6*M6-2.*D6*S6*C5* 1667 . S2*C2*S4*M6*X6+2.*D6*S6*S2*C2*C4*Y6*M6+8.*D6*C3*S3*S5*C5*C2**2* 1668 . C4*Z6*M6-4.*D6*C3*S3*S5*C5*C4*Z6*M6+4.*D6*C3*S3*S5*D4*C2**2*C4* 1669 . M6-2.*D6*C3*S3*S5*D4*C4*M6+4.*06*C3*S3*C5**2*S2*C2*C4**2*Z6*M6+ 1670 . 4.*D6*C3*S3*C5**2*S2*C2*Z6*M6+4.*D6*C3*S3*C5*S2*D4*C2*M6-4.*D6*C3 1671 . *S3*S2*C2*C4**2*Z6*M6-8.*D6*S3**2*S5*C5*S2*C2*C4*Z6*M6-4.*D6*S3 1672 . **2*S5*S2*D4*C2*C4*M6+4.*D6*S3**2*C5**2*C2**2*C4**2*Z6*M6+4.*D6* 1673 . S3**2*C5**2*C2**2*Z6*M6-2.*D6*S3**2*C5**2*C4**2*Z6*M6-2.*D6*S3** 1674 . 2*C5**2*Z6*M6+4.*D6*S3**2*C5*D4*C2**2*M6-2.*D6*S3**2*C5*D4*M6-4. 1675 . *D6*S3**2*C2**2*C4**2*Z6*M6+2.*06*S3**2*C4**2*Z6*M6+4.*D6*S5*C5* 1676 . S2*C2*C4*Z6*M6+2.*D6*S5*D2*S4*M6+2.*D6*S5*S2*D4*C2*C4*M6-2.*D6* 1677 . C5**2*C2**2*C4**2*Z6*M6-2.*D6*C5**2*C2**2*Z6*M6+2 . *D6*C5**2*C4* * 1678 . 2*Z6*M6-2.*D6*C5*D4*C2**2*M6+2.*D6*C5*D4*M6+2.*D6*C2**2*C4**2*Z6 1679 . *M6+ANS3 1680 ANS1«D6**2*M6-4.*D6*C6*C3*S3*S5*C5*S2*C2*C4**2*M6*X6-4.*D6*C6*C3* 1681 . S3*S5*C5*S2*C2*M6*X6+4.*D6*C6*C3*S3*S5*S2*C2*S4*C4*Y6*M6+8.*D6* 1682 . C6*C3*S3*C5**2*C2**2*C4*M6*X6-4.*D6*C6*C3*S3*C5**2*C4*M6*X6-4.* 164. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CC1d=DAC0 Page 30 1683 . D6*C6*C3*S3*C5*C2**2*S4*Y6*M6+2.*D6*C6*C3*S3*C5*S4*Y6*M6-4.*D6* 1684 . C6*C3*S3*C2**2*C4*M6*X6+2.*D6*C6*C3*S3*C4*M6*X6-4.*D6*C6*S3**2* 1685 . S5*C5*C2**2*C4**2*M6*X6-4.*D6*C6*S3**2*S5*C5*C2**2*M6*X6+2.*D6* 1686 . C6*S3**2*S5*C5*C4**2*M6*X6+2.*D6*C6*S3**2*S5*C5*M6*X6+4.*D6*C6* 1687 . S3**2*S5*C2**2*S4*C4*Y6*M6-2.*D6*C6*S3**2*S5*S4*C4*Y6*M6-8.*D6* 1688 . C6*S3**2*C5**2*S2*C2*C4*M6*X6+4.*D6*C6*S3**2*C5*S2*C2*S4*Y6*M6+ 1689 . 4.*D6*C6*S3**2*S2*C2*C4*M6*X6+2.*D6*C6*S5*C5*C2**2*C4**2*M6*X6+2 . 1690 . *D6*C6*S5*C5*C2**2*M6*X6-2.*D6*C6*S5*C5*C4**2*M6*X6-2.*D6*C6*S5* 1691 . C2**2*S4*C4*Y6*M6+2.*D6*C6*S5*S4*C4*Y6*M6+4.*D6*C6*C5**2*S2*C2* 1692 . C4*M6*X6-2.*D6*C6*C5*S2*C2*S4*Y6*M6-2.*D6*C6*S2*C2*C4*M6*X6+4.* 1693 . D6*S6*C3*S3*S5*C5*S2*C2*C4**2*Y6*M6+4.*D6*S6*C3*S3*S5*C5*S2*C2* 1694 . Y6*M6+4.*D6*S6*C3*S3*S5*S2*C2*S4*C4*M6*X6-8.*D6*S6*C3*S3*C5**2* 1695 . C2**2*C4*Y6*M6+4.*D6*S6*C3*S3*C5**2*C4*Y6*M6-4.*D6*S6*C3*S3*C5* 1696 . C2**2*S4*M6*X6+2.*D6*S6*C3*S3*C5*S4*M6*X6+4.*D6*S6*C3*S3*C2**2* 1697 . C4*Y6*M6-2.*D6*S6*C3*S3*C4*Y6*M6+4.*D6*S6*S3**2*S5*C5*C2**2*C4** 1698 . 2*Y6*M6+4.*D6*S6*S3**2*S5*C5*C2**2*Y6*M6-2.*D6*S6*S3**2*S5*C5*C4 1699 . **2*Y6*M6+ANS2 1700 D11F=A2**2*C2**2*M6+2.*A2*D6*C3*S5*C2**2*C4*M6+2.*A2*D6*C3*C5*S2* 1701 . C2*M6-2.*A2*D6*S3*S5*S2*C2*C4*M6+2.*A2*D6*S3*C5*C2**2*M6-2.*A2* 1702 . C6*C3*S5*S2*C2*M6*X6+2.*A2*C6*C3*C5*C2**2*C4*M6*X6-2.*A2*C6*C3* 1703 . C2**2*S4*Y6*M6-2.*A2*C6*S3*S5*C2**2*M6*X6-2.*A2*C6*S3*C5*S2*C2* 1704 . C4*M6*X6+2.*A2*C6*S3*S2*C2*S4*Y6*M6+2.*A2*S6*C3*S5*S2*C2*Y6*M6-1705 . 2.*A2*S6*C3*C5*C2**2*C4*Y6*M6-2.*A2*S6*C3*C2**2*S4*M6*X6+2.*A2*S6 1706 . *S3*S5*C2**2*Y6*M6+2.*A2*S6*S3*C5*S2*C2*C4*Y6*M6+2.*A2*S6*S3*S2* 1707 . C2*S4*M6*X6+2.*A2*C3*S5*C2**2*C4*Z6*M6+2.*A2*C3*C5*S2*C2*Z6*M6+ 1708 . 2.*A2*C3*S2*D4*C2*M6-2.*A2*S3*S5*S2*C2*C4*Z6*M6+2.*A2*S3*C5*C2**2 1709 . *Z6*M6+2.*A2*S3*D4*C2**2*M6+4.*D6**2*C3*S3*S5*C5*C2**2*C4*M6-2.* 1710 . D6**2*C3*S3*S5*C5*C4*M6+2.*D6**2*C3*S3*C5**2*S2*C2*C4**2*M6+2 . * 1711 . D6**2*C3*S3*C5**2*S2*C2*M6-2.*D6**2*C3*S3*S2*C2*C4**2*M6-4.*D6** 1712 . 2*S3**2*S5*C5*S2*C2*C4*M6+2.*D6**2*S3**2*C5**2*C2**2*C4**2*M6+2. 1713 . *D6**2*S3**2*C5**2*C2**2*M6-D6**2*S3**2*C5**2*C4**2*M6-D6**2*S3 1714 . **2*C5**2*M6-2.*D6**2*S3**2*C2**2*C4**2*M6+D6**2*S3**2*C4**2*M6+ 1715 . 2.*D6**2*S5*C5*S2*C2*C4*M6-D6**2*C5**2*C2**2*C4**2*M6-06**2*C5**2 1716 . *C2**2*M6+D6**2*C5**2*C4**2*M6+06**2*C2**2*C4**2*M6-D6**2*C4**2* 1717 . M6+ANS1 1718 C 1719 D1 1=D1 1A+D1 1B+D11C+D11D+D11E+D11F 1720 C 1721 D12A=A2*Z2*M2*S2+A2*M2*D2*S2+X2*M2*D2*S2+Y2*M2*D2*C2+I2YZ*C2+I2XZ 1722 . *S2 1723 C 1724 D12B=A2*Y3*M3*S2+A2*M3*D2*S2-Z3*C3*M3*D2*C2+Z3*S3*M3*D2*S2-I3YZ* 1725 . C3*C2+I3YZ*S3*S2+C3*M3*X3*D2*S2+C3*I3XY*S2+S3*M3*X3*D2*C2+S3* 1726 .. I3XY*C2 1727 C 1728 D12C=A2*Z4*M4*S2*C4+A2*M4*X4*S2*S4+A2*M4*D2*S2-C3*Z4*M4*D2*S2*S4-1729 . C3*Z4*M4*D4*C2*C4+C3*Y4*M4*D2*C2+C3*I4YZ*C2*C4+C3*M4*X4*D2*S2*C4 1730 . -C3*M4*X4*D4*C2*S4-C3*M4*D2*D4*C2+2.*C3*I4XZ*S2*C4**2-C3*I4XZ*S2 1731 . +C3*I4XY*C2*S4-C3*I4XX*S2*S4*C4+C3*I4ZZ*S2*S4*C4-S3*Z4*M4*D2*C2* 1732 . S4+S3*Z4*M4*S2*D4*C4-S3*Y4*M4*D2*S2-S3*I4YZ*S2*C4+S3*M4*X4*D2*C2 1733 . *C4+S3*M4*X4*S2*D4*S4+S3*M4*D2*S2*D4+2.*S3*I4XZ*C2*C4**2-S3*I4XZ 1734 . *C2-S3*I4XY*S2*S4-S3*I4XX*C2*S4*C4+S3*I4ZZ*C2*S4*C4 1735 C 1736 D12D=A2*S5*Z5*S2*M5*S4+A2*C5*S2*M5*X5*S4+A2*D2*S2*M5+A2*S2*Y5*M5* 1737 . C4+2.*C3*S5*C5*S2*I5XZ*S4*C4-C3*S5*C5*C2*I5XX*S4+C3*S5*C5*C2*S4* 1738 . I5ZZ+C3*S5*Z5*02*S2*M5*C4-C3*S5*Z5*D4*C2*M5*S4+C3*S5*02*C2*M5*X5 1739 . +2.*C3*S5*S2*I5YZ*C4**2-C3*S5*S2*I5YZ+C3*S5*C2*I5XY*C4-C3*C5**2* 1740 . S2*I5XX*S4*C4+C3*C5**2*S2*S4*I5ZZ*C4-2.*C3*C5**2*C2*I5XZ*S4-C3* 165. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 31 1741 . C5*Z5*D2*C2*M5+C3*C5*D2*S2*M5*X5*C4+2.*C3*C5*S2*I5XY*C4**2-C3*C5 1742 . *S2*I5XY-C3*C5*D4*C2*M5*X5*S4-C3*C5*C2*I5YZ*C4-C3*02*S2*Y5*M5*S4 1743 . -C3*D2*D4*C2*M5+C3*S2*S4*I5YY*C4-C3*S2*S4*I5ZZ*C4-C3*04*C2*Y5*M5 1744 . *C4+C3*C2*I5XZ*S4+S3*S5*C5*S2*I5XX*S4-S3*S5*C5*S2*S4*I5ZZ+2.*S3* 1745 . S5*C5*C2*I5XZ*S4*C4+S3*S5*Z5*D2*C2*M5*C4+S3*S5*Z5*S2*D4*M5*S4-S3 1746 . *S5*D2*S2*M5*X5-S3*S5*S2*I5XY*C4+2.*S3*S5*C2*I5YZ*C4**2-S3*S5*C2 1747 . *I5YZ+2.*S3*C5**2*S2*I5XZ*S4-S3*C5**2*C2*I5XX*S4*C4+S3*C5**2*C2* 1748 . S4*I5ZZ*C4+S3*C5*Z5*D2*S2*M5+S3*C5*D2*C2*M5*X5*C4+S3*C5*S2*D4*M5 1749 . *X5*S4+S3*C5*S2*I5YZ*C4+2.*S3*C5*C2*I5XY*C4**2-S3*C5*C2*I5XY+S3* 1750 . D2*S2*D4*M5-S3*D2*C2*Y5*M5*S4+S3*S2*D4*Y5*M5*C4-S3*S2*I5XZ*S4+S3 1751 . *C2*S4*I5YY*C4-S3*C2*S4*I5ZZ*C4 1752 C 1753 ANS2=S6**2*S3*C5**2*C2*S4*C4*I6XX-S6**2*S3*C5**2*C2*S4*C4*I6YY-4. 1754 . *S6**2*S3*C5*C2*C4**2*I6XY+2.*S6**2*S3*C5*C2*I6XY+S6**2*S3*C2*S4 1755 . *C4*I6XX-S6**2*S3*C2*S4*C4*I6YY-2.*S6*C3*S5*C5*S2*S4*C4*I6YZ-S6* 1756 . C3*S5*D2*C2*Y6*M6+2.*S6*C3*S5*S2*C4**2*I6XZ-S6*C3*S5*S2*I6XZ+2.* 1757 . S6*C3*C5**2*C2*S4*I6YZ-S6*C3*C5*D2*S2*C4*Y6*M6+S6*C3*C5*04*C2*S4 1758 . *Y6*M6-S6*C3*C5*C2*C4*I6XZ-S6*C3*D2*S2*S4*M6*X6-S6*C3*D4*C2*C4* 1759 . M6*X6-S6*C3*C2*S4*I6YZ-2.*S6*S3*S5*C5*C2*S4*C4* 16YZ+S6*S3*S5*D2* 1760 . S2*Y6*M6+2.*S6*S3*S5*C2*C4**2*I6XZ-S6*S3*S5*C2*I6XZ-2.*S6*S3*C5 1761 . **2*S2*S4*I6YZ-S6*S3*C5*D2*C2*C4*Y6*M6-S6*S3*C5*S2*D4*S4*Y6*M6+ 1762 . 56*S3*C5*S2*C4*I6XZ-S6*S3*D2*C2*S4*M6*X6+S6*S3*S2*D4*C4*M6*X6+S6 1763 . *S3*S2*S4*I6YZ-C3*S5*C5*C2*S4*I6XX+C3*S5*C5*C2*S4*I6ZZ+C3*S5*D2* 1764 . S2*C4*Z6*M6-C3*S5*D4*C2*S4*Z6*M6+C3*S5*C2*C4*I6XY-C3*C5**2*S2*S4 1765 . *C4*I6XX+C3*C5**2*S2*S4*C4*I6ZZ-C3*C5*D2*C2*Z6*M6+2.*C3*C5*S2*C4 1766 . **2*I6XY-C3*C5*S2*I6XY-C3*D2*D4*C2*M6+C3*S2*S4*C4*I6YY-C3*S2*S4* 1767 . C4*I6ZZ+S3*S5*C5*S2*S4*I6XX-S3*S5*C5*S2*S4*I6ZZ+S3*S5*D2*C2*C4* 1768 . Z6*M6+S3*S5*S2*D4*S4*Z6*M6-S3*S5*S2*C4*I6XY-S3*C5**2*C2*S4*C4* 1769 . I6XX+S3*C5**2*C2*S4*C4*I6ZZ+S3*C5*D2*S2*Z6*M6+2.*S3*C5*C2*C4**2* 1770 . I6XY-S3*C5*C2*I6XY+S3*D2*S2*D4*M6+S3*C2*S4*C4*I6YY-S3*C2*S4*C4* 1771 . I6ZZ 1772 ANSI=-2.*C6*S6*C3*C5**2*S2*S4*C4*I6XY-2.*C6*S6*C3*C5*S2*C4**2* 1773 . I6XX+2.*C6*S6*C3*C5*S2*C4**2*I6YY+C6*S6*C3*C5*S2*I6XX-C6*S6*C3* 1774 . C5*S2*I6YY-2.*C6*S6*C3*S2*S4*C4*I6XY+2.*C6*S6*S3*S5*C5*S2*S4* 1775 . I6XY+C6*S6*S3*S5*S2*C4*I6XX-C6*S6*S3*S5*S2*C4*I6YY-2.*C6*S6*S3* 1776 . C5**2*C2*S4*C4*I6XY-2.*C6*S6*S3*C5*C2*C4**2*I6XX+2.*C6*S6*S3*C5* 1777 . C2*C4**2*I6YY+C6*S6*S3*C5*C2*I6XX-C6*S6*S3*C5*C2*I6YY-2.*C6*S6* 1778 . S3*C2*S4*C4*I6XY+2.*C6*C3*S5*C5*S2*S4*C4*I6XZ+C6*C3*S5*D2*C2*MG* 1779 . X6+2.*C6*C3*S5*S2*C4**2*I6YZ-C6*C3*S5*S2*I6YZ-2.*C6*C3*C5**2*C2* 1780 . S4*I6XZ+C6*C3*C5*D2*S2*C4*M6*X6-C6*C3*C5*D4*C2*S4*M6*X6-C6*C3*C5 1781 . *C2*C4*I6YZ-C6*C3*D2*S2*S4*Y6*M6-C6*C3*D4*C2*C4*Y6*M6+C6*C3*C2* 1782 . S4*I6XZ+2.*C6*S3*S5*C5*C2*S4*C4*I6XZ-C6*S3*S5*D2*S2*M6*X6+2 . *C6* 1783 . S3*S5*C2*C4**2*I6YZ-C6*S3*S5*C2*I6YZ+2.*C6*S3*C5**2*S2*S4*I6XZ+ 1784 . C6*S3*C5*D2*C2*C4*M6*X6+C6*S3*C5*S2*04*S4*M6*X6+C6*S3*C5*S2*C4* 1785 . I6YZ-C6*S3*D2*C2*S4*Y6*M6+C6*S3*S2*D4*C4*Y6*M6-C6*S3*S2*S4*I6XZ+ 1786 . S6**2*C3*S5*C5*C2*S4*I6XX-S6**2*C3*S5*C5*C2*S4*I6YY-2.*S6**2*C3* 1787 . S5*C2*C4*I6XY+S6**2*C3*C5**2*S2*S4*C4*I6XX-S6**2*C3*C5**2*S2*S4* 1788 . C4*I6YY-4.*S6**2*C3*C5*S2*C4**2*I6XY+2.*S6**2*C3*C5*S2*I6XY+S6** 1789 . 2*C3*S2*S4*C4*I6XX-S6**2*C3*S2*S4*C4*I6YY-S6**2*S3*S5*C5*S2*S4* 1790 . I6XX+S6**2*S3*S5*C5*S2*S4*I6YY+2.*S6**2*S3*S5*S2*C4*I6XY+ANS2 1791 D12E=A2*D6*S5*S2*S4*M6+A2*C6*C5*S2*S4*M6*X6+A2*C6*S2*C4*Y6*M6-A2* 1792 . S6*C5*S2*S4*Y6*M6+A2*S6*S2*C4*M6*X6+A2*S5*S2*S4*Z6*M6+A2*D2*S2* 1793 . M6-D6**2*C3*S5*C5*C2*S4*M6-D6**2*C3*C5**2*S2*S4*C4*M6+06**2*C3* 1794 . S2*S4*C4*M6+D6**2*S3*S5*C5*S2*S4*M6-D6**2*S3*C5**2*C2*S4*C4*M6+ 1795 . D6**2*S3*C2*S4*C4*M6+2.*D6*C6*C3*S5*C5*S2*S4*C4*M6*X6+2.*D6*C6* 1796 . C3*S5*S2*C4**2*Y6*M6-D6*C6*C3*S5*S2*Y6*M6-2.*D6*C6*C3*C5**2*C2* 1797 . S4*M6*X6-D6*C6*C3*C5*C2*C4*Y6*M6+D6*C6*C3*C2*S4*M6*X6+2.*D6*C6* 1798 . S3*S5*C5*C2*S4*C4*M6*X6+2.*D6*C6*S3*S5*C2*C4**2*Y6*M6-D6*C6*S3* 166. L i s t i n g of ROBSIM.FOR at 1 5 : 4 5 : 1 3 on MAY 25. 1985 f o r CCid=DACO Page 32 1799 . S5*C2*Y6*M6+2.*D6*C6*S3*C5**2*S2*S4*M6*X6+06*C6*S3*C5*S2*C4*Y6* 1800 . M6-D6*C6*S3*S2*S4*M6*X6-2.*D6*S6*C3*S5*C5*S2*S4*C4*Y6*M6+2 . *D6* 1801 . S6*C3*S5*S2*C4**2*M6*X6-06*S6*C3*S5*S2*M6*X6+2.*D6*S6*C3*C5**2* 1802 . C2*S4*Y6*M6-D6*S6*C3*C5*C2*C4*M6*X6-D6*S6*C3*C2*S4*Y6*M6-2.*D6* 1803 . S6*S3*S5*C5*C2*S4*C4*Y6*M6+2.*D6*S6*S3*S5*C2*C4**2*M6*X6-D6*S6* 1804 . S3*S5*C2*M6*X6-2.*D6*S6*S3*C5**2*S2*S4*Y6*M6+D6*S6*S3*C5*S2*C4* 1805 . M6*X6+D6*S6*S3*S2*S4*Y6*M6-2.*D6*C3*S5*C5*C2*S4*Z6*M6+D6*C3*S5* 1806 . D2*S2*C4*M6-D6*C3*S5*D4*C2*S4*M6-2.*06*C3*C5**2.*S2*S4*C4*Z6*M6-1807 . D6*C3*C5*D2*C2*M6+2.*D6*C3*S2*S4*C4*Z6*M6+2.*D6*S3*S5*C5*S2*S4* 1808 . Z6*M6+D6*S3*S5*D2*C2*C4*M6+D6*S3*S5*S2*D4*S4*M6-2.*D6*S3*C5**2* 1809 . C2*S4*C4*Z6*M6+D6*S3*C5*D2*S2*M6+2.*D6*S3*C2*S4*C4*Z6*M6-2.*C6* 1 8 1 0 . S 6 * C 3 * S 5 * C 5 * C 2 * S 4 * I 6 X Y - C 6 * S 6 * C 3 * S 5 * C 2 * C 4 * I 6 X X + C 6 * S 6 * C 3 * S 5 * C 2 * C 4 * 1811 . I6YY+ANS1 1812 C 1813 D12=D12A+D12B+D12C+D12D+D12E 1814 C 1815 D13A=-Z3*C3*M3*D2*C2+Z3*S3*M3*D2*S2-I3YZ*C3*C2+I3YZ*S3*S2+C3*M3* 1816 . X3*D2*S2+C3*I3XY*S2+S3*M3*X3*D2*C2+S3*I3XY*C2 1817 C 1818 1819 1820 1821 1822 1823 1824 1825 C 1826 D13C=2.*C3*S5*C5*S2*I5XZ*S4*C4-C3*S5*C5*C2*I5XX*S4+C3*S5*C5*C2*S4 1827 . *I5ZZ+C3*S5*Z5*D2*S2*M5*C4-C3*S5*Z5*D4*C2*M5*S4+C3*S5*D2*C2*M5* 1828 . X 5 + 2 . * C 3 * S 5 * S 2 * I 5 Y Z * C 4 * * 2 - C 3 * S 5 * S 2 * I 5 Y Z + C 3 * S 5 * C 2 * I 5 X Y * C 4 - C 3 * C 5 * * 1829 . 2 * S 2 * I 5 X X * S 4 * C 4 + C 3 * C 5 * * 2 * S 2 * S 4 * I 5 Z Z * C 4 - 2 . * C 3 * C 5 * * 2 * C 2 * I 5 X Z * S 4 - C 3 1 8 3 0 . *C5*Z5*D2*C2*M5+C3*C5*D2*S2*M5*X5*C4+2.*C3*C5*S2*I5XY*C4**2-C3* 1B31 . C5*S2*I5XY-C3*C5*D4*C2*M5*X5*S4-C3*C5*C2*I5YZ*C4-C3*D2*S2*Y5*M5* 1832 . S4-C3*D2*04*C2*M5+C3*S2*S4*I5YY*C4-C3*S2*S4*I5ZZ*C4-C3*D4*C2*Y5* 1833 . M 5 * C 4 + C 3 * C 2 * I 5 X Z * S 4 + S 3 * S 5 * C 5 * S 2 * I 5 X X * S 4 - S 3 * S 5 * C 5 * S 2 * S 4 * I 5 Z Z + 2 . * 1834 . S3*S5*C5*C2*I5XZ*S4*C4+S3*S5*Z5*D2*C2*M5*C4+S3*S5*Z5*S2*D4*M5*S4 1835 . - S 3 * S 5 * D 2 * S 2 * M 5 * X 5 - S 3 * S 5 * S 2 * I 5 X Y * C 4 + 2 . * S 3 * S 5 * C 2 * I 5 Y Z * C 4 * * 2 - S 3 * S 5 1836 . * C 2 * I 5 Y Z + 2 . * S 3 * C 5 * * 2 * S 2 * I 5 X Z * S 4 - S 3 * C 5 * * 2 * C 2 * I 5 X X * S 4 * C 4 + S 3 * C 5 * * 2 * 1837 . C2*S4*I5ZZ*C4+S3*C5*Z5*D2*S2*M5+S3*C5*D2*C2*M5*X5*C4+S3*C5*S2*D4 1838 . *M5*X5*S4+S3*C5*S2*I5YZ*C4+2.*S3*C5*C2*I5XY*C4**2TS3*C5*C2*I5XY+ 1839 . S3*D2*S2*D4*M5-S3*D2*C2*Y5*M5*S4+S3*S2*D4*Y5*M5*C4-S3*S2*I5XZ*S4 1840 . + S 3 * C 2 * S 4 * I 5 Y Y * C 4 - S 3 * C 2 * S 4 * I 5 Z Z * C 4 1841 C 1842 A N S 2 = 2 . * S 6 * * 2 * S 3 * C 5 * C 2 * I 6 X Y + S 6 * * 2 * S 3 * C 2 * S 4 * C 4 * I 6 X X - S 6 * * 2 * S 3 * C 2 * S 4 1843 . «C4*I6YY-2.*S6*C3*S5*C5*S2*S4*C4*I6YZ-S6*C3*S5*D2*C2*Y6*M6+2.*S6 1844 . * C 3 * S 5 * S 2 * C 4 * * 2 * I 6 X Z - S 6 * C 3 * S 5 * S 2 * I 6 X Z + 2 . * S 6 * C 3 * C 5 * * 2 * C 2 * S 4 * I 6 Y Z -1845 . S6*C3*C5*D2*S2*C4*Y6*M6+S6*C3*C5*D4*C2*S4*Y6*M6-S6*C3*C5*C2*C4* 1846 . I6XZ-S6*C3*D2*S2*S4*M6*X6-S6*C3*D4*C2*C4*M6*X6-S6*C3*C2*S4*I6YZ-1847 . 2.*S6*S3*S5*C5*C2*S4*C4*I6YZ+S6*S3*S5*02*S2*Y6*M6+2.*S6*S3*S5*C2* 1848 . C 4 * * 2 * I 6 X Z - S 6 * S 3 * S 5 * C 2 * I 6 X Z - 2 . * S 6 * S 3 * C 5 * * 2 * S 2 * S 4 * I 6 Y Z - S 6 * S 3 * C 5 * 1849 . D2*C2*C4*Y6*M6-S6*S3*C5*S2*D4*S4*Y6*M6+S6*S3*C5*S2*C4*I6XZ-S6*S3 1 8 5 0 . *D2*C2*S4*M6*X6+S6*S3*S2*D4*C4*M6*X6+S6*S3*S2*S4*I6YZ-C3*S5*C5* 1851 . C2*S4*I6XX+C3*S5*C5*C2*S4*I6ZZ+C3*S5*D2*S2*C4*Z6*M6-C3*S5*D4*C2* 1852 . S4*Z6*M6+C3*S5*C2*C4*I6XY-C3*C5**2*S2*S4*C4*I6XX+C3*C5**2*S2*S4* 1853 . C 4 * I 6 Z Z - C 3 * C 5 * D 2 * C 2 * Z 6 * M 6 + 2 . * C 3 * C 5 * S 2 * C 4 * * 2 * I 6 X Y - C 3 * C 5 * S 2 * I 6 X Y -1854 . C3*D2*D4*C2*M6+C3*S2*S4*C4*I6YY-C3*S2*S4*C4*I6ZZ+S3*S5*C5*S2*S4* 1855 . I6XX-S3*S5*C5*S2*S4*I6ZZ+S3*S5*D2*C2*C4*Z6*M6+S3*S5*S2*D4*S4*Z6* 1856 . M 6 - S 3 * S 5 * S 2 * C 4 * I 6 X Y - S 3 * C 5 * * 2 * C 2 * S 4 * C 4 * I 6 X X + S 3 * C 5 * * 2 * C 2 * S 4 * C 4 * 013B=-C3*Z4*M4*D2*S2*S4-C3*Z4*M4*D4*C2*C4+C3*Y4*M4*D2*C2+C3*I4YZ* . C2*C4+C3*M4*X4*D2*S2*C4-C3*M4*X4*D4*C2*S4-C3*M4*D2*D4*C2+2.*C3* . I 4 X Z * S 2 * C 4 * * 2 - C 3 * I 4 X Z * S 2 + C 3 * I 4 X Y * C 2 * S 4 - C 3 * I 4 X X * S 2 * S 4 * C 4 + C 3 * I 4 Z Z * . S2*S4*C4-S3*Z4*M4*D2*C2*S4+S3*Z4*M4*S2*D4*C4-S3*Y4*M4*D2*S2-S3* . I4YZ*S2*C4+S3*M4*X4*D2*C2*C4+S3*M4*X4*S2*D4*S4+S3*M4*D2*S2*D4+2. . * S 3 * I 4 X Z * C 2 * C 4 * * 2 - S 3 * I 4 X Z * C 2 - S 3 * I 4 X Y * S 2 * S 4 - S 3 * I 4 X X * C 2 * S 4 * C 4 + S 3 * . I 4 Z Z * C 2 * S 4 * C 4 167. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DAC0 Page 33 1857 . I6ZZ+S3*C5*D2*S2*Z6*M6+2.*S3*C5*C2*C4**2*IGXY-S3*C5*C2*I6XY+S3* 1858 . D2*S2*D4*M6+S3*C2*S4*C4*I6YY-S3*C2*S4*C4*I6ZZ 1859 ANS1=-C6*S6*C3*C5*S2*I6YY-2.*CG*S6*C3*S2*S4*C4*I6XY+2.*CG*SG*S3* 1860 . S5*C5*S2*S4*I6XY+C6*S6*S3*S5*S2*C4*I6XX-C6*S6*S3*S5*S2*C4*I6YY-1861 . 2.*C6*S6*S3*C5**2*C2*S4*C4*I6XY-2.*C6*S6*S3*C5*C2*C4**2*I6XX+2.* 1862 . C6*S6*S3*C5*C2*C4**2*I6YY+C6*S6*S3*C5*C2*I6XX-C6*S6*S3*C5*C2* 1863 . I6YY-2.*C6*S6*S3*C2*S4*C4*I6XY+2.*C6*C3*S5*C5*S2*S4*C4*I6XZ+C6* 1864 . C3*S5*D2*C2*M6*X6+2.*C6*C3*S5*S2*C4**2*I6YZ-C6*C3*S5*S2*I6YZ-2.* 1865 . C6*C3*C5**2*C2*S4*I6XZ+C6*C3*C5*D2*S2*C4*M6*X6-C6*G3*C5*D4*C2*S4 1866 . *M6*X6-C6*C3*C5*C2*C4*I6YZ-C6*C3*D2*S2*S4*Y6*M6-C6*C3*04*C2*C4* 1867 . Y6*MG+C6*C3*C2*S4*I6XZ+2.*C6*S3*S5*C5*C2*S4*C4*I6XZ-C6*S3*S5*D2* 1868 . S2*M6*X6+2.*C6*S3*S5*C2*C4**2*I6YZ-C6*S3*S5*C2*I6YZ+2.*C6*S3*C5 1869 . **2*S2*S4*I6XZ+C6*S3*C5*D2*C2*C4*M6*X6+C6*S3*C5*S2*D4*S4*M6*X6+ 1870 . C6*S3*C5*S2*C4*I6YZ-C6*S3*D2*C2*S4*Y6*M6+C6*S3*S2*D4*C4*Y6*M6-C6 1871 . *S3*S2*S4*I6XZ+S6**2*C3*S5*C5*C2*S4*I6XX-S6**2*C3*S5*C5*C2*S4* 1872 . I6YY-2 . *S6**2*C3*S5*C2*C4*I6XY+S6**2*C3*C5**2*S2*S4*C4*I6XX-S6** 1873 . 2*C3*C5**2*S2*S4*C4*I6YY-4.*S6**2*C3*C5*S2*C4**2*I6XY+2.*S6**2* 1874 . C3*C5*S2*I6XY+S6**2*C3*S2*S4*C4*I6XX-S6**2*C3*S2*S4*C4*IGYY-S6** 1875 . 2*S3*S5*C5*S2*S4*I6XX+S6**2*S3*S5*C5*S2*S4*I6YY+2.*S6**2*S3*S5* 1876 . S2*C4*I6XY+S6**2*S3*C5**2*C2*S4*C4*I6XX-SG**2*S3*C5**2*C2*S4*C4* 1877 . I6YY-4.*S6**2*S3*C5*C2*C4**2*I6XY+ANS2 1878 D13D=-D6**2*C3*S5*C5*C2*S4*M6-D6**2*C3*C5**2*S2*S4*C4*M6+D6**2*C3 1879 . *S2*S4*C4*M6+D6**2*S3*S5*C5*S2*S4*M6-D6**2*S3*C5**2*C2*S4*C4*M6+ 1880 . D6**2*S3*C2*S4*C4*M6+2.*06*C6*C3*S5*C5*S2*S4*C4*M6*X6+2.*D6*C6* 1881 . C3*S5*S2*C4**2*Y6*M6-D6*C6*C3*S5*S2*Y6*M6-2.*D6*C6*C3*C5**2*C2* 1882 . S4*M6*X6-D6*C6*C3*C5*C2*C4*Y6*M6+D6*C6*C3*C2*S4*M6*X6+2.*D6*C6* 1883 . S3*S5*C5*C2*S4*C4*M6*X6+2.*D6*C6*S3*S5*C2*C4**2*Y6*M6-D6*C6*S3* 1884 . S5*C2*Y6*M6+2.*D6*C6*S3*C5**2*S2*S4*M6*X6+D6*C6*S3*C5*S2*C4*Y6* 1885 . M6-D6*C6*S3*S2*S4*M6*X6-2.*D6*S6*C3*S5*C5*S2*S4*C4*Y6*M6+2.*D6* 1886 . S6*C3*S5*S2*C4**2*M6*X6-D6*S6*C3*S5*S2*M6*X6+2.*D6*S6*C3*C5**2* 1887 . C2*S4*Y6*M6-D6*S6*C3*C5*C2*C4*M6*X6-D6*S6*C3*C2*S4*Y6*M6-2.*D6* 1888 . S6*S3*S5*C5*C2*S4*C4*Y6*M6+2.*D6*S6*S3*S5*C2*C4**2*M6*X6-DG*S6* 1889 . S3*S5*C2*M6*X6-2.*06*S6*S3*C5**2*S2*S4*Y6*M6+D6*S6*S3*C5*S2*C4* 1890 . M6*X6+D6*S6*S3*S2*S4*Y6*M6-2.*D6*C3*S5*C5*C2*S4*ZG*MG+06*C3*S5* 1891 . D2*S2*C4*M6-D6*C3*S5*D4*C2*S4*M6-2.*D6*C3*C5**2*S2*S4*C4*Z6*M6-1892 . D6*C3*C5*D2*C2*M6+2.*D6*C3*S2*S4*C4*Z6*M6+2.*DG*S3*S5*C5*S2*S4* 1893 . Z6*M6+D6*S3*S5*D2*C2*C4*M6+D6*S3*S5*S2*D4*S4*M6-2.*D6*S3*C5**2* 1894 . C2*S4*C4*Z6*M6+D6*S3*C5*D2*S2*M6+2.*D6*S3*C2*S4*C4*Z6*M6-2.*C6* 1895 . S6*C3*S5*C5*C2*S4*I6XY-C6*S6*C3*S5*C2*C4*I6XX+C6*S6*C3*S5*C2*C4* 1896 . I6YY-2.*C6*S6*C3*C5**2*S2*S4*C4*I6XY-2.*C6*S6*C3*C5*S2*C4**2* 1897 . I6XX+2.*C6*S6*C3*C5*S2*C4**2*I6YY+C6*S6*C3*C5*S2*I6XX+ANS1 1898 C 1899 D13=D13A+D13B+D13C+D13D 1900 C 1901 D22A=A2**2*M2+2.*A2*X2*M2+I2ZZ 1902 C 1903 D22B=A2**2*M3+2.*A2*Z3*M3*S3+2.*A2*M3*X3*C3+I3YY 1904 C 1905 D22C=A2**2*M4+2.*A2*D4*M4*S3-2.*A2*S4*Z4*M4*C3+2.*A2*C4*M4*X4*C3-1906 . 2.*A2*Y4*M4*S3+D4**2*M4-2.*D4*Y4*M4-2.*S4*C4*I4XZ-C4**2*I4XX+C4** 1907 . 2*I4ZZ+I4XX 1908 C 1909 D22D=A2**2*M5+2.*A2*D4*M5*S3-2.*A2*S4*Y5*M5*C3+2.*A2*C4*S5*Z5*M5* 1910 . C3+2.*A2*C4*C5*M5*X5*C3-2.*A2*S5*M5*X5*S3+2.*A2*C5*Z5*M5*S3+D4** 1911 . 2*M5-2.*D4*S5*M5*X5+2.*D4*C5*Z5*M5-2.*S4*C4*S5*I5YZ-2.*S4*C4*C5* 1912 . I5XY+2.*C4**2*S5*C5*I5XZ-C4**2*C5**2*I5XX+C4**2*C5**2*I5ZZ+C4**2 1913 . *I5YY-C4**2*I5ZZ-2.*S5*C5*I5XZ+C5**2*I5XX-C5**2*I5ZZ+I5ZZ 1914 C 168. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CC1d=DAC0 Page 34 1915 D22E=A2**2*M6+2.*A2*D4*S3*M6-2.*A2*S4*Y6*C3*M6*C6-2.*A2*S4*C3*M6* 1916 . X6*S6+2.*A2*C4*S5*D6*C3*M6+2.*A2*C4*S5*Z6*C3*M6-2.*A2*C4*C5*Y6* 1917 . C3*M6*S6+2.*A2*C4*C5*C3*M6*X6*C6+2.*A2*S5*Y6*S3*M6*S6-2.*A2*S5* 1918 . S3*M6*X6*C6+2.*A2*C5*D6*S3*M6+2.*A2*C5*Z6*S3*M6+D4**2*M6+2.*04* 1919 . S5*Y6*M6*S6-2.*D4*S5*M6*X6*C6+2.*D4*C5*D6*M6+2.*D4*C5*Z6*M6-2.* 1920 . S4*C4*S5*D6*Y6*M6*C6-2.*S4*C4*S5*D6*M6*X6*S6-2.*S4*C4*S5*I6YZ*C6 1921 . -2.*S4*C4*S5*I6XZ*S6+4.*S4*C4*C5*I6XY*S6**2-2.*S4*C4*C5*I6XY+2.* 1922 . S4*C4*C5*I6XX*C6*S6-2.*S4*C4*C5*I6YY*C6*S6-2.*C4**2*S5*C5*D6*Y6* 1923 . M6*S6+2.*C4**2*S5*C5*D6*M6*X6*C6-2.*C4**2*S5*C5*I6YZ*S6+2.*C4**2 1924 . *S5*C5*I6XZ*C6-C4**2*C5**2*D6**2*M6-2.*C4* *2*C5**2*06*Z6*M6-2 . * 1925 . C4**2*C5**2*I6XY*C6*S6+C4**2*C5**2*I6XX*S6**2-C4**2*C5**2*I6XX-1926 . C4**2*C5**2*I6YY*S6**2+C4**2*C5**2*I6ZZ+C4**2*D6**2*M6+2.*C4**2* 1927 . 06*Z6*M6-2.*C4**2*I6XY*C6*S6+C4**2*I6XX*S6**2-C4**2*I6YY*S6**2+ 1928 . C4**2*I6YY-C4**2*I6ZZ+2.*S5*C5*D6*Y6*M6*S6-2.*S5*C5*D6*M6*X6*C6+ 1929 . 2.*S5*C5*I6YZ*S6-2.*S5*C5*I6XZ*C6+C5**2*D6**2*M6+2.*C5**2*D6*Z6* 1930. . M6+2.*C5**2*I6XY*C6*S6-C5**2*I6XX*S6**2+C5**2*I6XX+C5**2*I6YY*S6 1931 . **2-C5**2*I6ZZ+I6ZZ 1932 C 1933 D22=D22A+D22B+D22C+D22D+022E 1934 C 1935 D23A=A2*Z3*M3*S3+A2*M3*X3*C3+I3YY 1936 C 1937 023B=A2*D4*M4*S3-A2*S4*Z4*M4*C3+A2*C4*M4*X4*C3-A2*Y4*M4*S3+D4**2* 1938 . M4-2.*D4*Y4*M4-2.*S4*C4*I4XZ-C4**2*I4XX+C4**2*I4ZZ+I4XX 1939 C 1940 D23C=A2*04*M5*S3-A2*S4*Y5*M5*C3+A2*C4*S5*Z5*M5*C3+A2*C4*C5*M5*X5* 1941 . C3-A2*S5*M5*X5*S3+A2*C5*Z5*M5*S3+D4**2*M5-2.*D4*S5*M5*X5+2.*D4* 1942 . C5*Z5*M5-2.*S4*C4*S5*I5YZ-2.*S4*C4*C5*I5XY+2.*C4**2*S5*C5*I5XZ-1943 . C4**2*C5**2*I5XX+C4**2*C5**2*I5ZZ+C4**2*I5YY-C4**2*I5ZZ-2.*S5*C5 1944 . *I5XZ+C5**2*I5XX-C5**2*I5ZZ+I5ZZ 1945 C 1946 D23D=A2*D4*S3*M6-A2*S4*Y6*C3*M6*C6-A2*S4*C3*M6*X6*S6+A2*C4*S5*D6* 1947 . C3*M6+A2*C4*S5*Z6*C3*M6-A2*C4*C5*Y6*C3*M6*S6+A2*C4*C5*C3*M6*X6* 1948 . C6+A2*S5*Y6*S3*M6*S6-A2*S5*S3*M6*X6*C6+A2*C5*D6*S3*M6+A2*C5*Z6* 1949 . S3*M6+D4**2*M6+2.*D4*S5*Y6*M6*S6-2.*D4*S5*M6*X6*C6+2.*D4*C5*D6* 1950 . M6+2.*D4*C5*Z6*M6-2.*S4*C4*S5*D6*Y6*M6*C6-2.*S4*C4*S5*D6*M6*X6* 1951 . S6-2.*S4*C4*S5*I6YZ*C6-2.*S4*C4*S5*I6XZ*S6+4.*S4*C4*C5*I6XY*S6** 1952 . 2-2.*S4*C4*C5*I6XY + 2 . *S4*C4*C5*16XX*C6*S6-2.*S4*C4*C5*16YY*C6*S6 1953 . -2.*C4**2*S5*C5*D6*Y6*M6*S6+2.*C4**2*S5*C5*D6*M6*X6*C6-2.*C4**2* 1954 . S5*C5*I6YZ*S6+2 . *C4**2*S5*C5*I6XZ*C6-C4**2*C5**2*D6**2*M6-2.*C4 1955 . **2*C5**2*D6*Z6*M6-2.*C4**2*C5**2*I6XY*C6*S6+C4**2*C5**2*I6XX*S6 1956 . **2-C4**2*C5**2*I6XX-C4**2*C5**2*I6YY*S6**2+C4**2*C5**2*I6ZZ+C4 1957 . **2*D6**2*M6+2 . *C4**2*D6*Z6*M6-2.*C4**2*I6XY*C6*S6+C4**2*I6XX*S6 1958 . **2-C4**2*I6YY*S6**2+C4**2*I6YY-C4**2*I6ZZ+2.*S5*C5*D6*Y6*M6*S6-1959 . 2.*S5*C5*D6*M6*X6*C6+2.*S5*C5*I6YZ*S6-2.*S5*C5*I6XZ*C6+C5**2*D6** 1960 . 2*M6+2.*C5**2*D6*Z6*M6+2.*C5**2*I6XY*C6*S6-C5**2*I6XX*S6**2+C5** 1961 . 2*I6XX+C5**2*I6YY*S6**2-C5**2*I6ZZ+I6ZZ 1962 C 1963 D23=D23A+D23B+023C+D23D 1964 C 1965 D33A=I3YY 1S66 C 1967 D33B=-2.*I4XZ*C4*S4-I4ZZ*S4**2+I4ZZ+I4XX*S4**2-2.*Y4*M4*D4+M4*D4 1968 . **2 1969 C 1970 D33C=-2.*I5XZ*C5*S5*S4**2-2.*I5XY*C5*C4*S4-2.*I5YZ*S5*C4*S4+I5ZZ* 1971 . S5**2*S4**2-I5YY*S4**2+I5YY-I5XX*S5**2*S4**2+I5XX*S4**2+2.*Z5*C5 1972 . *M5*D4-2.*X5*S5*M5*D4+M5*D4**2 169. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 35 1973 C 1974 D33D=-2.*I6XZ*S6*S5*C4*S4-2. *I6XZ*C6*C5*S5*S4**2+2.*I6YZ*SS*C5*S5 1975 . *S4**2-2. *I6YZ*C6*S5*C4*S4+I6ZZ*S5**2*S4**2-2.*I6YY*S6*C6*C5*C4* 1976 . S4+I6YY*C6**2*S5**2*S4**2-2.*I6YY*C6**2*S4**2+I6YY*C6**2-I6YY*S5 1977 . **2*S4**2+I6YY*S4**2+2.*I6XX*S6*C6*C5*C4*S4-I6XX*C6**2*S5**2*S4 1978 . **2+2.*I6XX*C6**2*S4**2-I6XX*C6**2-I6XX*S4**2+I6XX-2.*I6XY*S6*C6 1979 . *S5**2*S4**2+4.*I6XY*S6*C6*S4**2-2.*I6XY*S6*C6-4.*I6XY*C6**2*C5* 1980 . C4*S4+2.*I6XY*C5*C4*S4-2.*Z6*D6*M6*S5**2*S4**2+2.*Z6*D6*M6+2.*Z6 1981 . *M6*C5*D4+2.*Y6*S6*D6*M6*C5*S5*S4**2+2.*Y6*S6*M6*S5*D4-2.*Y6*C6* 1982 . D6*M6*S5*C4*S4-2.*S6*X6*D6*M6*S5*C4*S4-2.*X6*C6*D6*M6*C5*S5*S4** 1983 . 2-2.*X6*C6*M6*S5*D4-D6**2*M6*S5**2*S4**2+D6**2*M6+2.*D6*M6*C5*D4 1984 . +M6*D4**2 1985 C 1986 D33=D33A+D33B+D33C+D33D 1987 1988 C 1989 RETURN 1990 END 1991 C 1992 c****************************************************** 1993 C 1994 SUBROUTINE GRAV(G1,G2,G3) 1995 REAL M1,M2,M3,M4,M5,M6 1996 C0MM0N/BL0CK3/D2,A2,D4,D6,M1,M2,M3,M4,M5,M6,GX,GY,GZ, 1997 X1,X2,X3,X4,X5,X6,Y1,Y2,Y3,Y4,Y5,Y6,Z1,Z2,Z3,Z4,Z5,Z6 1998 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 1999 C 2000 G1A=M1*(C1*Z1*GX-C1*X1*GY+S1*Z1*GY+S1*X1*GX) 2001 C 2002 G1B=M2*(-A2*C1*GY*C2+A2*S1*GX*C2+C1*Z2*GX-C1*X2*GY*C2+C1*Y2*GY*S2 2003 . +C1*GX*D2+S1*Z2*GY+S1*X2*GX*C2-S1*Y2*GX*S2+S1*GY*D2) 2004 C 2005 G1C=M3*(-A2*C1*GY*C2+A2*S1*GX*C2-C1*Z3*C3*GY*S2-C1*Z3*S3*GY*C2+C1 2006 . *Y3*GX-C1*X3*C3*GY*C2+C1*X3*S3*GY*S2+C1*GX*D2+S1*Z3*C3*GX*S2+S1* 2007 . Z3*S3*GX*C2+S1*Y3*GY+S1*X3*C3*GX*C2-S1*X3*S3*GX*S2+S1*GY*D2) 2008 C 2009 G1D=M4*(-A2*C1*GY*C2+A2*S1*GX*C2+C1*Z4*C3*GY*C2*S4-C1*Z4*S3*GY*S2 2010 . *S4+C1*Z4*GX*C4+C1*Y4*C3*GY*S2+C1*Y4*S3*GY*C2-C1*X4*C3*GY*C2*C4+ 201 1 . C1*X4*S3*GY*S2*C4+C1*X4*GX*S4-C1*C3*GY*S2*D4-C1*S3*GY*C2*D4+C1* 2012 . GX*D2-S1*Z4*C3*GX*C2*S4+S1*Z4*S3*GX*S2*S4+S1*Z4*GY*C4-S1*Y4*C3* 2013 . GX*S2-S1*Y4*S3*GX*C2+S1*X4*C3*GX*C2*C4-S1*X4*S3*GX*S2*C4+S1*X4* 2014 . GY*S4+S1*C3*GX*S2*D4+S1*S3*GX*C2*D4+S1*GY*D2) 2015 C 2016 G1E=M5*(-A2*C1*GY*C2+A2*S1*GX*C2-C1*C3*Z5*GY*S2*C5-C1*C3*Z5*GY*C2 2017 . *S5*C4+C1*C3*Y5*GY*C2*S4+C1*C3*X5*GY*S2*S5-C1*C3*X5*GY*C2*C5*C4-2018 . C1*C3*GY*S2*D4+C1*S3*Z5*GY*S2*S5*C4-C1*S3*Z5*GY*C2*C5-C1*S3*Y5* 2019 . GY*S2*S4+C1*S3*X5*GY*S2*C5*C4+C1*S3*X5*GY*C2*S5-C1*S3*GY*C2*D4+ 2020 . C1*Z5*GX*S5*S4+C1*Y5*GX*C4+C1*X5*GX*C5*S4+C1*GX»1D2+S1*C3*Z5*GX* 2021 . S2*C5+S1*C3*Z5*GX*C2*S5*C4-S1*C3*Y5*GX*C2*S4-S1*C3*X5*GX*S2*S5+ 2022 . S1*C3*X5*GX*C2*C5*C4+S1*C3*GX*S2*D4-S1*S3*Z5*GX*S2*S5*C4+S1*S3* 2023 . Z5*GX*C2*C5+S1*S3*Y5*GX*S2*S4-S1*S3*X5*GX*S2*C5*C4-S1*S3*X5*GX* 2024 . C2*S5+S1*S3*GX*C2*D4+S1*Z5*GY*S5*S4+S1*Y5*GY*C4+S1*X5*GY*C5*S4+ 2025 . S1*GY*D2) 2026 C 2027 G1F=M6*(-A2*C1*GY*C2+A2*S1*GX*C2-C1*D6*C3*GY*S2*C5-C1*D6*C3*GY*C2 2028 . *S5*C4+C1*D6*S3*GY*S2*S5*C4-C1*D6*S3*GY*C2*C5+C1*D6*GX*S5*S4-C1* 2029 . C3*Z6*GY*S2*C5-C1*C3*Z6*GY*C2*S5*C4+C1*C3*Y6*GY*C6*C2*S4-C1*C3* 2030 . Y6*GY*S6*S2*S5+C1*C3*Y6*GY*S6*C2*C5*C4+C1*C3*X6*GY*C6*S2*S5-C1* 170. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 36 2031 . C3*X6*GY*C6*C2*C5*C4+C1*C3*X6*GY*S6*C2*S4-C1*C3*GY*S2*04+C1*S3* 2032 . Z6*GY*S2*S5*C4-C1*S3*Z6*GY*C2*C5-C1*S3*Y6*GY*C6*S2*S4-C1*S3*Y6* 2033 . GY*S6*S2*C5*C4-C1*S3*Y6*GY*S6*C2*S5+C1*S3*X6*GY*C6*S2*C5*C4+C1* 2034 . S3*X6*GY*C6*C2*S5-C1*S3*X6*GY*S6*S2*S4-C1*S3*GY*C2*D4+C1*Z6*GX* 2035 . S5*S4+C1*Y6*GX*C6*C4-C1*Y6*GX*S6*C5*S4+C1*X6*GX*C6*C5*S4+C1*X6* 2036 . GX*S6*C4+C1*GX*D2+S1*D6*C3*GX*S2*C5+S1*D6*C3*GX*C2*S5*C4-S1*D6* 2037 . S3*GX*S2*S5*C4+S1*D6*S3*GX*C2*C5+S1*D6*GY*S5*S4+S1*C3*Z6*GX*S2* 2038 . C5+S1*C3*Z6*GX*C2*S5*C4-S1*C3*Y6*GX*C6*C2*S4+S1*C3*Y6*GX*S6*S2* 2039 . S5-S1*C3*Y6*GX*S6*C2*C5*C4-S1*C3*X6*GX*C6*S2*S5+S1*C3*X6*GX*C6* 2040 . C2*C5*C4-S1*C3*X6*GX*S6*C2*S4+S1*C3*GX*S2*D4-S1*S3*Z6*GX*S2*S5* 2041 . C4+S1*S3*Z6*GX*C2*C5+S1*S3*Y6*GX*C6*S2*S4+S1*S3*Y6*GX*S6*S2*C5* 2042 . C4+S1*S3*Y6*GX*S6*C2*S5-S1*S3*X6*GX*C6*S2*C5*C4-S1*S3*X6*GX*C6* 2043 . C2*S5+S1*S3*X6*GX*S6*S2*S4+S1*S3*GX*C2*D4+S1*Z6*GY*S5*S4+S1*Y6* 2044 . GY*C6*C4-S1*Y6*GY*S6*C5*S4+S1*X6*GY*C6*C5*S4+S1*X6*GY*S6*C4+S1* 2045 . GY*D2) 2046 C 2047 G1=G1A+G1B+G1C+G1D+G1E+G1F 2048 C 2049 G2A=M2*(A2*C1*GX*S2+A2*S1*GY*S2+A2*GZ*C2+C1*X2*GX*S2+C1*Y2*GX*C2+ 2050 . S1*X2*GY*S2+S1*Y2*GY*C2+X2*GZ*C2-Y2*GZ*S2) 2051 C 2052 G2B=M3*(A2*C1*GX*S2+A2*S1*GY*S2+A2*GZ*C2-C1*Z3*C3*GX*C2+C1*Z3*S3* 2053 . GX*S2+C1*X3*C3*GX*S2+C1*X3*S3*GX*C2-S1*Z3*C3*GY*C2+S1*Z3*S3*GY* 2054 . S2+S1*X3*C3*GY*S2+S1*X3*S3*GY*C2+Z3*C3*GZ*S2+Z3*S3*GZ*C2+X3*C3* 2055 . GZ*C2-X3*S3*GZ*S2) 2056 C 2057 G2C=M4*(A2*C1*GX*S2+A2*S1*GY*S2+A2*GZ*C2-C1*Z4*C3*GX*S2*S4-C1*Z4* 2058 . S3*GX*C2*S4+C1*Y4*C3*GX*C2-C1*Y4*S3*GX*S2+C1*X4*C3*GX*S2*C4+C1* 2059 . X4*S3*GX*C2*C4-C1*C3*GX*C2*D4+C1»S3*GX*S2*D4-S1*Z4*C3*GY*S2*S4-2060 . S1*Z4*S3*GY*C2*S4+S1*Y4*C3*GY*C2-S1*Y4*S3*GY*S2+S1*X4*C3*GY*S2* 2061 . C4+S1*X4*S3*GY*C2*C4-S1*C3*GY*C2*D4+S1*S3*GY*S2*D4-Z4*C3*GZ*C2* 2062 . S4+Z4*S3*GZ*S2*S4-Y4*C3*GZ*S2-Y4*S3*GZ*C2+X4*C3*GZ*C2*C4-X4*S3* 2063 . GZ*S2*C4+C3*GZ*S2*D4+S3*GZ*C2*D4) 2064 C 2065 G2D=M5*(A2*C1*GX*S2+A2*S1*GY*S2+A2*GZ*C2+C1*C3*Z5*GX*S2*S5*C4-C1* 2066 . C3*Z5*GX*C2*C5-C1*C3*Y5*GX*S2*S4+C1*C3*X5*GX*S2*C5*C4+C1*C3*X5* 2067 . GX*C2*S5-C1*C3*GX*C2*D4+C1*S3*Z5*GX*S2*C5+C1*S3*Z5*GX*C2*S5*C4-2068 . C1*S3*Y5*GX*C2*S4-C1*S3*X5*GX*S2*S5+C1*S3*X5*GX*C2*C5*C4+C1*S3* 2069 . GX*S2*D4+S1*C3*Z5*GY*S2*S5*C4-S1*C3*Z5*GY*C2*C5-S1*C3*Y5*GY*S2* 2070 . S4+S1*C3*X5*GY*S2*C5*C4+S1*C3*X5*GY*C2*S5-S1*C3*GY*C2*D4+S1*S3* 2071 . Z5*GY*S2*C5+S1*S3*Z5*GY*C2*S5*C4-S1*S3*Y5*GY*C2*S4-S1*S3*X5*GY* 2072 . S2*S5+S1*S3*X5*GY*C2*C5*C4+S1*S3*GY*S2*D4+C3*Z5*GZ*S2*C5+C3*Z5* 2073 . GZ*C2*S5*C4-C3*Y5*GZ*C2*S4-C3*X5*GZ*S2*S5+C3*X5*GZ*C2*C5*C4+C3* 2074 . GZ*S2*D4-S3*Z5*GZ*S2*S5*C4+S3*Z5*GZ*C2*C5+S3*Y5*GZ*S2*S4-S3*X5* 2075 . GZ*S2*C5*C4-S3*X5*GZ*C2*S5+S3*GZ*C2*D4) 2076 C 2077 G2E=M6*(A2*C1*GX*S2+A2*S1*GY*S2+A2*GZ*C2+C1*D6*C3*GX*S2*S5*C4-C1* 2078 . D6*C3*GX*C2*C5+C1*D6*S3*GX*S2*C5+C1*D6*S3*GX*C2*S5*C4+C1*C3*Z6* 2079 . GX*S2*S5*C4-C1*C3*Z6*GX*C2*C5-C1*C3*Y6*GX*C6*S2*S4-C1*C3*Y6*GX* 2080 . S6*S2*C5*C4-C1*C3*Y6*GX*S6*C2*S5+C1*C3*X6*GX*C6*S2*C5*C4+C1*C3* 2081 . X6*GX*C6*C2*S5-C1*C3*X6*GX*S6*S2*S4-C1*C3*GX*C2*04+C1*S3*Z6*GX* 2082 . S2*C5+C1*S3*Z6*GX*C2*S5*C4-C1*S3*Y6*GX*C6*C2*S4+C1*S3*Y6*GX*S6* 2083 . S2*S5-C1*S3*Y6*GX*S6*C2*C5*C4-C1*S3*X6*GX*C6*S2*S5+C1*S3*X6*GX* 2084 . C6*C2*C5*C4-C1*S3*X6*GX*S6*C2*S4+C1*S3*GX*S2*D4+S1*D6*C3*GY*S2* 2085 . S5*C4-S1*D6*C3*GY*C2*C5+S1*D6*S3*GY*S2*C5+S1*D6*S3*GY*C2*S5*C4+ 2086 . S1*C3*Z6*GY*S2*S5*C4-S1*C3*Z6*GY*C2*C5-S1*C3*Y6*GY*C6*S2*S4-S 1 * 2087 . C3*Y6*GY*S6*S2*C5*C4-S1*C3*Y6*GY*S6*C2*S5+S1*C3*X6*GY*C6*S2*C5* 2088 . C4+S1*C3*X6*GY*C6*C2*S5-S1*C3*X6*GY*S6*S2*S4-S1*C3*GY*C2*D4+S1* 171. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 37 2089 . S3*Z6*GY*S2*C5+S1*S3*Z6*GY*C2*S5*C4-S1*S3*Y6*GY*C6*C2*S4+S1*S3* 2090 . Y6*GY*S6*S2*S5-S1*S3*Y6*GY*S6*C2*C5*C4-S1*S3*X6*GY*C6*S2*S5+S1* 2091 . S3*X6*GY*C6*C2*C5*C4-S1*S3*X6*GY*S6*C2*S4+S1*S3*GY*S2*D4+D6*C3* 2092 . GZ*S2*C5+DG*C3*GZ*C2*S5*C4-06*S3*GZ*S2*S5*C4+D6*.S3*GZ*C2*C5+C3* 2093 . Z6*GZ*S2*C5+C3*Z6*GZ*C2*S5*C4-C3*Y6*GZ*C6*C2*S4+C3*Y6*GZ*S6*S2* 2094 . S5-C3*Y6*GZ*S6*C2*C5*C4-C3*X6*GZ*C6*S2*S5+C3*X6*GZ*C6*C2*C5*C4-2095 . C3*X6*GZ*S6*C2*S4+C3*GZ*S2*D4-S3*Z6*GZ*S2*S5*C4+S3*Z6*GZ*C2*C5+ 2096 . S3*Y6*GZ*C6*S2*S4+S3*Y6*GZ*S6*S2*C5*C4+S3*YG*GZ*S6*C2*S5) 2097 C TOO MANY CHARACTERS IN BLOCK FOR VAX. BREAK INTO 2. NOTE G3D HAS 21 2098 C LINES ALSO BUT IT HAS FEWER CHARACTERS AND THUS WORKS OK. 2099 G2E=G2E+M6*(-S3*XG* 2100 . GZ*C6*S2*C5*C4-S3*X6*GZ*C6*C2*S5+S3*X6*GZ*S6*S2*S4+S3*GZ*C2*D4) 2101 C 2102 G2=G2A+G2B+G2C+G2D+G2E 2103 C 2104 G3A=M3*(-C1*Z3*C3*GX*C2+C1*Z3*S3*GX*S2+C1*X3*C3*GX*S2+C1*X3*S3*GX 2105 . *C2-S1*Z3*C3*GY*C2+S1*Z3*S3*GY*S2+S1*X3*C3*GY*S2+S1*X3*S3*GY*C2+ 2106 . Z3*C3*GZ*S2+Z3*S3*GZ*C2+X3*C3*GZ*C2-X3*S3*GZ*S2) 2107 C 2108 G3B=M4*(-C1*Z4*C3*GX*S2*S4-C1*Z4*S3*GX*C2*S4+C1*Y4*C3*GX*C2-C1*Y4 2109 . *S3*GX*S2+C1*X4*C3*GX*S2*C4+C1*X4*S3*GX*C2*C4-C1*C3*GX*C2*D4+C1* 2110 . S3*GX*S2*D4-S1*Z4*C3*GY*S2*S4-S1*Z4*S3*GY*C2*S4+S1*Y4*C3*GY*C2-2111 . S1*Y4*S3*GY*S2+S1*X4*C3*GY*S2*C4+S1*X4*S3*GY*C2*C4-S1*C3*GY*C2* 2112 . D4+S1*S3*GY*S2*D4-Z4*C3*GZ*C2*S4+Z4*S3*GZ*S2*S4-Y4*C3*GZ*S2-Y4* 2113 . S3*GZ*C2+X4*C3*GZ*C2*C4-X4*S3*GZ*S2*C4+C3*GZ*S2*D4+S3*GZ*C2*D4) 2114 C 2115 G3C=M5*(C1*C3*Z5*GX*S2*S5*C4-C1*C3*Z5*GX*C2*C5-C1*C3*Y5*GX*S2*S4+ 2116 . C1*C3*X5*GX*S2*C5*C4+C1*C3*X5*GX*C2*S5-C1*C3*GX*C2*D4+C1*S3*Z5* 2117 . GX*S2*C5+C1*S3*Z5*GX*C2*S5*C4-C1*S3*Y5*GX*C2*S4-C1*S3*X5*GX*S2* 2118 . S5+C1*S3*X5*GX*C2*C5*C4+C1*S3*GX*S2*D4+S1*C3*Z5*GY*S2*S5*C4-S1* 2119 . C3*Z5*GY*C2*C5-S1*C3*Y5*GY*S2*S4+S1*C3*X5*GY*S2*C5*C4+S1*C3*X5* 2120 . GY*C2*S5-S1*C3*GY*C2*D4+S1*S3*Z5*GY*S2*C5+S1*S3*Z5*GY*C2*S5*C4-2121 . S1*S3*Y5*GY*C2*S4-S1*S3*X5*GY*S2*S5+S1*S3*X5*GY*C2*C5*C4+S1*S3* 2122 . GY*S2*D4+C3*Z5*GZ*S2*C5+C3*Z5*GZ*C2*S5*C4-C3*Y5*GZ*C2*S4-C3*X5* 2123 . GZ*S2*S5+C3*X5*GZ*C2*C5*C4+C3*GZ*S2*D4-S3*Z5*GZ*S2*S5*C4+S3*Z5* 2124 . GZ*C2*C5+S3*Y5*GZ*S2*S4-S3*X5*GZ*S2*C5*C4-S3*X5*GZ*C2*S5+S3*GZ* 2125 . C2*D4) 2126 C 2127 G3D=M6*(C1*D6*C3*GX*S2*S5*C4-C1*D6*C3*GX*C2*C5+C1*D6*S3*GX*S2*C5+ 2128 . C1*D6*S3*GX*C2*S5*C4+C1*C3*Z6*GX*S2*S5*C4-C1*C3*Z6*GX*C2*C5-C1* 2129 . C3*Y6*GX*C6*S2*S4-C1*C3*Y6*GX*S6*S2*C5*C4-C1*C3*Y6*GX*S6*C2*S5+ 2130 . C1*C3*X6*GX*C6*S2*C5*C4+C1*C3*X6*GX*C6*C2*S5-C1*C3*X6*GX*S6*S2* 2131 . S4-C1*C3*GX*C2*D4+C1*S3*Z6*GX*S2*C5+C1*S3*Z6*GX*C2*S5*C4-C1*S3* 2132 . Y6*GX*C6*C2*S4+C1*S3*Y6*GX*S6*S2*S5-C1*S3*Y6*GX*S6*C2*C5*C4-C1* 2133 . S3*X6*GX*C6*S2*S5+C1*S3*X6*GX*C6*C2*C5*C4-C1*S3*X6*GX*S6*C2*S4+ 2134 . C1*S3*GX*S2*D4+S1*D6*C3*GY*S2*S5*C4-S1*D6*C3*GY*C2*C5+S1*D6*S3* 2135 . GY*S2*C5+S1*D6*S3*GY*C2*S5*C4+S1*C3*Z6*GY*S2*S5*C4-S1*C3*Z6*GY* 2136 . C2*C5-S1*C3*Y6*GY*C6*S2*S4-S1*C3*Y6*GY*S6*S2*C5*C4-S1*C3*Y6*GY* 2137 . S6*C2*S5+S1*C3*X6*GY*C6*S2*C5*C4+S1*C3*X6*GY*CG*C2*S5-S1*C3*X6* 2138 . GY*S6*S2*S4-S1*C3*GY*C2*D4+S1*S3*Z6*GY*S2*C5+S1*S3*Z6*GY*C2*S5* 2139 . C4-S1*S3*Y6*GY*C6*C2*S4+S1*S3*Y6*GY*S6*S2*S5-S1*S3*Y6*GY*S6*C2* 2140 . C5*C4-S1*S3*X6*GY*C6*S2*S5+S1*S3*X6*GY*C6*C2*C5*C4-S1*S3*X6*GY* 2141 . S6*C2*S4+S1*S3*GY*S2*D4+D6*C3*GZ*S2*C5+D6*C3*GZ*C2*S5*C4-D6*S3* 2142 . GZ*S2*S5*C4+D6*S3*GZ*C2*C5+C3*Z6*GZ*S2*C5+C3*Z6*GZ*C2*S5*C4-C3* 2143 . Y6*GZ*C6*C2*S4+C3*Y6*GZ*S6*S2*S5-C3*Y6*GZ*S6*C2*C5*C4-C3*X6*GZ* 2144 . C6*S2*S5+C3*X6*GZ*C6*C2*C5*C4-C3*X6*GZ*S6*C2*S4+C3*GZ*S2*D4-S3* 2145 . Z6*GZ*S2*S5*C4+S3*Z6*GZ*C2*C5+S3*Y6*GZ*C6*S2*S4+S3*Y6*GZ*SG*S2* 2146 . C5*C4+S3*Y6*GZ*S6*C2*S5-S3*X6*GZ*C6*S2*C5*C4-S3*X6*GZ*C6*C2*S5+ 172. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 38 2147 . S3*X6*GZ*S6*S2*S4+S3*GZ*C2*D4) 2148 C 2149 G3=G3A+G3B+G3C+G3D 2150 C 2151 RETURN 2152 END 2153 C 2154 C***************************************************** 2155 C 2156 SUBROUTINE CORIOL(D112,D113,D123,D223) 2157 REAL M1,M2,M3,M4,M5,M6, 2158 11XX,11YY,11ZZ,I1XY,I1XZ,I 1YZ,12XX,12YY,12ZZ,12XY,12XZ,12YZ, 2159 I3XX,I3YY,13ZZ,I3XY,13XZ,13YZ,I4XX,I4YY.I4ZZ,I4XY,I4XZ, I4YZ, 2160 I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ 2161 C0MM0N/BL0CK2/ 2162 11XX,I1YY,11ZZ,I1XY,I1XZ,I 1YZ,12XX,12YY,I2ZZ,I2XY,I2XZ,I2YZ, 2163 I3XX,I3YY,I3ZZ,13XY,I3XZ,13YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ , I4YZ, 2164 I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,I6YZ 2165 C0MM0N/BL0CK3/D2,A2,D4,D6,M1,M2,M3,M4,M5,M6,GX,GY,GZ, 2166 X1 ,X2,X3,X4,X5,X6,Y1 , Y2,Y3,Y4,Y5,Y6,Z1,Z2,Z3,Z4,Z5,Z6 2167 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 2168 C 2 169 D112A = -A2**2*M2*S2*C2-2.*A2*X2*M2*S2*C2-2.*A2*Y2*M2*C2**2+A2*Y2* 2170 . M2-I2YY*S2*C2+I2XX*S2*C2-2.*I2XY*C2**2+I2XY 2171 C 2172 D112B=-A2**2*M3*S2*C2-2.*A2*Z3*M3*S3*S2*C2+2.*A2*Z3*M3*C2**2*C3-2173 . A2*Z3*M3*C3-2.*A2*X3*M3*S3*C2**2+A2*X3*M3*S3-2.*A2*X3*M3*S2*C2* 2174 . C3-2.*I3ZZ*S3*C2**2*C3+I3ZZ*S3*C3-2.*I3ZZ*S2*C2*C3**2+I3ZZ*S2*C2 2175 . +2.*I3XX*S3*C2**2*C3-I3XX*S3*C3+2.* 13XX*S2*C2*C3**2-13XX*S2*C2-2176 . 4.*I3XZ*S3*S2*C2*C3+4 . * 13XZ*C2**2*C3**2-2.* 13XZ*C2**2-2.*I3XZ*C3 2177 . **2+I3XZ 2178 C 2179 ANS1=2.*M4*Z4*D4*S4*C2**2+2.*M4*Z4*D4*S4*C3**2-M4*Z4*D4*S4+2.*M4* 2180 . D4**2*S3*C2**2*C3-M4*D4**2*S3*C3+2.*M4*D4**2*S2*C2*C3**2-M4*D4* * 2181 . 2*S2*C2 2182 D112C=-A2**2*M4*S2*C2+2.*A2*Y4*M4*S3*S2*C2-2.*A2*Y4*M4*C2**2*C3+ 2183 . A2*Y4*M4*C3-2.*A2*X4*M4*S3*C4*C2**2+A2*X4*M4*S3*C4-2.*A2*X4*M4* 2184 . S2*C4*C2*C3+2.*A2*M4*Z4*S3*S4*C2**2-A2*M4*Z4*S3*S4+2.*A2*M4*Z4* 2185 . S2*S4*C2*C3-2.*A2*M4*D4*S3*S2*C2+2.*A2*M4*D4*C2**2*C3-A2*M4*D4* 2186 . C3-4.*I4YZ*S3*S2*S4*C2*C3+4.* 14YZ*S4*C2**2*C3**2-2.* 14YZ*S4*C2* * 2187 . 2-2.*I4YZ*S4*C3**2+I4YZ*S4-4.*Y4*M4*D4*S3*C2**2*C3+2.*Y4*M4*D4* 2188 .S3*C3-4.*Y4*M4*D4*S2*C2*C3**2+2.*Y4*M4*D4*S2*C2+4.*I4XY*S3*S2*C4 2189 . *C2*C3-4.*I4XY*C4*C2**2*C3**2+2.*I4XY*C4*C2**2+2.*I4XY*C4*C3**2-2190 . I4XY*C4-2.*I4ZZ*S3*C4**2*C2**2*C3+I4ZZ*S3*C4**2*C3+2.*I4ZZ*S3*C2 2191 . **2*C3-I4ZZ*S3*C3-2.*I4ZZ*S2*C4**2*C2*C3**2+I4ZZ*S2*C4**2*C2+2 . * 2192 . I4ZZ*S2*C2*C3**2-I4ZZ*S2*C2-2.*I4YY*S3*C2**2*C3+I4YY*S3*C3-2.* 2193 . I4YY*S2*C2*C3**2+I4YY*S2*C2+2.* 14XX*S3*C4**2*C2**2*C3-14XX*S3*C4 2194 . **2*C3+2.*I4XX*S2*C4**2*C2*C3**2-I4XX*S2*C4**2*C2+4.*I4XZ*S3*S4* 2195 . C4*C2**2*C3-2.*I4XZ*S3*S4*C4*C3+4.*I4XZ*S2*S4*C4*C2*C3**2-2 . * 2196 . I4XZ*S2*S4*C4*C2-4 . *X4*M4*D4*S3*S2*C4*C2*C3+4.*X4*M4*D4*C4*C2**2 2197 . *C3**2-2.*X4*M4*D4*C4*C2**2-2.*X4*M4*D4*C4*C3**2+X4*M4*D4*C4+4 . * 2198 . M4*Z4*D4*S3*S2*S4*C2*C3-4.*M4*Z4*D4*S4*C2**2*C3**2+ANS1 2199 C 2200 ANS2=-4.*I5XX*C4*C5*S3*C2*S2*C3*S5-4.*I5XX*C4*C5*S2**2*C3**2*S5+ 2201 . 2.*I5XX*C4*C5*S2**2*S5+2.*I5XX*C4*C5*C3**2*S5-I5XX*C4*C5*S5+2.* 2202 . I5XX*S3*S2**2*C3*S5**2-I5XX*S3*C3*S5**2-2.*I5XX*C2*S2*C3**2*S5** 2203 . 2+I5XX*C2*S2*S5**2+4.*I5XZ*C4**2*C5*S3*S2**2*C3*S5-2.*I5XZ*C4**2 2204 . *C5*S3*C3*S5-4.*I5XZ*C4**2*C5*C2*S2*C3**2*S5+2.*I5XZ*C4**2*C5*C2 173. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 39 2205 . *S2*S5+8.*I5XZ*C4*S3*C2*S2*C3*S5**2-4.*I5XZ*C4*S3*C2*S2*C3+8.* 2206 . I5XZ*C4*S2**2*C3**2*S5**2-4.*I5XZ*C4*S2**2*C3**2-4.*I5XZ*C4*S2** 2207 . 2*S5**2+2.* 15XZ*C4*S2**2-4. *I5XZ*C4*C3**2*S5**2 + 2.*I5XZ*C4*C3**2 2208 . +2.*I5XZ*C4*S5**2-I5XZ*C4+4.*I5XZ*C5*S3*S2**2*C3*S5-2.*I5XZ*C5* 2209 . S3*C3*S5-4.*I5XZ*C5*C2*S2*C3**2*S5+2.*I5XZ*C5*C2*S2*S5 2210 ANS1=4.*D4*X5*M5*S3*S2**2*C3*S5-2.*D4*X5*M5*S3*C3*S5-4.*D4*X5*M5* 2211 . C2*S2*C3**2*S5+2.*D4*X5*M5*C2*S2*S5-4.*I5XY*S4*C4*C5*S3*S2**2*C3 2212 . +2.*I5XY*S4*C4*C5*S3*C3+4.*I5XY*S4*C4*C5*C2*S2*C3**2-2.*I5XY*S4* 2213 . C4*C5*C2*S2-4.*I5XY*S4*S3*C2*S2*C3*S5-4.*I5XY*S4*S2**2*C3**2*S5+ 2214 . 2.*I5XY*S4*S2**2*S5+2.* 15XY*S4*C3**2*S5-15XY*S4*S5-2.*I5ZZ*C4**2* 2215 . S3*S2**2*C3*S5**2+I5ZZ*C4**2*S3*C3*S5**2+2.*I5ZZ*C4**2*C2*S2*C3 2216 . **2*S5**2-I5ZZ*C4**2*C2*S2*S5**2+4.*I5ZZ*C4*C5*S3*C2*S2*C3*S5+4. 2217 . *I5ZZ*C4*C5*S2**2*C3**2*S5-2 . *I5ZZ*C4*C5*S2**2*S5-2.*I5ZZ*C4*C5* 2218 . C3**2*S5+I5ZZ*C4*C5*S5-2.*I5ZZ*S3*S2**2*C3*S5**2+2.*I5ZZ*S3*S2** 2219 . 2*C3+I5ZZ*S3*C3*S5**2-I5ZZ*S3*C3+2.*I5ZZ*C2*S2*C3**2*S5**2-2.* 2220 . I5ZZ*C2*S2*C3**2-I5ZZ*C2*S2*S5**2+I5ZZ*C2*S2+2..*I5YY*C4**2*S3*S2 2221 . **2*C3-I5YY*C4**2*S3*C3-2.*I5YY*C4**2*C2*S2*C3**2+I5YY*C4**2*C2* 2222 . S2-2.*I5YY*S3*S2**2*C3+I5YY*S3*C3+2.*I5YY*C2*S2*C3**2-I5YY*C2*S2 2223 . +2.*I5XX*C4**2*S3*S2**2*C3*S5**2-2.*I5XX*C4**2*S3*S2**2*C3-I5XX* 2224 . C4**2*S3*C3*S5**2+I5XX*C4**2*S3*C3-2.*I5XX*C4**2*C2*S2*C3**2*S5 2225 . **2+2.*I5XX*C4**2*C2*S2*C3**2+I5XX*C4**2*C2*S2*S5**2-I5XX*C4**2* 2226 . C2*S2+ANS2 2227 D112D=-A2**2*M5*C2*S2-2.*A2*Y5*M5*S4*S3*S2**2+A2*Y5*M5*S4*S3+2.* 2228 . A2*Y5*M5*S4*C2*S2*C3+2.*A2*Z5*M5*C4*S3*S2**2*S5-A2*Z5*M5*C4*S3* 2229 . S5-2.*A2*Z5*M5*C4*C2*S2*C3*S5-2.*A2*Z5*M5*C5*S3*C2*S2-2.*A2*Z5* 2230 . M5*C5*S2**2*C3+A2*Z5*M5*C5*C3-2.*A2*D4*M5*S3*C2*S2-2.*A2*D4*M5* 2231 . S2**2*C3+A2*D4*M5*C3+2.*A2*X5*M5*C4*C5*S3*S2**2-A2*X5*M5*C4*C5* 2232 . S3-2.*A2*X5*M5*C4*C5*C2*S2*C3+2.*A2*X5*M5*S3*C2*52*S5+2. *A2*X5* 2233 . M5*S2**2*C3*S5-A2*X5*M5*C3*S5-4.* 15YZ*S4*C4*S3*S2**2*C3*S5+2 . * 2234 . I5YZ*S4*C4*S3*C3*S5+4.*I5YZ*S4*C4*C2*S2*C3**2*S5-2. *I5YZ*S4*C4* 2235 . C2*S2*S5+4.*I5YZ*S4*C5*S3*C2*S2*C3+4.*I5YZ*S4*C5*S2**2*C3**2-2.* 2236 . I5YZ*S4*C5*S2**2-2.*I5YZ*S4*C5*C3**2+I5YZ*S4*C5+4.*Y5*D4*M5*S4* 2237 . S3*C2*S2*C3+4.*Y5*D4*M5*S4*S2**2*C3**2-2.*Y5*D4*M5*S4*S2**2-2.* 2238 . Y5*D4*M5*S4*C3**2+Y5*D4*M5*S4-4.*Z5*D4*M5*C4*S3*C2*S2*C3*S5-4 . * 2239 . Z5*D4*M5*C4*S2**2*C3**2*S5+2.*Z5*D4*M5*C4*S2**2*S5+2.*Z5*D4*M5* 2240 . C4*C3**2*S5-Z5*D4*M5*C4*S5-4.*Z5*D4*M5*C5*S3*S2**2*C3+2.*Z5*D4* 2241 . M5*C5*S3*C3+4.*Z5*D4*M5*C5*C2*S2*C3**2-2.*Z5*D4*M5*C5*C2*S2-2.* 2242 . D4**2*M5*S3*S2**2*C3+D4**2*M5*S3*C3+2.*D4**2*M5*C2*S2*C3**2-D4** 2243 . 2*M5*C2*S2-4.*D4*X5*M5*C4*C5*S3*C2*S2*C3-4.*D4*X5*M5*C4*C5*S2**2 2244 . *C3**2+2.*D4*X5*M5*C4*C5*S2**2+2.*D4*X5*M5*C4*C5*C3**2-D4*X5*M5* 2245 . C4*C5+ANS1 2246 C 2247 ANS8=M6*D6**2*S3*C3-2.*M6*06**2*C2*S2*C3**2*S5**2+2.*M6*D6**2*C2* 2248 . S2*C3**2+M6*D6**2*C2*S2*S5**2-M6*D6**2*C2*S2-4.*I6YZ*S6*C4**2*C5 2249 . *S3*S2**2*C3*S5+2 . *I6YZ*S6*C4**2*C5*S3*C3*S5+4.*I6YZ*S6*C4**2*C5 2250 . *C2*S2*C3**2*S5-2 . *I6YZ*S6*C4**2*C5*C2*S2*S5-8.*I6YZ*S6*C4*S3*C2 2251 . *S2*C3*S5**2+4.*I6YZ*S6*C4*S3*C2*S2*C3-8.*I6YZ*S6*C4*S2**2*C3**2 2252 . *S5**2+4.*I6YZ*S6*C4*S2**2*C3**2+4.*I6YZ*S6*C4*S2**2*S5**2-2 . * 2253 . I6YZ*S6*C4*S2**2+4.*I6YZ*S6*C4*C3**2*S5**2-2.*I6YZ*S6*C4*C3**2-2254 . 2.*I6YZ*S6*C4*S5**2+I6YZ*S6*C4-4.*I6YZ*S6*C5*S3*S2**2*C3*S5+2.* 2255 . I6YZ*S6*C5*S3*C3*S5+4.*I6YZ*S6*C5*C2*S2*C3**2*S5-2.*I6YZ*S6*C5* 2256 . C2*S2*S5-4.* 16YZ*S4*C4*S3*S2**2*C3*C6*S5+2.*I6YZ*S4*C4*S3*C3*C6* 2257 . S5+4.*I6YZ*S4*C4*C2*S2*C3**2*C6*S5-2.* 16YZ*S4*C4*C2*S2*C6*S5+4 . * 2258 . I6YZ*S4*C5*S3*C2*S2*C3*C6+4.*I6YZ*S4*C5*S2**2*C3**2*C6-2.*I6YZ* 2259 . S4*C5*S2**2*C6-2.*I6YZ*S4*C5*C3**2*C6+I6YZ*S4*C5*C6 2260 ANS7=4.*M6*Y6*D6*S6*C4*C3**2*S5**2-2.*M6*Y6*D6*S6*C4*C3**2-2.*M6* 2261 . Y6*D6*S6*C4*S5**2+M6*Y6*D6*S6*C4-4.*M6*Y6*06*S6*C5*S3*S2**2*C3* 2262 . S5+2.*M6*Y6*D6*S6*C5*S3*C3*S5+4.*M6*Y6*D6*S6*C5*C2*S2*C3**2*S5-174. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 40 2263 . 2.*M6*Y6*D6*S6*C5*C2*S2*S5-4.*M6*Y6*D6*S4*C4*S3*S2**2*C3*C6*S5+2. 2264 . *M6*Y6*D6*S4*C4*S3*C3*C6*S5+4.*M6*Y6*D6*S4*C4*C2*S2*C3**2*C6*S5-2265 . 2.*M6*Y6*D6*S4*C4*C2*S2*C6*S5+4.*M6*Y6*D6*S4*C5*S3*C2*S2*C3*C6+4. 2266 . *M6*Y6*D6*S4*C5*S2**2*C3**2*C6-2.*M6*Y6*D6*S4*C5*S2**2*C6-2.*M6* 2267 . Y6*D6*S4*C5*C3**2*C6+M6*Y6*D6*S4*C5*C6-2.*M6*D4**2*S3*S2**2*C3+ 2268 . M6*D4**2*S3*C3+2.*M6*D4**2*C2*S2*C3**2-M6*04**2*C2*S2-4.*M6*D4* 2269 . D6*C4*S3*C2*S2*C3*S5-4.*M6*D4*06*C4*S2**2*C3**2*S5+2.*M6*D4*D6* 2270 . C4*S2**2*S5+2.*M6*04*D6*C4*C3**2*S5-M6*D4*D6*C4*S5-4.*M6*D4*D6* 2271 . C5*S3*S2**2*C3+2.*M6*D4*D6*C5*S3*C3+4.*M6*D4*D6*C5*C2*S2*C3**2-2272 . 2.*M6*D4*D6*C5*C2*S2+2.*M6*D6**2*C4**2*S3*S2**2*C3*S5**2-M6*D6**2 2273 . *C4**2*S3*C3*S5**2-2.*M6*D6**2*C4**2*C2*S2*C3**2*S5**2+M6*D6**2* 2274 . C4**2*C2*S2*S5**2-4.*M6*D6**2*C4*C5*S3*C2*S2*C3*S5-4.*M6*D6**2* 2275 . C4*C5*S2**2*C3**2*S5+2.*M6*D6**2*C4*C5*S2**2*S5+2.*M6*D6**2*C4* 2276 . C5*C3**2*S5-M6*D6**2*C4*C5*S5+2.*M6*D6**2*S3*S2**2*C3*S5**2-2 . * 2277 . M6*D6**2*S3*S2**2*C3-M6*D6**2*S3*C3*S5**2+ANS8 2278 ANS6=4.*M6*Z6*D6*C4**2*S3*S2**2*C3*S5**2-2.*M6*Z6*D6*C4**2*S3*C3* 2279 . S5**2-4.*M6*Z6*D6*C4**2*C2*S2*C3**2*S5**2+2.*M6*Z6*D6*C4**2*C2* 2280 . S2*S5**2-8.*M6*Z6*D6*C4*C5*S3*C2*S2*C3*S5-8.*M6*Z6*D6*C4*C5*S2** 2281 . 2*C3**2*S5+4.*M6*Z6*D6*C4*C5*S2**2*S5+4.*M6*Z6*D6*C4*C5*C3**2*S5 2282 . -2.*M6*Z6*D6*C4*C5*S5+4.*M6*Z6*D6*S3*S2**2*C3*S5**2-4.*M6*Z6*D6* 2283 . S3*S2**2*C3-2.*M6*Z6*D6*S3*C3*S5**2+2.*M6*Z6*06*S3*C3-4.*M6*Z6* 2284 . D6*C2*S2*C3**2*S5**2+4.*M6*Z6*D6*C2*S2*C3**2+2.*M6*Z6*D6*C2*S2* 2285 . S5**2-2.*M6*Z6*06*C2*S2+4.*M6*Y6*D4*S6*C4*C5*S3*C2*S2*C3+4.*M6* 2286 . Y6*04*S6*C4*C5*S2**2*C3**2-2.*M6*Y6*D4*S6*C4*C5*S2* *2-2.*M6*Y6* 2287 . D4*S6*C4*C5*C3**2+M6*Y6*D4*S6*C4*C5-4.*M6*Y6*D4*S6*S3*S2**2*C3* 2288 . S5+2.*M6*Y6*D4*S6*S3*C3*S5+4.*M6*Y6*D4*S6*C2*S2*C3**2*S5-2.*M6* 2289 . Y6*D4*S6*C2*S2*S5+4.*M6*Y6*D4*S4*S3*C2*S2*C3*C6+4.*M6*Y6*D4*S4* 2290 . S2**2*C3**2*C6-2.*M6*Y6*D4*S4*S2**2*C6-2.*M6*Y6*D4*S4*C3**2*C6+ 2291 . M6*Y6*D4*S4*C6-4.*M6*Y6*D6*S6*C4* *2*C5*S3*S2**2*C3*S5+2.*M6*Y6* 2292 . D6*S6*C4**2*C5*S3*C3*S5+4.*M6*Y6*D6*S6*C4**2*C5*C2*S2*C3**2*S5-2293 . 2.*M6*Y6*D6*S6*C4**2*C5*C2*S2*S5-8.*M6*Y6*D6*S6*C4*S3*C2*S2*C3*S5 2294 . **2+4.*M6*Y6*D6*S6*C4*S3*C2*S2*C3-8.*M6*Y6*D6*S6*C4*S2**2*C3**2* 2295 . S5**2+4.*M6*Y6*D6*S6*C4*S2**2*C3**2+4.*M6*Y6*D6*S6*C4*S2**2*S5** 2296 . 2-2.*M6*Y6*D6*S6*C4*S2**2+ANS7 2297 ANS5=2.*X6*M6*D4*C4*C5*C3**2*C6-X6*M6*D4*C4*C5*C6+4.*X6*M6*D4*S3* 2298 . S2**2*C3*C6*S5-2.*X6*M6*D4*S3*C3*C6*S5-4.*X6*M6*D4*C2*S2*C3**2* 2299 . C6*S5+2 . *X6*M6*D4*C2*S2*C6*S5-4.*X6*M6*D6*S6*S4*C4*S3*S2**2*C3* 2300 . S5+2.*X6*M6*D6*S6*S4*C4*S3*C3*S5+4.*X6*M6*D6*S6*S4*C4*C2*S2*C3** 2301 . 2*S5-2.*X6*M6*D6*S6*S4*C4*C2*S2*S5+4.*X6*M6*D6*S6*S4*C5*S3*C2*S2 2302 . *C3+4.*X6*M6*06*S6*S4*C5*S2**2*C3**2-2.*X6*M6*D6*S6*S4*C5*S2**2-2303 . 2.*X6*M6*D6*S6*S4*C5*C3**2+X6*M6*D6*S6*S4*C5+4.*X6*M6*D6*C4**2*C5 2304 . *S3*S2**2*C3*C6*S5-2.*X6*M6*D6*C4**2*C5*S3*C3*C6*S5-4.*X6*M6*D6* 2305 . C4**2*C5*C2*S2*C3**2*C6*S5+2.*X6*M6*D6*C4**2*C5*C2*S2*C6*S5+8.* 2306 . X6*M6*D6*C4*S3*C2*S2*C3*C6*S5**2-4.*X6*M6*D6*C4*S3*C2*S2*C3*C6+ 2307 . 8.*X6*M6*D6*C4*S2**2*C3**2*C6*S5**2-4.*X6*M6*D6*C4*S2**2*C3**2*C6 2308 . -4.*X6*M6*D6*C4*S2**2*C6*S5**2+2.*X6*M6*D6*C4*S2**2*C6-4.*X6*M6* 2309 . D6*C4*C3**2*C6*S5**2+2.*X6*M6*D6*C4*C3**2*C6+2;*X6*M6*D6*C4*C6* 2310 . S5**2-X6*M6*D6*C4*C6+4.*X6*M6*D6*C5*S3*S2**2*C3*C6*S5-2.*X6*M6* 2311 . D6*C5*S3*C3*C6*S5-4.*X6*M6*D6*C5*C2*S2*C3**2*C6*S5+2.*X6*M6*D6* 2312 . C5*C2*S2*C6*S5-4.*M6*Z6*D4*C4*S3*C2*S2*C3*S5-4.*M6*Z6*D4*C4*S2** 2313 . 2*C3**2*S5+2.*M6*Z6*D4*C4*S2**2*S5+2.*M6*Z6*D4*C4*C3**2*S5-M6*Z6 2314 . *04*C4*S5-4.*M6*Z6*D4*C5*S3*S2**2*C3+2.*M6*Z6*D4*C5*S3*C3+4.*M6* 2315 . Z6*D4*C5*C2*S2*C3**2-2.*M6*Z6*04*C5*C2*S2+ANS6 2316 ANS4=-2.*I6XX*S6*S4*S2**2*C6*S5-2.*I6XX*S6*S4*C3**2*C6*S5+I6XX*S6 2317 . *S4*C6*S5+2.*I6XX*C4**2*S3*S2**2*C3*C6**2*S5**2-4.*I6XX*C4**2*S3 2318 . *S2**2*C3*C6**2+2.*I6XX*C4**2*S3*S2**2*C3-I6XX*C4**2*S3*C3*C6**2 2319 . *S5**2+2.*I6XX*C4**2*S3*C3*C6**2-I6XX*C4**2*S3*C3-2.*I6XX*C4**2* 2320 . C2*S2*C3**2*C6**2*S5**2+4.*I6XX*C4**2*C2*S2*C3**2*C6**2-2.*I6XX* 175. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 41 2321 . C4**2*C2*S2*C3**2+I6XX*C4**2*C2*S2*C6**2*S5**2-2.*I6XX*C4**2*C2* 2322 . S2*C6**2+I6XX*C4**2*C2*S2-4.*IGXX*C4*C5*S3*C2*S2*C3*C6**2*S5-4 . * 2323 . I6XX*C4*C5*S2**2*C3**2*C6**2*S5+2.*I6XX*C4*C5*S2**2*C6**2*S5+2.* 2324 . I6XX*C4*C5*C3**2*C6**2*S5-I6XX*C4*C5*C6**2*S5+2.*I6XX*S3*S2**2* 2325 . C3*C6**2*S5**2+2.*I6XX*S3*S2**2*C3*C6**2-2.* 16XX*S3*S2**2*C3-2326 . I6XX*S3*C3*C6**2*S5**2-I6XX*S3*C3*C6**2+I6XX*S3*C3-2.*I6XX*C2*S2 2327 . *C3**2*C6**2*S5**2-2.*I6XX*C2*S2*C3**2*C6**2+2.*I6XX*C2*S2*C3**2 2328 . +I6XX*C2*S2*C6**2*S5**2+I6XX*C2*S2*C6**2-I6XX*C2*S2+4.*X6*M6*D4* 2329 . S6*S4*S3*C2*S2*C3+4.*X6*M6*D4*S6*S4*S2**2*C3**2-2.*X6*M6*D4*S6* 2330 . S4*S2**2-2.*X6*M6*D4*S6*S4*C3**2+X6*M6*D4*S6*S4-4.*X6*M6*D4*C4* 2331 . C5*S3*C2*S2*C3*C6-4.*X6*M6*D4*C4*C5*S2**2*C3**2*C6+2.*X6*M6*D4* 2332 . C4*C5*S2**2*C6+ANS5 2333 ANS3=-2.*I6YY*C4**2*S3*C3*C6**2-I6YY*C4**2*S3*C3*S5**2+I6YY*C4**2 2334 . *S3*C3+2.*I6YY*C4**2*C2*S2*C3**2*C6**2*S5**2-4.*I6YY*C4**2*C2*S2 2335 . *C3**2*C6**2-2.*I6YY*C4**2*C2*S2*C3**2*S5**2+2.*I6YY*C4**2*C2*S2 2336 . *C3**2-I6YY*C4**2*C2*S2*C6**2*S5**2+2.*I6YY*C4**2*C2*S2*C6**2+ 2337 . I6YY*C4**2*C2*S2*S5**2-I6YY*C4**2*C2*S2+4.*I6YY*C4*C5*S3*C2*S2* 2338 . C3*C6**2*S5-4.*I6YY*C4*C5*S3*C2*S2*C3*S5+4.*I6YY*C4*C5*S2**2*C3 2339 . **2*C6**2*S5-4.*I6YY*C4*C5*S2**2*C3**2*S5-2.*I6YY*C4*C5*S2**2*C6 2340 . **2*S5+2.*I6YY*C4*C5*S2**2*S5-2.*I6YY*C4*C5*C3**2*C6**2*S5+2.* 2341 . I6YY*C4*C5*C3**2*S5+I6YY*C4*C5*C6**2*S5-I6YY*C4*C5*S5-2.*I6YY*S3 2342 . *S2**2*C3*C6**2*S5**2-2.*I6YY*S3*S2**2*C3*C6**2+2.*I6YY*S3*S2**2 2343 . *C3*S5**2+I6YY*S3*C3*C6**2*S5**2+I6YY*S3*C3*C6**2-I6YY*S3*C3*S5 2344 . **2+2.*I6YY*C2*S2*C3**2*C6**2*S5**2+2.*I6YY*C2*S2*C3**2*C6**2-2. 2345 . *I6YY*C2*S2*C3**2*S5**2-I6YY*C2*S2*C6**2*S5**2-I6YY*C2*S2*C6**2+ 2346 . I6YY*C2*S2*S5**2+4.*I6XX*S6*S4*C4*C5*S3*S2**2*C3*C6-2.*I6XX*S6* 2347 . S4*C4*C5*S3*C3*C6-4.*I6XX*S6*S4*C4*C5*C2*S2*C3**2*C6+2.*I6XX*S6* 2348 . S4*C4*C5*C2*S2*C6+4.*I6XX*S6*S4*S3*C2*S2*C3*C6*S5+4.*I6XX*S6*S4* 2349 . S2**2*C3**2*C6*S5+ANS4 2350 ANS2=2.*I6XY*S4*C4*C5*C2*S2-8.*I6XY*S4*S3*C2*S2*C3*C6**2*S5+4.* 2351 . I6XY*S4*S3*C2*S2*C3*S5-8.*I6XY*S4*S2**2*C3**2*C6**2*S5+4.*I6XY* 2352 . S4*S2**2*C3**2*S5+4.*I6XY*S4*S2**2*C6**2*S5-2.*I6XY*S4*S2**2*S5+ 2353 . 4.*I6XY*S4*C3**2*C6**2*S5-2.*I6XY*S4*C3**2*S5-2.*I6XY*S4*C6**2*S5 2354 . +I6XY*S4*S5-2.*I6ZZ*C4**2*S3*S2**2*C3*S5**2+I6ZZ*C4**2*S3*C3*S5 2355 . **2+2.*I6ZZ*C4**2*C2*S2*C3**2*S5**2-I6ZZ*C4**2*C2*S2*S5**2+4.* 2356 . I6ZZ*C4*C5*S3*C2*S2*C3*S5+4. *I6ZZ*C4*C5*S2**2*C3**'2*S5-2.*I6ZZ* 2357 . C4*C5*S2**2*S5-2.*I6ZZ*C4*C5*C3**2*S5+I6ZZ*C4*C5*S5-2.*I6ZZ*S3* 2358 . S2**2*C3*S5**2+2.*I6ZZ*S3*S2**2*C3+I6ZZ*S3*C3*S5**2-I6ZZ*S3*C3+ 2359 . 2.*I6ZZ*C2*S2*C3**2*S5**2-2.*I6ZZ*C2*S2*C3**2-I6ZZ*C2*S2*S5**2+ 2360 . I6ZZ*C2*S2-4.*I6YY*S6*S4*C4*C5*S3*S2**2*C3*C6+2.* 16YY*S6*S4*C4* 2361 . C5*S3*C3*C6+4.*I6YY*S6*S4*C4*C5*C2*S2*C3**2*C6-2.*I6YY*S6*S4*C4* 2362 . C5*C2*S2*C6-4.*I6YY*S6*S4*S3*C2*S2*C3*C6*S5-4.*I6YY*S6*S4*S2**2* 2363 . C3**2*C6*S5+2.*I6YY*S6*S4*S2**2*C6*S5+2.* 16YY*S6*S4*C3**2*C6*S5-2364 . I6YY*S6*S4*C6*S5-2.*I6YY*C4**2*S3*S2**2*C3*C6**2*S5**2+4.*I6YY* 2365 . C4**2*S3*S2**2*C3*C6**2+2 . *I6YY*C4**2*S3*S2**2*C3*S5**2-2.*I6YY* 2366 . C4**2*S3*S2**2*C3+I6YY*C4**2*S3*C3*C6**2*S5**2+ANS3 2367 ANS1=8.*I6XZ*C4*S2**2*C3**2*C6*S5**2-4.*I6XZ*C4*S2**2*C3**2*C6-4. 2368 . *I6XZ*C4*S2**2*C6*S5**2+2 . *I6XZ*C4*S2**2*C6-4.* 16XZ*C4*C3**2*C6* 2369 . S5**2+2.*I6XZ*C4*C3**2*C6+2.*I6XZ*C4*C6*S5**2-I6XZ*C4*C6+4.*I6XZ 2370 . *C5*S3*S2**2*C3*C6*S5-2.*I6XZ*C5*S3*C3*C6*S5-4.* 16XZ*C5*C2*S2*C3 2371 . **2*C6*S5+2.*I6XZ*C5*C2*S2*C6*S5+4.*I6XY*S6*C4**2*S3*S2**2*C3*C6 2372 . *S5**2-8.* 16XY*S6*C4* *2*S3*S2**2*C3*C6-2.* 16XY*S6*C4**2*S3*C3*C6 2373 . *S5**2+4.*I6XY*S6*C4**2*S3*C3*C6-4.*I6XY*S6*C4**2*C2*S2*C3**2*C6 2374 . *S5**2+8.*I6XY*S6*C4**2*C2*S2*C3**2*C6+2.*I6XY*S6*C4**2*C2*S2*C6 2375 . *S5**2-4.*I6XY*S6*C4**2*C2*S2*C6-8.*I6XY*S6*C4*C5*S3*C2*S2*C3*C6 2376 . *S5-8.*I6XY*S6*C4*C5*S2**2*C3**2*C6*S5+4.*I6XY*S6*C4*C5*S2**2*C6 2377 . *S5+4.*I6XY*S6*C4*C5*C3**2*C6*S5-2.*I6XY*S6*C4*C5*C6*S5+4.*I6XY* 2378 . S6*S3*S2**2*C3*C6*S5**2+4.*I6XY*S6*S3*S2**2*C3*C6-2.*I6XY*S6*S3* 176. L i s t i n g of ROBSIM.FOR at 1 5 : 4 5 : 1 3 on MAY 2 5 , 1985 f o r CCid=DACO Page 42 2 3 7 9 . C3*C6*S5**2-2 . * I 6 X Y * S 6 * S 3 * C 3 * C 6 - 4 . * I 6 X Y * S 6 * C 2 * S 2 * C 3 * * 2 * C 6 * S 5 * * 2 -2 3 8 0 . 4 . * I 6 X Y * S 6 * C 2 * S 2 * C 3 * * 2 * C 6 + 2 . * I 6 X Y * S 6 * C 2 * S 2 * C 6 * S 5 * * 2 + 2.* 16XY*S6*C2 2381 . * S 2 * C 6 - 8 . * I 6 X Y * S 4 * C 4 * C 5 * S 3 * S 2 * * 2 * C 3 * C 6 * * 2 + 4 . * I 6 X Y * S 4 * C 4 * C 5 * S 3 * S 2 2 3 8 2 . * * 2 * C 3 + 4 . * I 6 X Y * S 4 * C 4 * C 5 * S 3 * C 3 * C 6 * * 2 - 2 . * I 6 X Y * S 4 * C 4 * C 5 * S 3 * C 3 + 8 . * 2 3 8 3 . I 6 X Y * S 4 * C 4 * C 5 * C 2 * S 2 * C 3 * * 2 * C 6 * * 2 - 4 . * I 6 X Y * S 4 * C 4 * C 5 * C 2 * S 2 * C 3 * * 2 - 4 . * 2 3 8 4 . I6XY*S4*C4*C5*C2*S2*C6**2+ANS2 2 3 8 5 D112E=-A2**2*M6*C2*S2-2.*A2*X6*M6*S6*S4*S3*S2**2+A2*X6*M6*S6*S4* 238G . S3+2.*A2*XG*M6*S6*S4*C2*S2*C3+2.*A2*X6*M6*C4*C5*S3*S2**2*C6-A2* 2 3 8 7 . X6*M6*C4*C5*S3*C6-2.*A2*X6*M6*C4*C5*C2*S2*C3*C6+2.*A2*X6*M6*S3* 2 3 8 8 . C2*S2*C6*S5+2.*A2*X6*M6*S2**2*C3*C6*S5-A2*X6*M6*C3*C6*S5+2.*A2* 2 3 8 9 . M6*Z6*C4*S3*S2**2*S5-A2*M6*Z6*C4*S3*S5-2.*A2*M6*Z6*C4*C2*S2*C3* 2 3 9 0 . S5-2.*A2*MG*Z6*C5*S3*C2*S2-2.*A2*M6*Z6*C5*S2**2*C3+A2*M6*Z6*C5* 2391 . C3-2.*A2*M6*Y6*S6*C4*C5*S3*S2**2+A2*M6*Y6*SG*C4*C5*S3+2.*A2*M6* 2 3 9 2 . Y6*S6*C4*C5*C2*S2*C3-2.*A2*M6*YG*SG*S3*C2*S2*S5T2.*A2*MG*Y6*S6* 2 3 9 3 . S2**2*C3*S5+A2*M6*Y6*S6*C3*S5-2.*A2*M6*Y6*S4*S3*S2**2*CG+A2*MG* 2394 . Y6*S4*S3*C6+2.*A2*M6*YG*S4*C2*S2*C3*C6-2.*A2*M6*D4*S3*C2*S2-2 . * 2 3 9 5 . A2*M6*D4*S2**2*C3+A2*M6*D4*C3+2.*A2*M6*D6*C4*S3*S2**2*S5-A2*M6* 239G . D6*C4*S3*S5-2.*A2*M6*D6*C4*C2*S2*C3*S5-2.*A2*M6*D6*C5*S3*C2*S2-2 3 9 7 . 2.*A2*M6*D6*C5*S2**2*C3+A2*M6*D6*C5*C3-4.*I6XZ*S6*S4*C4*S3*S2**2* 2 3 9 8 . C3*S5+2.*IGXZ*S6*S4*C4*S3*C3*S5+4. * I 6 X Z * S 6 * S 4 * C 4 * C 2 * S 2 * C 3 * * 2 * S 5 -2 3 9 9 . 2 . * I 6 X Z * S G * S 4 * C 4 * C 2 * S 2 * S 5 + 4 . * I 6 X Z * S 6 * S 4 * C 5 * S 3 * C 2 * S 2 * C 3 + 4 . * I 6 X Z * S 6 2 4 0 0 . * S 4 * C 5 * S 2 * * 2 * C 3 * * 2 - 2 . * I 6 X Z * S 6 * S 4 * C 5 * S 2 * * 2 - 2 . * I 6 X Z * S G * S 4 * C 5 * C 3 * * 2 2401 . + I 6 X Z * S 6 * S 4 * C 5 + 4 . * I 6 X Z * C 4 * * 2 * C 5 * S 3 * S 2 * * 2 * C 3 * C 6 * S 5 - 2 . * I 6 X Z * C 4 * * 2 * 2 4 0 2 . C 5 * S 3 * C 3 * C 6 * S 5 - 4 . * 1 6 X Z * C 4 * * 2 * C 5 * C 2 * S 2 * C 3 * * 2 * C 6 * S 5 + 2 . * I 6 X Z * C 4 * * 2 * 2 4 0 3 . C 5 * C 2 * S 2 * C 6 * S 5 + 8 . * I 6 X Z * C 4 * S 3 * C 2 * S 2 * C 3 * C 6 * S 5 * * 2 - 4 . * I 6 X Z * C 4 * S 3 * C 2 * 2 4 0 4 . S2*C3*C6+ANS1 2 4 0 5 C 2 4 0 6 D112=D1 12A+D112B+D112C+D112D+D112E 2 4 0 7 C 2 4 0 8 D113A=-A2*Z3*M3*S3*S2*C2+A2*Z3*M3*C2**2*C3-A2*X3*M3*S3*C2**2-A2* 2 4 0 9 . X 3 * M 3 * S 2 * C 2 * C 3 - 2 . *1 3 Z Z * S 3 * C 2 * * 2 * C 3 + I 3 Z Z * S 3 * C 3 - 2 . *1 3 Z Z * S 2 * C 2 * C 3 * * 2 4 1 0 . 2+I3ZZ*S2*C2+2.* 1 3 X X * S 3 * C 2 * * 2 * C 3 -1 3 X X * S 3 * C 3 + 2 . * I 3 X X * S 2 * C 2 * C 3 * * 2 -2 4 1 1 . I 3 X X * S 2 * C 2 - 4 . * I 3 X Z * S 3 * S 2 * C 2 * C 3 + 4 . * I 3 X Z * C 2 * * 2 * C 3 * * 2 - 2 . * I 3 X Z * C 2 * * 2 2 4 1 2 . - 2 . * I 3 X Z * C 3 * * 2 + I 3 X Z 2 4 1 3 C 2 4 1 4 D113B=A2*Y4*M4*S3*S2*C2-A2*Y4*M4*C2**2*C3-A2*X4*M4*S3*C4*C2**2-A2 2 4 1 5 . *X4*M4*S2*C4*C2*C3+A2*M4*Z4*S3*S4*C2**2+A2*M4*Z4*S2*S4*C2*C3-A2* 2 4 1 6 . M4*D4*S3*S2*C2+A2*M4*D4*C2**2*C3-4.*I4YZ*S3*S2*S4*C2*C3+4 . * I 4 Y Z * 2 4 1 7 . S 4 * C 2 * * 2 * C 3 * * 2 - 2 . * I 4 Y Z * S 4 * C 2 * * 2 - 2 . * I 4 Y Z * S 4 * C 3 * * 2 + I 4 Y Z * S 4 - 4 . * Y 4 * 2 4 1 8 . M4*D4*S3*C2**2*C3+2.*Y4*M4*D4*S3*C3-4.*Y4*M4*D4*S2*C2*C3**2+2 . * 2 4 1 9 . Y4*M4*D4*S2*C2+4.*I4XY*S3*S2*C4*C2*C3-4.*I4XY*C4*C2**2*C3**2+2.* 2 4 2 0 . I 4 X Y * C 4 * C 2 * * 2 + 2 . * I 4 X Y * C 4 * C 3 * * 2 - I 4 X Y * C 4 - 2 . * I 4 Z Z * S 3 * C 4 * * 2 * C 2 * * 2 * C 3 2421 . + I 4 Z Z * S 3 * C 4 * * 2 * C 3 + 2 . * I 4 Z Z * S 3 * C 2 * * 2 * C 3 - I 4 Z Z * S 3 * C 3 - 2 . * I 4 Z Z * S 2 * C 4 * * 2 4 2 2 . 2 * C 2 * C 3 * * 2 + I 4 Z Z * S 2 * C 4 * * 2 * C 2 + 2 . * I 4 Z Z * S 2 * C 2 * C 3 * * 2 - I 4 Z Z * S 2 * C 2 - 2 . * 2 4 2 3 . I 4 Y Y * S 3 * C 2 * * 2 * C 3 + I 4 Y Y * S 3 * C 3 - 2 . * I 4 Y Y * S 2 * C 2 * C 3 * * 2 + I 4 Y Y * S 2 * C 2 + 2 . * 2 4 2 4 . I 4 X X * S 3 * C 4 * * 2 * C 2 * * 2 * C 3 - I 4 X X * S 3 * C 4 * * 2 * C 3 + 2 . * 1 4 X X * S 2 * C 4 * * 2 * C 2 * C 3 * * 2 4 2 5 . 2-I4XX*S2*G4**2*C2+4 . * I 4 X Z * S 3 * S 4 * C 4 * C 2 * * 2 * C 3 - 2 . * 1 4 X Z * S 3 * S 4 * C 4 * C 3 2 4 2 6 . + 4 . * I 4 X Z * S 2 * S 4 * C 4 * C 2 * C 3 * * 2 - 2 . * I 4 X Z * S 2 * S 4 * C 4 * C 2 - 4 . * X 4 * M 4 * D 4 * S 3 * S 2 2 4 2 7 . *C4*C2*C3+4.*X4*M4*D4*C4*C2**2*C3**2-2.*X4*M4*D4*C4*C2**2-2.*X4* 2 4 2 8 . M4*D4*C4*C3**2+X4*M4*D4*C4+4.*M4*Z4*D4*S3*S2*S4*C2*C3-4.*M4*Z4* 2 4 2 9 . D4*S4*C2**2*C3**2+2.*M4*Z4*D4*S4*C2**2+2.*M4*Z4*D4*S4*C3**2-M4* 2 4 3 0 . Z4*D4*S4+2.*M4*D4**2*S3*C2**2*C3-M4*D4**2*S3*C3+2.*M4*D4**2*S2* 24 3 1 . C2*C3**2-M4*D4**2*S2*C2 2 4 3 2 C 2 4 3 3 A N S 2 = 2 . * I 5 X X * C 4 * C 5 * S 2 * * 2 * S 5 + 2 . * I 5 X X * C 4 * C 5 * C 3 * * 2 * S 5 - I 5 X X * C 4 * C 5 * S 5 + 2 4 3 4 . 2 . * I 5 X X * S 3 * S 2 * * 2 * C 3 * S 5 * * 2 - I 5 X X * S 3 * C 3 * S 5 * * 2 - 2 . * I 5 X X * C 2 * S 2 * C 3 * * 2 * S 5 2 4 3 5 . * * 2 + I 5 X X * C 2 * S 2 * S 5 * * 2 + 4 . * I 5 X Z * C 4 * * 2 * C 5 * S 3 * S 2 * * 2 * C 3 * S 5 - 2 . * I 5 X Z * C 4 2 4 3 6 . * * 2 * C 5 * S 3 * C 3 * S 5 - 4 . * I 5 X Z * C 4 * * 2 * C 5 * C 2 * S 2 * C 3 * * 2 * S 5 + 2 . * I 5 X Z * C 4 * * 2 * C 5 177. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 43 2437 . *C2*S2*S5+8.*I5XZ*C4*S3*C2*S2*C3*S5**2-4.*I5XZ*C4*S3*C2*S2*C3+8. 2438 . *I5XZ*C4*S2**2*C3**2*S5**2-4.*I5XZ*C4*S2**2*C3**2-4.*I5XZ*C4*S2 2439 . **2*S5**2+2.* 15XZ*C4*S2**2-4.* 15XZ*C4*C3**2*S5**2+2.*I5XZ*C4*C3 2440 . **2+2.*I5XZ*C4*S5**2-I5XZ*C4+4.*I5XZ*C5*S3*S2**2*C3*S5-2.*I5XZ* 2441 . C5*S3*C3*S5-4.*I5XZ*C5*C2*S2*C3**2*S5+2.*I5XZ*C5*C2*S2*S5 2442 ANSI=-4.*D4*X5*M5*C2*S2*C3**2*S5+2.*D4*X5*M5*C2*S2*S5-4.*I5XY*S4* 2443 . C4*C5*S3*S2**2*C3+2.*I5XY*S4*C4*C5*S3*C3+4.* 15XY*S4*C4*C5*C2*S2* 2444 . C3**2-2.*I5XY*S4*C4*C5*C2*S2-4.*I5XY*S4*S3*C2*S2*C3*S5-4.*I5XY* 2445 . S4*S2**2*C3**2*S5+2.*I5XY*S4*S2**2*S5+2.*I5XY*S4*C3**2*S5-I5XY* 2446 . S4*S5-2.*I5ZZ*C4**2*S3*S2**2*C3*S5**2+I5ZZ*C4**2*S3*C3*S5**2 + 2 . * 2447 . I5ZZ*C4**2*C2*S2*C3**2*S5**2-I5ZZ*C4**2*C2*S2*S5**2+4.*I5ZZ*C4* 2448 . C5*S3*C2*S2*C3*S5+4.*I5ZZ*C4*C5*S2**2*C3**2*S5-2.*I5ZZ*C4*C5*S2 2449 . **2*S5-2.*I5ZZ*C4*C5*C3**2*S5+I5ZZ*C4*C5*S5-2.*I5ZZ*S3*S2**2*C3* 2450 . S5**2+2.*I5ZZ*S3*S2**2*C3+I5ZZ*S3*C3*S5**2-I5ZZ*S3*C3+2.*I5ZZ*C2 2451 . *S2*C3**2*S5**2-2.*I5ZZ*C2*S2*C3**2-I5ZZ*C2*S2*S5**2+I5ZZ*C2*S2+ 2452 . 2.*I5YY*C4**2*S3*S2**2*C3-I5YY*C4**2*S3*C3-2.*I5YY*C4**2*C2*S2*C3 2453 . **2+I5YY*C4**2*C2*S2-2.*I5YY*S3*S2**2*C3+I5YY*S3*C3+2.*I5YY*C2* 2454 . S2*C3**2-I5YY*C2*S2+2.*I5XX*C4**2*S3*S2**2*C3*S5**2-2.*I5XX*C4** 2455 . 2*S3*S2**2*C3-I5XX*C4**2*S3*C3*S5**2+I5XX*C4**2*S3*C3-2.*I5XX*C4 2456 . **2*C2*S2*C3**2*S5**2+2.*I5XX*C4**2*C2*S2*C3**2+I5XX*C4**2*C2*S2 2457 . *S5**2-I5XX*C4**2*C2*S2-4.*I5XX*C4*C5*S3*C2*S2*C3*S5-4.*I5XX*C4* 2458 . C5*S2**2*C3**2*S5+ANS2 2459 D113C=-A2*Y5*M5*S4*S3*S2**2+A2*Y5*M5*S4*S3+A2*Y5*M5*S4*C2*S2*C3+ 2460 . A2*Z5*M5*C4*S3*S2**2*S5-A2*Z5*M5*C4*S3*S5-A2*Z5*M5*C4*C2*S2*C3* 2461 . S5-A2*Z5*M5*C5*S3*C2*S2-A2*Z5*M5*C5*S2**2*C3+A2*Z5*M5*C5*C3-A2* 2462 . D4*M5*S3*C2*S2-A2*D4*M5*S2**2*C3+A2*D4*M5*C3+A2*X5*M5*C4*C5*S3* 2463 . S2**2-A2*X5*M5*C4*C5*S3-A2*X5*M5*C4*C5*C2*S2*C3+A2*X5*M5*S3*C2* 2464 . S2*S5+A2*X5*M5*S2**2*C3*S5-A2*X5*M5*C3*S5-4.* 15YZ*S4*C4*S3*S2**2 2465 . *C3*S5+2.*I5YZ*S4*C4*S3*C3*S5+4.* 15YZ*S4*C4*C2*S2*C3**2*S5-2 . * 2466 . I5YZ*S4*C4*C2*S2*S5+4.*I5YZ*S4*C5*S3*C2*S2*C3+4.*I5YZ*S4*C5*S2** 2467 . 2*C3**2-2.*I5YZ*S4*C5*S2**2-2.*I5YZ*S4*C5*C3**2+I5YZ*S4*C5+4.*Y5 2468 . *D4*M5*S4*S3*C2*S2*C3+4.*Y5*D4*M5*S4*S2**2*C3**2-2.*Y5*D4*M5*S4* 2469 . S2**2-2.*Y5*D4*M5*S4*C3**2+Y5*04*M5*S4-4.*Z5*D4*M5*C4*S3*C2*S2* 2470 . C3*S5-4.*Z5*D4*M5*C4*S2**2*C3**2*S5+2.*Z5*D4*M5*C4*S2**2*S5+2 . * 2471 . Z5*D4*M5*C4*C3**2*S5-Z5*D4*M5*C4*S5-4.*Z5*D4*M5*C5*S3*S2**2*C3+ 2472 . 2.*Z5*D4*M5*C5*S3*C3+4.*Z5*D4*M5*C5*C2*S2*C3**2-2.*Z5*D4*M5*C5*C2 2473 . *S2-2.*D4**2*M5*S3*S2**2*C3+D4**2*M5*S3*C3+2.*D4**2*M5*C2*S2*C3 2474 . **2-D4**2*M5*C2*S2-4.*D4*X5*M5*C4*C5*S3*C2*S2*C3-4.*D4*X5*M5*C4* 2475 . C5*S2**2*C3**2+2.*D4*X5*M5*C4*C5*S2**2+2.*D4*X5*M5*C4*C5*C3**2-2476 . D4*X5*M5*C4*C5+4.*D4*X5*M5*S3*S2**2*C3*S5-2.*D4*X5*M5*S3*C3*S5+ 2477 . ANSI 2478 C 2479 ANS8=M6*D6**2*C2*S2*S5**2-M6*D6**2*C2*S2-4.* 16YZ*S6*C4**2*C5*S3* 2480 . S2**2*C3*S5+2.*I6YZ*S6*C4**2*C5*S3*C3*S5+4.*I6YZ*S6*C4**2*C5*C2* 2481 . S2*C3**2*S5-2.*I6YZ*S6*C4**2*C5*C2*S2*S5-8.*I6YZ*S6*C4*S3*C2*S2* 2482 . C3*S5**2+4.*I6YZ*S6*C4*S3*C2*S2*C3-8.*I6YZ*S6*C4*S2**2*C3**2*S5 2483 . **2+4.*I6YZ*S6*C4*S2**2*C3**2+4. * 16YZ*S6*C4*S2**2*S5**2-2.*I6YZ* 2484 . S6*C4*S2**2+4 . *I6YZ*S6*C4*C3**2*S5**2-2.*I6YZ*S6*C4*C3**2-2 . * 2485 . I6YZ*S6*C4*S5**2+I6YZ*S6*C4-4.*I6YZ*S6*C5*S3*S2**2*C3*S5+2.*I6YZ 2486 . *S6*C5*S3*C3*S5+4.*I6YZ*S6*C5*C2*S2*C3**2*S5-2.*I6YZ*S6*C5*C2*S2 2487 . *S5-4.*I6YZ*S4*C4*S3*S2**2*C3*C6*S5+2.*I6YZ*S4*C4*S3*C3*C6*S5+4. 2488 . *I6YZ*S4*C4*C2*S2*C3**2*C6*S5-2.*I6YZ*S4*C4*C2*S2*C6*S5+4.*I6YZ* 2489 . S4*C5*S3*C2*S2*C3*C6+4.*I6YZ*S4*C5*S2**2*C3**2*C6-2.*I6YZ*S4*C5* 2490 . S2**2*C6-2.*I6YZ*S4*C5*C3**2*C6+I6YZ*S4*C5*C6 2491 ANS7=M6*Y6*D6*S6*C4-4.*M6*Y6*D6*S6*C5*S3*S2**2*C3*S5+2.*M6*Y6*D6* 2492 . S6*C5*S3*C3*S5+4.*M6*Y6*D6*S6*C5*C2*S2*C3**2*S5-2.*M6*Y6*D6*S6* 2493 . C5*C2*S2*S5-4.*M6*Y6*D6*S4*C4*S3*S2**2*C3*C6*S5+2.*M6*Y6*D6*S4* 2494 . C4*S3*C3*C6*S5+4.*M6*Y6*D6*S4*C4*C2*S2*C3**2*C6*S5-2.*M6*Y6*D6* 178. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 19B5 f o r CCid=DACO Page 44 2495 . S4*C4*C2*S2*C6*S5+4.*M6*Y6*D6*S4*C5*S3*C2*S2*C3*C6+4.*M6*Y6*D6* 2496 . S4*C5*S2**2*C3**2*C6-2.*M6*Y6*D6*S4*C5*S2**2*C6-2.*M6*Y6*D6*S4* 2497 . C5*C3**2*C6+M6*Y6*D6*S4*C5*C6-2.*M6*D4**2*S3*S2**2*C3+M6*D4**2* 2498 . S3*C3+2.*M6*D4**2*C2*S2*C3**2-M6*D4**2*C2*S2-4.*M6*D4*D6*C4*S3* 2499 . C2*S2*C3*S5-4.*M6*D4*D6*C4*S2**2*C3**2*S5+2.*M6*D4*D6*C4*S2**2* 2500 . S5+2.*M6*D4*D6*C4*C3**2*S5-M6*D4*D6*C4*S5-4.*M6*D4*D6*C5*S3*S2* * 2501 . 2*C3+2.*M6*D4*D6*C5*S3*C3+4.*M6*D4*D6*C5*C2*S2*C3**2-2.*M6*D4*D6 2502 . *C5*C2*S2+2.*M6*D6**2*C4**2*S3*S2**2*C3*S5**2-M6*D6**2*C4**2*S3* 2503 . C3*S5**2-2.*M6*D6**2*C4**2*C2*S2*C3**2*S5**2+M6*D6**2*C4**2*C2* 2504 . S2*S5**2-4.*M6*D6**2*C4*C5*S3*C2*S2*C3*S5-4.*M6*D6**2*C4*C5*S2** 2505 . 2*C3**2*S5+2.*M6*D6**2*C4*C5*S2**2*S5+2.*M6*06**2*C4*C5*C3**2*S5 2506 . -M6*D6**2*C4*C5*S5+2.*M6*D6**2*S3*S2**2*C3*S5**2-2.*M6*D6**2*S3* 2507 . S2**2*C3-M6*D6**2*S3*C3*S5**2+M6*D6**2*S3*C3-2.*M6*D6**2*C2*S2* 2508 . C3**2*S5**2+2.*M6*D6**2*C2*S2*C3**2+ANS8 2509 ANS6=-4.*M6*Z6*D6*C4**2*C2*S2*C3**2*S5**2+2.*M6*Z6*06*C4**2*C2*S2 2510 . *S5**2-8.*M6*Z6*D6*C4*C5*S3*C2*S2*C3*S5-8.*M6*Z6*D6*C4*C5*S2**2* 2511 . C3**2*S5+4.*M6*Z6*D6*C4*C5*S2**2*S5+4.*M6*Z6*D.G*C4*C5*C3**2*S5-2512 . 2.*M6*Z6*D6*C4*C5*S5+4.*M6*Z6*D6*S3*S2**2*C3*S5**2-4.*M6*Z6*D6*S3 2513 . *S2**2»C3-2 . *M6*Z6*D6*S3*C3*S5**2+2.*M6*Z6*D6*S3*C3-4.*M6*Z6*DG* 2514 . C2*S2*C3**2*S5**2+4.*M6*Z6*D6*C2*S2*C3**2+2.*M6*Z6*06*C2*S2*S5** 2515 . 2-2.*M6*Z6*06*C2*S2+4.*M6*Y6*D4*S6*C4*C5*S3*C2*S2*C3+4.*M6*Y6*D4 2516 . *S6*C4*C5*S2**2*C3**2-2.*M6*Y6*D4*S6*C4*C5*S2**2-2.*M6*Y6*D4*S6* 2517 . C4*C5*C3**2+M6*Y6*D4*S6*C4*C5-4.*M6*Y6*D4*S6*S3*S2**2*C3*S5+2.* 2518 . M6*Y6*D4*S6*S3*C3*S5+4.*M6*Y6*D4*S6*C2*S2*C3**2*S5-2.*M6*Y6*D4* 2519 . S6*C2*S2*S5+4.*M6*Y6*D4*S4*S3*C2*S2*C3*C6+4.*M6*Y6*D4*S4*S2**2* 2520 . C3**2*C6-2.*M6*Y6*D4*S4*S2**2*C6-2.*M6*Y6*D4*S4*C3**2*C6+M6*Y6* 2521 . D4*S4*C6-4.*M6*Y6*D6*S6*C4**2*C5*S3*S2**2*C3*S5+2.*M6*Y6*D6*S6* 2522 . C4**2*C5*S3*C3*S5+4.*M6*Y6*D6*S6*C4**2*C5*C2*S2*C3**2*S5-2.*M6* 2523 . Y6*D6*S6*C4**2*C5*C2*S2*S5-8.*M6*Y6*D6*S6*C4*S3*C2*S2*C3*S5**2+ 2524 . 4.*M6*Y6*D6*S6*C4*S3*C2*S2*C3-8.*M6*Y6*D6*S6*C4*S2**2*C3**2*S5**2 2525 . +4.*M6*Y6*D6*S6*C4*S2**2*C3**2+4.*M6*Y6*D6*S6*C4*S2**2*S5**2-2.* 2526 . M6*Y6*D6*S6*C4*S2**2+4.*M6*Y6*D6*S6*C4*C3**2*S5**2-2.*M6*Y6*D6* 2527 . S6*C4*C3**2-2.*M6*Y6*D6*S6*C4*S5**2+ANS7 2528 ANS5=-2.*X6*M6*D4*S3*C3*C6*S5-4.*X6*M6*D4*C2*S2*C3**2*C6*S5+2.*X6 2529 . *M6*D4*C2*S2*C6*S5-4.*X6*M6*D6*S6*S4*C4*S3*S2**2*C3*S5+2.*X6*M6* 2530 . D6*S6*S4*C4*S3*C3*S5+4.*X6*M6*D6*S6*S4*C4*C2*S2*C3**2*S5-2.*X6* 2531 . M6*D6*S6*S4*C4*C2*S2*S5+4.*X6*M6*D6*S6*S4*C5*S3*C2*S2*C3+4.*X6* 2532 . M6*D6*S6*S4*C5*S2**2*C3**2-2.*X6*M6*D6*S6*S4*C5*S2**2-2.*X6*M6* 2533 . 06*S6*S4*C5*C3**2+X6*M6*06*S6*S4*C5+4.*X6*M6*D6*C4**2*C5*S3*S2** 2534 . 2*C3*C6*S5-2.*X6*M6*D6*C4**2*C5*S3*C3*C6*S5-4.*X6*M6*D6*C4**2*C5 2535 . *C2*S2*C3**2*C6*S5+2.*X6*M6*D6*C4**2*C5*C2*S2*C6*S5+8.*X6*M6*D6* 2536 . C4*S3*C2*S2*C3*C6*S5**2-4.*X6*M6*D6*C4*S3*C2*S2*C3*C6+8.*X6.*M6* 2537 . D6*C4*S2**2*C3**2*C6*S5**2-4.*X6*M6*D6*C4*S2**2*C3**2*C6-4.*X6* 2538 . M6*D6*C4*S2**2*C6*S5**2+2.*X6*M6*D6*C4*S2**2*C6-4.*X6*M6*D6*C4* 2539 . C3**2*C6*S5**2+2.*X6*M6*D6*C4*C3**2*C6+2.*X6*M6*D6*C4*C6*S5**2-2540 . X6*M6*06*C4*C6+4.*X6*M6*D6*C5*S3*S2**2*C3*C6*S5-2.*X6*M6*D6*C5* 2541 . S3*C3*C6*S5-4.*X6*M6*06*C5*C2*S2*C3**2*C6*S5+2.*X6*M6*D6*C5*C2* 2542 . S2*C6*S5-4.*M6*Z6*D4*C4*S3*C2*S2*C3*S5-4.*M6*Z6*D4*C4*S2**2*C3** 2543 . 2*S5+2.*M6*Z6*D4*C4*S2**2*S5+2.*M6*Z6*D4*C4*C3**2*S5-M6*Z6*D4*C4 2544 . *S5-4.*M6*Z6*D4*C5*S3*S2**2*C3+2.*M6*Z6*D4*C5*S3*C3+4.*M6*Z6*D4* 2545 . C5*C2*S2*C3**2-2.*M6*Z6*D4*C5*C2*S2+4.*M6*Z6*D6*C4**2*S3*S2**2* 2546 . C3*S5**2-2.*M6*Z6*D6*C4**2*S3*C3*S5*»2+ANS6 • 2547 ANS4=2.*I6XX*C4**2*S3*S2**2*C3*C6**2*S5**2-4.*I6XX*C4**2*S3*S2**2 2548 . *C3*C6**2+2.*I6XX*C4**2*S3*S2**2*C3-I6XX+C4**2*S3*C3*C6**2*S5**2 2549 . +2.*I6XX*C4**2*S3*C3*C6**2-I6XX*C4**2*S3*C3-2.*I6XX*C4**2*C2*S2* 2550 . C3**2*C6**2*S5**2+4.*I6XX*C4**2*C2*S2*C3**2*C6**2-2.*I6XX*C4**2* 2551 . C2*S2*C3**2+I6XX*C4**2*C2*S2*C6**2*S5**2-2.*I6XX*C4**2*C2*S2*C6 2552 . **2+I6XX*C4**2*C2*S2-4.*I6XX*C4*C5*S3*C2*S2*C3*C6**2*S5-4.*I6XX* 179. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 45 2553 . C4*C5*S2**2*C3**2*C6**2*S5+2 . *IGXX*C4*C5*S2**2*C6**2*S5+2.*I6XX* 2554 . C4*C5*C3**2*C6**2*S5-I6XX*C4*C5*C6**2*S5+2.*I6XX*S3*S2**2*C3*C6 2555 . **2*S5**2+2.*I6XX*S3*S2**2*C3*C6**2-2.*I6XX*S3*S2**2*C3-I6XX*S3* 2556 . C3*C6**2*S5**2-I6XX*S3*C3*C6**2+I6XX*S3*C3-2.*I6XX*C2*S2*C3**2* 2557 . C6**2*S5**2-2.*I6XX*C2*S2*C3**2*C6**2+2.*I6XX*C2*S2*C3**2+I6XX* 2558 . C2*S2*C6**2*S5**2+I6XX*C2*S2*C6**2-I6XX*C2*S2+4.*X6*M6*D4*S6*S4* 2559 . S3*C2*S2*C3+4.*X6*M6*D4*S6*S4*S2**2*C3**2-2.*X6*M6*04*S6*S4*S2** 2560 . 2-2.*X6*M6*D4*S6*S4*C3**2+X6*M6*D4*S6*S4-4.*X6*M6*D4*C4*C5*S3*C2 2561 . *S2*C3*C6-4.*X6*M6*D4*C4*C5*S2**2*C3**2*C6+2.*X6*M6*D4*C4*C5*S2 2562 . **2*C6+2.*X6*M6*D4*C4*C5*C3**2*C6-X6*M6*D4*C4*C5*C6+4.*X6*MG*D4* 2563 . S3*S2**2*C3*C6*S5+ANS5 2564 ANS3=2.*I6YY*C4**2*C2*S2*C3**2*C6**2*S5**2-4.*I6YY*C4**2*C2*S2*C3 2565 . **2*C6**2-2 . *I6YY*C4**2*C2*S2*C3**2*S5**2 + 2.*I6YY*C4**2*C2*S2*C3 2566 . **2-I6YY*C4**2*C2*S2*C6**2*S5**2+2.*I6YY*C4**2*C2*S2*C6**2+I6YY* 2567 . C4**2*C2*S2*S5**2-I6YY*C4**2*C2*S2+4.*I6YY*C4*C5*S3*C2*S2*C3*C6 2568 . **2*S5-4.*I6YY*C4*C5*S3*C2*S2*C3*S5+4. *I6YY*C4*C5*S2**2*C3**2*C6 2569 . **2*S5-4.*I6YY*C4*C5*S2**2*C3**2*S5-2.*I6YY*C4*C5*S2**2*C6**2*S5 2570 . +2.*I6YY*C4*C5*S2**2*S5-2 . *I6YY*C4*C5*C3**2*C6**2*S5+2.*I6YY*C4* 2571 . C5*C3**2*S5+I6YY*C4*C5*C6**2*S5-I6YY*C4*C5*S5-2.* 16YY*S3*S2**2* 2572 . C3*C6**2*S5**2-2.*I6YY*S3*S2**2*C3*C6**2+2.*I6YY*S3*S2**2*C3*S5 2573 . **2+I6YY*S3*C3*C6**2*S5**2+I6YY*S3*C3*C6**2-I6YY*S3*C3*S5**2 + 2 . * 2574 . I6YY*C2*S2*C3**2*C6**2*S5**2+2.*I6YY*C2*S2*C3**2*C6**2-2.*I6YY* 2575 . C2*S2*C3**2*S5**2-I6YY*C2*S2*C6**2*S5**2-I6YY*C2*S2*C6**2+I6YY* 2576 . C2*S2*S5**2+4.*I6XX*S6*S4*C4*C5*S3*S2**2*C3*C6-2.*I6XX*S6*S4*C4* 2577 . C5*S3*C3*C6-4.*I6XX*S6*S4*C4*C5*C2*S2*C3**2*C6+2.*I6XX*S6*S4*C4* 2578 . C5*C2*S2*C6+4.*I6XX*S6*S4*S3*C2*S2*C3*C6*S5+4.*IGXX*S6*S4*S2**2* 2579 . C3**2*C6*S5-2.*I6XX*S6*S4*S2**2*C6*S5-2.*I6XX*S6*S4*C3**2*C6*S5+ 2580 . I6XX*S6*S4*C6*S5+ANS4 258 1 ANS2 = -8.*I6XY*S4*S2**2*C3**2*C6**2*S5+4.*I6XY*S4*S2**2*C3**2*S5 + 2582 . 4.*I6XY*S4*S2**2*C6**2*S5-2.*I6XY*S4*S2**2*S5+4.* 16XY*S4*C3**2*C6 2583 . **2*S5-2 . *I6XY*S4*C3**2*S5-2.*I6XY*S4*C6**2*S5+I6XY*S4*S5-2 . * 2584 . I6ZZ*C4**2*S3*S2**2*C3*S5**2+I6ZZ*C4**2*S3*C3*S5**2+2.*I6ZZ*C4** 2585 . 2*C2*S2*C3**2*S5**2-I6ZZ*C4**2*C2*S2*S5**2+4.*I6ZZ*C4*C5*S3*C2* 2586 . S2*C3*S5+4.*I6ZZ*C4*C5*S2**2*C3**2*S5-2.*I6ZZ*C4*C5*S2**2*S5-2. * 2587 . I6ZZ*C4*C5*C3**2*S5+I6ZZ*C4*C5*S5-2.*I6ZZ*S3*S2**2*C3*S5**2+2.* 2588 . I6ZZ*S3*S2**2*C3+I6ZZ*S3*C3*S5**2-I6ZZ*S3*C3+2.*I6ZZ*C2*S2*C3**2 2589 . *S5**2-2.*I6ZZ*C2*S2*C3**2-I6ZZ*C2*S2*S5**2+I6ZZ*C2*S2-4. *I6YY* 2590 . S6*S4*C4*C5*S3*S2**2*C3*C6+2.*I6YY*S6*S4*C4*C5*S3*C3*C6+4.*I6YY* 2591 . S6*S4*C4*C5*C2*S2*C3**2*C6-2.*I6YY*S6*S4*C4*C5*C2*S2*C6-4.*I6YY* 2592 . S6*S4*S3*C2*S2*C3*C6*S5-4. *I6YY*S6*S4*S2**2*C3**2*C6*S5+2.*I6YY* 2593 . S6*S4*S2**2*C6*S5+2.*I6YY*S6*S4*C3**2*C6*S5-16YY*S6*S4*C6*S5-2 . * 2594 . I6YY*C4**2*S3*S2**2*C3*C6**2*S5**2+4.*I6YY*C4**2*S3*S2**2*C3*C6 2595 . **2+2.*I6YY*C4**2*S3*S2**2*C3*S5**2-2.*I6YY*C4**2*S3*S2**2*C3+ 2596 . I6YY*C4**2*S3*C3*C6**2*S5**2-2.*I6YY*C4**2*S3*C3*C6**2-I6YY*C4** 2597 . 2*S3*C3*S5**2+I6YY*C4**2*S3*C3+ANS3 2598 ANSI=-4.*I6XZ*C4*S2**2*C6*S5**2+2.*I6XZ*C4*S2**2*C6-4.*I6XZ*C4*C3 2599 . **2*C6*S5**2+2.*I6XZ*C4*C3**2*C6+2.*I6XZ*C4*C6*S5**2-I6XZ*C4*C6+ 2600 . 4.*I6XZ*C5*S3*S2**2*C3*C6*S5-2.*I6XZ*C5*S3*C3*C6*S5-4.*I6XZ*C5*C2 2601 . *S2*C3**2*C6*S5+2.*I6XZ*C5*C2*S2*C6*S5+4.*I6XY*S6*C4**2*S3*S2**2 2602 . *C3*C6*S5**2-8.*I6XY*S6*C4**2*S3*S2**2*C3*C6-2.*I6XY*S6*C4**2*S3 2603 . +C3*C6*S5**2+4.*I6XY*S6*C4**2*S3*C3*C6-4,*I6XY*S6*C4**2*C2*S2*C3 2604 . **2*C6*S5**2+8.*I6XY*S6*C4**2*C2*S2*C3**2*C6+2.* 16XY*S6*C4**2*C2 2605 . *S2*C6*S5**2-4.*I6XY*S6*C4**2*C2*S2*C6-8.*I6XY*S6*C4*C5*S3*C2*S2 2606 . *C3*C6*S5-8.*I6XY*S6*C4*C5*S2**2*C3**2*C6*S5+4.*I6XY*S6*C4*C5*S2 2607 . **2*C6*S5+4.*I6XY*S6*C4*C5*C3**2*C6*S5-2.*I6XY*S6*C4*C5*C6*S5+4. 2608 . *I6XY*S6*S3*S2**2*C3*C6*S5**2+4.*I6XY*S6*S3*S2**2*C3*C6-2.*I6XY* 2609 . S6*S3*C3*C6*S5**2-2.*I6XY*S6*S3*C3*C6-4.*I6XY*S6*C2*S2*C3**2*C6* 2610 . S5**2-4.*I6XY*S6*C2*S2*C3**2*C6+2.*I6XY*S6*C2*S2*C6*S5**2+2.* 180. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DAC0 Page 46 2611 . I6XY*S6*C2*S2*C6-8.*I6XY*S4*C4*C5*S3*S2**2*C3*C6**2+4.*I6XY*S4* 2612 . C4*C5*S3*S2**2*C3+4.*I6XY*S4*C4*C5*S3*C3*C6**2-2.*I6XY*S4*C4*C5* 2613 . S3*C3+8.*I6XY*S4*C4*C5*C2*S2*C3**2*C6**2-4.*I6XY*S4*C4*C5*C2*S2* 2614 . C3**2-4.*I6XY*S4*C4*C5*C2*S2*C6**2+2.*I6XY*S4*C4*C5*C2*S2-8.* 2615 . I6XY*S4*S3*C2*S2*C3*C6**2*S5+4.*I6XY*S4*S3*C2*S2*C3*S5+ANS2 2616 D1 13D = -A2*X6*M6*S6*S4*S3*S2**2+A2*X6*M6*S6*S4*S3+A2*X6*M6*S6*S4* 2617 . C2*S2*C3+A2*X6*M6*C4*C5*S3*S2**2*C6-A2*X6*M6*C4*C5*S3*C6-A2*X6* 2618 . M6*C4*C5*C2*S2*C3*C6+A2*X6*M6*S3*C2*S2*C6*S5+A2*X6*M6*S2**2*C3* 2619 . C6*S5-A2*X6*M6*C3*C6*S5+A2*M6*Z6*C4*S3*S2**2*S5-A2*M6*Z6*C4*S3* 2620 . S5-A2*M6*Z6*C4*C2*S2*C3*S5-A2*M6*Z6*C5*S3*C2*S2-A2*M6*Z6*C5*S2** 2621 . 2*C3+A2*M6*Z6*C5*C3-A2*M6*Y6*S6*C4*C5*S3*S2**2+A2*M6*Y6*S6*C4*C5 2622 . *S3+A2*M6*Y6*S6*C4*C5*C2*S2*C3-A2*M6*Y6*S6*S3*C2*S2*S5-A2*M6*Y6* 2623 . S6*S2**2*C3*S5+A2*M6*Y6*S6*C3*S5-A2*M6*Y6*S4*S3*S2**2*C6+A2*M6* 2624 . Y6*S4*S3*C6+A2*M6*Y6*S4*C2*S2*C3*C6-A2*M6*D4*S3*C2*S2-A2*M6*D4* 2625 . S2**2*C3+A2*M6*D4*C3+A2*M6*D6*C4*S3*S2**2*S5-A2*M6*D6*C4*S3*S5-2626 . A2*M6*D6*C4*C2*S2*C3*S5-A2*M6*D6*C5*S3*C2*S2-A2*M6*06*C5*S2**2* 2627 . C3+A2*M6*D6*C5*C3-4. *I6XZ*S6*S4*C4*S3*S2**2*C3*S5+2.*I6XZ*S6*S4* 2628 . C4*S3*C3*S5+4.*I6XZ*S6*S4*C4*C2*S2*C3**2*S5-2.*I6XZ*S6*S4*C4*C2* 2629 . S2*S5+4.*I6XZ*S6*S4*C5*S3*C2*S2*C3+4.*I6XZ*S6*S4*C5*S2**2*C3**2-2630 . 2.*I6XZ*S6*S4*C5*S2**2-2.*I6XZ*S6*S4*C5*C3**2+I6XZ*S6*S4*C5+4.* 2631 . I6XZ*C4**2*C5*S3*S2**2*C3*C6*S5-2.*I6XZ*C4**2*C5*S3*C3*C6*S5-4 . * 2632 . I6XZ*C4**2*C5*C2*S2*C3**2*C6*S5+2.*I6XZ*C4**2*C5*C2*S2*C6*S5+8.* 2633 . I6XZ*C4*S3*C2*S2*C3*C6*S5**2-4.*I6XZ*C4*S3*C2*S2*C3*C6+8.*I6XZ* 2634 . C4*S2**2*C3**2*C6*S5**2-4.*I6XZ*C4*S2**2*C3**2*C6+ANS1 2635 C 2636 D113=D113A+D113B+D113C+D113D 2637 C 2638 D123A=Z3*M3*D2*S3*C2+Z3*M3*D2*S2*C3+I3YZ*S3*C2+I3YZ*S2*C3-I3XY*S3 2639 . *S2+I3XY*C2*C3-X3*M3*D2*S3*S2+X3*M3*D2*C2*C3 2640 C 2641 D123B=-I4YZ*S3*C4*C2-I4YZ*S2*C4*C3-Y4*M4*D2*S3*C2-Y4*M4*D2*S2*C3-2642 . I4XY*S3*S4*C2-I4XY*S2*S4*C3-I4ZZ*S3*S2*S4*C4+I4ZZ*S4*C4*C2*C3+ 2643 . I4XX*S3*S2*S4*C4-I4XX*S4*C4*C2*C3-2.*I4XZ*S3*S2*C4**2+I4XZ*S3*S2 2644 . +2.*I4XZ*C4**2*C2*C3-I4XZ*C2*C3-X4*M4*D2*S3*S2*C4+X4*M4*D2*C4*C2 2645 . *C3+X4*M4*D4*S3*S4*C2+X4*M4*D4*S2*S4*C3+M4*Z4*D2*S3*S2*S4-M4*Z4* 2646 . D2*S4*C2*C3+M4*Z4*D4*S3*C4*C2+M4*Z4*D4*S2*C4*C3+M4*D2*D4*S3*C2+ 2647 . M4*D2*D4*S2*C3 2648 C 2649 D123C=-2.*I5YZ*C4**2*S3*S2*S5+2.*I5YZ*C4**2*C2*C3*S5+I5YZ*C4*C5* 2650 . S3*C2+I5YZ*C4*C5*S2*C3+I5YZ*S3*S2*S5-I5YZ*C2*C3*S5+Y5*D2*M5*S4* 2651 . S3*S2-Y5*D2*M5*S4*C2*C3+Y5*D4*M5*C4*S3*C2+Y5*04*M5*C4*S2*C3-Z5* 2652 . D2*M5*C4*S3*S2*S5+Z5*D2*M5*C4*C2*C3*S5+Z5*D2*M5*C5*S3*C2+Z5*D2* 2653 . M5*C5*S2*C3+Z5*D4*M5*S4*S3*C2*S5+Z5*D4*M5*S4*S2*C3*S5+D2*04*M5* 2654 . S3*C2+D2*D4*M5*S2*C3-D2*X5*M5*C4*C5*S3*S2+D2*X5*M5*C4*C5*C2*C3-2655 . D2*X5*M5*S3*C2*S5-02*X5*M5*S2*C3*S5+D4*X5*M5*S4*C5*S3*C2+D4*X5* 2656 . M5*S4*C5*S2*C3-2.*I5XY*C4**2*C5*S3*S2+2.*I5XY*C4**2*C5*C2*C3-2657 . I5XY*C4*S3*C2*S5-I5XY*C4*S2*C3*S5+I5XY*C5*S3*S2-I5XY*C5*C2*C3+ 2658 . I5ZZ*S4*C4*S3*S2*S5**2-I5ZZ*S4*C4*C2*C3*S5**2-I5ZZ*S4*C5*S3*C2* 2659 . S5-I5ZZ*S4*C5*S2*C3*S5-I5YY*S4*C4*S3*S2+I5YY*S4*C4*C2*C3-I5XX*S4 2660 . *C4*S3*S2*S5**2+I5XX*S4*C4*S3*S2+I5XX*S4*C4*C2*C3*S5**2-I5XX*S4* 2661 . C4*C2*C3+I5XX*S4*C5*S3*C2*S5+I5XX*S4*C5*S2*C3*S5-2.*I5XZ*S4*C4* 2662 . C5*S3*S2*S5+2.*I5XZ*S4*C4*C5*C2*C3*S5-2.* 15XZ*S4*S3*C2*S5**2 + 2663 . I5XZ*S4*S3*C2-2.*I5XZ*S4*S2*C3*S5**2+I5XZ*S4*S2*C3 2664 C 2665 ANS2=M6*Y6*D2*S4*S3*S2*C6-M6*Y6*D2*S4*C2*C3*C6-M6*Y6*D4*S6*S4*C5* 2666 . S3*C2-M6*Y6*D4*S6*S4*C5*S2*C3+M6*Y6*D4*C4*S3*C2*C6+M6*Y6*D4*C4* 2667 . S2*C3*C6+2.*M6*Y6*D6*S6*S4*C4*C5*S3*S2*S5-2.*M6*Y6*06*S6*S4*C4* 2668 . C5*C2*C3*S5+2 . *M6*Y6*D6*S6*S4*S3*C2*S5**2-M6*Y6*D6*S6*S4*S3*C2+ 181. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 47 2669 . 2.*M6*Y6*D6*S6*S4*S2*C3*S5**2-M6*Y6*D6*S6*S4*S2*C3-2.*M6*Y6*D6*C4 2670 . **2*S3*S2*C6*S5+2.*M6*Y6*D6*C4**2*C2*C3*C6*S5+M6*Y6*D6*C4*C5*S3* 2671 . C2*C6+M6*Y6*D6*C4*C5*S2*C3*C6+M6*Y6*D6*S3*S2*C6*S5-M6*Y6*D6*C2* 2672 . C3*C6*S5+M6*D2*D4*S3*C2+M6*D2*D4*S2*C3-M6*D2*D6*C4*S3*S2*S5+M6* 2673 . D2*D6*C4*C2*C3*S5+M6*D2*D6*C5*S3*C2+M6*D2*D6*C5*S2*C3+M6*D4*D6* 2674 . S4*S3*C2*S5+M6*D4*D6*S4*S2*C3*S5-M6*06**2*S4*C4*S3*S2*S5**2+M6* 2675 . 06**2*S4*C4*C2*C3*S5**2+M6*D6**2*S4*C5*S3*C2*S5+M6*D6**2*S4*C5* 2676 . S2*C3*S5+2.*I6YZ*S6*S4*C4*C5*S3*S2*S5-2.*I6YZ*S6*S4*C4*C5*C2*C3* 2677 . S5+2.*I6YZ*S6*S4*S3*C2*S5**2-I6YZ*S6*S4*S3*C2+2.*I6YZ*S6*S4*S2* 2678 . C3*S5**2-I6YZ*S6*S4*S2*C3-2.*I6YZ*C4**2*S3*S2*C6*S5+2.*I6YZ*C4** 2679 . 2*C2*C3*C6*S5+I6YZ*C4*C5*S3*C2*C6+I6YZ*C4*C5*S2*C3*C6+I6YZ*S3*S2 2680 . *C6*S5-I6YZ*C2*C3*C6*S5 2681 ANS1=I6YY*S4*C5*S3*C2*S5-I6YY*S4*C5*S2*C3*C6**2*S5+I6YY*S4*C5*S2* 2682 . C3*S5+2.*I6XX*S6*C4**2*C5*S3*S2*C6-2.*I6XX*S6*C4**2*C5*C2*C3*C6+ 2683 . I6XX*S6*C4*S3*C2*C6*S5+I6XX*S6*C4*S2*C3*C6*S5-I6XX*S6*C5*S3*S2* 2684 . C6+I6XX*S6*C5*C2*C3*C6-I6XX*S4*C4*S3*S2*C6**2*S5**2+2.*I6XX*S4* 2685 . C4*S3*S2*C6**2-I6XX*S4*C4*S3*S2+I6XX*S4*C4*C2*C3*C6**2*S5**2-2.* 2686 . I6XX*S4*C4*C2*C3*C6**2+I6XX*S4*C4*C2*C3+I6XX*S4*C5*S3*C2*C6**2* 2687 . S5+I6XX*S4*C5*S2*C3*C6**2*S5+X6*M6*D2*S6*S4*S3*S2-X6*M6*D2*S6*S4 2688 . *C2*C3-X6*M6*D2*C4*C5*S3*S2*C6+X6*M6*D2*C4*C5*C2*C3*C6-X6*M6*02* 2689 . S3*C2*C6*S5-X6*M6*D2*S2*C3*C6*S5+X6*M6*D4*S6*C4*S3*C2+X6*M6*D4* 2690 . S6*C4*S2*C3+X6*M6*D4*S4*C5*S3*C2*C6+X6*M6*D4*S4*C5*S2*C3*C6-2.* 2691 . X6*M6*D6*S6*C4**2*S3*S2*S5+2.*X6*M6*D6*S6*C4**2*C2*C3*S5+X6*M6* 2692- . D6*S6*C4*C5*S3*C2+X6*M6*D6*S6*C4*C5*S2*C3+X6*M6*D6*S6*S3*S2*S5-2693 . X6*M6*D6*S6*C2*C3*S5-2.*X6*M6*D6*S4*C4*C5*S3*S2*C6*S5+2.*X6*M6* 2694 . D6*S4*C4*C5*C2*C3*C6*S5-2.*X6*M6*D6*S4*S3*C2*C6*S5**2+X6*M6*D6* 2695 . S4*S3*C2*C6-2.*X6*M6*D6*S4*S2*C3*C6*S5**2+X6*M6*D6*S4*S2*C3*C6-2696 . M6*Z6*D2*C4*S3*S2*S5+M6*Z6*D2*C4*C2*C3*S5+M6*Z6*D2*C5*S3*C2+M6* 2697 . . Z6*D2*C5*S2*C3+M6*Z6*D4*S4*S3*C2*S5+M6*Z6*D4*S4*S2*C3*S5-2.*M6* 2698 . Z6*D6*S4*C4*S3*S2*S5**2+2.*M6*Z6*D6*S4*C4*C2*C3*S5**2+2.*M6*Z6* 2699 . D6*S4*C5*S3*C2*S5+2.*M6*Z6*06*S4*C5*S2*C3*S5+M6*Y6*D2*S6*C4*C5* 2700 . S3*S2-M6*Y6*D2*S6*C4*C5*C2*C3+M6*Y6*D2*S6*S3*C2*S5+M6*Y6*D2*S6* 2701 . S2*C3*S5+ANS2 2702 D123D=-2.*I6XZ*S6*C4**2*S3*S2*S5+2.*I6XZ*S6*C4**2*C2*C3*S5+I6XZ* 2703 . S6*C4*C5*S3*C2+I6XZ*S6*C4*C5*S2*C3+I6XZ*S6*S3*S2*S5-I6XZ*S6*C2* 2704 . C3*S5-2.*I6XZ*S4*C4*C5*S3*S2*C6*S5+2. *I6XZ*S4*C4*C5*C2*C3*C6*S5-2705 . 2.*I6XZ*S4*S3*C2*C6*S5**2+I6XZ*S4*S3*C2*C6-2.*I6XZ*S4*S2*C3*C6*S5 2706 . **2+I6XZ*S4*S2*C3*C6-2.*I6XY*S6*S4*C4*S3*S2*C6*S5**2+4.*I6XY*S6* 2707 . S4*C4*S3*S2*C6+2.*I6XY*S6*S4*C4*C2*C3*C6*S5**2-4.* 16XY*S6*S4*C4* 2708 . C2*C3*C6+2.*I6XY*S6*S4*C5*S3*C2*C6*S5+2.*I6XY*S6*S4*C5*S2*C3*C6* 2709 . S5-4.*I6XY*C4**2*C5*S3*S2*C6**2+2.*I6XY *C4 * *2 *C5* S3*S2+4.*I6XY* 2710 . C4**2*C5*C2*C3*C6**2-2.*I6XY*C4**2*C5*C2*C3-2.*I6XY*C4*S3*C2*C6 2711 . "-*2*S5+I6XY*C4*S3*C2*S5-2 . *I6XY*C4*S2*C3*C6**2*S5+I6XY*C4*S2*C3* 2712 . S5+2.*I6XY*C5*S3*S2*C6**2-I6XY*C5*S3*S2-2.*I6XY*C5*C2*C3*C6**2+ 2713 . I6XY*C5*C2*C3+I6ZZ*S4*C4*S3*S2*S5**2-I6ZZ*S4*C4*C2*C3*S5**2-I6ZZ 2714 . *S4*C5*S3*C2*S5-I6ZZ*S4*C5*S2*C3*S5-2.*I6YY*S6*C4**2*C5*S3*S2*C6 2715 . +2.*I6YY*S6*C4**2*C5*C2*C3*C6-I6YY*S6*C4*S3*C2*C6*S5-I6YY*S6*C4* 2716 . S2*C3*C6*S5+I6YY*S6*C5*S3*S2*C6-I6YY*S6*C5*C2*C3*C6+I6YY*S4*C4* 2717 . S3*S2*C6**2*S5**2-2.*I6YY*S4*C4*S3*S2*C6**2-I6YY*S4*C4*S3*S2*S5 2718 . **2+I6YY*S4*C4*S3*S2-I6YY*S4*C4*C2*C3*C6**2*S5**2+2.*I6YY*S4*C4* 2719 . C2*C3*C6**2+I6YY*S4*C4*C2*C3*S5**2-I6YY*S4*C4*C2*C3-I6YY*S4*C5* 2720 . S3*C2*C6**2*S5+ANS1 2721 C 2722 D123«D123A+D123B+D123C+D123D 2723 C 2724 D223A=A2*M3*(Z3*C3-X3*S3) 2725 C 2726 D223B=A2*M4*(-Y4*C3-X4*S3*C4+Z4*S3*S4+D4*C3) 182. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 fo r CC1d=DAC0 Page 48 2727 C 2728 D223C=A2*M5*(-Z5*C4*S3*S5+Z5*C5*C3+04*C3-X5*C4*C5*S3-X5*C3*S5+S4* 2729 . S3*Y5) 2730 C 2731 D2230=A2*M6*(XG*SS*S4*S3-X6*C4*C5*S3*C6-XG*C3*C6*S5-Z6*C4*S3*S5+ 2732 . Z6*C5*C3+YG*SG*C4*C5*S3+YG*S6*C3*S5+YG*S4*S3*C6+D4*C3-D6*C4*S3* 2733 . S5+DS*C5*C3) 2734 C 2735 D223=D223A+D223B+D223C+D223D 2736 C 2737 RETURN 2738 END 2739 C 2740 c************************************************** 2741 C 2742 SUBROUTINE CENTRI(D122,D211,D311,D322) 2743 REAL M1,M2,M3,M4,M5,M6, 2744 . I1XX,I1YY,11ZZ,I1XY,I 1XZ,I 1YZ,I2XX,I2YY,12ZZ,12XY,12XZ,I2YZ, 2745 . I3XX,I3YY,I3ZZ,I3XY,I3XZ,I3YZ,I4XX,I4YY,I4ZZ,I4XY,I4XZ,I4YZ, 2746 I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,16YZ 2747 C0MM0N/BL0CK2/ 2748 I1XX,I1YY,I 1ZZ.I1XY,I 1XZ,I1YZ,I2XX,I2YY,I2ZZ,I2XY,I2XZ,I2YZ, 2749 I3XX,I3YY,I3ZZ,I3XY,13XZ,13YZ,I4XX,14YY,14ZZ,I4XY,14XZ , I4YZ, 2750 . I5XX,I5YY,I5ZZ,I5XY,I5XZ,I5YZ,I6XX,I6YY,I6ZZ,I6XY,I6XZ,16YZ 2751 C0MM0N/BL0CK3/D2,A2,D4,D6,M1,M2,M3,M4,M5,M6,GX,GY,GZ, 2752 X1,X2,X3,X4,X5,X6,Y1 ,Y2,Y3,Y4,Y5,Y6,Z1,Z2,Z3,Z4,Z5,Z6 2753 C0MM0N/BL0CK4/S1,C1,S2,C2,S3,C3,S23,C23,S4,C4,S5,C5,S6,C6,RAD 2754 C 2755 D122A=A2*Z2*M2*C2+A2*M2*D2*C2+X2*M2*D2*C2-I2YZ*S2-Y2*M2*D2*S2+ 2756 . I2XZ*C2 2757 C 2758 D122B=A2*Y3*M3*C2+A2*M3*D2*C2+Z3*M3*02*S3*C2+Z3*M3*D2*S2*C3+I3YZ* 2759 . S3*C2+I3YZ*S2*C3-I3XY*S3*S2+I3XY*C2*C3-X3*M3*D2*S3*S2+X3*M3*D2* 2760 . C2*C3 2761 C 2762 D122C=A2*X4*M4*S4*C2+A2*M4*Z4*C4*C2+A2*M4*D2*C2-I4YZ*S3*C4*C2-2763 . I4YZ*S2*C4*C3-Y4*M4*D2*S3*C2-Y4*M4*D2*S2*C3-I4XY*S3*S4*C2-I4XY* 2764 . S2*S4*C3-I4ZZ*S3*S2*S4*C4+I4ZZ*S4*C4*C2*C3+I4XX*S3*S2*S4*C4-I4XX 2765 . *S4*C4*C2*C3-2.*I4XZ*S3*S2*C4**2+I4XZ*S3*S2+2.*I4XZ*C4**2*C2*C3-2766 . I4XZ*C2*C3-X4*M4*D2*S3*S2*C4+X4*M4*D2*C4*C2*C3+X4*M4*D4*S3*S4*C2 2767 . +X4*M4*D4*S2*S4*C3+M4*Z4*D2*S3*S2*S4-M4*Z4*D2*S4*C2*C3+M4*Z4*D4* 2768 . S3*C4*C2+M4*Z4*D4*S2*C4*C3+M4*D2*D4*S3*C2+M4*D2*D4*S2*C3 2769 C 2770 D122D=A2*Y5*M5*C4*C2+A2*Z5*M5*S4*C2*S5+A2*D2*M5*C2+A2*X5*M5*S4*C5 2771 . *C2-2.*I5YZ*C4**2*S3*S2*S5+2.*I5YZ*C4**2*C2*C3*S5+I5YZ*C4*C5*S3* 2772 . C2+I5YZ*C4*C5*S2*C3+I5YZ*S3*S2*S5-I5YZ*C2*C3*S5+Y5*D2*M5*S4*S3* 2773 . S2-Y5*D2*M5*S4*C2*C3+Y5*D4*M5*C4*S3*C2+Y5*D4*M5*C4*S2*C3-Z5*D2* 2774 . M5*C4*S3*S2*S5+Z5*D2*M5*C4*C2*C3*S5+Z5*D2*M5*C5*S3*C2+Z5*D2*M5* 2775 . C5*S2*C3+Z5*D4*M5*S4*S3*C2*S5+Z5*D4*M5*S4*S2*C3*S5+D2*D4*M5*S3* 2776 . C2+D2*D4*M5*S2*C3-D2*X5*M5*C4*C5*S3*S2+D2*X5*M5*C4*C5*C2*C3-D2* 2777 . X5*M5*S3*C2*S5-D2*X5*M5*S2*C3*S5+D4*X5*M5*S4*C5*S3*C2+D4*X5*M5* 2778 . S4*C5*S2*C3-2.*15XY*C4* *2*C5*S3*S2+2.* 15XY*C4**2*C5*C2*C3-15XY* 2779 . C4*S3*C2*S5-I5XY*C4*S2*C3*S5+I5XY*C5*S3*S2-I5XY*C5*C2*C3+I5ZZ*S4 2780 . *C4*S3*S2*S5**2-I5ZZ*S4*C4*C2*C3*S5**2-I5ZZ*S4*C5*S3*C2*S5-I5ZZ* 2781 . S4*C5*S2*C3*S5-I5YY*S4*C4*S3*S2+I5YY*S4*C4*C2*C3-I5XX*S4*C4*S3* 2782 . S2*S5**2+I5XX*S4*C4*S3*S2+I5XX*S4*C4*C2*C3*S5**2-I5XX*S4*C4*C2* 2783 . C3+I5XX*S4*C5*S3*C2*S5+I5XX*S4*C5*S2*C3*S5-2.*I5XZ*S4*C4*C5*S3* 2784 . S2*S5+2.* 15XZ*S4*C4*C5*C2*C3*S5-2.*I5XZ*S4*S3*C2*S5**2+I5XZ*S4* 183. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 49 2785 . S3*C2-2.*I5XZ*S4*S2*C3*S5**2+I5XZ*S4*S2*C3 2786 C 2787 ANS2=2.*M6*Z6*D6*S4*C4*C2*C3*S5**2+2.*M6*Z6*06*S4*C5*S3*C2*S5+2.* 2788 . M6*Z6*D6*S4*C5*S2*C3*S5+M6*Y6*D2*S6*C4*C5*S3*S2-M6*Y6*D2*S6*C4* 2789 . C5*C2*C3+M6*Y6*D2*S6*S3*C2*S5+M6*Y6*D2*S6*S2*C3*S5+M6*Y6*D2*S4* 2790 . S3*S2*C6-M6*Y6*02*S4*C2*C3*C6-M6*Y6*D4*S6*S4*C5*S3*C2-M6*YG*D4* 2791 . S6*S4*C5*S2*C3+M6*Y6*D4*C4*S3*C2*C6+M6*Y6*04*C4*S2*C3*C6+2.*M6* 2792 . Y6*06*S6*S4*C4*C5*S3*S2*S5-2.*M6*Y6*06*S6*S4*C4*C5*C2*C3*S5+2.* 2793 . M6*Y6*D6*S6*S4*S3*C2*S5**2-M6*Y6*D6*S6*S4*S3*C2+2.*M6*Y6*D6*S6* 2794 . S4*S2*C3*S5**2-M6*Y6*D6*S6*S4*S2*C3-2.*M6*Y6*D6*C4**2*S3*S2*C6* 2795 . S5+2.*M6*Y6*D6*C4**2*C2*C3*C6*S5+M6*Y6*D6*C4*C5*S3*C2*C6+M6*Y6* 2796 . D6*C4*C5*S2*C3*C6+M6*Y6*D6*S3*S2*C6*S5-M6*Y6*D6*C2*C3*C6*S5+M6* , 2797 . D2*D4*S3*C2+M6*D2*D4*S2*C3-M6*02*D6*C4*S3*S2*S5+M6*D2*D6*C4*C2* 2798 . C3*S5+M6*D2*D6*C5*S3*C2+M6*02*D6*C5*S2*C3+M6*D4*D6*S4*S3*C2*S5+ 2799 . M6*D4*D6*S4*S2*C3*S5-M6*D6**2*S4*C4*S3*S2*S5**2+M6*D6**2*S4*C4* 2800 . C2*C3*S5**2+M6*D6**2*S4*C5*S3*C2*S5+M6*D6**2*S4*C5*S2*C3*S5+2.* 2801 . I6YZ*S6*S4*C4*C5*S3*S2*S5-2.*I6YZ*S6*S4*C4*C5*C2*C3*S5+2.*I6YZ* 2802 . S6*S4*S3*C2*S5**2-I6YZ*S6*S4*S3*C2+2.*I6YZ*S6*S4*S2*C3*S5**2-2803 . I6YZ*S6*S4*S2*C3-2.*I6YZ*C4**2*S3*S2*C6*S5+2.*I6YZ*C4**2*C2*C3* 2804 . C6*S5+I6YZ*C4*C5*S3*C2*C6+I6YZ*C4*C5*S2*C3*C6+I6YZ*S3*S2*C6*S5-2805 . I6YZ*C2*C3*C6*S5 2806 ANS1=-I6YY*S4*C4*C2*C3*C6**2*S5**2+2.*I6YY*S4*C4*C2*C3*C6**2+I6YY 2807 . *S4*C4*C2*C3*S5**2-I6YY*S4*C4*C2*C3-I6YY*S4*C5*S3*C2*C6**2*S5+ 2808 . I6YY*S4*C5*S3*C2*S5-I6YY*S4*C5*S2*C3*C6**2*S5+I6YY*S4*C5*S2*C3* 2809 . S5+2.*I6XX*S6*C4**2*C5*S3*S2*C6-2.*I6XX*S6*C4**2*C5*C2*C3*C6+ 2810 . I6XX*S6*C4*S3*C2*C6*S5+I6XX*S6*C4*S2*C3*C6*S5-I6XX*S6*C5*S3*S2* 2811 . C6+I6XX*S6*C5*C2*C3*C6-I6XX*S4*C4*S3*S2*C6**2*S5**2+2.*I6XX*S4* 2812 . C4*S3*S2*C6**2-I6XX*S4*C4*S3*S2+I6XX*S4*C4*C2*C3*C6**2*S5**2-2.* 2813 . I6XX*S4*C4*C2*C3*C6**2+I6XX*S4*C4*C2*C3+I6XX*S4*C5*S3*C2*C6**2* 2814 . S5+I6XX*S4*C5*S2*C3*C6**2*S5+X6*M6*D2*S6*S4*S3.*S2-X6*M6*D2*S6*S4 2815 . *C2*C3-X6*M6*D2*C4*C5*S3*S2*C6+X6*M6*D2*C4*C5*C2*C3*C6-X6*M6*D2* 2816 . S3*C2*C6*S5-X6*M6*D2*S2*C3*C6*S5+X6*M6*D4*S6*C4*S3*C2+X6*M6*04* 2817 . S6*C4*S2*C3+X6*M6*D4*S4*C5*S3*C2*C6+X6*M6*D4*S4*C5*S2*C3*C6-2.* 2818 . X6*M6*D6*S6*C4**2*S3*S2*S5+2.*X6*M6*D6*S6*C4**2*C2*C3*S5+X6*M6* 2819 . D6*S6*C4*C5*S3*C2+X6*M6*D6*S6*C4*C5*S2*C3+X6*M6*06*S6*S3*S2*S5-2820 . X6*M6*06*S6*C2*C3*S5-2.*X6*M6*D6*S4*C4*C5*S3*S2*C6*S5+2.*X6*MG* 2821 . D6*S4*C4*C5*C2*C3*C6*S5-2.*X6*M6*D6*S4*S3*C2*C6*S5**2+X6*M6*06* 2822 . S4*S3*C2*C6-2.*X6*M6*D6*S4*S2*C3*C6*S5**2+X6*M6*D6*S4*S2*C3*C6-2823 . M6*Z6*D2*C4*S3*S2*S5+M6*Z6*02*C4*C2*C3*S5+M6*Z6*D2*C5*S3*C2+M6* 2824 . Z6*D2*C5*S2*C3+M6*Z6*D4*S4*S3*C2*S5+M6*Z6*D4*S4*S2*C3*S5-2.*M6* 2825 . Z6*D6*S4*C4*S3*S2*S5**2+ANS2 2826 D122E=A2*X6*M6*S6*C4*C2+A2*X6*M6*S4*C5*C2*C6+A2*M6*Z6*S4*C2*S5-A2 2827 . *M6*Y6*S6*S4*C5*C2+A2*M6*Y6*C4*C2*C6+A2*M6*D2*C2+A2*M6*D6*S4*C2* 2828 . S5-2.*I6XZ*S6*C4**2*S3*S2*S5+2.*I6XZ*S6*C4**2*C2*C3*S5+I6XZ*S6* 2829 . C4*C5*S3*C2+I6XZ*S6*C4*C5*S2*C3+I6XZ*S6*S3*S2*S5-I"6XZ*S6*C2*C3* 2830 . S5-2 . *I6XZ*S4*C4*C5*S3*S2*C6*S5+2. *I6XZ*S4*C4*C5*C2*C3*C6*S5-2.* 2831 . I6XZ*S4*S3*C2*C6*S5**2+I6XZ*S4*S3*C2*C6-2.*I6XZ*S4*S2*C3*C6*S5** 2832 . 2+I6XZ*S4*S2*C3*C6-2.*I6XY*S6*S4*C4*S3*S2*C6*S5**2+4.*I6XY*S6*S4 2833 . *C4*S3*S2*C6+2.*I6XY*S6*S4*C4*C2*C3*C6*S5**2-4.*I6XY*S6*S4*C4*C2 2834 . *C3*C6+2 . *I6XY*S6*S4*C5*S3*C2*C6*S5+2.*I6XY*S6*S4*C5*S2*C3*C6*S5 2835 . -4.*I6XY*C4**2*C5*S3*S2*C6**2+2.*I6XY*C4* *2*C5*S3*S2+4.*I6XY*C4 2836 . **2»C5*C2*C3*C6**2-2.*I6XY*C4**2*C5*C2*C3-2.*I6XY*C4*S3*C2*C6**2 2837 . *S5+I6XY*C4*S3*C2*S5-2 . *I6XY*C4*S2*C3*C6**2*S5+16XY*C4*S2*C3*S5+ 2838 . 2.*I6XY*C5*S3*S2*C6**2-I6XY*C5*S3*S2-2.*I6XY*C5*C2*C3*C6**2+I6XY* 2839 . C5*C2*C3+I6ZZ*S4*C4*S3*S2*S5**2-I6ZZ*S4*C4*C2*C3*S5**2-I6ZZ*S4* 2840 . C5*S3*C2*S5-I6ZZ*S4*C5*S2*C3*S5-2.*I6YY*S6*C4**2*C5*S3*S2*C6+2 . * 2841 . I6YY*S6*C4**2*C5*C2*C3*C6-I6YY*S6*C4*S3*C2*C6*S5-I6YY*S6*C4*S2* 2842 . C3*C6*S5+I6YY*S6*C5*S3*S2*C6-I6YY*S6*C5*C2*C3*C6+I6YY*S4*C4*S3* 184. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 50 2843 . S2*C6**2*S5**2-2.*I6YY*S4*C4*S3*S2*C6**2-I6YY*S4*C4*S3*S2*S5**2+ 2844 . I6YY*S4*C4*S3*S2+ANS1 2845 C 2846 D122=D122A+D122B+D122C+D122D+D122E 2847 C 2848 D211A=A2**2*M2*S2*C2+2.*A2*X2*M2*S2*C2+2.*A2*Y2*M2*C2**2-A2*Y2*M2 2849 . +I2YY*S2*C2-I2XX*S2*C2+2.* 12XY*C2**2-12XY 2850 C 2851 0211B=A2**2*M3*S2*C2+2.*A2*Z3*M3*S3*S2*C2-2.*A2*Z3*M3*C2**2*C3+A2 2852 . *Z3*M3*C3+2.*A2*X3*M3*S3*C2**2-A2*X3*M3*S3+2.*A2*X3*M3*S2*C2*C3+ 2853 . 2.*I3ZZ*S3*C2**2*C3-I3ZZ*S3*C3+2.*I3ZZ*S2*C2*C3**2-I3ZZ*S2*C2-2.* 2854 . I3XX*S3*C2**2*C3+I3XX*S3*C3-2.*I3XX*S2*C2*C3**2+I3XX*S2*C2+4 . * 2855 . I3XZ*S3*S2*C2*C3-4.*I3XZ*C2**2*C3**2+2.* I3XZ*C2**2+2.*I3XZ*C3**2 2856 . -I3XZ 2857 C 2858 ANSI = -2.*M4*Z4*04*S4*C2**2-2.*M4*Z4*D4*S4*C3**2+M4*Z4*04*S4-2 . *M4 2859 . *D4**2*S3*C2**2*C3+M4*D4**2*S3*C3-2.*M4*D4**2*S2*C2*C3**2+M4*04 2860 . **2*S2*C2 2861 0211C=A2**2*M4*S2*C2-2.*A2*Y4*M4*S3*S2*C2+2.*A2*Y4*M4*C2**2*C3-A2 2862 . *Y4*M4*C3+2.*A2*X4*M4*S3*C4*C2**2-A2*X4*M4*S3*C4+2.*A2*X4*M4*S2* 2863 . C4*C2*C3-2.*A2*M4*Z4*S3*S4*C2**2+A2*M4*Z4*S3*S4-2.*A2*M4*Z4*S2* 2864 . S4*C2*C3+2.*A2*M4*D4*S3*S2*C2-2.*A2*M4*D4*C2**2*C3+A2*M4*D4*C3+ 2865 . 4.*I4YZ*S3*S2*S4*C2*C3-4.*I4YZ*S4*C2**2*C3**2+2.*I4YZ*S4*C2**2+2. 2866 . *I4YZ*S4*C3**2-I4YZ*S4+4.*Y4*M4*D4*S3*C2**2*C3-2.*Y4*M4*D4*S3*C3 2867 . +4.*Y4*M4*D4*S2*C2*C3**2-2.*Y4*M4*D4*S2*C2-4.*I4XY*S3*S2*C4*C2* 2868 . C3+4.*I4XY*C4*C2**2*C3**2-2.* 14XY*C4*C2**2-2.* 14XY*C4*C3**2+14XY 2869 . *C4+2.*I4ZZ*S3*C4**2*C2**2*C3-I4ZZ*S3*C4**2*C3-2.*I4ZZ*S3*C2**2* 2870 . C3+I4ZZ*S3*C3+2.*I42Z*S2*C4**2*C2*C3**2-I4ZZ*S2*C4**2*C2-2.*I4ZZ 2871 . *S2*C2*C3**2+I4ZZ*S2*C2+2 . *I4YY*S3*C2**2*C3-I4YY*S3*C3+2.*I4YY* 2872 . S2*C2*C3**2-I4YY*S2*C2-2 . * 14XX*S3*C4**2*C2**2*C3+14XX*S3*C4**2* 2873 . C3-2.*I4XX*S2*C4**2*C2*C3**2+I4XX*S2*C4**2*C2-4.*I4XZ*S3*S4*C4* 2874 . C2**2*C3+2.* 14XZ*S3*S4*C4*C3-4.*I4XZ*S2*S4*C4*C2*C3**2+2.*I4XZ* 2875 . S2*S4*C4*C2+4.*X4*M4*D4*S3*S2*C4*C2*C3-4.*X4*M4*D4*C4*C2**2*C3** 2876 . 2+2.*X4*M4*D4*C4*C2**2+2.*X4*M4*D4*C4*C3**2-X4*M4*D4*C4-4.*M4*Z4 2877 . *D4*S3*S2*S4*C2*C3+4.*M4*Z4*D4*S4*C2**2*C3**2+ANS1 2878 C 2879 ANS2=4.*I5XZ*C4*S2**2*C3**2+4.*I5XZ*C4*S2**2*S5**2-2.*I5XZ*C4*S2 2880 . **2+4.*I5XZ*C4*C3**2*S5**2-2.* 15XZ*C4*C3**2-2.*I5XZ*C4*S5**2+ 2881 . I5XZ*C4-4.*I5XZ*C5*S3*S2**2*C3*S5+2. *I5XZ*C5*S3*C3*S5+4.*I5XZ*C5 2882 . *C2*S2*C3**2*S5-2.*I5XZ*C5*C2*S2*S5+4.*I5XY*S4*C4*C5*S3*S2**2*C3 2883 . -2.*I5XY*S4*C4*C5*S3*C3-4.*I5XY*S4*C4*C5*C2*S2*C3**2+2.*I5XY*S4* 2884 . C4*C5*C2*S2+4.*I5XY*S4*S3*C2*S2*C3*S5+4.* 15XY*S4*S2**2*C3**2*S5-2885 . 2.*I5XY*S4*S2**2*S5-2.*I5XY*S4*C3**2*S5+I5XY*S4*S5+4 . *S4*C4*S3*S2 2886 . **2*C3*S5*I5YZ-2.*S4*C4*S3*C3*S5*I5YZ-4.*S4*C4*C2*S2*C3**2*S5* 2887 . I5YZ+2.*S4*C4*C2*S2*S5*I5YZ-4.*S4*C5*S3*C2*S2*C3*I5YZ-4.*S4*C5* 2888 . S2**2*C3**2*I5YZ+2.*S4*C5*S2**2* 15YZ + 2.*S4*C5*C3**2* 15YZ-S4*C5* 2889 . I5YZ 2890 ANS1=-4.*I5ZZ*C4*C5*S3*C2*S2*C3*S5-4.*I5ZZ*C4*C5*S2**2*C3**2*S5+ 2891 . 2.*I5ZZ*C4*C5*S2**2*S5+2.*I5ZZ*C4*C5*C3**2*S5-I5ZZ*C4*C5*S5+2.* 2892 . I5ZZ*S3*S2**2*C3*S5**2-2.*I5ZZ*S3*S2**2*C3-I5ZZ*S3*C3*S5**2+I5ZZ 2893 . *S3*C3-2.*I5ZZ*C2*S2*C3**2*S5**2+2.*I5ZZ*C2*S2*C3**2+I5ZZ*C2*S2* 2894 . S5**2-I5ZZ*C2*S2-2.*I5YY*C4**2*S3*S2**2*C3+I5YY*C4**2*S3*C3+2 . * 2895 . I5YY*C4**2*C2*S2*C3**2-I5YY*C4**2*C2*S2+2.*I5YY*S3*S2**2*C3-I5YY 2896 . *S3*C3-2.*I5YY*C2*S2*C3**2+I5YY*C2*S2-2.* 15XX*C4**2*S3*S2**2*C3* 2897 . S5**2+2.*I5XX*C4**2*S3*S2**2*C3+I5XX*C4**2*S3*C3*S5**2-I5XX*C4** 2898 . 2*S3*C3+2.*I5XX*C4**2*C2*S2*C3**2*S5**2-2.*I5XX*C4**2*C2*S2*C3** 2899 . 2-I5XX*C4**2*C2*S2*S5**2+I5XX*C4**2*C2*S2+4.*I5XX*C4*C5*S3*C2*S2 2900 . *C3*S5+4.*I5XX*C4*C5*S2**2*C3**2*S5-2.*I5XX*C4*C5*S2**2*S5-2 . * 185. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 51 2901 . I5XX*C4*C5*C3**2*S5+I5XX*C4*C5*S5-2.*I5XX*S3*S2**2*C3*S5*+2+I5XX 2902 . *S3*C3*S5**2+2.*I5XX*C2*S2*C3**2*S5**2-I5XX*C2*S2*S5**2-4.*I5XZ* 2903 . C4**2*C5*S3*S2**2*C3*S5+2.*I5XZ*C4**2*C5*S3*C3*S5+4.*I5XZ*C4**2* 2904 . C5*C2*S2*C3**2*S5-2.*I5XZ*C4**2*C5*C2*S2*S5-8.*I5XZ*C4*S3*C2*S2* 2905 . C3*S5**2+4.*15XZ*C4*S3*C2*S2*C3-8.*I5XZ*C4*S2**2*C3**2*S5**2+ 2906 . ANS2 2907 D211D=A2**2*M5*C2*S2-2.*A2*Z5*M5*C4*S3*S2**2*S5+A2*Z5*M5*C4*S3*S5 2908 . +2.*A2*Z5*M5*C4*C2*S2*C3*S5+2.*A2*Z5*M5*C5*S3*C2*S2+2.*A2*Z5*M5* 2909 . C5*S2**2*C3-A2*Z5*M5*C5*C3+2.*A2*D4*M5*S3*C2*S2+2.*A2*D4*M5*S2** 2910 . 2*C3-A2*D4*M5*C3-2.*A2*X5*M5*C4*C5*S3*S2**2+A2*X5*M5*C4*C5*S3+2. 2911 . *A2*X5*M5*C4*C5*C2*S2*C3-2.*A2*X5*M5*S3*C2*S2*S5-2.*A2*X5*M5*S2 2912 . **2*C3*S5+A2*X5*M5*C3*S5+2.*A2*M5*S4*S3*S2**2*Y5-A2*M5*S4*S3*Y5-2913 . 2.*A2*M5*S4*C2*S2*C3*Y5+4.*Z5*D4*M5*C4*S3*C2*S2*C3*S5+4.*Z5*D4*M5 2914 . *C4*S2**2*C3**2*S5-2.*Z5*04*M5*C4*S2**2*S5-2.*Z5*D4*M5*C4*C3**2* 2915 . S5+Z5*D4*M5*C4*S5+4.*Z5*D4*M5*C5*S3*S2**2*C3-2.*Z5*D4*M5*C5*S3* 2916 . C3-4.*Z5*D4*M5*C5*C2*S2*C3**2+2.*Z5*D4*M5*C5*C2*S2+2.*D4**2*M5* 2917 . S3*S2**2*C3-D4**2*M5*S3*C3-2.*D4**2*M5*C2*S2*C3**2+D4**2*M5*C2* 2918 . S2+4.*D4*X5*M5*C4*C5*S3*C2*S2*C3+4.*D4*X5*M5*C4*C5*S2**2*C3**2-2919 . 2.*D4*X5*M5*C4*C5*S2**2-2.*D4*X5*M5*C4*C5*C3**2+D4*X5*M5*C4*C5-4. 2920 . *D4*X5*M5*S3*S2**2*C3*S5+2.*D4*X5*M5*S3*C3*S5+4.*D4*X5*M5*C2*S2* 2921 . C3**2*S5-2.*D4*X5*M5*C2*S2*S5-4.*D4*M5*S4*S3*C2*S2*C3*Y5-4.*D4* 2922 . M5*S4*S2**2*C3**2*Y5+2.*D4*M5*S4*S2**2*Y5+2.*D4*M5*S4*C3**2*Y5-2923 . D4*M5*S4*Y5+2.*I5ZZ*C4**2*S3*S2**2*C3*S5**2-I5ZZ*C4**2*S3*C3*S5 2924 . **2-2.*I5ZZ*C4**2*C2*S2*C3**2*S5**2+I5ZZ*C4**2*C2*S2*S5**2+ANS1 2925 C 2926 ANS8=-M6*D6**2*S3*C3+2.*M6*D6**2*C2*S2*C3**2*S5**2-2.*M6*D6**2*C2 2927 . *S2*C3**2-M6*D6**2*C2*S2*S5**2+M6*D6**2*C2*S2+4.*I6YZ*S6*C4**2* 2928 . C5*S3*S2**2*C3*S5-2.*I6YZ*S6*C4**2*C5*S3*C3*S5-4.*I6YZ*S6*C4**2* 2929 . C5*C2*S2*C3**2*S5+2.*I6YZ*S6*C4**2*C5*C2*S2*S5+8.*I6YZ*S6*C4*S3* 2930 . C2*S2*C3*S5**2-4.*I6YZ*S6*C4*S3*C2*S2*C3+8.*I6YZ*S6*C4*S2**2*C3 2931 . **2*S5**2-4.*I6YZ*S6*C4*S2**2*C3**2-4.*I6YZ*S6*C4*S2**2*S5**2+2. 2932 . *I6YZ*S6*C4*S2**2-4.*I6YZ*S6*C4*C3**2*S5**2+2.*I6YZ*S6*C4*C3**2+ 2933 . 2.*I6YZ*S6*C4*S5**2-I6YZ*S6*C4+4.*I6YZ*S6*C5*S3*S2**2*C3*S5-2 . * 2934 . I6YZ*S6*C5*S3*C3*S5-4 . *I6YZ*S6*C5*C2*S2*C3**2*S5+2.*I6YZ*S6*C5* 2935 . C2*S2*S5+4.*I6YZ*S4*C4*S3*S2**2*C3*C6*S5-2.*I6YZ*S4*C4*S3*C3*C6* 2936 . S5-4.*I6YZ*S4*C4*C2*S2*C3**2*C6*S5+2.*I6YZ*S4*C4*C2*S2*C6*S5-4.* 2937 . I6YZ*S4*C5*S3*C2*S2*C3*C6-4.*I6YZ*S4*C5*S2**2*C3**2*C6+2.*I6YZ* 2938 . S4*C5*S2**2*C6+2.*I6YZ*S4*C5*C3**2*C6-I6YZ*S4*C5*C6 2939 ANS7=-4.*M6*Y6*D6*S6*C4*C3**2*S5**2+2.*M6*Y6*D6*S6*C4*C3**2+2.*M6 2940 . *Y6*D6*S6*C4*S5**2-M6*Y6*06*S6*C4+4.*M6*YG*D6*S6*C5*S3*S2**2*C3* 2941 . S5-2.*M6*Y6*D6*S6*C5*S3*C3*S5-4.*M6*Y6*D6*S6*C5*C2*S2*C3**2*S5+ 2942 . 2 . *M6*Y6*D6*S6*C5*C2*S2*S5+4 . *M6*Y6*D6*S4*C4*S3*S2**2*C3*C6*S5-2 . 2943 . *M6*Y6*D6*S4*C4*S3*C3*C6*S5-4.*M6*Y6*D6*S4*C4*C2*S2*C3**2*C6*S5+ 2944 . 2.*M6*Y6*D6*S4*C4*C2*S2*C6*S5-4.*M6*Y6*D6*S4*C5*S3*C2*S2*C3*C6-4. 2945 . *M6*Y6*D6*S4*C5*S2**2*C3**2*C6+2.*M6*Y6*D6*S4*C5*S2**2*C6+2.*M6* 2946 . Y6*D6*S4*C5*C3**2*C6-M6*Y6*D6*S4*C5*C6+2.*M6*D4**2*S3*S2**2*C3-2947 . M6*D4**2*S3*C3-2.*M6*D4**2*C2*S2*C3**2+M6*D4* *2*C2*S2+4.*M6*D4* 2948 . D6*C4*S3*C2*S2*C3*S5+4.*M6*D4*D6*C4*S2**2*C3**2*S5-2.*M6*D4*D6* 2949 . C4*S2**2*S5-2.*M6*D4*D6*C4*C3**2*S5+M6*D4*D6*C4*S5+4.*M6*D4*D6* 2950 . C5*S3*S2**2*C3-2.*M6*D4*D6*C5*S3*C3-4.*M6*D4*D6*C5*C2*S2*C3**2+ 2951 . 2.*M6*04*D6*C5*C2*S2-2.*M6*D6**2*C4**2*S3*S2**2*C3*S5**2+M6*D6**2 2952 . *C4**2*S3*C3*S5**2+2.*M6*06**2*C4**2*C2*S2*C3**2*S5**2-M6*D6**2* 2953 . C4**2*C2*S2*S5**2+4.*M6*D6**2*C4*C5*S3*C2*S2*C3*S5+4.*M6*D6**2* 2954 . C4*C5*S2**2*C3**2*S5-2.*M6*D6**2*C4*C5*S2**2*S5-2.*M6*D6**2*C4* 2955 . C5*C3**2*S5+M6*D6**2*C4*C5*S5-2.*M6*06**2*S3*S2**2*C3*S5**2+2.* 2956 . M6*D6**2*S3*S2**2*C3+M6*D6**2*S3*C3*S5**2+ANS8 2957 ANS6=-4.*M6*Z6*D6*C4**2*S3*S2**2*C3*S5**2+2.*M6*Z6*D6*C4**2*S3*C3 2958 . *S5**2+4.*M6*Z6*D6*C4**2*C2*S2*C3**2*S5**2-2.*M6*Z6*D6*C4**2*C2* 186. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 52 2959 . S2*S5**2+8.*M6*Z6*D6*C4*C5*S3*C2*S2*C3*S5+8.*M6*Z6*D6*C4*C5*S2** 2960 . 2*C3**2*S5-4.*M6*Z6*D6*C4*C5*S2**2*S5-4.*M6*Z6*D6*C4*C5*C3**2*S5 2961 . +2.*M6*Z6*D6*C4*C5*S5-4.*M6*Z6*D6*S3*S2**2*C3*S5**2+4.*M6*Z6*D6* 2962 . S3*S2**2*C3+2.*M6*Z6*D6*S3*C3*S5**2-2.*M6*Z6*D6*S3*C3+4.*M6*Z6* 2963 . D6*C2*S2*C3**2*S5**2-4.*M6*Z6*D6*C2*S2*C3**2-2.*M6*Z6*D6*C2*S2* 2964 . S5**2+2.*M6*Z6*D6*C2*S2-4.*M6*Y6*D4*S6*C4*C5*S3*C2*S2*C3-4.*M6* 2965 . Y6*D4*S6*C4*C5*S2**2*C3**2+2.*M6*Y6*D4*S6*C4*C5*S2**2+2.*M6*Y6* 2966 . D4*S6*C4*C5*C3**2-M6*Y6*D4*S6*C4*C5+4.*M6*Y6*D4*S6*S3*S2**2*C3* 2967 . S5-2.*M6*Y6*D4*S6*S3*C3*S5-4.*M6*Y6*D4*S6*C2*S2*C3**2*S5+2.*M6* 2968 . Y6*D4*S6*C2*S2*S5-4.*M6*Y6*04*S4*S3*C2*S2*C3*C6-4.*M6*Y6*D4*S4* 2969 . S2**2*C3**2*C6+2.*M6*Y6*D4*S4*S2**2*C6+2.*M6*Y6*D4*S4*C3**2*C6-2970 . M6*Y6*D4*S4*C6+4.*M6*Y6*06*S6*C4**2*C5*S3*S2**2*C3*S5-2.*M6*Y6* 2971 . D6*S6*C4**2*C5*S3*C3*S5-4.*M6*Y6*D6*S6*C4**2*C5*C2*S2*C3**2*S5+ 2972 . 2.*M6*Y6*D6*S6*C4**2*C5*C2*S2*S5+8.*M6*Y6*D6*S6*C4*S3*C2*S2*C3*S5 2973 . **2-4.*M6*Y6*D6*S6*C4*S3*C2*S2*C3+8.*M6*Y6*D6*S6*C4*S2**2*C3**2* 2974 . S5**2-4.*M6*Y6*D6*S6*C4*S2**2*C3**2-4.*M6*Y6*D6*S6*C4*S2**2*S5** 2975 . 2+2.*M6*Y6*D6*S6*C4*S2**2+ANS7 2976 ANS5=-2.*X6*M6*D4*C4*C5*C3**2*C6+X6*M6*D4*C4*C5*C6-4.*X6*M6*D4*S3 2977 . *S2*»2*C3*C6*S5+2.»X6*M6*D4*S3*C3*C6*S5+4.*X6*M6*D4*C2*S2*C3**2* 2978 . C6*S5-2.*X6*M6*D4*C2*S2*C6*S5+4.*X6*M6*D6*S6*S4*C4*S3*S2**2*C3* 2979 . S5-2.*X6*M6*D6*S6*S4*C4*S3*C3*S5-4.*X6*M6*D6*S6*S4*C4*C2*S2*C3** 2980 . 2*S5+2.*X6*M6*D6*S6*S4*C4*C2*S2*S5-4.*X6*M6*D6*S6*S4*C5*S3*C2*S2 2981 . *C3-4.*X6*M6*D6*S6*S4*C5*S2**2*C3**2+2.*X6*M6*D6*S6*S4*C5*S2**2+ 2982 . 2.*X6*M6*D6*S6*S4*C5*C3**2-X6*M6*D6*S6*S4*C5-4.*X6*M6*D6*C4**2*C5 2983 . *S3*S2**2*C3*C6*S5+2.*X6*M6*06*C4**2*C5*S3*C3*C6*S5+4.*X6*M6*D6* 2984 . C4**2*C5*C2*S2*C3**2*C6*S5-2.*X6*M6*D6*C4**2*C5*C2*S2*C6*S5-8.* 2985 . X6*M6*D6*C4*S3*C2*S2*C3*C6*S5**2+4 . *X6*M6*D6*C4*S3*C2*S2*C3*C6-2986 . 8.*X6*M6*D6*C4*S2**2*C3**2*C6*S5**2+4.*X6*M6*D6*C4*S2**2*C3**2*C6 2987 . +4.*X6*M6*D6*C4*S2**2*C6*S5**2-2.*X6*M6*D6*C4*S2**2*C6+4.*X6*M6* 2988 . D6*C4*C3**2*C6*S5**2-2.*X6*M6*D6*C4*C3**2*C6-2.*X6*M6*D6*C4*C6* 2989 . S5**2+X6*M6*D6*C4*C6-4.*X6*M6*D6*C5*S3*S2**2*C3*C6*S5+2.*X6*M6* 2990 . D6*C5*S3*C3*C6*S5+4.*X6*M6*D6*C5*C2*S2*C3**2*C6*S5-2.*X6*M6*D6* 2991 . C5*C2*S2*C6*S5+4.*M6*Z6*D4*C4*S3*C2*S2*C3*S5+4.*M6*Z6*D4*C4*S2** 2992 . 2*C3**2*S5-2.*M6*Z6*04*C4*S2**2*S5-2.*M6*Z6*D4*C4*C3**2*S5+M6*Z6 2993 . *D4*C4*S5+4.*M6*Z6*D4*C5*S3*S2**2*C3-2.*M6*Z6*D4*C5*S3*C3-4.*M6* 2994 . Z6*D4*C5*C2*S2*C3**2+2.*M6*Z6*D4*C5*C2*S2+ANS6 2995 ANS4 = -8.* 16XY*S6*C4**2*C2*S2*C3**2*C6-2.*I6XY *S6*C4* *2 *C2 *S2 *C6* 2996 . S5**2+4.*I6XY*S6*C4**2*C2*S2*C6+8.* 16XY*S6*C4*C5*S3*C2*S2*C3*C6* 2997 . S5+8.*I6XY*S6*C4*C5*S2**2*C3**2*C6*S5-4.*I6XY*S6*C4*C5*S2**2*C6* 2998 . S5-4.*I6XY*S6*C4*C5*C3**2*C6*S5+2.*I6XY*S6*C4*C5*C6*S5-4.*I6XY* 2999 . S6*S3*S2**2*C3*C6*S5**2-4.*I6XY*S6*S3*S2**2*C3*C6+2.*I6XY*S6*S3* 3000 . C3*C6*S5**2+2.* 16XY*S6*S3*C3*C6+4.*I6XY*S6*C2*S2*C3**2*C6*S5**2+ 3001 . 4.*I6XY*S6*C2*S2*C3**2*C6-2.*I6XY*S6*C2*S2*C6*S5**2-2.*I6XY*S6*C2 3002 . *S2*C6+8.*I6XY*S4*C4*C5*S3*S2**2*C3*C6**2-4.* 16XY*S4*C4*C5*S3*S2 3003 . **2*C3-4.*I6XY*S4*C4*C5*S3*C3*C6**2+2.*I6XY*S4*C4*C5*S3*C3-8.* 3004 . I6XY*S4*C4*C5*C2*S2*C3**2*C6**2+4.*I6XY*S4*C4*C5*C2*S2*C3**2+4 . * 3005 . I6XY*S4*C4*C5*C2*S2*C6**2-2.*I6XY*S4*C4*C5*C2*S2+8.*I6XY*S4*S3* 3006 . C2*S2*C3*C6**2*S5-4.*I6XY*S4*S3*C2*S2*C3*S5+8.*I6XY*S4*S2**2*C3 3007 . **2*C6**2*S5-4.*I6XY*S4*S2**2*C3**2*S5-4.*I6XY*S4*S2**2*C6**2*S5 3008 . +2.*I6XY*S4*S2**2*S5-4 . *I6XY*S4*C3**2*C6**2*S5+2.*I6XY*S4*C3**2* 3009 . S5+2.*I6XY*S4*C6**2*S5-I6XY*S4*S5-4.*X6*M6*D4*S6*S4*S3*C2*S2*C3-3010 . 4.*X6*M6*04*S6*S4*S2**2*C3**2+2.*X6*M6*04*S6*S4*S2**2+2.*X6*M6*D4 3011 . *S6*S4*C3**2-X6*M6*D4*S6*S4+4.*X6*M6*D4*C4*C5*S3*C2*S2*C3*C6+4 . * 3012 . X6*M6*D4*C4*C5*S2**2*C3**2*C6-2.*X6*M6*D4*C4*C5*S2**2*C6+ANS5 3013 ANS3=-4.*I6XX*S6*S4*S2**2*C3**2*C6*S5+2.*I6XX*S6*S4*S2**2*C6*S5+ 3014 .2.*I6XX*S6*S4*C3**2*C6*S5-I6XX*S6*S4*C6*S5-2.*I6XX*C4**2*S3*S2**2 3015 . *C3*C6**2*S5**2+4.*I6XX*C4**2*S3*S2**2*C3*C6**2-2.*I6XX*C4**2*S3 3016 . *S2**2*C3+I6XX*C4**2*S3*C3*C6**2*S5**2-2.*I6XX*C4**2*S3*C3*C6**2 187. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 53 3017 . +I6XX*C4**2*S3*C3+2.*I6XX*C4**2*C2*S2*C3**2*C6**2*S5**2-4.*ISXX* 3018 . C4**2*C2*S2*C3**2*C6**2+2.*I6XX*C4**2*C2*S2*C3**2-I6XX*C4**2*C2* 3019 . S2*C6**2*S5**2+2.*I6XX*C4**2*C2*S2*C6**2-I6XX*C4**2*C2*S2+4.* 3020 . I6XX*C4*C5*S3*C2*S2*C3*C6**2*S5+4.*I6XX*C4*C5*S2**2*C3**2*C6**2* 3021 . S5-2.*I6XX*C4*C5*S2**2*CG**2*S5-2.*I6XX*C4*C5*C3**2*C6**2*S5+ 3022 . I6XX*C4*C5*C6**2*S5-2.*I6XX*S3*S2**2*C3*C6**2*S5**2-2.*I6XX*S3* 3023 . S2**2*C3*C6**2+2.*I6XX*S3*S2**2*C3+I6XX*S3*C3*C6**2*S5**2+I6XX* 3024 . S3*C3*C6**2-I6XX*S3*C3+2.*I6XX*C2*S2*C3**2*C6**2*S5**2+2.*I6XX* 3025 . C2*S2*C3**2*C6**2-2. *I6XX*C2*S2*C3**2-I6XX*C2*S2*C6**2*S5**2-3026 . I6XX*C2*S2*C6**2+I6XX*C2*S2-4.*I6XY*S6*C4**2*S3*S2**2*C3*C6*S5** 3027 . 2+8.*I6XY*S6*C4**2*S3*S2**2*C3*C6+2.*I6XY*S6*C4 * *2 *S3*C3*C6*S5* * 3028 . 2-4.*I6XY*S6*C4**2*S3*C3*C6+4.*I6XY*S6*C4**2*C2*S2*C3**2*C6*S5** 3029 . 2+ANS4 3030 ANS2=-I6YY*C4**2*S3*C3*C6**2*S5**2+2.*I6YY*C4**2*S3*C3*C6**2+I6YY 3031 . *C4**2*S3*C3*S5**2-I6YY*C4**2*S3*C3-2.*I6YY*C4**2*C2*S2*C3**2*C6 3032 . **2*S5**2+4.*I6YY*C4**2*C2*S2*C3**2*C6**2+2.*I6YY*C4**2*C2*S2*C3 3033 . **2*S5**2-2.*I6YY*C4**2*C2*S2*C3**2+I6YY*C4**2*C2*S2*C6**2*S5**2 3034 . -2.*I6YY*C4**2*C2*S2*C6**2-I6YY*C4**2*C2*S2*S5**2+I6YY*C4**2*C2* 3035 . S2-4.*I6YY*C4*C5*S3*C2*S2*C3*C6**2*S5+4.*I6YY*C4*C5*S3*C2*S2*C3* 3036 . S5-4.*I6YY*C4*C5*S2**2*C3**2*C6**2*S5+4.*I6YY*C4*C5*S2**2*C3**2* 3037 . S5+2 . *I6YY*C4*C5*S2**2*C6**2*S5-2.*I6YY*C4*C5*S2**2*S5+2.*I6YY* 3038 . C4*C5*C3**2*C6**2*S5-2.*I6YY*C4*C5*C3**2*S5-I6YY*C4*C5*C6**2*S5+ 3039 . I6YY*C4*C5*S5+2.*I6YY*S3*S2**2*C3*C6**2*S5**2+2.*I6YY*S3*S2**2* 3040 . C3*C6**2-2.*I6YY*S3*S2**2*C3*S5**2-I6YY*S3*C3*C6**2*S5**2-I6YY* 3041 . S3*C3*C6**2+I6YY*S3*C3*S5**2-2.* 16YY*C2*S2*C3**2*C6**2*S5**2-2.* 3042 . I6YY*C2*S2*C3**2*C6**2+2.*I6YY*C2*S2*C3**2*S5**2+I6YY*C2*S2*C6** 3043 . 2*S5**2+I6YY*C2*S2*C6**2-I6YY*C2*S2*S5**2-4.*I6XX*S6*S4*C4*C5*S3 3044 . *S2**2*C3*C6+2.*I6XX*S6*S4*C4*C5*S3*C3*C6+4.*I6XX*S6*S4*C4*C5*C2 3045 . *S2*C3**2*C6-2.*I6XX*S6*S4*C4*C5*C2*S2*C6-4.*I6XX*S6*S4*S3*C2*S2 3046 . *C3*C6*S5+ANS3 3047 ANSI=-8.*I6XZ*C4*S2**2*C3**2*C6*S5**2+4.*I6XZ*C4*S2**2*C3**2*C6+ 3048 . 4.*I6XZ*C4*S2**2*C6*S5**2-2.* 16XZ*C4*S2**2*C6+4.*I6XZ*C4*C3**2*C6 3049 . *S5**2-2.*I6XZ*C4*C3**2*C6-2.*I6XZ*C4*C6*S5**2+I6XZ*C4*C6-4 . * 3050 . I6XZ*C5*S3*S2**2*C3*C6*S5+2 . *I6XZ*C5*S3*C3*CG*S5+4.*IGXZ*C5*C2* 3051 . S2*C3**2*C6*S5-2.*I6XZ*C5*C2*S2*C6*S5+2.*I6ZZ*C4**2*S3*S2**2*C3* 3052 . S5**2-I6ZZ*C4**2*S3*C3*S5**2-2.*I6ZZ*C4**2*C2*S2*C3**2*S5**2+ 3053 . I6ZZ*C4**2*C2*S2*S5**2-4.*I6ZZ*C4*C5*S3*C2*S2*C3*S5-4.*I6ZZ*C4* 3054 . C5*S2**2*C3**2*S5+2.*I6ZZ*C4*C5*S2**2*S5+2.*I6ZZ*C4*C5*C3**2*S5-3055 . I6ZZ*C4*C5*S5+2.*I6ZZ*S3*S2**2*C3*S5**2-2.* 16ZZ*S3*S2**2*C3-I6ZZ 3056 . *S3*C3*S5**2+IGZZ*S3*C3-2.*I6ZZ*C2*S2*C3**2*S5**2+2.*I6ZZ*C2*S2* 3057 . C3**2+I6ZZ*C2*S2*S5**2-I6ZZ*C2*S2+4.*I6YY*S6*S4*C4*C5*S3*S2**2* 3058 . C3*C6-2.*I6YY*S6*S4*C4*C5*S3*C3*CG-4.*I6YY*S6*S4*C4*C5*C2*S2*C3 3059 . **2*C6+2.*I6YY*S6*S4*C4*C5*C2*S2*C6+4.*I6YY*S6*S4*S3*C2*S2*C3*C6 3060 . *S5+4.*I6YY*S6*S4*S2**2*C3**2*C6*S5-2.*I6YY*S6*S4*S2**2*C6*S5-2. 3061 . *I6YY*S6*S4*C3**2*C6*S5+I6YY*S6*S4*C6*S5+2.* 16YY*C4**2*S3*S2**2* 3062 . C3*C6**2*S5**2-4.* 16YY*C4**2*S3*S2**2*C3*C6**2-2.* 16YY*C4**2*S3* 3063 . S2**2*C3*S5**2+2.* 16YY*C4**2*S3*S2**2*C3+ANS2 3064 D211E=A2**2*M6*C2*S2+2.*A2*X6*M6*S6*S4*S3*S2**2-A2*X6*M6*S6*S4*S3 3065 . -2.*A2*X6*M6*S6*S4*C2*S2*C3-2.*A2*X6*M6*C4*C5*S3*S2**2*C6+A2*XG* 3066 . MG*C4*C5*S3*C6+2.*A2*X6*M6*C4*C5*C2*S2*C3*C6-2.*A2*X6*MG*S3*C2* 3067 . S2*C6*S5-2.*A2*X6*M6*S2**2*C3*C6*S5+A2*X6*M6*C3*C6*S5-2.*A2*MG* 3068 . Z6*C4*S3*S2**2*S5+A2*M6*Z6*C4*S3*S5+2.*A2*M6*Z6*C4*C2*S2*C3*S5+ 3069 . 2.*A2*MG*Z6*C5*S3*C2*S2+2.*A2*M6*Z6*C5*S2**2*C3-A2*M6*Z6*C5*C3+2 . 3070 . *A2*M6*Y6*S6*C4*C5*S3*S2**2-A2*M6*Y6*S6*C4*C5*S3-2.*A2*M6*Y6*S6* 3071 . C4*C5*C2*S2*C3+2.*A2*M6*Y6*S6*S3*C2*S2*S5+2.*A2*MG*Y6*S6*S2**2* 3072 . C3*S5-A2*M6*Y6*S6*C3*S5+2.*A2*MG*Y6*S4*S3*S2**2*C6-A2*M6*Y6*S4* 3073 . S3*C6-2.*A2*M6*Y6*S4*C2*S2*C3*C6+2.*A2*M6*D4*S3*C2*S2+2.*A2*M6* 3074 . D4*S2**2*C3-A2*M6*D4*C3-2.*A2*M6*D6*C4*S3*S2**2*S5+A2*M6*D6*C4* 188. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 54 3075 . S3*S5+2.*A2*M6*D6*C4*C2*S2*C3*S5+2.*A2*MG*D6*C5*S3*C2*S2+2.*A2* 3076 . M6*D6*C5*S2**2*C3-A2*M6*D6*C5*C3+4.*I6XZ*S6*S4*C4*S3*S2**2*C3*S5 3077 . -2.*I6XZ*S6*S4*C4*S3*C3*S5-4.* 16XZ*S6*S4*C4*C2*S2*C3**2*S5+2.* 3078 . I6XZ*S6*S4*C4*C2*S2*S5-4.*I6XZ*S6*S4*C5*S3*C2*S2*C3-4.*I6XZ*S6* 3079 . S4*C5*S2**2*C3**2+2.*I6XZ*S6*S4*C5*S2**2+2. *I6XZ*S6*S4*C5*C3**2-3080 . I6XZ*S6*S4*C5-4 . *I6XZ*C4**2*C5*S3*S2**2*C3*C6*S5+2. *I6XZ*C4**2* 3081 . C5*S3*C3*C6*S5+4.*I6XZ*C4**2*C5*C2*S2*C3**2*C6*S5-2.*I6XZ*C4**2* 3082 . C5*C2*S2*C6*S5-8.*I6XZ*C4*S3*C2*S2*C3*C6*S5**2+4.*I6XZ*C4*S3*C2* 3083 . S2*C3*C6+ANS1 3084 C 3085 D211=D211A+D211B+D211C+D211D+D211E 3086 C 3087 0311A=A2*Z3*M3*S3*S2*C2-A2*Z3*M3*C2**2*C3+A2*X3*M3*S3*C2**2+A2*X3 3088 . *M3*S2*C2*C3+4 . *I3XZ*S3*S2*C2*C3-4.*I3XZ*C2**2*C3**2+2.*I3XZ*C2 3089 . **2+2.*I3XZ*C3**2-I3XZ+2.*I3ZZ*S3*C2**2*C3-I3ZZ*S3*C3+2.*I3ZZ*S2 3090 . *C2*C3**2-I3ZZ*S2*C2-2.*I3XX*S3*C2**2*C3+I3XX*S3*C3-2.*I3XX*S2* 3091 . C2*C3**2+I3XX*S2*C2 3092 C 3093 D311B=-A2*Y4*M4*S3*S2*C2+A2*Y4*M4*C2**2*C3+A2*X4*M4*S3*C4*C2**2+ 3094 . A2*X4*M4*S2*C4*C2*C3-A2*M4*Z4*S3*S4*C2**2-A2*M4*Z4*S2*S4*C2*C3+ 3095 . A2*M4*D4*S3*S2*C2-A2*M4*D4*C2**2*C3+4.*I4YZ*S3*S2*S4*C2*C3-4.* 3096 . I4YZ*S4»C2**2*C3**2+2.*I4YZ*S4*C2**2+2.*I4YZ*S4*C3**2-I4YZ*S4+4. 3097 . *Y4*M4*D4*S3*C2**2*C3-2.*Y4*M4*D4*S3*C3+4.*Y4*M4*D4*S2*C2*C3**2-3098 . 2.*Y4*M4*D4*S2*C2-4.* 14XY*S3*S2*C4*C2*C3+4.* 14XY*C4*C2**2*C3**2-3099 . 2.*I4XY*C4*C2**2-2.*I4XY*C4*C3**2+I4XY*C4+2.*I4ZZ*S3*C4**2*C2**2* 3100 . C3-I4ZZ*S3*C4**2*C3-2.*I4ZZ*S3*C2**2*C3+I4ZZ*S3*C3+2.*I4ZZ*S2*C4 3101 . **2*C2*C3**2-I4ZZ*S2*C4**2*C2-2.*I4ZZ*S2*C2*C3**2+I4ZZ*S2*C2+2.* 3102 . I4YY*S3*C2**2*C3-I4YY*S3*C3+2.* 14YY*S2*C2*C3**2-14YY*S2*C2-2.* 3103 . I4XX*S3*C4**2*C2**2*C3+I4XX*S3*C4**2*C3-2.* 14XX*S2*C4**2*C2*C3** 3104 . 2+I4XX*S2*C4**2*C2-4. *I4XZ*S3*S4*C4*C2**2*C3+2.*I4XZ*S3*S4*C4*C3 3105 . -4.*I4XZ*S2*S4*C4*C2*C3**2+2.* 14XZ*S2*S4*C4*C2 + 4.*X4*M4*D4*S3*S2 3106 . *C4*C2*C3-4.*X4*M4*D4*C4*C2**2*C3**2+2.*X4*M4*D4*C4*C2**2+2.*X4* 3107 . M4*D4*C4*C3**2-X4*M4*D4*C4-4.*M4*Z4*D4*S3*S2*S4*C2*C3+4.*M4*Z4* 3108 . D4*S4*C2**2*C3**2-2.*M4*Z4*D4*S4*C2**2-2.*M4*Z4*04*S4*C3**2+M4* 3109 . Z4*D4*S4-2.*M4*D4**2*S3*C2**2*C3+M4*D4**2*S3*C3-2.*M4*D4**2*S2* 3110 . C2*C3**2+M4*04**2*S2*C2 3111 C 3112 ANS2=4.*I5XZ*C4*S2**2*S5**2-2.*I5XZ*C4*S2**2+4.*I5XZ*C4*C3**2*S5 3113 . **2-2.*I5XZ*C4*C3**2-2.*I5XZ*C4*S5**2+I5XZ*C4-4.* 15XZ*C5*S3*S2* * 3114 . 2*C3*S5+2.*I5XZ*C5*S3*C3*S5+4 . *I5XZ*C5*C2*S2*C3**2*S5-2.*I5XZ*C5 3115 . *C2*S2*S5+4.* 15XY*S4*C4*C5*S3*S2**2*C3-2.* 15XY*S4*C4*C5*S3*C3-4. 3116 . *I5XY*S4*C4*C5*C2*S2*C3**2+2.*I5XY*S4*C4*C5*C2*S2+4.*I5XY*S4*S3* 3117 . C2*S2*C3*S5+4.*I5XY*S4*S2**2*C3**2*S5-2 . *I5XY*S4*S2**2*S5-2.* 3118 . I5XY*S4*C3**2*S5+I5XY*S4*S5+4.*S4*C4*S3*S2**2*C3*S5*I5YZ-2.*S4* 3119 . C4*S3*C3*S5*I5YZ-4.*S4*C4*C2*S2*C3**2*S5*I5YZ+2.*S4*C4*C2*S2*S5* 3120 . I5YZ-4.*S4*C5*S3*C2*S2*C3*I5YZ-4.*S4*C5*S2**2*C3**2*I5YZ+2.*S4* 3121 . C5*S2**2*I5YZ+2.*S4*C5*C3**2*I5YZ-S4*C5*I5YZ 3122 ANSI=-4.*I5ZZ*C4*C5*S2**2*C3**2*S5+2.*I5ZZ*C4*C5*S2**2*S5+2.*I5ZZ 3123 . *C4*C5*C3**2*S5-I5ZZ*C4*C5*S5+2.*I5ZZ*S3*S2**2*C3*S5**2-2.*I5ZZ* 3124 . S3*S2**2*C3-I5ZZ*S3*C3*S5**2+I5ZZ*S3*C3-2.*I5ZZ*C2*S2*C3**2*S5** 3125 . 2+2.*I5ZZ*C2*S2*C3**2+I5ZZ*C2*S2*S5**2-I5ZZ*C2*S2-2.*I5YY*C4**2* 3126 . S3*S2**2*C3 + I5YY*C4**2*S3*C3 + 2.* 15YY*C4**2*C2*S2*C3**2-15YY*C4* * 3127 . 2*C2*S2+2.*I5YY*S3*S2**2*C3-I5YY*S3*C3-2.*I5YY*C2*S2*C3**2+I5YY* 3128 . C2*S2-2.*I5XX*C4**2*S3*S2**2*C3*S5**2+2.*I5XX*C4**2*S3*S2**2*C3+ 3129 . I5XX*C4**2*S3*C3*S5**2-I5XX*C4**2*S3*C3+2.*I5XX*C4**2*C2*S2*C3** 3130 . 2*S5**2-2.*I5XX*C4**2*C2*S2*C3**2-I5XX*C4**2*C2*S2*S5**2+I5XX*C4 3131 . **2*C2*S2+4.*I5XX*C4*C5*S3*C2*S2*C3*S5+4.*I5XX*C4*C5*S2**2*C3**2 3132 . *S5-2.*I5XX*C4*C5*S2**2*S5-2.*I5XX*C4*C5*C3**2*S5+I5XX*C4*C5*S5-189. L i s t i n g of ROBSIM.rOR at 15:45:13 on MAY 25. 1985 f o r CC1d=DAC0 Page 55 3133 .2.*I5XX*S3*S2**2*C3*S5**2+I5XX*S3*C3*S5**2+2.*I5XX*C2*S2*C3**2*S5 3134 . **2-I5XX*C2*S2*S5**2-4.*I5XZ*C4**2*C5*S3*S2**2*C3*S5+2.*I5XZ*C4 3135 . **2*C5*S3*C3*S5+4.*I5XZ*C4**2*C5*C2*S2*C3**2*S5-2.*I5XZ*C4**2*C5 3136 . *C2*S2*S5-8.*I5XZ*C4*S3*C2*S2*C3*S5**2+4.*I5XZ*C4*S3*C2*S2*C3-8. 3137 . *I5XZ*C4*S2**2*C3**2*S5**2+4.*I5XZ*C4*S2**2*C3**2+ANS2 3138 D311C=A2*Y5*M5*S4*S3*S2**2-A2*Y5*M5*S4*S3-A2*Y5*M5*S4*C2*S2*C3-A2 3139 . *Z5*M5*C4*53*S2**2*S5+A2*Z5*M5*C4*S3*S5+A2*Z5*M5*C4*C2*S2*C3*S5+ 3140 . A2*Z5*M5*C5*S3*C2*S2+A2*Z5*M5*C5*S2**2*C3-A2*Z5*M5*C5*C3+A2*D4* 3141 . M5*S3*C2*S2+A2*D4*M5*S2**2*C3-A2*D4*M5*C3-A2*X5*M5*C4*C5*S3*S2** 3142 . 2+A2*X5*M5*C4*C5*S3+A2*X5*M5*C4*C5*C2*S2*C3-A2*X5*M5*S3*C2*S2*S5 3143 . -A2*X5*M5*S2**2*C3*S5+A2*X5*M5*C3*S5-4.*Y5*D4*M5*S4*S3*C2*S2*C3-3144 .4.*Y5*D4*M5*S4*S2**2*C3**2+2.*Y5*D4*M5*S4*S2**2+2.*Y5*D4*M5*S4*C3 3145 . **2-Y5*D4*M5*S4+4.*Z5*D4*M5*C4*S3*C2*S2*C3*S5+4.*Z5*D4*M5*C4*S2 3146 . **2*C3**2*S5-2.*Z5*D4*M5*C4*S2**2*S5-2.*Z5*D4*M5*C4*C3**2*S5+Z5* 3147 . D4*M5*C4*S5+4.*Z5*D4*M5*C5*S3*S2**2*C3-2.*Z5*D4*M5*C5*S3*C3-4 . * 3148 . Z5*D4*M5*C5*C2*S2*C3**2+2.*Z5*D4*M5*C5*C2*S2+2.*D4**2*M5*S3*S2** 3149 . 2*C3-D4**2*M5*S3*C3-2.*D4**2*M5*C2*S2*C3**2+D4**2.*M5*C2*S2+4.*D4 3150 . *X5*M5*C4*C5*S3*C2*52*C3+4.*D4*X5*M5*C4*C5*S2**2*C3**2-2.*D4*X5* 3151 . M5*C4*C5*S2**2-2.*D4*X5*M5*C4*C5*C3**2+D4*X5*M5*C4*C5-4.*D4*X5* 3152 . M5*53*S2**2*C3*S5+2.*D4*X5*M5*S3*C3*S5+4.*D4*X5*M5*C2*S2*C3**2* 3153 . S5-2.*D4*X5*M5*C2*S2*S5+2.*I5ZZ*C4**2*S3*S2**2*C3*S5**2-I5ZZ*C4 3154 . **2*S3*C3*S5**2-2.*I5ZZ*C4**2*C2*S2*C3**2*S5**2+I5ZZ*C4**2*C2*S2 3155 . *S5**2-4.*I5ZZ*C4*C5*S3*C2*S2*C3*S5+ANS1 3156 C 3157 ANS8=-M6*D6**2*C2*S2*S5**2+M6*D6**2*C2*S2+4.*I6YZ*S6*C4**2*C5*S3* 3158 . S2**2*C3*S5-2.*I6YZ*S6*C4**2*C5*S3*C3*S5-4.*I6YZ*S6*C4**2*C5*C2* 3159 . S2*C3**2*S5+2.*I6YZ*S6*C4**2*C5*C2*S2*S5+8.*I6YZ*S6*C4*S3*C2*S2* 3160 . C3*S5**2-4.* 16YZ*S6*C4*S3*C2*S2*C3+8.* 16YZ*SG*C4*S2**2*C3**2*S5 3161 . **2-4 . *I6YZ*S6*C4*S2**2*C3**2-4.* 16YZ*SG*C4*S2**2*S5**2+2.*I6YZ* 3162 . S6*C4*S2**2-4.*I6YZ*S6*C4*C3**2*S5**2 + 2.* 16YZ*S6*C4*C3* *2+2.* 3163 . I6YZ*S6*C4*S5**2-I6YZ*S6*C4+4.* 16YZ*S6*C5*S3*S2**2*C3*S5-2.*I6YZ 3164 . *S6*C5*S3*C3*S5-4 . * 16YZ*S6*C5*C2*S2*C3**2*S5+2.*I6YZ*S6*C5*C2*S2 3165 . *S5+4.*I6YZ*54*C4*S3*S2**2*C3*C6*S5-2.*I6YZ*S4*C4*53*C3*C6*S5-4 . 3166 . *I6YZ*S4*C4*C2*S2*C3**2*C6*55+2.* 16YZ*S4*C4*C2*S2*C6*55-4.*I6YZ* 3167 . S4*C5*S3*C2*S2*C3*C6-4.*I6YZ*S4*C5*S2**2*C3**2*C6+2.*I6YZ*S4*C5* 3168 . S2**2*C6+2.*I6YZ*S4*C5*C3**2*C6-I6YZ*S4*C5*C6 3169 ANS7=-M6*Y6*D6*S6*C4+4.*M6*Y6*D6*S6*C5*S3*S2**2*C3*S5-2.*M6*Y6*DG 3170 . *S6*C5*S3*C3*S5-4.*M6*Y6*D6*S6*C5*C2*S2*C3**2*S5+2.*M6*Y6*D6*S6* 3171 . C5*C2*S2*55+4.*M6*Y6*D6*S4*C4*S3*S2**2*C3*C6*S5-2.*M6*Y6*D6*S4* 3172 . C4*S3*C3*C6*S5-4.*M6*Y6*D6*S4*C4*C2*S2*C3**2*C6*S5+2.*M6*Y6*D6* 3173 . S4*C4*C2*S2*C6*S5-4.*M6*Y6*D6*S4*C5*S3*C2*S2*C3*C6-4.*M6*Y6*D6* 3174 . S4*C5*S2**2*C3**2*C6+2.*M6*Y6*D6*S4*C5*S2**2*C6+2.*M6*Y6*D6*S4* 3175 . C5*C3**2*C6-M6*Y6*D6*S4*C5*C6+2.*M6*04**2*S3*S2**2*C3-M6*D4**2* 3176 . S3*C3-2.*M6*D4**2*C2*S2*C3**2+M6*D4**2*C2*S2+4.*M6*D4*D6*C4*S3* 3177 . C2*S2*C3*S5+4.*M6*D4*D6*C4*S2**2*C3**2*S5-2.*M6*D4*D6*C4*S2**2* 3178 . 55-2.*M6*D4*06*C4*C3**2*S5+M6*D4*D6*C4*S5+4.*M6*D4*06*C5*S3*S2** 3179 . 2*C3-2.*M6*D4*D6*C5*S3*C3-4.*M6*D4*D6*C5*C2*S2*C3**2+2.*M6*D4*D6 3180 . *C5*C2*S2-2.*M6*D6**2*C4**2*S3*S2**2*C3*S5**2+M6*D6**2*C4**2*S3* 3181 . C3*S5**2+2.*M6*D6**2*C4**2*C2*S2*C3**2*S5**2-M6*D6**2*C4**2*C2* 3182 . S2*S5**2 + 4.*M6*D6**2*C4*C5*S3*C2*S2*C3*S5+4.*MG*D6**2*C4*C5*S2* * 3183 . 2*C3**2*S5-2.*M6*06**2*C4*C5*S2**2*S5-2.*M6*D6**2*C4*C5*C3**2*S5 3184 . +M6*D6**2*C4*C5*S5-2.*M6*D6**2*S3*S2**2*C3*S5**2+2.*M6*D6**2*S3* 3185 . S2**2*C3+M6*D6**2*S3*C3*S5**2-M6*D6**2*S3*C3+2.*M6*D6**2*C2*S2* 3186 . C3**2*S5**2-2.*M6*D6**2*C2*S2*C3**2+ANS8 3187 ANS6=4.*M6*Z6*D6*C4**2*C2*S2*C3**2*S5**2-2.*M6*Z6*D6*C4**2*C2*S2* 3188 . S5**2+8.*M6*Z6*D6*C4*C5*S3*C2*S2*C3*S5+8.*M6*Z6*D6*C4*C5*S2**2* 3189 . C3**2*S5-4.*M6*Z6*D6*C4*C5*S2**2*S5-4.*M6*Z6*D6*C4*C5*C3**2*S5+ 3190 . 2.*M6*Z6*D6*C4*C5*S5-4.*M6*Z6*D6*S3*S2**2*C3*S5**2+4.*M6*Z6*D6*S3 190. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CCid=DACO Page 56 3191 . *S2**2*C3+2.*M6*Z6*D6*S3*C3*S5**2-2.*M6*Z6*D6*S3*C3+4.*M6*Z6*D6* 3192 . C2*S2*C3**2*S5**2-4.*M6*Z6*D6*C2*S2*C3**2-2.*M6*Z6*D6*C2*S2*S5** 3193 . 2+2.*M6*Z6*D6*C2*S2-4.*M6*Y6*D4*S6*C4*C5*S3*C2*S2*C3-4.*M6*Y6*D4 3194 . *S6*C4*C5*S2**2*C3**2+2.*M6*Y6*04*S6*C4*C5*S2**2+2.*M6*Y6*D4*S6* 3195 . C4*C5*C3**2-M6*Y6*04*S6*C4*C5+4.*M6*Y6*04*S6*S3*S2**2*C3*S5-2.* 3196 . M6*Y6*D4*S6*S3*C3*S5-4.*M6*Y6*D4*S6*C2*S2*C3**2*S5+2.*M6*Y6*D4* 3197 . S6*C2*S2*S5-4.*M6*Y6*D4*S4*S3*C2*S2*C3*C6-4.*M6*Y6*D4*S4*S2**2* 3198 . C3**2*C6+2.*M6*Y6*D4*S4*S2**2*C6+2.*M6*Y6*D4*S4*C3**2*C6-M6*Y6* 3199 . D4*S4*C6+4.*M6*Y6*D6*S6*C4**2*C5*S3*S2**2*C3*S5-2.*M6*Y6*D6*S6* 3200 . C4**2*C5*S3*C3*S5-4.*M6*Y6*D6*S6*C4**2*C5*C2*S2*C3**2*S5+2.*M6* 3201 . Y6*D6*S6*C4**2*C5*C2*S2*S5+8.*M6*Y6*D6*S6*C4*S3*C2*S2*C3*S5**2-3202 . 4.*M6*Y6*D6*S6*C4*S3*C2*S2*C3+8.*M6*Y6*D6*S6*C4*S2**2*C3**2*S5* *2 3203 . -4.*M6*Y6*D6*S6*C4*S2**2*C3**2-4.*M6*Y6*D6*S6*C4*S2**2*55**2+2.* 3204 . M6*Y6*06*S6*C4*S2**2-4.*M6*Y6*D6*S6*C4*C3**2*S5**2+2.*M6*Y6*D6* 3205 . S6*C4*C3**2+2.*M6*Y6*D6*S6*C4*S5**2+ANS7 3206 ANS5 = -4.*X6*M6*04*S3*S2**2*C3*C6*S5+2.*X6*M6*D4*S3*C3*C6*S5+4 . *X6 3207 . *M6*D4*C2*S2*C3**2*C6*S5-2.*X6*M6*D4*C2*S2*C6*S5+4.*X6*M6*D6*S6* 3208 . S4*C4*S3*S2**2*C3*S5-2 . *X6*M6*D6*S6*S4*C4*S3*C3*S5-4.*X6*M6*D6* 3209 . S6*S4*C4*C2*S2*C3**2*S5+2.*X6*M6*D6*S6*S4*C4*C2*S2*S5-4.*X6*M6* 3210 . D6*S6*S4*C5*S3*C2*S2*C3-4.*X6*M6*D6*S6*S4*C5*S2**2*C3**2+2.*X6*. 3211 . M6*D6*S6*S4*C5*S2**2+2.*X6*M6*D6*S6*S4*C5*C3**2-X6*M6*D6*S6*S4* 3212 . C5-4.*X6*M6*D6*C4**2*C5*S3*S2**2*C3*C6*S5+2.*X6*M6*D6*C4**2*C5* 3213 . S3*C3*C6*S5+4.*X6*M6*D6*C4**2*C5*C2*S2*C3**2*C6*S5-2.*X6*M6*D6* 3214 . C4**2*C5*C2*S2*C6*S5-8.*X6*M6*D6*C4*S3*C2*S2*C3*C6*S5**2+4.*X6* 3215 . M6*D6*C4*S3*C2*S2*C3*C6-8.*X6*M6*D6*C4*S2**2*C3**2*C6*S5**2+4.* 3216 . X6*M6*D6*C4*S2**2*C3**2*C6+4.*X6*M6*D6*C4*S2**2*C6*S5**2-2.*X6* 32 17 . M6*D6*C4*S2**2*C6+4.*X6*M6*D6*C4*C3**2*C6*S5**2-2.*X6*M6*D6*C4* 3218 . C3**2*C6-2.*X6*M6*D6*C4*C6*S5**2+X6*M6*D6*C4*C6-4.*X6*M6*D6*C5* 3219 . S3*S2**2*C3*C6*S5+2.*X6*M6*06*C5*S3*C3*C6*S5+4.*X6*M6*D6*C5*C2* 3220 . S2*C3**2*C6*S5-2.*X6*M6*D6*C5*C2*S2*C6*S5+4.*M6*Z6*D4*C4*S3*C2* 3221 . S2*C3*S5+4.*M6*Z6*D4*C4*S2**2*C3**2*S5'-2.*M6*Z6*D4*C4*S2**2*S5-3222 . 2.*M6*Z6*D4*C4*C3**2*S5+M6*Z6*04*C4*S5+4.*M6*Z6*D4*C5*S3*S2**2*C3 3223 . -2.*M6*Z6*D4*C5*S3*C3-4.*M6*Z6*D4*C5*C2*S2*C3**2+2.*M6*Z6*D4*C5* 3224 . C2*S2-4.*M6*Z6*D6*C4**2*S3*S2**2*C3*S5**2+2.*M6*Z6*D6*C4**2*S3* 3225 . C3*S5**2+ANS6 3226 ANS4 = -2.*I6XY *S6*C4* *2*C2*S2 *C6*S5* *2+4.*I6XY*S6*C4**2*C2*S2*C6+ 3227 . 8.*I6XY*S6*C4*C5*S3*C2*S2*C3*C6*S5+8.*I6XY*S6*C4*C5*S2**2*C3**2* 3228 . C6*S5-4.*I6XY*S6*C4*C5*S2**2*C6*S5-4.*I6XY*S6*C4*C5*C3**2*C6*S5+ 3229 . 2.*I6XY*S6*C4*C5*C6*S5-4.*I6XY*S6*S3*S2**2*C3*C6*S5**2-4.*I6XY*S6 3230 . *S3*S2**2*C3*C6+2.*I6XY*S6*S3*C3*C6*S5**2+2.*I6XY*S6*S3*C3*C6+4. 3231 . *I6XY*S6*C2*S2*C3**2*C6*S5**2+4.*I6XY*S6*C2*S2*C3**2*C6-2.*I6XY* 3232 . S6*C2*S2*C6*S5**2-2.*I6XY*S6*C2*S2*C6+8.*I6XY*S4*C4*C5*S3*S2**2* 3233 . C3*C6**2-4.*I6XY*S4*C4*C5*S3*S2**2*C3-4.*I6XY*S4*C4*C5*S3*C3*C6 3234 . **2+2.*I6XY*S4*C4*C5*S3*C3-8.*I6XY*S4*C4*C5*C2*S2*C3**2*C6**2+4 . 3235 . *I6XY*S4*C4*C5*C2*S2*C3**2+4.*I6XY*S4*C4*C5*C2*S2*C6**2-2.*I6XY* 3236 . S4*C4*C5*C2*S2+8.*I6XY*S4*S3*C2*S2*C3*C6**2*S5-4.*I6XY*S4*S3*C2* 3237 . S2*C3*S5+8.*I6XY*S4*S2**2*C3**2*C6**2*S5-4.*I6XY*S4*S2**2*C3**2* 3238 . S5-4.*I6XY*S4*S2**2*C6**2*S5+2.*I6XY*S4*S2**2*S5-4.*I6XY*S4*C3** 3239 . 2*C6**2*S5+2.*I6XY*S4*C3**2*S5+2.*I6XY*S4*C6**2*S5-I6XY*S4*S5-4 . 3240 . *X6*M6*D4*S6*S4*S3*C2*S2*C3-4.*X6*M6*D4*S6*S4*S2**2*C3**2+2.*X6* 3241 . M6*D4*S6*S4*S2**2+2.*X6*M6*D4*S6*S4*C3**2-X6*M6*D4*S6*S4+4.*X6* 3242 . M6*D4*C4*C5*S3*C2*S2*C3*C6+4.*X6*M6*D4*C4*C5*S2**2*C3**2*C6-2.* 3243 . X6*M6*D4*C4*C5*S2**2*C6-2.*X6*M6*D4*C4*C5*C3**2*C6+X6*M6*D4*C4* 3244 . C5*C6+ANS5 3245 ANS3=2.*I6XX*S6*S4*C3**2*C6*S5-16XX*S6*S4*C6*S5-2.* 16XX*C4**2*S3* 3246 • . S2**2*C3*C6**2*S5**2+4.*I6XX*C4**2*S3*S2**2*C3*C6**2-2.*I6XX*C4 3247 . **2*S3*S2**2*C3+I6XX*C4**2*S3*C3*C6**2*S5**2-2.*I6XX*C4**2*S3*C3 3248 . *C6**2+I6XX*C4**2*S3*C3+2.*I6XX*C4**2*C2*S2*C3**2*C6**2*S5**2-4. 191. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 f o r CC1d=DAC0 Page 57 3249 . *I6XX*C4**2*C2*S2*C3**2*C6**2+2 . *I6XX*C4**2*C2*S2*C3**2-I6XX*C4 3250 . **2*C2*S2*C6**2*S5**2+2.*I6XX*C4**2*C2*S2*C6**2-I6XX*C4**2*C2*S2 3251 . +4.*I6XX*C4*C5*S3*C2*S2*C3*C6**2*S5+4.*I6XX*C4*C5*S2**2*C3**2*C6 3252 . **2*S5-2.*I6XX*C4*C5*S2**2*C6**2*S5-2. *I6XX*C4*C5*C3**2*C6**2*S5 3253 . +I6XX*C4*C5*C6**2*S5-2.*I6XX*S3*S2**2*C3*C6**2*S5**2-2.*I6XX*S3* 3254 . S2**2*C3*C6**2+2.*I6XX*S3*S2**2*C3+I6XX*S3*C3*CG**2*S5**2+IGXX* 3255 . S3*C3*C6**2-I6XX*S3*C3+2 . *I6XX*C2*S2*C3**2*C6**2*S5**2+2.*IGXX* 3256 . C2*S2*C3**2*C6**2-2.*I6XX*C2*S2*C3**2-I6XX*C2*S2*C6**2*S5**2-3257 . I6XX*C2*S2*C6**2+I6XX*C2*S2-4 . *I6XY*S6*C4**2*S3*S2**2*C3*C6*S5** 3258 . 2+8.*I6XY*S6*C4**2*S3*S2**2*C3*C6+2.*I6XY*S6*C4**2*S3*C3*C6*S5** 3259 . 2-4.*I6XY*S6*C4**2*S3*C3*C6+4 . *I6XY*S6*C4**2*C2*S2*C3**2*C6*S5** 3260 . 2-8.*I6XY*S6*C4**2*C2*S2*C3**2*C6+ANS4 3261 ANS2=I6YY*C4**2*S3*C3*S5**2-I6YY*C4**2*S3*C3-2.*I6YY*C4**2*C2*S2* 3262 . C3**2*C6**2*S5**2+4.*I6YY*C4**2*C2*S2*C3**2*C6**2+2.*I6YY*C4**2* 3263 . C2*S2*C3**2*S5**2-2.*I6YY*C4**2*C2*S2*C3**2+I6YY*C4**2*C2*S2*C6 3264 . **2*S5**2-2.*I6YY*C4**2*C2*S2*C6**2-I6YY*C4**2*C2*S2*S5**2+I6YY* 3265 . C4**2*C2*S2-4.*I6YY*C4*C5*S3*C2*S2*C3*C6**2*S5+4.*I6YY*C4*C5*S3* 3266 . C2*S2*C3*S5-4.*I6YY*C4*C5*S2**2*C3**2*C6**2*S5+4.*I6YY*C4*C5*S2 3267 . **2*C3**2*S5+2.*I6YY*C4*C5*S2**2*C6**2*S5-2.*I6YY*C4*C5*S2**2*S5 3268 . +2.*I6YY*C4*C5*C3**2*C6**2*S5-2.*I6YY*C4*C5*C3**2*S5-16YY*C4*C5* 3269 . C6**2*S5+I6YY*C4*C5*S5+2.*I6YY*S3*S2**2*C3*C6**2*S5**2+2.*I6YY* 3270 . S3*S2**2*C3*C6**2-2.*I6YY*S3*S2**2*C3*S5**2-I6YY*S3*C3*C6**2*S5 3271 . **2-I6YY*S3*C3*C6**2+I6YY*S3*C3*S5**2-2.*I6YY*C2*S2*C3**2*C6**2* 3272 . S5**2-2.*I6YY*C2*S2*C3**2*C6**2+2 .*I6YY*C2*S2*C3**2*S5**2+I6YY* 3273 . C2*S2*C6**2*S5**2+I6YY*C2*S2*C6**2-I6YY*C2*S2*'S5**2-4. *I6XX*S6* 3274 . S4*C4*C5*S3*S2**2*C3*C6+2.*I6XX*S6*S4*C4*C5*S3*C3*C6+4.*I6XX*S6* 3275 . S4*C4*C5*C2*S2*C3**2*C6-2.*I6XX*S6*S4*C4*C5*C2*S2*C6-4.*I6XX*S6* 3276 . S4*S3*C2*S2*C3*C6*S5-4.* 16XX*S6*S4*S2**2*C3**2*C6*S5+2.*I6XX*S6* 3277 . S4*S2**2*C6*S5+ANS3 3278 ANS1=4.* 16XZ*C4*S2**2*C6*S5**2-2.* 16XZ*C4*S2**2*C6+4.*I6XZ*C4*C3 3279 . **2*C6*S5**2-2.*I6XZ*C4*C3**2*C6-2.*I6XZ*C4*C6*S5**2+I6XZ*C4*C6-3280 . 4.*I6XZ*C5*S3*S2**2*C3*C6*S5+2.* 16XZ*C5*S3*C3*C6*S5+4.*I6XZ*C5*C2 3281 . *S2*C3**2*C6*S5-2.*I6XZ*C5*C2*S2*C6*S5+2.*I6ZZ*C4**2*S3*S2**2*C3 3282 . *S5**2-I6ZZ*C4**2*S3*C3*S5**2-2.* 16ZZ*C4**2*C2*S2*C3**2*S5**2+ 3283 . I6ZZ*C4**2*C2*S2*S5**2-4 . *I6ZZ*C4*C5*S3*C2*S2*C3*S5-4.*I6ZZ*C4* 3284 . C5*S2**2*C3**2*S5+2.*I6ZZ*C4*C5*S2**2*S5+2.*I6ZZ*C4*C5*C3**2*S5-3285 . I6ZZ*C4*C5*S5+2.*I6ZZ*S3*S2**2*C3*S5**2-2.*I6ZZ*S3*S2**2*C3-I6ZZ 3286 . *S3*C3*S5**2+I6ZZ*S3*C3-2 . *I6ZZ*C2*S2*C3**2*S5**2+2.*I6ZZ*C2*S2* 3287 . C3**2+I6ZZ*C2*S2*S5**2-I6ZZ*C2*S2+4.*I6YY*S6*S4*C4*C5*S3*S2**2* 3288 . C3*C6-2.*I6YY*S6*S4*C4*C5*S3*C3*C6-4.*I6YY*S6*S4*C4*C5*C2*S2*C3 3289 . **2*C6+2.*I6YY*S6*S4*C4*C5*C2*S2*C6+4.*I6YY*S6*S4*S3*C2*S2*C3*C6 3290 . *S5+4.*I6YY*S6*S4*S2**2*C3**2*C6*S5-2.*I6YY*S6*S4*S2**2*C6*S5-2. 3291 . *I6YY*S6*S4*C3**2*C6*S5+I6YY*S6*S4*C6*S5+2.*I6YY*C4**2*S3*S2**2* 3292 . C3*C6**2*S5**2-4.*I6YY*C4**2*S3*S2**2*C3*C6**2-2.*16YY*C4* *2*S3* 3293 . S2**2*C3*S5**2+2.*I6YY*C4**2*S3*S2**2*C3-I6YY*C4**2*S3*C3*C6**2* 3294 . S5**2+2.*I6YY*C4**2*S3*C3*C6**2+ANS2 3295 D311D=A2*X6*M6*S6*S4*S3*S2**2-A2*X6*M6*S6*S4*S3-A2*XG*M6*S6*S4*C2 3296 . *S2*C3-A2*X6*M6*C4*C5*S3*S2**2*C6+A2*X6*M6*C4*C5*S3*C6+A2*X6*M6* 3297 . C4*C5*C2*S2*C3*C6-A2*X6*M6*S3*C2*S2*C6*S5-A2*X6*M6*S2**2*C3*C6* 3298 . S5+A2*X6*M6*C3*C6*S5-A2*M6*Z6*C4*S3*S2**2*S5+A2*M6*Z6*C4*S3*S5+ 3299 . A2*M6*Z6*C4*C2*S2*C3*S5+A2*M6*Z6*C5*S3*C2*S2+A2*M6*Z6*C5*S2**2* 3300 . C3-A2*M6*Z6*C5*C3+A2*M6*Y6*S6*C4*C5*S3*S2**2-A2*M6*Y6*S6*C4*C5* 3301 . S3-A2*M6*Y6*S6*C4*C5*C2*S2*C3+A2*M6*Y6*S6*S3*C2*S2*S5+A2*M6*Y6* 3302 . S6*S2**2*C3*S5-A2*M6*Y6*S6*C3*S5+A2*M6*Y6*S4*S3*S2**2*C6-A2*M6* 3303 . Y6*S4*S3*C6-A2*M6*Y6*S4*C2*S2*C3*C6+A2*M6*D4*S3*C2*S2+A2*M6*D4* 3304 . S2**2*C3-A2*M6*D4*C3-A2*MG*DG*C4*S3*S2**2*S5+A2*M6*D6*C4*S3*S5+ 3305 . A2*M6*06*C4*C2*S2*C3*S5+A2*M6*D6*C5*S3*C2*S2+A2*M6*D6*C5*S2**2* 3306 . C3-A2*M6*D6*C5*C3+4.*I6XZ*S6*S4*C4*S3*S2**2*C3*S5-2.*I6XZ*S6*S4* 192. L i s t i n g of ROBSIM.FOR at 15:45:13 on MAY 25, 1985 for CCid=DACO Page 58 3307 . C4*S3*C3*S5-4.*IGXZ*S6*S4*C4*C2*S2*C3**2*S5+2.*I6XZ*SG*S4*C4*C2* 3308 . S2*S5-4 . *I6XZ*S6*S4*C5*S3*C2*S2*C3-4.*I6XZ*S6*S4*C5*S2**2*C3**2+ 3309 . 2.*I6XZ*S6*S4*C5*S2**2+2.*I6XZ*S6*S4*C5*C3**2-I6XZ*S6*S4*C5-4.* 3310 . I6XZ*C4**2*C5*S3*S2**2*C3*C6*S5+2. *I6XZ*C4**2*C5*S3*C3*C6*S5+4.* 3311 . I6XZ*C4**2*C5*C2*S2*C3**2*CG*S5-2.*I6XZ*C4**2*C5*C2*S2*C6*S5-8.* 3312 . I6XZ*C4*S3*C2*S2*C3*C6*S5**2+4.*I6XZ*C4*S3*C2*S2*C3*C6-8.*I6XZ* 3313 . C4*S2**2*C3**2*C6*S5**2+4.*I6XZ*C4*S2**2*C3**2*C6+ANS1 3314 C 3315 D311=D311A+D311B+D311C+D311D 3316 C 3317 D322A=A2*M3*(-Z3*C3+X3*S3) 3318 C 3319 D322B=A2*M4*(Y4*C3+X4*S3*C4-Z4*S3*S4-D4*C3) 3320 C 3321 D322C=A2*M5*(-Y5*S4*S3+Z5*C4*S3*S5-Z5*C5*C3-D4*C3+X5*C4*C5*S3+X5* 3322 . C3*S5) 3323 C 3324 D322D=A2*M6*(-X6*S6*S4*S3+X6*C4*C5*S3*C6+X6*C3*C6*S5+Z6*C4*S3*S5-3325 . Z6*C5*C3-Y6*S6*C4*C5*S3-Y6*S6*C3*S5-Y6*S4*S3*C6-D4*C3+D6*C4*S3* 3326 . S5-D6*C5*C3) 3327 C 3328 D322=D322A+D322B+D322C+0322D 3329 C 3330 RETURN 3331 END 3332 C 3333 c******************************************^ 3334 C 3335 C THIS IS THE END OF THE SIMULATION PACKAGE ROBSIM.FOR 3336 C 3337 c*****************************************^ APPENDIX B C0MMAND.COM AND SETUP.DAT PROGRAM LISTINGS 194. L i s t i n g of C0MMAND.COM at 16:32:32 on MAY 25, 1985 for CCid=DAC0 Page 1 1 SDEFINE SYS$OUTPUT TEXT.DAT 2 $DEFINE SYSSINPUT SETUP.DAT 3 $RUN ROBSIM.EXE 4 $EXIT L i s t i n g of SETUP.DAT at 16:33:14 on MAY 25, 1985 for CCid=DAC0 Page 1 1 080 TRIAL NUMBER 2 N DO YOU WISH TO PLOT RESULTS? 3 N DO YOU WISH TO CHANGE CONTROL GAINS? 4 N DO YOU WISH TO IGNORE CORIOLIS & CENTRIPETAL EFFECTS? 5 N DO YOU WISH TO IGNORE GRAVITY EFFECTS? 6 N DO YOU WISH TO IGNORE INERTIAL COUPLING? 7 N DO YOU WISH TO IGNORE ARMATURE INDUCTANCE? 8 N DO YOU WISH TO IGNORE BACK EMF? 9 N DO YOU WISH TO IGNORE MOTOR DAMPING? 10 N DO YOU WISH TO RUN ROBOT WITHOUT SUPERVISORY CONTROLLER? 1 1 Y DO YOU WISH ROBOT TO START AT ZERO POSITION WITH ZERO VELOCITY? 12 10. THETA1 DESIRED 13 30. THETA2 DESIRED 14 60. THETA3 DESIRED 15 .2 STEP TIME 16 100. SIMULATION END TIME 17 Y DO YOU WISH TO USE GRAVITY COMPENSATION? 18 0.O03 VALUE FOR STOP 19 N IS SUPERVISORY CONTROLLER CORRECTING FOR POSITION ERRORS? 20 0.0001 VALUE FOR "TOL" OR "STABLE" AS APPLICABLE 21 1000. VALUE FOR "ALPHA" OR "KX" AS APPLICABLE 22 1000. VALUE FOR "DPX" OR "KY" AS APPLICABLE 23 1000. VALUE FOR "DPY" OR "KZ" AS APPLICABLE 24 1 . VALUE FOR "DPZ" OR "FTOL" AS APPLICABLE 25 0.0 VALUE FOR "ALFA" IF APPLICABLE 26 10.o VALUE FOR "FXD" IF APPLICABLE 27 20.0 VALUE FOR "FYD" IF APPLICABLE 28 0.0 VALUE FOR "FZD" IF APPLICABLE APPENDIX C SHORTEN.DAT PROGRAM LISTING 196. L i s t i n g of SHORTEN.DAT at 16:31:47 on MAY 25, 1985 f o r CCid=DACO Page 1 1 Q******** * ********** * ***** * ******************* * ******* * * * 2 3 A c PROGRAM NAME SHORTEN.DAT ** 5 C THIS PROGRAM READS IN DATA FROM A HOLDING FILE LOGICAL UNIT 8 6 C WHICH FOR VAX SYSTEM IS FILE "F0ROO8.DAT" AND THEN 7 C SELECTIVELY REDUCES THE NUMBER OF DATA POINTS UNTIL WE HAVE A SET 8 Q C OF DATA SUITABLE FOR GRAPHING USING "TELAGRAF" AND "TELLABANK". 10 Is C THE RESULTS ARE OUTPUT TO LOGICAL UNIT 7 WHICH FOR VAX SYSTEM IS 11 C FILE "F0R007.DAT". 12 C 13 DIMENSION X(10000),Y1(10000),Y2(10000),Y3(10000),Y4(10000), 14 Y5(10000),Y6(10000),Y7(10000),Y8(10000), 15 Y9(10000),Y10(10000),Y11(10000).Y12(10000) 16 C 17 READ(8.20)ITRIAL 18 20 FORMAT(1X,13) 19 C 20 i=o 21 C 22 70 1 = 1 + 1 23 READ(8,80,END=100)X(I).Y1(I),Y2(I),Y3(I),Y4(I),Y5(I),Y6(I), 24 Y7(I ) ,Y8(I ) ,Y9(I),Y10(I),Y11(I),Y12(I) 25 80 FORMAT(1X,F9.4,6F10.4,3F10.3.3F10.4) 26 GO TO 70 27 C 28 C ALL VALUES READ IN 29 C 30 100 NTOTAL = I- 1 31 C 32 C NOW REDUCE DATA BY SPACING AT APPROX. EVEN TIME INTERVALS 33 C 34 NMPTS=600 35 NSPACE=NMPTS-2 36 C 37 TSPACE=(X(NTOTAL)-X(1))/FLOAT(NSPACE) 38 T=X(1)+TSPACE 39 WRITE(7,120)ITRIAL 40 120 FORMAT('1'13) 41 C 42 DO 160 1=1,2 43 WRITE(7, 140)X(I),Y1(I),Y2(I),Y3(I),Y4(I),Y5( I ),Y6( I ) , 44 Y7(I),Y8(I),Y9(I),Y10(I),Y11(I).Y12(I) 45 140 FORMAT(1X,F9.4,6F10.4,3F10.3,3F10.4) 46 160 CONTINUE 47 C 48 M=NT0TAL-1 49 C 50 DO 220 1=3,M 51 IF( X(I) .LT. T) GO TO 220 52 C 53 WRITE(7,240)X(I),Y1(I),Y2(I),Y3(I),Y4(I),Y5(I),Y6(I), 54 Y7(I ) ,Y8(I ) ,Y9(I ) ,Y10(I),Y11(I),Y12(I) 55 240 FORMAT(1X,F9.4,6F10.4,3F10.3,3F10.4) 56 T=T+TSPACE 57 220 CONTINUE 58 C 197. L i s t i n g of SHORTEN.DAT at 16:31:47 on MAY 25, 1985 f o r CCid=DACO.Page 2 59 C FINALLY WE RECORD LAST DATA POINT 60 C 61 I=NTOTAL 62 WRITE(7,300)X(I),Y1(I),Y2(I),Y3(I),Y4(I),Y5(I),Y6(I), 63 Y7(I ) ,Y8(I),Y9(I),Y10(I).Y11(I),Y12(I) 64 300 F0RMAT(1X,F9.4,6F10.4,3F1O.3,3F10.4) . 65 C 66 STOP 67 END 68 C £ 9 c****************************************************** 70 71 

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0096322/manifest

Comment

Related Items