THE AUTOMATIC OFF-LINE GENERATION OF WELDING ROBOT TRAJECTORIES WITH EMPHASIS ON KINEMATIC FEASIBILITY AND COLLISION DETECTION by RALPH 0. BUCHAL B . A . S c , U n i v e r s i t y of B r i t i s h Columbia, 1980 A THESIS SUBMITTED IN PARTIAL FULFILMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF APPLIED SCIENCE i n FACULTY OF GRADUATE STUDIES Department of Mechanical Engineering We accept th i s thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA September 1984 © Ralph 0. Buchal , 1984 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at THE UNIVERSITY OF BRITISH COLUMBIA, I agree that the Library shall make i t freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the Head of my Department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of Mechanical Engineering THE UNIVERSITY OF BRITISH COLUMBIA 2075 Wesbrook Place Vancouver, Canada V6T 1W5 Date: CPcT" , / q 84-i i ABSTRACT This thesis defines and discusses the problems involved i n automatic o f f - l i n e programming of a robot welding workstation with a minimum of human intervention, and proposes some solutions. The work i s motivated by the desire for faster and more powerful programming c a p a b i l i t i e s combined with reduced robot down-time for programming, r e s u l t i n g i n increased f l e x i b i l i t y , e f f i c i e n c y and pr o d u c t i v i t y . The system proposed i n the thesis uses data generated by commercial Computer Aided Design (CAD) systems. An "Expert Welder" software module selects the correct welder settings based on workpiece and workstation c h a r a c t e r i s t i c s . Provision i s made for the eventual incorporation of a real-time seam tracking system. The kinematics of a welding robot and workstation are considered i n d e t a i l . The welding torch p o s i t i o n and o r i e n t a t i o n r e l a t i v e to the robot are found from a series of known r e l a t i v e homogeneous coordinate transformations. Path f e a s i b i l i t y i s determined by the kinematic constraints of j o i n t l i m i t s and the work envelope, as well as by physical interference between the arm and other objects i n the workstation. The inverse kinematics so l u t i o n i s found and the solutions are checked against the j o i n t l i m i t s and work envelope for each desired robot p o s i t i o n . The s i x t h degree of freedom i s redundant for welding, and t h i s redundancy i s used to search for f e a s i b l e kinematic solutions. A s i m p l i f i e d interference detection algorithm i s proposed. The workstation i s modelled as a c o l l e c t i o n of s o l i d polyhedra. The robot l i n k s sweep out volumes of space which are approximated by volumes bounded i i i by parametric ruled surfaces. A number of simple tests can be made to determine whether any intersections exist between the swept volumes and the stationary polyhedra, i n d i c a t i n g i n t e r f e r a n c e . Much of th i s fundamental work i s incorporated i n a preliminary i n t e r a c t i v e programming software package c a l l e d AUTOP. The interference detection algorithm i s demonstrated by a program c a l l e d TESTIN. iv ACKNOWLEDGEMENT I would like to thank Professor Dale Cherchas for his enthusiastic help and guidance throughout my work. A large part of this thesis is based on work done for the Industrial Materials Research Institute, a division of the National Research Council under DSS Contract 12SD, 31155-2-5016. V TABLE OF CONTENTS Page ABSTRACT i i ACKNOWLEDGEMENT iv LIST OF TABLES x LIST OF FIGURES x i i NOMENCLATURE xiv LIST OF SYMBOLS xv 1. INTRODUCTION 1 2. PROBLEM DEFINITION 6 3. DESCRIPTION OF WORKSTATION 9 3.1 Description of Workpiece Using CAD 9 3.2.1 Geometrical Description 10 3.1.2 Weld Seam Description 10 3.2 Description of Robot 11 3.2.1 Link Geometry and Dimensions 11 3.3 Description of Welding Machine 14 3.4 Description of Positioning Table 15 4. AUTOMATIC WELDING PARAMETER SELECTION USING AN "EXPERT WELDER" SOFTWARE MODULE 16 5. KINEMATICS 17 5.1 Review of Homogeneous Transformations 17 5.1.1 Homogeneous Coordinate Transformation 17 5.1.2 Serial Combination of Homogeneous Transformations 19 5.1.3 Serial Combination of Homogeneous Frames 19 v i Page 5.2 Statement of Kinematics Problem 22 5.3 Intermediate Homogeneous Coordinate Frames 22 5.3.1 Table Frame Relative to Robot Base 22 5.3.2 Workpiece Frame Relative to Table 25 5.3.3 Weld Seam Frame Relative to Workpiece 25 5.3.4 Tool Frame Relative to Weld Seam 25 5.4 Calculation of the Tool Position and Orientation Relative to World Coordinates 28 6. PATH FEASIBILITY 29 6.1 Statement of Feasibility Problem 29 6.2 Kinematic Feasibility 29 6.2.1 Definition of Robot Joint Frames and Resulting Transformations 31 6.2.2 Inverse Kinematics Solution 32 6.2.2.1 General Approach to Inverse Kinematics Problem 32 6.2.2.2 PUMA 560 Solution 34 6.2.2.3 Discussion of PUMA Solutions 44 6.2.3 Comparison of Five Degree of Freedom and Six Degree of Freedom Robots 44 6.2.3.1 Solution for a Five Degree of Freedom Robot 44 6.2.3.2 ASEA IRB60 Solution 57 6.2.3.3 Redundancy of Sixth Joint for Welding . 59 v i i Page 6.2.4 Specification of Robot Coordinates 60 6.2.4.1 Calculation of PUMA Tool Location and Rotation Coordinates 62 6.3 Collision Avoidance 66 6.3.1 World Model 68 6.3.2 Development of Swept Volume 71 6.3.3 Interference Conditions 75 6.3.3.1 Condition 1 - Intersection of an Edge and a Parametric Surface 77 6.3.3.2 Condition 2 - Intersection of an Edge and a Cylinder 80 6.3.3.3 Condition 3 - Intersection of an Edge and a Polyhedral Face 83 6.3.3.4 Condition 4 - Intersection of a Cylinder and a Polyhedral Face 85 6.3.3.5 Condition 5 - Any Polyhedron is Totally Enclosed by a Swept Volume Bounded by Parametric Surfaces 88 6.4 Interference Detection Demonstration Program TESTIN ... 89 6.4.1 Functional Description of TESTIN 90 6.4.2 List and Description of a l l Subroutines used by TESTIN 90 6.4.3 Demonstration of TESTIN 96 6.5 Possible Improvements to the Algorithm 102 v i i i Page 7. SOFTWARE DESIGN AND DEVELOPMENT 103 7.1 Design Approach 103 7.2 Features and Capabilities of AUTOP 104 7.3 General Functional Description of AUTOP 105 7.3.1 List and Description of a l l Subroutines called from AUTOP 107 7.4 Data Structures 116 7.4.1 Description of a l l I/O Files and their Formats 117 7.5 Graphical Depiction of Solids Using Panels 132 7.6 User's Guide to Running Program AUTOP 135 7.7 Demonstration of AUTOP and Results 137 7.8 Future Capabilities 150 7.8.1 Real-time Seam Tracking 150 7.8.2 CAD System Interface 150 7.8.3 Generation of High-Level Descriptive Program .. 150 8. CONCLUSIONS 154 8.1 Analysis and Evaluation of Work Completed 154 8.2 Suggestions for Further Work 155 REFERENCES 157 ix Page APPENDIX I SOFTWARE DOCUMENTATION 160 1.1 Implementation Specific Software 161 1.2 AUTOP Program Listing 162 1.3 CAD File Generating Program CADGEN Listing 266 1.4 TESTIN Program Listing 270 X LIST OF TABLES Page 3.1 Manipulator Link Geometry Fil e Format 14 6.1 PUMA Link Parameters 36 6.2 Five Degree of Freedom PUMA Link Parameters 45 7.1 F i l e Name List FILIST 118 7.2 CAD Fil e List CADLST 119 7.3 Processed Seam File List WPCLST 119 7.4 Robot Location Data Fil e List ROBLST 119 7.5 Object Geometry Data Fil e List OBJLST 120 7.6 Seam CAD File 120 7.7 Processed Seam Data File 121 7.8 Robot Location Data Fil e 121 7.9 Object Geometry Data Files 122 7.10 System Configuration Fi l e WKSTAT 123 7.11 Example of File WKSTAT 124 7.12 Positioning Table Specification File TABLE 125 7.13 Example of File TABLE 126 7.14 Graphic System Initialization F i l e GRAPH 127 7.15 Example of GRAPH 127 7.16 Workstation Setup File for Current Seam LOCSET 128 7.17 Example of LOCSET for seams 5,1,3 128 7.18 Current Table Position Data STOVAR 129 7.19 Example of STOVAR for 3 Degree of Freedom Table 129 7.20 Temporary Weld Parameter Fil e WPARAM 130 7.21 Example of WPARAM for a Seam with 10 Locations 130 xi Page 7.22 Fortran File Formats 131 7.23 Subroutines Accessing Data Files 132 7.24 Contents of Seam CAD Fil e 142 7.25 Contents of Processed Seam Data File 143 7.26 Joint Angles Calculated during Kinematic Feasibility Search 144 7.27 Generated Robot Location Data 149 7.28 Task Precedence Table 153 x i i LIST OF FIGURES Page 1.1 Simplified Hardware Schematic for an Automatically Programmed Robot Welding Workstation 5 3.1 Link Geometry and Frames for a Revolute Joint 12 3.2 Link Geometry and Frames for a Prismatic Joint 13 5.1 Coordinate Transformation 18 5.2 Serial Combination of Homogeneous Transformations 20 5.3 Homogeneous Coordinate Frame Defined by n, o, a, p .... 21 5.4 Series of Homogeneous Coordinate Frames 23 5.5 Weld Frame Relative to Workpiece 26 5.6 Tool Frame Relative to Weld Seam 27 6.1 PUMA Joint Frames and Link Parameters 35 6.2 Relative Geometry of PUMA Link Frames F 3, F^, F 5, F 6 38 6.3 Tool Frame Relative to Robot Tool Mounting Flange 46 6.4 Rotation of Tool Frame about Tool Axis 61 6.5 Definition of o, a, t angles for the PUMA Arm 63 6.6 World Model 70 6.7 Cylindrical Approximation of a Manipulator Link 72 6.8 Parametric Approximation of Swept Volume 74 6.9 Interference Conditions 76 6.10 Algorithm for Interference Detection 78 6.11 Intersection of an Edge and a Parametric Surface 79 6.12 Intersection of an Edge and a Cylinder 81 6.13 Intersection of an Edge and a Polyhedral Face 84 x i i i Page 6.14 Intersection of Cylinder End Face and Polyhedral Face 86 6.15 Intersection Condition 1 between Face and Swept Volume 98 6.16 Intersection Condition 3 between Face and Swept Volume 99 6.17 Intersection Condition 2 between Face and Swept Volume 100 6.18 Intersection Condition 1 between Face and Swept Volume Generated by Pure Translation 101 7.1 Software Structure and Data Flow of AUTOP 106 7.2 Simplified Flowchart of AUTOP 108 7.3 Relationship of Two Lines to View Point 134 7.4 Graphical Depiction of the Workpiece by AUTOP 139 7.5 New Workpiece Position after Interactively Changing Table Position 140 7.6 Workpiece Shown from a Different View Point 141 7.7 Simplified High-Level Task Description 152 x i v NOMENCLATURE AUTOP: the automatic o f f - l i n e robot programming software package. CAD: Computer-Aided Design. IGL: Tektronix Interactive Graphics Library software package. PUMA: the Unimation PUMA 560 i n d u s t r i a l robot. TESTIN: an elementary interference t e s t i n g demonstration program. X V LIST OF SYMBOLS a_ t h i r d basis vector of a frame r e l a t i v e to the robot, a^ length of the common normal between the j o i n t i and jo i n t i - l axes of a robot. a-| the common normal between the jo i n t i and j o i n t i - l axes of a robot. d± the distance between l i n k s i and i - l of a robot. Fg the workpiece coordinate frame. F c the instantaneous weld seam coordinate frame. F G the too l or torch coordinate frame, th F^ the i robot l i n k coordinate frame. F R the robot base coordinate frame. F-p the positioning table frame. H , a homogeneous coordinate transformation from frame F to frame X X j X X F i - 1 . J_ the i t b j o i n t axis of a robot. 11 the f i r s t basis vector of a frame r e l a t i v e to the robot. o_ the second basis vector of a frame r e l a t i v e to the robot. P_ the pos i t i o n of a frame o r i g i n r e l a t i v e to the robot. the p o s i t i o n of the robot wrist center r e l a t i v e to the robot, r ^ a point l y i n g on a bounding parametric surface. r_p a point on a parent parametric surface. TJJ transformation matrix giving torch p o s i t i o n as a function of n joi n t v a r i a b l e s . x v i U Q one of the vectors defining the boundaries of a parametric surface patch. u_i one of the vectors defining the boundaries of a parametric surface patch. v 0 one of the vectors defining the boundaries of a parametric surface patch, x^ x-axis of a frame F^. y-i y-axis of a frame F^. z ± z-axis of a frame Fj_. a l o n g i t u d i n a l angular o f f s e t of torch from seam. ctji the twist angle of the i t h robot l i n k 3 l a t e r a l angular o f f s e t of torch from seam. 8^ the i t h jo i n t variable of a robot. X u a parameter of a parametric surface. ^ v a parameter of a parametric surface. \j a parameter of a parametric volume. P torch stick-out gap. - 1 -1. INTRODUCTION As technology progresses, robots are becoming an in c r e a s i n g l y important t o o l i n industry. Continual progress i n programming and con t r o l methods combined with the incorporation of sensory feedback promise to va s t l y increase the scope of applications for robots i n the factory. A great deal of fundamental research remains to be done, however, before the f u l l p o t e n t i a l of robots can be r e a l i z e d . Although robots have already had a tremendous impact i n manu-fact u r i n g , they are s t i l l very l i m i t e d i n t h e i r c a p a b i l i t i e s . Programming of a robot can be time consuming and d i f f i c u l t f o r tasks of even moderate complexity. T y p i c a l l y , a robot i s programmed by p h y s i c a l l y guiding i t through i t s motions under manual c o n t r o l . Positions are recorded along the way so that the motions can be repeated or played-back as often as -desired. The user has l i t t l e or no c a p a b i l i t y to edit or modify the re s u l t i n g program. The program i s completely developed using the robot to define l o c a t i o n s , and e x i s t i n g Computer-Aided Design (CAD) data i s not u t i l i z e d . The programming i s done on-line, removing the robot from production f o r possibly unreasonable lengths of time. These shortcomings are p a r t i c u l a r l y acute for small batch production where the robot must be reprogrammed frequently. Since most commercial robots have no sensory c a p a b i l i t i e s , they cannot adapt to random va r i a t i o n s i n the tasks they perform. This necessitates very s t r i c t s t ructuring and control of the workstation to insure a high degree of accuracy and r e p e a t a b i l i t y for the process. Process v a r i a t i o n s can occur due to va r i a t i o n s i n workpiece positioning, - 2 -and variations between different samples of the workpiece. The require-ment for exact repeatability could be greatly relaxed i f the robot could adapt to process variations using real-time sensory feedback. We can formulate two main objectives for the improvement of robot performance. Fi r s t , we would like to simplify and speed-up the robot programming process, and perform as much of the programming as possible of f - l i n e . The method must be easy to use, fast and flexible. Second, we must incorporate sensory feedback and real-time correction of process variations. This thesis considers the f i r s t problem only. The problem of off-line programming has been studied by a number of investigators, but a commercially feasible system has not yet been demonstrated. Two basic approaches have so far been demonstrated for generation of a robot program, interactive simulation and explicit task description using high-level languages. In the f i r s t approach, simulation of the process is done using interactive graphics. The robot and workstation are depicted graphically, and the operator interactively generates a program which Is simulated on a graphics display. It is d i f f i c u l t to specify locations exactly with this method, and there may be problems visualizing the scene. The demonstrations described in the literature [Sata, Fumihiko and Akio 1981; Ambler, Popplestone and Kempf 1982; Sjolund and Donath 1983] provide only limited software tools to aid the user. A second and more promising approach to off-line programming is offered by high-level robot languages. These languages allow the user to ful l y describe the robot task in a descriptive, natural way. Locations can be specified explicitly, or can be referred to a mathematical world - 3 -model. Many languages have been devised by robotics researchers, but off-line programming has been discussed more than demonstrated. Ambler, Popplestone and Kempf [1982] generated an explicit program in the VAL language for a PUMA robot, including a l l of the location data, using the high-level language RAPT to describe a task. Their experience showed a need for graphical simulation of the program for verification and debugging. Other existing high-level robot languages are summarized in a number of recent papers [Gruver et al 1983; Kempf 1982; Tarvin 1980]. The off-line programming problem encompasses a number of d i f f i c u l t sub-problems. Two problems which are considered in detail in this thesis concern kinematic fe a s i b i l i t y and co l l i s i o n detection. The kinematic f e a s i b i l i t y of a proposed robot trajectory must be automatically checked to ensure that a valid set of joint angle solutions exists. A robot position is kinematically feasible i f an inverse kinematic solution exists for the joint angles, and i f a l l the joint angles are within their physical limits. We have found by experience that even when programming a robot on-line i t is d i f f i c u l t to avoid paths which lead to a joint angle limit being exceeded, particularly when moving in straight lines. To verify kinematic f e a s i b i l i t y , we must solve the Inverse kinematic problems to find the joint angles at each point along a proposed trajectory. The inverse kinematic problem has been well studied by many investigators (see Section 6.2 for a review). An explicit solution for most real robots can be derived by matrix algebra methods described by Paul [1981]. In order to increase the chance of finding a feasible solution for a given trajectory we w i l l exploit the redundancy of the - 4 -s i x t h degree of freedom i n welding to search for a f e a s i b l e set of j o i n t solutions at each point on the t r a j e c t o r y . The c o l l i s i o n detection problem i s one part of the far more d i f f i c u l t problem of path planning to avoid obstacles. Many algorithms have been proposed for finding intersections between stationary geometric s o l i d s and between stationary and moving s o l i d s . Previous work i n geometric model-l i n g and interference detection i s reviewed i n Section 6.3. The more d i f f i c u l t path planning problem has not yet been solved i n a s a t i s f a c t o r y way, although some promising strategies have been proposed for simple non-redundant systems. No work has yet emerged i n the l i t e r a t u r e dealing with path planning for systems with redundancy. This thesis discusses the problems associated with developing an integrated o f f - l i n e welding robot programming system, and offers some solu t i o n s . The solutions are incorporated into a f u n c t i o n a l software package c a l l e d AUTOP. P a r t i c u l a r emphasis i s placed on the kinematic f e a s i b i l i t y problem, and on elementary methods for c o l l i s i o n detection. Many other v i t a l issues are beyond the scope of t h i s thesis and are discussed only b r i e f l y . F i g . 1.1 i l l u s t r a t e s the required hardware structure for an automatically programmed welding workstation. - 5 -Interactive Terminal Welding Data Base Welding Machine Controller I Robot Controller I P o s i t i o n i n g Table Co n t r o l l e r T Welding Machine Robot Positioning Table \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ Figure 1.1 S i m p l i f i e d Hardware Schematic for an Automatically Programmed Robot Welding Workstation. - 6 -2. PROBLEM DEFINITION The ultimate objective of the work described i n this thesis i s the development of a system for automatically programming a welding robot using data derived from CAD f i l e s . An expert welder data base w i l l provide the correct welding parameter values, and a real-time seam tracking system w i l l correct for real-world v a r i a t i o n s . The structure of the r e s u l t i n g program w i l l be h i e r a r c h i c a l , with the highest l e v e l c o n t r o l l i n g the entire workstation as an integrated u n i t . We can i s o l a t e a number of sub-problems which must be solved before an automatic programming system can be r e a l i z e d : 1. Choice of World Model and Description We must choose a way to represent the workpiece e x p l i c i t l y , without ambiguity. Important information includes seam l o c a t i o n data, workpiece geometry and welding parameters. We must also represent the workstation, including the robot, positioning table and welding machine. 2. Generation of a Welding Procedure An acceptable and f e a s i b l e welding procedure must be generated. The procedure includes the order i n which seams are welded, workpiece repositioning, inter-weld paths, etc. 3. Automatic Generation of Welding Parameter Values Welding output parameters such as current and speed can be - 7 -determined automatically i f a l l relevant workpiece and welding machine c h a r a c t e r i s t i c s are known. 4. Generation of Welding Torch T r a j e c t o r i e s We must generate torch t r a j e c t o r y data including torch position and o r i e n t a t i o n along each seam i n terms of robot coordinates. 5. Path F e a s i b i l i t y Testing The calculated torch t r a j e c t o r y may not be f e a s i b l e due to robot work envelope and j o i n t l i m i t constraints. Also, interference may occur between the robot and other objects In the workstation. 6. Generation of High-Level Program to Direct and Coordinate the Welding Process We need a d e s c r i p t i v e , h i g h - l e v e l method for c o n t r o l l i n g and coordinating the d i f f e r e n t devices i n the workstation. Welding requires a c a r e f u l l y controlled i n t e r a c t i o n between the robot, positioning table and welding machine. 7. Program Modification i n Response to Sensory Feedback Due to real-world v a r i a t i o n s and uncertainties i n positioning and welding, a seam tracking system based on real-time sensory feedback i s an important c a p a b i l i t y . 8. Hardware and Software Interfaces Hardware and software interfaces must be designed between the - 8 -various components i n the workstation. A host computer must be able to communicate with the robot c o n t r o l l e r , positioning table c o n t r o l l e r and welding machine. These problems w i l l be discussed and some solutions w i l l be proposed. - 9 -3. DESCRIPTION OF WORKSTATION We must describe a l l the important c h a r a c t e r i s t i c s of the workstation i n a concise way. The workstation i s composed of the workpiece, pos i t i o n i n g table, welding machine, robot and any other peripheral equipment. 3.1 Description of Workpiece Using CAD A complete des c r i p t i o n of the workpiece i s generated by an external CAD system. The CAD des c r i p t i o n must contain the following information: 1. Geometry a) dimensions and shape of object b) weld seam t r a j e c t o r i e s and surface normals adjacent to seams i n d i g i t i z e d form. 2. Welding Information a) material b) thickness c) weld j o i n t types d) weld cross s e c t i o n a l area. The CAD f i l e must be translated from the CAD output format into the s p e c i f i e d input format for the software system, which we w i l l c a l l AUTOP. This t r a n s l a t i o n i s required so that any external CAD system may be used. Details of the CAD system need not be known at t h i s point, as long as the sp e c i f i e d information i s generated i n some form. - 10 -3.1.1 Geometrical Description We wish to r e p r e s e n t the geometry of the workpiece by an approximating polyhedron, whose v e r t i c e s , edges and faces are stored i n a h i e r a r c h i c a l , f i l e - b a s e d data structure. At the top of the hierarchy i s a l i s t of the object faces, including t h e i r surface normals, distance from o r i g i n and number of v e r t i c e s . Each face record contains the coordinates of the face v e r t i c e s i n any sequential order around the face. Each vertex of the polyhedron i s shared by at least three faces. A polyhedral representation i s well suited to graphical depiction and interference detection. 3.1.2 Weld Seam Description Seam data i s also stored i n a h i e r a r c h i c a l , f i l e - b a s e d structure. A l i s t of seam i d e n t i f i e r s i s stored i n a f i l e . Each entry points to a corresponding seam data f i l e . The head of each seam data f i l e contains welding data relevant to that seam, including material thicknesses, seam types and weld cross s e c t i o n a l area. The rest of the f i l e contains d i g i t i z e d coordinates of the seam t r a j e c t o r y and adjacent surface normals r e l a t i v e to a reference frame fixed on the workpiece. Each seam defined i n the CAD f i l e i s treated as a sin g l e , i n d i v i s i b l e e n t i t y by AUTOP. Long seams should be broken down into shorter segments which can be treated separately. For example, a c i r c u l a r weld around the circumference of a cylinder should be divided into two or four segments so that the workpiece can be repositioned before welding each segment. - 11 -3.2 Description of Robot AUTOP requires a careful and complete s p e c i f i c a t i o n of the robot i n use. Data which are required are: 1. Number of degrees of freedom. 2. Complete l i n k geometry and dimensions 3. Joint l i m i t s . 4. Tool geometry r e l a t i v e to tool mounting flange. 5. Default robot configuration. 6. Position of robot base r e l a t i v e to the world coordinate frame, stored as a homogeneous coordinate transformation. 3.2.1 Link Geometry and Dimensions We w i l l follow the conventions of Denavit and Hartenberg [1955] i n describing l i n k geometry and dimensions. We can define an n degree of freedom robot as a series of n r i g i d l i n k s connected by n joints (Figs. 3.1, 3.2). The i ^ l i n k i s characterized by a length a^ and a twist angle . a^ i s the length of the common normal a.^ between the two joint axes, and ct^ i s the angle between the axes i n a plane perpendicular to _a . The r e l a t i v e displacement between a, , and a^ at the joint i axis i s —1-1 — i d^, the distance between the l i n k s . 6^ i s the angular displacement of a.^ and about the joint i axis and i s called the angle between the l i n k s . Parameters a^ and ct^ are f i x e d f o r a given l i n k . For a prismatic j o i n t where motion i s along the j o i n t a x i s , d^ varies and 8^ i s fixe d . For a revolute j o i n t , d^ i s fixed and 9^ varies. Given these parameters, we can completely define the kinematics of Figure 3.1 Link geometry and frames for a Revolute J o i n t . - 13 -Joint n Figure 3.2 Link geometry and frames for a Prismatic Joint - 14 -the robot. The parameters are stored i n a f i l e with the format of Table 3.1. Table 3.1 - Manipulator Link Geometry F i l e Format l i n k # i d a a d 9 1 2 n i d = 1 revolute i d = 2 prismatic 3.3 Description of Welding Machine The welder c h a r a c t e r i s t i c s and s p e c i f i c a t i o n s are stored i n a data f i l e where they are e a s i l y referenced by the Expert Welder module. The f i l e contains a l l fixed settings and c h a r a c t e r i s t i c s of the welder which are relevant to the welding process. The f i l e might include some or a l l of the following: a) voltage setting b) wire diameter c) wire type d) wire feed rate - 15 -e) gas type, flow rate f ) p o l a r i t y g) minimum, maximum current ( i f under program c o n t r o l ) , or current s e t t i n g . The contents of the f i l e depend on the p a r t i c u l a r welder i n use. The format has not yet been completely s p e c i f i e d . 3.4 Description of Posi t i o n i n g Table AUTOP requires a complete d e s c r i p t i o n of the positioning table, which i s stored In a data f i l e . The following information i s stored: a) number of degrees of freedom b) l i n k geometry and dimensions c) j o i n t types (revolute or prismatic) d) j o i n t motion (continuous or discre t e steps) e) j o i n t l i m i t s f) f i r s t j o i n t l o c a t i o n r e l a t i v e to world coordinates g) table coordinate frame l o c a t i o n r e l a t i v e to l a s t j o i n t frame. The table i s treated as analogous to an n degree of freedom manipulator, so the l i n k geometry and dimension parameters have the same d e f i n i t i o n s as for a robot. We can e a s i l y accommodate a t o t a l l y d i f f e r e n t type of positioning table simply by r e v i s i n g the entries i n t h i s data f i l e . - 16 -4. AUTOMATIC WELDING PARAMETER SELECTION USING AN "EXPERT WELDER" SOFTWARE MODULE A welding data base and interactive software module have been developed by Dr. F. Sassani of the Department of Mechanical Engineering, The University of British Columbia. A detailed description of this system, henceforth referred to as the Expert Welder, can be found in a previous report [Buchal et al 1984]. Essentially, the Expert Welder module uses a data base derived from standard welding practice to choose appropriate welding parameter settings as a function of given workpiece and fixed welder parameters. - 17 -5. KINEMATICS 5.1 Review of Homogeneous Transformations 5.1.1 Homogeneous Coordinate Transformation It is often useful to transform the coordinates of a point in space from one coordinate frame to another (Fig. 5.1). A rotational transforma-tion i s required i f corresponding axes of two coordinate frames F^ ^, F^ are not parallel. A translational transformation is required i f the frame origins are not coincident. In general, both a rotation and a translation are required. A homogeneous coordinate transformation allows us to perform both rotation and translation in a single operation using a 4x4 homogeneous transformation matrix. For example, a vector _r expressed relative to and projected onto a coordinate frame Fj_ can be transformed into a vector R r e l a t i v e to and projected onto a frame F^_^ by the following matrix multiplication: where H . i s a 4 X4 homogeneous transformation matrix relating frame I I»I F^ to Frame The vectors R and r_ are homogeneous 4x1 column vectors, where the f i r s t three elements are the cartesian coordinates and the last element is a scaling factor. For our purposes this scaling factor is always set to 1. H. . . has the following form: -1-1, i - 18 -i - l Figure 5.1 Coordinate Transformation. - 19 -rotation translation 3 x 3 P X rotation matrix P y p z 0 0 o 1 (5.2) A more detailed description of homogeneous transformations can be found in other references [Paul 1981]. 5.1.2 Serial Combination of Homogeneous Transformations We can combine homogeneous transforms in series as shown in Fig. 5.2. Suppose we want to find R given _r. If we know H^ 2 and H^ ^ we can find H „ as the product of H, and H : 5-1,3 = -1,2 3-2,3 * ( 5 ' 3 ) Thus, R = 3 jr . (5.4) 5.1.3 Serial Combination of Homogeneous Frames We can show that the homogeneous transformation H^ ^ ^ defines a coordinate frame F. relative to F, , . I i - l EJ , . can be written as - l - l . i i = <5-5> where ti, o_, a_y £ are 4 ^ 1 column vectors. ii, o_, ji define the basis vectors of F^ relative to F^_^,and P_ is the origin of F^ relative to F^ ^ , as shown in Fig.5.3. - 20 -Figure 5.2 S e r i a l Combination of Homogeneous Transformations. - 21 -Figure 5.3 Homogeneous Coordinate Frame defined by n, o_, a_, g_. - 22 -Note that i f we know the position and orientation of Frame F^ relative to F^ ^, ^ is also known. 5.2 Statement of Kinematics Problem To generate a robot program, we must find the tool position and orientation at each point along the weld seam trajectory. Mathematically stated, we wish to find the homogeneous coordinate frame F of the tool G relative to and projected onto the robot base coordinate frame F . F is R G defined by the homogeneous coordinate transformation ^ which takes us from F to F . Thus, we wish to find IL . We can define a string of G R "iv, G intermediate coordinate frames F^ for which H^_^ • ^ can be calculated, as shown in Fig. 5.4. We define the following homogeneous coordinate frames: a) F - robot base frame. R b) F T - positioning table frame. c) F - workpiece frame. B d) F^ - weld seam frame. e) F - tool frame. G We can solve for the homogeneous transformations H_ , H , H , —R,T —T,B —B,C IT, . We can then find H_ —C,G ~R,G SR.G = V T - T , B V c SC.G ' ( 5 , 6 ) 5.3 Intermediate Homogeneous Coordinate Frames 5.3.1 Table Frame Relative to Robot Base We wish to define the position of the table frame Ff relative to - 23 -Figure 5.4 Series of Homogeneous Coordinate Frames. - 24 -the robot base c o o r d i n a t e frame F . The problem i s to find the trans-R formation ^. Suppose we analyse the table as an n-link system, s i m i l a r to a robot. We can then define the transformation H. , . from l i n k i - l to l i n k i by — i - l , i the Denavit-Hartenberg [1955] equations (see Section 6.2.2.1 for de r i v a t i o n ) : H, . - i - l , i cose, Sin6 -Sin 8 Cos a. Sin 8. Sin a. a.CosS^ i i i i l i Cos 8. Cos ct. -Cos 8. Sin a. a. Sin 8. l i i i i i Sinct. Cos a, d. l (5.7) where 8^ i s the i ^ j o i n t angle and ct^ and a^ are fixed l i n k geometry p a r a m e t e r s . E i t h e r 6^ ( r e v o l u t e j o i n t ) or d^ ( p r i s m a t i c j o i n t ) i s v a r i a b l e . We can combine the transformation for a l l n l i n k s as T+ - H ••• H • . (5.8) —0,n —U,1 — l , z —n-1, n th This gives us the n l i n k coordinate frame F r e l a t i v e to the f i r s t n l i n k coordinate frame F 0 We a l s o wish to s p e c i f y the loc a t i o n of the table frame F r e l a t i v e to F , and the f i r s t l i n k frame F^ r e l a t i v e to the robot base frame F . These two transformations H , H are fixed and known. Thus, R, 0 n, T H_ • _ H _ • H _ -R,0 -0,n -n,T (5.9) - 25 -5.3.2 Workpiece Frame Relative to Table We can safely assume that the workpiece i s f i r m l y attached to the t a b l e . Thus, H i s some fixed r o t a t i o n and t r a n s l a t i o n r e l a t i v e to F_ —T , B T which needs to be s p e c i f i e d only once during i n i t i a l i z a t i o n . 5.3.3 Weld Seam Frame Relative to Workpiece We d e f i n e the weld seam frame F as shown i n Figure 5.5, where T i s tangent to the seam traj e c t o r y and A bisects the surface normals N_ , N^. ( i . e . , A i s normal to weld fa c e ) . The transformation H i s then — B, C [ T, B, A, R ] (5.10) 5.3.4 Tool Frame Relative to Weld Seam The welding rod i s positioned r e l a t i v e to the weld as required by standard welding p r a c t i c e . The p o s i t i o n and o r i e n t a t i o n of the to o l r e l a t i v e to the weld are characterized by angular o f f s e t s from the weld surface normal, a and 3, and a tool gap p ( F i g . 5.6). p, a, 3 are welding parameters determined by the Expert Welder module. We know that the elements of L can be found from -C,G From trigonometry: [ n, o, a, P ] - s i n a s i n 3 cos a cos 3 0 and P_ = - p £ . n, o are a r b i t r a r y — l e t us define them as (5.11) (5.12) (5.13) Figure 5.5 Weld Frame Relative to Workpiece. 27 -n A = Z , \ \ \ \ V \ \ F G = T O O L F R A M E / / / / / / / Y7 / / / B = Y C F c = W E L D F R A M E W E L D S E A M T = X C Figure 5.6 Tool Frame Relative to Weld Seam. - 28 -0 cos 3 cos a si n 3 0 (5.14) and a x n (5.15) We can now f i n d elements of IT = [n, o, a, P] —C, G — — — 5.4 C a l c u l a t i o n of the Tool P o s i t i o n and Orientation Relative to World Coordinates Once we have s p e c i f i e d a l l the intermediate homogeneous trans-formations, we can calculate the to o l p o s i t i o n and o r i e n t a t i o n r e l a t i v e to F . T h i s i n f o r m a t i o n i s given by the homogeneous transformation H K. —R, G which takes us from the tool coordinates to robot coordinates. By combination of homogeneous transforms, we can ca l c u l a t e ^: H „ -R,G —R,T —T,B -B,C -C,G (5.16) The e l e m e n t s of H have the form o f f o u r column v e c t o r s , n_, o_, a_, P_: We know that the vectors n., o^ , and ji define the basis vectors of the t o o l coordinate frame F projected onto the robot base frame F , and P i s G R — the displacement of F r e l a t i v e to F . I f we wish to define a world G R coordinate frame which i s d i f f e r e n t from the robot base frame, they can be simply related by a fixed transformation. 6. PATH FEASIBILITY 6.1 Statement of Feasibility Problem When we calculate a nominal robot tool trajectory through space, we may find that the trajectory is not feasible. The f e a s i b i l i t y issue can be broken into two fundamental problems: 1. Kinematic constraints of the robot. 2. Interference or co l l i s i o n with objects in the workstation. We can define a trajectory as being feasible i f the robot can follow the trajectory smoothly, without violating the kinematic constraints and without colliding with or contacting any of the objects in the work-station. Of the two problems we have defined, the kinematics problem is the least d i f f i c u l t , and we w i l l discuss i t f i r s t . 6.2 Kinematic Feasibility Mathematically the conditions which must be satisfied for kinematic fe a s i b i l i t y are: 1. The work envelope must not be exceeded. 2. The robot joint angles must remain within their physical limits. In order for us to test these conditions i t is necessary to solve for the joint angles as a function of the desired tool location and orienta-tion r e l a t i v e to the robot base (i.e., H^ This is commonly known as the inverse kinematics solution. There are two particular cases which are of practical interest. Suppose we define a robot gripper state vector X defining n independent state variables. Let us also define a joint - 30 -variable vector _6 defining m joint variables. If n = m, we can find a f i n i t e set of solutions of 9 for any set X in the work envelope. If n < m, an infinite set of solutions exists and we say the robot has redundant degrees of freedom. The kinematic problem has been considered by many researchers over the years. Denavit and Hartenberg [1955] f i r s t formally specified the geometry of connected links with homogeneous matrices. This work has been fundamental to most later work. Whitney [1969; 1972] developed the mathematics for the kinematics of multi-degree-of-freedom manipulators and prosthetic arms, and solved the inverse problem by finding the inverse Jacobian matrix, J - r e l a t i n g g r i p p e r motion to joint rate ( i . e . _9 = J_~ 1 X_). This method allows relative cartesian motion along world coordinates, based on real-time calculations of instantaneous J_( 0) and J~ \ 6). No general solution has been found for the inverse problem for absolute positions. An iterative solution based on f i n i t e displacement analysis has been proposed by Konstantinov [Konstantinov and Markov 1980; Konstantinov, Genova and Zahariev 1981; Konstantinov, Markov and Nechev 1981; Konstantinov and Patarinski 1982]. Several general recursive techniques have been proposed based on the approximate decoupling of the minor and major axis of motion [Gaglio et al 1981; Milenkov and Huang 1983; Benati et al 1982]. Explicit solutions can be found for most robot configurations of practical importance. Lee and Ziegler [1983] have derived an explicit solution for the PUMA robot based on geometry. Explicit solutions can - 31 -also be obtained by matrix algebra using the Denavit-Hartenberg matrices [Paul, Shimano and Mayer 1981; Paul 1981]. Redundant degrees of freedom are useful for avoiding joint contraints and allowing collision-free configurations to be found. The inverse problem can be solved by optimizing some criterion function. Redundant systems have been considered in more detail by a number of investigators [Fournier and Khalil 1977; Liegeois 1977; Konstantinov Markov and Nechev 1981; Benati, Morasso and Tagliasco 1982]. We w i l l develop an explicit inverse kinematics solution based on the matrix method of Paul [Paul, Shimano and Mayer 1981; Paul 1981]. 6.2.1 Definition of Robot Joint Frames and Resulting Transformations A robot arm consists of a series of rigid links connected by revolute or prismatic joints. Coordinate frames are attached to each link according to the specification of Denavit and Hartenberg [1955]. The geometry is shown in Figs. 3.1 and 3.2. First let us state some definitions: J_ = joint i axis of rotation or translation a^ = common normal of axes J^, For a revolute joint, the origin of F is located at the intersection of a n d If :LL + I intersect, the origin is at the intersection point. If J, , J, ,, are p a r a l l e l , a. i s chosen to intersect J.., at the — i —i+1 — i — l + l same place as • If joint i+1 is prismatic, replace above with the axis of the next revolute j o i n t . The o r i g i n of F^ is defined for - 32 -d i + l = °-The z axis, z^, of F^ i s along T n e x-axis, of F^ i s along a.. If J . , J. ,, i n t e r s e c t , x. i s p a r a l l e l or a n t i - p a r a l l e l to J. x J — i — i —i+1 — i — l — l + l . 9^ i s zero when _x = x . For a prismatic j o i n t , becomes the jo i n t v a r i a b l e . In this case a_^ = 0. The o r i g i n of F^ i s c o i n c i d e n t with the o r i g i n of F ^ f o r d^ = 0. z^ i s a l o n g a n c * *± *-s p a r a l l e l or a n t i - p a r a l l e l to J x j . -i+1 6.2.2 Inverse Kinematics Solution 6.2.2.1 General Approach to Inverse Kinematics Problem Once we have defined the j o i n t frames as shown, we can calculate the homogeneous t r a n s f o r m a t i o n r e l a t i n g frame F^ to frame The transformation i s composed of the following sequence: 1. rotate about z axis of F^_^ by an angle 9^ ; 2. translate along z axis of F^_^ by distance d^; 3. translate along new x axis of by length a^; and 4. rotate about new x axis of F^_^ by the twist angle . These four transformations can be combined as follows: H. , . = Rot (z,9 ) Trans (z,d.) Trans (x,a.) Rot (x,a ) . —1—1,1 i i i i H, . . 0 0 -5 9^^ 0 C9 ± 0 0 1 0 0 0 0 0 1 1 0 0 a d 0 1 0 0 0 0 1 d^ 0 0 0 1 1 0 0 0 0 0 0 Ca ± " S ^ 0 So^ Co i 0 0 0 1 (6.1) - 33 -where C9^ = Cos 9^ , S9^ = Sin 9^ , etc. Mu l t i p l y i n g out, we get c e . -sg^Ca. SQjSa. a.CQJ i i i i x i i S9^ C8 Ca 4 -C9.,Soi a . s e ^ i i i i i l i 0 0 Set, Ca, (6.2) This transformation was f i r s t derived by Denavit and Hartenberg [1955]. Once we have defined H 1 for the l i n k s i = l , ... n, we can combine i — l , i them to f i n d each l i n k frame r e l a t i v e to the robot base frame F , i . e . , S R f i = \ l ^ l , 2 — * i - l , i <6-3> for i = 1, n. We can also fin d the transformation H. = H. , ••• H (6.4) — i , n — i , i + l — n - l , n t i l r e l a t i n g the l a s t frame to the i frame. We now wish to solve for the j o i n t variables f o r a given t o o l p o s i t i o n and o r i e n t a t i o n . We can f i n d the homogeneous transformation H which s p e c i f i e s the to o l o r i e n t a t i o n and p o s i t i o n as a function of —~R, n the j o i n t v a r i a b l e s 9^, 8 , ••• 9^. We know the t o o l l o c a t i o n and o r i e n t a t i o n r e l a t i v e to the robot base, ^. The tool frame i s related to the l a s t j o i n t frame by a f i x e d known transformation H , so we can - n, G e a s i l y f i n d H„ = H H The elements of H„ are now known: 1 —R,n —R,G —n,G —R,n i . e . , where H = [ n, o, a, P ] , (6.5) —K, n — — — — - 34 -n, o, a are basis vectors of link n frame relative to F ; — R P_ is position vector of link n frame relative to F R. Let us c a l l this matrix T^. We can now equate T and H element by element, searching for explicit —n —R,n equations for the joint angles 9 ^ , • • • 9 ^ . If necessary, we can also compare the elements of other equivalent matrices defined below. T = H —n —R,n = - l , n ^ 1 , 2 h ] l ^ n = h , n We continue un t i l we have explicit solutions for a l l the joint variables. The actual solution of a particular robot is generally based on intuition and simplification. The particular solution for a UNIMATION PUMA 560 follows. 6.2.2.2 PUMA 560 Solution For the PUMA robot, we can define a set of link frames as shown in Fig. 6.1. We can also specify the appropriate link parameters in Table 6.1. (6.6) (6.7) , etc. (6.8) Figure 6.1 PUMA Joint Frames and l i n k parameters. - 36 -Table 6.1 - PUMA Link Parameters Link Variable a a d 1 91 -90 0 0 2 e2 0 a 2 d 2 3 6 3 90 ~ a 3 0 4 \ -90 0 5 95 90 0 0 6 66 0 0 d 6 Note that the sign of a 3 is negative to conform to our definition of x 2. For the PUMA, the link parameters have the following values: a 2 = 432 mm d 2 = 149.5 mm a3 = 20.5 mm d^ = 432 mm d 6 = 56.5 mm Note that the following analysis also applies to any other 6 degrees of freedom robot with the same geometry. The correct values of the link parameters a ^ , d 2 ' ^4' a n C* ^6 a r e s * m P l v substituted into the solutions. We know the position and orientation of F relative to F from o 0 SQ,6 = t £ » £ » £ ' I 1 ' ( 6 , 9 ) where n, o, a are the F, basis vectors projected onto F ; - 37 -P Is the origin relative to F n . — b U Note that F^ i s the frame attached to link 0 by our definition, and is e q u i v a l e n t to the robot base frame F . F, i s attached to the tool R u mounting flange. We have defined the inter-link transformations H. for each link - i - l , i and we know that H_ , = H_ . H . * * • H. . y X y X X y £• X X y X (6.10) From the geometry of the arm (Figure 6.2) we note that the origin of F^ is dependent on the variables 9^ , 0^ , 0^ only, and is coincident with the origin of F,.. Let q = P - d, a (origin of F c) , (6.11) — — o — J and q^ = 4*"** column of HQ ^ (origin of F^) . We can find ji by multiplying HQ ^ ^ £ 2 3 H 3 4 ( w e n e e c * t n e 4 t ' 1 column only). The 4 t h column of H_ , is S 3 d 4 + a 3 C 3 " C 3 d 4 + a 3 S 3 0 1 - 39 -The 4th column of H, . is -1,4 C 2(S 3d 4 + a 3C 3) + S 2(C 3d 4 S 2 ( S 3 d 4 + a 3C 3) - C 2 ( C 3 d 4 a3 S3> + a2 C2 a 3S 3) + a 2S 2 q = 4th column of H •,. — HJ, 4 Multiplying out, we get q = C 1 [ c 2 ( S 2 d 4 + a 3C 3) + S 2(C 3d 4 - a 3S 3) + a ^ ] S 1 [ c 2 ( S 3 d 4 + a 3C 3) + S 2(C 3d 4 - a 3S 3) + a ^ ] -S 2(S 3d 4 + a 3C 3) + C 2(C 3d 4 - a 3S 3) - a ^ 1 q can be simplified as follows: , r j . „ n 1 _ S D C l d 2 S l d 2 C l d 2 q. = W 2 3 + a 3C 2 3 + a 2C 2 S l K S23 + a 3C 2 3 + a 2C 2 d4 C23 " a3 S23 " a2 S2 1 (6.12) (6.13) where 323 = sin( 6 2 + 93) u23 = cos(6 2 + 8 ) - 40 -Solution f o r 9^ From the f i r s t two rows of (6.13) we can show that + , 2 ^ 2 ,2,1/2 , fl -1 r * q 2 ( q l + q2 ~ V - d 2 ^ 1 1 ..... 1 q l ( q l + q2 " d2> + d 2 q 2 + for l e f t handed arm configuration, - for right handed arm configuration. Note: For 9 = tan 1 (y) ; 0 < 9 < 90° for x > 0 and y > 0; 90° < 9 < 180° for x < 0 and y > 0; -180° < 9 < -90° for x < 0 and y < 0; -90° < 9 < 0 for x > 0 and y < 0. Solution f o r 9^ Let 2 2 2 2 2 2 2 q l + q2 + q3 " d2 ~ d4 " a3 " a2 2 a2 (6.15) B = - d 4 ; C = a 3 . (6.16),(6.17) We can show that A = -B s i n 9^ + C cos 9^. The so l u t i o n has the following form: 8 3 = t a n " 1 (|) - t a n " 1 ( ± A - ) (6.18) ^ 2 * 2 - A'' where 2 2 2 r = B + C . (6.19) - 41 -Substituting (6.16), (6.17) into (6.18) we get 8 = t a n _ 1 ( - ^ ) - tan" 1 ( " A " ] (6.20) J " d4 ± f l ~2 r - A 2 2 2 where r = a^ + (6.21) Two different solutions result depending on the sign: + for elbow above configuration, and - for elbow below configuration, (for lef t arm configuration); or - for elbow above configuration, and + for elbow below configuration, (for right arm configuration). Solution for 8^ From (6.13) we get q3 = d4 C23 ~ a3 S23 " a2 S2 " ( 6 * 2 2 ) Expanding, we get q3 = d J C 2 C 3 " S 2 S 3 ] " a3 t S2 C3 + C2 S3 ^ " a2 S2 ' ( 6 * 2 3 ) Rearranging we get q3 = ^ d4 C3 " a3 S3 J C2 " K S3 + a3°3 + a2 ^ S2 * ( 6* 2 4> Let A = d 4C 3 - a 3S 3 , (6.25) B = d 4 s 3 + a 3C 3 + a 2 . (6.26) Then the sol u t i o n i s t a n - 1 (• A :)-tan" 1( — z = — — ) • (6.27) B ± / 2 - 2 r q 3 Substituting (6.25), (6.26) into (6.27) we get - 42 -•> d. C_ - a_S_ , q. °2 • ^ V . ^ > -1 «.») where 2 2 2 r = A + B . (6.29) Expanding (6.29), we get r 2 = (d 4C 3 - a 3 S 3 ) 2 + (d 4S 3 + a 3C 3 + a 2 ) 2 . (6.30) Again, there are two solutions for (6.28); + is for the lef t handed arm configuration and - is for the right handed arm configuration. There are two solutions for 8^ because the parameters and q^ are not in the equation. It is intuitively obvious that for a given waist r o t a t i o n 8^, there exist two possible shoulder rotations, » which position the arm at the same height q 3. The possible rotations position the arm in opposite quadrants of the horizontal plane. The correct solu-t i o n can be determined by substituting 0^ into the equation for the parameter q^ or q^ (equation 6.1) and comparing the sign of the result. It turns out, however, that the positive solution is always associated with the l e f t arm configuration and the negative solution with the right arm configuration. Solution for 8., 8 JT Z T — — — _ Set 6^ such that is normal to the plane formed by z^ and a_, i.e., (z- • a ) z. = ± - H = 1- (6.31) \L3 ' £ T and - 4 3 -"4 fe3 * ^ ' ( 6 ' 3 2 ) cos e 4 . = ± t 3 ' £ 4 ) • < 6 - 3 3 > Note that x^, are the f i r s t two columns of ^. Expanding out ^ and s u b s t i t u t i n g 3 ^ , _V_ and £^ into (6.32), (6.33), we can show where + gives wrist not f l i p p e d configuration and - gives wrist f l i p p e d configuration. Set 8 5 so that £5 = £ > (6.35) i . e . , s i n 8,. = x^ * £ » (6.36) and cos 8^ = -y^ * £ . (6.37) x., y, are the f i r s t two columns of H. ,• Expanding out H . , and —4 —4 —0,4 0 —0,4 sub s t i t u t i n g x^, y^ into (6.36), (6.37) we can show - i r ( C 1 C 2 3 C 4 - S l S 4 ) a x + C2 t C3 C4 C5 S2 [ C3 C4 C5 - s 3 s 4 c 5 ] - s 2 [ s 3 c 4 c 5 + c 3 s 4 c 5 ] - S 3S 4C 5] + C 0[S,C AC q + 0,8,0,.] 2 l"3 4 5 3"4 5J - 53 -*1,6<4) = s 2[ • • • ] - s 2 [ ] + c2[ ] +a 2C 2 ] +a 2S 2 S cd + C cd + d„ 5 x 5 y 2 (6.64) ^0,6 [ n, o, a, P ] . (6.65) Let So|l So,6 where and f u ( n ) f n ( o ) f u ( a ) f u ( P ) f 1 2 ( n ) f 1 2 ( o ) f 1 2 ( a ) f 1 2 ( P ) f 1 3 ( n ) f 1 3 ( o ) f 1 3 ( a ) f 1 3 ( P ) 0 0 0 1 f n ( A ) = C l A x + S l A y , f 1 2 ( A ) = -Az , f 1 3 ( A ) = " S ^ + C ^ , A is n, o, a or P. -1 (6.66) (6.67) (6.68) (6.69) We also know H„ n H_ , = H., ,, which we have found as a function of 9„. —U , J. —U,D — 1 , 0 Z 6^, 6^, 9^. Equating elements we find - 54 -0 = -S.,n + 1 X C.n , 1 y (6.70) C5 = -5,0 + 1 X 1 y (6.71) S5 1 X 1 y (6.72) d2 5 x 5 d = -S.P + C..P y 1 x 1 y (6.73) We know the po s i t i o n P_ and axis o r i e n t a t i o n a_ of the t o o l . We now solve for the remaining t o o l o r i e n t a t i o n vectors n, o, as a function of a, P. Multiply (6.71) by d y , multiply (6.72) by d x and subtract (6.71) & (6.72) from (6.73). D - d o -y y y x y d. = -S. [P - d o - d a ] + C. [p d a ] . (6.74) 2 l L x y x x x J 1 L y y v x v J Rewrite as d2 = S i a + C 1 B (6-75) where a = P - d o - d a (6.76) x y x x x v ' and 3 = P - d o - d a (6.77) y y y x y Let a = r cos , r = / a 2 + 32 , (6.78), (6.79) 3 = r s i n , • - t a n " 1 (-|) . (6.80), (6.81) Substitute for a, 3 i n (6.75). s i n <|> cos 8 - cos s i n 8^ = — (6.82) d2 or sin(<{> - 9^ = — (6.83) and - 55 -cos(<|>- 9 ) = ± J d T T . (6.84) 1 1 " (—) v r We can then show 6 = t a n - 1 ( | ) - t a n ' M — — = = ) (6.85) r - d 2 where a = P - d o - d a , (6.86) x y x x x p = P - d o - d a , (6.87) y y y x y 2 2 2 r = a + 3 • (6.88) Noti c e that 9^ i s a f u n c t i o n of j>, which i s what we are tr y i n g to f i n d . The sol u t i o n i s s i m p l i f i e d i f we i n s i s t that d = 0. y Now a = P - d a , (6.89) X X X 3 = P - d a . (6.90) y x y We can solve for 9^ since we know P, a_. From (6.70) we know n = n t a n " 1 9. . (6.91) y x 1 We also know that the vectors are unit vectors and that they are ortho-gonal . Thus, 2 2 2 n + n + n = 0 (6.92) x y z and n a + n a + n a = 0 . (6.93) x x y y z z - 56 -Combining and solving for n^, we get . + r,. 2 2 2. 2 . 2,, 2, 2 . 2. 2. J./2 -2n a a ± |(4n a a ) - 4(a + a )(n (a + a ) - a ) x x y L x x y y z x x z z J „ _ _ .(6.94) 7 2 (a + a ) v y z ' Combining (6.91) and (6.94) we get e a 2 1/2 n x = ± -2 ~ 5 r ] (6.95) e tan 9 + 2e tan 9, a a + e(a + a ) 1 1 x y x z where and e = a 2 + a 2 (6.96) y z v ' v 2 2 n = ± l - n - n . (6.97) z x y v ' Choose the sign of n^ which gives a_ • ii = 0 . Note that there are two antiparallel solutions for ja. We can now find o simply as o_ = a_ x n . (6.98) We could now find explicit solutions of the joint angles as functions of P_ and a_. However, having found n, o_, we have defined two unique tool orientations for which the 6 degree of freedom PUMA solution is guaranteed to give 9^ = 0. If we post multiply H^ ^ by the inverse tool transforma-tion H c ^ " , we get the tool mounting flange location and orientations, J , o i.e., «0,5 = «0,6 «5*6 ' <6'"> This i s equivalent to ^ = [n, j3, a_, PJ for the 6 degree of freedom case. Thus the 6 degree of freedom solution for this H_ , w i l l yield the —U, o - 57 -5 degree of freedom j o i n t angles. 6 . 2 . 3 . 2 ASEA I R B 6 0 Solution For a 5 degree of freedom robot such as the ASEA I R B 6 0 , we can specify the tool l o c a t i o n jp and just one tool frame basis vector, a. The other basis vectors n, o_ are determined by P and a. We can solve for n, o as we did previously for the 5 degree of free-dom PUMA s o l u t i o n . The complete to o l frame i s then known. We can modify the 6 degree of freedom PUMA solutions by su b s t i t u t i n g the ASEA l i n k parameters and solving for the angles given H R j T " [ H> £» £» 1 ] • ( 6 . 1 0 0 ) As we noted i n S e c t i o n 6 . 2 . 3 . 1 , t h i s ^ i s guaranteed to give a wrist r o t a t i o n 9 ^ = 0 . Thus, i f we neglect the 9^ s o l u t i o n and reassign the other j o i n t angles as 9^ : = 9^, 9^ := 9^ we can generate the solu-tions for the ASEA I R B 6 0 . The r e s u l t s of Section 6 . 2 . 3 . 1 can be s i m p l i f i e d for an ASEA I R B 6 0 by noting that = 0 . From ( 6 . 8 5 ) we get P - d a ( 6 . 1 0 1 ) X X X for the following tool transformation: 0 0 -1 0 0 1 0 0 1 0 0 0 X ( 6 . 1 0 2 ) 0 d - 58 -From (6.91), (6.95) and (6.97) we can calculate n as a function of a_, 9^ . (a Z + a Z ] a ^ y z 1 z (a +aZ )tan Z 9 + 2 (aZ+a )a a tan 9, + (a^+a ) fa +a 1 y z y l _ ^ v y z ^ x y 1 v y z ^ x z - _ n = n tan 9, , 1/2 , (6.103) (6.104) _ n = ± 1 - n - n z x y 21 (6.105) The sign of n i s chosen to give a • n = 0. Note that there are two z — — antiparallel solutions for n. Generally only one of the solutions w i l l lead to a feasible set of joint angles. We can now find o_ from (6.98), i.e. o = a * n The joint solutions can be found by plugging the appropriate components of n, o_, a, p. Into the joint angle equations from Section 6.2.2.2. The equations can be simplified for the ASEA IRB60 by noting that the link parameters , a^ and the wrist rotation 9^ are zero. Rename the j o i n t angles, 94 : = 95 • 95 := 66 * The resulting solutions are as follows: 6 = tan p - d a _ 1 ( J - / J ) . d a x x (6.106) = tan -1 .(. ,2 2 . z, z , z ,z z.z v _ ± l 4 d 4 a 2 ~ ( q l + q 2 " ^ 3 4~ a2 ) ' 2, 2, 2 .2 2 q l + q 2 + q 3 " d 4 " a 2 2~2 " ~ T~2 2.2 a/2 (6.107) - 59 -92 = t a n -1 d4 c3 d 4 s 3+a 2 - tan -1 . ± ( d 4 + d 4 a 2 s 3 , 2 2 N1/2 +a 2-q 3) J , (6.108) 9. = tan 4 -1 C l C 2 3 a x + S l c 2 3 a y ~ s23 az i_c s ,a + s s 0,a + c.,a 1— 1 23 x 1 23 y 23 z (6.109) = tan -1 -s,n + c,n 1 x 1 y -s,o + c,o 1 x 1 y (6.110) where c. = cos 9 s. = sin 9 c. . = cos (9+9.) s l t = sin(9+9.). i i ' i i ' i j i ] i ] i y The link parameter values for the ASEA IRB60 are: a 2 = 800 mm, a 3 = 1150 mm, d,. = 200 mm + z component of the tool transformation. 6.2.3.3 Redundancy of the Sixth Joint for Welding It is important to note that the welding torch is axisymmetric. Thus, we need only specify the position of the torch tip and the orienta-tion vector of the torch axis. Any arbitrary rotation of the torch frame about the torch axis does not change the effective torch orientation (Fig. 6.4). We find that only 5 degrees of freedom are needed to define a position P_ and a single orientation vector a_. We can show that the other two basis vectors ri, o_, of F^ are then determined as a function of the joint angles and of P and a. - 60 -If we have 6 degrees of freedom, we can independently define the position P^ and any 2 of the basis vectors n, o_, ji (the third basis vector i s defined by the other 2). This means that we can explicitly rotate F c 6 about the torch axis ji without changing the tools effective orientation (Fig. 6.4). We w i l l use this trick, in searching for feasible solutions of a trajectory by specifying a rotation of F,. about a, calculating a new b — HQ g and testing the resulting joint angles for f e a s i b i l i t y . 6.2.4 Specification of Robot Coordinates The position of a robot may be uniquely specified two ways: 1. a l l joint angles specified (explicit); and 2. position and orientation of tool frame specified. The f i r s t case is t r i v i a l once we have solved for the joint angles. The second case requires transformation of the tool frame specification F £ 6 into position and orientation coordinates for the target robot. We know FG = [ »» £ . £» I ] » (6.111) where n, o, a are basis vectors of F projected onto F , — — — (J R and P is the position vector of the F origin relative to F . — O R For example, for the PUMA 560 robot we must specify coordinates X, Y, Z, 0, A, T where X, Y, Z are the Cartesian coordinates of the tool frame F^ r e l a t i v e to the robot base F and 0, A, T are three Euler rotation angles of F, about the axes of F^. —G —R - 61 -Figure 6.4 Rotation of Tool Frame About Tool Axis. - 62 -Calculations of the robot position and orientation specifications from the j o i n t angles or from F depends on the robot being used and how —o i t defines i t s locations. 6.2.4.1 Calculation of PUMA Tool Location and Rotation Coordinates The PUMA robot defines locations by the Cartesian coordinates of the tool center, X, Y, Z, and three Euler angles o, a, t, the orienta-t i o n , a ltitude and tool angles (Fig. 6.5). For o, a, t angles a l l equal to zero, the tool frame F^ , i s related to the robot base frame F by a transformation IL R —R, T 0 1 0 X 0 0 -1 Y -1 0 0 z 0 0 0 1 (6.112) where X,Y,Z are the position vectors of the or i g i n of F re l a t i v e to F . 1 R X, Y, Z do not change with angles o, a, t . The rotated tool frame F^ i s found by performing an Euler angle rota-tio n sequence t , a, o to find the transformation ^. MR T = rot(o,3) ro t ( a , l ) rot(t,2) , (6.113) where rot(o,3) means rotate about z axis ( i , ) of F by an angle o, etc. — j R rot(o,3) cos o sin o 0 0 -s i n o cos o 0 0 0 0 1 0 0 0 0 1 (6.114) - 63 -Figure 6.5 Definition of o, a, t angles for the PUMA Arm. - 64 -rot(a,1) = rot(t,2) = 1 0 0 0 cos t 0 -sin t 0 0 cos a sin a 0 0 1 0 0 0 -sin a cos a 0 0 0 0 1 sin t 0 cos t 0 0 0 0 1 (6.115) (6.116) Multiplying out we get V where -C S -S S C o t o a t -S S +C S C o t o a t -C C a t C C -S S S t o t o a t S C +C S S t o t o a t -C S„ a t cos(o), S = sin(o), etc. o S C o a -C C o a -S a 0 Y Z (6.117) If a = - 90° , rotations o, t are coaxial. In this case we w i l l arbi-t r a r i l y set t = 0. To f i n d the angles o, a, t from a known transformation H„ m we —R, T proceed as follows: We know that x = [ VN, VO, VA, VP ] (6.118) where - 65 -VP = X Y Z 1 VN] VN = VN2 , etc. VN, 0 Premultiply both sides of (6.118) by rot(o >z) \ rotto.z)" 1 E^- = r o t ( o , z ) _ 1 [VN, VO, VA, VP ] . Multiplying out and equating elements we get 0 cos(t) -sin(o) VA1 - cos(o) VA sin(t) sin(a) cos(a) cos(o) V0± + sin(o) V0 2 -cos(o) VN, - sin(o) VN. -VA„ sin(o) VA, - cos(o) VA, From these equations we get o = tan i - V A i _ 1 f 1 V A2 o = o + 180 a = tan -1 -VA„ sin(o) VA1-cos(o) VA2 (6.119) (6.120) (6.121) (6.122) (6.123) (6.124) (6.125) (6.126) -cos(o) VN -sin(o) VN„ _ -1 r 1 I i L cos(o) VO^ sin(o) V0 2 J (6.127) For the PUMA, the angle t is defined as the negative of the usual Euler rotation convention. If we change the sign of t to conform to the PUMA definition we get: cos(o) VN, + sin(o) VN„ t = tan _ 1 [ 1 2 cos(o) V0 1 + sin(o) V0 2 ] (6.128) Also the following is true: (o, a, t) = (o + 180, 180 - a, t + 180). - 66 -6.3 Collision Avoidance In order to successfully generate a robot program off-line, i t is essential that we be able to reliably avoid collisions between the robot and objects in the workstation. This is a d i f f i c u l t problem which is the subject of much current research. To handle this problem our world model must contain complete geo-metric descriptions of the robot and objects in the workstation. The geometric models can be provided by CAD systems in several forms. Solids are usually represented by a description of their surfaces, or as a composite of primitive solid elements. The most common surface repre-sentation is the polyhedral model which defines a solid polyhedron by specifying a l l of i t s vertices, edges and faces [Hosaka, Kimura and Kakishita 1974; Wesley et al 1980]. The resulting description is simple, uniform and unambiguous. More advanced CAD systems use constructive solid geometry to generate complex solids by performing boolean operations on a set of simple solid primitives such as boxes and cylinders [Braid 1975; Requicha 1980]. The resulting representation must contain a record of the composite primitives as well as the sequence of operations. More complete surveys of solid representations can be found in the literature [Baer, Eastman and Henrion 1979; Requicha 1980]. The c o l l i s i o n avoidance problem has been well summarized by Lozano-Perez [Brady et a l 1982]. The three classes of co l l i s i o n avoidance algorithm are: hypothesize and test, penalty functions and explicit free space. - 67 -The hypothesize and test algorithm generates a collision free path by t r i a l and error. Each time a c o l l i s i o n is detected an alternate path is formulated and tried. This method is simple but does not lead to an optimum solution and may not find a solution at a l l because the knowledge of the world is localized only and thus a global strategy is impossible. The penalty function scheme attempts to define a penalty function whose value reflects the probability of co l l i s i o n as a function of position in space. The algorithm then generates a path following the local minima of the penalty function. This method has been successfully applied to extremely simple world models only, and the algorithm becomes very complex for more re a l i s t i c situations. The third class of algorithm generates a set of a l l robot positions which are free of collisions and calls this set the free space. The shortest path through the free space joining an i n i t i a l and fi n a l position is then the optimum path. The main d i f f i c u l t y lies in generating the free space, and a l l algorithms to date incorporate simplifications and approxi-mations which restrict their generality. Several free space path planning algorithms have been presented in the literature [Udupa 1977; Lozano-Perez and Wesley 1979; Lozano-Perez 1983; Brooks 1983]. The problem of simply detecting collisions between the robot and objects in the workstation is a simpler problem to tackle at this point. However, a feasible path must then be found iteratively or by t r i a l and error. We w i l l not consider the alternate path selection problem, other than to note that in the case of welding we must reposition the workpiece i f a co l l i s i o n is detected. - 68 -The simplest interference detection problem is the problem of inter-ference between fixed, stationary objects. Hosaka [1974] describes a method for completely specifying the intersection between two stationary convex polyhedra. Maruyama [1972] has proposed a simple method for determining intersections between polyhedral solids using a hierarchical set of tests. The problem of interference between moving and stationary polyhedra was considered by Boyse [1979]. The tests which he devised apply only to the simple cases of pure translation and rotation and cannot be extended to the more general case of complicated motion. An algorithm for testing for interferance between two moving convex polyhedra is suggested by Schwartz [1981]. His method finds the minimum distance between two convex polyhedra as a function of time i f their individual trajectories are known functions of time. Schwartz demonstrates the algorithm for the two dimensional case only. Another way of handling interference detection between moving objects is to define a swept volume generated by the moving object. The problem is then reduced to the simpler case of interference between stationary solids. This method is discussed by Requicha [1980] and Lozano-Perez and Wesley [1979]. We w i l l develop a detailed interference detection algo-rithm based on the swept volume method. 6.3.1 World Model Any world model representation embodies a trade-off between accuracy and conciseness for solids of arbitrary shapes. If we use a polyhedral representation we can choose any number of surface facets to - 69 -approximate curved surfaces to any degree of accuracy. In p r a c t i c e , however, we w i l l generally use a coarse approximation to reduce the s i z e of the data structure and increase the speed of any processing we wish to do. Unfortunately, any sim p l i f y i n g approximations that we make introduce uncertainty into our c o l l i s i o n p r e d i c t i o n s . We f i n d that we must trade off the s i m p l i c i t y of our mathematical model and algorithm against the r e l i a b i l i t y of the r e s u l t i n g c o l l i s i o n p r e d ictions. We can bias our model for the worst case, however, to ensure that a l l c o l l i s i o n s w i l l be predicted. The price we pay i s a higher p r o b a b i l i t y of c o l l i s i o n s being predicted where none occur. This can be an important consideration i f we must move near the objects i n the workstation, as i n welding. The world model which we w i l l adopt w i l l be a simple one for the purpose of demonstrating the basic concepts of interference detection. The general approach which w i l l be described can be applied to a more sophisticated and accurate world model and interference detection scheme. We w i l l consider a world model which divides the workstation into two sets of objects, moving s o l i d s and stationary s o l i d s ( F i g . 6.6). The moving s o l i d s include the robot manipulator and a l l parts attached to i t . Stationary s o l i d s include the workpiece, positioning table, f i x t u r e s , e t c . Although the table may be movable we w i l l assume that i t s motion cannot cause interference. Stationary objects i n the workstation w i l l be modelled as s o l i d polyhedra. A polyhedral representation allows our model to have any - 70 -I n i t i a l P o s i t i o n s i t i o n Figure 6.6 World Model - 71 -degree of accuracy we desire, and allows a simple data structure of vertices, plane faces and straight edges. For simplicity, we w i l l model the robot links as circular cylinders. The axes of the cylinders w i l l coincide with the axes of the links and the cylinder radius and length w i l l be chosen such that the cylinder f u l l y contains the link (see Fig. 6.7). Each cylinder w i l l be defined relative to the corresponding link coordinate frame. 6.3.2 Development of Swept Volume As the robot links move they sweep out a volume of space, as shown in Fig. 6.6. If we can analytically describe that volume, we can test i f the volume intersects any fixed solid in the workstation. In order to simplify the algorithm and minimize the required calcula-tions we w i l l adopt a method for approximating the swept volume in a concise way. To illustrate the method, we w i l l consider the motion of one cylindrical solid through space. Suppose we have a cylinder of radius R and length L. It has a central axis or spine with endpoints A and B. If the cylinder is moved from an i n i t i a l position Pi_ to some other position P2, then some volume has been swept out by the cylinder. If we assume the motion was smooth and continuous between Pi and P2, we can use the following approximation for the swept volume. The i n i t i a l and f i n a l endpoints of the spine are connected with straight lines A]A2> B]_B2« These line segments, along with the i n i t i a l and fi n a l spine positions, define a ruled, parametric surface. The surface has a simple, non-linear parametric equation of the form - 72 -Figure 6.7 C y l i n d r i c a l Approximation of a Manipulator Link. - 73 -r (A , A ) = r + A U + Av + A A (u - u ) (6.129) —p u v —o u—o v—o u v M. —o where r , u , v , u, are defined as shown in Fig. 6.8. —o —o —o —1 If we restrict the ranges of the parameters to 0 < \^ < 1, 0 < Av < 1, we obtain a surface patch or region bounded by the vectors £o» iLl> Zo» Z l a s s n o w n 1 1 1 Fig* 6.8. We can now define two bounding parametric surfaces offset from this parent surface by a distance R on each side. To do this, we define the corner points of the new surfaces by finding the surface normals of the parent surface at each corner, and translating along those normals by distances ± R. The two offset bounding surfaces can be expressed relative to the parent surface by the following equation: r u(A , A ) = r (A , A ) ±R n( A , A ) (6.130) —b u v —p u v — u v where R is the cylinder radius, A , A are the surface parameters, u v ii is the unit surface normal of the parent surface, r is any point on the parent surface —P satisfying equation (6.129). The unit surface normal is defined as follows: V X V £ = |7 x 71 (6.131) '—u -v 1 3 r where 7 = ^2. = u + \ ( u - u ) (6.132) —u a A —o v —1 — O u 3r V = = v + A ( U l - u ) . (6.133) —v 3 A —o u —1 —o F l 8 U f e 6'8 Para m, S w e P t Volume. - 75 -If we expand these equations and collect the terms, we find that the bounding surfaces can be expressed by equations of the form r = A + A B + A C + A A D (6.134) —b — u— v— u v— where A , B_ , C_ , D_ are known. Thus, the bounding surfaces are also ruled, parametric surfaces. Given these two bounding surfaces, we can easily derive similar surfaces over each of the four open faces of the volume between the bound-ing surfaces. Only two of these surfaces need to be found since the other two faces are bounded by the cylindrical surfaces of the link in i t s i n i t i a l and fin a l position. The interference detection problem is now to find intersections between the polyhedra representing stationary objects and the parametric swept volumes generated by the moving objects. A series of simple tests can be performed which w i l l determine i f intersection occurs. In practice, we would generate swept volumes for each moving link of interest. The motion of the manipulator would be divided into a series of Intermediate motions, with each intermediate motion generating a swept volume. 6.3.3 Interference Conditions We can formulate five simple intersection conditions (see Fig.6.9): 1. Any solid polyhedral edge intersects any swept volume bounding parametric surface. 2. Any polyhedral edge Intersects the cylindrical solid of a link in it s i n i t i a l or fi n a l position. Figure 6.9 Interference Conditions. - 77 -3. Any swept surface edge intersects any solid polyhedral surface. 4. The cylindrical volume of any link in its i n i t i a l or fin a l position intersects any polyhedral face. 5. Any polyhedral solid is wholly enclosed by any swept volume. The interference algorithm now consists of testing for each of these cases for every possible combination of solid polyhedra and swept volumes. A hierarchy of tests can be devised to determine i f intersection w i l l occur, as shown in Fig. 6.10. Each test w i l l now be considered in detail. 6.3.3.1 Condition 1 - Intersection of an Edge and a Parametric Surface (Fig. 6.11) If we are given 4 points in space, P_^ , P_ , P^, P^, we can define a ruled, parametric surface with the following equation: r = P + A u + A v + A A (u - u ) , .... — —1 u —o v —o u v — 1 — o (6.135) where u = P_ - P, , —o —2 —1 »1 " *3 " *4 ' A , A are scalar parameters, u v Also, i f we are given the end points of a line segment, Ej, JJ2, we can define a line with the following equation: r = E. + A u _ 1 E _ E (6.136) where u^ , = - E_^ Combining (6.135), (6.136) we get the following system of equations: - 78 -Any s o l i d polyhedral edge intersects any swept volume bounding parametric surface. Yes No Any s o l i d polyhedral edge in t e r s e c t s the l i n k cylinder at i t s i n i t i a l or f i n a l p o s i t i o n . Yes fr-No Any swept volume edge intersects any polyhedral face Yes i No Link cylinder i n t e r s e c t s any polyhedral face i n i n i t i a l or f i n a l p o s i t i o n Yes No Any polyhedron vertex l i e s inside the swept volume Yes •• t < No Intersection Intersection Figure 6.10 Algorithm for Interference Detection. - 7 9 -Figure 6.11 Intersection of an Edge and a Parametric Surface. - 80 -X u + X v - X u + X X (u -u ) + P - E = 0 u—o v—o E—E u v —1 —o —1 —1 ( 6 . 1 3 7 ) This system of non-linear equations can be solved i t e r a t i v e l y using Newton's method. The edge i n t e r s e c t s the surface within i t s boundaries i f 0