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.. Buchal, Ralph O. 1984-12-31

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 ]
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
Original Record: 1.0080813 +original-record.json
Full Text
1.0080813.txt
Citation
1.0080813.ris

Full Text

THE AUTOMATIC OFF-LINE GENERATION OF WELDING ROBOT WITH EMPHASIS  TRAJECTORIES  ON KINEMATIC FEASIBILITY AND  COLLISION DETECTION  by RALPH 0. B.A.Sc,  BUCHAL  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  in FACULTY OF GRADUATE STUDIES Department of  Mechanical  Engineering  t h e s i s as  conforming  We accept t h i s to  the  required  THE UNIVERSITY  OF BRITISH COLUMBIA  September © Ralph 0.  standard  1984  Buchal,  1984  In presenting this thesis i n p a r t i a l fulfilment of the requirements advanced degree at THE UNIVERSITY  OF BRITISH COLUMBIA, I agree that the  Library s h a l l make i t freely available for reference and study. agree that  representatives.  by the Head of my Department or by h i s or her  I t i s understood  for financial  gain  shall  that  copying  not be  permission.  Department of Mechanical Engineering THE UNIVERSITY OF BRITISH COLUMBIA 2075 Wesbrook Place Vancouver, Canada V6T 1W5  Date:  I further  permission for extensive copying of this thesis for scholarly  purposes may be granted  thesis  for an  CPcT"<?be\- (G> , / q 8 4 -  or publication  allowed  without  my  of this written  ii  ABSTRACT  This off-line  t h e s i s d e f i n e s and programming  of  human i n t e r v e n t i o n , and the d e s i r e f o r f a s t e r with  reduced  robot  down-time  thesis  data  An  made f o r the The  are  The  found  some s o l u t i o n s .  for  by  of  a  welding from  a  welding  resulting  The  commercial  system  Computer  module  selects  in  limits  degree of  and  of  and  known  feasibility and  workstation  Aided  increased  the  in  Design  are  arm  is  determined  and  other and  objects  to search f o r f e a s i b l e k i n e m a t i c  Provision i s  as  a  system.  considered  by  coordinate  the  kinematic  as w e l l as by  physical  i n the w o r k s t a t i o n .  f o r welding,  and  this  in  to the robot  the s o l u t i o n s are checked  The  against The  redundancy i s  solutions.  interference detection  i s modelled  welder  f o r each d e s i r e d robot p o s i t i o n .  i s redundant  the (CAD)  correct  homogeneous  the work envelope,  work envelope  freedom  simplified  combined  proposed  relative  Path  between  robot  i n v e r s e k i n e m a t i c s s o l u t i o n i s found  workstation  by  series  the  A  The work i s motivated  orientation relative  interference  used  of  t o r c h p o s i t i o n and  limits  sixth  a  i n c o r p o r a t i o n of a r e a l - t i m e seam t r a c k i n g  c o n s t r a i n t s of j o i n t  joint  minimum  programming,  software  with  automatic  programming c a p a b i l i t i e s  productivity.  Welder"  eventual  transformations.  the  workstation  on workpiece and w o r k s t a t i o n c h a r a c t e r i s t i c s .  kinematics  detail.  and  generated  "Expert  s e t t i n g s based  welding  and more powerful  efficiency  systems.  robot  proposes  flexibility, uses  a  d i s c u s s e s the problems i n v o l v e d i n  collection  of  algorithm solid  is  proposed.  polyhedra.  l i n k s sweep out volumes of space which are approximated  The  The robot  by volumes bounded  iii  by  parametric  determine  ruled  whether  surfaces.  A number  of simple  any i n t e r s e c t i o n s e x i s t between  tests  can be made to  the swept  volumes and  the s t a t i o n a r y p o l y h e d r a , i n d i c a t i n g i n t e r f e r a n c e . Much interactive  of  this  fundamental work  programming  detection algorithm  software  i s incorporated  package c a l l e d  AUTOP.  i s demonstrated by a program c a l l e d  in a  preliminary  The i n t e r f e r e n c e TESTIN.  iv  ACKNOWLEDGEMENT  help based  I would l i k e  to thank Professor  Dale Cherchas  and guidance  throughout my work.  for h i s enthusiastic  A large part  on work done f o r the I n d u s t r i a l Materials  of this thesis i s  Research  Institute, a  d i v i s i o n 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  xii  NOMENCLATURE  xiv  LIST OF SYMBOLS  xv  1.  INTRODUCTION  1  2.  PROBLEM DEFINITION  6  3.  DESCRIPTION OF WORKSTATION  9  3.1  9  3.2  4.  5.  Description of Workpiece Using CAD 3.2.1  Geometrical Description  10  3.1.2  Weld Seam Description  10  Description of Robot  11  3.2.1  11  Link Geometry and Dimensions  3.3  Description of Welding Machine  14  3.4  Description of Positioning Table  15  AUTOMATIC WELDING PARAMETER SELECTION USING AN "EXPERT WELDER" SOFTWARE MODULE  16  KINEMATICS  17  5.1  Review of Homogeneous Transformations  17  5.1.1  Homogeneous Coordinate Transformation  17  5.1.2  S e r i a l Combination  of Homogeneous Transformations  19  5.1.3  S e r i a l Combination  of Homogeneous Frames  19  vi  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  6. PATH FEASIBILITY  28  29  6.1  Statement of F e a s i b i l i t y Problem  29  6.2  Kinematic F e a s i b i l i t y  29  6.2.1  6.2.2  D e f i n i t i o n of Robot Joint Frames and Resulting Transformations  31  Inverse Kinematics Solution  32  6.2.2.1  6.2.3  General Approach to Inverse Kinematics Problem  32  6.2.2.2  PUMA 560 Solution  34  6.2.2.3  Discussion of PUMA Solutions  44  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  vii  Page 6.2.4  Specification of Robot Coordinates 6.2.4.1  Calculation of PUMA Tool Location and Rotation Coordinates  6.3  60  62  C o l l i s i o n 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  6.3.3.2  Surface  Condition 2 - Intersection of an Edge and a Cylinder  6.3.3.3  83  Condition 4 - Intersection of a Cylinder and a Polyhedral Face  6.3.3.5  80  Condition 3 - Intersection of an Edge and a Polyhedral Face  6.3.3.4  77  85  Condition 5 - Any Polyhedron i s Totally Enclosed by a Swept Volume Bounded by Parametric  6.4  88  Interference Detection Demonstration Program TESTIN ...  89  6.4.1  Functional Description of TESTIN  90  6.4.2  L i s t and Description of a l l Subroutines used by TESTIN  90  Demonstration of TESTIN  96  Possible Improvements to the Algorithm  102  6.4.3 6.5  Surfaces  viii  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  L i s t and Description of a l l Subroutines called from AUTOP  7.4  8.  107  Data Structures  116  7.4.1  117  Description of a l l I/O F i l e s and their Formats  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  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 L i s t i n g  162  1.3  CAD F i l e Generating Program CADGEN L i s t i n g  266  1.4  TESTIN Program L i s t i n g  270  X  LIST OF TABLES Page 3.1  Manipulator Link Geometry F i l 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 L i s t FILIST  118  7.2  CAD F i l e L i s t CADLST  119  7.3  Processed Seam F i l e L i s t WPCLST  119  7.4  Robot Location Data F i l e L i s t ROBLST  119  7.5  Object Geometry Data F i l e L i s t OBJLST  120  7.6  Seam CAD F i l e  120  7.7  Processed Seam Data F i l e  121  7.8  Robot Location Data F i l e  121  7.9  Object Geometry Data F i l e s  122  7.10  System Configuration F i l e WKSTAT  123  7.11  Example of F i l e WKSTAT  124  7.12  Positioning Table Specification F i l e TABLE  125  7.13  Example of F i l e TABLE  126  7.14  Graphic System I n i t i a l i z a t i o n  7.15  Example of GRAPH  127  7.16  Workstation Setup F i l e 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 F i l e WPARAM  130  7.21  Example of WPARAM for a Seam with 10 Locations  130  F i l e GRAPH  127  xi Page 7.22  Fortran F i l e Formats  131  7.23  Subroutines Accessing Data F i l e s  132  7.24  Contents of Seam CAD F i l e  142  7.25  Contents of Processed Seam Data F i l e  143  7.26  Joint Angles Calculated during Kinematic Feasibility  Search  144  7.27  Generated Robot Location Data  149  7.28  Task Precedence Table  153  xii  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  S e r i a l 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 , F^, F , F 3  5  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  D e f i n i t i o n of o, a, t angles for the PUMA Arm  63  6.6  World Model  70  6.7  C y l i n d r i c a l 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  xiii  Page 6.14  Intersection of Cylinder End Face and Polyhedral Face  6.15  Intersection Condition 1 between Face and Swept Volume  6.16  99  Intersection Condition 2 between Face and Swept Volume  6.18  98  Intersection Condition 3 between Face and Swept Volume  6.17  86  100  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  xiv  NOMENCLATURE  AUTOP:  the automatic o f f - l i n e robot programming  software package.  CAD:  Computer-Aided D e s i g n .  IGL:  T e k t r o n i x I n t e r a c t i v e G r a p h i c s L i b r a r y software package.  PUMA:  the Unimation  PUMA 560 i n d u s t r i a l r o b o t .  TESTIN:  an elementary  interference  t e s t i n g demonstration  program.  XV  LIST OF SYMBOLS  a_  t h i r d b a s i s v e c t o r of a frame r e l a t i v e  to the r o b o t ,  a^  l e n g t h of the common normal between the j o i n t i and j o i n t i - l axes of a r o b o t .  a-|  the common normal between the j o i n t i and j o i n t i - l axes o f a robot.  d±  the d i s t a n c e between l i n k s i and i - l of a r o b o t .  Fg  the workpiece c o o r d i n a t e  F  c  the instantaneous  F  G  the t o o l or t o r c h c o o r d i n a t e  frame.  weld seam c o o r d i n a t e  frame.  frame,  th F^  the i  F  the robot base c o o r d i n a t e  R  robot  l i n k coordinate  frame.  F-p  the p o s i t i o n i n g t a b l e frame.  H ,  a homogeneous c o o r d i n a t e  X  X  frame.  t r a n s f o r m a t i o n from frame F  jX  t o frame X  F  i-1.  J_  the i  11  the f i r s t  o_  the second b a s i s v e c t o r of a frame r e l a t i v e  P_  the p o s i t i o n of a frame o r i g i n  t  b  j o i n t a x i s of a r o b o t . b a s i s v e c t o r of a frame r e l a t i v e  relative  to the r o b o t . to the r o b o t .  to the r o b o t .  the p o s i t i o n of the robot w r i s t c e n t e r r e l a t i v e r^  a p o i n t l y i n g on a bounding p a r a m e t r i c  r_p  a p o i n t on a parent  TJJ  transformation matrix joint variables.  parametric  to the r o b o t ,  surface.  surface.  g i v i n g t o r c h p o s i t i o n as a f u n c t i o n of n  xvi  UQ  one of the v e c t o r s d e f i n i n g surface  u_i  v  0  the boundaries  of a p a r a m e t r i c  the boundaries  of a parametric  patch.  one of the v e c t o r s d e f i n i n g surface  of a p a r a m e t r i c  patch.  one of the v e c t o r s d e f i n i n g surface  the boundaries  patch,  x^  x - a x i s of a frame F^.  y-i  y - a x i s of a frame F^.  z±  z - a x i s of a frame Fj_.  a  l o n g i t u d i n a l angular o f f s e t  ctji  the t w i s t angle of the i t h robot  3  l a t e r a l angular o f f s e t of t o r c h from  8^  the i t h j o i n t v a r i a b l e of a r o b o t .  X  u  a parameter of a p a r a m e t r i c s u r f a c e .  ^  v  a parameter of a p a r a m e t r i c s u r f a c e .  of t o r c h from link  \j  a parameter of a p a r a m e t r i c volume.  P  t o r c h s t i c k - o u t gap.  seam.  seam.  - 1 -  1.  As  technology  important methods vastly  progresses,  tool i n industry. combined  INTRODUCTION  with  robots  are  becoming  an  increasingly  C o n t i n u a l progress i n programming and c o n t r o l  the i n c o r p o r a t i o n of sensory  i n c r e a s e the scope of a p p l i c a t i o n s  feedback  f o r robots  promise t o  i n the f a c t o r y .  A  g r e a t d e a l of fundamental r e s e a r c h remains to be done, however, b e f o r e t h e full  p o t e n t i a l of robots can be r e a l i z e d . Although  robots  have  facturing,  they are s t i l l  of  can be time  a robot  complexity.  i t s motions  the  so t h a t  way  -desired. resulting define  program.  locations,  utilized.  The  production  very l i m i t e d  a robot  tremendous  control.  or no  f o r p o s s i b l y unreasonable acute  Programming  by p h y s i c a l l y  guiding i t  P o s i t i o n s a r e recorded or played-back  capability  Computer-Aided  i s done  i n manu-  f o r tasks of even moderate  to e d i t  The program i s c o m p l e t e l y developed and e x i s t i n g  impact  i n their capabilities.  can be repeated  has l i t t l e  programming  are p a r t i c u l a r l y  a  i s programmed  under manual  the motions  The user  had  consuming and d i f f i c u l t  Typically,  through  already  Design  on-line,  as o f t e n as  o r modify  the  u s i n g the robot to (CAD) data  removing  l e n g t h s o f time.  along  i s not  the robot  These  from  shortcomings  f o r s m a l l batch p r o d u c t i o n where the robot must be  reprogrammed f r e q u e n t l y . Since cannot  most  adapt  necessitates insure Process  a  commercial  to  random  very  strict  high  degree  variations  of  robots  variations structuring accuracy  can occur  have  no  sensory  i n the t a s k s and c o n t r o l and  they  perform.  they This  o f the w o r k s t a t i o n to  repeatability  due to v a r i a t i o n s  capabilities,  f o r the  i n workpiece  process.  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 We  can  formulate  performance.  First,  two we  programming process, and off-line. must  main objectives for the improvement would  like  perform  to simplify  and  of  robot  speed-up the  robot  as much of the programming as possible  The method must be easy to use, fast and f l e x i b l e .  incorporate  variations. The  feedback.  sensory  feedback  and  Second, we  real-time correction  of  process  This thesis considers the f i r s t problem only.  problem of o f f - l i n e programming has been studied by a number of  investigators,  but  a  commercially  feasible  demonstrated.  Two  basic approaches have so  system  has  not  yet  been  far been demonstrated for  generation of a robot program, interactive simulation and  explicit  task  description using high-level languages. In  the  first  approach,  interactive graphics. and  method,  demonstrations  and  It i s d i f f i c u l t  there  process  i s done  using  may  be  a program which Is simulated  problems  and Kempf 1982;  on  to specify locations exactly with  described i n the l i t e r a t u r e  Ambler, Popplestone  the  The robot and workstation are depicted graphically,  the operator i n t e r a c t i v e l y generates  a graphics display. this  simulation of  visualizing  the  scene.  The  [Sata, Fumihiko and Akio  1981;  Sjolund and Donath 1983]  provide only  limited software tools to aid the user. A  second  and  more promising  approach  offered by high-level robot languages.  to  off-line  programming i s  These languages allow the user to  f u l l y describe the robot task i n a descriptive, natural way. can be specified  explicitly,  or can be referred  Locations  to a mathematical world  - 3 -  model. line  Many languages have been devised by robotics researchers, but o f f -  programming  Popplestone  and  has  been  Kempf  discussed  more  [1982] generated  an  than  demonstrated.  explicit  Ambler,  program i n 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. need  for  graphical  debugging.  simulation  the  program  for  Other existing high-level robot languages  number of recent papers The  of  Their experience showed a  off-line  sub-problems.  [Gruver et a l 1983; Kempf 1982;  verification  are summarized i n a Tarvin 1980].  programming problem encompasses a number of  Two  and  difficult  problems which are considered i n d e t a i l i n this thesis  concern kinematic f e a s i b i l i t y and c o l l i s i o n detection. The  kinematic  feasibility  automatically checked exists.  A  robot  kinematic  solution  of  a  proposed  robot  trajectory  must  be  to ensure that a v a l i d set of joint angle solutions position  exists  is  kinematically feasible  for the  joint  angles are within their physical l i m i t s .  angles, and  i f an  inverse  i f a l l the  joint  We have found by experience that  even when programming a robot on-line i t i s d i f f i c u l t to avoid paths which lead  to a joint  angle l i m i t  being exceeded, p a r t i c u l a r l y when moving i n  straight l i n e s . To v e r i f y kinematic f e a s i b i l i t y , we must solve the Inverse kinematic problems  to  trajectory.  Paul  joint  angles  (see Section 6.2  robots can be  [1981].  solution  the  at  each  point  along  a  proposed  The inverse kinematic problem has been well studied by many  investigators most r e a l  find  In  order  for a given  for a review).  An e x p l i c i t  solution for  derived by matrix algebra methods described by to  increase the  trajectory  we  will  chance exploit  of  finding  a  feasible  the redundancy of  the  - 4 -  sixth  degree of  freedom i n welding  to  search  f o r a f e a s i b l e set of  joint  s o l u t i o n s at each p o i n t on the t r a j e c t o r y . The problem  c o l l i s i o n d e t e c t i o n problem i s one of  path  planning  to  avoid  part of the f a r more d i f f i c u l t  obstacles.  Many a l g o r i t h m s  have been  proposed f o r f i n d i n g i n t e r s e c t i o n s between s t a t i o n a r y geometric s o l i d s between s t a t i o n a r y and  moving  ling  detection  and  interference  difficult way,  path p l a n n i n g  path p l a n n i n g This integrated solutions. package  No work has  discusses  off-line The  called  AUTOP.  feasibility  problem,  Many  vital  other  discussed  only  and  issues  briefly.  reviewed not  work i n geometric  in  Section  yet been s o l v e d  yet emerged i n the  6.3.  model-  The  problems  robot are  in a satisfactory  programming  incorporated  elementary  are  beyond  Fig.  s t r u c t u r e f o r an a u t o m a t i c a l l y  l i t e r a t u r e dealing  associated  P a r t i c u l a r emphasis on  more  nonwith  redundancy.  the  welding  solutions  Previous  s t r a t e g i e s have been proposed f o r simple  f o r systems w i t h  thesis  is  problem has  a l t h o u g h some promising  redundant systems.  solids.  and  1.1  system,  into is  methods  the  scope  illustrates  programmed welding  with  a  developing  and  offers  functional  placed  on  the  for c o l l i s i o n of  this the  workstation.  some  software kinematic  detection.  thesis  required  an  and  are  hardware  - 5 -  Interactive Terminal  Welding Data Base  Welding Machine Controller  Robot Controller  I  I  Positioning Table Controller  T  Robot Positioning Table  Welding Machine  \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ F i g u r e 1.1  S i m p l i f i e d Hardware Schematic f o r an A u t o m a t i c a l l y Programmed Robot Welding W o r k s t a t i o n .  \  - 6 -  2.  The  ultimate  objective  development  of  using  derived  data  a  system  provide  the  tracking  system w i l l  the  resulting  controlling We  of  CAD  files.  welding correct  program  the work d e s c r i b e d  for automatically  from  correct  PROBLEM DEFINITION  will  An  parameter  be  must  ambiguity.  a number of sub-problems  2. An  choose  a way  Important  information  i s the  welding  robot  welder  data  base  will  and  real-time  seam  a  with  The the  s t r u c t u r e of  highest  level  unit.  which  must be s o l v e d b e f o r e  realized:  Description  to r e p r e s e n t  and welding parameters.  including  values,  hierarchical,  C h o i c e of World Model and  geometry  expert  thesis  a  for real-world v a r i a t i o n s .  an automatic programming system can be  We  programming  the e n t i r e w o r k s t a t i o n as an i n t e g r a t e d  can i s o l a t e  1.  i n this  the workpiece  includes We  explicitly,  seam l o c a t i o n  must a l s o r e p r e s e n t  the r o b o t , p o s i t i o n i n g t a b l e and welding  without  d a t a , workpiece the w o r k s t a t i o n ,  machine.  G e n e r a t i o n o f a Welding Procedure a c c e p t a b l e and f e a s i b l e w e l d i n g procedure must be g e n e r a t e d .  procedure  includes  the  order  in  which  seams  are  welded,  The  workpiece  r e p o s i t i o n i n g , i n t e r - w e l d paths, e t c .  3.  Automatic G e n e r a t i o n o f Welding Parameter Values  Welding  output  parameters  such  as  current  and  speed  can  be  - 7 -  determined  automatically  c h a r a c t e r i s t i c s are  4. We  if  a l l relevant  generate  torch  Path F e a s i b i l i t y  The  calculated  work envelope and between the  6.  welding  machine  Trajectories  trajectory  data i n c l u d i n g  o r i e n t a t i o n along each seam i n terms of robot  5.  and  known.  G e n e r a t i o n of Welding T o r c h must  workpiece  torch  joint  robot and  position  and  coordinates.  Testing trajectory  limit  may  not  constraints.  other o b j e c t s  G e n e r a t i o n of  torch  be  Also,  In the  feasible  due  interference  to may  robot occur  workstation.  H i g h - L e v e l Program to D i r e c t  and  Coordinate  the  Welding Process We  need  coordinating carefully  a  the  descriptive,  high-level  method  for  d i f f e r e n t d e v i c e s i n the w o r k s t a t i o n .  controlled  i n t e r a c t i o n between the  controlling  Welding r e q u i r e s  robot, p o s i t i o n i n g  table  and a and  welding machine.  7. Due  Program M o d i f i c a t i o n to  real-world  w e l d i n g , a seam t r a c k i n g important  8.  i n Response to Sensory Feedback  variations  and  system based on  uncertainties real-time  in  positioning  sensory feedback i s  and an  capability.  Hardware and  Hardware  and  Software  software  Interfaces interfaces  must  be  designed  between  the  - 8 -  various  components  communicate  with  i n the w o r k s t a t i o n .  the robot  controller,  A host  computer must  positioning  table  be a b l e to  controller  and  welding machine.  These proposed.  problems  will  be  discussed  and  some  solutions  will  be  - 9 -  3.  DESCRIPTION OF WORKSTATION  We must d e s c r i b e a l l the important c h a r a c t e r i s t i c s of the w o r k s t a t i o n in  a  concise  positioning  way.  The  table,  workstation  is  machine,  robot  welding  composed and  of  any  the  workpiece,  other  peripheral  equipment.  3.1  D e s c r i p t i o n of Workpiece U s i n g A  CAD  complete  system. 1.  description  The CAD  CAD  of the workpiece  i s generated by an  d e s c r i p t i o n must c o n t a i n the f o l l o w i n g  external  information:  Geometry a) dimensions and b) weld  seam  trajectories  in digitized 2.  shape of o b j e c t and  s u r f a c e normals  a d j a c e n t to seams  form.  Welding I n f o r m a t i o n a) m a t e r i a l b) t h i c k n e s s c) weld  joint  types  d) weld c r o s s s e c t i o n a l The  CAD  specified This  file  input  must  be  translated  of the CAD  specified  from  the CAD  output format i n t o  format f o r the software system, which we w i l l  t r a n s l a t i o n i s required  Details  area.  so that any e x t e r n a l CAD  the  c a l l AUTOP.  system may  be used.  system need not be known at t h i s p o i n t , as long as the  i n f o r m a t i o n i s generated i n some form.  - 10 -  3.1.1 We  Geometrical wish  to  Description  represent  the  geometry  of  the  workpiece  by  approximating polyhedron, whose v e r t i c e s , edges and f a c e s a r e s t o r e d h i e r a r c h i c a l , f i l e - b a s e d data list  of the o b j e c t  faces,  structure.  Each f a c e  surface record  of the f a c e v e r t i c e s i n any s e q u e n t i a l order of  the  polyhedron  representation  i s shared  i s well  by  suited  at  to  in a  At the top of the h i e r a r c h y i s a  including their  o r i g i n and number of v e r t i c e s .  an  normals, d i s t a n c e  contains  the c o o r d i n a t e s  around the f a c e .  least  graphical  three  Each  faces.  depiction  from  A  and  vertex  polyhedral  interference  detection.  3.1.2 Seam A list  Weld Seam D e s c r i p t i o n data  welding  data  and  digitized relative  seam data relevant  weld  cross  coordinates  file.  in a file.  The head  of each  file-based structure.  Each e n t r y seam data  seam, i n c l u d i n g m a t e r i a l  sectional  area.  The  rest  of  Long  points  file  the f i l e  to a  contains  thicknesses,  o f the seam t r a j e c t o r y and adjacent  to a r e f e r e n c e  by AUTOP.  i n a hierarchical,  i s stored  to that  Each seam d e f i n e d entity  stored  of seam i d e n t i f i e r s  corresponding  types  i s also  seam  contains  surface  normals  frame f i x e d on the workpiece. i n the CAD f i l e seams should  i s t r e a t e d as a s i n g l e , i n d i v i s i b l e be broken down i n t o s h o r t e r  segments  which can be t r e a t e d s e p a r a t e l y .  F o r example, a c i r c u l a r weld around the  circumference  be d i v i d e d i n t o two or four segments so  of a c y l i n d e r should  that the workpiece can be r e p o s i t i o n e d b e f o r e welding each segment.  - 11 -  3.2 D e s c r i p t i o n of Robot AUTOP requires a c a r e f u l 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 t o o l mounting flange. 5.  Default robot c o n f i g u r a t i o n .  6. P o s i t i o n 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 f o l l o w the conventions of Denavit and Hartenberg describing l i n k geometry and dimensions.  [1955] i n  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 j o i n t s 3.1, 3.2). .  (Figs.  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 j o i n t 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 j o i n t i axis i s —1-1 — i  d^, the distance between the l i n k s . and  6^ i s the angular displacement of a.^  about the j o i n t i axis and i s c a l l e d the angle between the l i n k s . Parameters  a^ and ct^ a r e f i x e d f o r a g i v e n 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^ v a r i e s and 8^ i s f i x e d . For a revolute j o i n t , d^ i s fixed and 9^ v a r i e s . Given these parameters,  we can completely define the kinematics of  F i g u r e 3.1  L i n k geometry and frames f o r a R e v o l u t e J o i n t .  - 13 -  Joint n  F i g u r e 3.2  L i n k geometry and frames f o r a P r i s m a t i c  Joint  - 14  the  robot.  Table  3.1.  The  parameters  Table 3.1  are  -  stored  in  a  file  - M a n i p u l a t o r L i n k Geometry F i l e  link #  a  id  a  with  the  format  of  Format  d  9  1 2  n  id  3.3  = 1  revolute  prismatic  D e s c r i p t i o n of Welding Machine The  welder they  characteristics  file  where  file  contains a l l fixed  are  easily  the  following:  a) v o l t a g e s e t t i n g b) wire  diameter  c) wire  type  d) wire feed r a t e  and  specifications  r e f e r e n c e d by  settings  are r e l e v a n t to the welding of  id = 2  and  process.  the E x p e r t  characteristics The  file  might  are  stored i n a  Welder module. of the welder  data The  which  i n c l u d e some or a l l  - 15 -  e) gas type, flow r a t e f)  polarity  g) minimum,  maximum  current  ( i f under  program  control),  or c u r r e n t  setting. The c o n t e n t s of the f i l e  depend on the p a r t i c u l a r welder  The  format has not yet been completely  3.4  D e s c r i p t i o n of P o s i t i o n i n g Table AUTOP  requires  a  complete  which i s s t o r e d In a data f i l e .  of  b) l i n k geometry and  dimensions  (continuous or d i s c r e t e  e) j o i n t  limits  f)  joint location relative  table,  steps)  to world c o o r d i n a t e s  g) t a b l e c o o r d i n a t e frame l o c a t i o n r e l a t i v e table  manipulator,  positioning  types ( r e v o l u t e or p r i s m a t i c )  d) j o i n t motion  The  the  The f o l l o w i n g i n f o r m a t i o n i s s t o r e d :  of freedom  first  specified.  description  a) number of degrees  c) j o i n t  i n use.  i s treated  so the l i n k  as  analogous  to  to l a s t an  geometry and dimension  n  joint  degree  parameters  frame. of  have  freedom the same  d e f i n i t i o n s as f o r a r o b o t . We  can e a s i l y  accommodate  t a b l e simply by r e v i s i n g  a totally  different  the e n t r i e s i n t h i s data  type  file.  of  positioning  - 16 -  4.  AUTOMATIC WELDING PARAMETER SELECTION USING AN "EXPERT WELDER" SOFTWARE MODULE  A  welding  developed The  data  base  software  module  have  been  by Dr. F. Sassani of the Department of Mechanical Engineering,  University  of B r i t i s h  system, henceforth referred previous  and i n t e r a c t i v e  report  [Buchal  Columbia.  A detailed  description  of this  to as the Expert Welder, can be found  et a l 1984].  Essentially,  the Expert  in a  Welder  module uses a data base derived from standard welding practice to choose appropriate welding and fixed welder  parameter settings as a function of given workpiece  parameters.  - 17 -  5.  5.1  Review of Homogeneous 5.1.1  KINEMATICS  Transformations  Homogeneous Coordinate  Transformation  It i s often useful to transform the coordinates of a point i n space from one coordinate frame to another  ( F i g . 5.1). A r o t a t i o n a l  transforma-  t i o n i s r e q u i r e d i f corresponding axes of two coordinate frames F^ ^, F^ are not p a r a l l e l .  A t r a n s l a t i o n a l transformation i s required i f the frame  origins are not coincident.  In general, both a rotation and a translation  are required. A homogeneous rotation  coordinate  and translation  transformation matrix. projected onto a  transformation  i n a single  allows  us to perform  operation using  a 4x4 homogeneous  For example, a vector _r expressed  coordinate  both  frame Fj_ can be transformed  r e l a t i v e to and into a vector R  r e l a t i v e to and projected onto a frame ^_^ by the following matrix F  multiplication:  where H  . i s a 4 4 homogeneous transformation matrix relating frame I I»I X  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 l a s t element  i s a scaling  factor.  always set to 1. H. . . has the following form: -1-1, i  For our purposes this  scaling  factor i s  - 18  -  i-l  F i g u r e 5.1  Coordinate  Transformation.  - 19 -  rotation  translation  3x3  P  (5.2)  X  rotation matrix  P p  0  o  0  y  z 1  A more detailed description of homogeneous transformations  can be  found i n other references [Paul 1981].  5.1.2 We  S e r i a l Combination of Homogeneous can  combine  homogeneous  transforms  Transformations in  series  as  shown i n  F i g . 5.2. Suppose we want to find R given _r. find H  „ as the product of H,  5-1,3 Thus,  R 5.1.3  -1,2 3-2,3  =  =  3  jr  and H  I f we know H^  2  and H^ ^ we can  :  *  ( 5  .  '  3 )  (5.4)  S e r i a l Combination of Homogeneous Frames  We can show that the homogeneous transformation  H^ ^ ^ defines a  coordinate frame F. r e l a t i v e to F, , . I i - l EJ , . can be written as -l-l.i i where  <->  =  ti, o_, a_ £ are 4 ^ 1 y  5  column vectors.  5  ii, o_, ji define the basis  vectors of F^ r e l a t i v e to F^_^,and P_ i s the o r i g i n of F^ r e l a t i v e to F^ ^, as shown i n Fig.5.3.  - 20 -  Figure  5.2  S e r i a l Combination  of Homogeneous  Transformations.  - 21 -  F i g u r e 5.3  Homogeneous C o o r d i n a t e Frame d e f i n e d by n, o_, a_, g_.  - 22 -  Note that i f we know the p o s i t i o n and o r i e n t a t i o n of Frame F^ r e l a t i v e to F^ ^,  5.2  ^ i s also known.  Statement of Kinematics Problem To  generate a robot  program,  we must  find  the tool  orientation at each point along the weld seam trajectory.  position and  Mathematically  s t a t e d , we wish to f i n d  the homogeneous coordinate frame F of the tool G r e l a t i v e to and projected onto the robot base coordinate frame F . F is R G d e f i n e d by the homogeneous coordinate transformation ^ which takes us from F  G  to F . R  Thus, we wish to find IL  "iv, G . We can define a s t r i n g of  i n t e r m e d i a t e coordinate frames F^ for which H^_^ • ^ can be calculated, as shown i n F i g . 5.4. We define the following homogeneous coordinate frames: a) F R b) F T  c) F B d) F^  -  robot base frame.  -  positioning table frame.  -  workpiece frame.  -  weld seam frame.  e) F - tool frame. G We can solve f o r the homogeneous transformations H_ ,H ,H , —R,T —T,B —B,C IT, . We can then find H_ —C,G ~R,G SR.G  5.3  =  VT  -T,B  Vc  SC.G  '  (  5  ,  6  )  Intermediate Homogeneous Coordinate Frames 5.3.1 We  Table Frame Relative to Robot Base  wish to define  the position  of the table  frame Ff r e l a t i v e to  - 23 -  F i g u r e 5.4  S e r i e s of Homogeneous C o o r d i n a t e Frames.  - 24 -  the  robot  base  formation  coordinate  can then  the  R  .  The problem i s t o f i n d the t r a n s -  ^.  Suppose we analyse We  frame F  the t a b l e as an n - l i n k system, s i m i l a r to a r o b o t .  d e f i n e the t r a n s f o r m a t i o n  Denavit-Hartenberg  [1955]  H. , . from l i n k i - l to l i n k i by —i-l,i  equations  (see  Section  6.2.2.1  for  derivation):  cose,  H, . -i-l,i  Sin6  -Sin 8 Cos a. i i  S i n 8. S i n a. i i  a.CosS^ l i  Cos 8. Cos ct. l i  -Cos 8. S i n a. i i  a. S i n 8. i i  Sinct.  where  8^ i s t h e i  parameters.  ^ joint  Either  angle  and  Cos a,  d. l  ct^ and a ^ a r e f i x e d l i n k  6^ ( r e v o l u t e j o i n t )  (5.7)  geometry  o r d^ ( p r i s m a t i c j o i n t ) i s  variable. We can combine T+ —0,n  -  the t r a n s f o r m a t i o n f o r a l l n l i n k s as H  —U,1 — l , z  T h i s g i v e s us the n l i n k c o o r d i n a t e frame F We to  F  th  ••• H  •  —n-1, n  .  (5.8)  l i n k c o o r d i n a t e frame F r e l a t i v e to the f i r s t n  0  a l s o w i s h t o s p e c i f y the l o c a t i o n of the t a b l e frame F , and t h e f i r s t  relative  l i n k frame F^ r e l a t i v e to the robot base frame F .  These two t r a n s f o r m a t i o n s  H  , H are f i x e d and known. n, T H_ • _ H _ • H _ -R,0 -0,n -n,T  Thus,  R, 0  (5.9)  - 25 -  5.3.2 We table.  Workpiece Frame R e l a t i v e t o T a b l e  can  Thus,  which needs  5.3.3 We tangent  safely  assume  that  the workpiece  attached  to the  H i s some f i x e d 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  to be s p e c i f i e d only once d u r i n g  initialization.  Weld Seam Frame R e l a t i v e t o Workpiece  define  the weld  to t h e seam  seam frame F  The t r a n s f o r m a t i o n H  [ T, B, A, R  5.3.4  as shown i n F i g u r e 5.5, where T i s  t r a j e c t o r y and A b i s e c t s the s u r f a c e normals N_ , N^.  ( i . e . , A i s normal to weld f a c e ) . —  The  i s firmly  B, C  i s then  ]  (5.10)  T o o l Frame R e l a t i v e to Weld Seam w e l d i n g rod  i s positioned  practice.  relative  The  position  to  standard  welding  and  relative  to the weld  are c h a r a c t e r i z e d  by  s u r f a c e normal, a and  3, and a t o o l gap  p (Fig.  the weld  as  orientation  angular o f f s e t s 5.6).  required of  the  by tool  from the weld  p, a, 3 are welding  parameters determined by the E x p e r t Welder module. We know that  the elements of L [ n,  -C,G  can be found from o,  a,  P  ]  (5.11)  From t r i g o n o m e t r y : - sin a sin 3 cos a cos 0 and n, o  P_  =  are a r b i t r a r y —  - p£  (5.12) 3  .  l e t us d e f i n e them as  (5.13)  F i g u r e 5.5  Weld Frame R e l a t i v e  to Workpiece.  27 -  n F  G  =  TOOL  FRAME  A=Z,  / / /  / /  \  /  /  \  Y7 /  \  /  B=Y  /  \  V \ \  F = c  WELD  FRAME  WELD SEAM  T = X  F i g u r e 5.6  T o o l Frame R e l a t i v e to Weld Seam.  C  C  - 28 -  cos  and  a  x  0 3 cos a sin 3 0  n  We can now f i n d elements of  5.4  (5.14)  (5.15) IT —C, G  =  [n, o, a, P] — — —  C a l c u l a t i o n o f the T o 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 t o World Once  Coordinates we  have  specified  a l l the  intermediate  homogeneous  trans-  f o r m a t i o n s , we can c a l c u l a t e the t o 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  K.  .  This  which  information  takes  us  from  i s given  the  tool  by t h e homogeneous coordinates  combination of homogeneous t r a n s f o r m s , H „ -R,G The  —R,T  elements  of H  —T,B have  to  transformation H —R, G  robot  we can c a l c u l a t e  -B,C  coordinates. ^:  (5.16)  -C,G  the form  By  of four  column  vectors,  n_, o_, a_, P_:  We know that the v e c t o r s n., o^, and j i d e f i n e the b a s i s v e c t o r s o f the tool the  coordinate displacement  coordinate simply  frame F p r o j e c t e d onto the robot base frame F , and P i s G R — of F  G  relative  frame which i s d i f f e r e n t  r e l a t e d by a f i x e d  to F  R  .  I f we w i s h to d e f i n e a world  from the robot base frame, they can be  transformation.  6.  6.1  Statement of F e a s i b i l i t y Problem When we  may  PATH FEASIBILITY  calculate a nominal robot  tool trajectory through space, we  find that the trajectory i s not f e a s i b l e .  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 c o l l i s i o n with objects i n the  We the  workstation.  can define a trajectory as being f e a s i b l e i f the robot can follow  trajectory smoothly, without v i o l a t i n g the kinematic  without  colliding  with  or  contacting  any  of  the  constraints and  objects  i n the work-  station. Of  the  two  problems we  have defined,  the kinematics  problem i s 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 F e a s i b i l i t y Mathematically  the conditions which must be s a t i s f i e d  for  kinematic  f e 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 l i m i t s .  In order for us to test these conditions i t i s necessary to solve for the  joint angles  tion relative  as a function of the desired tool location and orienta-  to the robot base ( i . e . , H^  the inverse kinematics  solution.  There are two p a r t i c u l a r cases which are  of p r a c t i c a l i n t e r e s t .  Suppose we  defining  state  n  independent  This i s commonly known as  define a robot gripper state vector X  variables.  Let  us  also  define  a  joint  - 30 -  variable vector _6 defining m joint variables. finite n  set of solutions  < m,  redundant The  an  infinite  If n = m, we can find a  of  9 for any  set X i n the work envelope.  set of  solutions  exists  and  we  say  robot  has  degrees of freedom. kinematic problem  the years.  Denavit  and  has been considered by many researchers over  Hartenberg  [1955] f i r s t  formally specified  geometry of connected links with homogeneous matrices. fundamental  the  This work has been  to most later work.  Whitney  [1969; 1972]  multi-degree-of-freedom inverse  the  If  problem  by  gripper  motion  relative  cartesian  developed  the mathematics for the kinematics of  manipulators and prosthetic arms, and  finding  the  inverse Jacobian matrix,  r a t e ( i . e . _9 = J_~ X_).  to j o i n t  1  motion  along  world  solved the  J relating -  T h i s method allows  coordinates, based  on real-time  calculations of instantaneous J_( 0) and J ~ \ 6 ) . No  general  solution  absolute positions. analysis 1980;  has  been  An  has  iterative  proposed  Konstantinov,  been  by  Genova and  found  for  the  solution based Konstantinov  Zahariev  Nechev 1981; Konstantinov and Patarinski  inverse problem  on f i n i t e  displacement  [Konstantinov  and  Konstantinov,  Markov  1981;  for  Markov and  1982].  Several general recursive techniques have been proposed based on the approximate  decoupling of the minor and major axis of motion  [Gaglio et  a l 1981; Milenkov and Huang 1983; Benati et a l 1982]. Explicit practical solution  solutions  importance.  can Lee  be and  found Ziegler  for the PUMA robot based  f o r most  robot  configurations of  [1983] have derived an  on geometry.  Explicit  explicit  solutions  can  - 31 -  also be obtained  by matrix algebra using the Denavit-Hartenberg  [Paul, Shimano and Mayer 1981;  matrices  Paul 1981].  Redundant degrees of freedom are useful for avoiding joint contraints and  allowing  collision-free  problem can be  solved by optimizing  systems have been considered [Fournier and 1981;  configurations  K h a l i l 1977;  to  be  found.  some c r i t e r i o n  The  function.  inverse Redundant  i n more d e t a i l by a number of investigators  Liegeois 1977;  Konstantinov Markov and Nechev  Benati, Morasso and Tagliasco 1982]. We w i l l develop an e x p l i c i t inverse kinematics  matrix method of Paul [Paul, Shimano and Mayer 1981;  6.2.1  solution based on the Paul 1981].  D e f i n i t i o n of Robot Joint Frames and Resulting Transformations  A  robot  arm  consists  revolute or prismatic j o i n t s . according  to  geometry  is  the  of  in  Figs.  series  Coordinate  specification  shown  a  of  3.1  of  links  frames are attached  Denavit and  rigid  and  3.2.  Hartenberg First  l e t us  connected  by  to each l i n k [1955].  The  state  some  definitions: J_  =  joint i axis of rotation or t r a n s l a t i o n  a^  =  common normal of axes J ^ ,  For a revolute j o i n t , the o r i g i n of F of point.  a n  d  i s located at the intersection  :LL I intersect, the o r i g i n i s at the intersection  If  +  I f 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  the a x i s of the next r e v o l u t e  i+1 i s prismatic, replace joint.  The  above with  o r i g i n of F^ i s defined for  - 32 -  d  i l = °+  The a..  z a x i s , z^, of F^ i s along  I f J . , J . ,, i n t e r s e c t ,  —i  — i —i+1  For = 0. = 0.  J  x j . -i+1  of F^ i s along to J . x J  i  — l —l+l.  joint,  becomes the j o i n t v a r i a b l e .  a n c  Inverse Kinematics  6.2.2.1  the  x-axis,  In t h i s  case  The o r i g i n o f F^ i s c o i n c i d e n t w i t h t h e o r i g i n o f F ^ for z^ i s a l o n g * *± *- p a r a l l e l o r a n t i - p a r a l l e l t o  6.2.2  Once  e  = x.  a prismatic  a_^ d^  n  x. i s p a r a l l e l or a n t i - p a r a l l e l —  9^ i s zero when _x  T  we  Solution  G e n e r a l Approach t o I n v e r s e K i n e m a t i c s Problem have  homogeneous  transformation  s  defined  the j o i n t  transformation  frames  relating  as  shown, we  can c a l c u l a t e  f r a m e F^ t o f r a m e  i s composed of the f o l l o w i n g  The  sequence:  1. r o t a t e about z a x i s of F^_^ by an angle  9^;  2. t r a n s l a t e along z a x i s of F^_^ by d i s t a n c e d^; 3. t r a n s l a t e along new x a x i s of  by l e n g t h  a^; and  4. r o t a t e about new x a x i s of F^_^ by the t w i s t angle These four t r a n s f o r m a t i o n s H. , . —1—1,1  =  .  can be combined as f o l l o w s :  Rot ( z , 9 ) Trans (z,d.) Trans (x,a.) Rot ( x , a ) . i i i i  H, . .  -5 9^^  0  0  1  0  C9  0  0  0  1 0  ±  0  1  0  0  0  Ca  a  d  ±  0  0  1  0  0  0  1  d^  0  So^  0  0  0  1  0  0  0  1  0  0  0  0  "S^ 0 Co 0  i  0 1  (6.1)  - 33 -  where C9^  = Cos 9^, S9^  = S i n 9^, e t c .  c e . -sg^Ca. i  i  S9^ i  SQjSa.  i  i  J  i  i  a.se^ l i  4  Set,  (6.2)  a.CQ  x  C8 C a -C9.,Soi i i i i  0  M u l t i p l y i n g out, we get  Ca,  0  This  t r a n s f o r m a t i o n was Once we  f i r s t d e r i v e d by Denavit and H a r t e n b e r g [1955].  have d e f i n e d H  them to f i n d  each l i n k SR i  i = 1, n.  We  can a l s o =  i = l , ... n, we  can  combine  l , i  \ l ^ l , 2  =  H. —i,n  —  frame r e l a t i v e  f  for  f o r the l i n k s  1  i  find  to the robot  —  base frame F , i . e . ,  *i-l,i  <-> 6  3  the t r a n s f o r m a t i o n  H. , —i,i+l  •••  H —n-l,n  (6.4)  til relating We  the l a s t now  frame to the i  wish  to  solve  for  frame. the  joint  variables  for a  given  tool  position and o r i e n t a t i o n . We can f i n d the homogeneous t r a n s f o r m a t i o n H w h i c h s p e c i f i e s the t o o l o r i e n t a t i o n and p o s i t i o n as a f u n c t i o n of —~R, n the  joint  variables  orientation  relative  to  joint  the l a s t  easily 1  find  H„ —R,n  9^,  8 ,  •••  9^.  We  to the robot base,  frame  by  = H —R,G  know t h e t o o l ^.  The t o o l  location  frame i s  and  related  a fixed  H —n,G  known t r a n s f o r m a t i o n H , so we can - n, G The e l e m e n t s o f H„ a r e now known: —R,n  i.e., H —K, n where  =  [ n, o, a, P — — — —  ]  ,  (6.5)  - 34 -  n, o, a —  are basis vectors of l i n k n frame r e l a t i v e to F ; R  P_  i s position vector of l i n k n frame r e l a t i v e to F . R  Let us c a l l this matrix T^. We can now equate T and H element by element, searching for e x p l i c i t —n —R,n equations f o r the j o i n t angles 9 ^ , • • • 9 ^ . I f necessary, we can also compare the elements of other equivalent matrices defined T —n  = H —R,n  =  ^1,2  We  continue  h ] l  until  below.  ^ n  =  (6.6) (6.7)  -l,n ,  h , n  we have  etc.  explicit  (6.8)  solutions  f o r a l l the joint  variables. The  actual  intuition  solution  of a p a r t i c u l a r robot  and s i m p l i f i c a t i o n .  i s generally  based on  The p a r t i c u l a r solution f o r a UNIMATION  PUMA 560 follows.  6.2.2.2  PUMA 560 Solution  For the PUMA robot, we can define a set of l i n k frames as shown i n Fig.  6.1.  Table 6.1.  We  can also  specify  the appropriate  link  parameters i n  F i g u r e 6.1  PUMA J o i n t Frames and l i n k  parameters.  - 36 -  Table 6.1 - PUMA Link Parameters  a  a  d  1  -90  0  0  e  0  3  90  ~ 3  \  -90  0  Variable  Link  1  9  2  2  3  6  4  a  d  2  0  a  5  9  5  90  0  6  6  6  0  0  2  0 d  6  Note that the sign of a 3 i s negative to conform to our d e f i n i t i o n of x . 2  For the PUMA, the l i n k parameters have the following a  2  =  432 mm  d  a3  =  20.5 mm  d^ = d  Note that  2  149.5 mm 432 mm  =  6  56.5 mm  the following analysis also applies to any other 6 degrees  of freedom robot with the same geometry. parameters  =  values:  a ^ ,  d  2 ' ^4'  a n C  *  ^6  a  r  e  The correct values of the l i n k s  * Pl m  v  substituted  i n t o the  solutions. We know the position and orientation of F o  SQ,6  =  t£»£»£'  I 1  r e l a t i v e to F  from  0  '  where n, o, a  are the F, basis vectors projected onto F ;  (  6  ,  9  )  -  P  Is the  —  b  37  -  o r i g i n r e l a t i v e to F  Note that F^ i s the frame attached equivalent  to the robot  .  n  U  to l i n k 0 by our d e f i n i t i o n , and i s  base frame F .  F, i s attached  R  to the t o o l  u  mounting flange. We have d e f i n e d and we know that  the i n t e r - l i n k transformations  H_ , = H_ . H yX  yX  . * * • H. X y £•  X  H. -i-l,i  for each l i n k  .  (6.10)  XyX  From the geometry of the arm (Figure 6.2) we note that the o r i g i n of F^ i s dependent on the variables 9^, 0^, 0^ only, and i s coincident with the o r i g i n of F,.. Let  q —  =  P —  - d, a o —  and  q^  =  4*"**  ( o r i g i n of F ) , J  column of  ^  HQ  We can find ji by multiplying  ( o r i g i n of F^) .  HQ ^  £23  ^  only).  The 4 t h column of H_ , i s  (6.11)  c  3 4  +  a  3 3  " 3 4  +  a  3 3  S  C  d  d  0 1  C  S  H3 4  (  w e  n e e c  *  t  n  e  4 ' t  1  column  - 39 -  The 4th column of H, . i s C ( S d -1,4 2  3  S (S d 2  3  4  + a C ) + S (C d  4  a  4  + a C ) - C (C d  4  a S ) + a S  3  3  3  2  3  3  2  3  3 3> S  3  3  +  a  2 2 C  2  2  q = 4th column of H •,. — HJ, 4 Multiplying out, we get  C [c (S d  4  + a C ) + S (C d  4  - a S ) + a ^ ]  S  l 2  S [c (S d  4  + a C ) + S (C d  4  - a S ) + a ^ ]  C  l 2  1  q  =  1  2  2  2  3  -S (S d 2  3  4  3  3  3  2  3  3  2  3  + a C ) + C (C d 3  3  2  3  3  3  3  3  d  d  (6.12)  - a S ) - a ^  4  3  3  1 q  can be simplified as follows: q. =  W S  3  2  2 3  j +. „a nC  2  + a C  2 3  + a C  2  3  lK 23 S  d  + a, rC 3  C  a  S  where  u  23  23  2  4 23 " 3 23 " 2 2 a  1  3  2  =  sin( 6 + 9 )  =  cos(6  2  2  3  + 8 )  S  1 _  S  C  D  l 2 d  (6.13)  - 40 -  S o l u t i o n f o r 9^ From the f i r s t  two rows o f (6.13) we can show t h a t +  -1 r * 2  fl  q  1  Note:  l  q  (  , 2 ^ 2 ,2,1/2 , l 2 ~ V - 2 ^ 1 q  (  q  +  l  q  +  d  +  d  2 2 q  for left  -  f o r r i g h t handed arm c o n f i g u r a t i o n . (y)  1  handed arm c o n f i g u r a t i o n ,  ;  0 < 9 < 90°  for  x > 0  and  y > 0;  for  x < 0  and  y > 0;  < 9 < -90°  for  x < 0  and  y < 0;  < 9 <0  for  x > 0  and  y < 0.  90° < 9 < 180°  -90°  2 " 2>  1  +  F o r 9 = tan  -180°  q  .....  d  S o l u t i o n f o r 9^ Let  q  2 l  2  +  q  2  +  q  2 2 2 2 3 " 2 ~ 4 " 3 " 2 d  d  2  B  =  -d  4  a  a  2  a  (6.15)  2  ;  C  =  We can show that A = -B s i n 9^ + C cos 9^.  a  3  .  (6.16),(6.17)  The s o l u t i o n has the f o l l o w i n g  form: 8  3  =  t a n " (|) 1  tan" ( 1  ±  A  - )  (6.18)  ^ 2 -*A'' 2 where r  2  =  2 2 B + C  .  (6.19)  - 41 -  Substituting (6.16), (6.17) into (6.18) we get 8  =  tan  _ 1  2  r  =  "  1  ±  d  2 where  ( - ^ ) - tan" ( " 4  J  " ]  A  (6.20)  ~2  f l  r  - A  2  a^ +  (6.21)  Two different solutions result depending on the sign: + for elbow above configuration, and - for elbow below configuration,  ( f o r l e f t arm configuration);  or - for elbow above configuration, and + for elbow below configuration,  ( f o r right arm configuration).  Solution f o r 8^ From (6.13) we get q  3  q  3  =  d  4 23 ~ 3 23 " 2 2 C  a  S  a  "  S  ( 6  *  2 2 )  Expanding, we get =  d  J 2 3 " 2 C  C  S  S  ] " 3 t 2 3 a  3  S  C  +  C  2 3 ^" 2 2 ' S  a  S  ( 6  *  2 3 )  Rearranging we get q  3  =  ^43 " 3 3 J2 " K 3 d  C  a  S  C  S  +  a  3°3  +  a  2 ^2 * S  ( 6  * > 2 4  Let A  =  d C 4  3  - a S  3  B  =  d s  3  + a C  3  4  3  3  , + a  (6.25) .  2  (6.26)  Then the s o l u t i o n i s  tan  - 1  (• )-tan" ( A  1  :  B  —z=——) ± / 2 - 2 r q 3  Substituting (6.25), (6.26) into (6.27) we get  •  (6.27)  - 42 -  •>  d. C_ - a_S_  ° •  ,  >-  ^ V . ^  2  q.  «.»)  1  where 2  2  r  =  2  A  + B  .  (6.29)  Expanding (6.29), we get r  =  2  (d C 4  3  - a S ) 3  3  2  + (d S 4  + a C  3  3  3  + a )  2  2  .  (6.30)  Again, there are two solutions for (6.28); + i s for the l e f t handed arm and  -  There are two not  i s for the right handed arm  8^,  It i s i n t u i t i v e l y obvious  there e x i s t two  p o s i t i o n the arm  possible  at the same height q . 3  the arm i n opposite quadrants tion  configuration.  s o l u t i o n s for 8^ because the parameters  i n the equation.  rotation  configuration  can be determined  that  for a given waist  shoulder r o t a t i o n s ,  » which  The possible rotations position  of the horizontal plane.  by s u b s t i t u t i n g  and q^ are  The correct solu-  0^ i n t o the equation f o r the  parameter q^ or q^ ( e q u a t i o n 6.1) and comparing the sign of the r e s u l t . It  turns out, however, that  with the l e f t arm arm  the positive solution i s always  configuration and  associated  the negative solution with the right  configuration.  Solution f o r 8., 8 JT ZT———_ Set 6^ such that z.  =  i s normal to the plane formed by z^ and a_, i . e . ,  (z±-H=  \L  3  and  • a ) 1-  '  £ T  (6.31)  -  "4 cos  Note  e  4  x^,  that  fe  3  .  =  Set  8  3  '  ' £ 4 )  •  a r e the f i r s t  two columns of  and  -  gives wrist  flipped  Set  a  n  8 o  £  =  s i n 8,. =  and  cos 8^  =  ( C  1 23 4 - l 4 C  C  S  S  so that  >  ^  configuration.  (6.36)  -y^ * £ .  (6.37) o f H. ,• —0,4  o u t H . , and —0,4  Expanding 0  (6.36), (6.37) we can show ) a  y, = o —o —  x  <SlC23C4-  +  +  -ClV  S  y " 4 23 C  S  ,  a Z  J'  ( 6  ,  '  3 8 )  (6.39) ,  (6.40)  .  (6.41)  c  0  and  3  3  x^ * £ »  s i n Q. = y • n  i.e.,  )  configuration  C.S-.a + S . S „ a + C„a 1 23 x 1 23 y 23 z  L  -  2  (6.35)  two columns  into  6  3  Expanding out  >  i.e.,  a r e the f i r s t  ^.  '  (6.32), (6.33), we can show  not f l i p p e d  £5  6  <  gives wrist  -ir t  (  +  s u b s t i t u t i n g x^, y^  =  -  where  so that  5  x., y, —4 —4  5  t  *^  s u b s t i t u t i n g 3 ^ , _V_ and £ ^ i n t o  and  9  ±  43  — J  —  cos 8 = y • o o — J —  y,. i s the second column o f H. . —5 -0,5 c  o u t H Q ,. and s u b s t i t u t i n g  Multiplying  y^ i n t o  (6.40),  ( 6 . 4 1 ) we c a n  show .  -1 r - l 4 - l 2 3 4  ) n  <- l 4- l 23 4  ) o  (  fl  6  L  S  S  C  C  C  C  C  C  S  S  x  +  x  +  ( C  1 4 ~ l 23 4 C  S  C  S  ) n  < l 4- l 23V°y C  C  S  C  +  S  y  +  S  4 23 S  4 23°z S  ...  n Z  }  J  *  (  '  - 44 -  6.2.2.3 Discussion of PUMA Solutions When we inspect and  the joint angle solutions, we note that joints 1, 3  4 have 2 solutions  robot  configurations  each.  These solutions correspond  which y i e l d  to d i f f e r e n t  the same t o o l p o s i t i o n H  For  rt  —0, o  instance, tion,  the arm may be i n a left-handed  or right-handed  configura-  the elbow may be above or below the t o o l , and the wrist may be  flipped.  This suggests that different configurations might be tried when  searching  for a feasible configuration.  In order  between points, however, we must i n s i s t  that  to obtain smooth motion  the same configuration be  maintained throughout a given t r a j e c t o r y . 6.2.3  Comparison of Five Degree of Freedom and Six Degree of Freedom Robots  A  6 degree  of freedom  robot  allows  tool position and orientation i n space. we note that  us to completely  specify the  In the case of welding, however,  the tool i s symmetric about i t s axis.  This means that any  rotation about the tool axis does not change the e f f e c t i v e tool orientation.  Thus we see that we need only  single orientation vector a_.  specify the tool position and a  Five degrees of freedom are s u f f i c i e n t and a  6th degree of freedom i s 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 i n the  following analysis for a PUMA robot with the 4th joint omitted.  6.2.3.1 Let  us  Solution for a Five Degree of Freedom Robot now  define  a  5  degree  of freedom  robot  which i s  - 45 -  geometrically and dimensionally previously analysed at  i d e n t i c a l to the 6 degree of freedom PUMA  except that the 4th joint (wrist) i s now r i g i d l y fixed  6 = 0 . 4  We can redefine the link parameters as follows:  Table 6.2 - Five Degrees of Freedom PUMA Link Parameters  a  link  1  -90  a  d  0  0  2  0  a  2  d  3  0  a  2  0  4  90  0  5  90  0  2  0 d  5  Let us also define a tool r e l a t i v e to the last l i n k mounting transformation matrix  0  H  0  flange  -5,  1  d  (6.43)  x  "5,6 0  1  0  d  -1  0  0  d  0  0  0  y z 1  It can be shown that we can achieve any location and orientation of the tool axis Z  e  —o  i f Z, i s perpendicular —o  to Z . c  — J  F i g u r e 6.3  T o o l Frame R e l a t i v e to Robot T o o l Mounting  Flange.  - 47 -  S o l v e f o r t o o l frame F, r e l a t i v e to F  The Denavit-Hartenberg  6  matrices are: C9 S 9  1  1  0  S 9  0  C 9  0  1  0  -1  0  0  0  0  0  1  C 9  2  S9  2  (6.44)  0  - 1  -S9  2  0  a C9  2  C9  2  0  a S9  2  2  2  0  0  1  d  0  0  0  1  2  C9  3  -S9  3  0  a C9  3  S9  3  C9  3  0  a S9  3  3  3  0  0  1  0  0  0  0  1  C 9  4  S 9  4  0  S 6  4  0 0  0  (6.45)  0  1  0  0  0  0  0  1  (6.46)  (6.47)  - 48 -  ^4,5  ce  5  se  5  -se  5  0  0  0  0  0  0  1  d  0  0  0  1  0  0  1  d  0  1  0  d  -1  0  0  d  0  0  0  1  0  0  0  -1  0  c.  0  0  0  1  (6.48)  5  X  (6.49)  y z  The i n v e r s e m a t r i c e s a r e : -1  So,i  1 0 -S 1 0  (6.50)  -  =  2  C  S6  S  0  0  0  0  = "  =  "  S  3  S  3  C  3  -  0 1  0  "  2  D  0  1  0 _  0  0 0  0  0  0  1  0  0  1  0  0  0  0  1  0  0  0  0  4  0  "  C  0  4 0  = S  C  0  4  5 5 0  1 D  0  0  0  1  (6.52)  3  A  1  S  (6.51)  2  A  0  0  S  2  -  0  4  C  0  2  C9  2  49  5  ~  (6.53)  ~~  (6.54)  - 50 -  The i n t e r m e d i a t e  H, , -^,6  0  0  -1  0  1  0  -d  1  0  0  -d  0  0  0  (6.55)  x  transformations are:  0  0  1  0  1  0  -1  0  0  0  0  0  0 0 -1 0  -s, 5  J  0  (6.56)  C d -S d 5 x 5 y c  J  5 0  c  S,d +C d 5 x 5 y c  d  z  +d 5 c  (6.57)  - 51 -  -S,  ^3,6  - 4 5 C  S  -s s 4  0  5  '5  C  4 5  S  4 5  (6.58)  C.(C d -S d ) 4 5 x 5 y c  C  c  +s (d 4  C  z +  d ) 5  -c (d +d ) 4  5  z  5  S d +C_d 5 x 5 y r  0  Let d  s  0  0  (6.59)  = d +d . 5 z c  52.6  " 3 4  S  3 4  " 3 4 5  " 3 4  C  3 4  " 3 4 5 "  C  S  S  S  C  C  C  S  C  C  S  S  S  3 4 5 C  C  +  C  5 0  S  (6.60)  -S_[S.(C,d - S„d ) - C.d ] + a C . 3 4 5 x 5 y 4 s 3 3 0  S„[C.(C\d - S.d ) + S,d ] 4 s 3 4 5 x 5 y' J  L  C  1 S  S  J  L  3 4 5 1 S  3 4 5  C„[C.(C.d - S.d ) + S.d ] 4 s 3 h 5 x 5 y  C  1 S  C  S  0  3 4 5 " 3 4 5 1 C  S  '5  0  C  3 4 5  S  S  0  C  +  +C„[S,(C,d - S.d ) - C,d ] + a_S. 4 x 3 3 3 4 5 x 5 y J  1  S d + C d 5 x 5 y c  1  1  IL , i s broken down i n t o —1,6  c  columns.  —  - 52 -  «1,6  ( 1 )  "  c2[-c3s4  s3c4] - s2[-s3s4 + c3c4]  s2[-c3s4  s3c4] + c2[-s3s4 + c3c4]  (6.61)  0 0  c2[-c3c4s5 + s3s4s5] - s2[-s3c4s5 - c3s4s5] s2[-c3c4s5 + s3s4s5] + c4[-s3c4c5 - c3s4s5]  »1,6< > 3  C  2 3 4 5 - s3s4c5] - s2[s3c4c5 + c3s4c5]  S  S , C C + 0,8,0,.] 2 3 4 5 - S S C ] + C 2["3 3"4 5 4 5  tC  [C  C  C  C  C  3  4  5  0  l  A  q  J  - 53 -  *1,6<  •  =  4)  • •  ]-s [ 2  2  2  2  ] +a S  2  2  ] + c[  s[  ] +a C  2  (6.64)  S d + C d + d„ 5 x 5 y 2 c  c  [ n, o, a, P ] .  ^0,6  (6.65)  Let  So|l So,6  f (n)  f (o)  f (a)  f (P)  f (n)  f (o)  f (a)  f (P)  f (n)  f (o)  f (a)  f (P)  0  0  0  1  n  u  1 2  1 2  1 3  1 3  u  1 2  1 3  u  (6.66)  1 2  1 3  where f (A)  =  f (A)  =  -A  f (A)  =  " S ^ + C ^  n  1 2  1 3  C  l  A z  x  +  S  l  A  y  ,  (6.67) (6.68)  , ,  (6.69)  and A i s n, o, a or P. -1 We a l s o know H„  n  6^, 6^, 9^.  H_ , = H., ,, which we have found as a function of 9„.  — U , J. —U,D — 1 , 0 Equating elements we find  Z  - 54 -  0 C  We  5  S  5  d  2  =  -S.,n + C.n 1 y 1 X  =  -5,0 1 1  + X  1 y  X  1 y  5 x  d  5  ,  (6.71) (6.72) =  y  -S.P + C..P 1 x 1 y  know the p o s i t i o n P_ and a x i s  f o r the remaining  d. 2  =  Rewrite  as  d  2  t o o l o r i e n t a t i o n v e c t o r s n, o, as a f u n c t i o n (6.71) by d  multiply  (6.72) by d  subtract  -S. [P - d o l x y x  - d  i  S  a  +  C  1  We  now  solve  of a, P.  ,  y  x  (6.71) & (6.72) from  L  =  (6.73)  o r i e n t a t i o n a_ of the t o o l .  Multiply  and  (6.70)  a x x  J  (6.73).  ] + C. [p o -- d a ] D - d 1 yy yy vy xx vy L  .  (6.74)  J  (6-75)  B  where  and  a  =  P  x  - d o - d a y x x x  3  =  P  y  - d o - d a y y x y  a  =  r cos <> j ,  r  =  / a  3  =  r s i n <t> ,  •  -  tan"  v  (6.76) ' (6.77)  Let  Substitute  sin  and  + 1  3  2  (-|) .  ,  (6.78), (6.79) (6.80), (6.81)  f o r a, 3 i n (6.75).  <|> cos 8  - cos <J> s i n 8^ d  or  2  sin(<{> - 9 ^  =  =  —  (6.82)  2  —  (6.83)  - 55 -  cos(<|>- 9 )  ± J  =  d T T 1 "  1  v  .  (6.84)  (r— )  We can then show  6  =  tan (|) - 1  tan'M——== r  - d  )  (6.85)  2  where a  =  P x  d o - d a y x x  x  p  =  P y  d o - d a y y x  y  2  2  r Notice  =  that  a  ,  (6.86)  ,  (6.87)  2 + 3  •  (6.88)  9^ i s a f u n c t i o n  The s o l u t i o n i s s i m p l i f i e d  o f j>, which i s what we a r e t r y i n g to f i n d .  i f we i n s i s t  Now a  =  3  =  P  X  - d X  X  a  that d = 0. y  ,  (6.89)  P - d a . y x y We can s o l v e f o r 9^ s i n c e we know P, a_.  (6.90)  From (6.70) we know n We  also  gonal .  y  =  n t a n " 9. x 1  know t h a t  1  .  the v e c t o r s  (6.91) are unit vectors  and that  they a r e o r t h o -  Thus, n  2 2 2 + n + n x y z  =  0  (6.92)  and n a + n a + n a x x y y z z  = 0  .  (6.93)  - 56 -  Combining and solving for n^, we get . -2n „ 7  ,. 2 2 2. 2 . 2,, 2, 2 . 2. 2. J./2 a a ± |(4n a a ) - 4(a + a )(n (a + a ) - a ) x y x x y y z x x z z _ _ .(6.94) 2 (a + a ) y z ' +  x  r  L  J  v  Combining (6.91) and (6.94) we get e a  1/2  2  n  =  x  ±  -  e  5  ~  2  r  tan 9 + 2e tan 9, a a + e(a + a ) 1 1 x y x z  ]  (6.95)  where  e  = a + a 2  y  (6.96)  2  z  '  v  and n Choose  z  =  ±  v l -  2  n  - n x  2  .  y  the s i g n of n^ which gives  are two a n t i p a r a l l e l solutions for ja.  (6.97) '  v  a_ • ii = 0 . Note that there  We can now find o simply as  o_ = a_ x n .  (6.98)  We could now find e x p l i c i t 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 to give  9^ = 0.  tion Hc^", J  PUMA  solution i s guaranteed  I f we post multiply H^ ^ by the inverse tool  we get the t o o l mounting  transforma-  flange l o c a t i o n and orientations,  ,o  i.e.,  «0,5  =  This i s equivalent case.  «0,6 «5*6 to  '  <'"> 6  ^ = [n, j3, a_, P J for the 6 degree of freedom  Thus the 6 degree of freedom solution for this H_ , w i l l y i e l d the —U, o  -  5 degree of freedom j o i n t  6.2.3.2  For specify other  a  ASEA I R B 6 0  5  degree  57  -  angles.  Solution  of freedom  robot  the t o o l l o c a t i o n jp and j u s t  such  one t o o l  as the ASEA  IRB60,  we can  frame b a s i s v e c t o r , a. The  b a s i s v e c t o r s n, o_ a r e determined by P and a. We can s o l v e f o r n, o as we d i d p r e v i o u s l y f o r the 5 degree of f r e e -  dom  PUMA  modify  solution.  the 6 degree  The complete o f freedom  tool  PUMA  frame  i s then known.  We can  s o l u t i o n s by s u b s t i t u t i n g the ASEA  l i n k parameters and s o l v i n g f o r the angles g i v e n HR  As we n o t e d rotation other  [ H>  i n Section  9 ^ = 0 .  joint  "  j T  £»  6.2.3.1,  Thus,  a n g l e s as  £»  ]  1  •  this  ^ i s guaranteed to g i v e a w r i s t  i f we n e g l e c t 9^ : = 9^,  (6.100)  the 9^ s o l u t i o n and r e a s s i g n the  9^ := 9^  we can generate the s o l u -  t i o n s f o r the ASEA I R B 6 0 . The r e s u l t s of S e c t i o n 6 . 2 . 3 . 1 can be s i m p l i f i e d noting  that  f o r an ASEA I R B 6 0 by  = 0.  From ( 6 . 8 5 )  we g e t  P  - d a (6.101)  X  f o r the f o l l o w i n g t o o l  X  X  transformation: 0  0  0  1  0  0  -1  0  0  d  0  0  0  1  X  (6.102)  - 58 -  From (6.91), (6.95) and (6.97) we can calculate n as a function of a_, 9^.  1/2  (a + a ] a ^ y z z Z  Z  1  (a +a )tan 9 + 2 (a +a )a a tan 9, + (a^+a ) fa +a 1 y z l _ ^ y z ^ x y 1 y z ^ x z - _ Z  Z  v  n  =  n  = ±  n  v  tan  ,  9,  (6.104)  _ The s i g n of n  z  21  1 - n - n x y  i s chosen to give  z  , (6.103)  Z  y  a n t i p a r a l l e l solutions for n.  (6.105)  a • n = 0. — —  Generally  lead to a feasible set of joint  Note that  there  are two  only one of the solutions w i l l  angles.  We can now find o_ from (6.98), i . e . o The  joint  components  =  a *n  solutions  can  be  of n, o_, a, p. Into  found  by  the joint  plugging  angle  the  equations  appropriate from  Section  6.2.2.2. The equations can be s i m p l i f i e d f o r the ASEA IRB60 by noting that the l i n k parameters  , a^ and the wrist rotation 9^ are zero.  Rename the j o i n t  angles,  9  4  : =  9  5 •  9  5 = 6 * :  6  The resulting solutions are as follows: p 6  = tan  _  = tan  1  -1  ( J  - d  a  - / J ) d a x x  (6.106)  .  2, 2, 2 .2 2 l 2 3" 4" 2 2 " ~zT,~z2 ,z2.2z.z a/2v .(. ,2 2 2.~ z, _ l 4 ~ l 2"^3 4~ 2 ' q  ±  4 d  a  + q  ( q  2  + q  + q  d  a  a  )  (6.107)  - 59 -  -1 9  2  =  d  d  t a n  4  3 s +a c  4  3  - tan  -1  2  . ( 4 ±  =  9.  4  tan  -1  l 23 x  C  C  a  +  S  l 23 y c  i_c 1  =  tan  where c. = cos i  -1  9  i '  s  0  c,n 1 y  -s,o + 1 x  c,o 1 y  + d  , 2 2 N1/2 +a -q ) J 4 2 a  s  2  ,  (6.108)  3  3  ~ 23 z  a  s ,a + s s ,a + — 1 23 x 1 23 y  -s,n + 1 x  d  a  (6.109)  c.,a 23 z  (6.110)  s. = s i n 9 i i '  c. . = cos ( 9 + 9 . ) ij i ]  s  l t  i]  = sin(9+9.). i y  The l i n k parameter values for the ASEA IRB60 are: a  2  =  800  a  3  =  1150  =  200 mm + z component of the tool  d,.  6.2.3.3 It Thus, we  is  mm, mm,  Redundancy of the Sixth Joint for Welding  important  to  note  that  the  welding  the  ( F i g . 6.4).  torch We  axis  does  not  Any  is  axisymmetric. the orienta-  a r b i t r a r y rotation of the torch frame  change  the  e f f e c t i v e torch  orientation  find that only 5 degrees of freedom are needed to define a  position P_ and a single orientation vector a_. two  torch  need only specify the position of the torch t i p and  tion vector of the torch a x i s . about  transformation.  basis vectors  ri, o_, of F^  joint angles and of P and  a.  are  We  can show that the other  then determined as a function of the  - 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 is  d e f i n e d by the other 2).  This means that we can e x p l i c i t l y rotate F  c  6  about the torch axis ji without (Fig. of  6.4).  changing the tools e f f e c t i v e orientation  We w i l l use this trick, i n searching f o r feasible solutions  a t r a j e c t o r y 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  S p e c i f i c a t i o n of Robot Coordinates  The position of a robot may be uniquely specified two ways:  The  1.  a l l joint angles specified ( e x p l i c i t ) ; and  2.  position and orientation of tool frame s p e c i f i e d .  first  case  is t r i v i a l  once we have solved f o r the joint angles.  The second case requires transformation of the tool frame s p e c i f i c a t i o n F 6 £  into position and orientation coordinates for the target robot. We know F  =  G  [ »» £ . £» I  ]  »  (6.111)  where n, o, a —  and  P  —  are basis vectors of F  —  projected onto F , R  (J  i s the position vector of the F  —  O  o r i g i n r e l a t i v e to F . R  For example, f o r the PUMA 560 robot we must specify coordinates X, Y, Z, 0, A, T F^ r e l a t i v e  where X, Y, Z are the Cartesian coordinates of the tool frame to the robot base F  angles of F, about the axes of F^. —G  —R  and 0, A, T are three Euler rotation  - 61  F i g u r e 6.4  -  R o t a t i o n of T o o l Frame About T o o l A x i s .  - 62 -  Calculations  of the robot  position  and o r i e n t a t i o n  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 l o c a t i o n s .  6.2.4.1  C a l c u l a t i o n of PUMA Tool Location and Rotation Coordinates  The  PUMA robot  the t o o l center,  defines  l o c a t i o n s by the Cartesian coordinates of  X, Y, Z, and three Euler angles  o, a, t , the o r i e n t a -  t i o n , a l t i t u d e and t o o l angles ( F i g . 6.5). For o, a, t angles a l l equal to zero, the t o o l frame F^, i s r e l a t e d 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 p o s i t i o n vectors of the o r i g i n of F  r e l a t i v e to F . 1 R  X, Y, Z  do not change with angles o, a, t .  The rotated t o o l frame F^ i s found by performing an Euler angle rotat i o n sequence t , a, o to f i n d the transformation MR where  = T  rot(o,3) r o t ( a , l ) r o t ( t , 2 )  ^.  (6.113)  ,  rot(o,3) means rotate about z axis ( i , ) of F by an angle o, e t c . —j  rot(o,3)  R  cos o  -sin o  0  0  sin o  cos o  0  0  0  0  1  0  0  0  0  1  (6.114)  - 63 -  Figure 6.5  D e f i n i t i o n of o, a, t angles for the PUMA Arm.  - 64 -  rot(a,1)  rot(t,2)  =  0  1  0  0  0  cos a  -sin a  0  0  sin a  cos a  0  0  0  0  (6.115)  1  cos t  0  sin t  0  0  1  0  0  =  -sin t  0  0  0  cos t 0  (6.116)  0 1  Multiplying out we get  V  -C S -S S C o t o a t  C C -S S S o t o a t t  S C o a  -S S +C S C o t o a t  S C +C S S o t o a t  -C C o a  Y  -C S„ a t  -S a  Z  t  (6.117) -C C a t  0 where cos(o), S o  If  =  sin(o), e t c .  a = - 90° , rotations  o, t  are c o a x i a l .  In this case we w i l l a r b i -  t r a r i l y set t = 0. To f i n d  the angles o, a, t from a known t r a n s f o r m a t i o n H„  m  —R, T  we  proceed as follows: We know that where  x  = [ VN, VO, VA, VP ]  (6.118)  - 65 -  VN  X  VP  Y  =  VN  =  ]  VN  , etc.  2  Z  VN,  1  0 Premultiply both sides of (6.118) by r o t ( o z )  \  >  rotto.z)"  E^-  1  =  rot(o,z)  [VN, VO, VA, VP ] .  _ 1  (6.119)  Multiplying out and equating elements we get 0  -sin(o) VA - cos(o) VA  (6.120)  1  cos(o) V0  cos(t)  + sin(o) V 0  ±  (6.121)  2  sin(t)  -cos(o) VN, - sin(o) VN.  (6.122)  sin(a)  -VA„  (6.123)  cos(a)  sin(o) VA, - cos(o) VA,  (6.124)  From these equations we get  o  = tan  _  i  -  V  A  o = o + 180  1 V A  a  i  f  1  2 -VA„  -1 = tan  sin(o) VA -cos(o) VA 1  _  -1  r L  For  (6.125)  (6.126) 2  -cos(o) VN -sin(o) VN„ 1  i  I  cos(o) V O ^ sin(o) V 0  J  (6.127)  2  the PUMA, the angle t i s defined as the negative of the usual Euler  rotation convention.  I f we change the sign of t to conform to the PUMA  d e f i n i t i o n we get: cos(o) VN, + sin(o) VN„ t  =  tan [ _ 1  1  2  cos(o) V0 + sin(o) V 0 1  ]  2  Also the following i s true: (o, a, t)  =  (o + 180,  180 - a,  t + 180).  (6.128)  - 66 -  6.3  C o l l i s i o n Avoidance In order  to successfully generate a robot  essential that we  program o f f - l i n e ,  i t is  be able to r e l i a b l y avoid c o l l i s i o n s between the robot  and objects i n the workstation.  This i s a d i f f i c u l t problem which i s the  subject of much current research. To  handle  metric  this  problem our  descriptions of  the  world  robot  and  model must contain  objects i n the workstation.  geometric models can be provided by CAD are  usually  represented  composite of sentation  a  primitive s o l i d  i s the  specifying  by  polyhedral  a l l of  Kakishita 1974;  systems i n several forms.  description of  elements.  The  their  Wesley et a l 1980].  edges  and  surfaces,  a  faces  solid  The Solids  or  most common surface  model which defines  i t s vertices,  complete geo-  as  repre-  polyhedron  [Hosaka,  a  Kimura  by and  The resulting description i s simple,  uniform and unambiguous. More advanced CAD  systems use constructive s o l i d geometry to generate  complex solids by performing  boolean operations  on a set of simple  primitives such as boxes and  cylinders [Braid 1975;  Requicha 1980].  solid The  resulting representation must contain a record of the composite primitives as well as  the  sequence of operations.  More complete surveys  representations can be found i n the l i t e r a t u r e 1979;  of  solid  [Baer, Eastman and Henrion  Requicha 1980]. The  Perez  collision  [Brady  algorithm are: space.  et  avoidance problem has been well summarized by Lozanoal  1982].  hypothesize  The  three  classes  of  collision  avoidance  and test, penalty functions and e x p l i c i t free  - 67 -  The hypothesize and test algorithm generates a c o l l i s i o n free path by t r i a l and error. formulated  and  Each time a c o l l i s i o n i s detected an alternate path i s  tried.  This method  optimum solution and may of  i s simple but does not  lead  to an  not find a solution at a l l because the knowledge  the world i s l o c a l i z e d only and thus a global strategy i s impossible. The  whose  penalty function  value  position  reflects  i n space.  scheme attempts  the The  probability  to define a penalty function  of  collision  as  a  function  algorithm then generates a path following  l o c a l minima of the penalty function.  of the  This method has been successfully  applied to extremely simple world models only, and the algorithm becomes very complex for more r e 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 c o l l i s i o n s  and  calls  this  set the free  space.  The  shortest path through the free space joining an i n i t i a l and f i n a l position i s then the optimum path.  The main d i f f i c u l t y l i e s i n generating the free  space, and a l l algorithms to date incorporate s i m p l i f i c a t i o n s and approximations which r e s t r i c t their generality.  Several free space path planning  algorithms have been presented i n the l i t e r a t u r e Perez and Wesley 1979; Lozano-Perez The  problem  of  simply  1983; Brooks  detecting  Lozano-  1983].  collisions  objects i n the workstation i s a simpler problem  [Udupa 1977;  between the robot  to tackle at this point.  However, a feasible path must then be found i t e r a t i v e l y or by t r i a l error.  and  and  We w i l l not consider the alternate path selection problem, other  than to note that i n the case of welding we must reposition the workpiece i f a c o l l i s i o n i s detected.  - 68 -  The  simplest interference detection problem i s the problem of i n t e r -  ference  between  fixed, stationary  method for completely convex  polyhedra.  determining  objects.  Hosaka  [1974] describes  specifying the i n t e r s e c t i o n between two Maruyama  [1972] has  proposed  intersections between polyhedral  a  simple  a  stationary method  for  solids using a h i e r a r c h i c a l  set of tests. The was  problem of interference between moving and  considered  the simple  by Boyse [1979].  The  cases of pure translation and  interferance  Schwartz [1981]. polyhedra  between  two  moving  rotation and  motion. convex  cannot be extended  An algorithm for testing  polyhedra  i s suggested  of  dimensional  His method finds the minimum distance between two convex  time.  Schwartz  demonstrates  the  algorithm  the  two  of handling interference detection between moving objects  i s to define a swept volume generated by the moving object. then reduced  Wesley  for  case only.  Another way  solids.  by  as a function of time i f their i n d i v i d u a l t r a j e c t o r i e s are known  functions  is  polyhedra  tests which he devised apply only to  to the more general case of complicated for  stationary  to  the  simpler  case of  The problem  interference between stationary  This method i s discussed by Requicha [1980] and Lozano-Perez and [1979].  We  will  develop a detailed interference detection algo-  rithm based on the swept volume method.  6.3.1  World Model  Any accuracy  world and  polyhedral  model  conciseness  representation  embodies  a  trade-off  for solids of a r b i t r a r y shapes.  representation we  can  choose any  If we  between use  a  number of surface facets to  - 69 -  approximate  curved  however, we w i l l of  surfaces  generally  to any degree  of a c c u r a c y .  use a c o a r s e approximation  the data s t r u c t u r e and i n c r e a s e  In p r a c t i c e ,  t o reduce the s i z e  the speed of any p r o c e s s i n g  we wish to  do. Unfortunately, uncertainty off  our c o l l i s i o n  the s i m p l i c i t y  reliability for  into  any s i m p l i f y i n g approximations that we make  predicted  o f our mathematical  of the r e s u l t i n g c o l l i s i o n  the worst  predicted.  case,  however,  where none o c c u r .  purpose The  world  model  general  approach  We w i l l c o n s i d e r sets  of o b j e c t s ,  Stationary Although  solids  trade  a g a i n s t the  We can b i a s our model  a l lcollisions  will  p r o b a b i l i t y of c o l l i s i o n s  being  as i n w e l d i n g .  will  concepts  be  c o n s i d e r a t i o n i f we  be a simple  one f o r  the  of i n t e r f e r e n c e  detection.  can be a p p l i e d  to a more  be d e s c r i b e d  world model and i n t e r f e r e n c e d e t e c t i o n  the robot  solids include  the t a b l e  we must  a world model which d i v i d e s the w o r k s t a t i o n  moving  moving s o l i d s i n c l u d e  the b a s i c  which w i l l  s o p h i s t i c a t e d and accurate  that  i n the w o r k s t a t i o n , adopt  that  and a l g o r i t h m  can be an important  which we w i l l  o f demonstrating  find  predictions.  to ensure  This  We  model  The p r i c e we pay i s a h i g h e r  must move near the o b j e c t s The  predictions.  introduce  and s t a t i o n a r y  solids  scheme. i n t o two  ( F i g . 6.6).  m a n i p u l a t o r and a l l p a r t s  attached  The  to i t .  the workpiece, p o s i t i o n i n g t a b l e , f i x t u r e s , e t c .  may be movable we w i l l  assume that  i t s motion  cannot  cause i n t e r f e r e n c e . Stationary polyhedra.  A  objects polyhedral  i n the w o r k s t a t i o n representation  will  allows  be modelled our model  as  solid  to have any  -  70  -  Initial  Position  sition  F i g u r e 6.6  World Model  - 71 -  degree  of  accuracy  we  desire,  and  allows  a  simple  data  structure of  v e r t i c e s , plane faces and straight edges. For s i m p l i c i t y , we w i l l model the robot links as c i r c u l a r  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  contains the l i n k (see F i g . 6.7). to  chosen such  that the cylinder  fully  Each cylinder w i l l be defined r e l a t i v e  the corresponding l i n k coordinate frame.  6.3.2 As  Development of Swept Volume the robot links move they sweep out a volume of space, as shown  i n F i g . 6.6.  If we can a n a l y t i c a l l y describe that volume, we can test i f  the volume intersects any fixed s o l i d i n the workstation. In order to simplify the algorithm and minimize the required calculations  we  will  concise way.  adopt  a method  for approximating  the  swept volume  in a  To i l l u s t r a t e the method, we w i l l consider the motion of one  c y l i n d r i c a l s o l i d through Suppose we  space.  have a cylinder  of  radius R and  length L.  It has  a  central axis or spine with endpoints A and B.  If the cylinder i s moved  from  position  an  initial  position  Pi_ to  some  other  volume has been swept out by the cylinder. smooth  and  continuous  between  Pi  and  If we  P2,  we  P2,  then  assume the motion  can  use  spine  are  some was  the following  approximation for the swept volume. The straight initial  initial lines and  and  final  A]A2> B]_B2«  final  spine  endpoints These  positions,  of  the  line  segments,  connected along  with  with the  define a ruled, parametric surface.  The surface has a simple, non-linear parametric equation of the form  -  F i g u r e 6.7  72  -  C y l i n d r i c a l Approximation  of a M a n i p u l a t o r L i n k .  - 73 -  r (A , A ) = r + A + Av + A A (u - u ) —p u v —o u—o v—o u v M. —o  (6.129)  U  where r —o If 0 £o»  < A  v  , u —o we  ,  v —o  ,  u, —1  restrict  < 1, we  iLl> Zo» Z l  are defined as shown i n F i g .  the  ranges  of  the  obtain a surface patch a s  s  n  o  w  n  1 1 1  6.8.  0  can  define  two  each side.  To do t h i s , we define the corner points of the new surface  normals  of  the  parent  now  surfaces  the  <  \^  <  1,  or region bounded by the vectors We  from this  to  parametric  finding  offset  Fig*  parameters  6.8.  parent  bounding  surface by a distance R on  surface at  surfaces by  each corner,  and  r e l a t i v e to  the  translating along those normals by distances ± R. The  two  offset  bounding  surfaces  can  be  expressed  parent surface by the following equation: r (A , A ) = r —b u v —p u  where  R  (A , A ) ± R n( A , A ) u v — u v  (6.130)  i s the cylinder radius,  A , A are the surface parameters, u v ii  i s the unit surface normal of the parent surface,  r —P  i s any point on the parent  surface  s a t i s f y i n g equation (6.129). The unit surface normal i s defined as follows: £  =  V X V |7 x 71 '—u -v  (6.131)  1  3r  where  7 = ^2. —u aA u  =  u  —o  + \  v  (  —1  u  -  u  )  —O  (6.132)  3r  V = —v 3A  =  v + A ( - u ) . —o u —1 —o Ul  (6.133)  F  l  8  U  f  e  6  '8  Para , m  S w e  Pt  Volume.  - 75 -  If  we expand  these  equations  and c o l l e c t  the bounding surfaces can be expressed  the terms, we find  by equations  of the form  r = A + A B + A C + A A D —b — u— v— u v— where  A , B_ , C_ , D_  (6.134)  are known.  Thus, the bounding surfaces are also ruled, parametric Given  these  that  two bounding  surfaces,  we  surfaces.  can e a s i l y  derive  similar  surfaces over each of the four open faces of the volume between the bounding  surfaces.  two  faces  Only two of these surfaces need to be found since the other  are bounded by the c y l i n d r i c a l  surfaces  of the l i n k  in its  i n i t i a l and f i n a l p o s i t i o n . The  interference  between the polyhedra  detection  problem  representing  i s now to find  intersections  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 i n t e r s e c t i o n occurs. In practice, we would generate swept volumes for each moving link of interest.  The motion of the manipulator  Intermediate  motions, with  would be divided into a series of  each intermediate  motion generating  a swept  volume.  6.3.3  Interference Conditions  We can formulate five simple i n t e r s e c t i o n conditions (see Fig.6.9): 1.  2.  Any s o l i d  polyhedral  parametric  surface.  edge  intersects any swept  volume  bounding  Any polyhedral edge Intersects the c y l i n d r i c a l s o l i d of a l i n k i n i t s i n i t i a l or f i n a l p o s i t i o n .  F i g u r e 6.9  Interference  Conditions.  - 77 -  3.  Any swept surface edge intersects any s o l i d polyhedral surface.  4.  The c y l i n d r i c a l  volume of any l i n k i n i t s i n i t i a l  or f i n a l position  intersects any polyhedral face. 5.  Any polyhedral s o l i d i s wholly enclosed by any swept volume. The interference algorithm now consists of testing f o r each of these  cases f o r every possible combination A hierarchy of tests occur,  of s o l i d polyhedra and swept volumes.  can be devised  as shown i n F i g . 6.10.  to determine i f intersection  Each test  will  now  will  be considered i n  detail.  6.3.3.1  Condition 1 - Intersection of an Edge and a Parametric Surface ( F i g . 6.11)  I f we are given 4 points i n space, P_^, P_ , P^, P^, we can define a ruled, parametric surface with the following equation: r — where  = P —1  + A u u —o  u = P_ - P, —o —2 —1  ,  »1 " *3 " *4  '  + A v v —o  + A A (u - u ) u v — 1 — o  ,  .... (6.135)  A , A are scalar parameters, u v Also,  i f we are given  the end points of a l i n e  segment, E j , JJ2,  we can define a l i n e with the following equation: r  =  E. + A _ 1  where  u^, =  E  u _  E  (6.136)  - E_^  Combining (6.135), (6.136) we get the following system of equations:  - 78 -  Any s o l i d p o l y h e d r a l edge i n t e r s e c t s any swept volume bounding p a r a m e t r i c s u r f a c e .  Yes  No  Any s o l i d p o l y h e d r a l edge i n t e r s e c t s the l i n k c y l i n d e r at i t s i n i t i a l or f i n a l position.  Yes  fr-  No  Any swept volume edge i n t e r s e c t s any p o l y h e d r a l face  Yes  No i  L i n k c y l i n d e r i n t e r s e c t s any p o l y h e d r a l 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 v e r t e x l i e s swept volume  i n s i d e the  Yes  ••  t No I n t e r s e c t i o n  F i g u r e 6.10  Algorithm for Interference  <  Intersection  Detection.  -  Figure  6.11  Intersection  79  -  of an Edge and  a Parametric  Surface.  -  80  -  X + X - Xu + X X ( u -u ) + P - E = u—o v—o E—E u v —1 —o —1 —1 u  This  system  of non-linear  Newton's method.  6.3.3.2  line  iteratively  (6.136). R.  joining  C^  i s defined  The c y l i n d e r i s d e f i n e d  Ci and £2 d e f i n e and C 2 d e f i n e s  1 and i s r e p r e s e n t e d  as f o r C o n d i t i o n  by two p o i n t s  the ends  the c e n t r a l  The ends o f the c y l i n d e r a r e f l a t  common  (see  Fig.  normal and i n t e r s e c t i o n p o i n t s  C^ and C J J ,  o f the c y l i n d e r and axis  or  spine  the  o f the  and normal to the s p i n e .  The edge and the spine l i e on two skewed l i n e s i n space. a  using  6.12)  the edge  a radius  cylinder.  solved  C o n d i t i o n 2 - I n t e r s e c t i o n o f an Edge and a C y l i n d e r  Suppose  and  can be  0<X <1 a t the i n t e r s e c t i o n p o i n t , E ^  (Fig.  equation  equations  (6.137)  The edge i n t e r s e c t s the s u r f a c e w i t h i n i t s boundaries i f  0<X <1 0<X <1 , u ' v '  by  0  v  J j . » 12  o  n  t  n  e  edge  We can f i n d and s p i n e  6.12).  The common normal and d i s t a n c e between two skewed l i n e s as f o l l o w s .  Suppose the two l i n e s have the f o l l o w i n g  L i n e 1 (edge) where  r = E, + A u —1 E —E  —  ^E  Line 2 (spine)  r  where  u  = =  can be found  equations: (6.138)  E  -2  C, + A u s —s  We can show t h a t the c l o s e s t d i s t a n c e D i s g i v e n by  (6.139)  - 81 -  F i g u r e 6.12  I n t e r s e c t i o n of an Edge and  a  Cylinder.  - 82 -  The  points  distance  on  are  each  found  line by  lying  on  substituting  the the  common  normal  following  at  the  closest  parameter values  into  equations (6.138), (6.139): X  X  E  = <£i  " V  *  s  " (iX  " £ ) x  iig  •  (6.141)  * iis  •  (6.142)  Let us c a l l these points P„,  P  —E  —s  Pg l i e s on the edge i f 0 < X  E  < 1 .  P l i e s on the spine i f 0 < X < 1 . —s s We  can  cylinder.  Case 1  now  define  four  different  cases of  an  edge intersecting  a  Each case i s handled d i f f e r e n t l y .  Pg l i e s on the edge, and Pg l i e s on the spine.  In this case, intersection occurs i f the minimum distance (6.140) i s less than the radius  Case 2  D given by  R.  Fg l i e s on the edge, and P  fi  does not l i e on the spine  In this case, i f the edge intersects either end i t Intersects  the cylinder.  face of the  This can be tested in several  1.  Test i f edge intersects face plane of cylinder.  2.  If step 1 i s true, find intersection point.  3.  Find distance  4.  If distance  cylinder  steps:  from spine to intersection point.  i s 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 i s less  than the radius R,  intersection occurs. i.e. i f  I 1  or  E - P —1 —s  I  < R  1  II "^ I  < R  2  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 i s treated the same as case 2.  6.3.3.3  Condition 3 - Intersection of an Edge and a Polyhedral Face (Fig.6.13)  Let defined  us define an edge by equation as the planar  planar vertices,  (6.136).  A polyhedral  region bounded by an n-sided  face i s  polygon with  n co-  , V^, ... V^.  The face i s a region of a known plane with the following equation: r_ • n - Q = 0 where  n i s the unit surface normal, Q i s the closest distance to the o r i g i n .  The edge i s defined by equation (6.136), i.e.  r = E, + X_ u_ — —1 E —E  where  u  = E„ - E, .  (6.143)  Figure  6.13  I n t e r s e c t i o n of an Edge and  a Polyhedral  Face.  - 85  Combining (6.136) and parameter  (6.143) we  get  the  following  Q - E  The  We  can  1  = _  ^  • ri = 0,  exists.  • n  •  the  —  the  .  n  line  is parallel  to  the  plane and  edge intersects the face plane i f 0 < Ag now  intersects the  formulate  a  series  of  tests  to  intersection  <1. i f the  edge  face.  Test i f the edge intersects the face plane.  2.  If  (1)  is  true,  generate  intersection point P_£ If the  nt  and  ray crosses the  intersection has  6.3.3.4  no  determine  1.  3.  equation for  :  X If uj  -  any  ray  originating  from  the  lying on the face plane.  face boundaries an odd  number of times,  occurred.  Condition 4 - Intersection of a Cylinder and  a  Polyhedral Face It i s possible  for a cylinder  to intersect a face without any  face edges intersecting the cylinder.  There are two  possible  of  the  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  -  I n t e r s e c t i o n of C y l i n d e r End Face and P o l y h e d r a l  Face.  - 87 -  Case 1  This test i s the same as for intersection between an edge and a face, as previously described.  Case 2  In this cylinder  case, we  face center  will  find  the shortest distance  from the  to the i n t e r s e c t i o n l i n e of the cylinder  face plane and polyhedral face plane.  I f the distance i s less  than the radius R, i n t e r s e c t i o n occurs. Let us define a vector R_' i n the cylinder end face R* = u —  where  U —s ii  x (u x n) S  ~~  = =  (6.145)  """"  C„ —2  -  C, , spine d i r e c t i o n vectors, —1  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 i n the cylinder end faces, one i n each end; £  x  =  C +  £  2  =  £  A R  ,  (6.147)  and  We  now  test  + 2  X  -  '  i f either  (6.148) line  intersects  the  polyhedral  I n t e r s e c t i o n occurs i f -1 < A < 1 and i f the i n t e r s e c t i o n point P —int inside the polyhedral face boundaries. J  face. lies  - 88 -  6.3.3.5  Condition 5 - Any  Polyhedron  i s Totally  Enclosed  by a  Swept Volume Bounded by Parametric Surfaces We  can now  write an equation f o r a parametric swept volume i n the  following form: r(A,A,A)=r(A,A) + ARn, U V w —1 U V w —  —  0 < A <1, u  (6.149)  0 < A < 1 , v -1 < A < 1 , w where  R , = P + A u + A v + A A (u, - u ) —1 — u—o v—o u v —1 —o  and  n  where  V —u  =  V —u  x  V  V —u  x  V —v  — V  V are as d e f i n e d i n equations —v  words,  (6.132),  (6.153).  In other  i s any point on the parent parametric surface and ri i s the unit  surface normal. Let  n*  =  —  V  x  — u  V  (6.150)  — v  This can be written as n*  =  —  L —  +  AM u—  +  AN  (6.151)  v—  We can rewrite (6.149) as r —  =  r, + A n' , -a < A < oc , —1 w— w R where a = ,—p. Expanding and rearranging, r = A + A + A L + A A (n -u ) + A A M + A A N + P — u—o v—o w— u v —1 —o w u— w v— — u  V  (6.152)  ,  (6.153)  - 89 0 < A u  < 1  ,  0 < A v  < 1  ,  -a The  < A w  < a  result  i s a system of n o n - l i n e a r  equations  which can be  solved  i t e r a t i v e l y u s i n g Newton's method. If a l l previous of the  i n t e r s e c t i o n t e s t s have been f a l s e , and i f any v e r t e x  a polyhedron l i e s  within  e n t i r e polyhedron  lies  the swept volume, then a l l v e r t i c e s and thus within  the volume.  It i s sufficient  to t e s t  any s i n g l e a r b i t r a r y v e r t e x of the polyhedron. The method i s as f o l l o w s : 1.  S u b s t i t u t e any v e r t e x  2.  f o r r and s o l v e f o r A , A , A . — u v w F i n d the s u r f a c e normal _n' and i t s l e n g t h |n'| .  3.  The v e r t e x i s i n s i d e the volume i f 0  <  A u  <  1 , and  0  <  A v  <  1 , and  \f\ 6.4  w  <  ijh •  <  I n t e r f e r e n c e D e t e c t i o n Demonstration Program TESTIN The  tests  implemented polyhedral easily  for  and  in a face the  algorithms  simple  against more  test a  program,  single  general  described  swept case  i n t e r s e c t i n g a number of swept volumes.  above  were  combined  TESTIN.  TESTIN  tests  volume.  TESTIN  can be  of  several  many-sided  a  and  single  expanded polyhedra  - 90 -  6.4.1  Functional Description of TESTIN  TESTIN follows rotates  a  single  generates a swept  the logic shown i n Fig.6.11. an  initial  to a  TESTIN translates and  cylinder  from  final  position and  volume.  The user s p e c i f i e s the cylinder  length and  radius and the i n i t i a l and f i n a l positions as specified by two homogeneous transformations. and  The user inputs the number of vertices on the test face,  their coordinates.  TESTIN then tests for intersections of the face  and the swept volume.  6.4.2 This  L i s t and Description of a l l Subroutines used by TESTIN section  contains  a  complete  list  subroutines used by the interference detection subroutine links are given. with asterisks (*).  Tektronix  and  descriptions  of  all  test program TESTIN. A l l  IGL graphics  subroutines are noted  - 91 -  Subroutines used i n TESTIN: CORNER  CYLEND  CYLINT  DET2X2  DET3X3  DIFF  DOTPRD  DRCYL  DRPINT  DRVOL  ECROSS  EDGCRS  EDGINT  ELIST  FACE  FACINT  FACYL  GENVOL  INITSV  INPROJ  INSIDE  INVOL  LINK  MATDET  NEWTON  NWTVOL  ONRAY  OPEN  PLANE  ROTK  SHUT  SOL3X3  SOLVE  SURFIN  SWPVOL  TCROSS  TESTIN  TRANS3  UNIT  VCROSS  VLEN  VRTCRS  XPROD  XPROD2  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 BYCYLINT:  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 FROMDRCYL:  CYLINT,FACE,FACYL,PLANE,TCROSS  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 FROMDRVOL:  FACINT, FACYL  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 FROMEDGINT:  INSIDE  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 FROMFACE:  INSIDE  allows the user to i n t e r a c t i v e l y 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  f o r intersection  between  a cylinder  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  CALLS- OPEN,SHUT CALLED FROM- SWPVOL  parameters  end face and a  - 94 -  INSIDE:  tests i f an intersection point l i e s within the boundaries of a  CALLS-  face ECROSS,ELIST,VCROSS  CALLED FROM- FACINT INVOL:  tests i f a point l i e s inside a parametric swept volume  CALLS- NWTVOL,VLEN CALLED FROM- TESTIN LINK:  calculates the i n i t i a l and f i n a 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  solving  a  system  of a l i n e  of non-linear  and a swept surface equations  iteratively  by using  Newton's method CALLS-  SOL3X3,VLEN  CALLED FROMNWTVOL:  solves  SURFIN a system  of non-linear  Newton's method to find  i f a point  volume CALLS-  SOL3X3,VLEN  CALLED FROMONRAY:  INVOL  tests i f a point l i e s on a ray  CALLS-  NONE  CALLED FROM- VCROSS  equations lies  iteratively inside  using  a parametric  - 95 -  PLANE:  tests i f a l i n e segment intersects a plane  CALLS-  DOTPRD  CALLED FROMROTK:  FACINT  finds a rotation matrix to rotate about some given d i r e c t i o n 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 l i n e a r equations  CALLS- MATDET CALLED FROM- NEWTON,NWTVOL SOLVE:  finds the common normal of two skewed lines  CALLS-  NONE  CALLED FROMSURFIN:  CYLINT  finds the intersection of a l i n e and a parametric surface  CALLS-  NEWTON,CMCLOS*,CMOPEN*,DRAW3D*,M0VE3D*  CALLED FROM- TESTIN SWPVOL:  generates  the volume swept out by a c y l i n d r i c a l l i n k moving  from an i n i t i a l to a f i n a 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 l i e s 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 FROMVLEN:  INSIDE  finds the length of a vector  CALLS-  NONE  CALLED FROM- CYLEND,CYLINT,DRCYL,NEWTON,NWTVOL XPROD:  finds the normalized  CALLS-  NONE  CALLED FROMXPR0D2:  cross product of two vectors  DRCYL,FACE,FACYL,TCROSS  finds the cross product  CALLS-  NONE  CALLED FROM-  6.4.3  of two vectors  CYLINT  Demonstration of TESTIN  The interference detection program TESTIN was tested by defining a swept  volume  cylindrical interference.  generated  link,  from  and then  an i n i t i a l simulating  The c y l i n d r i c a l  and a  final  situations known  link was defined  position  of a  to result i n  to have a length of 30  - 97 -  units  and a radius of 5 units.  along  the X-axis,  The i n i t i a l cylinder  starting at the o r i g i n .  defined by a translation of 30 units 45° about the Y-axis.  spine position lay  The l i n k f i n a l  position  was  along the Y-axis, and a rotation of  The resulting wire-frame depiction of the resulting  swept volume i s shown i n 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 In  (10, 15, 15), (10, 30, 15), (10, 10, -20).  this case at least one edge of the triangle  parametric surface of the swept volume.  intersects  at least one  TESTIN terminates as soon as an  intersection i s found, so other intersections may exist as well. Fig. vertices this  6.16 shows the swept volume and a rectangular region defined by (40, 10, 20), (40, 10, -20), (-20, 10, -20), (-20, 10, 20).  case,  an edge of a parametric  surface  intersects  In  the rectangular  region. Fig. 20),  6.17  shows a rectangular  region defined  by vertices  (40, -2, -20), (-20, -2, -20), (-20, -2, 20).  (40, -2,  In this case, the  cylinder intersects the rectangular region. Fig.  6.18  shows the swept volume resulting  with no rotation.  from a pure translation  An edge of the rectangular region intersects a para-  metric surface which i n this case i s planar. All examples.  five  intersection  The graphical  cases were tested  depiction  and v e r i f i e d with  was not very clear  however, so they have not been reproduced here. TESTIN, interference  numerous  for most of them,  During the running of  messages appear i n 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.  -  F i g u r e 6.15  98  -  I n t e r s e c t i o n C o n d i t i o n 1 between Face and Swept Volume  - 99 -  F i g u r e 6.16  I n t e r s e c t i o n C o n d i t i o n 3 between Face and Swept Volume.  - 100 -  F i g u r e 6.17  I n t e r s e c t i o n C o n d i t i o n 2 between Face and Swept Volume.  -  Figure  6.18  101  -  I n t e r s e c t i o n C o n d i t i o n 1 between Face and Swept Volume Generated by Pure T r a n s l a t i o n .  - 102  6.5  P o s s i b l e Improvements to the A l g o r i t h m We  the  solid  can  easily  polyhedra  which are normal if the  -  find  the  and  swept  smallest  bounding  volumes.  to the world c o o r d i n a t e  any bounding boxes i n t e r s e c t .  Further  I f we  boxes choose  containing boxes  each  with  axes, i t i s v e r y simple to tests  of  faces test  need o n l y be conducted on  p o l y h e d r a and swept volumes whose bounding boxes  intersect.  - 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, f o r automatically generating a robot program f o r welding.  The software package was developed on a VAX 11-750 computer running under the  VMS  (IGL)  operating system.  software  package  The Tektronix Interactive  provided  Graphics  the graphics routines.  Library  A DEC VT-101  terminal f i t t e d 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 v i s u a l editor for program development.  The software package, AUTOP, currently requires about 100 K  of memory, and this w i l l increase as AUTOP i s expanded.  7.1  Design Approach Since  essential  the software  to c a r e f u l l y  initially,  package  formulate  and to develop  was  to be  quite  extensive  i t was  some design guidelines and objectives  the software  i n a planned,  structured manner, following good programming practice.  consistent and The software was  developed with the following goals i n mind.  a)  Portability  The System  software was designed  dependent  possible.  to be as system  sections of code  independent  have been c l e a r l y  as possible.  identified,  where  No system l i b r a r i e s are c a l l e d , and no external subroutines are  - 104 -  called,  other  than  the T e k t r o n i c s  IGL g r a p h i c s  subroutines.  Standard  hierarchical  structure.  FORTRAN i s f o l l o w e d as c l o s e l y as p o s s i b l e .  b)  Modularity  The  software  was  designed  with  Each f u n c t i o n a l module has e x p l i c i t l y passed  v i a argument l i s t s ,  a modular,  d e f i n e d i n p u t s and o u t p u t s .  common data b l o c k s and f i l e s ,  Data i s  depending on the  requirements.  c)  A d a p t a b i l i t y and G e n e r a l i t y  The of  software  was designed  to be e a s i l y  p o s s i b l e w o r k s t a t i o n implementations.  modules  are  parameters the  as  being  geometry  general read  Wherever  possible,  workstation  and dimensions  e a s i l y changed without  7.2  from  as  configured  with  possible,  specific  description  of the robot  t o a wide  files.  and p o s i t i o n i n g  range  the software implementation F o r example, table  can be  changes to the s o f t w a r e .  F e a t u r e s and C a p a b i l i t i e s o f AUTOP The  specific  features  and  capabilities  of  AUTOP  at i t s current  s t a t e of development are summarized below: a)  AUTOP i n p u t and output  data a r e s t o r e d i n f i l e s .  b)  Initial  system  files.  T h i s a l l o w s easy m o d i f i c a t i o n of parameters.  c o n f i g u r a t i o n parameters a r e s t o r e d i n and read  c)  I n t e r a c t i v e g r a p h i c s i s used  d)  AUTOP generates  a welding  f o r communications w i t h the u s e r .  procedure  i n t e r a c t i v e l y with the u s e r .  from  - 105 -  e)  Automatic  i t e r a t i v e searching f o r kinematically feasible torch paths  i s incorporated. f)  An  Expert  values.  Welder  automatically selects  welding  parameter  This module was developed separately and i s mentioned  for completeness.  7.3  module  here  For d e t a i l s , refer to Buchal et a l [1984].  General Functional Description of AUTOP It  details  i s not p r a c t i c a l of AUTOP.  to describe completely  The software l i s t i n g s  such a description would be largely description, AUTOP.  actual  are extensively  redundant.  documented so  A simplified  functional  however, i s useful i n understanding the basic operation of  The  approximate  a l l of the functional  following  order.  description  No attempt  will  flow of control of AUTOP.  outlines  the major  functions i n  be made to rigorously  The simplified  follow the  software structure i s  shown i n Fig.7.1. AUTOP i s designed to interface with a separate CAD system which w i l l generate the CAD data used by AUTOP. test  CAD f i l e s  programs. The  No CAD system i s currently used, so  are generated both manually  and by s p e c i f i c user-written  The data formats which we have specified are closely followed. CAD f i l e s  contain workpiece  paths, as well  as the workpiece  into  seam data  processed  files  normal vectors at each point.  weld  geometry.  parameters  and d i g i t i z e d seam  AUTOP converts the CAD data  containing calculated  seam tangents and  - 106 -  CAD Data Process workpiece data i n c l u d i n g welding data  workstation configuration data  E x p e r t Welder data base  Interactively specify welding procedure, including table p o s i t i o n s , seam o r d e r and weld d i r e c t i o n s , s u b j e c t to t e s t s o f Kinematic f e a s i b i l i t y  processed workpiece seam data  table position and f i x e d welder s e t t i n g s f o r each seam  robot c o o r d i n a t e s and v a r i a b l e welder s e t t i n g s f o r each seam  F i g u r e 7.1  Software S t r u c t u r e and Data Flow of AUTOP  - 107 -  Workstation  and  graphics  enviroment  the parameters are i n i t i a l i z e d .  data  are  read  from  files  and  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 and  determines  instructed  i f the current setup  to reset  the welder  setup, scans a l l the seam data f i l e s i s appropriate. If not, the user i s  to the settings determined  by the Expert  Welder module. AUTOP draws the workpiece and change the table position and  seams on  the eye  'picks' the seams to be welded.  the screen.  position.  The user  The  user  interactively  AUTOP then automatically searches for a  kinematically feasible robot path to follow the seam.  If a path i s found,  the table position and feasible robot configuration are recorded. coordinate  list  i s generated  for the  seam and  present  the current and  speed  can  file.  appropriate welding  as voltage, current, speed, etc. for the seam.  implementation,  A robot  stored i n a data  Also, the Expert Welder module i s called to determine parameters such  can  vary along  In the  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 i s f i l l e d i n graphically on the screen. and  picking  seams u n t i l  The operator continues moving the workpiece  a l l the seams have been processed.  The basic  operation of AUTOP i s summarized i n F i g . 7.2.  7.3.1 This  L i s t and Description of a l l Subroutines c a l l e d from AUTOP section  lists  and  briefly  describes  the  function of a l l  - 108 -  START 4»  Draw workpiece and seams•  Draw welded  seams.  Yes  P i c k next seam to weld.  T e s t a l l c o n f i g u r a t i o n s u n t i l f e a s i b l e path i s found. Rotate t o o l about a x i s to s e a r c h f o r f e a s i b l e s o l u t i o n .  no  Save seam number, t a b l e p o s i t i o n , r o b o t c o n f i g u r a t i o n i n a procedure r e c o r d f i l e .  C a l c u l a t e and save robot c o o r d i n a t e s 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  F i g u r e 7.2  Simplified  Flowchart of AUTOP.  - 109 -  subroutines separated  called  into  subroutines noted with  i n the software  general  are l i s t e d .  functional  package AUTOP. categories  A l l Tektronix  and  The s u b r o u t i n e s are a l l links  'IGL' g r a p h i c s  to  subroutines  asterisks(*).  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 DOTPRD:  Operations  finds  CALLS-  the dot product  NONE  CALLED FROM-  DRPANL  of two v e c t o r s  other are  - 110 -  HMULT:  multiplies  two  homogeneous  transformations  in  double  precision CALLS-  NONE  CALLED FROMHMULTS:  TABPOS.WORPOS  l i k e HMULT, i n s i n g l e  CALLS-  NONE  CALLED FROMMATCNV:  precision  WORPOS  converts  double  precision  s i n g l e p r e c i s i o n and v i c e CALLS-  versa  WORPOS  NONE  CALLED FROM-  INIT  performs r o t a t i o n a l  p a r t o f a homogeneous t r a n s f o r m a t i o n  point CALLS-  NONE  CALLED FROMTRANS:  DRPANL,DRSEAM  performs a homogeneous t r a n s f o r m a t i o n on a p o i n t  CALLS-  NONE  CALLED FROMUNIT:  into  i n v e r t s a homogeneous t r a n s f o r m a t i o n m a t r i x  CALLS-  ROTAT:  transformation  NONE  CALLED FROMMATINV:  homogeneous  DRPANL,DRSEAM,DRWELD  normalizes a vector CALLS-  to u n i t  NONE  CALLED FROM-  DRSEAM,DRWELD  length  on a  - Ill  XPROD:  -  performs a v e c t o r c r o s s product  CALLS-  NONE  CALLED FROM-  DRSEAM,DRWELD,LINDIS,NOAP5,PROCES,WORPOS  G r a p h i c a l Output DRAXES:  draws the world axes  CALLS-  MOVE3D*,DRAW3D*  CALLED FROMDRPANL:  SCENE  draws the workpiece  CALLS-  DOTPRD,LINDIS.ROTAT,TRANS,APPEAR*,DRAW3D*,MOVE3D*, PANL3D*,POLY3D*,REMOVE*  CALLED FROMDRSEAM:  SCENE  draws the seams  CALLS-  NORM,OPEN,RDFILE,ROTAT,SHUT,TRANS,UNIT,XPROD, APPEAR*, CLOSEG*,DASHPT*,DRAW3D*,MOVE3D*.OPNSEG*.REMOVE*,SETDET*  CALLED FROMDRWELD:  SCENE  draws the welds  CALLS-  NORM,OPEN,RDFILE,SHUT,TRANS,UNIT,XPROD,APPEAR*,CLOSEG*, DRAW3D*,MOVE3D*,OPNSEG*,REMOVE*,SETDET*,SETHIL*  CALLED FROMEYELOC:  SCENE  chooses eye p o s i t i o n through user input  CALLS-  NONE  CALLED FROM-  MAIN  -  SCENE:  draws complete g r a p h i c a l  CALLS-  112 -  scene  DRAXES,DRPANL,DRSEAM,DRWELD,TABPOS,TABSET,CMCLOS*, CMOPEN*,DELSEG*.EYEBAL*, NEWPAG*,VRP3D*  CALLED FROM-  MAIN  Kinematics ARCLEN:  finds  the a r c l e n g t h  parameter  's' f o r a  point  on a  seam  trajectory CALLS-  NONE  CALLED FROMCONFIG:  changes  PROCES robot  configuration  until  a  feasible  trajectory i s  found CALLS-  WORPOS  CALLED FROMJOINTS:  calculates and  CALLS-  tool  joint  angles  f o r a given  WORPOS their physical  limits  NONE  CALLED FROMfinds  WORPOS  the PUMA 560 l o c a t i o n  l o c a t i o n and o r i e n t a t i o n CALLS-  configuration  position  checks the robot j o i n t angles a g a i n s t  CALLS-  N0AP1:  the robot  NONE  CALLED FROMLIMITS:  MAIN  NONE  CALLED FROM-  WORPOS  parameters g i v e n the d e s i r e d  tool  - 113 -  N0AP5:  finds the  the 5 degree of freedom PUMA l o c a t i o n parameters  desired  tool  location  and the o r i e n t a t i o n  given  of i t s c e n t r a l  axis CALLS-  XPROD  CALLED FROMNORM:  WORPOS  f i n d s the seam normal  CALLS-  NONE  CALLED FROMTABPOS:  DRSEAM,DRWELD,PROCES  calculates the  CALLS-  table  the t a b l e joint  frame r e l a t i v e to the world  frame g i v e n  positions  OPEN,SHUT,HMULT  CALLED FROMTABSET:  vector  allows  SCENE the user  to  interactively  specify  new  table  joint  positions CALLS-  OPEN,SHUT  CALLED FROMTANGEN:  calculates  CALLS-  at each p o i n t  along a seam  PROCES  performs a r o t a t i o n about the t o o l a x i s  CALLS-  NONE  CALLED FROMWORPOS:  the seam tangent v e c t o r  NONE  CALLED FROMTOLROT:  SCENE  WORPOS  generates welding parameters, robot l o c a t i o n data f o r a weld  CALLS-  JOINTS,HMULT,HMULTS.LIMITS,MATCNV,NOAP1,NOAP OPEN,RDFILE,SHUT,TOLROT,XPROD  CALLED FROM-  CONFIG  5,  - 114 -  Expert Welder CALC:  calculates the welding current correction factors as a function of the gravity vector  CALLS- TRANS CALLED FROMEXPERT:  finds  EXPERT  correct  welding  parameter  welder setup and workpiece CALLS-  values  given  the i n i t i a l  parameters  CALCS,OPEN,SHUT,WIRE1,WIRE2,WIRE3  CALLED FROM- AUTOP WELSET:  checks i f i n i t i a l welder setup i s appropriate for a given job and recommends changes i f necessary  CALLS-  OPEN,RDFILE,SHUT  CALLED FROM- AUTOP WIRE  calculates welding parameters CALLS-  NONE  CALLED FROMWIRE2  EXPERT  calculates welding parameters for .035 i n . wire.  CALLS-  NONE  CALLED FROMWIRE3  for .030 i n . wire  EXPERT  calculates welding parameters  CALLS-  NONE  CALLED FROM-  EXPERT  for .045 i n . wire  - 115 -  Data Management CADFIL:  converts  object  geometry f i l e s  into  internal  arrays  for faster  processing CALLS-  OPEN,RDFILE,SHUT  CALLED FROMINIT:  MAIN  initializes CALLS-  MAIN  opens a FORTRAN CALLS-  constants  MATINV,OPEN,RDFILE,SHUT  CALLED FROMOPEN:  global  file  NONE  CALLED FROM-  MAIN,CADFIL,DRSEAM,DRWELD,INIT,PROCESS,TABPOS, TABSET,WORPOS  RDFILE:  reads a g i v e n r e c o r d from a f i l e  CALLS-  NONE  CALLED FROMSHUT:  MAIN,CADFIL,DRSEAM,DRWELD,INIT,WORPOS  c l o s e s a FORTRAN CALLS-  file  NONE  CALLED FROM-  MAIN,CADFIL,DRSEAM,DRWELD,INIT,PROCES, TABPOS,TABSET,WORPOS  Other CHECK:  checks i f a picked  CALLS-  NONE  CALLED FROM-  MAIN  seam has been picked  previously  - 116 o  LINDIS:  finds  which  of two  lines  i s further  away  as viewed  from  the  c u r r e n t eye l o c a t i o n CALLS-  XPROD  CALLED FROMPROCES:  DRPANL  processes  the i n i t i a l  CAD  seam data  files  into  a more  useful  form. CALLS-  ARCLEN,NORM,OPEN,SHUT,TANGEN,XPROD  CALLED FROMSETUP:  SETUP  manipulates  CALLS-  files  f o r p r o c e s s i n g by PROCES  OPEN,PROCES,RDFILE,SHUT  CALLED FROM-  AUTOP  Functions C a l l e d LENGTH  f i n d s the l e n g t h of a c h a r a c t e r s t r i n g  CALLED FROM-  7.4  Data S t r u c t u r e s AUTOP  makes  interchange. they CAD each  RDFILE  extensive  changed  i s also  seam h a v i n g  because  a separate  file.  storage  i n a linked  Output  through list  and  where  the e d i t o r .  s t r u c t u r e with  parameters and robot  location  as w e l l . arrays  the  are  files  f o r data  parameters a r e s t o r e d i n f i l e s ,  in files,  use of i n t e r n a l data  files  by AUTOP or by the user  stored  data are s t o r e d i n f i l e s The  of e x t e r n a l  System i n i t i a l i z a t i o n  can be e a s i l y seam data  use  f o r temporary  large,  r e q u i r e s a l l a r r a y s to have a f i x e d  and  of  data  storage  variable  i s minimized  length.  s i z e , d e f i n e d at compile  time.  FORTRAN Arrays  - 117 -  must  be dimensioned  memory.  f o r the l a r g e s t  Instead, f i l e  whenever  which wastes  r e c o r d s are read d i r e c t l y and processed  possible.  increased  c o n c e i v a b l e requirement,  The  flexibility,  penalty  of  simplified  slower  data  data  access  structures  immediately  i s offset  and  reduced  by  memory  requirements. The in  largest  unformated  The  small  format  files,  form  system  f o r ease  the CAD and processed  f o r a substantial  parameter of e d i t i n g  fication. is  completed,  7.4.1 The files by  format  are stored  space.  i n list-directed  The robot l o c a t i o n data f o r debugging  files  and ease of v e r i -  program g e n e r a t i o n and downloading  should be changed to unformatted  module  form.  D e s c r i p t i o n of a l l I/O F i l e s and t h e i r Formats following  is a  complete  list  accessed by the program AUTOP.  the VAX  (i.e.  these f i l e s  files  and debugging.  When the automatic  are stored  reduction i n required disk  and setup  are a l s o c u r r e n t l y i n l i s t - d i r e c t e d  seam data f i l e s ,  computer  and d e s c r i p t i o n  The DEC f i l e  requires a l l f i l e  of a l l the data  naming convention  names t o have  the e x t e n s i o n  used .DAT  NAME.DAT).  FILIST:  List  CADLST:  List  of f i l e names used of names  of CAD  i n a particular data  files  implementation.  corresponding  t o each  seam  (i.e.CADDAT). CADDAT: WPCLST:  List  File  c o n t a i n i n g CAD data f o r a p a r t i c u l a r seam.  of names of processed  seam ( i . e .  CAD data f i l e s  c o r r e s p o n d i n g to each  WPCDAT).  WPCDAT:  F i l e c o n t a i n i n g processed CAD d a t a f o r a seam.  ROBLST:  List  of names of robot seam l o c a t i o n data f i l e s  ( i . e . ROBDAT).  - 118 -  ROBDAT:  File  c o n t a i n i n g robot  l o c a t i o n s and welding  data f o r a  seam path. OBJLST:  List  o f names  of f i l e s  containing  geometric  description  of  objects i n workstation. OBJDAT:  File  c o n t a i n i n g p o l y h e d r a l geometric  s p e c i f i c a t i o n of  an o b j e c t . WKSTAT:  File  c o n t a i n i n g complete s p e c i f i c a t i o n o f robot and welder.  TABLE:  File  c o n t a i n i n g s p e c i f i c a t i o n of w o r k t a b l e .  GRAPH:  F i l e c o n t a i n i n g parameters f o r g r a p h i c s system.  LOCSET:  File  STOVAR:  Storage  WPARAM:  Temporary weld parameter storage  The  c o n t a i n i n g w o r k s t a t i o n setup file  f o r current table  contents and formats  f o r each seam to be welded.  position. file.  of each of these d a t a f i l e s f o l l o w .  FILE NAME LISTS  FILIST.DAT:  a s s i g n s names to a l l the f i l e s accessed by AUTOP TABLE 7.1 RECORD NO.  -  F i l e Name L i s t  FILIST  CONTENTS  1  CADLST.DAT  2  WPCLST.DAT  3  WKSTAT.DAT  4  TABLE.DAT  5  STOVAR.DAT  6  GRAPH.DAT  7  OBJLST.DAT  8  R0BLST.DAT  9  LOCSET.DAT  10  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 F i l e L i s t CADLST RECORD NO. 1 2 3  WPCLST:  CONTENTS  DESCRIPTION  *TEXT 5  TEXT HEADING NUMBER OF SEAMS  CADI.DAT  SEAM 1 CAD FILE SEAM 2 CAD FILE  4 5  CAD2.DAT  6 7  CAD4.DAT  SEAM 3 CAD FILE SEAM 4 CAD FILE  CAD5.DAT  SEAM 5 CAD FILE  CAD3.DAT  Contains a l i s t of processed seam f i l e s . TABLE 7.3 - Processed Seam F i l e L i s t WPCLST RECORD NO. 1 2 3 4 5 6 7  ROBLST:  CONTENTS *TEXT 5 WPC1.DAT WPC2.DAT WPC3.DAT WPC4.DAT WPC5.DAT  DESCRIPTION TEXT HEADING NUMBER OF SEAMS PROCESSED SEAM 1 DATA FILE PROCESSED SEAM 2 DATA FILE PROCESSED SEAM 3 DATA FILE PROCESSED SEAM 4 DATA FILE PROCESSED SEAM 5 DATA FILE  Contains a l i s t of robot location f i l e s f o r each seam TABLE 7.4 - Robot Location Data F i l e L i s t ROBLST RECORD NO. 1 2  CONTENTS *ROBOT XYZOAT COORDS. PUMA1.DAT  DESCRIPTION TEXT HEADING SEAM 1 ROBOT DATA FILE SEAM 2 ROBOT DATA FILE  3 4  PUMA2.DAT  5 6  PUMA4.DAT  SEAM 3 ROBOT DATA FILE SEAM 4 ROBOT DATA FILE  PUMA5.DAT  SEAM 5 ROBOT DATA FILE  PUMA3.DAT  - 120 -  OBJLST:  Contains  a list  of geometric  CAD f i l e s  f o r objects  in  world model. TABLE 7.5 - Object Geometry Data F i l e L i s t OBJLST RECORD NO. 1  CONTENTS CYLIND.DAT  DESCRIPTION GEOMETRY OF WORKPIECE GEOMETRY OF WORK TABLE  2 3  WKTAB.DAT TOOL.DAT  4  F0RARM.DAT  GEOMETRY OF ROBOT FOREARM  5  UPARM.DAT  GEOMETRY OF ROBOT UPPER ARM  GEOMETRY OF ROBOT TOOL  SEAM DATA FILES  TABLE 7.6 - Seam CAD F i l e RECORD NO. 1 2 NPOINT+1  CONTENTS JTYPE,XSECT,TI,T2,NPOINT,ICURVE INDEX,R(3),N1(3),N2(3)  DESCRIPTION WELDING PARAMETERS TRAJECTORY DATA  INDEX,R(3),N1(3),N2(3)  TRAJECTORY DATA  JTYPE:  joint type code  XSECT:  weld cross sectional area  T1,T2:  parent metal thicknesses  NPOINT:  number of points i n f i l e  ICURVE:  trajectory type s p e c i f i e r 0:  straight  line  1:  space curve  INDEX:  location  index  R(3):  position vector of current location  N1(3),N2(3):  adjacent  surface normal vectors  the  - 121 -  TABLE 7.7 - Processed RECORD NO.  Seam Data  File DESCRIPTION  CONTENTS  1  JTYPE,XSECT,T1,T2,NPOINT,ICURVE  SEAM  2  INDEX,S,R(3),T(3),A(3),B(3)  SEAM DATA  INDEX,S,R(3),T(3),A(3),B(3)  SEAM DATA  NPOINT+1  JTYPE,XSECT,T1.T2,NPOINT,ICURVE:  as d e f i n e d f o r CAD  PARAMETERS  file  INDEX:  l o c a t i o n index  S:  a r c l e n g t h to c u r r e n t 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 v e c t o r of c u r r e n t 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  position A(3):  u n i t v e c t o r normal to weld  B(3):  B=A X T ( t o g i v e o r t h o g o n a l  surface s e t of b a s i s  TABLE 7.8 - Robot L o c a t i o n Data RECORD NO.  CONTENTS ALPHA,BETA,STKOUT,VOLT,FEED,GFR,  vectors)  File DESCRIPTION SEAM GLOBAL PARAMETERS  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  STKOUT:  torch stickout distance  VOLT:  welding  FEED:  wire  GFR:  s h i e l d i n g gas flow r a t e  NPOINT:  number of l o c a t i o n s i n f i l e  INDEX:  location  X,Y,Z:  robot  tool location (translations)  0,A,T:  robot  tool orientation (rotations)  CURRENT:  welding  current  SPEED:  welding  speed  angle  voltage  feed  rate  index  SEAM LOCAL PARAMETERS  - 122 -  OBJECT GEOMETRY DATA FILES  OBJDAT: F i l e c o n t a i n i n g p o l y h e d r a l model of an o b j e c t i n the work s t a t i o n TABLE 7 . 9 - Object RECORD NO. 2  CONTENTS  NVERT1+1  I:=NVERTl+2  DESCRIPTION  ID1,N0RM1,DIST1,NVERT1  FACE1 HEADER  VERTNO,VERT(3)  VERTEX  ID2,NORM2,DIST2,NVERT2  1+1...I+NVERT2+1 etc.  Geometry Data F i l e s  INDEX,COORDINATES  FACE2 HEADER  VERTNO,VERT(3)  VERTEX INDEX,  COORDINATE  f o r a l l faces i n the o b j e c t  End of f i l e  i s indicated  by a face i d = 0  ID:  f a c e ID number  NORM:  f a c e outward s u r f a c e  DIST:  normal  distance  normal from  face  origin NVERT:  number of v e r t i c e s i n the face  VERTNO:  v e r t e x ID number  VERT(3):  vertex  coordinates  plane  to c o o r d i n a t e  frame  - 123 -  SYSTEM CONFIGURATION FILES  WKSTAT: contains robot and welder specifications TABLE 7.10 - System Configuration F i l e WKSTAT NO. OF RECORDS 1 1  CONTENTS *TEXT HEADER NRDOF  1  *TEXT HEADER  1  *TEXT HEADER LINKNO,ALPHA,LEN,  NRDOF  DIST,MIN,MAX 1 1 1 4 1 4 1 1 1 1 1  DESCRIPTION NUMBER OF ROBOT DEGREES OF FREEDOM  LINK PARAMETERS  *TEXT IC0NF(6) TEXT HOO(4,4) *TEXT H6G(4,4) *TEXT IMAX,DELTA *TEXT *TEXT WSIZE  CURRENT ROBOT CONFIGURATION ROBOT LOCATION REL. TO WORLD TOOL FRAME REL. TO LAST LINK TOOL ROTATION PARAMETERS  CURRENTLY INSTALLED WELDER WIRE SIZE  LINKNO:  l i n k number  ALPHA: LEN: DIST:  l i n k twist angle l i n k length joint displacement  MIN,MAX: ICONF: HOO:  joint l i m i t s current robot default configuration homogeneous transformation r e l a t i n g zeroth robot frame to world frame homog. trans, r e l a t i n g tool frame to last l i n k frame maximum number of rotation steps about tool axis (when searching for a feasible path)  H6G: IMAX:  along i t s axis  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 settings are programmable.  parameter  - 124 -  TABLE 7.11 - Example of F i l e 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 0  0  1  1  11 0  0  *WORLD COORDINATE TO ROBOT COORDINATE TRANSFORMATION MATRIX  12 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: SIZE (DEGREES) 18  10  MAX.No.OF STEPS, ANGLE STEP 23 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 F i l e TABLE NO. OF RECORDS 1 1 NDOF  CONTENTS NDOF *TEXT  DESCRIPTION NUMBER OF TABLE DEGREES OF FREEDOM TEXT HEADER  ID,ITYPE,STEP,RMIN, RMAX,THETA,ALPHA DISP.LEN  1 4 1 4 1 4  TABLE LINK PARAMETERS  *TEXT HTW(4,4) *TEXT HPRE(4,4)  WORKPIECE RELATIVE TO TABLE FRAME FIRST LINK RELATIVE TO WORLD  *TEXT HP0ST(4,4)  TABLE FRAME RELATIVE TO LAST LINK  NDOF:  number of table degrees of freedom  ID:  joint type l=revolute (rotational) 2=prismatic ( t r a n s l a t i o n a l )  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:  l i n k 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 l i n k to world HPOST(4,4):homog. transformation r e l a t i n g table frame to last link  - 126 -  TABLE 7.13 - Example of F i l e TABLE CONTENTS  RECORD NO. 1  3 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 e a s i l y changed TABLE 7.14 - Graphic System I n i t i a l i z a t i o n F i l e GRAPH  NO. OF RECORDS  CONTENTS  DESCRIPTION  *TEXT VRP(3)  VIEW REFERENCE POINT COORDINATES  *TEXT XMIN,XMAX,YMIN, YMAX;ZMIN,ZMAX  VIEW WINDOW LIMITS  TABLE 7.15 - Example of GRAPH CONTENTS  RECORD NO.  *VIEW REFERENCE POINT  1  0  2  0  0  *WINDOW DIMENSIONS, -600.  600.  XMIN,XMAX,YMIN,YMAX,ZMIN,ZMAX  -600.  500.  -600. 600.  3 4  - 128 -  FILE OF WORKSTATION SETUP FOR EACH SEAM  LOCSET:  contains  information  configuration  the  table  position  and  robot  to weld a g i v e n seam  TABLE 7.16 - Workstation NO. OF RECORDS:  about  Setup F i l e  CONTENTS  f o r Current Seam LOCSET  DESCRIPTION  SEAMID  SEAM IDENTIFICATION NUMBER  VAR(6)  TABLE JOINT POSITIONS  IC0NF(6)  ROBOT CONFIGURATION  SEAMID  SEAM IDENTIFICATION NUMBER  VAR(6)  TABLE JOINT POSITIONS  IC0NF(6)  ROBOT CONFIGURATION  SPECIFICATION  SPECIFICATION  ETC.  TABLE 7.17 - Example of LOCSET f o r Seams 5, 1, 3 CONTENTS  RECORD 1  300.0000  600.0000  -90.00000  0.0000000E+00  0.0000000E+00  2  0.0000000E+00 0  0  3  1 300.0000  4 600.0000  -90.00000  0.0000000E+00  0.0000000E+O0  5  0.0000000E4O0 0  0  6  3 300.0000  7 600.0000  0.0000000E+00 0  0  -90.00000  0.0000000E+00  0.0000000E+00  8  - 129 -  CURRENT TABLE POSITION DATA  STOVAR:  c o n t a i n s the c u r r e n t t a b l e 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 *JOINT VARIABLES  RECORD NO. 1  300.0000  2  600.0000  3  -90.00000  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 F i l e 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 - F o r t r a n F i l e FILE NAME  Formats FORMAT  FILIST  LIST-DIRECTED  CADLST  LIST-DIRECTED  CADDAT  UNFORMATTED  WPCLST  LIST-DIRECTED  WPCDAT  UNFORMATTED  ROBLST  LIST-DIRECTED  ROBDAT  FORMATTED  OBJLST  LIST-DIRECTED  OBJDAT  LIST-DIRECTED  WKSTAT  LIST-DIRECTED  TABLE  LIST-DIRECTED  GRAPH  LIST-DIRECTED  LOCSET  LIST-DIRECTED  STOVAR  LIST-DIRECTED  WPARAM  LIST-DIRECTED  - 132 -  TABLE 7.23 - Subroutines Accessing Data F i l e s FILE  7.5  SUBROUTINES  CADDAT  DRSEAM,DRWELD,PROCE S,SETUP  CADLST  AUTOP,DRSEAM,DRWELD,SETUP  FILIST  AUTOP  GRAPH  AUTOP  LOCSET  INIT,WORPOS  OBJDAT  CADFIL  OBJLST  CADFIL  ROBDAT  INIT,WORPOS  ROBLST  INIT,WORPOS  STOVAR  TABSET,WORPOS  TABLE  INIT,TABPOS,TABSET  WKSTAT  EXPERT,INIT,WELSET  WPARAM  EXPERT,WORPOS  WPCDAT  EXPERT,PROCES,SETUP,WESET.WORPOS  WPCLST  SETUP,WORPOS  Graphical Depiction of Solids Using If  we attempt  wire-frame hidden  IGL. solids  depiction, we find  lines  generally  to represent  are removed.  polyhedra  i n a clear  graphically using  the usual  the image ambiguous and confusing Hidden l i n e  not available i n simple  We have developed  Panels  removal i s n o n - t r i v i a l , and i s  graphics  packages such  an alternative method  and unambiguous  unless  as Tektronix  f o r depicting polyhedral  way without  explicit  hidden  line  calculations by taking advantage of the c h a r a c t e r i s t i c s of the Tektronix 4027 graphics terminal.  - 133 -  The graphics  Tektronix  commands.  screen.  PANEL  i s depicted  front,  matically  of i t s face  relative  i s superimposed  panels.  Each panel  determining  on the  already  on the  which  can c o n t a c t  of two panels  along  o f two f a c e  following  a r e superimposed  Hidden  surfaces  from back  a r e thus  auto-  be v i s i b l e . i n the c o r r e c t o r d e r , we need a t e s t f o r i s " c l o s e r " to the v i e w p o i n t .  the f a c e polygons o f a p o l y h e d r a l  which  polygon  i s bounded by a dark v e c t o r border so that  To superimpose the panels  but  filled  on anything  The panels  to the v i e w p o i n t .  covered.  edges w i l l  that  as a  PANEL  We have developed an a l g o r i t h m which d e p i c t s a polyhedron as the  superposition  all  A  A new PANEL image  screen.  to  4027 i s a r a s t e r - t y p e t e r m i n a l which supports  solid  common edges o n l y .  polygons  lies  cannot  We  intersect  We  each  note  other,  can determine  heuristically  " c l o s e r " to the viewpoint  by u s i n g the  principle:  Suppose we have two l i n e s i n space, L^ and L^, and a l i n e of s i g h t vector  V —s  d e f i n i n g t h e view d i r e c t i o n  normal  d i r e c t e d f r o m L^ to L^ •  ( F i g . 7.3). L e t N.„ be the common —12  Then we can say L ^ appears to cross over  from the g i v e n view d i r e c t i o n i f V, We  can now  '  N  u  state  >  0  .  (7.1)  the f o l l o w i n g  algorithm  f o r determining  the  " s t a c k i n g " of f a c e s : 1. Each l i n e c o n t a i n i n g an edge of a f a c e i s compared to a l l other lines the  c o n t a i n i n g edges of a l l other  given  line  defined  i s i n f r o n t of another  f a c t o r i s incremented by 1.  faces.  line,  T h i s i s repeated  Each time  that edges weight f o r a l l edges on  - 134  -  Line Ll  N - common normal Vs -1ine o f s i g h t  F i g u r e 7.3  Relationship  of two l i n e s  t o view p o i n t .  - 135 -  the  f a c e , and the edge weights  weight.  An average  face  a r e summed  weight  to get a t o t a l  i s obtained  face  by d i v i d i n g the  t o t a l f a c e weight by the number of edges.  2. Repeat the above f o r every f a c e .  3. "Stack" average  7.6  the f a c e s  graphically  i n ascending  order  of  their  weights.  User's Guide t o Running Program AUTOP AUTOP i s a p r e l i m i n a r y program o n l y , and does not i n c l u d e a l l the  necessary  capabilities  programming industrial  system.  of  a  complete  and  The f o l l o w i n g u s e r ' s  comprehensive  guide  u s e r , but r a t h e r e x p l a i n s the running  automatic  i s not intended  f o r an  of AUTOP a t i t s c u r r e n t  s t a t e of development.  C o m p i l i n g AUTOP The  main program and a l l the s u b r o u t i n e s  running  the command f i l e  can be compiled  a t once by  C0MPIL.COM. ( i . e . enter @C0MPIL).  L i n k i n g AUTOP All  the compiled  running  o b j e c t modules  the command f i l e  can be l i n k e d  into  a task image by  AUT0P.COM. ( i . e . enter @AUT0P).  Running AUTOP 1.  I f the i n i t i a l  set-up  files  FILIST,  WKSTAT, TABLE and GRAPH  have  - 136 -  not been set up, set them up as described i n Section 7.4.1. 2.  Generate  a pseudo CAD f i l e as described i n Section 7.4.1. Currently  a direct  CAD interface has not been developed.  generates a set of test CAD f i l e s .  The program CADFIL  The corresponding geometric model  of the workpiece has been generated manually i n 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  list  FILIST.  Currently, the name Is FILIST.DAT, i n  correspondence with the DEC data f i l e naming convention.  A different  name can be specified i f the program i s implemented on a system with a different system  file  compatible  naming convention. data  file  Similarly, FILIST.DAT contains  names f o r a l l the data  files  used by  AUTOP. 6.  AUTOP automatically scans a l l the seam CAD f i l e s current welder setup i s correct. to  reset  the welder.  and checks i f the  I f not, the operator i s instructed  The new setup  parameters  are recorded  in a  file. 7.  AUTOP next prompts the user for the eye position. determines  The eye position  the view d i r e c t i o n , directed from the eye position to the  o r i g i n (0.,0.,0.). 8.  AUTOP asks the user i f a new table position i s desired.  I f the user  answers Y (yes), AUTOP prompts for a new position. 9.  AUTOP draws the workpiece, screen.  seams and a set of reference axes on the  The seams are represented by 'herringbone' lines pointing i n  the default weld directions.  -  137 -  10.  AUTOP prompts the user f o r new eye p o s i t i o n and/or t a b l e p o s i t i o n .  11.  I f the eye or t a b l e p o s i t i o n i s changed, AUTOP proceeds from step 7.  12.  When no more changes a r e r e q u e s t e d , AUTOP draws a s e t of c r o s s h a i r s and  prompts  the user  to p i c k  moves the c r o s s h a i r s  the f i r s t  to the d e s i r e d  seam to be welded.  seam and p i c k s  The user  i t by p r e s s i n g any  key. 13.  AUTOP checks the s e l e c t e d feasible,  seam f o r path f e a s i b i l i t y .  the weld i s f i l l e d  in.  I f the path i s  Otherwise, the u s e r i s prompted to  r e p o s i t i o n the t a b l e and t r y a g a i n . 14.  The user all  seams have  robot for 15.  picking  been  configurations,  picked.  seams and r e p o s i t i o n i n g AUTOP  stores  the t a b l e  the t a b l e  until  p o s i t i o n s and  and generates robot l o c a t i o n s and welding  data  each seam.  AUTOP t e r m i n a t e s when a l l the seams have been  7.7  picked.  Demonstration o f AUTOP This  section  presenting For  continues  this  illustrates  the  procedure  followed  a c t u a l n u m e r i c a l data generated by AUTOP d u r i n g demonstration,  we  have  r e s u l t s and contents of data f i l e s  extracted  and converted  by  AUTOP  a test  by  session.  the n u m e r i c a l  i n t o a form s u i t a b l e f o r i n s p e c t i o n and  verification. The  workpiece which we have d e f i n e d  of a c y l i n d e r welded to a b l o c k . equal  length  segments  a l o n g the l e n g t h  around  f o r this  F i v e weld  consists  seams have been d e f i n e d ;  the c i r c u m f e r e n c e  of the c y l i n d e r .  demonstration  of the c y l i n d e r  four  and one  - 138 -  The d i g i t i z e d CAD seam data was generated by a simple FORTRAN program written e x p l i c i t l y for this p a r t i c u l a r workpiece. entered manually into the object geometry f i l e . be  a PUMA 560 and we a r b i t r a r i l y  defined  The geometric data was The robot was defined to  a positioning table with one  revolute and two prismatic degrees of freedom. AUTOP was run, and the workpiece was i n t e r a c t i v e l y repositioned u n t i l a kinematically f e a s i b l e path was found for welding the seam of i n t e r e s t . For this example, we have chosen seam one (see F i g . 7.4). Figs. 7.4, 7.5 and  7.6  show  the appearance  of the workpiece  on  the screen,  and  demonstrate the e f f e c t of changing the table position and the user's view point. The  contents of the CAD seam  data  file  f o r seam one are shown i n  Table 7.24. These data are processed by AUTOP into the more useful form shown  i n Table  automatically  7.25.  For a given  tests d i f f e r e n t robot  seam  and table  position, AUTOP  configurations and rotates the torch  about i t s axis within user-defined  limits  f e a s i b l e path.  Table 7.26 contains  the calculated joint angles f o r each  robot  along  position  rotations.  Note  the seam  that  a number  i n search  of a kinematically  f o r d i f f e r e n t configurations of configurations  were tested  and torch before a  feasible one was found. When a feasible path has been found, AUTOP records the table position and  robot  configuration  for later  reference.  robot location data i n the appropriate  AUTOP then generates the  form for downloading to the robot.  Table 7.27 shows a l i s t i n g of the r e s u l t i n g X, Y, Z, 0, A, T coordinates for  the PUMA, as well as the correct welding current  point.  These location values  and speed at each  have been v e r i f i e d by manually downloading  - 139 -  Seam 5  Seam 1  F i g u r e 7.4  G r a p h i c a l D e p i c t i o n of Workpiece by AUTOP.  - 140 -  F i g u r e 7.5  New  Workpiece P o s i t i o n a f t e r  Changing T a b l e  Position.  Interactively  - 141  F i g u r e 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  1  0.500  NIIEX 1 i  3  4 5  6  7  8  9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26  R<X> 200.000 199.803 199.211 198.229 196.858 195.106 192.978 190.483 187.631 184.433 180.902 177.051 172.897 168.455 163.742 158.779 153.583 148.175 142.578 136.813 130.902 124.869 118.738 112.533 106.279 100.000  Tl 0.250  T2 0.500  NPOINT  ICURVE  26  1  R(Y)  R(Z)  N1(X>  NKY)  NKZ)  N2(X)  N2(Y)  N2<Z)  100.000 106.279 112.533 118.738 124.869 130.902 136.812 142.578 148.175 153.583 158,779 163.742 168.455 172.897 177.051 180.902 184.433 187.631 190.483 192.978 195.106 196.858 198.229 199.211 199.803 200.000  100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100,000 100.000  1,000 0.998 0.992 0.982 0.969 0.951 0.930 0.905 0.876 0.844 0.809 0.771 0,729 0.685 0.637 0.588 0.536 0.482 0.426 0.368 0.309 0.249 0.187 0.125 0.063 0.000  0.000 0.063 0.125 0.187 0.249 0.309 0.368 0.426 0.482 0.536 0.588 0.637 0.685 0.729 0.771 0.809 0.844 0.876 0.905 0.930 0.951 0.969 0.982 0.992 0.998  0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000  0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000  0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000  1.000 1.000 1.000  1.000  1.000 1.000  1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 1.000 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 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  S 0.000 6.282 12.564 18.846 25.129 31.411 37.693 43.975 50.257 56.539 62.821 69.104 75.386 81.668 87.950 94.232 100.514 106.797 113.079 119.361 125.643 131.925 138.207 144.489 150.772 157.054  R(X) 2Q0.000 199.803 199.211 198.229 196.858 195.106 192.978 190.483 187.631 184.433 180.902 177.051 172.897 168.455 163.742 158.779 153.583 148.175 142.578 136.813 130.902 124.869 118.738 112.S33 106.279 100.000  R(Y)  R(Z)  100.000 106.279 112.533 118.738 124.869 130.902 136.812 142.578 148.175 153.583 158.779 163.742 168.455 172.897 177.051 180.902 184.433 187.631 190.483 192.978 195.106 196.858 198.229 199.211 199.803 200.000  100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000 100.000  T(X) -0.031 -0.063 -0.125 -0.187 -0.249 -0.309 -0.368 -0.426 -0.482 -0.536 -0.588 -0.637 -0.685 -0.729 -0.771 -0.809 -0.844 -0.876 -0.905 -0.930 -0.951 -0.969 -0.982 -0.992 -0.9V8 -1.000  T(Y>  T(Z)  MX)  MY)  MZ)  1.000 0.998 0.992 0.982 0.969 0.951 0.930 0.905 0.876 0.844 0.809 0.771 0.729 0.685 0.637 0.588 0.536 0.482 0.426 0.368 0.309 0.249 0.187 0.125 0.063 0.031  0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000  0.707 0.705 0.701 0.694 0.684 0.672 0.657 0.639 0.619 0.596 0.571 0.544 0.515 0.484 0.450 0.415 0.379 0.340 0.301 0.260 0,218 0,176 0.132 0.089 0.044 0.022  0.022 0.044 0.089 0.132 0.176 0.218 0.260 0.301 0.340 0.379 0.415 0.450 0.484 0.515 0,544 0.571 0.596 0.619 0.639 0.657 0,672 0.684 0.694 0.701 0.705 0.707  0.707 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.708 0.707  B(X)  B(Y)  B(Z)  -0.707 -0.706 -0.702 -0.695 -0.686 -0.673 -0.658 -0.640 -0.620 -0.598 -0.573 -0.545 -0.516 -0.485 -0.451 -0.416 -0.379 -0.341 -0.301 -0.261 -0.219 -0.176 -0.133 -0.089 -0.044 -0,022  -0.022 -0.044 -0.089 -0.133 -0.176 -0,219 -0.261 -0.301 -0.341 -0.379 -0.416 -0.451 -0.485 -0.516 -0.545 -0.573 -0.598 -0.620 -0.640 -0.658 -0.673 -0.686 -0.695 -0.702 -0.706 -0.707  0.707 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0.706 0,706 0.706 0.706 0.706 0.706 0.707  -  144  -  T a b l e 7*26 - J o i n t A n g l e s C a l c u l a t e d Kinematic F e a s i b i l i t y Search*  0  0  1  1  0  0  LEFTY BELOW F L I P  JOINT ANGLES INDEX i 1 1 1 1 1 1 1 1 1 1 3 4 5 6 7 8 9 10  11  12 13 14 15 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16  JI  J2  J3  -6.622 -15.533 1.934 -23.432 9.213 -29.204 14.867 -32.589 18.922 -33.991 21.564 23.044 24,754 26.424 28.053 29.640 31.182 32.679 34,128 35,526 36,871 38,159 39.385 40.546 41.636 42.647 38.475 45.565 33.086 47.332 26.766 48.106 20.135 48.055 14.035 47.335 9.197 46.083 5.927 44.419 4.119 42.442 3.473  16.015 13.294 19.744 12.170 23.869 12.847 27.948 14.993 31.719 18.010 35.056 35.421 35.970 36.533 37.107 37.689 38.278 38.871 39.465 40.055 40.639 41.212 41.769 42.304 42.809 43.278 37.889 48.083 32.310 52.087 27.090 55.213 22.834 57.475 20.008 58.936 18.748 59.685 18.834 59.811 19.868 59.406 21.479  -50.077 -50.437 -49.250 -50.308 -48.006 -49.703 -46.416 -48.661 -44.561 -47.247 -42.524 -42.404 -42.832 -43.311 -43.840 -44.420 -45.049 -45.727 -46.454 -47.227 -48.048 -48.915 -49.828 -50.785 -51.787 -52.833 -53.846 -51.330 -54.290 -49.435 -54.127 -47.252 -53.364 -44.881 -52.055 -42.410 -50.286 -39.917 -48.162 -37.470 -45.788 -35.127 -43.267  during  CONFIGURATION  (DEGREES) J4 -165.980 -129.587 163.429 -104.463 144.246 -90.092 131.030 -80.849 120.298 -73.811 110.545 110.839 112.562 114.323 116.123 117.966 119.854 121.788 123.772 125.807 127.894 130.035 132.233 134.489 136.807 139.191 148.929 129.697 160.471 119.593 176.471 108.481 -160.138 96.368 -131.926 83.650 -108.091 70.993 -91.535 59,058 -79.772 48.262 -70.605  J5 -148.506 -147.050 -140.524 -138.041 -128.539 -127.680 -116.493 -118.916 -105.845 -112.241 -97.011 -96.392 -95.680 -95.033 -94.458 -93.960 -93.547 -93.226 -93.003 -92.886 -92.882 -93.000 -93.248 -93.635 -94.170 -94.866 -108.479 -82.751 -122.654 -72.640 -135.671 -64.807 -144.740 -59.387 -147.283 -56.382 -144.409 -55.630 -139.649 -56.811 -135.107 -59.518 -131.349  * * * * * * * * * * NO F E A S I B L E PATH FOUND * * * * * * * * * *  J6  JOINT LIMITS EXCEEDED 1 <•> 3 4 5 6  172.931 -159.095 151.316 -142.106 141.902 -135.899 139.435 -134.963 140.484 -136.320 143.488 143.565 142.869 142.093 141.240 140.316 139.325 138.271 137.159 135.996 134.786 133.538 132.258 130.955 129.637 128.313 126.679 132.566 128.722 138.979 136.549 147.302 152.536 157.190 173.638 167.998 -169.518 178.812 -159.767 -171.253 -154.506 -162.748 -151.436  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 1 0 1 0 1 0 0 0 0 0 0 0 0  1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0 0 0 0 0 0 0 0  1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  TOOL R01 Y Y Y Y Y Y Y Y Y Y  Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y Y  - 145 -  Table 0 1  1  1  1  1  1 1  1 1 1 1 3 4 5 6 7 8 9 10  11  12 13 14 15 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16 16  0  -6.622 -15.533 1.934 -23.432 9.213 -29.204 14.867 -32.589 18.922 -33.991 21.564 23,044 24.754 26.424 28.053 29.640 31.182 32.679 34.128 35.526 36.871 38.159 39.385 40,546 41.636 42.647 38.475 45.565 33.086 47.332 26.766 48,106 20.135 48,055 14.035 47.335 9.197 46,083 5,927 44.419 4,119 42.442 3,473  1 0  0  7.26/cont. 0  LEFTY BELOW NOFLIP CONFIGURATION  -50.077 14.020 16.015 -50,437 50,414 13,294 -49.250 -16.571 19.744 75.537 -50,308 12,170 -48,006 23.869 -35.755 12,847 -49.703 89.908 -46.416 -48.970 27,948 99.152 14,993 .-48,661 -44.561 -59.703 31.719 -47.247 106.190 18.010 -42,524 35.056 -69.455 35.421 -42,404 -69.161 -42.832 -67.438 35.970 -43.311 -65.678 36.533 -43.840 -63.877 37.107 -44.420 -62.034 37.689 -45.049 -60.146 38.278 38,871 -45.727 -58.212 -46.454 39,465 -56,228 40.055 -47,227 -54.193 -48,048 40.639 -52,106 -48,915 -49.965 41,212 -49,828 -47.767 41,769 -50,785 42.304 -45.511 -51,787 42.809 -43.193 43,278 -52.833 -40,809 37,889 -53.846 -31.072 -51.330 48.083 -50,303 -54,290 -19.529 32.310 -49,435 52.087 -60.407 -54.127 27.090 -3.529 -47.252 -71.519 55.213 -53.364 22,834 19.862 -44.881 -83,632 57.475 -52.055 48.074 20.008 58.936 -42.410 -96.350 -50,286 71.909 18.748 59.685 -39.917 -109.007 -48.162 88,465 18.834 -37,470 -120.943 59.811 19.868 -45.788 100.228 59.406 -35.127 -131.738 -43.267 21,479 109.395  148.506 147.050 140.524 138.041 128.539 127.680 116.493 118,916 105.845 112,241 97.011 96.392 95.680 95,033 94.458 93.960 93.547 93.226 93.003 92.886 92.882 93,000 93,248 93,635 94,170 94.866 108.479 82,751 122.654 72,640 135.671 64.807 144.740 59.387 147.283 56.382 144.409 55,630 139.649 56,811 135.107 59.518 131,349  * * * * * * * * * * NO F E A S I B L E PATH FOUND * * * * * * * * * *  -7.069 20.906 -28.684 37.894 -38.098 44,102 -40.565 45,038 -39.516 43.680 -36.512 -36.435 -37.131 -37.908 -38.760 -39.684 -40.675 -41,729 -42,841 -44.005 -45,214 -46.462 -47,742 -49.045 -50.364 -51.687 -53.321 -47,434 -51.278 -41,021 -43.451 -32.698 -27.464 -22.810 -6.363 -12.002 10.482 -1.188 20.233 8.747 25.495 17.252 28.564  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 1 0 1 0 1 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0  1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  - 146 -  0 1 1 1 1 1 1 1 3 3 3 4 5 6 7 7 7 8 9 10 10 10 11 12 12 12 13 14 14 14 15 16 16 16 16 16 16 16 16 16 16 16 .1.6 16 16 16 16 16 16  0  0  Table  7.26/cont,  1 0  0  -6.622 -127.357 -15.533 -130,443 1.934 -122.787 -23.432 -131.437 9.213 -117.398 -29.204 -130.144 14.867 -111.706 16.438 -111.159 17.992 -111,084 12.386 -116.682 22.065 -105,462 23.651 -105,423 25.192 -105.429 26.687 -105.483 28.134 -105.585 23.730 -111.269 31.182 -99.991 32.679 -100.085 34,128 -100.228 35.526 -100.422 32.162 -106.218 37.670 -94.874 39.124 -95.021 40.528 -95.217 38,159 -100.978 41.814 -89,870 43.273 -89.993 44,681 -90.159 -95.769 43.172 45.239 -85.107 46.698 -85.181 48.106 -85.290 47.332 -90.632 48.055 -80.624 45.565 -96.564 47.335 -76.660 42.647 -102.900 46.083 -73,391 38.475 -109.321 44.419 -70,793 33.086 -115,354 42.442 -68.833 26.766 -120.408 40.235 -67.479 20.135 -123.886 37.865 -66.698 14.035 -125.376 35.387. -66.459 9.197 -124.836  LEFTY ABOVE F L I P  -124.502 -124.142 -125.329 -124.270 -126.573 -124.876 -128.163 -128.304 -127,995 -126,523 -129.764 -129.335 -128.860 -128.339 -127.773 -126.335 -129.530 -128.852 -128.125 -127.352 -125,806 -129.211 -128,340 -127.419 -125.664 -129.451 -128.451 -127.399 -125,430 -129,604 -128.491 -127.327 -125.144 -129.698 -123.249 -132.169 -121.746 -134.662 -120.733 -137.109 -120.289 -139.452 -120.452 -141.641 -121.215 -143,633 -122.524 -145.388 -124,293  -133.965 -119.274 91.741 -116.376 79.061 -115.454 77.822 77.000 74,785 75.333 75.895 73.803 71.678 69.518 67.318 65,426 69.547 67.435 65.288 63.102 60.438 65.891 63.779 61.630 58.598 64.783 62.708 60.596 57.206 64.130 62.102 60.037 56,244 64.028 52.576 68.310 48.887 73.050 44.679 78,531 37.913 85.196 13.349 93.712 -94.731 104.943 -116.452 119.595 -122,530  CONFIGURATION  -10.127 115.326 0 0 1 1 0 0 -28.721 98.042 0 0 1 1 0 0 -10.452 -107.475 0 0 1 0 0 0 -46.273 92.552 0 0 1 1 0 0 -27.743 -91.496 0 0 1 0 0 0 -61.225 88.686 0 0 1 1 0 0 -43.685 -86.588 0 0 0 0 0 0 -43,941 -86.257 0 0 0 0 0 0 -43.004 -85.888 0 0 1 0 0 0 -26.972 -90.091 0 0 1 0 0 0 -57.359 -83.042 0 0 0 0 0 0 -56.564 -82.802 0 0 0 0 0 0 -55.741 -82.549 0 0 0 0 0 0 -54.889 -82.279 0 0 0 0 0 0 -54,005 -81.990 0 0 1 0 0 0 -38.939 -84.115 0 0 1 0 0 0 -67,500 -79.883 0 0 0 0 0 0 -66.781 -79.659 0 0 0 0 0 0 -66,036 -79.420 0 0 0 0 0 0 -65,261 -79.164 0 0 1 0 0 0 -51.142 -80.967 0 0 1 0 0 0 -77.900 -77.146 0 0 0 0 0 0 -77.307 -76.913 0 0 0 0 0 0 -76.694 -76.667 0 0 1 0 0 0 -63.613 -78.594 0 0 1 0 0 0 -88.397 -74.504 0 0 0 0 0 0 -87.967 -74,248 0 0 0 0 0 0 -87.526 -73.982 0 0 1 0 0 0 -75.393 -76.134 0 0 1 0 0 0 -98.385 -71.561 0 0 0 0 0 0 -98.121 -71,260 0 0 0 0 0 0 -97,859 -70.950 0 0 1 0 0 0 -86.606 -73.418 0 0 1 0 0 0 -107,936 -68.144 0 0 0 0 1 0 -73,971 -75.537 0 0 1 0 0 0 -117.038 -64.914 0 0 0 0 1 0 -59.805 -77.134 0 0 1 0 0 0 -125.329 -61.085 0 0 0 0 1 0 -44.120 -77,653 0 0 1 0 0 0 -132.913 -56.367 0 0 0 0 1 0 -27.261 -75.019 0 0 1 0 0 0 -139.810 -50.318 0 0 0 0 1 0 -10,736 -54.043 0 0 1 0 0 0 -145.928 -42.294 0 0 0 0 1 0 -11.351 50.982 0 0 1 0 0 0 -151,019 -31.470 0 0 0 0 1 0 -26.690 69.617 0 0 1 1 0 0 -154.632 -17.215 0 0 0 0 1 0 -41.009 72.580 0 0 1 1 0 0  * * * * * * * * * * NO F E A S I B L E PATH FOUND * * * * * * * * * *  - 147 -  Table O 1 1 1 1 1 1 1 *—  3 3 3 4 5 6 6 6 7 7 7 8 9 9 9 10 10 10 11 12 12 12 12 12 12 12 12 12 12 12 12 12 12 12 12 12 12  O  O  -6,622 -15.533 1 .934 -23.432 9.213 -29.204 14.867 16.438 17.992 12.386 22.065 23.651 25.192 26.687 22.371 29.640 31.182 28.134 33.029 34.620 36.167 34.128 37.163 38.757 37.670 38.958 40.590 42.183 41.814 41.795 40.528 40.795 38.159 39.303 34.556 37.426 29.654 35.251 23.564 32.853 16.704 30.293 9.829 27.623 3.826  O  O  7.26/cont.  O  -127.357 -130.443 -122.787 -131.437 -117.398 -130.144 -111.706 -111.159 -111.084 -116.682 -105.462 -105.423 -105.429 -105.483 -111.146 -99.942 -99.991 -105.585 -94.695 -94.713 -94.772 -100.228 -89,737 -89.744 -94.874 -85.129 -85.078 -85.057 -89.870 -80.835 -95.217 -77,224 -100.978 -74.225 -106.944 -71.826 -112.785 -70.010 -118.043 -68.758 -122.169 -68.050 -124.643 -67.865 -125.168  LEFTY ABOVE NOFLIP CONFIGURATION -124.502 -124.142 -125.329 -124.270 -126.573 -124.876 -128,163 -128.304 -127.995 -126.523 -129.764 -129.335 -128.860 -128.339 -126.813 -130,159 -129.530 -127.773 -131.521 -130.802 -130.032 -128.125 -132.139 -131.296 -129.211 -133.520 -132.622 -131.670 -129.451 -133.991 -127.419 -136.339 -125.664 -138.648 -124.279 -140.860 -123.350 -142.925 -122.941 -144.801 -123.085 -146.450 -123.774 -147.840 -124.964  46.035 60.726 -88.259 63.625 -100.939 64,546 -102.178 -103.000 -105,215 -104.667 -104.105 -106.197 -108.322 -110.482 -112.146 -108.373 -110.453 -112.682 -107.963 -109.983 -112.031 -114.712 -109.176 -111.162 -114.110 -108.048 -109.966 -111.908 -115.217 -108.401 -118.370 -104.608 -121.402 -100.364 -124.450 -95.387 -128.116 -89.232 -136.077 -81,217 111.544 -70.406 65.765 -55.911 60.579  10.127 -64.674 0 0 1 0 28.721 -81,958 0 0 1 0 10.452 72.525 0 0 1 0 46.273 -87,448 0 0 1 0 27.743 88.505 0 0 1 0 61.225 -91.314 0 0 1 0 43.685 93.412 0 0 0 0 43.941 93.743 0 0 0 0 43.004 94.112 0 0 1 0 26.972 89.909 0 0 1 0 96.958 0 0 0 0 57.359 56.564 97.198 0 0 0 0 97.451 0 0 0 0 55.741 54.889 97.721 0 0 0 1 40.007 95.384 0 0 1 1 68.194 99.906 0 0 0 0 100.117 0 0 0 1 67.500 54.005 98,010 0 0 1 1 79.566 102.223 0 0 0 0 79.028 102,422 0 0 0 0 78.473 102.633 0 0 0 1 66.036 100,581 0 0 0 1 104.791 0 0 0 0 89.630 89,228 105.016 0 0 0 1 77.900 102.854 0 0 0 1 99.452 107.341 0 0 0 0 99.184 107.600 0 0 0 0 107.869 0 0 0 1 98.917 88.397 105.496 0 0 0 1 108.459 110.503 0 0 0 0 76.694 103.333 0 0 1 1 117.202 113.516 0 0 0 0 63.613 101.406 0 0 1 1 117.097 0 0 0 0 125.282 49.025 99.895 0 0 1 1 132.776 121.538 0 0 0 0 32.968 99.463 0 0 1 1 127.284 0 0 0 0 139.690 103.805 0 0 1 1 15.842 135.001 0 0 0 0 145.921 4.344 -146.938 0 0 1 0 145.588 0 0 0 0 151.208 20,264 -104.146 0 0 1 0 159.867 0 0 0 0 155.077 36,131 -101.906 0 0 1 0  * * * * * * * * * * NO F E A S I B L E PATH FOUND * * * * * * * * * *  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  - 148 -  Table 1 1 3 4 5 6 7 8 9 10 11 12 13 14 15 16 16 17 18 19 20 21 22 22 23 24 25 26  1  -98.962 -98.225 -97.183 -96.089 -94.946 -93.755 -92.518 -91.237 -89.912 -88.543 -87.132 -85.678 -84.181 -82.639 -81.053 -79.419 -86.488 -85.015 -83.498 -81.937 -80.330 -78.676 -76.972 -85.331 -83.691 -82.007 -80.278 -78.432  1  1  7.26/cont.  0  -52.644' -53.398 -53.823 -54.198 -54.521 -54.795 -55.018 -55.190 -55.311 -55.380 -55.396 -55.357 -55.263 -55.111 -54.899 -54.624 -55.164 -55.113 -55.005 -54.839 -54.613 -54.323 -53.966 -57.729 -57.592 -57.397 -57.139 -56.701  * * * * * * * * * * * * FEASIBLE  0  RIGHTY ABOVE F L I P  -50.077 -49.854 -49.805 -49.788 -49.803 -49.850 -49.930 -50.042 -50.185 -50.361 -50.567 -50.805 -51.073 -51.371 -51.699 -52.055 -50.286 -50.532 -50.808 -51.115 -51.450 -51.814 -52.206 -49.444 -49.774 -50.137 -50.531 -51.010 PATH  -67.121 -67.355 -67.582 -67.736 -67.815 -67.817 -67.740 -67.581 -67.336 -67.003 -66.577 -66.056 -65,435 -64.711 -63.881 -62.940 -51.503 -49.679 -47,739 -45,688 -43,536 -41,298 -38.987 -14.283 -11.733 -9.302 -6.999 -6.902  CONFIGURATION  -75.397 -74.593 -72.849 -71.153 -69,505 -67.903 -66,349 -64.841 -63.382 -61.973 -60.615 -59.311 -58.065 -56.881 -55.763 -54.717 -41.809 -41.024 -40.346 -39.780 -39.332 -39.009 -38.815 -32.257 -32.880 -33.613 -34.451 -35.065  !!! * * * * * * * * * * * * * *  -98.233 -98.659 -100.365 -102.082 -103.814 -105.566 -107.342 -109.146 -110.982 -112.855 -114.769 -116.728 -118.737 -120.798 -122.916 -125.094 -137.531 -140.528 -143.598 -146.735 -149.925 -153.154 -156.406 177,540 174.018 170.664 167.488 167.575  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  Table  7.27  -  Generated  Robot  Location  and weld P a r a m e t e r  PHA  BETA  STICKOUT  VOLTAGE  FEED  GFR  i.00  0.00  0.403  16.00  217.09  21.11  IDEX  X  Y  Z  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  221.4 220.1 217.7 214.7 211.4 207.7 203.6 199.1 194.2 189.1 183.6 177,8 171.8 165.5 159.0 165.5 159.0 152.3 145.4 138.3 131.2 138.9 131.4 123.9 116,3 109.6  131.8 138.6 145.9 153.0 159.8 166.5 172.9 179.0 184.8 190.2 195.3 200.0 204.4 208.3 211.8 208.9 212.4 215.5 218.1 220.3 222.0 224.9 226.7 227.9 228.6 228.9  205.0 204.6 203.6 202.6 201.4 200.0 198.5 196.9 195.1 193.2 191.2 189.1 187.0 184.7 182.3 186.8 184.4 181.9 179.4 176,9 174.3 177.1 174.4 171,6 168,9 166.4  0 -88.60 -87.19 -84.38 -81.57 -78.76 -75.95 -73.14 -70.34 -67.53 -64.72 -61.92 -59.11 -56.30 -53.49 -50.67 -37.13 -34.29 -31.43 -28.56 -25.65 -22.71 -6.69 -3.43 -0.10 3.33 5.07  A 155.01 155.03 154.94 154.79 154.58 154.31 153.98 153,59 153.15 152.65 152.10 151.49 150.83 150.13 149.37 143.23 142.23 141.20 140.13 139.04 137.92 131.94 130.71 129.48 128.25 127.60  Data  NPOINT  T 164.32 163.64 162.29 160.94 159.60 158.26 156.95 155.64 154.36 153.09 151.85 150.64 149.45 148.29 147.17 152.08 151.18 150.33 149.53 148.79 148.12 156.81 156.65 156.59 156.64 156.72  26 CURRENT  SPEED  137.00 136.95 136.84 136.72 136.59 136.45 136.30 136.15 136.00 135.84 135.68 135.52 135.37 135.21 135.05 134.90 134.75 134.60 134.46 134.33 134.20 134.07 133.96 133.85 133.75 133.70  1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1,228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 1.228 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 C a p a b i l i t i e s Several  important  capabilities  and implemented i n AUTOP.  7.8.1  have  not yet been  fully  developed  These c a p a b i l i t i e s are summarized below:  Real-time Seam Tracking  Eventually, we w i l l  require AUTOP to run i n a real-time environment  with sensory feedback from the welding process.  AUTOP w i l 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 The  CAD  system  software  will  ultimately  will  operate  be required  be generated independently  to translate  by a commercial from  CAD  system.  AUTOP, but additional  the CAD output  data  into the  correct format for input to AUTOP.  7.8.3 A remotely welding  Generation of High-level Descriptive Program  sophisticated controlled machine.  programming  welding  workstation  devices, including In order  i t i s desirable  contains several  programmable  a robot, positioning  to r e a l i z e  the f u l l  to be able  benefit  to program  table and  of automatic  and control the  workstation i n a single, task-oriented high-level language. Clearly actions  the welding  occurring  in a  process given  can be broken sequence.  down into  Actions  must  a number of be  taken  by  - 151 -  different  physical  devices,  either  independently  or i n synchronization  with other a c t i o n s . An example of independent a c t i o n s : the  table.  move  An example of s y n c h r o n i z e d  the t o r c h  along  the seam.  description i s illustrated Correct scheduling  t u r n on the welder and r e p o s i t i o n  A  in Fig.  synchronization  of  actions:  simplified  t u r n on the welder and high  device  as i n Table  welding  actions  requires  some  before  form  of  We c o u l d c r e a t e a task  7.28 f o r s c h e d u l i n g c o n t r o l .  at a g i v e n l e v e l must be completed  task  7.7.  based on task precedence requirements.  precedence t a b l e such  level  A l l tasks  any tasks a t the next  l e v e l can  start. This descriptive  type  of  action  programming  language  time o p e r a t i n g system. be use  used  to a c h i e v e  device  completion implemented  Standard  can be such  implemented  polling  action  as l o w - l e v e l  from the h i g h - l e v e l program.  a  high  under  level  a real-  r e a l - t i m e o p e r a t i n g system p r a c t i c e s can  of d e v i c e  to the program. device  In  as FORTRAN, running  correct action synchronization.  interrupts, o f an  table  dependent  flags  F o r example, we can  or semaphores  The d e v i c e macros  called  to s i g n a l  drivers as  can be  subroutines  - 152 -  initialize welder  initialize robot  initialize table  1  <  position table  move welding t o o l to s t a r t of seam  s t a r t welder  move welding t o o l along seam  stop welder  No  F i g u r e 7.7  S i m p l i f i e d H i g h - l e v e l Task D e s c r i p t i o n .  - 153 -  T a b l e 7.28  Task Precedence T a b l e  Welder  Robot  initialize  initialize  Table  initialize  position  move to seam  start  welder  move along seam  stop welder  - 154  8.  The  work  commercially  described useful  in  -  CONCLUSIONS  this  automatic  thesis  provides  programming  a  solid  system.  basis  Some  for  a  necessary  a d d i t i o n s and d e s i r a b l e improvements are suggested which r e q u i r e more time and  effort  to develop, debug and  techniques  gained  a p p l i c a t i o n , and  from  a fully  can be r e a d i l y adapted  8.1  this  implement.  project  automatic  to other  The knowledge, experience  are  highly  programming  useful  system  any  robot  f o r a welding  robot  applications.  A n a l y s i s and E v a l u a t i o n o f Work Completed Solutions  described  in  to  some  this  of  thesis  major  and  the  solutions  been  developed  implemented,  c a p a b i l i t i e s of the software package developed  following  list  while  2.  I n t e r a c t i v e s e l e c t i o n of t a b l e p o s i t i o n s and  3.  Automatic  4.  G e n e r a t i o n of robot l o c a t i o n c o o r d i n a t e s f o r each seam.  simplify  the  use  output  programming  be done.  of  generated the  the  selection. seam welding  checking f o r robot t r a j e c t o r y k i n e m a t i c  the  other  to date:  Automatic  can  and  summarizes  1.  We  parameter  The  have  important  weld  work.  problems  require  still  further  the  problems  and  for  and  by  the  r o b o t , but  software  at  order.  feasibility.  this  stage  some manual programming  A simple i n t e r f e r e n c e d e t e c t i o n scheme has been  to  must  developed,  implemented i n computer program TESTIN but has not yet been i n c o r p o r a -  ted i n t o AUTOP.  - 155  8.2  Suggestions f o r F u r t h e r Work A  been  -  number  considered  require  of  the  i n any  further  work  Develop  and  problem  areas  depth.  We  before  a  discussed  can  in  identify  commercially  a  this  thesis  number  of  feasible  have  areas  system  not  which  can  be  realized:  1.  programming necessary robot, To  generate methods  i f we  table)  date,  there  for  wish  as  to  an  has  high-level  Refine  the  been  AUTOP.  method The.  optimization,  control  little  a  for  can  be  of  from  a  central of  procedure  the  welding  (welder, computer.  languages  should  procedure  include  seam o r i e n t a t i o n o p t i m i z a t i o n ,  with  is  or  purpose.  specifying  accommodated  This  machines  standardization  o p t i m i z a t i o n of welding parameters. table  descriptive  workstation.  number  system  for this  welding weld  integrated  integrated  communications p r o t o c o l s  2.  an  task-oriented  and  A continuously  modifications  duty  in  cycle  continuous  repositioning  to  the  current  software.  3.  Develop  a  scheme.  practical Initially,  simulation Eventually  and  path an  planning  or  i n t e r a c t i v e system  collision  detection  collision  avoidance  employing  should  be  graphical developed.  a more s o p h i s t i c a t e d p a t h - p l a n n i n g a l g o r i t h m  used to minimize o p e r a t o r  input.  should  be  - 156 -  Develop  and i n c o r p o r a t e  requirement,  the seam  information nominal  to AUTOP  path  appropriate for  seam  should  i n real-time.  tracking. provide  positional  AUTOP w i l l  Real-time weld  As a minimum error  then modify the  puddle a n a l y s i s and  weld parameter m o d i f i c a t i o n i s a d e s i r a b l e c a p a b i l i t y uniformly  f o r future  Interface develop  tracker  appropriately.  ensuring  area  real-time  weld  quality,  and i s a worthwhile  research.  the  a  high  simple  software  to  dedicated  a CAD  commercial system  CAD  system  and/or  f o r generating  object  s p e c i f i c a t i o n s from a drawing o r p r o t o t y p e .  Interface  AUTOP  high-level  to a r e a l  task-oriented  machine communication.  welding  programs  workstation. and r e a l - t i m e  This host  The i n t e r f a c e must be customized  requires computer/ to each  i n s t a l l a t i o n due to l a c k of s t a n d a r d i z a t i o n .  Refine  the E x p e r t  Welder  comprehensive welding data  Continually  evaluate,  and  develop  a  reliable,  base.  modify  encountered and experience  module,  and r e f i n e  i s gained.  AUTOP  as problems a r e  - 157 -  REFERENCES  1.  Ambler, A.P., Popplestone, R.J., Kempf, K.G., "An Experiment i n the O f f l i n e Programming of Robots", Proc. 12th I n t . 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 f o r 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 f o r 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., K h a l i l , 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 i n 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 Proc. Robots VI, Detroit, 1982.  Intelligence",  15.  Konstantinov, M., Markov, M., "Discrete Positions Method i n 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 I n d u s t r i a l 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., P a t a r i n s k i , S., "A Contribution to the Inverse Kinematic Problem for I n d u s t r i a l 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 I n t . 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.868871.  21.  Lozano-Perez, T., Wesley, M., "An Algorithm f o r Planning C o l l i s i o n 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 f o r Simple Manipulators", IEEE Transactions on Systems, Man, and Cybernetics, Vol.SMC-11, No.6, June 1981, pp.449-455.  26.  Paul, R. Robot Manipulators: MIT Press, 1981.  27.  Mathematics, Programming and Control,  Requicha, A., "Representations of Solids: Theory, Methods and Systems", Computing Surveys, Vol.12, No.4, 1980, pp.437-465.  - 159 -  28.  29.  Sata, T., Fumihiko, K., Akio, A., "Robot Simulation System as a Task Programming Tool", Proc. 11th Int. Symp. on Industrial Robots, Tokyo, 1981. 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 O f f - l i n e Programming a Heavy Duty Industrial Robot", Proc. 10th Int. Symp. on Industrial Robots, Milan, 1980.  31.  Udupa, S., " C o l l i s i o n Detection and Avoidance i n Computer Manipulators", Proc. 5th Int. Joint Conf. on Intelligence, Cambridge, 1977.  32.  Wesley, M, Lozano-Perez, T., Lieberman, L., Lavin, M., Grossman, D., "A Geometric M o d e l l i n g System f o r 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.MMS10, 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.  Controlled Artificial  - 160 -  APPENDIX I  SOFTWARE DOCUMENTATION  - 161 -  APPENDIX 1.1  IMPLEMENTATION SPECIFIC SOFTWARE  S u b r o u t i n e s OPEN, SHUT The method of a c c e s s i n g e x t e r n a l from  one FORTRAN  implementation  OPEN s u b r o u t i n e c a l l In  AUTOP,  a l l files  another  to another.  to open a f i l e , a r e opened  s u b r o u t i n e s OPEN and SHUT.  from a FORTRAN program DEC FORTRAN uses  u s i n g a DEC s p e c i f i c and  closed  by  calls  The software can then be e a s i l y  system w i t h d i f f e r e n t  s u b r o u t i n e s OPEN and SHUT.  files  file  management procedures  a FORTRAN  argument to  varies  list.  the AUTOP  t r a n s p o r t e d to  by r e w r i t i n g the  - 162 -  APPENDIX 1.2  AUTOP PROGRAM LISTING  - 163 -  Listing  o f AUTOP.FOR  a t 14:39:07  o n AUG  1  C  PROGRAM AUTOP  2 3 4  c c  T H I S MAIN PROGRAM IS WELDING ROBOT.  c c c c c  INPUTS:  C  D  6 7  8  9 10 4 4  1 1  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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  OUTPUTS:  9.  1984  f o r CC1d=DAC0 P a g e  FOR THE AUTOMATIC PROGRAMMING  1  OF A  NONE NONE  FILES  READ:  FI LI S T , C A D L S T . G R A P H  FILES  WRITTEN:  NONE  p  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 cc  c  c c  SUBROUTINES C A L L E D :  'IGL'  CADFIL CHECK CONFIG DRWELD EXPERT EYELOC INIT OPEN RDFILE SCENE SHUT WELSET  GRAPHICS SUBROUTINES:  CMCLOS CMOPEN GETPIK GRSTOP GRSTRT VUP3D VWPT3D WIND3D  BYTE F I 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 ) 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 ) BYTE L 0 C S E T ( 2 0 ) , W P A R A M ( 2 0 ) , W P C D A T ( 2 0 ) BYTE B E L ( 3 ) , Y E S . N O . A N S . A N S 2 DOUBLE P R E C I S I O N H O T ( 4 . 4 ) . H O W ( 4 , 4 ) , H T W ( 4 , 4 ) REAL VERT(1000.3).FACES(100.4),H(4.4),EYEPOS(3),VRP(3).VAR(6) - REAL SHOT(4,4),SH0W(4,4),H00(4,4),H6G(4.4),INH00(4,4),INH6A(4.4) REAL S H T W ( 4 . 4 ) . D E L T A REAL W I N S I Z ( 6 ) , V U P 0 ( 3 ) , V U P 1 ( 3 ) REAL ALPHA(6 ) , L E N ( 6 ) , D I S T ( 6 ) , R M I N ( 6 ) , R M A X ( 6 ) INTEGER I C 0 N F ( 6 ) , I M A X INTEGER INTEGER  I0BJ(10,2),IFACE(100,2) ,IFLAG(1000),IDIR(1) IPICK(IOO)  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 COMMON / V I E W / E Y E P O S , V R P COMMON / W I N S I Z / W I N S I Z COMMON / F I L E / F I L I S T  - 164 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78  79  80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96N 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 111 112 113 114 115 1 16  of  AUTOP.FOR  C C  at  14:39:07  COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON DATA DATA  on  AUG  9,  1984  for  CC1d=DAC0 Page  /CADLST/CADLST /WPCLST/WPCLST /WKSTAT/WKSTAT /TABLE/TABLE /STOVAR/STOVAR /GRAPH/GRAPH /OBJLST/OBJLST /ROBLST/ROBLST /LOCSET/LOCSET /MATRIX/HOT,HOW,SHOT,SHOW /ROBOT/NRDOF.ALPHA,LEN,DIST,RMIN,RMAX,H0O,H6G,INHOO,INH6A /CONFIG/ICONF /TABDAT/HTW,SHTW /TOLROT/IMAX,DELTA / 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 /NSEAMS/NSEAMS  BEL/3*7/,YES/89/.NO/78/ VUP0/3*0/,VUP1/0.,0.,1./  c****«  ENTER THE NAME OF  10  WRITE(5.10) F O R M A T C ENTER THE NAME OF THE F I L E CALL R D F I L E ( 5 , 1 . F I L I S T . 2 0 , I E N D )  C  C C***»* C  C C***»* C C C  c****» C  C C***»* C  C  c»**** C  FILE  'FILIST'.  CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL  OPEN(1.FILIST.4,1) R D F I L E ( 1 , 1 , C A D L S T , 2 0 , I END) RDFILE(1.2.WPCLST,20.IEND) RDFILE(1,3,WKSTAT,20,IEND) RDFILE(1,4,TABLE,20,IEND) RDFILE(1,5,STOVAR,20,IEND) RDFILE(1,6.GRAPH.20,IEND) R D F I L E ( 1 , 7 , O B J L S T , 2 0 , I END) R D F I L E ( 1 , 8 , R O B L S T , 2 0 , I END) R D F I L E ( 1 , 9 , L O C S E T . 2 0 , IEND) RDFILE(1,10,WPARAM,20.IEND) SHUT(1) THE WORKPIECE  FROM F I L E  SEAM F I L E S  LIST...  ',$)  'FILIST'.  FROM THE  SEAM CAD  FILES  SETUP  OPEN CAD F I L E CALL  SYSTEM F I L E S  LIST  NAMES  GENERATE  OF  THE  READ  CALL  2  LIST,  'CADLST'.  OPEN(1.CADLST.4,1)  S K I P THE  F I R S T RECORD AND READ THE NUMBER OF  READ( 1 , * ) READ( 1,*) NSEAMS CALL SHUT(1) OPEN GRAPHICS PARAMETERS  FILE,  'GRAPH'.  SEAMS,  'NSEAMS' .  - 165 -  Listing 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  of  AUTOP.FOR  at  14:39:07  on  AUG  9,  1984  for  CC1d=DAC0 P a g e  3  CALL OPEN(1.GRAPH,4,1) READ(1,*) C c****»  c c c****» c c c c**»** c c c c**»«* c  c c c***»* c *  cc * * * » * c c c***** c c c cc ** ** *» »* **  c c c c  'VRP'.  R E A D ( 1 , * ) ( V R P ( I ) , 1 = 1,3) READ( 1,*) READ CURRENT WINDOW PARAMETERS.  'WINSIZ'.  READ( 1 ,* ) ( W I N S I Z ( I ) , 1 = 1,6) CALL S H U T O )  INITIALIZE CALL  ROBOT  PARAMETERS  WELDER  SETUP  WELSET(WPCLST.WKSTAT)  INITIALIZE CALL CALL  THE  INIT(NSEAMS)  INITIALIZE CALL  c cc * * « » »  c  READ CURRENT VIEW REFERENCE POINT,  IGL GRAPHICS  SYSTEM  GRSTRT(4027.1) VUP3D(VUPO,VUP1)  DEFINE  3D WINDOW AND  VIEWPORT.  CALL WIND3D(WINSIZ(1),WINSIZ(2).WINSIZ(3),WINSIZ(4),WINS WINSIZ(6)) CALL VWPT3D(0.,100.,0.,100.,0.,100.) INITIALIZE  THE  IZ(5)  WELDED SEAM COUNT  IC0UNT=1 DEFINE  EYEBALL  LOCATION.  C A L L CMCLOS CALL EYELOC(VRP,EYEPOS) CALL CMOPEN  CONVERT EXTERNAL CAD OBJECT DESCRIPTION ARRAYS FOR FAST PROCESSING. CALL CALL CALL  CMCLOS CADFIL CMOPEN  DRAW COMPLETE CALL  SCENE  SCENE,  INCLUDING  SEAMS  FILES  INTO  INTERNAL  - 166  Listing  of  175  C  176  C**  177  c  178  6 0 0  179  C  180 181 182 183  at  AUTOP.FOR  c** c  UNTIL  14:39:07  ( A L L  SEAMS  UNTIL  (NO  MORE  186  161  4  PICKED)  YOU  WISH  TO  CHANGE  THE  EYEBALL  POSITION?  (Y/N)  ANS  I F ( A N S . N E . Y E S ) CALL  191  50  192  C  GO  TO  50  EYELOC(VRP.EYEPOS)  CONTINUE  193  WRITE(5.162)  194  162  FORMAT($,'  195  DO  R E A D ( 5 , 1 7 0 )  196  CALL  198  C  199  C**  200  c  YOU  WISH  TO  REPOSITION  YES)  TRANSFORM  IF  ANSWER  I S  IF(ANS.NE.YES.AND.ANS2.NE.YES)  202  CALL IF  206  1 )  DRAW  IF(ICOUNT.EO.1) DO  210  c c*** * * c*** * *  21 1  C****»  212  C  8  F I L L  8  216  C  217 218  c** c  219  8 0  220  C  END  C  223  C**  224  C  226  c** c  227  6 0  228  C C  C  231  C  ALL GO  TO  END  I F  END  UNTIL  WELD.  'SHOW'  RELATING I S  AN  THE  P  I  UNTIL  C  K  JHE  (PICK  SCENE.  60  PICKED  ARRAY  I S OF  WELDS.  SEAM  TO  SUCCESSFUL)  BE  HOMOGENEOUS  WELDED  TO  PREVIOUSLY  DRWELD(SHOW,IPICK(K))  70  NEXT  A  WORKPIECE  CONTINUE  * * * » *  c**  THE  80  CONTINUE TO  TO  PREVIOUSLY  I F  GO  222  230  GO  CONTINUE  221  229  I N  'IPICK' CALL  215  AND  K=1.ICOUNT-1  MATRIX  213 C  REDRAW  ')  SCENE  (ICOUNT.GT.  207  214  (Y/N)  CMOPEN  (EITHER  201  c c** c  TABLE?  ANS2  FORMAT(A 1 )  170  197  232  Page  C  190  225  DO  READ(5.170)  189  209  CCid=DACO  CMCLOS  FORMAT($,'  187  208  for  CONTINUE  70  W R I T E ( 5 , 1 6 1 )  205  1984  c  CALL  204  9,  CHANGES)  185  203  AUG  CONTINUE  184  188  on  -  WORLD  TRANSFORMATION COORDINATES.  PICKED  WELDS.  - 167 -  Listing 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290  o f AUTOP.FOR C 611 C  a t 14:39:07 o n AUG  1984 f o r CC1d=DAC0  9,  Page  5  CONTINUE CALL CMCLOS WRITE(5,260) FORMAT(' PICK SEAM CALL CMOPEN  260  TO BE WELDED NEXT- HIT ANY  KEY TO PICK' )  C  c»»»** c***»* c c c** c  IF  PICK SEAM 'IDSEAM'.  GETPIK RETURNS THE  SEAM  ID,  CALL GETPIK(IDSEAM,IDPIK) (PICK  UNSUCCESSFUL) I F ( I D S E A M . N E . 0 ) GO  c  TO  190  CALL CMCLOS WRITE(5, 191 ) BEL FORMAT($, ' PICK UNSUCCESSFUL-TRY CALL CMOPEN GO TO 6 1 0  191  C C** C 190 C C  WITH CROSSHAIRS.  AGAIN  ( H I T RETURN)' ,3A1  IF  END  CONTINUE  CHECK  c  IF THE SEAM  CALL  c c**»* * c*»*** c* * * '** c****»  HAS  ALREADY  BEEN PICKED.  CMCLOS  ' I P I C K ' IS AN ARRAY CONTAINING ALL PICKED SEAMS. 'ICOUNT' IS THE NUMBER OF SEAMS ALREADY PICKED. 'IDSEAM' IS THE CURRENT SEAM ID 'IPREV.' IS A STATUS FLAG. IF 'IDSEAM' ALREADY PICKED. IPREV^ CALL C H E C K ( I P I C K , I C O U N T , I D S E A M , I P R E V )  c c c** c  CALL IF  (SEAM  PICKED) TO  350  C A L L CMCLOS W R I T E ( 5 , 1 3 0 ) BEL FORMAT($,' SEAM ALREADY CALL CMOPEN  130  c  350 C  PREVIOUSLY  I F ( I P R E V . E Q . O ) GO  c  c c** c  CMOPEN  GO ELSE  c * * * * •» c  (PICK  TO  P I C K E D - TRY AGAIN  ',3A1)  610  SUCCESSFUL)  CONTINUE ADD  'IDSEAM' TO ARRAY OF PICKED SEAMS,  'IPICK'  - 168 -  Listing 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348  o f AUTOP.FOR  a t 14:39:07 o n AUG  9,  1984 f o r CC1d=DAC0  Page  6  IPICK(ICOUNT)=IDSEAM C GO C C** C 610 C C** C 360 C  TO  360  END I F CONTINUE GO TO 611 END  160 C C***** C  UNTIL CONTINUE C A L L CMCLOS W R I T E ( 5 , 1 6 0 ) IDSEAM FORMAT($, ' IDSEAM=',I2) FIND THE CORRESPONDING SEAM  FILE  'WPCDAT'.  CALL 0PEN(2,WPCLST,4,1) C A L L RDFILE(2,IDSEAM+2,WPCDAT,20,IEND) CALL SHUT(2) C C«**** C 290 140 C C***** C***»* C  ENTER  CONTINUE WRITE(5,140) FORMAT($,' ENTER R E A D ( 5 , * ) IDIR  c***«*  IS TO BE WELDED.  1=REVERSE)  ')  C A L L THE EXPERT WELDER MODULE TO FIND THE TORCH ANGLES STICKOUT. AND THE WELDING PARAMETERS.  AND  CALL C C*****  D I R E C T I O N IN WHICH SEAM  WELD DIRECTION- (0=NORMAL,  EXPERT(WPCDAT)  SEARCH FOR A F E A S I B L E ROBOT SOLUTION. I S T A T : 0=OK, 1=R0B0T ENVELOPE EXCEEDED, 2=R0B0T JOINT L I M I T S EXCEEDED  C C A L L CONFIG(WPCDAT,IDSEAM,IDIR,ISTAT) C C A L L CMOPEN C C** C  850 C C***** C  I F (ROBOT  JOINT L I M I T S EXCEEDED) R E P O S I T I O N WORKPIECE AND  I F ( I S T A T . E Q . O ) GO TO 285 I F ( I S T A T . E O . 1 ) GO TO 286 " WRITE(5.850) FORMAT(' JOINT L I M I T S EXCEEDED-TRY REMOVE SEAM  FROM PICKED  RETRY.  AGAIN')  LIST  IPICK(ICOUNT)=0 C GO C C** C 286  OR  TO 6 0 0  IF (ROBOT CONTINUE  ENVELOPE  EXCEEDED) REPOSITION WORKPIECE AND  RETRY.  - 169 -  L i s t i n g of AUTOP.FOR at 14:39:07 on AUG 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387  851 C C***** C C 285 C C** C C** C  9, 1984 f o r CC1d=DAC0 Page  7  WRITE(5,851) F0RMAT(' ROBOT ENVELOPE EXCEEDED-REPOSITION') REMOVE SEAM FROM PICKED LIST IPICK(IC0UNT)=O GO TO 600 CONTINUE END IF  OR IF (IDIR NOT O OR 1) RE-ENTER IDIR IF(ISTAT.NE.1) GO TO 295 GO TO 290 CONTINUE  295 C C** ELSE (PATH OK) C** END IF C C****» FILL IN THE WELDED SEAM 'IDSEAM'. 'SHOW' IS A HOMOGENEOUS C***** TRANSFORMATION MATRIX RELATING THE WORKPIECE FRAME TO THE WORLD C***** COORDINATE FRAME. C CALL ORWELD(SHOW,IDSEAM) C C**«** INCREMENT THE WELDED SEAM COUNTER C IC0UNT=IC0UNT+1 IF(ICOUNT.LE.NSEAMS) GO TO 600 C C** END UNTIL C C CALL GRSTOP C STOP END  -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 2 1 22 23 24 25 26 27 28  o f ARCLEN.FOR  a t 14:39:10 o n AUG  SUBROUTINE C C C C  c c c c c c c  170 -  9,  1984 f o r CC1d=DAC0 P a g e  ARCLEN(R,DELS)  T H I S SUBROUTINE  C A L C U L A T E S ARC  LENGTH  S.  INPUTS: R ( 3 , 3 ) = P 0 S I T I 0 N VECTORS  AT POINTS  1-1.  OUTPUT: DELS=ARC  LENGTH  FROM POINT  I TO  REAL R ( 3 , 3 ) , X . Y . Z . L A S T X , L A S T Y , L A S T Z . D E L S DOUBLE P R E C I S I O N DELX. DELY, DELZ X = R(3. 1 ) Y=R(3,2) Z=R(3.3) LASTX=R(2,1) LASTY=R(2.2) LASTZ=R(2,3) DELX=X-LASTX DELY=Y-LASTY DELZ=Z-LASTZ DELS=DSQRT(DELX**2+DELY**2+DELZ**2) RETURN END  POINT  1  -  171  -  L i s t i n g of CADFIL.FOR a t 14:39:12 on AUG  9, 1984 f o r CC1d=DAC0 Page  1  V  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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  c  SUBROUTINE CADFIL C C THIS SUBROUTINE CONVERTS CAD DATA FILE INTO ARRAYS IN MAIN C MEMORY FOR FAST PROCESSING. C C INPUTS: NONE C C OUTPUTS: NONE C C COMMON BLOCKS CHANGED: /DATA/ C C FILES READ: OBJLST, FILES LISTED IN OBJLST C C OBJLST- CONTAINS LIST OF SEPARATE OBJECT CAD FILES C IE. FILE1- WORKPIECE CAD FILE C FILE2- TABLE CAD FILE (MOVING PART) C FILE3- TABLE CAD FILE (FIXED PART) C FILE4- ROBOT FOREARM C FILE5- ROBOT HAND AND TOOL C C DATA ARRAYS (COMMON): C VERT(1000,3)- VERTEX LIST Vx.Vy.Vz C FACES(100,4)- FACE PARAMETERS U=(Ux,Uy,Uz),P C WHERE U=SURFACE NORMAL C P=DISTANCE TO ORIGIN C IOBJ(10,2)LOCATION OF FIRST FACE,NUMBER OF FACES C IN FACE INDEX ' IFACE' C IFACE(100,2)- LOCATION OF FIRST VERTEX,NUMBER OF C VERTICES IN VERTEX INDEX 'IVERT' C C FOR EACH FACE, THE ROUTINE REPEATS THE FIRST VERTEX AT THE END OF C THE VERTEX LIST SINCE EVERY FACE IS A CLOSED POLYGON. THIS C SIMPLIFIES LATER PROCESSING. C IE. FOR A FACE WITH 3 VERTICES. V1.V2.V3. THE VERTEX LIST WOULD C CONTAIN V I . V2, V3. V1. C C SUBROUTINES CALLED: OPEN C RDFILE C SHUT C »«*.**»,««»*»«*«*****.«.**.»***.**»«**»«*»«««*****»*******«****.**..»«»***»»** C BYTE FILE(20),0BJLST(20) REAL VERT(1000.3).FACES(100,4) INTEGER IOBJ(10,2),IFACE(100,2) C COMMON /OBJLST/OBJLST COMMON /DATA/VERT.FACES,IFACE,IOBJ C C***»* INITIALIZE COUNTERS C NVERT=1 NFACE=1 N0BJ=1 C C** REPEAT UNTIL THERE ARE NO MORE OBJECT FILES C  - 172 -  Listing  59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 111 112 113 114 1 15 1 16  o f CADFIL.FOR  105  a t 14:39:12 o n AUG  9, 1984 f o r CC1d=DAC0 P a g e  2  CONTINUE  C C OPEN OBJECT  c  CALL  c c**»** c  OPEN(1.OBJLST,4,1)  FIND OBJECT  c  IF  THE END OF THE OBJECT  IF  ( I E N D . E O . 1 . ) GO TO 5 0 0  c c * * * * *OPEN c  THE CURRENT OBJECT  LIST  I S REACHED, GO TO END  FILE  CALL O P E N ( 1 . F I L E , 4 , 1 )  c c***»* c  STORE  THE LOCATION  OF THE F I R S T FACE OF OBJECT  'NOBJ'  IOBJ(NOBJ,1)=NFACE  c  INITIALIZE  c C**  FILE  CALL R D F I L E ( 1 , N O B J , F I L E . 2 0 , I E N D ) C A L L SHUT(1 )  c  c  LIST  FACE  COUNTER  NUFACE=0 R  3  c 100 c c c***«* c c c c c***«* c c c* * * * * c  EAT  UNTIL  THERE  ARE ND MORE  FACES  CONTINUE  READ THE FACE READ( 1 , * )  ID,FACE PARAMETERS AND NUMBER OF V E R T I C E S  ID,(FACES(NFACE.J).J=1,4),NUVERT  IF  ID=0,  THERE  ARE NO MORE  IF  (ID.EQ.O) GO TO 150  STORE THE LOCATION VERTICES  FACES  OF THE F I R S T VERTEX,  AND THE NUMBER OF  I F A C E ( N F A C E , 1) = NVERT IFACE(NFACE,2)=NUVERT READ AND STORE  THE COORDINATES OF THE V E R T I C E S .  DO  110 c c* * » * * c  110 J=1,NUVERT READ( 1 ,*) I T E M P , ( V E R T ( N V E R T , K ) , K = 1 . 3 ) NVERT=NVERT+1 CONTINUE REPEAT DO  THE F I R S T VERTEX  AT THE END OF THE L I S T  115 J=1 ,3 VERT(NVERT,J)=VERT(NVERT-NUVERT,J)  - 173 -  Listing 117 118 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145  of  C A D F I L . F O R a t 14:39:12 on AUG 115 C C***»* C  9, 1984 for  CC1d=DAC0  Page  3  CONTINUE NVERT=NVERT+1 INCREMENT  THE FACE POINTER AND THE FACE  COUNTER  NFACE=NFACE+1 NUFACE=NUFACE+1 C GO TO 100 C 150 C  CONTINUE CALL  C C****» C  SHUT( 1 )  STORE THE NUMBER  OF FACES OF OBJECT 'NOBJ'  I0BJ(N0BJ,2)=NUFACE C C****» C***** C  INCREMENT OBJECTS  THE OBJECT COUNTER,  N0BJ=N0BJ+1 C GO TO 105 C 500 C  CONTINUE RETURN END  AND REPEAT UNTIL THERE ARE NO MORE  -  L i s t i n g of 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 38 39 40 41 42 43  CALC5.FOR a t  -  1 4 : 3 9 : 1 3 on AUG  SUBROUTINE C C C C C  174  9.  1984 f o r CC1d=DAC0 Page  1  CALCS(CPHI,CTHE)  T H I S SUBROUTINE C A L C U L A T E S THE ORIENTATION FACTORS ' C P H I ' . ' C T H E ' AS FUNCTIONS OF THE TORCH ORIENTATION R E L A T I V E TO THE GRAVITY VECTOR. DOUBLE P R E C I S I O N H O T ( 4 , 4 ) , H O W ( 4 , 4 ) C0MM0N/C0EF/TW(3).AW(3),T(3).A(3) COMMON/MATRIX/HOT,HOW.SHOT(4.4),SHOW( 4 , 4 )  C C0NV=180./3.14159 C C***** C C****» C»»*«*  c*****  'CONV  CONVERTS ANGLES IN  RADIAN TO DEGREES  ROTATE THE SEAM TANGENT AND NORMAL VECTORS INTO THE WORLD COORDINATE FRAME. T W ( 3 ) , AW(3) ARE THE COMPONENTS OF TW.AW ALONG THE GRAVITY VECTOR.  C  C A L L ROTATE(SHOW.T.TW) C A L L ROTATE(SHOW,A,AW) C C***«* C**»** C  'PHI' IS THE ANGLE BETWEEN +Z TO THE WELD SEAM  (UPWARD)  AXIS AND THE TANGENT  P H I = A C 0 S ( T W ( 3 ) / S Q R T ( T W ( 1 )**2+TW(2)**2+TW(3 ) * * 2 ) ) C C***»* C***»* C  ' T H E T A ' IS THE ANGLE BETWEEN +2 TO THE WELD SEAM  (UPWARD)  AXIS AND THE NORMAL  THETA=AC0S(AW(3)/SQRT(AW(1) * « 2 + A W ( 2 ) * * 2 + AW(3)**2)) PHI=PHI*CONV C C***«* c*«**» C  DETERMINE ORIENTATION FACTORS THE ORIENTATION ANGLES  'CPHI'  AND  'CTHE'  USING  THETA=THETA *CONV CPHI=5.889*(10.0**-4.)*PHI+o.95 IF(PHI.GT.90.0) CPHI=-5.89*(10.0**-4.)*PHI+1.05 CTHE=-5.556*(10.O**-4.)*THETA+1. , I F ( T H E T A . G T . 9 0 . 0 ) CTHE = - 1 . 111 * ( 1 0 . O * * - 3 . )*THETA+1 C RETURN END  -  - 175 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31  o f CHECK.FOR a t 14:39:13 o n AUG  SUBROUTINE C C C C C C C  c c c c c c  OUTPUTS:  1984 f o r CC1d=DAC0 P a g e  CHECKS IF A SEGMENT HAS  c  INTEGER  ICOUNT-  TOTAL NUMBER OF  IDSEAM-  ID OF  c  c  SEGMENT TO BE CHECKED  IPREV- 0 I F SEAM NOT 1 OTHERWIZE  PREVIOUSLY  IPICK(IOO)  IPREV=1  200  c  100 1=1,ICOUNT I F ( I P I C K ( I ) .EQ.IDSEAM) GO CONTINUE IPREV=0 CONTINUE RETURN END  ID  SEGMENTS  DO 100  ALREADY  I P I C K ( I O O ) - ARRAY OF PICKED SEGMENT  c c  1  CHECK(IPICK,ICOUNT,IDSEAM,IPREV)  T H I S SUBROUTINE PICKED. INPUTS:  9,  TO  200  PICKED  BEEN  INTEGERS  - 176 -  L i s t i n g of 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 38 39  40  41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58.  CONFIG.FOR at  14:39:33 on AUG  SUBROUTINE  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 C C C C C C  9,  1984  for  CC1d=DAC0 Page  1  CONFIG(WPCDAT,IDSEAM.IDIR,ISTAT)  THIS SUBROUTINE TESTS ALL CONFIGURATIONS OF THE ROBOT UNTIL A FEASIBLE SOLUTION IS FOUND. INPUTS: WPCDAT- FILE NAME OF CURRENT SEAM TO BE SOLVED IDSEAM- ID OF CURRENT SEAM TO BE SOLVED IDIRDESIRED DIRECTION OF TRAVEL. OUTPUTS:  ISTAT-  FEASIBILITY STATUS 0=FEASIBLE TRAJECTORY FOUND 1=R0B0T ENVELOPE EXCEEDED 2=R0B0T JOINT LIMIT EXCEEDED  COMMON BLOCKS CHANGED: VALUES OF IC0NF(6): O O O O O O 0 0 0 1 0 0 0 0 1 0 O 0 0 0 1 1 0 0 1 1 1 0 0 0 1 1 1 1 0 0 1 1 0 0 0 0 110 100 WHERE:  /CONFIG/ (FOR PUMA 560  L A NF L A F L B NF L B F R A NF RAF R B NF RBF  TYPE RQBOT)  MODIFY CONFIGURATION IN THIS DIRECTION ONLY  V  R- RIGHT HANDED ARM CONFIGURATION L- LEFT HANDED ARM CONFIG. A- ELBOW ABOVE CONFIGURATION B- ELBOW BELOW CONFIG. NF- WRIST NOT FLIPPED F- WRIST FLIPPED  SUBROUTINES CALLED:  WORPOS  c******* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * » . * * * * » * * * * * » * » * * « « « * *  C  * C C  C 100 C C****» C  BYTE WPCDAT(20) REAL ALPHA(6),LEN(6),DIST(6),RMIN(6),RMAX(6),H0O(4,4), H6G(4,4),INH0O(4,4).INH6A(4,4) INTEGER IC0NF(6) COMMON /ROBOT/NRDOF,ALPHA,LEN,DIST,RMIN,RMAX,H0O.H6G,INHOO,INH6A COMMON /CONFIG/ICONF 1=2 J =2 K=2 CONTINUE TEST CURRENT CONFIGURATION FOR FEASIBILITY CALL  WORPOS(WPCDAT,IDSEAM,IDIR,ISTAT)  - 177 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102^ 103 104 105 106 107 108 109 1 10 1 1 1 112 113 1 14  o f CONFIG.FOR  a t .14:39:33 o n AUG  9.  1984 f o r CC1d=DACO  C  c***«* c c c***«* c***** c c*«»**  c  10 20 C  IF  A FEASIBLE  SOLUTION WAS  IF(ISTAT.EQ.O)  GO TO  FOUND, GO  200  E L S E TRY A D I F F E R E N T CONFIGURATION CONFIGURATIONS HAVE BEEN TRIED REVERSE WRIST F L I P  TO  UNTIL  CONFIGURATION  IF(IC0NF(4).EQ.O) IC0NF(4)=0 GO TO 20 CONTINUE IC0NF(4)=1 CONTINUE 1 = 1-1 I F ( I . G T . 0 ) GO TO  GO  TO  10  100  C  c  c****»  c  11 21  c  1=2 REVERSE  ELBOW ABOVE/BELOW  IF(IC0NF(3).EO.O) IC0NF(3)=0 GO TO 21 CONTINUE IC0NF(3)=1 CONTINUE J=d-1 I F ( J . G T . O ) GO TO  GO  CONFIGURATION  TO 11  100  C J =2 C  c»**»* c 300  12  22 C  REVERSE  L E F T / R I G H T ARM  CONTINUE IF(ICONF(1).EQ.0) ICONF(1)=0 IC0NF(2)=0 GO TO 22 CONTINUE ICONFC 1 ) = 1 IC0NF(2)=1 CONTINUE K=K-1 I F ( K . G T . O ) GO TO I F ( K . E Q . O ) GO TO  C 200 C  CONTINUE RETURN END  100 300  GO  CONFIGURATION  TO  12  Page  -  Listing  1 2 3 4 5 e 7 8 9  10 11 12 13 14 15 16  of  DOTPRD.FOR a t  C C C C C C C C  C  14:39:35  SUBROUTINE  INPUTS: OUTPUTS:  - '  o n AUG  9,  1984  for  CC1d=DAC0 P a g e  DOTPRD(V1,V2.PROD)  THIS SUBROUTINE V1(3),V2(3).  REAL C  178  FINDS THE DOT PRODUCT OF TWO  V 1 ( 3 ) , V 2 ( 3 ) - TWO VECTORS PROD- THE DOT PRODUCT PR0D=V1.V2  V1(3).V2(3)  PR0D=V1(1)*V2(1)+V1(2)*V2(2)+V1(3)*V2(3) RETURN END  1  - 179 -  Listing  o f DRAXES.FOR  1 2 3 4 5  6 7 8 9 10 11 12 13 14 15  a t 14:39:35  SUBROUTINE C C C  c  THIS  o n AUG  9, 1984 f o r CC1d=DAC0  Page  DRAXES  SUBROUTINE DRAWS THE WORLD COORDINATE  AXES.  *„ft**.********...*******.***....*»**,»*»»,»*»*•*,,***»» C C A L L M0VE3D(O..0.,0.) C A L L DRAW3D(1000..0..0.) C A L L M0VE3D(O.,0..0.) C A L L DRAW3D(0.,1000.,0.) CALL M0VE3D(0.,0.,0.) C A L L DRAW3D(0.,0..1000.) C RETURN END  1  - 180 -  L i s t i n g o f DRPANL.FOR at 14:39:35 o n 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  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. NOBJOUTPUTS:  IDENTITY OF OBJECT TO BE DRAWN NONE  COMMON BLOCKS ACCESSED: /DATA/,/VIEW/ COMMON BLOCKS CHANGED: NONE FILES READ OR WRITTEN: NONE SUBROUTINES CALLED:  DOTPRD LINDIS ROTAT TRANS  'IGL' SUBROUTINES:  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 -  Listing  59 SO 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 OO 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16  o f DRPANL.FOR  11 C c****» c***»* c****» c»**** c c**»** c c c c c****» c***«* c 201 c c***»* c***** c c c***»* c c c c c c***** c c 500 200 c c c****» c***»* c*»**« c***»* c»**** c***»* c****» c****» c c»**** c  at  14:39:35  o n AUG  9, 1984  for  CC1d=DAC0  Page  2  USIGHT(I)=-EYEPOS(I) CONTINUE THIS SECTION CALCULATES WHICH FACES WILL BE VISIBLE FROM THE GIVEN LINE OF SIGHT ('USIGHT'). THE DOT PRODUCTS OF 'USIGHT' AND THE FACE SURFACE NORMALS ARE TAKEN. A NEGATIVE RESULT INDICATES VISIBILITY. INITIALIZE THE VISIBLE FACE COUNTER NSHOW=0 DO FOR EACH FACE DO 200 I=1,NFACES GET SURFACE NORMALS 'UFACE' FROM THE FACE PARAMETER ARRAY 'FACES'. DO 201 J=1.3 UFACE(d)=FACES(IFASTA+I-1,J) CONTINUE ROTATE 'UFACE' BY TRANSFORMATION 'H' TO GET 'UFACE 1' IN WORLD COORDINATES CALL  ROTAT(H,UFACE.UFACE1)  TEST FOR 'VISIBILITYCALL D0TPRD(UFACE1.USIGHT,UDOT) IF(UDOT.GE.-.0001) GO TO 500 IF THE FACE IS 'VISIBLE', INCREMENT THE VISIBLE FACE COUNTER NSH0W=NSH0W+1 STORE VISIBLE FACE POINTERS IN ARRAY 'IFLIST' IFLIST(NSH0W)=IFASTA+I-1 CONTINUE CONTINUE THIS SECTION CALCULATES THE ORDER OF FACE GENERATION SO THAT CLOSER FACES ARE SUPERIMPOSED ON, OR COVER, MORE DISTANT FACES WEIGHTS ARE ASSIGNED TO EACH FACE, THE HIGHEST BEING THE CLOSEST. THE ALGORITHM IS AS FOLLOWS: GIVEN A PAIR OF EDGES, A WEIGHT OF ONE IS ASSIGNED IF THE SECOND EDGE IS BEHIND THE FIRST EDGE AS VIEWED ALONG THE LINE OF SIGHT. THIS IS TESTED FOR EACH EDGE OF A FACE AGAINST ALL EDGES IN THE OBJECT, AND THE SUMMED WEIGHT DIVIDED BY THE NUMBER OF EDGES ON THE FACE GIVES THE FINAL WEIGHT. DO FOR EACH VISIBLE FACE  - 182 -  Listing 117 1 18 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  o f DRPANL.FOR a t 14:39:35 o n AUG  9, 1984 f o r CC1d=DAC0  Page  3  DO 202 I=1,NSH0W C C***** C  INITIALIZE  FACE WEIGHT  WEIGHT(I)=0. C C***** C***«* C  FIND LOCATION OF F I R S T VERTEX 'NSTART' AND NUMBER OF V E R T I C E S 'NVERTS' FROM FACE PARAMETER ARRAY ' I F A C E ' . NSTART=IFACE(IFLIST(I).1) NVERTS=IFACE(IFLIST(I),2)  C C***«* C  DO FOR EACH EDGE  BOUNDING FACE  DO 203 J=1,NVERTS C C***»* C  204 C C***** C  FIND ENDPOINTS  A1.A2 OF FACE  EDGE.  DO 204 K=1,3 A1(K)=VERT(NSTART+J-1,K) A2(K)=VERT(NSTART+J,K) IF (<J . EQ . NVERTS ) A2(K ) =VERT( NSTART , K ) CONTINUE TRANSFORM  INTO WORLD COORDS  C A L L TRANS(H,A 1,P1) C A L L TRANS(H,A2,P2) C C*»»*» C  DO FOR A L L V I S I B L E  FACES  DO 2 0 5 L=1.NSHOW C C***** C  FINO SECOND FACE ISTART=IFACE(IFLIST(L),1) IVERTS=IFACE(IFLIST(L),2)  C DO 206 M=1,IVERTS C C***»* C  301 C C***** C  FIND ENDPOINTS  EDGES  DO 301 N=1,3 B1(N)=VERT(ISTART+M-1,N) B2(N)=VERT(ISTART+M.N) IF(M.EQ.IVERTS) B2(N)=VERT(ISTART,N) CONTINUE TRANSFORM  SECOND EDGE CALL CALL  C C***** C****» C  81,B2 OF SECOND FACE  INTO WORLD COORDS.  TRANS(H,B1,01) TRANS(H,B2,Q2)  T E S T I F SECOND EDGE I F TRUE, ISIGN=1.  I S BEHIND  FIRST  EDGE.  CALL LINDIS(P1,P2,Q1,Q2,USIGHT,ISIGN) C  - 183 -  Listing 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 21 1 212 213 214 215 216 217 218 219 220 22 1 222 223 224 225 226 227 228 229 230 231 232  o f DRPANL.FOR  a t 14:39:35 o n AUG  9, 1984 f o r CC1d=DAC0  Page  4  I F ( I S I G N . E O . O ) GO TO 6 0 0 C  c*»«»* c  IF  SECOND EDGE  I S BEHIND  F I R S T EDGE,  INCREMENT  FACE WEIGHT  WEIGHT(I)=WEIGHT(I) + 1 .  c  600 CONTINUE C 206 CONTINUE C 205 CONTINUE C 203 CONTINUE C C C * * * * * C A L C U L A T E AVERAGED WEIGHT C WEIGHT(I)=WEIGHT(I)/FLOAT(NVERTS)  c  202 C  c*»*»* c«**»* c  CONTINUE T H I S SECTION GENERATES WEIGHT.  A FACE  LIST  I N ORDER OF INCREASING  FACE  DO  700 21 1  210 C C  c***»* c c**»** c c c***«» c***»* c  c c***»* c****» c***** c c 803  210' 1 = 1.NSHOW TEST=10000. DO 211 J=1,NSHOW I F ( W E I G H T ( J ) . G E . T E S T ) GO TO 700 TEST=WEIGHT(J) JSAVE=d CONTINUE CONTINUE WEIGHT*JSAVE)=10000. IORDER(I)=JSAVE CONTINUE  T H I S SECTION  PERFORMS THE GRAPHIC  DO  FOR EACH V I S I B L E  DO  2 2 0 1 = 1..NSHOW  OUTPUT  FACE  FIND THE FURTHEST FACE 'NVERTS'.  STARTING  POINT  'NSTART', NO. OF V E R T I C E S  IND=IORDER(I) NSTART=IFACE(IFLIST(IND),1) NVERTS = I F A C E ( I F L I S T ( I N D ) , 2 ) DRAW THE FACE ON THE SCREEN GRAPHICS ROUTINE DEPENDS ON NUMBER OF V E R T I C E S ON F A C E . 3 , 4 , 5 , 6 . 7 , 8 , 9 , 1 0 CORNERED POLYGONS ARE ALLOWED. GO TO ( 8 0 3 , 8 0 4 , 8 0 5 , 8 0 6 , 8 0 7 , 8 0 8 , 8 0 9 , 8 1 0 ) , NVERTS-2 CONTINUE DO 333. d=1.NVERTS  - 184 -  Listing 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290  o f DRPANL.FOR a t 14:39:35 C C****« C  303 C  c*****  FIND  FACE  o n AUG  VERTICES  9, 1984 f o r CCid=DACO P a g e  FROM VERTEX  LIST  5  'VERT'  DO 3 0 3 K=1,3 V(K)=VERT(NSTART+J-1,K) CONTINUE TRANSFORM  VERTICES  INTO  WORLD COORDINATES.  C  333 C C«**** C  CALL TRANS(H,V,V1) VX3(J)=V1(1) I F ( V X 3 ( J ) . L T . . 0 1 ) VX3(d)=0. VY3(J)=V1(2) I F ( V Y 3 ( d ) . LT . .01) V Y 3 ( d ) = 0 . VZ3(d)=V1(3) I F ( V Z 3 ( d ) .LT. .01) V Z 3 ( J ) = 0 . CONTINUE DRAW  FACE  AS A F I L L E D PANEL  SURROUNDED BY A DARK BORDER.  CALL P A N L 3 D O , VX3.VY3.VZ3) CALL M 0 V E 3 D ( V X 3 ( 1 ) , V Y 3 ( 1 ) , V Z 3 ( 1 ) ) C A L L REMOVE CALL P0LY3D(3.VX3.VY3.VZ3) CALL D R A W 3 D ( V X 3 ( 1 ) . V Y 3 ( 1 ) . V Z 3 ( 1 ) ) CALL APPEAR GO TO 9 0 0 C 804  304  C 344  C 805  305  CONTINUE DO 344 d=1.NVERTS DO 304 K=1,3 V(K)=VERT(NSTART+d-1,K) CONTINUE CALL TRANS(H,V,V1) VX4(d)=V1(1) I F ( A B S ( V X 4 ( d ) ) . L T . . 0 1 ) VX4(d)=0. VY4(d)=V1(2) I F ( A B S ( V Y 4 ( d ) ) . L T . . 0 1 ) VY4(d)=0. VZ4(d)=V1(3) I F ( A B S ( V Z 4 ( d ) ) .LT. .01) V Z 4 ( d ) = 0 . CONTINUE CALL PANL3D(4,VX4,VY4,VZ4) CALL M O V E 3 D ( V X 4 ( 1 ) , V Y 4 ( 1 ) , V Z 4 ( 1 ) ) CALL REMOVE CALL P0LY3D(4,VX4.VY4.VZ4) CALL D R A W 3 D ( V X 4 ( 1 ) , V Y 4 ( 1 ) , V Z 4 ( 1 ) ) CALL APPEAR GO TO 9 0 0 • „ CONTINUE DO 355 d=1.NVERTS DO 305 K=1.3 V(K)=VERT(NSTART+d-1.K) CONTINUE CALL TRANS(H,V,V1) VX5(d)=V1(1) I F ( V X 5 ( d ) . L T . . 0 1 ) VX5(d)=0.  - 185 -  Listing 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348  o f DRPANL.FOR  355  C 806  306  366'  807  307  377  808  308  at  14:39:35  o n AUG  9,  1984  f o r CCid=DACO P a g e  VY5(d)=V1(2) I F ( V Y 5 ( J ) . L T . . 0 1 ) VY5(d)=0. VZ5(J)=V1(3) I F ( V Z 5 ( d ) . L T . . 0 1 ) VZ5(d)=0. CONTINUE CALL PANL3D(5,VX5,VY5.VZ5) CALL M 0 V E 3 D ( V X 5 ( 1 ) , V Y 5 ( 1 ) . V Z 5 ( 1 ) ) CALL REMOVE CALL P0LY3D(5,VX5,VY5.VZ5) CALL D R A W 3 D ( V X 5 ( 1 ) . V Y 5 ( 1 ) . V Z 5 ( 1 ) ) CALL APPEAR GO TO 900 CONTINUE DO 366 0=1.NVERTS DO 306 K=1,3 V(K)=VERT(NSTART+d-1,K) CONTINUE C A L L TRANS(H,V,V1) VX6(d)=V1(1) IF(VX6(d).LT..01) VX6(J)=0. VY6(d)=V1(2) IF(VY6(d).LT..01) VY6(J)=0. VZ6(d)=V1(3) IF(VZ6(d).LT..01) VZ6(d)=0. CONTINUE CALL PANL3D(6,VXG.VY6,VZ6) CALL M0VE3D(VX6( 1 ) , V Y 6 ( 1 ) . V Z 6 ( 1 ) ) CALL REMOVE CALL POLY30(6,VX6,VY6,VZ6) CALL DRAW3D(VX6(1 ) . V Y 6 ( 1 ) . V Z 6 ( 1 ) ) CALL APPEAR GO TO 9 0 0 CONTINUE DO 377 d=1.NVERTS DO 307 K=1,3 V(K)=VERT(NSTART+d-1,K) CONTINUE CALL TRANS(H,V,V1 ) VX7(d)=V1(1) I F ( A B S ( V X 7 ( d ) ) . L T . . 0 1 ) VX7(d)=0. VY7(d)=V1(2) I F ( A B S ( V Y 7 ( d ) ) . L T . . 0 1 ) VY7(d)=0. VZ7(d)=V1(3) I F ( A B S ( V Z 7 ( J ) ) . L T . .01) V Z 7 ( d ) = 0 . " CONTINUE CALL PANL3D(7.VX7,VY7.VZ7) CALL M 0 V E 3 D ( V X 7 ( 1 ) , V Y 7 ( 1 ) . V Z 7 ( 1 ) ) CALL REMOVE CALL P0LY3D(7,VX7,VY7,VZ7) CALL DRAW3D(VX7(1),VY7(1).VZ7( 1)) CALL APPEAR GO TO 9 0 0 CONTINUE DO 388 d=1,NVERTS DO 308 K=1,3 V(K)=VERT(NSTART+d-1,K) CONTINUE  6  - 186 -  Listing 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406  o f DRPANL.FOR  388  809  309  399  810  310  31 1  C 900 220  a t 14:39:35 o n AUG  9.  1984  f o r CCid=DACO P a g e  C A L L TRANS(H,V,V1) VX8(d)=V1(1) IF(ABS(VX8(J)).LT..01) VX8(J)=0. VY8(J)=V1(2) I F ( A B S ( V Y 8 ( J ) ) . L T . . 0 1 ) VY8(d)=0. VZ8(d)=V1(3) I F ( A B S ( V Z 8 ( J ) ) . L T . . 0 1 ) VZ8(d)=0. CONTINUE CALL P A N L 3 D ( 8 , V X 8 , V Y 8 , V Z 8 ) CALL M 0 V E 3 D ( V X 8 ( 1 ) . V Y 8 ( 1 ) , V Z 8 ( 1 ) ) C A L L REMOVE CALL P 0 L Y 3 D ( 8 . V X 8 , V Y 8 . V Z 8 ) CALL D R A W 3 D ( V X 8 ( 1 ) , V Y 8 ( 1 ) . V Z 8 ( 1 ) ) CALL APPEAR GO TO 9 0 0 CONTINUE DO 399 d=1.NVERTS DO 309 K=1,3 V(K)=VERT(NSTART+J-1,K) CONTINUE CALL TRANS(H,V,V1) VX9(J)=V1(1 ) I F ( A B S ( V X 9 ( J ) ) . L T . .01) V X 9 ( J ) = 0 . VY9(d)=V1(2) IF(ABS(VY9(J)).LT..01) VY9(J)=0. VZ9(d)=V1(3) IF(ABS(VZ9(d)).LT..01) VZ9(J)=0. CONTINUE CALL P A N L 3 D O , VX9.VY9, VZ9) CALL M 0 V E 3 D ( V X 9 ( 1 ) , V Y 9 ( 1 ) , V Z 9 ( 1 ) ) CALL REMOVE CALL POLY3D(9,VX9,VY9,VZ9) CALL DRAW3D(VX9( 1 ) . V Y 9 ( 1 ) , V Z 9 ( 1 ) ) C A L L APPEAR GO TO 9 0 0 CONTINUE DO 311 d=1,NVERTS DO 310 K=1,3 V ( K ) = V E R T ( N S T A R T + J - 1 ,K) CONTINUE CALL TRANS(H,V,V1) VX10(J)=V1(1) IF(ABS(VX10(J)).LT..01) VX10(J)=0. VY10(«J)=V1(2) I F ( A B S ( V Y 1 0 ( J ) ) . L T . . 0 1 ) VY10(d)=0. VZ10(d)=V1(3) IF(ABS(VZ10(d)).LT..01) VZ10(J)=0. CONTINUE C A L L PANL3DC10.VX10.VY10.VZ10) CALL M 0 V E 3 D ( V X 1 O ( 1 ) , V Y 1 0 ( 1 ) . V Z 1 0 ( 1 ) ) CALL REMOVE CALL P 0 L Y 3 D ( 1 0 , V X 1 0 , V Y 1 0 , V Z 1 0 ) CALL DRAW3D(VX10(1).VY10(1),VZ10(1)) CALL APPEAR GO TO 9 0 0 CONTINUE CONTINUE  7  - 187 -  itlng 407 408 409  of  DRPANL.FOR  at  C RETURN END  14:39:35 on AUG  9.  1984 f o r  CC1d=DAC0 Pa-  - 188 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f DRSEAM.FOR a t 14:39:57 o n AUG SUBROUTINE 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 c c  9,  1984 f o r CCid=DACO  Page  1  ORSEAM(HOW)  T H I S SUBROUTINE DRAWS THE INPUTS: H0W(4,4)-  SEAMS  MATRIX S P E C I F Y I N G WORKPIECE POSITION R E L A T I V E TO WORLD COORDINATES.  OUTPUTS: NONE INPUT  FILES:  OUTPUT F I L E S :  WPCLST NONE  COMMON BLOCKS ACCESSED: COMMON BLOCKS ALTERED:  /WPCLST/ NONE  SUBROUINES C A L L E D : NORM OPEN RDFILE ROTAT SHUT TRANS UNIT XPROD IGL  SUBROUTINES:  APPEAR CLOSEG DRAW3D M0VE3D OPNSEG REMOVE SETDET SETHIL  Q* ****************************************************************************  *  c c  c c  BYTE WPCDAT(20) BYTE W P C L S T ( 2 0 ) REAL W I N S I Z ( 6 ) REAL R ( 3 ) , H 0 W ( 4 , 4 ) , R W ( 3 ) ,A(3),AW(3),AUW(3) REAL T ( 3 ) , B ( 3 ) , B W ( 3 ) , B U W ( 3 ) , R L W ( 3 ) , T A N W ( 3 ) , U T A N W ( 3 ) COMMON /WPCLST/WPCLST •COMMON /WINSIZ/WINSIZ  Q*»*** I N I T I A L I Z E THE T I C K SPACING Q*****VIEW WINDOW S I Z E  PARAMETER  'TICSPA', BASED ON  c  c c**«** c c c****» C  TICSPA=(WINSIZ(2)-WINSIZ(1))/100. OPEN THE CALL  L I S T OF  SEAM DATA  FILES,  'WPCLST'.  OPEN(1,WPCLST,4,1)  READ THE NUMBER OF  SEAMS,  'NSEAMS'.  THE  - 189 -  Listing  o f DRSEAM.FOR  59 60 61 6  2  63 64 65 66 6  7  68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116  a t 14:39:57 o n AUG  9. 1984 f o r CC1d=DAC0  Page  2  READ( 1 ,* ) R E A D ( 1 , * ) NSEAMS c  C .***« C  S E T THE GRAPHIC CALL CALL  C c***»* C  A T T R I B U T E S : D E T E C T A B I L I T Y = T R U E , REMOVE=TRUE  S E T D E T ( - 2 , .TRUE. ) REMOVE  REPEAT FOR A L L SEAMS 00 100 1-1,NSEAMS  C C***»* C  CREATE A SEGMENT I CALL O P N S E G ( I )  C C***»* C  FIND AND OPEN THE SEAM CALL CALL  C C**»«* C  DATA  FILE  FOR SEAM I .  RDFILEd,1+2,WPCDAT,20,IEND) OPEN(2,WPCDAT,1,1)  S K I P THE F I R S T  RECORD  READ(2) C C***«* C * C c*»*** C  READ SEAM  TRANSFORM  INTO WORLD  COORDINATES  TRANS(HOW,R,RW)  MOVE TO F I R S T CALL  C c***»* C  FILE I  READ(2,END = 5 5 0 ) J , S . ( R ( K ) , K = 1 , 3 ) , ( T ( L ) . L = 1 , 3 ) . ( A ( M ) . M = 1 . 3 ) , (B(N),N=1,3)  CALL C C****« C  DATA  POINT  M0VE30(RW(1),RW(2),RW(3))  INITIALIZE  SIGN  SIGN=1. C C***»* C  22  33  DRAW  A CROSS AT SEAM  START  C A L L ROTAT(HOW,A,AW) C A L L UNIT(AW.AUW) C A L L ROTAT(HOW.B.BW) C A L L UNIT(BW.BUW) C A L L ROTAT(HOW,T,TANW) C A L L UNIT(TANW,UTANW) DO 22 K-1,3 RLW(K)=RW(K)+SIGN*2.*TICSPA*BUW( K ) CONTINUE CALL M0VE3D(RLW(1),RLW(2),RLW(3)) DO 33 K=1,3 RLW(K)=RW(K)-SIGN*2.*TICSPA*BUW(K) CONTINUE C A L L DRAW3D(RLW(1),RLW(2),RLW(3))  - 190 -  Listing 1 17 1 18 1 19 120 121 122 123 124 125 128 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  o f DRSEAM.FOR  a t 14:39:57 o n AUG  9, 1984 f o r CCid=DAC0 P a g e  3  DO  44  55  44 K=1 ,3 RLW(K)=RW(K)+SIGN*2."TICSPA*AUW(K) CONTINUE CALL M 0 V E 3 D ( R L W ( 1 ) . R L W ( 2 ) , R L W ( 3 ) ) DO 55 K=1 .3 RLW(K)=RW(K)-SIGN*2.*TICSPA*AUW(K) CONTINUE CALL DRAW3D(RLW( 1 ) , R L W ( 2 ) , R L W ( 3 ) )  C  c c c  Q.*«»»  c 150 C  c***»* c c c c c*»«** c* * * * * c  c c c c***** c*»»** c  INITIALIZE SLAST=0. IC0UNT=1  REPEAT FOR A L L POINTS ALONG  8  5 C 500 C  SEAM  CONTINUE READ SEAM DATA  FOR CURRENT POINT  READ(2,END=550) I N D E X , S . ( R ( K ) , K = 1 , 3 ) , ( T ( L ) ,L=1,3), (A(M),M=1.3),(B(N),N=1,3) I F ( ( S - S L A S T ) . L T . T I C S P A ) GO TO 5 0 0 SLAST=S TRANSFORM 'R','B','T' AND NORMALIZE CALL CALL CALL CALL CALL  INTO WORLD COORDS  'RW'.'BW','UTANW  TRANS(HOW.R.RW) ROTAT(HOW,B,BW) UNIT(BW.BUW) ROTAT(HOW,T,TANW) UNIT(TANW.UTANW)  CHANGE SIGN SIGN=-SIGN DRAW HERRINGBONE TICK PATTERN, POINTING IN DIRECTION OF SEAM DO  2  COUNTERS  2 K=1,3 RLW(K)=RW(K)+SIGN*TICSPA*BUW(K) CONTINUE C A L L M0VE3D(RLW(1),RLW(2) .RLW(3)) DO 8 K=1,3 RLW(K)=RW(K)+TICSPA*UTANW(K) CONTINUE CALL DRAW3D(RLW(1),RLW(2),RLW(3)) DO 5 K=1,3 RLW(K)=RW(K)-SIGN*TICSPA*BUW(K) CONTINUE CALL DRAW3D(RLW(1),RLW(2),RLW(3)) CONTINUE  - 191 -  Listing 175 176 177 178 179 180 181 182 183 184 185 186 187 188  o f DRSEAM.FOR a t  14:39:57  GO C 550 C  100 C  TO  150  CONTINUE CALL CLOSEG CALL S H U T ( 2 ) CONTINUE CALL  SHUT(1)  CALL  APPEAR  C C RETURN END  o n AUG  9,  1984  f o r CCid=DACO P a g e  4  - 192 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f DRWELD.FOR  a t 14:39:59 o n AUG  SUBROUTINE 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 c c c c c c  9,  1984 f o r CC1d=DAC0  Page  1  DRWELD(HOW,IDSEAM)  T H I S SUBROUTINE  FILLS  IN THE WELDED SEAMS  GRAPHICALLY  INPUTS: H0W(4,4)- MATRIX S P E C I F Y I N G P O S I T I O N OF WORKPIECE R E L A T I V E TO THE WORLD COORDINATE FRAME. IDSEAM-  I D E N T I F I E S THE SEAM NUMBER WHICH HAS BEEN WELDED (TO BE GRAPHICALLY F I L L E D I N ) .  OUTPUTS: NONE INPUT F I L E S :  WPCLST-  CONTAINS A L I S T  WPCDAT- DATA OUTPUT  FILES:  OF SEAM DATA  FILES  FOR A GIVEN SEAM  NONE  COMMON BLOCKS ACCESSED: / I O F I L E / COMMON BLOCKS ALTERED: NONE SUBROUTINES C A L L E D :  NORM OPEN RDFILE SHUT TRANS UNIT XPROD  IGL  APPEAR CLOSEG DRAW3D M0VE3D OPNSEG REMOVE SETDET SETHIL  SUBROUTINES:  c  c c c+* * * * c***«* c c c***** c  BYTE WPCDAT(20) BYTE W P C L S T ( 2 0 ) REAL W I N S I Z ( 6 ) REAL R ( 3 ) , H 0 W ( 4 , 4 ) , R W ( 3 ) , N 1 ( 3 . 3 ) , N 2 ( 3 , 3 ) , A ( 3 ) , A W ( 3 ) , R L ( 3 ) REAL T A N ( 3 ) , B ( 3 ) , B W ( 3 ) , B U W ( 3 ) , T ( 3 ) COMMON /WPCLST/WPCLST COMMON /WINSIZ/WINSIZ CALCULATE T I C K SPACING PARAMETER VIEW WINDOW S I Z E  'TICSPA'.BASED ON  TICSPA=(WINSIZ(2)-WINSIZ(1))/lOO. OPEN SEAM DATA  FILE  LIST,  C A L L OPEN(1.WPCLST,4, 1 )  'WPCLST'  GRAPHIC  - 193 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 1 1 1 1 12 1 13 1 14 1 15 1 16  o f DRWELD.FOR  a t 14:39:59 o n AUG  9,  1984 f o r CC1d=DAC0  Page  C READ(1,* ) R E A D ( 1 , * ) NSEAMS C  c***«* c****» c c  SET SEGMENT D E T E C T A B I L I T Y TRUE  SEGMENT  c c***** c c  CALL  OPEN  SEGMENT  OPNSEG(NSEG)  FIND AND  OPEN  SEAM OATA  FILE  C A L L RDFILE(1,IDSEAM+2,WPCDAT,20,1 END) CALL 0PEN(2.WPCDAT,1,1 ) SKIP  c  FIRST  RECORD  READ(2)  c  Q * * * * # READ F I R S T  c  *  c c****» c c Q***** c  POINT DATA  READ(2,END=550) (B (N ),N=1,3) TRANSFORM CALL  TO FIND SEAM  STARTING POINT.  d,S.(R(K),K=1,3),(T(L),L-1,3),(A(M).M=  INTO WORLD  COORDINATES  TRANS(HOW,R,RW)  MOVE TO STARTING POINT CALL  c  M0VE3D(RW(1),RW(2),RW(3))  SIGN=-1.  c  REPEAT FOR  c  150  c c****» c *  c c c****» c c  ID AND  NSEG=NSEAMS+IDSEAM  c  c  'REMOVE'  CALL S E T D E T ( - 2 , .FALSE. ) C A L L S E T H I L ( - 2 , .TRUE . ) CALL REMOVE DETERMINE  c  OFF, HI LIGHTING ON,  ALL POINTS ALONG  THE  SEAM  CONTINUE READ CURRENT POINT READ( 2 , END = 5 5 0 ) J . S . ( R(K ) , K= 1 , 3 ) , ( T( L ) , L= 1 , 3 ) . (A (Mi) , (B(N).N=1,3) TRANSFORM CALL  INTO WORLD  TRANS(HOW,R.RW)  ROTATE TRANSVERSE CALL  COORDINATES  VECTOR  ROTAT(HOW,B,BW)  'B' INTO WORLD  COORDINATES, 'BW'  -- 194 -  Listing 117 1 18 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152  o f DRWELD.FOR C****» C  a t 14:39:59 o n AUG NORMALIZE CALL  C C***»* C****» C  'BW'  9,  TO GET  1984 f o r CCid=DACO Page 'BUW'  UNIT(BW.BUW)  DRAW A Z I G - Z A G PATTERN OVER WELD  THE SEAM TO REPRESENT A F I L L E D  SIGN=-SIGN C DO 2 C  2 K=1,3 RW(K)=RW(K)+SIGN*TICSPA*BUW(K) CONTINUE  C A L L M0VE3D(RW(1),RW(2),RW(3)) C  6 C  DO 6 K=1,3 RW(K)=RW(K)-STGN*2.*TICSPA*BUW(K) CONTINUE CALL  DRAW3D(RW(1),RW(2),RW(3))  C  c***»*  THEN GO TO NEXT WELDED  C  GO C 550  TO  150  CONTINUE CALL CLOSEG CALL S H U T ( 2 )  C CALL S H U T ( 1 ) C CALL  APPEAR  C RETURN END  3  SEAM  - 195 -  Listing 1 2 3 4 5 6 7 8 9 10 11 12 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43  o f EYELOC.FOR  a t 14:40:00 o n AUG  9,  1984 f o r CC1d=DAC0 Page  1  SUBROUTINE EYELOC(VRP,EYEPOS ) C C C C C C C C C C C  T H I S SUBROUTINE ALLOWS THE USER TO CHOOSE THE ' I G L ' EYE P O S I T I O N . THE VIEW UP VECTOR IS ASSUMED TO BE THE Z-DIRECTION, SO AN ERROR OCCURS IF THE VIEW DIRECTION COINCIDES WITH THE Z AXIS. INPUTS:  VRP(3)-  THE  ' I G L ' VIEW REFERENCE  POINT.  OUTPUTS:  EYEPOSO)-  THE THREE COMPONENTS OF THE EYE POSITION IN WORLD COORDINATES.  C BYTE B E L ( 3 ) . Y E S , N 0 REAL E Y E P O S O ) , V R P ( 3 ) C DATA C C*» C 400 C  UNTIL  BEL/3»7/,YES/89/,NO/78/  (EYE POSITION  IS  VALID)  CONTINUE  c**«**  PROMPT THE USER FOR  THE  DESIRED  EYE  POSITION.  C WRITE(5,100) FORMAT($,' ENTER EYE P O S I T I O N R E A D ( 5 , * ) ( E Y E P O S ( K ),K= 1 ,3)  100 C C***** C  CHECK X,Y  COMPONENTS OF  VIEW  (X.Y.Z)  ')  DIRECTION.  I F ( E Y E P O S ( 1 ) . N E . 0 . . O R . E Y E P O S ( 2 ) . N E . 0 . ) GO  TO  200  C WRITE(5.SOO) BEL FORMAT(' EYE P O S I T I O N ON GO TO 4 0 0  300 C C** C 200 C  END  UNTIL CONTINUE RETURN END  Z-AXIS. TRY  AGAIN  ' .3A1 )  - 196 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f EXPERT.FOR a t 14:40:00 o n AUG SUBROUTINE 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 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  9,  1984 f o r CC1d=DAC0  Page  1  EXPERT(WPCDAT)  T H I S SUBROUTINE IS THE EXPERT WELDER MODULE. FIXED AND CONTINUOUSLY VARYING PARAMETERS ARE CHOSEN AS A FUNCTION OF G I V E N WORKSTATION AND WORKPIECE PARAMETERS. INPUT ARGUMENTS: WPCDAT- NAME OF SEAM DATA OUTPUT FILES  FILE  ARGUMENTS: NONE READ:  WPCOAT- F I L E CONTAINING SEAM DATA WKSTAT- F I L E CONTAINING ROBOT AND WELDER PARAMETERS.  F I L E S WRITTEN:  WPARAM- TEMPORARY F I L E CONTAINING CURRENT,SPEED DATA FOR EACH POINT ON THE CURRENT SEAM.  COMMON BLOCKS ACCESSED: /MATRIX/HOT,HOW,SHOT,SHOW COMMON BLOCKS CHANGED:  FORMAT OF F I L E  /EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR, WALPHA,BETA,STKOUT /WKSTAT/WKSTAT  '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 F I L E RECORDS  'WKSTAT'.  1 TO 26:-  CONTAIN HEADINGS,  FIXEO ROBOT  PARAMETERS.  RECORD 27: WSIZE  VARIABLE  DEFINITIONS: JTYPE-  CODE  I D E N T I F Y I N G JOINT  XSECT- WELD CROSS  TYPE  SECTIONAL AREA  T 1 . T 2 - THICKNESSES OF PARENT MATERIAL NPOINT- NUMBER OF POINTS DEFINED ALONG ICURVE- SEAM T Y P E - 0=STRAIGHT L I N E 1=SPACE CURVE  SEAM  - 197 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 7* 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 10O 101 102 103 104 105 106 107 108 109 1 10 1 1 1 112 1 13 1 14 1 15 1 16  o f EXPERT.FOR a t 14:40:00 o n AUG 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 c c  IFXVAR-  9. 1984 f o r CC1d=DAC0  WELDING  2  METHOD- 0=FIXED PARAMETERS (USED ONLY WHEN ICURVE=1) 1=VARYING PARAMETERS (USED ONLY WHEN ICURVE=1)  VOLT- WELDER VOLTAGE WSIZE- WIRE  Page  SETTING  SIZE  FEED- WIRE FEED RATE GFR-  S H I E L D I N G GAS FLOW RATE  CURRNT- WELDER  CURRENT  SPEED- WELDING  SPEED  T(3),A(3)-  VECTORS D E F I N I N G SEAM TANGENT AND NORMAL DIRECTIONS R E L A T I V E TO THE WORKPIECE  TW(3).AW(3)-  'T'.'A' R E L A T I V E TO WORLD  FRAME  SH0W(4.4)- HOMOGENEOUS TRANSFORMATION MATRIX DEFINING CURRENT WORKPIECE LOCATION AND ORIENTATION R E L A T I V E TO WORLD COORDINATES. SUBROUTINES C A L L E D :  CALCS OPEN SHUT WIRE 1 WIRE2 WIRE3  c  c  c c c****» c  BYTE WKSTAT(20).WPARAM(20) BYTE WPCDAT(20) BYTE YES,ANS REAL S H 0 T ( 4 , 4 ) , S H 0 W ( 4 , 4 ) REAL T E M P O ) DOUBLE P R E C I S I O N H 0 T ( 4 . 4 ) , H 0 W ( 4 . 4 ) COMMON /EXPERT/WPARAM,VOLT.WSIZE,FEED.GFR.WALPHA,BETA,STKOUT COMMON /WKSTAT/WKSTAT COMMON /MATRIX/HOT,HOW,SHOT,SHOW COMMON /W123/CURRNT,SPEED,XSECT,TX C0MM0N/C0EF/TW(3),AW(3),T(3) . A ( 3 ) DATA  YES/89/  INITIALIZE  PARAMETER  'XMIN'.  XMIN== 1 .  c c * * * * * OPEN c  FILES.  CALL OPEN(12.WPARAM,4,1)  - 198 -  Listing 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  o f EXPERT.FOR a t 14:40:00 o n AUG  9,  1984 f o r CC1d=DAC0  Page  3  C A L L OPEN(13,WPCDAT, 1 , 1 ) CALL OPEN(14,WKSTAT,4,1) C C****» C  SKIP  FIRST  28 RECORDS OF F I L E  'WKSTAT'.  DO 150 C C***** C  150 1 = 1 ,26 READ(14,») CONTINUE  READ  FIXED WELDING  PARAMETERS  FROM  'WKSTAT'.  READ( 14,*) WSIZE C C 300  WRITE(5,300) FORMAT(' ENTER METHOD OF WELDING: * PARAMETERS ',$)  0=FIXED  PARAMETERS,1=VARIABLE  C R E A D ( 5 , * ) IFXVAR C C****» C  READ  FIXED  READ(13) C****» C  DETERMINE  SEAM WELDING  PARAMETERS  FROM  'WPCDAT'.  JTYPE,XSECT,T1,T2,NPOINT,ICURVE THE MINIMUM  (SMALLER) THICKNESS  TX=AMIN1(T1,T2) C C * * * » * CHECK WIRE S I Z E AND BRANCH OFF TO CORRESPONDING ROUTINE C 220 I F ( W S I Z E . E O . 0 . 0 3 0 ) GO TO 240 I F ( W S I Z E . E O . 0 . 0 3 5 ) GO TO 250 I F ( W S I Z E . E O . 0 . 0 4 5 ) GO TO 260 W R I T E ( 5 , * ) 'WIRE S I Z E ERROR' ERR0R=2 GO TO 1001 C C * * * * » SUB. WIRE 1 DETERMINES WELDING PARAMETERS FOR WIRE S I Z E OF 0.030 C***»* INCHES. C 240 C A L L WIRE 1 C C * * * » * GO TO 500 FOR P O S S I B L E ORIENTATION ADJUSMENT C GO TO 5 0 0 C C * * * » * SUB. WIRE2 DETERMINES WELDING PARAMETERS FOR WIRE S I Z E OF 0.035 c****» INCHES. C 250 CALL WIRE2 C C***** GO TO 500 FOR P O S S I B L E ORIENTATION ADJUSTMENT C GO TO 500 C C * * * * » SUB. WIRE3 DETERMINES WELDING PARAMETERS FOR WIRE S I Z E OF 0.045 C****» INCHES. C  - 199 -  Listing 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 21 1 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232  o f EXPERT.FOR  1 4 : 4 0 : 0 0 o n AUG  at  CALL  260 C  c***** c 500  GO  9,  1984  f o r CC1d=DAC0  4  Page  WIRE3  TO 5 0 0 FOR O R I E N T A T I O N  CURNNT=CURRNT IF(ICURVE.EQ.1)  GO TO  ADJUSTMENT  700  C  c***** c  CURRENT  ADJUSTMENT  FOR S I M P L E  (STRAIGHT)  SEAMS  READ(13) INDEX,S.(TEMP(L),L=1.3).(T(d),J=1,3), ,K=1,3),(TEMP(L),L=1,3)  *(A(K)  c c****» C A L L ' C A L C S c* * * * *F A C T O R S FOR  ' T O D E T E R M I N E AND R E T U R N A D J U S T M E N T ( O R I E N T A T I O N ) S I M P L E S E A M S T H I S I S DONE FOR ONE OF THE P O I N T S Q * * * * * ON T H E S E A M S I N C E A L L T H E P O I N T S A R E I D E N T I C A L .  c  CALL CALCS(CPHI,CTHE) CURRNT=CURNNT*CPHI*CTHE GO TO 9 0 0  c  CONTINUE 700 C c * * » * * CURRENT  c  ADJUSTMENT  READ! 13,END = 7 5 0 )  FOR C O M P L E X  (CURVED)  SEAMS  I N D E X , S , ( T E M P ( L ) . L = 1 , 3 ) . ( T( J ) , J= 1 , 3 ) ,  *(A(K),K=1,3),(TEMP(L),L=1,3)  c c»**** c***** c  c c* «*** c»**** c  CALL ' C A L C S 'TO DETERMINE F A C T O R S P O I N T BY P O I N T  AND R E T U R N A D J U S T M E N T  CALL CALCS(CPHI.CTHE) CURRNT=CURNNT*CPHI*CTHE I F ( I F X V A R . E Q . O ) GO TO 8 5 0 RECORD C A L C U L A T E D CURRENT SEAMS. WRITE(12.*)  AND S P E E D  POINT  BY  POINT  Q * * * * * FOR F I X E D W E L D I N G P A R A M E T E R S M I N I M U M A D J U S T M E N T c***«* ( T H E M A X I M U M A D J U S T M E N T F A C T O R I S 1 . 0 0 ) 850 C 750 C  FOR C O M P L E X  CURRNT,SPEED  c c  (ORIENTATION)  FACTOR  IS  USED  XMIN=AMINKXMIN.CPHI*CTHE) GO TO 7 0 0 CONTINUE IF(IFXVAR.EQ.1)  GO TO  1001  C FIXED  c c c c c c c  WELDING  PARAMETERS,  OVERRIDE  O P T I O N MAY B E  USED  CURRNT=CURNNT*XMIN  OBSERVE  AND/OR  OVERRIDE  ********************  - 200 -  Listing 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290  of  EXPERT.FOR C 900 301 325 C 201 302  352 C 202 303  C 203 304  C 204 305  C 205 306  C 206 307  C 207 308  a t 14:40:00 on  AUG  9.  1984 f o r  CCid=DAC0  Page  WRITE(5,301) F O R M A T C DO YOU WISH TO OBSERVE/OVERRIDE(Y/N)? R E A D ( 5 , 3 2 5 ) ANS FORMAT(A1 ) IF (ANS.NE.YES) GO TO 1000 W R I T E ( 5 . 3 0 2 ) CURRNT F O R M A T C PRESENT CURRENT-'.F6.1.'AMPS..DO * OVERRIDE(Y/N)? '.$) R E A D ( 5 , 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 202 WRITE(5,352) F O R M A T C ENTER NEW VALUE '.$) R E A D ( 5 . * ) CURRNT  YOU  W R I T E ( 5 , 3 0 3 ) VOLT F O R M A T C PRESENT V O L T A G E - ' , F 6 . 2 . ' ( V O L T S ) . . D O * OVERRIDE(Y/N)? ',$) R E A D ( 5 . 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 203 WRITE(5,352) READ ( 5 , * ) VOLT W R I T E ( 5 . 3 0 4 ) FEED FORMAT(' PRESENT WIRE FEED=' .F6. 1 , ' ( I P M ) . .DO * OVERRIDE(Y/N)? ',$) R E A D ( 5 , 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 204 WRITE(5,352) READ ( 5 . * ) FEED  5  ',$)  WISH TO  YOU  YOU  WISH TO  WISH TO  W R I T E ( 5 , 3 0 5 ) GFR FORMATC PRESENT GAS FLOW R A T E - ' , F 6 . 2 , ' ( C F H ) . . D O YOU * OVERRIDE(Y/N)? ',$) R E A D ( 5 , 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 205 WRITE(5.352) READ ( 5 , * ) GFR W R I T E ( 5 , 3 0 6 ) SPEED FORMAT(' PRESENT SPEED-' ,F6.2, ' ( I P M ) . .DO * OVERRIDE(Y/N)? '.$) R E A D ( 5 . 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 206 WRITE(5.352) READ ( 5 . * ) , SPEED  YOU  WISH TO  W R I T E ( 5 , 3 0 7 ) STKOUT FORMATC PRESENT S T I C K - O U T - ' , F 6 . 3 . ' ( I N C H E S ) . . D O YOU * OVERRIDE(Y/N)? ',$) R E A D ( 5 , 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 207 WRITE(5,352) READ ( 5 , * ) STKOUT W R I T E ( 5 , 3 0 8 ) WALPHA F O R M A T C PRESENT LONGITUDINAL  WIsH TO  WISH TO  ANGLE-',F6.2,'(DEG)..DO  YOU  WISH  - 201 -  Listing 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 31 1 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330  o f EXPERT.FOR  *  C 208 309  C 209  *  a t 14:40:00 o n AUG  W R I T E ( 5 , 3 0 9 ) BETA FORMATC PRESENT TRANSVERSE OVERRIDE*Y/N)? ',$) R E A D ( 5 , 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 209 WRITE(5.352) READ ( 5 , * ) BETA  c  YOU  WISH TO  »*»*»»»«*..«*••*«.»«•»**«».»»*»»»»«*».«**«*»»«********»**«****««**  CALCULATED  DO  c  ANGLE=',F6.2,'(DEG)..D0  CONTINUE IF(IFXVAR.EO.1.AND.ICURVE.EO.1)  c c * * * *'* RECORD * c  c  6  CONTINUE GO TO 9 0 0  c  120 C 1001  1984 f o r CC1d=DAC0 P a g e  TO OVERRRIDE(Y/N)7 ',$) R E A D ( 5 , 3 2 5 ) ANS I F ( A N S . N E . Y E S ) GO TO 208 WRITE(5,352) READ ( 5 , * ) WALPHA  C C C  1000  9,  CURRENT AND  120 1=1,NPOINT W R I T E ( 1 2 , * ) CURRNT,SPEED CONTINUE CONTINUE CLOSE  CALL CALL CALL  FILES. SHUT(12) SHUT(13) SHUT(14)  RETURN END  GO  SPEED  TO  1001  FOR  SIMPLE  SEAMS  -  Listing 1 2 3 4 5 6 7 Q  9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24  o f HMULT.FOR  202 -  a t 14:40:20 o n AUG  9, 1984 f o r CC1d=DAC0 Page  1  SUBROUTINE HMULT(A,B,C) C C C C C C  THIS  SUBROUTINE M U L T I P L I E S TWO 4X4 HOMOGENEOUS TRANSFORMATIONS. C=AB, WHERE A, B, C ARE HOMOGENEOUS TRANSFORMATIONS.  THE M U L T I P L I C A T I O N  I S DOUBLE  PRECISION.  (;*,,**»»*»***,,•*«*,**,*»,**,»*»,***.,,*,«**•»**.»**.*»*».»*****.****. C DOUBLE P R E C I S I O N  A(4,4 ) , B ( 4 . 4 ) , C ( 4 , 4 )  C DO  2C0 10O C  100 1=1.3 DO 2 0 0 d=1,4 C(I, d) = A (I,1)*B(1,d )+A(I,2)*B(2,J)+A(I,3)*B(3,d) IF(d.E0.4) C(I,J)=C(I,J)+A(I.d) C(4,1)=0. C(4,2)=0. C(4,3)=0. C(4,4)=1. CONTINUE CONTINUE  RETURN END  - 203 -  Listing 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24  o f HMULTS.FOR  a t 14:40:21  o n AUG  9, 1984 f o r CC1d=DAC0  Page  1  SUBROUTINE HMULTS(A.B,C ) C C C C C C  T H I S SUBROUTINE M U L T I P L I E S TWO 4X4 HOMOGENEOUS TRANSFORMATIONS C=AB, WHERE A, B, C ARE HOMOGENEOUS TRANSFORMATIONS. THE  MULTIPLICATION  IS " ' S I N G L E * *  PRECISION.  c c  REAL A ( 4 , 4 ) , B ( 4 , 4 ) , C ( 4 , 4 ) DO  200 100 C  100 1=1,3 DO 200 J=1,4 C ( I , d ) = A ( I , 1 )*B( 1 , d ) + A ( 1 , 2 ) * B ( 2 . « J ) + A ( I , 3 ) * B ( 3 . J ) IF(d.E0.4) C(I,J)=C(I,J)+A(I,J) C(4,1)=0. C(4,2)=0. C(4,3)=0. C(4.4)=1. CONTINUE CONTINUE RETURN END  - 204 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f INIT.FOR  a t 14:40:21 SUBROUTINE  C C C C C C C C C C C C  o n AUG  9, 1984 f o r CC1d=DAC0 P a g e  1  INIT(NSEAMS)  T H I S SUBROUTINE I N I T I A L I Z E S THE VALUES OF A L L CONSTANT PARAMETERS WHICH ARE STORED IN F I L E S AND SAVES THEM IN COMMON BLOCKS. INPUTS: NSEAMS-  THE NUMBER OF SEAMS TO BE WELDED  OUTPUTS: NONE COMMON BLOCKS CHANGED:  c c c c c c c c c c c c c c c c  FILES  READ: WKSTATTABLE-  /ROBOT/ - ROBOT GEOMETRIC PARAMETERS /TABDAT/ - FIXED TABLE TRANSFORMATIONS /TOLROT/ - NUMBER AND S I Z E OF TOOL ROTATIONS ABOUT TOOL AXIS FOR 6 DEGREE OF FREEDOM ROBOT.  ROBOT DESCRIPTION TABLE DESCRIPTION  F I L E S CREATED: ROBLST- L I S T OF ROBOT COORDINATE OUTPUT F I L E S ROBDAT- ROBOT COORDINATE F I L E S LOCSET- F I L E RECORDING WELDING SEQUENCE. ROBOT CONFIGURATIONS AND TABLE POSITIONS. SUBROUTINES C A L L E D :  MATINV OPEN RDFILE SHUT  c  * c  c c c***»* c c c c***** c  BYTE T A B L E ( 2 0 ) . R O B L S T ( 2 0 ) . L O C S E T ( 2 0 ) . W K S T A T ( 2 0 ) DOUBLE P R E C I S I O N HTW(4,4) REAL 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 ) , INH0O(4,4).INH6G(4,4),SHTW(4,4) REAL DELTA INTEGER I C 0 N F ( 6 ) . I M A X COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON  READ  /ROBOT/NRDOF,ALPHA,LEN.DIST.RMIN,RMAX,H00.H6G,INHOO.INH6G /WKSTAT/WKSTAT /TABLE/TABLE /ROBLST/ROBLST /LOCSET/LOCSET /TABDAT/HTW,SHTW /TOLROT/IMAX.DELTA /CONFIG/ICONF  F I X E D ROBOT PARAMETERS  ****************************************  C A L L OPEN(1.WKSTAT,4, 1 ) READ(1,* ) READ THE NUMBER OF DEGREES OF FREEDOM R E A D ( 1 , * ) NRDOF READ(1,*)  - 205 -  L i s t i n g of 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 10O 101 102 103 104 105 106 107 108 109 1 10 111 112 1 13 1 14 115 1 16  INIT.FOR a t 14:40:21 on AUG 9. 1984 f o r C C i d = D A C 0 Page 2 READ(1,*) C READ THE ROBOT GEOMETRY PARAMETERS AND JOINT LIMITS c DO 10 1=1.6 READ(1,* ) LINK,ALPHA(I),LEN(I),DI ST(I),RMIN(I),RMAX(I) 10 CONTINUE c c***** READ INITIAL ROBOT CONFIGURATION c READ( 1,* ) READ( 1 ,* ) (ICONF(I),1 = 1,6) c CI * * * * * READ THE WORLD TO ROBOT BASE HOMOGENEOUS TRANSFORMATION 'HOO' c READ(1,* ) DO 20 1=1,4 READ( 1 ,* ) (H0O(I,J),J=1,4) 20 CONTINUE C c****» READ THE TOOL TO LAST LINK HOMOGENEOUS TRANSFORMATION 'H6G' c READ(1,*) DO 30 1=1.4 READ( 1 ,* ) (H6G(I,J) ,J=1,4) 30 CONTINUE C READ THE NUMBER AND SIZE OF THE DESIRED ROTATIONS ABOUT THE TOOL c****» AXIS (6 DEG. OF FREEDOM ROBOT ONLY) c READ( 1,*) READ( 1 , * ) IMAX,DELTA c CALL SHUT( 1 ) c DELTA=DELTA*3.14159/180. c c***** CALCULATE INVERSE MATRICES FOR USE LATER. c CALL MATINV(HOO,INHOO) CALL MATINV(H6G,INH6G) c c****« READ TABLE PARAMETERS. ************************* ********************** c CALL OPEN(1.TABLE,4, 1 ) c c***»* READ THE NUMBER OF DEGREES OF FREEDOM c READ(1,*) NUMDOF DO 40 1=1.NUMDOF+2 READ(1,*) CONTINUE 40 c c***** READ THE WORKPIECE TO TABLE TRANSFORMATION MATRIX 'HTW' c DO 50 1=1,4 READ(1,*) (HTW(I.J),J=1,4) CONTINUE 50  - 206 -  Listing 1 17 1 18 1 19 120 121 122 123 124 125 12S 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145  o f INIT.FOR a t 14:40:21 o n AUG  9,  1984 f o r CCid=DAC0 P a g e  3  C  c****» c  CONVERT  'HTW'  TO SINGLE P R E C I S I O N  'SHTW  DO  70 60 C  6 0 1=1,4 DO 70 J=1,4 SHTW(I,J)=HTW(I,U) CONTINUE CONTINUE CALL  SHUT(1)  C  c***»* c  80  CREATE  EMPTY ROBOT COORDINATE  F I L E S TO BE F I L L E D  LATER.  CALL OPEN(1,R0BLST,4.1) DO 80 ISEAM=1.NSEAMS CALL ROFILE(1,ISEAM+1,ROBOAT,20,IEND) CALL 0PEN(2,R0BDAT,6,1) CALL SHUT(2) CONTINUE CALL S H U T ( 1 )  C  c***»*  c****»  c c  CREATE EMPTY SEAM,TABLE P O S I T I O N AND F I L E TO BE F I L L E D LATER. CALL CALL  OPEN(1.LOCSET.6,1) SHUT(1)  RETURN END  ROBOT  CONFIGURATION  ***************  - 207 -  Listing 1 3 4 5 6 7 Q O  9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f JOINTS.FOR a t 14:40:22 o n AUG r C C  c c c  r» L.  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 c c c c c c c c c c c c c c c c  SUBROUTINE  9,  1984 f o r CC1d=DAC0  Page  1  J0INTS(H06,THETA,IFLAG)  T H I S SUBROUTINE CALCULATES THE JOINT ANGLES OF A 6 DEGREE OF FREEDOM MANIPULATOR GIVEN THE DESIRED LOCATION AND ORIENTATION OF THE GRIPPER. THE MANIPULATOR TO BE SOLVED MUST BE GEOMETRICALLY IDENTICAL TO A UNIMATION PUMA 5 6 0 . WITH THE JOINT DIMENSIONS BEING S P E C I F I A B L E . INPUTS: H 0 6 ( 4 , 4 ) -  OUTPUTS:  HOMOGENEOUS TRANSFORMATION MATRIX FROM THE S I X T H JOINT FRAME 'F6' TO THE ROBOT BASE WORLD FRAME 'FO'. 'H06' IS OF THE FORM [ n . o . a . p ] , WHERE n . o . a ARE UNIT VECTORS DESCRIBING THE ORIENTATION OF FRAME 'F6', AND p IS THE POSITION VECTOR FOR THE ORIGIN OF 'F6' R E L A T I V E TO 'FO'.  THETA(6):  A REAL ARRAY DEGREES.  CONTAINING THE JOINT ANGLES IN  IFLAG:  THIS FLAG IS SET TO 1 IF THE ROBOT WORK ENVELOPE IS EXCEEDED. OTHERWISE IT IS 0.  COMMON: / C O N F I G / I C O N F ( 6 ) - I N T E G E R ARRAY DESCRIBING DESIRED CONFIGURATION OF EACH JOINT, WHEN MORE ONE CONFIGURATION IS P O S S I B L E . POSSIBLE 0 0 0 0 1 1 1 1  COMMON:  FILES:  0 0 0 0 1 1 1 1  0 0 1 1 1 1 0 0  0 1 0 1 0 1 0 1  THAN  CONFIGURATIONS: 0 0 0 0 0 0 0 0  0 0 0 0 0 0 0 0  -  L L L L R R R R  A A B B A A B B  NF F NF F NF F NF F  WHERE-  R=RIGHT SHOULDER CONFIG. L=LEFT SHOULDER CONFIG. A=ELBOW ABOVE HAND B=ELBOW BELOW HAND NF=WRIST NOT F L I P P E D F=WRIST F L I P P E D  NOTE:  I C 0 N F ( 5 - 6 ) NOT  USED  /ROBOT/NRDOF,ALPHA(6),LEN(6).DIST(6),RMIN(6),RMAX(6), H00(4.4),H6G(4.4),INH0O(4,4),INH6A(4.4)  NONE  SUBROUTINES C A L L E D :  NONE  ************************************************ REAL  HOG(4,4),THETA(6),ALPHA(6),LEN(6),DIST(6),RMIN(6),RMAX(6)  -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 1 1 1 1 12 113 1 14 1 15 1 16  o f JOINTS.FOR  *  208 -  a t 14:40:22 o n AUG  9, 1984 f o r CC1d=DAC0 Page  2  REAL X.Y.TEMPI,TEMP2,TEMP3,TEMP4,TEMP5,TEMP6,H6G(4,4 ) , INH0O(4,4),H00(4,4),H06(4,4),Q(6),INH6A(4.4) INTEGER I C 0 N F ( 6 ) . I F L A G  C COMMON COMMON  c c c***»* c c c***** c  /ROBOT/NRDOF.ALPHA,LEN,DIST,RMIN,RMAX,H0O.H6G,INHOO,INH6A /CONFIG/ICONF  INITIALIZE  'IFLAG'  IFLAG=0 CALCULATE  COMPONENTS OF VECTOR  '0(3)'  DO  20 1=1,3 0(I)=H06(I.4)-DIST(6)*H06(I,3) CONTINUE 0(4)=1 .  20 C  FIND T H E T A ( 1 )  *************************************************  c TEST=Q(1)**2+0(2)**2-DIST(2)**2  c  IF(ENVELOPE  c  EXCEEDED) SET FLAG AND RETURN  I F ( T E S T . L T . O . ) GO TO 8 5 0  c  c c***»* c  TEMP=SQRT(Q(1)**2+Q(2)**2-DIST(2)**2) T1=0(2)*TEMP T2=0(1)*TEMP CHOOSE SOLUTION DEPENDING ON ROBOT CONFIGURATION. IF IF  (ICONF(1).GT.1) (ICONF(1).NE.1) T1=-T1 T2=-T2 CONTINUE  100  c  Y=T1-DIST(2)*0(1) X=T2+DIST(2)*0(2)  c c c»**** c  * c c***«* c c  GO TO 9 0 0 GO TO 100  THETA(1)=ATAN2(Y,X) FIND T H E T A ( 3 )  *************************************************  Y=(0(1)**2+Q(2)*»2+Q(3)**2-DIST(4)**2-LEN(2)**2-DIST(2)**2LEN(3)**2)/(2.*LEN(2)) RSQ=LEN(3)**2+DIST(4)**2 TEST2=RS0-Y**2 IF(ENVELOPE  EXCEEDED) SET FLAG AND RETURN  IF(TEST2.LT.O.)  GO TO 8 5 0  X=SQRT(TEST2) THETA1=ATAN2(LEN(3),-DIST(4))  c  -  209  -  L i s t i n g o f JOINTS.FOR a t 14:40:22 o n AUG 117 1 18 1 19 120 121 122 123 124 125 128 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  c***«* c c c c***«* c  c c****» c c c c***** c c  3  CHOOSE SOLUTION DEPENDING ON CONFIGURATION. IF (I CONF ( 3 ) . GT . 1 ) GO TO 900 IF (IC0NF(3).EO.1) X=-X THETA(3)=THETA1-ATAN2(Y.X) FIND THETA(2) ************************************ C3 = C0S(THETA(3) ) S3 = SIN(THETA(3) ) TEMP1=DIST(4)*C3-LEN(3)*S3 TEMP2=DIST(4)*S3+LEN(3)*C3+LEN(2) THETA1=ATAN2(TEMP1.TEMP2) RSQ=TEMP1**2+TEMP2**2 TEST3=RSQ-Q(3)**2 IF(ENVELOPE EXCEEDED) SET FLAG AND RETURN IF(TEST3.LT.O.) GO TO 850 Y=0(3) X=SQRT(TEST3) CHOOSE SOLUTION DEPENDING ON CONFIGURATION. IF(IC0NF(2).NE.0) X=-X THETA(2)=THETA1-ATAN2(Y.X)  c  FIND THETA(4) *************************************************  c  • *  c c***»» c  101 c c c***»* c  * *  c  9, 1984 f o r CC1d=DAC0 Page  Y=COS(THETA(1))*H06(2.3)-SIN(THETA(1))*H06(1.3) X=COS(THETA(1))*COS(THETA(2)+THETA(3))*H06(1,3) + SIN(THETA(1))*C0S(THETA(2)+THETA(3))*H06(2,3)SIN(THETA(2)+THETA(3))*H06(3.3) CHOOSE SOLUTION DEPENDING ON CONFIGURATION. IF(ICONF(4).GT.1 ) GO TO 900 IF(ICONF(4).NE.1) GO TO 101 x=-x Y = -Y CONTINUE THETA(4)=ATAN2(Y,X ) FIND THETA(5) ************************************************* TEMP1=(C0S(THETA(1))*C0S(THETA(2)+THETA(3))*COS(THETA(4))SIN(THETA(1))*SIN(THETA(4)))*H06(1,3) TEMP2=(SIN(THETA(1))*COS(THETA(2)+THETA(3))*COS(THETA(4))+ COS(THETA(1))*SIN(THETA(4)))*H06(2.3) TEMP3=C0S(THETA(4))*SIN(THETA(2)+THETA(3))*H06(3,3) TEMP4=C0S(THETA(1))*SIN(THETA(2)+THETA(3))*H06(1,3) TEMP5=SIN(THETA(1))*SIN(THETA(2)+THETA(3))*H06(2,3) TEMP6=<:0S( THETA( 2 )+THETA( 3 ) ) *H06( 3 , 3 )  - 210 -  Listing 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223  of  JOINTS.FOR  at  14:40:22 on  AUG  9,  1984  for  CC1d=DAC0 Page  4  Y=TEMP1+TEMP2-TEMP3 X=TEMP4+TEMP5+TEMP6 C T H E T A ( 5 ) = ATAN2(Y,X ) C c*»*»*  FIND T H E T A ( 6 ) * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  c *  * *  c  •  c  TEMP1 = ( - S I M T H E T A ( 1 ) ) * C O S ( T H E T A ( 4 ) ) - C O S ( T H E T A ( 1 ) )* C 0 S ( T H E T A ( 2 ) + THETA(3))*SIN(THETA(4)))*H06(1,1) TEMP2=(C0S(THETA(1) )*COS(THETA(4))-SIN(THETA(1))*C0S(THETA(2)+ THETA(3))*SIN(THETA(4)))*H06(2,1) TEMP3=SIN(THETA(4))*SIN(THETA(2)+THETA(3))*H06(3,1) TEMP4=(-SIN(THETA(1))*COS(THETA(4))-COS(THETA(1 ) * )C0S(THETA(2)+ THETA(3))*SIN(THETA(4)))*H06(1,2) TEMP5=(COS(THETA(1))*COS(THETA(4))-SIN(THETA(1))* C0S(THETA(2)+ THETA(3))*SIN(THETA(4)))*H06(2,2) T E M P 6 = S I N ( T H E T A ( 4 ) ) * S I N ( T H E T A ( 2 ) + T H E T A ( 3 ) ) * H 0 6 ( 3 . 2) Y=TEMP1+TEMP2+TEMP3 X=TEMP4+TEMP5+TEMP6 T H E T A ( 6 ) =ATAN2(Y,X)  c  c****»  c  30 C  CONVERT ANGLES  INTO DEGREES,  INSURE THAT  -180.LE. THETA.LE.180.  C0NV=180./3.14159 DO 30 1=1,6 THETA ( I ) = THETA ( I )*CONV IF(THETA(I).GT.180.) THETA(I)=THETA(I)-360. IF(THETA(I).LT.-180) THETA(I)=THETA(I)+360. CONTINUE GO  TO  800  C  c***** c 850  C c****«  P O S I T I O N OUTSIDE  SET  FLAG  CONTINUE' IFLAG=1 GO TO 800 ERROR MESSAGES  c 900 901 C 800 C  ENVELOPE,  ********************************** * * * * * * * * * * * * *  WRITE(5,901) F O R M A T C VALUE OF CONTINUE RETURN END  PARAMETER  ICONF NOT  0 OR  1')  -  Listing 1 2 3 4 5  6  7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f LIMITS.FOR  -  a t 14:40:47 o n AUG  SUBROUTINE C C C C C C C C C C C C C C C C C C C  211  9, 1984 f o r CC1d=DAC0  Page.  1  LIMITS(THETA.I FLAG.IJOINT)  THIS SUBROUTINE CHECKS THE CALCULATED JOINT ANGLES 'THETA' AGAINST THEIR PHYSICAL L I M I T S . I F ANY JOINT ANGLE IS EXCEEDED, ' I F L A G ' I S S E T .  INPUTS: T H E T A ( 6 ) - J O I N T COMMON  OUTPUTS:  ANGLES  IN DEGREES  /ROBOT/ALPHA,LEN.DIST,RMIN,RMAX,H00.H6G.INHOO. INH6A  I F L A G - 0-ANGLES WITHIN L I M I T S 1-JOINT L I M I T EXCEEDED I J O I N T ( G ) - A R R A Y WHICH I D E N T I F I E S JOINTS WHOSE L I M I T S HAVE BEEN EXCEEDED. I E . 0 0 0 1 0 0 - JOINT 4 EXCEEDED  c c c*»***»*«»*»**«***«**«*««*«»»*»*«***«»««***«*»*««*******«**«*  c c c c****» c  REAL T H E T A ( 6 ) , R M I N ( 6 ) , R M A X ( 6 ) , A L P H A ( 6 ) , D I S T ( 6 ) , L E N ( 6 ) REAL H 0 0 ( 4 , 4 ) , H 6 G ( 4 , 4 ) , I N H 0 0 ( 4 , 4 ) , I N H 6 A ( 4 , 4 ) INTEGER I J 0 I N T ( 6 ) COMMON /ROBOT/NRDOF,ALPHA,LEN,01 ST,RMIN,RMAX,HOO,H6G,INHOO,INH6A  INITIALIZE  'IJOINT','IFLAG'  DO 11  c  c c***** c***** c c c  11 1=1.6 IJ0INT(I)=O CONTINUE IFLAG=0 TEST EACH JOINT 'RMIN'.'RMAX'.  ANGLE  'THETA'AGAINST I T S S P E C I F I E D  DO 20 1=1,6 I F ( R M I N ( I ) . L E . R M A X ( I ) ) GO TO 200  200  IF(THETA(I).GE.RMIN(I).OR.THETA(I).LE.RMAX(I)) IFLAG=1 IJ0INT(I)=1 GO TO 100 CONTINUE  100  IF(THETA(I).GE.RMIN(I).AND.THETA(I).LE.RMAX(I)) IFLAG= 1 IJOINT(I ) = 1 CONTINUE  c  c  20  c  LIMITS  CONTINUE  GO TO 100  GO J?  100  -  Listing 59 60  of  LIMITS.FOR a t RETURN END  212  -  1 4 : 4 0 : 4 7 on AUG  9,  1984 f o r  CC1d=DAC0 Page  -  Listing 1  o *l 3 4 5 £  D 7 8 9 10 1 1 12 13 14  o f L I N D I S . F O R a t 1 4 : 4 0 : 4 7 o n AUG  Page  1  LINDIS(P1,P2,01.02.USIGHT,I SIGN)  T H I S S U B R O U T I N E F I N D S T H E S H O R T E S T V E C T O R FROM L I N E P 1 P 2 T O L I N E 0 1 0 2 . AND T E S T S I F P 1 P 2 I S I N F R O N T O F OR B E H I N D 0 1 0 2 F O R A G I V E N L I N E O F S I G H T V E C T O R 'USIGHT'.  c c c c c c c  INPUTS:  r* K,  c  OUTPUTS:  REAL  P1(3),P2(3)01(3).02(3)USIGHTO)ISIGN-  0 = 0 1 0 2 I N FRONT OF P 1 P 2 1= 0 1 0 2 B E H I N D P 1 P 2  UXP 1 ( 3 ) , U X P ( 3 )  INITIALIZE  c  E N D P O I N T S OF F I R S T LINE E N D P O I N T S OF S E C O N D L I N E L I N E OF S I G H T VECTOR  P1(3),P2(3),Q1(3),Q2(3),USIGHT(3),UP(3),U0(3),DELT(3)  REAL  ISIGN  ISIGN=0  c c***** c»**** c  FIND FIND DO  THE EDGE A VECTOR  VECTORS 'UP'.'UO' FROM T H E E N D P O I N T S ' D E L T ' F R O M E D G E ' U P ' T O E D G E 'UO'  10 1=1,3  UP(I)=P2(I)-PKD UQ(I)=Q2(I)-Q1(I) D E L T ( I ) = 0 1 ( I )-P 1 ( I ) CONTINUE  10  c c***** F I N D c* * * * * T H I S c CALL  c c* * * ** c  39 40 41  THE CROSS PRODUCT G I V E S U S T H E COMMON  U X P = U P X UO NORMAL O F T H E TWO  EDGES  XPROD(UP.UO.UXP)  TEST  I F 'UXP' I S ZERO  ( I E . 'UP'.'UO'  ARE P A R A L L E L )  D0T1=UXP(1 )**2+UXP(2)**2+UXP(3)**2  42 43  c  44 45 46 47 48 49  c  55 56 57 58  1984 f o r CCid=DACO  c c c  18 19 20 21 22  50 51 52 53 54  9,  \*  c  30 31 32 33 34 35 36 37 38  SUBROUTINE  r*  15 16 17  23 24 25 26 27 28 29  -  213  IF(D0T1.LT..001) GO T O 6 0 0 IF  500  c c***»* c  GO  'UXP' I S ZERO,  TO 5 0 0  FIND  THE CROSS  PRODUCT  U X P 1 = UP X  DELT  CONTINUE CALL XPR0D(UP,DELT,UXP1 ) IF  'UXP' I S A G A I N  ZERO,  'UP'='UO'.  THIS  I S AN  ERROR  CONDITION.  DIST=UXP1( 1 )**2+UXP1(2)**2+UXP1(3)**2  c c  IF(DIST.LT..001) THE CALL  COMMON  c  NORMAL  TO 7 0 0 ' U X P ' I S NOW  XPR0D(UP,UXP1,UXP)  c 600  GO  CONTINUE  FOUND  FROM  UXP  = UP  X UXP1  - 214 -  Listing  o f L I N D I S . F O R a t 14:40:47 o n AUG  59 60 61 62 63 64 65 6  6  67 63 69 70 7 1  72 73 74 75 76 77 78  c****» C**»*» C  FIND THE DOT SIGHT VECTOR CALL  c  C C****» C***** *»»»» C***»* C  9,  1984 f o r CC1d=DAC0  PRODUCT OF THE COMMON NORMAL 'USIGHT'.  'UXP' AND  THE LINE OF  FIND THE D I R E C T I O N OF 'UXP' R E L A T I V E TO EDGES 'UP'.'UQ' BY TAKING THE DOT PRODUCT OF ANY VECTOR 'DELT' DIRECTED FROM 'UP' TO 'UO' AND 'UXP'. THE RESULT WILL BE P O S I T I V E I F 'UXP' IS DIRECTED FROM 'UP' TO 'UO'. C A L L D0TPRD(DELT,UXP,D0T2) 'UQ'  IS BEHIND  'UP'  IF D0T*D0T2  C I F ( D 0 T * D 0 T 2 . G T . 0 . 0 0 0 1 ) ISIGN=1 C 700 C  2  DOTPRD(UXP.USIGHT,DOT)  C  c*****  Page  CONTINUE RETURN END  IS P O S I T I V E  - 215 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30  o f MATCNV.FOR  a t 14:40:48  SUBROUTINE C C C C C  o n AUG  9,  0H(4.4)  I F ( I C 0 N V . E Q . 2 ) GO TO C DO  1 I=1.4 DO 2 J=1,4 SH(I,d)=DH(I,d) CONTINUE CONTINUE GO TO 200 CONTINUE DO  4 3 C 2C0 C  1  T H I S SUBROUTINE CONVERTS A DOUBLE P R E C I S I O N 4X4 MATRIX TO SINGLE P R E C I S I O N IF IC0NV=1. THE INVERSE OCCURS IF IC0NV=2.  C  C 100 C  Page  MATCNV(DH,SH.ICONV)  REAL S H ( 4 , 4 ) DOUBLE P R E C I S I O N  2 1  1984 f o r CC1d=DAC0  3 1=1,4 DO 4 J=1,4 DH(I,d)=SH(I,J) CONTINUE CONTINUE CONTINUE RETURN END  100  - 216 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 1S 17 18 19 20 21 22 23 24 25 26 27 28 29 30  o f MATINV.FOR  a t 14:40:48 o n AUG  SUBROUTINE C C C C C C C C  9,  1984 f o r CCid=DACO Page  1  MATINV(H.HINV)  T H I S SUBROUTINE FINDS THE INVERSE TRANSFORMATION MATRIX 'H'. INPUTS: H ( 4 , 4 ) -  'HINV  OF A HOMOGENEOUS  HOMOGENEOUS TRANSFORMATION  OUTPUTS: H I N V ( 4 . 4 ) -  MATRIX  INVERSE OF 'H'  C C REAL  H(4.4),HINV(4,4)  C DO  2 1 C  1 1=1,3 DO 2 J=1 ,3 HINV(I,J)=H(J.I ) CONTINUE CONTINUE HINV(1.4)= (H(1,4)*H(1,1)+H(2,4)*H(2,1)+H(3,4)*H(3,1)) HINV(2.4)=-(H(1,4)«H(1,2)+H(2,4)*H(2,2)+H(3,4)*H(3,2)) HINV(3,4)=-(H(1,4)*H(1,3)+H(2,4)*H(2,3)+H(3,4)*H(3,3)) J  C HINV(4,1)=0. HINV(4,2)=0. HINV(4,3)=0. HINV(4.4)=1. C RETURN END  - 217 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f NOAP1.FOR  a t 14:41:10  o n AUG  9.  1984 f o r CC1d=DAC0  Page  1  SUBROUTINE NOAP1(H06.XYZOAT) C C T H I S SUBROUTINE FINDS THE PUMA LOCATION PARAMETERS X.Y.Z.O.A.T C GIVEN THE HOMOGENEOUS TRANSFORMATION 'H06' FOR THE PUMA 560 C ROBOT. C C C INPUTS: H 0 6 ( 4 . 4 ) -HOMOGENEOUS COORD TRANSFORMATION FROM 6TH LINK FRAME F6 TO ROBOT BASE FRAME FO. C C C OUTPUTS: X Y Z 0 A T ( 6 ) -ARRAY CONTAINING ROBOT LOCATION PARAMETERS X.Y.Z.O.A.T WHERE X.Y.Z ARE THE COORDINATES C OF THE TOOL CENTER REL. TO FO AND O.A.T ARE C C THE WRIST ROTATION ANGLES ABOUT THE C AXES OF FO. C C*«**«****************»*»»«*»*******^  c c c*»*** c c c*****  c****»  c  REAL REAL  H06(4,4).PO.PA.PT XYZ0AT(6)  CALCULATE  PO.PA.PT  C0NV=18O./3.14159 TEST IF THE APPROACH Z-AXIS)  VECTOR  (ALONG ROBOT BASE  400  I F ( A B S ( H 0 6 ( 1 , 3 ) ) . L T . .001 . A N D . A B S ( H 0 6 ( 2 . 3 ) ) . L T . .001) GO TO  c  c*»***  c  *  c  c»*»»*  c  IF  FALSE  P0=ATAN2(-HO6(1,3).H06(2,3)) TEMP=SIN(P0)*HO6(1,3)-COS(PO)*H06(2,3) PA=ATAN2(-H06(3,3),SIN(PO)*H06(1,3)-COS(PO)*H06(2,3)) PT=ATAN2(C0S(P0)*HO6(1,1)+SIN(PO)*H06(2,1),COS(PO)* H06(1,2)+SIN(P0)*HO6(2,2)) GO TO 4 1 0 IF  CONTINUE  410  I F ( H 0 6 ( 3 , 3 ) . G T . O . ) PA=90. I F ( H 0 6 ( 3 . 3 ) . L T . O . ) PA=-90. PT=0. P0=ATAN2(HO6(2,2),H06(1,2)) CONTINUE  c  c c***** c  '  TRUE  400  303 C  ' a ' IS VERTICAL  COORDINATES  X.Y.Z ARE THE SAME AS THE LAST  DO  303 J=1.3 XYZOAT(J)=H06(J.4) CONTINUE XYZ0AT(4)=P0*C0NV XYZ0AT(5)=PA*C0NV XYZ0AT(6)=PT*C0NV  COLUMN OF  'H06'  - 218 -  L i s t i n g of 59 60 61  NOAP1.FOR a t C  1 4 : 4 1 : 1 0 on AUG  RETURN END  9,  1984 f o r  CC1d=DAC0 Page  2  - 219 -  Listing  of N0AP5.FOR at  1  1 4 : 4 1 : 1 1  SUBROUTINE  on  AUG  9,  1984  f o r CC1d=DAC0 P a g e  1  NOAPS(ISOLN)  r» \*  3 4 5 e o 7 8 9 10 1 1 12 13 14 15 16 17 18 19  c c c  THIS ' P ' ,  AND  FINDS  THE  TOOL  REAL REAL REAL  22  *  23 24  AXIS  ALPHA(6 ) , L E N ( 6 )  INTEGER  COMMON  /ROBPOS/HOG  28  c c***** c  32  30  36  c  37  30  , 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 ) .  THE  VALUES  OF  TOOL  ' A ' . ' P ' .  THE  DIRECTION  H O G ( 1 , 4 )  THE  MOUNTING  TOOL  OFFSET  IN  X  R E L A T I V E  TO  THE  TOOL  FLANGE.  D T = H 6 G ( 1 , 4 )  c c***** c  CALCULATE  THETA1  T1=P(1 )-DT*A(  45  T 2 ' = P ( 2 ) - D T * A ( 2 )  46  R S 0 = T 1 * * 2 + T 2 * * 2  1 )  47  A T A N 1 = A T A N 2 ( T 2 , T 1 )  48  T 3 = S 0 R T ( R S 0 - D I S T ( 2 ) * * 2 )  49  I F ( I C O N F ( 1 ) . E O . 1 )  T 3 = - T 3  50  A T A N = A T A N 2 ( D I S T ( 2 )  , T 3 )  51  THET1=ATAN1-ATAN  55  VECTORS  1=1,3  44  54  FRAME  CONTINUE FIND  c***»* c  40  52  VECTOR  ' A ' .  / 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  P(I)=  35  53  LOCATION  A ( I ) = H O G ( 1 , 3 )  34  43  READ DO  33  42  VECTOR  PUMA  LOCATION  I C 0 N F ( 6 )  / C O N F I G / I C O N F  41  ORIENTATION  FREEDOM CENTER  c COMMON  39  OF  TOOL  I N H 0 O ( 4 , 4 ) , I N H 6 A ( 4 , 4 )  COMMON  38  THE  N ( 3 ) . 0 ( 3 ) , A ( 3 ) , P ( 3 )  27  30  DEGREE  H O G ( 4 . 4 )  26  31  5  GIVEN  r\j  21  29  THE  X . Y . Z . O . A . T  INPUTS: ISOLNFLAG SPECIFYING WHICH OF T H E TWO 5 D E G R E E OF c c FREEDOM SOLUTIONS TO FIND. 0 SOLUTION 1 c 1SOLUTION 2 c c c COMMON B L O C K S READ: / R O B O T / ROBOT PARAMETERS / C O N F I G / CURRENT ROBOT CONFIGURATION c c / R O B P O S / TOOL FRAME. H0G=[n,o.a.p] c c COMMON B L O C K S A L T E R E D : / R O B P O S / TOOL FRAME. H0G=[n.o,a,p] c c***********************»*************#**************************************** c  20  25  SUBROUTINE  PARAMETERS  c c***** c* » » * * c  CALCULATE  N ( 1 ) .  VALUE  ' I S O L N ' .  OF  THERE  56  C 1 = A ( 2 ) * * 2 + A ( 3 ) * * 2  57  C 2 = C 1 » A ( 3 ) * * 2  58  C 3 = C 1 * * 2 * T A N ( T H E T 1 ) * * 2  ARE  TWO  POSIBLE  SOLUTIONS,  SELECTED  BY  THE  - 220 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92  o f N0AP5.FOR  a t 14:41:11  o n AUG  9,  1984 f o r CC1d=DAC0  C4 = 2 . * C 1 * T A N ( T H E T 1 ) * A ( 1 ) * A ( 2 ) C5=C1*(A(1)**2+A(3)**2) N(1)=SQRT(C2/(C3+C4+C5)) IF(ISOLN.EQ.I) N(1)=-N(1) C  c****» c c c c • c c c****» c  c c c***** c c c c****» c  CALCULATE N ( 2 ) D1=N(1 )*A(1 ) * A ( 2 ) D2=SQRT(4.*D1**2-4.*(A(2)**2+A(3) **2)*N( A(3)**2)-A(3)**2) 03 = 2. * ( A ( 2 ) * * 2 + A ( 3 ) * * 2 ) N(2)=N(1)*TAN(THET1 ) CALCULATE N ( 3 ) N(3)=SQRT(1.-N(1 )**2-N(2)**2) DOT=ABS(N(1)*A(i)+N(2)*A(2)+N(3)* A(3)) IF(DOT.GT..OOO1) N ( 3 ) = - N ( 3 )  CALCULATE B A S I S CALL  200  '0'  XPROD(A,N,0)  WRITE B A S I S DO  c  VECTOR  VECTORS  200 1=1,3 H O G ( I , 1 )=N(I ) HOG(I,2)=0(I ) CONTINUE RETURN END  'N','0'  INTO  'HOG'.  Page  - 221 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33  o f NORM.FOR a t 14:41:11 o n AUG SUBROUTINE C C C C C C C C C C C  9,  1984 f o r CCid=DACO  Page  1  NORM(N1,N2,A)  T H I S SUBROUTINE B I S E C T S THE TWO  C A L C U L A T E S THE NORMAL VECTOR, 'A', PARENT SURFACE NORMALS 'N1'.'N2'.  WHICH  INPUTS: N1(3,3),N2(3,3)=N0RMAL VECTORS OF TWO INTERSECTION POINTS 1-1. I , 1+1.  SURFACES  OUTPUT: A ( 3 ) = U N I T BISECTOR  OF N1  AND  C C REAL  N1(3,3),N2(3,3).A(3).LX,LY,LZ.L  C D1=N1(3,1)+N2(3,1) D2=N1(3,2)+N2(3,2) D3=N1(3,3)+N2(3,3) C LX=D1**2 LY=D2**2 LZ=D3**2 C L=SQRT(LX+LY+LZ) C A(1)=D1/L A(2)=D2/L A(3)=D3/L C RETURN END  N2  AT POINT  1+1.  AT  - 222 -  L i s t i n g o f OPEN.FOR a t  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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  1 4 : 4 1 : 1 1 o n AUG  9,  1984 f o r CC1d=DAC0 Page  1  SUBROUTINE OPEN(IUNIT.NAME,ITYPE.IREC) C C THIS SUBROUTINE OPENS A FILE CALLED (NAME), AND ASSIGNS C IT TO LOGIC UNIT (IUNIT). THE TYPE OF FILE IS DETERMINED C BY (ITYPE). THIS SUBROUTINE IS COMPATIBLE WITH VAX-11 FORTRAN. C (IREC) IS THE RECORD LENGTH FOR DIRECT ACCESS FILES. C C INPUTS: C IUNIT=A VALID LOGIC UNIT NUMBER C NAME=A COMPLETE FILE NAME C ITYPE=FILE TYPE C 1=UNFORMATTED,EXI STING C 2=UNFORMATTED,NEW C 3 = UNF0RMATTED,DIRECT ACCESS,EXI STING C 4=F0RMATTED,SEQUENTIAL,EXISTING C 5=UNF0RMATTED,DIRECT ACCESS,NEW c 6=F0RMATTED,SEQUENTIAL,NEW c IREC=RECORD LENGTH (FOR FILE TYPE 5 ONLY) c c OUTPUTS: NONE. c c c».«.«»»»«*»««.*»*«««.*.«*«.,»«*•».».•«,.»» c BYTE NAME(20) c c** CASE OF (ITYPE) . c IF(ITYPE.LT.1.OR.ITYPE.GT.6) GO TO 600 GO TO (100,200.300,400.500.510) ITYPE c c** CASE (1) c c*** * * OPEN EXISTING, UNFORMATTED, SEQUENTIAL FILE. c 100 CONTINUE OPEN (UNIT=IUNIT,FILE=NAME,FORM='UNFORMATTED',STATUS='OLD', * ERR=10) GO TO 900 c c** CASE (2) c c***** OPEN NEW. UNFORMATTED, SEQUENTIAL FILE. c 200 CONTINUE OPEN (UNIT=IUNIT,FILE=NAME,FORM='UNFORMATTED',STATUS 'NEW', ERR=10) GO TO 900 c c*» CASE(3) c c*»»** OPEN EXISTING, DIRECT ACCESS FILE. c 300 CONTINUE OPEN (UNIT=IUNIT.FILE=NAME.ACCESS''DIRECT',STATUS='OLD', * ERR=10) GO TO 900 c 3  •  - 223 -  L i s t i n g o f OPEN.FOR a t 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98  1 4 : 4 1 : 1 1 on AUG  9,  1984 f o r CC1d=DAC0 Page  C** CASE(4) C c*** * * OPEN EXISTING, FORMATTED, SEQUENTIAL FILE. c CONTINUE 40O OPEN (UNIT=IUNIT,FILE=NAME,STATUS='OLD',ERR=10) GO TO 900 C CASE(5) c«* C c***»* OPEN NEW, DIRECT ACCESS FILE. c CONTINUE 500 OPEN (UNIT=IUNIT,FILE=NAME,FORM='UNFORMATTED', * ACCESS='DIRECT',RECL=IREC,STATUS-'NEW',ERR=10) GO TO 90O C c* * * * * OPEN NEW,FORMATTED FILE. c CONTINUE 510 OPEN (UNIT=IUNIT,FILE=NAME.STATUS='NEW,ERR=10) GO TO 900 C c** EL SE c CONTINUE 600 WRITE(5.20) FORMAT(' VALUE OF PARAMETER ITYPE NOT VALID') 20 GO TO 900 C CONTINUE 10 WRITE(5,30) FORMAT(' ERROR ENCOUNTERED IN OPEN STATEMENT IN 30 OPEN') * C C** END CASE C CONTINUE 900 RETURN END  2  - 224 -  :1ng o f 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  PROCES..FOR at 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 c c c c c c c c c c c c c  c  c  14:41:34 o n AUG  SUBROUTINE  9, 1984 f o r CC1d=DAC0  Page  1  PROCES  THIS PROGRAM READS THE WELDING PARAMETERS. R, N 1 , N2 FROM CAD F I L E . CALCULATES ARCLENGTH S. THE POSITION VECTOR R, AND THE TOOL FRAME AXES T. A, B. THE RESULTS ARE STORED I N AN OUTPUT F I L E . INPUTS: FROM CAD F I L E J T Y P E - JOINT TYPE CODE XSECT- WELD CROSS SECTIONAL AREA T1.T2- PARENT METAL THICKNESSES NPOINT- NUMBER OF LOCATIONS I N F I L E ICURVE- SEAM TYPE- 0=STRAIGHT LINE 1=SPACE CURVE INDEX=POINT INDEX. R ( 3 , 3 ) = P 0 S I T I 0 N VECTORS FOR POINTS 1 - 1 . I .1+1 . N 1 ( 3 . 3 ) , N 2 ( 3 . 3 ) = U N I T NORMAL VECTORS OF TW ]i SURFACES AT POINTS 1 - 1 . I . 1+1. OUTPUTS: TO OUTPUT F I L E . J T Y P E - JOINT TYPE CODE XSECT- WELD CROSS SECTIONAL AREA T1.T2- PARENT METAL THICKNESSES NPOINT- NUMBER OF LOCATIONS IN F I L E ICURVE- SEAM TYPE- 0=STRAIGHT LINE 1=SPACE CURVE INDEX- POINT INDEX S(3)=ARCLENGTH AT POINTS 1 - 1 , I . 1+1. R ( 3 , 3 ) = P 0 S I T I 0 N VECTORS FOR POINTS 1 - 1 , I ,1 + 1 . T(3)=TANGENT VECTOR OF CURVE AT POINT I . A ( 3 ) = U N I T BISECTOR OF N1, N2 AT POINT I . B ( 3 ) = C R 0 S S PRODUCT T X A. F I L E S READ: CAD- THE CAD F I L E F I L E S WRITTEN: WPCDAT- THE OUTPUT F I L E SUBROUTINES CALLED:  ARCLEN NORM OPEN SHUT TANGEN XPROD  I M P L I C I T DOUBLE P R E C I S I O N ( D ) BYTE C A D ( 2 0 ) , WPCDAT(20) REAL R ( 3 , 3 ) , N 1 ( 3 . 3 ) , N 2 ( 3 , 3 ) . T ( 3 ) . A ( 3 ) . B ( 3 ) , S ( 3 ) COMMON /CAD/CAD COMMON /WPCDAT/WPCDAT COMMON /CADLST/CADLST COMMON /WPCLST/WPCLST DATA R/9*0./ DATA ICAD/1/,IOUT/2/  -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 1 1 1 112 113 1 14 1 15 1 16  of  PROCES.FOR c»*«**  at  225  -  14:41:34 on AUG  9,  1984 f o r  CC1d=DAC0  Page  OPEN CAD INPUT F I L E AS L.U. ICAD. OPEN OUTPUT F I L E AS L.U. IOUT.  C CALL CALL  OPEN(ICAD,CAD,1,1) OPEN(IOUT,WPCDAT,2,1)  C INITIALIZE  c c c**»** c***»* c c c  C**  *  c c»**** c* * * * * c* » * * *  c c***»* c c c c  Q* * * * *  c  555  556 C Q** * * *  c  READ JOINT T Y P E . X-SECTIONAL AREA, PARENT THICKNESSES 1ST RECORD OF 'CAD' AND WRITE THEM INTO 1ST RECORD OF READ(ICAD) JTYPE,XSECT,T1,T2,NPOINT,ICURVE WRITE(IOUT) JTYPE,XSECT,T1.T2,NPOINT,ICURVE  (INPUT  .LE. EOF)  CONTINUE  c***** c  c  'I FLAG'  CONTINUE  150 C 170 C  c  'S', FLAG  S(3)=0. IFLAG=0  WHILE  c  ARCLENGTH  READ INDEX, R, N1,*N2, FROM CAD READ(ICAD,END=555) (N2(3,L),L=1,3)  FILE.  INDEX,(R(3,I),I=1.3).(N1(3.K),K=  READ F I R S T TWO POINTS BEFORE BEGINNING CALCULATIONS SET FLAGS IF SECOND POINT OR LAST POINT IS READ. THESE POINTS REQUIRE A MODIFIED PROCEDURE. FIRST  POINT  I F ( I N D E X . E Q . 1 ) GO IFLAG=2 SECOND  TO  700  POINT  I F ( I N D E X . E Q . 2 ) IFLAG=1 GO TO 556 LAST  POINT  CONTINUE INDEX=INDEX+1 IF LAG = 3 CONTINUE  CALCULATE ARCLENGTH  ARCLENGTH S AT P O S I T I O N R. FROM R ( I - 1 ) TO R ( I )  'DELS' IS THE  CALL ARCLEN(R.DELS) S(3)=S(2)+DELS CALCULATE  THE TANGENT VECTOR  'T' AT P O S I T I O N  'R'  - 226 -  Listing 117 118 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171  o f PROCES.FOR  a t 14:41:34 o n AUG  9,  1984 f o r CCid=DACO  Page  3  C CALL  TANGEN(R,S,T.IFLAG)  C  c***»* c»*»*» c  CALCULATE THE NORMAL VECTOR 'NI'.'N2' CALL  c c c**»** c c c***»* c***** c c c»**** c * c c***«» c****-» . c 300  c 700  c c***** c  LATERAL VECTOR  C 100 C C** C C  c c  'B', B=A  X T (CROSS  PRODUCT)  THIS STEP ENSURES AN ORTHOGONAL  XPROD(T,B,A)  WRITE RESULTS  INTO OUTPUT  FILE.  dd=INDEX-1 WRITE(IOUT) dd,S(2),(R(2.I).I=1,3).(T(K),K=1,3),(A(L),L=1 (B(M),M=1,3) UPDATE CURRENT P O S I T I O N , POINT 1-1.  I , BY  REDEFINING POINT  -  DO  300 K=1,3 R(1,K)=R(2,K) CONTINUE S(1)=S(2) CONTINUE SIMILARLY,  REDEFINE POINT  DO  301  NORMALS  XPROD(A,T,B)  A=T X A (CROSS PRODUCT). FRAME. CALL  SURFACE  N0RM(N1,N2,A)  CALCULATE THE CALL  'A' FROM THE  301 K=1,3 R(2,K)'R(3.K) N1(2,K)=N1(3,K) N2(2.K)=N2(3,K) CONTINUE S(2)=S(3) IF  ( I FLAG.NE.3) GOTO  150  ENCl WHILE  CLOSE CALL CALL  INPUT  AND  SHUT(ICAD) SHUT(IOUT)  RETURN END  OUTPUT  FILES.  1+1  AS POINT I .  I AS  - 227 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f RDFILE.FOR  a t 14:41:35 o n AUG  9.  1984 f o r CCid=DACO  1  Page  SUBROUTINE RDF ILE(IUNIT,IRECNO.NAME.LEN,IEND) C C C C C  SUBROUTINE R D F I L E READS RECORD NUMBER 'IRECNO' OF A F I L E ON LOGICAL UNIT 'IUNIT'. THE RECORD IS RETURNED AS A CHARACTER STRING 'NAME' OF LENGTH 'LEN'.  c c c c c c c c c c c c c c c c  INPUTS:  I U N I T - LOGICAL UNIT NUMBER OF OPEN IRECNO- RECORD NUMBER TO BE READ  OPEN  FILE  OUTPUTS: NAME- BYTE ARRAY OF LENGTH 'LEN' CONTAINING CONTENTS OF RECORD 'IRECNO'. I E . BYTE NAME(LEN) LENLENGTH OF BYTE ARRAY. IEND- FLAG INDICATING END OF F I L E ENCOUNTERED DURING READ. IEND=0 READ SUCCESSFUL IEND=1 END OF F I L E ENCOUNTERED  FUNCTIONS C A L L E D :  Q*  LENGTH-  FINDS THE STRING  LENGTH  OF A CHARACTER  *********************************************************************•  c BYTE  c c c** c  IEND=0 IF  c c»**«* c****» c  300  c c** c  NAME(LEN)  (IRECNO IF  (IRECNO  1) GO  TO  100 STEP FORWARD TO RECORD  ' IRECNO'  REWIND IUNIT DO 30O 1 = 1 ,IRECNO-1 READ(IUNIT,150,END=400) CONTINUE READ(IUNIT.150.END=400) (NAME(J),d=1.LEN) GO TO 200  CONTINUE  Q** * * *  IF OF  c***»* c  END  F I R S T RECORD IS TO BE READ, REWIND F I L E F I R S T RECORD.  REWIND IUNIT READ(IUNIT,150,END=400) IF  200  CONTINUE  150  FORMAT(80A1)  c  .EO.  IF (IRECNO.GT.1) REWIND F I L E . AND READ CONTENTS.  10O  ' c  1)  ELSE  c  c c** c  .GT.  (NAME(J),J=1,LEN)  AND  READ CONTENTS  - 228 -  Listing  59 60 61 62 63 64 65 66 e 7  68 69 70 71 72 73 74  o f R D F I L E . F O R a t 14:41:35 o n AUG  c***»* c****»  9,  FIND LENGTH OF CHARACTER ZEROS.  1984 f o r CC1d=DAC0 STRING AND  L=LENGTH(NAME,LEN) NAME(L+1)=0 C GO TO 500 C  C 40O C 500  END OF F I L E CONTINUE IEND=1 CONTINUE RETURN END  ENCOUNTERED.  SET FLAG  2  PAD TO RIGHT WITH A S C I I  C  c*****  Page  'IEND'.  - 229 -  L i s t i n g of ROTAT.FOR a t 14:41:35 on AUG 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 13 20 21  c  9, 1984 f o r CC1d=DAC0 Page  1  SUBROUTINE ROTAT(H,A,B) C C THIS SUBROUTINE PERFORMS THE ROTATIONAL PART OF A C HOMOGENEOUS TRANSFORMATION ,B=H*A, WHERE H(4,4) IS A C HOMOGENEOUS TRANSFORMATION MATRIX AND A(3).B(3) ARE VECTORS. C C INPUTS: H(4,4)- HOMOGENEOUS TRANSFORMATION MATRIX C A(3) - VECTOR C C OUTPUTS: B(3) - VECTOR C *******»***************************************** ****************** C REAL H(4.4),A(3),B(3) C B(1)=H(1,1)*A(1)+H(1,2)*A(2)+H(1.3)*A(3) B(2)=H(2,1)*A(1)+H(2,2)*A(2)+H(2,3)*A(3) B(3)=H(3,1)*A(1)+H(3.2)*A(2)-HH(3.3)*A(3) C RETURN END  - 230 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f SCENE.FOR  a t 14:41:35 o n AUG  SUBROUTINE 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  9. 1984 f o r CCid=DACO  Page  1  SCENE  T H I S SUBROUTINE DRAWS THE WORKSTATION  ,OBJECT  AND SEAMS.  INPUTS: NONE OUTPUTS: NONE ' COMMON BLOCKS ACCESSED: COMMON BLOCKS CHANGED: FILES  /VIEW/./MATRIX/ /MATRIX/  READ OR WRITTEN: NONE  SUBROUTINES C A L L E D :  ' I G L ' GRAPHICS  DRAXES DRPANL DRSEAM TABPOS TABSET  SUBROUTINES:  CMCLOS CMOPEN DELSEG EYEBAL NEWPAG VRP3D  £*****«•* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  C BYTE B E L ( 3 ) . Y E S . N 0 C REAL V R P ( 3 ) , E Y E P 0 S ( 3 ) ,VAR(6) , S H O T ( 4 , 4 ) , S H O W ( 4 , 4 ) , H ( 4 , 4 ) REAL V E R T ( 1 0 0 0 ) . F A C E S ( 1 0 0 ) DOUBLE P R E C I S I O N HOT(4,4),HOW(4,4) C INTEGER  IOBJ(10,2),I FACE(100,2),IDIR(1),IPICK(100)  C DATA  BEL/3*7/,YES/89/,NO/78/  C COMMON / D A T A / V E R T , F A C E S , I F A C E , I O B J COMMON /VIEW/EYEPOS,VRP COMMON /MATRIX/HOT,HOW,SHOT,SHOW C C***»* C  CLOSE  I G L COMMUNICATIONS  C A L L CMCLOS C C***»* C  S E T WORKTABLE  PARAMETERS  C A L L TABSET(VAR ) C C****» C  CALCULATE CALL  C C***»* C*«***  RESULTING  HOMOGENEOUS TRANSFORMATIONS  'HOT','HOW'.  TABPOS(VAR,HOT,HOW)  CONVERT DOUBLE P R E C I S I O N 'SHOT'. 'SHOW'.  'HOT','HOW', INTO  SINGLE  PRECISION  - 231 -  : 1ng o f SC ENE . FOR a t 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90  14:41:35 o n AUG  9, 1984 f o r CC1d=DAC0 I  C C A L L MATCNV(HOT,SHOT,1) C A L L MATCNV(HOW,SHOW.1) C C* * * * » OPEN IGL COMMUNICATIONS C C A L L CMOPEN C C* * * * * D E L E T E A L L SEGMENTS AND CLEAR SCREEN C CALL D E L S E G ( - 1 ) CALL NEWPAG C C* * * * * SET VIEW REFERENCE POINT 'VRP' . EYE POSITION C C A L L V R P 3 D ( V R P ( 1 ) , V R P ( 2 ) ,VRP(3) ) C A L L EYEBAL( EYEPOS( 1 ) , E Y E P 0 S ( 2 ) . E Y E P O S O ) ) C c* * * * * DRAW WORLD COORDINATE FRAME AXES  c  CALL DRAXES  c c* * * * * c  DRAW OBJECT AS A POLYHEDRON  c c* * * * * c  DRAW THE  c  CALL DRPANL(SHOT,1) SEAMS  C A L L DRSEAM(SHOW) RETURN END  - 232 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f SETUP.FOR  a t 14:42:07 o n AUG  SUBROUTINE C C C C C C  c c c c c c c c c c c c c c c  c c  9, 1984 f o r CCid=DACO  SETUP  T H I S SUBROUTINE  PROCESSES  FILES  READ:  FILIST CADLST CAD WPCLST  FILES  WRITTEN:  WPCDAT  SUBROUTINES:  COMMON / F I L I S T / F I L I S T COMMON /CADLST/CADLST COMMON /WPCLST/WPCLST COMMON /CAD/CAD COMMON /WPCDAT/WPCDAT OPEN CAD SEAM CALL  DATA  FILE  SKIP F I R S T RECORD, THEN 'CADLST'  CALL  c  'CADLST'  SEAM  DATA  FILE  LIST,  'WPCLST'  0PEN(2.WPCLST,4,1) READ THE NUMBER OF SEAMS  READ(1,*) R E A D ( 1 , * ) NSEAMS  c c* * * * * REPEAT c  c* * * * *  LIST,  OPEN(1.CADLST,4,1)  c c***** c***»* c  c  DATA  BYTE C A D ( 2 0 ) , W P C D A T ( 2 0 ) , C A D L S T ( 2 0 ) BYTE W P C L S T ( 2 0 ) , F I L I S T ( 2 0 )  OPEN PROCESSED  c  THE CAD SEAM  OPEN PROCES RDFILE SHUT  c c***»* c  c  Page  DO  FOR A L L SEAMS...  110 1=1.NSEAMS  FIND  'CAD','WPCDAT' F I L E S  FOR SEAM I  ' C A L L R D F I L E ( 1 , 1 + 2 , C A D , 2 0 , I END) C A L L RDFILE(2,I+2,WPCDAT,20,I END) CLOSE CALL CALL  LOGIC UNITS SHUT(1) SHUT(2)  1,2  -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76  233  -  o f SETUP.FOR a t 14:42:07 o n AUG C C***»* C  PROCESS CALL  C C»**** C  9.  1984 f o r CC1d=DAC0  'CAD' SEAM DATA  AND  PROCES  REOPEN  LOGIC UNITS  1,2  CALL OPEN( 1 .CADLST,4,1) CALL 0 P E N ( 2 , W P C L S T , 4 , 1 ) . C 110 C C  CONTINUE  CALL S H U T ( 1 ) CALL SHUT(2) C RETURN END  GENERATE  Page  2  'WPCDAT' SEAM  DATA  - 234 -  Listing  o f SHUT.FOR a t  1 2 3 4  C C C  5  c  6 7 8 9 10  14:42:07 o n AUG  SUBROUTINE  9,  1984  f o r CC1d=DAC0 P a g e  1  SHUT(IUNIT)  T H I S SUBROUTINE CLOSES LOGIC UNIT  IUNIT.  ***********************************************************  C CLOSE(UNIT=IUNIT) C RETURN END  - 235 -  Listing  of  TABPOS.FOR  1  43 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  14:42:08  SUBROUTINE  n  •c  at  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 c c c c c c c c c c c c c c c c c c c c c c c c  o n AUG  9.  1984 f o r  CC1d=DAC0 P a g e  1  TABPOS(VAR,HOT,HOW)  T H I S SUBROUTINE C A L C U L A T E S THE TABLE COORDINATE TO WORLD COORDINATE TRANSFORMATION MATRIX, ' H O T ' . THE FIXED TABLE TABLE P O S I T I O N PARAMETERS ARE READ FROM F I L E ' T A B L E ' . THESE VARIABLES, 'VAR(I)',ARE PASSED FROM THE MAIN PROGRAM. VARIABLES ARE ANGLES AND/OR DISPLACEMENTS, DEPENDING ON THE TABLE D E S I G N . THE PROGRAM CALCULATES THE TABLE TRANSFORMATION, R E L A T I V E TO THE F I R S T JOINT FRAME. T H I S TRANSFORMATION IS THEI P R E M U L T I P L I E D BY A TRANSFORMATION ' H P R E ' FROM THE WORLD FRAME TO THE F I R S T JOINT FRAME AND THE RESULT IS POSTMULTIPLIED BY A TRANSFORMATION ' H P O S T ' FROM THE LAST JOINT FRAME TO THE TABLE REFERENCE FRAME. INPUTS: OUTPUTS:  VAR(6)-  ARRAY OF TABLE JOINT COORDINATES I E . ANGLES ( D E G ) AND DISPLACEMENTS  H0T(4.4)H0W(4,4)-  COMMON BLOCKS FILES  HOMOGENEOUS TRANSFORMATION MATRIX FROM TABLE FRAME ' F T ' TO WORLD FRAME ' F O ' HOMOGENEOUS TRANSFORMATION MATRIX FROM WORKPIECE FRAME ' F W ' TO WORLD FRAME ' F O '  ACCESSED:  /IOFILE/  READ: STOVAR- CONTAINS CURRENT ' V A R ' AND CURRENT ' H O T ' T A B L E - F I L E CONTAINING WORK TABLE S P E C I F I C A T I O N S  *  *  FORMAT OF T A B L E : RECORD NO. 1  JOINT NO.  2 3  *  CONTENTS NUMDOF TEXT  1  *  (LIST-DIRECTED  *  *  FORMAT)  *  DESCRIPTION NUMBER OF DEGREE > OF FREEDOM  HEADING  ID,ITYPE,STEP, RMIN,RMAX, ALPHA,DIST,LEN  4  2  AS A B O V E , FOR JOINT 2  5  3  JOINT  6  4  JOINT 4  7  5  JOINT  ID-JOINT  TYPE,  1 =ROTARY 2=PRISMATIC I T Y P E - 1=DISCRETE 2=C0NTINU0US S T E P - STEP S I Z E FOR DISCRETE POSITIONS RMIN,RMAX- MINIMUM,MAXIMUM L I M I T S OF JOINT MOT A L P H A - TWIST ANGLE D I S P - JOINT DISPLACEMENT L E N - JOINT LENGTH  3  5  ''  - 236 -  L i s t i n g of TABPOS.FOR a t 14:42:08 on AUG 59 60 61 62 63 84 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 9495 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 111 112 113 1 14 1 15 116  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 c c c c c c c c c c c c c c c c c c c  8  6  9  9, 1984 f o r CC1d=DAC0 Page  JOINT 6  2  ''  TEXT HEADING  10  N/A  HTW(1,J) .J=1,4  FIRST ROW OF WORKPIECE FRAME TO TABLE FRAME TRANSFORMATION MATRIX 'HTW'  11  N/A  HTW(2,J).d=1,4  SECOND ROW OF 'HTW'  12  N/A  HTW(3,J),J=1,4  THIRD ROW OF 'HTW'  13  N/A  HTW(4,J) ,J= 1 ,4  FOURTH ROW OF 'HTW'  14  TEXT HEADING  15  N/A  HPRE( 1 ,J) ,J=1,4 FIRST ROW OF FIRST JOINT TO WORLD FRAME TRANSFORMATION MATRIX 'HPRE'  16  N/A  HPRE(2,J ) ,J=1 ,4 SECOND ROW OF 'HPRE'  17  N/A  HPRE(3,J ) ,J= 1 ,4 THIRD ROW OF 'HPRE'  18  N/A  HPRE(4,J ) ,J=1 ,4 FOURTH ROW OF 'HPRE'  19  N/A  TEXT HEADING  20  N/A  HPOST( 1 . J) ,J=1,4FIRST ROW OF TA3LE FRAME TO LAST JOINT TRANSFORMATION MATRIX 'HPOST'  21  N/A  HP0ST(2,d),J=1,4  22  N/A  HPOST(3,J),J= 1 ,4 THIRD ROW OF 'HPOST'  23  N/A  HPOST(4,J) ,J= 1 ,4 FOURTH ROW OF 'HPOST'  SECOND ROW OF 'HPOST'  FORMAT OF 'STOVAR'  c  *  RECORD NO. 1 2 3  CONTENTS;  DESCRIPTION  TEXT HEADING VAR( 1 ) VAR(2)  CURRENT POSITION OF JOINT 1 CURRENT POSITION OF JOINT 2  7  VAR(6)  CURRENT POSITION OF JOINT 6  IMPLICIT DOUBLE PRECISION (D) BYTE TABLE(20) DOUBLE PRECISION HA(4,4),HB(4,4) , H0T(4,4), HPRE(4,4), HP0ST(4,4),HTEMP(4,4), DTHETA(6),DALPHA(6),HTW(4,4),HOW(4,4) REAL STEP(6),RMIN(6),RMAX(6),THETA(6),ALPHA(6),DISP(6),LEN(6)  - 237 -  Listing  o f TABPOS.FOR  117 1 18 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 . 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  a t 14:42:08 o n AUG  9,  1984 f o r CCid=DACO P a g e  3  REAL V A R ( 6 ) INTEGER I D ( 6 ) , I T Y P E ( 6 ) C COMMON  /TABLE/TABLE  C C OPEN THE TABLE  c  SPECIFICATION  FILE  'TABLE-  CALL OPEN(1.TABLE,4,1)  c  READ  c  THE NUMBER OF DEGREES OF FREEDOM,  'NUMDOF'.  READ( 1.*) NUMDOF  c  c***»*  c  S K I P A RECORD,  THEN READ  THE TABLE  LINK  PARAMETERS  READ(1,*)  * 123 C  READ( 1 , * ) I D ( I ) , I T Y P E ( I ) , S T E P ( I ) , R M I N ( I ) , , RMAX ( I ) , THETA ( '.  DO A123 L P H A 1=1,NUMDOF (I),DISP(I).LEN(I) CONTINUE  c**«** c  SKIP  A RECORD,  THEN READ  THE  TRANSFORMATION MATRIX  'HTW'  TRANSFORMATION MATRIX  'HPRE'  READ(1,*) R E A D ( 1 , * ) ( H T W ( I , d ) , J =1,4) DO 131 1=1,4 CONTINUE  131 C  SKIP  c  A RECORD,  THEN READ  THE  READ(1 , * ) READ( 1 , * ) ( H P R E ( I , d ) , J = 1 , 4 ) DO 124 1=1,4 CONTINUE  124 C  c»**** c  S K I P A RECORD,  THEN READ  THE  TRANSFORMATION MATRIX  'HPOST'  READ(1,* ) READ( 1 ,* ) ( H P O S T ( I , J ) , J = 1 , 4 ) DO 125 1=1,4 CONTINUE  125 C  CALL SHUT(1) C  c****» c c  C** C  'OCONV  IS A CONVERSION  FACTOR  FROM DEGREES TO  DC0NV=4.DO*DATAN(1.D0)/180. WHILE  ( I . L E . NUMDOF)  IF(ID(I).EO.1)  VAR(I)=VAR(I)*DCONV  DO D40O 1=1.NUMDOF THETA(I)=THETA(I)*DCONV  DALPHA(I)=ALPHA(I)*DCONV C C**  c  IF  THE JOINT  IS REVOLUTE (ROTATIONAL)  RADIANS  -  st  i  ng  o f  T A B P O S . FOR  175 176 177 178 179  at  238  1 4 : 4 2 : 0 8  -  o n  AUG  I F ( I D ( I ) . N E . 1 )  GO  9, TO  1984  f o r  CCid=DACO  100  C  c* * + * * c***»* c  CALCULATE  THE  RELATING  LINK  DENAVIT-HARTENBERG I  TO  LINK  ) = C O S ( V A R ( I )  TRANSFORMATION  H A ( 1 , 1  181  H A ( 1 , 2 ) =- S I N ( V A R ( I  182  H A ( 1 , 3 ) = S I N ( V A R ( I ) ) * S I N ( D A L P H A ( I ) )  183  H A ( 1 , 4 ) = L E N ( I ) * C O S ( V A R ( I ) )  184  H A ( 2 , 1 ) = S I N ( V A R ( I ) )  185  H A ( 2 , 2 ) = C 0 S ( V A R ( I ) ) " C O S ( D A L P H A ( I ) )  186  )  H A ( 2 , 3 ) = - C 0 S ( V A R ( I ) ) * S I N ( D A L P H A ( I ) )  ) )  * C O S ( D A L P H A ( I ) )  187  H A ( 2 , 4 ) = L E N ( I ) * S I N ( V A R ( I ) )  188  H A ( 3 , 1 ) = 0 .  189  H A ( 3 , 2 ) = S I N ( D A L P H A ( I ) )  190  H A ( 3 , 3 ) =COS(DALPHA(I  191  H A ( 3 , 4 ) =DISP(I  192  H A ( 4 , 1 ) = 0 .  193  H A ( 4 , 2 ) = 0 .  194  H A ( 4 , 3 ) = 0 .  195  H A ( 4 , 4 ) = 1 .  199 200 201 202 203 204 205  ) )  )  c  197 198  GO  c c** c  ELSE  TO  THE  2 0 0 JOINT  100  CONTINUE  c c****»  CALCULATE  c***»»  RELATING  IS  THE LINK  PRISMATIC  (TRANSLATIONAL)  DENAVIT-HARTENBERG I  TO  LINK  TRANSFORMATION  1-1  c  206  H A ( 1 , 1 ) = C O S ( D T H E T A ( I ) )  207  H A ( 1 , 2 ) = - S I N ( D T H E T A ( I ) ) * C O S ( D A L P H A ( I ) )  208  H A ( 1 , 3 ) = S I N ( D T H E T A ( I  209  H A ( 1 , 4 ) = L E N ( I ) * C 0 S ( D T H E T A ( I ) )  210  H A ( 2 . 1 ) = S I N ( D T H E T A ( I ) )  ) ) * S I N ( D A L P H A ( I ) )  211  H A ( 2 , 2 ) = C 0 S ( D T H E T A ( I ) )  212  H A ( 2 , 3 ) = - C O S ( D T H E T A ( I ) ) * S I N ( D A L P H A ( I ) )  213  H A ( 2 , 4 ) = L E N ( I ) * S I N ( D T H E T A ( I,)  214  H A ( 3 , 1 ) = 0 .  215  H A ( 3 , 2 ) = S I N ( D A L P H A ( I  ) )  216  H A ( 3 , 3 ) = C O S ( D A L P H A ( I  ) )  217  HA ( 3 , 4 ) =DI  218  H A ( 4 , 1 ) = 0 .  219  H A ( 4 , 2 ) = 0 .  220  H A ( 4 , 3 ) = 0 .  221  H A ( 4 , 4 ) = 1 .  222  200  224  c  225  C * *  226  C  227  C  228  C * *  229  C  230 232  SP (I  )+VAR( I )  c  223  231  MATRIX  1-1  180  196  Page  CONTINUE ENDIF  IF  ( I . E Q . 1 ) I F ( I . G T  . 1)  GO  DO  J = 1 , 4  C 101  TO  3 0 0  " C O S ( D A L P H A ( I ) ) )  MATRIX  - 239 -  Listing 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 26 1 262 263 264 265 266 267 268 269 270 271 272 273  o f TABPOS.FOR  at  14:42:08 o n AUG  9,  1984 f o r CC1d=DAC0  Page  DO  102 K=1.4 HB(J,K)=HA(d , K ) CONTINUE CONTINUE  102 101 C  GO TO 5 0 0 C  c*» c  ELSE CONTINUE  300 C  c***»* c  MULTIPLY CALL  c c***** c  777 666 C 50O C C** C 400 C  INTERMEDIATE  TRANSFORMATION  HMULT(HB,HA,HOT)  THEN L E T 'HB' := 'HOT' DO 6 6 6 d= 1 ,4 DO 777 K=1,4 HB(d,K)=HOT(d,K) CONTINUE CONTINUE CONTINUE  END IF CONTINUE  c** END WHILE c c c***** PREMULTIPLY BY 'HPRE' AND POSTMULTIPLY c* * *w * REQUIRED TRANSFORMATION MATRIX 'HOW' c  c  MATRICES  CALL CALL CALL  HMULT(HPRE,HOT,HTEMP) HMULT(HTEMP,HPOST,HOT) HMULT(HOT,HTW,HOW)  RETURN END  BY  'HPOST'  5  - 240 -  L i s t i n g of TABSET.FOR at 14:42:09 on AUG 9, 1984 f o r CC1d=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 38 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  1  SUBROUTINE TABSET(VAR) THIS SUBROUTINE ALLOWS THE USER TO INTERACTIVELY MODIFY THE WORK TABLE DESCRIPTION FILE 'TABLE'. THE USER IS PROMPTED FOR CHANGES IN THE WORK TABLE POSITION. THE FIXED WORK TABLE PARAMETERS ARE INITIALLY ENTERED INTO 'TABLE' USING THE FILE EDITOR, AND ARE NOT CHANGED CONSEQUENTLY. VARIABLE TABLE PARAMETERS, JOINT VARIABLES 'VAR' \RE STORED IN FILE 'STOVAR'. INPUTS:  NONE  OUTPUTS:  VAR(6)- TABLE JOINT VARIABLES  COMMON BLOCKS READ:  /TABLE/ /STOVAR/  FILES READ:  TABLE.STOVAR  FILES WRITTEN:  STOVAR  SUBROUTINES CALLED:  OPEN,SHUT  BYTE YCAP,YLOW,NCAP.NLOW.ANS,LABEL(10) BYTE TABLE(20),STOVAR(20) BYTE BELL REAL VAR(6),STEP(6),RMIN(6),RMAX(6),ALPHA(6),DISP(6),RLEN(6) DOUBLE PRECISION HOT(4,4),HPRE(4,4),HPOST(4,4) INTEGER ITYPE(6),10(6) COMMON /TABLE/TABLE COMMON /STOVAR/STOVAR DATA BELL/7/ DATA YCAP/89/,YL0W/121/.NCAP/78/,NL0W/l10/  c c***»* READ AND SAVE CONTENTS OF FILE 'TABLE' c* * * * * NOTE THAT RECORDS MUST BE READ IN CORRECT ORDER c CALL OPEN(1.TABLE,4,1) c c***»* READ RECORD 1, NUMBER OF DEGREES OF FREEDOM, 'NUMDOF' c READ( 1 ,*) NUMDOF c Q * * * * * SKIP RECORD 2 c READ(1,*) c READ RECORDS 3 TO 8, FIXED TABLE PARAMETERS c DO 170 J=1.NUMDOF READ( 1 ,*) ID(J) .ITYPE(J),STEP(J),RMIN(J),RMAX(J).ALPHA(J). DISP(J ) ,RLEN(J) *  -  241  -  L i s t i n g o f TABSET.FOR a t 14:42:09 o n AUG 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 1 11 112 113 114 115 1 16  170 C C***»* C C c***** C 175 C C***** C C c***** C 180 C C***** C C C***»* C 185 C C C****» C***»* C C C»**** C C C****» C 190 C  9, 1984 f o r CC1d=DAC0 Page  2  CONTINUE S K I P RECORD 9 READ( 1 ,* ) S K I P RECORDS  10 TO 13, TRANSFORMATION MATRIX 'WW  (NOT USED)  DO  175 J=1 ,4 READ( 1 , * ) CONTINUE  S K I P RECORD  14  READ( 1 ,* ) READ RECORDS  15 TO 18, TRANSFORMATION MATRIX 'HPRE'  DO  180 J=1 ,4 READ(1,») ( H P R E ( J , K ) ,K=1,4) CONTINUE  S K I P RECORD 19 READ(1,* ) READ RECORDS 20 TO 2 3 , TRANSFORMATION MATRIX 'HPOST' DO  185 vi=1 ,4 READ( 1 ,* ) ( H P O S T ( J , K ) , K = 1 , 4 ) CONTINUE  CALL SHUT(1) READ CURRENT VALUES OF JOINT VARIABLES 'VAR' FROM F I L E 'STOVAR'.  CURRENT  CALL OPEN(1,STOVAR,4. 1 ) S K I P RECORD 1 READ(1,* ) READ RECORDS 2.NUMD0F+1 DO  190 J=1.NUMDOF READ(1,*) V A R ( J ) CONTINUE  CALL SHUT(1) C C»* UNTIL (ANSWER .NE. YES) C 250 CONTINUE C C****» WRITE THE CURRENT TABLE JOINT POSITIONS ONTO THE SCREEN, AND C***»* PROMPT FOR CHANGES C  - 242 -  Listing 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 14-2 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  of  at  TABSET.FOR  on  AUG  9,  for  1984  W R I T E ( 5 , 2 3 0 ) (VAR( J ) , J = 1 , 3 ) F O R M A T C J O I N T V A R ( 1 )= ' , F 8 . 2 , ' ' JOINT VAR(3)=',F8.2)  230  -  14:42:09  *  CC1d=DAC0  JOINT  Page  3  VAR(2 ) = ' , F 8 . 2 ,  C WRITE(5,10O) F O R M A T ( $ , ' DO YOU W I S H TO C H A N G E THE T A B L E R E A D ( 5 , 3 0 1 ) ANS FORMAT(A1 )  100 301 C  c** c  IF  (ANS  ELSE  c** c  END  c c«* c  IF  IF END  END  650  NOT V A L I D )  NUMBER  PRINT  YOU W I S H TO CHANGE  MESSAGE.  TRY  (INTEGER)  AGAIN  ( 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  IF WRITE(5,102) F O R M A T ( $ , ' E N T E R T H E NEW P O S I T I O N READ(5.,») VAR(JOINT)  AGAIN  IF (JOINT  TYPE  IS  DISCRETE)  .IF(ITYPE(JOINT).NE.1)  TEST  GO TO  TEMP=VAR(JOINT)/STEP(JOINT) ITEMP=INT(TEMP) REM=ABS(TEMP-FLOAT(ITEMP)) IF(REM.GT..001) CONTINUE END  TRY  ')  IF(VAR(JOINT). LT.RMIN(JOINT).OR.VAR(JOINT).GT.RMAX(JOINT)) GO TO 8 0 0  c  MESSAGE,  REAL)  *  IF  PRINT  OR M M . ,  (VARIABLE  END  OUT OF R A N G E )  (DEG.  IF  c  c  500  '  c  c  GO TO  IF  (JOINT  102  850 C c**  CONTINUE  WRITE(5,101) F O R M A T ( $ , ' ENTER' J O I N T READ(5,* ) JOINT  101  c c** c c* c  (Y/N)  CONTINUE  500 C  c c*» c  GO TO  GO TO  c  c** c  YES)  IF(ANS.EO.YCAP.OR.ANS.EO.YLOW)  c c** c  C  .EO.  POSITION?  IF GO TO  250  GO TO  900  FOR V A L I D  850  V A L U E OF  'VAR'  - 243 -  Listing 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214  o f TABSET.FOR C** C 650 C  END  a t 14:42:09 o n AUG  9,  1984 f o r CC1d=DAC0 P a g e  4  UNTIL CONTINUE STORE  c  NEW  VALUES  OF  J O I N T VARIABLES  'VAR'  IN F I L E  'STOVAR'.  CALL 0PEN(1,STOVAR.4.1) READ( 1 , * ) DO 123 J=1,NUMD0F WRITE(1.*) V A R ( J ) CONTINUE CALL SHUT(1)  123 C  c****» c c c c****» c c  c 701 108  C 600  TO  END  GO  TO  600  ERROR MESSAGES  WRITE(5,106) BELL FORMAT($,' P O S I T I O N ENTERED OUTSIDE READ(5,* ) GO TO 250  800 106  C 900 107  GO  *  WRITE(5,107) BELL FORMAT($,' P O S I T I O N ENTERED NOT AGAIN',A1) READ(5,*) GO TO 250 WRITE(5,108) BELL FORMAT($,' JOINT NUMBER NOT READ(5,») GO TO 250 CONTINUE RETURN END  ONE  OF  RANGE-TRY  OF THE  VALID-TRY  AGAIN  AGAIN  POSSIBLE  '.AD  ',A1)  VALUES-TRY  - 244 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f TANGEN.FOR a t 14:42:31 o n AUG  9,  1984 f o r CC1d=DAC0  Page  1  SUBROUTINE TANGEN(R,S,T,I FLAG) C C C C C C C C C C C C C C  c c  THIS SUBROUTINE CALCULATES TANGENT INPUTS:  R ( 3 , 3 ) = P 0 S I T I 0 N VECTORS FOR POINTS 1-1, I , 1+1. S(3)=ARC LENGTHS AT POINTS 1-1, I . 1+1. IFLAG=CONDITION FLAG WHERE 1=1...SECOND POINT ON CURVE. 1=2...INTERMEDIATE POINTS. 1=3...LAST POINT ON CURVE. OUTPUTS: T(3)=TANGENT VECTOR  REAL R ( 3 , 3 ) , S ( 3 ) ,  c c** c  CASE OF  c c** c  CASE ( 1 )  100  101  c c** c 200  102 C C**  c  300 103  OF CURVE, T.  AT POINT I .  T(3)  (IFLAG)  IF ( I F L A G . L T . 1 . O R . I F L A G . G T . 3 ) GO TO 400 GO TO ( 1 0 0 . 2 0 0 , 3 0 0 ) . I F L A G  CONTINUE DO 101 J=1.3 T(J) = (R(3,J)-R(2.J) )/S(3) CONTINUE GO TO 500 CASE ( 2 ) CONTINUE DO 102 J=1,3 D1=R(3, 1 ) - R ( 1 , 1 ) D2=R(3,2)-R(1,2) D3=R(3.3)-R(1,3) DS=SQRT(D1**2+D2**2+D3**2) T(J)=(R(3,d)-R(1.J))/DS CONTINUE GO TO 5 0 0 CASE ( 3 ) CONTINUE DO 103 J=1,3 T(d)=(R(2.J)-R(1,J))/(S(2)-S(O) CONTINUE GO TO 5 0 0  C  c** c 400 150  CASE.ELSE CONTINUE WRITE(5,150) FORMAT(' CASE INDEX S P E C I F I C A T I O N  ERROR.')  -  sting 59 60 61 62  of  TANGEN.FOR C 500  at  245  14:42:31  CONTINUE RETURN END  on  -  AUG  9,  1984  for  CC1d=DAC0  Page  -  Listing  o f TOLROT.FOR  246 -  a t 14:42:32 o n AUG  9,  1984 f o r CC1d=DACO  Page  1  1 2 3 4 5 G 7 8 9 10 11 12 13 ^4 .  SUBROUTINE TOLROT(ROTOLD,ICOUNT.ROTNEW.INHAG) C C T H I S SUBROUTINE C A L C U L A T E S AN INVERSE ROTATION MATRIX C FOR A ROTATION 'ROTNEW ABOUT THE TOOL A X I S . C THE TOOL IS ROTATED ALTERNATELY BY AN ANGLE OF ICOUNT*DELTA C IN THE P O S I T I V E AND NEGATIVE DIRECTIONS. C C INPUTS: ROTOLD- PREVIOUS ROTATION ANGLE C ICOUNT- NUMBER OF ROTATIONS C C OUTPUTS: ROTNEW- NEW ROTATION ANGLE C I N H A G ( 4 . 4 ) - INVERSE ROTATION MATRIX C (;,»«,»,*«,««*»*»***»«*»«.*.*»*«,»»»««.«»**.*****«*.*»*««««*»*».*«*«*»*«•«*•*»*»  15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36  C REAL I N H A G ( 4 , 4 ) .DELTA INTEGER IMAX C COMMON  /TOLROT/IMAX,DELTA  C  c***»*  FIND THE NEW  ROTATION  ANGLE  C ROTNEW = ROTOLD+FLOAT((- 1)**ICOUNT*ICOUNT/2)*DELTA C C***»* C***»* C  C A L C U L A T E THE ROTATION MATRIX UNROTATED TOOL. INHAG(1,1)=COS(ROTNEW) INHAG(1,2)=SIN(R0TNEW) I N H A G ( 2 , 1 ) = -INHAG ( 1,2) INHAG(2.2) = INHAG( 1 , 1) INHAG(3.3)=1. INHAG(4.4)=1.  C RETURN END  RELATING THE ROTATED  TOOL  TO THE  - 247 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 1G 17 18 19 20 21 22  o f TRANS.FOR  a t 14:42:32 o n AUG  SUBROUTINE C C C C C C C C C  9.  1984 f o r CCid=DACO P a g e  TRANS(H,P1,P2)  T H I S SUBROUTINE TRANSFORMATION.  PERFORMS THE  FOLLOWING  P2(4)=H(4,4)*P1(4) WHERE P1.P2 ARE 4X1 VECTORS AND H IS A COORDINATE TRANSFORMATION MATRIX.  C REAL  H(4,4),P1(4),P2(4)  C DO 10O C  100 1=1,3 P2(I)=H(I,1)*P1(1)+H(I,2)*P1(2)+H(I CONTINUE P2(4)=1.  C RETURN END  -  Listing 1 2 3 4  5  6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20  of  UNIT.FOR  at  14:42:33  SUBROUTINE C C C C C C C  c c c  c  THIS  on  OUTPUTS:  AUG  9,  1984  for  CC1d=DAC0  Page  UNIT(VECT,UVECT)  SUBROUTINE  INPUTS:  REAL  248 -  VECT(3)-  NORMALIZES  A VECTOR.  A VECTOR  UVECT(3)-  VECT  NORMALIZED.  VECT(3),UVECT(3),LENGTH  LENGTH-SORT(VECT(1)**2+VECT(2)**2+VECT(3)**2) UVECT(1)=VECT(1)/LENGTH UVECT(2)=VECT(2)/LENGTH UVECT(3)=VECT(3)/LENGTH RETURN END  1  - 249 -  Listing 1 2 3 4 5 6 7 8 9 10 11 12 1-3 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f WELSET.FOR  a t 14:42:59 o n AUG  SUBROUTINE 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  100  c c  c c****« c c c****»  9,  1984 f o r CC1d=DAC0  Page  1  WELSET(WPCLST,WKSTAT)  T H I S SUBROUTINE SCANS THE WELD PARAMETERS J T Y P E , X S E C T , T 1 , T 2 , FOR A L L SEAMS TO BE WELDED. THE CURRENT WELDING MACHINE S E T T I N G S ARE READ FROM THE WELDER SETUP F I L E . I F NECESSARY, THE SETUP F I L E IS MODIFIED AND THE OPERATOR IS INSTRUCTED TO RESET WELDER. INPUTS: WPCLST- NAME OF WORKPIECE SEAM DATA F I L E WKSTAT- NAME OF WORKSTATION DATA F I L E FILES  READ:  WPCLST WKSTAT  F I L E S WRITTEN: SUBROUTINES:  WKSTAT OPEN RDFILE SHUT  BYTE W P C L S T ( 2 0 ) , W P C D A T ( 2 0 ) , W K S T A T ( 2 0 ) REAL W I R E S ( 3 . 2 ) , W R S I Z E ( 3 ) DATA W R S I Z E / . 0 3 0 , . 0 3 5 , . 0 4 5 / DATA WIRES/.025, .125, .037. .25, . 125, .5/ COMMON  /NSEAMS/NSEAMS  C A L L OPEN(1.WPCLST,4,1 ) TMIN=1000. TMAX=0. DO  100 1=1.NSEAMS CALL RDFILE(1,1+2,WPCDAT.20.IEND) CALL 0PEN(2,WPCDAT,1.1) READ(2) JTYPE,XSECT,T1,T2,NPOINT,ICURVE CALL S H U T ( 2 ) PROCESS DATA HERE, SAVING MINIMUM,MAXIMUM VALUES ETC. FOR S E L E C T I O N OF OPTIMUM WELDER SETTINGS. TX=AMIN1(T1,T2) TMIN=AMIN1(TMIN,TX) TMAX=AMAX1(TMAX,TX)  CONTINUE  CALL SHUT(1) OPEN CURRENT  WELDER  SETUP F I L E  C A L L OPEN(1.WKSTAT, 4, 1 ) SKIP  F I R S T 26 RECORDS  'WKSTAT'.  - 250 -  Listing 59  eo  61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106  o f WELSET.FOR a t 14:42:59 o n AUG  9, 1984 f o r CC1d=DAC0 Page  2  C DO  10 1=1,26 READ(1,*) CONTINUE  10 C  c**»*» c c c«**** c***** c 20  c  30  c  C 600 C  INTERACIVELY UPDATE WELDER SETUP I F REQUIRED. PARAMETERS HAVE BEEN CHOSEN, UPDATE 'WKSTAT'  WHEN NEW  DO 20 J=1.3 I F ( T M I N . G E . W I R E S ( J . 1 ) .AND.TMAX.LE.WIRES(d,2)) GO TO 5 0 0 CONTINUE  CONTINUE I F ( W S I Z E . E Q . W R S I Z E ( J ) ) GO TO 6 0 0  C  33  READ( 1 ,* ) WSIZE  WRITE(5,30) FORMATC NO APPROPRIATE WIRE S I Z E CAN BE FOUND. HIT RETURN TO * CONTINUE ') READ(5,*) GO TO 9 0 0  500 C  31  READ CURRENT WELOER SETUP FROM 'WKSTAT'.  *  BACKSPACE*1) W R I T E O . * ) WRSIZE(vJ) W R I T E ( 5 , 3 1 ) WSIZE.WRSIZE(d) FORMATC CURRENT WIRE SIZE= ' . F 5 . 3 , ' I N . , NEW WIRE SIZE= ', F5.3,' IN') WRITE(5,33) FORMAT(' INSTALL NEW WIRE, THEN HIT RETURN TO CONTINUE') READ(5,*) GO TO 9 0 0 CONTINUE  32  WRITE(5,32) FORMATC CURRENT WIRE S I Z E I S OK. HIT RETURN TO CONTINUE READ(5 , * )  C 900 C  CONTINUE  C  CALL SHUT(1 ) RETURN END  ')  - 251 -  Listing  o f WIRE 1.FOR  1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34  a t 14:42:59 o n AUG  9,  1984 f o r CCid-DACO P a g e  1  SUBROUTINE WIRE 1 C  c»***« c***** c****» c****» c c c c c c c c c c c c c c  c  T H I S SUBROUTINE DETERMINES CURRENT, VOLTAGE, WIRE FEED, WELDING WELDING SPEED, GAS FLOW RATE, WIRE STICK-OUT, LONGITUDINAL ANGLE AND TRANSVERSE ANGLE AS A FUNCTION OF THICKNESS AND CROSS SECTIONAL AREA. D E F I N I T I O N OF V A R I A B L E S : CURRNT-CURRENT(AMPS) VOLT-VOLTAGE ( V O L T S ) FEED-WIRE FEED RATE ( I P M ) GFR-GAS FLOW RATE (CFM) SPEED-WELDING SPEED ( I P M ) STKOUT-WIRE STICK-OUT ( I N C H E S ) WALPHA-LONGITUDINAL TORCH ANGLE BETA-TRANSVERSE TORCH ANGLE TX-MINIMUM PARENT METAL THICKNESS XSECT-WELD CROSS SECTIONAL AREA ( S O . I N . ) BYTE WPARAM(20) COMMON/EXPERT/WPARAM.VOLT,WSIZE,FEED,GFR.WALPHA.BETA.STKOUT COMMON/W123/CURRNT.SPEED.XSECT.TX CURRNT-199.07+43.04*AL0G(TX) VOLT-16. FEED-290.38+52.87*ALOG(TX) GFR-23.38+1 6 4 * A L 0 G ( T X ) SPEED-3. 1415*( . 0 3 0 * * 2 ) * F E E D / ( X S E C T ) STKOUT-.75+.25*AL0G(TX ) WALPHA-15. BETA-0.0 RETURN END  - 252 -  Listing 1 4.  3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26  o f WlRE2.FOR r*  I*  c***** c***»* c»**»* c***** c****» c  c  c  a t 14:42:59 o n AUG  SUBROUTINE  9.  1984 f o r CC1d=DAC0  Page  1  WIRE2  WIRE S I Z E IS 0.035 INCHES T H I S SUBROUTINE DETERMINES CURRENT, VOLTAGE, WIRE FEED, WELDING SPEED, GAS FLOW RATE, WIRE STICK-OUT, LONGITUDINAL ANGLE AND TRANSVERSE ANGLE AS A FUNCTION OF THICKNESS AND CROSS SECTIONAL AREA. BYTE WPARAM(20) COMMON/EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR,WALPHA,BETA,STKOUT COMMON/W123/CURRNT,SPEED.XSECT,TX CURRNT=222.64+44.41*ALOG(TX) I F ( C U R R N T . G T . 1 5 0 . ) CURRNT=150. V0LT=23.91+2.12*AL0G(TX) FEED=435.77+101.51*AL0G(TX) I F ( F E E D . G T . 2 6 5 . ) FEED=265. GFR=40.55+7.17*AL0G(TX) SPEED=3.1415*(.035**2)*FEED/(XSECT*4.) I F ( G F R . G T . 2 2 . 5 ) GFR=22.5 STKOUT=.75+.25*ALOG(TX ) WALPHA=15. BETA=0.0 RETURN END  -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23  o f WIRE3.FOR  253  -  a t 14:42:59 o n AUG  SUBROUTINE  9,  1984 f o r CC1d=DAC0  Page  1  WIRE3  C  c***»* c**»** c****» c***«* c***** c  c  c  WIRE S I Z E IS 0.045 INCHES T H I S SUBROUTINE DETERMINES CURRENT, VOLTAGE, WIRE FEED, WELDING SPEED, GAS FLOW RATE, WIRE STICK-OUT, LONGITUDINAL ANGLE AND TRANSVERSE ANGLE AS A FUNCTION OF THICKNESS AND CROSS SECTIONAL AREA. BYTE WPARAM(20) COMMON/EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR,WALPHA.BETA,STKOUT COMMON/W123/CURRNT.SPEED.XSECT,TX CURRNT=207.88+8.72*AL0G(TX) V0LT=22. FEED=296.88+21.77*AL0G(TX) GFR=22.5 SPEED = 3.1415*( . 0 4 5 * * 2 ) * F E E D / ( X S E C T * 4 . ) STKOUT=.75+.25*AL0G(TX) WALPHA=15. BETA=0.0 RETURN END  -  : 1 ng o f  1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  254 -  WORPOS. FOR a t 14:43:16 o n AUG SUBROUTINE 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 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  9,  1984 f o r CC1d=DAC0 P a g e  1  WORPOS(WPCDAT.IDSEAM,IDIR,ISTAT)  T H I S SUBROUTINE DOES THE COORDINATE TRANSFORMATIONS TO FIND THE TOOL LOCATION DATA FOR THE TOOL COORDINATE FRAME FG R E L A T I V E TO THE WORLD COORDINATE FRAME FO. THE SUBROUTINE FINDS THE HOMOGENEOUS TRANSFORMATION HOG, AND STORES THE TOOL POSITION AND ORIENTATION IN AN OUTPUT F I L E . IDSEAM- SEAM I D E N T I F I C A T I O N NUMBER  INPUTS:  IDIR-  OUTPUTS:  SEAM TRAVERSE D I R E C T I O N 0=NORMAL D I R E C T I O N 1=REVERSE D I R E C T I O N  ISTAT-  INPUT F I L E S :  F E A S I B I L I T Y STATUS 0=TRAJECTORY IS F E A S I B L E 1=R0B0T WORK ENVELOPE EXCEEDED 2=R0B0T JOINT L I M I T EXCEEDED 3=IDIR NOT 0 OR 1  TABLE  A F I L E CONTAINING THE WORKPIECE AND TABLE TRANSFORMATION MATRICES, HTW, HOT  -  WPCLST  -  A FILE  WPCDAT -  ROBLST OUTPUT  FILES:  S P E C I F Y I N G NAME OF WPCDAT.  A F I L E CONTAINING THE POSITION AND ORIENTATION DATA FOR THE WELD FRAME F C . R E L A T I V E TO THE WORKPIECE FRAME FW. -  A FILE  S P E C I F Y I N G NAME OF  LOCDAT.  ROBDAT  AN EXTERNALLY S P E C I F I E D F I L E CONTAINING THE LOCATION AND ORIENTATION DATA OF THE TOOL R E L A T I V E TO THE ROBOT COORDINATE FRAME FO, THE WELDING CURRENT AND SPEED SETTINGS, AND D I R E C T I O N VECTORS FOR NORMAL AND LATERAL PATH CORRECTIONS.  LOCSET  CONTAINS, IN WELDING FOLLOWING:  SEQUENCE, THE  SEAM NUMBER TABLE JOINT VARIABLES ROBOT CONFIGURATION SUBROUTINES C A L L E D -  HMULT HMULTS JOINTS LIMITS MATCNV NOAP 1 N0AP5 OPEN RDFILE SHUT TOLROT  255 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 11 1 1 12 1 13 1 14 1 1-5 1 16  o f WORPOS.FOR a t 14:43:16 C C  o n AUG  9, 1984 f o r CC1d=DAC0 P a g e  2  XPROD ***»*»«*»«*******»**^  c c  *  c  c c c  c c c****» c c  c c c c c****»  I M P L I C I T DOUBLE P R E C I S I O N ( D ) DOUBLE P R E C I S I O N HOT(4,4),HTW(4,4),HWC(4,4),HCG(4,4) DOUBLE P R E C I S I O N HOW(4,4),HOC(4,4),HOG(4,4) REAL R(3),T(3).A(3),B(3),N(3),0(3),X(3),Y(3),Z(3), THETA(6),SHOW(4,4),SHOT(4,4),INHOO(4,4) REAL X Y Z 0 A T ( 6 ) , S H 0 G ( 4 , 4 ) , I N H A G ( 4 , 4 ) , H 6 G ( 4 , 4 ) , H 0 6 ( 4 . 4 ) , H O G ( 4 , 4 ) REAL INH6A(4,4),HOA(4,4),DELTA,TABVAR(6),H0G5(4,4) REAL 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 ) , V A R ( 6 ) , H 0 0 ( 4 , 4 ) REAL SHOC(4,4),HOC(4,4),ENORM(3),ELAT(3) INTEGER I T 0 0 L ( 3 ) . I O F F ( 2 ) ,I J O I N T ( 6 ) , I C 0 N F ( 6 ) , I M A X BYTE W P C L S T ( 2 0 ) . S T O V A R ( 2 0 ) . R O B L S T ( 2 0 ) , L 0 C L S T ( 2 O ) BYTE WPCDAT(20),LOCDAT(20),ROBDAT(20),LOCSET(20) BYTE BELL(3),WPARAM(20) COMMON COMMON COMMON COMMON. COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON  /FILE/FILIST /LOCSET/LOCSET /ROBLST/ROBLST /STOVAR/STOVAR /WPCLST/WPCLST /ROBOT/NRDOF.ALPHA,LEN,DIST,RMIN,RMAX,H0O.H6G,INHOO.INH6A /CONFIG/ICONF /MATRIX/HOT,HOW,SHOT,SHOW /TABDAT/HTW,SHTW /TOLROT/IMAX,DELTA /ROBPOS/HOG /EXPERT/WPARAM,VOLT,WSIZE,FEED,GFR,WALPHA,BETA,STKOUT  DATA B E L L / 3 * 7 / INITIALIZE  PARAMETERS  ISOLN=0 ISTAT=0 IC0UNT=1 IFEAS=1 R0TOLD=O.  FIND  AND OPEN F I L E S  'WPCDAT','ROBDAT','LOCSET','WPARAM'  CALL  OPEN(2,WPCDAT,1,1)  CALL CALL CALL CALL  0PEN(3,ROBLST,4,1) RDFILE(3,IDSEAM+1,ROBDAT,20,I SHUT(3) 0PEN(3,R0BDAT,4,1)  CALL  0PEN(4,LOCSET,4,1)  CALL  0PEN(7,WPARAM,4,1)  END)  READ F I R S T RECORD OF 'WPCDAT', THEN BACKSPACE TO BEGINNING.  - 256 -  L i s t i n g o f WORPOS.FOR a t 1 17 1 18 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  C  14:43:16 o n AUG  9, 1984 f o r CC1d=DAC0 Page  3  READ(2) JTYPE,XSECT,T1,T2,NPOINT,ICURVE BACKSPACE 2  C  c c****»  STORE WELDING PARAMETERS 'WALPHA','BETA','STKOUT','VOLT', 'FEED', 'GFR' FOUND BY THE EXPERT WELDER MODULE, AND NUMBER OF LOCATIONS 'NPOINT' AT HEAD OF ROBOT LOCATION FILE 'ROBDAT' FOR CURRENT SEAM  c* * * » *  c***»*  c***»*  c  WRITE(3,* ) WALPHA.BETA.STKOUT,VOLT,FEED,GFR,NPOINT  c c c  c**«»*  REPEAT UNTIL ALL CONFIGURATIONS HAVE BEEN TESTED OR UNTIL A  160  CONTINUE  c* * * * * FEASIBLE TRAJECTORY HAS BEEN FOUND c c c  READ FIRST RECORD OF 'WPCDAT'  c c  READ(2) JTYPE,XSECT,T1,T2,NPOINT,ICURVE  Q** * * * *  c c c c* * * c  ..»  c  890  c c***** c 560  556 C  c*** c c  891 C C  c*** c 150 C  c*** c c  SET  INDEX  INDEX=NPOINT IF (DIRECTION.EQ.REVERSE.AND.PATH FILES 'WPCDAT'.'WPARAM'  IS FEASIBLE) STEP TO END OF  IF (IDIR.EQ.1.AND.IFEAS.EQ.O) GO TO 890 GO TO 891 CONTINUE REPEAT UNTIL END OF FILE REACHED CONTINUE READ(2.END=556) READ(7,*) GO TO 560 CONTINUE . END IF CONTINUE REPEAT FOR ALL LOCATIONS  IN FILE 'WPCDAT'  CONTINUE IF (DIRECTION .EQ. NORMAL)  ( I E . IDIR=0)  IF (IDIR.NE.0.AND.IFEAS.EQ.O) GO TO 855  -  Listing 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 2O0 201 202 203 204 205 206 207 208 209 210 21 1 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232  o f WORPOS.FOR  c****» c c  *  2 5 7  -  a t 14:43:16 o n AUG READ DATA  9,  FROM INPUT  1984 f o r CCid=DACO FILE  Page  4  'WPCDAT'.  READ(2,END=555) J J , S , ( R ( I ) . 1 = 1 , 3 ) , ( T ( K ) , K = 1 , 3 ) , ( A ( L ) , L = 1 ,3).(B(M),M=1,3) GO TO 8 8 0  c 855 C C  CONTINUE  c*** c  OR I F ( D I R E C T I O N  c c***»* c*»***  c  *  c c*** c 860  IF  .EO. REVERSE  .AND. PATH  IS F E A S I B L E )  ( I D I R . N E . 1 ) GO TO 8 6 0  READ F I L E IN REVERSE ORDER FROM END TO BEGINNING 'INDEX' I S NUMBER OF RECORDS REMAINING IF (INDEX.EO.O) GO TO 555 BACKSPACE 2 READ(2 ) d J , S , ( R ( I ) , I = 1 . 3 ) . ( T ( K ) , K = 1 . 3 ) , ( A ( L ) , L = 1 . 3 ) , (B(M),M=1,3) BACKSPACE 2 INDEX=INDEX-1 Jd=NP0INT-JJ+1 GO TO 8 8 0 ELSE  (ERROR)  CONTINUE GO TO 861  C  c*** c 880 C C  c****» c****» c  c c****» c***»*  END I F CONTINUE  SET UP TRANSFORMATION MATRIX 'HWC. THE COLUMNS THE VECTORS T. B, A, R, OBTAINED FROM 'WPCDAT'.  CONTAIN  HWC(1, 1 )=T( 1 ) HWC(2.1)=T(2) HWC(3,1)=T(3) HWC(4,1)=0. HWC(1,2)=B( 1 ) HWC(2,2)=B(2) HWC(3,2)=B(3) HWC(4,2)=0. HWC(1,3)=A(1) HWC(2,3)=A(2) HWC(3,3)=A(3) HWC(4,3)=0. HWC(1,4)=R(1 ) HWC(2,4)=R(2) HWC(3,4)=R(3) HWC(4,4)=1. CONVERT ANGLES 'WALPHA', INTO M I L L I M E T E R S  'BETA' INTO RADIANS  AND  'RHO'  -  Listing 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290  258  -  o f WORPOS.FOR a t 14:43:16 o n AUG  9,  1984 f o r  C C i d = O A C O Page  5  C DP I=4.DO*DATAN(1.DO) DE2RAD=DPI/180. DALPHA=WALPHA*DE2RAD DBETA=BETA*DE2RAD DRHO=STKOUT C  c  c c***»* c***»* c c  65 C  CALCULATE  ELEMENTS DF  MATRIX  'HCG'.  X(1)=0. X(2)=DC0S(DALPHA ) *DCOS(DBETA ) X(3)=DSIN(DBETA) Z(1)=-DSIN(DALPHA) Z(2)=DSIN(DBETA) Z(3)=-DCOS(DALPHA ) *DCOS(DBETA ) H C G ( 1 , 4 ) = -DRH0*Z( 1 ) HCG(2,4)=-DRH0*Z(2) HCG(3,4)=-DRH0*Z(3) HCG(4.4)=1. THE  SECOND COLUMN I S THE CROSS PRODUCT OF THE F I R S T COLUMNS  CALL  THIRD  AND  XPROD(Z.X.Y)  DO  65 1=1,3 H C G ( I , 1 ) = X( I ) HCG(I,2)=Y(I ) H C G ( I , 3 ) = Z( I ) CONTINUE HCG(4,1)=0. HCG(4,2)=0. HCG(4,3)=0.  C C  c****» c***** c  c c c***»* c c c c***»* c***** c****»  c c c  PERFORM MATRIX MULTIPLICATIONS HOG=HOT*HTW*HWC*HCG CALL CALL CALL  TO OBTAIN  HMULT(HOT.HTW,HOW) HMULT(HOW,HWC,HOC) HMULT(HOC,HCG,HOG)  CONVERT DOUBLE P R E C I S I O N ' H O T ' , ' H O W ' . ' H O G ' P R E C I S I O N 'SHOT'.SHOW','SHOG'. CALL CALL  HOG.  INTO  SINGLE  MATCNV(HOC,SHOC,1) MATCNV(HOG,SHOG,1)  CALCULATE HOG=INH0O*H0G 'INHOO' IS THE INVERSE HOMOGENEOUS TRANSFORMATION RELATING THE WORLD COORDINATE FRAME TO THE ROBOT BASE FRAME CALL  HMULTS(INHOO,SHOG,HOG)  - 2 5 9 -  Listing 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348  o f WORPOS.FOR a t 14:43:16 o n AUG  :  895 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 c c c c c***** c***** c c c c c c* * * * * £«*»*» c***»* c***»* c****»  9.  1984 f o r CC1d=DAC0  Page  6  CONTINUE  I CALCULATE ROBOT JOINT ANGLES, TEST FOR L I M I T S  I  '  IF THE ROBOT HAS 5 DEGREES OF FREEDOM, THEN CALCULATE VECTORS 'N'.'O' AS FUNCTIONS OF VECTOR 'A' AND POSITION 'P'. THERE ARE TWO SOLUTIONS, SELECTED BY SETTING 'ISOLN'=0 OR 1. THE RESULT IS ONE OF TWO UNIQUE TRANSFORMATIONS 'HOG'=[n,o,a,pj IF(NRDOF.EQ.5) CALL  WHILE(PATH NOT  N0AP5(IS0LN)  FEASIBLE  AND  COUNT L T . MAX)  ROTATE TOOL  SEARCH FOR A F E A S I B L E SOLUTION BY ROTATING THE TOOL ABOUT I T S A X I S . THIS A P P L I E S TO 6 DEGREE OF FREEDOM ROBOTS ONLY, FOR WHICH THE TOOL FRAME VECTORS 'N'.'O' CAN BE ARBITRARILY S P E C I F I E D . THE I N I T I A L 'N'.'O' ARE AS CALCULATED PREVIOUSLY. THE FIRST CALL TO 'TOLROT' Y I E L D S A ROTATION OF ZERO. CALL  TOLROT(ROTOLD,ICOUNT,ROTNEW,INHAG)  CALCULATE ROBOT TRANSFORMATION H06 = HOG *INHAG*INH6A CALL CALL  MATRIX  'H06'.  HMULTS(HOG.INHAG.HOA) HMULTS(HOA,INH6A,H06)  CALCULATE JOINT ANGLES FOR A GIVEN ROBOT TOOL MOUNTING FLANGE FRAME MATRIX 'H06' . I FLAG1 : 0- OK, 1- ENVELOPE EXCEEDED CALL  JOINTS(H06,THETA,IFLAG1)  I F ( I F L A G 1 . N E . 0 ) GO TO 861  TEST THE CALCULATED JOINT POSITIONS AGAINST THEIR PHYSIl L I M I T S . I F L A G 2 : 0- OK, 1- JOINT L I M I T EXCEEDED CALL  LIMITS(THETA,IFLAG2,IJOINT)  I F ( I F L A G 2 . N E . 0 ) GO TQ 776 TEST JOINT 6 L I M I T S (CHECK I F JOINT 6 ROTATES IN ONE D I R E C T I O N THROUGH A JOINT L I M I T ) . ALTHOUGH JOINT 6 ALLOWS MORE THAN 360. DEGREES OF ROTATION, WE CANNOT DRIVE THE JOINT CONTINUOUSLY IN ONE DIRECTION. IF A L I M I T IS REACHED, THE JOINT WILL ROTATE ONE ROTATION AWAY FROM THE L I M I T BEFORE PROCEEDING. THIS CAN RESULT  -  Listing 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406  260  -  o f WORPOS.FOR a t 14:43:16 o n AUG  c***** c*»**» c****« c****« c***** c****» c****»  IN A CATASTROPHIC EACH LOCATION HAS  c***«* c***»* c****» c****» c  I F ( J J . G T . 1 ) GO INITIALIZE  c  CHECK  TO  777  PARAMETERS AT  I F JOINT 6 CAN  IF NOT, CHECK ROTATION  START  OF  SEAM.  BE ROTATED ONE  IF JOINT CAN BE  112  TO  ROTATION 111  ROTATED MINUS  I F ( T 6 I N I T - 0 N E R 0 T . G T . R M I N ( 6 ) ) GO GO TO 113  c  TO  ONE  112  CONTINUE 0NER0T=-36O. GO TO 111 CONTINUE  113  c c****» c***»* c*»««* c  IF JOINT 6 CANNOT BE ROTATED ONE ROTATION IN EITHER D I R E C T I O N FROM THE I N I T I A L POSITION, SET ' F T E S T ' , 'J6FLG2' FTEST=1 J6FLG2=1  c  c c***** c  DISCONTINUITY OF MOTION EVEN THOUGH A VALID SOLUTION.  I F ( T 6 I N I T + 0 N E R 0 T . L T . R M A X ( 6 ) ) GO  c c****» c***** c  c  7  T6LAST=0. T6SUM=0. T6SUM2=0. J6FLG1=0 J6FLG2=0 T6INIT=THETA(6) 0NER0T=36O.  c c* * * * * c  111 777 C  1984 f o r C C 1 O D A C 0 P a g e  T 6 L A S T - LAST JOINT 6 ANGLE T6SUM- JOINT 6 NET ROTATION FROM I N I T I A L POSITION T6SUM2- T6SUM + 360. J 6 F L G 1 - 1=T6SUM EXCEEDED JOINT L I M I T S J G F L G 2 - 1=T6SUM2 EXCEEDED JOINT L I M I T S T 6 I N I T - I N I T I A L JOINT POSITION ONEROT- ONE ROTATION ( 3 6 0 . DEG.) F T E S T - 1=0NE ROTATION IN EITHER DIRECTION FROM I N I T I A L ANGLE EXCEEDS JOINT L I M I T S  Q** * * *  c  9.  CONTINUE CONTINUE UPDATE CURRENT JOINT P O S I T I O N  'T6SUM'  T6SUM=T6SUM+THETA(6)-T6LAST T6LAST=THETA(6) IF  'TSUM' EXCEEDS  JOINT  L I M I T , SET  'J6FLG1'  IF(T6SUM.LT.RMIN(6).OR.T6SUM.GT.RMAX(6))  J6FLG1=1  - 261 -  Listing 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 4 30 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464  o f WORPOS.FOR a t  14:43:16 o n AUG  9,  1984 f o r CCid=DACO P a g e  8  C I F ( F T E S T . E O . 1 ) GO TO 118 T6SUM2=T6SUM+0NER0T C  c***** c  IF  c**»*» c***»* c c  776 C C  c**»** c**»*» c***«* c«*»** c c c***** c»**»* c  830 C  c****« c****» c c c**» c  Q*****  c c 885 C  c«*«»* c***»* c c c***»* c****»  EXCEEDS JOINT  LIMIT,  SET  'J6FLG2'  IF(T6SUM2.LT.RMIN(6).OR.T6SUM2.GT.RMAX(6))  c 1 18 C C  'T6SUM2'  J6FLG2'1  CONTINUE  IF EITHER ' J 6 F L G 1 ' OR THROUGH A J O I N T L I M I T  'J6FLG2'  IS SET, JOINT 6 HAS MOVED  I F ( J 6 F L G 1 . E O . O . O R . J 6 F L G 2 . E 0 . 0 ) GO  TO 885  CONTINUE  IF  PATH NOT F E A S I B L E , ROTATE TOOL AND TRY AGAIN UNTIL ALL S P E C I F I E D ROTATIONS HAVE BEEN T R I E D . FOR A 5 DEGREE OF FREEDOM ROBOT, TEST THE TWO POSSIBLE JOINT SOLUTIONS. I F ( N R D O F . N E . 5 ) GO  TO 830  'ISOLN'=0 OR 1 S E L E C T S THE TWO FREEDOM SOLUTIONS  POSSIBLE  5 DEGREE OF  IS0LN=IS0LN+1 I F ( I S O L N . G T . 1 ) GO TO 8 5 0 GO TO 895 CONTINUE FOR A 6 DEGREE OF FREEDOM ROBOT, INCREMENT THE TOOL AXIS ROTATION COUNTER 'ICOUNT' AND RETRY, UNTIL A MAXIMUM ROTATION IS REACHED. IC0UNT=IC0UNT+1 I F ( I C O U N T . L E . I M A X ) GO END  TO 895  WHILE  RESET TOOL ROTATION COUNTER  'ICOUNT'  ICOUNT=1 GO TO 8 5 0 CONTINUE RESET TOOL ROTATION COUNTER 'ICOUNT' AND RE DEFINE THE I N I T I A L ROTATION OF THE TOOL AS THE CURRENT ROTATION ICOUNT=1 ROTOLD=ROTNEW IF A F E A S I B L E SECTION.  PATH HAS NOT  BEEN FOUND, SKIP THE NEXT  - 262  Listing 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 48 1 482 483 484 485 486 487 488 489 490 49 1 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 51 1 512 513 514 515 516 517 518 519 520 521 522  -  o f WORPOS.FOR a t 14:43:16 o n AUG  9,  1984 f o r CCid=DACO P a g e  9  C I F ( I F E A S . N E . O ) GO TO 789 C C  c »•«» * * c c  c***»*  c** • * * c  ., CALCULATE CALL  HOC=INHOO*HOC  HMULTS(INHOO.SHOC.HOC)  FIND SEAM COORDS.  LATERAL  AND  NORMAL DIRECTIONS R E L A T I V E  10 ROBOT  DO  1 15 C C  c****» c»»*** c  c c c**«** c***** c»****  c  225 226 C C  c** * * * c***»* c 99 98 C C 789 C  c*»* c c c c***** c 861  115 1=1,3 EN0RM(I)=HOC(I,3) ELAT(I)=H0C(I,2) CONTINUE  CALCULATE PUMA COORDINATES X.Y.Z.O.A.T MOUNTING FLANGE FRAME MATRIX 'H06'.  FROM THE ROBOT TOOL  CALL NOAP1(H06,XYZOAT)  READ THE WELDING CURRENT AND SPEED FROM THE WELD PARAMETER F I L E 'WPARAM'. IF WELDING DIRECTION IS REVERSE, READ THE F I L E FROM END TO BEGINNING I F ( I D I R . E Q . O ) GO TO 225 BACKSPACE 7 READ(7 , * ) CURRNT,SPEED BACKSPACE 7 GO TO 226 CONTINUE READ(7,* ) CURRNT,SPEED CONTINUE  WRITE ROBOT COORDINATES, WELDING CURRENT AND SPEED, AND SEAM LATERAL AND NORMAL D I R E C T I O N VECTORS INTO FILE 'ROBDAT'. WRITE(3,99) Jd,(XYZOAT(I).I=1,6),CURRNT,SPEED FORMAT(IX.I4.3F8.1.3F8.2.F8.2.F7.3) WRITE(3,98) ( E N O R M ( I ) , 1 = 1 , 3 ) . ( E L A T ( J ) , J = 1 , 3 ) F0RMAT(5X,6F11.3)  CONTINUE END  WHILE  GO TO  150  ERROR  MESSAGES  ROBOT ENVELOPE EXCEEDED CONTINUE  - 263 -  L i s t i n g o f WORPOS.FOR a t 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580  C  c  850 C 555 C C  c«***» c***** c»»**» c  c c  c*»*«*  c  c c 420  410 C  c***»* c c  450 400 C c»****  c  c c c  14:43:16 on AUG  9, 1984 f o r CC1d=DAC0 Page  10  ISTAT=1 GO TO 555 JOINT LIMIT EXCEEDED CONTINUE ISTAT=2 GO TO 555 CONTINUE IF A FEASIBLE PATH HAS BEEN FOUND, REPEAT THE LAST PATH AND STORE THE RESULTS IN THE OUTPUT FILES. ' IFEAS' =0 INDICATES A FEASIBLE PATH HAS BEEN FOUND. IF(ISTAT.NE.O) GO TO 75 IFEAS=IFEAS-1 IF(IFEAS.LT.O) GO TO 75 GET CURRENT TABLE POSITION FROM TEMPORARY FILE  -  'STOVAR'.  CALL OPEN(1,STOVAR,4,1) 'IVAR' IS A TEMPORARY COUNTER IVAR=1 READ(1.* ) CONTINUE READ(1,*,END=410) TABVAR(IVAR) IVAR=IVAR+1 GO TO 420 CONTINUE CALL SHUTO) WRITE SEAM NUMBER, TABLE POSITION AND ROBOT CONFIGURATION INTO FILE 'LOCSET' . GO TO CURRENT END OF FILE CONTINUE READ(4,*.END=400) GO TO 450 CONTINUE APPEND PARAMETERS WRITE(4,*) WRITE(4,*) WRITE(4,*) WRITE(4,*)  DESCRIBING SEAM  'IDSEAM'  IDSEAM (TABVAR(I),1=1,6) (ICONF(I),1=1,6) VOLT.WSIZE,FEED,GFR  REWIND FILE 'WPCDAT' REWIND(2) RESET THE TOOL ROTATION ANGLE TO ZERO  - 264 -  Listing 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602  o f WORPOS.FOR a t 14:43:16 o n AUG  9,  1984 f o r CC1d=DAC0 P a g e  11  C ROTOLD=0. C C C****» C***»* C  REPEAT LAST PATH, WHICH IS F E A S I B L E , F I L E 'ROBDAT' T H I S TIME. GO  C C 75 C C***** C  TO  AND  GENERATE  160  CONTINUE CLOSE CALL CALL CALL CALL  DATA  FILES  SHUT(2) SHUT(3) SHUT(4) SHUT(7)  C C RETURN END  'WPCDAT',  'ROBDAT'.  'LOCSET'.  THE  ROBOT DATA  -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30  of  265  -  XPROD.FOR a t 14:43:20 o n AUG SUBROUTINE C C C C C C C C C  THIS  1984 f o r  9,  CC1d=DAC0  c c  c  100  SUBROUTINE  CALCULATES VECTOR C = A X B (CROSS PRODUCT).  INPUTS: A ( 3 ) = F I R S T VECTOR B(3)=SEC0ND VECTOR OUTPUT: C(3)=N0RMALIZED  CROSS  PRODUCT,  REAL A ( 3 ) , B ( 3 ) , C ( 3 ) , ABS C(1)=A(2)*B(3)-A(3)*B(2) C ( 2 ) = A ( 3 ) * B ( 1 )-A(1 ) * B ( 3 ) C(3)=A(1 )*B(2)-A(2)*B( 1 ) ABS = S Q R T ( C ( 1 ) * * 2 + C ( 2 ) * * 2 + C ( 3 ) * * 2 ) I F ( A B S . E 0 . O . ) GO TO 100 C(1 )=C(1 )/ABS C(2)=C(2)/ABS C(3)=C(3)/ABS CONTINUE RETURN END  1  XPROD(A.B.C)  c c***»* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * c c c  Page  A X B  - 266 -  APPENDIX  1.3  CAD FILE GENERATING PROGRAM  CADGEN LISTING  -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  of  CADGEN.FOR  at  c c c  -  14:50:38 o n AUG  PROGRAM C C C C C C C  267  9.  1984 f o r  CC1d=DAC0  Page  CADGEN  T H I S PROGRAM GENERATES LOCATION AND SURFACE NORMAL DATA FOR A V E R T I C A L CYLINDER INTERSECTING A HORIZONTAL PLATE THE DATA IS STORED IN CAD F I L E S IN THE CORRECT FORMAT FOR T E S T I N G AND DEMONSTRATION DF PROGRAM 'AUTOP'.  SUBROUTINES  CALLED:  OPEN SHUT  c BYTE B U F ( 8 0 ) , B U F 2 ( 8 0 ) , B U F 3 ( 8 0 ) , B U F 4 ( 8 0 ) , B U F 5 ( 8 0 ) REAL R ( 3 ) , N 1 ( 3 ) , N 2 ( 3 ) , R 0 ( 3 ) INTEGER ISTAT.IWELD DATA ISTAT/O/,IWELD/O/ DATA B U F / ' C . 'A ' .'D ' . ' 1 ' . ' . ' , 'D','A', 'T'.72*0/ DATA B U F 2 / ' C , ' A ' . ' D ' , ' 2 ' , ' . ' , ' D ' , ' A ' . ' T ' . 7 2 * 0 / DATA B U F 3 / ' C , ' A ' , ' D ' , ' 3 ' , ' . ' . ' D ' , ' A ' , ' T ' . 7 2 * 0 / DATA B U F 4 / ' C , ' A ' . ' D ' , ' 4 ' , ' . ' . ' D ' , ' A ' , ' T ' , 7 2 * 0 / DATA B U F 5 / ' C , ' A ' . ' D ' , ' 5 ' , ' . ' , ' D ' . ' A ' . ' T ' . 7 2 * 0 / DATA R O / 1 0 0 . , 1 0 0 . . 1 0 0 . /  c  c*«*»*  CREATE  FORMATTED  FILES  c IEND=10O  c RADIUS=100. PI=3.14159 TW0PI=2.*PI HALFPI=.5*PI STEP=TWOPI/100. THETA=0.  c c****» c  DUMMY WELD  PARAMETERS  JTYPE=1 XSECT=.5 T1=.25 T2=.5 NP0INT=26 ICURVE=1  c DO 5 0 0 IND=1.4 INDEX=1 I F ( I N D . E O . I ) CALL IF(IND.EQ.2) CALL I F U N D . E Q . 3 ) CALL I F ( I N D . E 0 . 4 ) CALL  OPEN(1,BUF,2,1) OPEN(1,BUF2,2,1) OPEN( 1 , BUF 3 ,2,1) OPEN(1,BUF4,2,1)  c  c»*»**  WRITE  JTYPE,XSECT.T1,T2  INTO CAD  FILE.  c WRITE( 1 ) J T Y P E . X S E C T , T 1 , T 2 , N P O I N T . I C U R V E  c c  1  THETA=FLOAT(IND-1)*HALFPI  - 268 -  Listing 59 GO 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 1 1 1 1 12 1 13 1 14 1 15 116  o f CADGEN.FOR a t 14:50:38 C C  WHILE  (THETA  o n AUG  9,  1984  f o r CC1d=DAC0 P a g e  2  .LT. TWOPI)  DO 10Q IP0i'NT=T,26 R(1 )=R0(1 )+RADIUS*COS(THETA) R(2)=R0(2)+RADIUS*SIN(THETA) R(3)=R0(3) N1(1 )=COS(THETA) N1(2)=SIN(THETA) N1(3)=0. N2(1)=0. N2(2)=0. N2(3)=1. C C C  WRITE RESULTS  * 900 C C C  100 C C C 500 C  TO  FILE.  WRITE( 1 ) I N D E X . ( R ( I ) , I = 1 . 3 ) , ( N 1 ( K ) , K = 1 , 3 ) . ( N 2 ( L ) , L = 1 , 3 ) , ISTAT,IWELD WRITE(5,900) INDEX,(R(I),1=1,3) F0RMAT(I5,3F12.4) INCREMENT  THETA,INDEX  THETA=THETA+STEP INDEX=INDEX+1 CONTINUE END  WHILE  CALL S H U T ( 1 ) CONTINUE R(1)=200. R(2)=100. R(3)=100. N1( 1 ) = 1 . N1(2)=0. N1(3)=0. N2(t)-1 . N2(2)=0. N2(3)=0. CALL OPEN(1.BUF5.2,1)  C NP0INT=50 ICURVE=0 C WRITE(1 )  JTYPE,XSECT,T1,T2.NPOINT,ICURVE  C DO  N  30O C  3 0 0 I = 1 ,50 WRITE( 1 ) I , ( R ( K ) , K = 1 , 3 ) , ( N 1 ( L ) , L = 1 . 3 ) , ( N 2 ( M ) , M = 1 , 3 ) , I S T A T R(3)=R(3)+4. W R I T E ( 5 , 9 0 0 ) I . ( R ( J ) .J=1.3) CONTINUE GO  C 10 1 1 C  TO  999  WRITE(5.11 ) FORMAT(' ERROR ENCOUNTERED IN OPEN  STATEMENT')  -  Listing 117 1 18 119 120 121 122  o f CADGEN.FOR a t 999 C  14:50:38  CONTINUE CALL  C STOP END  SHUT(1)  269  -  o n AUG  9.  1984  f o r CC1d=DAC0 P a g e  - 270 -  APPENDIX 1.4  TESTIN  PROGRAM LISTING  - 271 -  Listing 1 2 3 4 5 6 7 8 9 10 11 T2 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 38 39 40 41 ai 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f TESTIN.FOR  a t 14:48:01  o n AUG  9. 1984 f o r C C i d = 0 A C 0 P a g e  1  C PROGRAM T E S T I N C C T H I S PROGRAM T E S T S FOR INTERFERENCE BETWEEN A FACE OF A SOLID C POLYHEDRON AND THE VOLUME SWEPT OUT BY A CYLINDER AS IT IS C TRANSLATED AND ROTATED FROM AN I N I T I A L P O S I T I O N TO A FINAL C POSITION. C C D E F I N I T I O N OF VARIABLES AND CONSTANTS: C C A1.B1ENDPOINTS OF I N I T I A L CYLINDER SPINE POSITION. C A2.B2ENDPOINTS OF FINAL CYLINDER SPINE POSITION. C P1.P2....P8CORNER POINTS OF PARAMETRIC SWEPT VOLUME. C VERTPOLYHEDRA VERTEX L I S T . C F A C E S - POLYHEDRA FACE PLANE PARAMETER L I S T . C I F A C E - POLYHEDRA FACE L I S T . C RADIUS- RADIUS OF CYLINDER C E 1 . E 2 - ENDPOINTS OF CURRENT EDGE C K1.K2- SPINE D I R E C T I O N VECTORS. C K1N.K2N- NORMALIZED K1.K2 C C SUBROUTINES C A L L E D : C C CYLINT C DIFF C DRCYL C FACE C FACINT C FACYL C INVOL C SURFIN C SWPVOL C UNIT C C ' I G L ' SUBROUTINES C A L L E D : C C CMCLOS C CMOPEN C EYEBAL C GRSTOP C GRSTRT C VRP3D C WIND3D C ZPERSP C C C*«***«*«**««*****«»«»*******«.«»*****««***«*»*»««****»»•«*»**«»**«****»* C REAL E 1 ( 3 ) . E 2 ( 3 ) REAL K 1 ( 3 ) , K 2 ( 3 ) , K 1 N ( 3 ) , K 2 N ( 3 ) C COMMON / S P I N E S / A 1 ( 3 ) , B 1 ( 3 ) , A 2 ( 3 ) , B 2 ( 3 ) COMMON / D A T A / V E R T ( 1 0 0 O , 3 ) , F A C E S ( 1 0 0 , 4 ) , I F A C E ( 1 0 0 , 2 ) , I O B J ( 10, 2 ) 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 ) COMMON /RADIUS/RADIUS C C****» I N I T I A L I Z E ' I G L ' GRAPHICS ENVIRONMENT C CALL GRSTRT(4027.1)  - 272 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 1 1 1 112 1 13 1 14 115 1 16  o f TESTIN.FOR  a t 14:48:01  CALL CALL CALL CALL CALL C Q* * * * *  c c c***»* c  c c».» * ** c c c**»»* c c*» * * * c c  Q****  c  SWEPT  AND  f o r CC1d=DAC0 P a g e  2  VOLUME.  DRAW CYLINDERS AT  INITIAL  AND  FINAL  POSITIONS.  DIFF(B1,A1,K1) UNIT(K1,K1N) DIFF(B2,A2.K2) UNIT(K2,K2N) DRCYL(A 1,B1,K1N) DRCYL(A2,B2,K2N)  INTERACTIVELY  SPECIFY  CALL  FACE  FIND  INTERSECTIONS  REPEAT DO  1984  SWPVOL  CALCULATE CALL CALL CALL CALL CALL CALL  9,  ZPERSP EYEBAL(100.,100.,100.) VRP3D(0.,0..0.) WIND3D(-50..50.,-50.,50.,-50.,50.) CMCLOS  GENERATE CALL  o n AUG  FOR  A TYPICAL  BETWEEN FACE  EACH EDGE OF  THE  POLYHEDRON FACE  EDGES AND  FOR  TESTING.  SWEPT VOLUME  SURFACES  FACE...  10 I = 1 . I F A C E ( 1 , 2 )  FIND  THE  ENDPOINTS E1.E2 OF  THE  CURRENT'EDGE.  00  15  c  c* * * * * c c c  15 d=1,3 E1(d)=VERT(1,0) E2(J)=VERT(1+1.J) CONTINUE  T E S T FOR INTERSECTION BETWEEN THE CURRENT EDGE AND BOUNDING PARAMETRIC FACES OF THE SWEPT VOLUME.  THE  FOUR  CALL SURFIN(P1,P2.P3.P4.E1,E2,INT) IF ( I N T . E O . 1 ) GO TO 9 0 0 CALL SURFIN(P5,P6,P7,P8,E1,E2.INT) IF ( I N T . E O . 1 ) GO TO 9 0 0 CALL SURFIN(P1,P5,P6,P2,E1,E2,INT) IF ( I N T . E O . 1 ) GO TO 900  C  c c c  CALL SURFIN(P3,P4.P8.P7.E1,E2.INT) IF ( I N T . E O . 1 ) GO TO 9 0 0 FIND  INTERSECTIONS  BETWEEN EDGES AND  CALL C Y L I N T ( A 1,B1,E1,E2,INT) I F ( I N T . E O . 1 ) GO TO 901 CALL C Y L I N T ( A 2 , B 2 . E 1 , E 2 , I N T ) I F ( I N T . E Q . 1 ) GO TO 901  CYLINDRICAL  VOLUMES  - 273 -  Listing 1 17 1 18 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174  o f TESTIN.FOR C 10  c c***»* c c****»  c c c c c c c c c c c c c  c****»  c c c  Q** * * *  c c c  a t 14:48:01  o n AUG  9.  1984 f o r CC1d=DAC0 P a g e  3  CONTINUE END OF DO LOOP TEST  I F ANY  ***************************************  SWEPT  VOLUME  EDGE INTERSECTS  FACE.  CALL FACINT(1,P1,P2,INT) I F U N T . E Q . 1 ) GO TO 9 8 0 CALL FACINT(1,P2,P3,INT) I F ( I N T . E O . 1 ) GO TO 9 8 0 CALL F A C I N T ( 1 . P 3 . P 4 . I N T ) I F ( I N T . E O . 1 ) GO TO 9 8 0 CALL FACINT(1.P4.P1,INT) I F ( I N T . E O . 1 ) GO TO 9 8 0 CALL F A C I N T ( 1 , P 1 , P 5 . I N T ) I F ( I N T . E O . 1 ) GO TO 9 8 0 CALL FACINT(1.P2.P6,INT) I F ( I N T . E Q . 1 ) GO TO 9 8 0 CALL FACINT(1.P3.P7,INT) I F ( I N T . E Q . 1 ) GO TO 9 8 0 CALL F A C I N T ( 1 , P 4 , P 8 , I N T ) I F ( I N T . E Q . 1 ) GO TO 9 8 0 CALL FACINT(1.P5.P6,INT) I F ( I N T . E Q . 1 ) GO TD 9 8 0 CALL FACINT(1.P6.P7,INT) I F ( I N T . E Q . 1 ) GO TO 9 8 0 CALL F A C I N T ( 1 , P 7 , P 8 . I N T ) I F ( I N T . E Q . 1 ) GO TO 9 8 0 CALL F A C I N T ( 1 . P 8 . P 5 . I N T ) I F ( I N T . E Q . 1 ) GO TO 9 8 0 TEST  I F CYINDER IN I N I T I A L  OR  FINAL POSITION  INTERSECTS ANY  CALL FACYL(1,A1,B1,INT) I F ( I N T . E Q . 1 ) GO TO 9 9 0 CALL FACYL(1,A2,B2,INT) I F ( I N T . E Q . 1 ) GO TO 9 9 0 TEST  IF. A POLYHEDRON L I E S  CALL I N V 0 L ( 1 , I N T ) I F ( I N T . E Q . 1 ) GO TO 991 GO TO 998  INSIDE THE SWEPT  VOLUME.  FACE.  - 274 -  L i st1ng  175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198  o f TESTIN FOR c**«** c 900 901 980 990 991 C 998 C 999 C C  at  14:48  INTERSECTON WRITE(5,*) GO TO 999 WRITE(5,*) GO TO 999 CONTINUE WRITE(5.*) GO TO 999 WRITE(5,*) GO TO 999 WRITE(5,*) GO TO 999  ********* FACE EDGE INTERSECTED SWEPT SURFACE ' FACE EDGE INTERSECTED CYLINDRICAL ENDS ' SWEPT VOLUME EDGE INTERSECTED FACE ' CYLINDER INTERSECTED FACE '  POLYHEDRON LIES ENTIRELY INSIDE SWEPT VOLUME  CONTINUE WRITE(5.*) CONTINUE CALL CMOPEN CALL GRSTOP STOP END  1 * * * * * * * * 4  NO INTERSECTIONS WERE DETECTED '  - 275 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f CORNER.FOR a t 14:43:49 o n AUG  9, 1984 f o r CC1d=DAC0  Page  1  SUBROUTINE CORNER C T H I S SUBROUTINE FINDS THE CORNER POINTS OF THE SWEPT VOLUME. C C COMMON BLOCKS ACCESSED: /LINK/./RADIUS/./VOLUME/./SNORM/ C COMMON BLOCKS MODIFIED: /CORNER/ C C C************************************************* * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  c  c  REAL LEN.RADIUS REAL AN(3),BN(3),CN(3) REAL N ( 3 ) COMMON /LINK/H1(4.4),H2(4,4).LEN COMMON /RADIUS/RADIUS COMMON / V 0 L U M E / P ( 3 ) , U 0 ( 3 ) , U 1 ( 3 ) . V 0 ( 3) . V1 ( 3 ) COMMON / S N 0 R M / 0 A N ( 3 ) , D B N ( 3 ) , D C N ( 3 ) COMMON / C 0 R N E R / P 1 ( 3 ) , P 2 ( 3 ) , P 3 ( 3 ) , P 4 ( 3 ) , P 5 ( 3 ) > 6 ( 3 ) , P 7 ( 3 ) , P 8 ( 3 )  c c c****» c  LAMBDAU=LAMBDAV=0 10 1=1.3 N(I)=DAN(I ) CONTINUE  DO 10  c c  ALPHA = SQRT(N(1 ) » * 2 + N ( 2 ) * * 2 + N ( 3 ) * * 2 ) 20 1=1.3 P1(I)=P(I)-RAOIUS*DAN(I)/ALPHA P5(I)=P1(I)+2.*RADIUS«DAN(I)/ALPHA CONTINUE DO  20  c c c***«* c  LAMBDAU=1.LAMBDAV=0 30 1=1,3 N(I)=DAN(I)+DBN(I) CONTINUE DO  30 C  ALPHA=SQRT(N(1)»*2+N(2)**2+N(3)**2)  c DO  40 C C  c****» c  4 0 1=1.3 P2(I)=P(I)+U0(I)-RADIUS*N(I)/ALPHA P6(I)=P2(I)+2.*RADIUS*N(I)/ALPHA -CONTINUE  LAMBDAU=1.LAMBDAV=1 5 0 1=1.3 N(I)=DAN(I)+DBN(I)+DCN(I) CONTINUE DO  50 C  ALPHA=SORT(N(1)**2+N(2)**2+N(3)*«2) C DO  6 0 1=1.3 P3(I)=P(I)+U0(I)+V1(I)-RADIUS*N(I)/ALPHA  - 276 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77  o f CORNER.FOR a t 14:43:49 o n AUG  60 C C C***«*  c  70  c  9,  1984 f o r CC1d=DAC0  P 7 ( I )=P3(I ) + 2.*RADIUS*N(I)/ALPHA CONTINUE  LAMBDAU=0,LAMBDAV=1 DO 7 0 I=1.3 N(I)=DAN(I)+DCN(I) CONTINUE ALPHA=SQRT(N(1)**2+N(2)**2+N(3)**2)  c  80 C  DO 8 0 1-1.3 P4(I)=P(I)+V0(I)-RADIUS*N(I)/ALPHA P8(I)=P4(I)+2.*RADIUS*N(I)/ALPHA CONTINUE RETURN END  Page  2  - 277 -  Listing 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 23 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f CYLEND.FOR a t 14:43:50 o n AUG SUBROUTINE C C C C C  c c c c c c c c c c c  9.  1984 f o r CC1d=DAC0 P a g e  1  CYLEND(P.U,PC,U1,01.02.INT)  T H I S SUBROUTINE T E S T S IF AN EDGE WITH ENDPOINTS 01,02 I N T E R S E C T S THE END OF A CYLINDER OF RADIUS 'RADIUS'. P , U ( 3 ) DEFINE THE PLANE OF THE C I R C L E AND P C ( 3 ) D E F I N E S I T S CENTER. U1 IS THE EDGE D I R E C T I O N VECTOR. INPUT  ARGUMENTS: P,U(3 ) , P C ( 3 ) ,U1(3 ) , 0 1 ( 3 ) , 0 2 ( 3 )  OUTPUT ARGUMENTS:  INT- 0=NO INTERSECTION 1=INTERSECTI0N  SUBROUTINES C A L L E D :  DIFF DOTPRD VLEN  »**«.«»«»«»»..«**.«*««»«««*,«**.«««*»»»**««»«*.««»**«*«*»*«*»**.».*»»  c  c c c c c****» c c c  REAL  COMMON  TEST  I F BOTH ENDPOINTS ARE  CALL CALL  D0TPRD(Q1,U,01U) DOTPRD(02,U.Q2U)  I F ( Z . G T . O . ) GO  c c*»*»* c  IF  20  c c***«* c c c  ON  THE  SAME SIDE OF  THE  PLANE.  Z=(Q1U-P)*(Q2U-P)  TEST  c c***** c****» c  /RADIUS/RADIUS  INT=0  c c***»* c  c  U ( 3 ) , P C ( 3 ) , U 1 ( 3 ) ,01(3) ,02(3),TEMP(3),PINT(3)  I F THE  TO  SPINE  I F ( 0 1 U . E 0 . 0 U 2 ) GO THE  SPINE  80O IS PARALLEL TO  TO THE  PLANE.  800  INTERSECTS  THE  PLANE,  FIND  THE  INTERSECTION  POINT  RLAMB=(P-Q1U)/(Q2U-Q1U) DO  20 1=1.3 PINT(I)=Q1(I)+RLAMB*U1(I) CONTINUE FIND THE CYLINDER  DISTANCE FACE.  FROM THE  CALL CALL  OIFF(PINT,PC,TEMP) VLEN(TEMP.DIST)  TEST  I F THE DISTANCE  I F ( D I S T . G T . R A D I U S ) GO INT=1  INTERSECTION  I S L E S S THAN THE TO  800  POINT  RADIUS.  TO THE  CENTER  OF  - 278 -  Listing 59 60 61  o f CYLEND.FOR 800  a t 14:43:50  CONTINUE RETURN END  o n AUG  9,  1984 f o r CC1d=DAC0  Page  -  Listing 1 o •c  3 4 5  c  to 7 8 Q y  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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f CYLINT.FOR r>  a t 14:43:50  SUBROUTINE  279 -  o n AUG  9, 1984 f o r CC1d=DAC0 P a g e  1  CYLINT(P1.P2,01.02.INT)  \*  c c  T H I S SUBROUTINE T E S T S I F A LINE SEGMENT WITH ENDP3INTS 01,02 INTERSECTS A CYLINDER WITH SPINE ENDPOINTS P 1 . P 2 . AND A RADIUS 'RADIUS'.  c c  INPUT ARGUMENTS: P 1 ( 3 ) , P 2 ( 3 ) - C Y L I N D E R SPINE ENDPOINTS. 0 1 ( 3 ) , 0 2 ( 3 ) - L I N E SEGMENT ENDPOINTS.  C  /*  V-  c c c c c c c c c c c c c c c c c c c  OUTPUT ARGUMENTS:  I N T - 0=NO INTERSECTION 1=INTERSECTI0N  D E F I N I T I O N OF CONSTANTS AND  VARIABLES.  LAMB 1,LAMB2- PARAMETERS S P E C I F Y I N G LOCATION OF COMMON NORMAL. D I S T - DISTANCE BETWEEN EDGE LINE AND SPINE L I N E . PA.PB- DISTANCES FROM ORIGIN TO CYLINDER END FACE PLANES. UCYL- SURFACE NORMAL OF CYLINDER END FACES. SUBROUTINES C A L L E D :  CYLEND DIFF DOTPRD SOLVE UNIT VLEN XPROD  £**»«*« * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * c  c  c c*-***« c c*«*** c  c  Q** * * *  c c 120  REAL REAL REAL REAL  LAMB 1,LAMB2 P 1 ( 3 ) , P 2 ( 3 ) . 0 1 ( 3 ) , 0 2 ( 3 ) ,U1(3),U2(3),UN1(3),UN2(3),TEMP(3) T1(3),T2(3),UN(3).U(3),T3(3).T4(3),DIST1(3),DIST2(3) UTEMP(3)  COMMON COMMON COMMON FIND  /PARAM/LAMB1,LAMB2,DIST,D1,D2 /RADIUS/RADIUS /ENDS/PA.PB,UCYL(3)  DISTANCE FIRST  FIND  BETWEEN EDGE LINE AND SPINE COMMON  NORMAL  CALL D I F F ( P 1 . 0 1 , T 1 ) CALL D I F F ( P 2 , P 1 , U 1 ) CALL D I F F ( Q 2 , Q 1 , U 2 ) C A L L UNIT(U1,UN1 ) CALL UNIT(U2,UN2) C A L L XPR0D2(UN1,UN2,U) CALL VLEN(U,A2) TEST  I F L I N E S ARE P A R A L L E L .  I F ( A 2 . L T . . 0 0 0 0 1 ) GO TO 120 GO TO 130 CONTINUE C A L L XPR0D2(T1,UN1.UDIST)  LINE  - 280 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116  of  CYLINT.FOR a t  14:43:50 o n AUG  1984 f o r  9,  CC1d=DAC0 Page  2  C A L L XPR0D2(UDIST,UN1,U) CALL VLEN(UDIST.DIST) IPARAL=1 C C****» C  130  140 C C c***** C  TEST  I F L I N E S ARE  I F ( D I S T . L E . . 0 0 0 0 0 1 ) GO GO TO 140 CONTINUE CALL DOTPRD(T1,U,A 1 ) DIST=ABS(A1/A2) CONTINUE  TEST DISTANCE  AGAINST  I F ( D I S T . G T . R A D I U S ) GO C c***** C*«*** C  I F THE SPINE AND IF EITHER END OF  FIND  LENGTHS OF  TO  800  RADIUS TO  900  EDGE ARE PARALLEL, INTERSECTION CAN THE EDGE IS INSIDE THE CYLINDER.  I F ( I P A R A L . E O . 1 ) GO C C****» C  COINCIDENT.  TO  200  SPINE.RLEN1.AND  EDGE.RLEN2.  CALL VLEN(U1,RLEN1) CALL VLEN(U2.RLEN2 ) C C***»* C  FIND  VALUES  OF  LAMB 1.LAMB2  CALL  UNIT(U.UN)  C CALL S0LVE(P1,01,U1,U2,UN,DIST,LAMB C C*«*** C  TEST  1,LAMB2)  LAMB 1 ,LAMB2  IF(LAMB 1.GE.O. .AND.LAMB 1.LE.1.) IF(LAMB2.GE.0..AND.LAMB2.LE.1.)  IFLG1 = 1 IFLG2=1  C I F ( I F L G 1 . E O . 1 . A N D . I F L G 2 . E O . 1 ) GO C C***** C  FIND DISTANCES  OF  TO  800  EDGE ENDPOINTS 01,02  FROM  SPINE  CALL XPR0D2(T1,UN1,DIST1) CALL VLEN(DIST1,D1) CALL D I F F ( P 1 , 0 2 , T 2 ) C A L L XPR0D2(T2,UN 1,DIST2) CALL VLEN(DIST2,D2) C C****» C  TEST  IF DISTANCES  ARE  GREATER THAN THE  IF(D1.GT.RADIUS.AND.D2.GT.RADIUS) GO  TO  C  c***«*  CALCULATE  PA.PB.UCYL FOR  C CALL CALL CALL  DIFF(P2,P1.UTEMP) UNIT(UTEMP,UCYL) D0TPRD(P1,UCYL.PA)  CYLINDER  ENDS  RADIUS. 900  ONLY OCCUR  - 281 -  Listing 1 17 118 1 19 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154  o f CYLINT.FOR  C C***»*  c c  100  c  c c 20O C  c***»* c  c  c c 800 C  a t 14:43:50 o n AUG  CALL  D0TPRD(P2,UCYL,PB)  TEST  FOR  I N T E R S E C T I O N OF  I F ( I F L G 1 . E O . O ) GO GO TO 200  TO  1984 f o r CC1d=DAC0 P a g e  EDGES  WITH ENDS OF  3  CYLINDER  100  CONTINUE  I  CALL CYLEND(PA.UCYL.P1,U2.Q1,02,INT1) I F ( I N T 1 . E Q . 1 ) GO.TO 8 0 0  |  CALL CYLEND(PB,UCYL,P2,U2,Q1.Q2.INT2) I F ( I N T 2 . E Q . 1 ) GO TO 8 0 0 CONTINUE TEST  I F EITHER ENDPOINT  OF THE  EDGE  LIES  CALL DIFF(Q1,P1,T3) C A L L XPR0D2(T3.UN2,TEMP) CALL VLEN(TEMP,LAMB2) IF(LAMB2.GE.0..AND.LAMB2.LE.1.)  GO  TO  800  CALL D I F F ( Q 2 , P 1 , T 4 ) CALL XPR0D2(T4.UN2,TEMP) CALL VLEN(TEMP,LAMB2) IF(LAMB2.GE.0..AND.LAMB2.LE.1.)  GO  TO  800  GO  TO  900  CONTINUE INT=1  C 900  9,  CONTINUE RETURN END  INSIDE THE  CYLINDER  - 282 -  Listing 1 2 3 4 5 6 1  8 9 10  o f DET2X2.FOR  a t 14:44:07 o n AUG  SUBROUTINE C C C C r*  9,  1984 f o r CCid=DACO  Page  1  DET2X2(V1,V2,DET)  T H I S SUBROUTINE FINDS THE DETERMINANT OF A 2 X 2 MATRIX OF THE FORM [ V 1 . V 2 ] , WHERE V 1 ( 2 ) . V 2 ( 2 ) . REAL  V1(2),V2(2)  DET=V1(1)*V2(2)-V1(2)*V2(1) RETURN ENO  - 283 -  Listing  o f DET3X3.FOR  1 ti.  3 4 e  3  61 •T 8 9 10 11 12  a t 14:44:07 o n AUG  SUBROUTINE  c c  9,  1984 f o r CCid=DACO  Page  1  DET3X3(V1,V2.V3,DET)  T H I S SUBROUTINE FINDS THE DETERMINANT OF A 3 X 3 MATRIX OF THE FORM [ V 1 . V 2 . V 3 ] , V 1 ( 3 ) , V 2 ( 3 ) .V3(3 ) .  L,  REAL  *  V1(3).V2(3).V3(3)  D E T = V 1 ( 1 ) * V 2 ( 2 ) * V 3 ( 3 ) + V1 (2 ) * V 2 ( 3 ) * V 3 ( 1 ) + V 1 ( 3 ) * V 2 ( 1 ) * V 3 ( 2 ) V1(3)*V2(2)*V3(1)-V1(2)*V2(1 )*V3(3)-V1(1)*V2(3)*V3(2 )  c RETURN END  - 284 -  Listing 1 2 3 4 5 € 7 8 9 10 11 12 13  of  DIFF.FOR  at  14:44:07 on AUG  9,  1984 f o r  CC1d=DAC0  Page  SUBROUTINE D I F F ( V 2 , V 1 . D I F ) C C C C  T H I S SUBROUTINE DIFF=V2-V1 REAL  PERFORMS THE VECTOR D I F F E R E N C E  V1(3).V2(3).DIF(3)  C DO 10 C  10 1=1,3 DIF(I)=V2(I)-V1(I) CONTINUE RETURN END  OPERATION  -  Listing 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 1G  285  -  o f DOTPRD.FOR a t 14:44:28 o n AUG SUBROUTINE C C C C C C C C  c  9,  1984 f o r CC1d=DAC0  DOTPRD(V1,V2.PROD)  T H I S SUBROUTINE V1(3),V2(3).  FINDS THE DOT  INPUTS: V 1 ( 3 ) , V 2 ( 3 ) -  TWO  OUTPUTS: PROD- THE DOT  PRODUCT OF TWO  VECTORS  PRODUCT  PR0D=V1.V2  REAL V 1 ( 3 ) , V 2 ( 3 ) PR0D=V1(1)*V2(1)+V1(2)*V2(2)+V1(3)*V2(3)  c RETURN END  Page  VECTORS,  1  - 286 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f DRCYL.FOR  a t 14:44:29 o n AUG  9, 1984 f o r CCid=DACO Page  1  SUBROUTINE D R C Y L ( A . B . K ) 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  100 200  c c  c c****« c***»* c  T H I S SUBROUTINE DRAWS A CYLINDER AS AN OCTAHEDRAL APPROXIMATION. THE CYLINDER IS DEFINED BY SPINE ENDPOINTS 'A'.'B' AND BY SPINE D I R E C T I O N VECTOR ' K' . INPUT ARGUMENTS:  A ( 3 ) . B ( 3 ) - CYLINDER SPINE ENDPOINTS. K ( 3 ) - SPINE D I R E C T I O N VECTOR.  COMMON BLOCKS ACCESSED: /RADIUS/ SUBROUTINES C A L L E D :  ' I G L ' GRAPHICS  REAL REAL  ROTK TRANS3 UNIT VLEN XPROD  SUBROUTINES:  CMCLOS CMOPEN DRAW3D M0VE3D  A(3),B(3),K(3).R(3).ROT(4,4),RW(3).PA(8.3),PB(8,3) X(3),Y(3),RN(3)  DATA X / 1 . , 0 . . 0 . / DATA Y / 0 . . 1 . . 0 . / DATA DTHETA/O.7854/ COMMON  /RADIUS/RADIUS  FIND A RADIUS  VECTOR  'R' NORMAL TO THE SPINE 'K'.  CALL XPROD(X.K.R) CALL VLEN(R.RLEN) I F ( R L E N . L T . . 0 0 0 0 1 ) GO TO 100 GO TO 2 0 0 CONTINUE C A L L XPROD(Y.K.R) CONTINUE CALL  UNIT(R.RN)  THETA=0. ROTATE THE RADIUS ON THE CYLINDER. DO  VECTOR  ABOUT THE SPINE TO GENERATE  10 1=1,8 CALL ROTK(K,THETA.ROT) CALL TRANS3(ROT,RN,RW) DO 20 d=1.3 PA(I,d)=A(J)+RADIUS*RW( J ) PB(I,J)=B(d)+RADIUS*RW(d)  POINTS  - 287 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85  o f DRCYL.FOR 20 10 C  a t 14:44:29 o n AUG  30 C  40 C  CMOPEN  DRAW THE CYLINDER. CALL M 0 V E 3 D ( P A ( 8 , 1 ) , P A ( 8 , 2 ) , P A ( 8 , 3 ) ) DO 30 1=1.8 CALL D R A W 3 D ( P A ( I , 1 ) , P A ( I , 2 ) , P A ( I , 3 ) ) CONTINUE CALL M 0 V E 3 D ( P B ( 8 , 1 ) , P B ( 8 . 2 ) , P B ( 8 , 3 ) ) DO 40 1=1.8 CALL D R A W 3 D ( P B ( I , 1 ) , P B ( I . 2 ) . P B ( I . 3 ) ) CONTINUE DO  50 C  1984 f o r CC1d=DAC0  CONTINUE THETA=THETA+DTHETA CONTINUE CALL  C C***»* C  9.  50 1=1.8 CALL M 0 V E 3 D ( P A ( I , 1 ) , P A ( I , 2 ) , P A ( I , 3 ) ) CALL D R A W 3 D ( P B ( I . 1 ) , P B ( I , 2 ) , P B ( I . 3 ) ) CONTINUE  CALL  CMCLOS  C RETURN END  Page  2  - 288 -  Listing 1 2 3 4 5 6 7 8 9  10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30  o f DRPINT.FOR a t 14:44:29 SUBROUTINE C C***«* C C C C C C  THIS  o n AUG  9. 1984 f o r CCid=DACO P a g e  DRPINT(PINT)  SUBROUTINE DRAWS A POINT  ' I G L ' GRAPHICS  'PINT' ON THE SCREEN AS A CROSS.  SUBROUTINES CALLED:  CMCLOS CMOPEN DRAW3D M0VE3D  c*********************************************************** c REAL  PINT(3)  C CALL CMOPEN P1=PINT(1)-1. P2=PINT(1)+1. CALL M 0 V E 3 D ( P 1 , P I N T ( 2 ) , P I N T ( 3 ) ) CALL D R A W 3 D ( P 2 . P I N T ( 2 ) . P I N T ( 3 ) ) P1=PINT(2)-1. P2=PINT(2)+1. CALL M 0 V E 3 D ( P I N T ( 1 ) , P 1 , P I N T ( 3 ) ) CALL DRAW3D(PINT(1),P2.PINT(3)) P1=PINT(3)-1. P2=PINT(3)+1• CALL M 0 V E 3 D ( P I N T ( 1 ) , P I N T ( 2 ) , P 1 ) CALL DRAW3D(PINT(1),PINT(2),P2) C A L L CMCLOS C RETURN END  1  - 289 -  Listing  o f DRVOL.FOR  1 2 3 4 5 6 7 8 9 10 1 1  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 38 39 40 41 42 43 44 45 46 47 48 49 50  a t 14:45:00  SUBROUTINE C C C C C C C C C C  c  THIS  o n AUG  9, 1984 f o r CC1d=DAC0 P a g e  1  DRVOL  SUBROUTINE DRAWS THE SWEPT VOLUME ON THE SCREEN.  COMMON BLOCKS ACCESSED:  /CORNER/  ' I G L ' GRAPHICS SUBROUTINES CALLED:  CMCLOS CMOPEN DRAW3D M0VE3D  «**«*.»****»«*.«*»*»»«*««*.**»»*«***«»*«** C COMMON  /CORNER/P1(3),P2(3),P3(3),P4(3),P5(3),P6(3).P7(3),P8(3)  C C C c***»* C  CALL  CMOPEN  DRAW  SWEPT  CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL CALL C C****» C  VOLUME.  M0VE3D(P1(1),P1(2),P1(3)) DRAW3D(P2(1).P2(2),P2(3)) DRAW3D(P3(1),P3(2),P3(3)) DRAW3D(P4(1),P4(2 ) , P 4 ( 3 ) ) DRAW3D(P1( 1 ) .P 1 (2 ) ,P 1 ( 3 ) ) DRAW3D(P5(1),P5(2),P5(3)) DRAW3D(P6(1),P6(2 ) , P 6 ( 3 ) ) DRAW3D(P7(1),P7(2) ,P7(3) ) DRAW3D(P8( 1 ) . P 8 ( 2 ) , P 8 ( 3 ) ) DRAW3D(P5(1).P5(2).P5(3)) M0VE3DCP2(1).P2(2),P2(3)) DRAW3D(P6(1 ) ,P6(2 ) ,P6(3 ) ) M 0 V E 3 D ( P 3 ( 1 ) . P 3 ( 2 ) .P3(3 ) ) DRAW3D(P7(1 ) ,P7(2 ) , P 7 ( 3 ) ) M0VE3D(P4(1 ) , P 4 ( 2 ) , P 4 ( 3 ) ) DRAW3D(P8(1 ) , P 8 ( 2 ) , P 8 ( 3 ) )  DRAW AXES ON SCREEN CALL M0VE3D(0.,0..0.) CALL DRAW3D(100.,0.,0.) C A L L M0VE30C0.,0..0.) C A L L DRAW3D(0.,100..0.) C A L L M0VE3D(O.,0..0.) CALL DRAW3D(0..0.,10O.)  C CALL  CMCLOS  C RETURN END  - 290 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56  o f ECROSS.FOR  a t 14:45:01 o n AUG  9,  1984 f o r CC1d=DAC0  Page  1  SUBROUTINE E C R O S S ( N V E R T . I C O U N T , U R , P I N T , L I S T E , N E C R S S ) C C C C C C C C C C C C C C C C C C C  T H I S SUBROUTINE COUNTS THE NUMBER OF TIMES A PROJECTED RAY CROSSES FACE BOUNDARIES WHOSE ENDPOINTS ARE NOT ON THE RAY. INPUT  ARGUMENTS: NVERT- INDEX OF F I R S T VERTEX OF FACE. ICOUNT- NUMBER OF V E R T I C E S ON FACE. UR- D I R E C T I O N VECTOR OF RAY. P I N T - ORIGIN OF RAY. L I S T E - L I S T OF EDGES WITH NEITHER VERTEX ON THE  OUTPUT  ARGUMENTS: NECRSS- NUMBER OF TIMES THE RAY EDGES.  RAY.  CROSSES FACE BOUNDARIES  COMMON BLOCKS ACCESSED: /DATA/ SUBROUTINES C A L L E D :  EDGINT  c REAL U R ( 3 ) , P I N T ( 3 ) , P 1 ( 3 ) , P 2 ( 3 ) INTEGER I E D G E ( 2 0 ) , L I S T E ( 1 0 )  c  COMMON / D A T A / V E R T ( 1 0 0 0 , 3 ) . F A C E S ( 1 0 O . 4 ) , I F A C E ( 1 0 0 , 2 ) , I O B J ( 1 0 , 2 )  c c c****» c c c****» c c c***»* c  NECRSS=0 T E S T A L L EDGES WHICH HAVE NEITHER VERTEX ON THE DO  15 1=1.100 FIND THE NEXT  EDGE.  INDEX=LISTE(I) IF(.INDEX.EO.O) GO TO 9 0 0 FIND THE EDGE  ENDPOINTS, 'P1','P2'.  DO  20  c c**»** c c  15  ,•  c  900 C  20 J=1,3 P1(J)=VERT(INDEX.J) P2(J)=VERT(INDEX+1,J) CONTINUE TEST  I F THE RAY  I N T E R S E C T S THE EDGE.  CALL E D G I N T ( P 1 , P 2 , U R , P I N T , I N T ) I F ( I N T . E O . I ) NECRSS=NECRSS+1 CONTINUE CONTINUE RETURN END  RAY.  AT  - 291 -  L i s t i n g of EDGCRS.FOR a t 14:45:01 on AUG 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 38 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  9, 1984 f o r CC1d=DAC0 Page  1  SUBROUTINE EDGCRS(UR,P1,P2,INT) THIS SUBROUTINE FINDS IF A LINE SEGMENT AND A RAY INTERSECT. THE LINE SEGMENT IS DEFINED BY ENDPOINTS P1(3),P2(3). THE RAY STARTING POINT IS THE ORIGIN, WITH DIRECTION GIVEN BY UR(3). LET UE(3)=P2(3)-P1(3) LINE SEGMENT: RAY: COMBINING:  R(3 ) =P 1 (3)+TLAMB1*UE(3). R(3)=TLAMB2*UR(3),  0.LE.TLAMB1.LE.1 0.LE.TLAMB2  TLAMB1*UE(3)-TLAMB2*UR(3)=-P1(3)  c********************************  c c c c  10  c c  c c***** c***»* c c c  c c c  REAL P1(3).P2(3) ,UR(3) ,M1(2),M2(2),UE(3),M3(2) INT=0 FIND THE LINE DIRECTION VECTOR 'UE'. DO 10 1=1,3 UE(I)=P2(I)-P1(I) CONTINUE MAKE SURE THAT THE LINE SEGMENT LENGTH IS GREATER THAN ZERO. UELEN=UE(1)**2+UE(2)**2+UE(3)**2 IF(UELEN.LE..00001) GO TO 500 THERE ARE THREE EQUATIONS AND TWO UNKNOWNS. WHICH GIVE A NON-SINGULAR 2X2 MATRIX. DO 20 1=1,3 J = I+1 IF(I.EQ.3) d=1 M1(1)=UE(I) M1(2)=UE(U) M2(1) = -UR(I ) M2(2)=-UR(d) CALL DET2X2(M1,M2,DET) IF(ABS(DET).GT..0OOO1) GO TO 100  20  CONTINUE  100  CONTINUE  c  c c*«*** c c c***»*  FIND TWO EQUATIONS  IF THERE IS NO NON-SINGULAR MATRIX, NO SOLUTION EXISTS. IF(ABS(DET).LE..00001) GO TO 500 SOLVE THE TWO EQUATIONS FOR THE PARAMETERS  'TLAMB1','TLAMB2'.  - 292  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77  o f EDGCRS.FOR  a t 14:45:01  -  o n AUG  9,  1984 f o r CC1d=DAC0 P a g e  2  C M3(1)=-P1(I) M3(2)=-P1(J) C C A L L DET2X2(M3,M2,TEMP 1) CALL DET2X2(M1.M3.TEMP2) C TLAMB1=TEMP1/DET TLAMB2=TEMP2/DET C  c****»  C****» C  TEST LINE  I F THE INTERSECTION POINT L I E S ON THE RAY SEGMENT.  IF(TLAMB1.LT.O..0R.TLAMB1.GT.1.) GO TO 500 I F ( T L A M B 2 . G T . O . ) INT=1 C 500  CONTINUE RETURN END  AND  ON THE  -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  of  EDGINT.FOR  at  293  -  14:45:19 o n AUG  9,  1984 f o r  CCid=DACO Page  1  SUBROUTINE E D G I N T ( P 1 , P 2 , U R , P I N T , I N T ) 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  T H I S SUBROUTINE INPUT  FINDS I F A LINE  SEGMENT  AND  A RAY  ARGUMENTS: P 1 ( 3 ) , P 2 ( 3 ) - END POINTS OF THE LINE U R ( 3 ) - D I R E C T I O N VECTOR OF THE RAY P I N T ( 3 ) - ORIGIN OF THE RAY  OUTPUT  INTERSECT.  SEGMENT  ARGUMENTS: INT- 0=NO I N T E R S E C T I O N 1=INTERSECTI0N  SUBROUTINES C A L L E D :  EQUATION OF LINE EQUATION OF RAY:  DET2X2  SEGMENT:  R=P1+TLAMB1*UE, R=PINT+TLAMB2*UR.  0.LE.TLAMB1.LE.1 TLAMB2.GEO  COMBINING: TLAMB1*UE-TLAMB2*UR=PINT-P1 WE GET THREE EQUATIONS IN TWO UNKNOWNS. SOLVE FOR PARAMETERS 'TLAMB 1 '.'TLAMB2' AND  TEST  THEIR VALUES.  REAL P1 ( 3 ) , P 2 ( 3 ) , U R O ) . P I N T O ) ,M1(2 ) ,M2(2) , U E ( 3 ) ,M3(2)  c c c***»* c 10  c c***** c****» c  INT=0 CALCULATE THE D I R E C T I O N VECTOR 10 1=1,3 UE(I)=P2(I)-P1(I) CONTINUE FIND TWO EQUATIONS OF THE THREE WHICH WILL GIVE SOLUTION.  c  c c****» c***»* c  20 1=1,3 d= I + 1 IF(I.EQ.3)  A  NON-SINGULAR  d=1  M1( 1 )=UE(I ) M1(2)=UE(J) M2(1) = -UR(I ) M2(2) = - U R ( J )  c  20  SEGMENT.  DO  DO  c  'UE' OF THE LINE  CALL  DET2X2(M1,M2,DET)  I F ( D E T . N E . 0 . ) GO CONTINUE IF  TO  100  NO NON-SINGULAR SOLUTION E X I S T S , THE LINE AND THERE IS NO INTERSECTION.  AND  RAY  ARE  PARALLEL  - 294 -  Listing  59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84  of  EDGINT.FOR  C 100 C C****» C***** C C C**"** C C C c***»* C C 500  at  14:45:19  o n AUG  9,  1984  for  CCid=DACO P a g e  2  GO TO 500 CONTINUE FIND THE CORRESPONDING VALUES ON THE RIGHT HAND SIDE OF THE TWO EQUATIONS. M3(1)=PINT(I)-P1(I) M3(2)=PINT(J)-P1(d) SOLVE FOR THE PARAMETERS  'TLAMB1','TLAMB2'.  CALL DET2X2(M3,M2.TEMPI) CALL DET2X2(M1,MS,TEMP2) TLAMB1=TEMP1/DET TLAMB2=TEMP2/DET TEST ' TLAMB1', 'TLAMB2' . IF(TLAMB1.LT O..0R.TLAMB1.GT.1.) GO TO 500 IF(TLAMB2.GT.0.) INT=1 CONTINUE RETURN END  i i  - 295  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  -  o f E L I S T . F O R a t 14:45:20 o n AUG SUBROUTINE C C C C C  c c c c c  9,  1984 f o r CCid=DACO  1  ELI ST(NVERT.ICOUNT.LISTV,LISTE)  T H I S SUBROUTINE GENERATES A L I S T OF EDGE STARTING V E R T I C E S OF EDGES FOR WHICH NEITHER VERTEX L I E S ON THE TEST RAY. INPUTS:  NVERT- LOCATION OF F I R S T VERTEX IN FACE VERTEX L I S T . ICOUNT- NUMBER OF FACE V E R T I C E S . L I S T V - L I S T OF FACE V E R T I C E S WHICH L I E ON THE TEST RAY.  OUTPUTS: L I S T E - L I S T OF FACE V E R T I C E S NOT ON TEST RAY.  t ) « ) > t l t > > > l < « t > > l l < > « < t > > l > t < . « . t t l t >  C  Page  t  t M « » > < t t l l l l  t  > > l « > t « > t » » > * > < >  c c c c c  ***«*  c c c  c c****« c  INTEGER  LISTV(10),LISTE(100)  INDEX=1 FIND  THE F I R S T VERTEX  IN L I S T V .  IND=LISTV(1)-NVERT IF  IND.LE.1,  THERE ARE NO EDGES BEFORE L I S T V ( I )  I F ( I N D . L E . 1 ) GO TO 150 ELSE  ENTER  EDGES  INTO  LISTE.  10  10 I=1,IND-1 LISTE(INDEX)=I+NVERT-1 INDEX=INDEX+1 CONTINUE  150  CONTINUE  c  c c***»* c***»* c  c c  30  c  20 C  DO  FIND THE V E R T I C E S BETWEEN THE ENTRIES EDGES INTO L I S T E .  IN L I S T V ,  DO 2 0 1=1,100 MV1=LISTV(I) MV2 = L I S T V ( 1 + 1 ) I F ( L I S T V ( 1 + 1 ) . E Q . O ) GO TO 2 0 0 IF(MV2.EO.NVERT ) MV2 = NVERT+ICOUNT-1 NC0UNT=MV2-MV1 DO 30 d=1.NCOUNT-2 LISTE(INDEX)=MV1+J INDEX=INDEX+1 CONTINUE CONTINUE  c***** c  FIND  20O C  CONTINUE  ANY EDGES AFTER  THE LAST VERTEX  LEFT=NVERT+IC0UNT-1-MV1 I F ( L E F T . L T . 2 ) GO TO 9 0 0  IN L I S T V  AND  ENTER THE  - 296 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75  o f E L I S T . F O R a t 14:45:20 o n AUG  9,  1984 f o r CC1d=DAC0 P a g e  2  C DO  40 C C***»* C 900  950 999 C  40 K=1,LEFT-1 LISTE(INDEX)=MV1+K INDEX=INDEX+1 CONTINUE  TEST  I F THE  LAST VERTEX  TO THE  F I R S T V E R T E X . I S A VALID  CONTINUE IF(LEFT.GT.O.AND.IND.NE.O) GO GO TO 999 CONTINUE LISTE(INDEX)=NVERT+IC0UNT-1 CONTINUE RETURN END  TO  950  EDGE.  - 297 -  L i s t i n g  o f  F A C E . F O R  1 2  a t  1 4 : 4 5 : 2 0  SUBROUTINE  o n A U G  9 .  1984 f o r CC1d=DAC0  Page  1  FACE  C  3  C  THIS  4  C  ARBITRARY  SUBROUTINE  ALLOWS  NUMBER  5  C  FACE.  6  C  STRUCTURES  T H EFACE  7  C COMMON  T H EU S E R  OF COPLANAR DATA  IS  T O INTERACTIVELY  VERTICES  STORED  IN  DEFINING  T H ECORRECT  A  C  9  C  1.0  C  11  C  12  C  13  C  14  C  15  C  DRAW3D  16  C  M0VE3D  FORMAT  BLOCKS  SUBROUTINES  ACCESSED:  C A L L E D :  XPROD ' I G L ' GRAPHICS  SUBROUTINES:  CMCLOS CMOPEN  C C Q*****«»»*»»**«*«»«*.«*»«»..«**««**«»***«**»«««»*»«»*»**»  20  C REAL  COMMON  25  C » « * * * C  27  USER  ENTERS  T H ENUMBER  F O R M A T ( $ , '  ENTER  R E A D ( 5 , * )  OF  VERTICES.  NUMBER  OF FACE  VERTICES  I F A C E ( 1 , 1 ) = 1  32  I 0 B d ( 1 , 1 ) = 1  33  I 0 B J ( 1 , 2 ) = 1 C  35  C * * * » *  36  C  37  USER DO  38  ENTERS  10  VERTEX  101  F O R M A T ( $ , ' R E A D ( 5 , » )  41  10  42  C  43  COORDINATES.  1 = 1 , I F A C E ( 1 , 2 )  W R I T E ( 5 , 1 0 1 )  40  I  ENTER  VERTEX('.12,')(X,Y,Z)  ( V E R T ( I , d )  CONTINUE DO 5 0  11=1.3  V E R T ( I F A C E ( 1, 2 ) + 1 , I I  45  V 1 ( I I )=VERT(  1. I I  )=VERT(1,11)  )  46  T 1 ( I I ) = V E R T ( 2 . 1 1 ) - V E R T ( 1 , I I )  47  T 2 ( I I ) = V E R T ( 3 , I I ) - V E R T ( 1 , I I )  48  5 0  49  C  50  c***»*  51  C  52  CONTINUE CALCULATE CALL DO 51  55  FACE  PLANE  11 =  51  57  C  58  C * * * * »  NORMAL.  1 , 3  F A C E S ( 1 , I I  56  SURFACE  X P R 0 D ( T 1 , T 2 . U )  C  54  ' )  . J = 1 , 3 )  44  53  ' )  I F A C E ( 1 , 2 )  C  31  39  / 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(10,2)  W R I T E ( 5 , 1 0 0 ) 1 0 0  29  3.4  , P 1 ( 3 ) , P I N T ( 3 ) , P 2 ( 3 ) . V 1 ( 3 )  C  26  30  T 1 ( 3 ) , T 2 ( 3 ) , U ( 3 )  C  23  28  T H E DATA  DOTPRD  18 ig  24  IN  / D A T A /  17  22  A N  POLYHEDRAL  'V E R T ' , ' F A C E S ' .  8  21  SPECIFY  ) = U ( I I ) / S 0 R T ( U ( 1 ) * * 2 + U ( 2 ) * * 2 + U ( 3 ) * * 2 )  CONTINUE CALCULATE  PLANE  DISTANCE  FROM  ORIGIN.  - 298 -  Listing 59 SO 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78  o f FACE.FOR  a t 14:45:20 o n AUG  9.  1984 f o r CCid=DAC0  Page  2  C CALL  D0TPRD(V1,U.FACES(1,4))  C CALL  CMOPEN  C NVERT=IFACE(1.2) C C****»  c  DRAW RESULTING CALL  FACE  POLYGON.  MOVE3D(VERT(NVERT,1),VERT(NVERT,2),VERT(NVERT.3))  c DO 30 C  30 1=1.NVERT CALL D R A W 3 D ( V E R T ( I , 1 ) , V E R T ( I . 2 ) , V E R T ( I , 3 ) ) CONTINUE CALL  CMCLOS  C C RETURN END  - 299 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f FACINT.FOR  a t 14:45:50 o n AUG  9, 1984 f o r CCid=DACO  Page  1  C RETURN END SUBROUTINE C C C C  c c c c c c c c c c c c c c c c***** c c c 201  c c c  c c***»* c c 900  c  c c c c c c c  FACINT(NFACE,P1,P2.INT)  T H I S SUBROUTINE T E S T S INPUT  ARGUMENTS: NFACE- INDEX P1(3),P2(3)-  OUTPUT  I F A L I N E SEGMENT  A FACE.  I D E N T I F Y I N G FACE LOCATION IN FACE END POINTS OF L I N E SEGMENT.  LIST.  ARGUMENTS: INT- 0=N0 INTERSECTION FOUND 1=INTERSECTI0N FOUND  SUBROUTINES CALLED:  REAL  INTERSECTS  DRPINT INSIDE PLANE  T1(3),T2(3),U(3),P1(3),PINT(3),P2(3),V1(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 F A C E ( 1 0 0 . 2 ) , I O B J ( 1 0 . 2 ) INT=0 TEST  I F L I N E SEGMENT  INTERSECTS  CALL  PLANE(NFACE.P1.P2,INT,PINT)  FACE  PLANE.  I F ( I N T . E O . O ) GO TO 9 0 0 I F ( I N T . E O . 1 ) GO TO 201 CONTINUE. TEST  I F INTERSECTION  POINT  CALL  INSIDE(NFACE.PINT,INT)  LIES  INSIDE THE FACE  BOUNDARIES.  I F ( I N T . E Q . O ) GO TO 9CO DRAW THE I N T E R S E C T I O N CALL  POINT  ON THE SCREEN.  DRPINT(PINT)  CONTINUE RETURN END SUBROUTINE  FACYL(NFACE,A,B,INT)  T H I S SUBROUTINE T E S T S A FACE. INPUTS:  I F EITHER  END OF A CYLINDER  NFACE- FACE I D E N T I F I E R . A.B- ENDPOINTS OF CYLINDER  SPINE.  INTERSECTS  - 300 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 1 10 1 1 1 112 1 13 1 14 1 15 1 16  o f FACINT.FOR C C C C C C C C  a t 14:45:50  OUTPUTS:  o n AUG  9. 1984 f o r CC1d=DAC0  I N T - 0=NO INTERSECTION,  SUBROUTINES C A L L E D :  Page  1=INTERSECTION,  DIFF DOTPRD DRPINT FACINT XPROD  C REAL A ( 3 ) , B ( 3 ) , E N D P T ( 2 , 3 ) , P I N T ( 3 ) REAL T E M P ( 3 ) , R K ( 3 ) , 0 ( 3 ) , R A D ( 3 ) , U ( 3 ) . P O I N T ( 3 ) C  c c c****» c  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 , COMMON /RADIUS/RADIUS INT=0 FIND  THE FACE  PLANE PARAMETERS  U(I),P  10 1=1.3 U(I)=FACES(NFACE.I) CONTINUE  DO 10  c  c c*»*** c  P=FACES(NFACE,4) SET UP AN ENDPOINTS ARRAY. 20 1=1,3 ENDPT(1,I)=A(I) ENDPT(2,I )=B(I ) CONTINUE  DO 20  c c c  CALL DO  DIFF(B.A.RK)  5 0 0 IND=1,2 30 1=1,3 TEMP(I)=ENDPT(IND , I ) CONTINUE  DO 30 C  C A L L DOTPRD(TEMP,U,DOT) .DIST=DOT-P IF(DIST.GT.RADIUS.OR.DIST.LT.0.)  GO TO 9 0 0  C CALL CALL  XPR0D2(U.RK,0) XPROD(0,RK,RAD)  C  c c  CALL DOTPRD(RAD,U,UDOT ) I F ( A B S ( U D O T ) .LT. .000001) GO TO 9 0 0 RADLEN=DIST/UDOT IF(RADLEN.GT.RADIUS)  40 1=1,3 POINT(I)=TEMP(I)+RADIUS*RAD(I ) CONTINUE  DO 40 C  GO TO 9 0 0  - 301 -  Listing 117 1 18 1 19 120 121 122 123 124 125 126  o f FACINT.FOR  a t 14:45:50 o n AUG  9,  1984 f o r CC1d=DAC0 Page  CALL F A C I N T ( N F A C E , T E M P , P O I N T , I N T , P I N T ) I F ( INT.EO.1) GO TO 9 0 0 C 500 C 900  CONTINUE CONTINUE I F ( I N T . E O . I ) CALL  C RETURN END  DRPINT(PINT)  3  - 302 -  Listing 1 2 3 4  o f FACYL.FOR  a t 14:45:52  SUBROUTINE  o n AUG  5  e  C C  INPUTS:  7  C  8  C  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 38  Page  1  FACYL(NFACE.A.B,INT)  THIS SUBROUTINE A FACE.  9  1984 f o r CC1d=DACO  C C C  10 11  9,  C  c c c c c c c  TESTS  IF EITHER  END OF  NFACE- FACE I D E N T I F I E R . A . B - E N D P O I N T S OF C Y L I N D E R  OUTPUTS:  I N T - 0=N0  SUBROUTINES  INTERSECTION.  CALLED:  A CYLINDER  INTERSECTS  SPINE. 1 = I N T E R S E C T ION.  DIFF DOTPRD DRPINT FACINT XPROD  c REAL REAL  A(3),B(3),ENDPT(2,3),PINT(3) TEMP(3),RK(3),0(3).RAD(3),U(3),P0INT(3)  c c c c***** c  COMMON  /DATA/VERT(1000,3).FACES(100.4),I FACE(100,2),IOBd(10,2)  COMMON  /RADIUS/RADIUS  INT=0 FIND  THE FACE  PLANE  PARAMETERS  DO 10  c  10 1=1,3 U(I)=FACES(NFACE,I ) CONTINUE  P=FACES(NFACE,4)  c c****» c  SET DO  UP AN  ENDPOINTS  2 0 1=1,3 ENDPT(1.I )=A(I )  ARRAY.  U(I),P  -  Listing  o f GENVOL.FOR  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 38 39 40 41. 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  -  a t 14:45:52 o n AUG  SUBROUTINE C C C C C C C C C  303  9, 1984 f o r CC1d=DAC0 P a g e  1  GENVOL(A1,B1,A2,B2)  T H I S SUBROUTINE  FINDS THE PARAMETERS WHICH DESCRIBE THE SWEPT VOLUME.  INPUT ARGUMENTS: A 1 ( 3 ) . B 1 ( 3 ) A2(3),B2(3)COMMON BLOCKS COMMON BLOCKS  ACCESSED: MODIFIED:  I N I T I A L CYLINDER SPINE ENDPOINTS. FINAL CYLINDER SPINE ENDPOINTS.  /LINK/,/RADIUS/ /VOLUME/,/SNORM/  C REAL A 1 ( 3 ) , B 1 ( 3 ) , A 2 ( 3 ) , B 2 ( 3 ) REAL A N ( 3 ) , B N ( 3 ) , C N ( 3 ) REAL LEN,RADIUS C COMMON COMMON COMMON COMMON C C***»* C C  10 C C C****» C  /V0LUME/P(3),U0(3),U1(3),V0(3),V1(3) /SN0RM/DAN(3),DBN(3),DCN(3) /LINK/H1(4,4),H2(4,4),LEN /RADIUS/RADIUS  C A L C U L A T E VECTORS  U0.U1.V0.VI  DO 10 1=1.3 U0(I)=A2(I)-A1(I) U1(I)=B2(I)-B1(I) V0(I)=B1(I)-A1(I) V 1 ( I ) = B 2 ( I ) -A2( I ) CONTINUE  C A L C U L A T E THE UNIT  SURFACE  NORMAL  AN(1)=U0(2)*V0(3)-U0(3)*V0( 2) AN(2)=-U0(1)*VO(3)+UO(3)*VO(1) AN(3)=U0(1)*VO(2)-UO(2)*VO(1) C •  BN(1)=U0(2)*(V1(3)-V0(3))-U0(3)*(V1(2)-V0(2)) BN(2) = - U 0 ( 1 ) * ( V 1 ( 3 ) - V 0 ( 3 ) ) + U 0 ( 3 ) * ( V 1 ( 1 ) - V 0 ( 1)) B N ( 3 ) = U 0 ( 1 ) * ( V 1 ( 2 ) - V O ( 2 ) ) - U 0 ( 2 ) * ( V 1 ( 1)-V0( 1 ) ) C CN(1 ) = V O ( 3 ) * ( V 1 ( 2 ) - V O ( 2 ) ) - V 0 ( 2 ) * ( V I ( 3 ) - V 0 ( 3 ) ) C N ( 2 ) = .-V0(3)*(V1 ( 1 )-V0( 1 ) )+V0( 1 )*( V 1 ( 3 ) -V0( 3 ) ) CN(3)=V0(2)«(V1(1)-V0(1))-V0(1)*(V1(2)-V0(2)) C C***** C***»* C  MULTIPLY THE UNIT SURFACE 'DAN'.'DBN','DCN'. DO 30 I=1.3 DAN(I)=RADIUS*AN(I) DBN(I)=RADIUS*BN(I) DCN(I)=RADIUS»CN(I) CONTINUE  30 C C ,  RETURN END  NORMAL BY THE RADIUS  TO GET  - 304 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46  o f INITSV.FOR  a t 14:46:10 o n AUG  SUBROUTINE  9, 1984 f o r CC1d=DAC0  Page  1  INITSV(FILNAM)  C  c****« c c c c c c c c c c c c c c c c c  T H I S SUBROUTINE  I N I T I A L I Z E S THE LINK  INPUT PARAMETERS:  COMMON BLOCKS  PARAMETERS.  FILNAM- NAME OF F I L E CONTAINING LINK DIMENSIONS AND I N I T I A L AND FINAL POSITIONS.  I N I T I A L I Z E D : /LINK/,/RADIUS/  SUBROUTINES C A L L E D :  OPEN SHUT  D E F I N I T I O N OF CONSTANTS  AND VARIABLES.  H1.H2-  HOMOGENEOUS TRANSFORMATIONS S P E C I F Y I N G FINAL POSITION OF THE LINK. LEN.RADIUS- LINK LENGTH AND RADIUS.  THE I N I T I A L AND  *»««,*»»»*«»«»*.»»«**««»«*«.*».»«*«»«***.«*«***«**«»««**«»«**«*«****«**  c c c c c  10  20  c c  BYTE REAL  FILNAM(20) LEN,RADIUS  COMMON COMMON READ  /LINK/H1(4,4),H2(4,4),LEN /RADIUS/RADIUS  'H1'.'H2'.'LEN'.'RADIUS'  CALL OPEN(1.FILNAM,4,1) READ(1 , * ) DO 10 1=1,4 R E A D ( 1 , « ) ( H 1 ( I . J ) ,J=1 ,4) CONTINUE READ( 1 .*) DO 20 1=1.4 READ(1,*) ( H 2 ( I , d ) . J = 1 . 4 ) CONTINUE READ( 1 ,* ) R E A D O . * ) LEN,RADIUS CALL  SHUTO)  RETURN END  FROM THE F I L E  'FILNAM'.  - 305 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51  of  INPROJ.FOR a t 14:46:11 SUBROUTINE  o n AUG  9,  1984  INPR0J(P1.P2,P3.P4,INT)  C REAL  P1(3),P2(3).P3(3),P4(3)  C INT=0 IC0UNT=O C CALL E0GCRS(P1.P2.P3.INT1) IF ( I N T 1 . E 0 . 1 ) IC0UNT=IC0UNT+1 CALL E0GCRS(P1,P4,P3,INT1) IF ( I N T 1 . E 0 . 1 ) IC0UNT=IC0UNT+1 C ICASE=1 C TOTAL 1=0. T0TAL2=O. C DO  10 C  c c 100  c 200  C 300  C 400 C  10 1=1,3 DIFF1=(P1(I)-P2(I))**2 DIFF2=(P1 (I ) - P 4 ( I ) ) * * 2 TOTAL1=T0TAL1+DIFF1 TOTAL2=TOTAL2+DIFF2 CONTINUE I F ( T 0 T A L 1 . L T . . 0 0 0 0 1 ) ICASE=2 I F ( T 0 T A L 2 . LT . .00001) ICASE = 3 GO  TO  (100,200,300)  ICASE  CONTINUE CALL VRTCRS(P1,P2,P4,INT2) IF ( I N T 2 . E 0 . 1 ) IC0UNT=IC0UNT+1 GO TO 4 0 0 CONTINUE CALL VRTCRS(P1.P3.P4,INT2) IF ( I N T 2 . E 0 . 1 ) IC0UNT=IC0UNT+1 GO TO 4 0 0 CONTINUE CALL VRTCRS(P1.P2.P3,INT2) IF ( I N T 2 . E Q . 1 ) ICOUNT*ICOUNT+1 CONTINUE ITEST=MOD(ICOUNT,2)  C IF  ( I T E S T . E Q . 1 ) INT=1  C RETURN END  f o r CC1d=DAC0 P a g e  1  -  Listing  o f INSIDE.FOR  1 3 4  9,  1984  f o r CC1d=DAC0 P a g e  1  INSIDE(NFACE.PINT,INT)  C C  THIS SUBROUTINE TESTS IF AN INTERSECTION POINT LIES INSIDE THE FACE 'NFACE'  c c c  INPUT ARGUMENTS: NFACE- INDEX IDENTIFYING FACE IN FACE LIST. PINT- INTERSECTION POINT LYING IN PLANE OF FACE.  e  r+ Is  y  Q  I*  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 38 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  6 7 8  -  a t 14:46:11 o n AUG  SUBROUTINE \*  306  p  c c c  'PINT'  OUTPUT ARGUMENTS: INT- 0='PINT' NOT ON FACE. 1='PINT' ON FACE. COMMON BLOCKS ACCESSED: /DATA/VERT.FACES.I FACE,IOBd SUBROUTINES CALLED:  ECROSS ELIST VCROSS  REAL P I N T O ) ,V(3) ,UR(3) INTEGER LISTV(10),LISTE(100) COMMON /DATA/VERT(1000.3),FACES(100,4),I F A C E ( 1 0 0 , 2 ) , I 0 B J ( 1 0 . 2 ) NCROSS=0 INT=0  c c* * * * * FIND STARTING LOCATION 'NVERT' AND NUMBER OF VERTICES 'NCOUNT' IN c*«*»*  c c  VERTEX LIST.  NVERT=IFACE(NFACE.1 ) NCOUNT*IFACE(NFACE,2)  c*»»** c*«***  DEFINE A RAY 'UR' WITH ORIGIN AT 'PINT' VERTEX ' V IN THE FACE VERTEX LIST.  10  DO 10 1=1.3 V(I)=VERT(NVERT,I) UR( I )=V(I)-PINT(I ) CONTINUE  c  c c****« c***»* c c  c* * * * *  c c c****« c c  FIND THE NUMBER OF TIMES THE RAY CROSSES FACE VERTICES.  AND CONTAINING THE FIRST  FACE BOUNDARIES AT  CALL VCROSS(NVERT.NCOUNT,UR.PINT,LISTV,NVCRSS) MAKE A LIST OF ALL THE FACE EDGES WHICH HAVE NEITHER VERTEX ON THE RAY. CALL ELIST(NVERT,NCOUNT.LISTV.LISTE) FIND THE NUMBER OF EDGES IN THE LIST CALL  'LISTE'  CROSSED BY THE RAY.  ECROSS(NVERT.NCOUNT,UR,PINT.LISTE,NECRSS)  - 307 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72  o f INSIDE.FOR a t 14:46:11 o n AUG  c***»*  9, 1984 f o r CC1d=DAC0  Page  2  THE TOTAL NUMBER OF BOUNDARY CROSSINGS IS THE SUM OF THE NUMBER O F CROSSINGS AT V E R T I C E S AND THE NUMBER OF CROSSINGS AT EDGES.  c NCROSS=NVCRSS+NECRSS  c c***»* c****» c c c  IF. THE NUMBER OF BOUNDARY CROSSINGS 'PINT' L I E S ON THE FACE. ITEST=M0D(NCR0SS,2) IF(ITEST.EO.1) RETURN END  INT=1  IS ODD. THE INTERSECTION POINT  - 308 -  Listing  o f INVOL.FOR  1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  a t 14:46:32  SUBROUTINE L.  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  o n AUG  9, 1984 f o r CC1d=DACO P a g e  1  INVOL(NFACE,INT)  T H I S SUBROUTINE CALCULATES WHETHER A GIVEN POINT L I E S INSIDE A PARAMETRIC SWEPT VOLUME. SINCE ANY POINT CAN BE USED, THE ROUTINE USES THE F I R S T VERTEX OF THE FACE 'NFACE'. INPUT  ARGUMENTS: NFACE-  OUTPUT ARGUMENTS:  THE SURFACE  TO FACE  I N T - INTERSECTION  SUBROUTINES C A L L E D :  LET  POINTER  LOCATION  IN FACE  LIST.  FLAG, 0=N0 INTERSECTION, 1=INTERSECTI0N.  NWTVOL VLEN  NORMAL BE N,  N=DAN+RLAMB(1)»DBN+RLAMB(2)*DCN THE  VOLUME  EQUATION  IS THEN  F(X)=RLAMB(1)*U0+RLAMB(2)*VO+RLAMB(3)*DAN+RLAMB(1)*RLAMB(2)*DELU+ RLAMB(1)*RLAMB(3)*DBN+RLAMB(2)*RLAMB(3)*DCN+G O.LE.RLAMB(1 ) .LE. 1 . 0.LE.RLAMB(2).LE . 1 . -RADIUS/LENGTH(N).LE.RLAMB(3).LE.RADIUS/LENGTH(N) WHERE DELU=U1-U0 G=P-VERT  c c  c c c  REAL D E L U ( 3 ) , R L A M B O ( 3 ) , R L A M B ( 3 ) , G ( 3 ) , N ( 3 ) COMMON COMMON COMMON COMMON  /DATA/VERT(10O0,3).FACES(IOO,4),IFACE(100.2),IOBJ(10.2) /V0LUME/P(3),U0(3),U1(3),V0(3).V1(3) /SNORM/DAN(3),DBN(3),DCN(3) /RADIUS/RADIUS  INT=0 CALCULATE  CONSTANTS,  INITIALIZE  F I R S T GUESS FOR SOLUTIONS  RLAMB(3).  DO  10  10 1=1,3 DELU(I)=U1(I)-U0(I) G(I )=P(I )-VERT(1,I) RLAMB(I)=.5 CONTINUE  c c***»* c***»* c  SOLVE THE NON-LINEAR SYSTEM FOR R L A M B ( 3 ) . THE ERROR L I M I T I S .01, AND THE MAXIMUM NUMBER OF ITERATIONS  c c*»*** c  CALCULATE  IS 10.  C A L L NWTVOL(UO,VO,DAN,DELU,DBN,DCN,G, .01,10,RLAMBO.RLAMB,I FLAG)  DO  SURFACE  NORMAL AT POINT  DEFINED  BY R L A M B ( 1 ) , R L A M B ( 2 ) .  20 1=1,3 N(I)=DAN(I)+RLAMB(1)*DBN(I)+RLAMB(2)*DCN(I)  -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78  of INVOL.FOR at 20 C  309 -  14:46:32 on AUG  9,  1984 f o r CC1d=DAC0 Page  2  CONTINUE CALL VLEN(N.RLEN)  C C*»*** C  SCALE RLAMB(3)  C C***«* C*"*** C  TEST THE PARAMETERS RLAMB(I) AGAINST LIMITS TO DETERMINE IF POINT LIES INSIDE VOLUME.  30 C 900 C  RLAMB(3)=RLAMB(3)*RLEN/(2.*RADIUS)+.5  DO 30 1=1.3 IF(RLAMB(I).LE.O.0R.RLAMB(I).GE.1.) CONTINUE INT=1 CONTINUE RETURN END  GO TO 900  - 310 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47  o f LINK.FOR a t 14:46:32 o n AUG SUBROUTINE C C C C C C C C C C C C C C  c c c c c c c c c  ****«  c  9, 1984 f o r CC1d=DAC0  10  c c*««»*  T H I S SUBROUTINE FINDS THE COORDINATES OF THE LINK AT THE BEGINNING AND END OF A MOTION. INPUT ARGUMENTS: A 1 ( 3 ) , B 1 ( 3 ) A2(3),B2(3)-  SPINE ENDPOINTS SPINE ENDPOINTS  SPINE  ENDPOINTS  IN I N I T I A L P O S I T I O N . IN FINAL P O S I T I O N .  COMMON BLOCKS ACCESSED: /LINK/,/RADIUS/ SUBROUTINES C A L L E D :  TRANS3  D E F I N I T I O N OF CONSTANTS  AND VARIABLES:  H1.H2- HOMOGENEOUS TRANSFORMATIONS FOR THE I N I T I A L A 1 . B 1 - SPINE ENDPOINTS IN I N I T I A L P O S I T I O N . A2.B2- SPINE ENDPOINTS IN FINAL P O S I T I O N .  AND FINAL POSITIONS  REAL A 1 ( 3 ) , A 2 ( 3 ) , B 1 ( 3 ) , B 2 ( 3 ) , T E M P ( 3 ) REAL LEN,RADIUS COMMON / L I N K / H 1 ( 4 , 4 ) , H 2 ( 4 , 4 ) . L E N COMMON /RADIUS/RADIUS DATA  TEMP/3*0./  POINTS FRAMES  'A1'.'A2' ARE AT THE ORIGINS OF THE HOMOGENEOUS 'H1','H2'.  10 1=1,3 A1(I)=H1(I,4) A2(I)=H2(I,4) CONTINUE TEMP(1)=LEN TRANSLATE  ALONG  X-AXIS TO FIND 'B1','B2'  C  c c  1  L I N K ( A 1,B1,A2,B2)  DO  c  Page  CALL CALL  TRANS3(H1,TEMP,B1) TRANS3(H2,TEMP,B2)  RETURN END  COORDINATE  -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21  of  MATDET.FOR  at  14:46:32  SUBROUTINE C C C C C C C C C C  311  -  on  AUG  9,  1984  for  CC1d=DAC0  MATDET(RMAT,DET)  T H I S SUBROUTINE F I N D S THE DETERMINANT OF THE FORM M A T R I X ( 3 . 3 ) INPUT  ARGUMENT: RMAT(3,3)-  OUTPUT  Page  3X3  OF A 3 X 3  MATRIX  MATRIX  ARGUMENT: D E T - DETERMINANT  OF  'RMAT'  C REAL  RMAT(3,3)  C DET=RMAT(1,1)*(RMAT(2,2)*RMAT(3,3)-RMAT(2.3)*RMAT(3.2))RMAT(1,2)*(RMAT(2,1)*RMAT(3,3)-RMAT(2,3)*RMAT(3,1))+ RMAT(1,3)*(RMAT(2,1)*RMAT(3,2)-RMAT(2,2)*RMAT(3,1)) C RETURN END  - 312 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f NEWTON.FOR a t 14:46:51 o n AUG SUBROUTINE C C C C  c c c c c c c c c c c c c c c c  9,  1984 f o r CC1d=DAC0  Page  1  NEWTON(A,B,C,0,E,TOL,NUM,XO,X,IFLAG)  T H I S SUBROUTINE USES NEWTONS METHOD FOR OF NON-LINEAR EQUATIONS OF THE FORM:  SOLVING A SYSTEM  F(X)=A*X(1 )+B*X(2)+C*X(3)+D*X(1)*X(2) + E INPUT  ARGUMENTS: A ( 3 ) , B ( 3 ) . C ( 3 ) , D ( 3 ) . E ( 3 ) - KNOWN C O E F F I C I E N T TOL- MAXIMUM ALLOWABLE ERROR. NUM- MAXIMUM NUMBER OF ITERATIONS. X 0 ( 3 ) - I N I T I A L GUESS FOR THE SOLUTION.  OUTPUT  VECTORS.  ARGUMENTS: X ( 3 ) - APPROXIMATE SOLUTION. IFLAG0=SOLUTION FOUND. 1=N0 SOLUTION. SINGULARITY E X I S T S .  SUBROUTINES  CALLED:  S0L3X3 VLEN  Q. » . » » » • * » . . * * « « . » * * « » « * » * * . » « » » » « » * » . . . « » * » « * » » * « « * * * » . * » » . « . « * . . * .  c c c c  1  c c* » * * * c c c 10  c c  11  c c  c c***»* c  REAL  *»««*••«*«  A(3).B(3),C(3),D(3).E(3),X0(3),X(3),FX(3).JAC(3.3).Y(3)  IFLAG=0 INITIALIZE  THE F I R S T  APPROXIMATION  TO  'X' WITH THE VALUE OF  DO  1 1=1.3 X(I)=XO(I) CONTINUE ITERATE  DO  THE SOLUTION A MAXIMUM  OF  'NUM'  TIMES.  150 ICOUNT=1.NUM  SOLVE  FOR  THE FUNCTION  'FX' FOR  DO  THE CURRENT  VALUE OF  'X'.  10 1=1,3 F X ( I ) = A ( I )*X(1 )+B(I ) * X ( 2 ) + C ( I ) * X ( 3 ) + D ( I ) * X ( 1 ) * X ( 2 ) + E ( I ) CONTINUE SOLVE THE JACOBIAN  MATRIX.  DO  11 1=1,3 J A C ( I , 1 )=A(I )+D(I ) * X ( 2 ) JAC(I,2 ) =B(I)+D(I)*X(1) JAC(I,3)=C(I) CONTINUE SOLVE  FOR  THE ERROR VECTOR  CALL  S0L3X3(JAC,FX,Y,ISOLN)  TEST  I F A SOLUTION WAS  I F ( I S O L N . E Q . 1 ) GO  TO  'Y'.  FOUND.  950  'XO'.  - 313 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86  of  NEWTON.FOR a t  14:46:51 o n  AUG  1984 f o r  9,  CC1d=DAC0 Page  2  C  c***»* c****» c 12 C c«*»*«  c c c***»« c c  150 C 950 C  ADD THE ERROR VECTOR ' Y' TO APPROXIMATION TO 'X'.  LAST VALUE  OF  'X' TO GET  THE  DO  12 1=1,3 X(I)=X(I)+Y(I) CONTINUE TEST  IF THE MAGNITUDE OF  CALL V L E N ( Y . Y L E N ) I F ( Y L E N . L T . T O L ) GO TEST  I F THE  TO  CONTINUE CONTINUE  CONTINUE RETURN END  THE  ERROR  IS LESS THAN  'TOL'.  900  5 0 L U T I 0 N IS DIVERGING.  CALL VLEN(X.XLEN) I F ( X L E N . G T . 1 0 0 0 . ) GO  IFLAG=1 C 900 C  THE  TO  950  INDICATING  SINGULARITY.  NEXT  - 314 -  L i s t i n g o f NWTVOL.FOR a t 1 4.  3 4 5 6 T  1 4 : 4 6 : 5 1 o n AUG  SUBROUTINE  c-  9,  1984 f o r CC1d=DACO Page  1  NWTVOL(A.B,C,D,E,F,G,TOL,NUM,XO,X,IFLAG)  T H I S SUBROUTINE USES NEWTONS METHOD FOR SOLVING A SYSTEM OF NON-LINEAR EQUATIONS OF THE FORM: F ( X ) = A * X ( 1 ) + B * X ( 2 ) + C * X ( 3 ) + D * X ( 1 ) * X ( 2 ) + E*X< 1 ) * X ( 3 ) + F*X(2)*X(3)+G  C C  c c  /  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 38 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  INPUTS: A ( 3 ) , B ( 3 ) , C ( 3 ) , D ( 3 ) , E ( 3 ) , F ( 3 ) . G ( 3 ) - CONSTANT VECTORS . TOL- MAXIMUM OESIRED ERROR. NUM- NUMBER OF ITERATIONS ALLOWED. XO- I N I T I A L GUESS FOR THE SOLUTION.  COEFFICIENT  OUTPUTS: X- SOLUTION VECTOR. I F L A G - 0-OK. 1-NO SOLUTION. SUBROUTINES C A L L E D :  c  REAL REAL  S0L3X3 VLEN  A(3),B(3),C(3),0(3),E(3).X0(3),X(3).FX(3),JAC(3,3),Y(3) F(3),G(3)  c IFLAG=0  c  INITIALIZE  c  THE F I R S T GUESS  FOR X ( I ) .  DO  1 1=1,3 X(I)=XO(I) CONTINUE  1  c  c***»*  c  ITERATE DO  150  A MAXIMUM  OF  'NUM'  TIMES.  IC0UNT=1,NUM  c C A L C U L A T E CURRENT  VALUE OF FUNCTION 'FX'.  c DO  * 10  c c»**** c  10 1=1,3 F X ( I ) = A ( I ) * X ( 1 ) + B ( I )*X(2)+C(I )*X(3)+D(I ) * X ( 1 ) * X ( 2 ) + E ( I ) * X ( 1 )*X(3) + F(I )*X(2)*X(3)+G(I ) CONTINUE CALCULATE  THE JACOBIAN MATRIX.  DO  11  11 1=1.3 JAC(I,1)=A(I)+D(I)*X(2)+E(I)*X(3) JAC(I,2)=B(I)+D(I)*X(1)+F(I)*X(3) J A C ( I , 3 ) = C ( I ) + E ( I )»X( 1 ) + F ( I ) * X ( 2 ) CONTINUE  c  c***»«  c  SOLVE FOR D I F F E R E N C E VECTOR CALL  S0L3X3(JAC,FX,Y,ISOLN)  c I F ( I S O L N . E Q . 1 ) GO TO 9 5 0  c c****»  X ( K ) = X ( K - 1 )+Y(K-1 )  'Y'.  - 315 -  sting 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83  o f NWTVOL.FOR  a t 14:46:51  o n AUG  9,  1984 f o r CC1d=DAC0 P a g e  C DO 12 C  c***»* c  12 1=1,3 X(I)=X(I)+Y(I) CONTINUE TEST  SIZE  OF ERROR.  CALL VLEN(Y.YLEN) I F ( Y L E N . L T . T O L ) GO TO 9 0 0  c c c 150 C 950 C  TEST  I F SOLUTION  CALL VLEN(X.XLEN) I F ( X L E N . G T . 1 0 0 0 . ) GO TO 9 5 0 CONTINUE CONTINUE IFLAG=1  C 900 C  I S DIVERGING  CONTINUE RETURN END  - 316 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48. 49 50 51 52 53 54 55 56 57 58  o f ONRAY.FOR a t 14:46:51 o n AUG  9,  1984 f o r CC1d=DAC0  Page  1  SUBROUTINE ONRAY(UR,PINT,V.ION) C C T H I S SUBROUTINE DETERMINES IF A POINT ' V IS ON RAY DEFINED BY C POINT 'PINT' AND D I R E C T I O N VECTOR 'UR'. C C INPUT ARGUMENTS: C U R ( 3 ) - D I R E C T I O N VECTOR OF RAY C P I N T ( 3 ) - ORIGIN OF RAY C V ( 3 ) - POINT TO BE TESTED C C OUTPUT ARGUMENTS: C ION- 0=NOT ON RAY C 1=0N RAY C C POINT ' V IS ON THE RAY I F : C C TLAMB*UR+PINT-V=0 C C»»*•***«*•***»**•*«»«*********«****************************».***«* C REAL U R ( 3 ) , P I N T ( 3 ) , V ( 3 ) , T L A M B ( 3 ) C I0N=O C K=0 C c***** T E S T EACH ONE OF THE THREE EQUATIONS. C DO 10 1=1,3 I F ( I . E Q . 3 ) J=1 C C***** I F U R ( I ) = 0 . TLAMB IS ARBITRARY. C I F ( U R ( I ) . E Q . O . ) GO TO 100 K=K+1 I F ( K . G T . 2 ) GO TO 20O C C****» C A L C U L A T E TLAMB FOR EQUATION I . C TLAMB(K)=(V(I)-PINT(I))/UR(I) I F ( K . E Q . 1 ) GO TO 9 0 C C**»** T E S T I F VALUE AGREES WITH OTHER CALCULATED VALUE. c***»* IF SO, ' V L I E S ON THE RAY. C I F ( A B S ( T L A M B ( K ) - T L A M B ( K - 1 ) ) . G T . . 0 0 0 0 1 ) GO TO 4 0 0 90 CONTINUE GO TO 10 C 100 CONTINUE C c***»* TEST IF V ( I ) - P I N T ( I ) = 0 C I F ( V ( I ) . N E . P I N T ( I ) ) GO TO 400 C 10 CONTINUE C 200 CONTINUE  - 317 -  Listing 59 60 61 62 63 64 65 66 67 68  o f ONRAY.FOR  a t 14:46:51  o n AUG  9.  C  £»****  c c  40O C  TEST  RANGE OF  TLAMB  IF(TLAMB(1).GT.O.) GO TO 4 0 0 CONTINUE RETURN END  I0N=1  1984 f o r CC1d=DAC0  Page  2  - 318 -  L i s t i n g o f PLANE.FOR a t 1  o  3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  r* c c c c c c c c c c c c c c c c c c c c  1 4 : 4 7 : 2 1 o n AUG  SUBROUTINE  9,  1984 f o r CC1d=DAC0 Page  1  PLANE(NFACE,P1,P2.INT,PINT)  \j  c c c c c c***** c**»** c 10  c c 15  c c****» c***»* c»****  c c c c c  T H I S SUBROUTINE WITH A PLANE INPUT  T E S T S FOR INTERSECTION  OF A LINE  SEGMENT  ARGUMENTS: N F A C E - INDEX LOCATING FACE IN FACE L I S T . P 1 ( 3 ) . P 2 ( 3 ) - END POINTS OF LINE SEGMENT.  OUTPUT  ARGUMENTS: I N T - 0=NO INTERSECTION OF PLANE. 1=PLANE INTERSECTED BY LINE P I N T ( 3 ) - INTERSECTION POINT.  SEGMENT.  COMMON BLOCKS ACCESSED: /DATA/VERT,FACES,IFACE,IOBd SUBROUTINES  CALLED:  EQUATION OF P L A N E : EQUATION OF L I N E :  REAL  DOTPRD R(3 ) » U ( 3 ) - P = 0 RLAMB*U1(3)+P1=R(3)  P1(3),P2(3),PINT(3),U(3).U1(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 F A C E ( 1 0 0 . 2 ) , I O B d ( 1 0 . 2 )  INT=0 FIND THE FACE PLANE SURFACE NORMAL ORIGIN ' P ' .  'U'  AND DISTANCE  FROM THE  DO  10 1=1.3 U(I ) =FACES(NFACE.I) CONTINUE P=FACES(NFACE,4) FIND THE D I R E C T I O N VECTOR  ' U 1 ' OF THE LINE  SEGMENT.  DO 15 1=1.3 U1(I)=P2(I)-P1(I) CONTINUE S U B S T I T U T E THE LINE ENDPOINTS INTO THE EQUATION OF THE P L A N E . IF THE RESULTS ARE OPPOSITE IN SIGN. THE POINTS ARE ON OPPOSITE S I D E S OF THE PLANE AND THE L I N E SEGMENT INTERSECTS THE PLANE. CALL CALL  D0TPRD(P1.U.P1U) D0TPR0(P2.U,P2U)  Z=(P1U-P)*(P2U-P) IF(Z.GE.O)  GO TO 8 0 0  FIND THE VALUE OF THE PARAMETER  'RLAMB'.  - 319 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71  o f PLANE.FOR  a t 14:47:21 o n AUG  9.  1984 f o r CC1d=DAC0 P a g e  RLAMB«(P-P1U)/(P2U-P1U) C c*»**»  c  FIND THE DO  20 C  20 1=1,3 PINT(I)=P1(I)+RLAM8*U1(I) CONTINUE INT=1  C 800  I N T E R S E C T I O N POINT  CONTINUE RETURN END  'PINT'  2  - 320 -  : l n g o f ROTK. FOR a t 14:47:21 on AUG 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 23 29 30 31 32 33 34 35 36 37 38 39  SUBROUTINE C C C C C C C C  9,  1984 f o r  CCid=DAC0 P a g e  1  ROTK(K,THETA.ROT)  T H I S SUBROUTINE C A L C U L A T E S THE ROTATION MATRIX FOR A ROTATION ABOUT A VECTOR 'K' BY AN ANGLE OF 'THETA'. INPUT ARGUMENTS: K ( 3 ) - ROTATION AXIS VECTOR THETA- ROTATION ANGLE (RAD) OUTPUT  ARGUMENTS: R 0 T ( 4 , 4 ) - RESULTING HOMOGENEOUS ROTATION  c c  MATRIX  »*«»,**»*«»*«**«*,*«**.**»***««*»*««.«**«««».***««****.«*«*»**»***«».**«  c  REAL R 0 T ( 4 , 4 ) , K ( 3 )  c c  c  c  c  RCOS=COS(THETA) RSIN=SIN(THETA) VERS=1.-RCOS XX=K(1 ) * K ( 1 ) * V E R S YY=K(2)*K(2)*VERS ZZ=K(3)*K(3)*VERS XY=K( 1 ) * K ( 2 ) * V E R S XZ=K( 1 ) * K ( 3 ) * V E R S YZ=K(2)*K(3)*VERS ROT(1, 1 ) = XX + RCOS ROT( 1 , 2 ) = X Y - K ( 3 ) * R S I N ROT(1,3)=XZ+K(2)*RSIN R0T(2,1)=XY+K(3)*RSIN R0T(2,2)=YY+RC0S R0T(2,3)=YZ-K(1)*RSIN R O T O , 1 )=XZ-K(2)*RSIN R0T(3,2)=YZ+K(1)*RSIN R O T O , 3)=ZZ+RC0S R0T(4,4)=1. RETURN END  i  - 321 -  Listing 1 2 3 4 5 6 7  a  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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f S0L3X3.F0R  a t 14:47:21 o n AUG  SUBROUTINE C C C C C C C  c c c c c c c c c  9.  1984 f o r CCid=DACO P a g e  1  S0L3X3(RMAT,CONST,Y,I SOL)  T H I S SUBROUTINE FINDS THE SOLUTION OF A SET OF LINEAR EQUATIONS OF THE FOLLOWING FORM: RMAT(3,3)*Y(3)+CONST(3)=0. INPUT ARGUMENTS: R M A T ( 3 , 3 ) - MATRIX OF C O E F F I C I E N T S . C 0 N S T ( 3 ) - VECTOR OF CONSTANTS. OUTPUT  ARGUMENTS: Y ( 3 ) - SOLUTION TO THE SYSTEM OF I S O L - 0=SOLUTION E X I S T S . 1=N0 SOLUTION E X I S T S .  SUBROUTINES C A L L E D :  EQUATIONS.  MATDET  C*****»**********************»***********************"  c c c c c c c***** c  REAL RMAT(3,3) .CONST ( 3 ) . Y ( 3 ) , T E M P O , 3 ) IS0L=O FIND THE DETERMINANT  CALL MATDET(RMAT,DET ) I F ( A B S ( D E T ) .LT. . O O O 0 1 ) GO SOLVE DO  c  CONTINUE  20  CALL MATDET(TEMP,DTEMP) Y(I)=DTEMP/DET  c  100  c 200  c  K1=K1-3 K2=K2-3  TEMP(J,I)=-CONST(J) TEMP(J,K1)=RMAT(J,K1) TEMP(d,K2)=RMAT(J.K2)  c  c  'Y ' .  K1=I+1 K2=I+2 IF(K1.GE.4) IF(K2.GE.4)  c  10  FOR  TO  10 1=1.3 DO 20 J=1.3  c  c  OF THE MATRIX  CONTINUE GO  TO 2 0 0  CONTINUE IS0L=1 CONTINUE RETURN  100  'RMAT' AND  CHECK  FOR SINGULARITY  - 322 -  o L i s t i n g of 59  S0L3X3.F0R a t END  1 4 : 4 7 : 2 1 on AUG  9,  1984 f o r  CC1d=DAC0 Page  - 323 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f SOLVE.FOR a t 14:47:22 o n AUG SUBROUTINE C C C C C C  9,  1984 f o r CCid=DACO P a g e  1  S0LVE(P1,01,U1,U2,U,DIST,LAMB 1,LAMB2)  T H I S SUBROUTINE SOLVES FOR THE PARAMETERS LAMB 1,LAMB2 D E F I N I N G THE COMMON NORMAL POINTS ON TWO L I N E S . LINE 1: R ( 3 ) = P 1 ( 3 ) + LAMB1*U1(3) LINE2: R(3)=Q1(3)+LAMB2*U2(3)  £******«*»*******»*»**»******** c REAL P 1 ( 3 ) , 0 2 ( 3 ) , U 1 ( 3 ) , U 2 ( 3 ) , U ( 3 ) , P I N T ( 3 ) , M 1 ( 2 ) , M 2 ( 2 ) , M 3 ( 2 ) REAL C ( 3 ) , 0 P ( 3 ) REAL LAMB 1,LAMB2  c IC0UNT=O  c c***»*  FIND TWO EQUATIONS WHICH GIVE THE DENOMINATOR  A NON-ZERO DETERMINANT  FOR  c DO  20 1=1.3 d=I+1 K = I+2 I F ( I . E 0 . 3 ) d=1 I F ( I . G T . 1 ) K=K-3  c M1(1)=U1(I) M1(2)=U1(J) M2(1)=U2(I) M2(2)=U2(d)  c c  CALL  DET2X2(M1.M2.DET)  ISAVE = I , . JSAVE=d KSAVE=K  c c 20  I F ( D E T . N E . 0 . ) GO TO CONTINUE  100  c 100  c c c.**»»* c  10  CONTINUE IC0UNT=IC0UNT+1 I F ( I C O U N T . G T . 2 ) GO TO 800 •  -CALCULATE THE VALUE OF THE RIGHT HAND SIDE OF THE CALL DIFF(Q1,P1,QP) DO 10 1=1.3 C(I)=QP(I)-DIST*U(I) CONTINUE  c  M3(1 ) = C ( I S A V E ) M3(2)=C(JSAVE)  c  Q* * * * *  c  SOLVE FOR THE UNKNOWNS LAMB 1,LAMB2 CALL CALL  DET2X2(M3.M2.TEMPI) DET2X2(M1.M3.TEMP2)  EQUATION  - 324 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79  o f SOLVE.FOR  a t 14:47:22 o n AUG  9,  1984 f o r CC1d=DAC0 P a g e  2  C LAMB1=TEMP1/DET LAMB2=-TEMP2/DET C C***»* C  T E S T I F T H I S IS A VALID  SOLUTION  TEST=LAMB1*U1(KSAVE)-LAMB2*U2(KSAVE) I F ( A B S ( T E S T - C ( K ) ) . L T . . 0 0 0 0 1 ) GO TO 900 C C***»* C  I F NOT.  CHANGE THE  SIGN OF THE VECTOR  'U' AND  SOLVE  AGAIN  DIST=-DIST GO TO 100 C 800 C 900 C  CONTINUE WRITE(5,*) CONTINUE RETURN END  'ERROR - NO  SOLUTION WAS  FOUND IN SUBROUTINE  SOLVE'  - 325 -  Listing 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 1S 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36  37  38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  of  SURFIN.FOR  a t 14:47:40 o n  AUG  9,  1984 f o r  CC1d=DAC0  Page  1  SUBROUTINE S U R F I N ( P 1 . P 2 . P 3 , P 4 , E 1 , E 2 . I N T ) 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 C C C  T H I S SUBROUTINE T E S T S FOR INTERSECTION BETWEEN A STRAIGHT EDGE DEFINED BY ENDPOINTS E1.E2, AND A PARAMETRIC SURFACE DEFINED BY STRAIGHT BOUNDARIES AND CORNER POINTS P1.P2.P3.P4'. INPUT  ARGUMENTS: P1(3).P2(3).P3(3).P4(3)E 1 ( 3 ) , E 2 ( 3 ) - ENDPOINTS  OUTPUT  CORNER POINTS OF A PARAMETRIC RULED SURFACE. OF A STRAIGHT EDGE.  ARGUMENTS: INT- 0=NO I N T E R S E C T I O N ^INTERSECTION  SUBROUTINES  CALLED:  ' I G L ' GRAPHICS  NEWTON  SUBROUTINES:  CMCLOS CMOPEN DRAW3D M0VE3D  EQUATION OF THE SURFACE: R(3)=P1(3)+RLAMB1*U0(3)+RLAMB2*V0(3)+RLAMB1«RLAMB2»(U1(3)-U0(3)) EQUATION COMBINING  OF THE WE  EDGE:  R ( 3 ) = E 1 ( 3 )+RLAMB3*UE(3)  GET:  RLAMB1*UO(3)+RLAMB2*VO(3)-RLAMB3*UE(3)+RLAMB1*RLAMB2*(U1(3)-UO(3)+ P1(3)-E1(3)=0 T H I S IS A NON-LINEAR NEWTONS METHOD.  SYSTEM OF  EQUATIONS  WHICH WE  WILL  SOLVE  USING  ^****#*****************************  C  REAL REAL REAL REAL  UO(3),VO(3).U1(3),UE(3).RLAMB(3) T1(3).T2(3).T3(3).T4(3).X0(3) P1(3),P2(3).P3(3).P4(3).V1(3).E1(3),E2(3) DELT(3),CONST(3)  DATA  XO/3*.5/  C C C INT=0 C C****» C  D E F I N E THE PARAMETRIC DO  SURFACE BOUNDARY VECTORS  10 1=1,3 U1(I)=P3(I)-P4(I) V1(I)=P3(I)-P2(I) U0(I)=P2(I)-P1(I) V0(I)=P4(I)-P1(I) UE(I)=E1(I)-E2(I) DELT(I)=U1(I)-U0(I) C0NST(I)=P1(I)-E1(I)  'VO','V1','UO','U1'.  -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99  3 2 6  -  o f SURFIN.FOR a t 14:47:40 o n AUG 10 C  c****» c»**»* c***»* c c c c*»**» c****» c  9,  1984 f o r CC1d=DAC0  Page  2  CONTINUE SOLVE FOR THE PARAMETER VALUES AT THE I N T E R S E C T I O N POINT USING NEWTONS METHOD FOR A SYSTEM OF NON-LINEAR EQUATIONS. THE MAXIMUM ERROR IS SET TO .01 AND THE MAXIMUM NUMBER OF ITERATIONS IS 10. CALL NEWT0N(UO.VO.UE.DELT.CONST, I F ( I F L A G . E Q . 1 ) GO TO 900  .01,10,XO,RLAMB,I FLAG)  T E S T THE VALUES OF THE PARAMETERS TO DETERMINE INTERSECTS THE SURFACE WITHIN I T S BOUNDARIES.  IF THE  DO 35 C  c** * * * c***»* c  60 C  35 1=1,3 IF(RLAMB(I).LT.(-0.001).OR.RLAMB(I).GT.(1.001)) CONTINUE  EDGE  DO 6 0 1=1.3 T1(I)=P1(I)+RLAMB(1)*U0(I) T2(I)=P1(I)+V0(I)+RLAMB(1)*U1(I) T3(I)=P1(I)+RLAMB(2)*V0(I) T4(I)=P1(I)+U0(I)+RLAMB(2)*V1(I) CONTINUE CALL  CMOPEN  CALL M 0 V E 3 D ( T 1 ( 1 ) , T 1 ( 2 ) , T 1 ( 3 ) ) CALL DRAW3D(T2(1),T2(2),T2(3)) CALL M 0 V E 3 0 ( T 3 ( 1 ) , T 3 ( 2 ) , T 3 ( 3 ) ) CALL DRAW3D(T4(1),T4(2).T4(3)) C CALL  CMCLOS  C  900  TO 900  IF THERE IS AN INTERSECTION, INDICATE THE INTERSECTION POINT GRAPHICALLY BY DRAWING TWO CROSSED L I N E S ON THE SURFACE AT THAT  C  c  GO  INT=1 CONTINUE RETURN END  0  POINT  - 327 i i  Listing  o f SWPVOL.FOR a t 14:47:40 o n AUG  1 at  3 4 5 6  •f7  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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  SUBROUTINE  9,  1984 f o r CC1d=DAC0 Page  1  SWPVOL  r> \*  c c c c  T H I S SUBROUTINE CYLINDER.  DEFINES  D E F I N I T I O N OF C O N S T A N T S  T H E VOLUME SWEPT OUT BY  AND  A  MOVING  VARIABLES:  r*  L.  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  A 1 , A 2 , B 1 , B 2 - CYLINDER SPINE ENDPOINTS H 1 . H 2 - HOMOGENEOUS C O O R D I N A T E T R A N S F O R M A T I O N S S P E C I F Y I N G THE I N I T I A L AND F I N A L P O S I T I O N OF THE C Y L I N D E R . L E N , R A D I U S - L E N G T H AND R A D I U S . O F THE C Y L I N D E R . P , U O , U 1 , V O . V 1 - C O E F F I C I E N T S OF P A R A M E T R I C S U R F A C E G E N E R A T E D BY THE SPINE. P 1 . P 2 . . . . P 8 - CORNER P O I N T S OF T H E SWEPT V O L U M E . SUBROUTINES  CALLED:  CORNER DRVOL GENVOL INITSV LINK  REAL REAL BYTE  LEN,RADIUS HW0RL(4,4) FILNAM(20),0BJLST(2O)  COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON COMMON  /LINK/H1(4,4),H2(4,4),LEN /OBJLST/OBJLST / 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 ) , IOBJ( 1 0 , 2 ) / V O L U M E / P O ) , U 0 ( 3 ) , U 1 ( 3 ) , V 0 ( 3 ) , V1 ( 3 ) /SNORM/DAN(3),DBN(3),DCN(3) /CORNER/P1(3),P2(3),P3(3).P4(3),P5(3).P6(3),P7(3).P8(3) /SPINES/A 1 ( 3 ) , B 1 ( 3 ) . A 2 ( 3 ) , B 2 ( 3 ) /VIEW/EYEPOSO) ,VRP(3) /RADIUS/RADIUS  DATA E Y E P O S / 1 . , 1 . , 1 . / DATA V R P / O . . 0 . , 0 . / DATA O B J L S T / ' 0 ' . ' B ' . ' J ' . ' L ' . ' S ' . ' T ' . ' . ' . ' D ' . ' A ' . ' T ' , 1 0 * 0 / DATA F I L N A M / ' F ' . ' I ' . ' L ' . ' N ' . ' A ' . ' M ' . ' . ' . ' D ' . ' A ' . ' T ' , 1 0 * 0 / DATA H W 0 R L / 1 6 * 0 . / HWORL(1,1 ) = 1 . HW0RL(2,2) = 1 . HW0RL(3,3) = 1 . HW0RL(4,4) = 1 . FIND CALL GET CALL  T H E SWEPT  VOLUME  PARAMETERS  INITSV(FILNAM) THE L I N K  PARAMETERS  LINK(A 1,B1,A2.B2)  AND S P I N E  ENDPOINTS.  - 328 -  Listing 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73  o f SWPVOL.FOR a t 14:47:40 o n AUG C C****» C  FIND THE PARAMETRIC  9,  1984 f o r CC1d=DAC0 P a g e  SURFACE  GENERATED  BY  2  THE CYLINDER S P I N E .  CALL GENV0L(A1.B1,A2,B2) C C****» C  FIND THE CORNER POINTS OF THE PARAMETRIC C A L L CORNER  C C****» C  DRAW THE  SWEPT VOLUME.  C A L L DRVOL C RETURN END  SWEPT VOLUME.  -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51  o f TCROSS.FOR  329 -  a t 14:47:41 o n AUG  SUBROUTINE  9, 1984 f o r CC1d=DAC0  Page  1  T C R O S S ( I V 1 , IV2 , I V 3 , U R , P I N T , I N T )  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  THIS SUBROUTINE DETERMINES I F A RAY CONTAINING AT LEAST ONE FACE VERTEX CROSSES THE FACE BOUNDARY AT THAT VERTEX. THIS IS DONE BY DETERMINING I F THE ADJACENT V E R T I C E S ON THE FACE L I E ON OPPOSITE S I D E S OF THE LINE CONTAINING THE RAY. INPUT  ARGUMENTS: I V 1 . I V 3 - ADJACENT VERTEX INDEXES. I V 2 - INDEX OF VERTEX CROSSED BY THE RAY. UR- D I R E C T I O N VECTOR OF THE RAY. P I N T - ORIGIN OF THE RAY.  OUTPUT  ARGUMENTS: INT- 0=NO FACE BOUNDARY CROSSED 1=FACE BOUNDARY CROSSEO  COMMON BLOCK ACCESSED: /DATA/VERT(1000,3).FACESC100,4).I FACE(100,2),IOBJ(10.2) SUBROUTINES C A L L E D :  DOTPRD XPROD  REAL PROD 1 ( 3 ) . P R O D 2 ( 3 ) REAL U R ( 3 ) . P I N T O ) ,U1(3) ,U2(3) COMMON  /DATA/VERT(1000.3),FACES(100.4),IFACE(100.2),IOBJ(10,2)  INT=0 FIND THE D I R E C T I O N VECTORS  'U1','U2' OF THE ADJACENT  EDGES.  DO  10  c c***** c c c**«** c c  10 1=1,3 U1(I)=VERT(IV1,I)-VERT(IV2,I) U2(I)=VERT(IV3,I )-VERT(IV2,I ) CONTINUE FIND THE CROSS  PRODUCTS  OF THE ADJACENT EDGES AND THE RAY.  CALL XPR0D(U1,UR,PR0D1 ) C A L L XPR0D(U2,UR,PR0D2) TEST  I F THE EDGES ARE ON OPPOSITE SIDES OF THE RAY.  CALL DOTPRD(PROD 1,PR0D2,DOT) I F ( D 0 T . L T . O . ) INT-1 RETURN END  - 330 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20  o f TRANS3.FOR  a t 14:48:02 o n AUG  SUBROUTINE C C C C C C C C C  9, 1984 f o r CC1d=DAC0  Page  1  TRANS3(H.P1.P2)  T H I S SUBROUTINE TRANSFORMATION.  PERFORMS THE  FOLLOWING HOMOGENEOUS  P2(3)=H(4,4)*P1(3) WHERE P1.P2 ARE 3X1 VECTORS AND H I S A 4X4 HOMOGENEOUS COORDINATE TRANSFORMATION MATRIX.  C REAL  H(4.4),P1(3),P2(3)  C DO 100 C  100 1=1,3 P2(I)=H(I,1)*P1(1)+H(I,2)*P1(2)+H(I, , 3)*P1(3)+H(1,4) CONTINUE RETURN END  - 331 -  Listing 1 2 3 4 5 6 7 8 g 10 11 12 13 14 15 16 17 18 19 20  o f UNIT.FOR a t 14:48:02  o n AUG  9.  1984 f o r CC1d=0AC0 P a g e  SUBROUTINE U N I T ( V E C T , U V E C T ) C C T H I S SUBROUTINE NORMALIZES A VECTOR. C C INPUTS: V E C T ( 3 ) - A VECTOR C C OUTPUTS: U V E C T ( 3 ) - VECT NORMALIZED. C Q»m** *****************************************  ***********<•****************** ***  c REAL V E C T ( 3 ) , U V E C T ( 3 ) , L E N G T H C LENGTH=5QRT(VECT(1)**2+VECT(2)**2+VECT(3)**2) C UVECT(1)=VECT(1)/LENGTH UVECT(2)=VECT(2)/LENGTH UVECT(3)=VECT(3)/LENGTH C  RETURN END  1  - 332 -  Listing 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 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58  o f VCROSS.FOR  a t 14:48:28 o n AUG  SUBROUTINE 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  9, 1984 f o r CCid=DACO Page  VCROSS(NVERT,ICOUNT,UR,PINT,I LI ST,NVCRSS)  T H I S SUBROUTINE FINDS THE NUMBER OF TIMES A RAY CROSSES THE POLYGONAL FACE BOUNDARY AT V E R T I C E S . INPUT  ARGUMENTS: NVERT- INDEX OF F I R S T FACE VERTEX. ICOUNT- NUMBER OF V E R T I C E S ON FACE. UR- D I R E C T I O N VECTOR OF THE RAY. P I N T - O R I G I N OF RAY.  OUTPUT  ARGUMENTS: I L I S T - L I S T OF V E R T I C E S ON RAY. NVCRSS- NUMBER OF TIMES THE RAY CROSSES FACE BOUNDARIES AT VERTICES.  COMMON BLOCKS ACCESSED:/DATA/ SUBROUTINES C A L L E D :  ONRAY TCROSS  REAL P I N T ( 3 ) , U R ( 3 ) , V ( 3 ) INTEGER I L I S T ( 1 0 ) . N V C R S S 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 J ( 10,2) NVCRSS=0 NUM=0 T E S T A L L V E R T I C E S ON THE FACE. DO  10 1=1,ICOUNT FIND THE NEXT VERTEX TO BE TESTED.  15  15 J=1,3 V(J)=VERT(NVERT+I-1,J) CONTINUE  Q*****  TEST  I F THE VERTEX  CALL  ONRAY(UR.PINT,V.ION)  c c  1  DO  I S ON THE RAY.  c c c***»* c c 10O 10  c c c*»«**  I F ( I O N . E Q . O ) GO TO 100 UPDATE  'NUM' AND  'ILIST'.  NUM=NUM+1 ILIST(NUM)=NVERT+I-1 CONTINUE CONTINUE  TEST ALL VERTICES ADJACENT V E R T I C E S  LYING ON RAY FOR FACE BOUNDARY CROSSINGS. ARE THE CLOSEST V E R T I C E S TO EITHER SIDE OF  - 333 -  Listing 59 60 61 62 69 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98  99  100 101 102 103 104 105 106 107 108 109 1 10 111 112 113 1 14 115 116  o f VCROSS.FOR C*»*** C  THE CURRENT * DO  C C****« C  a t 14:48:28 o n AUG  9,  1984 f o r CCld=DACO P a g e  VERTEX WHICH DO NOT  L I E ON THE  RAY.  2 0 1=1,NUM FINO CURRENT  VERTEX.  M=ILIST(I) C  c***»*  FIND ADJACENT  VERTICES.  C MV2=M+1 MV1=M-1 IF(1+1.GT.NUM) GO TO 6 0 0 GO TO 6 1 0 C  c****»  C 600  C 620 C 610  LAST VERTEX  IN L I S T .  CONTINUE IF(M.E0.NVERT+IC0UNT-1) GO TO 6 2 0 GO TO 310 MV2=NVERT I F ( I L I S T ( 1 ) . E O . N V E R T ) GO  TO 500  CONTINUE I F ( I L I S T ( 1 + 1 ).NE.M+1) GO TO 310  C MV2=M+2 GO TO 350 C 310  CONTINUE I F ( I LI ST(1-1).NE.M-1 ) GO  TO 350  C MV1=M-2 C 3E0 C  CONTINUE IF(MV2.GT.NVERT+IC0UNT-1) GO TO 500 I F ( M V 1 . L T . N V E R T ) GO TO 500  C  c****»  TEST  FOR CROSSING AT VERTEX.  C C A L L TCR0SS(MV1,M,MV2.UR.PINT,INT) I F U N T . E Q . 1 ) NVCRSS = NVCRSS+1 C C 500 20 C  CONTINUE CONTINUE I F ( I L I S T ( 1 ) . N E . N V E R T ) GO TO 510 I F ( I L I S T ( 2 ) . N E . N V E R T + 1 ) GO TO 511  C MV2=NVERT+2 MV1=NVERT+IC0UNT-1 GO TO 5 5 0 C 511  CONTINUE IF(ILIST.(NUM) . NE . NVERT + ICOUNT-1 ) GO  TO 512  - 334 -  Listing 1 17 1 18 119 120 121 122 123 124 125 12G 127 128 129 130 131 132 133 134 135 136 137  o f VCROSS.FOR  a t 14:48:28 o n AUG  9,  1984 f o r CCid=DACO P a g e  C MV2=NVERT+1 MV1=NVERT+IC0UNT-2 GO TO 5 5 0 C 512 C  CONTINUE MV2=NVERT+1 MV1=NVERT+IC0UNT-1  C 550 C  c***«* c c 510 C  CONTINUE TEST  FOR CROSSING AT VERTEX.  C A L L TCR0SS(MV1,M,MV2,UR,PINT,INT) I F ( I N T . E Q . I ) NVCRSS=NVCRSS+1 CONTINUE RETURN END  3  -  Listing 1 2 3 4 5 6 7 8 9 10  o f VLEN.FOR  a t 14:48:29 o n AUG SUBROUTINE  C C C  335 -  1984 f o r CCid=DACO  Page  1  VLEN(VECTOR,LENGTH)  T H I S SUBROUTINE REAL  9,  FINDS THE SCALAR  LENGTH OF A  VECTOR.  LENGTH,VECTOR(3)  C LENGTH=SORT(VECTOR(1)**2+VECTOR(2)**2+VECT0R(3)**2) C RETURN END  - 336  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30  o f VRTCRS.FOR  a t 14:48:29 o n AUG  SUBROUTINE C C C C C C C C C  -  9,  1984 f o r CC1d=DAC0  Page  VRTCRS(UR,P1,P2,INT)  j  T H I S SUBROUTINE DETERMINES I F A RAY CONTAINING AT LEAST ONE FACE VERTEX CROSSES THE FACE BOUNDARY AT THAT VERTEX. THIS IS DONE BY DETERMINING IF THE ADJACENT V E R T I C E S ON THE FACE L I E ON OPPOSITE SIDES OF THE LINE CONTAINING THE RAY. U R ( 3 ) - RAY D I R E C T I O N VECTOR. P 1 ( 3 ) . P 2 ( 3 ) - TWO ADJACENT V E R T I C E S .  C REAL  UR(3),P1(3),P2(3),U1(3),U2(3),PR0D1(3),PR0D2(3)  C INT=0 C DO  1C C  1  10 1=1,3 U1(I)=P1(I)-UR(I) U2(I)=P2(I )-UR(I) CONTINUE CALL CALL  XPR0D2(U1 ,UR,PRODI) XPR0D2(U2,UR,PR0D2)  C C A L L D0TPRD(PR0D1,PR002.RESULT) C IF(RESULT.LT.O.) C RETURN END  INT=1  - 337 -  Listing 1 2 3 4 5 6 7 8 9 10 1 1 12 13 14 15 16 17 18 19 20 21 22  o f XPR0D2.FOR  a t 14:50:37 o n AUG  SUBROUTINE C C C C C C C C C C  9,  c  T H I S SUBROUTINE  C A L C U L A T E S VECTOR  C = A X B (CROSS  INPUTS: A ( 3 ) = F I R S T VECTOR B ( 3 ) = S E C 0 N D VECTOR OUTPUT: C ( 3 ) = C R 0 S S PRODUCT, A X B [*********************************************  REAL A ( 3 ) , B ( 3 ) . C ( 3 ) . ABS C( 1 ) = A ( 2 ) * B ( 3 ) - A ( 3 ) * B ( 2 ) C(2)=A(3)*B(1)-A(1)*B(3) C(3)=A( 1 )*B(2)-A(2)*B( 1 ) RETURN END  Page  1  XPR0D2(A,B,C )  c c c  1984 f o r CC1d=DAC0  PRODUCT).  

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Country Views Downloads
United States 19 0
China 11 22
Japan 5 0
France 2 0
Russia 2 0
Sweden 1 0
Poland 1 0
India 1 0
United Kingdom 1 0
Ukraine 1 0
City Views Downloads
Unknown 10 1
Shenzhen 8 22
Ashburn 7 0
Mountain View 4 0
Beijing 3 0
Tokyo 3 0
Jackson 2 0
Saint Petersburg 1 0
Philadelphia 1 0
Federal Way 1 0
University Park 1 0
Atlanta 1 0
Redmond 1 0

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

Share

Embed

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

Comment

Related Items