UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

The automatic off-line generation of welding robot trajectories with emphasis on kinematic feasibility.. 1984

You don't seem to have a PDF reader installed, try download the pdf

Item Metadata

Download

Media
UBC_1984_A7 B82.pdf [ 17.2MB ]
UBC_1984_A7 B82.pdf
Metadata
JSON: 1.0080813.json
JSON-LD: 1.0080813+ld.json
RDF/XML (Pretty): 1.0080813.xml
RDF/JSON: 1.0080813+rdf.json
Turtle: 1.0080813+rdf-turtle.txt
N-Triples: 1.0080813+rdf-ntriples.txt
Citation
1.0080813.ris

Full Text

THE AUTOMATIC OFF-LINE GENERATION OF WELDING ROBOT TRAJECTORIES WITH EMPHASIS ON KINEMATIC FEASIBILITY AND COLLISION DETECTION by RALPH 0. BUCHAL B . A . S c , U n i v e r s i t y of B r i t i s h Columbia, 1980 A THESIS SUBMITTED IN PARTIAL FULFILMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE i n FACULTY OF GRADUATE STUDIES Department of Mechanical Engineering We accept th i s thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA September 1984 © Ralph 0. Buchal , 1984 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at THE UNIVERSITY OF BRITISH COLUMBIA, I agree that the Library shall make i t freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the Head of my Department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of Mechanical Engineering THE UNIVERSITY OF BRITISH COLUMBIA 2075 Wesbrook Place Vancouver, Canada V6T 1W5 Date: CPcT"<?be\- (G> , / q 84- i i ABSTRACT This thesis defines and discusses the problems involved i n automatic o f f - l i n e programming of a robot welding workstation with a minimum of human intervention, and proposes some solutions. The work i s motivated by the desire for faster and more powerful programming c a p a b i l i t i e s combined with reduced robot down-time for programming, r e s u l t i n g i n increased f l e x i b i l i t y , e f f i c i e n c y and pr o d u c t i v i t y . The system proposed i n the thesis uses data generated by commercial Computer Aided Design (CAD) systems. An "Expert Welder" software module selects the correct welder settings based on workpiece and workstation c h a r a c t e r i s t i c s . Provision i s made for the eventual incorporation of a real-time seam tracking system. The kinematics of a welding robot and workstation are considered i n d e t a i l . The welding torch p o s i t i o n and o r i e n t a t i o n r e l a t i v e to the robot are found from a series of known r e l a t i v e homogeneous coordinate transformations. Path f e a s i b i l i t y i s determined by the kinematic constraints of j o i n t l i m i t s and the work envelope, as well as by physical interference between the arm and other objects i n the workstation. The inverse kinematics so l u t i o n i s found and the solutions are checked against the j o i n t l i m i t s and work envelope for each desired robot p o s i t i o n . The s i x t h degree of freedom i s redundant for welding, and t h i s redundancy i s used to search for f e a s i b l e kinematic solutions. A s i m p l i f i e d interference detection algorithm i s proposed. The workstation i s modelled as a c o l l e c t i o n of s o l i d polyhedra. The robot l i n k s sweep out volumes of space which are approximated by volumes bounded i i i by parametric ruled surfaces. A number of simple tests can be made to determine whether any intersections exist between the swept volumes and the stationary polyhedra, i n d i c a t i n g i n t e r f e r a n c e . Much of th i s fundamental work i s incorporated i n a preliminary i n t e r a c t i v e programming software package c a l l e d AUTOP. The interference detection algorithm i s demonstrated by a program c a l l e d TESTIN. iv ACKNOWLEDGEMENT I would like to thank Professor Dale Cherchas for his enthusiastic help and guidance throughout my work. A large part of this thesis is based on work done for the Industrial Materials Research Institute, a division of the National Research Council under DSS Contract 12SD, 31155- 2-5016. V TABLE OF CONTENTS Page ABSTRACT i i ACKNOWLEDGEMENT iv LIST OF TABLES x LIST OF FIGURES x i i NOMENCLATURE xiv LIST OF SYMBOLS xv 1. INTRODUCTION 1 2. PROBLEM DEFINITION 6 3. DESCRIPTION OF WORKSTATION 9 3.1 Description of Workpiece Using CAD 9 3.2.1 Geometrical Description 10 3.1.2 Weld Seam Description 10 3.2 Description of Robot 11 3.2.1 Link Geometry and Dimensions 11 3.3 Description of Welding Machine 14 3.4 Description of Positioning Table 15 4. AUTOMATIC WELDING PARAMETER SELECTION USING AN "EXPERT WELDER" SOFTWARE MODULE 16 5. KINEMATICS 17 5.1 Review of Homogeneous Transformations 17 5.1.1 Homogeneous Coordinate Transformation 17 5.1.2 Serial Combination of Homogeneous Transformations 19 5.1.3 Serial Combination of Homogeneous Frames 19 v i Page 5.2 Statement of Kinematics Problem 22 5.3 Intermediate Homogeneous Coordinate Frames 22 5.3.1 Table Frame Relative to Robot Base 22 5.3.2 Workpiece Frame Relative to Table 25 5.3.3 Weld Seam Frame Relative to Workpiece 25 5.3.4 Tool Frame Relative to Weld Seam 25 5.4 Calculation of the Tool Position and Orientation Relative to World Coordinates 28 6. PATH FEASIBILITY 29 6.1 Statement of Feasibility Problem 29 6.2 Kinematic Feasibility 29 6.2.1 Definition of Robot Joint Frames and Resulting Transformations 31 6.2.2 Inverse Kinematics Solution 32 6.2.2.1 General Approach to Inverse Kinematics Problem 32 6.2.2.2 PUMA 560 Solution 34 6.2.2.3 Discussion of PUMA Solutions 44 6.2.3 Comparison of Five Degree of Freedom and Six Degree of Freedom Robots 44 6.2.3.1 Solution for a Five Degree of Freedom Robot 44 6.2.3.2 ASEA IRB60 Solution 57 6.2.3.3 Redundancy of Sixth Joint for Welding . 59 v i i Page 6.2.4 Specification of Robot Coordinates 60 6.2.4.1 Calculation of PUMA Tool Location and Rotation Coordinates 62 6.3 Collision Avoidance 66 6.3.1 World Model 68 6.3.2 Development of Swept Volume 71 6.3.3 Interference Conditions 75 6.3.3.1 Condition 1 - Intersection of an Edge and a Parametric Surface 77 6.3.3.2 Condition 2 - Intersection of an Edge and a Cylinder 80 6.3.3.3 Condition 3 - Intersection of an Edge and a Polyhedral Face 83 6.3.3.4 Condition 4 - Intersection of a Cylinder and a Polyhedral Face 85 6.3.3.5 Condition 5 - Any Polyhedron is Totally Enclosed by a Swept Volume Bounded by Parametric Surfaces 88 6.4 Interference Detection Demonstration Program TESTIN ... 89 6.4.1 Functional Description of TESTIN 90 6.4.2 List and Description of a l l Subroutines used by TESTIN 90 6.4.3 Demonstration of TESTIN 96 6.5 Possible Improvements to the Algorithm 102 v i i i Page 7. SOFTWARE DESIGN AND DEVELOPMENT 103 7.1 Design Approach 103 7.2 Features and Capabilities of AUTOP 104 7.3 General Functional Description of AUTOP 105 7.3.1 List and Description of a l l Subroutines called from AUTOP 107 7.4 Data Structures 116 7.4.1 Description of a l l I/O Files and their Formats 117 7.5 Graphical Depiction of Solids Using Panels 132 7.6 User's Guide to Running Program AUTOP 135 7.7 Demonstration of AUTOP and Results 137 7.8 Future Capabilities 150 7.8.1 Real-time Seam Tracking 150 7.8.2 CAD System Interface 150 7.8.3 Generation of High-Level Descriptive Program .. 150 8. CONCLUSIONS 154 8.1 Analysis and Evaluation of Work Completed 154 8.2 Suggestions for Further Work 155 REFERENCES 157 ix Page APPENDIX I SOFTWARE DOCUMENTATION 160 1.1 Implementation Specific Software 161 1.2 AUTOP Program Listing 162 1.3 CAD File Generating Program CADGEN Listing 266 1.4 TESTIN Program Listing 270 X LIST OF TABLES Page 3.1 Manipulator Link Geometry Fil e Format 14 6.1 PUMA Link Parameters 36 6.2 Five Degree of Freedom PUMA Link Parameters 45 7.1 F i l e Name List FILIST 118 7.2 CAD Fil e List CADLST 119 7.3 Processed Seam File List WPCLST 119 7.4 Robot Location Data Fil e List ROBLST 119 7.5 Object Geometry Data Fil e List OBJLST 120 7.6 Seam CAD File 120 7.7 Processed Seam Data File 121 7.8 Robot Location Data Fil e 121 7.9 Object Geometry Data Files 122 7.10 System Configuration Fi l e WKSTAT 123 7.11 Example of File WKSTAT 124 7.12 Positioning Table Specification File TABLE 125 7.13 Example of File TABLE 126 7.14 Graphic System Initialization F i l e GRAPH 127 7.15 Example of GRAPH 127 7.16 Workstation Setup File for Current Seam LOCSET 128 7.17 Example of LOCSET for seams 5,1,3 128 7.18 Current Table Position Data STOVAR 129 7.19 Example of STOVAR for 3 Degree of Freedom Table 129 7.20 Temporary Weld Parameter Fil e WPARAM 130 7.21 Example of WPARAM for a Seam with 10 Locations 130 xi Page 7.22 Fortran File Formats 131 7.23 Subroutines Accessing Data Files 132 7.24 Contents of Seam CAD Fil e 142 7.25 Contents of Processed Seam Data File 143 7.26 Joint Angles Calculated during Kinematic Feasibility Search 144 7.27 Generated Robot Location Data 149 7.28 Task Precedence Table 153 x i i LIST OF FIGURES Page 1.1 Simplified Hardware Schematic for an Automatically Programmed Robot Welding Workstation 5 3.1 Link Geometry and Frames for a Revolute Joint 12 3.2 Link Geometry and Frames for a Prismatic Joint 13 5.1 Coordinate Transformation 18 5.2 Serial Combination of Homogeneous Transformations 20 5.3 Homogeneous Coordinate Frame Defined by n, o, a, p .... 21 5.4 Series of Homogeneous Coordinate Frames 23 5.5 Weld Frame Relative to Workpiece 26 5.6 Tool Frame Relative to Weld Seam 27 6.1 PUMA Joint Frames and Link Parameters 35 6.2 Relative Geometry of PUMA Link Frames F 3, F^, F 5, F 6 38 6.3 Tool Frame Relative to Robot Tool Mounting Flange 46 6.4 Rotation of Tool Frame about Tool Axis 61 6.5 Definition of o, a, t angles for the PUMA Arm 63 6.6 World Model 70 6.7 Cylindrical Approximation of a Manipulator Link 72 6.8 Parametric Approximation of Swept Volume 74 6.9 Interference Conditions 76 6.10 Algorithm for Interference Detection 78 6.11 Intersection of an Edge and a Parametric Surface 79 6.12 Intersection of an Edge and a Cylinder 81 6.13 Intersection of an Edge and a Polyhedral Face 84 x i i i Page 6.14 Intersection of Cylinder End Face and Polyhedral Face 86 6.15 Intersection Condition 1 between Face and Swept Volume 98 6.16 Intersection Condition 3 between Face and Swept Volume 99 6.17 Intersection Condition 2 between Face and Swept Volume 100 6.18 Intersection Condition 1 between Face and Swept Volume Generated by Pure Translation 101 7.1 Software Structure and Data Flow of AUTOP 106 7.2 Simplified Flowchart of AUTOP 108 7.3 Relationship of Two Lines to View Point 134 7.4 Graphical Depiction of the Workpiece by AUTOP 139 7.5 New Workpiece Position after Interactively Changing Table Position 140 7.6 Workpiece Shown from a Different View Point 141 7.7 Simplified High-Level Task Description 152 x i v NOMENCLATURE AUTOP: the automatic o f f - l i n e robot programming software package. CAD: Computer-Aided Design. IGL: Tektronix Interactive Graphics Library software package. PUMA: the Unimation PUMA 560 i n d u s t r i a l robot. TESTIN: an elementary interference t e s t i n g demonstration program. X V LIST OF SYMBOLS a_ t h i r d basis vector of a frame r e l a t i v e to the robot, a^ length of the common normal between the j o i n t i and jo i n t i - l axes of a robot. a-| the common normal between the jo i n t i and j o i n t i - l axes of a robot. d± the distance between l i n k s i and i - l of a robot. Fg the workpiece coordinate frame. F c the instantaneous weld seam coordinate frame. F G the too l or torch coordinate frame, th F^ the i robot l i n k coordinate frame. F R the robot base coordinate frame. F-p the positioning table frame. H , a homogeneous coordinate transformation from frame F to frame X X j X X F i - 1 . J_ the i t b j o i n t axis of a robot. 11 the f i r s t basis vector of a frame r e l a t i v e to the robot. o_ the second basis vector of a frame r e l a t i v e to the robot. P_ the pos i t i o n of a frame o r i g i n r e l a t i v e to the robot. the p o s i t i o n of the robot wrist center r e l a t i v e to the robot, r ^ a point l y i n g on a bounding parametric surface. r_p a point on a parent parametric surface. TJJ transformation matrix giving torch p o s i t i o n as a function of n joi n t v a r i a b l e s . x v i U Q one of the vectors defining the boundaries of a parametric surface patch. u_i one of the vectors defining the boundaries of a parametric surface patch. v 0 one of the vectors defining the boundaries of a parametric surface patch, x^ x-axis of a frame F^. y-i y-axis of a frame F^. z ± z-axis of a frame Fj_. a l o n g i t u d i n a l angular o f f s e t of torch from seam. ctji the twist angle of the i t h robot l i n k 3 l a t e r a l angular o f f s e t of torch from seam. 8^ the i t h jo i n t variable of a robot. X u a parameter of a parametric surface. ^ v a parameter of a parametric surface. \j a parameter of a parametric volume. P torch stick-out gap. - 1 - 1. INTRODUCTION As technology progresses, robots are becoming an in c r e a s i n g l y important t o o l i n industry. Continual progress i n programming and con t r o l methods combined with the incorporation of sensory feedback promise to va s t l y increase the scope of applications for robots i n the factory. A great deal of fundamental research remains to be done, however, before the f u l l p o t e n t i a l of robots can be r e a l i z e d . Although robots have already had a tremendous impact i n manu- fact u r i n g , they are s t i l l very l i m i t e d i n t h e i r c a p a b i l i t i e s . Programming of a robot can be time consuming and d i f f i c u l t f o r tasks of even moderate complexity. T y p i c a l l y , a robot i s programmed by p h y s i c a l l y guiding i t through i t s motions under manual c o n t r o l . Positions are recorded along the way so that the motions can be repeated or played-back as often as -desired. The user has l i t t l e or no c a p a b i l i t y to edit or modify the re s u l t i n g program. The program i s completely developed using the robot to define l o c a t i o n s , and e x i s t i n g Computer-Aided Design (CAD) data i s not u t i l i z e d . The programming i s done on-line, removing the robot from production f o r possibly unreasonable lengths of time. These shortcomings are p a r t i c u l a r l y acute for small batch production where the robot must be reprogrammed frequently. Since most commercial robots have no sensory c a p a b i l i t i e s , they cannot adapt to random va r i a t i o n s i n the tasks they perform. This necessitates very s t r i c t s t ructuring and control of the workstation to insure a high degree of accuracy and r e p e a t a b i l i t y for the process. Process v a r i a t i o n s can occur due to va r i a t i o n s i n workpiece positioning, - 2 - and variations between different samples of the workpiece. The require- ment for exact repeatability could be greatly relaxed i f the robot could adapt to process variations using real-time sensory feedback. We can formulate two main objectives for the improvement of robot performance. Fi r s t , we would like to simplify and speed-up the robot programming process, and perform as much of the programming as possible of f - l i n e . The method must be easy to use, fast and flexible. Second, we must incorporate sensory feedback and real-time correction of process variations. This thesis considers the f i r s t problem only. The problem of off-line programming has been studied by a number of investigators, but a commercially feasible system has not yet been demonstrated. Two basic approaches have so far been demonstrated for generation of a robot program, interactive simulation and explicit task description using high-level languages. In the f i r s t approach, simulation of the process is done using interactive graphics. The robot and workstation are depicted graphically, and the operator interactively generates a program which Is simulated on a graphics display. It is d i f f i c u l t to specify locations exactly with this method, and there may be problems visualizing the scene. The demonstrations described in the literature [Sata, Fumihiko and Akio 1981; Ambler, Popplestone and Kempf 1982; Sjolund and Donath 1983] provide only limited software tools to aid the user. A second and more promising approach to off-line programming is offered by high-level robot languages. These languages allow the user to ful l y describe the robot task in a descriptive, natural way. Locations can be specified explicitly, or can be referred to a mathematical world - 3 - model. Many languages have been devised by robotics researchers, but off- line programming has been discussed more than demonstrated. Ambler, Popplestone and Kempf [1982] generated an explicit program in the VAL language for a PUMA robot, including a l l of the location data, using the high-level language RAPT to describe a task. Their experience showed a need for graphical simulation of the program for verification and debugging. Other existing high-level robot languages are summarized in a number of recent papers [Gruver et al 1983; Kempf 1982; Tarvin 1980]. The off-line programming problem encompasses a number of d i f f i c u l t sub-problems. Two problems which are considered in detail in this thesis concern kinematic fe a s i b i l i t y and co l l i s i o n detection. The kinematic f e a s i b i l i t y of a proposed robot trajectory must be automatically checked to ensure that a valid set of joint angle solutions exists. A robot position is kinematically feasible i f an inverse kinematic solution exists for the joint angles, and i f a l l the joint angles are within their physical limits. We have found by experience that even when programming a robot on-line i t is d i f f i c u l t to avoid paths which lead to a joint angle limit being exceeded, particularly when moving in straight lines. To verify kinematic f e a s i b i l i t y , we must solve the Inverse kinematic problems to find the joint angles at each point along a proposed trajectory. The inverse kinematic problem has been well studied by many investigators (see Section 6.2 for a review). An explicit solution for most real robots can be derived by matrix algebra methods described by Paul [1981]. In order to increase the chance of finding a feasible solution for a given trajectory we w i l l exploit the redundancy of the - 4 - s i x t h degree of freedom i n welding to search for a f e a s i b l e set of j o i n t solutions at each point on the t r a j e c t o r y . The c o l l i s i o n detection problem i s one part of the far more d i f f i c u l t problem of path planning to avoid obstacles. Many algorithms have been proposed for finding intersections between stationary geometric s o l i d s and between stationary and moving s o l i d s . Previous work i n geometric model- l i n g and interference detection i s reviewed i n Section 6.3. The more d i f f i c u l t path planning problem has not yet been solved i n a s a t i s f a c t o r y way, although some promising strategies have been proposed for simple non- redundant systems. No work has yet emerged i n the l i t e r a t u r e dealing with path planning for systems with redundancy. This thesis discusses the problems associated with developing an integrated o f f - l i n e welding robot programming system, and offers some solu t i o n s . The solutions are incorporated into a f u n c t i o n a l software package c a l l e d AUTOP. P a r t i c u l a r emphasis i s placed on the kinematic f e a s i b i l i t y problem, and on elementary methods for c o l l i s i o n detection. Many other v i t a l issues are beyond the scope of t h i s thesis and are discussed only b r i e f l y . F i g . 1.1 i l l u s t r a t e s the required hardware structure for an automatically programmed welding workstation. - 5 - Interactive Terminal Welding Data Base Welding Machine Controller I Robot Controller I P o s i t i o n i n g Table Co n t r o l l e r T Welding Machine Robot Positioning Table \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ Figure 1.1 S i m p l i f i e d Hardware Schematic for an Automatically Programmed Robot Welding Workstation. - 6 - 2. PROBLEM DEFINITION The ultimate objective of the work described i n this thesis i s the development of a system for automatically programming a welding robot using data derived from CAD f i l e s . An expert welder data base w i l l provide the correct welding parameter values, and a real-time seam tracking system w i l l correct for real-world v a r i a t i o n s . The structure of the r e s u l t i n g program w i l l be h i e r a r c h i c a l , with the highest l e v e l c o n t r o l l i n g the entire workstation as an integrated u n i t . We can i s o l a t e a number of sub-problems which must be solved before an automatic programming system can be r e a l i z e d : 1. Choice of World Model and Description We must choose a way to represent the workpiece e x p l i c i t l y , without ambiguity. Important information includes seam l o c a t i o n data, workpiece geometry and welding parameters. We must also represent the workstation, including the robot, positioning table and welding machine. 2. Generation of a Welding Procedure An acceptable and f e a s i b l e welding procedure must be generated. The procedure includes the order i n which seams are welded, workpiece repositioning, inter-weld paths, etc. 3. Automatic Generation of Welding Parameter Values Welding output parameters such as current and speed can be - 7 - determined automatically i f a l l relevant workpiece and welding machine c h a r a c t e r i s t i c s are known. 4. Generation of Welding Torch T r a j e c t o r i e s We must generate torch t r a j e c t o r y data including torch position and o r i e n t a t i o n along each seam i n terms of robot coordinates. 5. Path F e a s i b i l i t y Testing The calculated torch t r a j e c t o r y may not be f e a s i b l e due to robot work envelope and j o i n t l i m i t constraints. Also, interference may occur between the robot and other objects In the workstation. 6. Generation of High-Level Program to Direct and Coordinate the Welding Process We need a d e s c r i p t i v e , h i g h - l e v e l method for c o n t r o l l i n g and coordinating the d i f f e r e n t devices i n the workstation. Welding requires a c a r e f u l l y controlled i n t e r a c t i o n between the robot, positioning table and welding machine. 7. Program Modification i n Response to Sensory Feedback Due to real-world v a r i a t i o n s and uncertainties i n positioning and welding, a seam tracking system based on real-time sensory feedback i s an important c a p a b i l i t y . 8. Hardware and Software Interfaces Hardware and software interfaces must be designed between the - 8 - various components i n the workstation. A host computer must be able to communicate with the robot c o n t r o l l e r , positioning table c o n t r o l l e r and welding machine. These problems w i l l be discussed and some solutions w i l l be proposed. - 9 - 3. DESCRIPTION OF WORKSTATION We must describe a l l the important c h a r a c t e r i s t i c s of the workstation i n a concise way. The workstation i s composed of the workpiece, pos i t i o n i n g table, welding machine, robot and any other peripheral equipment. 3.1 Description of Workpiece Using CAD A complete des c r i p t i o n of the workpiece i s generated by an external CAD system. The CAD des c r i p t i o n must contain the following information: 1. Geometry a) dimensions and shape of object b) weld seam t r a j e c t o r i e s and surface normals adjacent to seams i n d i g i t i z e d form. 2. Welding Information a) material b) thickness c) weld j o i n t types d) weld cross s e c t i o n a l area. The CAD f i l e must be translated from the CAD output format into the s p e c i f i e d input format for the software system, which we w i l l c a l l AUTOP. This t r a n s l a t i o n i s required so that any external CAD system may be used. Details of the CAD system need not be known at t h i s point, as long as the sp e c i f i e d information i s generated i n some form. - 10 - 3.1.1 Geometrical Description We wish to r e p r e s e n t the geometry of the workpiece by an approximating polyhedron, whose v e r t i c e s , edges and faces are stored i n a h i e r a r c h i c a l , f i l e - b a s e d data structure. At the top of the hierarchy i s a l i s t of the object faces, including t h e i r surface normals, distance from o r i g i n and number of v e r t i c e s . Each face record contains the coordinates of the face v e r t i c e s i n any sequential order around the face. Each vertex of the polyhedron i s shared by at least three faces. A polyhedral representation i s well suited to graphical depiction and interference detection. 3.1.2 Weld Seam Description Seam data i s also stored i n a h i e r a r c h i c a l , f i l e - b a s e d structure. A l i s t of seam i d e n t i f i e r s i s stored i n a f i l e . Each entry points to a corresponding seam data f i l e . The head of each seam data f i l e contains welding data relevant to that seam, including material thicknesses, seam types and weld cross s e c t i o n a l area. The rest of the f i l e contains d i g i t i z e d coordinates of the seam t r a j e c t o r y and adjacent surface normals r e l a t i v e to a reference frame fixed on the workpiece. Each seam defined i n the CAD f i l e i s treated as a sin g l e , i n d i v i s i b l e e n t i t y by AUTOP. Long seams should be broken down into shorter segments which can be treated separately. For example, a c i r c u l a r weld around the circumference of a cylinder should be divided into two or four segments so that the workpiece can be repositioned before welding each segment. - 11 - 3.2 Description of Robot AUTOP requires a careful and complete s p e c i f i c a t i o n of the robot i n use. Data which are required are: 1. Number of degrees of freedom. 2. Complete l i n k geometry and dimensions 3. Joint l i m i t s . 4. Tool geometry r e l a t i v e to tool mounting flange. 5. Default robot configuration. 6. Position of robot base r e l a t i v e to the world coordinate frame, stored as a homogeneous coordinate transformation. 3.2.1 Link Geometry and Dimensions We w i l l follow the conventions of Denavit and Hartenberg [1955] i n describing l i n k geometry and dimensions. We can define an n degree of freedom robot as a series of n r i g i d l i n k s connected by n joints (Figs. 3.1, 3.2). The i ^ l i n k i s characterized by a length a^ and a twist angle . a^ i s the length of the common normal a.̂  between the two joint axes, and ct̂  i s the angle between the axes i n a plane perpendicular to _a . The r e l a t i v e displacement between a, , and a^ at the joint i axis i s —1-1 — i d^, the distance between the l i n k s . 6̂  i s the angular displacement of a.̂ and about the joint i axis and i s called the angle between the l i n k s . Parameters a^ and ct^ are f i x e d f o r a given l i n k . For a prismatic j o i n t where motion i s along the j o i n t a x i s , d^ varies and 8^ i s fixe d . For a revolute j o i n t , d^ i s fixed and 9^ varies. Given these parameters, we can completely define the kinematics of Figure 3.1 Link geometry and frames for a Revolute J o i n t . - 13 - Joint n Figure 3.2 Link geometry and frames for a Prismatic Joint - 14 - the robot. The parameters are stored i n a f i l e with the format of Table 3.1. Table 3.1 - Manipulator Link Geometry F i l e Format l i n k # i d a a d 9 1 2 n i d = 1 revolute i d = 2 prismatic 3.3 Description of Welding Machine The welder c h a r a c t e r i s t i c s and s p e c i f i c a t i o n s are stored i n a data f i l e where they are e a s i l y referenced by the Expert Welder module. The f i l e contains a l l fixed settings and c h a r a c t e r i s t i c s of the welder which are relevant to the welding process. The f i l e might include some or a l l of the following: a) voltage setting b) wire diameter c) wire type d) wire feed rate - 15 - e) gas type, flow rate f ) p o l a r i t y g) minimum, maximum current ( i f under program c o n t r o l ) , or current s e t t i n g . The contents of the f i l e depend on the p a r t i c u l a r welder i n use. The format has not yet been completely s p e c i f i e d . 3.4 Description of Posi t i o n i n g Table AUTOP requires a complete d e s c r i p t i o n of the positioning table, which i s stored In a data f i l e . The following information i s stored: a) number of degrees of freedom b) l i n k geometry and dimensions c) j o i n t types (revolute or prismatic) d) j o i n t motion (continuous or discre t e steps) e) j o i n t l i m i t s f) f i r s t j o i n t l o c a t i o n r e l a t i v e to world coordinates g) table coordinate frame l o c a t i o n r e l a t i v e to l a s t j o i n t frame. The table i s treated as analogous to an n degree of freedom manipulator, so the l i n k geometry and dimension parameters have the same d e f i n i t i o n s as for a robot. We can e a s i l y accommodate a t o t a l l y d i f f e r e n t type of positioning table simply by r e v i s i n g the entries i n t h i s data f i l e . - 16 - 4. AUTOMATIC WELDING PARAMETER SELECTION USING AN "EXPERT WELDER" SOFTWARE MODULE A welding data base and interactive software module have been developed by Dr. F. Sassani of the Department of Mechanical Engineering, The University of British Columbia. A detailed description of this system, henceforth referred to as the Expert Welder, can be found in a previous report [Buchal et al 1984]. Essentially, the Expert Welder module uses a data base derived from standard welding practice to choose appropriate welding parameter settings as a function of given workpiece and fixed welder parameters. - 17 - 5. KINEMATICS 5.1 Review of Homogeneous Transformations 5.1.1 Homogeneous Coordinate Transformation It is often useful to transform the coordinates of a point in space from one coordinate frame to another (Fig. 5.1). A rotational transforma- tion i s required i f corresponding axes of two coordinate frames F^ ^, F^ are not parallel. A translational transformation is required i f the frame origins are not coincident. In general, both a rotation and a translation are required. A homogeneous coordinate transformation allows us to perform both rotation and translation in a single operation using a 4x4 homogeneous transformation matrix. For example, a vector _r expressed relative to and projected onto a coordinate frame Fj_ can be transformed into a vector R r e l a t i v e to and projected onto a frame F^_^ by the following matrix multiplication: where H . i s a 4 X4 homogeneous transformation matrix relating frame I I»I F^ to Frame The vectors R and r_ are homogeneous 4x1 column vectors, where the f i r s t three elements are the cartesian coordinates and the last element is a scaling factor. For our purposes this scaling factor is always set to 1. H. . . has the following form: -1-1, i - 18 - i - l Figure 5.1 Coordinate Transformation. - 19 - rotation translation 3 x 3 P X rotation matrix P y p z 0 0 o 1 (5.2) A more detailed description of homogeneous transformations can be found in other references [Paul 1981]. 5.1.2 Serial Combination of Homogeneous Transformations We can combine homogeneous transforms in series as shown in Fig. 5.2. Suppose we want to find R given _r. If we know Ĥ  2 and Ĥ  ^ we can find H „ as the product of H, and H : 5-1,3 = -1,2 3-2,3 * ( 5 ' 3 ) Thus, R = 3 jr . (5.4) 5.1.3 Serial Combination of Homogeneous Frames We can show that the homogeneous transformation Ĥ  ^ ̂  defines a coordinate frame F. relative to F, , . I i - l EJ , . can be written as - l - l . i i = <5-5> where ti, o_, a_y £ are 4 ^ 1 column vectors. ii, o_, ji define the basis vectors of F^ relative to F^_^,and P_ is the origin of F^ relative to F^ ̂ , as shown in Fig.5.3. - 20 - Figure 5.2 S e r i a l Combination of Homogeneous Transformations. - 21 - Figure 5.3 Homogeneous Coordinate Frame defined by n, o_, a_, g_. - 22 - Note that i f we know the position and orientation of Frame F^ relative to F^ ^, ^ is also known. 5.2 Statement of Kinematics Problem To generate a robot program, we must find the tool position and orientation at each point along the weld seam trajectory. Mathematically stated, we wish to find the homogeneous coordinate frame F of the tool G relative to and projected onto the robot base coordinate frame F . F is R G defined by the homogeneous coordinate transformation ^ which takes us from F to F . Thus, we wish to find IL . We can define a string of G R "iv, G intermediate coordinate frames F^ for which H^_^ • ̂  can be calculated, as shown in Fig. 5.4. We define the following homogeneous coordinate frames: a) F - robot base frame. R b) F T - positioning table frame. c) F - workpiece frame. B d) F^ - weld seam frame. e) F - tool frame. G We can solve for the homogeneous transformations H_ , H , H , —R,T —T,B —B,C IT, . We can then find H_ —C,G ~R,G SR.G = V T - T , B V c SC.G ' ( 5 , 6 ) 5.3 Intermediate Homogeneous Coordinate Frames 5.3.1 Table Frame Relative to Robot Base We wish to define the position of the table frame Ff relative to - 23 - Figure 5.4 Series of Homogeneous Coordinate Frames. - 24 - the robot base c o o r d i n a t e frame F . The problem i s to find the trans- R formation ^. Suppose we analyse the table as an n-link system, s i m i l a r to a robot. We can then define the transformation H. , . from l i n k i - l to l i n k i by — i - l , i the Denavit-Hartenberg [1955] equations (see Section 6.2.2.1 for de r i v a t i o n ) : H, . - i - l , i cose, Sin6 -Sin 8 Cos a. Sin 8. Sin a. a.CosS^ i i i i l i Cos 8. Cos ct. -Cos 8. Sin a. a. Sin 8. l i i i i i Sinct. Cos a, d. l (5.7) where 8̂  i s the i ^ j o i n t angle and ct̂  and a^ are fixed l i n k geometry p a r a m e t e r s . E i t h e r 6̂  ( r e v o l u t e j o i n t ) or d^ ( p r i s m a t i c j o i n t ) i s v a r i a b l e . We can combine the transformation for a l l n l i n k s as T+ - H ••• H • . (5.8) —0,n —U,1 — l , z —n-1, n th This gives us the n l i n k coordinate frame F r e l a t i v e to the f i r s t n l i n k coordinate frame F 0 We a l s o wish to s p e c i f y the loc a t i o n of the table frame F r e l a t i v e to F , and the f i r s t l i n k frame F^ r e l a t i v e to the robot base frame F . These two transformations H , H are fixed and known. Thus, R, 0 n, T H_ • _ H _ • H _ -R,0 -0,n -n,T (5.9) - 25 - 5.3.2 Workpiece Frame Relative to Table We can safely assume that the workpiece i s f i r m l y attached to the t a b l e . Thus, H i s some fixed r o t a t i o n and t r a n s l a t i o n r e l a t i v e to F_ —T , B T which needs to be s p e c i f i e d only once during i n i t i a l i z a t i o n . 5.3.3 Weld Seam Frame Relative to Workpiece We d e f i n e the weld seam frame F as shown i n Figure 5.5, where T i s tangent to the seam traj e c t o r y and A bisects the surface normals N_ , N^. ( i . e . , A i s normal to weld fa c e ) . The transformation H i s then — B, C [ T, B, A, R ] (5.10) 5.3.4 Tool Frame Relative to Weld Seam The welding rod i s positioned r e l a t i v e to the weld as required by standard welding p r a c t i c e . The p o s i t i o n and o r i e n t a t i o n of the to o l r e l a t i v e to the weld are characterized by angular o f f s e t s from the weld surface normal, a and 3, and a tool gap p ( F i g . 5.6). p, a, 3 are welding parameters determined by the Expert Welder module. We know that the elements of L can be found from -C,G From trigonometry: [ n, o, a, P ] - s i n a s i n 3 cos a cos 3 0 and P_ = - p £ . n, o are a r b i t r a r y — l e t us define them as (5.11) (5.12) (5.13) Figure 5.5 Weld Frame Relative to Workpiece. 27 - n A = Z , \ \ \ \ V \ \ F G = T O O L F R A M E / / / / / / / Y7 / / / B = Y C F c = W E L D F R A M E W E L D S E A M T = X C Figure 5.6 Tool Frame Relative to Weld Seam. - 28 - 0 cos 3 cos a si n 3 0 (5.14) and a x n (5.15) We can now f i n d elements of IT = [n, o, a, P] —C, G — — — 5.4 C a l c u l a t i o n of the Tool P o s i t i o n and Orientation Relative to World Coordinates Once we have s p e c i f i e d a l l the intermediate homogeneous trans- formations, we can calculate the to o l p o s i t i o n and o r i e n t a t i o n r e l a t i v e to F . T h i s i n f o r m a t i o n i s given by the homogeneous transformation H K. —R, G which takes us from the tool coordinates to robot coordinates. By combination of homogeneous transforms, we can ca l c u l a t e ^: H „ -R,G —R,T —T,B -B,C -C,G (5.16) The e l e m e n t s of H have the form o f f o u r column v e c t o r s , n_, o_, a_, P_: We know that the vectors n., ô , and ji define the basis vectors of the t o o l coordinate frame F projected onto the robot base frame F , and P i s G R — the displacement of F r e l a t i v e to F . I f we wish to define a world G R coordinate frame which i s d i f f e r e n t from the robot base frame, they can be simply related by a fixed transformation. 6. PATH FEASIBILITY 6.1 Statement of Feasibility Problem When we calculate a nominal robot tool trajectory through space, we may find that the trajectory is not feasible. The f e a s i b i l i t y issue can be broken into two fundamental problems: 1. Kinematic constraints of the robot. 2. Interference or co l l i s i o n with objects in the workstation. We can define a trajectory as being feasible i f the robot can follow the trajectory smoothly, without violating the kinematic constraints and without colliding with or contacting any of the objects in the work- station. Of the two problems we have defined, the kinematics problem is the least d i f f i c u l t , and we w i l l discuss i t f i r s t . 6.2 Kinematic Feasibility Mathematically the conditions which must be satisfied for kinematic fe a s i b i l i t y are: 1. The work envelope must not be exceeded. 2. The robot joint angles must remain within their physical limits. In order for us to test these conditions i t is necessary to solve for the joint angles as a function of the desired tool location and orienta- tion r e l a t i v e to the robot base (i.e., H^ This is commonly known as the inverse kinematics solution. There are two particular cases which are of practical interest. Suppose we define a robot gripper state vector X defining n independent state variables. Let us also define a joint - 30 - variable vector _6 defining m joint variables. If n = m, we can find a f i n i t e set of solutions of 9 for any set X in the work envelope. If n < m, an infinite set of solutions exists and we say the robot has redundant degrees of freedom. The kinematic problem has been considered by many researchers over the years. Denavit and Hartenberg [1955] f i r s t formally specified the geometry of connected links with homogeneous matrices. This work has been fundamental to most later work. Whitney [1969; 1972] developed the mathematics for the kinematics of multi-degree-of-freedom manipulators and prosthetic arms, and solved the inverse problem by finding the inverse Jacobian matrix, J - r e l a t i n g g r i p p e r motion to joint rate ( i . e . _9 = J_~ 1 X_). This method allows relative cartesian motion along world coordinates, based on real-time calculations of instantaneous J_( 0) and J~ \ 6). No general solution has been found for the inverse problem for absolute positions. An iterative solution based on f i n i t e displacement analysis has been proposed by Konstantinov [Konstantinov and Markov 1980; Konstantinov, Genova and Zahariev 1981; Konstantinov, Markov and Nechev 1981; Konstantinov and Patarinski 1982]. Several general recursive techniques have been proposed based on the approximate decoupling of the minor and major axis of motion [Gaglio et al 1981; Milenkov and Huang 1983; Benati et al 1982]. Explicit solutions can be found for most robot configurations of practical importance. Lee and Ziegler [1983] have derived an explicit solution for the PUMA robot based on geometry. Explicit solutions can - 31 - also be obtained by matrix algebra using the Denavit-Hartenberg matrices [Paul, Shimano and Mayer 1981; Paul 1981]. Redundant degrees of freedom are useful for avoiding joint contraints and allowing collision-free configurations to be found. The inverse problem can be solved by optimizing some criterion function. Redundant systems have been considered in more detail by a number of investigators [Fournier and Khalil 1977; Liegeois 1977; Konstantinov Markov and Nechev 1981; Benati, Morasso and Tagliasco 1982]. We w i l l develop an explicit inverse kinematics solution based on the matrix method of Paul [Paul, Shimano and Mayer 1981; Paul 1981]. 6.2.1 Definition of Robot Joint Frames and Resulting Transformations A robot arm consists of a series of rigid links connected by revolute or prismatic joints. Coordinate frames are attached to each link according to the specification of Denavit and Hartenberg [1955]. The geometry is shown in Figs. 3.1 and 3.2. First let us state some definitions: J_ = joint i axis of rotation or translation a^ = common normal of axes J^, For a revolute joint, the origin of F is located at the intersection of a n d If :LL + I intersect, the origin is at the intersection point. If J, , J, ,, are p a r a l l e l , a. i s chosen to intersect J.., at the — i —i+1 — i — l + l same place as • If joint i+1 is prismatic, replace above with the axis of the next revolute j o i n t . The o r i g i n of F^ is defined for - 32 - d i + l = °- The z axis, z^, of F^ i s along T n e x-axis, of F^ i s along a.. If J . , J. ,, i n t e r s e c t , x. i s p a r a l l e l or a n t i - p a r a l l e l to J. x J — i — i —i+1 — i — l — l + l . 9̂  i s zero when _x = x . For a prismatic j o i n t , becomes the jo i n t v a r i a b l e . In this case a_̂  = 0. The o r i g i n of F^ i s c o i n c i d e n t with the o r i g i n of F ^ f o r d^ = 0. z^ i s a l o n g a n c * *± *-s p a r a l l e l or a n t i - p a r a l l e l to J x j . -i+1 6.2.2 Inverse Kinematics Solution 6.2.2.1 General Approach to Inverse Kinematics Problem Once we have defined the j o i n t frames as shown, we can calculate the homogeneous t r a n s f o r m a t i o n r e l a t i n g frame F^ to frame The transformation i s composed of the following sequence: 1. rotate about z axis of F^_^ by an angle 9̂ ; 2. translate along z axis of F^_^ by distance d^; 3. translate along new x axis of by length a^; and 4. rotate about new x axis of F^_^ by the twist angle . These four transformations can be combined as follows: H. , . = Rot (z,9 ) Trans (z,d.) Trans (x,a.) Rot (x,a ) . —1—1,1 i i i i H, . . 0 0 -5 9̂^ 0 C9 ± 0 0 1 0 0 0 0 0 1 1 0 0 a d 0 1 0 0 0 0 1 d^ 0 0 0 1 1 0 0 0 0 0 0 Ca ± " S ^ 0 So^ Co i 0 0 0 1 (6.1) - 33 - where C9^ = Cos 9̂ , S9^ = Sin 9̂ , etc. Mu l t i p l y i n g out, we get c e . -sg^Ca. SQjSa. a.CQJ i i i i x i i S9^ C8 Ca 4 -C9.,Soi a . s e ^ i i i i i l i 0 0 Set, Ca, (6.2) This transformation was f i r s t derived by Denavit and Hartenberg [1955]. Once we have defined H 1 for the l i n k s i = l , ... n, we can combine i — l , i them to f i n d each l i n k frame r e l a t i v e to the robot base frame F , i . e . , S R f i = \ l ^ l , 2 — * i - l , i <6-3> for i = 1, n. We can also fin d the transformation H. = H. , ••• H (6.4) — i , n — i , i + l — n - l , n t i l r e l a t i n g the l a s t frame to the i frame. We now wish to solve for the j o i n t variables f o r a given t o o l p o s i t i o n and o r i e n t a t i o n . We can f i n d the homogeneous transformation H which s p e c i f i e s the to o l o r i e n t a t i o n and p o s i t i o n as a function of —~R, n the j o i n t v a r i a b l e s 9^, 8 , ••• 9^. We know the t o o l l o c a t i o n and o r i e n t a t i o n r e l a t i v e to the robot base, ^. The tool frame i s related to the l a s t j o i n t frame by a f i x e d known transformation H , so we can - n, G e a s i l y f i n d H„ = H H The elements of H„ are now known: 1 —R,n —R,G —n,G —R,n i . e . , where H = [ n, o, a, P ] , (6.5) —K, n — — — — - 34 - n, o, a are basis vectors of link n frame relative to F ; — R P_ is position vector of link n frame relative to F R. Let us c a l l this matrix T^. We can now equate T and H element by element, searching for explicit —n —R,n equations for the joint angles 9 ^ , • • • 9 ^ . If necessary, we can also compare the elements of other equivalent matrices defined below. T = H —n —R,n = - l , n ^ 1 , 2 h ] l ^ n = h , n We continue un t i l we have explicit solutions for a l l the joint variables. The actual solution of a particular robot is generally based on intuition and simplification. The particular solution for a UNIMATION PUMA 560 follows. 6.2.2.2 PUMA 560 Solution For the PUMA robot, we can define a set of link frames as shown in Fig. 6.1. We can also specify the appropriate link parameters in Table 6.1. (6.6) (6.7) , etc. (6.8) Figure 6.1 PUMA Joint Frames and l i n k parameters. - 36 - Table 6.1 - PUMA Link Parameters Link Variable a a d 1 91 -90 0 0 2 e2 0 a 2 d 2 3 6 3 90 ~ a 3 0 4 \ -90 0 5 95 90 0 0 6 66 0 0 d 6 Note that the sign of a 3 is negative to conform to our definition of x 2. For the PUMA, the link parameters have the following values: a 2 = 432 mm d 2 = 149.5 mm a3 = 20.5 mm d^ = 432 mm d 6 = 56.5 mm Note that the following analysis also applies to any other 6 degrees of freedom robot with the same geometry. The correct values of the link parameters a ^ , d 2 ' ^4' a n C* ^6 a r e s * m P l v substituted into the solutions. We know the position and orientation of F relative to F from o 0 SQ,6 = t £ » £ » £ ' I 1 ' ( 6 , 9 ) where n, o, a are the F, basis vectors projected onto F ; - 37 - P Is the origin relative to F n . — b U Note that F^ i s the frame attached to link 0 by our definition, and is e q u i v a l e n t to the robot base frame F . F, i s attached to the tool R u mounting flange. We have defined the inter-link transformations H. for each link - i - l , i and we know that H_ , = H_ . H . * * • H. . y X y X X y £• X X y X (6.10) From the geometry of the arm (Figure 6.2) we note that the origin of F^ is dependent on the variables 9̂ , 0̂ , 0̂  only, and is coincident with the origin of F,.. Let q = P - d, a (origin of F c) , (6.11) — — o — J and q̂  = 4*"** column of HQ ^ (origin of F^) . We can find ji by multiplying HQ ^ ^ £ 2 3 H 3 4 ( w e n e e c * t n e 4 t ' 1 column only). The 4 t h column of H_ , is S 3 d 4 + a 3 C 3 " C 3 d 4 + a 3 S 3 0 1  - 39 - The 4th column of H, . is -1,4 C 2(S 3d 4 + a 3C 3) + S 2(C 3d 4 S 2 ( S 3 d 4 + a 3C 3) - C 2 ( C 3 d 4 a3 S3> + a2 C2 a 3S 3) + a 2S 2 q = 4th column of H •,. — HJ, 4 Multiplying out, we get q = C 1 [ c 2 ( S 2 d 4 + a 3C 3) + S 2(C 3d 4 - a 3S 3) + a ^ ] S 1 [ c 2 ( S 3 d 4 + a 3C 3) + S 2(C 3d 4 - a 3S 3) + a ^ ] -S 2(S 3d 4 + a 3C 3) + C 2(C 3d 4 - a 3S 3) - a ^ 1 q can be simplified as follows: , r j . „ n 1 _ S D C l d 2 S l d 2 C l d 2 q. = W 2 3 + a 3C 2 3 + a 2C 2 S l K S23 + a 3C 2 3 + a 2C 2 d4 C23 " a3 S23 " a2 S2 1 (6.12) (6.13) where 323 = sin( 6 2 + 93) u23 = cos(6 2 + 8 ) - 40 - Solution f o r 9^ From the f i r s t two rows of (6.13) we can show that + , 2 ^ 2 ,2,1/2 , fl -1 r * q 2 ( q l + q2 ~ V - d 2 ^ 1 1 ..... 1 q l ( q l + q2 " d2> + d 2 q 2 + for l e f t handed arm configuration, - for right handed arm configuration. Note: For 9 = tan 1 (y) ; 0 < 9 < 90° for x > 0 and y > 0; 90° < 9 < 180° for x < 0 and y > 0; -180° < 9 < -90° for x < 0 and y < 0; -90° < 9 < 0 for x > 0 and y < 0. Solution f o r 9^ Let 2 2 2 2 2 2 2 q l + q2 + q3 " d2 ~ d4 " a3 " a2 2 a2 (6.15) B = - d 4 ; C = a 3 . (6.16),(6.17) We can show that A = -B s i n 9^ + C cos 9^. The so l u t i o n has the following form: 8 3 = t a n " 1 (|) - t a n " 1 ( ± A - ) (6.18) ^ 2 * 2 - A'' where 2 2 2 r = B + C . (6.19) - 41 - Substituting (6.16), (6.17) into (6.18) we get 8 = t a n _ 1 ( - ^ ) - tan" 1 ( " A " ] (6.20) J " d4 ± f l ~2 r - A 2 2 2 where r = a^ + (6.21) Two different solutions result depending on the sign: + for elbow above configuration, and - for elbow below configuration, (for lef t arm configuration); or - for elbow above configuration, and + for elbow below configuration, (for right arm configuration). Solution for 8^ From (6.13) we get q3 = d4 C23 ~ a3 S23 " a2 S2 " ( 6 * 2 2 ) Expanding, we get q3 = d J C 2 C 3 " S 2 S 3 ] " a3 t S2 C3 + C2 S3 ^ " a2 S2 ' ( 6 * 2 3 ) Rearranging we get q3 = ^ d4 C3 " a3 S3 J C2 " K S3 + a3°3 + a2 ^ S2 * ( 6* 2 4> Let A = d 4C 3 - a 3S 3 , (6.25) B = d 4 s 3 + a 3C 3 + a 2 . (6.26) Then the sol u t i o n i s t a n - 1 (• A :)-tan" 1( — z = — — ) • (6.27) B ± / 2 - 2 r q 3 Substituting (6.25), (6.26) into (6.27) we get - 42 - •> d. C_ - a_S_ , q. °2 • ^ V . ^ > - 1 «.») where 2 2 2 r = A + B . (6.29) Expanding (6.29), we get r 2 = (d 4C 3 - a 3 S 3 ) 2 + (d 4S 3 + a 3C 3 + a 2 ) 2 . (6.30) Again, there are two solutions for (6.28); + is for the lef t handed arm configuration and - is for the right handed arm configuration. There are two solutions for 8̂  because the parameters and q^ are not in the equation. It is intuitively obvious that for a given waist r o t a t i o n 8^, there exist two possible shoulder rotations, » which position the arm at the same height q 3. The possible rotations position the arm in opposite quadrants of the horizontal plane. The correct solu- t i o n can be determined by substituting 0̂  into the equation for the parameter q^ or q^ (equation 6.1) and comparing the sign of the result. It turns out, however, that the positive solution is always associated with the l e f t arm configuration and the negative solution with the right arm configuration. Solution for 8., 8 JT Z T — — — _ Set 6̂  such that is normal to the plane formed by z^ and a_, i.e., (z- • a ) z. = ± - H = 1- (6.31) \L3 ' £ T and - 4 3 - "4 fe3 * ̂  ' ( 6 ' 3 2 ) cos e 4 . = ± t 3 ' £ 4 ) • < 6 - 3 3 > Note that x^, are the f i r s t two columns of ^. Expanding out ^ and s u b s t i t u t i n g 3 ^ , _V_ and £^ into (6.32), (6.33), we can show where + gives wrist not f l i p p e d configuration and - gives wrist f l i p p e d configuration. Set 8 5 so that £5 = £ > (6.35) i . e . , s i n 8,. = x^ * £ » (6.36) and cos 8̂  = -y^ * £ . (6.37) x., y, are the f i r s t two columns of H. ,• Expanding out H . , and —4 —4 —0,4 0 —0,4 sub s t i t u t i n g x^, y^ into (6.36), (6.37) we can show - i r ( C 1 C 2 3 C 4 - S l S 4 ) a x + <SlC23C4- + - C l V S y " C 4 S 2 3 a Z , 95 = t a n L C.S-.a + S . S „ a + C „ a J ' ( 6 ' 3 8 ) 1 23 x 1 23 y 23 z Set 8 so that y, = o , (6.39) o —o — i . e . , s i n Q. = y c • n , (6.40) 0 — J — and cos 8 = y • o . (6.41) o — J — y,. i s the second column of H. c . —5 -0,5 M u l t i p l y i n g out HQ ,. and s u b s t i t u t i n g y^ i n t o (6.40), (6.41) we can show . f l -1 r ( - S l C 4 - C l C 2 3 S 4 ) n x + ( C 1 C 4 ~ S l C 2 3 S 4 ) n y + S 4 S 2 3 n Z } ... 6 L < - S l C 4 - C l C 2 3 S 4 ) o x + < C l C 4 - S l C 2 3 V ° y + S4 S23°z J * ( ' - 44 - 6.2.2.3 Discussion of PUMA Solutions When we inspect the joint angle solutions, we note that joints 1, 3 and 4 have 2 solutions each. These solutions correspond to different robot c o n f i g u r a t i o n s which y i e l d the same tool position Hrt For —0, o instance, the arm may be in a left-handed or right-handed configura- tion, the elbow may be above or below the tool, and the wrist may be flipped. This suggests that different configurations might be tried when searching for a feasible configuration. In order to obtain smooth motion between points, however, we must insist that the same configuration be maintained throughout a given trajectory. 6.2.3 Comparison of Five Degree of Freedom and Six Degree of Freedom Robots A 6 degree of freedom robot allows us to completely specify the tool position and orientation in space. In the case of welding, however, we note that the tool i s symmetric about i t s axis. This means that any rotation about the tool axis does not change the effective tool orienta- tion. Thus we see that we need only specify the tool position and a single orientation vector a_. Five degrees of freedom are sufficient and a 6th degree of freedom is redundant. For a 5 degree of freedom robot, the tool frame basis vectors n, o are found to be functions of the orientation vector a and the position vector P. The relationships are derived in the following analysis for a PUMA robot with the 4th joint omitted. 6.2.3.1 Solution for a Five Degree of Freedom Robot Let us now define a 5 degree of freedom robot which i s - 45 - geometrically and dimensionally identical to the 6 degree of freedom PUMA previously analysed except that the 4th joint (wrist) is now rigidly fixed at 6 = 0 . 4 We can redefine the link parameters as follows: Table 6.2 - Five Degrees of Freedom PUMA Link Parameters link a a d 1 -90 0 0 2 0 a 2 d 2 3 0 a 2 0 4 90 0 0 5 90 0 d 5 Let us also define a tool relative to the last link mounting flange H "5,6 transformation matrix -5, 0 0 1 d x 0 1 0 d y -1 0 0 d z 0 0 0 1 (6.43) It can be shown that we can achieve any location and orientation of the tool axis Ze i f Z, is perpendicular to Z c. —o —o — J Figure 6.3 Tool Frame Relative to Robot Tool Mounting Flange. - 47 - Solve f o r t o o l frame F, r e l a t i v e to F 6 matrices are: C9 1 0 - S 9 1 0 S 9 1 0 C 9 1 0 0 -1 0 0 0 0 0 1 C 9 2 -S9 2 0 a 2C9 2 S9 2 C9 2 0 a 2S9 2 0 0 1 d2 0 0 0 1 C9 3 -S9 3 0 a 3C9 3 S9 3 C9 3 0 a 3S9 3 0 0 1 0 0 0 0 1 C 94 0 S 6 4 0 S 94 0 0 0 1 0 0 0 0 0 1 The Denavit-Hartenberg (6.44) (6.45) (6.46) (6.47) - 48 - ^4,5 c e 5 - s e 5 0 0 s e 5 0 0 0 0 1 d 5 0 0 0 1 0 0 1 d X 0 1 0 d y -1 0 0 d z 0 0 0 1 (6.48) (6.49) The inverse matrices are: -1 So,i -S 1 0 1 0 0 c . 0 -1 0 0 0 0 0 1 (6.50) - 4 9 - = C 2 S 2 0 - A 2 S6 2 C9 2 0 0 0 0 1 " D 2 0 0 0 1 = S 3 0 _ A 3 " S 3 C 3 0 0 0 0 1 0 0 0 0 1 = " C 4 S 4 0 0 ~ 0 0 1 0 S 4 " C 4 0 0 0 0 0 1 = S 5 0 0 ~~ C 5 0 0 0 0 1 D 5 0 0 0 1 ( 6 . 5 1 ) ( 6 . 5 2 ) ( 6 . 5 3 ) ( 6 . 5 4 ) - 50 - 0 0 1 0 0 1 0 0 -1 0 0 0 -d -d x The intermediate transformations are: 0 0 -1 0 0 1 0 0 1 0 0 0 H, , -^,6 0 0 -1 0 -s, J5 0 J5 0 C cd -S cd 5 x 5 y S,d +C cd 5 x 5 y d +d c z 5 (6.55) (6.56) (6.57) - 51 - ^3,6 -S, 0 0 - C4 S5 - s 4 s 5 '5 0 C 4 C 5 S 4 C 5 5 0 C.(C cd -S cd ) 4 5 x 5 y + s 4 ( d z + d 5 ) -c 4(d z+d 5) S rd +C_d 5 x 5 y (6.58) Let d = d c+d . s 5 z (6.59) 52.6 " C 3 S 4 " S3 S4 S 3 C 4 C 3 C 4 0 0 " C 3 C 4 S 5 + S 3 S 4 S 5 " S 3 C 4 S 5 " C 3 S 4 S 5 '5 0 C 3 C 4 C 5 " S 3 S 4 C 5 1 C„[C.(C.d - S.d ) 3 L h 5 x 5 y + S.d ] 4 s J 1 -S_[S.(C,d - S„d ) 3 4 5 x 5 y - C.d ] + a 0C. 4 s 3 3 S 3 C 4 C 5 + C 3 S 4 C 5 1 S„[C.(C\d - S.d ) 3 L 4 5 x 5 y' + S,d ] 4 s J 1 +C„[S,(C,d - S.d ) 3 4 5 x 5 y - C,d ] + a_S. 4 x J 3 3 S5 1 S cd + C cd 5 x 5 y 0 1 1 — (6.60) IL , i s broken down into columns. —1,6 - 52 - «1,6 ( 1 ) " c 2 [ -c 3 s 4 s 2 [ -c 3 s 4 s 3 c 4 ] - s 2 [-s 3 s 4 + c 3 c 4 ] s 3 c 4 ] + c 2 [ - s 3 s 4 + c 3 c 4 ] 0 0 (6.61) c 2 [ - c 3 c 4 s 5 + s 3 s 4 s 5 ] - s 2 [ - s 3 c 4 s 5 - c 3 s 4 s 5 ] s 2 [ - c 3 c 4 s 5 + s 3 s 4 s 5 ] + c 4 [ - s 3 c 4 c 5 - c 3 s 4 s 5 ] »1,6<3> C2 t C3 C4 C5 S2 [ C3 C4 C5 - s 3 s 4 c 5 ] - s 2 [ s 3 c 4 c 5 + c 3 s 4 c 5 ] - S 3S 4C 5] + C 0[S,C AC q + 0,8,0,.] 2 l"3 4 5 3"4 5J - 53 - *1,6<4) = s 2[ • • • ] - s 2 [ ] + c2[ ] +a 2C 2 ] +a 2S 2 S cd + C cd + d„ 5 x 5 y 2 (6.64) ^0,6 [ n, o, a, P ] . (6.65) Let So|l So,6 where and f u ( n ) f n ( o ) f u ( a ) f u ( P ) f 1 2 ( n ) f 1 2 ( o ) f 1 2 ( a ) f 1 2 ( P ) f 1 3 ( n ) f 1 3 ( o ) f 1 3 ( a ) f 1 3 ( P ) 0 0 0 1 f n ( A ) = C l A x + S l A y , f 1 2 ( A ) = -Az , f 1 3 ( A ) = " S ^ + C ^ , A is n, o, a or P. -1 (6.66) (6.67) (6.68) (6.69) We also know H„ n H_ , = H., ,, which we have found as a function of 9„. —U , J. —U,D — 1 , 0 Z 6^, 6^, 9^. Equating elements we find - 54 - 0 = -S.,n + 1 X C.n , 1 y (6.70) C5 = -5,0 + 1 X 1 y (6.71) S5 1 X 1 y (6.72) d2 5 x 5 d = -S.P + C..P y 1 x 1 y (6.73) We know the po s i t i o n P_ and axis o r i e n t a t i o n a_ of the t o o l . We now solve for the remaining t o o l o r i e n t a t i o n vectors n, o, as a function of a, P. Multiply (6.71) by d y , multiply (6.72) by d x and subtract (6.71) & (6.72) from (6.73). D - d o - y y y x y d. = -S. [P - d o - d a ] + C. [p  d a ] . (6.74) 2 l L x y x x x J 1 L y y v x v J Rewrite as d2 = S i a + C 1 B (6-75) where a = P - d o - d a (6.76) x y x x x v ' and 3 = P - d o - d a (6.77) y y y x y Let a = r cos <j> , r = / a 2 + 32 , (6.78), (6.79) 3 = r s i n <t> , • - t a n " 1 (-|) . (6.80), (6.81) Substitute for a, 3 i n (6.75). s i n <|> cos 8 - cos <J> s i n 8̂  = — (6.82) d2 or sin(<{> - 9^ = — (6.83) and - 55 - cos(<|>- 9 ) = ± J d T T . (6.84) 1 1 " (—) v r We can then show 6 = t a n - 1 ( | ) - t a n ' M — — = = ) (6.85) r - d 2 where a = P - d o - d a , (6.86) x y x x x p = P - d o - d a , (6.87) y y y x y 2 2 2 r = a + 3 • (6.88) Noti c e that 9^ i s a f u n c t i o n of j>, which i s what we are tr y i n g to f i n d . The sol u t i o n i s s i m p l i f i e d i f we i n s i s t that d = 0. y Now a = P - d a , (6.89) X X X 3 = P - d a . (6.90) y x y We can solve for 9^ since we know P, a_. From (6.70) we know n = n t a n " 1 9. . (6.91) y x 1 We also know that the vectors are unit vectors and that they are ortho- gonal . Thus, 2 2 2 n + n + n = 0 (6.92) x y z and n a + n a + n a = 0 . (6.93) x x y y z z - 56 - Combining and solving for n^, we get . + r,. 2 2 2. 2 . 2,, 2, 2 . 2. 2. J./2 -2n a a ± |(4n a a ) - 4(a + a )(n (a + a ) - a ) x x y L x x y y z x x z z J „ _ _ .(6.94) 7 2 (a + a ) v y z ' Combining (6.91) and (6.94) we get e a 2 1/2 n x = ± -2 ~ 5 r ] (6.95) e tan 9 + 2e tan 9, a a + e(a + a ) 1 1 x y x z where and e = a 2 + a 2 (6.96) y z v ' v 2 2 n = ± l - n - n . (6.97) z x y v ' Choose the sign of n^ which gives a_ • ii = 0 . Note that there are two antiparallel solutions for ja. We can now find o simply as o_ = a_ x n . (6.98) We could now find explicit solutions of the joint angles as functions of P_ and a_. However, having found n, o_, we have defined two unique tool orientations for which the 6 degree of freedom PUMA solution is guaranteed to give 9^ = 0. If we post multiply Ĥ  ̂  by the inverse tool transforma- tion H c ^ " , we get the tool mounting flange location and orientations, J , o i.e., «0,5 = «0,6 «5*6 ' <6'"> This i s equivalent to ^ = [n, j3, a_, PJ for the 6 degree of freedom case. Thus the 6 degree of freedom solution for this H_ , w i l l yield the —U, o - 57 - 5 degree of freedom j o i n t angles. 6 . 2 . 3 . 2 ASEA I R B 6 0 Solution For a 5 degree of freedom robot such as the ASEA I R B 6 0 , we can specify the tool l o c a t i o n jp and just one tool frame basis vector, a. The other basis vectors n, o_ are determined by P and a. We can solve for n, o as we did previously for the 5 degree of free- dom PUMA s o l u t i o n . The complete to o l frame i s then known. We can modify the 6 degree of freedom PUMA solutions by su b s t i t u t i n g the ASEA l i n k parameters and solving for the angles given H R j T " [ H> £» £» 1 ] • ( 6 . 1 0 0 ) As we noted i n S e c t i o n 6 . 2 . 3 . 1 , t h i s ^ i s guaranteed to give a wrist r o t a t i o n 9 ^ = 0 . Thus, i f we neglect the 9̂  s o l u t i o n and reassign the other j o i n t angles as 9^ : = 9^, 9̂  := 9̂  we can generate the solu- tions for the ASEA I R B 6 0 . The r e s u l t s of Section 6 . 2 . 3 . 1 can be s i m p l i f i e d for an ASEA I R B 6 0 by noting that = 0 . From ( 6 . 8 5 ) we get P - d a ( 6 . 1 0 1 ) X X X for the following tool transformation: 0 0 -1 0 0 1 0 0 1 0 0 0 X ( 6 . 1 0 2 ) 0 d - 58 - From (6.91), (6.95) and (6.97) we can calculate n as a function of a_, 9̂ . (a Z + a Z ] a ^ y z 1 z (a +aZ )tan Z 9 + 2 (aZ+a )a a tan 9, + (a^+a ) fa +a 1 y z y l _ ^ v y z ^ x y 1 v y z ^ x z - _ n = n tan 9, , 1/2 , (6.103) (6.104) _ n = ± 1 - n - n z x y 21 (6.105) The sign of n i s chosen to give a • n = 0. Note that there are two z — — antiparallel solutions for n. Generally only one of the solutions w i l l lead to a feasible set of joint angles. We can now find o_ from (6.98), i.e. o = a * n The joint solutions can be found by plugging the appropriate components of n, o_, a, p. Into the joint angle equations from Section 6.2.2.2. The equations can be simplified for the ASEA IRB60 by noting that the link parameters , a^ and the wrist rotation 9̂  are zero. Rename the j o i n t angles, 94 : = 95 • 95 := 66 * The resulting solutions are as follows: 6 = tan p - d a _ 1 ( J - / J ) . d a x x (6.106) = tan -1 .(. ,2 2 . z, z , z ,z z.z v _ ± l 4 d 4 a 2 ~ ( q l + q 2 " ^ 3 4~ a2 ) ' 2, 2, 2 .2 2 q l + q 2 + q 3 " d 4 " a 2 2~2 " ~ T~2 2.2 a/2 (6.107) - 59 - 92 = t a n -1 d4 c3 d 4 s 3+a 2 - tan -1 . ± ( d 4 + d 4 a 2 s 3 , 2 2 N1/2 +a 2-q 3) J , (6.108) 9. = tan 4 -1 C l C 2 3 a x + S l c 2 3 a y ~ s23 az i_c s ,a + s s 0,a + c.,a 1— 1 23 x 1 23 y 23 z (6.109) = tan -1 -s,n + c,n 1 x 1 y -s,o + c,o 1 x 1 y (6.110) where c. = cos 9 s. = sin 9 c. . = cos (9+9.) s l t = sin(9+9.). i i ' i i ' i j i ] i ] i y The link parameter values for the ASEA IRB60 are: a 2 = 800 mm, a 3 = 1150 mm, d,. = 200 mm + z component of the tool transformation. 6.2.3.3 Redundancy of the Sixth Joint for Welding It is important to note that the welding torch is axisymmetric. Thus, we need only specify the position of the torch tip and the orienta- tion vector of the torch axis. Any arbitrary rotation of the torch frame about the torch axis does not change the effective torch orientation (Fig. 6.4). We find that only 5 degrees of freedom are needed to define a position P_ and a single orientation vector a_. We can show that the other two basis vectors ri, o_, of F^ are then determined as a function of the joint angles and of P and a. - 60 - If we have 6 degrees of freedom, we can independently define the position P̂  and any 2 of the basis vectors n, o_, ji (the third basis vector i s defined by the other 2). This means that we can explicitly rotate F c 6 about the torch axis ji without changing the tools effective orientation (Fig. 6.4). We w i l l use this trick, in searching for feasible solutions of a trajectory by specifying a rotation of F,. about a, calculating a new b — HQ g and testing the resulting joint angles for f e a s i b i l i t y . 6.2.4 Specification of Robot Coordinates The position of a robot may be uniquely specified two ways: 1. a l l joint angles specified (explicit); and 2. position and orientation of tool frame specified. The f i r s t case is t r i v i a l once we have solved for the joint angles. The second case requires transformation of the tool frame specification F £ 6 into position and orientation coordinates for the target robot. We know F G = [ »» £ . £» I ] » (6.111) where n, o, a are basis vectors of F projected onto F , — — — (J R and P is the position vector of the F origin relative to F . — O R For example, for the PUMA 560 robot we must specify coordinates X, Y, Z, 0, A, T where X, Y, Z are the Cartesian coordinates of the tool frame F^ r e l a t i v e to the robot base F and 0, A, T are three Euler rotation angles of F, about the axes of F^. —G —R - 61 - Figure 6.4 Rotation of Tool Frame About Tool Axis. - 62 - Calculations of the robot position and orientation specifications from the j o i n t angles or from F depends on the robot being used and how —o i t defines i t s locations. 6.2.4.1 Calculation of PUMA Tool Location and Rotation Coordinates The PUMA robot defines locations by the Cartesian coordinates of the tool center, X, Y, Z, and three Euler angles o, a, t, the orienta- t i o n , a ltitude and tool angles (Fig. 6.5). For o, a, t angles a l l equal to zero, the tool frame F̂ , i s related to the robot base frame F by a transformation IL R —R, T 0 1 0 X 0 0 -1 Y -1 0 0 z 0 0 0 1 (6.112) where X,Y,Z are the position vectors of the or i g i n of F re l a t i v e to F . 1 R X, Y, Z do not change with angles o, a, t . The rotated tool frame F^ i s found by performing an Euler angle rota- tio n sequence t , a, o to find the transformation ^. MR T = rot(o,3) ro t ( a , l ) rot(t,2) , (6.113) where rot(o,3) means rotate about z axis ( i , ) of F by an angle o, etc. — j R rot(o,3) cos o sin o 0 0 -s i n o cos o 0 0 0 0 1 0 0 0 0 1 (6.114) - 63 - Figure 6.5 Definition of o, a, t angles for the PUMA Arm. - 64 - rot(a,1) = rot(t,2) = 1 0 0 0 cos t 0 -sin t 0 0 cos a sin a 0 0 1 0 0 0 -sin a cos a 0 0 0 0 1 sin t 0 cos t 0 0 0 0 1 (6.115) (6.116) Multiplying out we get V where -C S -S S C o t o a t -S S +C S C o t o a t -C C a t C C -S S S t o t o a t S C +C S S t o t o a t -C S„ a t cos(o), S = sin(o), etc. o S C o a -C C o a -S a 0 Y Z (6.117) If a = - 90° , rotations o, t are coaxial. In this case we w i l l arbi- t r a r i l y set t = 0. To f i n d the angles o, a, t from a known transformation H„ m we —R, T proceed as follows: We know that x = [ VN, VO, VA, VP ] (6.118) where - 65 - VP = X Y Z 1 VN] VN = VN2 , etc. VN, 0 Premultiply both sides of (6.118) by rot(o >z) \ rotto.z)" 1 E^- = r o t ( o , z ) _ 1 [VN, VO, VA, VP ] . Multiplying out and equating elements we get 0 cos(t) -sin(o) VA1 - cos(o) VA sin(t) sin(a) cos(a) cos(o) V0± + sin(o) V0 2 -cos(o) VN, - sin(o) VN. -VA„ sin(o) VA, - cos(o) VA, From these equations we get o = tan i  - V A i _ 1 f 1 V A2 o = o + 180 a = tan -1 -VA„ sin(o) VA1-cos(o) VA2 (6.119) (6.120) (6.121) (6.122) (6.123) (6.124) (6.125) (6.126) -cos(o) VN -sin(o) VN„ _ -1 r 1 I i L cos(o) VO^ sin(o) V0 2 J (6.127) For the PUMA, the angle t is defined as the negative of the usual Euler rotation convention. If we change the sign of t to conform to the PUMA definition we get: cos(o) VN, + sin(o) VN„ t = tan _ 1 [ 1 2 cos(o) V0 1 + sin(o) V0 2 ] (6.128) Also the following is true: (o, a, t) = (o + 180, 180 - a, t + 180). - 66 - 6.3 Collision Avoidance In order to successfully generate a robot program off-line, i t is essential that we be able to reliably avoid collisions between the robot and objects in the workstation. This is a d i f f i c u l t problem which is the subject of much current research. To handle this problem our world model must contain complete geo- metric descriptions of the robot and objects in the workstation. The geometric models can be provided by CAD systems in several forms. Solids are usually represented by a description of their surfaces, or as a composite of primitive solid elements. The most common surface repre- sentation is the polyhedral model which defines a solid polyhedron by specifying a l l of i t s vertices, edges and faces [Hosaka, Kimura and Kakishita 1974; Wesley et al 1980]. The resulting description is simple, uniform and unambiguous. More advanced CAD systems use constructive solid geometry to generate complex solids by performing boolean operations on a set of simple solid primitives such as boxes and cylinders [Braid 1975; Requicha 1980]. The resulting representation must contain a record of the composite primitives as well as the sequence of operations. More complete surveys of solid representations can be found in the literature [Baer, Eastman and Henrion 1979; Requicha 1980]. The c o l l i s i o n avoidance problem has been well summarized by Lozano- Perez [Brady et a l 1982]. The three classes of co l l i s i o n avoidance algorithm are: hypothesize and test, penalty functions and explicit free space. - 67 - The hypothesize and test algorithm generates a collision free path by t r i a l and error. Each time a c o l l i s i o n is detected an alternate path is formulated and tried. This method is simple but does not lead to an optimum solution and may not find a solution at a l l because the knowledge of the world is localized only and thus a global strategy is impossible. The penalty function scheme attempts to define a penalty function whose value reflects the probability of co l l i s i o n as a function of position in space. The algorithm then generates a path following the local minima of the penalty function. This method has been successfully applied to extremely simple world models only, and the algorithm becomes very complex for more re a l i s t i c situations. The third class of algorithm generates a set of a l l robot positions which are free of collisions and calls this set the free space. The shortest path through the free space joining an i n i t i a l and fi n a l position is then the optimum path. The main d i f f i c u l t y lies in generating the free space, and a l l algorithms to date incorporate simplifications and approxi- mations which restrict their generality. Several free space path planning algorithms have been presented in the literature [Udupa 1977; Lozano- Perez and Wesley 1979; Lozano-Perez 1983; Brooks 1983]. The problem of simply detecting collisions between the robot and objects in the workstation is a simpler problem to tackle at this point. However, a feasible path must then be found iteratively or by t r i a l and error. We w i l l not consider the alternate path selection problem, other than to note that in the case of welding we must reposition the workpiece i f a co l l i s i o n is detected. - 68 - The simplest interference detection problem is the problem of inter- ference between fixed, stationary objects. Hosaka [1974] describes a method for completely specifying the intersection between two stationary convex polyhedra. Maruyama [1972] has proposed a simple method for determining intersections between polyhedral solids using a hierarchical set of tests. The problem of interference between moving and stationary polyhedra was considered by Boyse [1979]. The tests which he devised apply only to the simple cases of pure translation and rotation and cannot be extended to the more general case of complicated motion. An algorithm for testing for interferance between two moving convex polyhedra is suggested by Schwartz [1981]. His method finds the minimum distance between two convex polyhedra as a function of time i f their individual trajectories are known functions of time. Schwartz demonstrates the algorithm for the two dimensional case only. Another way of handling interference detection between moving objects is to define a swept volume generated by the moving object. The problem is then reduced to the simpler case of interference between stationary solids. This method is discussed by Requicha [1980] and Lozano-Perez and Wesley [1979]. We w i l l develop a detailed interference detection algo- rithm based on the swept volume method. 6.3.1 World Model Any world model representation embodies a trade-off between accuracy and conciseness for solids of arbitrary shapes. If we use a polyhedral representation we can choose any number of surface facets to - 69 - approximate curved surfaces to any degree of accuracy. In p r a c t i c e , however, we w i l l generally use a coarse approximation to reduce the s i z e of the data structure and increase the speed of any processing we wish to do. Unfortunately, any sim p l i f y i n g approximations that we make introduce uncertainty into our c o l l i s i o n p r e d i c t i o n s . We f i n d that we must trade off the s i m p l i c i t y of our mathematical model and algorithm against the r e l i a b i l i t y of the r e s u l t i n g c o l l i s i o n p r e d ictions. We can bias our model for the worst case, however, to ensure that a l l c o l l i s i o n s w i l l be predicted. The price we pay i s a higher p r o b a b i l i t y of c o l l i s i o n s being predicted where none occur. This can be an important consideration i f we must move near the objects i n the workstation, as i n welding. The world model which we w i l l adopt w i l l be a simple one for the purpose of demonstrating the basic concepts of interference detection. The general approach which w i l l be described can be applied to a more sophisticated and accurate world model and interference detection scheme. We w i l l consider a world model which divides the workstation into two sets of objects, moving s o l i d s and stationary s o l i d s ( F i g . 6.6). The moving s o l i d s include the robot manipulator and a l l parts attached to i t . Stationary s o l i d s include the workpiece, positioning table, f i x t u r e s , e t c . Although the table may be movable we w i l l assume that i t s motion cannot cause interference. Stationary objects i n the workstation w i l l be modelled as s o l i d polyhedra. A polyhedral representation allows our model to have any - 70 - I n i t i a l P o s i t i o n s i t i o n Figure 6.6 World Model - 71 - degree of accuracy we desire, and allows a simple data structure of vertices, plane faces and straight edges. For simplicity, we w i l l model the robot links as circular cylinders. The axes of the cylinders w i l l coincide with the axes of the links and the cylinder radius and length w i l l be chosen such that the cylinder f u l l y contains the link (see Fig. 6.7). Each cylinder w i l l be defined relative to the corresponding link coordinate frame. 6.3.2 Development of Swept Volume As the robot links move they sweep out a volume of space, as shown in Fig. 6.6. If we can analytically describe that volume, we can test i f the volume intersects any fixed solid in the workstation. In order to simplify the algorithm and minimize the required calcula- tions we w i l l adopt a method for approximating the swept volume in a concise way. To illustrate the method, we w i l l consider the motion of one cylindrical solid through space. Suppose we have a cylinder of radius R and length L. It has a central axis or spine with endpoints A and B. If the cylinder is moved from an i n i t i a l position Pi_ to some other position P2, then some volume has been swept out by the cylinder. If we assume the motion was smooth and continuous between Pi and P2, we can use the following approximation for the swept volume. The i n i t i a l and f i n a l endpoints of the spine are connected with straight lines A]A2> B]_B2« These line segments, along with the i n i t i a l and fi n a l spine positions, define a ruled, parametric surface. The surface has a simple, non-linear parametric equation of the form - 72 - Figure 6.7 C y l i n d r i c a l Approximation of a Manipulator Link. - 73 - r (A , A ) = r + A U + Av + A A (u - u ) (6.129) —p u v —o u—o v—o u v M. —o where r , u , v , u, are defined as shown in Fig. 6.8. —o —o —o —1 If we restrict the ranges of the parameters to 0 < \̂  < 1, 0 < Av < 1, we obtain a surface patch or region bounded by the vectors £o» iLl> Zo» Z l a s s n o w n 1 1 1 Fig* 6.8. We can now define two bounding parametric surfaces offset from this parent surface by a distance R on each side. To do this, we define the corner points of the new surfaces by finding the surface normals of the parent surface at each corner, and translating along those normals by distances ± R. The two offset bounding surfaces can be expressed relative to the parent surface by the following equation: r u(A , A ) = r (A , A ) ±R n( A , A ) (6.130) —b u v —p u v — u v where R is the cylinder radius, A , A are the surface parameters, u v ii is the unit surface normal of the parent surface, r is any point on the parent surface —P satisfying equation (6.129). The unit surface normal is defined as follows: V X V £ = |7 x 71 (6.131) '—u -v 1 3 r where 7 = ^2. = u + \ ( u - u ) (6.132) —u a A —o v —1 — O u 3r V = = v + A ( U l - u ) . (6.133) —v 3 A —o u —1 —o F l 8 U f e 6'8 Para m, S w e P t Volume. - 75 - If we expand these equations and collect the terms, we find that the bounding surfaces can be expressed by equations of the form r = A + A B + A C + A A D (6.134) —b — u— v— u v— where A , B_ , C_ , D_ are known. Thus, the bounding surfaces are also ruled, parametric surfaces. Given these two bounding surfaces, we can easily derive similar surfaces over each of the four open faces of the volume between the bound- ing surfaces. Only two of these surfaces need to be found since the other two faces are bounded by the cylindrical surfaces of the link in i t s i n i t i a l and fin a l position. The interference detection problem is now to find intersections between the polyhedra representing stationary objects and the parametric swept volumes generated by the moving objects. A series of simple tests can be performed which w i l l determine i f intersection occurs. In practice, we would generate swept volumes for each moving link of interest. The motion of the manipulator would be divided into a series of Intermediate motions, with each intermediate motion generating a swept volume. 6.3.3 Interference Conditions We can formulate five simple intersection conditions (see Fig.6.9): 1. Any solid polyhedral edge intersects any swept volume bounding parametric surface. 2. Any polyhedral edge Intersects the cylindrical solid of a link in it s i n i t i a l or fi n a l position. Figure 6.9 Interference Conditions. - 77 - 3. Any swept surface edge intersects any solid polyhedral surface. 4. The cylindrical volume of any link in its i n i t i a l or fin a l position intersects any polyhedral face. 5. Any polyhedral solid is wholly enclosed by any swept volume. The interference algorithm now consists of testing for each of these cases for every possible combination of solid polyhedra and swept volumes. A hierarchy of tests can be devised to determine i f intersection w i l l occur, as shown in Fig. 6.10. Each test w i l l now be considered in detail. 6.3.3.1 Condition 1 - Intersection of an Edge and a Parametric Surface (Fig. 6.11) If we are given 4 points in space, P_̂ , P_ , P^, P^, we can define a ruled, parametric surface with the following equation: r = P + A u + A v + A A (u - u ) , .... — —1 u —o v —o u v — 1 — o (6.135) where u = P_ - P, , —o —2 —1 »1 " *3 " *4 ' A , A are scalar parameters, u v Also, i f we are given the end points of a line segment, Ej, JJ2, we can define a line with the following equation: r = E. + A u _ 1 E _ E (6.136) where û , = - E_̂ Combining (6.135), (6.136) we get the following system of equations: - 78 - Any s o l i d polyhedral edge intersects any swept volume bounding parametric surface. Yes No Any s o l i d polyhedral edge in t e r s e c t s the l i n k cylinder at i t s i n i t i a l or f i n a l p o s i t i o n . Yes fr- No Any swept volume edge intersects any polyhedral face Yes i No Link cylinder i n t e r s e c t s any polyhedral face i n i n i t i a l or f i n a l p o s i t i o n Yes No Any polyhedron vertex l i e s inside the swept volume Yes •• t < No Intersection Intersection Figure 6.10 Algorithm for Interference Detection. - 7 9 - Figure 6.11 Intersection of an Edge and a Parametric Surface. - 80 - X u + X v - X u + X X (u -u ) + P - E = 0 u—o v—o E—E u v —1 —o —1 —1 ( 6 . 1 3 7 ) This system of non-linear equations can be solved i t e r a t i v e l y using Newton's method. The edge i n t e r s e c t s the surface within i t s boundaries i f 0<X <1 0<X <1 , 0<X <1 at the i n t e r s e c t i o n point, u ' v ' E ^ 6 . 3 . 3 . 2 Condition 2 - Intersection of an Edge and a Cylinder ( F i g . 6 . 1 2 ) Suppose the edge i s defined as for Condition 1 and i s represented by equation ( 6 . 1 3 6 ) . The cylinder i s defined by two points C^ and C J J , and a radius R. Ci and £2 define the ends of the cylinder and the l i n e joining C^ and C 2 defines the c e n t r a l axis or spine of the cy l i n d e r . The ends of the cylinder are f l a t and normal to the spine. The edge and the spine l i e on two skewed l i n e s i n space. We can fi n d a common normal and i n t e r s e c t i o n points J j . » 12 o n t n e edge and spine (see F i g . 6 . 1 2 ) . The common normal and distance between two skewed l i n e s can be found as follows. Suppose the two l i n e s have the following equations: Line 1 (edge) r = E, + A u — —1 E —E ( 6 . 1 3 8 ) where ^E = E - 2 Line 2 (spine) r = C, + A u ( 6 . 1 3 9 ) s —s where u We can show that the closest distance D i s given by - 81 - Figure 6.12 Intersection of an Edge and a Cylinder. - 82 - The points on each line lying on the common normal at the closest distance are found by substituting the following parameter values into equations (6.138), (6.139): X E = <£i " V * iig • (6.141) X s " (iX " £ x ) * iis • (6.142) Let us c a l l these points P„, P —E —s Pg lies on the edge i f 0 < XE < 1 . P lies on the spine i f 0 < X < 1 . —s s We can now define four different cases of an edge intersecting a cylinder. Each case is handled differently. Case 1 Pg lies on the edge, and Pg lies on the spine. In this case, intersection occurs i f the minimum distance D given by (6.140) is less than the radius R. Case 2 Fg lies on the edge, and Pfi does not l i e on the spine In this case, i f the edge intersects either end face of the cylinder i t Intersects the cylinder. This can be tested in several steps: 1. Test i f edge intersects face plane of cylinder. 2. If step 1 is true, find intersection point. 3. Find distance from spine to intersection point. 4. If distance is less than the radius, intersection has occurred. - 83 - Case 3. Pj; does not l i e on the edge, and P R l i e s on the spine. In this case, we find the minimum distance from each end point of the edge to the spine. If either distance is less than the radius R, intersection occurs. i.e. i f I E - P I < R 1 —1 —s 1 o r I I 2 " ̂  I < R intersection occurs. Case 4. Pg does not l i e on the edge, and Pg does not l i e on the spine. This case is treated the same as case 2. 6.3.3.3 Condition 3 - Intersection of an Edge and a Polyhedral Face (Fig.6.13) Let us define an edge by equation (6.136). A polyhedral face is defined as the planar region bounded by an n-sided polygon with n co- planar vertices, , V^, ... V^. The face is a region of a known plane with the following equation: r_ • n - Q = 0 (6.143) where n is the unit surface normal, Q is the closest distance to the origin. The edge is defined by equation (6.136), i.e. r = E, + X_ u_ — —1 E —E where u = E„ - E, . Figure 6.13 Intersection of an Edge and a Polyhedral Face. - 85 - Combining (6.136) and (6.143) we get the following equation for the parameter : Q - E • n X = _ 1 — . ^ • n If uj • ri = 0, the line i s parallel to the plane and no intersection exists. The edge intersects the face plane i f 0 < Ag <1. We can now formulate a series of tests to determine i f the edge intersects the face. 1. Test i f the edge intersects the face plane. 2. If (1) i s true, generate any ray originating from the intersection point P_£nt and lying on the face plane. 3. If the ray crosses the face boundaries an odd number of times, intersection has occurred. 6.3.3.4 Condition 4 - Intersection of a Cylinder and a Polyhedral Face It is possible for a cylinder to intersect a face without any of the face edges intersecting the cylinder. There are two possible intersection cases: 1. The cylinder spine intersects the face plane inside the face boundaries. 2. At least one cylinder end face intersects the face plane inside the face boundary (Fig.6.14). Tests can be devised for each of these cases. - 86 - Figure 6.14 Intersection of Cylinder End Face and Polyhedral Face. - 87 - Case 1 This test is the same as for intersection between an edge and a face, as previously described. Case 2 In this case, we w i l l find the shortest distance from the cylinder face center to the intersection line of the cylinder face plane and polyhedral face plane. If the distance is less than the radius R, intersection occurs. Let us define a vector R_' in the cylinder end face R* = u x (u x n) (6.145) ~~— S """" where U = C„ - C, , spine direction vectors, —s —2 —1 ii = polyhedral face surface normal. Let us now scale R' to length R, the radius R' R - R — . (6.146) We can now define two lines lying in the cylinder end faces, one in each end; £ x = C + A R , (6.147) and £ 2 = £ 2 + X - ' (6.148) We now test i f either line intersects the polyhedral face. Intersection occurs i f -1 < A < 1 and i f the intersection point PJ l i e s — i n t inside the polyhedral face boundaries. - 88 - 6.3.3.5 Condition 5 - Any Polyhedron is Totally Enclosed by a Swept Volume Bounded by Parametric Surfaces We can now write an equation for a parametric swept volume in the following form: r ( A , A , A ) = r ( A , A ) + A R n , 0 < A < 1 , (6.149) — U V w — 1 U V w — u 0 < A < 1 , v -1 < A < 1 , w where R , = P + A u + Av + A A (u, - u ) —1 — u—o v—o u v —1 —o and n = V x V —u — V V x V —u —v where V V are as defined i n equations (6.132), (6.153). In other —u —v words, is any point on the parent parametric surface and ri is the unit surface normal. Let n* = V x V (6.150) — — u — v This can be written as n* = L + AM + AN (6.151) — — u— v— We can rewrite (6.149) as r = r, + A n' , -a < A < oc , (6.152) — —1 w— w R where a = ,—p. Expanding and rearranging, r = A u + A V + A L + A A (n -u ) + A A M + A A N + P , (6.153) — u—o v—o w— u v —1 —o w u— w v— — - 89 - 0 < A < 1 , u 0 < A < 1 , v -a < A < a w The r e s u l t i s a system of non-linear equations which can be solved i t e r a t i v e l y using Newton's method. If a l l previous i n t e r s e c t i o n tests have been f a l s e , and i f any vertex of a polyhedron l i e s within the swept volume, then a l l v e r t i c e s and thus the entire polyhedron l i e s within the volume. It i s s u f f i c i e n t to test any single a r b i t r a r y vertex of the polyhedron. The method i s as follows: 1. Substitute any vertex for r and solve for A , A , A . — u v w 2. Find the surface normal _n' and i t s length |n'| . 3. The vertex i s inside the volume i f 0 < A < 1 , and u 0 < A < 1 , and v \f\ < w < i j h • 6.4 Interference Detection Demonstration Program TESTIN The t e s t s and a l g o r i t h m s d e s c r i b e d above were combined and implemented i n a simple test program, TESTIN. TESTIN tests a single polyhedral face against a single swept volume. TESTIN can be expanded e a s i l y f o r the more general case of several many-sided polyhedra i n t e r s e c t i n g a number of swept volumes. - 90 - 6.4.1 Functional Description of TESTIN TESTIN follows the logic shown in Fig.6.11. TESTIN translates and rotates a single cylinder from an i n i t i a l to a fin a l position and generates a swept volume. The user specifies the cylinder length and radius and the i n i t i a l and fin a l positions as specified by two homogeneous transformations. The user inputs the number of vertices on the test face, and their coordinates. TESTIN then tests for intersections of the face and the swept volume. 6.4.2 List and Description of a l l Subroutines used by TESTIN This section contains a complete l i s t and descriptions of a l l subroutines used by the interference detection test program TESTIN. A l l subroutine links are given. Tektronix IGL graphics subroutines are noted with asterisks (*). - 91 - Subroutines used in TESTIN: CORNER DET2X2 DOTPRD DRVOL EDGINT FACINT INITSV INVOL NEWTON OPEN SHUT SURFIN TESTIN VCROSS XPROD CYLEND DET3X3 DRCYL ECROSS ELIST FACYL INPROJ LINK NWTVOL PLANE SOL3X3 SWPVOL TRANS3 VLEN XPROD2 CYLINT DIFF DRPINT EDGCRS FACE GENVOL INSIDE MATDET ONRAY ROTK SOLVE TCROSS UNIT VRTCRS Subroutines DOTPRD, OPEN, SHUT, UNIT, XPROD are common to the programs AUTOP and TESTIN. Main Program TESTIN TESTIN: main program for testing interference detection algorithm CALLS-CYLINT,DIFF,DRCYL,FACE,FACINT,FACYL,INVOL,SURFIN,SWPVOL,UNIT, CMCLOS*,CMOPEN*,EYEBAL*,GRSTOP*,GRSTRT*,VRP3D*,WIND3D*,ZPERSP* Subroutines CORNER: calculates the corner points of a parametric swept volume CALLS- NONE CALLED BY- SWPVOL - 92 - CYLEND: tests i f an edge intersects either end face of a cylinder CALLS- DIFF, DOTPRD, VLEN CALLED BY- CYLINT CYLINT: tests i f an edge intersects a cylinder CALLS- CYLEND,DIFF,DOTPRD,SOLVE,UNIT,VLEN,XPROD2 CALLED FROM- TESTIN DIFF: finds the difference of two vectors CALLS- NONE CALLED FROM-CYLEND,CYLINT,FACYL,TESTIN DET2X2: finds the determinant of a 2X2 matrix CALLS- NONE CALLED FROM- EDGINT DOTPRD: finds the dot product of two vectors CALLS- NONE CALLED FROM- CYLINT,FACE,FACYL,PLANE,TCROSS DRCYL: draws the end cylinders as polyhedral approximations CALLS- ROTK,TRANS 3,UNIT,VLEN,XPROD,CMCLOS *,CMOPEN*,DRAW3D*,MOVE 3D * CALLED FROM- TESTIN DRPINT: draws an intersection point as a small cross CALLS- CMCLOS*,CMOPEN*,DRAW3D*,MOVE3D* CALLED FROM- FACINT, FACYL DRVOL: draws the swept volume CALLS- CMCLOS*,CMOPEN*,DRAW3D*.MOVE3D* CALLED FROM- SWPVOL - 93 - ECROSS: counts the number of times a ray crosses face boundaries CALLS- EDGINT CALLED FROM- INSIDE EDGINT: tests for intersection between an edge and a test ray CALLS- DET2X2 CALLED FROM- ECROSS ELIST: generates a l i s t of edges which have neither endpoint on the test ray CALLS- NONE CALLED FROM- INSIDE FACE: allows the user to interactively specify a test face CALLS- DOTPRD,XPROD,CMCLOS*,CMOPEN*,DRAW3D*,MOVE3D* CALLED FROM- TESTIN FACINT: tests for intersection between an edge and a face CALLS- DRPINT,INSIDE,PLANE CALLED FROM- TESTIN FACYL: tests for intersection between a cylinder end face and a polyhedral face CALLS- DIFF,DOTPRD,DRPINT,FACINT,XPROD CALLED FROM- TESTIN GENVOL: generates the parametric volume parameters CALLS- NONE CALLED FROM- SWPVOL INITSV: i n i t i a l i z e s the manipulator link parameters CALLS- OPEN,SHUT CALLED FROM- SWPVOL - 94 - INSIDE: tests i f an intersection point lies within the boundaries of a face CALLS- ECROSS,ELIST,VCROSS CALLED FROM- FACINT INVOL: tests i f a point lies inside a parametric swept volume CALLS- NWTVOL,VLEN CALLED FROM- TESTIN LINK: calculates the i n i t i a l and fina l positions of the link CALLS- TRANS3 CALLED FROM- SWPVOL MATDET: finds the determinant of a 3X3 matrix CALLS- NONE CALLED FROM- S0L3X3 NEWTON: finds the intersection of a line and a swept surface by solving a system of non-linear equations iteratively using Newton's method CALLS- SOL3X3,VLEN CALLED FROM- SURFIN NWTVOL: solves a system of non-linear equations iteratively using Newton's method to find i f a point l i e s inside a parametric volume CALLS- SOL3X3,VLEN CALLED FROM- INVOL ONRAY: tests i f a point l i e s on a ray CALLS- NONE CALLED FROM- VCROSS - 95 - PLANE: tests i f a line segment intersects a plane CALLS- DOTPRD CALLED FROM- FACINT ROTK: finds a rotation matrix to rotate about some given direction vector CALLS- NONE CALLED FROM- DRCYL SHUT: closes a FORTRAN f i l e CALLS- NONE CALLED FROM- INITSV SOL3X3: finds the solution of a 3X3 system of linear equations CALLS- MATDET CALLED FROM- NEWTON,NWTVOL SOLVE: finds the common normal of two skewed lines CALLS- NONE CALLED FROM- CYLINT SURFIN: finds the intersection of a line and a parametric surface CALLS- NEWTON,CMCLOS*,CMOPEN*,DRAW3D*,M0VE3D* CALLED FROM- TESTIN SWPVOL: generates the volume swept out by a cylindrical link moving from an i n i t i a l to a fina l position CALLS- CORNER,DRVOL,GENVOL,INITSV,LINK CALLED FROM- TESTIN TRANS3: performs a 3-D translation CALLS- NONE CALLED FROM- LINK - 96 - TCROSS: tests i f a ray crosses the face boundary at a vertex point which lies on the ray CALLS- DOTPRD, XPROD CALLED FROM- VCROSS UNIT: converts a vector to unit length CALLS- NONE CALLED FROM- CYLINT,DRCYL,TESTIN VCROSS: counts the number of times a test ray crosses the face boundary at vertices CALLS- ONRAY,TCROSS CALLED FROM- INSIDE VLEN: finds the length of a vector CALLS- NONE CALLED FROM- CYLEND,CYLINT,DRCYL,NEWTON,NWTVOL XPROD: finds the normalized cross product of two vectors CALLS- NONE CALLED FROM- DRCYL,FACE,FACYL,TCROSS XPR0D2: finds the cross product of two vectors CALLS- NONE CALLED FROM- CYLINT 6.4.3 Demonstration of TESTIN The interference detection program TESTIN was tested by defining a swept volume generated from an i n i t i a l and a fi n a l position of a cylindrical link, and then simulating situations known to result in interference. The cylindrical link was defined to have a length of 30 - 97 - units and a radius of 5 units. The i n i t i a l cylinder spine position lay along the X-axis, starting at the origin. The link f i n a l position was defined by a translation of 30 units along the Y-axis, and a rotation of 45° about the Y-axis. The resulting wire-frame depiction of the resulting swept volume is shown in Figs. 6.15, 6.16, 6.17. Fig.6.15 shows the intersection of the swept volume with a triangular face defined by the vertices (10, 15, 15), (10, 30, 15), (10, 10, -20). In this case at least one edge of the triangle intersects at least one parametric surface of the swept volume. TESTIN terminates as soon as an intersection is found, so other intersections may exist as well. Fig. 6.16 shows the swept volume and a rectangular region defined by vertices (40, 10, 20), (40, 10, -20), (-20, 10, -20), (-20, 10, 20). In this case, an edge of a parametric surface intersects the rectangular region. Fig. 6.17 shows a rectangular region defined by vertices (40, -2, 20), (40, -2, -20), (-20, -2, -20), (-20, -2, 20). In this case, the cylinder intersects the rectangular region. Fig. 6.18 shows the swept volume resulting from a pure translation with no rotation. An edge of the rectangular region intersects a para- metric surface which in this case is planar. A l l five intersection cases were tested and verified with numerous examples. The graphical depiction was not very clear for most of them, however, so they have not been reproduced here. During the running of TESTIN, interference messages appear in the text f i e l d of the 4027 screen. Unfortunately, these messages are not reproduced by a graphics screen dump for a hard copy record. - 98 - Figure 6.15 Intersection Condition 1 between Face and Swept Volume - 99 - Figure 6.16 Intersection Condition 3 between Face and Swept Volume. - 100 - Figure 6.17 Intersection Condition 2 between Face and Swept Volume. - 101 - Figure 6.18 Intersection Condition 1 between Face and Swept Volume Generated by Pure Tra n s l a t i o n . - 102 - 6.5 Possible Improvements to the Algorithm We can e a s i l y f i n d the smallest bounding boxes containing each of the s o l i d polyhedra and swept volumes. I f we choose boxes with faces which are normal to the world coordinate axes, i t i s very simple to test i f any bounding boxes i n t e r s e c t . Further tests need only be conducted on the polyhedra and swept volumes whose bounding boxes i n t e r s e c t . - 103 - 7. SOFTWARE DESIGN AND DEVELOPMENT Many of the concepts and much of the theory presented thus far have been successfully incorporated into a substantial FORTRAN software package called AUTOP, for automatically generating a robot program for welding. The software package was developed on a VAX 11-750 computer running under the VMS operating system. The Tektronix Interactive Graphics Library (IGL) software package provided the graphics routines. A DEC VT-101 terminal fitted with a Retrographics DQ-650 enhancement was used, allowing the VT-101 to emulate a Tektronix 4027 graphics terminal with a simple command while s t i l l allowing f u l l use of the DEC visual editor for program development. The software package, AUTOP, currently requires about 100 K of memory, and this w i l l increase as AUTOP is expanded. 7.1 Design Approach Since the software package was to be quite extensive i t was essential to carefully formulate some design guidelines and objectives i n i t i a l l y , and to develop the software in a planned, consistent and structured manner, following good programming practice. The software was developed with the following goals in mind. a) Portability The software was designed to be as system independent as possible. System dependent sections of code have been clearly identified, where possible. No system libraries are called, and no external subroutines are - 104 - c a l l e d , other than the Tektronics IGL graphics subroutines. Standard FORTRAN i s followed as cl o s e l y as possible. b) Modularity The software was designed with a modular, h i e r a r c h i c a l structure. Each functional module has e x p l i c i t l y defined inputs and outputs. Data i s passed v i a argument l i s t s , common data blocks and f i l e s , depending on the requirements. c) A d a p t a b i l i t y and Generality The software was designed to be e a s i l y configured to a wide range of possible workstation implementations. Wherever possible, the software modules are as general as possible, with s p e c i f i c implementation parameters being read from workstation d e s c r i p t i o n f i l e s . For example, the geometry and dimensions of the robot and positioning table can be e a s i l y changed without changes to the software. 7.2 Features and C a p a b i l i t i e s of AUTOP The s p e c i f i c features and c a p a b i l i t i e s of AUTOP at i t s current state of development are summarized below: a) AUTOP input and output data are stored i n f i l e s . b) I n i t i a l system configuration parameters are stored i n and read from f i l e s . This allows easy modification of parameters. c) Interactive graphics i s used for communications with the user. d) AUTOP generates a welding procedure i n t e r a c t i v e l y with the user. - 105 - e) Automatic iterative searching for kinematically feasible torch paths is incorporated. f) An Expert Welder module automatically selects welding parameter values. This module was developed separately and is mentioned here for completeness. For details, refer to Buchal et al [1984]. 7.3 General Functional Description of AUTOP It is not practical to describe completely a l l of the functional details of AUTOP. The software listings are extensively documented so such a description would be largely redundant. A simplified functional description, however, is useful in understanding the basic operation of AUTOP. The following description outlines the major functions in approximate order. No attempt w i l l be made to rigorously follow the actual flow of control of AUTOP. The simplified software structure is shown in Fig.7.1. AUTOP is designed to interface with a separate CAD system which w i l l generate the CAD data used by AUTOP. No CAD system is currently used, so test CAD f i l e s are generated both manually and by specific user-written programs. The data formats which we have specified are closely followed. The CAD f i l e s contain workpiece weld parameters and digitized seam paths, as well as the workpiece geometry. AUTOP converts the CAD data into processed seam data f i l e s containing calculated seam tangents and normal vectors at each point. - 106 - CAD Data workstation configuration data Expert Welder data base processed workpiece seam data table p o s i t i o n and fixed welder settings for each seam Process workpiece data including welding data robot coordinates and variable welder settings for each seam I n t e r a c t i v e l y specify welding procedure, including table positions, seam order and weld d i r e c t i o n s , subject to tests of Kinematic f e a s i b i l i t y Figure 7.1 Software Structure and Data Flow of AUTOP - 107 - Workstation and graphics enviroment data are read from f i l e s and the parameters are i n i t i a l i z e d . The actual f i l e names for a l l data f i l e s are easily changed. AUTOP reads the current welder setup, scans a l l the seam data f i l e s and determines i f the current setup is appropriate. If not, the user is instructed to reset the welder to the settings determined by the Expert Welder module. AUTOP draws the workpiece and seams on the screen. The user can change the table position and the eye position. The user interactively 'picks' the seams to be welded. AUTOP then automatically searches for a kinematically feasible robot path to follow the seam. If a path is found, the table position and feasible robot configuration are recorded. A robot coordinate l i s t is generated for the seam and stored in a data f i l e . Also, the Expert Welder module is called to determine appropriate welding parameters such as voltage, current, speed, etc. for the seam. In the present implementation, the current and speed can vary along the seam. The voltage, gas flow rate, wire feed rate and torch orientation are fixed for each seam. Each seam which has been successfully picked and processed is f i l l e d in graphically on the screen. The operator continues moving the workpiece and picking seams until a l l the seams have been processed. The basic operation of AUTOP i s summarized in Fig. 7.2. 7.3.1 List and Description of a l l Subroutines called from AUTOP This section l i s t s and briefly describes the function of a l l - 108 - START 4» Draw workpiece and seams• Draw welded seams. Yes Pick next seam to weld. Test a l l configurations u n t i l f e a s i b l e path i s found. Rotate tool about axis to search for f e a s i b l e s o l u t i o n . no Save seam number, table p o s i t i o n , robot configuration i n a procedure record f i l e . Calculate and save robot coordinates and weld parameters i n a f i l e . Yes End of procedure s p e c i f i c a t i o n Figure 7.2 Si m p l i f i e d Flowchart of AUTOP. - 109 - subroutines c a l l e d i n the software package AUTOP. The subroutines are separated into general functional categories and a l l l i n k s to other subroutines are l i s t e d . A l l Tektronix 'IGL' graphics subroutines are noted with a s t e r i s k s ( * ) . Subroutines used i n 'AUTOP': ARCLEN CADFIL CALCS CHECK CONFIG DOTPRD DRAXES DRPANL DRSEAM DRWELD EYELOC EXPERT HMULT HMULTS INIT JOINTS LIMITS LINDIS MATCNV MATINV N0AP1 N0AP5 NORM OPEN PROCES RDFILE ROTAT SCENE SETUP SHUT TABPOS TABSET TANGEN TOLROT TRANS UNIT WELSET WIRE1 WTRE2 WIRE 3 W0RP0S XPROD Main Program AUTOP AUTOP: Main Program CALLS- CADFIL,CHECK,CONFIG,DRWELD.EXPERT,EYELOC,INIT,OPEN, RDFILE,SCENE,SHUT,WELSET,CMCLOS*,CMOPEN*GETPIK*,GRSTOP*, GRSTRT*, VUP3D*,VWPT3D*,WIND3D* Mathematical Operations DOTPRD: finds the dot product of two vectors CALLS- NONE CALLED FROM- DRPANL - 110 - HMULT: m u l t i p l i e s two homogeneous t r a n s f o r m a t i o n s i n double pr e c i s i o n CALLS- NONE CALLED FROM- TABPOS.WORPOS HMULTS: l i k e HMULT, i n single p r e c i s i o n CALLS- NONE CALLED FROM- WORPOS MATCNV: converts double p r e c i s i o n homogeneous transformation into s i n g l e p r e c i s i o n and vice versa CALLS- NONE CALLED FROM- WORPOS MATINV: inverts a homogeneous transformation matrix CALLS- NONE CALLED FROM- INIT ROTAT: performs r o t a t i o n a l part of a homogeneous transformation on a point CALLS- NONE CALLED FROM- DRPANL,DRSEAM TRANS: performs a homogeneous transformation on a point CALLS- NONE CALLED FROM- DRPANL,DRSEAM,DRWELD UNIT: normalizes a vector to unit length CALLS- NONE CALLED FROM- DRSEAM,DRWELD - I l l - XPROD: performs a vector cross product CALLS- NONE CALLED FROM- DRSEAM,DRWELD,LINDIS,NOAP5,PROCES,WORPOS Graphical Output DRAXES: draws the world axes CALLS- MOVE3D*,DRAW3D* CALLED FROM- SCENE DRPANL: draws the workpiece CALLS- DOTPRD,LINDIS.ROTAT,TRANS,APPEAR*,DRAW3D*,MOVE3D*, PANL3D*,POLY3D*,REMOVE* CALLED FROM- SCENE DRSEAM: draws the seams CALLS- NORM,OPEN,RDFILE,ROTAT,SHUT,TRANS,UNIT,XPROD, APPEAR*, CLOSEG*,DASHPT*,DRAW3D*,MOVE3D*.OPNSEG*.REMOVE*,SETDET* CALLED FROM- SCENE DRWELD: draws the welds CALLS- NORM,OPEN,RDFILE,SHUT,TRANS,UNIT,XPROD,APPEAR*,CLOSEG*, DRAW3D*,MOVE3D*,OPNSEG*,REMOVE*,SETDET*,SETHIL* CALLED FROM- SCENE EYELOC: chooses eye pos i t i o n through user input CALLS- NONE CALLED FROM- MAIN - 112 - SCENE: draws complete graphical scene CALLS- DRAXES,DRPANL,DRSEAM,DRWELD,TABPOS,TABSET,CMCLOS*, CMOPEN*,DELSEG*.EYEBAL*, NEWPAG*,VRP3D* CALLED FROM- MAIN Kinematics ARCLEN: finds the arc length parameter 's' for a point on a seam tra j e c t o r y CALLS- NONE CALLED FROM- PROCES CONFIG: changes robot configuration u n t i l a f e a s i b l e trajectory i s found CALLS- WORPOS CALLED FROM- MAIN JOINTS: calculates the robot j o i n t angles f o r a given configuration and t o o l p o s i t i o n CALLS- NONE CALLED FROM- WORPOS LIMITS: checks the robot j o i n t angles against t h e i r physical l i m i t s CALLS- NONE CALLED FROM- WORPOS N0AP1: finds the PUMA 560 lo c a t i o n parameters given the desired t o o l l o c a t i o n and orie n t a t i o n CALLS- NONE CALLED FROM- WORPOS - 113 - N0AP5: finds the 5 degree of freedom PUMA lo c a t i o n parameters given the desired tool l o c a t i o n and the or i e n t a t i o n of i t s ce n t r a l axis CALLS- XPROD CALLED FROM- WORPOS NORM: finds the seam normal vector CALLS- NONE CALLED FROM- DRSEAM,DRWELD,PROCES TABPOS: calculates the table frame r e l a t i v e to the world frame given the table j o i n t positions CALLS- OPEN,SHUT,HMULT CALLED FROM- SCENE TABSET: allows the user to i n t e r a c t i v e l y s p e c i f y new table j o i n t positions CALLS- OPEN,SHUT CALLED FROM- SCENE TANGEN: calculates the seam tangent vector at each point along a seam CALLS- NONE CALLED FROM- PROCES TOLROT: performs a rot a t i o n about the too l axis CALLS- NONE CALLED FROM- WORPOS WORPOS: generates welding parameters, robot l o c a t i o n data for a weld CALLS- JOINTS,HMULT,HMULTS.LIMITS,MATCNV,NOAP1,NOAP 5, OPEN,RDFILE,SHUT,TOLROT,XPROD CALLED FROM- CONFIG - 114 - Expert Welder CALC: calculates the welding current correction factors as a function of the gravity vector CALLS- TRANS CALLED FROM- EXPERT EXPERT: finds correct welding parameter values given the i n i t i a l welder setup and workpiece parameters CALLS- CALCS,OPEN,SHUT,WIRE1,WIRE2,WIRE3 CALLED FROM- AUTOP WELSET: checks i f i n i t i a l welder setup is appropriate for a given job and recommends changes i f necessary CALLS- OPEN,RDFILE,SHUT CALLED FROM- AUTOP WIRE calculates welding parameters for .030 in. wire CALLS- NONE CALLED FROM- EXPERT WIRE2 calculates welding parameters for .035 i n . wire. CALLS- NONE CALLED FROM- EXPERT WIRE3 calculates welding parameters for .045 in. wire CALLS- NONE CALLED FROM- EXPERT - 115 - Data Management CADFIL: converts object geometry f i l e s into i n t e r n a l arrays for fa s t e r processing CALLS- OPEN,RDFILE,SHUT CALLED FROM- MAIN INIT: i n i t i a l i z e s global constants CALLS- MATINV,OPEN,RDFILE,SHUT CALLED FROM- MAIN OPEN: opens a FORTRAN f i l e CALLS- NONE CALLED FROM- MAIN,CADFIL,DRSEAM,DRWELD,INIT,PROCESS,TABPOS, TABSET,WORPOS RDFILE: reads a given record from a f i l e CALLS- NONE CALLED FROM- MAIN,CADFIL,DRSEAM,DRWELD,INIT,WORPOS SHUT: closes a FORTRAN f i l e CALLS- NONE CALLED FROM- MAIN,CADFIL,DRSEAM,DRWELD,INIT,PROCES, TABPOS,TABSET,WORPOS Other CHECK: checks i f a picked seam has been picked previously CALLS- NONE CALLED FROM- MAIN - 116 - o LINDIS: finds which of two l i n e s i s further away as viewed from the current eye lo c a t i o n CALLS- XPROD CALLED FROM- DRPANL PROCES: processes the i n i t i a l CAD seam data f i l e s into a more useful form. CALLS- ARCLEN,NORM,OPEN,SHUT,TANGEN,XPROD CALLED FROM- SETUP SETUP: manipulates f i l e s for processing by PROCES CALLS- OPEN,PROCES,RDFILE,SHUT CALLED FROM- AUTOP Functions C a l l e d LENGTH finds the length of a character s t r i n g CALLED FROM- RDFILE 7.4 Data Structures AUTOP makes extensive use of external f i l e s f o r data storage and interchange. System i n i t i a l i z a t i o n parameters are stored i n f i l e s , where they can be e a s i l y changed by AUTOP or by the user through the e d i t o r . CAD seam data i s also stored i n f i l e s , i n a linked l i s t structure with each seam having a separate f i l e . Output parameters and robot l o c a t i o n data are stored i n f i l e s as w e l l . The use of i n t e r n a l arrays for temporary data storage i s minimized because the data f i l e s are large, and of variable length. FORTRAN requires a l l arrays to have a fixed s i z e , defined at compile time. Arrays - 117 - must be dimensioned for the largest conceivable requirement, which wastes memory. Instead, f i l e records are read d i r e c t l y and processed immediately whenever possible. The penalty of slower data access i s o f f s e t by increased f l e x i b i l i t y , s i m p l i f i e d data structures and reduced memory requirements. The largest f i l e s , the CAD and processed seam data f i l e s , are stored i n unformated form f o r a substantial reduction i n required disk space. The small system parameter and setup f i l e s are stored i n l i s t - d i r e c t e d format for ease of e d i t i n g and debugging. The robot l o c a t i o n data f i l e s are also currently i n l i s t - d i r e c t e d format for debugging and ease of v e r i - f i c a t i o n . When the automatic program generation and downloading module i s completed, these f i l e s should be changed to unformatted form. 7.4.1 Description of a l l I/O F i l e s and t h e i r Formats The following i s a complete l i s t and de s c r i p t i o n of a l l the data f i l e s accessed by the program AUTOP. The DEC f i l e naming convention used by the VAX computer requires a l l f i l e names to have the extension .DAT ( i . e . NAME.DAT). FILIST: L i s t of f i l e names used i n a p a r t i c u l a r implementation. CADLST: L i s t of names of CAD data f i l e s corresponding to each seam (i.e.CADDAT). CADDAT: F i l e containing CAD data for a p a r t i c u l a r seam. WPCLST: L i s t of names of processed CAD data f i l e s corresponding to each seam ( i . e . WPCDAT). WPCDAT: F i l e containing processed CAD data for a seam. ROBLST: L i s t of names of robot seam lo c a t i o n data f i l e s ( i . e . ROBDAT). - 118 - ROBDAT: F i l e containing robot locations and welding data for a seam path. OBJLST: L i s t of names of f i l e s containing geometric de s c r i p t i o n of objects i n workstation. OBJDAT: F i l e containing polyhedral geometric s p e c i f i c a t i o n of an object. WKSTAT: F i l e containing complete s p e c i f i c a t i o n of robot and welder. TABLE: F i l e containing s p e c i f i c a t i o n of worktable. GRAPH: F i l e containing parameters for graphics system. LOCSET: F i l e containing workstation setup for each seam to be welded. STOVAR: Storage f i l e for current table p o s i t i o n . WPARAM: Temporary weld parameter storage f i l e . The contents and formats of each of these data f i l e s follow. FILE NAME LISTS FILIST.DAT: assigns names to a l l the f i l e s accessed by AUTOP TABLE 7.1 - F i l e Name L i s t FILIST RECORD NO. 1 2 3 4 5 6 7 8 9 10 CONTENTS CADLST.DAT WPCLST.DAT WKSTAT.DAT TABLE.DAT STOVAR.DAT GRAPH.DAT OBJLST.DAT R0BLST.DAT LOCSET.DAT WPARAM.DAT - 119 - CADLST: Contains a l i s t of the CAD f i l e s describing each weld seam. TABLE 7.2 - CAD File List CADLST RECORD NO. CONTENTS DESCRIPTION 1 *TEXT TEXT HEADING 2 5 NUMBER OF SEAMS 3 CADI.DAT SEAM 1 CAD FILE 4 CAD2.DAT SEAM 2 CAD FILE 5 CAD3.DAT SEAM 3 CAD FILE 6 CAD4.DAT SEAM 4 CAD FILE 7 CAD5.DAT SEAM 5 CAD FILE WPCLST: Contains a l i s t of processed seam f i l e s . TABLE 7.3 - Processed Seam File List WPCLST RECORD NO. CONTENTS DESCRIPTION 1 *TEXT TEXT HEADING 2 5 NUMBER OF SEAMS 3 WPC1.DAT PROCESSED SEAM 1 DATA FILE 4 WPC2.DAT PROCESSED SEAM 2 DATA FILE 5 WPC3.DAT PROCESSED SEAM 3 DATA FILE 6 WPC4.DAT PROCESSED SEAM 4 DATA FILE 7 WPC5.DAT PROCESSED SEAM 5 DATA FILE ROBLST: Contains a l i s t of robot location f i l e s for each seam TABLE 7.4 - Robot Location Data F i l e List ROBLST RECORD NO. CONTENTS DESCRIPTION 1 *ROBOT XYZOAT COORDS. TEXT HEADING 2 PUMA1.DAT SEAM 1 ROBOT DATA FILE 3 PUMA2.DAT SEAM 2 ROBOT DATA FILE 4 PUMA3.DAT SEAM 3 ROBOT DATA FILE 5 PUMA4.DAT SEAM 4 ROBOT DATA FILE 6 PUMA5.DAT SEAM 5 ROBOT DATA FILE - 120 - OBJLST: Contains a l i s t of geometric CAD f i l e s for objects in the world model. TABLE 7.5 - Object Geometry Data File List OBJLST RECORD NO. CONTENTS DESCRIPTION 1 CYLIND.DAT GEOMETRY OF WORKPIECE 2 WKTAB.DAT GEOMETRY OF WORK TABLE 3 TOOL.DAT GEOMETRY OF ROBOT TOOL 4 F0RARM.DAT GEOMETRY OF ROBOT FOREARM 5 UPARM.DAT GEOMETRY OF ROBOT UPPER ARM SEAM DATA FILES TABLE 7.6 - Seam CAD F i l e RECORD NO. CONTENTS DESCRIPTION 1 JTYPE,XSECT,TI,T2,NPOINT,ICURVE WELDING PARAMETERS 2 INDEX,R(3),N1(3),N2(3) TRAJECTORY DATA NPOINT+1 INDEX,R(3),N1(3),N2(3) TRAJECTORY DATA JTYPE: XSECT: T1,T2: NPOINT: ICURVE: INDEX: R(3): N1(3),N2(3): joint type code weld cross sectional area parent metal thicknesses number of points in f i l e trajectory type specifier 0: straight line 1: space curve location index position vector of current location adjacent surface normal vectors - 121 - TABLE 7.7 - Processed Seam Data F i l e RECORD NO. CONTENTS DESCRIPTION 1 JTYPE,XSECT,T1,T2,NPOINT,ICURVE SEAM PARAMETERS 2 INDEX,S,R(3),T(3),A(3),B(3) SEAM DATA NPOINT+1 INDEX,S,R(3),T(3),A(3),B(3) SEAM DATA JTYPE,XSECT,T1.T2,NPOINT,ICURVE: as defined for CAD f i l e INDEX: l o c a t i o n index S: arc length to current p o s i t i o n along t r a j e c t o r y R(3): p o s i t i o n vector of current p o s i t i o n T(3): unit vector tangent to seam t r a j e c t o r y at current p o s i t i o n A(3): unit vector normal to weld surface B(3): B=A X T (to give orthogonal set of basis vectors) RECORD NO. TABLE 7.8 - Robot Location Data F i l e CONTENTS DESCRIPTION ALPHA,BETA,STKOUT,VOLT,FEED,GFR, NPOINT INDEX,X.Y.Z.O,A,T,CURRENT,SPEED NPOINT+1 INDEX,X,Y,Z,0,A,T,CURRENT,SPEED ALPHA: torch lead angle BETA: torch l a t e r a l angle STKOUT: torch stickout distance VOLT: welding voltage FEED: wire feed rate GFR: shielding gas flow rate NPOINT: number of locations i n f i l e INDEX: loc a t i o n index X,Y,Z: robot tool l o c a t i o n ( t r a n s l a t i o n s ) 0,A,T: robot t o o l o r i e n t a t i o n (rotations) CURRENT: welding current SPEED: welding speed SEAM GLOBAL PARAMETERS SEAM LOCAL PARAMETERS - 122 - OBJECT GEOMETRY DATA FILES OBJDAT: F i l e containing polyhedral model of an object i n the work s t a t i o n TABLE 7 . 9 - Object Geometry Data F i l e s RECORD NO. 2 NVERT1+1 CONTENTS ID1,N0RM1,DIST1,NVERT1 VERTNO,VERT(3) I:=NVERTl+2 ID2,NORM2,DIST2,NVERT2 1+1...I+NVERT2+1 VERTNO,VERT(3) DESCRIPTION FACE1 HEADER VERTEX INDEX,COORDINATES FACE2 HEADER VERTEX INDEX, COORDINATE etc. for a l l faces i n the object End of f i l e i s indicated by a face i d = 0 ID: face ID number NORM: face outward surface normal DIST: normal distance from face plane to coordinate frame o r i g i n NVERT: number of v e r t i c e s i n the face VERTNO: vertex ID number VERT(3): vertex coordinates - 123 - SYSTEM CONFIGURATION FILES WKSTAT: contains robot and welder specifications TABLE 7.10 - System Configuration Fi l e WKSTAT NO. OF RECORDS 1 1 1 1 NRDOF 1 1 1 4 1 4 1 1 1 1 1 CONTENTS *TEXT HEADER NRDOF *TEXT HEADER *TEXT HEADER LINKNO,ALPHA,LEN, DIST,MIN,MAX *TEXT IC0NF(6) TEXT HOO(4,4) *TEXT H6G(4,4) *TEXT IMAX,DELTA *TEXT *TEXT WSIZE DESCRIPTION NUMBER OF ROBOT DEGREES OF FREEDOM LINK PARAMETERS CURRENT ROBOT CONFIGURATION ROBOT LOCATION REL. TO WORLD TOOL FRAME REL. TO LAST LINK TOOL ROTATION PARAMETERS CURRENTLY INSTALLED WELDER WIRE SIZE LINKNO: link number ALPHA: link twist angle LEN: link length DIST: joint displacement along i t s axis MIN,MAX: joint limits ICONF: current robot default configuration HOO: homogeneous transformation relating zeroth robot frame to world frame H6G: homog. trans, relating tool frame to last link frame IMAX: maximum number of rotation steps about tool axis (when searching for a feasible path) DELTA: size of incremental angular rotations about tool axis WSIZE: current welder wire size NOTE: The Expert Welder currently assumes a l l other welding parameter settings are programmable. - 124 - TABLE 7.11 - Example of File WKSTAT CONTENTS RECORD NO. *NUMBER OF DEGREES OF FREEDOM, 'NRDOF' 1 6 2 *ROBOT LINK PARAMETERS 3 LINK ALPHA LENGTH DIST. MIN. MAX. 4 1 -90. 0. 0. -160. 160. 5 2 0. 432. 149.5 137. 43. 6 3 90. -20.5 0. -52. -128. 7 4 -90. 0. 433. -110. 170. 8 5 90. 0. 0. -100. 100. 9 6 0. 0. 56.5 -266. 266. 10 *INITIAL CONFIGURATION ARRAY ICONF 11 0 0 1 1 0 0 12 *WORLD COORDINATE TO ROBOT COORDINATE TRANSFORMATION MATRIX 13 'HOO' 1. 0. 0. 0. 14 0. 1. 0. 0. 15 0. 0. 1. 0. 16 0. 0. 0. 1. 17 *GRIPPER FLANGE TO TOOL COORDINATE TRANSFORMATION MATRIX'H6G* 18 0. 0. 1. 120. 19 0. 1. 0. 0. 20 -1 0. 0. 92. 21 0. 0. 0. 1. 22 *T00L ROTATION PARAMETERS: MAX.No.OF STEPS, ANGLE STEP SIZE (DEGREES) 23 18 10 24 *WELDER SETUP 25 *WIRE SIZE 26 0.030 - 125 - POSITIONING TABLE SPECIFICATION FILE TABLE: contains geometric and kinematic specifications of positioning table TABLE 7.12 - Positioning Table Specification File TABLE NO. OF RECORDS 1 1 NDOF 1 4 1 4 1 4 CONTENTS NDOF *TEXT ID,ITYPE,STEP,RMIN, RMAX,THETA,ALPHA DISP.LEN *TEXT HTW(4,4) *TEXT HPRE(4,4) *TEXT HP0ST(4,4) DESCRIPTION NUMBER OF TABLE DEGREES OF FREEDOM TEXT HEADER TABLE LINK PARAMETERS WORKPIECE RELATIVE TO TABLE FRAME FIRST LINK RELATIVE TO WORLD TABLE FRAME RELATIVE TO LAST LINK NDOF: number of table degrees of freedom ID: joint type l=revolute (rotational) 2=prismatic (translational) ITYPE: joint motion type l=discrete positions 2=continuous positions STEP: joint motion step size for itype=l RMIN,RMAX:joint limits THETA: i n i t i a l joint angle ALPHA: link twist angle DISP: i n i t i a l joint displacement along i t s axis LEN: link length HTW(4,4): homog. transformation matrix relating workpiece to table HPRE(4,4): homog. transformation matrix relating f i r s t link to world HPOST(4,4):homog. transformation relating table frame to last link - 126 - TABLE 7.13 - Example of File TABLE CONTENTS RECORD NO. 3 1 ID ITYPE STEP RMIN RMAX THETA ALPHA DISP LEN 2 2 2 0. -1000. 1000. -90. -90. 0. 0. 3 2 2 0. -1000. 1000. -90. 90. 0. 0. 4 1 2 0. -1000. 1000. 90. 90. 0. 0. 5 *HTW 6 1. 0. 0. 0. 7 0. 1. 0. 0. 8 0. 0. 1. 0. 9 0. 0. 0. 1. 10 *HPRE 11 1. 0. 0. 0. 12 0. 1. 0. 0. 13 0. 0. 1. 0. 14 0. 0. 0. 1. 15 *HP0ST 16 1. 0. 0. 0. 17 0. 1. 0. 0. 18 0. 0. 1. 0. 19 0. 0. 0. 1. 20 - 127 - GRAPHIC SYSTEM INITIALIZATION FILE GRAPH: contains graphics parameters which can be easily changed TABLE 7.14 - Graphic System Init i a l i z a t i o n F i l e GRAPH NO. OF RECORDS CONTENTS DESCRIPTION VIEW REFERENCE POINT COORDINATES *TEXT VRP(3) *TEXT XMIN,XMAX,YMIN, YMAX;ZMIN,ZMAX VIEW WINDOW LIMITS TABLE 7.15 - Example of GRAPH CONTENTS RECORD NO. *VIEW REFERENCE POINT 1 0 0 0 2 *WINDOW DIMENSIONS, XMIN,XMAX,YMIN,YMAX,ZMIN,ZMAX 3 -600. 600. -600. 500. -600. 600. 4 - 128 - FILE OF WORKSTATION SETUP FOR EACH SEAM LOCSET: contains information about the table p o s i t i o n and robot configuration to weld a given seam TABLE 7.16 - Workstation Setup F i l e for Current Seam LOCSET NO. OF RECORDS: CONTENTS SEAMID VAR(6) IC0NF(6) SEAMID VAR(6) IC0NF(6) ETC. DESCRIPTION SEAM IDENTIFICATION NUMBER TABLE JOINT POSITIONS ROBOT CONFIGURATION SPECIFICATION SEAM IDENTIFICATION NUMBER TABLE JOINT POSITIONS ROBOT CONFIGURATION SPECIFICATION TABLE 7.17 - Example of LOCSET for Seams 5, 1, 3 CONTENTS RECORD 300.0000 600.0000 0.0000000E+00 0 0 1 300.0000 600.0000 0.0000000E4O0 0 0 3 300.0000 600.0000 0.0000000E+00 0 0 -90.00000 0.0000000E+00 0.0000000E+00 -90.00000 0.0000000E+00 0.0000000E+O0 -90.00000 0.0000000E+00 0.0000000E+00 1 2 3 4 5 6 7 8 - 129 - CURRENT TABLE POSITION DATA STOVAR: contains the current table j o i n t positions TABLE 7.18 - Current Table P o s i t i o n Data STOVAR NO. OF RECORDS CONTENTS DESCRIPTION 1 *TEXT 1 VAR(l) JOINT (1) POSITION 1 VAR(NDOF) JOINT (N) POSITION TABLE 7.19 - Example of STOVAR f o r 3 Degree of Freedom Table CONTENTS RECORD NO. *JOINT VARIABLES 300.0000 600.0000 -90.00000 1 2 3 4 - 130 - TEMPORARY WELD PARAMETER FILE WPARAM: contains the welding current and speed parameters for a l l the locations of the current seam TABLE 7.20 - Temporary Weld Parameter File WPARAM NO. OF RECORDS CONTENTS DESCRIPTION 1 CURRNT,SPEED PARAMETERS FOR LOCATION 1 1 CURRNT,SPEED PARAMETERS FOR LOCATION 2 1 CURRNT,SPEED • PARAMETERS FOR LOCATION 'NPOINT' TABLE 7.21 - Example of WPARAM for a Seam with 10 Locations CONTENTS RECORD 30 10 1 31 10 2 31.5 10.5 3 33 12 4 34.5 12 5 34 11.5 6 33 11 7 33 11 8 32 10 9 31 10 10 - 131 - TABLE 7.22 - Fortran F i l e Formats FILE NAME FORMAT FILIST CADLST CADDAT WPCLST WPCDAT ROBLST ROBDAT OBJLST OBJDAT WKSTAT TABLE GRAPH LOCSET STOVAR WPARAM LIST-DIRECTED LIST-DIRECTED UNFORMATTED LIST-DIRECTED UNFORMATTED LIST-DIRECTED FORMATTED LIST-DIRECTED LIST-DIRECTED LIST-DIRECTED LIST-DIRECTED LIST-DIRECTED LIST-DIRECTED LIST-DIRECTED LIST-DIRECTED - 132 - FILE TABLE 7.23 - Subroutines Accessing Data Files SUBROUTINES CADDAT CADLST FILIST GRAPH LOCSET OBJDAT OBJLST ROBDAT ROBLST STOVAR TABLE WKSTAT WPARAM WPCDAT WPCLST DRSEAM,DRWELD,PROCE S,SETUP AUTOP,DRSEAM,DRWELD,SETUP AUTOP AUTOP INIT,WORPOS CADFIL CADFIL INIT,WORPOS INIT,WORPOS TABSET,WORPOS INIT,TABPOS,TABSET EXPERT,INIT,WELSET EXPERT,WORPOS EXPERT,PROCES,SETUP,WESET.WORPOS SETUP,WORPOS 7.5 Graphical Depiction of Solids Using Panels If we attempt to represent polyhedra graphically using the usual wire-frame depiction, we find the image ambiguous and confusing unless hidden lines are removed. Hidden line removal is non-trivial, and i s generally not available in simple graphics packages such as Tektronix IGL. We have developed an alternative method for depicting polyhedral solids in a clear and unambiguous way without explicit hidden line calculations by taking advantage of the characteristics of the Tektronix 4027 graphics terminal. - 133 - The Tektronix 4027 i s a raster-type terminal which supports PANEL graphics commands. A PANEL i s depicted as a f i l l e d polygon on the screen. A new PANEL image i s superimposed on anything already on the screen. We have developed an algorithm which depicts a polyhedron as the superposition of i t s face panels. The panels are superimposed from back to front, r e l a t i v e to the viewpoint. Hidden surfaces are thus auto- ma t i c a l l y covered. Each panel i s bounded by a dark vector border so that a l l edges w i l l be v i s i b l e . To superimpose the panels i n the correct order, we need a test for determining which of two panels i s "closer" to the viewpoint. We note that the face polygons of a polyhedral s o l i d cannot intersect each other, but can contact along common edges only. We can determine h e u r i s t i c a l l y which of two face polygons l i e s " c l oser" to the viewpoint by using the following p r i n c i p l e : Suppose we have two l i n e s i n space, L^ and L^, and a l i n e of sight v e c t o r V d e f i n i n g the view d i r e c t i o n ( F i g . 7.3). Let N.„ be the common —s —12 normal d i r e c t e d from L^ to L^ • Then we can say L^ appears to cross over from the given view d i r e c t i o n i f V, ' N u > 0 . (7.1) We can now state the following algorithm for determining the "stacking" of faces: 1. Each l i n e containing an edge of a face i s compared to a l l other l i n e s containing edges of a l l other defined faces. Each time the given l i n e i s i n front of another l i n e , that edges weight factor i s incremented by 1. This i s repeated for a l l edges on - 134 - Line Ll N - common normal Vs -1ine of s i g h t Figure 7.3 Relationship of two l i n e s to view point. - 135 - the face, and the edge weights are summed to get a t o t a l face weight. An average face weight i s obtained by div i d i n g the t o t a l face weight by the number of edges. 2. Repeat the above for every face. 3. "Stack" the faces g r a p h i c a l l y i n ascending order of t h e i r average weights. 7.6 User's Guide to Running Program AUTOP AUTOP i s a preliminary program only, and does not include a l l the necessary c a p a b i l i t i e s of a complete and comprehensive automatic programming system. The following user's guide i s not intended f o r an i n d u s t r i a l user, but rather explains the running of AUTOP at i t s current state of development. Compiling AUTOP The main program and a l l the subroutines can be compiled at once by running the command f i l e C0MPIL.COM. ( i . e . enter @C0MPIL). Linking AUTOP A l l the compiled object modules can be linked into a task image by running the command f i l e AUT0P.COM. ( i . e . enter @AUT0P). Running AUTOP 1. If the i n i t i a l set-up f i l e s FILIST, WKSTAT, TABLE and GRAPH have - 136 - not been set up, set them up as described in Section 7.4.1. 2. Generate a pseudo CAD f i l e as described in Section 7.4.1. Currently a direct CAD interface has not been developed. The program CADFIL generates a set of test CAD f i l e s . The corresponding geometric model of the workpiece has been generated manually in f i l e CYLIND.DAT. 3. Insure that the terminal emulates a Tektronix 4027. 4. Type RUN AUTOP. 5. The screen w i l l go blank, and AUTOP w i l l prompt the user for the name of the f i l e l i s t FILIST. Currently, the name Is FILIST.DAT, in correspondence with the DEC data f i l e naming convention. A different name can be specified i f the program is implemented on a system with a different f i l e naming convention. Similarly, FILIST.DAT contains system compatible data f i l e names for a l l the data f i l e s used by AUTOP. 6. AUTOP automatically scans a l l the seam CAD f i l e s and checks i f the current welder setup is correct. If not, the operator is instructed to reset the welder. The new setup parameters are recorded in a f i l e . 7. AUTOP next prompts the user for the eye position. The eye position determines the view direction, directed from the eye position to the origin (0.,0.,0.). 8. AUTOP asks the user i f a new table position is desired. If the user answers Y (yes), AUTOP prompts for a new position. 9. AUTOP draws the workpiece, seams and a set of reference axes on the screen. The seams are represented by 'herringbone' lines pointing in the default weld directions. - 137 - 10. AUTOP prompts the user for new eye position and/or table p o s i t i o n . 11. If the eye or table p o s i t i o n i s changed, AUTOP proceeds from step 7. 12. When no more changes are requested, AUTOP draws a set of crosshairs and prompts the user to pick the f i r s t seam to be welded. The user moves the crosshairs to the desired seam and picks i t by pressing any key. 13. AUTOP checks the selected seam for path f e a s i b i l i t y . If the path i s f e a s i b l e , the weld i s f i l l e d i n . Otherwise, the user i s prompted to re p o s i t i o n the table and t r y again. 14. The user continues picking seams and repositioning the table u n t i l a l l seams have been picked. AUTOP stores the table positions and robot configurations, and generates robot locations and welding data f o r each seam. 15. AUTOP terminates when a l l the seams have been picked. 7.7 Demonstration of AUTOP This section i l l u s t r a t e s the procedure followed by AUTOP by presenting actual numerical data generated by AUTOP during a test session. For t h i s demonstration, we have extracted and converted the numerical r e s u l t s and contents of data f i l e s into a form suitable f o r inspection and v e r i f i c a t i o n . The workpiece which we have defined for this demonstration consists of a cylinder welded to a block. Five weld seams have been defined; four equal length segments around the circumference of the cyl i n d e r and one along the length of the c y l i n d e r . - 138 - The digitized CAD seam data was generated by a simple FORTRAN program written explicitly for this particular workpiece. The geometric data was entered manually into the object geometry f i l e . The robot was defined to be a PUMA 560 and we arbitrarily defined a positioning table with one revolute and two prismatic degrees of freedom. AUTOP was run, and the workpiece was interactively repositioned until a kinematically feasible path was found for welding the seam of interest. For this example, we have chosen seam one (see Fig. 7.4). Figs. 7.4, 7.5 and 7.6 show the appearance of the workpiece on the screen, and demonstrate the effect of changing the table position and the user's view point. The contents of the CAD seam data f i l e for seam one are shown in Table 7.24. These data are processed by AUTOP into the more useful form shown in Table 7.25. For a given seam and table position, AUTOP automatically tests different robot configurations and rotates the torch about i t s axis within user-defined limits in search of a kinematically feasible path. Table 7.26 contains the calculated joint angles for each robot position along the seam for different configurations and torch rotations. Note that a number of configurations were tested before a feasible one was found. When a feasible path has been found, AUTOP records the table position and robot configuration for later reference. AUTOP then generates the robot location data in the appropriate form for downloading to the robot. Table 7.27 shows a l i s t i n g of the resulting X, Y, Z, 0, A, T coordinates for the PUMA, as well as the correct welding current and speed at each point. These location values have been verified by manually downloading - 139 - Seam 5 Seam 1 Figure 7.4 Graphical Depiction of Workpiece by AUTOP. - 140 - Figure 7.5 New Workpiece P o s i t i o n a f t e r I n t e r a c t i v e l y Changing Table P o s i t i o n . - 141 - Figure 7.6 Workpiece shown from a D i f f e r e n t Viewpoint T a b l e 7 . 2 4 - C o n t e n t s o f Seam CAD F i l e . rYPE XSECT T l T2 NPOINT ICURVE 1 0 .500 0 .250 0 .500 26 1 NIIEX R<X> R(Y) R (Z ) N1(X> N K Y ) N K Z ) N2(X) N2(Y) N2<Z) 1 200.000 100.000 100.000 1,000 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 i 1 9 9 . 8 0 3 1 0 6 . 2 7 9 100.000 0 . 9 9 8 0 . 0 6 3 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 3 1 9 9 . 2 1 1 1 1 2 . 5 3 3 100.000 0 . 9 9 2 0 . 1 2 5 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 4 1 9 8 . 2 2 9 1 1 8 . 7 3 8 1 0 0 . 0 0 0 0 . 9 8 2 0 . 1 8 7 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1.000 5 1 9 6 . 8 5 8 1 2 4 . 8 6 9 100.000 0 . 9 6 9 0 . 2 4 9 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1.000 6 1 9 5 . 1 0 6 1 3 0 . 9 0 2 1 0 0 . 0 0 0 0 . 9 5 1 0 . 3 0 9 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 7 1 9 2 . 9 7 8 1 3 6 . 8 1 2 1 0 0 . 0 0 0 0 . 9 3 0 0 . 3 6 8 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 8 1 9 0 . 4 8 3 1 4 2 . 5 7 8 1 0 0 . 0 0 0 0 . 9 0 5 0 . 4 2 6 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 9 1 8 7 . 6 3 1 1 4 8 . 1 7 5 1 0 0 . 0 0 0 0 . 8 7 6 0 . 4 8 2 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 10 1 8 4 . 4 3 3 1 5 3 . 5 8 3 1 0 0 . 0 0 0 0 . 8 4 4 0 . 5 3 6 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 11 1 8 0 . 9 0 2 1 5 8 , 7 7 9 1 0 0 . 0 0 0 0 . 8 0 9 0 . 5 8 8 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 12 1 7 7 . 0 5 1 1 6 3 . 7 4 2 1 0 0 . 0 0 0 0 . 7 7 1 0 . 6 3 7 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 13 1 7 2 . 8 9 7 1 6 8 . 4 5 5 1 0 0 . 0 0 0 0 , 7 2 9 0 . 6 8 5 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 14 1 6 8 . 4 5 5 1 7 2 . 8 9 7 1 0 0 . 0 0 0 0 . 6 8 5 0 . 7 2 9 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 15 1 6 3 . 7 4 2 1 7 7 . 0 5 1 1 0 0 . 0 0 0 0 . 6 3 7 0 . 7 7 1 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 16 1 5 8 . 7 7 9 1 8 0 . 9 0 2 1 0 0 . 0 0 0 0 . 5 8 8 0 . 8 0 9 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 17 1 5 3 . 5 8 3 1 8 4 . 4 3 3 1 0 0 . 0 0 0 0 . 5 3 6 0 . 8 4 4 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 18 1 4 8 . 1 7 5 1 8 7 . 6 3 1 1 0 0 . 0 0 0 0 . 4 8 2 0 . 8 7 6 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 19 1 4 2 . 5 7 8 1 9 0 . 4 8 3 1 0 0 . 0 0 0 0 . 4 2 6 0 . 9 0 5 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 20 1 3 6 . 8 1 3 1 9 2 . 9 7 8 1 0 0 . 0 0 0 0 . 3 6 8 0 . 9 3 0 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 21 1 3 0 . 9 0 2 1 9 5 . 1 0 6 100 .000 0 . 3 0 9 0 . 9 5 1 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 22 1 2 4 . 8 6 9 1 9 6 . 8 5 8 1 0 0 . 0 0 0 0 . 2 4 9 0 . 9 6 9 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 23 1 1 8 . 7 3 8 1 9 8 . 2 2 9 1 0 0 . 0 0 0 0 . 1 8 7 0 . 9 8 2 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 24 1 1 2 . 5 3 3 1 9 9 . 2 1 1 1 0 0 . 0 0 0 0 . 1 2 5 0 . 9 9 2 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 25 1 0 6 . 2 7 9 1 9 9 . 8 0 3 1 0 0 , 0 0 0 0 . 0 6 3 0 . 9 9 8 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 26 1 0 0 . 0 0 0 2 0 0 . 0 0 0 1 0 0 . 0 0 0 0 . 0 0 0 1.000 0 . 0 0 0 0 . 0 0 0 0 . 0 0 0 1 .000 Table 7.25 - Contents of Processed Seam Data F i l e . JTYPE XSECT TI T2 NPOINT ICURVE 1 0.500 0.250 0.500 26 1 INDEX S R(X) R(Y) R(Z) T(X) T(Y> 1 0.000 2Q0.000 100.000 100.000 -0.031 1.000 2 6.282 199.803 106.279 100.000 -0.063 0.998 3 12.564 199.211 112.533 100.000 -0.125 0.992 4 18.846 198.229 118.738 100.000 -0.187 0.982 5 25.129 196.858 124.869 100.000 -0.249 0.969 6 31.411 195.106 130.902 100.000 -0.309 0.951 7 37.693 192.978 136.812 100.000 -0.368 0.930 8 43.975 190.483 142.578 100.000 -0.426 0.905 9 50.257 187.631 148.175 100.000 -0.482 0.876 10 56.539 184.433 153.583 100.000 -0.536 0.844 11 62.821 180.902 158.779 100.000 -0.588 0.809 12 69.104 177.051 163.742 100.000 -0.637 0.771 13 75.386 172.897 168.455 100.000 -0.685 0.729 14 81.668 168.455 172.897 100.000 -0.729 0.685 15 87.950 163.742 177.051 100.000 -0.771 0.637 16 94.232 158.779 180.902 100.000 -0.809 0.588 17 100.514 153.583 184.433 100.000 -0.844 0.536 18 106.797 148.175 187.631 100.000 -0.876 0.482 19 113.079 142.578 190.483 100.000 -0.905 0.426 20 119.361 136.813 192.978 100.000 -0.930 0.368 21 125.643 130.902 195.106 100.000 -0.951 0.309 22 131.925 124.869 196.858 100.000 -0.969 0.249 23 138.207 118.738 198.229 100.000 -0.982 0.187 24 144.489 112.S33 199.211 100.000 -0.992 0.125 25 150.772 106.279 199.803 100.000 -0.9V8 0.063 26 157.054 100.000 200.000 100.000 -1.000 0.031 T(Z) MX) MY) MZ) B(X) B(Y) B(Z) 0.000 0.707 0.022 0.707 -0.707 -0.022 0.707 0.000 0.705 0.044 0.708 -0.706 -0.044 0.706 0.000 0.701 0.089 0.708 -0.702 -0.089 0.706 0.000 0.694 0.132 0.708 -0.695 -0.133 0.706 0.000 0.684 0.176 0.708 -0.686 -0.176 0.706 0.000 0.672 0.218 0.708 -0.673 -0,219 0.706 0.000 0.657 0.260 0.708 -0.658 -0.261 0.706 0.000 0.639 0.301 0.708 -0.640 -0.301 0.706 0.000 0.619 0.340 0.708 -0.620 -0.341 0.706 0.000 0.596 0.379 0.708 -0.598 -0.379 0.706 0.000 0.571 0.415 0.708 -0.573 -0.416 0.706 0.000 0.544 0.450 0.708 -0.545 -0.451 0.706 0.000 0.515 0.484 0.708 -0.516 -0.485 0.706 0.000 0.484 0.515 0.708 -0.485 -0.516 0.706 0.000 0.450 0,544 0.708 -0.451 -0.545 0.706 0.000 0.415 0.571 0.708 -0.416 -0.573 0.706 0.000 0.379 0.596 0.708 -0.379 -0.598 0.706 0.000 0.340 0.619 0.708 -0.341 -0.620 0.706 0.000 0.301 0.639 0.708 -0.301 -0.640 0.706 0.000 0.260 0.657 0.708 -0.261 -0.658 0.706 0.000 0,218 0,672 0.708 -0.219 -0.673 0,706 0.000 0,176 0.684 0.708 -0.176 -0.686 0.706 0.000 0.132 0.694 0.708 -0.133 -0.695 0.706 0.000 0.089 0.701 0.708 -0.089 -0.702 0.706 0.000 0.044 0.705 0.708 -0.044 -0.706 0.706 0.000 0.022 0.707 0.707 -0,022 -0.707 0.707 - 1 4 4 - Table 7*26 - J o i n t Angles C a l c u l a t e d d u r i n g Kinematic F e a s i b i l i t y Search* 0 0 1 1 0 0 LEFTY BELOW FLIP CONFIGURATION JOINT ANGLES (DEGREES) JOINT LIMITS EXCEEDED TOOL INDEX JI J2 J3 J4 J5 J6 1 <•> 3 4 5 6 R01 i -6.622 16.015 -50.077 -165.980 -148.506 172.931 0 0 0 1 1 0 Y 1 -15.533 13.294 -50.437 -129.587 -147.050 -159.095 0 0 0 1 1 0 Y 1 1.934 19.744 -49.250 163.429 -140.524 151.316 0 0 0 0 1 0 Y 1 -23.432 12.170 -50.308 -104.463 -138.041 -142.106 0 0 0 0 1 0 Y 1 9.213 23.869 -48.006 144.246 -128.539 141.902 0 0 0 0 1 0 Y 1 -29.204 12.847 -49.703 -90.092 -127.680 -135.899 0 0 0 0 1 0 Y 1 14.867 27.948 -46.416 131.030 -116.493 139.435 0 0 0 0 1 0 Y 1 -32.589 14.993 -48.661 -80.849 -118.916 -134.963 0 0 0 0 1 0 Y 1 18.922 31.719 -44.561 120.298 -105.845 140.484 0 0 0 0 1 0 Y 1 -33.991 18.010 -47.247 -73.811 -112.241 -136.320 0 0 0 0 1 0 Y 1 21.564 35.056 -42.524 110.545 -97.011 143.488 0 0 0 0 0 0 23.044 35.421 -42.404 110.839 -96.392 143.565 0 0 0 0 0 0 3 24,754 35.970 -42.832 112.562 -95.680 142.869 0 0 0 0 0 0 4 26.424 36.533 -43.311 114.323 -95.033 142.093 0 0 0 0 0 0 5 28.053 37.107 -43.840 116.123 -94.458 141.240 0 0 0 0 0 0 6 29.640 37.689 -44.420 117.966 -93.960 140.316 0 0 0 0 0 0 7 31.182 38.278 -45.049 119.854 -93.547 139.325 0 0 0 0 0 0 8 32.679 38.871 -45.727 121.788 -93.226 138.271 0 0 0 0 0 0 9 34,128 39.465 -46.454 123.772 -93.003 137.159 0 0 0 0 0 0 10 35,526 40.055 -47.227 125.807 -92.886 135.996 0 0 0 0 0 0 11 36,871 40.639 -48.048 127.894 -92.882 134.786 0 0 0 0 0 0 12 38,159 41.212 -48.915 130.035 -93.000 133.538 0 0 0 0 0 0 13 39.385 41.769 -49.828 132.233 -93.248 132.258 0 0 0 0 0 0 14 40.546 42.304 -50.785 134.489 -93.635 130.955 0 0 0 0 0 0 15 41.636 42.809 -51.787 136.807 -94.170 129.637 0 0 0 0 0 0 16 42.647 43.278 -52.833 139.191 -94.866 128.313 0 1 1 0 0 0 Y 16 38.475 37.889 -53.846 148.929 -108.479 126.679 0 0 1 0 1 0 Y 16 45.565 48.083 -51.330 129.697 -82.751 132.566 0 1 0 0 0 0 Y 16 33.086 32.310 -54.290 160.471 -122.654 128.722 0 0 1 0 1 0 Y 16 47.332 52.087 -49.435 119.593 -72.640 138.979 0 1 0 0 0 0 Y 16 26.766 27.090 -54.127 176.471 -135.671 136.549 0 0 1 1 1 0 Y 16 48.106 55.213 -47.252 108.481 -64.807 147.302 0 1 0 0 0 0 Y 16 20.135 22.834 -53.364 -160.138 -144.740 152.536 0 0 1 1 1 0 Y 16 48.055 57.475 -44.881 96.368 -59.387 157.190 0 1 0 0 0 0 Y 16 14.035 20.008 -52.055 -131.926 -147.283 173.638 0 0 1 1 1 0 Y 16 47.335 58.936 -42.410 83.650 -56.382 167.998 0 1 0 0 0 0 Y 16 9.197 18.748 -50.286 -108.091 -144.409 -169.518 0 0 0 0 1 0 Y 16 46.083 59.685 -39.917 70.993 -55.630 178.812 0 1 0 0 0 0 Y 16 5.927 18.834 -48.162 -91.535 -139.649 -159.767 0 0 0 0 1 0 Y 16 44.419 59.811 -37.470 59,058 -56.811 -171.253 0 1 0 0 0 0 Y 16 4.119 19.868 -45.788 -79.772 -135.107 -154.506 0 0 0 0 1 0 Y 16 42.442 59.406 -35.127 48.262 -59.518 -162.748 0 1 0 0 0 0 Y 16 3.473 21.479 -43.267 -70.605 -131.349 -151.436 0 0 0 0 1 0 Y ********** NO FEASIBLE PATH FOUND ********** - 145 - Table 7.26/cont. 0 0 1 0 0 0 LEFTY BELOW NOFLIP CONFIGURATION 1 -6.622 16.015 -50.077 14.020 148.506 -7.069 0 0 0 0 1 0 1 -15.533 13,294 -50,437 50,414 147.050 20.906 0 0 0 0 1 0 1 1.934 19.744 -49.250 -16.571 140.524 -28.684 0 0 0 0 1 0 1 -23.432 12,170 -50,308 75.537 138.041 37.894 0 0 0 0 1 0 1 9.213 23.869 -48,006 -35.755 128.539 -38.098 0 0 0 0 1 0 1 -29.204 12,847 -49.703 89.908 127.680 44,102 0 0 0 0 1 0 1 14.867 27,948 -46.416 -48.970 116.493 -40.565 0 0 0 0 1 0 1 -32.589 14,993 .-48,661 99.152 118,916 45,038 0 0 0 0 1 0 1 18.922 31.719 -44.561 -59.703 105.845 -39.516 0 0 0 0 1 0 1 -33.991 18.010 -47.247 106.190 112,241 43.680 0 0 0 0 1 0 1 21.564 35.056 -42,524 -69.455 97.011 -36.512 0 0 0 0 0 0 23,044 35.421 -42,404 -69.161 96.392 -36.435 0 0 0 0 0 0 3 24.754 35.970 -42.832 -67.438 95.680 -37.131 0 0 0 0 0 0 4 26.424 36.533 -43.311 -65.678 95,033 -37.908 0 0 0 0 0 0 5 28.053 37.107 -43.840 -63.877 94.458 -38.760 0 0 0 0 0 0 6 29.640 37.689 -44.420 -62.034 93.960 -39.684 0 0 0 0 0 0 7 31.182 38.278 -45.049 -60.146 93.547 -40.675 0 0 0 0 0 0 8 32.679 38,871 -45.727 -58.212 93.226 -41,729 0 0 0 0 0 0 9 34.128 39,465 -46.454 -56,228 93.003 -42,841 0 0 0 0 0 0 10 35.526 40.055 -47,227 -54.193 92.886 -44.005 0 0 0 0 0 0 11 36.871 40.639 -48,048 -52,106 92.882 -45,214 0 0 0 0 0 0 12 38.159 41,212 -48,915 -49.965 93,000 -46.462 0 0 0 0 0 0 13 39.385 41,769 -49,828 -47.767 93,248 -47,742 0 0 0 0 0 0 14 40,546 42.304 -50,785 -45.511 93,635 -49.045 0 0 0 0 0 0 15 41.636 42.809 -51,787 -43.193 94,170 -50.364 0 0 0 0 0 0 16 42.647 43,278 -52.833 -40,809 94.866 -51.687 0 1 1 0 0 0 16 38.475 37,889 -53.846 -31.072 108.479 -53.321 0 0 1 0 1 0 16 45.565 48.083 -51.330 -50,303 82,751 -47,434 0 1 0 0 0 0 16 33.086 32.310 -54,290 -19.529 122.654 -51.278 0 0 1 0 1 0 16 47.332 52.087 -49,435 -60.407 72,640 -41,021 0 1 0 0 0 0 16 26.766 27.090 -54.127 -3.529 135.671 -43.451 0 0 1 0 1 0 16 48,106 55.213 -47.252 -71.519 64.807 -32.698 0 1 0 0 0 0 16 20.135 22,834 -53.364 19.862 144.740 -27.464 0 0 1 0 1 0 16 48,055 57.475 -44.881 -83,632 59.387 -22.810 0 1 0 0 0 0 16 14.035 20.008 -52.055 48.074 147.283 -6.363 0 0 1 0 1 0 16 47.335 58.936 -42.410 -96.350 56.382 -12.002 0 1 0 0 0 0 16 9.197 18.748 -50,286 71.909 144.409 10.482 0 0 0 0 1 0 16 46,083 59.685 -39.917 -109.007 55,630 -1.188 0 1 0 0 0 0 16 5,927 18.834 -48.162 88,465 139.649 20.233 0 0 0 0 1 0 16 44.419 59.811 -37,470 -120.943 56,811 8.747 0 1 0 1 0 0 16 4,119 19.868 -45.788 100.228 135.107 25.495 0 0 0 0 1 0 16 42.442 59.406 -35.127 -131.738 59.518 17.252 0 1 0 1 0 0 16 3,473 21,479 -43.267 109.395 131,349 28.564 0 0 0 0 1 0 ********** NO FEASIBLE PATH FOUND ********** - 146 - Table 7.26/cont, 0 0 0 1 0 0 LEFTY ABOVE FLIP CONFIGURATION 1 -6.622 -127.357 -124.502 -133.965 -10.127 115.326 0 0 1 1 0 0 1 -15.533 -130,443 -124.142 -119.274 -28.721 98.042 0 0 1 1 0 0 1 1.934 -122.787 -125.329 91.741 -10.452 -107.475 0 0 1 0 0 0 1 -23.432 -131.437 -124.270 -116.376 -46.273 92.552 0 0 1 1 0 0 1 9.213 -117.398 -126.573 79.061 -27.743 -91.496 0 0 1 0 0 0 1 -29.204 -130.144 -124.876 -115.454 -61.225 88.686 0 0 1 1 0 0 1 14.867 -111.706 -128.163 77.822 -43.685 -86.588 0 0 0 0 0 0 16.438 -111.159 -128.304 77.000 -43,941 -86.257 0 0 0 0 0 0 3 17.992 -111,084 -127,995 74,785 -43.004 -85.888 0 0 1 0 0 0 3 12.386 -116.682 -126,523 75.333 -26.972 -90.091 0 0 1 0 0 0 3 22.065 -105,462 -129.764 75.895 -57.359 -83.042 0 0 0 0 0 0 4 23.651 -105,423 -129.335 73.803 -56.564 -82.802 0 0 0 0 0 0 5 25.192 -105.429 -128.860 71.678 -55.741 -82.549 0 0 0 0 0 0 6 26.687 -105.483 -128.339 69.518 -54.889 -82.279 0 0 0 0 0 0 7 28.134 -105.585 -127.773 67.318 -54,005 -81.990 0 0 1 0 0 0 7 23.730 -111.269 -126.335 65,426 -38.939 -84.115 0 0 1 0 0 0 7 31.182 -99.991 -129.530 69.547 -67,500 -79.883 0 0 0 0 0 0 8 32.679 -100.085 -128.852 67.435 -66.781 -79.659 0 0 0 0 0 0 9 34,128 -100.228 -128.125 65.288 -66,036 -79.420 0 0 0 0 0 0 10 35.526 -100.422 -127.352 63.102 -65,261 -79.164 0 0 1 0 0 0 10 32.162 -106.218 -125,806 60.438 -51.142 -80.967 0 0 1 0 0 0 10 37.670 -94.874 -129.211 65.891 -77.900 -77.146 0 0 0 0 0 0 11 39.124 -95.021 -128,340 63.779 -77.307 -76.913 0 0 0 0 0 0 12 40.528 -95.217 -127.419 61.630 -76.694 -76.667 0 0 1 0 0 0 12 38,159 -100.978 -125.664 58.598 -63.613 -78.594 0 0 1 0 0 0 12 41.814 -89,870 -129.451 64.783 -88.397 -74.504 0 0 0 0 0 0 13 43.273 -89.993 -128.451 62.708 -87.967 -74,248 0 0 0 0 0 0 14 44,681 -90.159 -127.399 60.596 -87.526 -73.982 0 0 1 0 0 0 14 43.172 -95.769 -125,430 57.206 -75.393 -76.134 0 0 1 0 0 0 14 45.239 -85.107 -129,604 64.130 -98.385 -71.561 0 0 0 0 0 0 15 46.698 -85.181 -128.491 62.102 -98.121 -71,260 0 0 0 0 0 0 16 48.106 -85.290 -127.327 60.037 -97,859 -70.950 0 0 1 0 0 0 16 47.332 -90.632 -125.144 56,244 -86.606 -73.418 0 0 1 0 0 0 16 48.055 -80.624 -129.698 64.028 -107,936 -68.144 0 0 0 0 1 0 16 45.565 -96.564 -123.249 52.576 -73,971 -75.537 0 0 1 0 0 0 16 47.335 -76.660 -132.169 68.310 -117.038 -64.914 0 0 0 0 1 0 16 42.647 -102.900 -121.746 48.887 -59.805 -77.134 0 0 1 0 0 0 16 46.083 -73,391 -134.662 73.050 -125.329 -61.085 0 0 0 0 1 0 16 38.475 -109.321 -120.733 44.679 -44.120 -77,653 0 0 1 0 0 0 16 44.419 -70,793 -137.109 78,531 -132.913 -56.367 0 0 0 0 1 0 16 33.086 -115,354 -120.289 37.913 -27.261 -75.019 0 0 1 0 0 0 16 42.442 -68.833 -139.452 85.196 -139.810 -50.318 0 0 0 0 1 0 .1.6 26.766 -120.408 -120.452 13.349 -10,736 -54.043 0 0 1 0 0 0 16 40.235 -67.479 -141.641 93.712 -145.928 -42.294 0 0 0 0 1 0 16 20.135 -123.886 -121.215 -94.731 -11.351 50.982 0 0 1 0 0 0 16 37.865 -66.698 -143,633 104.943 -151,019 -31.470 0 0 0 0 1 0 16 14.035 -125.376 -122.524 -116.452 -26.690 69.617 0 0 1 1 0 0 16 35.387. -66.459 -145.388 119.595 -154.632 -17.215 0 0 0 0 1 0 16 9.197 -124.836 -124,293 -122,530 -41.009 72.580 0 0 1 1 0 0 ********** NO FEASIBLE PATH FOUND ********** - 147 - Table 7.26/cont. O O O O O O LEFTY ABOVE NOFLIP CONFIGURATION 1 -6,622 -127.357 -124.502 46.035 10.127 -64.674 0 0 1 0 0 0 1 -15.533 -130.443 -124.142 60.726 28.721 -81,958 0 0 1 0 0 0 1 1 .934 -122.787 -125.329 -88.259 10.452 72.525 0 0 1 0 0 0 1 -23.432 -131.437 -124.270 63.625 46.273 -87,448 0 0 1 0 0 0 1 9.213 -117.398 -126.573 -100.939 27.743 88.505 0 0 1 0 0 0 1 -29.204 -130.144 -124.876 64,546 61.225 -91.314 0 0 1 0 0 0 1 14.867 -111.706 -128,163 -102.178 43.685 93.412 0 0 0 0 0 0 *— 16.438 -111.159 -128.304 -103.000 43.941 93.743 0 0 0 0 0 0 3 17.992 -111.084 -127.995 -105,215 43.004 94.112 0 0 1 0 0 0 3 12.386 -116.682 -126.523 -104.667 26.972 89.909 0 0 1 0 0 0 3 22.065 -105.462 -129.764 -104.105 57.359 96.958 0 0 0 0 0 0 4 23.651 -105.423 -129.335 -106.197 56.564 97.198 0 0 0 0 0 0 5 25.192 -105.429 -128.860 -108.322 55.741 97.451 0 0 0 0 0 0 6 26.687 -105.483 -128.339 -110.482 54.889 97.721 0 0 0 1 0 0 6 22.371 -111.146 -126.813 -112.146 40.007 95.384 0 0 1 1 0 0 6 29.640 -99.942 -130,159 -108.373 68.194 99.906 0 0 0 0 0 0 7 31.182 -99.991 -129.530 -110.453 67.500 100.117 0 0 0 1 0 0 7 28.134 -105.585 -127.773 -112.682 54.005 98,010 0 0 1 1 0 0 7 33.029 -94.695 -131.521 -107.963 79.566 102.223 0 0 0 0 0 0 8 34.620 -94.713 -130.802 -109.983 79.028 102,422 0 0 0 0 0 0 9 36.167 -94.772 -130.032 -112.031 78.473 102.633 0 0 0 1 0 0 9 34.128 -100.228 -128.125 -114.712 66.036 100,581 0 0 0 1 0 0 9 37.163 -89,737 -132.139 -109.176 89.630 104.791 0 0 0 0 0 0 10 38.757 -89.744 -131.296 -111.162 89,228 105.016 0 0 0 1 0 0 10 37.670 -94.874 -129.211 -114.110 77.900 102.854 0 0 0 1 0 0 10 38.958 -85.129 -133.520 -108.048 99.452 107.341 0 0 0 0 0 0 11 40.590 -85.078 -132.622 -109.966 99.184 107.600 0 0 0 0 0 0 12 42.183 -85.057 -131.670 -111.908 98.917 107.869 0 0 0 1 0 0 12 41.814 -89.870 -129.451 -115.217 88.397 105.496 0 0 0 1 0 0 12 41.795 -80.835 -133.991 -108.401 108.459 110.503 0 0 0 0 1 0 12 40.528 -95.217 -127.419 -118.370 76.694 103.333 0 0 1 1 0 0 12 40.795 -77,224 -136.339 -104.608 117.202 113.516 0 0 0 0 1 0 12 38.159 -100.978 -125.664 -121.402 63.613 101.406 0 0 1 1 0 0 12 39.303 -74.225 -138.648 -100.364 125.282 117.097 0 0 0 0 1 0 12 34.556 -106.944 -124.279 -124.450 49.025 99.895 0 0 1 1 0 0 12 37.426 -71.826 -140.860 -95.387 132.776 121.538 0 0 0 0 1 0 12 29.654 -112.785 -123.350 -128.116 32.968 99.463 0 0 1 1 0 0 12 35.251 -70.010 -142.925 -89.232 139.690 127.284 0 0 0 0 1 0 12 23.564 -118.043 -122.941 -136.077 15.842 103.805 0 0 1 1 0 0 12 32.853 -68.758 -144.801 -81,217 145.921 135.001 0 0 0 0 1 0 12 16.704 -122.169 -123.085 111.544 4.344 -146.938 0 0 1 0 0 0 12 30.293 -68.050 -146.450 -70.406 151.208 145.588 0 0 0 0 1 0 12 9.829 -124.643 -123.774 65.765 20,264 -104.146 0 0 1 0 0 0 12 27.623 -67.865 -147.840 -55.911 155.077 159.867 0 0 0 0 1 0 12 3.826 -125.168 -124.964 60.579 36,131 -101.906 0 0 1 0 0 0 ********** NO FEASIBLE PATH FOUND ********** - 148 - Table 7.26/cont. 1 1 1 1 0 0 RIGHTY ABOVE FLIP CONFIGURATION 1 -98.962 -52.644' -50.077 -67.121 -75.397 -98.233 0 0 0 0 0 0 -98.225 -53.398 -49.854 -67.355 -74.593 -98.659 0 0 0 0 0 0 3 -97.183 -53.823 -49.805 -67.582 -72.849 -100.365 0 0 0 0 0 0 4 -96.089 -54.198 -49.788 -67.736 -71.153 -102.082 0 0 0 0 0 0 5 -94.946 -54.521 -49.803 -67.815 -69,505 -103.814 0 0 0 0 0 0 6 -93.755 -54.795 -49.850 -67.817 -67.903 -105.566 0 0 0 0 0 0 7 -92.518 -55.018 -49.930 -67.740 -66,349 -107.342 0 0 0 0 0 0 8 -91.237 -55.190 -50.042 -67.581 -64.841 -109.146 0 0 0 0 0 0 9 -89.912 -55.311 -50.185 -67.336 -63.382 -110.982 0 0 0 0 0 0 10 -88.543 -55.380 -50.361 -67.003 -61.973 -112.855 0 0 0 0 0 0 11 -87.132 -55.396 -50.567 -66.577 -60.615 -114.769 0 0 0 0 0 0 12 -85.678 -55.357 -50.805 -66.056 -59.311 -116.728 0 0 0 0 0 0 13 -84.181 -55.263 -51.073 -65,435 -58.065 -118.737 0 0 0 0 0 0 14 -82.639 -55.111 -51.371 -64.711 -56.881 -120.798 0 0 0 0 0 0 15 -81.053 -54.899 -51.699 -63.881 -55.763 -122.916 0 0 0 0 0 0 16 -79.419 -54.624 -52.055 -62.940 -54.717 -125.094 0 0 1 0 0 0 16 -86.488 -55.164 -50.286 -51.503 -41.809 -137.531 0 0 0 0 0 0 17 -85.015 -55.113 -50.532 -49.679 -41.024 -140.528 0 0 0 0 0 0 18 -83.498 -55.005 -50.808 -47,739 -40.346 -143.598 0 0 0 0 0 0 19 -81.937 -54.839 -51.115 -45,688 -39.780 -146.735 0 0 0 0 0 0 20 -80.330 -54.613 -51.450 -43,536 -39.332 -149.925 0 0 0 0 0 0 21 -78.676 -54.323 -51.814 -41,298 -39.009 -153.154 0 0 0 0 0 0 22 -76.972 -53.966 -52.206 -38.987 -38.815 -156.406 0 0 1 0 0 0 22 -85.331 -57.729 -49.444 -14.283 -32.257 177,540 0 0 0 0 0 0 23 -83.691 -57.592 -49.774 -11.733 -32.880 174.018 0 0 0 0 0 0 24 -82.007 -57.397 -50.137 -9.302 -33.613 170.664 0 0 0 0 0 0 25 -80.278 -57.139 -50.531 -6.999 -34.451 167.488 0 0 0 0 0 0 26 -78.432 -56.701 -51.010 -6.902 -35.065 167.575 0 0 0 0 0 0 ************ FEASIBLE PATH !!! ************** T a b l e 7 . 2 7 - G e n e r a t e d Robot L o c a t i o n and w e l d P a r a m e t e r D a t a PHA BETA STICKOUT VOLTAGE FEED GFR NPOINT i . 0 0 0 . 0 0 0 . 4 0 3 1 6 . 0 0 2 1 7 . 0 9 2 1 . 1 1 26 IDEX X Y Z 0 A T CURRENT SPEED 1 2 2 1 . 4 1 3 1 . 8 2 0 5 . 0 - 8 8 . 6 0 155 .01 1 6 4 . 3 2 1 3 7 . 0 0 1 .228 2 2 2 0 . 1 1 3 8 . 6 2 0 4 . 6 - 8 7 . 1 9 1 5 5 . 0 3 163 .64 1 3 6 . 9 5 1 .228 3 2 1 7 . 7 1 4 5 . 9 2 0 3 . 6 - 8 4 . 3 8 154 .94 1 6 2 . 2 9 1 3 6 . 8 4 1 .228 4 2 1 4 . 7 1 5 3 . 0 2 0 2 . 6 - 8 1 . 5 7 1 5 4 . 7 9 160 .94 136 .72 1 .228 5 2 1 1 . 4 1 5 9 . 8 2 0 1 . 4 - 7 8 . 7 6 154 .58 1 5 9 . 6 0 1 3 6 . 5 9 1 .228 6 2 0 7 . 7 1 6 6 . 5 2 0 0 . 0 - 7 5 . 9 5 154 .31 1 5 8 . 2 6 1 3 6 . 4 5 1 .228 7 2 0 3 . 6 1 7 2 . 9 1 9 8 . 5 - 7 3 . 1 4 153 .98 1 5 6 . 9 5 1 3 6 . 3 0 1 .228 8 1 9 9 . 1 1 7 9 . 0 1 9 6 . 9 - 7 0 . 3 4 1 5 3 , 5 9 1 5 5 . 6 4 1 3 6 . 1 5 1 .228 9 1 9 4 . 2 1 8 4 . 8 195 .1 - 6 7 . 5 3 1 5 3 . 1 5 1 5 4 . 3 6 1 3 6 . 0 0 1 .228 10 189 .1 190 .2 193 .2 - 6 4 . 7 2 1 5 2 . 6 5 1 5 3 . 0 9 1 3 5 . 8 4 1 .228 11 1 8 3 . 6 1 9 5 . 3 1 9 1 . 2 - 6 1 . 9 2 1 5 2 . 1 0 1 5 1 . 8 5 1 3 5 . 6 8 1 .228 12 1 7 7 , 8 2 0 0 . 0 189 .1 - 5 9 . 1 1 151 .49 1 5 0 . 6 4 1 3 5 . 5 2 1 .228 13 1 7 1 . 8 2 0 4 . 4 1 8 7 . 0 - 5 6 . 3 0 1 5 0 . 8 3 1 4 9 . 4 5 1 3 5 . 3 7 1 .228 14 1 6 5 . 5 2 0 8 . 3 1 8 4 . 7 - 5 3 . 4 9 1 5 0 . 1 3 1 4 8 . 2 9 135 .21 1 ,228 15 1 5 9 . 0 2 1 1 . 8 1 8 2 . 3 - 5 0 . 6 7 1 4 9 . 3 7 1 4 7 . 1 7 1 3 5 . 0 5 1 .228 16 1 6 5 . 5 2 0 8 . 9 1 8 6 . 8 - 3 7 . 1 3 1 4 3 . 2 3 1 5 2 . 0 8 1 3 4 . 9 0 1 .228 17 1 5 9 . 0 2 1 2 . 4 184 .4 - 3 4 . 2 9 1 4 2 . 2 3 151 .18 1 3 4 . 7 5 1 .228 18 1 5 2 . 3 2 1 5 . 5 1 8 1 . 9 - 3 1 . 4 3 1 4 1 . 2 0 1 5 0 . 3 3 1 3 4 . 6 0 1 .228 19 1 4 5 . 4 218 .1 179 .4 - 2 8 . 5 6 140 .13 1 4 9 . 5 3 1 3 4 . 4 6 1 .228 20 1 3 8 . 3 2 2 0 . 3 1 7 6 , 9 - 2 5 . 6 5 139 .04 1 4 8 . 7 9 1 3 4 . 3 3 1 .228 21 1 3 1 . 2 2 2 2 . 0 174 .3 - 2 2 . 7 1 137 .92 148 .12 1 3 4 . 2 0 1 .228 22 1 3 8 . 9 2 2 4 . 9 177 .1 - 6 . 6 9 131 .94 156.81 1 3 4 . 0 7 1 .228 23 1 3 1 . 4 2 2 6 . 7 1 7 4 . 4 - 3 . 4 3 130 .71 1 5 6 . 6 5 1 3 3 . 9 6 1 .228 24 1 2 3 . 9 2 2 7 . 9 1 7 1 , 6 - 0 . 1 0 129 .48 1 5 6 . 5 9 1 3 3 . 8 5 1 .228 25 1 1 6 , 3 2 2 8 . 6 1 6 8 , 9 3 . 3 3 128 .25 156 .64 1 3 3 . 7 5 1 .228 26 1 0 9 . 6 2 2 8 . 9 166 .4 5 . 0 7 1 2 7 . 6 0 156 .72 1 3 3 . 7 0 1 .228 - 150 - to the PUMA and executing the resulting path program, but the Expert Welder output has not yet been tested. 7.8 Future Capabilities Several important capabilities have not yet been f u l l y developed and implemented in AUTOP. These capabilities are summarized below: 7.8.1 Real-time Seam Tracking Eventually, we wil l require AUTOP to run in a real-time environment with sensory feedback from the welding process. AUTOP wil l be required to automatically adapt the planned trajectory to any real-world deviations. 7.8.2 CAD System Interface CAD data w i l l ultimately be generated by a commercial CAD system. The CAD system w i l l operate independently from AUTOP, but additional software w i l l be required to translate the CAD output data into the correct format for input to AUTOP. 7.8.3 Generation of High-level Descriptive Program A sophisticated welding workstation contains several programmable remotely controlled devices, including a robot, positioning table and welding machine. In order to realize the f u l l benefit of automatic programming i t is desirable to be able to program and control the workstation in a single, task-oriented high-level language. Clearly the welding process can be broken down into a number of actions occurring in a given sequence. Actions must be taken by - 151 - d i f f e r e n t physical devices, either independently or i n synchronization with other actions. An example of independent actions: turn on the welder and reposition the table. An example of synchronized actions: turn on the welder and move the torch along the seam. A s i m p l i f i e d high l e v e l welding task d e s c r i p t i o n i s i l l u s t r a t e d i n F i g . 7.7. Correct synchronization of device actions requires some form of scheduling based on task precedence requirements. We could create a task precedence table such as i n Table 7.28 for scheduling c o n t r o l . A l l tasks at a given l e v e l must be completed before any tasks at the next l e v e l can s t a r t . This type of action table can be implemented In a high l e v e l d e s c r i p t i v e programming language such as FORTRAN, running under a r e a l - time operating system. Standard real-time operating system practices can be used to achieve correct action synchronization. For example, we can use device i n t e r r u p t s , p o l l i n g of device flags or semaphores to signal completion of an action to the program. The device drivers can be implemented as low-level device dependent macros c a l l e d as subroutines from the high-level program. - 152 - i n i t i a l i z e welder i n i t i a l i z e robot No i n i t i a l i z e table < 1 position table move welding t o o l to s t a r t of seam sta r t welder move welding t o o l along seam stop welder Figure 7.7 Sim p l i f i e d High-level Task Description. - 153 - Table 7.28 Task Precedence Table Welder Robot Table i n i t i a l i z e i n i t i a l i z e i n i t i a l i z e p o s i t i o n move to seam start welder move along seam stop welder - 154 - 8. CONCLUSIONS The work described i n t h i s thesis provides a s o l i d basis for a commercially useful automatic programming system. Some necessary additions and desirable improvements are suggested which require more time and e f f o r t to develop, debug and implement. The knowledge, experience and techniques gained from this project are highly useful for any robot a p p l i c a t i o n , and a f u l l y automatic programming system for a welding robot can be r e a d i l y adapted to other a p p l i c a t i o n s . 8.1 Analysis and Evaluation of Work Completed Solutions to some of the major problems have been developed and described i n t h i s thesis and the solutions implemented, while other problems require further work. The following l i s t summarizes the important c a p a b i l i t i e s of the software package developed to date: 1. Automatic weld parameter s e l e c t i o n . 2. Interactive s e l e c t i o n of table positions and seam welding order. 3. Automatic checking for robot t r a j e c t o r y kinematic f e a s i b i l i t y . 4. Generation of robot l o c a t i o n coordinates for each seam. We can use the output generated by the software at t h i s stage to sim p l i f y the programming of the robot, but some manual programming must s t i l l be done. A simple interference detection scheme has been developed, and implemented i n computer program TESTIN but has not yet been incorpora- ted into AUTOP. - 155 - 8.2 Suggestions f o r Further Work A number of the problem areas discussed i n t h i s thesis have not been considered i n any depth. We can i d e n t i f y a number of areas which require further work before a commercially f e a s i b l e system can be r e a l i z e d : 1. Develop and generate h i g h - l e v e l task-oriented descriptive programming methods for an integrated workstation. This i s necessary i f we wish to control a number of machines (welder, robot, table) as an integrated system from a c e n t r a l computer. To date, there has been l i t t l e standardization of languages or communications protocols for t h i s purpose. 2. Refine the method for specifying the welding procedure i n AUTOP. The. welding procedure should include duty cycle optimization, weld seam o r i e n t a t i o n optimization, and continuous optimization of welding parameters. A continuously repositioning table can be accommodated with modifications to the current software. 3. Develop a p r a c t i c a l path planning or c o l l i s i o n avoidance scheme. I n i t i a l l y , an i n t e r a c t i v e system employing graphical s i m u l a t i o n and c o l l i s i o n d e t e c t i o n should be developed. Eventually a more sophisticated path-planning algorithm should be used to minimize operator input. - 156 - Develop and incorporate real-time seam tracking. As a minimum requirement, the seam tracker should provide p o s i t i o n a l error information to AUTOP i n real-time. AUTOP w i l l then modify the nominal path appropriately. Real-time weld puddle analysis and appropriate weld parameter modification i s a desirable c a p a b i l i t y for ensuring uniformly high weld q u a l i t y , and i s a worthwhile area for future research. Interface the software to a commercial CAD system and/or develop a simple dedicated CAD system for generating object s p e c i f i c a t i o n s from a drawing or prototype. Interface AUTOP to a r e a l welding workstation. This requires h i g h - l e v e l task-oriented programs and real-time host computer/ machine communication. The interface must be customized to each i n s t a l l a t i o n due to lack of standardization. Refine the Expert Welder module, and develop a r e l i a b l e , comprehensive welding data base. Continually evaluate, modify and refine AUTOP as problems are encountered and experience i s gained. - 157 - REFERENCES 1. Ambler, A.P., Popplestone, R.J., Kempf, K.G., "An Experiment in the Offline Programming of Robots", Proc. 12th Int. Symp. on Industrial Robots, Paris 1982. 2. Baer, A., Eastman, C , Henrion, M., "Geometric Modelling: a Survey", Computer-aided Design, Vol.11, No.5, 1979, pp.253-72. 3. Benati, M., Morasso, P., Tagliasco, V., "The Inverse Kinematic Problem for Anthropomorphic Manipulator Arms", Transactions of the ASME, Vol.104, March 1982, pp.110-113. 4. Boyse, J., "Interference Detection Among Solids and Surfaces", Communications of the ACM, Vol.22, No.l, 1979, pp.3-9. 5. Brady, Hollerbach, Johnson, Lozano-Perez, Mason, eds., Robot Motion: Planning and Control, MIT Press, 1982. 6. Braid, I.C., "The Synthesis of Solids Bounded by Many Faces", Communications of the ACM, Vol.18, No.4, 1975, pp.209-216. 7. Brooks, R., "Planning Collision-Free Motions for Pick-and-Place Operations", International Journal of Robotics Research, Vol2, No.4, 1983, pp.19-44. 8. Buchal, R., Sassani, F., Cherchas, D., Duncan, J.P., "Development of an Automatic Welding System for a Welding Robot: Phase I", University of B.C. Research Contract Report for National Research Council, Industrial Materials Research Institute, 1984. 9. Denavit, J., Hartenberg, R.S., "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices", Journal of Applied Mechanics, Vol.22, No.l, 1955, pp.215-221. 10. Fournier, A., Khalil, W., "Coordination and Reconfiguration of Mechanical Redundant Systems", Proc. International Conf. Cybernetics Society, 1977. 11. Gaglio, S., Morasso, P., Tagliasco, V., Zaccaria, R., "Computation of Inverse Kinematics and Inverse Dynamics in Manipulator Arm Control", Proc. 11th Int. Symp. on Industrial Robots, Tokyo, 1981. 12. Gruver, W., Soroka, B., Craig, J., Turner, T., "Evaluation of Commercially Available Robot Programming Languages", Proc. 13th Int. Symp. on Industrial Robots, Chicago, 1983. 13. Hosaka, M., Kimura, F., Kakishita, N., "A Unified Method for Processing Polyhedra", Proc. Information Processing 74, 1974. - 158 - 14. Kempf, K., "Robot Command Languages and A r t i f i c i a l Intelligence", Proc. Robots VI, Detroit, 1982. 15. Konstantinov, M., Markov, M., "Discrete Positions Method in Kinematics and Control of Spatial Linkages", Mechanism and Machine Theory, Vol.15, 1980, pp.47-60. 16. Konstantinov, M., Genova, P., Zahariev, E., "Direct Kinematic Control of Industrial Manipulators and Robots", Proc. 11th Int. Symp. on Industrial Robots, Tokyo, 1981. 17. Konstantinov, M., Markov, M., Nechev, D., "Kinematic Control of Redundant Manipulators", Proc. 11th Int. Symp. on Industrial Robots, Tokyo, 1981. 18. Konstantinov, M., Patarinski, S., "A Contribution to the Inverse Kinematic Problem for Industrial Robots", Proc. 12th Int. Symp. on Industrial Robots, Paris, 1982. 19. Lee, C , Ziegler, M., "A Geometric Approach to Solving the Inverse Kinematics of PUMA Robots", Proc. 13th Int. Symp. on Industrial Robots, Chicago, 1983. 20. Liegeois, A., "Automatic Supervisory Control of the Configuration and Behaviour of Multi-body Mechanisms", IEEE Transactions on Systems, Man and Cybernetics, Vol.SMC-7, No.12, 1977, pp.868- 871. 21. Lozano-Perez, T., Wesley, M., "An Algorithm for Planning Collision- Free Paths Among Polyhedral Obstacles", Communications of the ACM, Vol.22, No.10, 1979, pp.560-570. 22. Lozano-Perez, T., "Spatial Planning: A Configuration Space Approach", IEEE Transactions on Computers, Vol.C-32, No.2, 1983, pp.108-120. 23. Maruyama, K., "A Procedure to Determine Intersections Between Polyhedral Objects", Int. Journal of Computer and Information Sciences, Vol.1, No.3, 1972, pp.255-265. 24. Milenkovic, V., Huang, B., "Kinematics of Major Robot Linkage", Proc. 13th Int. Symp. on Industrial Robots, Chicago, 1983. 25. Paul, R., Shimano, B., Mayer, G., Kinematic "Control Equations for Simple Manipulators", IEEE Transactions on Systems, Man, and Cybernetics, Vol.SMC-11, No.6, June 1981, pp.449-455. 26. Paul, R. Robot Manipulators: Mathematics, Programming and Control, MIT Press, 1981. 27. Requicha, A., "Representations of Solids: Theory, Methods and Systems", Computing Surveys, Vol.12, No.4, 1980, pp.437-465. - 159 - 28. Sata, T., Fumihiko, K., Akio, A., "Robot Simulation System as a Task Programming Tool", Proc. 11th Int. Symp. on Industrial Robots, Tokyo, 1981. 29. Schwartz, J., "Finding the Minimum Distance Between Two Convex Polygons", Information Processing Letters, Vol.13, No.4,5, 1981, pp.168-170. 30. Sjolund, P., Donath, M., "Robot Task Planning: Programming Using Interactive Computer Graphics", Proc. 13th Int. Symp. on Industrial Robots, Chicago, 1983. 31. Tarvin, R., "Considerations for Off-line Programming a Heavy Duty Industrial Robot", Proc. 10th Int. Symp. on Industrial Robots, Milan, 1980. 31. Udupa, S., "Collision Detection and Avoidance in Computer Controlled Manipulators", Proc. 5th Int. Joint Conf. on A r t i f i c i a l Intelligence, Cambridge, 1977. 32. Wesley, M, Lozano-Perez, T., Lieberman, L., Lavin, M., Grossman, D., "A Geometric Modelling System for Automated Mechanical Assembly", IBM Journal of Research and Development, Vol.24, No.l, 1980, pp.64-74. 33. Whitney, D., "Resolved Motion Rate Control of Manipulators and Human Prostheses", IEEE Transactions on Man-Machine Systems, Vol.MMS- 10, No.2, 1969, pp.47-53. 34. Whitney, D., "The Mathematics of Coordinated Control of Prosthetic Arms and Manipulators", ASME Journal of Dynamic Systems, Measurement and Control, Dec. 1972, pp.303-309. - 160 - APPENDIX I SOFTWARE DOCUMENTATION - 161 - APPENDIX 1.1 IMPLEMENTATION SPECIFIC SOFTWARE Subroutines OPEN, SHUT The method of accessing external f i l e s from a FORTRAN program varies from one FORTRAN implementation to another. DEC FORTRAN uses a FORTRAN OPEN subroutine c a l l to open a f i l e , using a DEC s p e c i f i c argument l i s t . In AUTOP, a l l f i l e s are opened and closed by c a l l s to the AUTOP subroutines OPEN and SHUT. The software can then be e a s i l y transported to another system with d i f f e r e n t f i l e management procedures by rewriting the subroutines OPEN and SHUT. - 162 - APPENDIX 1.2 AUTOP PROGRAM LISTING - 163 - L i s t i n g o f AUTOP.FOR a t 1 4 : 3 9 : 0 7 on AUG 9 . 1984 f o r CC1d=DAC0 Page 1 1 C PROGRAM AUTOP 2 3 c THIS MAIN PROGRAM IS FOR THE AUTOMATIC PROGRAMMING OF A 4 C c WELDING ROBOT. D 6 c INPUTS: NONE 7 c 8 c OUTPUTS: NONE 9 c 10 4 4 c p F I L E S READ: FI LI ST ,CADLST .GRAPH 1 1 12 c F I L E S WRITTEN: NONE 13 c 14 c SUBROUTINES C A L L E D : CADFIL 15 c CHECK 16 c CONFIG 17 c DRWELD 18 c EXPERT 19 c EYELOC 20 c INIT 21 c OPEN 22 c RDFILE 23 c SCENE 24 c SHUT 25 c WELSET 26 c 27 c ' I G L ' GRAPHICS SUBROUTINES: CMCLOS 28 c CMOPEN 29 c GETPIK 30 c GRSTOP 31 c GRSTRT 32 c VUP3D 33 c VWPT3D 34 c WIND3D 35 c 36 c 37 c- 38 c 39 BYTE FI L I S T ( 2 0 ) . C A D L S T ( 2 0 ) . W P C L S T ( 2 0 ) . W K S T A T ( 2 0 ) 40 BYTE T A B L E ( 2 0 ) . S T O V A R ( 2 0 ) , G R A P H ( 2 0 ) . O B J L S T ( 2 0 ) . R O B L S T ( 2 0 ) 41 BYTE L0CSET(20) ,WPARAM(20) ,WPCDAT(20) 42 BYTE B E L ( 3 ) , Y E S . N O . A N S . A N S 2 43 c 44 DOUBLE PRECISION H O T ( 4 . 4 ) . H O W ( 4 , 4 ) , H T W ( 4 , 4 ) 45 REAL V E R T ( 1 0 0 0 . 3 ) . F A C E S ( 1 0 0 . 4 ) , H ( 4 . 4 ) , E Y E P O S ( 3 ) , V R P ( 3 ) . V A R ( 6 ) 46 - REAL S H O T ( 4 , 4 ) , S H 0 W ( 4 , 4 ) , H 0 0 ( 4 , 4 ) , H 6 G ( 4 . 4 ) , I N H 0 0 ( 4 , 4 ) , I N H 6 A ( 4 . 4 ) 47 REAL S H T W ( 4 . 4 ) . D E L T A 48 REAL W I N S I Z ( 6 ) , V U P 0 ( 3 ) , V U P 1 ( 3 ) 49 REAL ALPHA(6 ) , L E N ( 6 ) , D I S T ( 6 ) , R M I N ( 6 ) , R M A X ( 6 ) 50 INTEGER IC0NF(6 ) , IMAX 51 c 52 INTEGER I 0 B J ( 1 0 , 2 ) , I F A C E ( 1 0 0 , 2 ) , I F L A G ( 1 0 0 0 ) , I D I R ( 1 ) 53 INTEGER IPICK(IOO) 54 c 55 COMMON / D A T A / V E R T , F A C E S , I F A C E , I O B J , I FLAG 56 COMMON / V I E W / E Y E P O S , V R P 57 COMMON /WINSIZ/WINSIZ 58 COMMON / F I L E / F I L I S T - 164 - L i s t i n g o f AUTOP.FOR a t 1 4 : 3 9 : 0 7 on AUG 9 , 1984 f o r CC1d=DAC0 Page 2 59 COMMON / C A D L S T / C A D L S T 60 COMMON /WPCLST/WPCLST 61 COMMON /WKSTAT/WKSTAT 62 COMMON / T A B L E / T A B L E 63 COMMON /STOVAR/STOVAR 64 COMMON /GRAPH/GRAPH 65 COMMON / O B J L S T / O B J L S T 66 COMMON /ROBLST/ROBLST 67 COMMON / L O C S E T / L O C S E T 68 COMMON /MATRIX/HOT,HOW,SHOT,SHOW 69 COMMON / R O B O T / N R D O F . A L P H A , L E N , D I S T , R M I N , R M A X , H 0 O , H 6 G , I N H O O , I N H 6 A 70 COMMON /CONFIG/ICONF 71 COMMON /TABDAT/HTW,SHTW 72 COMMON / T O L R O T / I M A X , D E L T A 73 COMMON / E X P E R T / W P A R A M , V O L T . W S I Z E , F E E D , G F R . I A L P H A , I B E T A , I STOUT 74 COMMON /NSEAMS/NSEAMS 75 C 76 DATA B E L / 3 * 7 / , Y E S / 8 9 / . N O / 7 8 / 77 DATA V U P 0 / 3 * 0 / , V U P 1 / 0 . , 0 . , 1 . / 78 C 79 c****« ENTER THE NAME OF THE F I L E LIST ' F I L I S T ' . 80 C 81 WRITE(5 .10 ) 82 10 FORMATC ENTER THE NAME OF THE F I L E L I S T . . . ' , $ ) 83 CALL R D F I L E ( 5 , 1 . F I L I S T . 2 0 , I E N D ) 84 C 85 C * * * » * READ NAMES OF SYSTEM FILES FROM FILE ' F I L I S T ' . 86 C 87 CALL O P E N ( 1 . F I L I S T . 4 , 1 ) 88 CALL R D F I L E ( 1 , 1 , C A D L S T , 2 0 , I END) 89 CALL R D F I L E ( 1 . 2 . W P C L S T , 2 0 . I E N D ) 90 CALL R D F I L E ( 1 , 3 , W K S T A T , 2 0 , I E N D ) 91 CALL R D F I L E ( 1 , 4 , T A B L E , 2 0 , I E N D ) 92 CALL R D F I L E ( 1 , 5 , S T O V A R , 2 0 , I E N D ) 93 CALL R D F I L E ( 1 , 6 . G R A P H . 2 0 , I E N D ) 94 CALL RDFILE( 1 , 7 , O B J L S T , 2 0 , I END) 95 CALL R D F I L E ( 1 , 8 , R O B L S T , 2 0 , I END) 96N CALL RDFILE( 1 , 9 , L O C S E T . 2 0 , IEND) 97 CALL R D F I L E ( 1 , 1 0 , W P A R A M , 2 0 . I E N D ) 98 CALL SHUT(1) 99 C 100 C * * * » * GENERATE THE WORKPIECE SEAM FILES FROM THE SEAM CAD FILES 101 C 102 CALL SETUP 103 C 104 C 105 c****» OPEN CAD F I L E L I S T , ' C A D L S T ' . 106 C 107 CALL O P E N ( 1 . C A D L S T . 4 , 1 ) 108 C 109 C * * * » * SKIP THE FIRST RECORD AND READ THE NUMBER OF SEAMS, 'NSEAMS' . 1 10 C 111 READ( 1 , * ) 112 READ( 1,*) NSEAMS 113 CALL SHUT(1) 114 C 115 c»**** OPEN GRAPHICS PARAMETERS F I L E , ' G R A P H ' . 1 16 C - 165 - L i s t i n g o f AUTOP.FOR a t 1 4 : 3 9 : 0 7 on AUG 9 , 1984 f o r CC1d=DAC0 Page 3 117 CALL OPEN(1.GRAPH,4,1) 118 READ(1,*) 119 C 120 c****» READ CURRENT VIEW REFERENCE POINT, 'VRP'. 121 c 122 READ(1,* ) (VRP(I),1 = 1,3) 123 READ( 1,*) 124 c 125 c * * * * » READ CURRENT WINDOW PARAMETERS. 'WINSIZ'. 126 c 127 READ( 1 ,* ) (WINSIZ(I),1 = 1,6) 128 CALL SHUTO) 129 c 130 c 131 c * * » * * INITIALIZE THE ROBOT PARAMETERS 132 c 133 CALL INIT(NSEAMS) 134 c 135 c 136 c**»«* INITIALIZE WELDER SETUP 137 c 138 CALL WELSET(WPCLST.WKSTAT) 139 c 140 c 141 c**«»» INITIALIZE IGL GRAPHICS SYSTEM 142 c 143 CALL GRSTRT(4027.1) 144 CALL VUP3D(VUPO,VUP1) 145 c 146 c * * * » * DEFINE 3D WINDOW AND VIEWPORT. 147 c 148 CALL WIND3D(WINSIZ(1),WINSIZ(2).WINSIZ(3),WINSIZ(4),WINS IZ(5) 149 * WINSIZ(6)) 150 CALL VWPT3D(0.,100.,0.,100.,0.,100.) 151 c 152 c***»* INITIALIZE THE WELDED SEAM COUNT 153 c 154 IC0UNT=1 155 c 156 c * * * * * DEFINE EYEBALL LOCATION. 157 c 158 CALL CMCLOS 159 CALL EYELOC(VRP,EYEPOS) 160 CALL CMOPEN 161 c 162 c 163 c * * * » * CONVERT EXTERNAL CAD OBJECT DESCRIPTION FILES INTO INTERNAL 164 c**»** ARRAYS FOR FAST PROCESSING. 165 c 166 CALL CMCLOS 167 CALL CADFIL 168 CALL CMOPEN 169 c 170 c 171 DRAW COMPLETE SCENE, INCLUDING SEAMS 172 c 173 CALL SCENE 174 c - 166 - L i s t i n g o f A U T O P . F O R a t 1 4 : 3 9 : 0 7 on A U G 9 , 1 9 8 4 f o r CCid=DACO P a g e 4 1 7 5 C 1 7 6 C * * U N T I L ( A L L S E A M S P I C K E D ) 1 7 7 c 1 7 8 6 0 0 C O N T I N U E 1 7 9 C 1 8 0 c** U N T I L ( N O M O R E C H A N G E S ) 1 8 1 c 1 8 2 7 0 C O N T I N U E 1 8 3 c 1 8 4 C A L L C M C L O S 1 8 5 W R I T E ( 5 , 1 6 1 ) 1 8 6 1 6 1 F O R M A T ( $ , ' D O Y O U W I S H T O C H A N G E T H E E Y E B A L L P O S I T I O N ? ( Y / N ) 1 8 7 R E A D ( 5 . 1 7 0 ) A N S 1 8 8 C 1 8 9 I F ( A N S . N E . Y E S ) G O T O 5 0 1 9 0 C A L L E Y E L O C ( V R P . E Y E P O S ) 1 9 1 5 0 C O N T I N U E 1 9 2 C 1 9 3 W R I T E ( 5 . 1 6 2 ) 1 9 4 1 6 2 F O R M A T ( $ , ' DO Y O U W I S H T O R E P O S I T I O N T A B L E ? ( Y / N ) ') 1 9 5 R E A D ( 5 , 1 7 0 ) A N S 2 1 9 6 1 7 0 F O R M A T ( A 1 ) 1 9 7 C A L L C M O P E N 1 9 8 C 1 9 9 C * * I F ( E I T H E R A N S W E R I S Y E S ) T R A N S F O R M A N D R E D R A W T H E S C E N E . 2 0 0 c 2 0 1 I F ( A N S . N E . Y E S . A N D . A N S 2 . N E . Y E S ) G O T O 6 0 2 0 2 C A L L S C E N E 2 0 3 c 2 0 4 c** I F ( I C O U N T . G T . 1 ) D R A W A L L P R E V I O U S L Y P I C K E D W E L D S . 2 0 5 c 2 0 6 I F ( I C O U N T . E O . 1 ) GO T O 8 0 2 0 7 D O 8 K = 1 . I C O U N T - 1 2 0 8 c 2 0 9 c*** * * F I L L I N W E L D . 'SHOW' I S A H O M O G E N E O U S T R A N S F O R M A T I O N 2 1 0 c*** * * M A T R I X R E L A T I N G T H E W O R K P I E C E T O W O R L D C O O R D I N A T E S . 2 1 1 C * * * * » ' I P I C K ' I S A N A R R A Y O F P R E V I O U S L Y P I C K E D W E L D S . 2 1 2 C 2 1 3 C A L L D R W E L D ( S H O W , I P I C K ( K ) ) 2 1 4 C 2 1 5 8 C O N T I N U E 2 1 6 C 2 1 7 c** E N D I F 2 1 8 c 2 1 9 8 0 C O N T I N U E 2 2 0 C 2 2 1 G O T O 7 0 2 2 2 C 2 2 3 C * * E N D I F 2 2 4 C 2 2 5 c** E N D U N T I L 2 2 6 c 2 2 7 6 0 C O N T I N U E 2 2 8 C 2 2 9 C * * * » * P I C K J H E N E X T S E A M T O B E W E L D E D 2 3 0 C 2 3 1 C 2 3 2 c** U N T I L ( P I C K S U C C E S S F U L ) - 167 - L i s t i n g o f AUTOP.FOR a t 14:39:07 o n AUG 9, 1984 f o r CC1d=DAC0 Page 5 233 C 234 611 CONTINUE 235 C 236 CALL CMCLOS 237 WRITE(5,260) 238 260 FORMAT(' PICK SEAM TO BE WELDED NEXT- HIT ANY KEY TO PICK' ) 239 CALL CMOPEN 240 C 241 c»»»** PICK SEAM WITH CROSSHAIRS. GETPIK RETURNS THE SEAM ID, 242 c***»* 'IDSEAM'. 243 c 244 CALL GETPIK(IDSEAM,IDPIK) 245 c 246 c** IF (PICK UNSUCCESSFUL) 247 c 248 IF(IDSEAM.NE.0) GO TO 190 249 c 250 CALL CMCLOS 251 WRITE(5, 191 ) BEL 252 191 FORMAT($, ' PICK UNSUCCESSFUL-TRY AGAIN (HIT RETURN)' ,3A1 253 CALL CMOPEN 254 GO TO 610 255 C 256 C** END IF 257 C 258 190 CONTINUE 259 C 260 C 261 CHECK IF THE SEAM HAS ALREADY BEEN PICKED. 262 c 263 CALL CMCLOS 264 c 265 c**»* * 'IPICK' IS AN ARRAY CONTAINING ALL PICKED SEAMS. 266 c*»*** 'ICOUNT' IS THE NUMBER OF SEAMS ALREADY PICKED. 267 c* * * * '  'IDSEAM' IS THE CURRENT SEAM ID 268 c****» 'IPREV.' IS A STATUS FLAG. IF 'IDSEAM' ALREADY PICKED. IPREV^ 269 270 CALL CHECK(IPICK,ICOUNT,IDSEAM,IPREV) 271 c 272 CALL CMOPEN 273 c 274 c** IF (SEAM PREVIOUSLY PICKED) 275 c 276 IF(IPREV.EQ.O) GO TO 350 277 c 278 CALL CMCLOS 279 WRITE(5,130) BEL 280 130 FORMAT($,' SEAM ALREADY PICKED- TRY AGAIN ',3A1) 281 CALL CMOPEN 282 c 283 GO TO 610 284 c 285 c** ELSE (PICK SUCCESSFUL) 286 c 287 350 CONTINUE 288 C 289 c * * * * •» ADD 'IDSEAM' TO ARRAY OF PICKED SEAMS, 'IPICK' 290 c - 168 - L i s t i n g of AUTOP.FOR a t 14:39:07 on AUG 9, 1984 f o r CC1d=DAC0 Page 6 291 IPICK(ICOUNT)=IDSEAM 292 C 293 GO TO 360 294 C 295 C** END IF 296 C 297 610 CONTINUE 298 GO TO 611 299 C 300 C** END UNTIL 301 C 302 360 CONTINUE 303 C 304 CALL CMCLOS 305 WRITE(5,160) IDSEAM 306 160 FORMAT($, ' IDSEAM=',I2) 307 C 308 C***** FIND THE CORRESPONDING SEAM FILE 'WPCDAT'. 309 C 310 CALL 0PEN(2,WPCLST,4,1) 311 CALL RDFILE(2,IDSEAM+2,WPCDAT,20,IEND) 312 CALL SHUT(2) 313 C 314 C«**** ENTER DIRECTION IN WHICH SEAM IS TO BE WELDED. 315 C 316 290 CONTINUE 317 WRITE(5,140) 318 140 FORMAT($,' ENTER WELD DIRECTION- (0=NORMAL, 1=REVERSE) ') 319 READ(5,* ) IDIR 320 C 321 C***** CALL THE EXPERT WELDER MODULE TO FIND THE TORCH ANGLES AND 322 C***»* STICKOUT. AND THE WELDING PARAMETERS. 323 C 324 CALL EXPERT(WPCDAT) 325 C 326 C***** SEARCH FOR A FEASIBLE ROBOT SOLUTION. ISTAT: 0=OK, 327 c***«* 1=R0B0T ENVELOPE EXCEEDED, 2=R0B0T JOINT LIMITS EXCEEDED 328 C 329 CALL CONFIG(WPCDAT,IDSEAM,IDIR,ISTAT) 330 C 331 CALL CMOPEN 332 C 333 C** IF (ROBOT JOINT LIMITS EXCEEDED) REPOSITION WORKPIECE AND RETRY. 334 C 335 IF(ISTAT.EQ.O) GO TO 285 336 IF(ISTAT.EO.1 ) GO TO 286 337 " WRITE(5.850) 338 850 FORMAT(' JOINT LIMITS EXCEEDED-TRY AGAIN') 339 C 340 C***** REMOVE SEAM FROM PICKED LIST 341 C 342 IPICK(ICOUNT)=0 343 C 344 GO TO 600 345 C 346 C** OR IF (ROBOT ENVELOPE EXCEEDED) REPOSITION WORKPIECE AND RETRY. 347 C 348 286 CONTINUE - 169 - L i s t i n g of AUTOP.FOR at 14:39:07 on AUG 9, 1984 for CC1d=DAC0 Page 7 349 WRITE(5,851) 350 851 F0RMAT(' ROBOT ENVELOPE EXCEEDED-REPOSITION') 351 C 352 C***** REMOVE SEAM FROM PICKED LIST 353 C 354 IPICK(IC0UNT)=O 355 C 356 GO TO 600 357 285 CONTINUE 358 C 359 C** END IF 360 C 361 C** OR IF (IDIR NOT O OR 1) RE-ENTER IDIR 362 C 363 IF(ISTAT.NE.1) GO TO 295 364 GO TO 290 365 295 CONTINUE 366 C 367 C** ELSE (PATH OK) 368 C** END IF 369 C 370 C****» FILL IN THE WELDED SEAM 'IDSEAM'. 'SHOW' IS A HOMOGENEOUS 371 C***** TRANSFORMATION MATRIX RELATING THE WORKPIECE FRAME TO THE WORLD 372 C***** COORDINATE FRAME. 373 C 374 CALL ORWELD(SHOW,IDSEAM) 375 C 376 C**«** INCREMENT THE WELDED SEAM COUNTER 377 C 378 IC0UNT=IC0UNT+1 379 IF(ICOUNT.LE.NSEAMS) GO TO 600 380 C 381 C** END UNTIL 382 C 383 C 384 CALL GRSTOP 385 C 386 STOP 387 END - 170 - L i s t i n g of ARCLEN.FOR a t 14:39:10 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 2 SUBROUTINE ARCLEN(R,DELS) 3 C 4 C THIS SUBROUTINE CALCULATES ARC LENGTH S. 5 C 6 C INPUTS: 7 c R(3,3)=P0SITI0N VECTORS AT POINTS 1-1. 8 c OUTPUT: 9 c DELS=ARC LENGTH FROM POINT I TO POINT 10 c 1 1 12 c 13 c 14 REAL R(3,3),X.Y.Z.LASTX,LASTY,LASTZ.DELS 15 DOUBLE PRECISION DELX. DELY, DELZ 16 c 17 X = R(3. 1 ) 18 Y=R(3,2) 19 Z=R(3.3) 20 LASTX=R(2,1) 2 1 LASTY=R(2.2) 22 LASTZ=R(2,3) 23 DELX=X-LASTX 24 DELY=Y-LASTY 25 DELZ=Z-LASTZ 26 DELS=DSQRT(DELX**2+DELY**2+DELZ**2) 27 RETURN 28 END - 1 7 1 - L i s t i n g of CADFIL.FOR at 14:39:12 on AUG 9, 1984 for CC1d=DAC0 Page 1 V 2 SUBROUTINE CADFIL 3 C 4 C THIS SUBROUTINE CONVERTS CAD DATA FILE INTO ARRAYS IN MAIN 5 C MEMORY FOR FAST PROCESSING. 6 C 7 C INPUTS: NONE 8 C 9 C OUTPUTS: NONE 10 C 11 C COMMON BLOCKS CHANGED: /DATA/ 12 C 13 C FILES READ: OBJLST, FILES LISTED IN OBJLST 14 C 15 C OBJLST- CONTAINS LIST OF SEPARATE OBJECT CAD FILES 16 C IE. FILE1- WORKPIECE CAD FILE 17 C FILE2- TABLE CAD FILE (MOVING PART) 18 C FILE3- TABLE CAD FILE (FIXED PART) 19 C FILE4- ROBOT FOREARM 20 C FILE5- ROBOT HAND AND TOOL 21 C 22 C DATA ARRAYS (COMMON): 23 C VERT(1000,3)- VERTEX LIST Vx.Vy.Vz 24 C FACES(100,4)- FACE PARAMETERS U=(Ux,Uy,Uz),P 25 C WHERE U=SURFACE NORMAL 26 C P=DISTANCE TO ORIGIN 27 C IOBJ(10,2)- LOCATION OF FIRST FACE,NUMBER OF FACES 28 C IN FACE INDEX ' IFACE' 29 C IFACE(100,2)- LOCATION OF FIRST VERTEX,NUMBER OF 30 C VERTICES IN VERTEX INDEX 'IVERT' 31 C 32 C FOR EACH FACE, THE ROUTINE REPEATS THE FIRST VERTEX AT THE END OF 33 C THE VERTEX LIST SINCE EVERY FACE IS A CLOSED POLYGON. THIS 34 C SIMPLIFIES LATER PROCESSING. 35 C IE. FOR A FACE WITH 3 VERTICES. V1.V2.V3. THE VERTEX LIST WOULD 36 C CONTAIN VI . V2, V3. V1. 37 C 38 C SUBROUTINES CALLED: OPEN 39 C RDFILE 40 C SHUT 41 C 42 c»«*.**»,««»*»«*«*****.«.**.»***.**»«**»«*»«««*****»*******«****.**..»«»***»»** 43 C 44 BYTE FILE(20),0BJLST(20) 45 REAL VERT(1000.3).FACES(100,4) 46 INTEGER IOBJ(10,2),IFACE(100,2) 47 C 48 COMMON /OBJLST/OBJLST 49 COMMON /DATA/VERT.FACES,IFACE,IOBJ 50 C 51 C***»* INITIALIZE COUNTERS 52 C 53 NVERT=1 54 NFACE=1 - 55 N0BJ=1 56 C 57 C** REPEAT UNTIL THERE ARE NO MORE OBJECT FILES 58 C - 172 - L i s t i n g o f CADFIL.FOR at 14:39:12 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 CONTINUE OPEN OBJECT LIST CALL OPEN(1.OBJLST,4,1) FIND OBJECT FILE CALL RDFILE(1,NOBJ,FILE.20,IEND) CALL SHUT(1 ) IF THE END OF THE OBJECT LIST IS REACHED, GO TO END IF (IEND.EO.1.) GO TO 500 OPEN THE CURRENT OBJECT FILE CALL OPEN(1.FILE,4,1) STORE THE LOCATION OF THE FIRST FACE OF OBJECT 'NOBJ' IOBJ(NOBJ,1)=NFACE INITIALIZE FACE COUNTER NUFACE=0 3EAT UNTIL THERE ARE ND MORE FACES CONTINUE READ THE FACE ID,FACE PARAMETERS AND NUMBER OF VERTICES READ( 1 , * ) ID,(FACES(NFACE.J).J=1,4),NUVERT IF ID=0, THERE ARE NO MORE FACES IF (ID.EQ.O) GO TO 150 STORE THE LOCATION OF THE FIRST VERTEX, AND THE NUMBER OF VERTICES I FACE(NFACE, 1) = NVERT IFACE(NFACE,2)=NUVERT READ AND STORE THE COORDINATES OF THE VERTICES. DO 110 J=1,NUVERT READ( 1 ,*) ITEMP,(VERT(NVERT,K),K=1.3) NVERT=NVERT+1 CONTINUE REPEAT THE FIRST VERTEX AT THE END OF THE LIST DO 115 J=1 ,3 VERT(NVERT,J)=VERT(NVERT-NUVERT,J) 59 105 60 C 61 C 62 63 c 64 65 c 66 c**»** 67 c 68 69 70 c 71 72 c 73 74 c 75 c * * * * * 76 c 77 78 c 79 c***»* 80 c 81 82 c 83 84 c 85 86 c 87 C** R 88 c 89 100 90 c 91 c 92 c***«* 93 c 94 95 c 96 97 c 98 99 c 100 c***«* 101 102 c 103 104 105 c 106 c* * * * * 107 c 108 109 1 10 1 1 1 110 112 c 113 c* * » * * 114 c 1 15 1 16 - 173 - L i s t i n g of CADFIL.FOR a t 14:39:12 on AUG 9, 1984 for CC1d=DAC0 Page 3 117 115 CONTINUE 118 NVERT=NVERT+1 1 19 C 120 C***»* INCREMENT THE FACE POINTER AND THE FACE COUNTER 121 C 122 NFACE=NFACE+1 123 NUFACE=NUFACE+1 124 C 125 GO TO 100 126 C 127 150 CONTINUE 128 C 129 CALL SHUT( 1 ) 130 C 131 C****» STORE THE NUMBER OF FACES OF OBJECT 'NOBJ' 132 C 133 I0BJ(N0BJ,2)=NUFACE 134 C 135 C****» INCREMENT THE OBJECT COUNTER, AND REPEAT UNTIL THERE ARE NO MORE 136 C***** OBJECTS 137 C 138 N0BJ=N0BJ+1 139 C 140 GO TO 105 141 C 142 500 CONTINUE 143 C 144 RETURN 145 END - 174 - L i s t i n g o f CALC5.FOR a t 14:39:13 on AUG 9. 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE CALCS(CPHI,CTHE) 2 C 3 C THIS SUBROUTINE CALCULATES THE ORIENTATION FACTORS ' C P H I ' . 4 C ' C T H E ' AS FUNCTIONS OF THE TORCH ORIENTATION RELATIVE 5 C TO THE GRAVITY VECTOR. 6 C 7 DOUBLE PRECISION HOT(4,4),HOW(4,4 ) 8 C0MM0N/C0EF/TW(3) .AW(3) ,T(3) .A(3) 9 COMMON/MATRIX/HOT,HOW.SHOT(4.4),SHOW( 4 ,4 ) 10 C 11 C0NV=180./3.14159 12 C 13 C * * * * * ' C O N V CONVERTS ANGLES IN RADIAN TO DEGREES 14 C 15 C * * * * » ROTATE THE SEAM TANGENT AND NORMAL VECTORS INTO THE WORLD 16 C » » * « * COORDINATE FRAME. TW(3), AW(3) ARE THE COMPONENTS OF TW.AW 17 c***** ALONG THE GRAVITY VECTOR. 18 C 19 CALL ROTATE(SHOW.T.TW) 20 CALL ROTATE(SHOW,A,AW) 21 C 22 C * * * « * ' P H I ' IS THE ANGLE BETWEEN +Z (UPWARD) AXIS AND THE TANGENT 23 C * * » * * TO THE WELD SEAM 24 C 25 PHI=AC0S(TW(3)/SQRT(TW(1 )**2+TW(2)**2+TW(3 ) * * 2 ) ) 26 C 27 C * * * » * ' THETA ' IS THE ANGLE BETWEEN +2 (UPWARD) AXIS AND THE NORMAL 28 C * * * » * TO THE WELD SEAM 29 C 30 THETA=AC0S(AW(3)/SQRT(AW(1) * « 2 + A W ( 2 ) * * 2 + AW(3)**2)) 31 PHI=PHI*CONV 32 C 33 C * * * « * DETERMINE ORIENTATION FACTORS 'CPH I ' AND 'CTHE ' USING - 34 c * « * * » THE ORIENTATION ANGLES 35 C 36 THETA=THETA *CONV 37 CPHI=5.889*(10.0** -4. )*PHI+o .95 38 I F ( P H I . G T . 9 0 . 0 ) CPH I= -5 .89* (10 .0 * * -4 . ) *PH I+1 .05 39 CTHE= -5 .556* (10 .O* * -4 . ) *THETA+1. 40 , I F (THETA.GT .90 .0 ) CTHE = - 1 . 111 * ( 1 0 . O * * - 3 . )*THETA+1 41 C 42 RETURN 43 END - 175 - L i s t i n g of CHECK.FOR a t 14:39:13 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 2 SUBROUTINE CHECK(IPICK,ICOUNT,IDSEAM,IPREV) 3 C 4 C THIS SUBROUTINE CHECKS IF A SEGMENT HAS ALREADY BEEN 5 C PICKED. 6 C 7 C INPUTS: IPICK(IOO)- ARRAY OF PICKED SEGMENT ID INTEGERS 8 C 9 C ICOUNT- TOTAL NUMBER OF SEGMENTS 10 c 1 1 c IDSEAM- ID OF SEGMENT TO BE CHECKED 12 c 13 c OUTPUTS: IPREV- 0 IF SEAM NOT PREVIOUSLY PICKED 14 c 1 OTHERWIZE 15 c 16 17 c 18 INTEGER IPICK(IOO) 19 c 20 IPREV=1 21 c 22 DO 100 1=1,ICOUNT 23 I F ( I P I C K ( I ) .EQ.IDSEAM) GO TO 200 24 100 CONTINUE 25 c 26 IPREV=0 27 c 28 200 CONTINUE 29 c 30 RETURN 31 END - 176 - L i s t i n g of CONFIG.FOR at 14:39:33 on AUG 9, 1984 for CC1d=DAC0 Page 1 1 SUBROUTINE CONFIG(WPCDAT,IDSEAM.IDIR,ISTAT) 2 C 3 C THIS SUBROUTINE TESTS ALL CONFIGURATIONS OF 4 C THE ROBOT UNTIL A FEASIBLE SOLUTION IS FOUND. 5 C 6 C INPUTS: WPCDAT- FILE NAME OF CURRENT SEAM TO BE SOLVED 7 C IDSEAM- ID OF CURRENT SEAM TO BE SOLVED 8 C IDIR- DESIRED DIRECTION OF TRAVEL. 9 C 10 C OUTPUTS: ISTAT- FEASIBILITY STATUS 11 C 0=FEASIBLE TRAJECTORY FOUND 12 C 1=R0B0T ENVELOPE EXCEEDED 13 C 2=R0B0T JOINT LIMIT EXCEEDED 14 C 15 C COMMON BLOCKS CHANGED: /CONFIG/ 16 C 17 C VALUES OF IC0NF(6): (FOR PUMA 560 TYPE RQBOT) 18 C 19 C O O O O O O L A NF MODIFY CONFIGURATION IN THIS 20 C 0 0 0 1 0 0 L A F DIRECTION ONLY 21 C 0 0 1 0 O 0 L B NF 22 C 0 0 1 1 0 0 L B F 23 C 1 1 1 0 0 0 R A NF 24 C 1 1 1 1 0 0 R A F 25 C 1 1 0 0 0 0 R B NF V 26 C 1 1 0 1 0 0 R B F 27 C 28 C WHERE: R- RIGHT HANDED ARM CONFIGURATION 29 C L- LEFT HANDED ARM CONFIG. 30 C 31 C A- ELBOW ABOVE CONFIGURATION 32 C B- ELBOW BELOW CONFIG. 33 C 34 C NF- WRIST NOT FLIPPED 35 C F- WRIST FLIPPED 36 C 37 C SUBROUTINES CALLED: WORPOS 38 C 39 C 40 c * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * » . * * * * » * * * * * » * » * * « « « * * 41 C 42 BYTE WPCDAT(20) 43 REAL ALPHA(6),LEN(6),DIST(6),RMIN(6),RMAX(6),H0O(4,4), 44 * H6G(4,4),INH0O(4,4).INH6A(4,4) 45 INTEGER IC0NF(6) 46 C 47 COMMON /ROBOT/NRDOF,ALPHA,LEN,DIST,RMIN,RMAX,H0O.H6G,INHOO,INH6A 48 COMMON /CONFIG/ICONF 49 C 50 1=2 51 J = 2 52 K = 2 53 C 54 100 CONTINUE 55 C 56 C * * * * » TEST CURRENT CONFIGURATION FOR FEASIBILITY 57 C 58. CALL WORPOS(WPCDAT,IDSEAM,IDIR,ISTAT) - 177 - L i s t i n g of CONFIG.FOR a t .14:39:33 on AUG 9. 1984 f o r CC1d=DACO Page 59 C 60 c***«* IF A FEASIBLE SOLUTION WAS FOUND, GO TO 61 c 62 IF(ISTAT.EQ.O) GO TO 200 63 c 64 c***«* ELSE TRY A DIFFERENT CONFIGURATION UNTIL 65 c***** CONFIGURATIONS HAVE BEEN TRIED 66 c 67 c*«»** REVERSE WRIST FLIP CONFIGURATION 68 c 69 IF(IC0NF(4).EQ.O) GO TO 10 70 IC0NF(4)=0 71 GO TO 20 72 10 CONTINUE 73 IC0NF(4)=1 74 20 CONTINUE 75 C 76 1 = 1-1 77 IF(I.GT.0) GO TO 100 78 C 79 1=2 80 c 81 c****» REVERSE ELBOW ABOVE/BELOW CONFIGURATION 82 c 83 IF(IC0NF(3).EO.O) GO TO 11 84 IC0NF(3)=0 85 GO TO 21 86 11 CONTINUE 87 IC0NF(3)=1 88 21 CONTINUE 89 c 90 J=d-1 91 IF(J.GT.O) GO TO 100 92 C 93 J = 2 94 C 95 c»**»* REVERSE LEFT/RIGHT ARM CONFIGURATION 96 c 97 300 CONTINUE 98 IF(ICONF(1).EQ.0) GO TO 12 99 ICONF(1)=0 100 IC0NF(2)=0 101 GO TO 22 102^ 12 CONTINUE 103 ICONFC 1 ) = 1 104 IC0NF(2)=1 105 22 CONTINUE 106 C 107 K=K-1 108 IF(K.GT.O) GO TO 100 109 IF(K.EQ.O) GO TO 300 1 10 C 1 1 1 200 CONTINUE 112 C 113 RETURN 1 14 END - 178 - ' L i s t i n g of DOTPRD.FOR a t 1 4 : 3 9 : 3 5 o n AUG 9 , 1984 for CC1d=DAC0 P a g e 1 1 SUBROUTINE DOTPRD(V1,V2.PROD) 2 C 3 C THIS SUBROUTINE FINDS THE DOT PRODUCT OF TWO 4 C V 1 ( 3 ) , V 2 ( 3 ) . 5 C e C INPUTS: V 1 ( 3 ) , V 2 ( 3 ) - TWO VECTORS 7 C 8 C OUTPUTS: PROD- THE DOT PRODUCT PR0D=V1.V2 9 C 10 11 REAL V 1 ( 3 ) . V 2 ( 3 ) 12 C 13 P R 0 D = V 1 ( 1 ) * V 2 ( 1 ) + V 1 ( 2 ) * V 2 ( 2 ) + V 1 ( 3 ) * V 2 ( 3 ) 14 C 15 RETURN 16 END - 179 - L i s t i n g o f DRAXES.FOR a t 14:39:35 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE DRAXES 2 C 3 C THIS SUBROUTINE DRAWS THE WORLD COORDINATE AXES. 4 C 5 c*„ft** .******** . . . ******* .*** . . . . *»** ,»*»»,»*»*•* , , ***»» 6 C 7 CALL M0VE3D(O..0.,0.) 8 CALL DRAW3D(1000..0..0.) 9 CALL M0VE3D(O.,0..0.) 10 CALL DRAW3D(0.,1000.,0.) 11 CALL M0VE3D(0.,0.,0.) 12 CALL DRAW3D(0.,0..1000.) 13 C 14 RETURN 15 END - 180 - Listing o f DRPANL.FOR at 14:39:35 on AUG 9, 1984 f o r CCid=DAC0 Page 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 33 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 C C C C C C C C C C C C C C C C C C c c c c c c c c c c c c c**> c SUBROUTINE DRPANL(H,NOBJ) THIS SUBROUTINE DRAWS OBJECT 'NOBJ', TRANSFORMED BY MATRIX 'H'. THE OBJECT IS REPRESENTED BY A SOLID POLYHEDRON, DRAWN AS A SET OF SUPERIMPOSED FACE PANELS. INPUTS: H(4,4)- HOMOGENEOUS TRANSFORMATION MATRIX DEFINING LOCATION OF OBJECT IN VIEW VOLUME. NOBJ- IDENTITY OF OBJECT TO BE DRAWN OUTPUTS: NONE COMMON BLOCKS ACCESSED: /DATA/,/VIEW/ COMMON BLOCKS CHANGED: NONE FILES READ OR WRITTEN: SUBROUTINES CALLED: 'IGL' SUBROUTINES: C C C****» c****» c c c***»* c***** c NONE DOTPRD LINDIS ROTAT TRANS APPEAR DRAW3D M0VE3D PANL3D P0LY3D REMOVE REAL VERT(1000.3),FACES(100.4),V(3),V1(3),H(4,4).V1ST(3) REAL VX(4).VY(4),VZ(4) .P1 (3) ,P2(3) ,01(3),02(3).UFACE1(3) REAL A1(3),A2(3),B1(3) .62(3) REAL EYEP0S(3) , VRP ( 3 ) , US IGHT ( 3 ) , UF ACE ( 3 ) , WE IGHT ( 20) , PO ( 3 ) REAL VX3(3),VY3(3),VZ3(3) ,VX4(4),VY4(4),VZ4(4) REAL VX5(5),VY5(5),VZ5(5 ) ,VX6(6 ) .VY6(6 ) ,VZ6(6 ) REAL VX7(7),VY7(7 ),VZ7(7) ,VX8(8) .VY8(8) ,VZ8(8) REAL VX9(9),VY9(9),VZ9(9),VX10(10),VY10(10),VZ10(10) [ INTEGER IOBJ(10,2).IFACE(100.2) ' INTEGER IFLIST(20),IORDER(20) COMMON /DATA/VERT,FACES.I FACE,IOBJ -COMMON /VIEW/EYEPOS.VRP FIND STARTING LOCATION OF FIRST FACE. 'IFASTA', AND THE NUMBER OF FACES. 'NFACES', FROM THE OBJECT PARAMETER ARRAY 'IOBJ'. IFASTA=IOBJ(NOBJ,1) NFACES=I0BJ(N0BJ,2) CALCULATE THE LINE OF SIGHT VECTOR 'USIGHT' EYE POSITION. DO 11 1=1.3 FROM THE GIVEN - 181 - L i s t i n g of DRPANL.FOR at 14:39:35 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 USIGHT(I)=-EYEPOS(I) SO 11 CONTINUE 61 C 62 c****» THIS SECTION CALCULATES WHICH FACES WILL BE VISIBLE FROM THE 63 c***»* GIVEN LINE OF SIGHT ('USIGHT'). THE DOT PRODUCTS OF 'USIGHT' 64 c****» AND THE FACE SURFACE NORMALS ARE TAKEN. A NEGATIVE RESULT 65 c»**** INDICATES VISIBILITY. 66 c 67 c**»** INITIALIZE THE VISIBLE FACE COUNTER 68 c 69 NSHOW=0 70 c 71 DO FOR EACH FACE 72 c 73 DO 200 I=1,NFACES 74 c 75 c****» GET SURFACE NORMALS 'UFACE' FROM THE FACE PARAMETER ARRAY 76 c***«* 'FACES'. 77 c 78 DO 201 J=1.3 79 UFACE(d)=FACES(IFASTA+I-1,J) 80 201 CONTINUE 81 c 82 c***»* ROTATE 'UFACE' BY TRANSFORMATION 'H' TO GET 'UFACE 1' IN WORLD 83 c***** COORDINATES 84 c 85 CALL ROTAT(H,UFACE.UFACE1) 86 c 87 c***»* TEST FOR 'VISIBILITY- 88 c 89 CALL D0TPRD(UFACE1.USIGHT,UDOT) 90 c 91 IF(UDOT.GE.-.0001) GO TO 500 92 c 93 IF THE FACE IS 'VISIBLE', INCREMENT THE VISIBLE FACE COUNTER 94 c 95 NSH0W=NSH0W+1 96 c 97 c***** STORE VISIBLE FACE POINTERS IN ARRAY 'IFLIST' 98 c 99 IFLIST(NSH0W)=IFASTA+I-1 OO c 01 500 CONTINUE 02 200 CONTINUE 03 c 04 c 05 c****» THIS SECTION CALCULATES THE ORDER OF FACE GENERATION SO THAT 06 c***»* CLOSER FACES ARE SUPERIMPOSED ON, OR COVER, MORE DISTANT FACES 07 c*»**« WEIGHTS ARE ASSIGNED TO EACH FACE, THE HIGHEST BEING THE 08 c***»* CLOSEST. THE ALGORITHM IS AS FOLLOWS: GIVEN A PAIR OF EDGES, 09 c»**** A WEIGHT OF ONE IS ASSIGNED IF THE SECOND EDGE IS BEHIND THE 10 FIRST EDGE AS VIEWED ALONG THE LINE OF SIGHT. THIS IS TESTED 1 1 c***»* FOR EACH EDGE OF A FACE AGAINST ALL EDGES IN THE OBJECT, AND 12 c****» THE SUMMED WEIGHT DIVIDED BY THE NUMBER OF EDGES ON THE FACE 13 c****» GIVES THE FINAL WEIGHT. 14 c 15 c»**** DO FOR EACH VISIBLE FACE 16 c - 182 - L i s t i n g of DRPANL.FOR a t 14:39:35 on AUG 9, 1984 f o r CC1d=DAC0 Page 3 117 DO 202 I=1,NSH0W 1 18 C 119 C***** INITIALIZE FACE WEIGHT 120 C 121 WEIGHT(I)=0. 122 C 123 C***** FIND LOCATION OF FIRST VERTEX 'NSTART' AND NUMBER OF VERTICES 124 C***«* 'NVERTS' FROM FACE PARAMETER ARRAY ' IFACE' . 125 C 126 NSTART=IFACE(IFLIST(I).1) 127 NVERTS=IFACE(IFLIST(I),2) 128 C 129 C***«* DO FOR EACH EDGE BOUNDING FACE 130 C 131 DO 203 J=1,NVERTS 132 C 133 C***»* FIND ENDPOINTS A1.A2 OF FACE EDGE. 134 C 135 DO 204 K=1,3 136 A1(K)=VERT(NSTART+J-1,K) 137 A2(K)=VERT(NSTART+J,K) 138 IF (<J . EQ . NVERTS ) A2(K ) =VERT( NSTART , K ) 139 204 CONTINUE 140 C 141 C***** TRANSFORM INTO WORLD COORDS 142 C 143 CALL TRANS(H,A 1,P1) 144 CALL TRANS(H,A2,P2) 145 C 146 C*»»*» DO FOR ALL VISIBLE FACES 147 C 148 DO 205 L=1.NSHOW 149 C 150 C***** FINO SECOND FACE 151 C 152 ISTART=IFACE(IFLIST(L),1) 153 IVERTS=IFACE(IFLIST(L),2) 154 C 155 DO 206 M=1,IVERTS 156 C 157 C***»* FIND ENDPOINTS 81,B2 OF SECOND FACE EDGES 158 C 159 DO 301 N=1,3 160 B1(N)=VERT(ISTART+M-1,N) 161 B2(N)=VERT(ISTART+M.N) 162 IF(M.EQ.IVERTS) B2(N)=VERT(ISTART,N) 163 301 CONTINUE 164 C 165 C***** TRANSFORM SECOND EDGE INTO WORLD COORDS. 166 C 167 CALL TRANS(H,B1,01) 168 CALL TRANS(H,B2,Q2) 169 C 170 C***** TEST IF SECOND EDGE IS BEHIND FIRST EDGE. 171 C****» IF TRUE, ISIGN=1. 172 C 173 CALL LINDIS(P1,P2,Q1,Q2,USIGHT,ISIGN) 174 C - 183 - L i s t i n g of DRPANL.FOR a t 14:39:35 on AUG 9, 1984 f o r CC1d=DAC0 Page 4 175 IF(ISIGN.EO.O) GO TO 600 176 C 177 c*»«»* IF SECOND EDGE IS BEHIND FIRST EDGE, INCREMENT FACE WEIGHT 178 c 179 WEIGHT(I)=WEIGHT(I) + 1 . 180 c 181 600 CONTINUE 182 C 183 206 CONTINUE 184 C 185 205 CONTINUE 186 C 187 203 CONTINUE 188 C 189 C 190 C * * * * * CALCULATE AVERAGED WEIGHT 191 C 192 WEIGHT(I)=WEIGHT(I)/FLOAT(NVERTS) 193 c 194 202 CONTINUE 195 C 196 c*»*»* THIS SECTION GENERATES A FACE LIST IN ORDER OF INCREASING FACE 197 c«**»* WEIGHT. 198 c 199 DO 210' 1 = 1.NSHOW 200 TEST=10000. 201 DO 211 J=1,NSHOW 202 IF(WEIGHT(J).GE.TEST) GO TO 700 203 TEST=WEIGHT(J) 204 JSAVE=d 205 700 CONTINUE 206 21 1 CONTINUE 207 WEIGHT*JSAVE)=10000. 208 IORDER(I)=JSAVE 209 210 CONTINUE 210 C 21 1 C 212 c***»* THIS SECTION PERFORMS THE GRAPHIC OUTPUT 213 c 214 c**»** DO FOR EACH VISIBLE FACE 215 c 216 DO 220 1 = 1..NSHOW 217 c 218 c***«» FIND THE FURTHEST FACE STARTING POINT 'NSTART', NO. OF VERTICES 219 c***»* 'NVERTS'. 220 c 22 1 IND=IORDER(I) 222 NSTART=IFACE(IFLIST(IND),1) 223 NVERTS = I FACE(IFLIST(IND),2) 224 c 225 c***»* DRAW THE FACE ON THE SCREEN 226 c****» GRAPHICS ROUTINE DEPENDS ON NUMBER OF VERTICES ON FACE. 227 c***** 3,4,5,6.7,8,9,10 CORNERED POLYGONS ARE ALLOWED. 228 c 229 GO TO (803,804,805,806,807,808,809,810), NVERTS-2 230 c 231 803 CONTINUE 232 DO 333. d=1.NVERTS - 184 - L i s t i n g o f DRPANL.FOR a t 14:39:35 on AUG 9, 1984 f o r CCid=DACO Page 5 233 C 234 C****« FIND FACE VERTICES FROM VERTEX LIST 'VERT' 235 C 236 DO 303 K=1,3 237 V(K)=VERT(NSTART+J-1,K) 238 303 CONTINUE 239 C 240 c***** TRANSFORM VERTICES INTO WORLD COORDINATES. 241 C 242 CALL TRANS(H,V,V1) 243 VX3(J)=V1(1) 244 I F ( V X 3 ( J ) . L T . . 0 1 ) VX3(d)=0. 245 VY3(J)=V1(2) 246 IF(VY3(d) . LT . .01) VY3(d)=0. 247 VZ3(d)=V1(3) 248 IF(VZ3(d) .LT. .01) VZ3(J)=0. 249 333 CONTINUE 250 C 251 C«**** DRAW FACE AS A FILLED PANEL SURROUNDED BY A DARK BORDER. 252 C 253 CALL PANL3DO, VX3.VY3.VZ3) 254 CALL M0VE3D(VX3(1),VY3(1),VZ3( 1 )) 255 CALL REMOVE 256 CALL P0LY3D(3.VX3.VY3.VZ3) 257 CALL DRAW3D(VX3(1).VY3(1).VZ3(1)) 258 CALL APPEAR 259 GO TO 900 260 C 261 804 CONTINUE 262 DO 344 d=1.NVERTS 263 DO 304 K=1,3 264 V(K)=VERT(NSTART+d-1,K) 265 304 CONTINUE 266 CALL TRANS(H,V,V1) 267 VX4(d)=V1(1) 268 IF(ABS(VX4(d)).LT..01) VX4(d)=0. 269 VY4(d)=V1(2) 270 IF(ABS(VY4(d)).LT..01) VY4(d)=0. 271 VZ4(d)=V1(3) 272 IF(ABS(VZ4(d) ) .LT. .01) VZ4(d)=0. 273 C 274 344 CONTINUE 275 CALL PANL3D(4,VX4,VY4,VZ4) 276 CALL MOVE3D(VX4(1),VY4(1),VZ4(1)) 277 CALL REMOVE 278 CALL P0LY3D(4,VX4.VY4.VZ4) 279 CALL DRAW3D(VX4(1),VY4(1),VZ4(1)) 280 CALL APPEAR 281 GO TO 900 282 C • „ 283 805 CONTINUE 284 DO 355 d=1.NVERTS 285 DO 305 K=1.3 286 V(K)=VERT(NSTART+d-1.K) 287 305 CONTINUE 288 CALL TRANS(H,V,V1) 289 VX5(d)=V1(1) 290 IF(VX5(d).LT..01) VX5(d)=0. - 185 - L i s t i n g o f DRPANL.FOR a t 14:39:35 on AUG 9, 1984 f o r CCid=DACO Page 6 291 VY5(d)=V1(2) 292 I F ( V Y 5 ( J ) . L T . . 0 1 ) VY5(d)=0. 293 VZ5(J)=V1(3) 294 IF ( V Z 5 ( d ) . L T . . 0 1 ) VZ5(d)=0. 295 355 CONTINUE 296 CALL PANL3D(5,VX5,VY5.VZ5) 297 CALL M0VE3D(VX5(1),VY5(1).VZ5(1)) 298 CALL REMOVE 299 CALL P0LY3D(5,VX5,VY5.VZ5) 300 CALL DRAW3D(VX5(1).VY5(1).VZ5(1)) 301 CALL APPEAR 302 GO TO 900 303 C 304 806 CONTINUE 305 DO 366 0=1.NVERTS 306 DO 306 K=1,3 307 V(K)=VERT(NSTART+d-1,K) 308 306 CONTINUE 309 CALL TRANS(H,V,V1) 310 VX6(d)=V1(1) 311 IF(VX6(d).LT..01) VX6(J)=0. 312 VY6(d)=V1(2) 313 IF(VY6(d).LT..01) VY6(J)=0. 314 VZ6(d)=V1(3) 315 IF(VZ6(d).LT..01) VZ6(d)=0. 316 366' CONTINUE 317 CALL PANL3D(6,VXG.VY6,VZ6) 318 CALL M0VE3D(VX6( 1 ) ,VY6(1).VZ6(1)) 319 CALL REMOVE 320 CALL POLY30(6,VX6,VY6,VZ6) 321 CALL DRAW3D(VX6(1 ) .VY6(1).VZ6(1)) 322 CALL APPEAR 323 GO TO 900 324 807 CONTINUE 325 DO 377 d=1.NVERTS 326 DO 307 K=1,3 327 V(K)=VERT(NSTART+d-1,K) 328 307 CONTINUE 329 CALL TRANS(H,V,V1 ) 330 VX7(d)=V1(1) 331 IF(ABS(VX7(d)).LT..01) VX7(d)=0. 332 VY7(d)=V1(2) 333 IF(ABS(VY7(d)).LT..01) VY7(d)=0. 334 VZ7(d)=V1(3) 335 IF(ABS(VZ7(J) ).LT. .01) VZ7(d)=0. 336 377 " CONTINUE 337 CALL PANL3D(7.VX7,VY7.VZ7) 338 CALL M0VE3D(VX7(1),VY7(1).VZ7(1)) 339 CALL REMOVE 340 CALL P0LY3D(7,VX7,VY7,VZ7) 341 CALL DRAW3D(VX7(1),VY7(1).VZ7( 1)) 342 CALL APPEAR 343 GO TO 900 344 808 CONTINUE 345 DO 388 d=1,NVERTS 346 DO 308 K=1,3 347 V(K)=VERT(NSTART+d-1,K) 348 308 CONTINUE - 186 - L i s t i n g of DRPANL.FOR a t 14:39:35 on AUG 9. 1984 f o r CCid=DACO Page 7 349 CALL TRANS(H,V,V1) 350 VX8(d)=V1(1) 351 IF(ABS(VX8(J) ) . L T . . 0 1 ) VX8(J)=0. 352 VY8(J)=V1(2) 353 I F ( A B S ( V Y 8 ( J ) ) . L T . . 0 1 ) VY8(d)=0. 354 VZ8(d)=V1(3) 355 I F ( A B S ( V Z 8 ( J ) ) . L T . . 0 1 ) VZ8(d)=0. 356 388 CONTINUE 357 CALL PANL3D(8,VX8,VY8,VZ8) 358 CALL M0VE3D(VX8(1).VY8(1),VZ8(1)) 359 CALL REMOVE 360 CALL P0LY3D(8.VX8,VY8.VZ8) 361 CALL DRAW3D(VX8(1),VY8(1).VZ8(1)) 362 CALL APPEAR 363 GO TO 900 364 809 CONTINUE 365 DO 399 d=1.NVERTS 366 DO 309 K=1,3 367 V(K)=VERT(NSTART+J-1,K) 368 309 CONTINUE 369 CALL TRANS(H,V,V1) 370 VX9(J)=V1(1 ) 371 I F ( A B S ( V X 9 ( J ) ) . L T . .01) VX9(J)=0. 372 VY9(d)=V1(2) 373 I F ( A B S ( V Y 9 ( J ) ) . L T . . 0 1 ) VY9(J)=0. 374 VZ9(d)=V1(3) 375 IF(ABS(VZ9(d)).LT..01) VZ9(J)=0. 376 399 CONTINUE 377 CALL PANL3DO, VX9.VY9, VZ9) 378 CALL M0VE3D(VX9(1),VY9(1),VZ9(1)) 379 CALL REMOVE 380 CALL POLY3D(9,VX9,VY9,VZ9) 381 CALL DRAW3D(VX9( 1 ).VY9(1),VZ9(1)) 382 CALL APPEAR 383 GO TO 900 384 810 CONTINUE 385 DO 311 d=1,NVERTS 386 DO 310 K=1,3 387 V(K)=VERT(NSTART+J-1 ,K) 388 310 CONTINUE 389 CALL TRANS(H,V,V1) 390 VX10(J)=V1(1) 391 IF(A B S ( V X 1 0 ( J ) ) . L T . . 0 1 ) VX10(J)=0. 392 VY10(«J)=V1(2) 393 IF(A B S ( V Y 1 0 ( J ) ) . L T . . 0 1 ) VY10(d)=0. 394 VZ10(d)=V1(3) 395 IF(ABS(VZ10(d)).LT..01) VZ10(J)=0. 396 31 1 CONTINUE 397 CALL PANL3DC10.VX10.VY10.VZ10) 398 CALL M0VE3D(VX1O(1),VY10(1).VZ10(1)) 399 CALL REMOVE 400 CALL P0LY3D(10,VX10,VY10,VZ10) 401 CALL DRAW3D(VX10(1).VY10(1),VZ10(1)) 402 CALL APPEAR 403 GO TO 900 404 C 405 900 CONTINUE 406 220 CONTINUE - 187 - i t l n g o f DRPANL.FOR a t 14:39:35 on AUG 9. 1984 f o r CC1d=DAC0 Pa- 407 C 408 RETURN 409 END - 188 - L i s t i n g of DRSEAM.FOR a t 14:39:57 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE ORSEAM(HOW) 2 C 3 C THIS SUBROUTINE DRAWS THE SEAMS 4 C 5 C INPUTS: H0W(4,4)- MATRIX SPECIFYING WORKPIECE POSITION RELATIVE 6 c TO WORLD COORDINATES. 7 c 8 c OUTPUTS: NONE 9 c 10 c INPUT FILES: WPCLST 1 1 c 12 c OUTPUT FILES: NONE 13 c 14 c COMMON BLOCKS ACCESSED: /WPCLST/ 15 c 16 c COMMON BLOCKS ALTERED: NONE 17 c 18 c SUBROUINES CALLED: NORM 19 c OPEN 20 c RDFILE 21 c ROTAT 22 c SHUT 23 c TRANS 24 c UNIT 25 c XPROD 26 c 27 c IGL SUBROUTINES: APPEAR 28 c CLOSEG 29 c DRAW3D 30 c M0VE3D 31 c OPNSEG 32 c REMOVE 33 c SETDET 34 c SETHIL 35 c 36 Q* **************************************************************************** * 37 c 38 c 39 BYTE WPCDAT(20) 40 BYTE WPCLST(20) 41 REAL WINSIZ(6) 42 REAL R(3),H0W(4,4),RW(3) ,A(3),AW(3),AUW(3) 43 REAL T(3),B(3),BW(3),BUW(3),RLW(3),TANW(3),UTANW(3) 44 c 45 COMMON /WPCLST/WPCLST 46 •COMMON /WINSIZ/WINSIZ 47 c 48 Q*»*** INITIALIZE THE TICK SPACING PARAMETER 'TICSPA', BASED ON THE 49 Q***** VIEW WINDOW SIZE 50 c 51 TICSPA=(WINSIZ(2)-WINSIZ(1))/100. 52 c 53 c**«** OPEN THE LIST OF SEAM DATA FILES, 'WPCLST'. 54 c 55 CALL OPEN(1,WPCLST,4,1) 56 c 57 c****» READ THE NUMBER OF SEAMS, 'NSEAMS'. 58 C - 189 - L i s t i n g o f DRSEAM.FOR a t 14:39:57 on AUG 9. 1984 f o r CC1d=DAC0 Page 2 59 READ( 1 ,* ) 60 READ(1,*) NSEAMS 61 C 6 2 c . * * * « SET THE GRAPHIC ATTRIBUTES: DETECTABILITY=TRUE, REMOVE=TRUE 63 C 64 CALL SETDET(-2, .TRUE. ) 65 CALL REMOVE 66 C 6 7 c * * * » * REPEAT FOR ALL SEAMS 68 C 69 00 100 1-1,NSEAMS 70 C 71 C***»* CREATE A SEGMENT I 72 C 73 CALL OPNSEG(I) 74 C 75 C***»* FIND AND OPEN THE SEAM DATA FILE FOR SEAM I. 76 C 77 CALL RDFILEd,1+2,WPCDAT,20,IEND) 78 CALL OPEN(2,WPCDAT,1,1) 79 C 80 C**»«* SKIP THE FIRST RECORD 81 C 82 READ(2) 83 C 84 C***«* READ SEAM DATA FILE I 85 C 86 READ(2,END = 550) J,S.(R(K ) ,K=1,3),(T(L).L=1,3).(A(M).M=1.3), 87 * (B(N),N=1,3) 88 C 89 c * » * * * TRANSFORM INTO WORLD COORDINATES 90 C 91 CALL TRANS(HOW,R,RW) 92 C 93 C****« MOVE TO FIRST POINT 94 C 95 CALL M0VE30(RW(1),RW(2),RW(3)) 96 C 9 7 c * * * » * INITIALIZE SIGN 98 C 99 SIGN=1. 100 C 101 C***»* DRAW A CROSS AT SEAM START 102 C 103 CALL ROTAT(HOW,A,AW) 104 CALL UNIT(AW.AUW) 105 CALL ROTAT(HOW.B.BW) 106 CALL UNIT(BW.BUW) 107 CALL ROTAT(HOW,T,TANW) 108 CALL UNIT(TANW,UTANW) 109 DO 22 K-1,3 110 RLW(K)=RW(K)+SIGN*2.*TICSPA*BUW( K) 111 22 CONTINUE 112 CALL M0VE3D(RLW(1),RLW(2),RLW(3)) 113 DO 33 K=1,3 114 RLW(K)=RW(K)-SIGN*2.*TICSPA*BUW(K) 115 33 CONTINUE 116 CALL DRAW3D(RLW(1),RLW(2),RLW(3)) - 190 - L i s t i n g o f DRSEAM.FOR at 14:39:57 on AUG 9, 1984 f o r CCid=DAC0 Page 3 1 17 DO 44 K=1 ,3 1 18 RLW(K)=RW(K)+SIGN*2."TICSPA*AUW(K) 1 19 44 CONTINUE 120 CALL M0VE3D(RLW(1).RLW(2),RLW(3)) 121 DO 55 K=1 .3 122 RLW(K)=RW(K)-SIGN*2.*TICSPA*AUW(K) 123 55 CONTINUE 124 CALL DRAW3D(RLW( 1 ),RLW(2),RLW(3)) 125 C 128 INITIALIZE COUNTERS 127 c 128 SLAST=0. 129 c IC0UNT=1 130 c 131 Q.*«»» REPEAT FOR ALL POINTS ALONG SEAM 132 c 133 150 CONTINUE 134 C 135 c***»* READ SEAM DATA FOR CURRENT POINT 136 c 137 READ(2,END=550) INDEX,S.(R(K),K=1,3),(T(L) ,L=1,3), 138 (A(M),M=1.3),(B(N),N=1,3) 139 c 140 IF((S-SLAST) .LT.TICSPA) GO TO 500 141 c 142 SLAST=S 143 c 144 c*»«** TRANSFORM 'R','B','T' INTO WORLD COORDS 'RW'.'BW','UTANW 145 c* * * * * AND NORMALIZE 146 c 147 CALL TRANS(HOW.R.RW) 148 CALL ROTAT(HOW,B,BW) 149 CALL UNIT(BW.BUW) 150 CALL ROTAT(HOW,T,TANW) 151 CALL UNIT(TANW.UTANW) 152 c 153 CHANGE SIGN 154 c 155 SIGN=-SIGN 156 c 157 c***** DRAW HERRINGBONE TICK PATTERN, POINTING IN DIRECTION OF 158 c*»»** SEAM 159 c 160 DO 2 K=1,3 161 RLW(K)=RW(K)+SIGN*TICSPA*BUW(K) 162 2 CONTINUE 163 CALL M0VE3D(RLW(1),RLW(2) .RLW(3)) 164 DO 8 K=1,3 165 RLW(K)=RW(K)+TICSPA*UTANW(K) 166 8 CONTINUE 167 CALL DRAW3D(RLW(1),RLW(2),RLW(3)) 168 DO 5 K=1,3 169 RLW(K)=RW(K)-SIGN*TICSPA*BUW(K) 170 5 CONTINUE 171 CALL DRAW3D(RLW(1),RLW(2),RLW(3)) 172 C 173 500 CONTINUE 174 C - 191 - L i s t i n g of DRSEAM.FOR a t 14:39:57 on AUG 9, 1984 f o r CCid=DACO Page 4 175 GO TO 150 176 C 177 550 CONTINUE 178 C 179 CALL CLOSEG 180 CALL SHUT(2) 181 100 CONTINUE 182 C 183 CALL SHUT(1) 184 C 185 CALL APPEAR 186 C 187 RETURN 188 END - 192 - L i s t i n g of DRWELD.FOR a t 14:39:59 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE DRWELD(HOW,IDSEAM) 2 C 3 C THIS SUBROUTINE FILLS IN THE WELDED SEAMS GRAPHICALLY 4 C 5 C INPUTS: H0W(4,4)- MATRIX SPECIFYING POSITION OF WORKPIECE 6 C RELATIVE TO THE WORLD COORDINATE FRAME. 7 c 8 c IDSEAM- IDENTIFIES THE SEAM NUMBER WHICH HAS BEEN 9 c WELDED (TO BE GRAPHICALLY FILLED IN). 10 c 11 c OUTPUTS: NONE 12 c 13 c INPUT FILES: WPCLST- CONTAINS A LIST OF SEAM DATA FILES 14 c 15 c WPCDAT- DATA FOR A GIVEN SEAM 16 c 17 c OUTPUT FILES: NONE 18 c 19 c COMMON BLOCKS ACCESSED: /IOFILE/ 20 c 21 c COMMON BLOCKS ALTERED: NONE 22 c 23 c SUBROUTINES CALLED: NORM 24 c OPEN 25 c RDFILE 26 c SHUT 27 c TRANS 28 c UNIT 29 c XPROD 30 c 31 c IGL SUBROUTINES: APPEAR 32 c CLOSEG 33 c DRAW3D 34 c M0VE3D 35 c OPNSEG 36 c REMOVE 37 c SETDET 38 c SETHIL 39 c 40 41 c 42 BYTE WPCDAT(20) 43 BYTE WPCLST(20) 44 REAL WINSIZ(6) 45 REAL R(3),H0W(4,4),RW(3),N1(3.3),N2(3,3),A(3),AW(3),RL(3) 46 REAL TAN(3),B(3),BW(3),BUW(3),T(3) 47 c 48 COMMON /WPCLST/WPCLST 49 COMMON /WINSIZ/WINSIZ 50 c 51 c+* * * * CALCULATE TICK SPACING PARAMETER 'TICSPA'.BASED ON GRAPHIC 52 c***«* VIEW WINDOW SIZE 53 c 54 TICSPA=(WINSIZ(2)-WINSIZ(1))/lOO. 55 c 56 c***** OPEN SEAM DATA FILE LIST, 'WPCLST' 57 c 58 CALL OPEN(1.WPCLST,4, 1 ) - 193 - L i s t i n g of DRWELD.FOR a t 14:39:59 on AUG 9, 1984 f o r CC1d=DAC0 Page 59 C 60 READ(1,* ) 61 READ(1,* ) NSEAMS 62 C 63 c***«* SET SEGMENT DETECTABILITY OFF, HI LIGHTING ON, 'REMOVE' 64 c****» TRUE 65 c 66 CALL SETDET(-2 , .FALSE. ) 67 CALL SETHIL(-2, .TRUE . ) 68 CALL REMOVE 69 c 70 DETERMINE SEGMENT ID AND OPEN SEGMENT 71 c 72 NSEG=NSEAMS+IDSEAM 73 c 74 CALL OPNSEG(NSEG) 75 c 76 c***** FIND AND OPEN SEAM OATA FILE 77 c 78 CALL RDFILE(1,IDSEAM+2,WPCDAT,20,1 END) 79 CALL 0PEN(2.WPCDAT,1,1 ) 80 c 81 SKIP FIRST RECORD 82 c 83 READ(2) 84 c 85 Q * * * * # READ FIRST POINT DATA TO FIND SEAM STARTING POINT. 86 c 87 READ(2,END=550) d,S.(R(K),K=1,3),(T(L),L-1,3),(A(M).M= 88 * (B (N ),N=1,3) 89 c 90 c****» TRANSFORM INTO WORLD COORDINATES 91 c 92 CALL TRANS(HOW,R,RW) 93 c 94 Q * * * * * MOVE TO STARTING POINT 95 c 96 CALL M0VE3D(RW(1),RW(2),RW(3)) 97 c 98 SIGN=-1. 99 c 100 REPEAT FOR ALL POINTS ALONG THE SEAM 101 c 102 150 CONTINUE 103 c 104 c****» READ CURRENT POINT 105 c 106 READ( 2 , END = 550) J . S . ( R(K ) , K= 1 , 3 ) , ( T( L ) , L= 1 , 3 ) . (A (Mi) , 107 * (B(N).N=1,3) 108 c 109 TRANSFORM INTO WORLD COORDINATES 1 10 c 1 1 1 CALL TRANS(HOW,R.RW) 1 12 c 1 13 c****» ROTATE TRANSVERSE VECTOR 'B' INTO WORLD COORDINATES, 1 14 c 1 15 CALL ROTAT(HOW,B,BW) 1 16 c 'BW' -- 194 - L i s t i n g of DRWELD.FOR at 14:39:59 on AUG 9, 1984 f o r CCid=DACO Page 3 117 C****» NORMALIZE 'BW' TO GET 'BUW' 1 18 C 1 19 CALL UNIT(BW.BUW) 120 C 121 C***»* DRAW A ZIG-ZAG PATTERN OVER THE SEAM TO REPRESENT A FILLED 122 C****» WELD 123 C 124 SIGN=-SIGN 125 C 126 DO 2 K=1,3 127 RW(K)=RW(K)+SIGN*TICSPA*BUW(K) 128 2 CONTINUE 129 C 130 CALL M0VE3D(RW(1),RW(2),RW(3)) 131 C 132 DO 6 K=1,3 133 RW(K)=RW(K)-STGN*2.*TICSPA*BUW(K) 134 6 CONTINUE 135 C 136 CALL DRAW3D(RW(1),RW(2),RW(3)) 137 C 138 c * * * » * THEN GO TO NEXT WELDED SEAM 139 C 140 GO TO 150 141 C 142 550 CONTINUE 143 144 CALL CLOSEG 145 CALL SHUT(2) 146 C 147 CALL SHUT(1) 148 C 149 CALL APPEAR 150 C 151 RETURN 152 END - 195 - L i s t i n g of EYELOC.FOR a t 14:40:00 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE EYELOC(VRP,EYEPOS ) 2 C 3 C THIS SUBROUTINE ALLOWS THE USER TO CHOOSE THE 'IGL' EYE 4 C POSITION. THE VIEW UP VECTOR IS ASSUMED TO BE THE Z-DIRECTION, 5 C SO AN ERROR OCCURS IF THE VIEW DIRECTION COINCIDES WITH THE 6 C Z AXIS. 7 C 8 C INPUTS: VRP(3)- THE 'IGL' VIEW REFERENCE POINT. 9 C 10 C OUTPUTS: EYEPOSO)- THE THREE COMPONENTS OF THE EYE 11 C POSITION IN WORLD COORDINATES. 12 C 14 C 15 BYTE BEL(3).YES,N0 16 REAL EYEPOSO),VRP(3) 17 C 18 DATA BEL/3»7/,YES/89/,NO/78/ 19 C 20 C*» UNTIL (EYE POSITION IS VALID) 21 C 22 400 CONTINUE 23 C 24 c**«** PROMPT THE USER FOR THE DESIRED EYE POSITION. 25 C 26 WRITE(5,100) 27 100 FORMAT($,' ENTER EYE POSITION (X.Y.Z) ') 28 READ(5,* ) (EYEPOS(K ),K= 1 ,3) 29 C 30 C***** CHECK X,Y COMPONENTS OF VIEW DIRECTION. 31 C 32 IF(EYEPOS(1).NE.0..OR.EYEPOS(2).NE.0.) GO TO 200 33 C 34 WRITE(5.SOO) BEL 35 300 FORMAT(' EYE POSITION ON Z-AXIS. TRY AGAIN ' .3A1 ) 36 GO TO 400 37 C 38 C** END UNTIL 39 C 40 200 CONTINUE 41 C 42 RETURN 43 END - 196 - L i s t i n g of EXPERT.FOR a t 14:40:00 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 SUBROUTINE EXPERT(WPCDAT) THIS SUBROUTINE IS THE EXPERT WELDER MODULE. FIXED AND CONTINUOUSLY VARYING PARAMETERS ARE CHOSEN AS A FUNCTION OF GIVEN WORKSTATION AND WORKPIECE PARAMETERS. INPUT ARGUMENTS: WPCDAT- NAME OF SEAM DATA FILE OUTPUT ARGUMENTS: NONE FILES READ: WPCOAT- FILE CONTAINING SEAM DATA WKSTAT- FILE CONTAINING ROBOT AND WELDER PARAMETERS. FILES WRITTEN: WPARAM- TEMPORARY FILE CONTAINING CURRENT,SPEED DATA FOR EACH POINT ON THE CURRENT SEAM. COMMON BLOCKS ACCESSED: /MATRIX/HOT,HOW,SHOT,SHOW COMMON BLOCKS CHANGED: /EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR, WALPHA,BETA,STKOUT /WKSTAT/WKSTAT FORMAT OF FILE 'WPCDAT'. RECORD 1: JTYPE,XSECT,T1,T2.NPOINT,ICURVE RECORDS 2 TO NPOINT+1: INDEX,S,R(3),T(3),A(3),B(3) FORMAT OF FILE 'WKSTAT'. RECORDS 1 TO 26:- CONTAIN HEADINGS, FIXEO ROBOT PARAMETERS. RECORD 27: WSIZE VARIABLE DEFINITIONS: JTYPE- CODE IDENTIFYING JOINT TYPE XSECT- WELD CROSS SECTIONAL AREA T1.T2- THICKNESSES OF PARENT MATERIAL NPOINT- NUMBER OF POINTS DEFINED ALONG SEAM ICURVE- SEAM TYPE- 0=STRAIGHT LINE 1=SPACE CURVE 1 2 C 3 C 4 C 5 C 6 C 7 C 8 C 9 C 10 c 1 1 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 c 45 c 46 c 47 c 48 c 49 c 50 c 51 c 52 c 53 c 54 c 55 c 56 c 57 c 58 c - 197 - L i s t i n g o f EXPERT.FOR a t 14:40:00 on AUG 9. 1984 f o r CC1d=DAC0 Page 2 59 C 60 C IFXVAR- WELDING METHOD- 0=FIXED PARAMETERS (USED ONLY 61 C WHEN ICURVE=1) 62 C 1=VARYING PARAMETERS (USED ONLY 63 C WHEN ICURVE=1) 64 C 65 C VOLT- WELDER VOLTAGE SETTING 66 C 67 C WSIZE- WIRE SIZE 68 c 69 c FEED- WIRE FEED RATE 70 c 71 c GFR- SHIELDING GAS FLOW RATE 72 c 7* c CURRNT- WELDER CURRENT 74 c 75 c SPEED- WELDING SPEED 76 c 77 c T ( 3 ) , A ( 3 ) - VECTORS DEFINING SEAM TANGENT AND NORMAL 78 c DIRECTIONS RELATIVE TO THE WORKPIECE 79 c 80 c TW(3).AW(3)- 'T'.'A' RELATIVE TO WORLD FRAME 81 c 82 c SH0W(4.4)- HOMOGENEOUS TRANSFORMATION MATRIX DEFINING 83 c CURRENT WORKPIECE LOCATION AND ORIENTATION 84 c RELATIVE TO WORLD COORDINATES. 85 c 86 c SUBROUTINES CALLED: CALCS 87 c OPEN 88 c SHUT 89 c WIRE 1 90 c WIRE2 91 c WIRE3 92 c 93 94 c 95 BYTE WKSTAT(20).WPARAM(20) 96 BYTE WPCDAT(20) 97 BYTE YES,ANS 98 REAL SH0T(4,4),SH0W(4,4) 99 REAL TEMPO) 10O DOUBLE PRECISION H0T(4.4),H0W(4.4) 101 c 102 COMMON /EXPERT/WPARAM,VOLT.WSIZE,FEED.GFR.WALPHA,BETA,STKOUT 103 COMMON /WKSTAT/WKSTAT 104 COMMON /MATRIX/HOT,HOW,SHOT,SHOW 105 COMMON /W123/CURRNT,SPEED,XSECT,TX 106 C0MM0N/C0EF/TW(3),AW(3),T(3) . A(3) 107 c 108 DATA YES/89/ 109 c 1 10 c****» INITIALIZE PARAMETER 'XMIN'. 1 1 1 c 112 XMIN= = 1 . 1 13 c 1 14 c * * * * * OPEN FILES. 1 15 c 1 16 CALL OPEN(12.WPARAM,4,1) - 198 - L i s t i n g of EXPERT.FOR a t 14:40:00 on AUG 9, 1984 f o r CC1d=DAC0 Page 3 117 CALL OPEN(13,WPCDAT, 1 , 1 ) 118 CALL OPEN(14,WKSTAT,4,1) 119 C 120 C****» SKIP FIRST 28 RECORDS OF FILE 'WKSTAT'. 121 C 122 DO 150 1 = 1 ,26 123 READ(14,») 124 150 CONTINUE 125 C 126 C***** READ FIXED WELDING PARAMETERS FROM 'WKSTAT'. 127 C 128 READ( 14,*) WSIZE 129 C 130 C 131 WRITE(5,300) 132 300 FORMAT(' ENTER METHOD OF WELDING: 0=FIXED PARAMETERS,1=VARIABLE 133 * PARAMETERS ',$) 134 C 135 READ(5,*) IFXVAR 136 C 137 C****» READ FIXED SEAM WELDING PARAMETERS FROM 'WPCDAT'. 138 C 139 READ(13) JTYPE,XSECT,T1,T2,NPOINT,ICURVE 140 141 C****» DETERMINE THE MINIMUM (SMALLER) THICKNESS 142 C 143 TX=AMIN1(T1,T2) 144 C 145 C***»* CHECK WIRE SIZE AND BRANCH OFF TO CORRESPONDING ROUTINE 146 C 147 220 IF(WSIZE.EO.0.030) GO TO 240 148 IF(WSIZE.EO.0.035) GO TO 250 149 IF(WSIZE.EO.0.045) GO TO 260 150 WRITE(5,*) 'WIRE SIZE ERROR' 151 ERR0R=2 152 GO TO 1001 153 C 154 C****» SUB. WIRE 1 DETERMINES WELDING PARAMETERS FOR WIRE SIZE OF 0.030 155 C***»* INCHES. 156 C 157 240 CALL WIRE 1 158 C 159 C***»* GO TO 500 FOR POSSIBLE ORIENTATION ADJUSMENT 160 C 161 GO TO 500 162 C 163 C***»* SUB. WIRE2 DETERMINES WELDING PARAMETERS FOR WIRE SIZE OF 0.035 164 c****» INCHES. 165 C 166 250 CALL WIRE2 167 C 168 C***** GO TO 500 FOR POSSIBLE ORIENTATION ADJUSTMENT 169 C 170 GO TO 500 171 C 172 C****» SUB. WIRE3 DETERMINES WELDING PARAMETERS FOR WIRE SIZE OF 0.045 173 C****» INCHES. 174 C - 199 - L i s t i n g of EXPERT.FOR a t 1 4 : 4 0 : 0 0 on AUG 9 , 1984 f o r CC1d=DAC0 Page 4 175 2 6 0 C A L L WIRE3 176 C 177 c***** GO TO 5 0 0 FOR O R I E N T A T I O N ADJUSTMENT 178 c 179 5 0 0 CURNNT=CURRNT 180 I F ( I C U R V E . E Q . 1 ) GO TO 7 0 0 181 C 182 c***** CURRENT ADJUSTMENT FOR S I M P L E ( S T R A I G H T ) SEAMS 183 c 184 R E A D ( 1 3 ) I N D E X , S . ( T E M P ( L ) , L = 1 . 3 ) . ( T ( d ) , J = 1 , 3 ) , 185 * ( A ( K ) , K = 1 , 3 ) , ( T E M P ( L ) , L = 1 , 3 ) 186 c 187 c****» C A L L ' C A L C S ' T O DETERMINE AND RETURN ADJUSTMENT ( O R I E N T A T I O N ) 188 c* * * * * FACTORS FOR S I M P L E SEAMS T H I S I S DONE FOR ONE OF THE PO INTS 189 Q * * * * * ON THE SEAM S I N C E A L L THE PO INTS ARE I D E N T I C A L . 190 c 191 C A L L C A L C S ( C P H I , C T H E ) 192 C U R R N T = C U R N N T * C P H I * C T H E 193 GO TO 9 0 0 194 c 195 7 0 0 CONT INUE 196 C 197 c * * » * * CURRENT ADJUSTMENT FOR COMPLEX (CURVED ) SEAMS 198 c 199 READ! 13 ,END = 7 5 0 ) I N D E X , S , ( T E M P ( L ) . L = 1 , 3 ) . ( T( J ) , J= 1 , 3 ) , 2 0 0 * ( A ( K ) , K = 1 , 3 ) , ( T E M P ( L ) , L = 1 , 3 ) 201 c 2 0 2 c»**** C A L L ' C A L C S ' T O DETERMINE AND RETURN ADJUSTMENT ( O R I E N T A T I O N ) 2 0 3 c***** FACTORS POINT BY PO INT 2 0 4 c 2 0 5 C A L L C A L C S ( C P H I . C T H E ) 2 0 6 C U R R N T = C U R N N T * C P H I * C T H E 2 0 7 I F ( I F X V A R . E Q . O ) GO TO 8 5 0 2 0 8 c 2 0 9 c* «*** RECORD C A L C U L A T E D CURRENT AND SPEED PO INT BY PO INT FOR COMPLEX 2 1 0 c»**** SEAMS. 21 1 c 2 1 2 W R I T E ( 1 2 . * ) C U R R N T , S P E E D 2 1 3 c 2 1 4 Q * * * * * FOR F I X E D WELDING PARAMETERS MINIMUM ADJUSTMENT FACTOR IS USED 2 1 5 c***«* (THE MAXIMUM ADJUSTMENT FACTOR I S 1 . 0 0 ) 2 1 6 c 2 1 7 8 5 0 X M I N = A M I N K X M I N . C P H I * C T H E ) 2 1 8 GO TO 7 0 0 2 1 9 C 2 2 0 7 5 0 CONT INUE 221 C 2 2 2 I F ( I F X V A R . E Q . 1 ) GO TO 1001 2 2 3 C 2 2 4 F I X E D WELDING PARAMETERS , OVERR IDE OPT ION MAY BE USED 2 2 5 c 2 2 6 CURRNT=CURNNT*XM IN 2 2 7 c 2 2 8 c 2 2 9 c 2 3 0 c OBSERVE AND/OR OVERR IDE * * * * * * * * * * * * * * * * * * * * 231 c 2 3 2 c - 200 - L i s t i n g o f EXPERT.FOR a t 14:40:00 on AUG 9. 1984 f o r CCid=DAC0 Page 5 233 C 234 900 WRITE(5,301) 235 301 FORMATC DO YOU WISH TO OBSERVE/OVERRIDE(Y/N)? ',$) 236 READ(5,325) ANS 237 325 FORMAT(A1 ) 238 IF (ANS.NE.YES) GO TO 1000 239 C 240 201 WRITE(5.302) CURRNT 241 302 FORMATC PRESENT CURRENT-'.F6.1.'AMPS..DO YOU WISH TO 242 * OVERRIDE(Y/N)? '.$) 243 READ(5,325) ANS 244 IF(ANS.NE.YES ) GO TO 202 245 WRITE(5,352) 246 352 FORMATC ENTER NEW VALUE '.$) 247 READ(5.* ) CURRNT 248 C 249 202 WRITE(5,303) VOLT 250 303 FORMATC PRESENT VOLTAGE-',F6.2.'(VOLTS)..DO YOU WISH TO 251 * OVERRIDE(Y/N)? ',$) 252 READ(5.325) ANS 253 IF(ANS.NE.YES) GO TO 203 254 WRITE(5,352) 255 READ (5,*) VOLT 256 C 257 203 WRITE(5.304) FEED 258 304 FORMAT(' PRESENT WIRE FEED=' .F6. 1 , '(IPM). .DO YOU WISH TO 259 * OVERRIDE(Y/N)? ',$) 260 READ(5,325) ANS 261 IF(ANS.NE.YES) GO TO 204 262 WRITE(5,352) 263 READ (5.*) FEED 264 C 265 204 WRITE(5,305) GFR 266 305 FORMATC PRESENT GAS FLOW RATE-',F6.2,'(CFH)..DO YOU WIsH TO 267 * OVERRIDE(Y/N)? ',$) 268 READ(5,325) ANS 269 IF(ANS.NE.YES) GO TO 205 270 WRITE(5.352) 271 READ (5,*) GFR 272 C 273 205 WRITE(5,306) SPEED 274 306 FORMAT(' PRESENT SPEED-' ,F6.2, '(IPM). .DO YOU WISH TO 275 * OVERRIDE(Y/N)? '.$) 276 READ(5.325) ANS 277 IF(ANS.NE.YES ) GO TO 206 278 WRITE(5.352) 279 READ (5.*) , SPEED 280 C 281 206 WRITE(5,307) STKOUT 282 307 FORMATC PRESENT STICK-OUT-',F6.3.'(INCHES)..DO YOU WISH TO 283 * OVERRIDE(Y/N)? ',$) 284 READ(5,325) ANS 285 IF(ANS.NE.YES) GO TO 207 286 WRITE(5,352) 287 READ (5,*) STKOUT 288 C 289 207 WRITE(5,308) WALPHA 290 308 FORMATC PRESENT LONGITUDINAL ANGLE-',F6.2,'(DEG)..DO YOU WISH - 201 - L i s t i n g of EXPERT.FOR a t 14:40:00 on AUG 9, 1984 f o r CC1d=DAC0 Page 6 291 * TO OVERRRIDE(Y/N)7 ',$) 292 READ(5,325) ANS 293 IF(ANS.NE.YES) GO TO 208 294 WRITE(5,352) 295 READ (5,*) WALPHA 296 C 297 208 WRITE(5,309) BETA 298 309 FORMATC PRESENT TRANSVERSE ANGLE=',F6.2,'(DEG)..D0 YOU WISH TO 299 * OVERRIDE*Y/N)? ',$) 300 READ(5,325) ANS 301 IF(ANS.NE.YES) GO TO 209 302 WRITE(5.352) 303 READ (5,*) BETA 304 C 305 209 CONTINUE 306 GO TO 900 307 C 308 C 309 C 310 » * » * » » » « * . . « * • • * « . » « • » * * « » . » » * » » » » « * » . « * * « * » » « * * * * * * * * » * * « * * * * « « * * 31 1 c 312 1000 CONTINUE 313 IF(IFXVAR.EO.1.AND.ICURVE.EO.1) GO TO 1001 314 c 315 c * * * * * * ' RECORD CALCULATED CURRENT AND SPEED FOR SIMPLE SEAMS 316 c 317 DO 120 1=1,NPOINT 318 WRITE(12,*) CURRNT,SPEED 319 120 CONTINUE 320 C 321 1001 CONTINUE 322 c 323 CLOSE FILES. 324 c 325 CALL SHUT(12) 326 CALL SHUT(13) 327 CALL SHUT(14) 328 c 329 RETURN 330 END - 202 - L i s t i n g o f HMULT.FOR a t 14:40:20 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE HMULT(A,B,C) 2 C 3 C THIS SUBROUTINE MULTIPLIES TWO 4X4 HOMOGENEOUS TRANSFORMATIONS. 4 C C=AB, WHERE A, B, C ARE HOMOGENEOUS TRANSFORMATIONS. 5 C 6 C THE MULTIPLICATION IS DOUBLE PRECISION. 7 C Q (;*,,**»»*»***,,•*«*,**,*»,**,»*»,***.,,*,«**•»**.»**.*»*».»*****.****. 9 C 10 DOUBLE PRECISION A(4,4 ) ,B(4.4),C(4,4) 1 1 C 12 DO 100 1=1.3 13 DO 200 d=1,4 14 C ( I , d ) = A ( I , 1 ) * B ( 1 , d )+A(I,2)*B(2,J)+A(I,3)*B(3,d) 15 IF(d.E0.4) C ( I , J ) = C ( I , J ) + A ( I . d ) 16 C(4,1)=0. 17 C(4,2)=0. 18 C(4,3)=0. 19 C(4,4)=1. 20 2C0 CONTINUE 21 10O CONTINUE 22 C 23 RETURN 24 END - 203 - L i s t i n g o f HMULTS.FOR at 14:40:21 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE HMULTS(A.B,C ) 2 C 3 C THIS SUBROUTINE MULTIPLIES TWO 4X4 HOMOGENEOUS TRANSFORMATIONS 4 C C=AB, WHERE A, B, C ARE HOMOGENEOUS TRANSFORMATIONS. 5 C 6 C THE MULTIPLICATION IS "'SINGLE** PRECISION. 7 C 8 9 c 10 REAL A(4,4),B(4,4) ,C(4,4) 11 c 12 DO 100 1=1,3 13 DO 200 J=1,4 14 C(I,d)=A(I,1 )*B( 1 ,d)+A(1,2)*B(2. «J)+A(I,3)*B(3.J) 15 IF(d.E0.4) C ( I , J ) = C ( I , J ) + A ( I , J ) 16 C(4,1)=0. 17 C(4,2)=0. 18 C(4,3)=0. 19 C(4.4)=1. 20 200 CONTINUE 21 100 CONTINUE 22 C 23 RETURN 24 END - 204 - L i s t i n g o f INIT.FOR a t 14:40:21 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE INIT(NSEAMS) 2 C 3 C THIS SUBROUTINE INITIALIZES THE VALUES OF ALL CONSTANT 4 C PARAMETERS WHICH ARE STORED IN FILES AND SAVES THEM IN 5 C COMMON BLOCKS. 6 C 7 C INPUTS: NSEAMS- THE NUMBER OF SEAMS TO BE WELDED 8 C 9 C OUTPUTS: NONE 10 C 11 C COMMON BLOCKS CHANGED: /ROBOT/ - ROBOT GEOMETRIC PARAMETERS 12 C /TABDAT/ - FIXED TABLE TRANSFORMATIONS 13 C /TOLROT/ - NUMBER AND SIZE OF TOOL 14 c ROTATIONS ABOUT TOOL AXIS 15 c FOR 6 DEGREE OF FREEDOM ROBOT. 16 c 17 c FILES READ: WKSTAT- ROBOT DESCRIPTION 18 c TABLE- TABLE DESCRIPTION 19 c 20 c FILES CREATED: ROBLST- LIST OF ROBOT COORDINATE OUTPUT FILES 21 c ROBDAT- ROBOT COORDINATE FILES 22 c LOCSET- FILE RECORDING WELDING SEQUENCE. ROBOT 23 c CONFIGURATIONS AND TABLE POSITIONS. 24 c 25 c SUBROUTINES CALLED: MATINV 26 c OPEN 27 c RDFILE 28 c SHUT 29 c 30 31 c 32 BYTE TABLE(20).ROBLST(20).LOCSET(20).WKSTAT(20) 33 DOUBLE PRECISION HTW(4,4) 34 REAL ALPHA(6),LEN(6) ,DIST(6),RMIN(6),RMAX(6),H00(4,4),H6G(4,4) , 35 * INH0O(4,4).INH6G(4,4),SHTW(4,4) 36 REAL DELTA 37 INTEGER IC0NF(6).IMAX 38 c 39 COMMON /ROBOT/NRDOF,ALPHA,LEN.DIST.RMIN,RMAX,H00.H6G,INHOO.INH6G 40 COMMON /WKSTAT/WKSTAT 41 COMMON /TABLE/TABLE 42 COMMON /ROBLST/ROBLST 43 COMMON /LOCSET/LOCSET 44 COMMON /TABDAT/HTW,SHTW 45 COMMON /TOLROT/IMAX.DELTA 46 COMMON /CONFIG/ICONF 47 c 48 c 49 c***»* READ FIXED ROBOT PARAMETERS **************************************** 50 c 51 CALL OPEN(1.WKSTAT,4, 1 ) 52 c 53 READ(1,* ) 54 c 55 c***** READ THE NUMBER OF DEGREES OF FREEDOM 56 c 57 READ(1,* ) NRDOF 58 READ(1,*) - 205 - L i s t i n g o f INIT.FOR a t 14:40:21 on AUG 9. 1984 f o r C C i d = D A C 0 Page 2 59 READ(1,*) 60 C 61 READ THE ROBOT GEOMETRY PARAMETERS AND JOINT LIMITS 62 c 63 DO 10 1=1.6 64 READ(1,* ) LINK,ALPHA(I),LEN(I),DI ST(I),RMIN(I),RMAX(I) 65 10 CONTINUE 66 c 67 c***** READ INITIAL ROBOT CONFIGURATION 68 c 69 READ( 1,* ) 70 READ( 1 ,* ) (ICONF(I),1 = 1,6) 71 c 72 CI * * * * * READ THE WORLD TO ROBOT BASE HOMOGENEOUS TRANSFORMATION 'HOO' 73 c 74 READ(1,* ) 75 DO 20 1=1,4 76 READ( 1 ,* ) (H0O(I,J),J=1,4) 77 20 CONTINUE 78 C 79 c****» READ THE TOOL TO LAST LINK HOMOGENEOUS TRANSFORMATION 'H6G' 80 c 81 READ(1,*) 82 DO 30 1=1.4 83 READ( 1 ,* ) (H6G(I,J) ,J=1,4) 84 30 CONTINUE 85 C 86 READ THE NUMBER AND SIZE OF THE DESIRED ROTATIONS ABOUT THE TOOL 87 c****» AXIS (6 DEG. OF FREEDOM ROBOT ONLY) 88 c 89 READ( 1,*) 90 READ( 1 , * ) IMAX,DELTA 91 c 92 CALL SHUT( 1 ) 93 c 94 DELTA=DELTA*3.14159/180. 95 c 96 c***** CALCULATE INVERSE MATRICES FOR USE LATER. 97 c 98 CALL MATINV(HOO,INHOO) 99 CALL MATINV(H6G,INH6G) 10O c 101 c****« READ TABLE PARAMETERS. ************************* ********************** 102 c 103 CALL OPEN(1.TABLE,4, 1 ) 104 c 105 c***»* READ THE NUMBER OF DEGREES OF FREEDOM 106 c 107 READ(1,*) NUMDOF 108 DO 40 1=1.NUMDOF+2 109 READ(1,*) 1 10 40 CONTINUE 1 1 1 c 112 c***** READ THE WORKPIECE TO TABLE TRANSFORMATION MATRIX 'HTW' 1 13 c 1 14 DO 50 1=1,4 115 READ(1,*) (HTW(I.J),J=1,4) 1 16 50 CONTINUE - 2 0 6 - L i s t i n g of INIT.FOR a t 14:40:21 on AUG 9, 1984 f o r CCid=DAC0 Page 3 1 17 C 1 18 c****» CONVERT 'HTW' TO SINGLE PRECISION 'SHTW 1 19 c 120 DO 60 1=1,4 121 DO 70 J=1,4 122 SHTW(I,J)=HTW(I,U) 123 70 CONTINUE 124 60 CONTINUE 125 C 12S CALL SHUT(1) 127 C 128 c***»* CREATE EMPTY ROBOT COORDINATE FILES TO BE FILLED LATER. *************** 129 c 130 CALL OPEN(1,R0BLST,4.1) 131 DO 80 ISEAM=1.NSEAMS 132 CALL ROFILE(1,ISEAM+1,ROBOAT,20,IEND) 133 CALL 0PEN(2,R0BDAT,6,1) 134 CALL SHUT(2) 135 80 CONTINUE 136 CALL SHUT(1) 137 C 138 c***»* CREATE EMPTY SEAM,TABLE POSITION AND ROBOT CONFIGURATION 139 c****» FILE TO BE FILLED LATER. 140 c 141 CALL OPEN(1.LOCSET.6,1) 142 CALL SHUT(1) 143 c 144 RETURN 145 END - 207 - L i s t i n g of JOINTS.FOR a t 14:40:22 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 r SUBROUTINE J0INTS(H06,THETA,IFLAG) 3 C THIS SUBROUTINE CALCULATES THE JOINT ANGLES OF A 6 DEGREE OF 4 C FREEDOM MANIPULATOR GIVEN THE DESIRED LOCATION AND ORIENTATION 5 c OF THE GRIPPER. THE MANIPULATOR TO BE SOLVED MUST BE 6 c GEOMETRICALLY IDENTICAL TO A UNIMATION PUMA 560. WITH 7 Q c r» THE JOINT DIMENSIONS BEING SPECIFIABLE. O 9 L. c INPUTS: H06(4,4)- HOMOGENEOUS TRANSFORMATION MATRIX FROM THE 10 c SIXTH JOINT FRAME 'F6' TO THE ROBOT BASE 1 1 c WORLD FRAME 'FO'. 'H06' IS OF THE FORM 12 c [ n . o . a . p ] , WHERE n.o.a ARE UNIT VECTORS 13 c DESCRIBING THE ORIENTATION OF FRAME 'F6', 14 c AND p IS THE POSITION VECTOR FOR THE ORIGIN 15 c OF 'F6' RELATIVE TO 'FO'. 16 c 17 c OUTPUTS: THETA(6): A REAL ARRAY CONTAINING THE JOINT ANGLES IN 18 c DEGREES. 19 c 20 c IFLAG: THIS FLAG IS SET TO 1 IF THE ROBOT WORK 21 c ENVELOPE IS EXCEEDED. OTHERWISE IT IS 0. 22 c 23 c COMMON: /CONFIG/ICONF(6)-INTEGER ARRAY DESCRIBING DESIRED 24 c CONFIGURATION OF EACH JOINT, WHEN MORE THAN 25 c ONE CONFIGURATION IS POSSIBLE. 26 c 27 c POSSIBLE CONFIGURATIONS: 28 c 29 c 0 0 0 0 0 0 - L A NF 30 c 0 0 0 1 0 0 - L A F 31 c 0 0 1 0 0 0 - L B NF 32 c 0 0 1 1 0 0 - L B F 33 c 1 1 1 0 0 0 - R A NF 34 c 1 1 1 1 0 0 - R A F 35 c 1 1 0 0 0 0 - R B NF 36 c 1 1 0 1 0 0 - R B F 37 c 38 c WHERE- R=RIGHT SHOULDER CONFIG. 39 c L=LEFT SHOULDER CONFIG. 40 c A=ELBOW ABOVE HAND 41 c B=ELBOW BELOW HAND 42 c NF=WRIST NOT FLIPPED 43 c F=WRIST FLIPPED 44 c 45 c NOTE: IC0NF(5-6) NOT USED 46 c 47 c COMMON: /ROBOT/NRDOF,ALPHA(6),LEN(6).DIST(6),RMIN(6),RMAX(6), 48 c H00(4.4),H6G(4.4),INH0O(4,4),INH6A(4.4) 49 c 50 c 51 c 52 c FILES: NONE 53 c 54 c SUBROUTINES CALLED: NONE 55 c 56 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 57 c 58 REAL HOG(4,4),THETA(6),ALPHA(6),LEN(6),DIST(6),RMIN(6),RMAX(6) - 208 - L i s t i n g o f JOINTS.FOR a t 14:40:22 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 REAL X.Y.TEMPI,TEMP2,TEMP3,TEMP4,TEMP5,TEMP6,H6G(4,4 ) , 60 * INH0O(4,4),H00(4,4),H06(4,4),Q(6),INH6A(4.4) 61 INTEGER IC0NF(6).IFLAG 62 C 63 COMMON /ROBOT/NRDOF.ALPHA,LEN,DIST,RMIN,RMAX,H0O.H6G,INHOO,INH6A 64 COMMON /CONFIG/ICONF 65 c 66 c 67 c***»* INITIALIZE 'IFLAG' 68 c 69 IFLAG=0 70 c 71 c***** CALCULATE COMPONENTS OF VECTOR ' 0 ( 3 ) ' 72 c 73 DO 20 1=1,3 74 0(I)=H06(I.4)-DIST(6)*H06(I,3) 75 20 CONTINUE 76 0(4)=1 . 77 C 78 FIND THETA(1) ************************************************* 79 c 80 TEST=Q(1)**2+0(2)**2-DIST(2)**2 81 c 82 IF(ENVELOPE EXCEEDED) SET FLAG AND RETURN 83 c 84 IF(TEST.LT.O.) GO TO 850 85 c 86 TEMP=SQRT(Q(1)**2+Q(2)**2-DIST(2)**2) 87 T1=0(2)*TEMP 88 T2=0(1)*TEMP 89 c 90 c***»* CHOOSE SOLUTION DEPENDING ON ROBOT CONFIGURATION. 91 c 92 IF (ICONF(1).GT.1) GO TO 900 93 IF (ICONF(1).NE.1) GO TO 100 94 T1=-T1 95 T2=-T2 96 100 CONTINUE 97 c 98 Y=T1-DIST(2)*0(1) 99 X=T2+DIST(2)*0(2) 100 c 101 THETA(1)=ATAN2(Y,X) 102 c 103 c»**** FIND THETA(3) ************************************************* 104 c 105 Y=(0(1)**2+Q(2)*»2+Q(3)**2-DIST(4)**2-LEN(2)**2-DIST(2)**2- 106 * L E N ( 3 ) * * 2 ) / ( 2 . * L E N ( 2 ) ) 107 RSQ=LEN(3)**2+DIST(4)**2 108 TEST2=RS0-Y**2 109 c 1 10 c***«* IF(ENVELOPE EXCEEDED) SET FLAG AND RETURN 1 1 1 c 1 12 IF(TEST2.LT.O.) GO TO 850 113 c 1 14 X=SQRT(TEST2) 1 15 THETA1=ATAN2(LEN(3),-DIST(4)) 1 16 c - 209 - L i s t i n g o f JOINTS.FOR a t 14:40:22 on AUG 9, 1984 f o r CC1d=DAC0 Page 3 117 c***«* CHOOSE SOLUTION DEPENDING ON CONFIGURATION. 1 18 c 1 19 IF (I CONF ( 3 ) . GT . 1 ) GO TO 900 120 IF (IC0NF(3).EO.1) X=-X 121 c 122 THETA(3)=THETA1-ATAN2(Y.X) 123 c 124 c***«* FIND THETA(2) ************************************ 125 c 128 C3 = C0S(THETA(3) ) 127 S3 = SIN(THETA(3) ) 128 TEMP1=DIST(4)*C3-LEN(3)*S3 129 TEMP2=DIST(4)*S3+LEN(3)*C3+LEN(2) 130 THETA1=ATAN2(TEMP1.TEMP2) 131 RSQ=TEMP1**2+TEMP2**2 132 TEST3=RSQ-Q(3)**2 133 c 134 c****» IF(ENVELOPE EXCEEDED) SET FLAG AND RETURN 135 c 136 IF(TEST3.LT.O.) GO TO 850 137 c 138 Y=0(3) 139 X=SQRT(TEST3) 140 c 141 c***** CHOOSE SOLUTION DEPENDING ON CONFIGURATION. 142 c 143 IF(IC0NF(2).NE.0) X=-X 144 c 145 THETA(2)=THETA1-ATAN2(Y.X) 146 c 147 FIND THETA(4) ************************************************* 148 c 149 Y=COS(THETA(1))*H06(2.3)-SIN(THETA(1))*H06(1.3) 150 X=COS(THETA(1))*COS(THETA(2)+THETA(3))*H06(1,3) + 151 • SIN(THETA(1))*C0S(THETA(2)+THETA(3))*H06(2,3)-152 * SIN(THETA(2)+THETA(3))*H06(3.3) 153 c 154 c***»» CHOOSE SOLUTION DEPENDING ON CONFIGURATION. 155 c 156 IF(ICONF(4).GT.1 ) GO TO 900 157 IF(ICONF(4).NE.1) GO TO 101 158 x=-x 159 Y = -Y 160 101 CONTINUE 161 c 162 THETA(4)=ATAN2(Y,X ) 163 c 164 c***»* FIND THETA(5) ************************************************* 165 c 166 TEMP1=(C0S(THETA(1))*C0S(THETA(2)+THETA(3))*COS(THETA(4))- 167 * SIN(THETA(1))*SIN(THETA(4)))*H06(1,3) 168 TEMP2=(SIN(THETA(1))*COS(THETA(2)+THETA(3))*COS(THETA(4))+ 169 * COS(THETA(1))*SIN(THETA(4)))*H06(2.3) 170 TEMP3=C0S(THETA(4))*SIN(THETA(2)+THETA(3))*H06(3,3) 171 TEMP4=C0S(THETA(1))*SIN(THETA(2)+THETA(3))*H06(1,3) 172 TEMP5=SIN(THETA(1))*SIN(THETA(2)+THETA(3))*H06(2,3) 173 TEMP6=<:0S( THETA( 2 )+THETA( 3 ) ) *H06( 3 , 3 ) 174 c - 210 - L i s t i n g o f JOINTS.FOR a t 14:40:22 on AUG 9, 1984 f o r CC1d=DAC0 Page 4 175 Y=TEMP1+TEMP2-TEMP3 176 X=TEMP4+TEMP5+TEMP6 177 C 178 THETA(5) = ATAN2(Y,X ) 179 C 180 c*»*»* FIND THETA(6) *********************************** * * * * * * * * * * * * * * 181 c 182 TEMP1 = (-SIMTHETA(1) )*COS(THETA(4 ) )-COS(THETA(1)) *C0S(THETA(2)+ 183 * THETA(3))*SIN(THETA(4)))*H06(1,1) 184 TEMP2=(C0S(THETA(1) )*COS(THETA(4))-SIN(THETA(1))* C0S(THETA(2)+ 185 * THETA(3))*SIN(THETA(4)))*H06(2,1) 186 TEMP3=SIN(THETA(4))*SIN(THETA(2)+THETA(3))*H06(3,1) 187 TEMP4=(-SIN(THETA(1))*COS(THETA(4))-COS(THETA(1 ) ) *C0S(THETA(2)+ 188 * THETA(3))*SIN(THETA(4)))*H06(1,2) 189 TEMP5=(COS(THETA(1))*COS(THETA(4))-SIN(THETA(1))* C0S(THETA(2)+ 190 • THETA(3))*SIN(THETA(4)))*H06(2,2) 191 TEMP6=SIN(THETA(4))*SIN(THETA(2)+THETA(3))*H06(3. 2) 192 c 193 Y=TEMP1+TEMP2+TEMP3 194 X=TEMP4+TEMP5+TEMP6 195 c 196 THETA(6 ) =ATAN2(Y,X) 197 c 198 c****» CONVERT ANGLES INTO DEGREES, INSURE THAT -180.LE. THETA.LE.180. 199 c 200 C0NV=180./3.14159 201 DO 30 1=1,6 202 THETA ( I ) = THETA ( I )*CONV 203 IF(THETA(I).GT.180.) THETA(I)=THETA(I)-360. 204 IF(THETA(I).LT.-180) THETA(I)=THETA(I)+360. 205 30 CONTINUE 206 C 207 GO TO 800 208 C 209 c***** POSITION OUTSIDE ENVELOPE, SET FLAG 210 c 211 850 CONTINUE' 212 IFLAG=1 213 GO TO 800 214 C 215 c****« ERROR MESSAGES ********************************* * * * * * * * * * * * * * * 216 c 217 900 WRITE(5,901) 218 901 FORMATC VALUE OF PARAMETER ICONF NOT 0 OR 1') 219 C 220 800 CONTINUE 221 C 222 RETURN 223 END - 2 1 1 - L i s t i n g of LIMITS.FOR a t 14:40:47 on AUG 9, 1984 f o r CC1d=DAC0 Page. 1 1 SUBROUTINE LIMITS(THETA.I FLAG.IJOINT) 2 C 3 C THIS SUBROUTINE CHECKS THE CALCULATED JOINT ANGLES 'THETA' 4 C AGAINST THEIR PHYSICAL LIMITS. IF ANY JOINT ANGLE IS 5 C EXCEEDED, 'IFLAG' IS SET. 6 C 7 C 8 C INPUTS: THETA(6)-JOINT ANGLES IN DEGREES 9 C 10 C COMMON /ROBOT/ALPHA,LEN.DIST,RMIN,RMAX,H00.H6G.INHOO. 1 1 C INH6A 12 C 13 C 14 C OUTPUTS: IFLAG- 0-ANGLES WITHIN LIMITS 15 C 1-JOINT LIMIT EXCEEDED 16 C 17 C IJOINT(G)-ARRAY WHICH IDENTIFIES JOINTS 18 C WHOSE LIMITS HAVE BEEN EXCEEDED. 19 C IE. 0 0 0 1 0 0 - JOINT 4 EXCEEDED 20 C 21 c 22 c 23 c*»***»*«»*»**«***«**«*««*«»»*»*«***«»««***«*»*««*******«**«* 24 REAL THETA(6),RMIN(6),RMAX(6),ALPHA(6),DIST(6),LEN(6) 25 REAL H00(4,4),H6G(4,4),INH00(4,4),INH6A(4,4) 26 INTEGER IJ0INT(6) 27 c 28 COMMON /ROBOT/NRDOF,ALPHA,LEN,01 ST,RMIN,RMAX,HOO,H6G,INHOO,INH6A 29 c 30 c 31 c****» INITIALIZE 'IJOINT','IFLAG' 32 c 33 DO 11 1=1.6 34 IJ0INT(I)=O 35 11 CONTINUE 36 c 37 IFLAG=0 38 c 39 c***** TEST EACH JOINT ANGLE 'THETA'AGAINST ITS SPECIFIED LIMITS 40 c***** 'RMIN'.'RMAX'. 41 c 42 DO 20 1=1,6 43 c 44 IF(RMIN(I).LE.RMAX(I)) GO TO 200 45 c 46 IF(THETA(I).GE.RMIN(I).OR.THETA(I).LE.RMAX(I)) GO TO 100 47 IFLAG=1 48 IJ0INT(I)=1 49 GO TO 100 50 200 CONTINUE 51 c 52 IF(THETA(I).GE.RMIN(I).AND.THETA(I).LE.RMAX(I)) GO J? 100 53 IFLAG= 1 54 IJOINT(I ) = 1 55 100 CONTINUE 56 c 57 20 CONTINUE 58 c - 212 - L i s t i n g o f LIMITS.FOR a t 1 4 : 4 0 : 4 7 on AUG 9 , 1984 f o r CC1d=DAC0 Page 59 RETURN 60 END - 2 1 3 - L i s t i n g o f L I N D I S . F O R a t 14 : 4 0 : 4 7 o n AUG 9, 1984 f o r C C i d = D A C O P a g e 1 1 o r* SUBROUTINE L I N D I S ( P 1 , P 2 , 0 1 . 0 2 . U S I G H T , I S I G N ) *l 3 \* c T H I S SUBROUTINE F I N D S THE SHORTEST VECTOR FROM 4 c L I N E P 1 P 2 TO L I N E 0 1 0 2 . AND T E S T S I F P1P2 I S I N FRONT OF 5 £ c r* OR BEHIND 0 1 0 2 FOR A G I V E N L I N E OF SIGHT VECTOR 'USIGHT'. D 7 K, c I N P U T S : P 1 ( 3 ) , P 2 ( 3 ) - ENDPOINTS OF F I R S T L I N E 8 c 0 1 ( 3 ) . 0 2 ( 3 ) - ENDPOINTS OF SECOND L I N E 9 c U S I G H T O ) - L I N E OF SIGHT VECTOR 10 c 1 1 c OUTPUTS: I S I G N - 0= 0 1 0 2 IN FRONT OF P1P2 12 c 1= 0 1 0 2 BEHIND P1P2 13 c 14 15 c 16 REAL P 1 ( 3 ) , P 2 ( 3 ) , Q 1 ( 3 ) , Q 2 ( 3 ) , U S I G H T ( 3 ) , U P ( 3 ) , U 0 ( 3 ) , D E L T ( 3 ) 17 REAL UXP 1 ( 3 ) , U X P ( 3 ) 18 c 19 I N I T I A L I Z E I S I G N 2 0 c 21 I S I G N = 0 22 c 23 c***** F I N D THE EDGE VECTORS 'UP'.'UO' FROM THE ENDPOINTS 24 c»**** FIND A VECTOR 'DELT' FROM EDGE 'UP' TO EDGE 'UO' 25 c 26 DO 10 1=1,3 27 U P ( I ) = P 2 ( I ) - P K D 28 U Q ( I ) = Q 2 ( I ) - Q 1 ( I ) 29 D E L T ( I ) = 0 1 ( I )-P 1 ( I ) 30 10 CONTINUE 31 c 32 c***** FIND THE CROSS PRODUCT UXP = UP X UO 33 c* * * * * T H I S G I V E S US THE COMMON NORMAL OF THE TWO EDGES 34 c 35 C A L L XPROD(UP.UO.UXP) 36 c 37 c* * * ** T E S T I F 'UXP' I S ZERO ( I E . 'UP'.'UO' ARE P A R A L L E L ) 38 c 39 D 0T1=UXP(1 ) * * 2 + U X P ( 2 ) * * 2 + U X P ( 3 ) * * 2 4 0 I F ( D 0 T 1 . L T . . 0 0 1 ) GO TO 5 0 0 41 GO TO 6 0 0 42 c 43 I F 'UXP' IS ZERO, FIND THE CROSS PRODUCT UXP1 = UP X DELT 44 c 4 5 5 0 0 CONTINUE 4 6 C A L L X P R 0 D ( U P , D E L T , U X P 1 ) 47 c 48 c***»* I F 'UXP' I S AGAIN ZERO, 'UP'='UO'. T H I S I S AN ERROR CONDITION. 4 9 c 5 0 D I S T = U X P 1 ( 1 ) * * 2 + U X P 1 ( 2 ) * * 2 + U X P 1 ( 3 ) * * 2 51 I F ( D I S T . L T . . 0 0 1 ) GO TO 7 0 0 52 c 53 THE COMMON NORMAL 'UXP' I S NOW FOUND FROM UXP = UP X UXP1 54 c 55 C A L L X P R 0 D ( U P , U X P 1 , U X P ) 56 c 57 6 0 0 CONTINUE 58 c - 214 - L i s t i n g of LINDIS.FOR a t 14:40:47 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 c****» FIND THE DOT PRODUCT OF THE COMMON NORMAL 'UXP' AND THE LINE OF 60 C**»*» SIGHT VECTOR 'USIGHT'. 61 C 62 CALL DOTPRD(UXP.USIGHT,DOT) 63 C 64 C****» FIND THE DIRECTION OF 'UXP' RELATIVE TO EDGES 'UP'.'UQ' BY 65 C***** TAKING THE DOT PRODUCT OF ANY VECTOR 'DELT' DIRECTED FROM 6 6 c * » » » » 'UP' TO 'UO' AND 'UXP'. THE RESULT WILL BE POSITIVE 67 C***»* IF 'UXP' IS DIRECTED FROM 'UP' TO 'UO'. 63 C 69 CALL D0TPRD(DELT,UXP,D0T2) 70 C 7 1 c***** 'UQ' IS BEHIND 'UP' IF D0T*D0T2 IS POSITIVE 72 C 73 IF(D0T*D0T2.GT.0.0001) ISIGN=1 74 C 75 700 CONTINUE 76 C 77 RETURN 78 END - 215 - L i s t i n g of MATCNV.FOR a t 14:40:48 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE MATCNV(DH,SH.ICONV) 2 C 3 C THIS SUBROUTINE CONVERTS A DOUBLE PRECISION 4X4 MATRIX 4 C TO SINGLE PRECISION IF IC0NV=1. THE INVERSE OCCURS IF 5 C IC0NV=2. 6 C 7 REAL SH(4,4) 8 DOUBLE PRECISION 0H(4.4) 9 C 10 IF(IC0NV.EQ.2) GO TO 100 1 1 C 12 DO 1 I=1.4 13 DO 2 J=1,4 14 SH(I,d)=DH(I,d) 15 2 CONTINUE 16 1 CONTINUE 17 GO TO 200 18 C 19 100 CONTINUE 20 C 21 DO 3 1=1,4 22 DO 4 J=1,4 23 DH(I,d)=SH(I,J) 24 4 CONTINUE 25 3 CONTINUE 26 C 27 2C0 CONTINUE 28 C 29 RETURN 30 END - 216 - L i s t i n g o f MATINV.FOR a t 14:40:48 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE MATINV(H.HINV) 2 C 3 C THIS SUBROUTINE FINDS THE INVERSE 'HINV OF A HOMOGENEOUS 4 C TRANSFORMATION MATRIX 'H'. 5 C 6 C INPUTS: H(4,4)- HOMOGENEOUS TRANSFORMATION MATRIX 7 C 8 C OUTPUTS: HINV(4.4)- INVERSE OF 'H' 9 C 10 C 1 1 C 12 REAL H(4.4),HINV(4,4) 13 C 14 DO 1 1=1,3 15 DO 2 J=1 ,3 1S HINV(I,J)=H(J.I ) 17 2 CONTINUE 18 1 CONTINUE 19 C 20 HINV(1.4)= J(H(1,4)*H(1,1)+H(2,4)*H(2,1)+H(3,4)*H(3,1)) 21 HINV(2.4)=-(H(1,4)«H(1,2)+H(2,4)*H(2,2)+H(3,4)*H(3,2)) 22 HINV(3,4)=-(H(1,4)*H(1,3)+H(2,4)*H(2,3)+H(3,4)*H(3,3)) 23 C 24 HINV(4,1)=0. 25 HINV(4,2)=0. 26 HINV(4,3)=0. 27 HINV(4.4)=1. 28 C 29 RETURN 30 END - 217 - L i s t i n g of NOAP1.FOR a t 14:41:10 on AUG 9. 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE NOAP1(H06.XYZOAT) 2 C 3 C THIS SUBROUTINE FINDS THE PUMA LOCATION PARAMETERS X.Y.Z.O.A.T 4 C GIVEN THE HOMOGENEOUS TRANSFORMATION 'H06' FOR THE PUMA 560 5 C ROBOT. 6 C 7 C 8 C INPUTS: H06(4.4) -HOMOGENEOUS COORD TRANSFORMATION FROM 6TH 9 C LINK FRAME F6 TO ROBOT BASE FRAME FO. 10 C 11 C OUTPUTS: XYZ0AT(6) -ARRAY CONTAINING ROBOT LOCATION PARAMETERS 12 C X.Y.Z.O.A.T WHERE X.Y.Z ARE THE COORDINATES 13 C OF THE TOOL CENTER REL. TO FO AND O.A.T ARE 14 C THE WRIST ROTATION ANGLES ABOUT THE 15 C AXES OF FO. 16 C 17 C*«**«****************»*»»«*»*******^ 18 c 19 REAL H06(4,4).PO.PA.PT 20 REAL XYZ0AT(6) 21 c 22 c * » * * * CALCULATE PO.PA.PT 23 c 24 C0NV=18O./3.14159 25 c 26 c * * * * * TEST IF THE APPROACH VECTOR 'a' IS VERTICAL (ALONG ROBOT BASE 27 c****» Z-AXIS) 28 c 29 IF(ABS(H06(1,3)).LT. .001 .AND.ABS(H06(2.3)).LT. .001) GO TO 400 30 c 31 c*»*** IF FALSE 32 c 33 P0=ATAN2(-HO6(1,3).H06(2,3)) 34 TEMP=SIN(P0)*HO6(1,3)-COS(PO)*H06(2,3) ' 35 PA=ATAN2(-H06(3,3),SIN(PO)*H06(1,3)-COS(PO)*H06(2,3)) 36 PT=ATAN2(C0S(P0)*HO6(1,1)+SIN(PO)*H06(2,1),COS(PO)* 37 * H06(1,2)+SIN(P0)*HO6(2,2)) 38 GO TO 410 39 c 40 c»*»»* IF TRUE 41 c 42 400 CONTINUE 43 c 44 IF(H06(3,3).GT.O.) PA=90. 45 IF(H06(3.3).LT.O.) PA=-90. 46 PT=0. 47 P0=ATAN2(HO6(2,2),H06(1,2)) 48 410 CONTINUE 49 c 50 c * * * * * COORDINATES X.Y.Z ARE THE SAME AS THE LAST COLUMN OF 'H06' 51 c 52 DO 303 J=1.3 53 XYZOAT(J)=H06(J.4) 54 303 CONTINUE 55 C 56 XYZ0AT(4)=P0*C0NV 57 XYZ0AT(5)=PA*C0NV 58 XYZ0AT(6)=PT*C0NV - 218 - L i s t i n g o f NOAP1.FOR a t 1 4 : 4 1 : 1 0 on AUG 9 , 1984 f o r CC1d=DAC0 Page 2 59 C 60 RETURN 61 END - 219 - L i s t i n g of N0AP5 . F O R a t 1 4 : 4 1 : 1 1 on AUG 9 , 1 9 8 4 f o r CC1d=DAC0 Page 1 1 r» S U B R O U T I N E N O A P S ( I S O L N ) 3 \* c T H I S S U B R O U T I N E F I N D S T H E 5 D E G R E E O F F R E E D O M P U M A L O C A T I O N 4 c P A R A M E T E R S X . Y . Z . O . A . T G I V E N T H E T O O L C E N T E R L O C A T I O N V E C T O R 5 e c r- ' P ' , A N D T H E T O O L A X I S O R I E N T A T I O N V E C T O R ' A ' . o 7 \j c I N P U T S : I S O L N - F L A G S P E C I F Y I N G W H I C H O F T H E T W O 5 D E G R E E O F 8 c F R E E D O M S O L U T I O N S T O F I N D . 9 c 0 - S O L U T I O N 1 1 0 c 1 - S O L U T I O N 2 1 1 c 1 2 c C O M M O N B L O C K S R E A D : / R O B O T / - R O B O T P A R A M E T E R S 1 3 c / C O N F I G / - C U R R E N T R O B O T C O N F I G U R A T I O N 1 4 c / R O B P O S / - T O O L F R A M E . H 0 G = [ n , o . a . p ] 1 5 c 1 6 c C O M M O N B L O C K S A L T E R E D : / R O B P O S / - T O O L F R A M E . H 0 G = [ n . o , a , p ] 1 7 c 1 8 c***********************»*************#**************************************** 1 9 c 2 0 R E A L N ( 3 ) . 0 ( 3 ) , A ( 3 ) , P ( 3 ) 2 1 R E A L H O G ( 4 . 4 ) 2 2 R E A L A L P H A ( 6 ) , L E N ( 6 ) , D I S T ( 6 ) . R M I N ( 6 ) , R M A X ( 6 ) , H 0 0 ( 4 , 4 ) , H 6 G ( 4 . 4 ) . 2 3 * I N H 0 O ( 4 , 4 ) , I N H 6 A ( 4 , 4 ) 2 4 I N T E G E R I C 0 N F ( 6 ) 2 5 c 2 6 C O M M O N / R O B O T / N R D O F . A L P H A , L E N , D I S T , R M I N , R M A X , H 0 0 . H 6 G , I N H O O , I N H 6 A 2 7 C O M M O N / C O N F I G / I C O N F 2 8 C O M M O N / R O B P O S / H O G 2 9 c 3 0 c***** R E A D T H E V A L U E S O F T O O L F R A M E V E C T O R S ' A ' . ' P ' . 3 1 c 3 2 D O 3 0 1 = 1 , 3 3 3 A ( I ) = H O G ( 1 , 3 ) 3 4 P ( I ) = H O G ( 1 , 4 ) 3 5 3 0 C O N T I N U E 3 6 c 3 7 F I N D T H E T O O L O F F S E T I N T H E X D I R E C T I O N R E L A T I V E T O T H E T O O L 3 8 c***»* M O U N T I N G F L A N G E . 3 9 c 4 0 D T = H 6 G ( 1 , 4 ) 4 1 c 4 2 c***** C A L C U L A T E T H E T A 1 4 3 c 4 4 T 1 = P ( 1 ) - D T * A ( 1 ) 4 5 T 2 ' = P ( 2 ) - D T * A ( 2 ) 4 6 R S 0 = T 1 * * 2 + T 2 * * 2 4 7 A T A N 1 = A T A N 2 ( T 2 , T 1 ) 4 8 T 3 = S 0 R T ( R S 0 - D I S T ( 2 ) * * 2 ) 4 9 I F ( I C O N F ( 1 ) . E O . 1 ) T 3 = - T 3 5 0 A T A N = A T A N 2 ( D I S T ( 2 ) , T 3 ) 5 1 T H E T 1 = A T A N 1 - A T A N 5 2 c 5 3 c***** C A L C U L A T E N ( 1 ) . T H E R E A R E T W O P O S I B L E S O L U T I O N S , S E L E C T E D B Y T H E 5 4 c* » » * * V A L U E O F ' I S O L N ' . 5 5 c 5 6 C 1 = A ( 2 ) * * 2 + A ( 3 ) * * 2 5 7 C 2 = C 1 » A ( 3 ) * * 2 5 8 C 3 = C 1 * * 2 * T A N ( T H E T 1 ) * * 2 - 220 - L i s t i n g of N0AP5.FOR at 14:41:11 on AUG 9, 1984 f o r CC1d=DAC0 Page 59 C4 = 2.*C1*TAN(THET1)*A(1 )*A(2) 60 C5=C1*(A(1)**2+A(3)**2) 61 N(1)=SQRT(C2/(C3+C4+C5)) 62 IF(ISOLN.EQ.I) N(1)=-N(1) 63 C 64 c****» CALCULATE N(2) 65 c 66 c D1=N(1 )*A(1 )*A(2) 67 c D2=SQRT(4.*D1**2-4.*(A(2)**2+A(3) **2)*N( 68 c • A ( 3 ) * * 2 ) - A ( 3 ) * * 2 ) 69 c 03 = 2. *(A(2)**2+A(3)**2) 70 N(2)=N(1)*TAN(THET1 ) 71 c 72 c****» CALCULATE N(3) 73 c 74 N(3)=SQRT(1.-N(1 )**2-N(2)**2) 75 DOT=ABS(N(1)*A(i)+N(2)*A(2)+N(3)* A(3 ) ) 76 IF(DOT.GT..OOO1) N(3)=-N(3) 77 c 78 c 79 c***** CALCULATE BASIS VECTOR '0' 80 c 81 CALL XPROD(A,N,0) 82 c 83 c 84 c****» WRITE BASIS VECTORS 'N','0' INTO 'HOG'. 85 c 86 DO 200 1=1,3 87 HOG(I, 1 )=N(I ) 88 HOG(I,2)=0(I ) 89 200 CONTINUE 90 c 91 RETURN 92 END - 221 - L i s t i n g of NORM.FOR a t 14:41:11 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE NORM(N1,N2,A) 2 C 3 C THIS SUBROUTINE CALCULATES THE NORMAL VECTOR, 'A', WHICH 4 C BISECTS THE TWO PARENT SURFACE NORMALS 'N1'.'N2'. 5 C 6 C INPUTS: 7 C N1(3,3),N2(3,3)=N0RMAL VECTORS OF TWO SURFACES AT 8 C INTERSECTION POINTS 1-1. I, 1+1. 9 C 10 C OUTPUT: 1 1 C A(3)=UNIT BISECTOR OF N1 AND N2 AT POINT 1+1. 12 C 13 14 C 15 C 16 REAL N1(3,3),N2(3,3).A(3).LX,LY,LZ.L 17 C 18 D1=N1(3,1)+N2(3,1) 19 D2=N1(3,2)+N2(3,2) 20 D3=N1(3,3)+N2(3,3) 21 C 22 LX=D1**2 23 LY=D2**2 24 LZ=D3**2 25 C 26 L=SQRT(LX+LY+LZ) 27 C 28 A(1)=D1/L 29 A(2)=D2/L 30 A(3)=D3/L 31 C 32 RETURN 33 END - 222 - L i s t i n g o f OPEN.FOR a t 1 4 : 4 1 : 1 1 on AUG 9 , 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE OPEN(IUNIT.NAME,ITYPE.IREC) 2 C 3 C THIS SUBROUTINE OPENS A FILE CALLED (NAME), AND ASSIGNS 4 C IT TO LOGIC UNIT (IUNIT). THE TYPE OF FILE IS DETERMINED 5 C BY (ITYPE). THIS SUBROUTINE IS COMPATIBLE WITH VAX-11 FORTRAN. 6 C (IREC) IS THE RECORD LENGTH FOR DIRECT ACCESS FILES. 7 C 8 C INPUTS: 9 C IUNIT=A VALID LOGIC UNIT NUMBER 10 C NAME=A COMPLETE FILE NAME 11 C ITYPE=FILE TYPE 12 C 1=UNFORMATTED,EXI STING 13 C 2=UNFORMATTED,NEW 14 C 3 = UNF0RMATTED,DIRECT ACCESS,EXI STING 15 C 4=F0RMATTED,SEQUENTIAL,EXISTING 16 C 5=UNF0RMATTED,DIRECT ACCESS,NEW 17 c 6=F0RMATTED,SEQUENTIAL,NEW 18 c IREC=RECORD LENGTH (FOR FILE TYPE 5 ONLY) 19 c 20 c OUTPUTS: NONE. 21 c 22 c 23 c».«.«»»»«*»««.*»*«««.*.«*«.,»«*•».».•«,.»» 24 c 25 BYTE NAME(20) 26 c 27 c** CASE OF (ITYPE) 28 . c 29 IF(ITYPE.LT.1.OR.ITYPE.GT.6) GO TO 600 30 GO TO (100,200.300,400.500.510) ITYPE 31 c 32 c** CASE (1) 33 c 34 c*** * * OPEN EXISTING, UNFORMATTED, SEQUENTIAL FILE. 35 c 36 100 CONTINUE 37 OPEN (UNIT=IUNIT,FILE=NAME,FORM='UNFORMATTED',STATUS='OLD', 38 * ERR=10) 39 GO TO 900 40 c 41 c** CASE (2) 42 c 43 c***** OPEN NEW. UNFORMATTED, SEQUENTIAL FILE. 44 c 45 200 CONTINUE 46 OPEN (UNIT=IUNIT,FILE=NAME,FORM='UNFORMATTED',STATUS3'NEW', 47 • ERR=10) 48 GO TO 900 49 c 50 c*» CASE(3) 51 c 52 c*»»** OPEN EXISTING, DIRECT ACCESS FILE. 53 c 54 300 CONTINUE 55 OPEN (UNIT=IUNIT.FILE=NAME.ACCESS''DIRECT',STATUS='OLD', 56 * ERR=10) 57 GO TO 900 58 c - 223 - L i s t i n g o f OPEN.FOR a t 1 4 : 4 1 : 1 1 on AUG 9 , 1984 f o r CC1d=DAC0 Page 2 59 C** CASE(4) 60 C 61 c*** * * OPEN EXISTING, FORMATTED, SEQUENTIAL FILE. 62 c 63 40O CONTINUE 64 OPEN (UNIT=IUNIT,FILE=NAME,STATUS='OLD',ERR=10) 65 GO TO 900 66 C 67 c«* CASE(5) 68 C 69 c***»* OPEN NEW, DIRECT ACCESS FILE. 70 c 71 500 CONTINUE 72 OPEN (UNIT=IUNIT,FILE=NAME,FORM='UNFORMATTED', 73 * ACCESS='DIRECT',RECL=IREC,STATUS-'NEW',ERR=10) 74 GO TO 90O 75 C 76 c* * * * * OPEN NEW,FORMATTED FILE. 77 c 78 510 CONTINUE 79 OPEN (UNIT=IUNIT,FILE=NAME.STATUS='NEW,ERR=10) 80 GO TO 900 81 C 82 c** EL SE 83 c 84 600 CONTINUE 85 WRITE(5.20) 86 20 FORMAT(' VALUE OF PARAMETER ITYPE NOT VALID') 87 GO TO 900 88 C 89 10 CONTINUE 90 WRITE(5,30) 91 30 FORMAT(' ERROR ENCOUNTERED IN OPEN STATEMENT IN 92 * OPEN') 93 C 94 C** END CASE 95 C 96 900 CONTINUE 97 RETURN 98 END - 2 2 4 - :1ng of PROCES. .FOR at 14:41:34 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE PROCES 2 C 3 C THIS PROGRAM READS THE WELDING PARAMETERS. R, N1, N2 FROM CAD 4 C FI L E . CALCULATES ARCLENGTH S. THE POSITION VECTOR R, AND THE 5 C TOOL FRAME AXES T. A, B. 6 C THE RESULTS ARE STORED IN AN OUTPUT FILE. 7 C 8 C INPUTS: FROM CAD FILE 9 C 10 C JTYPE- JOINT TYPE CODE 11 C XSECT- WELD CROSS SECTIONAL AREA 12 C T1.T2- PARENT METAL THICKNESSES 13 C NPOINT- NUMBER OF LOCATIONS IN FILE 14 C ICURVE- SEAM TYPE- 0=STRAIGHT LINE 15 C 1=SPACE CURVE 16 C INDEX=POINT INDEX. 17 C R(3,3)=P0SITI0N VECTORS FOR POINTS 1-1. I . 1+1 . 18 C N1(3.3),N2(3.3)=UNIT NORMAL VECTORS OF TW ] i SURFACES AT 19 C POINTS 1-1. I . 1+1. 20 C 21 c OUTPUTS: TO OUTPUT F I L E . 22 c 23 c JTYPE- JOINT TYPE CODE 24 c XSECT- WELD CROSS SECTIONAL AREA 25 c T1.T2- PARENT METAL THICKNESSES 26 c NPOINT- NUMBER OF LOCATIONS IN FILE 27 c ICURVE- SEAM TYPE- 0=STRAIGHT LINE 28 c 1=SPACE CURVE 29 c INDEX- POINT INDEX 30 c S(3)=ARCLENGTH AT POINTS 1-1, I . 1+1. 31 c R(3,3)=P0SITI0N VECTORS FOR POINTS 1-1, I , 1 + 1 . 32 c T(3)=TANGENT VECTOR OF CURVE AT POINT I . 33 c A(3)=UNIT BISECTOR OF N1, N2 AT POINT I . 34 c B(3)=CR0SS PRODUCT T X A. 35 c 36 c FILES READ: CAD- THE CAD FILE 37 c 38 c FILES WRITTEN: WPCDAT- THE OUTPUT FILE 39 c 40 c SUBROUTINES CALLED: ARCLEN 41 c NORM 42 c OPEN 43 c SHUT 44 c TANGEN 45 c XPROD 46 c 47 48 c 49 IMPLICIT DOUBLE PRECISION (D) 50 BYTE CAD(20), WPCDAT(20) 51 REAL R ( 3 , 3 ) , N 1 ( 3 . 3 ) , N 2 ( 3 , 3 ) . T ( 3 ) . A ( 3 ) . B ( 3 ) , S ( 3 ) 52 COMMON /CAD/CAD 53 COMMON /WPCDAT/WPCDAT 54 COMMON /CADLST/CADLST 55 COMMON /WPCLST/WPCLST 56 DATA R/9*0./ 57 DATA ICAD/1/,IOUT/2/ 58 c - 225 - L i s t i n g o f PROCES.FOR a t 14:41:34 on AUG 9, 1984 f o r CC1d=DAC0 Page 59 c»*«** OPEN CAD INPUT FILE AS L.U. ICAD. 60 OPEN OUTPUT FILE AS L.U. IOUT. 61 C 62 CALL OPEN(ICAD,CAD,1,1) 63 CALL OPEN(IOUT,WPCDAT,2,1) 64 C 65 INITIALIZE ARCLENGTH 'S', FLAG 'I FLAG' 66 c 67 S(3)=0. 68 IFLAG=0 69 c 70 c**»** READ JOINT TYPE. X-SECTIONAL AREA, PARENT THICKNESSES 71 c***»* 1ST RECORD OF 'CAD' AND WRITE THEM INTO 1ST RECORD OF 72 c 73 READ(ICAD) JTYPE,XSECT,T1,T2,NPOINT,ICURVE 74 WRITE(IOUT) JTYPE,XSECT,T1.T2,NPOINT,ICURVE 75 c 76 c 77 C** WHILE (INPUT .LE. EOF) 78 c 79 150 CONTINUE 80 C 81 170 CONTINUE 82 C 83 c***** READ INDEX, R, N1,*N2, FROM CAD FILE. 84 c 85 READ(ICAD,END=555) INDEX,(R(3,I),I=1.3).(N1(3.K),K= 86 * (N2(3,L),L=1,3) 87 c 88 c»**** READ FIRST TWO POINTS BEFORE BEGINNING CALCULATIONS 89 c* * * * * SET FLAGS IF SECOND POINT OR LAST POINT IS READ. 90 c* » * * * THESE POINTS REQUIRE A MODIFIED PROCEDURE. 91 c 92 c***»* FIRST POINT 93 c 94 IF(INDEX.EQ.1) GO TO 700 95 IFLAG=2 96 c 97 SECOND POINT 98 c 99 IF(INDEX.EQ.2) IFLAG=1 100 GO TO 556 101 c 102 Q* * * * * LAST POINT 103 c 104 555 CONTINUE 105 INDEX=INDEX+1 106 IF LAG = 3 107 556 CONTINUE 108 C 109 c 1 10 Q** * * * CALCULATE ARCLENGTH S AT POSITION R. 'DELS' IS THE 1 1 1 ARCLENGTH FROM R(I-1) TO R(I) 112 c 113 CALL ARCLEN(R.DELS) 1 14 S(3)=S(2)+DELS 1 15 c 1 16 CALCULATE THE TANGENT VECTOR 'T' AT POSITION 'R' - 226 - L i s t i n g of PROCES.FOR at 14:41:34 on AUG 9, 1984 f o r CCid=DACO Page 3 117 C 118 CALL TANGEN(R,S,T.IFLAG) 1 19 C 120 c***»* CALCULATE THE NORMAL VECTOR 'A' FROM THE SURFACE NORMALS 121 c»*»*» 'NI'.'N2' 122 c 123 CALL N0RM(N1,N2,A) 124 c 125 c 126 c**»** CALCULATE THE LATERAL VECTOR 'B', B=A X T (CROSS PRODUCT) 127 c 128 CALL XPROD(A,T,B) 129 c 130 c***»* A=T X A (CROSS PRODUCT). THIS STEP ENSURES AN ORTHOGONAL 131 c***** FRAME. 132 c 133 CALL XPROD(T,B,A) 134 c 135 c»**** WRITE RESULTS INTO OUTPUT FILE. 136 c 137 dd=INDEX-1 138 WRITE(IOUT) dd,S(2),(R(2.I).I=1,3).(T(K),K=1,3),(A(L),L=1 139 * (B(M),M=1,3) 140 c 141 c***«» UPDATE CURRENT POSITION, I, BY REDEFINING POINT I AS 142 c****-» POINT 1-1. 143 . c - 144 DO 300 K=1,3 145 R(1,K)=R(2,K) 146 300 CONTINUE 147 S(1)=S(2) 148 c 149 700 CONTINUE 150 c 151 c***** SIMILARLY, REDEFINE POINT 1+1 AS POINT I. 152 c 153 DO 301 K=1,3 154 R(2,K)'R(3.K) 155 N1(2,K)=N1(3,K) 156 N2(2.K)=N2(3,K) 157 301 CONTINUE 158 S(2)=S(3) 159 C 160 100 IF (I FLAG.NE.3) GOTO 150 161 C 162 C** ENC l WHILE 163 C 164 C 165 CLOSE INPUT AND OUTPUT FILES. 166 c 167 CALL SHUT(ICAD) 168 CALL SHUT(IOUT) 169 c 170 RETURN 171 END - 227 - L i s t i n g of RDFILE.FOR a t 14:41:35 on AUG 9. 1984 f o r CCid=DACO Page 1 1 SUBROUTINE RDF ILE(IUNIT,IRECNO.NAME.LEN,IEND) 2 C 3 C SUBROUTINE RDFILE READS RECORD NUMBER 'IRECNO' OF A FILE OPEN 4 C ON LOGICAL UNIT 'IUNIT'. THE RECORD IS RETURNED AS A 5 C CHARACTER STRING 'NAME' OF LENGTH 'LEN'. 6 C 7 c INPUTS: IUNIT- LOGICAL UNIT NUMBER OF OPEN FILE 8 c IRECNO- RECORD NUMBER TO BE READ 9 c 10 c OUTPUTS: NAME- BYTE ARRAY OF LENGTH 'LEN' CONTAINING CONTENTS 1 1 c OF RECORD 'IRECNO'. IE. BYTE NAME(LEN) 12 c LEN- LENGTH OF BYTE ARRAY. 13 c IEND- FLAG INDICATING END OF FILE ENCOUNTERED DURING 14 c READ. 15 c IEND=0 READ SUCCESSFUL 16 c IEND=1 END OF FILE ENCOUNTERED 17 c 18 c 19 c FUNCTIONS CALLED: LENGTH- FINDS THE LENGTH OF A CHARACTER 20 c STRING 21 c 22 c 23 Q* *********************************************************************• 24 c 25 BYTE NAME(LEN) 26 c 27 IEND=0 28 c 29 c** IF (IRECNO .GT. 1) 30 c 31 IF (IRECNO .EO. 1) GO TO 100 32 c 33 c»**«* IF (IRECNO.GT.1) REWIND FILE. STEP FORWARD TO RECORD ' IRECNO' 34 c****» AND READ CONTENTS. 35 c 36 REWIND IUNIT 37 DO 30O 1 = 1 ,IRECNO-1 38 READ(IUNIT,150,END=400) 39 300 CONTINUE 40 READ(IUNIT.150.END=400) (NAME(J),d=1.LEN) 41 GO TO 200 42 c 43 c** ELSE 44 c 45 10O CONTINUE 46 c 47 Q** * * * IF FIRST RECORD IS TO BE READ, REWIND FILE AND READ CONTENTS 48 c***»* OF FIRST RECORD. 49 c 50 REWIND IUNIT 51 READ(IUNIT,150,END=400) (NAME(J),J=1,LEN) 52 c 53 c** END IF 54 c 55 200 CONTINUE 56 ' c 57 150 FORMAT(80A1) 58 c - 228 - L i s t i n g of RDFILE.FOR a t 14:41:35 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 c***»* FIND LENGTH OF CHARACTER STRING AND PAD TO RIGHT WITH ASCII 60 c****» ZEROS. 61 C 62 L=LENGTH(NAME,LEN) 63 NAME(L+1)=0 64 C 65 GO TO 500 66 C e 7 c***** END OF FILE ENCOUNTERED. SET FLAG 'IEND'. 68 C 69 40O CONTINUE 70 IEND=1 71 C 72 500 CONTINUE 73 RETURN 74 END - 229 - L i s t i n g of ROTAT.FOR at 14:41:35 on AUG 9, 1984 for CC1d=DAC0 Page 1 1 SUBROUTINE ROTAT(H,A,B) 2 C 3 C THIS SUBROUTINE PERFORMS THE ROTATIONAL PART OF A 4 C HOMOGENEOUS TRANSFORMATION ,B=H*A, WHERE H(4,4) IS A 5 C HOMOGENEOUS TRANSFORMATION MATRIX AND A(3).B(3) ARE VECTORS. 6 C 7 C INPUTS: H(4,4)- HOMOGENEOUS TRANSFORMATION MATRIX 8 C A(3) - VECTOR 9 C 10 C OUTPUTS: B(3) - VECTOR 1 1 C 12 c*******»***************************************** ****************** 13 C 14 REAL H(4.4),A(3),B(3) 15 C 16 B(1)=H(1,1)*A(1)+H(1,2)*A(2)+H(1.3)*A(3) 17 B(2)=H(2,1)*A(1)+H(2,2)*A(2)+H(2,3)*A(3) 18 B(3)=H(3,1)*A(1)+H(3.2)*A(2)-HH(3.3)*A(3) 13 C 20 RETURN 21 END - 230 - L i s t i n g o f SCENE.FOR a t 14:41:35 on AUG 9. 1984 f o r CCid=DACO Page 1 1 SUBROUTINE SCENE 2 C 3 C THIS SUBROUTINE DRAWS THE WORKSTATION ,OBJECT AND SEAMS. 4 C 5 C INPUTS: NONE 6 C 7 C OUTPUTS: NONE 8 C ' 9 C COMMON BLOCKS ACCESSED: /VIEW/./MATRIX/ 10 C 11 C COMMON BLOCKS CHANGED: /MATRIX/ 12 C 13 C FILES READ OR WRITTEN: NONE 14 C 15 C SUBROUTINES CALLED: DRAXES 16 C DRPANL 17 C DRSEAM 18 C TABPOS 19 C TABSET 20 C 21 C 'IGL' GRAPHICS SUBROUTINES: CMCLOS 22 C CMOPEN 23 C DELSEG 24 C EYEBAL 25 C NEWPAG 26 C VRP3D 27 C 28 C 29 £*****«•* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 30 C 31 BYTE BEL(3).YES.N0 32 C 33 REAL VRP(3),EYEP0S(3) ,VAR(6) ,SHOT(4,4),SHOW(4,4),H(4,4) 34 REAL VERT(1000).FACES(100) 35 DOUBLE PRECISION HOT(4,4),HOW(4,4) 36 C 37 INTEGER IOBJ(10,2),I FACE(100,2),IDIR(1),IPICK(100) 38 C 39 DATA BEL/3*7/,YES/89/,NO/78/ 40 C 41 COMMON /DATA/VERT,FACES,IFACE,IOBJ 42 COMMON /VIEW/EYEPOS,VRP 43 COMMON /MATRIX/HOT,HOW,SHOT,SHOW 44 C 45 C***»* CLOSE IGL COMMUNICATIONS 46 C 47 CALL CMCLOS 48 C 49 C***»* SET WORKTABLE PARAMETERS 50 C 51 CALL TABSET(VAR ) 52 C 53 C****» CALCULATE RESULTING HOMOGENEOUS TRANSFORMATIONS 'HOT','HOW'. 54 C 55 CALL TABPOS(VAR,HOT,HOW) 56 C 57 C***»* CONVERT DOUBLE PRECISION 'HOT','HOW', INTO SINGLE PRECISION 58 C*«*** 'SHOT'. 'SHOW'. - 231 - : 1ng of SC ENE . FOR a t 14:41:35 on AUG 9, 1984 f o r CC1d=DAC0 I 59 C 60 CALL MATCNV(HOT,SHOT,1) 61 CALL MATCNV(HOW,SHOW.1) 62 C 63 C* * * * » OPEN IGL COMMUNICATIONS 64 C 65 CALL CMOPEN 66 C 67 C* * * * * DELETE ALL SEGMENTS AND CLEAR SCREEN 68 C 69 CALL DELSEG(-1 ) 70 CALL NEWPAG 71 C 72 C* * * * * SET VIEW REFERENCE POINT 'VRP' . EYE POSITION 73 C 74 CALL VRP3D(VRP(1),VRP(2 ) ,VRP(3) ) 75 CALL EYEBAL( EYEPOS( 1 ) , EYEP0S(2) . EYEPOSO) ) 76 C 77 c* * * * * DRAW WORLD COORDINATE FRAME AXES 78 c 79 CALL DRAXES 80 c 81 c* * * * * DRAW OBJECT AS A POLYHEDRON 82 c 83 CALL DRPANL(SHOT,1) 84 c 85 c* * * * * DRAW THE SEAMS 86 c 87 CALL DRSEAM(SHOW) 88 c 89 RETURN 90 END - 232 - L i s t i n g o f SETUP.FOR a t 14:42:07 on AUG 9, 1984 f o r CCid=DACO Page 1 SUBROUTINE SETUP 2 C 3 C THIS SUBROUTINE PROCESSES THE CAD SEAM DATA 4 C 5 C 6 C 7 C FILES READ: FILIST 8 c CADLST 9 c CAD 10 c WPCLST 11 c 12 c FILES WRITTEN: WPCDAT 13 c 14 c SUBROUTINES: OPEN 15 c PROCES 16 c RDFILE 17 c SHUT 18 c 19 c 20 21 c 22 c 23 BYTE CAD(20), WPCDAT(20),CADLST(20) 24 BYTE WPCLST(20),FILIST(20) 25 c 26 COMMON / F I L I S T / F I L I S T 27 COMMON /CADLST/CADLST 28 COMMON /WPCLST/WPCLST 29 COMMON /CAD/CAD 30 COMMON /WPCDAT/WPCDAT 31 c 32 OPEN CAD SEAM DATA FILE LIST, 'CADLST' 33 c 34 CALL OPEN(1.CADLST,4,1) 35 c 36 c***»* OPEN PROCESSED SEAM DATA FILE LIST, 'WPCLST' 37 c 38 CALL 0PEN(2.WPCLST,4,1) 39 c 40 c***** SKIP FIRST RECORD, THEN READ THE NUMBER OF SEAMS 41 c***»* 'CADLST' 42 c 43 READ(1,*) 44 READ(1,* ) NSEAMS 45 c 46 c* * * * * REPEAT FOR ALL SEAMS... 47 c 48 DO 110 1=1.NSEAMS 49 c 50 FIND 'CAD','WPCDAT' FILES FOR SEAM I 51 c 52 ' CALL RDFILE(1,1+2,CAD,20,I END) 53 CALL RDFILE(2,I+2,WPCDAT,20,I END) 54 c 55 c* * * * * CLOSE LOGIC UNITS 1,2 56 c 57 CALL SHUT(1) 58 CALL SHUT(2) - 233 - L i s t i n g of SETUP.FOR a t 14:42:07 on AUG 9. 1984 f o r CC1d=DAC0 Page 2 59 C 60 C***»* PROCESS 'CAD' SEAM DATA AND GENERATE 'WPCDAT' SEAM DATA 61 C 62 CALL PROCES 63 C 64 C»**** REOPEN LOGIC UNITS 1,2 65 C 66 CALL OPEN( 1 .CADLST,4,1) 67 CALL 0PEN(2,WPCLST,4,1) . 68 C 69 110 CONTINUE 70 C 71 C 72 CALL SHUT(1) 73 CALL SHUT(2) 74 C 75 RETURN 76 END - 234 - L i s t i n g of SHUT.FOR a t 14:42:07 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE SHUT(IUNIT) 2 C 3 C THIS SUBROUTINE CLOSES LOGIC UNIT IUNIT. 4 C 5 c * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 6 C 7 CLOSE(UNIT=IUNIT) 8 C 9 RETURN 10 END - 235 - L i s t i n g o f TABPOS.FOR a t 14 :42 :08 on AUG 9 . 1984 f o r CC1d=DAC0 Page 1 1 n SUBROUTINE TABPOS(VAR,HOT,HOW) •c 3 C THIS SUBROUTINE CALCULATES THE TABLE COORDINATE TO WORLD 4 c COORDINATE TRANSFORMATION MATRIX, ' H O T ' . THE FIXED TABLE 5 c PARAMETERS ARE READ FROM F I L E ' T A B L E ' . TABLE POSITION 6 c VARIABLES , ' V A R ( I ) ' , A R E PASSED FROM THE MAIN PROGRAM. THESE 7 c VARIABLES ARE ANGLES AND/OR DISPLACEMENTS, DEPENDING ON THE 8 c TABLE DESIGN. THE PROGRAM CALCULATES THE TABLE TRANSFORMATION, 9 c RELATIVE TO THE FIRST JOINT FRAME. THIS TRANSFORMATION IS THEI 10 c PREMULTIPLIED BY A TRANSFORMATION ' H P R E ' FROM THE WORLD FRAME 1 1 c TO THE FIRST JOINT FRAME AND THE RESULT IS POSTMULTIPLIED BY 12 c A TRANSFORMATION 'HPOST ' FROM THE LAST JOINT FRAME TO THE 13 c TABLE REFERENCE FRAME. 14 c 15 c INPUTS: 16 c V A R ( 6 ) - ARRAY OF TABLE JOINT COORDINATES 17 c I E . ANGLES (DEG) AND DISPLACEMENTS 18 c OUTPUTS: 19 c H 0 T ( 4 . 4 ) - HOMOGENEOUS TRANSFORMATION MATRIX FROM 20 c TABLE FRAME ' F T ' TO WORLD FRAME ' F O ' 21 c H 0 W ( 4 , 4 ) - HOMOGENEOUS TRANSFORMATION MATRIX FROM 22 c WORKPIECE FRAME 'FW' TO WORLD FRAME ' F O ' 23 c 24 c COMMON BLOCKS ACCESSED: / I O F I L E / 25 c 26 c F I L E S READ: 27 c 28 c STOVAR- CONTAINS CURRENT ' V A R ' AND CURRENT ' H O T ' 29 c T A B L E - F I L E CONTAINING WORK TABLE SPECIFICATIONS 30 c 31 c * * * * * * * 32 c FORMAT OF T A B L E : (L IST-DIRECTED FORMAT) 33 c 34 c RECORD JOINT CONTENTS DESCRIPTION 35 c NO. NO. 36 c 1 NUMDOF NUMBER OF DEGREE > OF FREEDOM 37 c 38 c 2 TEXT HEADING 39 c 40 c 3 1 I D , I T Y P E , S T E P , ID-JOINT T Y P E , 1 =ROTARY 41 c 2=PRISMATIC 42 c RMIN,RMAX, I T Y P E - 1=DISCRETE 2=C0NTINU0US 43 c A L P H A , D I S T , L E N S T E P - STEP SIZE FOR DISCRETE 44 c POSITIONS 45 c RMIN,RMAX- MINIMUM,MAXIMUM 46 c LIMITS OF JOINT MOT 47 c ALPHA- TWIST ANGLE 48 c DISP- JOINT DISPLACEMENT 49 c L E N - JOINT LENGTH 50 c 51 c 4 2 AS ABOVE, FOR 52 c JOINT 2 53 c 54 c 5 3 JOINT 3 55 c 56 c 6 4 JOINT 4 ' ' 57 c 58 c 7 5 JOINT 5 - 236 - L i s t i n g of TABPOS.FOR at 14:42:08 on AUG 9, 1984 for CC1d=DAC0 Page 2 59 C 60 C 8 6 JOINT 6 ' ' 61 C 62 C 9 TEXT HEADING 63 C 84 c 10 N/A HTW(1,J) .J=1,4 FIRST ROW OF WORKPIECE FRAME 65 c TO TABLE FRAME TRANSFORMATION 66 c MATRIX 'HTW' 67 c 68 c 11 N/A HTW(2,J).d=1,4 SECOND ROW OF 'HTW' 69 c 70 c 12 N/A HTW(3,J),J=1,4 THIRD ROW OF 'HTW' 71 c 72 c 13 N/A HTW(4,J) ,J= 1 ,4 FOURTH ROW OF 'HTW' 73 c 74 c 14 TEXT HEADING 75 c 76 c 15 N/A HPRE( 1 ,J) ,J=1,4 FIRST ROW OF FIRST JOINT TO 77 c WORLD FRAME TRANSFORMATION 78 c MATRIX 'HPRE' 79 c 80 c 16 N/A HPRE(2,J ) ,J=1 ,4 SECOND ROW OF 'HPRE' 81 c 82 c 17 N/A HPRE(3,J ) ,J= 1 ,4 THIRD ROW OF 'HPRE' 83 c 84 c 18 N/A HPRE(4,J ) ,J=1 ,4 FOURTH ROW OF 'HPRE' 85 c 86 c 19 N/A TEXT HEADING 87 c 88 c 20 N/A HPOST( 1 . J) ,J=1,4 FIRST ROW OF TA3LE FRAME TO 89 c LAST JOINT TRANSFORMATION 90 c MATRIX 'HPOST' 91 c 92 c 21 N/A HP0ST(2,d),J=1,4 SECOND ROW OF 'HPOST' 93 c 94- c 22 N/A HPOST(3,J),J= 1 ,4 THIRD ROW OF 'HPOST' 95 c 96 c 23 N/A HPOST(4,J) ,J= 1 ,4 FOURTH ROW OF 'HPOST' 97 c 98 c 99 c FORMAT OF 'STOVAR' 100 c 101 c RECORD CONTENTS ; DESCRIPTION 102 c NO. 103 c 1 TEXT HEADING 104 c 2 VAR( 1 ) CURRENT POSITION OF JOINT 1 105 c 3 VAR(2) CURRENT POSITION OF JOINT 2 106 c 107 c 7 VAR(6) CURRENT POSITION OF JOINT 6 108 c 109 c 1 10 1 1 1 c 112 IMPLICIT DOUBLE PRECISION (D) 113 BYTE TABLE(20) 1 14 DOUBLE PRECISION HA(4,4),HB(4,4) , H0T(4,4), HPRE(4,4), 1 15 * HP0ST(4,4),HTEMP(4,4), DTHETA(6),DALPHA(6),HTW(4,4),HOW(4,4) 116 REAL STEP(6),RMIN(6),RMAX(6),THETA(6),ALPHA(6),DISP(6),LEN(6) - 237 - L i s t i n g of TABPOS.FOR a t 14:42:08 on AUG 9, 1984 f o r CCid=DACO Page 3 117 REAL VAR(6) 1 18 INTEGER ID(6),ITYPE(6) 1 19 C 120 COMMON /TABLE/TABLE 121 C 122 C 123 OPEN THE TABLE SPECIFICATION FILE 'TABLE- 124 c 125 CALL OPEN(1.TABLE,4,1) 126 c 127 READ THE NUMBER OF DEGREES OF FREEDOM, 'NUMDOF'. 128 c 129 READ( 1.*) NUMDOF 130 c 131 c***»* SKIP A RECORD, THEN READ THE TABLE LINK PARAMETERS 132 c 133 READ(1,*) 134 DO 123 1=1,NUMDOF 135 READ( 1 , * ) ID( I ) , I T Y P E ( I ) , S T E P ( I ) , R M I N ( I ) , , RMAX ( I ) , THETA ( '. 136 * ALPHA(I),DISP(I).LEN(I) 137 123 CONTINUE 138 C 139 c**«** SKIP A RECORD, THEN READ THE TRANSFORMATION MATRIX 'HTW' 140 c 141 READ(1,*) 142 DO 131 1=1,4 143 READ(1,*) (HTW(I,d),J =1,4) 144 131 CONTINUE 145 . C 146 SKIP A RECORD, THEN READ THE TRANSFORMATION MATRIX 'HPRE' 147 c 148 READ(1 , * ) 149 DO 124 1=1,4 150 READ( 1 , * ) (HPRE(I,d),J=1,4) 151 124 CONTINUE 152 C 153 c»**** SKIP A RECORD, THEN READ THE TRANSFORMATION MATRIX 'HPOST' 154 c 155 READ(1,* ) 156 DO 125 1=1,4 157 READ( 1 ,* ) (HPOST(I,J),J=1,4) 158 125 CONTINUE 159 C 160 CALL SHUT(1) 161 C 162 c****» 'OCONV IS A CONVERSION FACTOR FROM DEGREES TO RADIANS 163 c 164 DC0NV=4.DO*DATAN(1.D0)/180. 165 c 166 C** WHILE (I .LE. NUMDOF) 167 C 168 DO 40O 1=1.NUMDOF 169 I F ( I D ( I ) . E O . 1 ) VAR(I)=VAR(I)*DCONV 170 DTHETA(I)=THETA(I)*DCONV 171 DALPHA(I)=ALPHA(I)*DCONV 172 C 173 C** IF THE JOINT IS REVOLUTE (ROTATIONAL) 174 c - 238 - s t i n g o f T A B P O S . F O R a t 1 4 : 4 2 : 0 8 o n A U G 9 , 1 9 8 4 f o r C C i d = D A C O P a g e 1 7 5 I F ( I D ( I ) . N E . 1 ) G O T O 1 0 0 1 7 6 C 1 7 7 c* * + * * C A L C U L A T E T H E D E N A V I T - H A R T E N B E R G T R A N S F O R M A T I O N 1 7 8 c***»* R E L A T I N G L I N K I T O L I N K 1 - 1 1 7 9 c 1 8 0 H A ( 1 , 1 ) = C O S ( V A R ( I ) ) 1 8 1 H A ( 1 , 2 ) = - S I N ( V A R ( I ) ) * C O S ( D A L P H A ( I ) ) 1 8 2 H A ( 1 , 3 ) = S I N ( V A R ( I ) ) * S I N ( D A L P H A ( I ) ) 1 8 3 H A ( 1 , 4 ) = L E N ( I ) * C O S ( V A R ( I ) ) 1 8 4 H A ( 2 , 1 ) = S I N ( V A R ( I ) ) 1 8 5 H A ( 2 , 2 ) = C 0 S ( V A R ( I ) ) " C O S ( D A L P H A ( I ) ) 1 8 6 H A ( 2 , 3 ) = - C 0 S ( V A R ( I ) ) * S I N ( D A L P H A ( I ) ) 1 8 7 H A ( 2 , 4 ) = L E N ( I ) * S I N ( V A R ( I ) ) 1 8 8 H A ( 3 , 1 ) = 0 . 1 8 9 H A ( 3 , 2 ) = S I N ( D A L P H A ( I ) ) 1 9 0 H A ( 3 , 3 ) = C O S ( D A L P H A ( I ) ) 1 9 1 H A ( 3 , 4 ) = D I S P ( I ) 1 9 2 H A ( 4 , 1 ) = 0 . 1 9 3 H A ( 4 , 2 ) = 0 . 1 9 4 H A ( 4 , 3 ) = 0 . 1 9 5 H A ( 4 , 4 ) = 1 . 1 9 6 c 1 9 7 G O T O 2 0 0 1 9 8 c 1 9 9 c** E L S E T H E J O I N T I S P R I S M A T I C ( T R A N S L A T I O N A L ) 2 0 0 c 2 0 1 1 0 0 C O N T I N U E 2 0 2 c 2 0 3 c****» C A L C U L A T E T H E D E N A V I T - H A R T E N B E R G T R A N S F O R M A T I O N 2 0 4 c***»» R E L A T I N G L I N K I T O L I N K 1 - 1 2 0 5 c 2 0 6 H A ( 1 , 1 ) = C O S ( D T H E T A ( I ) ) 2 0 7 H A ( 1 , 2 ) = - S I N ( D T H E T A ( I ) ) * C O S ( D A L P H A ( I ) ) 2 0 8 H A ( 1 , 3 ) = S I N ( D T H E T A ( I ) ) * S I N ( D A L P H A ( I ) ) 2 0 9 H A ( 1 , 4 ) = L E N ( I ) * C 0 S ( D T H E T A ( I ) ) 2 1 0 H A ( 2 . 1 ) = S I N ( D T H E T A ( I ) ) 2 1 1 H A ( 2 , 2 ) = C 0 S ( D T H E T A ( I ) ) " C O S ( D A L P H A ( I ) ) 2 1 2 H A ( 2 , 3 ) = - C O S ( D T H E T A ( I ) ) * S I N ( D A L P H A ( I ) ) 2 1 3 H A ( 2 , 4 ) = L E N ( I ) * S I N ( D T H E T A ( I,) ) 2 1 4 H A ( 3 , 1 ) = 0 . 2 1 5 H A ( 3 , 2 ) = S I N ( D A L P H A ( I ) ) 2 1 6 H A ( 3 , 3 ) = C O S ( D A L P H A ( I ) ) 2 1 7 H A ( 3 , 4 ) = D I S P ( I ) + V A R ( I ) 2 1 8 H A ( 4 , 1 ) = 0 . 2 1 9 H A ( 4 , 2 ) = 0 . 2 2 0 H A ( 4 , 3 ) = 0 . 2 2 1 H A ( 4 , 4 ) = 1 . 2 2 2 c 2 2 3 2 0 0 C O N T I N U E 2 2 4 c 2 2 5 C * * E N D I F 2 2 6 C 2 2 7 C 2 2 8 C * * I F ( I . E Q . 1 ) 2 2 9 C 2 3 0 I F ( I . G T . 1 ) G O T O 3 0 0 2 3 1 C 2 3 2 D O 1 0 1 J = 1 , 4 M A T R I X M A T R I X - 239 - L i s t i n g of TABPOS.FOR a t 14:42:08 on AUG 9, 1984 f o r CC1d=DAC0 Page 5 233 DO 102 K=1.4 234 HB(J,K)=HA(d , K ) 235 102 CONTINUE 236 101 CONTINUE 237 C 238 GO TO 500 239 C 240 c*» ELSE 241 c 242 300 CONTINUE 243 C 244 c***»* MULTIPLY INTERMEDIATE TRANSFORMATION MATRICES 245 c 246 CALL HMULT(HB,HA,HOT) 247 c 248 c***** THEN LET 'HB' := 'HOT' 249 c 250 DO 666 d= 1 ,4 251 DO 777 K=1,4 252 HB(d,K)=HOT(d,K) 253 777 CONTINUE 254 666 CONTINUE 255 C 256 50O CONTINUE 257 C 258 C** END IF 259 C 260 400 CONTINUE 26 1 C 262 c** END WHILE 263 c 264 c 265 c***** PREMULTIPLY BY 'HPRE' AND POSTMULTIPLY BY 'HPOST' 266 c* * * w * REQUIRED TRANSFORMATION MATRIX 'HOW' 267 c 268 CALL HMULT(HPRE,HOT,HTEMP) 269 CALL HMULT(HTEMP,HPOST,HOT) 270 CALL HMULT(HOT,HTW,HOW) 271 c 272 RETURN 273 END - 240 - L i s t i n g of TABSET.FOR at 14:42:09 on AUG 9, 1984 for CC1d=DAC0 Page 1 1 SUBROUTINE TABSET(VAR) 2 C 3 C THIS SUBROUTINE ALLOWS THE USER TO INTERACTIVELY MODIFY THE 4 C WORK TABLE DESCRIPTION FILE 'TABLE'. THE USER IS PROMPTED FOR 5 C CHANGES IN THE WORK TABLE POSITION. THE FIXED WORK TABLE 6 C PARAMETERS ARE INITIALLY ENTERED INTO 'TABLE' USING THE FILE 7 C EDITOR, AND ARE NOT CHANGED CONSEQUENTLY. 8 C 9 C VARIABLE TABLE PARAMETERS, JOINT VARIABLES 'VAR' \RE STORED IN 10 C FILE 'STOVAR'. 11 C 12 C INPUTS: NONE 13 C 14 C OUTPUTS: VAR(6)- TABLE JOINT VARIABLES 15 C 16 C COMMON BLOCKS READ: /TABLE/ 17 C /STOVAR/ 18 C 19 C FILES READ: TABLE.STOVAR 20 C 21 C FILES WRITTEN: STOVAR 22 C 23 C SUBROUTINES CALLED: OPEN,SHUT 24 C 25 26 c 27 BYTE YCAP,YLOW,NCAP.NLOW.ANS,LABEL(10) 28 BYTE TABLE(20),STOVAR(20) 29 BYTE BELL 30 c 31 REAL VAR(6),STEP(6),RMIN(6),RMAX(6),ALPHA(6),DISP(6),RLEN(6) 32 DOUBLE PRECISION HOT(4,4),HPRE(4,4),HPOST(4,4) 33 INTEGER ITYPE(6),10(6) 34 c 35 COMMON /TABLE/TABLE 36 COMMON /STOVAR/STOVAR 37 c 38 DATA BELL/7/ 39 DATA YCAP/89/,YL0W/121/.NCAP/78/,NL0W/l10/ 40 c 4 1 c***»* READ AND SAVE CONTENTS OF FILE 'TABLE' 42 c* * * * * NOTE THAT RECORDS MUST BE READ IN CORRECT ORDER 43 c 44 CALL OPEN(1.TABLE,4,1) 45 c 46 c***»* READ RECORD 1, NUMBER OF DEGREES OF FREEDOM, 'NUMDOF' 47 c 48 READ( 1 ,*) NUMDOF 49 c 50 Q * * * * * SKIP RECORD 2 51 c 52 READ(1,*) 53 c 54 READ RECORDS 3 TO 8, FIXED TABLE PARAMETERS 55 c 56 DO 170 J=1.NUMDOF 57 READ( 1 ,*) ID(J) .ITYPE(J),STEP(J),RMIN(J),RMAX(J).ALPHA(J). 58 * DISP(J ) ,RLEN(J) - 241 - L i s t i n g of TABSET.FOR a t 14:42:09 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 170 CONTINUE 60 C 61 C***»* SKIP RECORD 9 62 C 63 READ( 1 ,* ) 64 C 65 c***** SKIP RECORDS 10 TO 13, TRANSFORMATION MATRIX 'WW (NOT USED) 66 C 67 DO 175 J=1 ,4 68 READ( 1 , * ) 69 175 CONTINUE 70 C 71 C***** SKIP RECORD 14 72 C 73 READ( 1 ,* ) 74 C 75 c***** READ RECORDS 15 TO 18, TRANSFORMATION MATRIX 'HPRE' 76 C 77 DO 180 J=1 ,4 78 READ(1,») (HPRE(J,K) ,K=1,4) 79 180 CONTINUE 80 C 81 C***** SKIP RECORD 19 82 C 83 READ(1,* ) 84 C 85 C***»* READ RECORDS 20 TO 23, TRANSFORMATION MATRIX 'HPOST' 86 C 87 DO 185 vi=1 ,4 88 READ( 1 ,* ) (HPOST(J,K),K=1,4) 89 185 CONTINUE 90 C 91 CALL SHUT(1) 92 C 93 C****» READ CURRENT VALUES OF JOINT VARIABLES 'VAR' FROM CURRENT 94 C***»* FILE 'STOVAR'. 95 C 96 CALL OPEN(1,STOVAR,4. 1 ) 97 C 98 C»**** SKIP RECORD 1 99 C 100 READ(1,* ) 101 C 102 C****» READ RECORDS 2.NUMD0F+1 103 C 104 DO 190 J=1.NUMDOF 105 READ(1,*) VAR(J) 106 190 CONTINUE 107 C 108 CALL SHUT(1) 109 C 110 C»* UNTIL (ANSWER .NE. YES) 1 1 1 C 112 250 CONTINUE 113 C 114 C****» WRITE THE CURRENT TABLE JOINT POSITIONS ONTO THE SCREEN, AND 115 C***»* PROMPT FOR CHANGES 1 16 C - 242 - L i s t i n g of T A B S E T . F O R at 1 4 : 4 2 : 0 9 on AUG 9 , 1984 for CC1d=DAC0 Page 3 117 W R I T E ( 5 , 2 3 0 ) ( V A R ( J ) , J = 1 , 3 ) 118 2 3 0 F O R M A T C J O I N T V A R ( 1 )= ' , F8 . 2 , ' J O I N T V A R ( 2 ) = ' , F 8 . 2 , 119 - * ' J O I N T V A R ( 3 ) = ' , F 8 . 2 ) 1 2 0 C 121 W R I T E ( 5 , 1 0 O ) 122 100 F O R M A T ( $ , ' DO YOU WISH TO CHANGE THE TABLE P O S I T I O N ? ( Y / N ) 123 R E A D ( 5 , 3 0 1 ) ANS 124 301 FORMAT(A1 ) 125 C 126 c** I F (ANS . EO. Y E S ) CONT INUE 127 c 128 I F ( A N S . E O . Y C A P . O R . A N S . E O . Y L O W ) GO TO 5 0 0 1 2 9 c 130 c** E L S E GO TO END 131 c 132 GO TO 6 5 0 133 c 134 5 0 0 CONT INUE 135 C 136 c** END I F 137 c 138 W R I T E ( 5 , 1 0 1 ) 139 101 F O R M A T ( $ , ' ENTER ' J O I N T NUMBER YOU WISH TO CHANGE ( I N T E G E R ) 140 R E A D ( 5 , * ) J O I N T 141 c 14-2 c « * I F ( J O I N T NOT V A L I D ) P R I N T MESSAGE. TRY A G A I N 143 c 144 I F ( J O I N T . L T . 1 . O R . J O I N T . G t . N U M D O F ) GO TO 701 145 C ' 146 c** END I F 147 c 148 W R I T E ( 5 , 1 0 2 ) 149 102 F O R M A T ( $ , ' ENTER THE NEW P O S I T I O N ( D E G . OR MM. , R E A L ) ' ) 150 R E A D ( 5 . , » ) V A R ( J O I N T ) 151 c 152 c * » I F ( V A R I A B L E OUT OF RANGE) PR INT MESSAGE , TRY A G A I N 153 c 154 I F ( V A R ( J O I N T ) . L T . R M I N ( J O I N T ) . O R . V A R ( J O I N T ) . G T . R M A X ( J O I N T ) ) 155 * GO TO 8 0 0 156 c 157 c** END I F 158 c 159 c * I F ( J O I N T T Y P E I S D I S C R E T E ) TEST FOR V A L I D VALUE OF ' V A R ' 160 c 161 . I F ( I T Y P E ( J O I N T ) . N E . 1 ) GO TO 8 5 0 162 c 163 T E M P = V A R ( J O I N T ) / S T E P ( J O I N T ) 164 I T E M P = I N T ( T E M P ) 165 R E M = A B S ( T E M P - F L O A T ( I T E M P ) ) 166 c 167 I F ( R E M . G T . . 0 0 1 ) GO TO 9 0 0 168 c 169 8 5 0 CONT INUE 170 C 171 c * * END I F 172 c 173 GO TO 2 5 0 174 c - 243 - L i s t i n g of TABSET.FOR at 14:42:09 on AUG 9, 1984 f o r CC1d=DAC0 Page 4 175 C** END UNTIL 176 C 177 650 CONTINUE 178 C 179 STORE NEW VALUES OF JOINT VARIABLES 'VAR' IN FILE 'STOVAR'. 180 c 181 CALL 0PEN(1,STOVAR.4.1) 182 READ( 1 , * ) 183 DO 123 J=1,NUMD0F 184 WRITE(1.*) VAR(J) 185 123 CONTINUE 186 CALL SHUT(1) 187 C 188 c****» GO TO END 189 c 190 GO TO 600 191 c 192 c 193 c****» ERROR MESSAGES 194 c 195 c 196 800 WRITE(5,106) BELL 197 106 FORMAT($,' POSITION ENTERED OUTSIDE OF RANGE-TRY AGAIN ',A1) 198 READ(5,* ) 199 GO TO 250 200 C 201 900 WRITE(5,107) BELL 202 107 FORMAT($,' POSITION ENTERED NOT ONE OF THE POSSIBLE VALUES-TRY 203 * AGAIN',A1) 204 READ(5,*) 205 GO TO 250 206 c 207 701 WRITE(5,108) BELL 208 108 FORMAT($,' JOINT NUMBER NOT VALID-TRY AGAIN '.AD 209 READ(5,») 210 GO TO 250 211 C 212 600 CONTINUE 213 RETURN 214 END - 244 - L i s t i n g of TANGEN.FOR a t 14:42:31 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE TANGEN(R,S,T,I FLAG) 2 C 3 C THIS SUBROUTINE CALCULATES TANGENT OF CURVE, T. 4 C 5 C INPUTS: 6 C R(3,3)=P0SITI0N VECTORS FOR POINTS 1-1, I, 1+1. 7 C S(3)=ARC LENGTHS AT POINTS 1-1, I. 1+1. 8 C IFLAG=CONDITION FLAG WHERE 9 C 1=1...SECOND POINT ON CURVE. 10 C 1=2...INTERMEDIATE POINTS. 11 C 1=3...LAST POINT ON CURVE. 12 C 13 C OUTPUTS: 14 C T(3)=TANGENT VECTOR AT POINT I. 15 C 16 17 c 18 c 19 REAL R(3,3), S ( 3 ) , T(3) 20 c 21 c * * CASE OF (IFLAG) 22 c 23 IF (IFLAG.LT.1.OR.IFLAG.GT.3) GO TO 400 24 GO TO (100.200,300). IFLAG 25 c 26 c * * CASE (1) 27 c 28 100 CONTINUE 29 DO 101 J=1.3 30 T ( J ) = (R(3, J)-R(2. J ) )/S(3) 31 101 CONTINUE 32 GO TO 500 33 c 34 c * * CASE (2) 35 c 36 200 CONTINUE 37 DO 102 J=1,3 38 D1=R(3, 1 )-R(1, 1 ) 39 D2=R(3,2)-R(1,2) 40 D3=R(3.3)-R(1,3) 41 DS=SQRT(D1**2+D2**2+D3**2) 42 T( J)=(R(3,d)-R(1. J ))/DS 43 102 CONTINUE 44 GO TO 500 45 C 46 C** CASE (3) 47 c 48 300 CONTINUE 49 DO 103 J=1,3 50 T ( d ) = ( R ( 2 . J ) - R ( 1 , J ) ) / ( S ( 2 ) - S ( O ) 51 103 CONTINUE 52 GO TO 500 53 C 54 c * * CASE.ELSE 55 c 56 400 CONTINUE 57 WRITE(5,150) 58 150 FORMAT(' CASE INDEX SPECIFICATION ERROR.') - 245 - s t i n g o f T A N G E N . F O R a t 1 4 : 4 2 : 3 1 o n AUG 9 , 1984 f o r C C 1 d = D A C 0 P a g e 5 9 C 6 0 5 0 0 C O N T I N U E 61 R E T U R N 6 2 END - 246 - L i s t i n g of TOLROT.FOR a t 14:42:32 on AUG 9, 1984 f o r CC1d=DACO Page 1 1 SUBROUTINE TOLROT(ROTOLD,ICOUNT.ROTNEW.INHAG) 2 C 3 C THIS SUBROUTINE CALCULATES AN INVERSE ROTATION MATRIX 4 C FOR A ROTATION 'ROTNEW ABOUT THE TOOL AXIS. 5 C THE TOOL IS ROTATED ALTERNATELY BY AN ANGLE OF ICOUNT*DELTA G C IN THE POSITIVE AND NEGATIVE DIRECTIONS. 7 C 8 C INPUTS: ROTOLD- PREVIOUS ROTATION ANGLE 9 C ICOUNT- NUMBER OF ROTATIONS 10 C 11 C OUTPUTS: ROTNEW- NEW ROTATION ANGLE 12 C INHAG(4.4)- INVERSE ROTATION MATRIX 13 C ^4 . (;,»«,»,*«,««*»*»***»«*»«.*.*»*«,»»»««.«»**.*****«*.*»*««««*»*».*«*«*»*«•«*•*»*» 15 C 16 REAL INHAG(4,4) .DELTA 17 INTEGER IMAX 18 C 19 COMMON /TOLROT/IMAX,DELTA 20 C 21 c***»* FIND THE NEW ROTATION ANGLE 22 C 23 ROTNEW = ROTOLD+FLOAT((- 1)**ICOUNT*ICOUNT/2)*DELTA 24 C 25 C***»* CALCULATE THE ROTATION MATRIX RELATING THE ROTATED TOOL TO THE 26 C***»* UNROTATED TOOL. 27 C 28 INHAG(1,1)=COS(ROTNEW) 29 INHAG(1,2)=SIN(R0TNEW) 30 INHAG(2,1) = -INHAG ( 1,2) 31 INHAG(2.2) = INHAG( 1 , 1) 32 INHAG(3.3)=1. 33 INHAG(4.4)=1. 34 C 35 RETURN 36 END - 247 - L i s t i n g of TRANS.FOR a t 14:42:32 on AUG 9. 1984 f o r CCid=DACO Page 1 SUBROUTINE TRANS(H,P1,P2) 2 C 3 C THIS SUBROUTINE PERFORMS THE FOLLOWING 4 C TRANSFORMATION. 5 C 6 C P2(4)=H(4,4)*P1(4) 7 C 8 C WHERE P1.P2 ARE 4X1 VECTORS AND H IS A 9 C COORDINATE TRANSFORMATION MATRIX. 10 C 1 1 12 C 13 REAL H(4,4),P1(4),P2(4) 14 C 15 DO 100 1=1,3 1G P2(I)=H(I,1)*P1(1)+H(I,2)*P1(2)+H(I 17 10O CONTINUE 18 C 19 P2(4)=1. 20 C 21 RETURN 22 END - 248 - L i s t i n g o f U N I T . F O R a t 1 4 : 4 2 : 3 3 o n AUG 9 , 1984 f o r CC1d=DAC0 P a g e 1 1 SUBROUTINE U N I T ( V E C T , U V E C T ) 2 C 3 C T H I S SUBROUTINE NORMALIZES A VECTOR. 4 C 5 C I N P U T S : V E C T ( 3 ) - A VECTOR 6 C 7 C OUTPUTS: U V E C T ( 3 ) - VECT NORMALIZED. 8 C 9 10 c 1 1 REAL V E C T ( 3 ) , U V E C T ( 3 ) , L E N G T H 12 c 13 L E N G T H - S O R T ( V E C T ( 1 ) * * 2 + V E C T ( 2 ) * * 2 + V E C T ( 3 ) * * 2 ) 14 c 15 U V E C T ( 1 ) = V E C T ( 1 ) / L E N G T H 16 U V E C T ( 2 ) = V E C T ( 2 ) / L E N G T H 17 U V E C T ( 3 ) = V E C T ( 3 ) / L E N G T H 18 c 19 RETURN 2 0 END - 249 - L i s t i n g of WELSET.FOR a t 14:42:59 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE WELSET(WPCLST,WKSTAT) 2 C 3 C THIS SUBROUTINE SCANS THE WELD PARAMETERS JTYPE,XSECT,T1,T2, 4 C FOR ALL SEAMS TO BE WELDED. THE CURRENT WELDING MACHINE 5 C SETTINGS ARE READ FROM THE WELDER SETUP FI L E . IF NECESSARY, 6 c THE SETUP FILE IS MODIFIED AND THE OPERATOR IS INSTRUCTED TO 7 c RESET WELDER. 8 c 9 c INPUTS: WPCLST- NAME OF WORKPIECE SEAM DATA FILE 10 c WKSTAT- NAME OF WORKSTATION DATA FILE 11 c 12 c FILES READ: WPCLST 1-3 c WKSTAT 14 c 15 c FILES WRITTEN: WKSTAT 16 c 17 c SUBROUTINES: OPEN 18 c RDFILE 19 c SHUT 20 c 21 22 c 23 BYTE WPCLST(20),WPCDAT(20),WKSTAT(20) 24 REAL WIRES(3.2),WRSIZE(3) 25 c 26 DATA WRSIZE/.030,.035,.045/ 27 DATA WIRES/.025, .125, .037. .25, . 125, .5/ 28 c 29 COMMON /NSEAMS/NSEAMS 30 c 31 CALL OPEN(1.WPCLST,4,1 ) 32 c 33 TMIN=1000. 34 TMAX=0. 35 c 36 DO 100 1=1.NSEAMS 37 CALL RDFILE(1,1+2,WPCDAT.20.IEND) 38 CALL 0PEN(2,WPCDAT,1.1) 39 READ(2) JTYPE,XSECT,T1,T2,NPOINT,ICURVE 40 CALL SHUT(2) 41 c 42 c * * * » * PROCESS DATA HERE, SAVING MINIMUM,MAXIMUM VALUES ETC. 43 c * * » * * FOR SELECTION OF OPTIMUM WELDER SETTINGS. 44 c 45 TX=AMIN1(T1,T2) 46 TMIN=AMIN1(TMIN,TX) 47 TMAX=AMAX1(TMAX,TX) 48 c 49 100 CONTINUE 50 c 51 c 52 CALL SHUT(1) 53 c 54 c * * * * « OPEN CURRENT WELDER SETUP FILE 'WKSTAT'. 55 c 56 CALL OPEN(1.WKSTAT, 4, 1 ) 57 c 58 c * * * * » SKIP FIRST 26 RECORDS - 250 - L i s t i n g of WELSET.FOR a t 14:42:59 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C eo DO 10 1=1,26 61 READ(1,*) 62 10 CONTINUE 63 C 64 c**»*» READ CURRENT WELOER SETUP FROM 'WKSTAT'. 65 c 66 READ( 1 ,* ) WSIZE 67 c 68 c«**** INTERACIVELY UPDATE WELDER SETUP IF REQUIRED. WHEN NEW 69 c***** PARAMETERS HAVE BEEN CHOSEN, UPDATE 'WKSTAT' 70 c 71 DO 20 J=1.3 72 IF(TMIN.GE.WIRES(J.1 ) .AND.TMAX.LE.WIRES(d,2)) GO TO 500 73 20 CONTINUE 74 c 75 WRITE(5,30) 76 30 FORMATC NO APPROPRIATE WIRE SIZE CAN BE FOUND. HIT RETURN TO 77 * CONTINUE ') 78 READ(5,*) 79 GO TO 900 80 c 81 500 CONTINUE 82 C 83 IF(WSIZE.EQ.WRSIZE(J)) GO TO 600 84 C 85 BACKSPACE*1) 86 WRITEO.*) WRSIZE(vJ) 87 WRITE(5,31) WSIZE.WRSIZE(d) 88 31 FORMATC CURRENT WIRE SIZE= '.F5.3,' IN., NEW WIRE SIZE= ', 89 * F5.3,' IN') 90 WRITE(5,33) 91 33 FORMAT(' INSTALL NEW WIRE, THEN HIT RETURN TO CONTINUE') 92 READ(5,*) 93 GO TO 900 94 C 95 600 CONTINUE 96 C 97 WRITE(5,32) 98 32 FORMATC CURRENT WIRE SIZE IS OK. HIT RETURN TO CONTINUE ') 99 READ(5 , * ) 100 C 101 900 CONTINUE 102 C 103 CALL SHUT(1 ) 104 C 105 RETURN 106 END - 251 - L i s t i n g of WIRE 1.FOR a t 14:42:59 on AUG 9, 1984 f o r CCid-DACO Page 1 1 SUBROUTINE WIRE 1 2 C 3 c » * * * « THIS SUBROUTINE DETERMINES CURRENT, VOLTAGE, WIRE FEED, WELDING 4 c * * * * * WELDING SPEED, GAS FLOW RATE, WIRE STICK-OUT, LONGITUDINAL ANGLE 5 c * * * * » AND TRANSVERSE ANGLE AS A FUNCTION OF THICKNESS AND CROSS 6 c * * * * » SECTIONAL AREA. 7 c 8 c DEFINITION OF VARIABLES: 9 c CURRNT-CURRENT(AMPS) 10 c VOLT-VOLTAGE (VOLTS) 1 1 c FEED-WIRE FEED RATE (IPM) 12 c GFR-GAS FLOW RATE (CFM) 13 c SPEED-WELDING SPEED (IPM) 14 c STKOUT-WIRE STICK-OUT (INCHES) 15 c WALPHA-LONGITUDINAL TORCH ANGLE 16 c BETA-TRANSVERSE TORCH ANGLE 17 c TX-MINIMUM PARENT METAL THICKNESS 18 c XSECT-WELD CROSS SECTIONAL AREA (SO.IN.) 19 c 20 BYTE WPARAM(20) 21 COMMON/EXPERT/WPARAM.VOLT,WSIZE,FEED,GFR.WALPHA.BETA.STKOUT 22 COMMON/W123/CURRNT.SPEED.XSECT.TX 23 c 24 CURRNT-199.07+43.04*AL0G(TX) 25 VOLT-16. 26 FEED-290.38+52.87*ALOG(TX) 27 GFR-23.38+1 64*AL0G(TX) 28 SPEED-3. 1415*( .030**2)*FEED/(XSECT) 29 STKOUT-.75+.25*AL0G(TX ) 30 WALPHA-15. 31 BETA-0.0 32 c 33 RETURN 34 END - 252 - L i s t i n g of WlRE2.FOR a t 14:42:59 on AUG 9. 1984 f o r CC1d=DAC0 Page 1 1 r* SUBROUTINE WIRE2 4. 3 I* c***** WIRE SIZE IS 0.035 INCHES 4 c***»* THIS SUBROUTINE DETERMINES CURRENT, VOLTAGE, WIRE FEED, 5 c»**»* WELDING SPEED, GAS FLOW RATE, WIRE STICK-OUT, LONGITUDINAL 6 c***** ANGLE AND TRANSVERSE ANGLE AS A FUNCTION OF THICKNESS 7 c****» AND CROSS SECTIONAL AREA. 8 c 9 BYTE WPARAM(20) 10 COMMON/EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR,WALPHA,BETA,STKOUT 11 COMMON/W123/CURRNT,SPEED.XSECT,TX 12 c 13 CURRNT=222.64+44.41*ALOG(TX) 14 IF(CURRNT.GT.150.) CURRNT=150. 15 V0LT=23.91+2.12*AL0G(TX) 16 FEED=435.77+101.51*AL0G(TX) 17 IF(FEED.GT.265.) FEED=265. 18 GFR=40.55+7.17*AL0G(TX) 19 SPEED=3.1415*(.035**2)*FEED/(XSECT*4.) 20 IF(GFR.GT.22.5) GFR=22.5 21 STKOUT=.75+.25*ALOG(TX ) 22 WALPHA=15. 23 BETA=0.0 24 c 25 RETURN 26 END - 253 - L i s t i n g of WIRE3.FOR at 14:42:59 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE WIRE3 2 C 3 c***»* WIRE SIZE IS 0.045 INCHES 4 c**»** THIS SUBROUTINE DETERMINES CURRENT, VOLTAGE, WIRE FEED, 5 c****» WELDING SPEED, GAS FLOW RATE, WIRE STICK-OUT, LONGITUDINAL 6 c***«* ANGLE AND TRANSVERSE ANGLE AS A FUNCTION OF THICKNESS 7 c***** AND CROSS SECTIONAL AREA. 8 c 9 BYTE WPARAM(20) 10 COMMON/EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR,WALPHA.BETA,STKOUT 1 1 COMMON/W123/CURRNT.SPEED.XSECT,TX 12 c 13 CURRNT=207.88+8.72*AL0G(TX) 14 V0LT=22. 15 FEED=296.88+21.77*AL0G(TX) 16 GFR=22.5 17 SPEED = 3.1415*( .045**2)*FEED/(XSECT*4. ) 18 STKOUT=.75+.25*AL0G(TX) 19 WALPHA=15. 20 BETA=0.0 21 c 22 RETURN 23 END - 254 - : 1 ng of WORPOS. FOR a t 14:43:16 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE WORPOS(WPCDAT.IDSEAM,IDIR,ISTAT) 2 C 3 C THIS SUBROUTINE DOES THE COORDINATE TRANSFORMATIONS TO FIND 4 C THE TOOL LOCATION DATA FOR THE TOOL COORDINATE FRAME FG RELATIVE 5 C TO THE WORLD COORDINATE FRAME FO. THE SUBROUTINE FINDS THE 6 C HOMOGENEOUS TRANSFORMATION HOG, AND STORES THE TOOL POSITION 7 C AND ORIENTATION IN AN OUTPUT FILE. 8 C 9 C INPUTS: IDSEAM- SEAM IDENTIFICATION NUMBER 10 C 1 1 c IDIR- SEAM TRAVERSE DIRECTION 12 c 0=NORMAL DIRECTION 13 c 1=REVERSE DIRECTION 14 c 15 c OUTPUTS: ISTAT- FEASIBILITY STATUS 16 c 0=TRAJECTORY IS FEASIBLE 17 c 1=R0B0T WORK ENVELOPE EXCEEDED 18 c 2=R0B0T JOINT LIMIT EXCEEDED 19 c 3=IDIR NOT 0 OR 1 20 c 21 c INPUT FILES: TABLE - A FILE CONTAINING THE WORKPIECE AND 22 c TABLE TRANSFORMATION MATRICES, HTW, HOT 23 c 24 c WPCLST - A FILE SPECIFYING NAME OF WPCDAT. 25 c 26 c WPCDAT - A FILE CONTAINING THE 27 c POSITION AND ORIENTATION DATA FOR THE 28 c WELD FRAME FC. RELATIVE TO THE 29 c WORKPIECE FRAME FW. 30 c 31 c ROBLST - A FILE SPECIFYING NAME OF LOCDAT. 32 c 33 c OUTPUT FILES: ROBDAT AN EXTERNALLY SPECIFIED FILE CONTAINING 34 c THE LOCATION AND ORIENTATION DATA OF 35 c THE TOOL RELATIVE TO THE ROBOT 36 c COORDINATE FRAME FO, THE WELDING 37 c CURRENT AND SPEED SETTINGS, AND 38 c DIRECTION VECTORS FOR NORMAL AND 39 c LATERAL PATH CORRECTIONS. 40 c 4 1 c LOCSET CONTAINS, IN WELDING SEQUENCE, THE 42 c FOLLOWING: 43 c 44 c SEAM NUMBER 45 c TABLE JOINT VARIABLES 46 c ROBOT CONFIGURATION 47 c 48 c SUBROUTINES CALLED - HMULT 49 c HMULTS 50 c JOINTS 51 c LIMITS 52 c MATCNV 53 c NOAP 1 54 c N0AP5 55 c OPEN 56 c RDFILE 57 c SHUT 58 c TOLROT 255 - L i s t i n g o f WORPOS.FOR a t 14:43:16 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C XPROD 60 C 61 ***»*»«*»«*******»**^ 62 c 63 c 64 IMPLICIT DOUBLE PRECISION (D) 65 DOUBLE PRECISION HOT(4,4),HTW(4,4),HWC(4,4),HCG(4,4) 66 DOUBLE PRECISION HOW(4,4),HOC(4,4),HOG(4,4) 67 REAL R ( 3 ) , T ( 3 ) . A ( 3 ) , B ( 3 ) , N ( 3 ) , 0 ( 3 ) , X ( 3 ) , Y ( 3 ) , Z ( 3 ) , 68 * THETA(6),SHOW(4,4),SHOT(4,4),INHOO(4,4) 69 REAL XYZ0AT(6),SH0G(4,4),INHAG(4,4),H6G(4,4),H06(4.4),HOG(4,4) 70 REAL INH6A(4,4),HOA(4,4),DELTA,TABVAR(6),H0G5(4,4) 71 REAL ALPHA(6),LEN(6),DIST(6),RMIN(6),RMAX(6),VAR(6),H00(4,4) 72 REAL SHOC(4,4),HOC(4,4),ENORM(3),ELAT(3) 73 INTEGER IT00L(3) .IOFF(2 ) ,I JOINT(6),IC0NF(6),IMAX 74 BYTE WPCLST(20).STOVAR(20).ROBLST(20),L0CLST(2O) 75 BYTE WPCDAT(20),LOCDAT(20),ROBDAT(20),LOCSET(20) 76 BYTE BELL(3),WPARAM(20) 77 c 78 COMMON / F I L E / F I L I S T 79 COMMON /LOCSET/LOCSET 80 COMMON /ROBLST/ROBLST 81 COMMON. /STOVAR/STOVAR 82 COMMON /WPCLST/WPCLST 83 COMMON /ROBOT/NRDOF.ALPHA,LEN,DIST,RMIN,RMAX,H0O.H6G,INHOO.INH6A 84 COMMON /CONFIG/ICONF 85 COMMON /MATRIX/HOT,HOW,SHOT,SHOW 86 COMMON /TABDAT/HTW,SHTW 87 COMMON /TOLROT/IMAX,DELTA 88 COMMON /ROBPOS/HOG 89 COMMON /EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR,WALPHA,BETA,STKOUT 90 c 91 DATA BELL/3*7/ 92 c 93 INITIALIZE PARAMETERS 94 c 95 ISOLN=0 96 ISTAT=0 97 IC0UNT=1 98 IFEAS=1 99 R0TOLD=O. 100 c 101 c 102 c****» FIND AND OPEN FILES 'WPCDAT','ROBDAT','LOCSET','WPARAM' 103 c 104 CALL OPEN(2,WPCDAT,1,1) 105 c 106 CALL 0PEN(3,ROBLST,4,1) 107 CALL RDFILE(3,IDSEAM+1,ROBDAT,20,I END) 108 CALL SHUT(3) 109 CALL 0PEN(3,R0BDAT,4,1) 1 10 c 1 1 1 CALL 0PEN(4,LOCSET,4,1) 1 12 c 1 13 CALL 0PEN(7,WPARAM,4,1) 1 14 c 1 1-5 c 1 16 c****» READ FIRST RECORD OF 'WPCDAT', THEN BACKSPACE TO BEGINNING. - 256 - L i s t i n g o f WORPOS.FOR a t 14:43:16 on AUG 9, 1984 f o r CC1d=DAC0 Page 3 1 17 C 1 18 READ(2) JTYPE,XSECT,T1,T2,NPOINT,ICURVE 1 19 BACKSPACE 2 120 C 121 c 122 c****» STORE WELDING PARAMETERS 'WALPHA','BETA','STKOUT','VOLT', 123 c* * * » * 'FEED', 'GFR' FOUND BY THE EXPERT WELDER MODULE, AND NUMBER OF 124 c***»* LOCATIONS 'NPOINT' AT HEAD OF ROBOT LOCATION FILE 'ROBDAT' FOR 125 c***»* CURRENT SEAM 126 c 127 WRITE(3,* ) WALPHA.BETA.STKOUT,VOLT,FEED,GFR,NPOINT 128 c 129 c 130 c 131 c**«»* REPEAT UNTIL ALL CONFIGURATIONS HAVE BEEN TESTED OR UNTIL A 132 c* * * * * FEASIBLE TRAJECTORY HAS BEEN FOUND 133 c 134 c 135 160 CONTINUE 136 c 137 READ FIRST RECORD OF 'WPCDAT' 138 c 139 READ(2) JTYPE,XSECT,T1,T2,NPOINT,ICURVE 140 c 141 Q** * * * * SET INDEX 142 c 143 INDEX=NPOINT 144 c 145 c 146 c* * * IF (DIRECTION.EQ.REVERSE.AND.PATH IS FEASIBLE) STEP TO END OF 147 c..» FILES 'WPCDAT'.'WPARAM' 148 c 149 IF (IDIR.EQ.1.AND.IFEAS.EQ.O) GO TO 890 150 GO TO 891 151 890 CONTINUE 152 c 153 c***** REPEAT UNTIL END OF FILE REACHED 154 c 155 560 CONTINUE 156 READ(2.END=556) 157 READ(7,*) 158 GO TO 560 159 556 CONTINUE 160 C 161 c*** . END IF 162 c 163 c 164 891 CONTINUE 165 C 166 C 167 c*** REPEAT FOR ALL LOCATIONS IN FILE 'WPCDAT' 168 c 169 150 CONTINUE 170 C 171 c*** IF (DIRECTION .EQ. NORMAL) (IE. IDIR=0) 172 c 173 IF (IDIR.NE.0.AND.IFEAS.EQ.O) GO TO 855 174 c - 2 5 7 - L i s t i n g o f WORPOS.FOR a t 14:43:16 on AUG 9, 1984 f o r CCid=DACO Page 4 175 c****» READ DATA FROM INPUT FILE 'WPCDAT'. 176 c 177 READ(2,END=555) JJ,S,(R(I).1=1,3),(T(K),K=1,3), 178 * (A(L),L=1 ,3).(B(M),M=1,3) 179 c 180 GO TO 880 181 c 182 855 CONTINUE 183 C 184 C 185 c*** OR IF (DIRECTION .EO. REVERSE .AND. PATH IS FEASIBLE) 186 c 187 IF (IDIR.NE.1) GO TO 860 188 c 189 c***»* READ FILE IN REVERSE ORDER FROM END TO BEGINNING 190 c*»*** 'INDEX' IS NUMBER OF RECORDS REMAINING 191 c 192 IF (INDEX.EO.O) GO TO 555 193 BACKSPACE 2 194 READ(2 ) d J , S , ( R ( I ) , I = 1.3).(T(K),K=1.3),(A(L),L=1.3), 195 * (B(M),M=1,3) 196 BACKSPACE 2 197 INDEX=INDEX-1 198 Jd=NP0INT-JJ+1 199 GO TO 880 2O0 c 201 c*** ELSE (ERROR) 202 c 203 860 CONTINUE 204 GO TO 861 205 C 206 c*** END IF 207 c 208 880 CONTINUE 209 C 210 C 21 1 c****» SET UP TRANSFORMATION MATRIX 'HWC. THE COLUMNS CONTAIN 212 c****» THE VECTORS T. B, A, R, OBTAINED FROM 'WPCDAT'. 213 c 214 HWC(1, 1 )=T( 1 ) 215 HWC(2.1)=T(2) 216 HWC(3,1)=T(3) 217 HWC(4,1)=0. 218 HWC(1,2)=B( 1 ) 219 HWC(2,2)=B(2) 220 HWC(3,2)=B(3) 221 HWC(4,2)=0. 222 HWC(1,3)=A(1) 223 HWC(2,3)=A(2) 224 HWC(3,3)=A(3) 225 HWC(4,3)=0. 226 HWC(1,4)=R(1 ) 227 HWC(2,4)=R(2) 228 HWC(3,4)=R(3) 229 HWC(4,4)=1. 230 c 231 c****» CONVERT ANGLES 'WALPHA', 'BETA' INTO RADIANS AND 'RHO' 232 c***»* INTO MILLIMETERS - 2 5 8 - L i s t i n g of WORPOS.FOR a t 14:43:16 on AUG 9 , 1984 f o r CCid=OACO Page 5 233 C 234 DP I=4.DO*DATAN(1.DO) 235 DE2RAD=DPI/180. 236 DALPHA=WALPHA*DE2RAD 237 DBETA=BETA*DE2RAD 238 DRHO=STKOUT 239 C 240 CALCULATE ELEMENTS DF MATRIX 'HCG'. 241 c 242 X(1)=0. 243 X(2)=DC0S(DALPHA ) *DCOS(DBETA ) 244 X(3)=DSIN(DBETA) 245 Z(1)=-DSIN(DALPHA) 246 Z(2)=DSIN(DBETA) 247 Z(3)=-DCOS(DALPHA ) *DCOS(DBETA ) 248 HCG(1,4) = -DRH0*Z( 1 ) 249 HCG(2,4)=-DRH0*Z(2) 250 HCG(3,4)=-DRH0*Z(3) 251 HCG(4.4)=1. 252 c 253 c***»* THE SECOND COLUMN I S THE CROSS PRODUCT OF THE THIRD AND 254 c***»* FIRST COLUMNS 255 c 256 CALL XPROD(Z.X.Y) 257 c 258 DO 65 1=1,3 259 HCG(I, 1 ) = X( I ) 260 HCG(I,2)=Y(I ) 261 HCG(I,3) = Z( I ) 262 65 CONTINUE 263 C 264 HCG(4,1)=0. 265 HCG(4,2)=0. 266 HCG(4,3)=0. 267 C 268 C 269 c****» PERFORM MATRIX MULTIPLICATIONS TO OBTAIN HOG. 270 c***** HOG=HOT*HTW*HWC*HCG 271 c 272 CALL HMULT(HOT.HTW,HOW) 273 CALL HMULT(HOW,HWC,HOC) 274 CALL HMULT(HOC,HCG,HOG) 275 c 276 c 277 CONVERT DOUBLE P R E C I S I O N ' H O T ' , ' H O W ' . ' H O G ' INTO SINGLE 278 c***»* PRECISION 'SHOT'.SHOW','SHOG'. 279 c 280 CALL MATCNV(HOC,SHOC,1) 281 CALL MATCNV(HOG,SHOG,1) 282 c 283 c 284 c***»* CALCULATE HOG=INH0O*H0G 285 c***** 'INHOO' IS THE INVERSE HOMOGENEOUS TRANSFORMATION RELATING 286 c****» THE WORLD COORDINATE FRAME TO THE ROBOT BASE FRAME 287 c 288 CALL HMULTS(INHOO,SHOG,HOG) 289 c 290 c - 2 5 9 - L i s t i n g of WORPOS.FOR a t 14:43:16 on AUG 9. 1984 f o r CC1d=DAC0 Page 6 291 895 CONTINUE 292 : C I 293 c I 294 c»»»** CALCULATE ROBOT JOINT ANGLES, TEST FOR LIMITS ' 295 c 296 c**»»* IF THE ROBOT HAS 5 DEGREES OF FREEDOM, THEN CALCULATE 297 c***»* VECTORS 'N'.'O' AS FUNCTIONS OF VECTOR 'A' AND POSITION 298 c***** ' P ' . THERE ARE TWO SOLUTIONS, SELECTED BY SETTING 299 c***** 'ISOLN'=0 OR 1. THE RESULT IS ONE OF TWO UNIQUE 300 c**«»* TRANSFORMATIONS 'HOG'=[n,o,a,pj 301 c 302 IF(NRDOF.EQ.5) CALL N0AP5(IS0LN) 303 c 304 c 305 c*** WHILE(PATH NOT FEASIBLE AND COUNT LT. MAX) ROTATE TOOL 306 c 307 c****» SEARCH FOR A FEASIBLE SOLUTION BY ROTATING THE TOOL 308 c****» ABOUT ITS AXIS. THIS APPLIES TO 6 DEGREE OF FREEDOM 309 c***** ROBOTS ONLY, FOR WHICH THE TOOL FRAME VECTORS 'N'.'O' 310 c****» CAN BE ARBITRARILY SPECIFIED. THE INITIAL 'N'.'O' ARE 311 c* •* * * * AS CALCULATED PREVIOUSLY. THE FIRST CALL TO 'TOLROT' 312 c****» YIELDS A ROTATION OF ZERO. 313 c 314 CALL TOLROT(ROTOLD,ICOUNT,ROTNEW,INHAG) 315 c 316 c 317 c***»* CALCULATE ROBOT TRANSFORMATION MATRIX 'H06'. 318 c***** H06 = HOG *INHAG*INH6A 319 c 320 CALL HMULTS(HOG.INHAG.HOA) 321 CALL HMULTS(HOA,INH6A,H06) 322 c 323 c 324 c****» CALCULATE JOINT ANGLES FOR A GIVEN ROBOT TOOL MOUNTING 325 c*»»*« FLANGE FRAME MATRIX 'H06' . I FLAG1 : 0- OK, 1- ENVELOPE 326 c***«* EXCEEDED 327 c 328 CALL JOINTS(H06,THETA,IFLAG1) 329 c 330 c 331 IF(IFLAG1.NE.0) GO TO 861 332 c 333 c 334 c***** TEST THE CALCULATED JOINT POSITIONS AGAINST THEIR PHYSIl 335 c***** LIMITS. IFLAG2: 0- OK, 1- JOINT LIMIT EXCEEDED 336 c 337 CALL LIMITS(THETA,IFLAG2,IJOINT) 338 c 339 c 340 IF(IFLAG2.NE.0) GO TQ 776 341 c 342 c 343 c* * * * * TEST JOINT 6 LIMITS (CHECK IF JOINT 6 ROTATES IN ONE 344 £ « * » * » DIRECTION THROUGH A JOINT LIMIT). ALTHOUGH JOINT 6 345 c***»* ALLOWS MORE THAN 360. DEGREES OF ROTATION, WE CANNOT 346 DRIVE THE JOINT CONTINUOUSLY IN ONE DIRECTION. IF A 347 c***»* LIMIT IS REACHED, THE JOINT WILL ROTATE ONE ROTATION 348 c****» AWAY FROM THE LIMIT BEFORE PROCEEDING. THIS CAN RESULT - 260 - L i s t i n g of WORPOS.FOR a t 14:43:16 on AUG 9. 1984 f o r CC1ODAC0 Page 7 349 c***** IN A CATASTROPHIC DISCONTINUITY OF MOTION EVEN THOUGH 350 c*»**» EACH LOCATION HAS A VALID SOLUTION. 351 c****« 352 c****« T6LAST- LAST JOINT 6 ANGLE 353 c***** T6SUM- JOINT 6 NET ROTATION FROM INITIAL POSITION 354 c****» T6SUM2- T6SUM + 360. 355 c****» J6FLG1- 1=T6SUM EXCEEDED JOINT LIMITS 356 Q** * * * JGFLG2- 1=T6SUM2 EXCEEDED JOINT LIMITS 357 c***«* T6INIT- INITIAL JOINT POSITION 358 c***»* ONEROT- ONE ROTATION (360. DEG.) 359 c****» FTEST- 1=0NE ROTATION IN EITHER DIRECTION FROM INITIAL 360 c****» ANGLE EXCEEDS JOINT LIMITS 361 c 362 I F ( J J . G T . 1 ) GO TO 777 363 c 364 INITIALIZE PARAMETERS AT START OF SEAM. 365 c 366 T6LAST=0. 367 T6SUM=0. 368 T6SUM2=0. 369 J6FLG1=0 370 J6FLG2=0 371 T6INIT=THETA(6) 372 0NER0T=36O. 373 c 374 c* * * * * CHECK IF JOINT 6 CAN BE ROTATED ONE ROTATION 375 c 376 IF(T6INIT+0NER0T.LT.RMAX(6)) GO TO 111 377 c 378 c****» IF NOT, CHECK IF JOINT CAN BE ROTATED MINUS ONE 379 c***** ROTATION 380 c 381 IF(T6INIT-0NER0T.GT.RMIN(6)) GO TO 112 382 GO TO 113 383 c 384 112 CONTINUE 385 0NER0T=-36O. 386 GO TO 111 387 113 CONTINUE 388 c 389 c****» IF JOINT 6 CANNOT BE ROTATED ONE ROTATION IN EITHER 390 c***»* DIRECTION FROM THE INITIAL POSITION, SET 'FTEST', 391 c*»««* 'J6FLG2' 392 c 393 FTEST=1 394 J6FLG2=1 395 c 396 111 CONTINUE 397 777 CONTINUE 398 C 399 UPDATE CURRENT JOINT POSITION 'T6SUM' 400 c 401 T6SUM=T6SUM+THETA(6)-T6LAST 402 T6LAST=THETA(6) 403 c 404 c***** IF 'TSUM' EXCEEDS JOINT LIMIT, SET 'J6FLG1' 405 c 406 IF(T6SUM.LT.RMIN(6).OR.T6SUM.GT.RMAX(6)) J6FLG1=1 - 261 - L i s t i n g of WORPOS.FOR a t 14:43:16 on AUG 9, 1984 f o r CCid=DACO Page 8 407 C 408 IF(FTEST.EO.1) GO TO 118 409 T6SUM2=T6SUM+0NER0T 410 C 411 c***** IF 'T6SUM2' EXCEEDS JOINT LIMIT, SET 'J6FLG2' 412 c 413 IF(T6SUM2.LT.RMIN(6).OR.T6SUM2.GT.RMAX(6)) J6FLG2'1 414 c 415 1 18 CONTINUE 416 C 417 C 418 c**»*» IF EITHER 'J6FLG1' OR 'J6FLG2' IS SET, JOINT 6 HAS MOVED 419 c***»* THROUGH A JOINT LIMIT 420 c 421 IF(J6FLG1.EO.O.OR.J6FLG2.E0.0) GO TO 885 422 c 423 776 CONTINUE 424 C 425 C 426 c**»** IF PATH NOT FEASIBLE, ROTATE TOOL AND TRY AGAIN UNTIL 427 c**»*» ALL SPECIFIED ROTATIONS HAVE BEEN TRIED. 428 c***«* FOR A 5 DEGREE OF FREEDOM ROBOT, TEST THE TWO POSSIBLE 429 c«*»** JOINT SOLUTIONS. 4 30 c 431 IF(NRDOF.NE.5) GO TO 830 432 c 433 c***** 'ISOLN'=0 OR 1 SELECTS THE TWO POSSIBLE 5 DEGREE OF 434 c»**»* FREEDOM SOLUTIONS 435 c 436 IS0LN=IS0LN+1 437 IF(ISOLN.GT.1) GO TO 850 438 GO TO 895 439 830 CONTINUE 440 C 441 c****« FOR A 6 DEGREE OF FREEDOM ROBOT, INCREMENT THE TOOL AXIS 442 c****» ROTATION COUNTER 'ICOUNT' AND RETRY, UNTIL A MAXIMUM 443 ROTATION IS REACHED. 444 c 445 IC0UNT=IC0UNT+1 446 IF(ICOUNT.LE.IMAX) GO TO 895 447 c 448 c**» END WHILE 449 c 450 Q * * * * * RESET TOOL ROTATION COUNTER 'ICOUNT' 451 c 452 ICOUNT=1 453 GO TO 850 454 c 455 885 CONTINUE 456 C 457 c«*«»* RESET TOOL ROTATION COUNTER 'ICOUNT' AND RE DEFINE THE 458 c***»* INITIAL ROTATION OF THE TOOL AS THE CURRENT ROTATION 459 c 460 ICOUNT=1 461 ROTOLD=ROTNEW 462 c 463 c***»* IF A FEASIBLE PATH HAS NOT BEEN FOUND, SKIP THE NEXT 464 c****» SECTION. - 262 - L i s t i n g of WORPOS.FOR at 14:43:16 on AUG 9, 1984 f o r CCid=DACO Page 9 465 C 466 IF(IFEAS.NE.O) GO TO 789 467 C 468 C ., 469 c »•«» * * CALCULATE HOC=INHOO*HOC 470 c 471 CALL HMULTS(INHOO.SHOC.HOC) 472 c 473 c***»* FIND SEAM LATERAL AND NORMAL DIRECTIONS RELATIVE 10 ROBOT 474 c** • * * COORDS. 475 c 476 DO 115 1=1,3 477 EN0RM(I)=HOC(I,3) 478 ELAT(I)=H0C(I,2) 479 1 15 CONTINUE 480 C 48 1 C 482 c****» CALCULATE PUMA COORDINATES X.Y.Z.O.A.T FROM THE ROBOT TOOL 483 c»»*** MOUNTING FLANGE FRAME MATRIX 'H06'. 484 c 485 CALL NOAP1(H06,XYZOAT) 486 c 487 c 488 c**«** READ THE WELDING CURRENT AND SPEED FROM THE WELD PARAMETER 489 c***** FILE 'WPARAM'. IF WELDING DIRECTION IS REVERSE, READ THE 490 c»**** FILE FROM END TO BEGINNING 49 1 c 492 IF(IDIR.EQ.O) GO TO 225 493 BACKSPACE 7 494 READ(7 , * ) CURRNT,SPEED 495 BACKSPACE 7 496 GO TO 226 497 225 CONTINUE 498 READ(7,* ) CURRNT,SPEED 499 226 CONTINUE 500 C 501 C 502 WRITE ROBOT COORDINATES, WELDING CURRENT AND SPEED, 503 c** * * * AND SEAM LATERAL AND NORMAL DIRECTION VECTORS INTO 504 c***»* FILE 'ROBDAT'. 505 c 506 WRITE(3,99) Jd,(XYZOAT(I).I=1,6),CURRNT,SPEED 507 99 FORMAT(IX.I4.3F8.1.3F8.2.F8.2.F7.3) 508 WRITE(3,98) (ENORM(I),1=1,3).(ELAT(J),J=1,3) 509 98 F0RMAT(5X,6F11.3) 510 C 51 1 C 512 789 CONTINUE 513 C 514 c*»* END WHILE 515 c 516 GO TO 150 517 c 518 ERROR MESSAGES 519 c 520 c***** ROBOT ENVELOPE EXCEEDED 521 c 522 861 CONTINUE - 263 - L i s t i n g o f WORPOS.FOR a t 14:43:16 on AUG 9, 1984 f o r CC1d=DAC0 Page 10 523 ISTAT=1 524 GO TO 555 525 C 526 JOINT LIMIT EXCEEDED 527 c 528 850 CONTINUE 529 ISTAT=2 530 GO TO 555 531 C 532 555 CONTINUE 533 C 534 C 535 c«***» IF A FEASIBLE PATH HAS BEEN FOUND, REPEAT THE LAST PATH 536 c***** AND STORE THE RESULTS IN THE OUTPUT FILES. ' IFEAS' =0 537 c»»**» INDICATES A FEASIBLE PATH HAS BEEN FOUND. 538 c 539 IF(ISTAT.NE.O) GO TO 75 540 IFEAS=IFEAS-1 541 IF(IFEAS.LT.O) GO TO 75 542 c 543 c 544 c*»*«* GET CURRENT TABLE POSITION FROM TEMPORARY FILE 'STOVAR'. 545 c -546 CALL OPEN(1,STOVAR,4,1) 547 c 548 'IVAR' IS A TEMPORARY COUNTER 549 c 550 IVAR=1 551 READ(1.* ) 552 420 CONTINUE 553 READ(1,*,END=410) TABVAR(IVAR) 554 IVAR=IVAR+1 555 GO TO 420 556 410 CONTINUE 557 CALL SHUTO) 558 C 559 WRITE SEAM NUMBER, TABLE POSITION AND ROBOT CONFIGURATION 560 c***»* INTO FILE 'LOCSET' . 561 c 562 GO TO CURRENT END OF FILE 563 c 564 450 CONTINUE 565 READ(4,*.END=400) 566 GO TO 450 567 400 CONTINUE 568 C 569 c»**** APPEND PARAMETERS DESCRIBING SEAM 'IDSEAM' 570 c 571 WRITE(4,*) IDSEAM 572 WRITE(4,*) (TABVAR(I),1=1,6) 573 WRITE(4,*) (ICONF(I),1=1,6) 574 WRITE(4,*) VOLT.WSIZE,FEED,GFR 575 c 576 REWIND FILE 'WPCDAT' 577 c 578 REWIND(2) 579 c 580 RESET THE TOOL ROTATION ANGLE TO ZERO - 264 - L i s t i n g of WORPOS.FOR a t 14:43:16 on AUG 9, 1984 f o r CC1d=DAC0 Page 11 581 C 582 ROTOLD=0. 583 C 584 C 585 C****» REPEAT LAST PATH, WHICH IS FEASIBLE, AND GENERATE THE ROBOT DATA 586 C***»* FILE 'ROBDAT' THIS TIME. 587 C 588 GO TO 160 589 C 590 C 591 75 CONTINUE 592 C 593 C***** CLOSE DATA FILES 'WPCDAT', 'ROBDAT'. 'LOCSET'. 594 C 595 CALL SHUT(2) 596 CALL SHUT(3) 597 CALL SHUT(4) 598 CALL SHUT(7) 599 C 600 C 601 RETURN 602 END - 2 6 5 - L i s t i n g o f XPROD.FOR a t 14:43:20 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE XPROD(A.B.C) 2 C 3 C THIS SUBROUTINE CALCULATES VECTOR C = A X B (CROSS PRODUCT). 4 C 5 C INPUTS: 6 C A(3)=FIRST VECTOR 7 C B(3)=SEC0ND VECTOR 8 C 9 C OUTPUT: 10 C C(3)=N0RMALIZED CROSS PRODUCT, A X B 1 1 c 12 c***»* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 13 c 14 c 15 REAL A ( 3 ) , B ( 3 ) , C ( 3 ) , ABS 16 c 17 C ( 1 ) = A ( 2 ) * B ( 3 ) - A ( 3 ) * B ( 2 ) 18 C(2)=A(3)*B( 1 )-A(1 )*B(3) 19 C(3)=A(1 )*B(2)-A(2)*B( 1 ) 20 c 21 ABS = SQRT(C(1 )**2+C(2)**2+C(3)**2) 22 IF(ABS.E0.O.) GO TO 100 23 c 24 C(1 )=C(1 )/ABS 25 C(2)=C(2)/ABS 26 C(3)=C(3)/ABS 27 c 28 100 CONTINUE 29 RETURN 30 END - 266 - APPENDIX 1.3 CAD FILE GENERATING PROGRAM CADGEN LISTING - 267 - L i s t i n g o f CADGEN.FOR a t 14:50:38 on AUG 9. 1984 f o r CC1d=DAC0 Page 1 1 2 PROGRAM CADGEN 3 C 4 C THIS PROGRAM GENERATES LOCATION AND SURFACE NORMAL DATA 5 C FOR A VERTICAL CYLINDER INTERSECTING A HORIZONTAL PLATE 6 C THE DATA IS STORED IN CAD FILES IN THE CORRECT FORMAT 7 C FOR TESTING AND DEMONSTRATION DF PROGRAM 'AUTOP'. 8 C 9 C 10 c SUBROUTINES CALLED: OPEN 11 c SHUT 12 c 13 14 c 15 BYTE BUF(80),BUF2(80),BUF3(80),BUF4(80),BUF5(80) 16 REAL R(3),N1(3),N2(3),R0(3) 17 INTEGER ISTAT.IWELD 18 DATA ISTAT/O/,IWELD/O/ 19 DATA BUF/'C. 'A ' .'D ' . ' 1 ' . ' . ' , 'D','A', 'T'.72*0/ 20 DATA BUF2/'C,'A'.'D','2','.','D','A'.'T'.72*0/ 21 DATA BUF3/'C,'A','D','3','.'.'D','A','T'.72*0/ 22 DATA BUF4/'C,'A'.'D','4','.'.'D','A','T',72*0/ 23 DATA BUF5/'C,'A'.'D','5','.','D'.'A'.'T'.72*0/ 24 DATA RO/100.,100..100./ 25 c 26 c*«*»* CREATE FORMATTED FILES 27 c 28 IEND=10O 29 c 30 RADIUS=100. 31 PI=3.14159 32 TW0PI=2.*PI 33 HALFPI=.5*PI 34 STEP=TWOPI/100. 35 THETA=0. 36 c 37 c****» DUMMY WELD PARAMETERS 38 c 39 JTYPE=1 40 XSECT=.5 41 T1=.25 42 T2=.5 43 NP0INT=26 44 ICURVE=1 45 c 46 DO 500 IND=1.4 47 INDEX=1 48 IF(IND.EO.I) CALL OPEN(1,BUF,2,1) 49 IF(IND.EQ.2) CALL OPEN(1,BUF2,2,1) 50 IFUND.EQ.3) CALL OPEN( 1 , BUF 3 ,2,1) 51 IF(IND.E0.4) CALL OPEN(1,BUF4,2,1) 52 c 53 c»*»** WRITE JTYPE,XSECT.T1,T2 INTO CAD FILE. 54 c 55 WRITE( 1 ) JTYPE.XSECT,T1,T2,NPOINT.ICURVE 56 c 57 THETA=FLOAT(IND-1)*HALFPI 58 c - 268 - L i s t i n g of CADGEN.FOR a t 14:50:38 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C WHILE (THETA .LT. TWOPI) GO C 61 DO 10Q IP0i'NT=T,26 62 R(1 )=R0(1 )+RADIUS*COS(THETA) 63 R(2)=R0(2)+RADIUS*SIN(THETA) 64 R(3)=R0(3) 65 N1(1 )=COS(THETA) 66 N1(2)=SIN(THETA) 67 N1(3)=0. 68 N2(1)=0. 69 N2(2)=0. 70 N2(3)=1. 71 C 72 C WRITE RESULTS TO F I L E . 73 C 74 WRITE( 1 ) INDEX.(R(I),I=1.3),(N1(K),K=1,3).(N2(L),L=1,3), 75 * ISTAT,IWELD 76 WRITE(5,900) INDEX,(R(I),1=1,3) 77 900 F0RMAT(I5,3F12.4) 78 C 79 C INCREMENT THETA,INDEX 80 C 81 THETA=THETA+STEP 82 INDEX=INDEX+1 83 100 CONTINUE 84 C 85 C END WHILE 86 C 87 CALL SHUT(1) 88 500 CONTINUE 89 C 90 R(1)=200. 91 R(2)=100. 92 R(3)=100. 93 N1( 1 ) = 1 . 94 N1(2)=0. 95 N1(3)=0. 96 N2(t)-1 . 97 N2(2)=0. 98 N2(3)=0. 99 CALL OPEN(1.BUF5.2,1) 100 C 101 NP0INT=50 102 ICURVE=0 103 C 104 WRITE(1 ) JTYPE,XSECT,T1,T2.NPOINT,ICURVE 105 C 106 DO 300 I = 1 ,50 107N WRITE( 1 ) I,(R(K) ,K=1,3),(N1(L),L=1.3),(N2(M),M=1,3),ISTAT 108 R(3)=R(3)+4. 109 WRITE(5,900) I . ( R ( J ) .J=1.3) 1 10 30O CONTINUE 1 1 1 C 1 12 GO TO 999 1 13 C 1 14 10 WRITE(5.11 ) 1 15 1 1 FORMAT(' ERROR ENCOUNTERED IN OPEN STATEMENT') 116 C - 2 6 9 - L i s t i n g of CADGEN.FOR a t 14:50:38 on AUG 9. 1984 f o r CC1d=DAC0 Page 117 999 CONTINUE 1 18 C 119 CALL SHUT(1) 120 C 121 STOP 122 END - 270 - APPENDIX 1.4 TESTIN PROGRAM LISTING - 271 - L i s t i n g o f TESTIN.FOR a t 14:48:01 on AUG 9. 1984 f o r CCid=0AC0 Page 1 1 C PROGRAM TESTIN 2 C 3 C THIS PROGRAM TESTS FOR INTERFERENCE BETWEEN A FACE OF A SOLID 4 C POLYHEDRON AND THE VOLUME SWEPT OUT BY A CYLINDER AS IT IS 5 C TRANSLATED AND ROTATED FROM AN INITIAL POSITION TO A FINAL 6 C POSITION. 7 C 8 C DEFINITION OF VARIABLES AND CONSTANTS: 9 C 10 C A1.B1- ENDPOINTS OF INITIAL CYLINDER SPINE POSITION. 11 C A2.B2- ENDPOINTS OF FINAL CYLINDER SPINE POSITION. T2 C P1.P2....P8- CORNER POINTS OF PARAMETRIC SWEPT VOLUME. 13 C VERT- POLYHEDRA VERTEX LIST. 14 C FACES- POLYHEDRA FACE PLANE PARAMETER LIST. 15 C IFACE- POLYHEDRA FACE LIST. 16 C RADIUS- RADIUS OF CYLINDER 17 C E1.E2- ENDPOINTS OF CURRENT EDGE 18 C K1.K2- SPINE DIRECTION VECTORS. 19 C K1N.K2N- NORMALIZED K1.K2 20 C 21 C SUBROUTINES CALLED: 22 C 23 C CYLINT 24 C DIFF 25 C DRCYL 26 C FACE 27 C FACINT 28 C FACYL 29 C INVOL 30 C SURFIN 31 C SWPVOL 32 C UNIT 33 C 34 C 'IGL' SUBROUTINES CALLED: 35 C 36 C CMCLOS 37 C CMOPEN 38 C EYEBAL 39 C GRSTOP 40 C GRSTRT 41 C VRP3D ai C WIND3D 43 C ZPERSP 44 C 45 C 46 C*«***«*«**««*****«»«»*******«.«»*****««***«*»*»««****»»•«*»**«»**«****»* 47 C 48 REAL E 1 ( 3 ) . E 2 ( 3 ) 49 REAL K1(3),K2(3),K1N(3),K2N(3) 50 C 51 COMMON /SPINES/A1(3),B1(3),A2(3),B2(3) 52 COMMON /DATA/VERT(100O,3),FACES(100,4),IFACE(100,2),IOBJ( 10, 2) 53 COMMON /CORNER/P1(3),P2(3) ,P3(3) ,P4(3),P5(3),P6(3),P7(3),P8(3) 54 COMMON /RADIUS/RADIUS 55 C 56 C****» INITIALIZE 'IGL' GRAPHICS ENVIRONMENT 57 C 58 CALL GRSTRT(4027.1) - 272 - L i s t i n g of TESTIN.FOR a t 14:48:01 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 CALL ZPERSP 60 CALL EYEBAL(100.,100.,100.) 61 CALL VRP3D(0.,0..0.) 62 CALL WIND3D(-50..50.,-50.,50.,-50.,50.) 63 CALL CMCLOS 64 C 65 Q* * * * * GENERATE SWEPT VOLUME. 66 c 67 CALL SWPVOL 68 c 69 c***»* CALCULATE AND DRAW CYLINDERS AT INITIAL AND FINAL POSITIONS. 70 c 71 CALL DIFF(B1,A1,K1) 72 CALL UNIT(K1,K1N) 73 CALL DIFF(B2,A2.K2) 74 CALL UNIT(K2,K2N) 75 CALL DRCYL(A 1,B1,K1N) 76 CALL DRCYL(A2,B2,K2N) 77 c 78 c».» * ** INTERACTIVELY SPECIFY A TYPICAL POLYHEDRON FACE FOR TESTING. 79 c 80 CALL FACE 81 c 82 c**»»* FIND INTERSECTIONS BETWEEN FACE EDGES AND SWEPT VOLUME SURFACES 83 c 84 c*» * * * REPEAT FOR EACH EDGE OF THE FACE... 85 c 86 DO 10 I=1.IFACE(1,2) 87 c 88 Q * * * * FIND THE ENDPOINTS E1.E2 OF THE CURRENT'EDGE. 89 c 90 00 15 d=1,3 91 E1(d)=VERT(1,0) 92 E2(J)=VERT(1+1.J) 93 15 CONTINUE 94 c 95 TEST FOR INTERSECTION BETWEEN THE CURRENT EDGE AND THE FOUR 96 c* * * * * BOUNDING PARAMETRIC FACES OF THE SWEPT VOLUME. 97 c 98 CALL SURFIN(P1,P2.P3.P4.E1,E2,INT) 99 IF (INT.EO.1) GO TO 900 100 c 101 CALL SURFIN(P5,P6,P7,P8,E1,E2.INT) 102 IF (INT.EO.1) GO TO 900 103 c 104 CALL SURFIN(P1,P5,P6,P2,E1,E2,INT) 105 IF (INT.EO.1) GO TO 900 106 C 107 CALL SURFIN(P3,P4.P8.P7.E1,E2.INT) 108 IF (INT.EO.1) GO TO 900 109 c 1 10 FIND INTERSECTIONS BETWEEN EDGES AND CYLINDRICAL VOLUMES 1 1 1 c 112 CALL CYLINT(A 1,B1,E1,E2,INT) 1 13 IF(INT.EO.1) GO TO 901 1 14 c 115 CALL CYLINT(A2,B2.E1,E2,INT) 1 16 IF(INT.EQ.1) GO TO 901 - 273 - L i s t i n g of TESTIN.FOR a t 14:48:01 on AUG 9. 1984 f o r CC1d=DAC0 Page 3 1 17 C 1 18 10 CONTINUE 1 19 c 120 c***»* END OF DO LOOP *************************************** 121 c 122 c****» TEST IF ANY SWEPT VOLUME EDGE INTERSECTS FACE. 123 c 124 CALL FACINT(1,P1,P2,INT) 125 IFUNT.EQ.1) GO TO 980 126 c 127 CALL FACINT(1,P2,P3,INT) 128 IF(INT.EO.1) GO TO 980 129 c 130 CALL FACINT(1.P3.P4.INT) 131 IF(INT.EO.1) GO TO 980 132 c 133 CALL FACINT(1.P4.P1,INT) 134 IF(INT.EO.1 ) GO TO 980 135 c 136 CALL FACINT(1,P1,P5.INT) 137 IF(INT.EO.1) GO TO 980 138 c 139 CALL FACINT(1.P2.P6,INT) 140 IF(INT.EQ.1) GO TO 980 141 c 142 CALL FACINT(1.P3.P7,INT) 143 IF(INT.EQ.1) GO TO 980 144 c 145 CALL FACINT(1,P4,P8,INT) 146 IF(INT.EQ.1) GO TO 980 147 c 148 CALL FACINT(1.P5.P6,INT) 149 IF(INT.EQ.1) GO TD 980 150 c 151 CALL FACINT(1.P6.P7,INT) 152 IF(INT.EQ.1) GO TO 980 153 c 154 CALL FACINT(1,P7,P8.INT) 155 IF(INT.EQ.1) GO TO 980 156 c 157 CALL FACINT(1.P8.P5.INT) 158 IF(INT.EQ.1) GO TO 980 159 c 160 c****» TEST IF CYINDER IN INITIAL OR FINAL POSITION INTERSECTS ANY FACE. 161 c 162 CALL FACYL(1,A1,B1,INT) 163 IF(INT.EQ.1 ) GO TO 990 164 c 165 CALL FACYL(1,A2,B2,INT) 166 IF(INT.EQ.1) GO TO 990 167 c 168 Q** * * * TEST IF. A POLYHEDRON LIES INSIDE THE SWEPT VOLUME. 169 c 170 CALL INV0L(1,INT) 171 IF(INT.EQ.1) GO TO 991 172 c 173 GO TO 998 174 c - 274 - L i s t 1 n g of TESTIN FOR a t 14:48 175 c**«** INTERSECTON 176 c 177 900 WRITE(5,*) 178 GO TO 999 179 901 WRITE(5,*) 180 GO TO 999 181 980 CONTINUE 182 WRITE(5.*) 183 GO TO 999 184 990 WRITE(5,*) 185 GO TO 999 186 991 WRITE(5,*) 187 GO TO 999 188 C 189 998 CONTINUE 190 WRITE(5.*) 191 C 192 999 CONTINUE 193 C 194 CALL CMOPEN 195 CALL GRSTOP 196 C 197 STOP 198 END * * * * * * * * * 1 * * * * * * * * 4 FACE EDGE INTERSECTED SWEPT SURFACE ' FACE EDGE INTERSECTED CYLINDRICAL ENDS ' SWEPT VOLUME EDGE INTERSECTED FACE ' CYLINDER INTERSECTED FACE ' POLYHEDRON LIES ENTIRELY INSIDE SWEPT VOLUME NO INTERSECTIONS WERE DETECTED ' - 275 - L i s t i n g o f CORNER.FOR a t 14:43:49 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE CORNER 2 C 3 C THIS SUBROUTINE FINDS THE CORNER POINTS OF THE SWEPT VOLUME. 4 C 5 C COMMON BLOCKS ACCESSED: /LINK/./RADIUS/./VOLUME/./SNORM/ 6 C COMMON BLOCKS MODIFIED: /CORNER/ 7 C 8 C************************************************* ***************************** 9 c 10 REAL LEN.RADIUS 11 REAL AN(3),BN(3),CN(3) 12 REAL N(3) 13 c 14 COMMON /LINK/H1(4.4),H2(4,4).LEN 15 COMMON /RADIUS/RADIUS 16 COMMON /V0LUME/P(3),U0(3),U1(3).V0( 3) . V1 (3) 17 COMMON /SN0RM/0AN(3),DBN(3),DCN(3) 18 COMMON /C0RNER/P1(3) ,P2(3) ,P3(3) ,P4(3) , P 5 ( 3 ) > 6 ( 3 ) , P 7 ( 3 ) , P 8 ( 3 ) 19 c 20 c 21 c****» LAMBDAU=LAMBDAV=0 22 c 23 DO 10 1=1.3 24 N(I)=DAN(I ) 25 10 CONTINUE 26 c 27 ALPHA = SQRT(N(1 )»*2+N(2)**2+N(3)**2) 28 c 29 DO 20 1=1.3 30 P1(I)=P(I)-RAOIUS*DAN(I)/ALPHA 31 P5(I)=P1(I)+2.*RADIUS«DAN(I)/ALPHA 32 20 CONTINUE 33 c 34 c 35 c***«* LAMBDAU=1.LAMBDAV=0 36 c 37 DO 30 1=1,3 38 N(I)=DAN(I)+DBN(I) 39 30 CONTINUE 40 C 41 ALPHA=SQRT(N(1)»*2+N(2)**2+N(3)**2) 42 c 43 DO 40 1=1.3 44 P2(I)=P(I)+U0(I)-RADIUS*N(I)/ALPHA 45 P6(I)=P2(I)+2.*RADIUS*N(I)/ALPHA 46 40 -CONTINUE 47 C 48 C 49 c****» LAMBDAU=1.LAMBDAV=1 50 c 51 DO 50 1=1.3 52 N(I)=DAN(I)+DBN(I)+DCN(I) 53 50 CONTINUE 54 C 55 ALPHA=SORT(N(1)**2+N(2)**2+N(3)*«2) 56 C 57 DO 60 1=1.3 58 P3(I)=P(I)+U0(I)+V1(I)-RADIUS*N(I)/ALPHA - 276 - L i s t i n g of CORNER.FOR at 14:43:49 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 P7(I )=P3(I ) + 2.*RADIUS*N(I)/ALPHA 60 60 CONTINUE 61 C 62 C 63 C***«* LAMBDAU=0,LAMBDAV=1 64 c 65 DO 70 I=1.3 66 N(I)=DAN(I)+DCN(I) 67 70 CONTINUE 68 c 69 ALPHA=SQRT(N(1)**2+N(2)**2+N(3)**2) 70 c 71 DO 80 1-1.3 72 P4(I)=P(I)+V0(I)-RADIUS*N(I)/ALPHA 73 P8(I)=P4(I)+2.*RADIUS*N(I)/ALPHA 74 80 CONTINUE 75 C 76 RETURN 77 END - 277 - L i s t i n g of CYLEND.FOR a t 14:43:50 on AUG 9. 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE CYLEND(P.U,PC,U1,01.02.INT) 2 C 3 C THIS SUBROUTINE TESTS IF AN EDGE WITH ENDPOINTS 01,02 4 C INTERSECTS THE END OF A CYLINDER OF RADIUS 'RADIUS'. 5 C P,U(3) DEFINE THE PLANE OF THE CIRCLE AND PC(3) 6 C DEFINES ITS CENTER. U1 IS THE EDGE DIRECTION VECTOR. 7 c 8 c INPUT ARGUMENTS: P,U(3 ) , PC(3 ) ,U1(3 ) ,01(3),02(3) 9 c 10 c OUTPUT ARGUMENTS: INT- 0=NO INTERSECTION 11 c 1=INTERSECTI0N 12 c 13 c SUBROUTINES CALLED: DIFF 14 c DOTPRD 15 c VLEN 16 c 17 c » * * « . « » « » « » » . . « * * . « * « « » « « « * , « * * . « « « * » » » * * « « » « * . « « » * * « * « * » * « * » * * . » . * » » 18 c 19 REAL U(3),PC(3),U1(3) ,01(3) ,02(3),TEMP(3),PINT(3) 20 c 21 COMMON /RADIUS/RADIUS 22 c 23 c 24 INT=0 25 c 26 c * * * * » TEST IF BOTH ENDPOINTS ARE ON THE SAME SIDE OF THE PLANE. 27 c 23 CALL D0TPRD(Q1,U,01U) 29 CALL DOTPRD(02,U.Q2U) 30 c 31 Z=(Q1U-P)*(Q2U-P) 32 c 33 IF(Z.GT.O.) GO TO 80O 34 c 35 c * * * » * TEST IF THE SPINE IS PARALLEL TO THE PLANE. 36 c 37 IF(01U.E0.0U2) GO TO 800 38 c 39 c * » * » * IF THE SPINE INTERSECTS THE PLANE, FIND THE INTERSECTION POINT 40 c 41 RLAMB=(P-Q1U)/(Q2U-Q1U) 42 c 43 DO 20 1=1.3 44 PINT(I)=Q1(I)+RLAMB*U1(I) 45 20 CONTINUE 46 c 47 c * * * * * FIND THE DISTANCE FROM THE INTERSECTION POINT TO THE CENTER OF 48 c * * * * » CYLINDER FACE. 49 c 50 CALL OIFF(PINT,PC,TEMP) 51 CALL VLEN(TEMP.DIST) 52 c 53 c * * * « * TEST IF THE DISTANCE IS LESS THAN THE RADIUS. 54 c 55 IF(DIST.GT.RADIUS) GO TO 800 56 c 57 INT=1 58 c - 278 - L i s t i n g o f CYLEND.FOR a t 14:43:50 on AUG 9, 1984 f o r CC1d=DAC0 Page 59 800 CONTINUE 60 RETURN 61 END - 279 - L i s t i n g o f CYLINT.FOR a t 14:43:50 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 o r> SUBROUTINE CYLINT(P1.P2,01.02.INT) •c 3 \* c THIS SUBROUTINE TESTS IF A LINE SEGMENT WITH ENDP3INTS 4 C 01,02 INTERSECTS A CYLINDER WITH SPINE ENDPOINTS 5 c c /* P1.P2. AND A RADIUS 'RADIUS'. to 7 c INPUT ARGUMENTS: P1(3),P2(3)-CYLINDER SPINE ENDPOINTS. 8 Q c 01(3),02(3)-LINE SEGMENT ENDPOINTS. y 10 V- c OUTPUT ARGUMENTS: INT- 0=NO INTERSECTION 11 c 1=INTERSECTI0N 12 c 13 c 14 c DEFINITION OF CONSTANTS AND VARIABLES. 15 c 16 c LAMB 1,LAMB2- PARAMETERS SPECIFYING LOCATION OF COMMON NORMAL. 17 c DIST- DISTANCE BETWEEN EDGE LINE AND SPINE LINE. 18 c PA.PB- DISTANCES FROM ORIGIN TO CYLINDER END FACE PLANES. 19 c UCYL- SURFACE NORMAL OF CYLINDER END FACES. 20 c 21 c SUBROUTINES CALLED: CYLEND 22 c DIFF 23 c DOTPRD 24 c SOLVE 25 c UNIT 26 c VLEN 27 c XPROD 28 c 29 £ * * » « * « * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 30 c 31 REAL LAMB 1,LAMB2 32 REAL P 1 ( 3 ) , P 2 ( 3 ) . 0 1 ( 3 ) , 0 2 ( 3 ) ,U1(3),U2(3),UN1(3),UN2(3),TEMP(3) 33 REAL T 1 ( 3 ) , T 2 ( 3 ) , U N ( 3 ) . U ( 3 ) , T 3 ( 3 ) . T 4 ( 3 ) , D I S T 1 ( 3 ) , D I S T 2 ( 3 ) 34 REAL UTEMP(3) 35 c 36 COMMON /PARAM/LAMB1,LAMB2,DIST,D1,D2 37 COMMON /RADIUS/RADIUS 38 COMMON /ENDS/PA.PB,UCYL(3) 39 c 40 c*-***« FIND DISTANCE BETWEEN EDGE LINE AND SPINE LINE 41 c 42 c*«*** FIRST FIND COMMON NORMAL 43 c 44 CALL DIFF(P1.01,T1) 45 CALL DIFF(P2,P1,U1) 46 CALL DIFF(Q2,Q1,U2) 47 CALL UNIT(U1,UN1 ) 48 CALL UNIT(U2,UN2) 49 CALL XPR0D2(UN1,UN2,U) 50 CALL VLEN(U,A2) 51 c 52 Q** * * * TEST IF LINES ARE PARALLEL. 53 c 54 IF(A2.LT..00001) GO TO 120 55 GO TO 130 56 c 57 120 CONTINUE 58 CALL XPR0D2(T1,UN1.UDIST) - 280 - L i s t i n g o f CYLINT.FOR a t 14:43:50 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 CALL XPR0D2(UDIST,UN1,U) 60 CALL VLEN(UDIST.DIST) 61 IPARAL=1 62 C 63 C****» TEST IF LINES ARE COINCIDENT. 64 C 65 IF(DIST.LE..000001) GO TO 800 66 GO TO 140 67 130 CONTINUE 68 CALL DOTPRD(T1,U,A 1 ) 69 DIST=ABS(A1/A2) 70 140 CONTINUE 71 C 72 C 73 c * * * * * TEST DISTANCE AGAINST RADIUS 74 C 75 IF(DIST.GT.RADIUS) GO TO 900 76 C 77 c * * * * * IF THE SPINE AND EDGE ARE PARALLEL, INTERSECTION CAN ONLY OCCUR 78 C*«*** IF EITHER END OF THE EDGE IS INSIDE THE CYLINDER. 79 C 80 IF(IPARAL.EO.1) GO TO 200 81 C 82 C****» FIND LENGTHS OF SPINE.RLEN1.AND EDGE.RLEN2. 83 C 84 CALL VLEN(U1,RLEN1) 85 CALL VLEN(U2.RLEN2 ) 86 C 87 C***»* FIND VALUES OF LAMB 1.LAMB2 88 C 89 CALL UNIT(U.UN) 90 C 91 CALL S0LVE(P1,01,U1,U2,UN,DIST,LAMB 1,LAMB2) 92 C 93 C*«*** TEST LAMB 1 ,LAMB2 94 C 95 IF(LAMB 1.GE.O. .AND.LAMB 1.LE.1.) IFLG1 = 1 96 IF(LAMB2.GE.0..AND.LAMB2.LE.1.) IFLG2=1 97 C 98 IF(IFLG1.EO.1.AND.IFLG2.EO.1) GO TO 800 99 C 100 C***** FIND DISTANCES OF EDGE ENDPOINTS 01,02 FROM SPINE 101 C 102 CALL XPR0D2(T1,UN1,DIST1) 103 CALL VLEN(DIST1,D1) 104 CALL DIFF(P1,02,T2) 105 CALL XPR0D2(T2,UN 1,DIST2) 106 CALL VLEN(DIST2,D2) 107 C 108 C****» TEST IF DISTANCES ARE GREATER THAN THE RADIUS. 109 C 110 IF(D1.GT.RADIUS.AND.D2.GT.RADIUS) GO TO 900 111 C 112 c***«* CALCULATE PA.PB.UCYL FOR CYLINDER ENDS 113 C 114 CALL DIFF(P2,P1.UTEMP) 115 CALL UNIT(UTEMP,UCYL) 116 CALL D0TPRD(P1,UCYL.PA) - 281 - L i s t i n g of CYLINT.FOR a t 14:43:50 on AUG 9, 1984 f o r CC1d=DAC0 Page 3 1 17 CALL D0TPRD(P2,UCYL,PB) 118 C 1 19 C***»* TEST FOR INTERSECTION OF EDGES WITH ENDS OF CYLINDER 120 c 121 IF(IFLG1.EO.O) GO TO 100 122 GO TO 200 123 c 124 100 CONTINUE 125 c I 126 CALL CYLEND(PA.UCYL.P1,U2.Q1,02,INT1) | 127 IF(INT1.EQ.1) GO.TO 800 128 c 129 CALL CYLEND(PB,UCYL,P2,U2,Q1.Q2.INT2) 130 IF(INT2.EQ.1) GO TO 800 131 c 132 20O CONTINUE 133 C 134 c***»* TEST IF EITHER ENDPOINT OF THE EDGE LIES INSIDE THE CYLINDER 135 c 136 CALL DIFF(Q1,P1,T3) 137 CALL XPR0D2(T3.UN2,TEMP) 138 CALL VLEN(TEMP,LAMB2) 139 IF(LAMB2.GE.0..AND.LAMB2.LE.1.) GO TO 800 140 c 141 CALL DIFF(Q2,P1,T4) 142 CALL XPR0D2(T4.UN2,TEMP) 143 CALL VLEN(TEMP,LAMB2) 144 IF(LAMB2.GE.0..AND.LAMB2.LE.1.) GO TO 800 145 c 146 GO TO 900 147 c 148 800 CONTINUE 149 C 150 INT=1 151 C 152 900 CONTINUE 153 RETURN 154 END - 282 - L i s t i n g of DET2X2.FOR a t 14:44:07 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE DET2X2(V1,V2,DET) 2 C 3 C THIS SUBROUTINE FINDS THE DETERMINANT OF A 2 X 2 MATRIX 4 C OF THE FORM [V1.V2], WHERE V1 ( 2 ) . V 2 ( 2 ) . 5 C 6 r* REAL V1(2),V2(2) 1 8 DET=V1(1)*V2(2)-V1(2)*V2(1) 9 RETURN 10 ENO - 283 - L i s t i n g of DET3X3.FOR a t 14:44:07 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE DET3X3(V1,V2.V3,DET) ti. 3 c THIS SUBROUTINE FINDS THE DETERMINANT OF A 3 X 3 MATRIX 4 e c OF THE FORM [V1.V2.V3], V1(3),V2(3 ) .V3(3 ) . 3 6 •T L, REAL V1(3).V2(3).V3(3) 1 8 DET=V1(1)*V2(2)*V3(3) + V1 (2 ) *V2(3)*V3(1) + V1(3)*V2(1)*V3(2) 9 * V1 ( 3 ) * V 2 ( 2 ) * V 3 ( 1 ) - V 1 ( 2 ) * V 2 ( 1 )*V3(3)-V1(1)*V2(3)*V3(2 ) 10 c 11 RETURN 12 END - 284 - L i s t i n g o f DIFF.FOR a t 14:44:07 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 SUBROUTINE DIFF(V2,V1.DIF) 2 C 3 C THIS SUBROUTINE PERFORMS THE VECTOR DIFFERENCE OPERATION 4 C DIFF=V2-V1 5 C € REAL V 1 ( 3 ) . V 2 ( 3 ) . D I F ( 3 ) 7 C 8 DO 10 1=1,3 9 D I F ( I ) = V 2 ( I ) - V 1 ( I ) 10 10 CONTINUE 11 C 12 RETURN 13 END - 2 8 5 - L i s t i n g of DOTPRD.FOR a t 14:44:28 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE DOTPRD(V1,V2.PROD) 2 C 3 C THIS SUBROUTINE FINDS THE DOT PRODUCT OF TWO VECTORS, 4 C V 1 ( 3 ) , V 2 ( 3 ) . 5 C 6 C INPUTS: V 1 ( 3 ) , V 2 ( 3 ) - TWO VECTORS 7 C 8 C OUTPUTS: PROD- THE DOT PRODUCT PR0D=V1.V2 9 C 10 11 REAL V1(3),V2(3) 12 c 13 PR0D=V1(1)*V2(1)+V1(2)*V2(2)+V1(3)*V2(3) 14 c 15 RETURN 1G END - 286 - L i s t i n g o f DRCYL.FOR a t 14:44:29 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE DRCYL(A.B.K) 2 C 3 C THIS SUBROUTINE DRAWS A CYLINDER AS AN OCTAHEDRAL APPROXIMATION. 4 C THE CYLINDER IS DEFINED BY SPINE ENDPOINTS 'A'.'B' AND BY SPINE 5 C DIRECTION VECTOR ' K' . 6 C 7 C INPUT ARGUMENTS: A ( 3 ) . B ( 3 ) - CYLINDER SPINE ENDPOINTS. 8 C K ( 3 ) - SPINE DIRECTION VECTOR. 9 C 10 C COMMON BLOCKS ACCESSED: /RADIUS/ 1 1 C 12 c SUBROUTINES CALLED: ROTK 13 c TRANS3 14 c UNIT 15 c VLEN 16 c XPROD 17 c 18 c 'IGL' GRAPHICS SUBROUTINES: CMCLOS 19 c CMOPEN 20 c DRAW3D 21 c M0VE3D 22 c 23 c 24 25 c 26 REAL A(3),B(3),K(3).R(3).ROT(4,4),RW(3).PA(8.3),PB(8,3) 27 REAL X(3),Y(3),RN(3) 28 c 29 c 30 DATA X/1.,0..0./ 31 DATA Y/0..1..0./ 32 DATA DTHETA/O.7854/ 33 c 34 COMMON /RADIUS/RADIUS 35 c 36 c FIND A RADIUS VECTOR 'R' NORMAL TO THE SPINE 'K'. 37 c 38 CALL XPROD(X.K.R) 39 CALL VLEN(R.RLEN) 40 IF(RLEN.LT..00001) GO TO 100 41 GO TO 200 42 100 CONTINUE 43 CALL XPROD(Y.K.R) 44 200 CONTINUE 45 c 46 CALL UNIT(R.RN) 47 c 48 THETA=0. 49 c 50 c * * * * « ROTATE THE RADIUS VECTOR ABOUT THE SPINE TO GENERATE POINTS 51 c * * * » * ON THE CYLINDER. 52 c 53 DO 10 1=1,8 54 CALL ROTK(K,THETA.ROT) 55 CALL TRANS3(ROT,RN,RW) 56 DO 20 d=1.3 57 PA(I,d)=A(J)+RADIUS*RW( J) 58 PB(I,J)=B(d)+RADIUS*RW(d) - 287 - L i s t i n g of DRCYL.FOR a t 14:44:29 on AUG 9. 1984 f o r CC1d=DAC0 Page 2 59 20 CONTINUE 60 THETA=THETA+DTHETA 61 10 CONTINUE 62 C 63 CALL CMOPEN 64 C 65 C***»* DRAW THE CYLINDER. 66 C 67 CALL M0VE3D(PA(8,1),PA(8,2),PA(8,3)) 68 DO 30 1=1.8 69 CALL DRAW3D(PA(I,1),PA(I,2),PA(I,3)) 70 30 CONTINUE 71 C 72 CALL M0VE3D(PB(8,1),PB(8.2),PB(8,3)) 73 DO 40 1=1.8 74 CALL DRAW3D(PB(I,1),PB(I.2).PB(I.3)) 75 40 CONTINUE 76 C 77 DO 50 1=1.8 78 CALL M0VE3D(PA(I,1),PA(I,2),PA(I,3)) 79 CALL DRAW3D(PB(I.1),PB(I,2),PB(I.3)) 80 50 CONTINUE 81 C 82 CALL CMCLOS 83 C 84 RETURN 85 END - 288 - L i s t i n g o f DRPINT.FOR a t 14:44:29 on AUG 9. 1984 f o r CCid=DACO Page 1 1 SUBROUTINE DRPINT(PINT) 2 C 3 C***«* THIS SUBROUTINE DRAWS A POINT 'PINT' ON THE SCREEN AS A CROSS. 4 C 5 C 'IGL' GRAPHICS SUBROUTINES CALLED: CMCLOS 6 C CMOPEN 7 C DRAW3D 8 C M0VE3D 9 C 10 c*********************************************************** 1 1 c 12 REAL PINT(3) 13 C 14 CALL CMOPEN 15 P1=PINT(1)-1. 16 P2=PINT(1)+1. 17 CALL M0VE3D(P1,PINT(2),PINT(3)) 18 CALL DRAW3D(P2.PINT(2).PINT(3)) 19 P1=PINT(2)-1. 20 P2=PINT(2)+1. 21 CALL M0VE3D(PINT(1),P1,PINT(3)) 22 CALL DRAW3D(PINT(1),P2.PINT(3)) 23 P1=PINT(3)-1. 24 P2=PINT(3)+1• 25 CALL M0VE3D(PINT(1),PINT(2),P1) 26 CALL DRAW3D(PINT(1),PINT(2),P2) 27 CALL CMCLOS 28 C 29 RETURN 30 END - 289 - L i s t i n g o f DRVOL.FOR a t 14:45:00 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE DRVOL 2 C 3 C THIS SUBROUTINE DRAWS THE SWEPT VOLUME ON THE SCREEN. 4 C 5 C COMMON BLOCKS ACCESSED: /CORNER/ 6 C 7 C 'IGL' GRAPHICS SUBROUTINES CALLED: CMCLOS 8 C CMOPEN 9 C DRAW3D 10 C M0VE3D 1 1 C 12 c « * * « * . » * * * * » « * . « * » * » » « * « « * . * * » » * « * * * « » * « * * 13 C 14 COMMON /CORNER/P1(3),P2(3),P3(3),P4(3),P5(3),P6(3).P7(3),P8(3) 15 C 16 C 17 CALL CMOPEN 18 C 19 c * * * » * DRAW SWEPT VOLUME. 20 C 21 CALL M0VE3D(P1(1),P1(2),P1(3)) 22 CALL DRAW3D(P2(1).P2(2),P2(3)) 23 CALL DRAW3D(P3(1),P3(2),P3(3)) 24 CALL DRAW3D(P4(1),P4(2 ) ,P4(3)) 25 CALL DRAW3D(P1( 1 ) .P 1 (2 ) ,P 1 (3 ) ) 26 CALL DRAW3D(P5(1),P5(2),P5(3)) 27 CALL DRAW3D(P6(1),P6(2 ) , P6(3 ) ) 28 CALL DRAW3D(P7(1),P7(2) ,P7(3) ) 29 CALL DRAW3D(P8( 1 ).P8(2 ) ,P8(3)) 30 CALL DRAW3D(P5(1).P5(2).P5(3)) 31 CALL M0VE3DCP2(1).P2(2),P2(3)) 32 CALL DRAW3D(P6(1 ) ,P6(2 ) ,P6(3 ) ) 33 CALL M0VE3D(P3(1).P3(2 ) .P3(3 ) ) 34 CALL DRAW3D(P7(1 ) ,P7(2 ) ,P7(3 ) ) 35 CALL M0VE3D(P4(1 ) ,P4(2) ,P4(3) ) 36 CALL DRAW3D(P8(1 ) ,P8(2),P8(3)) 37 C 38 C****» DRAW AXES ON SCREEN 39 C 40 CALL M0VE3D(0.,0..0.) 41 CALL DRAW3D(100.,0.,0.) 42 CALL M0VE30C0.,0..0.) 43 CALL DRAW3D(0.,100..0.) 44 CALL M0VE3D(O.,0..0.) 45 CALL DRAW3D(0..0.,10O.) 46 C 47 CALL CMCLOS 48 C 49 RETURN 50 END - 2 9 0 - L i s t i n g of ECROSS.FOR a t 14:45:01 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE ECROSS(NVERT.ICOUNT,UR,PINT,LISTE,NECRSS) 2 C 3 C THIS SUBROUTINE COUNTS THE NUMBER OF TIMES A PROJECTED RAY 4 C CROSSES FACE BOUNDARIES WHOSE ENDPOINTS ARE NOT ON THE RAY. 5 C 6 C INPUT ARGUMENTS: 7 C NVERT- INDEX OF FIRST VERTEX OF FACE. 8 C ICOUNT- NUMBER OF VERTICES ON FACE. 9 C UR- DIRECTION VECTOR OF RAY. 10 C PINT- ORIGIN OF RAY. 11 C LISTE- LIST OF EDGES WITH NEITHER VERTEX ON THE RAY. 12 C 13 C OUTPUT ARGUMENTS: 14 C NECRSS- NUMBER OF TIMES THE RAY CROSSES FACE BOUNDARIES AT 15 C EDGES. 16 C 17 C COMMON BLOCKS ACCESSED: /DATA/ 18 C 19 C SUBROUTINES CALLED: EDGINT 20 C 21 22 c 23 REAL UR(3),PINT(3),P1(3),P2(3) 24 INTEGER IEDGE(20),LISTE(10) 25 c 26 COMMON /DATA/VERT(1000,3).FACES(10O.4),I FACE(100,2),IOBJ(10,2) 27 c 28 NECRSS=0 29 c 30 c****» TEST ALL EDGES WHICH HAVE NEITHER VERTEX ON THE RAY. 31 c 32 DO 15 1=1.100 33 c 34. c****» FIND THE NEXT EDGE. 35 c 36 INDEX=LISTE(I) 37 IF(.INDEX.EO.O) GO TO 900 38 c 39 c***»* FIND THE EDGE ENDPOINTS, 'P1','P2'. 40 c 41 DO 20 J=1,3 42 P1(J)=VERT(INDEX.J) 43 P2(J)=VERT(INDEX+1,J) 44 20 CONTINUE 45 c 46 c**»** TEST IF THE RAY INTERSECTS THE EDGE. 47 c 48 CALL EDGINT(P1,P2,UR,PINT,INT) 49 IF(INT.EO.I) NECRSS=NECRSS+1 50 c ,• 51 15 CONTINUE 52 c 53 900 CONTINUE 54 C 55 RETURN 56 END - 291 - L i s t i n g of EDGCRS.FOR a t 14:45:01 on AUG 9, 1984 for CC1d=DAC0 Page 1 1 SUBROUTINE EDGCRS(UR,P1,P2,INT) 2 C 3 C THIS SUBROUTINE FINDS IF A LINE SEGMENT AND A RAY INTERSECT. 4 C THE LINE SEGMENT IS DEFINED BY ENDPOINTS P1(3),P2(3). 5 C THE RAY STARTING POINT IS THE ORIGIN, WITH DIRECTION GIVEN BY 6 c UR(3). 7 c 8 c LET UE(3)=P2(3)-P1(3) 9 c 10 c LINE SEGMENT: R(3 ) =P 1 (3)+TLAMB1*UE(3). 0.LE.TLAMB1.LE.1 1 1 c RAY: R(3)=TLAMB2*UR(3), 0.LE.TLAMB2 12 c 13 c COMBINING: 14 c TLAMB1*UE(3)-TLAMB2*UR(3)=-P1(3) 15 c 16 c******************************** 17 c 18 REAL P1(3).P2(3) ,UR(3) ,M1(2),M2(2),UE(3),M3(2) 19 c 20. INT=0 21 c 22 FIND THE LINE DIRECTION VECTOR 'UE'. 23 c 24 DO 10 1=1,3 25 UE(I)=P2(I)-P1(I) 26 10 CONTINUE 27 c 28 MAKE SURE THAT THE LINE SEGMENT LENGTH IS GREATER THAN ZERO. 29 c 30 UELEN=UE(1)**2+UE(2)**2+UE(3)**2 31 IF(UELEN.LE..00001) GO TO 500 32 c 33 c * * * * * THERE ARE THREE EQUATIONS AND TWO UNKNOWNS. FIND TWO EQUATIONS 34 c * * * » * WHICH GIVE A NON-SINGULAR 2X2 MATRIX. 35 c 36 DO 20 1=1,3 37 c 38 J = I+1 39 IF(I.EQ.3) d=1 40 c 4 1 M1(1)=UE(I) 42 M1(2)=UE(U) 43 M2(1) = -UR(I ) 44 M2(2)=-UR(d) 45 c 46 CALL DET2X2(M1,M2,DET) 47 c 48 IF(ABS(DET).GT..0OOO1) GO TO 100 49 c 50 20 CONTINUE 51 c 52 100 CONTINUE 53 c 54 c * « * * * IF THERE IS NO NON-SINGULAR MATRIX, NO SOLUTION EXISTS. 55 c 56 IF(ABS(DET).LE..00001) GO TO 500 57 c 58 c * * * » * SOLVE THE TWO EQUATIONS FOR THE PARAMETERS 'TLAMB1','TLAMB2'. - 292 - L i s t i n g of EDGCRS.FOR a t 14:45:01 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C 60 M3(1)=-P1(I) 61 M3(2)=-P1(J) 62 C 63 CALL DET2X2(M3,M2,TEMP 1) 64 CALL DET2X2(M1.M3.TEMP2) 65 C 66 TLAMB1=TEMP1/DET 67 TLAMB2=TEMP2/DET 68 C 69 c****» TEST IF THE INTERSECTION POINT LIES ON THE RAY AND ON THE 70 C****» LINE SEGMENT. 71 C 72 IF(TLAMB1.LT.O..0R.TLAMB1.GT.1.) GO TO 500 73 IF(TLAMB2.GT.O.) INT=1 74 C 75 500 CONTINUE 76 RETURN 77 END - 293 - L i s t i n g o f EDGINT.FOR a t 14:45:19 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE EDGINT(P1,P2,UR,PINT,INT) 2 C 3 C THIS SUBROUTINE FINDS IF A LINE SEGMENT AND A RAY INTERSECT. 4 C 5 C INPUT ARGUMENTS: 6 C P 1 ( 3 ) , P 2 ( 3 ) - END POINTS OF THE LINE SEGMENT 7 C UR(3)- DIRECTION VECTOR OF THE RAY 8 C PINT(3)- ORIGIN OF THE RAY 9 c 10 c OUTPUT ARGUMENTS: 1 1 c INT- 0=NO INTERSECTION 12 c 1=INTERSECTI0N 13 c 14 c SUBROUTINES CALLED: DET2X2 15 c 16 c 17 c EQUATION OF LINE SEGMENT: R=P1+TLAMB1*UE, 0.LE.TLAMB1.LE.1 18 c EQUATION OF RAY: R=PINT+TLAMB2*UR. TLAMB2.GEO 19 c 20 c COMBINING: 21 c 22 c TLAMB1*UE-TLAMB2*UR=PINT-P1 23 c 24 c WE GET THREE EQUATIONS IN TWO UNKNOWNS. 25 c SOLVE FOR PARAMETERS 'TLAMB 1 '.'TLAMB2' AND TEST THEIR VALUES. 26 c 27 28 c 29 REAL P1 (3) ,P2(3) ,URO) . P I N T O ) ,M1(2 ) ,M2(2) ,UE(3) ,M3(2) 30 c 31 INT=0 32 c 33 c***»* CALCULATE THE DIRECTION VECTOR 'UE' OF THE LINE SEGMENT. 34 c 35 DO 10 1=1,3 36 U E ( I ) = P 2 ( I ) - P 1 ( I ) 37 10 CONTINUE 38 c 39 c***** FIND TWO EQUATIONS OF THE THREE WHICH WILL GIVE A NON-SINGULAR 40 c****» SOLUTION. 41 c 42 DO 20 1=1,3 43 d = I + 1 44 IF(I.EQ.3) d=1 45 c 46 M1( 1 )=UE(I ) 47 M1(2)=UE(J) 48 M2(1) = -UR(I ) 49 M2(2) = -UR(J ) 50 c 51 CALL DET2X2(M1,M2,DET) 52 c 53 IF(DET.NE.0.) GO TO 100 54 20 CONTINUE 55 c 56 c****» IF NO NON-SINGULAR SOLUTION EXISTS, THE LINE AND RAY ARE PARALLEL 57 c***»* AND THERE IS NO INTERSECTION. 58 c - 294 - L i s t i n g o f EDGINT.FOR a t 1 4 : 4 5 : 1 9 on AUG 9 , 1984 f o r CCid=DACO Page 2 59 GO TO 500 60 C 61 100 CONTINUE 62 C 63 C****» FIND THE CORRESPONDING VALUES ON THE RIGHT HAND SIDE OF THE TWO 64 C***** EQUATIONS. 65 C 66 M3(1)=PINT(I)-P1(I) 67 M3(2)=PINT(J)-P1(d) 68 C 69 C**"** SOLVE FOR THE PARAMETERS 'TLAMB1','TLAMB2'. 70 C 71 CALL DET2X2(M3,M2.TEMPI) 72 CALL DET2X2(M1,MS,TEMP2) 73 C 74 TLAMB1=TEMP1/DET i 75 TLAMB2=TEMP2/DET i 76 C 77 c***»* TEST ' TLAMB1', 'TLAMB2' . 78 C 79 IF(TLAMB1.LT O..0R.TLAMB1.GT.1.) GO TO 500 80 IF(TLAMB2.GT.0.) INT=1 81 C 82 500 CONTINUE 83 RETURN 84 END - 295 - L i s t i n g o f ELIST.FOR a t 14:45:20 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE ELI ST(NVERT.ICOUNT.LISTV,LISTE) 2 C 3 C THIS SUBROUTINE GENERATES A LIST OF EDGE STARTING VERTICES 4 C OF EDGES FOR WHICH NEITHER VERTEX LIES ON THE TEST RAY. 5 C 6 C INPUTS: NVERT- LOCATION OF FIRST VERTEX IN FACE VERTEX LIST. 7 c ICOUNT- NUMBER OF FACE VERTICES. 8 c LISTV- LIST OF FACE VERTICES WHICH LIE ON THE TEST RAY. 9 c 10 c OUTPUTS: LISTE- LIST OF FACE VERTICES NOT ON TEST RAY. 11 c 12 C t ) « ) > t l t > > > l < « t > > l l < > « < t > > l > t < . « . t t l t > t t M « » > < t t l l l l t > > l « > t « > t » » > * > < > 13 c 14 INTEGER LISTV(10),LISTE(100) 15 c 16 c 17 INDEX=1 18 c 19 c * * * « * FIND THE FIRST VERTEX IN LISTV. 20 c 21 IND=LISTV(1)-NVERT 22 c 23 IF IND.LE.1, THERE ARE NO EDGES BEFORE L I S T V ( I ) 24 c 25 IF(IND.LE.1) GO TO 150 26 c 27 c****« ELSE ENTER EDGES INTO LISTE. 28 c 29 DO 10 I=1,IND-1 30 LISTE(INDEX)=I+NVERT-1 31 INDEX=INDEX+1 32 10 CONTINUE 33 c 34 150 CONTINUE 35 c 36 c***»* FIND THE VERTICES BETWEEN THE ENTRIES IN LISTV, AND ENTER THE 37 c***»* EDGES INTO LISTE. 38 c 39 DO 20 1=1,100 40 MV1=LISTV(I) 4 1 MV2 = LISTV(1 + 1 ) 42 IF(LISTV(1+1).EQ.O) GO TO 200 43 c IF(MV2.EO.NVERT ) MV2 = NVERT+ICOUNT-1 44 c 45 NC0UNT=MV2-MV1 46 DO 30 d=1.NCOUNT-2 47 LISTE(INDEX)=MV1+J 48 INDEX=INDEX+1 49 30 CONTINUE 50 c 51 20 CONTINUE 52 C 53 c***** FIND ANY EDGES AFTER THE LAST VERTEX IN LISTV 54 c 55 20O CONTINUE 56 C 57 LEFT=NVERT+IC0UNT-1-MV1 58 IF(LEFT.LT.2 ) GO TO 900 - 296 - L i s t i n g of ELIST.FOR a t 14:45:20 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C 60 DO 40 K=1,LEFT-1 61 LISTE(INDEX)=MV1+K 62 INDEX=INDEX+1 63 40 CONTINUE 64 C 65 C***»* TEST IF THE LAST VERTEX TO THE FIRST VERTEX.IS A VALID EDGE. 66 C 67 900 CONTINUE 68 IF(LEFT.GT.O.AND.IND.NE.O) GO TO 950 69 GO TO 999 70 950 CONTINUE 71 LISTE(INDEX)=NVERT+IC0UNT-1 72 999 CONTINUE 73 C 74 RETURN 75 END - 297 - L i s t i n g o f F A C E . F O R a t 1 4 : 4 5 : 2 0 o n A U G 9 . 1 9 8 4 f o r C C 1 d = D A C 0 P a g e 1 1 S U B R O U T I N E F A C E 2 C 3 C T H I S S U B R O U T I N E A L L O W S T H E U S E R T O I N T E R A C T I V E L Y S P E C I F Y A N 4 C A R B I T R A R Y N U M B E R O F C O P L A N A R V E R T I C E S D E F I N I N G A P O L Y H E D R A L 5 C F A C E . T H E F A C E D A T A I S S T O R E D I N T H E C O R R E C T F O R M A T I N T H E D A T A 6 C S T R U C T U R E S ' V E R T ' , ' F A C E S ' . 7 C 8 C C O M M O N B L O C K S A C C E S S E D : / D A T A / 9 C 1.0 C S U B R O U T I N E S C A L L E D : D O T P R D 11 C X P R O D 1 2 C 1 3 C ' I G L ' G R A P H I C S S U B R O U T I N E S : C M C L O S 1 4 C C M O P E N 1 5 C D R A W 3 D 1 6 C M 0 V E 3 D 1 7 C 1 8 C i g Q * * * * * « » » * » » * * « * « » « * . « * » « » . . « * * « « * * « » * * * « * * » « « « » * » « » * » * * » 2 0 C 2 1 R E A L T 1 ( 3 ) , T 2 ( 3 ) , U ( 3 ) , P 1 ( 3 ) , P I N T ( 3 ) , P 2 ( 3 ) . V 1 ( 3 ) 2 2 C 2 3 C O M M O N / D A T A / V E R T ( 1 0 0 0 . 3 ) , F A C E S ( 1 0 0 , 4 ) , I F A C E ( 1 0 0 , 2 ) , I O B 1 ( 1 0 , 2 ) 2 4 C 2 5 C » « * * * U S E R E N T E R S T H E N U M B E R O F V E R T I C E S . 2 6 C 2 7 W R I T E ( 5 , 1 0 0 ) 2 8 1 0 0 F O R M A T ( $ , ' E N T E R N U M B E R O F F A C E V E R T I C E S ' ) 2 9 R E A D ( 5 , * ) I F A C E ( 1 , 2 ) 3 0 C 3 1 I F A C E ( 1 , 1 ) = 1 3 2 I 0 B d ( 1 , 1 ) = 1 3 3 I 0 B J ( 1 , 2 ) = 1 3.4 C 3 5 C * * * » * U S E R E N T E R S V E R T E X C O O R D I N A T E S . 3 6 C 3 7 D O 1 0 1 = 1 , I F A C E ( 1 , 2 ) 3 8 W R I T E ( 5 , 1 0 1 ) I 3 9 1 0 1 F O R M A T ( $ , ' E N T E R V E R T E X ( ' . 1 2 , ' ) ( X , Y , Z ) ' ) 4 0 R E A D ( 5 , » ) ( V E R T ( I , d ) . J = 1 , 3 ) 4 1 1 0 C O N T I N U E 4 2 C 4 3 D O 5 0 1 1 = 1 . 3 4 4 V E R T ( I F A C E ( 1 , 2 ) + 1 , I I )=VERT(1 , 1 1 ) 4 5 V 1 ( I I ) = V E R T ( 1 . I I ) 4 6 T 1 ( I I ) = V E R T ( 2 . 1 1 ) - V E R T ( 1 , I I ) 4 7 T 2 ( I I ) = V E R T ( 3 , I I ) - V E R T ( 1 , I I ) 4 8 5 0 C O N T I N U E 4 9 C 5 0 c***»* C A L C U L A T E F A C E P L A N E S U R F A C E N O R M A L . 5 1 C 5 2 C A L L X P R 0 D ( T 1 , T 2 . U ) 5 3 C 5 4 D O 5 1 11 = 1 , 3 5 5 F A C E S ( 1 , I I ) = U ( I I ) / S 0 R T ( U ( 1 ) * * 2 + U ( 2 ) * * 2 + U ( 3 ) * * 2 ) 5 6 5 1 C O N T I N U E 5 7 C 5 8 C * * * * » C A L C U L A T E P L A N E D I S T A N C E F R O M O R I G I N . - 298 - L i s t i n g of FACE.FOR at 14:45:20 on AUG 9. 1984 f o r CCid=DAC0 Page 2 59 C SO CALL D0TPRD(V1,U.FACES(1,4)) 61 C 62 CALL CMOPEN 63 C 64 NVERT=IFACE(1.2) 65 C 66 C****» DRAW RESULTING FACE POLYGON. 67 c 68 CALL MOVE3D(VERT(NVERT,1),VERT(NVERT,2),VERT(NVERT.3)) 69 c 70 DO 30 1=1.NVERT 71 CALL DRAW3D(VERT(I,1),VERT(I.2 ) ,VERT(I,3 ) ) 72 30 CONTINUE 73 C 74 CALL CMCLOS 75 C 76 C 77 RETURN 78 END - 299 - L i s t i n g of FACINT.FOR a t 14:45:50 on AUG 9, 1984 f o r CCid=DACO Page 1 1 C 2 RETURN 3 END 4 SUBROUTINE FACINT(NFACE,P1,P2.INT) 5 C 6 C THIS SUBROUTINE TESTS IF A LINE SEGMENT INTERSECTS A FACE. 7 C 8 C INPUT ARGUMENTS: 9 c NFACE- INDEX IDENTIFYING FACE LOCATION IN FACE LIST. 10 c P 1 ( 3 ) , P 2 ( 3 ) - END POINTS OF LINE SEGMENT. 1 1 c 12 c OUTPUT ARGUMENTS: 13 c INT- 0=N0 INTERSECTION FOUND 14 c 1=INTERSECTI0N FOUND 15 c 16 c SUBROUTINES CALLED: DRPINT 17 c INSIDE 18 c PLANE 19 c 20 21 c 22 REAL T 1 ( 3 ) , T 2 ( 3 ) , U ( 3 ) , P 1 ( 3 ) , P I N T ( 3 ) , P 2 ( 3 ) , V 1 ( 3 ) 23 c 24 COMMON /DATA/VERT(1000.3),FACES(100.4),I FACE(100.2),IOBJ(10.2) 25 c 26 INT=0 27 c 28 c***** TEST IF LINE SEGMENT INTERSECTS FACE PLANE. 29 c 30 CALL PLANE(NFACE.P1.P2,INT,PINT) 31 c 32 IF(INT.EO.O) GO TO 900 33 IF(INT.EO.1) GO TO 201 34 c 35 201 CONTINUE. 36 c 37 TEST IF INTERSECTION POINT LIES INSIDE THE FACE BOUNDARIES. 38 c 39 CALL INSIDE(NFACE.PINT,INT) 40 c 41 IF(INT.EQ.O) GO TO 9CO 42 c 43 c***»* DRAW THE INTERSECTION POINT ON THE SCREEN. 44 c 45 CALL DRPINT(PINT) 46 c 47 900 CONTINUE 48 c 49 RETURN 50 END 51 SUBROUTINE FACYL(NFACE,A,B,INT) 52 c 53 c THIS SUBROUTINE TESTS IF EITHER END OF A CYLINDER INTERSECTS 54 c A FACE. 55 c 56 c INPUTS: NFACE- FACE IDENTIFIER. 57 c A.B- ENDPOINTS OF CYLINDER SPINE. 58 c - 300 - L i s t i n g o f FACINT.FOR a t 14:45:50 on AUG 9. 1984 f o r CC1d=DAC0 Page 59 C OUTPUTS: INT- 0=NO INTERSECTION, 1=INTERSECTION, 60 C 61 C SUBROUTINES CALLED: DIFF 62 C DOTPRD 63 C DRPINT 64 C FACINT 65 C XPROD 66 67 C 68 C 69 REAL A(3),B(3),ENDPT(2,3),PINT(3) 70 REAL TEMP(3),RK(3),0(3),RAD(3),U(3).POINT ( 3) 71 C 72 COMMON /DATA/VERT(1000,3),FACES(100,4).I FACE(100, 73 COMMON /RADIUS/RADIUS 74 c 75 INT=0 76 c 77 c****» FIND THE FACE PLANE PARAMETERS U ( I ) , P 78 c 79 DO 10 1=1.3 80 U(I)=FACES(NFACE.I) 81 10 CONTINUE 82 c 83 P=FACES(NFACE,4) 84 c 85 c*»*** SET UP AN ENDPOINTS ARRAY. 86 c 87 DO 20 1=1,3 88 ENDPT(1,I)=A(I) 89 ENDPT(2,I )=B(I ) 90 20 CONTINUE 91 c 92 CALL DIFF(B.A.RK) 93 c 94 DO 500 IND=1,2 95 c 96 DO 30 1=1,3 97 TEMP(I)=ENDPT(IND , I) 98 30 CONTINUE 99 C 100 CALL DOTPRD(TEMP,U,DOT) 101 .DIST=DOT-P 102 IF(DIST.GT.RADIUS.OR.DIST.LT.0.) GO TO 900 103 C 104 CALL XPR0D2(U.RK,0) 105 CALL XPROD(0,RK,RAD) 106 C 107 CALL DOTPRD(RAD,U,UDOT ) 108 IF(ABS(UDOT) .LT. .000001) GO TO 900 109 c 1 10 RADLEN=DIST/UDOT 1 1 1 IF(RADLEN.GT.RADIUS) GO TO 900 112 c 1 13 DO 40 1=1,3 1 14 POINT(I)=TEMP(I)+RADIUS*RAD(I ) 1 15 40 CONTINUE 1 16 C - 301 - L i s t i n g of FACINT.FOR at 14:45:50 on AUG 9, 1984 f o r CC1d=DAC0 Page 3 117 CALL FACINT(NFACE,TEMP,POINT,INT,PINT) 1 18 IF( INT.EO.1) GO TO 900 1 19 C 120 500 CONTINUE 121 C 122 900 CONTINUE 123 IF(INT.EO.I) CALL DRPINT(PINT) 124 C 125 RETURN 126 END - 302 - L i s t i n g o f F A C Y L . F O R a t 14 : 4 5 : 5 2 o n AUG 9, 1984 f o r CC1d=DACO P a g e 1 1 SUBROUTINE F A C Y L ( N F A C E . A . B , I N T ) 2 C 3 C T H I S SUBROUTINE T E S T S I F E I T H E R END OF A CYL I N D E R I N T E R S E C T S 4 C A F A C E . 5 C e C I N P U T S : N F A C E - FACE I D E N T I F I E R . 7 C A.B- ENDPOINTS OF CYLINDER S P I N E . 8 C 9 C OUTPUTS: I N T - 0=N0 I N T E R S E C T I O N . 1 = I N T E R S E C T ION. 10 c 11 c SUBROUTINES C A L L E D : D I F F 12 c DOTPRD 13 c DRPINT 14 c F A C I N T 15 c XPROD 16 c 17 18 c 19 REAL A ( 3 ) , B ( 3 ) , E N D P T ( 2 , 3 ) , P I N T ( 3 ) 2 0 REAL T E M P ( 3 ) , R K ( 3 ) , 0 ( 3 ) . R A D ( 3 ) , U ( 3 ) , P 0 I N T ( 3 ) 21 c 22 COMMON / D A T A / V E R T ( 1 0 0 0 , 3 ) . F A C E S ( 1 0 0 . 4 ) , I F A C E ( 1 0 0 , 2 ) , I O B d ( 1 0 , 2 ) 23 COMMON /RADIUS/RADIUS 24 c 25 INT=0 26 c 27 c***** FIND THE FACE PLANE PARAMETERS U ( I ) , P 28 c 29 DO 10 1=1,3 30 U ( I ) = F A C E S ( N F A C E , I ) 31 10 CONTINUE 32 c 33 P = F A C E S ( N F A C E , 4 ) 34 c 35 c****» S E T UP AN ENDPOINTS ARRAY. 36 c 37 DO 2 0 1=1,3 38 E N D P T ( 1 . I ) = A ( I ) - 303 - L i s t i n g of GENVOL.FOR a t 14:45:52 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE GENVOL(A1,B1,A2,B2) 2 C 3 C THIS SUBROUTINE FINDS THE PARAMETERS WHICH DESCRIBE THE SWEPT VOLUME. 4 C 5 C INPUT ARGUMENTS: A 1 ( 3 ) . B 1 ( 3 ) - INITIAL CYLINDER SPINE ENDPOINTS. 6 C A 2 ( 3 ) , B 2 ( 3 ) - FINAL CYLINDER SPINE ENDPOINTS. 7 C 8 C COMMON BLOCKS ACCESSED: /LINK/,/RADIUS/ 9 C COMMON BLOCKS MODIFIED: /VOLUME/,/SNORM/ 10 C 11 12 C 13 REAL A 1(3),B1(3),A2(3),B2(3) 14 REAL AN(3),BN(3),CN(3) 15 REAL LEN,RADIUS 16 C 17 COMMON /V0LUME/P(3),U0(3),U1(3),V0(3),V1(3) 18 COMMON /SN0RM/DAN(3),DBN(3),DCN(3) 19 COMMON /LINK/H1(4,4),H2(4,4),LEN 20 COMMON /RADIUS/RADIUS 21 C 22 C***»* CALCULATE VECTORS U0.U1.V0.VI 23 C 24 C 25 DO 10 1=1.3 26 U 0 ( I ) = A 2 ( I ) - A 1 ( I ) 27 U 1 ( I ) = B 2 ( I ) - B 1 ( I ) 28 V 0 ( I ) = B 1 ( I ) - A 1 ( I ) 29 V1(I)=B2(I ) -A2( I ) 30 10 CONTINUE 31 C 32 C 33 C****» CALCULATE THE UNIT SURFACE NORMAL 34 C 35 AN(1)=U0(2)*V0(3)-U0(3)*V0( 2) 36 AN(2)=-U0(1)*VO(3)+UO(3)*VO(1) 37 AN(3)=U0(1)*VO(2)-UO(2)*VO(1) 38 C 39 • BN ( 1 ) = U 0 ( 2 ) * ( V 1 ( 3 ) - V 0 ( 3 ) ) - U 0 ( 3 ) * ( V 1 ( 2 ) - V 0 ( 2 ) ) 40 BN(2) = -U0(1)*(V1(3)-V0(3))+U0(3)*(V1(1)-V0( 1)) 41. BN(3)=U0(1)*(V1(2)-VO(2))-U0(2)*(V1( 1)-V0( 1)) 42 C 43 CN(1 ) = V O ( 3 ) * ( V 1 ( 2 ) - V O ( 2 ) ) - V 0 ( 2 ) * ( V I ( 3 ) - V 0 ( 3 ) ) 44 CN(2) = .-V0(3)*(V1 ( 1 )-V0( 1 ) )+V0( 1 )*( V 1 ( 3 ) -V0( 3 ) ) 45 CN(3)=V0(2)«(V1(1)-V0(1))-V0(1)*(V1(2)-V0(2)) 46 C 47 C***** MULTIPLY THE UNIT SURFACE NORMAL BY THE RADIUS TO GET 48 C***»* 'DAN'.'DBN','DCN'. 49 C 50 DO 30 I=1.3 51 DAN(I)=RADIUS*AN(I) 52 DBN(I)=RADIUS*BN(I) 53 DCN(I)=RADIUS»CN(I) 54 30 CONTINUE 55 C 56 C 57 , RETURN 58 END - 304 - L i s t i n g o f INITSV.FOR a t 14:46:10 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE INITSV(FILNAM) 2 C 3 c****« THIS SUBROUTINE INITIALIZES THE LINK PARAMETERS. 4 c 5 c INPUT PARAMETERS: FILNAM- NAME OF FILE CONTAINING LINK DIMENSIONS 6 c AND INITIAL AND FINAL POSITIONS. 7 c 8 c COMMON BLOCKS INITIALIZED: /LINK/,/RADIUS/ 9 c 10 c SUBROUTINES CALLED: OPEN 1 1 c SHUT 12 c 13 c 14 c DEFINITION OF CONSTANTS AND VARIABLES. 15 c 16 c H1.H2- HOMOGENEOUS TRANSFORMATIONS SPECIFYING THE INITIAL AND 17 c FINAL POSITION OF THE LINK. 18 c LEN.RADIUS- LINK LENGTH AND RADIUS. 19 c 20 c*»««,*»»»*«»«»*.»»«**««»«*«.*».»«*«»«***.«*«***«**«»««**«»«**«*«****«** 21 c 22 BYTE FILNAM(20) 23 REAL LEN,RADIUS 24 c 25 COMMON /LINK/H1(4,4),H2(4,4),LEN 26 COMMON /RADIUS/RADIUS 27 c 28 READ 'H1'.'H2'.'LEN'.'RADIUS' FROM THE FILE 'FILNAM'. 29 c 30 CALL OPEN(1.FILNAM,4,1) 31 c 32 READ(1 , * ) 33 DO 10 1=1,4 34 READ(1,«) ( H 1 ( I . J ) ,J=1 ,4) 35 10 CONTINUE 36 READ( 1 .*) 37 DO 20 1=1.4 38 READ(1,*) (H2(I,d).J=1.4) 39 20 CONTINUE 40 READ( 1 ,* ) 41 READO.*) LEN,RADIUS 42 c 43 CALL SHUTO) 44 c 45 RETURN 46 END - 305 - L i s t i n g of INPROJ.FOR a t 14:46:11 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE INPR0J(P1.P2,P3.P4,INT) 2 C 3 REAL P 1 ( 3 ) , P 2 ( 3 ) . P 3 ( 3 ) , P 4 ( 3 ) 4 C 5 INT=0 6 IC0UNT=O 7 C 8 CALL E0GCRS(P1.P2.P3.INT1) 9 IF (INT1.E0.1) IC0UNT=IC0UNT+1 10 CALL E0GCRS(P1,P4,P3,INT1) 11 IF (INT1.E0.1) IC0UNT=IC0UNT+1 12 C 13 ICASE=1 14 C 15 TOTAL 1=0. 16 T0TAL2=O. 17 C 18 DO 10 1=1,3 19 D I F F 1 = ( P 1 ( I ) - P 2 ( I ) ) * * 2 20 DIFF2=(P1 (I ) - P 4 ( I ) ) * * 2 21 TOTAL1=T0TAL1+DIFF1 22 TOTAL2=TOTAL2+DIFF2 23 10 CONTINUE 24 C 25 IF(T0TAL1.LT..00001) ICASE=2 26 IF(T0TAL2 . LT . .00001) ICASE = 3 27 c 28 GO TO (100,200,300) ICASE 29 c 30 100 CONTINUE 31 CALL VRTCRS(P1,P2,P4,INT2) 32 IF (INT2.E0.1) IC0UNT=IC0UNT+1 33 GO TO 400 34 c 35 200 CONTINUE 36 CALL VRTCRS(P1.P3.P4,INT2) 37 IF (INT2.E0.1) IC0UNT=IC0UNT+1 38 GO TO 400 39 C 40 300 CONTINUE 4 1 CALL VRTCRS(P1.P2.P3,INT2) 42 IF (INT2.EQ.1) ICOUNT*ICOUNT+1 43 C 44 400 CONTINUE 45 C 46 ITEST=MOD(ICOUNT,2) 47 C 48 IF (ITEST.EQ.1) INT=1 49 C 50 RETURN 51 END - 306 - L i s t i n g of INSIDE.FOR a t 14:46:11 on AUG 9, 1984 f o r CC1d=DAC0 P a g e 1 1 SUBROUTINE INSIDE(NFACE.PINT,INT) 3 \* C THIS SUBROUTINE TESTS IF AN INTERSECTION POINT 'PINT' 4 e C r+ LIES INSIDE THE FACE 'NFACE' 6 Is c INPUT ARGUMENTS: 7 c NFACE- INDEX IDENTIFYING FACE IN FACE LIST. 8 Q c p PINT- INTERSECTION POINT LYING IN PLANE OF FACE. y 10 I* c OUTPUT ARGUMENTS: 11 c INT- 0='PINT' NOT ON FACE. 12 c 1='PINT' ON FACE. 13 c 14 c COMMON BLOCKS ACCESSED: 15 c /DATA/VERT.FACES.I FACE,IOBd 16 c 17 c SUBROUTINES CALLED: ECROSS 18 c ELIST 19 c VCROSS 20 c 21 22 c 23 REAL PINTO) ,V(3) ,UR(3) 24 INTEGER LISTV(10),LISTE(100) 25 c 26 COMMON /DATA/VERT(1000.3),FACES(100,4),I FACE(100,2),I0BJ(10.2) 27 c 28 NCROSS=0 29 INT=0 30 c 31 c* * * * * FIND STARTING LOCATION 'NVERT' AND NUMBER OF VERTICES 'NCOUNT' IN 32 c * « * » * VERTEX LIST. 33 c 34 NVERT=IFACE(NFACE.1 ) 35 NCOUNT*IFACE(NFACE,2) 36 c 37 c * » » * * DEFINE A RAY 'UR' WITH ORIGIN AT 'PINT' AND CONTAINING THE FIRST 38 c * « * * * VERTEX ' V IN THE FACE VERTEX LIST. 39 c 40 DO 10 1=1.3 41 V(I)=VERT(NVERT,I) 42 UR( I )=V(I)-PINT(I ) 43 10 CONTINUE 44 c 45 c****« FIND THE NUMBER OF TIMES THE RAY CROSSES FACE BOUNDARIES AT 46 c***»* FACE VERTICES. 47 c 48 CALL VCROSS(NVERT.NCOUNT,UR.PINT,LISTV,NVCRSS) 49 c 50 c* * * * * MAKE A LIST OF ALL THE FACE EDGES WHICH HAVE NEITHER 51 VERTEX ON THE RAY. 52 c 53 CALL ELIST(NVERT,NCOUNT.LISTV.LISTE) 54 c 55 c****« FIND THE NUMBER OF EDGES IN THE LIST 'LISTE' CROSSED BY THE RAY. 56 c 57 CALL ECROSS(NVERT.NCOUNT,UR,PINT.LISTE,NECRSS) 58 c - 307 - L i s t i n g of INSIDE.FOR a t 14:46:11 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 c***»* THE TOTAL NUMBER OF BOUNDARY CROSSINGS IS THE SUM OF THE NUMBER OF 60 CROSSINGS AT VERTICES AND THE NUMBER OF CROSSINGS AT EDGES. 61 c 62 NCROSS=NVCRSS+NECRSS 63 c 64 c***»* IF. THE NUMBER OF BOUNDARY CROSSINGS IS ODD. THE INTERSECTION POINT 65 c****» 'PINT' LIES ON THE FACE. 66 c 67 ITEST=M0D(NCR0SS,2) 68 c 69 IF(ITEST.EO.1) INT=1 70 c 71 RETURN 72 END - 308 - L i s t i n g o f INVOL.FOR a t 14:46:32 on AUG 9, 1984 f o r CC1d=DACO Page 1 1 SUBROUTINE INVOL(NFACE,INT) 3 L. c THIS SUBROUTINE CALCULATES WHETHER A GIVEN POINT LIES INSIDE 4 c A PARAMETRIC SWEPT VOLUME. SINCE ANY POINT CAN BE USED, THE ROUTINE 5 c USES THE FIRST VERTEX OF THE FACE 'NFACE'. 6 c 7 c INPUT ARGUMENTS: NFACE- POINTER TO FACE LOCATION IN FACE LIST. 8 c 9 c OUTPUT ARGUMENTS: INT- INTERSECTION FLAG, 0=N0 INTERSECTION, 10 c 1=INTERSECTI0N. 11 c 12 c SUBROUTINES CALLED: NWTVOL 13 c VLEN 14 c 15 c 16 c LET THE SURFACE NORMAL BE N, 17 c 18 c N=DAN+RLAMB(1)»DBN+RLAMB(2)*DCN 19 c 20 c THE VOLUME EQUATION IS THEN 21 c 22 c F(X)=RLAMB(1)*U0+RLAMB(2)*VO+RLAMB(3)*DAN+RLAMB(1)*RLAMB(2)*DELU+ 23 c RLAMB(1)*RLAMB(3)*DBN+RLAMB(2)*RLAMB(3)*DCN+G 24 c 25 c O.LE.RLAMB(1 ) .LE. 1 . 26 c 0.LE.RLAMB(2).LE . 1 . 27 c -RADIUS/LENGTH(N).LE.RLAMB(3).LE.RADIUS/LENGTH(N) 28 c 29 c WHERE DELU=U1-U0 30 c G=P-VERT 31 32 c 33 REAL DELU(3),RLAMBO(3),RLAMB(3),G(3),N(3) 34 c 35 COMMON /DATA/VERT(10O0,3).FACES(IOO,4),IFACE(100.2),IOBJ(10.2) 36 COMMON /V0LUME/P(3),U0(3),U1(3),V0(3).V1(3) 37 COMMON /SNORM/DAN(3),DBN(3),DCN(3) 38 COMMON /RADIUS/RADIUS 39 c 40 INT=0 41 c 42 CALCULATE CONSTANTS, INITIALIZE FIRST GUESS FOR SOLUTIONS RLAMB(3). 43 c 44 DO 10 1=1,3 45 DELU(I)=U1(I)-U0(I) 46 G(I )=P(I )-VERT(1,I) 47 RLAMB(I)=.5 48 10 CONTINUE 49 c 50 c***»* SOLVE THE NON-LINEAR SYSTEM FOR RLAMB(3). 51 c***»* THE ERROR LIMIT IS .01, AND THE MAXIMUM NUMBER OF ITERATIONS IS 10. 52 c 53 CALL NWTVOL(UO,VO,DAN,DELU,DBN,DCN,G, .01,10,RLAMBO.RLAMB,I FLAG) 54 c 55 c*»*** CALCULATE SURFACE NORMAL AT POINT DEFINED BY RLAMB(1),RLAMB(2). 56 c 57 DO 20 1=1,3 58 N(I)=DAN(I)+RLAMB(1)*DBN(I)+RLAMB(2)*DCN(I) - 309 - L i s t i n g of INVOL.FOR at 14:46:32 on AUG 9, 1984 for CC1d=DAC0 Page 2 59 20 CONTINUE 60 C 61 CALL VLEN(N.RLEN) 62 C 63 C * » * * * SCALE RLAMB(3) 64 C 65 RLAMB(3)=RLAMB(3)*RLEN/(2.*RADIUS)+.5 66 C 67 C * * * « * TEST THE PARAMETERS RLAMB(I) AGAINST LIMITS TO DETERMINE 68 C * " * * * IF POINT LIES INSIDE VOLUME. 69 C 70 DO 30 1=1.3 71 IF(RLAMB(I).LE.O.0R.RLAMB(I).GE.1.) GO TO 900 72 30 CONTINUE 73 INT=1 74 C 75 900 CONTINUE 76 C 77 RETURN 78 END - 310 - L i s t i n g o f LINK.FOR at 14:46:32 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE LINK(A 1,B1,A2,B2) 2 C 3 C THIS SUBROUTINE FINDS THE COORDINATES OF THE LINK SPINE ENDPOINTS 4 C AT THE BEGINNING AND END OF A MOTION. 5 C 6 C INPUT ARGUMENTS: A 1 ( 3 ) , B 1 ( 3 ) - SPINE ENDPOINTS IN INITIAL POSITION. 7 C A 2 ( 3 ) , B 2 ( 3 ) - SPINE ENDPOINTS IN FINAL POSITION. 8 C 9 C COMMON BLOCKS ACCESSED: /LINK/,/RADIUS/ 10 C 1 1 C SUBROUTINES CALLED: TRANS3 12 C 13 C 14 C DEFINITION OF CONSTANTS AND VARIABLES: 15 C 16 c H1.H2- HOMOGENEOUS TRANSFORMATIONS FOR THE INITIAL AND FINAL POSITIONS 17 c A1.B1- SPINE ENDPOINTS IN INITIAL POSITION. 18 c A2.B2- SPINE ENDPOINTS IN FINAL POSITION. 19 c 20 21 c 22 REAL A1(3),A2(3) ,B1(3) ,B2(3),TEMP(3) 23 REAL LEN,RADIUS 24 c 25 COMMON /LINK/H1(4,4),H2(4,4).LEN 26 COMMON /RADIUS/RADIUS 27 c 28 DATA TEMP/3*0./ 29 c 30 POINTS 'A1'.'A2' ARE AT THE ORIGINS OF THE HOMOGENEOUS COORDINATE 31 c****« FRAMES 'H1','H2'. 32 c 33 DO 10 1=1,3 34 A1(I)=H1(I,4) 35 A2(I)=H2(I,4) 36 10 CONTINUE 37 c 38 TEMP(1)=LEN 39 c 40 c*««»* TRANSLATE ALONG X-AXIS TO FIND 'B1','B2' 41 C 42 CALL TRANS3(H1,TEMP,B1) 43 CALL TRANS3(H2,TEMP,B2) 44 c 45 c 46 RETURN 47 END - 3 1 1 - L i s t i n g o f MATDET.FOR a t 1 4 : 4 6 : 3 2 o n AUG 9 , 1984 f o r CC1d=DAC0 P a g e 1 2 C 3 C 4 C 5 C 6 C 7 C 8 C 9 C 10 C 1 1 C 12 13 C 14 15 C 16 17 18 19 C 2 0 2 1 SUBROUTINE MATDET(RMAT,DET) T H I S SUBROUTINE F INDS THE DETERMINANT OF A 3 X 3 MATRIX OF THE FORM M A T R I X ( 3 . 3 ) INPUT ARGUMENT: R M A T ( 3 , 3 ) - 3X3 MATRIX OUTPUT ARGUMENT: D E T - DETERMINANT OF 'RMAT' REAL R M A T ( 3 , 3 ) D E T = R M A T ( 1 , 1 ) * ( R M A T ( 2 , 2 ) * R M A T ( 3 , 3 ) - R M A T ( 2 . 3 ) * R M A T ( 3 . 2 ) ) - R M A T ( 1 , 2 ) * ( R M A T ( 2 , 1 ) * R M A T ( 3 , 3 ) - R M A T ( 2 , 3 ) * R M A T ( 3 , 1 ) ) + R M A T ( 1 , 3 ) * ( R M A T ( 2 , 1 ) * R M A T ( 3 , 2 ) - R M A T ( 2 , 2 ) * R M A T ( 3 , 1 ) ) RETURN END - 312 - L i s t i n g of NEWTON.FOR a t 14:46:51 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE NEWTON(A,B,C,0,E,TOL,NUM,XO,X,IFLAG) 2 C 3 C THIS SUBROUTINE USES NEWTONS METHOD FOR SOLVING A SYSTEM 4 C OF NON-LINEAR EQUATIONS OF THE FORM: 5 C 6 c F(X)=A*X(1 )+B*X(2)+C*X(3)+D*X(1)*X(2) + E 7 c 8 c INPUT ARGUMENTS: 9 c A ( 3 ) , B ( 3 ) . C ( 3 ) , D ( 3 ) . E ( 3 ) - KNOWN COEFFICIENT VECTORS. 10 c TOL- MAXIMUM ALLOWABLE ERROR. 1 1 c NUM- MAXIMUM NUMBER OF ITERATIONS. 12 c X 0 ( 3 ) - INITIAL GUESS FOR THE SOLUTION. 13 c 14 c OUTPUT ARGUMENTS: 15 c X ( 3 ) - APPROXIMATE SOLUTION. 16 c IFLAG- 0=SOLUTION FOUND. 17 c 1=N0 SOLUTION. SINGULARITY EXISTS. 18 c 19 c SUBROUTINES CALLED: S0L3X3 20 c VLEN 21 c 22 Q . » . » » » • * » . . * * « « . » * * « » « * » * * . » « » » » « » * » . . . « » * » « * » » * « « * * * » . * » » . « . « * . . * . * » « « * • • « * « 23 c 24 REAL A ( 3 ) . B ( 3 ) , C ( 3 ) , D ( 3 ) . E ( 3 ) , X 0 ( 3 ) , X ( 3 ) , F X ( 3 ) . J A C ( 3 . 3 ) . Y ( 3 ) 25 c 26 IFLAG=0 27 c 28 INITIALIZE THE FIRST APPROXIMATION TO 'X' WITH THE VALUE OF 'XO'. 29 c 30 DO 1 1=1.3 31 X(I)=XO(I) 32 1 CONTINUE 33 c 34 c* » * * * ITERATE THE SOLUTION A MAXIMUM OF 'NUM' TIMES. 35 c 36 DO 150 ICOUNT=1.NUM 37 c 38 SOLVE FOR THE FUNCTION 'FX' FOR THE CURRENT VALUE OF 'X'. 39 c 40 DO 10 1=1,3 41 FX(I ) = A(I )*X(1 )+B(I ) * X ( 2 ) + C ( I ) * X ( 3 ) + D ( I ) * X ( 1 ) * X ( 2 ) + E ( I ) 42 10 CONTINUE 43 c 44 SOLVE THE JACOBIAN MATRIX. 45 c 46 DO 11 1=1,3 47 J A C ( I , 1 )=A(I )+D(I )*X(2) 48 JAC(I,2 ) =B(I)+D(I)*X(1) 49 J A C ( I , 3 ) = C ( I ) 50 11 CONTINUE 51 c 52 SOLVE FOR THE ERROR VECTOR 'Y'. 53 c 54 CALL S0L3X3(JAC,FX,Y,ISOLN) 55 c 56 c * * * » * TEST IF A SOLUTION WAS FOUND. 57 c 58 IF(ISOLN.EQ.1) GO TO 950 - 313 - L i s t i n g o f NEWTON.FOR a t 14:46:51 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C 60 c * * * » * ADD THE ERROR VECTOR ' Y' TO THE LAST VALUE OF 'X' TO GET THE NEXT 61 c * * * * » APPROXIMATION TO 'X'. 62 c 63 DO 12 1=1,3 64 X ( I ) = X ( I ) + Y ( I ) 65 12 CONTINUE 66 C 67 c«*»*« TEST IF THE MAGNITUDE OF THE ERROR IS LESS THAN 'TOL'. 68 c 69 CALL VLEN(Y.YLEN) 70 IF(YLEN.LT.TOL) GO TO 900 71 c 72 c * * * » « TEST IF THE 50LUTI0N IS DIVERGING. INDICATING SINGULARITY. 73 c 74 CALL VLEN(X.XLEN) 75 IF(XLEN.GT.1000.) GO TO 950 76 c 77 150 CONTINUE 78 C 79 950 CONTINUE 80 C 81 IFLAG=1 82 C 83 900 CONTINUE 84 C 85 RETURN 86 END - 314 - L i s t i n g o f NWTVOL.FOR a t 1 4 : 4 6 : 5 1 on AUG 9 , 1984 f o r CC1d=DACO Page 1 1 c- SUBROUTINE NWTVOL(A.B,C,D,E,F,G,TOL,NUM,XO,X,IFLAG) 4. 3 C THIS SUBROUTINE USES NEWTONS METHOD FOR SOLVING A SYSTEM 4 C OF NON-LINEAR EQUATIONS OF THE FORM: 5 c F(X)=A*X(1 )+B*X(2)+C*X(3)+D*X( 1 )*X(2) + E*X< 1 )*X(3) + 6 T c F*X(2)*X(3)+G / 8 c INPUTS: A ( 3 ) , B ( 3 ) , C ( 3 ) , D ( 3 ) , E ( 3 ) , F ( 3 ) . G ( 3 ) - CONSTANT COEFFICIENT 9 c VECTORS . 10 c TOL- MAXIMUM OESIRED ERROR. 11 c NUM- NUMBER OF ITERATIONS ALLOWED. 12 c XO- INITIAL GUESS FOR THE SOLUTION. 13 c 14 c OUTPUTS: X- SOLUTION VECTOR. 15 c IFLAG- 0-OK. 1-NO SOLUTION. 16 c 17 c SUBROUTINES CALLED: S0L3X3 18 c VLEN 19 c 20 21 c 22 REAL A ( 3 ) , B ( 3 ) , C ( 3 ) , 0 ( 3 ) , E ( 3 ) . X 0 ( 3 ) , X ( 3 ) . F X ( 3 ) , J A C ( 3 , 3 ) , Y ( 3 ) 23 REAL F( 3 ) , G ( 3 ) 24 c 25 IFLAG=0 26 c 27 INITIALIZE THE FIRST GUESS FOR X ( I ) . 28 c 29 DO 1 1=1,3 - 30 X(I)=XO(I) 31 1 CONTINUE 32 c 33 c***»* ITERATE A MAXIMUM OF 'NUM' TIMES. 34 c 35 DO 150 IC0UNT=1,NUM 36 c 37 CALCULATE CURRENT VALUE OF FUNCTION 'FX'. 38 c 39 DO 10 1=1,3 40 F X ( I ) = A ( I ) * X ( 1 ) + B ( I )*X(2)+C(I )*X(3)+D(I )*X(1)*X(2) + 41 * E ( I ) * X ( 1 )*X(3) + F ( I )*X(2)*X(3)+G(I ) 42 10 CONTINUE 43 c 44 c»**** CALCULATE THE JACOBIAN MATRIX. 45 c 46 DO 11 1=1.3 47 J A C ( I , 1 ) = A ( I ) + D ( I ) * X ( 2 ) + E ( I ) * X ( 3 ) 48 J A C ( I , 2 ) = B ( I ) + D ( I ) * X ( 1 ) + F ( I ) * X ( 3 ) 49 JAC(I,3)=C(I ) + E ( I )»X( 1 ) + F ( I )*X(2) 50 11 CONTINUE 51 c 52 c***»« SOLVE FOR DIFFERENCE VECTOR 'Y'. 53 c 54 CALL S0L3X3(JAC,FX,Y,ISOLN) 55 c 56 IF(ISOLN.EQ.1) GO TO 950 57 c 58 c****» X(K)=X(K-1 )+Y(K-1 ) - 315 - s t i n g of NWTVOL.FOR a t 14:46:51 on AUG 9, 1984 f o r CC1d=DAC0 Page 59 C 60 DO 12 1=1,3 61 X ( I ) = X ( I ) + Y ( I ) 62 12 CONTINUE 63 C 64 c***»* TEST SIZE OF ERROR. 65 c 66 CALL VLEN(Y.YLEN) 67 IF(YLEN.LT.TOL) GO TO 900 68 c 69 TEST IF SOLUTION IS DIVERGING 70 c 71 CALL VLEN(X.XLEN) 72 IF(XLEN.GT.1000.) GO TO 950 73 c 74 150 CONTINUE 75 C 76 950 CONTINUE 77 C 78 IFLAG=1 79 C 80 900 CONTINUE 81 C 82 RETURN 83 END - 316 - L i s t i n g of ONRAY.FOR a t 14:46:51 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE ONRAY(UR,PINT,V.ION) 2 C 3 C THIS SUBROUTINE DETERMINES IF A POINT ' V IS ON RAY DEFINED BY 4 C POINT 'PINT' AND DIRECTION VECTOR 'UR'. 5 C 6 C INPUT ARGUMENTS: 7 C UR(3)- DIRECTION VECTOR OF RAY 8 C PINT(3)- ORIGIN OF RAY 9 C V ( 3 ) - POINT TO BE TESTED 10 C 11 C OUTPUT ARGUMENTS: 12 C ION- 0=NOT ON RAY 13 C 1=0N RAY 14 C 15 C POINT ' V IS ON THE RAY IF: 16 C 17 C TLAMB*UR+PINT-V=0 18 C 19 C » » * • * * * « * • * * * » * * • * « » « * * * * * * * * * « * * * * * * * * * * * * * * * * * * * * * * * * * * * * » . * * * « * 20 C 21 REAL UR(3),PINT(3),V(3),TLAMB(3) 22 C 23 I0N=O 24 C 25 K=0 26 C 27 c***** TEST EACH ONE OF THE THREE EQUATIONS. 28 C 29 DO 10 1=1,3 30 IF(I.EQ.3) J=1 31 C 32 C***** IF UR(I)=0. TLAMB IS ARBITRARY. 33 C 34 IF(UR(I).EQ.O.) GO TO 100 35 K=K+1 36 IF(K.GT.2) GO TO 20O 37 C 38 C****» CALCULATE TLAMB FOR EQUATION I. 39 C 40 TLAMB(K)=(V(I)-PINT(I))/UR(I) 41 IF(K.EQ.1) GO TO 90 42 C 43 C**»** TEST IF VALUE AGREES WITH OTHER CALCULATED VALUE. 44 c***»* IF SO, 'V LIES ON THE RAY. 45 C 46 IF(ABS(TLAMB(K)-TLAMB(K-1)).GT..00001) GO TO 400 47 90 CONTINUE 48. GO TO 10 49 C 50 100 CONTINUE 51 C 52 c***»* TEST IF V ( I ) - P I N T ( I ) = 0 53 C 54 I F ( V ( I ).NE.PINT(I)) GO TO 400 55 C 56 10 CONTINUE 57 C 58 200 CONTINUE - 317 - L i s t i n g of ONRAY.FOR a t 14:46:51 on AUG 9. 1984 f o r CC1d=DAC0 Page 2 59 C 60 £»**** TEST RANGE OF TLAMB 61 c 62 IF(TLAMB(1).GT.O.) I0N=1 63 GO TO 400 64 c 65 40O CONTINUE 66 C 67 RETURN 68 END - 318 - L i s t i n g o f PLANE.FOR a t 1 4 : 4 7 : 2 1 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 o r* SUBROUTINE PLANE(NFACE,P1,P2. INT,P INT) 3 \j c THIS SUBROUTINE TESTS FOR INTERSECTION OF A LINE SEGMENT 4 c WITH A PLANE 5 c 6 c INPUT ARGUMENTS: 7 c NFACE- INDEX LOCATING FACE IN FACE L IST. 8 c P 1 ( 3 ) . P 2 ( 3 ) - END POINTS OF LINE SEGMENT. 9 c 10 c OUTPUT ARGUMENTS: 1 1 c INT- 0=NO INTERSECTION OF PLANE. 12 c 1=PLANE INTERSECTED BY LINE SEGMENT. 13 c P I N T ( 3 ) - INTERSECTION POINT. 14 c 15 c COMMON BLOCKS ACCESSED: 16 c /DATA/VERT,FACES, IFACE, IOBd 17 c 18 c SUBROUTINES CALLED: DOTPRD 19 c 20 c EQUATION OF PLANE: R(3 ) » U ( 3 ) - P = 0 21 c EQUATION OF L INE: RLAMB*U1(3)+P1=R(3) 22 c 23 24 c 25 REAL P 1 ( 3 ) , P 2 ( 3 ) , P I N T ( 3 ) , U ( 3 ) . U 1 ( 3 ) 26 c 27 COMMON / D A T A / V E R T ( 1 0 0 0 , 3 ) , F A C E S ( 1 0 0 . 4 ) , I FACE (100 .2 ) , IOBd (10 .2 ) 28 c 29 c 30 INT=0 31 c 32 c***** FIND THE FACE PLANE SURFACE NORMAL ' U ' AND DISTANCE FROM THE 33 c**»** ORIGIN ' P ' . 34 c 35 DO 10 1=1.3 36 U(I ) =FACES(NFACE. I ) 37 10 CONTINUE 38 P=FACES(NFACE,4) 39 c 40 FIND THE DIRECTION VECTOR ' U 1 ' OF THE LINE SEGMENT. 41 c 42 DO 15 1=1.3 43 U 1 ( I ) = P 2 ( I ) - P 1 ( I ) 44 15 CONTINUE 45 c 46 c****» SUBSTITUTE THE LINE ENDPOINTS INTO THE EQUATION OF THE PLANE. 47 c***»* IF THE RESULTS ARE OPPOSITE IN SIGN. THE POINTS ARE ON OPPOSITE 48 c » * * * * SIDES OF THE PLANE AND THE LINE SEGMENT INTERSECTS THE PLANE. 49 c 50 CALL D0TPRD(P1.U.P1U) 51 CALL D0TPR0(P2.U,P2U) 52 c 53 Z= (P1U-P ) * (P2U-P ) 54 c 55 I F ( Z . G E . O ) GO TO 800 56 c 57 FIND THE VALUE OF THE PARAMETER 'RLAMB' . 58 c - 319 - L i s t i n g of PLANE.FOR at 14:47:21 on AUG 9. 1984 f o r CC1d=DAC0 Page 2 59 RLAMB«(P-P1U)/(P2U-P1U) 60 C 61 c*»**» FIND THE INTERSECTION POINT 'PINT' 62 c 63 DO 20 1=1,3 64 PINT(I)=P1(I)+RLAM8*U1(I) 65 20 CONTINUE 66 C 67 INT=1 68 C 69 800 CONTINUE 70 RETURN 71 END - 320 - : l n g o f ROTK. FOR at 14:47:21 on AUG 9, 1984 f o r CCid=DAC0 Page 1 1 SUBROUTINE ROTK(K,THETA.ROT) 2 C 3 C THIS SUBROUTINE CALCULATES THE ROTATION MATRIX FOR A 4 C ROTATION ABOUT A VECTOR 'K' BY AN ANGLE OF 'THETA'. 5 C 6 C INPUT ARGUMENTS: K ( 3 ) - ROTATION AXIS VECTOR 7 C THETA- ROTATION ANGLE (RAD) 8 C 9 C OUTPUT ARGUMENTS: R0T(4,4)- RESULTING HOMOGENEOUS ROTATION MATRIX 10 c 1 1 c»*«»,**»*«»*«**«*,*«**.**»***««*»*««.«**«««».***««****.«*«*»**»***«».**« 12 c 13 REAL R0T(4,4),K(3) 14 c 15 c 16 RCOS=COS(THETA) 17 RSIN=SIN(THETA) 18 VERS=1.-RCOS 19 c 20 XX=K(1 )*K(1)*VERS 21 YY=K(2)*K(2)*VERS 22 ZZ=K(3)*K(3)*VERS 23 XY=K( 1 )*K(2)*VERS 24 XZ=K( 1 )*K(3)*VERS 25 YZ=K(2)*K(3)*VERS 26 c 27 ROT(1, 1 ) = XX + RCOS 23 ROT( 1 ,2)=XY-K(3)*RSIN 29 ROT(1,3)=XZ+K(2)*RSIN 30 R0T(2,1)=XY+K(3)*RSIN 31 R0T(2,2)=YY+RC0S i 32 R0T(2,3)=YZ-K(1)*RSIN 33 ROTO, 1 )=XZ-K(2)*RSIN 34 R0T(3,2)=YZ+K(1)*RSIN 35 ROTO, 3)=ZZ+RC0S 36 R0T(4,4)=1. 37 c 38 RETURN 39 END - 321 - L i s t i n g of S0L3X3.F0R at 14:47:21 on AUG 9. 1984 f o r CCid=DACO Page 1 1 SUBROUTINE S0L3X3(RMAT,CONST,Y,I SOL) 2 C 3 C THIS SUBROUTINE FINDS THE SOLUTION OF A SET OF LINEAR EQUATIONS OF 4 C THE FOLLOWING FORM: 5 C RMAT(3,3)*Y(3)+CONST(3)=0. 6 C 7 C INPUT ARGUMENTS: a C RMAT(3,3)- MATRIX OF COEFFICIENTS. 9 c C0NST(3)- VECTOR OF CONSTANTS. 10 c 11 c OUTPUT ARGUMENTS: 12 c Y ( 3 ) - SOLUTION TO THE SYSTEM OF EQUATIONS. 13 c ISOL- 0=SOLUTION EXISTS. 14 c 1=N0 SOLUTION EXISTS. 15 c 16 c SUBROUTINES CALLED: MATDET 17 c 18 C*****»**********************»***********************" 19 c 20 REAL RMAT(3,3) .CONST (3) . Y(3) , TEMPO, 3) 21 c 22 IS0L=O 23 c 24 FIND THE DETERMINANT OF THE MATRIX 'RMAT' AND CHECK FOR SINGULARITY 25 c 26 CALL MATDET(RMAT,DET ) 27 c 28 IF(ABS(DET) .LT. .O O O 0 1) GO TO 100 29 c 30 c * * * * * SOLVE FOR 'Y ' . 31 c 32 DO 10 1=1.3 33 DO 20 J=1.3 34 c 35 K1=I+1 36 K2=I+2 37 IF(K1.GE.4) K1=K1-3 38 IF(K2.GE.4) K2=K2-3 39 c 40 TEMP(J,I)=-CONST(J) 41 TEMP(J,K1)=RMAT(J,K1) 42 TEMP(d,K2)=RMAT(J.K2) 43 c 44 20 CONTINUE 45 c 46 CALL MATDET(TEMP,DTEMP) 47 Y(I)=DTEMP/DET 48 c 49 10 CONTINUE 50 c 51 GO TO 200 52 c 53 100 CONTINUE 54 IS0L=1 55 c 56 200 CONTINUE 57 c 58 RETURN - 322 - o L i s t i n g o f S0L3X3.F0R a t 1 4 : 4 7 : 2 1 on AUG 9 , 1984 f o r CC1d=DAC0 Page 59 END - 323 - L i s t i n g o f SOLVE.FOR a t 14:47:22 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE S0LVE(P1,01,U1,U2,U,DIST,LAMB 1,LAMB2) 2 C 3 C THIS SUBROUTINE SOLVES FOR THE PARAMETERS LAMB 1,LAMB2 4 C DEFINING THE COMMON NORMAL POINTS ON TWO LINES. 5 C LINE 1: R(3) = P1(3) + LAMB1*U1(3) 6 C LINE2: R(3)=Q1(3)+LAMB2*U2(3) 7 C 8 £******«*»*******»*»**»******** 9 c 10 REAL P1(3),02(3),U1(3),U2(3),U(3),PINT(3),M1(2),M2(2),M3(2) 1 1 REAL C(3 ) , 0 P ( 3 ) 12 REAL LAMB 1,LAMB2 13 c 14 IC0UNT=O 15 c 16 c***»* FIND TWO EQUATIONS WHICH GIVE A NON-ZERO DETERMINANT FOR 17 THE DENOMINATOR 18 c 19 DO 20 1=1.3 20 d = I + 1 21 K = I+2 22 I F ( I . E 0 . 3 ) d=1 23 IF(I.GT.1) K=K-3 24 c 25 M1(1)=U1(I) 26 M1(2)=U1(J) 27 M2(1)=U2(I) 28 M2(2)=U2(d) 29 c 30 CALL DET2X2(M1.M2.DET) 31 c 32 ISAVE = I , . 33 JSAVE=d 34 KSAVE=K 35 c 36 c 37 IF(DET.NE.0.) GO TO 100 38 20 CONTINUE 39 c 40 100 CONTINUE 41 c 42 IC0UNT=IC0UNT+1 43 IF(ICOUNT.GT.2) GO TO 800 44 c • 45 c.**»»* -CALCULATE THE VALUE OF THE RIGHT HAND SIDE OF THE EQUATION 46 c 47 CALL DIFF(Q1,P1,QP) 48 DO 10 1=1.3 49 C( I ) = Q P ( I ) - D I S T * U ( I ) 50 10 CONTINUE 51 c 52 M3(1 )=C(ISAVE) 53 M3(2)=C(JSAVE) 54 c 55 Q* * * * * SOLVE FOR THE UNKNOWNS LAMB 1,LAMB2 56 c 57 CALL DET2X2(M3.M2.TEMPI) 58 CALL DET2X2(M1.M3.TEMP2) - 324 - L i s t i n g of SOLVE.FOR a t 14:47:22 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C 60 LAMB1=TEMP1/DET 61 LAMB2=-TEMP2/DET 62 C 63 C***»* TEST IF THIS IS A VALID SOLUTION 64 C 65 TEST=LAMB1*U1(KSAVE)-LAMB2*U2(KSAVE) 66 IF(ABS(TEST-C(K)).LT..00001) GO TO 900 67 C 68 C***»* IF NOT. CHANGE THE SIGN OF THE VECTOR 'U' AND SOLVE AGAIN 69 C 70 DIST=-DIST 71 GO TO 100 72 C 73 800 CONTINUE 74 WRITE(5,*) 'ERROR - NO SOLUTION WAS FOUND IN SUBROUTINE SOLVE' 75 C 76 900 CONTINUE 77 C 78 RETURN 79 END - 325 - L i s t i n g o f SURFIN.FOR a t 14:47:40 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE SURFIN(P1.P2.P3,P4,E1,E2.INT) 2 C 3 C THIS SUBROUTINE TESTS FOR INTERSECTION BETWEEN A STRAIGHT EDGE 4 C DEFINED BY ENDPOINTS E1.E2, AND A PARAMETRIC SURFACE DEFINED 5 C BY STRAIGHT BOUNDARIES AND CORNER POINTS P1.P2.P3.P4'. 6 C 7 C INPUT ARGUMENTS: 8 C P1 ( 3 ) . P 2 ( 3 ) . P 3 ( 3 ) . P 4 ( 3 ) - CORNER POINTS OF A PARAMETRIC 9 C RULED SURFACE. 10 C E 1 ( 3 ) , E 2 ( 3 ) - ENDPOINTS OF A STRAIGHT EDGE. 1 1 C 12 C OUTPUT ARGUMENTS: 13 C INT- 0=NO INTERSECTION 14 C ^INTERSECTION 15 C 1S C SUBROUTINES CALLED: NEWTON 17 C 18 C 'IGL' GRAPHICS SUBROUTINES: CMCLOS 19 C CMOPEN 20 C DRAW3D 21 C M0VE3D 22 C 23 C 24 C EQUATION OF THE SURFACE: 25 C R(3)=P1(3)+RLAMB1*U0(3)+RLAMB2*V0(3)+RLAMB1«RLAMB2»(U1(3)-U0(3)) 26 C 27 C EQUATION OF THE EDGE: R(3) = E1(3 )+RLAMB3*UE(3) 28 C 29 C COMBINING WE GET: 30 C 31 C RLAMB1*UO(3)+RLAMB2*VO(3)-RLAMB3*UE(3)+RLAMB1*RLAMB2*(U1(3)-UO(3)+ 32 C P1(3)-E1(3)=0 33 C 34 C THIS IS A NON-LINEAR SYSTEM OF EQUATIONS WHICH WE WILL SOLVE USING 35 C NEWTONS METHOD. 36 C 37 ^****#***************************** 38 C 39 REAL UO(3),VO(3).U1(3),UE(3).RLAMB(3) 40 REAL T 1 ( 3 ) . T 2 ( 3 ) . T 3 ( 3 ) . T 4 ( 3 ) . X 0 ( 3 ) 41 REAL P 1 ( 3 ) , P 2 ( 3 ) . P 3 ( 3 ) . P 4 ( 3 ) . V 1 ( 3 ) . E 1 ( 3 ) , E 2 ( 3 ) 42 REAL DELT(3),CONST(3) 43 C 44 DATA XO/3*.5/ 45 C 46 C 47 INT=0 48 C 49 C****» DEFINE THE PARAMETRIC SURFACE BOUNDARY VECTORS 'VO','V1','UO','U1'. 50 C 51 DO 10 1=1,3 52 U 1 ( I ) = P 3 ( I ) - P 4 ( I ) 53 V 1 ( I ) = P 3 ( I ) - P 2 ( I ) 54 U 0 ( I ) = P 2 ( I ) - P 1 ( I ) 55 V 0 ( I ) = P 4 ( I ) - P 1 ( I ) 56 U E ( I ) = E 1 ( I ) - E 2 ( I ) 57 DELT ( I ) = U 1 ( I ) - U 0 ( I ) 58 C 0 N S T ( I ) = P 1 ( I ) - E 1 ( I ) - 3 2 6 - L i s t i n g of SURFIN.FOR a t 14:47:40 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 10 CONTINUE 60 C 61 c****» SOLVE FOR THE PARAMETER VALUES AT THE INTERSECTION POINT USING 62 c»**»* NEWTONS METHOD FOR A SYSTEM OF NON-LINEAR EQUATIONS. THE MAXIMUM 63 c***»* ERROR IS SET TO .01 AND THE MAXIMUM NUMBER OF ITERATIONS IS 10. 64 c 65 CALL NEWT0N(UO.VO.UE.DELT.CONST, .01,10,XO,RLAMB,I FLAG) 66 IF(IFLAG.EQ.1) GO TO 900 67 c 68 c 69 c*»**» TEST THE VALUES OF THE PARAMETERS TO DETERMINE IF THE EDGE 70 c****» INTERSECTS THE SURFACE WITHIN ITS BOUNDARIES. 71 c 72 DO 35 1=1,3 73 IF(RLAMB(I).LT.(-0.001).OR.RLAMB(I).GT.(1.001)) GO TO 900 74 35 CONTINUE 75 C 76 c** * * * IF THERE IS AN INTERSECTION, INDICATE THE INTERSECTION POINT 77 c***»* GRAPHICALLY BY DRAWING TWO CROSSED LINES ON THE SURFACE AT THAT POINT 78 c 79 DO 60 1=1.3 80 T1(I)=P1(I)+RLAMB(1)*U0(I) 81 T2(I)=P1(I)+V0(I)+RLAMB(1)*U1(I) 82 T3(I)=P1(I)+RLAMB(2)*V0(I) 83 T4(I)=P1(I)+U0(I)+RLAMB(2)*V1(I) 84 60 CONTINUE 85 C 86 CALL CMOPEN 87 C 88 CALL M0VE3D(T1(1),T1(2),T1(3)) 89 CALL DRAW3D(T2(1),T2(2),T2(3)) 90 CALL M0VE30(T3(1 ) ,T3( 2 ) , T 3 ( 3 ) ) 91 CALL DRAW3D(T4(1),T4(2).T4(3)) 92 C 93 CALL CMCLOS 94 C 95 INT=1 96 c 97 900 CONTINUE 98 RETURN 99 END 0 - 327 - i i L i s t i n g of SWPVOL.FOR a t 14:47:40 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 r> SUBROUTINE SWPVOL at 3 \* c T H I S SUBROUT INE D E F I N E S THE VOLUME SWEPT OUT BY A MOVING 4 c C Y L I N D E R . 5 c 6 •7 c r* D E F I N I T I O N OF CONSTANTS AND V A R I A B L E S : f 8 L. c A 1 , A 2 , B 1 , B 2 - C Y L I N D E R S P I N E ENDPOINTS 9 c H 1 . H 2 - HOMOGENEOUS COORDINATE TRANSFORMATIONS S P E C I F Y I N G THE I N I T I A L 10 c AND F I N A L P O S I T I O N OF THE C Y L I N D E R . 1 1 c L E N , R A D I U S - LENGTH AND R A D I U S . O F THE C Y L I N D E R . 12 c P , U O , U 1 , V O . V 1 - C O E F F I C I E N T S OF PARAMETR IC SURFACE GENERATED BY THE 13 c S P I N E . 14 c P 1 . P 2 . . . . P 8 - CORNER P O I N T S OF THE SWEPT VOLUME. 15 c 16 c SUBROUT INES C A L L E D : 17 c 18 c CORNER 19 c DRVOL 2 0 c GENVOL 21 c I N I T S V 22 c L I N K 23 c 24 2 5 c 26 c 27 REAL L E N , R A D I U S 28 REAL H W 0 R L ( 4 , 4 ) 29 BYTE F I L N A M ( 2 0 ) , 0 B J L S T ( 2 O ) 3 0 c 31 COMMON / L I N K / H 1 ( 4 , 4 ) , H 2 ( 4 , 4 ) , L E N 32 COMMON / O B J L S T / O B J L S T 3 3 COMMON / D A T A / V E R T ( 1 0 0 0 , 3 ) , F A C E S ( 1 0 0 , 4 ) , I FACE ( 1 0 0 . 2 ) , I O B J ( 1 0 , 2 ) 34 COMMON / V O L U M E / P O ) , U 0 ( 3 ) , U 1 ( 3 ) , V0 ( 3 ) , V1 ( 3 ) 3 5 COMMON / S N O R M / D A N ( 3 ) , D B N ( 3 ) , D C N ( 3 ) 36 COMMON / C O R N E R / P 1 ( 3 ) , P 2 ( 3 ) , P 3 ( 3 ) . P 4 ( 3 ) , P 5 ( 3 ) . P 6 ( 3 ) , P 7 ( 3 ) . P 8 ( 3 ) 37 COMMON / S P I N E S / A 1 ( 3 ) , B 1 ( 3 ) . A 2 ( 3 ) , B 2 ( 3 ) 38 COMMON / V I E W / E Y E P O S O ) , V R P ( 3 ) 3 9 COMMON / R A D I U S / R A D I U S 4 0 c 41 DATA E Y E P O S / 1 . , 1 . , 1 . / 4 2 DATA V R P / O . . 0 . , 0 . / 4 3 DATA O B J L S T / ' 0 ' . ' B ' . ' J ' . ' L ' . ' S ' . ' T ' . ' . ' . ' D ' . ' A ' . ' T ' , 1 0 * 0 / 44 DATA F I L N A M / ' F ' . ' I ' . ' L ' . ' N ' . ' A ' . ' M ' . ' . ' . ' D ' . ' A ' . ' T ' , 1 0 * 0 / 45 DATA H W 0 R L / 1 6 * 0 . / 4 6 c 4 7 HWORL (1 , 1 ) = 1 . 4 8 H W 0 R L ( 2 , 2 ) = 1 . 4 9 H W 0 R L ( 3 , 3 ) = 1 . 5 0 H W 0 R L ( 4 , 4 ) = 1 . 51 c 52 c****» F I N D THE SWEPT VOLUME PARAMETERS 5 3 c 54 C A L L I N I T S V ( F I L N A M ) 5 5 c 56 c * * » * * GET THE L I N K PARAMETERS AND S P I N E ENDPO INTS . 57 c 58 C A L L L I N K ( A 1 , B 1 , A 2 . B 2 ) - 328 - L i s t i n g of SWPVOL.FOR a t 14:47:40 on AUG 9, 1984 f o r CC1d=DAC0 Page 2 59 C 60 C****» FIND THE PARAMETRIC SURFACE GENERATED BY THE CYLINDER SPINE. 61 C 62 CALL GENV0L(A1.B1,A2,B2) 63 C 64 C****» FIND THE CORNER POINTS OF THE PARAMETRIC SWEPT VOLUME. 65 C 66 CALL CORNER 67 C 68 C****» DRAW THE SWEPT VOLUME. 69 C 70 CALL DRVOL 71 C 72 RETURN 73 END - 329 - L i s t i n g of TCROSS.FOR a t 14:47:41 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE TCROSS(IV1 , IV2 , IV3,UR,PINT,INT) 2 C 3 C THIS SUBROUTINE DETERMINES IF A RAY CONTAINING AT LEAST ONE 4 C FACE VERTEX CROSSES THE FACE BOUNDARY AT THAT VERTEX. THIS IS 5 C DONE BY DETERMINING IF THE ADJACENT VERTICES ON THE FACE LIE ON 6 C OPPOSITE SIDES OF THE LINE CONTAINING THE RAY. 7 C 8 C INPUT ARGUMENTS: 9 C IV1.IV3- ADJACENT VERTEX INDEXES. 10 C IV2- INDEX OF VERTEX CROSSED BY THE RAY. 1 1 C UR- DIRECTION VECTOR OF THE RAY. 12 C PINT- ORIGIN OF THE RAY. 13 C 14 C OUTPUT ARGUMENTS: 15 C INT- 0=NO FACE BOUNDARY CROSSED 16 c 1=FACE BOUNDARY CROSSEO 17 c 18 c COMMON BLOCK ACCESSED: 19 c /DATA/VERT(1000,3).FACESC100,4).I FACE(100,2),IOBJ(10.2) 20 c 21 c SUBROUTINES CALLED: DOTPRD 22 c XPROD 23 24 c 25 c 26 REAL PROD 1(3).PROD2(3) 27 REAL UR(3) .PINTO) ,U1(3) ,U2(3) 28 c 29 COMMON /DATA/VERT(1000.3),FACES(100.4),IFACE(100.2),IOBJ(10,2) 30 c 31 INT=0 32 c 33 FIND THE DIRECTION VECTORS 'U1','U2' OF THE ADJACENT EDGES. 34 c 35 DO 10 1=1,3 36 U1(I)=VERT(IV1,I)-VERT(IV2,I) 37 U2(I)=VERT(IV3,I )-VERT(IV2,I ) 38 10 CONTINUE 39 c 40 c***** FIND THE CROSS PRODUCTS OF THE ADJACENT EDGES AND THE RAY. 41 c 42 CALL XPR0D(U1,UR,PR0D1 ) 43 CALL XPR0D(U2,UR,PR0D2) 44 c 45 c**«** TEST IF THE EDGES ARE ON OPPOSITE SIDES OF THE RAY. 46 c 47 CALL DOTPRD(PROD 1,PR0D2,DOT) 48 IF(D0T.LT.O.) INT-1 49 c 50 RETURN 51 END - 330 - L i s t i n g o f TRANS3.FOR a t 14:48:02 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE TRANS3(H.P1.P2) 2 C 3 C THIS SUBROUTINE PERFORMS THE FOLLOWING HOMOGENEOUS 4 C TRANSFORMATION. 5 C 6 C P2(3)=H(4,4)*P1(3) 7 C 8 C WHERE P1.P2 ARE 3X1 VECTORS AND H IS A 4X4 HOMOGENEOUS 9 C COORDINATE TRANSFORMATION MATRIX. 10 C 1 1 12 C 13 REAL H(4.4),P1(3),P2(3) 14 C 15 DO 100 1=1,3 16 P2(I)=H(I,1)*P1(1)+H(I,2)*P1(2)+H(I, , 3)*P1(3)+H(1,4) 17 100 CONTINUE 18 C 19 RETURN 20 END - 331 - L i s t i n g of UNIT.FOR a t 14:48:02 on AUG 9. 1984 f o r CC1d=0AC0 Page 1 1 SUBROUTINE UNIT(VECT,UVECT ) 2 C 3 C THIS SUBROUTINE NORMALIZES A VECTOR. 4 C 5 C INPUTS: VECT(3)- A VECTOR 6 C 7 C OUTPUTS: UVECT(3)- VECT NORMALIZED. 8 C g Q»m** ***************************************** ***********<•****************** *** 10 c 11 REAL VECT(3),UVECT(3),LENGTH 12 C 13 LENGTH=5QRT(VECT(1)**2+VECT(2)**2+VECT(3)**2) 14 C 15 UVECT(1)=VECT(1)/LENGTH 16 UVECT(2)=VECT(2)/LENGTH 17 UVECT(3)=VECT(3)/LENGTH 18 C 19 RETURN 20 END - 332 - L i s t i n g o f VCROSS.FOR a t 14:48:28 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE VCROSS(NVERT,ICOUNT,UR,PINT,I LI ST,NVCRSS) 2 C 3 C THIS SUBROUTINE FINDS THE NUMBER OF TIMES A RAY CROSSES 4 C THE POLYGONAL FACE BOUNDARY AT VERTICES. 5 C 6 C INPUT ARGUMENTS: 7 C NVERT- INDEX OF FIRST FACE VERTEX. 8 C ICOUNT- NUMBER OF VERTICES ON FACE. 9 C UR- DIRECTION VECTOR OF THE RAY. 10 c PINT- ORIGIN OF RAY. 11 c 12 c OUTPUT ARGUMENTS: 13 c ILIST- LIST OF VERTICES ON RAY. 14 c NVCRSS- NUMBER OF TIMES THE RAY CROSSES FACE BOUNDARIES AT 15 c VERTICES. 16 c 17 c COMMON BLOCKS ACCESSED:/DATA/ 18 c 19 c SUBROUTINES CALLED: ONRAY 20 c TCROSS 21 c 22 23 c 24 REAL PINT(3),UR(3),V(3) 25 INTEGER ILIST(10).NVCRSS 26 c 27 COMMON /DATA/VERT(1000.3),FACES(100,4),I FACE(100.2),IOBJ( 10,2) 28 c 29 NVCRSS=0 30 NUM=0 31 c 32 c****» TEST ALL VERTICES ON THE FACE. 33 c 34 DO 10 1=1,ICOUNT 35 c 36 FIND THE NEXT VERTEX TO BE TESTED. 37 c 38 DO 15 J=1,3 39 V(J)=VERT(NVERT+I-1,J) 40 15 CONTINUE 41 c 42 Q * * * * * TEST IF THE VERTEX IS ON THE RAY. 43 c 44 CALL ONRAY(UR.PINT,V.ION) 45 c 46 IF(ION.EQ.O) GO TO 100 47 c 48 c***»* UPDATE 'NUM' AND 'ILIST'. 49 c 50 NUM=NUM+1 51 ILIST(NUM)=NVERT+I-1 52 c 53 10O CONTINUE 54 10 CONTINUE 55 c 56 c 57 c*»«** TEST ALL VERTICES LYING ON RAY FOR FACE BOUNDARY CROSSINGS. 58 ADJACENT VERTICES ARE THE CLOSEST VERTICES TO EITHER SIDE OF - 333 - L i s t i n g of VCROSS.FOR a t 14:48:28 on AUG 9, 1984 f o r CCld=DACO Page 59 C*»*** THE CURRENT VERTEX WHICH DO NOT LIE ON THE RAY. 60 C 61 * DO 20 1=1,NUM 62 C 69 C****« FINO CURRENT VERTEX. 64 C 65 M=ILIST(I) 66 C 67 c***»* FIND ADJACENT VERTICES. 68 C 69 MV2=M+1 70 MV1=M-1 71 IF(1+1.GT.NUM) GO TO 600 72 GO TO 610 73 C 74 c****» LAST VERTEX IN LIST. 75 C 76 600 CONTINUE 77 IF(M.E0.NVERT+IC0UNT-1) GO TO 620 78 GO TO 310 79 C 80 620 MV2=NVERT 81 IF(ILIST(1).EO.NVERT) GO TO 500 82 C 83 610 CONTINUE 84 IF(ILIST(1+1 ).NE.M+1) GO TO 310 85 C 86 MV2=M+2 87 GO TO 350 88 C 89 310 CONTINUE 90 IF( I LI ST(1-1).NE.M-1 ) GO TO 350 91 C 92 MV1=M-2 93 C 94 3E0 CONTINUE 95 C 96 IF(MV2.GT.NVERT+IC0UNT-1) GO TO 500 97 IF(MV1.LT.NVERT) GO TO 500 98 C 99 c****» TEST FOR CROSSING AT VERTEX. 100 C 101 CALL TCR0SS(MV1,M,MV2.UR.PINT,INT) 102 IFUNT.EQ.1) NVCRSS = NVCRSS+1 103 C 104 C 105 500 CONTINUE 106 20 CONTINUE 107 C 108 IF(ILIST(1).NE.NVERT) GO TO 510 109 IF(ILIST(2).NE.NVERT+1) GO TO 511 1 10 C 111 MV2=NVERT+2 112 MV1=NVERT+IC0UNT-1 113 GO TO 550 1 14 C 115 511 CONTINUE 116 IF(ILIST.(NUM) . NE . NVERT + ICOUNT-1 ) GO TO 512 - 334 - L i s t i n g of VCROSS.FOR a t 14:48:28 on AUG 9, 1984 f o r CCid=DACO Page 3 1 17 C 1 18 MV2=NVERT+1 119 MV1=NVERT+IC0UNT-2 120 GO TO 550 121 C 122 512 CONTINUE 123 C 124 MV2=NVERT+1 125 MV1=NVERT+IC0UNT-1 12G C 127 550 CONTINUE 128 C 129 c***«* TEST FOR CROSSING AT VERTEX. 130 c 131 CALL TCR0SS(MV1,M,MV2,UR,PINT,INT) 132 IF(INT.EQ.I) NVCRSS=NVCRSS+1 133 c 134 510 CONTINUE 135 C 136 RETURN 137 END - 335 - L i s t i n g of VLEN.FOR a t 14:48:29 on AUG 9, 1984 f o r CCid=DACO Page 1 1 SUBROUTINE VLEN(VECTOR,LENGTH) 2 C 3 C THIS SUBROUTINE FINDS THE SCALAR LENGTH OF A VECTOR. 4 C 5 REAL LENGTH,VECTOR(3) 6 C 7 LENGTH=SORT(VECTOR(1)**2+VECTOR(2)**2+VECT0R(3)**2) 8 C 9 RETURN 10 END - 336 - L i s t i n g of VRTCRS.FOR a t 14:48:29 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE VRTCRS(UR,P1,P2,INT) j 2 C 3 C THIS SUBROUTINE DETERMINES IF A RAY CONTAINING AT LEAST ONE 4 C FACE VERTEX CROSSES THE FACE BOUNDARY AT THAT VERTEX. THIS IS 5 C DONE BY DETERMINING IF THE ADJACENT VERTICES ON THE FACE LIE ON 6 C OPPOSITE SIDES OF THE LINE CONTAINING THE RAY. 7 C 8 C UR(3)- RAY DIRECTION VECTOR. 9 C P 1 ( 3 ) . P 2 ( 3 ) - TWO ADJACENT VERTICES. 10 C 1 1 12 C 13 REAL UR(3),P1(3),P2(3),U1(3),U2(3),PR0D1(3),PR0D2(3) 14 C 15 INT=0 16 C 17 DO 10 1=1,3 18 U 1 ( I ) = P 1 ( I ) - U R ( I ) 19 U2(I)=P2(I )-UR(I) 20 1C CONTINUE 21 C 22 CALL XPR0D2(U1 ,UR,PRODI) 23 CALL XPR0D2(U2,UR,PR0D2) 24 C 25 CALL D0TPRD(PR0D1,PR002.RESULT) 26 C 27 IF(RESULT.LT.O.) INT=1 28 C 29 RETURN 30 END - 337 - L i s t i n g of XPR0D2.FOR a t 14:50:37 on AUG 9, 1984 f o r CC1d=DAC0 Page 1 1 SUBROUTINE XPR0D2(A,B,C ) 2 C 3 C THIS SUBROUTINE CALCULATES VECTOR C = A X B (CROSS PRODUCT). 4 C 5 C INPUTS: 6 C A(3)=FIRST VECTOR 7 C B(3)=SEC0ND VECTOR 8 C 9 C OUTPUT: 10 C C(3)=CR0SS PRODUCT, A X B 1 1 C 12 [********************************************* 13 c 14 c 15 REAL A(3) , B ( 3 ) . C(3) . ABS 16 c 17 C( 1 )=A(2)*B(3)-A(3)*B(2) 18 C ( 2 ) = A ( 3 ) * B ( 1 ) - A ( 1 ) * B ( 3 ) 19 C(3)=A( 1 )*B(2)-A(2)*B( 1 ) 20 c 21 RETURN 22 END

Cite

Citation Scheme:

    

Usage Statistics

Country Views Downloads
United States 8 0
Japan 3 0
France 1 0
Ukraine 1 0
City Views Downloads
Ashburn 4 0
Tokyo 3 0
Unknown 3 0
Federal Way 1 0
Mountain View 1 0
Redmond 1 0

{[{ mDataHeader[type] }]} {[{ month[type] }]} {[{ tData[type] }]}

Share

Share to:

Comment

Related Items