UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Determination of robot trajectories satisfying joint limit and interference constraints using an optimization… Buchal, Ralph Oliver 1987

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

Item Metadata

Download

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

Full Text

DETERMINATION OF ROBOT TRAJECTORIES SATISFYING JOINT LIMIT AND INTERFERENCE CONSTRAINTS USING AN OPTIMIZATION METHOD By RALPH OLIVER BUCHAL B.A.Sc, The U n i v e r s i t y of B r i t i s h Columbia, 1980 M.A.Sc., The U n i v e r s i t y of B r i t i s h Columbia, 1984 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY i n THE FACULTY OF GRADUATE STUDIES Department of Mechanical Engineering We accept t h i s thesis as conforming to the required standard THE UNIVERSITY OF BRITISH COLUMBIA Ju l y 1987 ® Ralph O l i v e r Buchal, 1987 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 it 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 1956 Main Mall Vancouver, Canada V6T 1Y3 Date July 22, 1987  DE-6(3/81) ABSTRACT An important problem i n robotics research i s the automatic o f f - l i n e planning of optimal robot t r a j e c t o r i e s to perform s p e c i f i e d tasks while s a t i s f y i n g p h y s i c a l c o n s t r a i n t s . This thesis proposes a method f o r f i n d i n g an optimal geometric robot t r a j e c t o r y subject to the constraints of j o i n t displacement l i m i t s and interference avoidance. A geometric method f o r c a l c u l a t i n g the distance between convex polyhedra i s presented, and the method i s implemented i n two dimensions f o r the c a l c u l a t i o n of interference. Point-to-point t r a j e c t o r y planning i s posed as a two-point boundary value problem i n the calculus of v a r i a t i o n s . The kinematic constraints are formulated as e x t e r i o r penalty functions and are combined with other optimization c r i t e r i a to form a cost f u n c t i o n a l . The problem i s solved by d i s c r e t i z i n g the problem and numerically minimizing the cost f u n c t i o n a l by using a steepest-descent approach to i t e r a t i v e l y modify the t r a j e c t o r y . Any s t a r t i n g t r a j e c t o r y which s a t i s f i e s the boundary conditions i s acceptable, but d i f f e r e n t s t a r t i n g t r a j e c t o r i e s may converge to d i f f e r e n t l o c a l l y optimal f i n a l t r a j e c t o r i e s . The method has been implemented f o r the two-dimensional case by an i n t e r a c t i v e FORTRAN program running on a VAX 11/750 computer. Successful r e s u l t s were obtained f o r a number of t e s t cases, and further work has been i d e n t i f i e d to allow a p p l i c a t i o n of the method to a wide range of problems. i i TABLE OF CONTENTS ABSTRACT i i LIST OF FIGURES i x NOMENCLATURE x i i i ACKNOWLEDGEMENT xx 1. INTRODUCTION 1 1.1 Background and Motivation ..1 1.2 A Summary of Previous Work i n Trajectory Planning 3 1.2.1 Configuration Space Methods 3 1.2.2 Local Optimization Methods 5 1.2.3 P o t e n t i a l F i e l d Methods 7 1.2.4 Optimal Control Methods 8 1.3 Contribution of t h i s Thesis 8 2. KINEMATICS 10 2.1 The Gripper P o s i t i o n as a Function of the J o i n t Variables 10 2.2 The Denavit-Hartenberg Transformation 12 2.3 The Forward Kinematic Equation 12 2.4 S p e c i f i c a t i o n of the Task Coordinate Vector 14 2.4.1 A Li n e a r i z e d Representation of Rotational Error 16 2.5 Inverse Kinematics 17 2.5.1 E x p l i c i t Solutions 17 2.5.2 A Linea r i z e d General Solution 18 2.6 Inverse Kinematic Solutions f o r Redundant Systems ....19 2.6.1 Finding an Optimal Inverse Solution .• 20 i i i 2.7 An I t e r a t i v e Numerical Solution 20 2.7.1 L i n e a r i z a t i o n Error 25 2.7.2 An I t e r a t i v e Algorithm 26 2.7.3 Numerical Results 27 3. ANALYSIS OF INTERFERENCE 45 3.1 A Mathematical D e f i n i t i o n of Interference Avoidance ..45 3.2 Approximation of the Swept Volume by Convex Hull s ....46 3.3 C a l c u l a t i o n of the Distance between Stationary Objects 47 3.4 The Distance Between Convex Polyhedra 50 3.4.1 C a l c u l a t i n g the Separating Distance f o r an A r b i t r a r y Normal 50 3.4.2 Finding the Optimal Face Plane 52 3.4.3 C a l c u l a t i o n of the Separating Distance f o r D i s j o i n t Polyhedra 53 3.4.3.1 Steepest Descent Graph Search 55 3.4.3.2 C a l c u l a t i n g the Optimal Tangent Plane and Separating Distance 56 3.4.3.3 Distance Calculations f o r D i s j o i n t Convex Polygons i n 2-D 65 3.4.4 C a l c u l a t i o n of Penetration f o r Int e r s e c t i n g Polyhedra 66 3.4.4.1 C a l c u l a t i o n of the Penetration Vector .67 3.4.4.2 C a l c u l a t i o n of the Net T r a n s l a t i o n of the k t h P o s i t i o n to Reduce Penetration 70 3.4.4.3 ' Best F i t (Minimum Norm) J o i n t Space i v Displacement to Reduce a Set of Penetration Vectors 72 TRAJECTORY OPTIMIZATION 80 4.1 Optimization by Minimizing a Functional 80 4.1.1 Local and Global Optima 80 4.1.2 Optimality Conditions for the Continuous Case 81 4.1.3 Optimality Conditions for the Discrete Case .83 4.1.4 The Gradient of a Discrete Fuctional 86 4.1.5 A Modified Newton-Raphson Method for Numerical Minimization of a Fuctional 88 4.1.6 Constraints as Penalty Functions 90 4.1.7 Dynamic and Geometric Trajectory Optimization 92 4.2 A Robot Trajectory Planning Problem 93 4.2.1 Geometric Constraints 95 4.2.2 Reduction of Penetration 96 4.2.3 Optimization Criteria 97 4.2.4 Proposed Quadratic Cost Functional 97 4.2.5 The Discrete Form of the Problem 99 4.2.6 The Discrete Gradient 100 4.2.7 Iterative Reduction of the Cost Functional 102 4.2.8 Boundary Conditions 104 4.2.9 Feasibility of Local Optima 105 NUMERICAL OPTIMIZATION v 106 5.1 A Discrete Approximation of the Trajectory 106 5.1.1 S p e c i f i c a t i o n of a S t a r t i n g Trajectory ....106 5.1.2 Generation of the Discrete Trajectory by Int e r p o l a t i o n 107 5.2 An Algorithm f o r the I t e r a t i v e Reduction of the Cost Functional 108 5.2.1 L i n e a r i z a t i o n Error I l l 5.2.2 C a l c u l a t i o n of the I t e r a t i o n Step Size ....112 5.2.3 Optimal Spacing of the Discrete Manipulator Configurations 113 5.3 S a t i s f a c t i o n of Constraints 114 5.4 Finding a Gl o b a l l y Optimal Trajectory 114 6. NUMERICAL RESULTS 116 6.1 Test Cases 119 6.1.1 A Benchmark Case with M u l t i p l e Obstacles - Case 1 120 6.1.2 Varying the J o i n t Limits - Cases 2 and 3 ..126 6.1.3 Varying the Maximum Trajectory L i n e a r i z a t i o n Error - Cases 4 and 5 126 6.1.4 Varying the Maximum I t e r a t i o n L i n e a r i z a t i o n Error 132 6.1.5 A D i f f e r e n t S t a r t i n g Trajectory - Case 6 ..132 6.1.6 A D i f f e r e n t Benchmark Case with a Single Obstacle - Case 7 137 6.1.7 Varying the I t e r a t i o n Step Size - Cases 8 and 9 137 6.1.8 A Case with Mu l t i p l e Links - Case 10 148 v i 7. FURTHER WORK AND POTENTIAL APPLICATIONS , 150 7.1 Extension to 3-D 150 7.2 Optimization of Continuous Path Problems 150 7.3 Optimization of System Dynamics 152 7.4 Workcell Layout Optimization 152 7.5 Mobile Robot Path Planning 153 8. CONCLUSIONS 154 REFERENCES 156 APPENDICES I. MATRIX ALGEBRA 158 1.1 Definition of the Pseudoinverse 158 1.2 Solution of a Linear System of Equations 159 1.3 The Singular Value Decomposition 159 1.4 Singularities and Rank Deficiency 161 1.5 Calculation of the Jacobian 162 II. CALCULATION.OF THE CONVEX HULL ENCLOSING TWO POLYGONS 165 II.1 Implementation of the Algorithm 168 III. GEOMETRY OF THE 3-D IMPLEMENTATION 170 IV. SOFTWARE DOCUMENTATION 173 IV. 1 Users Guide to OPT 173 IV.1.1 Compiling and Linking OPT 173 v i i IV.1.2 Running OPT 173 IV.1.3 Input F i l e s 175 IV.1.4 Output F i l e s 181 IV.2 Functional Description of OPT 182 IV.2.1 Nesting of Subroutines 182 IV.2.2 Functional Descriptions of Subroutines ....183 IV.2.3 U t i l i t y Routines 187 IV.2.4 Documentation f o r UBC Singular Value Decomposition Routines 188 IV.3 Common Variables 197 IV.4 Generation of a S t a r t i n g T rajectory - STRAJ 201 IV.4.1 Users Guide to STRAJ 201 IV.4.2 Functional D e s c r i p t i o n of Subroutines 205 IV. 5 Graphical Output Routines 206 IV.6 FORTRAN Source L i s t i n g s 207 v i i i LIST OF FIGURES Diagram of manipulator and obstacles 11 Link geometry and frames: a) revolute j o i n t b) prismatic j o i n t 13 Base and gripper reference frames 15 P o s i t i o n error versus i t e r a t i o n f o r t e s t 1 29 L i n e a r i z a t i o n e r r o r € versus i t e r a t i o n i f o r i t e s t 1 31 I t e r a t i o n f a c t o r a and dev i a t i o n weight X i & i versus i t e r a t i o n i f o r t e s t 1 32 Components of the search vector versus i t e r a t i o n f o r t e s t 1 33 Plot of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r t e s t 1. 34 T r a j e c t o r i e s f o r d i f f e r e n t d e v i a t i o n factors 7: a) 7 - 0.0 b) 7 - 0.6 c) 7 - 1.0 35 T r a j e c t o r i e s f o r d i f f e r e n t t r a j e c t o r y l i n e a r i z a t i o n errors e max a) e - 0.5 b) e - 0.2 c) e - 0.1 ..37 max max max Plot of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r te s t 6 39 Plot of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r te s t 7. 40 P o s i t i o n e r r o r Ax versus i t e r a t i o n i f o r i t e s t 8 42 Components of the search vector versus i x i t e r a t i o n f o r t e s t 8 43 2.15 P l o t of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r t e s t 8 44 3.1 A d i s c r e t e approximation of the swept volume. ...48 3.2 Distance between convex h u l l s 51 3.3 Two i n t e r s e c t i n g polygons with multiple l o c a l maxima 54 3.4 Possible cases f o r the distance vector d 57 3.5 Geometric t e s t f o r i n c l u s i o n of a point i n a convex polygonal region 60 3.6 The shortest vector connecting two edges 63 3.7 Graphical proof that the penetration vector i s normal to a face 69 3.8 Translations of l i n k p o s i t i o n s due to penetration of the swept volume 73 3.9 Minimum norm j o i n t change reducing two penetrations 75 3.10 Minimum norm t r a n s l a t i o n f o r two penetration f r a c t i o n s a: a) i n i t i a l p o s i t i o n b) a — 0 c) a - 1 77 3.11 O s c i l l a t i o n between t r a n s l a t i o n d i r e c t i o n s 78 3.12 Large minimum norm j o i n t change reducing two small penetrations 79 6.1 S t a r t i n g t r a j e c t o r y and obstacles f o r case 1. . .121 6.2 Evolution of the d i s c r e t e t r a j e c t o r y f o r the f i r s t few i t e r a t i o n s f o r case 1: a) s t a r t i n g t r a j e c t o r y b) 1 i t e r a t i o n c) 2 i t e r a t i o n s x d) 3 i t e r a t i o n s 122 6.3 Pl o t of the number of d i s c r e t e t r a j e c t o r y p o s i t i o n s at each i t e r a t i o n f o r case 1 123 6.4 The optimized t r a j e c t o r y f o r case 1 124 6.5 Pl o t of the cost f u n c t i o n a l versus the number of i t e r a t i o n s f o r case 1 125 6.6 Pl o t of the j o i n t t r a j e c t o r i e s f o r case 1 127 6.7 Pl o t of the maximum penetration versus the number of i t e r a t i o n s f o r case 1 128 6.8 Pl o t of the j o i n t t r a j e c t o r i e s f o r case 2 129 6.9 Pl o t of the j o i n t t r a j e c t o r i e s f o r case 3 130 6.10 Comparison of the cost func t i o n a l s f o r case 1 and case 3 131 6.11 Optimized t r a j e c t o r i e s with d i f f e r e n t t r a j e c t o r y l i n e a r i z a t i o n e r r o r s : a) c - 0.01 b) e - 0.1 c) e - 0.2 133 6.12 Comparison of the cost func t i o n a l s f o r cases 1, 5 and 6 f o r d i f f e r e n t maximum t r a j e c t o r y l i n e a r i z a t i o n errors 134 6.13 S t a r t i n g t r a j e c t o r y f o r case 6 ...135 6.14 Optimized t r a j e c t o r y f o r case 6 136 6.15 S t a r t i n g t r a j e c t o r y and obstacles f o r Case 7. ..138 6.16 Optimized t r a j e c t o r y f o r case 7. . 139 6.17 Plot of the cost f u n c t i o n a l f o r case 7 140 6.18 Plot of the j o i n t t r a j e c t o r i e s f o r case 7 141 6.19 Plot of the maximum penetration versus the x i number of i t e r a t i o n s f o r case 7 142 6.20 Plot of the maximum j o i n t l i m i t v i o l a t i o n versus the number of i t e r a t i o n s f o r case 7 143 6.21 Optimized t r a j e c t o r i e s with d i f f e r e n t i t e r a t i o n step s i z e s : a) a - 0.1 b) a - 0.6 c) a - 1.0 144 6.22 Comparison of the cost functionals of cases 7, 8 and 9 f o r three d i f f e r e n t i t e r a t i o n step si z e s 145 6.23 Comparison of the maximum j o i n t l i m i t v i o l a t i o n versus the number of i t e r a t i o n s f o r three i t e r a t i o n step s i z e s 146 6.24 Comparison of the maximum penetration versus the number of i t e r a t i o n s f o r three i t e r a t i o n step s i z e s 147 6.25 Optimized t r a j e c t o r y f o r case 10 149 111.1 PUMA 560 l i n k geometry 171 111.2 Sketches of t e s t workpieces :..172 x i i NOMENCLATURE Symbols manipulator l i n k i length a a vector p a r a l l e l to the common normal of two l i n e s A set of a l l optimal t r a j e c t o r i e s A weighting matrix of j o i n t l i m i t penalty quadratic b() vector state equation, q - b ( q(t),u(t),t) B weighting matrix of interference penalty quadratic C weighting matrix of a c c e l e r a t i o n quadratic, c o r i o l i s and c e n t r i p e t a l tensor i n Section 4.1.7. d(n) minimum distance between two objects i n the d i r e c t i o n n * * * d minimum distance between two objects, d • d(n ) minimum distance along a face normal manipulator l i n k i o f f s e t d i t h t r a n s l a t i o n vector i d t r a n s l a t i o n vector applied to l i n k L due to i , J r r i penetration of l i n k L^  d penetration vector pen * d minimum distance vector A A T T T T d combined t r a n s l a t i o n vector, d - [d , d d ] 1 2 h D i t e r a t i o n matrix i e optimal edge of a polygon, equivalent to an optimal x i i i face f f o r a polyhedron optimal face of convex polyhedron K^ . The face normal n^ minimizes d(n). i f actuator force vector f actuator force l i m i t vector lim F coordinate frame attached to manipulator l i n k i g the number of penetration vectors d such that II d | > a|| dj| g() cost function g^Q j o i n t l i m i t penalty function g2() interference penalty function g () augmented cost function g £() actuator force l i m i t penalty function g () combined penalty function p g g r a v i t y vector G homogeneous p r o j e c t i o n matrix, G - I - J + J h t o t a l number of penetration vectors a c t i n g on a l i n k h^() c o n s t r a i n t r e l a t i o n s h i p s H t homogeneous transformation r e l a t i n g frames ,^ convex set representing the convex h u l l containing L and L k-l k H Hessian matrix I i d e n t i t y matrix x i v J Jacobian matrix A A J combined Jacobian J - [ J , J , . . . , J ] 1 2 h k state index k i n i t i a l state index o k f i n a l state index f k equivalent axis of r o t a t i o n convex set representing object i K d e v i a t i o n weighting matrix Lfc convex set representing a l i n k at p o s i t i o n k m number of task degrees of freedom, number of s p a t i a l dimensions M p o s i t i v e d e f i n i t e weighting matrix, i n e r t i a matrix i n Section 4.1.7. n number of manipulator degrees of freedom, number of obj ects n u n i t normal n,o,a gripper frame basis vectors N the set of face normals of two polyhedra K^ , p normal distance from a plane to the o r i g i n p t r a n s l a t i o n of gripper frame r e l a t i v e to base frame p point on L to be tr a n s l a t e d by d F i , j v i J i.J p penetration point defined by equation (3.17) pen p^ a tangent point P set of tangent points xv i component or q manipulator j o i n t state vector an inverse kinematic s o l u t i o n s a t i s f y i n g x - f(q) di s c r e t e t r a j e c t o r y vector, . T T T T T , T q " [ q k - i . V q k + 1 q k ' q k + i ' T ] 0 0 0 f £ mid-point of j o i n t range l i n e a r estimate of nearest c o l l i s i o n - f r e e j o i n t state minimum and maximum j o i n t l i m i t s an optimal t r a j e c t o r y minimizing $(q(t)) the g l o b a l l y optimal t r a j e c t o r y a vector representing a point i n space the tangent vertex of a polyhedron which minimizes d(n) gripper p o s i t i o n vector p r o j e c t i o n of r onto a face plane c gripper o r i e n t a t i o n vector p r o j e c t i o n of a point onto a l i n e a ctual r o t a t i o n matrix r e l a t i v e to a reference frame desired r o t a t i o n matrix r e l a t i v e to a reference frame graph node i endpoints of an edge i set of graph nodes w^  x v i t i n i t i a l time o t f i n a l time f T homogeneous transformation matrix r e l a t i n g the gripper frame to the base frame u a c o n t r o l vector a vector p a r a l l e l to an edge v shortest connecting vector between an edge of face f and an edge containing r c V set of v e r t i c e s d e f i n i n g a convex polyhedron or polygon, set of possible shortest connecting vectors v x task state vector s p e c i f y i n g the gripper p o s i t i o n x a desired gripper p o s i t i o n (task state) z an a r b i t r a r y vector a i t e r a t i o n step s i z e l i n k i twist angle, i t e r a t i o n i step s i z e 7 deviation f a c t o r 6 p o s i t i o n error tolerance optimization tolerance e l i n e a r i z a t i o n error 8 angle of r o t a t i o n X d e v i a t i o n weighting f a c t o r u s c a l a r parameter H(n,p) a plane defined as II(n,p) - { r : r » n - p - 0 } x v i i r d i s c r e t e time i n t e r v a l a penetration f r a c t i o n <f> cost function <f>^ a cost function evaluated at graph node w^  $ cost f u n c t i o n a l * augmented cost f u n c t i o n a l a * penalty f u n c t i o n a l p « angular v e l o c i t y 0 (t) a set representing object i at time t Scalar and Vector Notation E,e symbols i n standard type r e f e r to s c a l a r q u a n t i t i e s e lower case symbols i n bold face r e f e r to vectors E upper case symbols i n bold face r e f e r to matrices E + pseudoinverse of E E T matrix transpose of E E 1 matrix inverse of E || e || E u c l i d i a n norm of e | E | determinant of E x v i i i Subscripts and Superscripts i t e r a t i o n index, object index d i s c r e t e state index an optimal value of the v a r i a b l e a maximum bound on a v a r i a b l e a minimum bound on a v a r i a b l e x i x ACKNOWLEDGEMENT I would l i k e to thank my supervisor, Professor Dale Cherchas, f o r h i s advice and encouragement throughout my Ph.D. work. Some funding was provided f o r t h i s work through the Canadian Natural Sciences and Engineering Research Council (NSERC) grant number A-4682. xx 1. INTRODUCTION 1.1 Background and Motivation In a competitive world economy, i t i s e s s e n t i a l that industry employ the most e f f e c t i v e methods a v a i l a b l e i n order to survive and f l o u r i s h . In every industry, modern technology provides opportunities f o r increased c a p a b l i t i e s and improved e f f i c i e n c y . In the manufacturing sector, many diverse technologies are being s u c c e s s f u l l y applied. Computer-Aided-Design (CAD) and Computer-Aided-Manufacturing (CAM) are being i n c r e a s i n g l y u t i l i z e d . F l e x i b l e , computer-controlled workstations allow automation of tasks not s u i t a b l e f o r s i n g l e purpose, "hard" automation. I n d u s t r i a l robots play an important r o l e i n modern manufacturing. Robots can be programmed to perform coordinated motions r e p e t i t i v e l y and accurately, and can be applied to many tasks previously done by humans. Because of t h e i r f l e x i b i l i t y , robots can be used on a stand-alone b a s i s , performing some well-defined, r e p e t i t i v e task. The robot i s programmed by p h y s i c a l l y leading i t through i t s desired actions, which are stored and then replayed to perform the required task. Applications of t h i s type account for most of the robots c u r r e n t l y i n use. I t i s now possible and desirable to integrate a l l aspects of the manufacturing process into a u n i f i e d , computer-controlled system. Such a system combines design, engineering analysis, 1 process planning, and execution and con t r o l of the manufacturing process. Most of the required technology e x i s t s , but some problems s t i l l require s a t i s f a c t o r y s o l u t i o n s . An important problem i s the o f f - l i n e programming of robots. Manual programming i s s a t i s f a c t o r y f o r many tasks, but s u f f e r s a number of drawbacks. F i r s t , the robot and i t s workstation must be withdrawn from production each time reprogramming i s required. The programming procedure may be lengthy, so robots are not e f f e c t i v e l y used f o r tasks which are changed frequently. There i s also danger of damage and i n j u r y i f errors are made during programming. F i n a l l y , manually programming does not u t i l i z e the robots a b i l i t y to accurately follow a p r e - s p e c i f i e d t r a j e c t o r y , e s s e n t i a l f o r the f u l l i n t e g r a t i o n of manufacturing under computer c o n t r o l . O f f - l i n e programming and simulation of robot workstations provides many d i s t i n c t advantages. F i r s t , information about the workstation and task can be drawn d i r e c t l y from e x i s t i n g CAD data bases, and used f o r generation of appropriate motions. Powerful mathematical techniques can be applied to optimize the r e s u l t i n g program. The program can be simulated and assessed g r a p h i c a l l y without danger. F i n a l l y , the robot can be programmed and c o n t r o l l e d i n concert with the r e s t of the integrated system, not as a stand-alone u n i t . One aspect of o f f - l i n e programming i s the problem of path or t r a j e c t o r y planning. A robot i s a complicated kinematic linkage governed by h i g h l y non-linear kinematic and dynamic equations of motion. In a r e a l i s t i c workstation, i t s motion Is r e s t r i c t e d by a number of constraints in c l u d i n g j o i n t l i m i t s , actuator force 2 l i m i t s and p h y s i c a l interference between the robot and sta t i o n a r y obstacles. The problem can be stated as follows: Given a s p e c i f i e d task, generate a robot t r a j e c t o r y which performs the task without v i o l a t i n g p h y s i c a l c o n s t r a i n t s . In addition, other performance c r i t e r i a may be optimized. 1.2 A Summary of Previous Work i n Traj ectorv Planning Robot t r a j e c t o r y planning i s r e c e i v i n g increasing a t t e n t i o n from researchers as i t s p r a c t i c a l importance becomes c l e a r . The approaches taken to date can be broadly categorized as follows: a) Configuration space methods b) Local optimization methods c) P o t e n t i a l f i e l d methods d) Optimal c o n t r o l methods. 1.2.1 Configuration Space Methods The con f i g u r a t i o n space approach was developed as a s o l u t i o n to the problem of f i n d i n g the shortest c o l l i s i o n - f r e e path to move an object through a f i e l d of obstacles. With t h i s approach, a global map of c o l l i s i o n - f r e e robot configurations (the free-space) i s generated. The problem i s then 3 reduced to finding a path through this resultant free-space. Many permutations of this method have been developed, but they a l l share some common d i f f i c u l t i e s . Arbitrary orientations of an object are not easi ly handled since the resulting free-space changes as a function of the orientation. Each additional degree of freedom in the motion of the object adds another dimension to the free space. Also, a different free-space exists for each dis t inct object. Thus, the effort required for expl ic i t calculation of the complete free-space for a general robot and workstation becomes prohibit ive, and methods for simplifying the problem must be sought. Lozano-Perez [1983] considers the problem of moving a polyhedral object among polyhedral obstacles. The configuration-space obstacles (or forbidden regions) are generated by shrinking the object to a point and correspondingly growing the obstacles. Higher dimensional configuration space obstacles are represented approximately by lower dimensional projections. Rotations are discretized and the extra dimension is approximated by a small number of projections. The intermediate positions are represented by an approximation of the swept volume between specified positions. The shortest co l l i s ion- free path is found by connecting vertices of the configuration-space obstacles. Brooks [1983] uses a variat ion of the configuration space method to solve a l imited class of pick-and-place problems for a revolute manipulator. Brooks simplifies the problem by specifying motions as pure translations followed by pure rotations, and he asserts that a single rotational degree of freedom is suff icient for many tasks. The robot arm is decomposed into i t s constituent 4 l i n k s , and the interference constraints are developed geometrically f o r each l i n k . Luh and Campbell [1984] consider the problem of a robot with a prismatic j o i n t , and propose the i n c l u s i o n of "pseudo-obstacles" to account f o r possible interference with the long boom of the robot. Park [1984] proposes to generate the free-space by exhaustive enumeration of a l l c o l l i s i o n - f r e e configurations. J o i n t space i s d i s c r e t i s e d and each d i s c r e t e configuration i s tested f o r c o l l i s i o n . The number of d i s c r e t e points increases exponentially with the number of j o i n t s , so an enormous number of c o l l i s i o n t e s t s must be performed f o r r e a l i s t i c s i t u a t i o n s . The r e s u l t i n g map of c o l l i s i o n - f r e e configurations requires a great deal of computer memory, even when data compression techniques are employed. The advantages of t h i s approach are i t s s i m p l i c i t y and ge n e r a l i t y , and the ease of working with the r e s u l t i n g joint-space representation of the c o l l i s i o n map. Configuration-space methods are purely geometrical and do not account f o r the kinematics and dynamics of robots. Thus, these methods cannot be n a t u r a l l y extended to deal with constraints such as j o i n t displacement l i m i t s and actuator force l i m i t s . 1.2.2 Local Optimization Methods Another approach to path planning has evolved out of the study of robot kinematics. The general inverse kinematic s o l u t i o n s f o r m u l t i - l i n k manipulators have been developed 5 extensively [ F o u r n i e r . K h a l i l 1977], [Konstantinov,Markov,Nechev 1981], [Konstantinov et a l . 1982]. Manipulators with redundant degrees of freedom allow the j o i n t s to be reconfigured to avoid constraints while tracking a s p e c i f i e d t r a j e c t o r y . The constraints can be j o i n t l i m i t s , v e l o c i t i e s , accelerations or interference with obstacles. The t r a j e c t o r y i n j o i n t space evolves by c a l c u l a t i n g the constraints instantaneously at each point along the t r a j e c t o r y , and moving the j o i n t s away from the constraints while following the s p e c i f i e d end-effector path. The method does not generate a g l o b a l l y optimal s o l u t i o n since i t i s based on instantaneous optimization. The r e s u l t i n g t r a j e c t o r y may be trapped i n a cul-de-sac, or may be very c i r c u i t o u s . Konstantinov et a l . [1981] have formulated an inverse kinematic s o l u t i o n which maximizes j o i n t a v a i l a b i l i t y f o r redundant robots. Vukobratovic and K i r c a n s k i [1982] consider the dynamics of the robot and propose a s o l u t i o n to minimize the energy consumption of the actuators over a s p e c i f i e d t r a j e c t o r y . Yoshikawa [1984] defines a measure of robot m a n i p u l a b i l i t y , and formulates a s o l u t i o n to maximize t h i s quantity. Interference avoidance i s also incorporated by s p e c i f y i n g an i n t e r f e r e n c e - f r e e manipulator p o s i t i o n . The redundancy i n the manipulator i s used to minimize the d i f f e r e n c e between the actual and desired p o s i t i o n . This approach does not guarantee that a c o l l i s i o n - f r e e t r a j e c t o r y w i l l be found even i f one e x i s t s . K i r c a n s k i and Vukobratovic [1984] have considered the obstacle avoidance problem by c a l c u l a t i n g distances between l i n k s and obstacles. A s o l u t i o n can be formulated to maximize the distance from obstacles while following a s p e c i f i e d path. 6 Maciejewski and K l e i n [1985] present a more d e t a i l e d treatment of the same approach. 1.2.3 P o t e n t i a l F i e l d Methods The p o t e n t i a l f i e l d method associates a force or p o t e n t i a l with each obstacle to be avoided. This " a r t i f i c i a l p o t e n t i a l " i s included i n the equations of motion, and has the e f f e c t of f o r c i n g the j o i n t s away from p o t e n t i a l c o l l i s i o n s . The approach proposed by Khatib and Le Matre [1978] i s summarized i n t h e i r paper: The manipulator moves i n a f i e l d of forces. The f i n a l p o s i t i o n to be reached i s an a t t r a c t i v e pole f o r the terminal device, and the obstacles are repulsive surfaces f o r a l l the manipulator parts. Ozaki and Mohri [1986] have r e f i n e d t h i s basic method s u b s t a n t i a l l y . They define a r t i f i c i a l p o t e n t i a l s associated with the d e v i a t i o n from the desired path, v i o l a t i o n of j o i n t l i m i t c o n s t r a i n t s , and interference. The d e v i a t i o n of the actual p o s i t i o n from the s p e c i f i e d p o s i t i o n i s defined as an objective function to be minimized. Linear i n e q u a l i t y constraints are formulated f o r actuator l i m i t s , j o i n t displacement l i m i t s , j o i n t v e l o c i t y l i m i t s and interferences. The problem i s then solved by l i n e a r programming at d i s c r e t e i n t e r v a l s along the proposed t r a j e c t o r y , and a f e a s i b l e t r a j e c t o r y i s thus obtained. I f there i s no f e a s i b l e t r a j e c t o r y which s a t i s f i e s a l l of the constraints •7 while following the s p e c i f i e d t r a j e c t o r y , the nearest f e a s i b l e t r a j e c t o r y i s obtained. This method does not f i n d an optimal f e a s i b l e t r a j e c t o r y . 1.2.4 Optimal Control Methods G i l b e r t and Johnson [1985] have proposed a method of path planning based on optimal c o n t r o l . A distance function i s developed to represent the distance between convex objects, and i t s d e r i v a t i v e i s defined. The distance i s incorporated into the cost f u n c t i o n a l as an i n t e r i o r penalty function, and the problem i s solved using numerical methods f o r optimal c o n t r o l . Because i n t e r i o r penalty functions are used, the s t a r t i n g t r a j e c t o r y must be f e a s i b l e , which means i t must be c o l l i s i o n - f r e e . Also, no algorithms are presented f o r a c t u a l l y c a l c u l a t i n g the distance between objects. 1.3 Contributions of t h i s Thesis This thesis proposes a method for f i n d i n g an optimal geometric robot t r a j e c t o r y subject to the constraints of j o i n t displacement l i m i t s and interference avoidance. A geometric method f o r c a l c u l a t i n g the distance between objects modelled as convex polyhedra i s presented. Point-to-point t r a j e c t o r y planning i s posed as a two-point boundary value problem i n the calculus of v a r i a t i o n s . The kinematic constraints are formulated as e x t e r i o r 8 penalty functions and are combined with other optimization c r i t e r i a to form a cost f u n c t i o n a l . The problem i s solved by d i s c r e t i z i n g the problem and numerically minimizing the cost f u n c t i o n a l by using a steepest-descent approach to i t e r a t i v e l y improve the t r a j e c t o r y . Any s t a r t i n g t r a j e c t o r y i s acceptable since the use of e x t e r i o r penalty functions means that a l l configurations are defined. However, d i f f e r e n t s t a r t i n g t r a j e c t o r i e s may converge to d i f f e r e n t l o c a l l y optimal f i n a l t r a j e c t o r i e s . In general, several s t a r t i n g t r a j e c t o r i e s are t r i e d u n t i l a r e s u l t i n g f i n a l t r a j e c t o r y i s found which s a t i s f i e s the c o n s t r a i n t s . 9 2. KINEMATICS 2.1 The Gripper P o s i t i o n as a Function of the J o i n t Variables Let us define a manipulator c o n s i s t i n g of n l i n k s connected by n j o i n t s . We say that the manipulator has n degrees of freedom. Let us also define a task defined by m coordinates which s p e c i f y the p o s i t i o n and o r i e n t a t i o n of the end e f f e c t o r or gripper of the robot. We w i l l say that there are m task degrees of freedom. We w i l l represent the j o i n t state by the vector q - [q.. q. 9 ] T , (2.1) i *, n and the task state by the vector x - [x , x x ] T . (2.2) 1 Z m Given the j o i n t state q we can always f i n d the task state x using the forward kinematic equation, x - f ( q ) , (2.3) where f"(q) , u s u a l l y a highly non-linear function, can be found f o r any r i g i d l i n k manipulator. Figure 2.1 shows a simple manipulator with four j o i n t degrees of freedom and two task degrees of freedom. 10 Figure 2.1 Diagram of manipulator and obstacles. 11 2.2 The Denavit-Hartenberg Transformation A transformation r e l a t i n g the coordinate frames F F ° i - i , i attached to adjacent l i n k s i - 1 , i of a linkage can be s p e c i f i e d i n a standard form known as the Denavit-Hartenberg transformation [Paul, 1981, pp.50-55], H i - l . i cos q s i n qi 0 0 •sin q cos a cos q cos a i s i n a s i n q s i n a a cos q ^1 i i U -cos q s i n a a s i n q i i cos a d (2.4) where cj - j o i n t v a r i a b l e , a - l i n k twist, a - l i n k length, d - l i n k o f f s e t . The l i n k geometry parameters and l i n k coordinate frames are i l l u s t r a t e d i n Figure 2.2, with q represented by 6 . 2.3 The Forward Kinematic Equation The net transformation r e l a t i n g the l a s t frame F to the n 12 Figure 2.2 Link geometry and frames: a) revolute j o i n t b) prismatic j o i n t . 13 f i r s t frame F (Figure 2.3) can be found as a product of the Denavit-Hartenberg transformations for each link pair [Buchal, 1984, pp.17-22], [Buchal et a l . , 1986, pp.16-20], [Paul, 1981, pp.41-42], H - H H . . . H l,n 1,2 2,3 n-1,n (2.5) The net transformation can also be expressed as a homogeneous transformation, T -n o a .p o o o : i (2.6) where the vectors n, o, a are the basis vectors of the gripper frame, and the vector p is the location of the gripper frame relative to the base reference frame. Thus, T specifies the task state of the robot. Equivalently, the vectors n, o, a compose the columns of a rotation matrix, and p represents a translation. We can now express the forward kinematic equation as T - H (q). (2.7) l ,n 2.4 Specification of the Task Coordinate Vector The robot task vector in 3-D task space can be defined as x - [ r T , r T ] T , (2.8) P o 14 15 where r i s a 3 x 1 p o s i t i o n vector and r i s a 3 x 1 o r i e n t a t i o n p o vector. The p o s i t i o n vector i s given d i r e c t l y by the t r a n s l a t i o n a l part of the homogeneous transformation. The o r i e n t a t i o n vector must be derived from the 3 x 3 r o t a t i o n component of the transformation matrix. The r o t a t i o n i s commonly represented by Euler angles, roll-pitch-yaw angles [Paul, 1981, pp.43-47], or an equivalent axis of r o t a t i o n [Paul, 1981, pp.29-32]. 2.4.1 A. L i n e a r i z e d Representation of Rotational Error Suppose the desired o r i e n t a t i o n of the robot gripper i s expressed as a 3 x 3 r o t a t i o n matrix Rd r e l a t i v e to a reference frame, and the current o r i e n t a t i o n i s given by a matrix R . We c can define a r o t a t i o n a l transformation from the current o r i e n t a t i o n to the desired o r i e n t a t i o n as R - rot(k.fl) R , (2.9) d c where rot(k,0) i s a r o t a t i o n of angle 6 about an axis k. Rearranging, we get rot(k,0) - RR" 1 . (2.10) d c We can solve f o r k, 9 by the method described i n [Paul, 1981, pp.25-32]. The o r i e n t a t i o n error vector we w i l l use i s 0k. Note 16 that the time d e r i v a t i v e of t h i s o r i e n t a t i o n vector i s the angular v e l o c i t y , u - 0k. Thus, t h i s choice of o r i e n t a t i o n vector allows the use of a Jacobian matrix derived f o r l i n e a r and angular v e l o c i t i e s . 2.5 Inverse Kinematics The inverse kinematics problem consists of f i n d i n g the j o i n t state corresponding to a given task state, q - g (x ) . (2.11) There are three po s s i b l e cases: 1. The system i s overconstrained and no solutions e x i s t . 2. There are a f i n i t e number of so l u t i o n s . 3. The robot has redundant degrees of freedom and an i n f i n i t e number of solutions e x i s t . 2.5.1 E x p l i c i t solutions I t has been shown [Paul, 1981, pp.65-83] that e x p l i c i t inverse solutions can be found f o r many p a r t i c u l a r non-redundant 17 (n - m) manipulators of p r a c t i c a l i n t e r e s t . These methods do not apply to general non-redundant or redundant manipulators. 2.5.2 A L i n e a r i z e d General Solution No closed-form inverse s o l u t i o n can be found f o r the general case. For incremental changes, however, a l i n e a r i z e d general s o l u t i o n can be defined as i x - J Sq (2.12) where J i s the Jacobian matrix. Equivalently, x - J q . (2.13) The three p o s s i b l e cases can now be defined as follows: 1. No inverse solutions e x i s t i f rank J * rank [JJ5x]. This i s u s u a l l y the case f o r overconstrained systems with m > n, or at singular configurations of non-redundant systems. 2. A f i n i t e number of inverse solutions e x i s t i f m - n - rank J. This i s the usual case f o r non-redundant systems. 3. An i n f i n i t e number of inverse solutions e x i s t i f m < n 18 and rank J - m or rank J - rank [Jio'x] < m. This i s the case f o r redundant systems. 2.6 Inverse Kinematic Solutions for Redundant Systems I t i s well known [Albert, 1972, pp.30-31] that a general inverse s o l u t i o n can be formulated as Sq - J + 6 x + (I-J +J)z (2.14) where J + i s the pseudo-inverse of J, (I-J +J) i s the p r o j e c t i o n onto the null-space of J and z i s an a r b i t r a r y vector (see Appendix 1.1.2). The forward s o l u t i o n Sx - J Sq i s i n v a r i a n t with respect to z. The term J +5x i s the minimum-norm p a r t i c u l a r s o l u t i o n , and the term (I-J +J)z i s the homogeneous s o l u t i o n . Two s p e c i f i c cases are worth noting: 1. For over-constrained systems, the s o l u t i o n Sq - J +5x (2.15) gives the b e s t - f i t (least-squares) s o l u t i o n minimizing 5x - J5q ||. (2.16) 2. For non-redundant systems (m - n), i f J i s nonsingular then J + - J " 1 . 19 2.6.1 Finding an Optimal Inverse Solution For systems with redundancy, i t i s p o s s i b l e to optimize the incremental s o l u t i o n 6q with respect to selected c r i t e r i a by choosing an appropriate z. The vector z can be chosen to maximize j o i n t a v a i l a b i l i t y [Konstantinov, Markov and Nechev, 1981], maximize j o i n t m a n i p u l a b i l i t y [Yoshikawa, 1984], and avoid c o l l i s i o n s [Kircanski and Vukobratovic, 1984], [Maciejewski and K l e i n , 1985]. This optimization i s s t r i c t l y instantaneous, and global c r i t e r i a such as elapsed time or t o t a l energy cannot be optimized i n t h i s way. The instantaneously optimal t r a j e c t o r y depends on the s t a r t i n g configuration, and the t r a j e c t o r y which i s g l o b a l l y optimal may not be instantaneously optimal. An absolute inverse s o l u t i o n q can be obtained by i t e r a t i v e l y applying the incremental s o l u t i o n u n t i l a desired task state x i s reached. One such I t e r a t i v e algorithm i s presented i n [Goldenberg, Benhabib, and Fenton, 1985]. 2.7 An I t e r a t i v e Numerical Solution The absolute inverse kinematics problem can be stated as follows: For a desired task vector x, f i n d a corresponding j o i n t vector q s a t i s f y i n g the forward kinematic equation, 20 x- f(q) (2.17) Note that f o r redundant systems, there may be i n f i n i t e l y many solu t i o n s q. Let q s p e c i f y a manipulator configuration and x - f ( q ) . Define the p o s i t i o n error Ax - x - x , (2.18) and the j o i n t e r r o r Aq - q - q . (2.19) The inverse kinematics problem can be posed as a minimization problem, where the function <j> to be minimized i s a quadratic of the p o s i t i o n error, <f> - (x - x ) T ( x - x) . (2.20) Equivalently, we can minimize a quadratic of the j o i n t error, * - (q - q ) T ( q - q) . (2.21) This function can be minimized by the method of steepest descent using an i t e r a t i v e sequence of the form q - q + a Aq , (2.22) ^i+i n i i n i 21 where a Aq i s known as the search vector. Let Aq be the s o l u t i o n of the redundant problem, Aq - J + A x + ( I - J + J ) z , (2.23) n i i i i i i v / where J + i s J + evaluated at q , Ax - x - x , and z i s chosen to i n i i i i optimize some set of c r i t e r i a . For a s u f f i c i e n t l y small step s i z e a^, the end e f f e c t o r w i l l move incrementally along a s t r a i g h t l i n e , i n the d i r e c t i o n Ax, from i t s i n i t i a l p o s i t i o n to the goal p o s i t i o n . There i s no ph y s i c a l reason to const r a i n end e f f e c t o r motion to a s t r a i g h t l i n e . A les s constrained s o l u t i o n i s Aq - J + A x + ( I - J + J )z + K z , (2.24) ^ i i i i i i i i ' where i s some p o s i t i v e d e f i n i t e weighting matrix. Using t h i s s o l u t i o n , the end e f f e c t o r moves l o c a l l y i n the d i r e c t i o n Ax^ + J K^z , and any end e f f e c t o r path i s possibl e , depending on the choice of z . i The expanded i t e r a t i v e sequence i s q - q + a (J +Ax + ( I - J + J )z + K z ) . (2.25) ^i+l n i i i i i i i . i i The i t e r a t i o n f a c t o r G [0,1] can be chosen to optimize convergence. When the error Aq^ i s large, a small produces a l i n e a r , steepest descent convergence. When the erro r becomes small, can approach 1 to give a quadratic convergence. In general, the step s i z e i s adjusted so that the l i n e a r i z a t i o n error 22 stays w i t h i n some predetermined bounds. We can insure that the sequence converges to an inverse s o l u t i o n q by choosing so that x- x II < A II x- x II , A < 1, (2.26) i+i" 11 i " x ' where x - f (q ). We can show that i+i x - x + A x + J K z , (2.27) i+l i i i i i and x - x - x - ( x + A x + J K z ) . (2.28) i+l i i i i i Combining (2.27) and (2.28) and c a n c e l l i n g terms, we get x - x - - J K z (2.29) i+l i i i Cl e a r l y , we can combine (2.18) and (2.29) to show that £ - xi+i|| < A.|| x - x j | , A < 1, (2.30) i f || J K t « i || < A 1 Ax t || , A < 1. (2.31) The redundancy of the system can be exploited to f i n d a s o l u t i o n q which optimizes some set of c r i t e r i a . We w i l l choose 23 to maximize the range of a v a i l a b l e motion at each j o i n t so that the s o l u t i o n w i l l be as f a r from the j o i n t l i m i t s as p o s s i b l e . I f we define the midpoint of the j o i n t range as q°, then we wish to minimize <t>x - (q - q°) TM <q - q°> (2.32) where M i s some p o s i t i v e - d e f i n i t e weighting matrix. The c r i t e r i a of minimum j o i n t e r r o r (2.21) and maximum j o i n t a v a i l a b i l i t y (2.32) can be combined to get a new cost function 4>^, 4>2 - (q - q ) T ( q - q) + (q - q°) TM (q - q°> • (2.33) An optimal inverse s o l u t i o n can be found by minimizing (2.33). The j o i n t e r r o r term i s reduced by Aq - J +Ax and the a v a i l a b i l i t y term i s reduced along the gradient, z - -M(q - q°). Combining these terms, an i t e r a t i v e sequence which minimizes (2.33) i s q - q + a [ J +Ax - ( I - J + J )M(q -q°) - K M(q -q°) ") (2.34) As the er r o r Ax^ i s reduced, the minimum norm component <JiAxi decreases. The deviation component K^MCq^-q0) also decreases as || K || i s reduced (2.31). As the s o l u t i o n i s optimized, the homogeneous component ( I - J + J )M(q -q°) decreases. 24 2.7.1 L i n e a r i z a t i o n Error L i n e a r i z a t i o n e r r o r a r i s e s due to the use of a l i n e a r approximation f o r a nonlinear function. The l i n e a r i z a t i o n e r r o r e can be defined as the di f f e r e n c e between the actual and predicted value of the next task state, c - II x - (x + a (x-x )) I , (2.35) i " i+i i l i " where x - f(q ) . This can be rewritten as i+i n+i « - || (1-a ) Ax - Ax I , (2.36) i 11 N i i i+l" where Ax - x -x R e c a l l that i i *i+l q + a Aq i i n i (2.22) so as a -> 0, i q -» q , x -> x and e -> 0. ^i+l i i+l i Thus we see that the l i n e a r i z a t i o n error can be reduced by reducing the step s i z e f a c t o r o^. In p r a c t i c e , we w i l l choose a to keep the l i n e a r i z a t i o n error below a s p e c i f i e d l i m i t , e max 25 2.7.2 An I t e r a t i v e Algorithm We can s i m p l i f y the i t e r a t i o n sequence (2.34) with the following r e s t r i c t i o n s : M - I K i s a diagonal matrix with a l l elements equal to A. The j o i n t center vector q i s the zero vector. The s i m p l i f i e d i t e r a t i o n sequence i s q - q + a (J +Ax - ( I - J + J )q - A q ) . (2.37) ^ i + l i i i i i n i i n i This i t e r a t i v e inverse s o l u t i o n can be implemented using the following algorithm: Specify the goal p o s i t i o n x and the i n i t i a l c o n f i g u r a t i o n V Specify the p o s i t i o n tolerance 6 , the optimization tolerance S , the allowable l i n e a r i z a t i o n e r r o r c , and the d e v i a t i o n 2 max f a c t o r 7 e [ 0 , 1 ] . Calculate the i n i t i a l parameters, v f ( V • 26 A A Ax - x -x , o o e - Ax , 0 " 011 o - min{ e 0 max ' 0 AX J q0 l i-0, UNTIL (|| x - x j | < ^ and a j | ( I - J ^ J ^ q J I < 6J DO q - q + a (J +(Ax ) - ( I - J + J )q - A q ) , n+1 n i i i i i i n i i * i II Ax I * 1 ~ i+l e - (1-a ) Ax - Ax i+l " l i i+l" i+l _ i max' i+l i - i + l , END UNTIL This algorithm i s implemented i n the FORTRAN subroutine INVERS. A source l i s t i n g i s contained i n Appendix IV.6. 27 2.7.3 Numerical Results Some numerical r e s u l t s were obtained f o r several i n i t i a l configurations q Q and goal p o s i t i o n s x. A 2-D, four l i n k manipulator with revolute j o i n t s and l i n k s of u n i t length was modelled. a) Test 1 For the f i r s t t e s t , the s p e c i f i c a t i o n s were the following: q Q- [ 90, 10, 10, 10 ] T degrees x- [-1, -1 ] T (these are the x and y t r a n s l a t i o n a l components of the 2 degrees of freedom case) Maximum l i n e a r i z a t i o n error « - 0.2 max Deviation f a c t o r 7 - 0 . 5 P o s i t i o n tolerance S - .01 l Optimization tolerance 6 — .01 In t h i s case, the algorithm converges to a s o l u t i o n i n 25 i t e r a t i o n s . Figure 2.4 shows the reduction of the p o s i t i o n error versus the number of i t e r a t i o n s . The convergence i s i n i t i a l l y 28 5 2 4 6 8 10 12 14 16 18 20 22 24 26 ITERATIONS Figure 2.4 P o s i t i o n e r r o r versus i t e r a t i o n f o r t e s t 1. 29 l i n e a r , becoming quadratic as the error i s reduced. Figure 2.5 shows the l i n e a r i z a t i o n error versus i t e r a t i o n number. Note that the l i n e a r i z a t i o n error remains below the s p e c i f i e d maximum of e - 0.2. max Figure 2.6 shows the evolution of the i t e r a t i o n f a c t o r a and the d e v i a t i o n weight A as the s o l u t i o n converges. The i t e r a t i o n f a c t o r a i s i n i t i a l l y small to give a l i n e a r steepest descent convergence, but increases toward 1 to give a more r a p i d quadratic convergence near the goal p o s i t i o n . The dev i a t i o n weight A i s i n i t i a l l y large to allow the gripper t r a j e c t o r y to deviate from the minimum norm s t r a i g h t l i n e . The dev i a t i o n weight A i s reduced to zero as the gripper nears i t s goal p o s i t i o n . Figure 2.7 shows the r e l a t i v e magnitudes of the components of the search vector. The minimum norm component J +Ax and the devi a t i o n component Aq contribute most to the search vector, with r e l a t i v e l y l i t t l e c o n t r i b u t i o n from the homogeneous component, ( I - J + J ) q . Figure 2.8 shows the i t e r a t e d t r a j e c t o r y . b) Test 2 Same as Test 1, with 7 - 0. With the deviation f a c t o r 7 set to zero, the s o l u t i o n i t e r a t e s along the minimum norm path without deviation. The gripper moves i n roughly a s t r a i g h t l i n e from i t s i n i t i a l p o s i t i o n to the goal p o s i t i o n , as shown i n Figure 2.9 a). The path i s not 30 2 4 6 8 10 12 14 16 18 20 22 24 26 ITERATIONS Figure 2.5 L i n e a r i z a t i o n e r r o r versus i t e r a t i o n i f o r te s t 1. 31 I 0.4 0.2 / ALPHA / LAHBDA / i f i / I * / * / * / * / * / * • / % / it y % y * / _^*^ \ ^^ "*^  % 1 % 1 K % \ / %^ \ / . . 2 4 6 8 10 12 14 16 18 20 22 24 26 ITERATIONS Figure 2.6 I t e r a t i o n f a c t o r and deviation weight A^ versus i t e r a t i o n i f o r te s t 1. 32 2 4 6 8 10 12 14 16 18 20 22 24 ITERATIONS Figure 2.7 Components of the search vector versus i t e r a t i o n for t e s t 1. 33 Figure 2.8 Plot of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r t e s t 1. 34 35 exactly s t r a i g h t due to the l i n e a r i t y error. c) Test 3 Same as t e s t 1, with 7 - 1.0 In t h i s case, the dev i a t i o n f a c t o r i s made as large as possi b l e while s t i l l ensuring convergence. Note that the gripper path i s now quite curved, and that the j o i n t s follow a more optimal t r a j e c t o r y (Figure 2.9 c ) ) . d) Test 4 Same as t e s t 1, with e - 0.5 . The r e s u l t i n g t r a j e c t o r y i s max shown i n Figure 2.10 a). e) Test 5 Same as t e s t 1, with e - 0.1 . The r e s u l t i n g t r a j e c t o r y i s max shown i n Figure 2.10 c ) . Figure 2.10 shows the reduction i n the i t e r a t i o n step s i z e as the maximum l i n e a r i z a t i o n error i s reduced. A smaller l i n e a r i z a t i o n e r r o r r e s u l t s i n a greater number of smaller, more accurate i t e r a t i o n steps being taken. The maximum l i n e a r i z a t i o n 36 e r r o r should be chosen to optimize the trade o f f between the number of i t e r a t i o n s and the accuracy of the l i n e a r i z a t i o n . f ) Test 6 Same as Test 1, with 3 degrees of freedom, x - [ -1. -1, 90 ] In t h i s case, the f i n a l l i n k o r i e n t a t i o n i s 90 degrees from h o r i z o n t a l . This s o l u t i o n was found a f t e r 22 i t e r a t i o n s . The i t e r a t e d t r a j e c t o r y i s shown i n Figure 2.11. g) Test 7 Same as t e s t 1, with a d i f f e r e n t s t a r t i n g configuration, q Q - [ 10, 10, 10, 10 ] T degrees In t h i s case, a d i f f e r e n t l o c a l l y optimal inverse s o l u t i o n was found a f t e r 27 i t e r a t i o n s , as shown i n Figure 2.12. h) Test 8 Same as t e s t 1, with a d i f f e r e n t s t a r t i n g c o n f i g u r a t i o n and 38 Figure 2.11 P l o t of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r t e s t 6. 39 1 ITERATED INVERSE SOLUTION Figure 2.12 Plot of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r t e s t 7. 40 goal p o s i t i o n , q Q- [ 0, 0, 180, 180 ] T degrees x- [2, 0 ] T (2 t r a n s l a t i o n a l degrees of freedom) This example i l l u s t r a t e s the behavior of the algorithm when the i n i t i a l gripper p o s i t i o n i s at or near the goal p o s i t i o n , but the j o i n t c o n f i g u r a t i o n i s sub-optimal. The i n i t i a l c o n f i g u r a t i o n has the manipulator l i n k s folded upon themselves, r e s u l t i n g i n large j o i n t angles. The robot j o i n t angles are i t e r a t i v e l y modified using the homogeneous part of the inverse s o l u t i o n to reduce the norm of the j o i n t angles without changing the gripper p o s i t i o n . Figure 2.13 shows the p o s i t i o n e r r o r as a function of the number of i t e r a t i o n s . Figure 2.14 shows that the homogeneous component, ( I - J + J ) q , i s the major contributor to the search vector. The homogeneous component i s reduced as the j o i n t c o n f i g u r a t i o n i s optimized. The r e s u l t i n g t r a j e c t o r y i s shown i n Figure 2.15. 41 0.2 2 4 6 8 10 12 ITERATIONS Figure 2.13 P o s i t i o n error Ax versus i t e r a t i o n i f o r t e s t 8. 42 5 4 ' MIN. NORM COMPONENT DEVIATION COMPONENT HOMOGENEOUS COMPONENT V A L U E 3 . 2 . \ \ / \/ / \ \ \ 18 12 ITERATIONS Figure 2.14 Components of the search vector versus i t e r a t i o n f o r t e s t 8. 43 STARTING CONFIGURATION OPTIMIZED CONFIGURATION GRIPPER POSITION Figure 2.15 P l o t of i t e r a t e d s o l u t i o n t r a j e c t o r y f o r t e s t 8, 44 3. ANALYSIS OF INTERFERENCE 3.1 A Mathematical D e f i n i t i o n of Interference Avoidance A robot workstation i s composed of a number of p h y s i c a l objects which move along various t r a j e c t o r i e s through space as a function of time. Interference avoidance requires that no objects c o l l i d e or i n t e r s e c t at any time. The problem can be stated mathematically as follows. Let us represent n p h y s i c a l objects by sets of points 0 ,^ fic R1", which are functions of time, i o^t), i-i,...,n, t e [ t o , t f ] . (3.1) The c o n d i t i o n f o r interference avoidance i s fiA(t) n 0 (t) - 0 , i - l , . . , n , j - l , . . , n , t e [ t f l , t £ ] (3.2) Define the volume swept out by an object 0^  as i t moves along a t r a j e c t o r y from time t to t as . (3.3) I f we d i s c r e t i z e time as t - k&t and replace t by k, we get a d i s c r e t e condition f o r interference avoidance, 45 n^k.k+l) n Q^k.k+l) - 0, L*j, i - l , . . , n , j - l , . . , n , k-kQ,..,k£-l. (3.4) 3.2 Approximation of the Swept Volume by Convex Hulls In the following analysis, we w i l l impose the r e s t r i c t i o n that a l l obstacles i n the workstation are f i x e d , and we w i l l assume that the manipulator cannot i n t e r f e r e with i t s e l f . The method to be described can be e a s i l y extended to the more general case. C o l l i s i o n s between the moving manipulator and st a t i o n a r y obstacles are avoided i f the volume of space swept out by the manipulator does not i n t e r s e c t any of the obstacles. In general, the manipulator traces a h i g h l y nonlinear path i n j o i n t space, and the r e s u l t i n g swept volume i s very complex. The path can be approximated by short, l i n e a r j o i n t displacements connecting d i s c r e t e j o i n t p o s i t i o n s . For c l o s e l y spaced d i s c r e t e p o s i t i o n s , acceptable r e s u l t s can be obtained by t e s t i n g each d i s c r e t e p o s i t i o n f o r interference. As the spacing between the p o s i t i o n s increases, however, the union of the s t a t i c manipulator volumes i s no longer a good approximation of the swept volume. In p a r t i c u l a r , gaps may appear i n the approximated swept volume. The swept volume can be represented more accurately by approximating the volume swept by a l i n k moving between adjacent p o s i t i o n s by the convex h u l l containing the two p o s i t i o n s . This i s a v a l i d 46 assumption i f the t r a j e c t o r y between two p o s i t i o n s i s approximately l i n e a r . This requirement i s s a t i s f i e d f o r any reasonable d i s c r e t e t r a j e c t o r y approximation. Interference detection i s then performed by t e s t i n g f o r i n t e r s e c t i o n between the set of connecting convex h u l l s and the stationary obstacles. The problem i s thus reduced to a set of s t a t i o n a r y interference problems. The d i s c r e t e approximation of the swept volume i s i l l u s t r a t e d i n Figure 3.1 f o r two adjacent d i s c r e t e p o s i t i o n s k, k+1. 3.3 C a l c u l a t i o n of the Distance Between Stationary Obiects Algorithms f o r c o l l i s i o n avoidance require some quan t i t a t i v e determination of interference and/or clearance between objects. Simple c o l l i s i o n detection requires only a true or f a l s e r e s u l t , while c o l l i s i o n avoidance requires magnitude and d i r e c t i o n information. G i l b e r t and Johnson [1985] present a general mathematical treatment of distance functions using set theory, but do not elaborate on algorithms for c a l c u l a t i n g the actual q u a n t i t i e s . We propose an a l t e r n a t i v e and f u n c t i o n a l l y equivalent distance function based on i n t u i t i v e l y c l e a r geometrical considerations. The method presented here i s an expansion of work described i n [Buchal et a l , 1986]. Define a plane n(n,p) - {r: (r«n)-p - 0 , r e Rm } , (3.5) 47 CONVEX HULL APPROXIMATION OF SWEPT VOLUME Figure 3.1 A d i s c r e t e approximation of the swept volume. 48 where n i s the u n i t plane normal, and ||p|| i s the distance of the plane from the o r i g i n . For two dimensional space, n^2, and f o r three dimensional space, m-3. Let K C Rm, i - l , . . . , n be convex sets representing objects. For any n € Rm, ||n|| - 1, there e x i s t s a unique € R such that the plane Il^n.p^) i s tangent to K and n points out of K at the point of tangency. There also e x i s t s a unique p G R such that the plane II (-n,p ) i s tangent to K and -n points out of K at the point of tangency. The distance between the two planes thus s p e c i f i e d i s d(n) - ( r - r )«n, r e n , r e n . (3.6) J i i i d J The minimum distance d between the two objects i s found by maximizing d ( n ) , d* - max | d(n) : n e R"\ ||n|| - 1 j . (3.7) * Let n be a u n i t vector for which d*- d(n). (3.8) Note that a p o s i t i v e distance r e s u l t s i f the objects are d i s j o i n t , and a negative distance r e s u l t s i f the objects i n t e r s e c t . P h y s i c a l l y , the distance i s the magnitude of the smallest pure t r a n s l a t i o n which w i l l b ring the two objects into contact. We can * * * define a distance vector d - d n , which i s the shortest t r a n s l a t i o n vector to move K into contact with K . The above i J 49 d e f i n i t i o n s are c l a r i f i e d i n Figure 3.2. Maximizing d(n) i s a standard problem of optimizing a function. The distance d — d(n ) i s a l o c a l minimum or maximum i f 3d(n) 3n (3.9) n-n 3.4 The Distance between Convex Polyhedra The problem i s considerably s i m p l i f i e d i f we consider only convex polyhedral objects. Convex polyhedra can be s p e c i f i e d c o n c i s e l y and unambiguously by simple boundary representations, and the minimization problem can be d i s c r e t i z e d and s i m p l i f i e d . Let us represent objects K^, K^, by t h e i r respective sets of v e r t i c e s , V , V . 3.4.1 C a l c u l a t i n g the Separating Distance f o r an A r b i t r a r y Normal Assume we are given a normal n and a corresponding tangent point r^e K . The separating distance along n i s then d(n) - min {(r- r ^ ^ n : r e K ). (3.10) In general, d(n) can be found numerically using gradient methods, since only one l o c a l minimum e x i s t s f o r two convex objects. 50 Figure 3.2 Distance between convex h u l l s . 51 I f i s a convex polyhedron, then every tangent plane contains at l e a s t one vertex of the polyhedron. We can now r e s t r i c t r to the set of v e r t i c e s , and d(n) - min | ( r - r ^ - n : r e j . (3.11) The minimization can be accomplished by a steepest descent graph search. The vertex of K which minimizes d(n) i s r . j c 3.4.2 Finding the Optimal Face Plane For convex polyhedra, the problem can be s i m p l i f i e d i f we consider only the tangent planes which contain the faces of the polyhedra K^, K^. I f we define the set N of a l l face normals of K , and i f we s p e c i f y that n e N, then the s o l u t i o n which maximizes d(n), n G N i s d* - max | ( r - r )«n: r e II , r e II , n e N, II xi TI - l i . (3.12) f L J 1 i i J J » II II J * The optimal face plane of K has the surface normal n f G N, and * * d < d . Combining equations (3.11) and (3.12), we get d* - max jmin | ( r - r ^ - n : r e v j :T€ IT^ , n G N, ||n|| - l | , (3.13) 52 or, equivalently, d f - max j d(n) : n e N, ||n|| - l j . (3.14) I f the polyhedra and K are interchanged, then another value of d f can be cal c u l a t e d . Because of the assymetry of the d e f i n i t i o n s , the interchanged case must be flagged to allow the correct order to be restored l a t e r . The optimal face f o r the p a i r of polyhedra belongs to the polyhedron which gives the l a r g e s t d f . The c l o s e s t vertex of the opposing polyhedron, r , i s found as a r e s u l t of the face evaluations. Once the optimal face i s found, * the separating vector d can be e a s i l y determined. I f the two polyhedra are d i s j o i n t , then the distance function d(n) has only one l o c a l maximum, and the optimal face plane can be found by performing a steepest descent graph search of the faces of each polyhedron. I f the two polyhedra i n t e r s e c t , then the distance function may have several l o c a l maxima, and an exhaustive search i s necessary. Figure 3.3 shows two i n t e r s e c t i n g polygons with more than one l o c a l maximum. 3.4.3 C a l c u l a t i o n of the Separating Distance for D i s j o i n t Polyhedra For convex polyhedra, we know that the separating distance d(n) increases monotonically along the gradient, with a si n g l e maximum. Polyhedra can be represented by a graph of v e r t i c e s , connected by edges. An a l t e r n a t i v e representation i s a graph of 53 n 4 Figure 3.3 Two intersecting polygons with multiple local maxima. 54 faces, connected by edges. We can use a steepest descent graph search to locate the maximum. 3.4.3.1 Steepest Descent Graph Search Define a graph as a network of connected nodes, which can be v e r t i c e s , faces or other elements. Let S be the set of a l l nodes s, and l e t S be the set of nodes connected to node s . Let <f> be ' i i i some function to be minimized, evaluated f o r node s . i Open a s t a r t i n g node and evaluate ^ . UNTIL there are no adjacent nodes or ^  i s minimized DO Evaluate ^ f o r a l l adjacent nodes e S^, saving the node s : <b - min {d> ) . k k r i Remove the nodes from the graph. IF <f> < d\ THEN <t> - <t> i k s — s i k ELSE 4> i s a minimum i ENDIF END UNTIL 55 This algorithm returns the optimal node, and i t s evaluation. 3.4.3.2 C a l c u l a t i n g the Optimal Tangent Plane and the Separating Distance Let the optimal face of polyhedron K be f and the c l o s e s t vertex of polyhedron K be r . We can show that the optimal tangent plane Il^n.p^) contains at l e a s t one vertex of f, and n (-n,p ) contains r . j i c Once the optimal face and c l o s e s t vertex of a p a i r of polyhedra are known, the optimal tangent plane, II and the minimum distance vector d can be e a s i l y found. The points, edges or faces of tangency are also found. Based on geometrical considerations, i t i s c l e a r that there are a f i n i t e number of po s s i b l e cases: a) d* i s normal to f. The tangent plane contains face f (Figure 3.4.a). * b) d i s the common normal of an edge of f and an edge containing r . The tangent plane contains an edge of c face f (Figure 3.4.b). c) d i s normal to an edge of f and contains r . The c tangent plane contains an edge of face f (Figure 3.4.c) 56 57 d) d i s normal to an edge containing r and contains a C vertex of face f . The tangent plane contains one vertex of face f (Figure 3.4.d). * e) d connects a vertex of f to r . The tangent plane c contains one vertex of face f (Figure 3.4.e). I f the p r o j e c t i o n of r onto the plane of f l i e s w ithin the * boundaries of f, then n i s the face normal and the p r o j e c t i o n * vector i s d . Otherwise, the shortest connecting vector v must c be found between each edge of f and each edge containing r . I f c the set of shortest connecting vectors i s V" , then the distance c vector i s d*: ||d*|| - min { || v || : v e V \ , d* e V (3.15) I c c c I c and the distance i s d* - || d*|| . (3.16) The set of tangent points p^ of object K i s P and the set of tangent points p^ of object K i s P^. For convex polyhedra, the sets P may contain a s i n g l e vertex, a l i n e segment or a planar * polygonal region. I f we t r a n s l a t e K by d , a point, edge or surface of K w i l l contact K without penetrating. The set of i j contact points on K w i l l be c a l l e d penetration points, p r i pen These penetration points s a t i s f y 58 p - p + d i pan p e P , p e P . rpen i r J J (3.17) P r o j e c t i o n of the Closest Vertex onto the Optimal Face Plane The p r o j e c t i o n of a point r onto a plane i s the i n t e r s e c t i o n of c the plane and the l i n e which i s normal to the plane and contains r . The equation of the plane i s c ( r • n) - p - 0 (3.18) and the equation of the p r o j e c t i o n vector i s r - r + pn. (3.19) C We can f i n d p. from (3.18), (3,19), p - r • n M (3.20) n • n Knowing that n • n - 1, we can substitute (3.20) into (3.19) to get the projected point r , r - r + (p - r • n) n. (3.21) proj c c I f the projected point r l i e s within the boundaries of r J proj the face f, then any other plane ir containing r ^ and not containing f i n t e r s e c t s the polygonal boundary of f at exactly two points, p , p^. The two points define the endpoints of a l i n e segment which contains r This geometric t e s t (Figure 3.5) proj 59 gure 3.5 Geometric t e s t f o r i n c l u s i o n of a point i n a convex polygonal region. 60 can be used to determine i f r i s on the face f. The points r and r are points of tangency. proj c Edge to Edge C a l c u l a t i o n I f the p r o j e c t i o n of r does not c l i e w i t h i n the boundaries of f, then the shortest connecting vectors f o r each p a i r of edges must be found. The shortest vector connecting two l i n e s i n space i s t h e i r common normal. Given two edges defined by endpoints s^, t^, l e t u - t - s 1 1 1 (3.22) and u - t - s 2 2 2 (3.23) We know that f o r some a . u , l 2 s + u u - s + u u + a , 1 1 1 2 2 2 (3.24) where a i s p a r a l l e l to the common normal. From standard vector geometry, we have a -( s - s ) - ( u x u ) 1 2 1 2 U X U 1 2 (u X u ) 1 2 (3.25) A s i n g u l a r i t y occurs i f the two l i n e s are p a r a l l e l . In that case, u x u - 0, and another method must be used. 1 2 ' We can rearrange equation (3.24) , (3.26) We can cases: now e a s i l y solve f o r n and fi . There are four p o s s i b l e 1. IF (0 < / i < 1) and (0 < < 1) THEN The common normal i n t e r s e c t s both edges, and i s the shortest connecting vector (Figure 3.6.a). 2. ELSE The shortest connecting vector STARTS at an endpoint of edge 1 and i s normal to edge 2 (Figure 3.6.b). Project each endpoint of edge 1 onto the l i n e of 62 Figure 3.6 The shortest vector connecting two 63 edge 2. I f the projected point l i e s between the endpoints of edge 2, i t represents a connecting vector. 3. ELSE The shortest connecting vector ENDS at an endpoint of edge 2 and i s normal to edge 1 (Figure 3.6.c). Do the same as above, with the edges reversed. 4. ELSE The shortest connecting vector j o i n s an endpoint of edge 1 to an endpoint of edge 2 (Figure 3.6.d). Connect a l l p a i r s of endpoints and s e l e c t the shortest r e s u l t i n g vector. Pro]ection of a Point onto a Line Suppose we have a l i n e gment with endpoints s, t, and a point r not on the l i n e . Let be the p r o j e c t i o n of r onto the l i n e . Let u - t - s, (3.27) v - r - s , (3.28) 64 and r' - s + u u (3.29) We can show that u • v u . (3.30) u • u I f 0 < u < 1, then r' l i e s on the l i n e segment. The algorithms and techniques described above f o r d i s j o i n t polyhedra have been s u c c e s s f u l l y implemented f o r 3-D interference detection [Buchal et a l , 1986]. A PUMA 560 robot was modelled kinematically, and the l i n k s were approximated by convex polyhedra. Continuous path t o o l t r a j e c t o r i e s were s p e c i f i e d f o r welding of a v a r i e t y of workpieces, which were modelled geometrically as c l u s t e r s of polyhedra. The robot and workstation geometry i s shown i n Appendix I I I . Separating distances between the l i n k s of the robot and the obstacles i n the workstation were c a l c u l a t e d and used to determine interference. In t h i s implementation, d i s c r e t e p o s i t i o n s were used f o r interference c a l c u l a t i o n s rather than swept volumes. 3.4.3.3 Distance Calculations f o r D i s i o i n t Convex Polygons i n 2-D C a l c u l a t i o n of the separating distance f o r two convex polygons i n two dimensions i s simpler than the case f o r polyhedra, although the procedure i s the same. Define the two polygonal 2 objects as K^, e R . We can summarize the approach as follows: 1. Using an ordered search, f i n d the optimal edge l i n e of each polygon which maximizes the distance between the polygons. Choose the best of the two optimal edges as the optimal edge e f o r the p a i r , and save the c l o s e s t vertex r of the opposing polygon. c 2. I f the p r o j e c t i o n of r l i e s on edge e, the p r o j e c t i o n * i s the distance d . Otherwize, the two edges of containing r must be tested against the optimal edge e c of K . i The distance vector i s one of the following: a) The normal vector connecting two p a r a l l e l edges. b) The p r o j e c t i o n of the c l o s e s t vertex onto the optimal edge. c) A vector connecting the c l o s e s t vertex to an endpoint of the optimal edge. 3.4.4 C a l c u l a t i o n of Penetration for I n t e r s e c t i n g Polyhedra The steepest descent graph search used for d i s j o i n t polyhedra 66 no longer applies i f two polyhedra i n t e r s e c t , because multiple maxima may e x i s t . In t h i s case, an exhaustive search of a l l tangent plane normals must be performed. 3.4.4.1 C a l c u l a t i o n of the Penetration Vector Define the penetration vector d as pen d - 0, d* > 0, (3.31) pen d - d*. d* < 0. (3.32) pen I f two polyhedra i n t e r s e c t , then the distance function d(n) may have multiple l o c a l maxima. The penetration vector d can pen be found by f i r s t using an exhaustive search to f i n d the optimal face plane which maximizes d(n). Note that f o r d < 0, t h i s corresponds to minimizing | d(n) |. Once the optimal face i s known, d can be found by evaluating the l i m i t e d set of pos s i b l e pen a l t e r n a t i v e s , as before. The case can be s i m p l i f i e d i n 2-D. We can show that d - u n, n e w , (3.33) pen where a i s some sca l a r , and N i s the set of edge normals. In other words, the penetration vector i s always p a r a l l e l to an edge normal. Thus, once the optimal edge i s found, d i s known. pen This a s s e r t i o n can be proven geometrically as follows. 67 Suppose we have found the optimal edge f and the c l o s e s t vertex r c * f o r two i n t e r s e c t i n g polygons, and K^. The tangent l i n e ^ ( n ) * contains edge f, the tangent l i n e II (-n ) contains r and the penetration vector i s d We can see by simple trigonometry pen that f o r any other p a i r of tangent l i n e s Il^n), n (-n), II d II > || d ||. Figure 3.7 i l l u s t r a t e s the above argument. pen The following algorithm i s used to determine i f two polygons i n t e r s e c t , and to c a l c u l a t e the penetration vector. The search terminates immediately i f the two polygons are determined to be d i s j o i n t . Otherwise, the separating distance d(n) i s c a l c u l a t e d f o r a l l edge normals of both polygons. The maximum d(n) i s the penetration distance. Since d(n) i s negative f o r i n t e r s e c t i o n , t h i s corresponds to the minimum absolute value |d(n)|. Let N , be the sets of edge normals n f o r the two polygons. d - 0. pen FOR i-1,2 FOR a l l n e N i c a l c u l a t e the separating distance d(n). IF d(n) > 0, the polygons are d i s j o i n t and d -0. pen ELSE IF |d(n) | < || d || THEN pen 68 69 d - d(n) n pen END IF END FOR END FOR 3.4.4.2 C a l c u l a t i o n of the Met T r a n s l a t i o n of the k P o s i t i o n to Reduce Penetration Penetration can be c a l c u l a t e d between the convex h u l l s containing sequential d i s c r e t e l i n k p o s i t i o n s and f i x e d obstacles i n the workstation. In order to reduce the penetration, we wish to t r a n s l a t e the penetration point p on the convex h u l l i n the pen d i r e c t i o n of the penetration vector d The convex h u l l i s pen determined by the contained l i n k p o s i t i o n s , so we need to transform the penetration vector into equivalent t r a n s l a t i o n s applied to the contained l i n k p o s i t i o n s . A manipulator l i n k at p o s i t i o n k i s contained i n two connecting convex h u l l s , the h u l l containing p o s i t i o n s k-1, k and the h u l l containing p o s i t i o n s k, k+1. Also, each convex h u l l contains two adjacent p o s i t i o n s . A penetration vector d and penetration point p can be found for each h u l l . pen Let the convex set representing a l i n k at p o s i t i o n k be and the convex h u l l containing i > be the convex set H^. Let the vector d be the t r a n s l a t i o n vector applied to L due to i.J i 70 penetration of h u l l H^, and l e t p^ ^  be the corresponding t r a n s l a t i o n point on the boundary of L^. Thus, each p o s i t i o n has two t r a n s l a t i o n vectors, d and d . The t r a n s l a t i o n k,k k,k+l vectors are determined by the following: I f p l i e s on the boundary of L , then d - d and pen k k,k pen k k p - p rk,k r p e n k I f p l i e s on the boundary of L , then d - d and *pen J k-1 k-l,k pen k k P " P rk-l,k rpen f c I f p l i e s on an edge of H connecting L , L , then pen k k-1 k k d - (1-M) d , (3.34) k-l,k pen k d t - u d , (3.35) k,k pen k f o r some s c a l a r u e [0,11. I f a vertex v of L and ^ 1 1 k-1 k-1 a vertex of are adjacent v e r t i c e s s p e c i f y i n g the connecting edge of the h u l l H^, then p - v , (3.36) rk-l,k k-1 p - v , (3.37) rk.k k,k' and the edge of H containing v k x > v k i s defined by 71 r - v + / i ( v -v ), 0 < it < 1 (3.38) k-1 ^ v k k - l ' ' v ' For the point p , we can f i n d u, ponk p - V r p e n k-1 u - . (3.39) V - V k k-1 Note that two penetration vectors are applied to L^, and d , due to the two h u l l s H , H containing L . k,k+l k k+1 ° k 3.4.4.3. B e s t - f i t (Minimum Norm) J o i n t Space Displacement to Reduce a Set of Penetration Vectors Suppose several t r a n s l a t i o n vectors act on a manipulator l i n k L due to penetration of h u l l s H , H by one or more obstacles, k r k k+l J The problem i s now to f i n d the j o i n t displacement which w i l l give the desired set of t r a n s l a t i o n s . In general, the problem i s overconstrained, so a b e s t - f i t s o l u t i o n i s sought. Suppose we have a set of h t r a n s l a t i o n vectors d^  e Rm and a corresponding set of t r a n s l a t i o n points p^ e Rm, i - l , . . . , h , as shown i n Figure 3.8. The Jacobian matrices evaluated at the points p^ are J^ € R"*". I f hm > n, the problem i s overconstrained. We can now define the problem as follows: 72 73 Define a combined t r a n s l a t i o n vector d - [d], d] d V , d £ R t o , (3.40) 1 2 n and a corresponding combined Jacobian, J - [ J , J J ], J € R1""". (3.41) 1 2 n We want to f i n d an incremental j o i n t change J Sq - d (3.42) A A * such that || d - d || i s minimized. The minimum norm s o l u t i o n which best f i t s the desired displacement i s Sq - J d (3.43) where J i s the pseudoinverse of the combined Jacobian. Figure 3.9 shows a minimum norm j o i n t change reducing two penetrations. Note that summing the j o i n t changes corresponding to each penetration vector, 5q - ) J d (3.44) i - 1 i s not equivalent, and does not give a minimum norm s o l u t i o n . Suppose the t o t a l set of h non-zero t r a n s l a t i o n vectors are ordered such that 74 1 LINK POSITION i + 1 » » LINK POSITION i OBSTACLE Figure 3.9 Minimum norm penetrations. j o i n t change reducing two 75 K« > «d2n >.... (3.45) We can choose a penetration f r a c t i o n a e [0,1] to define a subset of g < h t r a n s l a t i o n vectors such that | d j | > | | d 2 | | > , . . . > | | d j | >o H d J I . (3.46) I f a i s small, then a l l of the t r a n s l a t i o n s w i l l be applied with equal weight. The small t r a n s l a t i o n s w i l l tend to const r a i n the problem more than necessary, and w i l l r e s u l t i n a poor minimum norm s o l u t i o n f o r reducing the la r g e s t penetrations (Figure 3.10). I f a i s close to one, then only the l a r g e s t penetration w i l l be reduced. I f the second larges t penetration has a s i m i l a r magnitude and a nearly opposite d i r e c t i o n , then o s c i l l a t i o n between the two t r a n s l a t i o n d i r e c t i o n s can occur during successive It e r a t i o n s (Figure 3.11). This problem can be solved by reducing the i t e r a t i o n step s i z e . A large minimum norm s o l u t i o n may r e s u l t from two small t r a n s l a t i o n s which are c l o s e l y spaced and opposite i n d i r e c t i o n (Figure 3.12). 76 OBSTACLE K \ \ \ \ \ /ITERATION i \ / / > / / ITERATION i + 1 — > \ \ V dj +1 \ / \ OBSTACLE Figure 3.11 O s c i l l a t i o n between t r a n s l a t i o n d i r e c t i o n s . 78 OBSTACLE OBSTACLE Figure 3.12 Large minimum norm j o i n t change reducing two small penetrations. 79 4. TRAJECTORY OPTIMIZATION 4.1 Optimization by Minimizing a Functional We wish to develop a method for generating a robot t r a j e c t o r y which s a t i s f i e s c o l l i s i o n and j o i n t l i m i t constraints while optimizing some s p e c i f i e d global c r i t e r i a . We can formulate a general f u n c t i o n a l of the t r a j e c t o r y which s p e c i f i e s the cost or performance as a function of the geometric t r a j e c t o r y as w e l l as the v e l o c i t y and the a c c e l e r a t i o n p r o f i l e s : * ( q ( t ) ) - f f g(q(t),4(t),q(t),t) dt (4.1) o where q ( t ) , t e [ t 0 . t f ] , (4.2) i s the robot j o i n t t r a j e c t o r y . * Finding an optimal t r a j e c t o r y q (t) which s a t i s f i e s s p e c i f i e d boundary conditions and which minimizes * i s a non-linear two-point boundary value problem. 4.1.1 Local and Global Optima * An optimal t r a j e c t o r y q (t) minimizes $ i f *(q*) < *(q*+5q), (4 .3) 80 where 5q i s an a r b i t r a r y v a r i a t i o n of the t r a j e c t o r y . There may be more than one optimal t r a j e c t o r y , corresponding to d i f f e r e n t l o c a l minima. Let us define the set A of a l l optimal t r a j e c t o r i e s as A - ( q , , q , , . . . q } . (4.4) x 2 n * We can now define the g l o b a l l y optimal t r a j e c t o r y q as g *(q*) - min { * (q*) : q*e A ) . (4.5) 8 4.1.2 Qptimalitv Conditions for the Continuous Case The v a r i a t i o n of * can be expressed as t « < q . « q.«t ) - f { SqJ 3 * ( q ° + « q T H<^.k,t) + * e 1 3q 3 q o t +6t & q T M3Ai^£l ) d t + \ g ( q , q , q , t ) d t (4.6) aq f To get t h i s expression e n t i r e l y i n terms of Sq and 5t f, we can integrate by parts the terms containing Sq and 6q as follows. Let u - & (4.7) aq and dv - 5q dt. (4.8) 81 Integrating by parts, we have J* dv Tu - v T u - J v T du. (4.9) From (4.7) and (4.8) we can obtain v - Sq (4.10) and du - ±.[ is. 1 dt 3q dt. (4.11) Combining the above equations, we get _J 3q dq t t o o T d_ dt a q J dt. (4.12) Integrate by parts twice to get [ 6qJ °* dt - SqT *& * aq 3q - f . T d - 5q — : dt L 3q ag_ . dt 0 t 3q •> dt. (4.13) The second i n t e g r a l of (4.6) can be approximated by t +St J g(q,q,q,t)dt * g(q.q,q,t f) Stt (4.14) 82 Combining (4.6), (4.12), 4.13) and (4.14), we get d f _ *\ \$) dq dt dt2 ^8qJ + 6qrd-& f + £ q T ^ 3q Sq' dq ' f . T d - Sq -: dt •aq-+ 5(q.q.q.t f) s t f . (4.15) An optimal t r a j e c t o r y q (t) minimizes *, so the v a r i a t i o n * S$(q ,Sq,StJ must be zero f o r any Sq, Sfc^ . The following * .it _* c o n d i t i o n must hold f o r g — g(q ,q ,q , t ) : 8q d dt .* 3q dt2 ajr _* •3q J - 0 (4.16) for a l l t e [t ,t ] 1 o f' This i s the Euler-Lagrange equation. In addition, the terms outside the i n t e g r a l must be zero. These are the boundary conditions. 4.1.3 Optimalitv Conditions for the Discrete Case * In general, an a n a l y t i c a l t r a j e c t o r y q (t) s a t i s f y i n g the Euler-Lagrange equation cannot be found d i r e c t l y , but i f the problem i s stated i n d i s c r e t e form, then numerical methods can be 83 applied. The Euler-Lagrange equation i s a statement that the gradient of the cost f u n c t i o n a l i s zero at an optimum. C l e a r l y , an approximate s o l u t i o n can be found by i t e r a t i v e l y improving the t r a j e c t o r y u n t i l the gradient i s reduced to a small value. Let us divide time into equal i n t e r v a l s of length T, and l e t q^ be the di s c r e t e value of q(t) at C-kr. We can now sp e c i f y q(t) as a sequence of di s c r e t e states, k-ko,...kt. (4.17) Using the forward diffe r e n c e approximation f o r the f i r s t and second d e r i v a t i v e , q - q k + 1 . \ (4.18) q - 2q + q ^.hll 15 ± 1 (4.19) r f o r small r. We can now sp e c i f y a di s c r e t e cost f u n c t i o n a l as k *(q .O -k-k  F *<Vi'VW'*> r- ( 4 - 2 0 ) and we can write the v a r i a t i o n as 5$ 6q k - 1 k-k J Vi + 6qT & + Sq1 te + i Z d r \ r + ) g d T k 8q k + 1 3a dr 1 • L ^ Ts+l k-k (4.21) 84 I f we replace k by m+1, we can rewrite the f i r s t term of the summation, k +1 f ] 5 q T 5« - r ) Sq T (4.22) k-k m-k +1 0 dq Now i f we l e t k-m, and we l e t the summation go from k-k to k-k , ° o f we get k-k k-k + Sq ,. Ag ( q . q . q .fc+i) T * v n k ^k+1 M k + 2 a q . k-k -1 0 k-k (4.23) S i m i l a r l y , the t h i r d term can be rewritten as 1 ' v , k-k ^k+l k> a q . + Sq ^ dg ( q . q , q ,k-l) a q . k-k +1 f k-k (4.24) Thus, the v a r i a t i o n can be rewritten as 85 6$ - 5 K dg (q , q . q ,k+l) dg ( q , q , q ,k) ° n k n k + l n k+2 6 n k - l ^k M k+1 k-k a q . a# (q . q . q . f c - i ) a q . k-k dr + 6"q T A * ( v q k + i ' q k + 2 - k + 1 ) a q . k-k -1 0 k-k + 6*q a q . k-k +1 £ k-k (4.25) 4.1.4 , The Gradient of a Discrete Functional For the d i s c r e t e problem, define a t r a j e c t o r y vector q , . T T T [ q k -1' q k 1 q k +1' 0 0 0 T T . T . . . q k . q k + 1 , r ] . (4.26) and define a gradient V$, 7$ -a$ a $ T a $ T a $ T a $ T 3q 3q aq k -1 k k +1 0 0 0 3q aq k k +1 f £ a * 3T (4.27) 86 We can now write the v a r i a t i o n as 5*(q,6q) - 6qJ V* . (4.28) The components of the gradient V* can be found from (4.25) For k - k -1, o Sg (q .q - ,q ,k+i,r) o$ k k+1 k+2 — - r - — — — — . (4.29) 3q k 3q k For k - k , o r 3g (q ,q ,q ,k+l,r) 3g (q ,q ,q ,k,r) 3$ [ ° k nk+l ^k+2 a n k - l ^k n k + l ' ' 7 I 3q k ^ a q k 3q k (4.30) For k - k +1, k +2 -1, o o f ds ( q . . q ._ .q U i 2.*+i.o a$ ( q ^ ^ - q ^ ^ o a * _ r f ° g ^k- qk+i- qk+ ; 3q v I 3q u a q . ag (Va-Vx-V*-1'*) a q . } (4.31) For k - k , 52 - r { a q . L 3g (q .q ,q ,k,r) dg (q ,q ,q ,k-l,r) ° n k - l nk k+1 n k - 2 n k - l k a q . a q . (4.32) 87 For k - k +1, f a . . , | ' « ' V , - V , . V ^ » | i F i n a l l y , k dg(q .q .q .^.O e v ^ k - l ^k nk+l 7 " 5 (*(q*-i-VW*'° + dr k-k 0 (4.34) 4.1.5 A Modified Newton-Raphson Method for Numerical  Minimization of a Functional Using the usual method of steepest descent, we can minimize the f u n c t i o n a l * by i t e r a t i v e l y improving the t r a j e c t o r y vector q. A steepest descent i t e r a t i v e sequence has the form q - q - a V*(q ) , (4.35) where a i s some s c a l a r f a c t o r , i Methods with an i t e r a t i v e form q - q - a D V$(q ) (4.36) ^i+l n i i i n i are known as quasi-Newton methods [ G o t t f r i e d and Weisman, 1973, p.109]. 88 Let D - H " 1 , i i (4.37) where H i s the Hessian matrix evaluated at q . The Hessian i i matrix i s the matrix of second p a r t i a l d e r i v a t i v e s of $ evaluated at q ^ H -i a 2 * a 2 * a g 1 Bqiaq2 a 2 $ a 2 $ a 0 i a q n a 2 * dq dq a q (4.38) In general, the Hessian matrix i s computationally expensive to c a l c u l a t e . However, i f $ i s a quadratic of the form * - q TX q + y T q + z, (4.39) then a$ v ^ X q + y, 3q (4.40) and a 2 * Sq' - X, (4.41) H - X. (4.42) The r e s u l t i s a modified Newton-Raphson method, q - q - a X V$(q ) . ni+l n i i i i (4.43) 89 I f o — 1, (4.43) reduces to the standard Newton-Raphson form. 4.1.6 Constraints as Penalty Functions The problem discussed so f a r i s unconstrained. In robot t r a j e c t o r y planning, c e r t a i n constraints must be s a t i s f i e d by the tr a j e c t o r y . A more general problem statement i s : Minimize * ( q ( t ) ) - f £ g ( q ( t ) , q ( t ) , q ( t ) , t ) dt (4.44) o subject to some generally nonlinear constraints f 1 ( q ( t ) , q ( t ) , q ( t ) , t ) - 0, i - 1, m , (4.45) f i ( q ( C ) , q ( t ) , q ( t ) , t ) > 0, i - m+1, n . (4.46) One approach i s to transform t h i s general constrained problem into a simpler unconstrained problem. Let us define a penalty f u n c t i o n a l * , which i s a measure of p v i o l a t i o n of the constr a i n t s . Penalty functions which have a value of zero when constraints are s a t i s f i e d and have a r a p i d l y increasing p o s i t i v e value when the constraints are v i o l a t e d are known as e x t e r i o r penalty functions. This penalty f u n c t i o n a l can be added to the cost f u n c t i o n a l to create an augmented cost f u n c t i o n a l * . Minimization of the augmented f u n c t i o n a l implies minimization of the const r a i n t 90 v i o l a t i o n s . By appropriate weighting of the penalty functions, the s o l u t i o n can be forced a r b i t r a r i l y close to the c o n s t r a i n t boundaries. Let * <q(t)> - f f 8 ( q ( t ) , q ( t ) . q ( t ) , t ) dt (4.47) p «_J P t where g (q-(fc) ,q(t) ,q(t) , t) i s a penalty function r e f l e c t i n g the v i o l a t i o n of the constraints at time t. Combining the cost f u n c t i o n a l $ (4.44) and the penalty f u n c t i o n a l * (4.47), we get an p augmented cost f u c t i o n a l * , *• " I £{ S(<l(t),q(t),k(t),t) + * p < q ( t ) , q < t ) . q ( 0 . 0 j dt. i (4.48) We can combine functions g and g to get an augmented cost p function. g - g + g . (4.49) a p Sub s t i t u t i n g (4.49) into (4.48), we get * - f tg ( q ( t ) . q ( t ) . q ( t ) . t ) dt. (4.50) a a The problem i s now reduced to the unconstrained minimization of $ . 91 4.1.7 Dynamic and Geometric Traj ectory Optimization I f we solve the general problem with free (r free i n the d i s c r e t e case) we can f i n d the optimal dynamic t r a j e c t o r y as a function of time. I f we f i x t f (or f i x r i n the d i s c r e t e case) then we can f i n d an optimal geometric t r a j e c t o r y which i s independent of time. I f we consider the dynamics of the system, we can optimize c r i t e r i a such as minimum time, minimum energy or minimum actuator work subject to dynamic and geometric c o n s t r a i n t s . The dynamic constraints include j o i n t actuator force l i m i t s , j o i n t v e l o c i t y l i m i t s and interference avoidance when the obstacles are moving. Geometric constraints include j o i n t displacement l i m i t s , and avoidance of interference with stationary obstacles. From the dynamics equations of the manipulator, we can show that the j o i n t actuator generalized force vector f i s c e n t i p e t a l tensor and g(q) i s a vector of gra v i t y terms. We can formulate a j o i n t actuator force l i m i t penalty function g^ of the form f ( q , t ) - M(q) q + q TC(q)q + g(q). (4.51) where M(q) i s the i n e r t i a matrix, C(q) i s a c o r i o l i s and g - (f(q,£)-f ) T W ( f ( q . t ) - f ) f lim lim (4.52) where f lim i s the force l i m i t vector and W i s a weighting matrix. 92 The unconstrained two-point boundary value problem can now be solved to get the optimal t r a j e c t o r y q ( t ) . The optimal c o n t r o l which w i l l produce the desired t r a j e c t o r y i s given by (4.51). The dynamic optimization has not yet been taken beyond the establishment of t h i s mathematical framework. The c a l c u l a t i o n of the gradient requires the c a l c u l a t i o n of if__m3l q + q T £ C ( q ) • +3g (a ) (4.53) dq dq dq dq which may be computationally expensive f o r r e a l i s t i c manipulators. A simpler problem i s geometric optimization without consideration of the dynamics. This corresponds to the case with f i n a l time f i x e d . Dynamic constraints can be s a t i s f i e d by time s c a l i n g the optimal geometric t r a j e c t o r y . The dynamic t r a j e c t o r y r e s u l t i n g from t h i s two step procedure i s not generally optimal. For example, the large accelerations r e s u l t i n g from a high rate of curvature of the geometric t r a j e c t o r y can be reduced by reducing the speed. True dynamic optimization requires reducing the curvature of the t r a j e c t o r y . 4.2 A Robot Trai ectorv Planning Problem We can now develop a d e t a i l e d d e s c r i p t i o n of the t r a j e c t o r y planning problem we wish to solve. We w i l l consider point-to-point t r a j e c t o r i e s only, although the theory can be extended to continuous path problems as w e l l . We w i l l also r e s t r i c t ourselves to f i n d i n g a geometric t r a j e c t o r y . 93 Let us define a task t r a j e c t o r y x(£) s p e c i f y i n g the p o s i t i o n and o r i e n t a t i o n of the gripper. We can characterize a point-to-point t r a j e c t o r y as follows: The I n i t i a l robot j o i n t configuration, q ( t Q ) , i s s p e c i f i e d . The f i n a l gripper p o s i t i o n and o r i e n t a t i o n x ( t f ) i s s p e c i f i e d . Admissible j o i n t configurations q(£ f) must s a t i s f y the kinematic equation, For redundant manipulators, there may be i n f i n i t e l y many solutions q(t ). Any t r a j e c t o r y s a t i s f y i n g the above boundary conditions i s a s o l u t i o n . I f a geometric t r a j e c t o r y i s sought, then the v a r i a b l e t i s simply a parameter varying between f i x e d l i m i t s t Q , t f . We have defined an unconstrained two-point boundary value problem with the following conditions: x ( t f ) - f ( q ( t f ) ) . (4.54) f a) q ( t ), t are f i x e d . b) t i s f i x e d , f 94 From the kinematics, we can show that an admissible v a r i a t i o n 5q(t f) which does not change x ( t f ) must s a t i s f y where 6q(t f) - G(q(t f)) Sz, (4.55) G(q(t f)) - (I - J + J ) , (4.56) J - J ( q ( t ) ) , the manipulator Jacobian, and Sz i s an a r b i t r a r y v a r i a t i o n . 4.2.1 Geometric Constraints We can i d e n t i f y the following geometric constraints which must be s a t i s f i e d : J o i n t l i m i t s - The range of j o i n t motion i s constrained by p h y s i c a l l i m i t s . We can sp e c i f y the range of allowable j o i n t l i m i t s as q(t) e [q q ], t e [t , C ] . (4.57) min max 0 f Interference- No interference between objects may occur at any point along the t r a j e c t o r y . I f we define the * minimum distance between objects at time t as d ( t ) , then we require that 95 d (t) > 0, t e [ t o , t f ] . (4.58) 4.2.2 Reduction of Penetration As described i n Chapter 3, we can define a penetration vector d such that pen d - d*. for d* < 0, (4.59) pen d - 0, for d* > 0. (4.60) pen Thus, penetration occurs when the separating distance i s negative. Define a point p as the point of maximum penetration on pen the robot l i n k . We wish to move the robot such that p i s pen t r a n s l a t e d i n the d i r e c t i o n d . For a small penetration, we can pen s p e c i f y an incremental j o i n t change which w i l l reduce the penetration as 5q - J + ( q , p ) d . (4.61) pen pen where J + ( q , p ) i s the inverse Jacobian s p e c i f i e d at the point pen p We can rewrite t h i s as pen Sq - q - q (4.62) where q i s a l i n e a r estimate of the nearest j o i n t c onfiguration C f o r which the penetration i s zero. A more complete analysis of interference can be found i n Chapter 3. 4.2.3 Optimization C r i t e r i a We may wish to optimize the t r a j e c t o r y with respect to c e r t a i n c r i t e r i a , subject to the given c o n s t r a i n t s . An i n t u i t i v e l y desirable goal i s to f i n d the "smoothest" t r a j e c t o r y which s a t i s f i e s the c o n s t r a i n t s . This i s based on an appreciation that large accelerations and correspondingly large actuator forces are associated with a "jagged" t r a j e c t o r y . We can achieve t h i s r e s u l t by minimizing the following cost f u n c t i o n a l : t 1 rf •• T -9 - - q(t) C q(t) dt (4.63) 2 t o where C i s a p o s i t i v e d e f i n i t e weighting matrix. 4.2.4 Proposed Quadratic Cost Functional We propose to use a quadratic cost f u n c t i o n a l composed of terms representing the j o i n t l i m i t and interference penalty functions combined with a term representing an optimization c r i t e r i o n . For the j o i n t l i m i t penalty function, we w i l l use a simple quadratic of the form 97 *x-\ ( q- <W T A ( <i- cw ( 4 - 6 4 ) where A i s a p o s i t i v e diagonal weighting matrix with a - 0, q < q < q , (4.65) i , i M M U v ' m i n max c , otherwise, c some s c a l a r f a c t o r . i i I f a j o i n t l i m i t i s exceeded, then the act i v e l i m i t vector q i s J U im defined as follows. I f q < q then q - q i U ^ i min l im min or i f q > q then q — q . i ^ i max lim max The interference penalty function i s represented by a sum of quadratics, representing the net penalty f o r penetrations between a number of object p a i r s , g - - I (q-q ) . t B (q-q >.. <*.66) 2 2 i - i c 1 1 c 1 m - number of object p a i r s . The penalty functions (4;65), (4.66) can be combined with the optimization function (4.63) to get an augmented cost f u n c t i o n a l a 98 2 / ' { ( q ( t ) " q "- ) T A ( q ( t ) - q u J 0 m -\ + I ( q ( t ) - q e ( t ) ) T B i ( q ( t ) - q e ( t ) ) + q ( t ) T C q ( t ) l dt . i - l e e j (4.67) 4.2.5 The Discrete Form of the Problem The d i s c r e t e boundary conditions of the problem are the following: a) The i n i t i a l c onfiguration q^ i s f i x e d . o b) The number of dis c r e t e steps i s f i x e d , so k and k are r o f fix e d . c) The time increment r i s fi x e d . This corresponds to the f i n a l time t fi x e d i n the continuous case. f d) The f i n a l gripper p o s i t i o n i s f i x e d . The f i n a l j o i n t c o n f iguration q must s a t i s f y f f ( q k ) ~ \ • ( 4- 6 8) f f e) The manipulator s t a r t s from r e s t and returns to r e s t . Thus, 99 ^k - 2 ^k - l " \ 0 0 0 (4.69) W W q k : ( 4 - 7 0 ) f f f I f we a r b i t r a r i l y l e t r - 1, then the cost f u n c t i o n a l (4.67) can be expressed i n d i s c r e t e form as k f * - - ) ( (<L -q ) TA (q -q ) + 5 (q-q ) TB (q-q ) 2 ^ i-1 k-k 0 + ( V i - 2 V q ^ / c (q k. i- 2 V V i ) }. Henceforth, we w i l l drop the subscript from * , and * w i l l be understood to represent the augmented cost f u n c t i o n a l . (4.71) 4.2.6 The Discrete Gradient R e c a l l (4.28) that the v a r i a t i o n of the cost f u n c t i o n a l can be expressed as 5$ - 6~qTV* , (4.72) where q i s the d i s c r e t e t r a j e c t o r y vector, and V$ i s the gradient of the d i s c r e t e cost f u n c t i o n a l . 100 Let us define the following: 5$ - Sq T VS> , (4.73) where 3$ V* - — . (4.74) The d i s c r e t e gradient can now be stated as m V » - A ( q k - q i i m ) + X B <q-q > + C ( V / V V W ' i - 1 i c E [k +l.lc -1] . (4.75) 0 £ . The f u n c t i o n a l i s minimized when the v a r i a t i o n vanishes, S$ - 0. (4.76) This c o n d i t i o n f o r optimality can be expressed equivalently as 5q TV* - 0. (4.77) For t h i s c o n d i t i o n to hold for an a r b i t r a r y v a r i a t i o n Sq about an optimal t r a j e c t o r y q , the gradient V$ must be zero. This i s d i r e c t l y analogous to the minimization of a function of the form f(x). I f desired, the d i s c r e t e Euler-Lagrange equations and boundary conditions can be obtained by expanding t h i s equation. 101 4.2.7 I t e r a t i v e Reduction of the Cost Functional We can now speci f y a modified Newton-Raphson updating equation, ( q k ' i " °i H _ 1 V*k ' k € [ko+1'kt]- ( 4 - 7 8 ) The Hessian matrix H f o r the quadratic cost f u n c t i o n a l i s H - A + l B + 6 C , f or k 6 [ k + l , k f - l ] , (4.79) H - A + £ B + 2 C . f o r k-k f . (4.80) J- i J For k-k , o *q k- 0. (4.81) For k-k o+l, q^ - and V$ k - ACq^-q^, + I B i(q-q e) k + C< - 3 V + 6 ^ ^ + ^ ) i - i (4.82) For k e [k +2,k -2], V * k - A(q k-q u m) + l B i ( q - q c ) k + C ( V ^ V W ' i - i (4.83) 102 For k - k -1, a - a and f Ti+2 Tc+1 k - A (V<W + Z V q - q A + c (Va- 4Vi + 6 qk" 3Vi ) ' i-1 For k - fcf, V 2 - V l - q k and (4.84) 7* k " A ( q k - W + ^ B i ( q - q c ) k + c ( V 2 - 4 V i + 3 q k ) i-i (4.85) For k - k+1, ^ - V Vi and k " c<Va-Vi> (4.86) Since + - ^  , 6q^ - Sq^ and £ £ £ f k k k n k k +1 £ f £ £ f (4.87) Equivalently, 5$ - 6q k k £ £ v$ + v$ k k +1 *- £ £ ' (4.88) I f we redefine the gradient as V* - 7$ + 7* k k k +1 f f £ (4.89) 103 then f o r k-k , t V V A ( \ - \ J +1 + C ^ - V 2 V - ( 4- 9 0 ) i - l 4.2.8 Boundary Conditions In order to s a t i s f y the boundary condition that the gripper p o s i t i o n xfc i s f i x e d , must s a t i s f y the forward kinematic f f equation, x k - f ( q k ). (4.91) f f From the general inverse kinematics s o l u t i o n , we see that *xfc - J k Sq^ - 0 (4.92) f f £ fo r Sq^ - {1-3+ J k ) Sz, (4.93) £ f £ where J k i s the Jacobian evaluated at q^ , £ f J + i s the pseudoinverse, k • f and Sz i s an a r b i t r a r y v a r i a t i o n . The steepest descent v a r i a t i o n Sz i s i n the d i r e c t i o n of the gradient V$ . The cl o s e s t admissible v a r i a t i o n Sq i s the k f f p r o j e c t i o n of Sz onto the n u l l space of J, 104 S % " M G k V * k ' (4.94) £ f £ f o r some s c a l a r u, where G - I - J + J . (4.95) k k k We see that the boundary condition i s s a t i s f i e d by an i t e r a t i v e sequence of the form (q.) " ( q ) - a H^O V4 , k-k . (4.96) v T c i + l v V i i k k * £ 4.2.9 F e a s i b i l i t y of Local Optima Optimal t r a j e c t o r i e s may e x i s t which l o c a l l y minimize the cost f u n c t i o n a l , but which do not s a t i s f y the constr a i n t s . In other words, some points along the t r a j e c t o r y remain well w i t h i n the boundaries of the constraint penalty functions. We w i l l c a l l these non-feasible optimal t r a j e c t o r i e s . Since o p t i m a l i t y does not guarantee f e a s i b i l i t y , each optimal t r a j e c t o r y must be tested to determine i f the constraints are s a t i s f i e d . 105 5. NUMERICAL OPTIMIZATION 5.1 A Discrete Approximation of the Traj ectory In order to f i n d an optimal t r a j e c t o r y numerically, the t r a j e c t o r y must be represented i n di s c r e t e form. We w i l l represent the t r a j e c t o r y by a sequence of d i s c r e t e geometric j o i n t states, q^. K - l , n . (5.1) 5.1.1 S p e c i f i c a t i o n of a S t a r t i n g Traj ectory Numerical optimization requires the s p e c i f i c a t i o n of a s t a r t i n g t r a j e c t o r y . Any t r a j e c t o r y which s a t i s f i e s the boundary conditions can be used, but rapid convergence to a geometrically f e a s i b l e and po s s i b l y g l o b a l l y optimal t r a j e c t o r y requires a good choice f o r the s t a r t i n g t r a j e c t o r y . A good s t a r t i n g t r a j e c t o r y can be generated using i n t e r a c t i v e graphics, allowing the users i n t u i t i o n and judgement to be applied. A gross t r a j e c t o r y can be defined by s p e c i f y i n g a small number of intermediate posi t i o n s through which the s t a r t i n g t r a j e c t o r y should pass. Several a l t e r n a t i v e t r a j e c t o r i e s can be s p e c i f i e d i n t h i s way. In the current 2-D implementation, a simple but somewhat l i m i t e d scheme i s used to generate a s t a r t i n g t r a j e c t o r y . F i r s t , the user i n t e r a c t i v e l y s p e c i f i e s the j o i n t configuration of the 106 manipulator at i t s i n i t i a l p o s i t i o n . Next, the desired f i n a l gripper p o s i t i o n i s s p e c i f i e d i n task coordinates. I f necessary, several intermediate gripper p o s i t i o n s can be s p e c i f i e d along the t r a j e c t o r y . These intermediate p o s i t i o n s are ordered from the i n i t i a l p o s i t i o n to the goal p o s i t i o n . The j o i n t angles f o r each p o s i t i o n are c a l c u l a t e d by the i t e r a t i v e inverse kinematics method described i n Chapter 2, using the j o i n t c o n f i guration of the immediately preceeding configuration as a s t a r t i n g point. The main drawback of t h i s method i s that the user has no d i r e c t c o n t r o l over the j o i n t configurations at each intermediate p o s i t i o n . Using a predetermined algorithm f o r c a l c u l a t i n g the j o i n t configurations can lead to unpredictable j o i n t t r a j e c t o r i e s . A more successful approach may be to allow the user to s p e c i f y intermediate gripper p o s i t i o n s , and to i n t e r a c t i v e l y vary and s e l e c t the corresponding j o i n t configurations. 5.1.2 Generation of the Discrete T r a i e c t o r y by I n t e r p o l a t i o n The s t a r t i n g t r a j e c t o r y s p e c i f i c a t i o n i s very coarse. Before the numerical optimization can begin, the step s i z e between adjacent p o s i t i o n s must be reduced. This can be done by successively subdividing each i n t e r v a l and i n s e r t i n g i n t e r p o l a t e d p o s i t i o n s u n t i l the step s i z e i s small and the l i n e a r i z a t i o n error i s w i t h i n a s p e c i f i e d tolerance. C l e a r l y the number of di s c r e t e states i s not f i x e d i f t h i s approach i s used. The numerical method i s not affected, however, because the number of states i s a r b i t r a r y , and the cost f u n c t i o n a l 107 i s reduced f o r a f i x e d number of states at each i t e r a t i o n . 5.2 An Algorithm f o r the I t e r a t i v e Reduction of the Cost  Functional As shown previously, the cost f u n c t i o n a l can be reduced by i t e r a t i v e l y updating the t r a j e c t o r y . The t r a j e c t o r y i s modified by taking small steps i n the d i r e c t i o n of the modified Newton-Raphson search vector of the f u n c t i o n a l . The step s i z e i s chosen so that the l i n e a r i z a t i o n error remains within a s p e c i f i e d tolerance. An understanding of the algorithm used i n the software implementation can be obtained from the following q u a l i t a t i v e d e s c r i p t i o n . UNTIL the number of i t e r a t i o n s reaches a l i m i t , or the penetration and j o i n t l i m i t constraints are s a t i s f i e d , FOR each p o s i t i o n along the t r a j e c t o r y , Calculate the convex h u l l enclosing the current and previous p o s i t i o n s . Calculate the penetrations of the h u l l and the obstacles. IF penetrations are detected, 108 Calculate the b e s t - f i t j o i n t change to reduce the penetrations. END IF Calculate the modified Newton-Raphson search vector of the cost f u n c t i o n a l . IF the current p o s i t i o n i s the l a s t p o s i t i o n of the t r a j e c t o r y , M u l t i p l y the search vector by the p r o j e c t i o n ( I - J + J ) to get a j o i n t change which w i l l not change the gripper p o s i t i o n . M u l t i p l y the r e s u l t by the i t e r a t i o n f a c t o r to get the j o i n t change. Calculate the l i n e a r i t y error, and reduce the i t e r a t i o n f a c t o r i f necessary. Update the f i n a l j o i n t configuration. Calculate the r e s u l t i n g change i n the gripper p o s i t i o n due to l i n e a r i t y e r r o r . I f necessary, c a l c u l a t e the minimum norm j o i n t change which w i l l remove the p o s i t i o n error, 109 and modify the f i n a l configuration. ELSE Calculate the desired j o i n t change and estimate the l i n e a r i z a t i o n e r r o r . Reduce the i t e r a t i o n f a c t o r i f necessary. Update the manipulator configuration. Calculate the l i n e a r i z a t i o n e r r o r i n the di s c r e t e approximation of the t r a j e c t o r y , and save i t . ENDIF END FOR FOR every p o s i t i o n along the t r a j e c t o r y , Check the l o c a l l i n e a r i z a t i o n e r r o r . Insert or delete intermediate p o s i t i o n s i f necessary. END FOR 110 END UNTIL The d e t a i l s of the algorithm are well documented i n the software source l i s t i n g i n Appendix IV. 5.2.1 L i n e a r i z a t i o n Error Our l i n e a r approximation states that Sx - J(q) Sq (5.2) fo r s u f f i c i e n t l y small Sq. Given q^, Sq^, and x^, we can c a l c u l a t e a p r e d i c t i o n x f o r x , r k+l k+l ik+1-\ + j<v 6\ • (5-3) The exact value i s V i ' f v^5q) (5-4) and the l i n e a r i z a t i o n error i s e - || x - x . (5.5) 1  k+l k+l" 111 5.2.2 Ca l c u l a t i o n of the I t e r a t i o n Step Size We can now determine the lar g e s t i t e r a t i o n step s i z e which does not exceed some maximum l i n e a r i z a t i o n error € as max follows: 1. Calculate the modified Newton-Raphson j o i n t step Aq^ from Aq - -a a'1?* , (5.6) n i i i i where a € [0,11. i 2. For Aq , ca l c u l a t e x , x n i k+1 k+1 3. Calculate the i t e r a t i o n l i n e a r i z a t i o n e r r o r e. 4. I f e > e , then scale the step s i z e , max £ q - q + - S i i Aq . (5.7) ^i+i n i n e The maximum allowable i t e r a t i o n l i n e a r i z a t i o n e r r o r € can max be se l e c t e d by the user. 112 5.2.3 Optimal Spacing of the Discrete Manipulator Configurations Exactly the same method can be used to determine the optimal spacing of the d i s c r e t e manipulator configurations along the t r a j e c t o r y as follows. 1. Calculate the j o i n t change Aq^ between adjacent configurations, A v w \ - ( 5 - 8 ) 2. For Aq , c a l c u l a t e x , x Tc k+l k+l 3. Calculate the t r a j e c t o r y l i n e a r i z a t i o n e r r o r e. 4. I f « > e , then subdivide the i n t e r v a l by i n s e r t i n g a max new p o s i t i o n , V 2 - V i <5-9> ( 5 1 0 ) The maximum allowable t r a j e c t o r y l i n e a r i z a t i o n error can be selected by the user. 113 5.3 S a t i s f a c t i o n of Constraints Since external penalty functions are used, the constraints w i l l generally be v i o l a t e d , although the s o l u t i o n can be forced a r b i t r a r i l y close to the constraint boundary. This problem can be surmounted by enlarging the constraints by some tolerance, and reducing the c o n s t r a i n t v i o l a t i o n to below that tolerance. This approach i s summarized as follows: Decrease the j o i n t l i m i t s by an amount 6 , and increase J J j o i n t the s i z e of the obstacles by an amount 5 pen Iterate u n t i l the j o i n t l i m i t s are exceeded by l e s s than S and the maximum penetration i s l e s s than S j o i n t pen 5.4 Finding a G l o b a l l y Optimal Trajectory In general, a large number of l o c a l l y optimal t r a j e c t o r i e s e x i s t . Some of these solutions may not be f e a s i b l e because they v i o l a t e the constraints, and a f e a s i b l e s o l u t i o n may be d i f f i c u l t to f i n d i f one e x i s t s . I f a number of f e a s i b l e solutions e x i s t , how can the best one be found? There i s no general a n a l y t i c a l method f o r f i n d i n g a global optimum, but an acceptable s o l u t i o n can often be found using search techniques. The most appropriate and e f f e c t i v e method f o r f i n d i n g optimal s o l u t i o n s with a minimum of searching i s to u t i l i z e the judgement and i n t u i t i o n of a human operator to postulate several possible 114 s t a r t i n g t r a j e c t o r i e s . Each s t a r t i n g t r a j e c t o r y i s then optimized and assessed. Depending on the goodness of the choice of the s t a r t i n g t r a j e c t o r i e s , and the s e v e r i t y of the constraints, one or more f e a s i b l e f i n a l t r a j e c t o r i e s may be found. I f no f e a s i b l e t r a j e c t o r i e s are found, further s t a r t i n g t r a j e c t o r i e s can be t r i e d u n t i l the user i s convinced that no f e a s i b l e t r a j e c t o r y e x i s t s . I f several f e a s i b l e t r a j e c t o r i e s are found, the one with the minimum cost i s selected as the optimum. 115 6. NUMERICAL RESULTS The t r a j e c t o r y optimization method has been implemented i n a FORTRAN program c a l l e d OPT, running on a VAX 11/750 host computer. The software i s documented f u l l y i n Appendix IV. Numerical t r a j e c t o r y optimization for simple cases l i k e the ones described i n t h i s chapter t y p i c a l l y takes about 100 seconds of CPU time. OPT models a simple planar manipulator i n 2-D to t e s t and evaluate the t r a j e c t o r y optimization method. The user can s p e c i f y the workstation geometry, the s t a r t i n g t r a j e c t o r y , and the set of numerical parameters used i n the optimization. A. Workstation Geometry. The following parameters are specifed i n a data f i l e which can be edited by the user: a) the number of degrees of freedom and l i n k geometry of the manipulator. b) the number of task degrees of freedom. c) the l o c a t i o n and shape of convex polygonal obstacles. d) s p e c i f i c a t i o n of convex polygons representing the manipulator l i n k s . e) s p e c i f i c a t i o n of manipulator j o i n t l i m i t s . This parameter i s entered i n t e r a c t i v e l y when OPT i s run. 116 S t a r t i n g t r a j e c t o r y . The following s p e c i f i c a t i o n s of the s t a r t i n g t r a j e c t o r y are generated i n t e r a c t i v e l y by the user, as described i n Chapter 5. a) i n i t i a l manipulator j o i n t configuration. b) goal gripper p o s i t i o n . c) intermediate gripper p o s i t i o n s . Numerical parameters. The parameters used i n the numerical optimization are stored i n a f i l e . The user can i n t e r a c t i v e l y change any parameter when OPT i s run, and the new value w i l l be stored. For s i m p l i c i t y , the weighting matrices are s p e c i f i e d to be diagonal, with equal elements. Thus, the weighting matrices A, B, and C can be replaced by s c a l a r weighting f a c t o r s . The following parameters can be s p e c i f i e d : a) J o i n t l i m i t penalty function weighting f a c t o r . This s c a l a r f a c t o r i s equivalent to the weighting matrix A i n equation (4.75). b) Interference penalty function weighting f a c t o r . This s c a l a r factor i s equivalent to the weighting 117 matrices i n equation (4.75). The f a c t o r i s the same f o r a l l obstacles. c) Penetration f r a c t i o n . A l l penetration vectors with a magnitude larger than t h i s f r a c t i o n of the lar g e s t penetration w i l l be reduced by a b e s t - f i t procedure (see sect. 3.4.4.3). d) J o i n t a c c e l e r a t i o n weighting f a c t o r . This s c a l a r f a c t o r i s equivalent to the weighting matrix C i n equation (4.75). e) Maximum number of i t e r a t i o n s . This i s the maximum number of i t e r a t i o n s which w i l l be executed before the program terminates. f) I t e r a t i o n f a c t o r . This i s the fa c t o r a i n the i t e r a t i o n equation (4.78). g) Maximum allowable i t e r a t i o n l i n e a r i z a t i o n e r r o r . This i s an upper bound on the l i n e a r i z a t i o n e r r o r incurred by an i t e r a t i o n step, as described i n Section 5.2.2. h) Maximum allowable t r a j e c t o r y l i n e a r i z a t i o n e r r o r . This i s an upper bound on the l i n e a r i z a t i o n error due to the step s i z e between d i s c r e t e t r a j e c t o r y p o s i t i o n s , as described i n Section 5.2.3. 118 i ) Penetration and j o i n t l i m i t c o n s t r a i n t tolerances. These tolerances are used to 'grow' the constraints, allowing s a t i s f a c t i o n of the constraints as described i n Section 5.3. 6.1 Test Cases OPT was executed f o r a number of t e s t cases to evaluate and demonstrate the optimization method. The following parameter settings were used except where noted otherwise: J o i n t l i m i t penalty function weighting f a c t o r - 100. Interference penalty function weighting f a c t o r - 1000. Penetration f r a c t i o n - 0.5. J o i n t a c c e l e r a t i o n weighting f a c t o r - 10. Maximum number of i t e r a t i o n s - 50. I t e r a t i o n f a c t o r a - 0.6. Maximum allowable i t e r a t i o n l i n e a r i z a t i o n e r r o r - 0.1. Maximum allowable t r a j e c t o r y l i n e a r i z a t i o n error - 0.05. Penetration tolerance — .01. J o i n t l i m i t c onstraint tolerance - 5 degrees. Maximum j o i n t l i m i t s - 90 degrees f o r a l l j o i n t s . Minimum j o i n t l i m i t s - -90 degrees f o r a l l j o i n t s . 119 6.1.1 A Benchmark Case with M u l t i p l e Obstacles - Case 1 Figure 6.1 shows a set of obstacles, an i n i t i a l manipulator configuration, a gripper goal p o s i t i o n and two intermediate manipulator p o s i t i o n s d e f i n i n g a s t a r t i n g t r a j e c t o r y . The obstacles are predefined i n a data f i l e . The manipulator i n i t i a l c o n f i g u r a t i o n i s input by the user as a set of j o i n t angles. The goal p o s i t i o n i s s p e c i f i e d i n task coordinates. Intermediate p o s i t i o n s are also s p e c i f i e d i n task coordinates. An i t e r a t i v e inverse kinematic s o l u t i o n (Chapter 2) i s used to generate the corresponding j o i n t angles f o r each p o s i t i o n . A d d i t i o n a l p o s i t i o n s are inserted into the t r a j e c t o r y by i n t e r p o l a t i o n a f t e r each i t e r a t i o n u n t i l the l i n e a r i z a t i o n e r r o r i s below some bound. Figure 6.2 shows the evolution of the t r a j e c t o r y a f t e r one, two and three i t e r a t i o n s . Figure 6.3 shows the change i n the number of di s c r e t e p o s i t i o n s as a function of the number of i t e r a t i o n s . Note that the number of p o s i t i o n s increases r a p i d l y f o r the f i r s t few i t e r a t i o n s and then s t a b i l i z e s , although further adjustment can occur. The penetration and j o i n t v i o l a t i o n are reduced below the s p e c i f i e d tolerance a f t e r 26 i t e r a t i o n s f o r t h i s case. Figure 6.4 shows the r e s u l t i n g t r a j e c t o r y . The cost f u n c t i o n a l i s reduced as shown i n Figure 6.5. The anomaly i n the cost a f t e r the seventh i t e r a t i o n i s due to a large minimum norm j o i n t change Sq being s p e c i f i e d to reduce small penetrations i n a case such as shown i n Figure 3.12. The norm of Sq i s assumed to be l i n e a r l y r e l a t e d to the penetration magnitude, which i s inaccurate i n t h i s case. Thus, c a l c u l a t i o n of the 120 INITIAL CONFIGURATION OBSTACLES ^ GOAL POSITION Figure 6.1 S t a r t i n g t r a j e c t o r y and obstacles f o r case 1. 121 c) d) Figure 6.2 Evolution of the d i s c r e t e t r a j e c t o r y f o r the f i r s t few i t e r a t i o n s f o r .case 1: a) s t a r t i n g t r a j e c t o r y b) 1 i t e r a t i o n c) 2 i t e r a t i o n s d) 3 i t e r a t i o n s . 122 4 6 8 10 12 14 16 18 20 22 24 ITERATIONS Plot of the number of d i s c r e t e t r a j e c t o r y p o s i t i o n s at each i t e r a t i o n f o r case 1. 123 ' V " ' Figure 6 . 4 The optimized t r a j e c t o r y f o r case 1. 124 10 10 5 10 4 10 3J 10 2J 10 — TOTAL COST INTERFERENCE PENALTY — ACCELERATION TERM 2 4 6 8 10 12 14 16 18 20 22 24 26 ITERATIONS Figure 6.5 Plot of the cost f u n c t i o n a l versus the number of i t e r a t i o n s f o r case 1. 125 penetration magnitude based on 5q gives a large e r r o r . The anomaly i s s t r i c t l y l o c a l , and has no e f f e c t on l a t e r i t e r a t i o n s . The j o i n t t r a j e c t o r i e s a f t e r 26 i t e r a t i o n s are shown i n Figure 6.6. Note that the j o i n t l i m i t constraints of ±90 degrees are not a c t i v e at any point along the t r a j e c t o r y . Figure 6.7 shows the reduction of the maximum penetration as a function of the number of i t e r a t i o n s . The maximum penetration i s reduced to zero a f t e r 26 i t e r a t i o n s . 6.1.2 Varying the J o i n t Limits - Cases 2 and 3 Cases 2 and 3 are the same as case 1, with j o i n t l i m i t s s p e c i f i e d as ±75° and ±65°, r e s p e c t i v e l y . Figures 6.8 and 6.9 show the r e s u l t i n g j o i n t t r a j e c t o r i e s . With l i m i t s of ±75° the upper j o i n t l i m i t i s active f o r j o i n t 3. With l i m i t s of ±65° the upper l i m i t i s ac t i v e f o r j o i n t 3 and the lower l i m i t i s active f o r j o i n t 1. The imposition of more severe j o i n t constraints r e s u l t s i n a la r g e r cost f o r the re s u l t a n t t r a j e c t o r y as shown i n Figure 6.10. In addition, the constraints are not s a t i s f i e d i n the more constrained case even a f t e r 50 i t e r a t i o n s , and i n f a c t there may be no f e a s i b l e s o l u t i o n . 6.1.3 Varying the Maximum Trajectory L i n e a r i z a t i o n Error -Cases 4 and 5 Cases 4 and 5 are the same as case 1, with the maximum 126 — JOINT 4 2 4 6 8 10 12 14 16 18 20 22 24 26 POSITION Figure 6.6 P l o t of the j o i n t t r a j e c t o r i e s f o r case 1. 127 M 0 6 2 4 6 8 10 12 14 16 18 20 22 24 26 ITERATIONS Figure 6.7 P l o t of the maximum penetration versus the number of i t e r a t i o n s f o r case 1. 128 100 — JOINT 4 2 4 6 8 . 10 12 14 16 18 20 22 24 26 POSITION Figure 6.8 Plot of the j o i n t t r a j e c t o r i e s f o r case 2. 129 100 75 50 s-f- . *v A N 6 L E S 25] -25 J -50 . -75 . -100 / \ / \ v — JOINT I —" JOINT 2 — JOINT 3 — JOINT 4 2 4 6 8 10 12 14 16 18 20 22 24 26 POSITION Figure 6.9 Plot of the j o i n t t r a j e c t o r i e s f o r case 3. 130 10 C 0 s T 10' 18' NO JOINT LIMITS JOINT LIMITS = 65 DEG. 5 10 15 20 25 30 35 40 45 50 ITERATIONS Figure 6.10 Comparison of the cost f u n c t i o n a l s f o r case 1 and case 3. 131 t r a j e c t o r y l i n e a r i z a t i o n error £n>axt . s p e c i f i e d as 0.01 and 0.2, r e s p e c t i v e l y . The r e s u l t i n g t r a j e c t o r i e s f o r emax^  -0.01, 0.1, 0.2 are compared i n Figure 6.11. Note that a smaller £maxt r e s u l t s i n a f i n e r spacing of adjacent d i s c r e t e j o i n t p o s i t i o n s . A tra d e - o f f must be established between the accuracy of the d i s c r e t i z a t i o n and the number of d i s c r e t e p o s i t i o n s which must be processed. The cost functionals f o r the three cases are shown i n Figure 6.12. 6.1.4 Varying the Maximum I t e r a t i o n L i n e a r i z a t i o n Error Several d i f f e r e n t maximum allowable i t e r a t i o n l i n e a r i z a t i o n errors emax were specifed f o r the benchmark case, without i t e r r s i g n i f i c a n t e f f e c t on the r e s u l t s . The algorithm appears to be r e l a t i v e l y i n s e n s i t i v e to the choice of cmax J i t e r 6.1.5 A D i f f e r e n t S t a r t i n g Trajectory - Case 6 Case 6 i s the same as case 1, with a d i f f e r e n t s t a r t i n g t r a j e c t o r y as shown i n Figure 6.13. The r e s u l t i n g t r a j e c t o r y i s shown i n Figure 6.14. This d i f f e r e n t s t a r t i n g t r a j e c t o r y leads to a d i f f e r e n t l o c a l l y optimal t r a j e c t o r y than i n case 1. 132 Figure 6.11 Optimized t r a j e c t o r i e s with d i f f e r e n t t r a j e c t o r y l i n e a r i z a t i o n errors: a)e - 0.01 b ) e - 0.1 c) e - 0.2 . 133 10 c 0 s T 10 2. 10 MAX. LINEARIZATION ERROR = .01 MAX. LINEARIZATION ERROR = .1 MAX. LINEARIZATION ERROR = .2 5 10 15 20 25 30 35 40 45 50 ITERATIONS Figure 6.12 Comparison of the cost functionals f o r cases 1,. 5 and 6 f o r d i f f e r e n t maximum t r a j e c t o r y l i n e a r i z a t i o n e r r o r s . 134 INITIAL CONFIGURATION 1 I GOAL POSITION Figure 6.13 S t a r t i n g t r a j e c t o r y f o r case 6, 135 Figure 6.14 Optimized t r a j e c t o r y f or case 136 6.1.6 A D i f f e r e n t Benchmark Case with a Single Obstacle - Case 7 The obstacles were redefined, and a s t a r t i n g t r a j e c t o r y shown i n Figure 6.15 was s p e c i f i e d f o r case 7. The j o i n t l i m i t weighting f a c t o r was set to 1000. The r e s u l t i n g t r a j e c t o r y a f t e r 50 i t e r a t i o n s i s shown i n Figure 6.16. The cost f u n c t i o n a l i s reduced as shown i n Figure 6.17. Figure 6.18 shows the f i n a l j o i n t t r a j e c t o r i e s . In t h i s case, the j o i n t l i m i t of 90° i s acti v e f o r j o i n t 3 and the l i m i t of -90° i s act i v e f o r j o i n t 1. The maximum penetration i s reduced as shown i n Figure 6.19 and the maximum j o i n t l i m i t v i o l a t i o n i s reduced as shown i n Figure 6.20. 6.1.7 Varying the I t e r a t i o n Step Size - Cases 8. and 9 Cases 8 and 9 are the same as case 7, with an i t e r a t i o n step s i z e s of a - 0.1 and a — 1.0. The r e s u l t i n g t r a j e c t o r i e s are compared i n Figure 6.21. The costs f o r the three d i f f e r e n t step s i z e s are shown i n Figure 6.22. The convergence i s slow f o r a small step s i z e , and i s ra p i d but unstable f o r a large step s i z e of a - 1.0. A step s i z e of a - 0.6 gives a good compromise between r a p i d convergence and s t a b i l i t y . The j o i n t v i o l a t i o n s are r a p i d l y reduced f o r a l l three cases as shown i n Figure 6.23. Figure 6.24 shows that the maximum penetration i s also reduced f o r a l l cases, but with a - 0.1 the reduction i s slow due to the small step s i z e . 137 INITIAL CONFIGURATION 1 L GOAL POSITION gure 6.15 S t a r t i n g t r a j e c t o r y and obstacles f o r case 7. 138 Figure 6.16 Optimized t r a j e c t o r y f or case 139 c 0 s T 10 10 5 10 4 10 3 10 2 10 1 18 — TOTAL COST INTERFERENCE PENALTY — JOINT LIMIT PENALTY — OPTIMIZATION CRITERIA v V \ v. 5 10 15 20 25 30 35 40 45 50 ITERATIONS Figure 6.17 Plot of the cost f u n c t i o n a l f o r case 7. 140 128 90 L 0 E S -30 -60 -120 • / \ ^ / \ \ t \ «• > - . N \ s / v / \ \ N. \ N // \ / \ \ ^ / »  \ \ \ \ — JOINT 1 \ JOINT 2 — JOINT 3 — JOINT 4 1 1 1 1 1 1 1 1 1 1 1 1 1 2 4 6 8 10 12 14 16 18 20 22 24 26 28 POSITION Figure 6.18 Plot of the j o i n t t r a j e c t o r i e s f o r case 7. 141 I H 0.4 I 0.2 ._. 0 N 0 5 10 15 20 25 30 35 48 45 50 ITERATIONS Figure 6.19 Plot of the maximum penetration versus the number of i t e r a t i o n s f o r case 7. 142 50 J 0 I N T V I 0 L A T I 0 N D E 6 40 . 30 J 20 J 10 J 5 10 15 20 25 30 35 40 45 50 ITERATIONS Figure 6.20 Plot of the maximum j o i n t l i m i t v i o l a t i o n versus the number of i t e r a t i o n s f o r case 7. 143 144 c 0 s T 16 6 J-18 5 10 4 A W w u \ \ — ITERATION FACTOR = .1 ITERATION FACTOR = .6 — ITERATION FACTOR = 1.8 18 3. — W \ / I \ w 18 2J 18 IB i — i — i — i — i — i — i r 18 15 28 25 38 35 40 45 ITERATIONS Figure 6.22 Comparison of the cost fun c t i o n a l s f o r cases 7, 8 and 9 f o r three d i f f e r e n t i t e r a t i o n step s i z e s . 145 5 10 15 20 25 30 35 40 45 50 ITERATIONS Figure 6.23 Comparison of the maximum j o i n t l i m i t v i o l a t i o n versus the number of i t e r a t i o n s f or three i t e r a t i o n step si z e s . • 146 ITERATIONS Figure 6.24 Comparison of the maximum penetration versus the number of i t e r a t i o n s f o r three i t e r a t i o n step s i z e s . 147 6.1.8 A Case with Multiple Links - Case 10 The t e s t cases so f a r have modelled a si n g l e l i n k of the robot as a rectangular area (equivalent to a s o l i d i n 3-D). OPT allows any number of moving polygonal areas to be s p e c i f i e d . Figure 6.25 shows the r e s u l t s of running case 10, which i s the same as case 7 with the l a s t two l i n k s of the robot modelled by rectangular areas. 148 149 7. FURTHER WORK AND POTENTIAL APPLICATIONS The mathematical techniques and algorithms proposed i n t h i s thesis provide a strong foundation f o r a range of us e f u l a p p l i c a t i o n s . Some f r u i t f u l areas f o r further work can now be i d e n t i f i e d . 7.1 Extension to 3-D The numerical optimization algorithm can be extended to handle r e a l i s t i c 3-D manipulators by straightforward extension. C a l c u l a t i o n of separating distances f o r 3-D polyhedra has already been implemented [Buchal et a l , 1986]. Further e f f o r t i s required to develop and implement an e f f i c i e n t algorithm f o r c a l c u l a t i n g convex h u l l s i n 3-D. The proposed penetration c a l c u l a t i o n algorithm f o r convex polyhedra must be implemented. More so p h i s t i c a t e d data structures, such as developed i n [Buchal et a l , 1986] are required. Simulation and v e r i f i c a t i o n of t r a j e c t o r i e s i n 3-D requires 3-D graphics c a p a b i l i t i e s . To be u s e f u l , the software implementation should incorporate or in t e r f a c e with CAD and workstation simulation software. No fundamental d i f f i c u l t i e s are forseen i n the development of these c a p a b i l i t i e s . 7.2 Optimization of Continuous Path Problems The method demonstrated i n t h i s thesis solves a 150 point-to-point t r a j e c t o r y planning problem. Such t r a j e c t o r i e s are encountered i n many applications such as assembly and part t r a n s f e r . Many other a p p l i c a t i o n s , such as continuous arc welding, require that 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 be s p e c i f i e d continuously along the t r a j e c t o r y . This problem can be solved as an optimal c o n t r o l problem. Consider a problem of the general form t minimize $(u) - h ( q ( t £ ) , t £ ) + J" g(q(t) ,u(t) , t) dt (7.1) t o with the following state equation: q(t) - b(q(t),u(t) , t ) . (7.2) The following s p e c i a l case can be solved numerically: t (7.3) t o q(t) - J + ( q ) x(t) + ( I - J + J ) u, (7.4) where the t o o l t r a j e c t o r y x(t) , t e [ t Q l t f ] i s s p e c i f i e d . The redundancy of the system i s represented by the cont r o l u, which i s 151 optimized. 7 . 3 Optimization of System Dynamics I t i s possible to formulate and solve a general problem to f i n d an optimal t r a j e c t o r y as an e x p l i c i t function of time such that both the geometric and dynamic constraints are s a t i s f i e d . For example, the minimum-time t r a j e c t o r y can be found which s a t i s f i e s j o i n t l i m i t and interference constraints, as well as j o i n t v e l o c i t y and actuator force l i m i t s . Further study of t h i s approach i s needed to assess i t s f e a s i b i l i t y . 7.4 Workcell Layout Optimization For many a p p l i c a t i o n s , a robot workcell must be designed to perform some s p e c i f i e d task. Current workcell design methods are based l a r g e l y on t r i a l and error, and the r e s u l t i n g layout i s usua l l y sub-optimal. Let us pose a hypothetical but t y p i c a l problem. Suppose we wish to f i n d an optimal f i x e d p o s i t i o n f o r a workpiece such that a continuous path manipulator t o o l t r a j e c t o r y s p e c i f i e d r e l a t i v e to the workpiece i s f e a s i b l e and some cost f u n t i o n a l i s minimized. This problem can be solved numerically by making the cost f u n c t i o n a l a function of the t r a j e c t o r y and the workpiece p o s i t i o n . 152 7.5 Mobile Robot Tr a l e c t o r y Planning The methods described i n t h i s thesis can be e a s i l y applied to mobile robot path planning. In general, a mobile robot moves on a 2-D plane among a f i e l d of obstacles. The problem i s exactly analogous to the 2-D point-to-point problem. The problem i s s i m p l i f i e d because the robot can be c o n t r o l l e d d i r e c t l y i n ca r t e s i a n task coordinates. 153 8. CONCLUSIONS This thesis presents an innovative approach to the problem of o f f - l i n e robot t r a j e c t o r y planning which goes beyond any previous work. A general mathematical framework has been est a b l i s h e d to allow a bet t e r understanding of the fundamental problems involved i n t r a j e c t o r y planning. Based on t h i s mathematical foundation, a numerical method has been developed f o r optimization of robot t r a j e c t o r i e s subject to constraints due to j o i n t l i m i t s and interference. The method models objects as convex polyhedra, and allows any s t a r t i n g t r a j e c t o r y to be used f o r the optimization. A di s c r e t e cost f u n c t i o n a l i s proposed, and constraints are represented as penalty functions. A steepest descent numerical i t e r a t i o n scheme i s used to modify the t r a j e c t o r y so that the cost f u n c t i o n a l i s reduced. A d e t a i l e d analysis of interference between convex polyhedra i s presented, and methods f o r c a l c u l a t i n g both separating vectors for d i s j o i n t polyhedra and penetration vectors f o r i n t e r s e c t i n g polyhedra are developed. A convex h u l l approximation i s used f o r the volume swept by a polyhedron moving between d i s c r e t e p o s i t i o n s . This convex h u l l i s used f o r the c a l c u l a t i o n of c o l l i s i o n s with obstacles. Penetration i s reduced i t e r a t i v e l y by t r a n s l a t i n g the polyhedra i n a d i r e c t i o n which reduces the penetration vector. This i s a d i s c r e t e approximation of reducing a continuous distance function by a steepest descent move along the gradient. An a l t e r n a t i v e approach to s o l v i n g the inverse kinematic problem i s also presented. This method modifies the general 154 l i n e a r i z e d inverse s o l u t i o n to allow the i t e r a t e d gripper path to deviate from the s t r a i g h t l i n e path which r e s u l t s i f the minimum norm s o l u t i o n alone i s used to reduce the p o s i t i o n e r r o r . With t h i s approach, the j o i n t motion i s l e s s constrained when the gripper i s f a r from the goal, and the optimization of the j o i n t c o n f i g u r a t i o n can be more he a v i l y weighted. The r e s u l t i s often a better inverse s o l u t i o n . The above techniques have been implemented i n a FORTRAN program to f i n d an optimal geometric t r a j e c t o r y f o r a simple two dimensional point-to-point t r a j e c t o r y optimization problem. The program has s u c c e s s f u l l y found t r a j e c t o r i e s f o r a number of tes t cases. Implementation of the method i n 3-D i s straightforward. The approach can also be applied to r e l a t e d problems such as optimization of continuous robot t r a j e c t o r i e s with a s p e c i f i e d gripper path, optimization of the dynamic t r a j e c t o r y , and workstation layout optimization. 155 REFERENCES ALBERT,A., Regression and the Moore-Penrose Pseudoinverse, Academic Press, New York, 1972. BROOKS,R., "Planning C o l l i s i o n - F r e e Motions f o r Pick-and-Place Operations", The International Journal of Robotics Research, Vol.2, No.4, Winter 1983, pp.19-44. BUCHAL,R., The Automatic Off-line Generation of Welding Robot Trajectories with Emphasis on Kinematic Feasibility and Collision Detection, M.A.Sc. Thesis, U n i v e r s i t y of B r i t i s h Columbia Department of Mechanical Engineering, September 1984. BUCHAL,R., CHERCHAS,D., DUNCAN,J., SASSANI.F., Development of an Automatic Programming System for a Welding Robot - Phase 2: Techniques and Software for Interference Detection and Path Planning, U n i v e r s i t y of B r i t i s h Columbia Department of Mechanical Engineering, Computer Aided Manufacturing and Robotics Laboratory, Report 86-1, February 1986. FOURNIER,A., KHALIL,W., "Coordination and Reconfiguration of Mechanical Redundant Systems", Proceedings of the Int. Conf. Cybernetics Soc, 1977, pp.227-231. GILBERT,E., JOHNSON,D. "Distance Functions and t h e i r A p p l i c a t i o n to Robot Path Planning i n the Presence of Obstacles", IEEE Journal of Robotics and Automation, Vol. RA-1, No.l, March 1985, pp.21-30. GOLDENBERG,A., BENHABIB.B., FENTON,R., "A Complete Generalized So l u t i o n to the Inverse Kinematics of Robots", IEEE Journal of Robotics and Automation, V o l . RA-1, No.l, March 1985, pp.14-20. GOLUB,G., VAN LOAN.C., Matrix Computations, John Hopkins U n i v e r s i t y Press, Baltimore,Md., 1983. GOTTFRIED,B., WEISMAN.J., Introduction to Optimization Theory, P r e n t i c e - H a l l , Englewood C l i f f s , New Jersy, 1973. KHATIB,0., LE MAITRE,J.-F., "Dynamic Control of Manipulators Operating i n a Complex Environment", Proceedings of the 3rd Symposium on the Theory and Practice of Robots and Manipulators, Udine, I t a l y 1978, pp.267-282. KIRCANSKI,M., VUKOBRATOVIC,M., "Trajectory Planning f o r Redundant Manipulators i n the Presence of Obstacles", 5 CISM, 1984, pp.43-50. KLEMA.V., LAUB,A., "The Singular Value Decomposition: I t s Computation and Some Applications", IEEE Transactions on Automatic Control, Vol. AC-25, No.2, A p r i l 1980, pp.164-176. KONSTANTINOV,M., MARKOV,M., NENCHEV,D. "Kinematic Control of Redundant Manipulators", Proceedings of the 11th 156 International Symposium on Industrial Robots,, Tokyo, 1981, pp.561-568. KONSTANTINOV,M., PATARINSKI,S., ZAMANOV,V., NENCHEV,D., "A Contribution to the Inverse Kinematic Problem for I n d u s t r i a l Robots", Proceedings of the 12th International Conference on Industrial Robots, Paris, 1982, pp. 459-467. LOZANO-PEREZ,T., " S p a t i a l Planning: A Configuration Space Approach", IEEE Transactions on Computers, Vol. C-32, No.2, February 1983, pp.108-120. LUH.J., CAMPBELL,C, "Minimum Distance C o l l i s i o n - F r e e Path Planning f o r I n d u s t r i a l Robots with a Prismatic J o i n t " , IEEE Transactions on Automatic Control, Vol. AC-29, No.8, August 1984, pp.675-680. MACIEJEWSKI.A., KLEIN,C, "Obstacle Avoidance f o r Kinematically Redundant Manipulators i n Dynamically Varying Environments", The International Journal of Robotics Research, Vol.4.,No.3, F a l l 1985, pp.109-117. ORIN, D., SCHRADER, W., " E f f i c i e n t Computation of the Jacobian for Robot Manipulators", The International Journal of Robotics Research, Vol.3, No.4, Winter 1984, pp.66-75. OZAKI.H., MOHRI,A., "Planning of C o l l i s i o n - F r e e Movements of a Manipulator with Dynamic Constraints", Robotica, Vol.4, 1986, pp.163-169. PARK.W., "State-space Representations for Coordination of Mu l t i p l e Manipulators", Proceedings of the 14th International Symposium on Industrial Robots, Gothenburg, Sweden, 1984, pp.397-405. PAUL.R., Robot Manipulators: Mathematics, Programming and Control, MIT Press, Cambridge, Mass., 1981. VUKOBRATOVIC,M., KIRCANSKI,M., "Method for Optimal Synthesis of Manipulator Robot T r a j e c t o r i e s " , ASME Journal of Dynamic Systems, Measurement and Control, Vol.104, No.2, June 1982, pp.188-193. WAMPLER, C , "Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods", IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-16, No.l, January/February 1986, pp.93-101. YOSHIKAWA.T., "Analysis and Control of Robot Manipulators with Redundancy", Robotics Research, The First International Symposium, Brady,M., Paul.R., Eds., MIT Press, Cambridge, Mass. , 1984, p".735. 157 APPENDIX I. MATRIX ALGEBRA I.1 D e f i n i t i o n of the Pseudoinverse The pseudoinverse of a matrix can be defined as follows [Albert, 1972, pp. 15-42]. For any nxm matrix H, a pseudoinverse matrix H always e x i s t s . For any x G R , * + * m q - H x, q G R (1.1) i s the vector of minimum norm among a l l q which minimize x - Hq || . (1.2) I f H i s square and nonsingular, H + - H"1 . (1.3) I f the rows of H are l i n e a r l y independent, H + - H T(HH T) _ 1. (1.4) I f the columns of H are l i n e a r l y independent, H + -= (H TH)" XH T. (1.5) 158 1.2 Solution of a Linear System of Equations Suppose we have a set of l i n e a r equations, x - Hq. x € Rn, q € Rm, H e R1"™. (1.6) The general s o l u t i o n i s q - H +x + (I-H +H)z , z G Rm, z a r b i t r a r y (1.7) where H +x i s the minimum norm p a r t i c u l a r s o l u t i o n and (I-H +H)z i s the homogeneous s o l u t i o n . The matrix (I-H +H) i s a p r o j e c t i o n onto the null-space of H. A better understanding i s possible i f we now rewrite equation (A.6) as x - H (H +x) + H (I-H +H) z (1.8) From the d e f i n i t i o n of a pseudoinverse we know that H - HH+H, so the second term i s zero for a l l z and x i s i n v a r i a n t with respect to z. 1.3 The Singular Value Decomposition The s i n g u l a r value decomposition provides a stable and general method for determining the pseudoinverse of a matrix. A more d e t a i l e d discussion of the singular value decomposition can 159 be found i n [Klema and Laub, 1980] and [Golub and Van Loan, 1983] . Define any matrix H S R™ 1. There e x i s t orthogonal matrices U e R"™, V e R""1 such that H - U E V T (1.9) where 2 - S 0 0 0 (I.10) S - d i a g ( a i ( a 2 , . . . a ) , (1.11) a > a £ . . . > a > 0 . (1.12) 1 2 r ' The are c a l l e d the singular values of H. These are the p o s i t i v e square roots of the eigenvalues of HTH. The columns of U are the eigenvectors of HHT. The columns of V are the eigenvectors of HTH. The number r of non-zero singular values i s the rank of H, r - rank(H). (1.13) The pseudo-inverse H + can be found from the singular value decomposition as H + - V Z + U T (1.14) where 160 diag (a i t a2, . . . ff£) (1.15) and + for a > 0 (1.16) a i a i i 0 for a - 0. i The University of British Columbia Computing Center FORTRAN routine UBC SVD was used to calculate the singular value decomposition. If a matrix H of rank r is perturbed u n t i l two rows (or columns) become dependent, then a becomes zero and the rank is reduced to r - l . Matrix H is then said to be rank deficient. If H is the Jacobian of a manipulator, then rank deficiency corresponds to kinematic singularity. Numerical d i f f i c u l t i e s can arise during the transition when two rows (or columns) are nearly dependent, and a becomes very small. Physically, this can lead to very large joint velocities near manipulator singularities. A common solution is to reduce the rank of H when the smallest singular value becomes smaller than some threshold, i.e., i f a < e, then r rank H - r - 1, and a - 0. A more reliable measure of the nearness of rank deficiency is the condition number of H, 1.4 Singularities and Rank Deficiency r k(H) - aj a r . (1.17) 161 Wampler [1986] has proposed an improved method for dealing with near rank d e f i c i e n c y using a damped l e a s t squares approach. 1.5 C a l c u l a t i o n of the Jacobian The Jacobian J i s a matrix of p a r t i a l d e r i v a t i v e s which r e l a t e s incremental changes i n the j o i n t coordinates, S q , to incremental changes i n task coordinates (end e f f e c t o r p o s i t i o n and o r i e n t a t i o n ) , S x . fix - J S q , (1.18) where Sx R m , Sq Rn, J - ** (1.19) 3q The task coordinate vector i s composed of a p o s i t i o n vector r and p an o r i e n t a t i o n vector r , where r i s a vector of r o l l , p i t c h and o o yaw angles, x — (1.20) For a 2-D manipulator moving i n a plane, r e R , r e R. P o (1.21) 162 For a 3-D manipulator, r e R3, r e R 3. (1.22) p ° The Jacobian i s J - — (1.23) which can be decomposed into a r a r . . a r p p p dq 3r . . a r o 0 o dq ^2 8*. (1.24) 3r 3r L e t t i n g — - - B , — - - 7 , then dq 1 dq 1 i i B B ...B 1 2 n 7 1 7 2 - - 7 r (1.25) Several methods e x i s t f o r c a l c u l a t i n g the elements of the Jacobian [Orin and Schrader, 1984], [Paul, 1981, pp.101-108]. We w i l l use the method of Olson and Ribble as described by Orin and Schrader. The method i s summarized below. The transformation r e l a t i n g two adjacent l i n k s i - 1 , i i s given by the Denavit-Hartenberg matrix, 163 C 9 i -sq ca i sq sa n i '. a cq i i sqt eg ca i -cq sa '. a sq i 1 • i 0 sa i ca i : d i 0 0 0 : I (1.26) where sg^, cq^ s i g n i f y the sine and cosine of the j o i n t angles q , and a , a^ and d^ are l i n k parameters. The upper-left sub-matrix i s a 3x3 r o t a t i o n matrix, U , and the l a s t column i s the t r a n s l a t i o n vector p i - l , i Define T as the transformation from the robot base frame 0,1 to the i t h l i n k frame. T - I 0,0 (1.27) T - T T , i - 1 n+1 0,i 0 , i - l i - l , i (1.28) For a revolute j o i n t : 7 - U i 0 , i - l i - 1 n, (1.29) r - p - p i - l i - l n+1 (1.30) /9 - 7 x (-r ) , i - 1, . . . ,n. r l i i - l (1.31) For a prismatic j o i n t : 7 . ~ 0, i 164 (1.32) P. - u, 0,i-l l . . . n . (1.33) APPENDIX I I . CALCULATION OF THE CONVEX HULL ENCLOSING TWO  POLYGONS To f i n d the minimum convex h u l l enclosing two polygons, we can use a standard "gift-wrapping" algorithm. Define the set of v e r t i c e s v. ., 1-1.2, j-l,nverts(i) (II.1) where nverts(i) i s the number of v e r t i c e s of polygon i . The v e r t i c e s of each polygon are ordered i n a clockwise d i r e c t i o n . Define the edges as e - v - v (II.2) i , j i,j+l i , j and define the outward normal of e as i . j n •e 2 e l (II.3) i . J where e , are the components of the edge vector e. The equation of the l i n e containing e^ i s 165 (II.4) An edge e^ i s an edge of the convex h u l l i f v • n > d (II.5) k,m i,j i,J for k-1 i f i-2 , else k-2 i f i - 1 , m - 1 , n v e r t s ( k ) . A l l v e r t i c e s l i e on one side of the l i n e containing an edge of the convex h u l l . Given an edge of the convex h u l l e - v - v ° 1 2 1 and a set of v e r t i c e s v € V, the next edge e of the convex J ° i+l h u l l i s found by minimizing the dot product of edge e^ and candidate succeeding edges e^: e - e : min { e - e : e - v -v , v e y } . (II.6) i+l j i i i j 2 i The preceeding mathematical d e f i n i t i o n s are incorporated into an algorithm which i s descibed below. Test a l l edges of both polygons u n t i l an edge i s found such that a l l v e r t i c e s of both polygons l i e on the same side of the edge. This edge w i l l be taken as the f i r s t edge of the convex h u l l . I f no edges of e i t h e r polygon i s an edge of the h u l l , then te s t the l i n e segments created by j o i n i n g the f i r s t vertex of the f i r s t polygon to each vertex of the second polygon u n t i l 166 one s a t i s f i e s the above condition, and make that segment the f i r s t edge of the h u l l . The current edge i s the f i r s t h u l l edge found above. . The current, polygon i s the polygon containing the edge, or the f i r s t polygon. UNTIL the s t a r t i n g vertex i s the second vertex of the current edge, Test i f the next edge of the current polygon i s an edge of the convex h u l l . I f so make i t the current edge, and add i t to the h u l l . ELSE, j o i n the second vertex of the current edge to every vertex of the other polygon. The l i n e segment which makes the smallest angle (the smallest dot product) to the current edge becomes the current edge, and i s added to the h u l l . The other polygon becomes the current polygon. END IF END UNTIL 167 A f t e r a convex h u l l i s generated, i t i s processed to remove redundant v e r t i c e s which are within some threshold distance of adjacent v e r t i c e s . This corrects f o r the accumulation of very c l o s e l y spaced v e r t i c e s due to computer round-off e r r o r s , and allows more e f f i c i e n t and r e l i a b l e processing. II.1 Implementation of the Algorithm The convex h u l l algorithm i s implemented i n FORTRAN subroutine HULL2D, a l i s t i n g of which i s found i n Appendix IV. 6. Great care i s required i n i d e n t i f y i n g and dealing with degenerate s i t u a t i o n s . The following degenerate cases can be i d e n t i f i e d : 1. Two or more v e r t i c e s are coincident. 2. An edge of one polygon i s c o l l i n e a r with an edge of the second polygon. 3. A vertex of one polygon l i e s on the l i n e containing an edge of the other polygon. Some p a r t i c u l a r cases are: 1. A p a i r of edges are coincident. 2. The polygons are i d e n t i c a l and coincident. 168 2. The polygons are i d e n t i c a l and coincident. Numerical problems can be encountered for s i t u a t i o n s which are close to any of the singular conditions described above. Because the polygons are convex and t h e i r v e r t i c e s are ordered, i t i s possible to reduce the search required to f i n d edges of the convex h u l l . An improved search algorithm has not yet been implemented. 169 APPENDIX I I I . GEOMETRY OF 3-D IMPLEMENTATION The kinematic parameters and geometry of a PUMA 560 robot are shown i n Figure I I I . l . The values of the Denavit-Hartenberg parameters are given i n the following table: Link i Variable a. l a. l d. l 1 q l -90 0 0 2 q2 0 432 149.5 3 q3 90 -20.5 0 4 q4 -90 0 432 5 q 5 90 0 0 6 q6 0 0 56.5 where the dimensions are i n mil l i m e t e r s . The set of te s t workpieces i s shown i n Figure III.2. 170 z0 Figure I I I . l PUMA 560 l i n k geometry. 171 172 APPENDIX IV. SOFTWARE DOCUMENTATION IV.1 Users Guide to OPT IV.1.1 Compiling and Linking OPT The executable program OPT i s generated by compiling and l i n k i n g the following FORTRAN source f i l e s : OPT, CORECT, COSTT, CPUTIME, CVECT, FINPAT, GMULT, GROW, GVERTS, HULL2D, INPUT, INSERT, INTERF, INT2D, JACOB, RDFILE, SOLVE1, S0LVE2, SORT, SVDWS, TRANDB IV.1.2 Running OPT The FORTRAN program f or numerical optimization of a manipulator t r a j e c t o r y can be run by following these steps: 1. Set up the appropriate kinematic s p e c i f i c a t i o n s f o r the manipulator by e d i t i n g the f i l e PARAM1.DAT. 2. Set up the geometry of the objects i n the workstation by e d i t i n g the f i l e BOX.DAT. 3. Generate a s t a r t i n g t r a j e c t o r y by running the i n t e r a c t i v e program STRAJ. 173 4. Run the program OPT by entering RUN OPT OPT w i l l respond with the following: Do you wish to enter new parameters? (T/F) I f you want to change one or more parameters, respond by pressing T. OPT w i l l then d i s p l a y the current value of each parameter from PARAM.DAT, and w i l l ask i f you wish to change the value by prompting Change? (T/F) I f you wish to change the parameter, respond by pressing T. OPT w i l l then prompt f o r a new value. A f t e r a new value i s entered, OPT w i l l continue to the next parameter i n the l i s t . The parameter f i l e PARAM.DAT i s automatically updated with the new values. When the parameters have been selected, OPT begins the i t e r a t i v e optimization. A f t e r each i t e r a t i o n , the following information w i l l appear on the screen: IND1, LAST, COST, DMAX, FMAX where IND1 i s the i t e r a t i o n index, LAST i s the number of di s c r e t e t r a j e c t o r y p o s i t i o n s , COST i s the value of the 174 cost f u n t i o n a l f o r the current i t e r a t i o n , DMAX i s the larg e s t penetration magnitude for the current i t e r a t i o n , and FMAX i s the la r g e s t v i o l a t i o n of the j o i n t l i m i t s . In addition, a message w i l l appear i f t r a j e c t o r y p o s i t i o n s are added or removed a f t e r an i t e r a t i o n . When the stopping conditions are s a t i s f i e d , the message **** Interference and j o i n t l i m i t s s a t i s f i e d **** w i l l appear on the screen, and the program w i l l terminate. Otherwise, OPT w i l l terminate a f t e r the s p e c i f i e d maximum number of i t e r a t i o n s . 5. I f desired, the output f i l e s can be p l o t t e d using a package such as Tektronix EZGRAF. I f the Tektronix IGL graphics l i b r a r y i s a v a i l a b l e , then the manipulator t r a j e c t o r y can be shown g r a p h i c a l l y by running the program DRLINK. IV.1.3 Input F i l e s Filename: box.dat Description : contains the geometry of the objects i n the robot workstation 175 The f i l e i s read as follows: read(l,*) NLINKS do 1 i-l,NLINKS read(l,*) NVERTS(i),IDOF(i) do 2 j-l.NVERTS(i) read(l,*) (SVERTS(k,j,i),k=l,2) 2 continue 1 continue read(l,*) NOBST do 3 i-l,NOBST read(l,*) NVERTS(i) do 4 j-l.NVERTS(i) read(l,*) (SVERTS(k,j,i),k-l,2) 4 continue 3 continue Variables.: NLINKS: the number of manipulator l i n k s modelled NOBST: the number of obstacles modelled NVERTS(i): the number of v e r t i c e s of l i n k i (or obstacle i ) 176 IDOF(i): the number of degrees of freedom of l i n k i SVERTS: the numerical coordinates of the v e r t i c e s ' Filename: paraml.dat Description : contains the kinematic s p e c i f i c a t i o n s of the manipulator Contents: *ndof,nxdof NDOF.NXDOF *workstation s i z e SIZE *jtype JTYPE(I),I-1,ND0F *alpha ALPHA(I),1=1,NDOF * l e n LEN(I),1-1,NDOF * d i s t DIST(I),1-1,NDOF 177 The l i n e s s t a r t i n g with * indicate text e n t r i e s f o r guidance during f i l e e d i t i n g . V a r i a b l e s : NDOF: the number of l i n k degrees of freedom NXDOF: the number of task degrees of freedom SIZE: a s c a l i n g f a c t o r i n d i c a t i n g the order of magnitude of the workstation s i z e i n the chosen units JTYPE: an array i n d i c a t i n g the type of each j o i n t , 0- revolute 1- prismatic ALPHA: an array of the l i n k twist angles LEN: an array of the l i n k lengths DIST: an array of the l i n k j o i n t o f f s e t s Filename: param.dat Description: contains. the parameters needed f o r the numerical reduction of the cost f u n c t i o n a l Contents: JFACT 178 PFACT FRACTI AFACT NMAX TAU STEPSI CMAX MAXPEN,JTOLER VMIN(I),1-1,NDOF VMAX(I),1-1,NDOF Variabl e s : j f a c t : the j o i n t l i m i t penalty function weighting f a c t o r A (Equation 4.75) pfa c t : the interference penalty function weighting fa c t o r (Equation 4.75) f r a c t i : the penetration f r a c t i o n a (Section 3.4.4.3) afa c t : the a c c e l e r a t i o n cost function weighting fa c t o r C (Equation 4.75) nmax: the maximum number of i t e r a t i o n s tau: the i t e r a t i o n step s i z e f a c t o r a (Equation 4.78) s t e p s i : the maximum allowable t r a j e c t o r y i t e r a t i o n e r r o r (Section 5.2.2) cmax: the maximum allowable t r a j e c t o r y l i n e a r i z a t i o n error (Section 5.2.3) maxpen: the penetration tolerance S (Section 5.3) pen 179 j t o l e r : the j o i n t l i m i t (Section 5.3) vmin: the minimum j o i n t vmax: the maximum j o i n t v i o l a t i o n tolerance 6 joint l i m i t s q (Section 4.2.4) min l i m i t s q (Section 4.2.4) max Filename: t r a j . d a t Description: contains the set of d i s c r e t e j o i n t p o s i t i o n s s p e c i f y i n g a s t a r t i n g t r a j e c t o r y Contents: the f i l e i s read by the following routine, read(l,*) (XGOAL(I),I-l,3),ANGOAL read(l,*) LAST do 1 I - l , LAST read(l,*) (VALUES(J,I),J-1,NDOF 1 continue Varia b l e s : XGOAL: coordinates of desired gripper p o s i t i o n ANGOAL: desired gripper o r i e n t a t i o n angle LAST: the number of po s i t i o n s i n the s t a r t i n g t r a j e c t o r y VALUES: the set of j o i n t angles d e f i n i n g each d i s c r e t e p o s i t i o n 180 IV.1.4 Output F i l e s A number of output f i l e s are generated by OPT, p r i m a r i l y for debugging and v e r i f i c a t i o n . The f i l e s contain one record per l i n e to allow the use of the Tektronix EZGRAF p l o t t i n g package. The following f i l e s are generated: angl.dat: the f i n a l t r a j e c t o r y of j o i n t 1 angles ang2.dat: the f i n a l t r a j e c t o r y of j o i n t 2 angles ang3.dat: the f i n a l t r a j e c t o r y of j o i n t 3 angles ang4.dat: the f i n a l t r a j e c t o r y of j o i n t 4 angles fmax.dat: the maximum j o i n t l i m i t v i o l a t i o n f o r each i t e r a t i o n dmax.dat: the maximum penetration f o r each i t e r a t i o n l a s t . d a t : the number of di s c r e t e t r a j e c t o r y p o s i t i o n s f o r each i t e r a t i o n d i s t . d a t : penetration versus p o s i t i o n f o r the f i n a l i t e r a t i o n cosl.dat: the t o t a l cost f o r each i t e r a t i o n cos2.dat: the interference penalty cost f o r each i t e r a t i o n cos3.dat: the j o i n t l i m i t penalty cost f o r each i t e r a t i o n cos4.dat: the a c c e l e r a t i o n cost f o r each i t e r a t i o n 181 IV. 2 Functional Description of OPT OPT i s a FORTRAN program which finds an optimal robot t r a j e c t o r y subject to j o i n t l i m i t and interference c o n s t r a i n t s . A cost f u n c t i o n a l i s defined f o r the t r a j e c t o r y , using penalty functions to represent the constr a i n t s . A quasi-Newton i t e r a t i v e procedure i s used to reduce a cost f u n c t i o n a l . The current implementation applies to a 2-D planar manipulator moving among obstacles represented by convex polygons. The convex h u l l enclosing two adjacent d i s c r e t e l i n k p o s i t i o n s i s used f o r the interference c a l c u l a t i o n s . The i n i t i a l manipulator p o s i t i o n i s given, and the f i n a l c o n f i g u r a t i o n must s a t i s f y the gripper goal p o s i t i o n . OPT has been wr i t t e n f o r evaluation and demonstration of the methods developed f o r t r a j e c t o r y optimization, and i s not intended to be a complete or f i n a l software product. IV.2.1 Nesting of Subroutines The nesting of subroutine c a l l s i s s p e c i f i e d as follows. Each l e v e l of indentation corresponds to a l e v e l of nesting. OPT RDFILE INPUT GROW 182 FINPAT TRANDB GVERTS HULL2D INTERF INT2D CVECT CORECT SORT CVECT JACOB CVECT SOLVE2 SVD JACOB SOLVE1 SVD SOLVE2 SVD COSTT INSERT IV.2.2 Functional Description of Subroutines A l l subroutines are stored i n separate FORTRAN f i l e s with the same name as the subroutine, except as noted. For example, subroutine CORECT i s stored i n f i l e CORECT.FOR. The subroutines 183 used by OPT can be described f u n c t i o n a l l y as follows. CORECT Calculates a j o i n t space c o r r e c t i o n vector which - w i l l reduce interference between the swept volume of a moving object and a stationary obstacle. COSTT Evaluates the cost f u n c t i o n a l by summing the components of each d i s c r e t e p o s i t i o n along the t r a j e c t o r y . CVECT Accesses the database containing the c o r r e c t i o n vectors f o r the current and previous l i n k p o s i t i o n s . FINPAT This subroutine performs the i t e r a t i v e optimization of the robot t r a j e c t o r y . GROW Grows the obstacles by some amount DELTA to compensate fo r the use of e x t e r i o r penalty functions. I f the penetration of the grown obstacles i s reduced below DELTA, there i s no penetration of the true obstacles. GVERTS Accesses the geometry database f o r a given object. 184 HULL2D Finds the convex h u l l enclosing two convex polygons i n 2-D. INPUT I n i t i a l i z e s the parameters for the i t e r a t i v e t r a j e c t o r y optimization algorithm. The parameters are read from the f i l e PARAM.DAT, and can be i n t e r a c t i v e l y modified by the user. INSERT Inserts and removes d i s c r e t e p o s i t i o n s along the t r a j e c t o r y to maintain a desired l i n e a r i z a t i o n e r r o r i n the d i s c r e t e approximation of the t r a j e c t o r y . INTERF Tests f o r interference between a swept volume and an obstacle, and finds the necessary c o r r e c t i o n vectors to reduce interference. INT2D Tests two 2-D polygons f o r i n t e r s e c t i o n , and finds the penetration vector and penetration point r e l a t i v e to the f i r s t polygon. JACOB Calculates the Jacobian matrix f or a given manipulator c onf i gur a t i on. 185 RDFILE Reads the contents of the setup f i l e PARAM1.DAT and i n i t i a l i z e s the v a r i a b l e s . S0LVE1 Finds the p r o j e c t i o n onto the nullspace of a matrix using the singular value decomposition. S0LVE2 Finds the minimum norm inverse kinematic s o l u t i o n . SORT Tests a l l the penetration vectors and f l a g s those which are l a r ger than some f r a c t i o n of the la r g e s t penetration. SVD Performs a singular value decomposition to f i n d a minimum norm s o l u t i o n . This i s a U n i v e r s i t y of B r i t i s h Columbia routine, UBC SVD. The source code i s contained i n f i l e SVDWS.FOR. TRANDB Transforms the geometric database s p e c i f y i n g kinematic chains of r i g i d l i n k s by performing a chain of transformations on each l i n k . 186 IV.2.3 U t i l i t y Routines Several u t i l i t y subroutines are c a l l e d by OPT, and are l i s t e d here f o r completeness. GMULT: M u l t i p l i e s two matrices. Function GET_CPU_TIME: Returns elapsed CPU time. The source code i s stored i n f i l e CPUTIME.FOR. 187 IV.2.4 Documentation for UBC Singular Value Decomposition  Routines 188 THE UNIVERSITY OF BRITISH COLUMBIA 6356 Agricultural Road Vancouver. B.C.. Canada V6T 1W5 Computing Centre Name : UBC SVD Date : June 1973 Revised : January 1980 SINGULAR VALUE DECOMPOSITION OF A MATRIX Carolyn Moore Computing Centre Table of Contents 2. Acknowledgement 1 3. Preliminary Discussion 1 b. Least Squares Solutions of Overdetermined or Underdetermined Systems of c Solution of Homogeneous Equations 2 4. Purpose 2 5. Type of Routine — 3 6. Availability 3 7. How To Use 3 a. Double Precision Routines 3 b. Single Precision Routines 4 8. Restrictions 4 9. Accuracy 4 10. Sample Program ; 5 11. Timing 7 12. Method 7 13. Comments 7 14. References 7 189 1. NOTE The following changes have been made to the writeup UBC SVD since the version published July 1976. — The restriction of N ^ M is removed. The matrix A must be dimensioned at least max(M,N)xM. — The routines work when A is dimensioned lxl and for certain special cases when one or more singular values are calculated to be exactly zero. — The timings are updated. 2. ACKNOWLEDGEMENT This routine is adapted from the ALGOL routines given in section c 3. PRELIMLNARY DISCUSSION Consider an NxM matrix A of rank r^MTN(M,N). It has been shown that there exists a decomposition of the matrix A=UIV T T T where U U=V V=I and £ =diag(a,, o2 , . . . , o^). U is a matrix consisting of the T orthonorrnalized eigenvectors of AA . and V is a matrix consisting of the orthonormalized T T eigenvectors of A A. The o, are non-negative square roots of the eigenvalues of A A; they are called singular values. Assuming o , £ o 2 £ . . . £ o M and rank(A)=r then 0 r + l " s a r + 2 * * - * " a M = a The decomposition can be used for a variety of applications. Some examples are given below. a. Pseudoinverse Let A be an NxM real matrix. An MxN matrix A 4 is the pseudoinverse of A if it has the following four properties AA"A=A A*AA* = A* (AA*)T=AA* (A*A)T=A*A 190 A* is unique and can be calculated from a singular value decomposition by the formula: A*=VX*U T where I* =diag(Ioy) and <7j*=r f o r o^>0 L n f o r c *^=0 b. Least Squares Solutions of Overdetennined or Underdetennined Systems of Equations Let b be a vector such that the minimization of N I (b.-Ax.)J i=l 1 1 is required. If the rank of A is less than M (this is always the case for underdetennined systems of equations) then there is no unique solution. However, if the minimum of M is also required, then this is a unique solution. It can be shown that the unique solution x is given by x=A*b=VZ'UTb=VT-c T where Z* is defined above and c=U b. The vector c is returned by DSLSVD or DSVD if requested. In fact b may be a matrix with p columns. In this case a matrix c of p columns is returned by the routines. c Solution of Homogeneous Equations If A has rani r and V=[v, , v 2 i . . . , v m ] where v. is a column of V, then Av.=0 for i = r + l P . . . , M The columns v j + ^ , . . . r v ^ provide an orthonormal basis fox ail solutions of the homogeneous equations Ax=0. 4. PURPOSE This routine will find the singular values Cj i=l. , . . . , M of a real matrix A dimensioned NxM. If requested, the matrices U and/or V from the singular value decomposition are returned. 191 If the solution x of a system Ax=b is required, then the matrix U b is returned. 5. TYPE OF ROUTINE FORTRAN IV SUBROUTINE subprogram. 6. AVAILABILITY •LIBRARY and •WATLIB. 7. HOW TO USE The routines have three entry points, a. Double Precision Routines — To find the singular values only CALL D S I N G ( A , S , N D I M A U , N , M , S t n ) T — To find the matrix V and/or the matrix U b CALL D S L S V D ( A , S , V , N D I M A U , K D I M V , N , M , N P , tn) T — To find U and/or V and/or the matrix U b CALL D S V D ( A r S f U , V , N D I M A U , N D I M V , N , M , N P , N U , N V f6n) where A is a REAL*8 two-dimensional array dimensioned at least max(M.N)x(M + NP). On entry, the first M columns of A must contain the matrix whose decomposition is required. If NP^O, then the M + 1 , . . . , M + N P columns of A contain the right hand sides b of the system Ax=b to be solved. On exit, the first M columns of A are T destroyed and the M + 1 , . . . , M + N P columns contain U b. S is a REAL*8 one-dimensional array of dimension at least M. On exit, S contains the singular values of A such that S(1)^S(2)^.. . £S(M). U is a REAL*8 two-dimensional array dimensioned at least NxNU. On exit U contains the matrix U. V is a REAL*8 two-dimensional array dimensioned at least MxNV. On exit, V contains the matrix V. NDIMAU is an INTEGER variable or constant On entry, NDIMAU contains the first dimension of the arrays A and U. NDIMAU i> max(MJ<0. NDIMV is an INTEGER variable or constant On entry. NDIMV contains the first dimension of the array V. NDIMV ^ M . 192 N is an INTEGER variable or constant. On entry, N contains the number of rows in the array A to be decomposed. M is an INTEGER variable or constant On entry, M contains the number of columns in the array A to be decomposed. For the WATFTV versions, M^IOO. NP is an INTEGER variable or constant On entry, NP=the number of right hand sides b, if the solution of Ax=b is required. NP=0 otherwise. NU is an INTEGER variable or constant On entry, NU=min(M,N) if the NxNU matrix U is required, NU=0 otherwise. NV is an INTEGER variable or constant On entry, NV=min(M,N) if the MxNV matrix V is required. NV=0 otherwise. n is a statement number to which control is returned if any of the singular values are not found within 40 iterations, n is omitted on the WATFTV versions. Instead an error message is printed. When an error condition exists, the singular values are not sorted in decreasing order. b. Single Precision Routines CALL SING(A,S ,NDIMAU,N,M,&n) CALL SOLSVD(A,S,V,NDIMAU,NDIMV, N , M , N P , t n ) CALL S V D ( A , S , U , V , N D I M A U , N D I M V , N , M , N P , N U , N V , & n ) where A.S.U. and V are declared REALM. 8. RESTRICTIONS For the WATFTV version, M^IOO. 9. ACCURACY For the cases tested, using the single precision routines, singular values were found T T T accurate to five significant figures; and max»(U U-ty, max„(V V-I), and max.j(A-UlV ) lay within the interval (10~6.10"5). When finding the solution of the overdetermined system of equations Ax=b. a singular value (a^say) was considered negligible if o^/max.a^S, where S was an input tolerance usually around 5.E-7. was then set equal to zero. The library routine GMULT was used to find the matrix product T V I * U b (=solution x). Results were generally accurate to four or five significant figures. 193 10. SAMPLE PROGRAM The following program finds the singular value decomposition of a 8*6 matrix A given below, and solves the system Ax=0. and the system Ax=b for 3 right hand side vectors b. 22 10 2 3 7 -1 1 0 14 7 10 0 8 2 -1 1 -1 13 -1 -11 3 1 10 11 - 3 -2 13 -2 4 b i , b 2 , b 3 = 4 0 4 9 8 1 -2 4 0 - 6 - 6 9 1 -7 5 -1 - 3 6 3 2 - 6 6 5 1 1 11 12 4 5 0 -2 2 0 - 5 - 5 a , =35.327038, cr2 = 20, o 3 * 1 9.59591 6, a» = a 5 = 0. True s o l u t i o n s : - 1 / 1 2 0 - 1 / 1 2 0 0 0 1/4 0 1/4 - 1 / 1 2 0 - 1 / 1 2 1/12 0 1/12 IMPLICIT REAL*8(A-H,0-Z) DIMENSION A(8f8)fU(8,8),V(8,8),S(8)#X(8,3) C C READ THE MATRIX A AND PLACE 3 RIGHT HAND SIDES C IN COLUMNS M+l,M+2,M+3 OF A C READ (5,10) N,M,NP 10 FORMAT(3I2) READ(5,20) ((A(I,J),I=1,H),J«1,M) 20 FORMAT(4F10.0) M1=M+1 MNP=M+NP READ (5,20) ((A(I,J),I=1,N),J«M1,MNP) C C CALL DSVD TO FIND U,V, AND U-TRANSPOSE*B C NDIMAU=8 NDIMV=8 CALL TIME(0) CALL DSVD(A,S,U,V,NDIMAU,NDIMV,N,M,NP,M,M,H40) 194 CALL TIME(15,1) C C WRITE SINGULAR VALUES C WRITE (6,30) (S(I),I=1,M) 30 FORMAT('OSINGULAR VALUES ARE'#/,lX,5G12.5) C C THE FOLLOWING DETERMINES ZERO SINGULAR C VALUES. IF S(J)/S(1).LT.EPS THEN S(J)IS C CONSIDERED TO BE ZERO C EPS=.5D-6 SS=S(1)*EPS WRITE (6,40) 40 FORMAT('OCOLUMNS OF V CORRESPONDING TO NEGLIGIBLE S(J)') DO 60 J=1,M IF (S(J).LT.SS) GO TO 70 C C V*DIAG(S(1),...,S(M)) IS PLACED IN V C DO 50 1=1,M 50 V(I,J)«V(IfJ)/S(J) 60 CONTINUE J=M+1 GO TO 90 C C COLUMNS J,...,M OF V ARE A BASIS FOR ALL C . SOLUTIONS OF AX-0 C 70 WRITE (6,80) <<V(I,K),I=1,M),K=J,M) 80 FORMAT (1X,5G13.5) C C COLUMNS OF V FOR 0 SINGULAR VALUES ARE ZEROED C 90 IF (J.GT.M) GO TO 120 DO 110 K=J,M DO 110 1=1,M 110 V(I,K)=0.D0 C C FORM X=V*DIAG(S(l),...,S(M))*(U-TRANSPOSE*B) C 120 MP1=M+1 CALL DGMULT(V,A(1,MP1),X,M,M,NP,NDIMV,NDIMAU,8) WRITE (6,130) <(X(I,J),J=1,NP),I«=1,M) 130 FORMAT('OSOLUTION VECTORS',5(/,3G15.7)) STOP 140 WRITE (6,150) 150 FORMAT(* ERROR RETURN FROM DSVD') STOP END The output from the above program follows: 195 SINGULAR VALUES ARE 35.327 20.000 19.596 0.32169E-14 0.39048E-15 COLUMNS OF V CORRESPONDING TO NEGLIGIBLE S(J) -0.41910 0.44051 -0.52005E-01 0.67606 0.41298 0.0 0.41855 0.34879 0.24415 -0.80222 SOLUTION VECTORS -0.8333333E-01 -0.5473677E-17 -0.8333333E-01 -0.3480215E-16 -0.6887630E-16 -0.1136030E-15 0.2500000 0.4037565E-16 0.2500000 -0.8333333E-01 0.4706869E-16 -0.8333333E-01 0.8333333E-01 -0.4055573E-17 0.8333333E-01 11. TIMING The sample program takes .006 seconds of CPU time on the Amdahl 470 V/6-D computer using the DSVD in •LIBRARY. Using DSVD in the WATFTV library requires .039 seconds of CPU time. The single precision SVD requires .0045 and .034 CPU seconds for the •LIBRARY and •WATLIB versions respectively. 11 METHOD The matrix A is reduced to a bf- diagonal matrix J (whose singular values are the same as those of A) by Householder transformations. The singluar values of J are found by an iterative technique based on the QR algorithm for finding the eigenvalues of a matrix. 13. COMMENTS The method used to find the singular value decomposition of a matrix is numerically stable, although in certain circumstances there may be a slow down in the rate of convergence. The method used to determine whether a singular value is negligible or not can be stated in the following general terms. If a singular value is •small' compared to other singular values, then consider that singular value to be zero. What constitutes 'small' should be determined by the user. It depends on several factors, including the condition of the matrix and the amount of accuracy required; it can often only be determined by inspection of the singular values. 14. REFERENCES a. G. Golub, W. Kahan, "Calculating the Singular Values and Pseudoinverse of a Matrix". J. S1AM Numer. Anal. 2, 1965. pp. 205-224. b. G. Golub, "Least Squares, Singular Values and Matrix Approximations". Aplikace Matematiky. 13. 1968. pp. 44-51. c G. Golub, C. Reinsch, "Singular Value Decomposition and Least Squares Solutions", Numer. Math. JJ. 1970, pp. 403-420. 196 IV. 3 Common Variables The following i s a l i s t of the v a r i a b l e s passed i n common blocks. /cmax/cmax: maximum allowable t r a j e c t o r y l i n e a r i z a t i o n error /d2rad/d2rad: conversion f a c t o r from degrees to radians / f r a c t i / f r a c t i : penetration f r a c t i o n /goal/xgoal(3).angoal xgoal: coordinates of gripper goal p o s i t i o n angoal: gripper goal o r i e n t a t i o n (2-D) /ident/ident(4,4): 4x4 i d e n t i t y matrix / l a s t / l a s t : the number of dis c r e t e t r a j e c t o r y p o s i t i o n s /intdis/vectl(2,2,100),vect2(2,100),pointl(2,2,100),point2(2,100) vectl(klast,i,ipoint)- t r a n s l a t i o n vector applied to p o s i t i o n k-1 of l i n k ilink due to penetration of h u l l k-1 by obstacle iobst, where ipoint - (ilink-1) x nobst + iobst v e c t l ( k c u r r , i , i p o i n t ) - t r a n s l a t i o n vector applied to p o s i t i o n k of l i n k ilink due to penetration of 197 h u l l k by obstacle iobst. I f klast-1, then kcurr=2, else klast=2, kcurr=l. vect2(i, i p o i n t ) - t r a n s l a t i o n vector applied to p o s i t i o n k-1 of l i n k ilink due to penetration of h u l l ic by obstacle iobst p o i n t l , p o i n t 2 - the penetration points corresponding to the above t r a n s l a t i o n vectors. /limits/vmin(4),vmax(4) vmin: array of minimum j o i n t l i m i t s vmax: array of maximum j o i n t l i m i t s /limtol/maxpen,jtoler maxpen: the penetration tolerance used to grow the obstacles j t o l e r : the j o i n t l i m i t v i o l a t i o n tolerance / l i n d o f / l i n d o f ( 2 , 2 0 ) l i n d o f (1, idof) - i d of f i r s t l i n k with idof degrees of freedom l i n d o f ( 2 , i d o f ) - number of l i n k s with idof degrees of freedom /nmax/nmax: the maximum number of i t e r a t i o n s /nobj s/nlinks,nobst n l i n k s : the number of manipulator l i n k s 198 nobst: the number of obstacles /nrdof/ndof,nxdof ndof: the number of l i n k or j o i n t degrees of freedom nxdof: the number of task degrees of freedom /objs/boxes(2,10),sverts(2,100),tverts(2,100) boxes: array of pointers into vertex arrays boxes(1,i)- s t a r t i n g vertex of l i n k i boxes(2,i)- number of v e r t i c e s of l i n k i boxes(1 ,n l inks+i )- s t a r t i n g vertex of obstacle i boxes(2 , n l inks+ i ) - number of v e r t i c e s of obstacle i sverts: array of f i x e d vertex coordinates t v e r t s : array of transformed vertex coordinates /paraml/j f a c t , p f a c t j f a c t : j o i n t l i m i t penalty function weighting f a c t o r p f a c t : penetration penalty function weighting f a c t o r /param3/afact: a c c e l e r a t i o n cost weighting f a c t o r / r o b o t / l i n k ( 4 ) , j t y p e ( 4 ) , a l p h a ( 4 ) , l e n ( 4 ) ; t h e t a ( 4 ) , d i s t ( 4 ) This block contains the kinematic parameters of the manipulator. l i n k : not used jtype: array of j o i n t types 0-revolute, l=prismatic alpha: array of l i n k twist angles 199 len: theta: d i s t : array of l i n k lengths array of j o i n t displacements array of j o i n t o f f s e t s / s i z e / s i z e : the scale of the workstation i n the chosen un i t s , order of magnitude only / s t e p s i / s t e p s i : maximum allowable i t e r a t i o n l i n e a r i z a t i o n e r r o r /tau/tau: i t e r a t i o n step s i z e f a c t o r 200 IV. 4 Generation of a S t a r t i n g Trajectory - STRAJ A s t a r t i n g manipulator t r a j e c t o r y can be i n t e r a c t i v e l y generated by the user by running the program STRAJ. An i n i t i a l l i n k c o n f i g u r a t i o n and a gripper goal p o s i t i o n and o r i e n t a t i o n are s p e c i f i e d by the user. Intermediate gripper p o s i t i o n s are then s p e c i f i e d to define a coarse s t a r t i n g t r a j e c t o r y . The corresponding j o i n t angles are determined by an i t e r a t i v e inverse kinematics algorithm. STRAJ runs on a Retrographics DQ-650 terminal which can emulate a Tektronix graphics terminal. This permits graphical d e p i c t i o n of the t r a j e c t o r y while allowing easy user text i n t e r a c t i o n throught the text window at the bottom of the screen. STRAJ i s intended to be a simple t o o l to be used with OPT f o r t e s t i n g and v e r i f i c a t i o n , and as such i s somewhat l i m i t e d i n i t s c a p a b i l i t i e s . For p r a c t i c a l use, a more s o p h i s t i c a t e d software package i s needed. An executable f i l e can be generated by compiling and l i n k i n g the following source f i l e s : STRAJ.FOR, GMULT.FOR, INVERS.FOR, JACOB.FOR, RDFILE.FOR, ROTMAT.FOR, SVDWS.FOR. IV.4.1 Users Guide to STRAJ 1. Before running STRAJ, the kinematic parameters f i l e PARAM.DAT and the geometry f i l e BOX.DAT must e x i s t with the cor r e c t contents as s p e c i f i e d f o r OPT. 2. Put the terminal i n 4027 graphics mode. 201 To run STRAJ, enter 'RUN STRAJ'. STRAJ w i l l ask f o r the number of task coordinates.' I f only the two gripper p o s i t i o n coordinates are to be s p e c i f i e d , enter '2'. I f the gripper o r i e n t a t i o n angle i s also to be s p e c i f i e d , enter '3'. STRAJ w i l l ask f o r the i n i t i a l angles of the manipulator j o i n t s . This i s the i n i t i a l or s t a r t i n g p o s i t i o n , which i s known. Enter the angles, i n degrees. The screen w i l l go blank, and the obstacles and i n i t i a l manipulator configuration w i l l be drawn. STRAJ w i l l ask Save position? (T/F). Enter 'F'. STRAJ w i l l ask Quit? (T/F). Enter 'F'. STRAJ w i l l ask for the goal p o s i t i o n and o r i e n t a t i o n . 202 Enter the x and y p o s i t i o n coordinates and a counterclockwise o r i e n t a t i o n angle i n degrees, with the x axis being zero. I f two task degrees of freedom were s p e c i f i e d , the o r i e n t a t i o n angle w i l l be ignored • when the inverse s o l u t i o n i s c a l c u l a t e d . 9. STRAJ w i l l ask Erase screen? (T/F). I f you enter 'T', the screen w i l l be erased and redrawn. I f you enter 'F', a l l graphics w i l l be superimposed on top of the e x i s t i n g screen image. 10. A f t e r the goal p o s i t i o n has been entered and shown on the screen, STRAJ w i l l ask Save position? (T/F) I f you wish to save the goal p o s i t i o n , enter 'T', otherwise enter 'F'. 11. STRAJ w i l l ask Quit? (T/F). I f you don't wish to spec i f y any intermediate p o s i t i o n s , 203 enter 'T' and an i t e r a t i v e inverse kinematic s o l u t i o n w i l l be found for the goal p o s i t i o n using the i n i t i a l c o n f i g u ration as the s t a r t i n g point. Each time the i t e r a t i v e inverse kinematics routine i s run, STRAJ w i l l ask f o r the values of the maximum l i n e a r i z a t i o n error ERMAX and the deviation f a c t o r GAMMA. Values of 0.2 and 0.5, r e s p e c t i v e l y , are good choices. I f three task degrees of freedom were s p e c i f i e d , then the gripper o r i e n t a t i o n w i l l be preserved. 12. When a s a t i s f a c t o r y goal p o s i t i o n has been s p e c i f i e d , intermediate gripper p o s i t i o n s can be inserted, ordered from the i n i t i a l c o n f i g uration to the goal p o s i t i o n . A f t e r a desired gripper p o s i t i o n and o r i e n t a t i o n i s entered, an i t e r a t i v e inverse s o l u t i o n i s found for the j o i n t angles, using the previous c o n f i g u r a t i o n as a s t a r t i n g point. 13. When a s u i t a b l e number of intermediate p o s i t i o n s have been s p e c i f i e d , enter 'T' when STRAJ asks Quit? (T/F). An inverse s o l u t i o n w i l l be found for the j o i n t angles of the f i n a l configuration, using the l a s t intermediate c o n f i g u r a t i o n as a s t a r t i n g point. A l l of the j o i n t configurations from the i n i t i a l to the f i n a l 204 configuration w i l l be stored i n f i l e TRAJ.DAT, and STRAJ w i l l then terminate. IV.4.2 Functional Description of Subroutines The i n t e r a c t i v e s t a r t i n g t r a j e c t o r y generation program STRAJ c a l l s the following subroutines: INVERS: Implements an i t e r a t i v e algorithm to f i n d an inverse kinematic s o l u t i o n . JACOB: See Appendix II.2.2. RDFILE: See Appendix II.2.2. ROTMAT: Transforms a generalized r o t a t i o n axis and angle into the equivalent r o t a t i o n matrix. SVD: See Appendix I I . 2.2. 205 IV.5 Graphical Output Programs Several u t i l i t y programs have been wr i t t e n to generate graphical output. the programs are quite short, and the source code i s s u f f i c i e n t l y c l e a r that no further documentation i s required beyond a fu n c t i o n a l d e s c r i p t i o n . These programs require the a v a i l a b i l i t y of the Tektronix IGL graphics l i b r a r y , and have been run on a Retro-Graphics terminal which emulates Tektronix 4006 and 4027 graphics terminals. However, the programs can be adapted to any s i m i l a r graphics environment. Program name: DRLINK Description: This program draws a graphical d e p i c t i o n of the f i n a l optimized d i s c r e t e t r a j e c t o r y as a superposition of the dis c r e t e l i n k p o s i t i o n s composing the t r a j e c t o r y . The obstacles are also shown. The angles f o r each j o i n t are read from separate f i l e s which are s p e c i f i e d by the user. T y p i c a l l y , the f i l e s are the output f i l e s of OPT, ANG1.DAT, ANG2.DAT, ANG3.DAT and ANG4.DAT. Program name: DRTRAJ Description: This program shows the s t a r t i n g t r a j e c t o r y by reading the manipulator pos i t i o n s from a f i l e , t y p i c a l l y TRAJ.DAT, and superimposing them on the screen. Program name: DRINV Description: This program shows the t r a j e c t o r y of j o i n t p o s i t i o n s generated by the i t e r a t i v e inverse kinematic s o l u t i o n . 206 The user enters the. names of the f i l e s containing the j o i n t angles. The inverse kinematic routine INVERS generates j o i n t f i l e s Al.DAT, A2.DAT, A3.DAT and A4.DAT. 207 IV. 6 FORTRAN Source L i s t i n g s The following f i l e s are l i s t e d , i n alp h a b e t i c a l order: CORECT COSTT CVECT DRINV DRLINK FINPAT CPUTIME GMULT GVERTS HULL2D INPUT INSERT INT2D INVERS JACOB MINVER OPT RDFILE ROTMAT SOLVE1 SOLVE2 SORT STRAJ SVDWS TRANDB 208 13-May-19B7 12 i56 i l8 ll-May-19B7 14i05t55 VAX FORTRAN V4.4-177 Page 1 DUAO:CBUCHAL.OPT3CORECT. FOR,152 0001 subroutine corect ( thedot , thdot l ,vmaxl ) 0002 0003 c Calculate • j o in t apace correc t ion vector Which w i l l reduce in ter ference 0004 c betveen the swept volume of a moving object and a s ta t ionary obBtac le . 0005 0006 c Parametersi 0007 ' c thedot- the j o i n t change vector ( in deg) which i s the minimus 000B c norm so lu t ion minimizing the set of penetrat ion vec tors . 0009 c t h d o t l - the j o i n t change vector minimizing the larges t 0010 c penetrat ion . 0011 c vmaxl- the magnitude of the largest penetrat ion v e c t o r . 0012 0013 c V a r i a b l e s i 0014 c v l i s t t i . j ) - array po int ing to the two largest penetrat ions 0013 c v l i s t ( l , j > - l i n k i d 0016 c v l i s t ( 2 , j ) - obstacle i d 0017 c v l i s t ( 3 , j ) - current or previous penetrat ion O01B c vector 0019 c v l i s t ( 4 , j ) - number of degrees of freedom of 0020 c l i n k 0021 c f t a b l e d , j ) - table of penetrat ion vectors l arger than the 0022 c threshold 0023 c i= l ink i d , j » o b s t i d 0024 c va lues i 0025 c 1- current penetrat ion only 0026 c 2- previous penetrat ion only 0027 c 3- both penetrations 002B 0029 0030 r e a l thetot<4) 0031 r e a l obveK 2) ,vpolnt (2) 0032 rea l tne t (4 ,4 ) , jac (6 ,5 ) ,hmul t (4 ,4 ) 0033 rea l ident<4,4) , t inv<2,2) .p invt2) , thedot(4) 0034 Integer l indof(2 ,10) 0035 integer f table t 10,10) 0036 rea l matt 20,5) 0037 rea l p o i n t ( 2 , 2 ) , v e c t ( 2 , 2 ) , t p o i n t ( 2 ) 0038 r e a l lent 4) ,d is t<4) .a lphat4) , theta14) 0039 Integer j t y p e t 4 ) , l i n k ! 4 ) 0040 rea l vlens(2) 0041 integer v l i B t t 4 , 2 ) 0042 rea l thdot l (4 ) ,mat l f4 ,3 ) 0043 0044 common / r o b o t / l i n k , j t y p e , a l p h a , l e n . t h e t a , d i e t 0043 common / i d e n t / i d e n t 0046 common /nrdof /ndof ,nxdof 0047 common / l i n d o f / l i n d o f 0048 common /nobj s /n l inks ,nobs t 0049 common ItractlIfracti 0050 0051 d 2 r a d » . 0 1 7 4 5 3 3 0052 icount=0 0053 0054 do 1 1*1,4 0055 t h e t o t ( i ) » 0 . 0056 t h d o t l t i ) « 0 . 0057 1 continue CORECT 13-May-1987 12:36.18 VAX FORTRAN V4.4-177 Page 2 ll-May-19B7 14i05i55 DUAO iCBUCHAL. OFT3CORECT.FOR»152 0038 0039 c Find a l l penetration v e c t o r ! larger than some f r a c t i o n of the 0060 c largest penetrat ion. A l so f i n d the larges t and second larges t 0061 c penetrat ions. 0062 0063 f r a c t » l . - f r a c t i 0064 c a l l sort (f r a c t , vmajrl ,vmax2,vl l s t ,t table) 0063 0066 c Calcu la te correc t ion vectors for each l i n k of the robot . 0067 006B do 150 i n d « l , n d o f 0069 0070 v l e n » 0 . 0071 0072 i s t a r t - l i n d o f < l , i n d ) 0073 inumb-1indof(2,Ind) 0074 i f ( i s t a r t . n e . 0 ) then 0073 0076 c Ca lcu la te the net transformation matrix r e l a t i n g the f i n a l 0077 c l i n k frame to the world frame, ' t n e t ' . 007B 0079 c a l l J a c o b ( i d e n t , i d e n t , l e n , a l p h a , d i s t , t h e t a , J t y p e , i n d , t n e t , j a c ) OOBO 00B1 c Calcu la te the Inverse transformation, transforming world 0082 c coordinates to f i n a l l i n k coordinates . 00B3 O0B4 do B3 i -1 ,2 O0B5 do B6 J-1,2 0086 t i n v d , j ) « t n e t ( J , i ) 0087 86 continue 0088 B5 continue O0B9 p l n v l l ) » - < t n e t ( 1 , 4 ) * t n e t < 1 , 1 > + t n e t ( 2 , 4 ) A t n e t t 2 , 1 ) ) 0090 pinv(2) - - ( tnet<l ,4)*tnet ( l ,2 )+tnet(2 ,4)*tnet<2,2) ) 0091 0092 do 200 l l l n k - l s t a r t , l s t a r t + i n u n b - l 0093 do 230 iobst -1 ,nobst 0094 0095 i f ( f t a b l e ( i l l n k , l o b s t ) . n e . O ) then 0096 0097 c Get the penetrat ion vectors for the current p a i r of 009B c objects . 0099 0100 c a l l c v e c t ( 1 , l l i n k . l o b s t , v l s n s , v e c t . p o i n t ) 0101 0102 c M u l t i p l y the c loses t point i n world coordinates by t l n v to 0103 c f i n d the penetrat ion point on the l a s t l i n k . 0104 0103 do 133 l n d 4 « l , 2 0106 l f ( f t a b l e l i l i n k . i o b s t ) . e q . i n d 4 . o r . 0107 * f t a b l e d l i n k , i o b s t l . e q . 3) then oioa do 156 i - 1 , 2 0109 o b v e l ( 1 ) ° v e c t t i , i n d 4 ) 0110 t p o i n t ( i ) - p o i n t ( 1 , i n d 4 ) 0111 156 continue 0112 c a l l g m u l t ( t l n v , t p o i n t , v p o i n t , 2 , 2 , 1 , 2 , 2 , 2 ) 0113 do BB i -1 ,2 0114 vpoint t i ) - v p o i n t ( i ) + p i n v ( i ) CORECT 13-May-19B7 12136118 VAX FORTRAN V 4 . 4 - 1 7 7 Pag* 3 l l - M a y - 1 9 8 7 14i05i55 DUAO I [BUCHAL. OPT3CORECT.FOR (152 0115 B8 continue 0116 0117 do 266 1-1,4 0118 do 267 j - 1 , 4 0119 h m u l t l i , j ) - 0 . 0120 267 continue 0121 h m u l t < i , i ) « l . 0122 266 continue 0123 hmult(1 ,4)-vpoint(1) 0124 hmult(2 ,4)-vpoint(2) 0125 0126 c Ca lcu la te the jacobian of the penetrat ion po int . 0127 0128 c a l l j a c o b l i d e n t , h m u l t , l e n , a l p h a , d i e t , t h e t a , j t y p e , i n d , t n e t , j a c ) 0129 0130 c If the current penetrat ion vector l a the largest p e n e t r a t i o n . 0131 c c a l c u l a t e the minimum norm s o l u t i o n . 0132 0133 l f ( v l i s t ! 1 , 1 ) . e q . i l i n k . a n d . v l i s t ! 2 , 1 ) . e q . i o b s t . a n d . 0134 A v l i s t ( 3 , l ) . e q . l n d 4 . a n d . v l i s t ( 4 , l ) . e q . i n d ) then 0135 0136 do 373 1-1,2 0137 mat1(1,ndof+1)-obvel< i ) 013B do 380 j » l , n d o f 0139 m a t l l i , j ) « j a c ( i + 3 , J ) 0140 380 continue 0141 373 continue 0142 0143 c a l l so lve2(2 ,mat l ,4 ,4 ,2 ,ndof ,hoaog , thdot l ) 0144 0145 end i f 0146 0147 do 275 i - l , 2 0148 lcount-icount+1 0149 matt icount,ndof+1)-obvel(1) 0150 do 280 j - l , n d o f 0151 matUcount , j ) - j a c ( i + 3 , j) 0152 280 continue 0153 275 continue 0154 endlf 0133 155 continue 0156 0157 endlf 015B 0139 250 continue 0160 200 continue 0161 endlf 0162 150 continue 0163 0164 c Ca lcu la te the Inverse Jacobian and f ind the minimum norm s o l u t i o n 0163 c which gives the best f i t jo int motion to reduce a l l of the 0166 c penetrat ion vec tors . 0167 0168 c The des ired jo int angle change i s a l i n e a r combination of the minimum 0169 c norm so lu t ion for the largest penetrat ion and the min. norm s o l u t i o n 0170 c for the set of penetrat ion vec tors . Thus the so lut ion i s weighted 0171 c to favor the l a r g e B t penetration while minimizing the set of CORECT 13-May-1987 12156118 VAX FORTRAN V4.4-177 Page 4 ll-May-1987 1 4 i 0 5 « 5 5 _DUAO i t BUCHAL. OPT3CORECT.FOR)152 ro 0172 0173 0174 0175 0176 0177 0178 0179 0180 0181 01B2 0183 01B4 0185 0186 0187 0188 01B9 0190 0191 0192 0193 0194 0195 c penetrat ions . l f ( l count .g t .2 ) then c a l l solve2(2,mat ,20 ,4 , lcount ,ndof ,homog, thedot) amult"l.-vmax2A*2/vmaxl**2 do 900 i « l , n d o f t h e d o t ( i ) » a r a u l t * t h d o t l ( i ) + ( 1 . - a m u l t ) * t h e d o t ( i ) 900 continue e lse do 901 i « l , n d o f thedot ( i )= thdot l ( i ) 901 continue end i f c Convert to degrees and update the net change. do 312 1*1,ndof thedot ( i ) ' thedot ( i )Vd2rad t h d o t l ( i ) " t h d o t l ( i ) / d 2 r a d 312 continue return end PROGRAM SECTIONS Name Bytes 0 fiCODE B91 1 9PDATA 16 2 flLOCAL 1608 3 ROBOT 96 4 IDENT 64 5 NRDOF 8 6 LINDOF 80 7 N0BJS 8 B FRACTI 4 T o t a l Space A l l o c a t e d 2775 ENTRY POINTS Address Type Name 0-00000000 CORECT A t t r i b u t e s PIC CON REL LCL BHR EXE RD NOHRT LONG PIC CON REL LCL SHR NOEXE RD NONRT LONG PIC CON REL LCL N0SHR NOEXE RO HRT LONG PIC 0VR REL CBL SHR NOEXE RD WRT LONG PIC OVR REL CBL SHR NOEXE RD HRT LONG PIC 0VR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL CBL SHR NOEXE RD HRT LONG PIC OVR REL CBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG CORECT 13-May-19B7 12 l56 l l8 VAX FORTRAN V4.4-177 Page 5 ll-May-19B7 14i05i55 DUAO :CBUCHAL. OPT3CORECT.FOR» 152 VARIABLES Addreaa Type Name A A RA4 AMULT 2-00000514 RA4 HOMOG 2-000004FC 1*4 IND A A IA4 ISTART 7-00000004 1*4 NOBST 2-000004FB RA4 VHAX2 Addreaa Type Naae A A RA4 D2RAD A A IA4 I 2-00000510 1*4 IND4 A A 1*4 J 5-00000004 IA4 NXDOF Address Type Name Address Type Name 2-000004F4 RA4 FRACT 2-000004F0 IA4 ICOUNT A A IA4 I NUMB 5-00000000 IA4 NDOF A A RA4 VLEN B-00000000 RA4 FRACTI 2-00000504 IA4 ILINK 2-0000050B IA4 IOBST 7-00000000 IA4 NLINKS AP-0000000CB RA4 VMAX1 ARRAYS Address Type Name Bytes Dimensions 3-00000020 RA4 ALPHA 16 (4) 3-00000050 RA4 DIST 16 (4) 2-00000130 IA4 FTABLE 400 (10 , 10) 2-000000DB RA4 HMULT 64 (4, 4) 4-00000000 RA4 IDENT 64 (4, 4) 2-00000060 RA4 JAC , 120 (6, 5) 3-00000010 IA4 JTYPE 16 (4) 3-00000030 RA4 LEN 16 (4) 6-00000000 IA4 LINDOF BO (2, 10) 3-00000000 IA4 LINK 16 (4) 2-000002CO RA4 MAT 400 (20 . 5) 2-000004A0 RA4 MAT1 80 <4, 5) 2-00000010 RA4 OBVEL B (2) 2-0000012B RA4 PINV B (2) 2-00000450 RA4 POINT 16 (2, 2) AP-OOOOOOOBffl RA4 THDOTl 16 (4) AP-000000049 RA4 THEDOT 16 (4) 3-00000040 RA4 THETA 16 (4) 2-00000000 RA4 THETOT 16 (4) 2-0000011B RA4 TINV 16 (2, 2) 2-00000020 RA4 TNET 64 (4, 4) 2-00000470 RA4 TPOINT 8 (2) 2-00000460 R*4 VECT 16 (2, 2) 2-00000478 RA4 VLENS B (2) 2-00000480 1*4 VLIST 32 (4, 2) 2-0000001B RA4 VPOINT B (2) LABELS A A 1 A A 156 A A 280 Address Label A A 85 A A 200 A A 312 Addresa Label A A 86 A A 250 A A 375 Address Label A A SB A A 266 A A 3B0 Address Label A A 150 A A 267 A A 900 Address Label A A 155 A A 275 A A 901 rnuw-T 13-May-19B7 121561IB VAX FORTRAN V4.4-177 Page ll-May-1987 14i05!55 DUAO : [BUCHAL. 0PT3C0RECT. FOR j 152 FUNCTIONS AND SUBROUTINES REFERENCED Type Name Type Nana Type Name Type Name Type Name CVECT GMULT JACOB SOLVE2 SORT 13-May-19B7 13•10i49 VAX FORTRAN V4.4-177 Page 1 13-May-19B7 1 3 i l 0 l l O DUAOitBUCHAL.OPT3COSTT.FOR» 79 0001 subroutine c o s t t ( v a l u e s , v a i n , vmax, thesav , las t , 0002 A l u l , I u 2 , l u 3 , l u 4,cost) 0003 0004 c Evalt l a t e the cost func t iona l by summing the cost components at 0005 c d i scre te p o s i t i o n a l o n g the t r a j e c t o r y . 0006 0007 c ParametersI 000B c va lues - the j o i n t values along the t r a j e c t o r y 0009 c vmln,vmaz- the minimum and maximum jo int l i m i t s 0010 c thesav- the jo in t change corresponding to the maximum 0011 c penetrat ion 0012 c l a s t - number of jo in t pos i t ions 0013 c l u l , . . - l o g i c uni t numbers for the output f i l e s 0014 c cos t - the value of the coat func t iona l 0015 0016 0017 r e a l values(4,A) ,vmin(4) ,vmax(4) 001B r e a l thesav(4,*) 0019 r e a l cost 0020 r e a l teapo(4) , jmult(4) 0021 r e a l vllm(4) 0022 r e a l amult(4) 0023 r e a l part(4) 0024 r e a l temp(4),tnorm(4) 0025 0026 c o n on /pa r a m i / j f act ,pfact 0027 common /param3/afact 002B 0029 cos t '0 . 0030 do 300 i -1 ,4 0031 p a r t ( l ) ° 0 . 0032 300 continue 0033 0034 do 302 i « l , l a s t 0035 do 333 k"l ,4 0036 tnorm! k ) « 0 . 0037 333 continue O03B do 301 j -1 ,4 0039 0040 c Ca lcu la te j o i n t l i m i t f a c t o r s . 0041 0042 l f ( v a l u e s ! j , i ) . g t . v m a x ! j ) ) then 0043 j m u l t l j ) « j f a c t 0044 v l l m f j)<vmax( j ) 0045 e l s e i f ( v a l u e s * j , l ) . I t . v m l n ( j ) ) then 0046 jmul t l j )= j fac t 0047 v l i m l j)«vmin( j ) 0048 e lse 0049 j m u l t ! j ) « 0 . 0050 endif 0051 0052 c Calcu la te jo in t acce l era t ion f a c t o r s . 0053 O054 l f ( l . e q . l ) then 0055 a c c e l - v a l u e B ( j , i + 1 ) - v a l u e s ! j , 1 ) 0056 a c c e l l = 3 . A v a l u e s ! j , i ) - 4 . * v a l u e a ! j , i + l ) l v a l u e s ! j , i + 2 ) 0057 c accel= 0 . COSTT 13-Kay-19S7 13 l l0 l49 VAX FORTRAN V4.4-177 13-May-1987 1 3 i l 0 l l 0 _DUAO iCBUCHAL.OPT3COSTT.FOR|79 005B e l s e l f ( l . e q . 2 ) t h e n 0059 a c c e l l - - 3 . A v a l u e s l j , 1 - 1 ) + 6 .Avaluaa ( J , 1 ) 0060 * - 4 . * v a l u e s ( j , 1 + 1 ) + v a l u a s ( j , i + 2 ) 0061 a c c a l » v a l u « « ( j , i + l > - 2 . * v a l u e a ( j , i > + v a l u e s ( J , i - 1 ) 0062 a l a a i f ( 1 . e q . l a a t - 1 ) t h a n 0063 a c c e l l - v a l u e s l j , 1 - 2 ) - 4 . A v a l u e s I J , 1 - 1 ) + 6 . A v a l u e s ( j , i > -0064 A 3 .Avalueal J,i + 1 ) 0065 a c c e l - v a l u e a < j , i + l ) - 2 . A v a l u e s ( J , i ) + v a l u a a ( j , 1 - 1 ) 0066 a l a a i f ( i . e q . l a a t ) t h a n 0067 a c c a l l - v a l u e a ( J , i - 2 ) - 4 . A v a l u a a ( j , 1 - 1 } + 3 . A v a l u a a ( J , i ) 006B a c c e l - v a l u e s ( j , i - l ) - v a l u e s ( j , i ) 0069 e l a a 0070 a c c e l l - v a l u e s l j , i - 2 ) - 4 . A v a l u e s ( j , i - l ) + 6 . A v a l u e s ! j , i ) -0071 A 4.Avalues!j, 1 + 1)+valuas(j , 1 + 2 ) 0072 a c c e l - v a l u e a < j , i + l ) - 2 . A v a l u a a ( j , i ) + v a l u e a ( 5 , 1 - 1 ) 0073 e n d l f 0074 0075 a a u l t t j ) - a f a c t 0076 0077 te«p(l ) - . 5*pfact*ab»(theaav(j,i)*A2) 0078 t e a p ( 2 ) « . 5 A j m u l t l j ) A ( v a l u e a ( j , l ) - v l i a ( j ) ) A A 2 0079 tempt 3 ) - . 5 * a m u l t I j ) A a c c e l A A 2 0080 0081 do 334 i n d - 1 , 3 0082 t n o n l i n d ) - t n o r a ( l n d ) + t e a p ( i n d ) AA2 0083 334 c o n t i n u e 0084 0085 301 c o n t i n u e 0086 0087 do 335 i n d - 1 , 3 0088 tnoralind)«aqrt(tnoralind)) 0089 p a r t ( i n d ) - p a r t i i n d ) + t n o r m ( i n d ) 0090 335 c o n t i n u e 0091 0092 302 c o n t i n u e 0093 0094 do 303 i - l , 3 0095 p a r t i i ) - p a r t ( i ) / l a a t 0096 303 c o n t i n u e 0097 009B coat«part(l)+part ( 2)+part ( 3 ) 0099 0100 w r i t e ( A , A ) p a r t ( 1 ) . p a r t ( 2 ) , p a r t ( 3 ) 0101 w r i t e ( l u l , A ) c o s t 0102 write ( l u 2,A) p a r t i 1) 0103 write ( l u 3,A) part! 2 ) 0104 write ( l u 4,A) p a r t i 3) 0105 0106 r e t u r n 0107 end COSTT 13-Mey-1987 13I10I49 VAX FORTRAN V4.4-177 Page 3 13-May-1987 1 3 i l 0 H 0 DUAOiCBUCHAL.0PT3C0STT.FORj79 PROGRAM SECTIONS Name Bytes A t t r i b u t e s 0 (CODE 607 PIC CON REL LCL SHR EXE RD NOHRT LONG 2 «LOCAL 196 PIC CON REL LCL NOSHR NOEXE RD HRT LONG 3 PARAM1 8 PIC OVR REL GBL SHR NOEXE RD HRT LONG 4 PARAM 3 T o t a l Space Al located 4 815 PIC OVR REL GBL SHR NOEXE RD HRT LONG ENTRY POINTS Address Type Haae 0-00000000 COSTT N> VARIABLES Address Type Nana AA RA4 ACCEL 2-00000070 IA4 I AA IA4 K AP-000000209 1*4 LU3 Address Type Naae AA RA4 ACCEL1 AA IA4 IND AP-00000014Q 1*4 LAST AP-000O0O24O 1*4 LU4 Address Type Nana 4-00000000 RA4 AFACT AA 1*4 J AP-00000018O 1*4 LU1 3-00000004 RA4 PFACT Address Type Naae AP-00000028S RA4 COST 3-00000000 IA4 JFACT AP-0000001CB IA4 LU2 ARRAY8 Address Type Nana Bytes Distensions 2-00000030 RA4 AMULT 16 (4) 2-00000010 RA4 JMULT 16 (4) 2-00000040 RA4 PART 16 (4) 2-00000050 RA4 TEMP 16 (4) 2-00000000 R*4 TEMPO 16 (4) AP-00000010O RA4 THESAV AA (4, A) 2-00000060 RA4 TNORM 16 (4) AP-000000048 R*4 VALUES A A (4, A) 2-00000020 R*4 VLIM 16 (4) AP-OO000OOCB R*4 VMAX 16 (4) AP-0000000BO R*4 VMIN 16 (4) LABELS , Address Label Address Label Address Label Address Label Address Label Address Label AA 300 AA 301 AA 302 AA 303 AA 333 AA 334 A A 335 COSTT FUNCTIONS AND SUBROUTINES REFERENCED Typa Nana R*4 MTH$SQRT 13-May-19B7 13 l l0 l49 VAX FORTRAN V4.4-177 13-May-19B7 13I10I10 DUAO : C BUCHAL. OPT3COSTT. FOR »7 9 Page to oo 13-May-19B7 12i57i26 VAX FORTRAN V4.4-177 Page 1 ll-May-19B7 15l22l23 __DUAO: C BUCHAL. OPT3CVECT. FOR j 15 0001 subroutine c v e c t ( i m o d e , l l i n k , i o b e t , v l e n s , v e c t s , p o i n t s ) 0002 0003 c Access the database containing the c o r r e c t i o n vectors for the 0004 c current and previous l i n k p o s i t i o n s . 0005 0006 c Parameters I 0007 0008 c imode- 1- return v e c t l ( k - l ) , p o i n t l ( k - l ) , v e c t 2 ( k ) , p o i n t 2 ( k ) 0009 c 2- store ' ' 0010 c i l l n k - the moving l i n k i d 0011 c i o b s t - the s tat ionary obstacle i d 0012 c vlens- lengths of the c o r r e c t i o n vectors 0013 c vec t s - correc t ion vectors 0014 c p o i n t s - c o r r e c t i o n points 0015 0016 c Var iab les t 0017 c v e c t l ( k l a a t , i , i p o l n t ) - t r a n s l a t i o n vector appl ied to p o s i t i o n 0018 c k-1 of l i n k ' i l l n k ' due to penetrat ion of h u l l ( k - 1 ) 0019 c by obstacle ' i o b s t ' 0020 c where ipo int - ( i l ink-1>Anobst+iobs t 0021 c v e c t l ( k c u r r ,1 , i p o i n t ) - t r a n s l a t i o n vector appl ied to p o s i t i o n 0022 c k of l i n k ' l l i n k ' due to penetrat ion of h u l l ( k ) 0023 c by obstacle ' i o b s t ' 0024 c vect2- t r a n s l a t i o n vector appl ied to p o s i t i o n k-1 of l i n k 0025 c ' i l i n k ' due to penetrat ion of h u l l ( k ) by obstac le 0026 c ' i o b s t ' 0027 c 0028 c p o l n t l , p o i n t 2 - as above, for penetrat ion points on the l i n k s 0029 0030 c k c u r r , k l a s t - pointers to the s tructures for pos i t ions ( k ) , I k -0031 0032 rea l vec t l (2 ,2 ,100 ) ,po in t l (2 ,2 ,100 ) 0033 rea l vect2(2 ,100) ,point21 2,100) 0034 real v l ( 2 ) , v 2 ( 2 ) , p l ( 2 ) , p 2 ( 2 ) 0035 real vlens ( 2 ) , v e c t s ( 2 , 2 ) , p o i n t s ( 2 , 2 ) 0036 0037 common / i n t d i s / v e c t l , v e c t 2 , p o i n t 1 , p o i n t 2 0038 common / n o b j s / n l i n k s , n o b B t 0039 0040 save k c u r r , k l a s t 0041 0042 c I n i t i a l i s e . 0043 0044 l f (kcurr .eq .0) then 0045 k c u r r » l 0046 k l a s t - 2 0047 end i f 0048 0049 e For object pa ir l l i n k , i o b s t , 0050 c return v e c t l 1 k - 1 ) , p o i n t 1 1 k - 1 ) , v e c t 2 ( k ) , p o i n t 2 ( k ) . 0051 c and the vector lengths. 0052 0053 i f (imode.eq.l) then 0054 ipoint-(ilink - 1)Anobst+ l o b s t 0055 v l e n s ( l ) - 0 . 0056 v l e n s l 2 ) - 0 . 0057 do 20 1-1,2 CVECT 13-MaT-1987 12 l57 i 26 VAX FORTRAN V4.4-177 Page 2 ll-May-1987 15I22I23 _DUAO : CBUCHAL. 0PT3CVECT. FOR»15 005B v e c t s ( 1 , 1 ) - v e c t l ( k l a a t , 1 , i p o i n t ) 0059 p o i n t s ( i , l > » p o i n t l < k l a a t , i , i p o i n t ) 0060 v e c t a ( i , 2 ) - v e c t 2 ( i , i p o i n t ) 0061 p o i n t e d , 2 ) « p o i n t 2 ( i , i p o i n t ) 0062 , v l e n s ( l ) « v l e n s ( l ) + v e c t s d , l ) * * 2 0063 v l e n s ( 2 ) * v l e n B ( 2 ) + v e c t s ( i , 2)A * 2 0064 20 continue 0065 v l e n s ( l ) - s q r t ( v l e n s ( l ) ) 0066 v l e n s ( 2 ) » s q r t ( v l e n s ( 2 ) ) 0067 0068 e l s e i f (imode.eq.2) then 0069 0070 c For l i n k 'number', add v e c t l , p o l n t l , v e c t 2 , p o l n t 2 to database. 0071 0072 c Increment the p o s i t i o n . 0073 0074 l f ( i l i n k . e q . L a n d . i o b s t . a q . l ) then 0075 ksave=klast 0076 k l a s t - k c u r r 0077 kcurr-ksave 0078 endlf 0079 0080 l p o i n t - ( l l i n k - l ) * n o b s t + l o b s t ro 0081 do 50 1-1,2 K> 0082 v e c t l ( k c u r r , i , i p o i n t ) - v e c t s ( i , l ) ° 0083 p o i n t l ( k c u r r , i , i p o i n t ) - p o i n t s ( i , l ) 00B4 v e c t 2 ( i , i p o i n t ) - v e c t s ( 1 , 2 ) 00B5 p o l n t 2 ( i , i p o i n t ) - p o l n t s ( i , 2 ) 00B6 50 continue 00B7 0088 endlf 0089 0090 return 0091 end CVECT 13-May-1987 12i57>26 VAX FORTRAN V4.4-177 Page 3 ll-May-1987 15I22I23 DUAO : C BUCHAL. OPT3CVECT. FOR j 15 PROGRAM SECTIONS Nana 0 (CODE 2 fLOCAL 3 INTDIS 4 NOBJS T o t a l Space Al located Bytea A t t r i b u t e s 346 100 4800 8 5254 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE PIC OVR REL GBL SHR NOEXE PIC OVR REL GBL SHR NOEXE RD NONRT LONG RD RO RD WRT LONG HRT LONG WRT LONG ENTRY POINTS Address Type Name 0-00000000 CVECT VARIABLES Address Type Nana A* 1*4 I A* IA4 IPOINT 4-00000000 1*4 NLINKS Addraas Type Naae AP-0000000BO 1*4 ILINK 2-00000020 IA4 KCURR 4-00000004 1*4 NOBST Addraea Type Naae AP-00000004O 1*4 IMODE 2-00000024 1*4 KLAST Address Type Name AP-OOOOOOOCO) 1*4 IOBST A A 1*4 KSAVE ARRAYS Address Type Name Bytes Dimensions 2-00000010 RA4 PI B (2) 2-00000018 RA4 P2 B (2) 3-00000960 RA4 POINT1 1600 (2, 2, 100) 3-00O00FA0 RA4 POINT2 BOO (2, 100) AP-000000180 RA4 POINTS 16 (2, 2) 2-00000000 R*4 VI 8 (2) 2-00000008 R*4 V2 8 (2) 3-00000000 RA4 VECT1 1600 (2, 2, 100) 3-00000640 RA4 VECT2 800 (2, 100) AP-00000014O RA4 VECTS 16 (2, 2) AP-0000O010O RA4 VLENS B (2) LABELS Address Label Address Label A A 20 A A 50 CVECT 13-May-19B7 12IS7I26 l l-May-1987 15i22i23 VAX FORTRAN V4.4-177 DUAO : C BUCHAL. OPT3CVECT. FOR j 13 Page 4 FUNCTIONS AND SUBROUTINES REFERENCED Type Name R*4 MTH«SQRT COMMAND QUALIFIERS FOR/LIST OPT.FOR,RDFILE.FOR,INPUT.FOR,GROH.FOR,FINPAT.FOR,SOLVE1.FOR,INT2D.FOR,HULL2D.FOR,CMULT.FOR,JACOB.FOR,COSTT.FOR, / CHECK * (NOBOUNDS, OVERFLOW , NOUNDERFLOH) / DEBUG* (NOSTMBOLS, TRACEBACK) / STANDARD* (NOSYNT AX , NOSOURCE_FORM) / SHOW- (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONART, SINGLE) /WARNINGS*(GENERAL,NODECLARATIONS) /CONTINUATIONS = 19 / NOCROSS_REFERENCE /NOD_LINES /NOEXTEND_SOURCE /F77 /NOG FLOATING /I4 /NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run Timei 3.30 seconds Elapsed T l a e i 9.92 seconds Page F a u l t s • SB9 Dynamic Memory I B45 pages 13-Kay-19B7 13I00I10 l l-Hay-19B7 15i34t l5 VAX FORTRAN V4.4-177 DUAO:C BUCHAL.0PT3DRINV.FOR)11 Page 0001 0002 0003 0004 0005 0006 0007 OOOB 0009 0010 0011 0012 0013 0014 0013 0016 0017 001B 0019 0020 0021 0022 0023 0024 0025 0026 0027 002B 0029 0030 0031 0032 0033 0034 0035 0036 0037 003B 0039 0040 0041 0042 0043 0044 0045 0046 0047 0048 0049 0050 0051 0052 0053 0054 0055 0056 0057 111 c 13 29 112 113 program d r i n v Draw the t ra jec tory of jo int pos i t ions generated by the i t e r a t i v e inverse kinematic s o l u t i o n . character*20 f i l e ( 4 ) rea l obst<2,4) r e a l boxl<2,10,10> integer l inks(4) integer j t y p e t 4 ) , l i n k t 4 ) r e a l a lphaf4 ) , l en (4> , the ta l4 ) ,d i s t (4 ) common / r o b o t / l i n k , j t y p e , a l p h a , l e n , t h e t a , d i s t common /nrdof/ndof ,nxdof Read the kinematic parameters, c a l l r d f i l e Input the jo in t angle f i l e names. do 111 l « l , n d o f wr i t e !* ,* ) 'Enter j o i n t f i l e name' read!*,*) f i l e d ) continue I n i t i a l i z e the workstation geometry. o p e n ! 1 , f l i e * ' b o x . d a t ' . s t a t u s * ' o l d ' ) r e a d l l , * ) n l inks do 29 k - 1 , n l i n k s read(1,*) n v e r t , l i n k s ! k ) do 13 i » l , n v e r t r e a d l l , * ) I b o x l ( j , i , k > , j » l , 2 ) continue continue cloBel1) do 112 i * l , n d o f open! 10+i , f i l e » f i l e d ) , s t a t u s * ' o l d ' ) continue c a l l g r s t r t ( 4 0 0 6 , l ) c a l l newpag c a l l w i n d o w ! - 4 . , 4 . , - 4 . , 4 . ) c a l l vwport tO. ,100 . ,0 . ,100 . ) do 5 lndl '1 ,1000 do 113 i - l , n d o f readd0+i,*,end*100) the ta ( l ) continue c a l l t r i d n t f . f a l s e . ) i l i n k = l do 10 i n d - l , n d o f DRINV OOSB 0059 a n g l e « t h e t a ( i n d ) 0060 c a l l rotate langle ,angle ) 0061 c a l l t r a n s l ( l e n t i n d ) ,0 . ) 0062 0063 c a l l aove(0 . ,0 . ) 0064 l f ( l n d l . g t . l ) c a l l daahpt(l) 0065 c a l l d r a w l - l e n l i n d ) , 0 . ) 0066 i f ( i n d l . e q . l ) c a l l a r c ! . 0 5 , 0 . , 3 6 0 . 0067 c a l l danh.pt10) 0068 0069 i f ( ind.eq.ndof) then 0070 c a l l move(.2,.1> 0071 c a l l drawl 0 . , . 1 ) 0072 c a l l d r a w l O . , - . 1 ) 0073 c a l l d r a w l . 2 , - . l ) 0074 endif 0075 0076 10 continue 0077 0 0 7 B 0079 5 continue 0080 0081 100 continue 0082 O0B3 c a l l t r i d n t l . f a l s e . ) 0084 0085 do 11 i n d - l , n d o f 0086 00B7 ang le - theta l ind) 008B c a l l rotate I angle,angle) 00B9 c a l l t r a n s l ( l e n ( i n d ) , 0 . ) . 0090 0091 c a l l aove(0. ,0 . ) 0092 c a l l d r a w l - l e n ( i n d ) , 0 . ) 0093 c a l l a r c ! . 0 5 , 0 . , 3 6 0 . I 0094 c a l l dashpt(O) 0095 0096 11 continue 0097 0098 c a l l grstop 0099 0100 stop 0101 end 13-May-19B7 13|00|10 ll-May-19B7 15:34:15 VAX FORTRAN V4.4-177 Page DOA0 : t BUCHAL. 0PT3DRINV. FOR»11 DRINV 13-May-19B7 I 3 i 0 0 i l 0 l l-May-19B7 15i34j l5 VAX FORTRAN V4.4-177 Page 3 DUA0:tBUCHAL.OPT3DRINV.F0R>ll PROGRAM SECTIONS Nana Bytes A t t r i b u t e s 0 0CODE 729 PIC CON REL LCL SHR EXE RD NOHRT LONG 1 CPDATA Bl PIC CON REL LCL SHR NOEXE RD NOWRT LONG 2 (LOCAL 124B PIC CON REL LCL NOSHR NOEXE RD HRT LONG 3 ROBOT 96 PIC OVR REL GBL SHR NOEXE RD WRT LONG 4 NRDOF B PIC OVR REL GBL SHR NOEXE RD WRT LONG T o t a l Space Al loca ted 2162 ENTRY POINTS Address 0-00000000 Type Nane DRINV ro to VARIABLES Address Type Nane 2-000003B4 A A 2-000003AO RA4 ANGLE IA4 IND1 IA4 NLINKS Address Type Naae AA IA4 I AA 1*4 J 2-000003A4 1*4 NVERT Address Type Naae Address Type Naae AA AA 4-00000004 1*4 ILINK 1*4 K 1*4 NXDOF AA 4-00000000 IA4 IA4 IND NDOF ARRAYS Address Type Naae Byte a Diaensions 3-00000020 RA4 ALPHA 16 (4) 2-00000020 RA4 B0X1 BOO (2, 10, 10) 3-00000050 RA4 DIST 16 (4) 2-00000350 CHAR FILE BO (4) 3-00000010 IA4 JTYPE 16 (4) 3-00000030 RA4 LEN , 16 (4) 3-00000000 IA4 LINK 16 (4) 2-00000340 IA4 LINKS 16 (4) 2-00000000 RA4 OBST 32 (2, 4) 3-00000040 RA4 THETA 16 (4) LABELS Address AA AA Label 5 111 Address Label AA AA 10 112 Address Label AA AA 11 113 Address Label A A 1 3 Addreaa Label AA 29 Address Label 0-0000025F 100 DRINV FUNCTIONS AND SUBROUTINES REFERENCED Type Name Type Name Type Nana ARC DASHPT DRAW GRSTRT MOVE NEHFAG TRIDNT VWPORT WINDOW to to 13-May-19B7 13 l00 i l0 VAX FORTRAN V4.4-177 Page ll-May-19B7 15I34I15 DUAO:tBUCHAL.0PT3DRINV.FOR»11 Type Name Type Name Type Name FOR$CLOSE FOR$OPEN GRSTOP RDFILE ROTATE TRANSL 13-May-1987 13:00J24 VAX FORTRAN V4.4-177 Page 1 ll-May-1987 15837:48 DUAO:CBUCHAL.0PT3DRLINK.FOR;54 0001 program d r l l n k 0002 0003 c Draw the d i s cre te jo in t t r a j e c t o r y generated by OPT. 0004 0005 character*20 f i l e ( 4 ) 0006 rea l boxl(2,10,10),box2<2,10,10) 0007 integer llnk5<4> 0008 integer jtype 14>,link!4) 0009 rea l a l p h a ( 4 ) , l e n ( 4 ) , t h e t a l 4 ) , d i s t ( 4 ) . 0010 0011 common / r o b o t / l i n k , j t y p e , a l p h a , l e n , t h e t a , d i s t 0012 common /nrdof /ndof ,nxdof 0013 0014 c Read the kinematic parameter* from a f i l e . 0015 0016 c a l l r d f i l e 0017 001B c Enter the names of the j o i n t angle f i l e s . 0019 0020 do 111 l ' l , n d o f 0021 wri te !* ,* ) 'Enter j o i n t ' , i , ' f i l e name' 0022 read!*,*) f i l e d ) 0023 111 continue ro 0025 c I n i t i a l i z e the geometry of the workstat ion. 0026 0027 o p e n ! 1 , f i l e » ' b o x . d a t ' , s t a t u s * ' o l d ' ) 0028 r e a d d , * ) n l inks 0029 do 29 k « l , n l i n k s 0030 r e a d l l , * ) n v e r t , l i n k s ( k ) 0031 do 13 i » l , n v e r t 0032 r e a d l l , * ) ( b o x l I j , i , k ) , j * l ,2) 0033 13 continue 0034 29 continue 0035 r e a d l l , * ) nobst 0036 do 19 k-1,nobst 0037 r e a d l l , * ) nvert 0038 do 14 1*1,nvert 0039 r e a d l l , * ) Ibox21j , i , k ) , j -1 ,2 ) 0040 14 continue 0041 19 continue 0042 c l o s e d ) 0043 0044 do 112 1-1,ndof 0045 o p e n ! 1 0 + 1 , f i l e - f l i e ! 1 ) , s t a t u s - ' o l d ' ) 0046 112 continue 0047 0048 c a l l g r s t r t ( 4 0 0 6 , l ) 0049 c a l l newpag 0050 c a l l w i n d o w ( - 4 . , 4 . , - 4 . , 4 . ) 0031 c a l l vwport lO. ,100 . , 0 . , 100 . ) 0052 0053 do 22 k-1,nobst 0054 c a l l move lbox2 ( l , l , k ) ,box2 (2 , l ,k ) ) 0055 c a l l draw(box2(l,2,k),box2<2,2,k>) 0056 c a l l drawlbox2{1,3,k),box2{2,3,k)) 0057 c a l l draw(box2t l ,4 ,k) ,box2(2,4 ,k)) DRLIHK 13-May-19B7 13:00)24 VAX FORTRAN V4.4-177 Page 2 ll-May-19B7 15:37:4B DUAO:CBUCHAL.0PT3DRLINK.FOR:54 ro ro co 005B 0059 0060 0061 0062 0063 0064 0065 0066 0067 0068 0069 0070 0071 0072 0073 0074 0075 0076 0077 007B 0079 0080 O0B1 0082 0083 0084 0085 0086 O0B7 OOBB O0B9 0090 0091 0092 0093 0094 0095 0096 0097 009B 0099 0100 0101 0102 0103 0104 c a l l drawlbox2( l , l ,k ) ,box2(2 , l ,k>) 22 continue do 5 indl-1,1000 do 113 i « l , n d o f read(10+i,*,end=100) the ta ( l ) 113 continue c a l l t r i d n t l . f a l a e . ) i l i n k - 1 do 10 ind* l ,ndof ang 1 e » t he ta I i nd) c a l l rotate(angle ,angle) c a l l t r a n s l l l e n t i n d ) ,0 . ) c a l l a o v e l O . , 0 . ) c a l l dashpt( l ) i f d n d . e q . l i n k a d l i n k ) ) then c a l l n o v e l - l e n t i n d ) , 0 . ) e lee c a l l drawl - l en l ind) ,0 . ) endif c a l l daahpt(O) l f ( ind . eq. l i n k e d l i n k ) ) then c a l l « o v e ( b o x l 1 1 , 1 , i l i n k ) , b o x l ( 2 , 1 , i l i n k ) ) c a l l d r a w ( b o x l ( l , 2 , i l i n k ) , b o x l ( 2 , 2 , i l i n k ) ) c a l l d r a w l b o x l ( 1 , 3 , i l i n k ) , b o x l ( 2 , 3 , i l i n k ) ) c a l l d r a w ( b o x l ( l , 4 , i l i n k ) , b o x l ( 2 , 4 , i l i n k ) ) c a l l d r a w ( b o x l ( 1 , 1 , i l i n k ) , b o x l 1 2 , 1 , i l i n k ) ) i l i n k » i l i n k + l endif 10 continue 5 continue 100 continue c a l l gretop •top end DRUNK 13-May-19B7 13I00I24 VAX FORTRAN V4.4-177 Page 3 ll-May-1987 15i37:4B DUAO : C BUCHAL. 0PT3DRLINK. FOR 154 PROGRAM SECTIONS Name Bytea A t t r i b u t e s 0 (CODE 1001 PIC CON REL LCL SHR EXE RD NOHRT LONG 1 $PDATA e i PIC CON REL LCL SHR NOEXE RD NOWRT LONG 2 $ LOCAL 1964 PIC CON REL LCL NOSHR NOEXE RD WRT LONG 3 ROBOT 96 PIC OVR REL GBL SHR NOEXE RD WRT LONG 4 NRDOF B PIC OVR REL GBL SHR NOEXE RD WRT LONG T o t a l Space Al loca ted 3130 ENTRY POINTS Address 0-00000000 Type Nane DRLINK VARIABLES fo Address Type Nane 2-000006BB R*4 ANGLE A* IA4 IND1 2-000006AO 1*4 NLINKS Address Type Nane AA IA4 I AA 1*4 J 2-000006AB 1*4 NOBST Address Type Naae AA IA4 ILINK AA IA4 K 2-000006A4 1*4 NVERT Address Type Naae AA IA4 IND 4-00000000 IA4 NDOF 4-00000004 IA4 NXDOF ARRAYS Addreaa Type Nane Bytes Dimensions 3-00000020 RA4 ALPHA 16 (4) 2-00000000 RA4 B0X1 BOO i l , 10, 10) 2-00000320 R*4 BOX 2 BOO (2, 10, 10) 3-00000050 RA4 DIST 16 (4) 2-00000650 CHAR FILE BO (4) 3-00000010 IA4 JTYPE 16 (4) 3-00000030 RA4 LEN 16 (4) 3-00000000 IA4 LINK 16 (4) 2-00000640 IA4 LINKS 16 (4) 3-00000040 RA4 THETA 16 (4) LABELS Address Label Address Label Address Label Address Label Address Label Address Label AA 5 AA 10 AA 13 AA 14 AA 19 AA 22 AA 29 0-000003DB 100 AA 111 A* 112 AA 113 DRLINK FUNCTIONS AND SUBROUTINES REFERENCED Type Nane DASHPT HOVE VHPORT Type Naae DRAW NEHPAC WINDOW 13-May-19B7 13I00I24 VAX FORTRAN V4.4-177 Page ll-May-19B7 15i37i4B DUAO:CBUCHAL.OPT3DRLINK.FOR)54 Type Nana Type Name Type Nane Type Nane FOR$CLOSE FOR«OPEN GRSTOP GRSTHT RDFILE ROTATE TRANSL TRIDNT 13-Hay-1.987 12153132 VAX FORTRAN V4.4-177 P a g . 1 l l-May-1987 1 3 i l l i 2 2 DUAO iCBUCHAL. OPT3FINPAT.FOR»431 to 0001 subroutine f i n p a t l v a l u e s ) 0002 0003 c Th i s subroutine numerical ly optimizes a manipulator t r a j e c t o r y . 0004 c The I n i t i a l conf igurat ion i s g iven , and the f i n a l conf igurat ion 0003 c (the goal poai t ion) must s a t i s f y the f i n a l p o s i t i o n c o n s t r a i n t . 0006 c The f i n a l p o s i t i o n i s moved over the 0007 c constra int surface to optimize the f u n c t i o n a l . OOOB c 0009 c The cost func t iona l i s the Integral of several coat terms over the ] 0010 c The cost terms are of quadratic form and inc lude! 0011 c 1. j o i n t acce lerat ions 0012 c 2. j o in t l i m i t v i o l a t i o n s 0013 c 3. interference (penetration) approximated by j o i n t changes 0014 c 0013 c The cost func t iona l l a minimized by i t e r a t l v e l y modifying the 0016 c t ra jec tory along the gradient of the coat f u n t i o n a l using a 0017 c quasi-Hewton i t e r a t i o n s e r i e s . oo ia c The convex h u l l enclos ing two adjacent l i n k pos i t ions i s used 0019 c for the in ter ference c a l c u l a t i o n s . 0020 0021 c 0022 c va lues- an array of j o i n t values d e f i n i n g the t r a j e c t o r y . 0023 c On en try , contains the s t a r t i n g t r a j e c t o r y . 0024 c On e x i t , contains the optimized t r a j e c t o r y . 0023 0026 c The fo l lowing output tes t f i l e s are generated! 0027 0028 c fmax.dat- larges t jo in t v i o l a t i o n s for each I t e r a t i o n 0029 c dmax.dat- larges t penetrations f o r each i t e r a t i o n 0030 c l a a t . d a t - number of pos i t ions i n t r a j e c t o r y f o r each 0031 c i t e r a t i o n 0032 c d i a t . d a t - largest penetrations along t r a j e c t o r y 0033 c c o s t l . d a t - t o t a l value of cost func t iona l 0034 c cos t2 .dat - value of interference component 0035 c cos t3 .dat - value of jo in t l i m i t component 0036 c cos t4 .dat - value of j o i n t a c c e l e r a t i o n component 0037 c a n g l . d a t - f i l e of I th j o i n t angles 003B 0039 0040 0041 r e a l l e n t 4 ) , d i s t ( 4 ) . a l p h a ( 4 ) , t h e t a ( 4 ) , t h e t a l ! 4 ) , t h e t a 2 ( 4 ) 004 2 integer j t y p e ( 4 ) , l i n k ( 4 ) 0043 r e a l xgoal(3) 0044 r e a l mat(4,5) ,matl(4,3) 0043 r e a l tnet(4,4) 0046 r e a l values(4,100) 0047 r e a l nvalue(4,100) 004B r e a l d e l l ( 4 ) , d l a s t ( 4 ) 0049 r e a l cost(lOOO) 0030 r e a l thedot(4) 0051 r e a l thetot(4) 0052 r e a l delx(2) 0053 r e a l temp(4) 0054 r e a l homog(4,4) 0055 rea l vmin(4),vmax(4) 0056 r e a l vllm(4) 0057 r e a l jmult(4) FINPAT 13-May-19B7 12I93I32 VAX FORTRAN V4.4-177 Page 2 ll-May-19B7 1 3 l l l i 2 2 PUA0:CBUCHAL.0PT3FINPAT.F0R»431 0098 r e a l pault(4) 0059 r e a l theaavl 4 ,100) ooeo l o g i c a l i n t f l g 0061 r e a l b o x e s ( 2 , 1 0 , 1 0 ) , t b o x e » ( 2 , 1 0 , 1 0 ) 0062 r e a l boxl(2,10),box2(2,10,10) 0063 r e a l vec t (2 ) ,po int (2 ) 0064 r e a l hu l l (2 ,20 ) 0065 l o g i c a l lnf lag( lOO) 0066 Integer lverta(2 ,20) 0067 r e a l lboxl2,10) ,obox(2,10) 006B r e a l aault<4) 0069 r e a l j ac (6 ,5 ) , ident (4 ,4 ) 0070 integer adl iat (100) ,aublat (100) 0071 r e a l accC100),vel(100) 0072 r e a l lndlat ( lOO) 0073 r e a l maxpen, j t o l e r 0074 l o g i c a l atop 0075 r e a l thdotl (4) 0076 r e a l delxl<2),delx2(2) 0077 r e a l theta3(4),theta4(4) 0078 r e a l x laa t (2 ) ,xcurr (2 ) 0079 r e a l dangle(4) 0080 r e a l xeat(2,100),xact(2,100) 0081 r e a l atera(4) 0082 O0B3 c o n o n / r o b o t / l i n k , j t y p e , a l p h a , l e n , t h e t a , d i s t 00B4 common / p a r a m l / j f a c t , p f a c t 0089 common /paraa3/afact 00B6 common /nmax/nmax 00B7 common / t a u / t a u 00B8 common /goa l /xgoa l ,angoa l 00B9 common / i d e n t / i d e n t 0090 common / l a s t / l a s t 0091 common / l i a i t s / v m l n , v a « x 0092 common / n o b j s / n l i n k s , n o b s t 0093 common /cmax/caax 0094 common / s t e p s i / s t e p s l 0099 common / l lmto l /maxpen ,J to l er 0096 common /nrdof /ndof ,nxdof 0097 0098 c I n i t i a l i s e parameters. 0099 0100 d2rad-.0174533 0101 to ler* .01 0102 to ler l - .00001 0103 cmin"4. 0104 s t o p * . f a l s a . 0105 0106 c Copy the i n i t i a l and f i n a l conf igurat ion j o i n t values . 0107 010B do 222 1-1,ndof 0109 theta1(1) -valuea( i ,1 ) 0110 theta2(1) -va lues(1 , las t ) 0111 222 continue 0112 0113 c Open some f i l e s for a tor lng r e s u l t s . 0114 FINPAT 0115 0116 ' 0117 0118 0119 0120 0121 0122 0123 0124 0125 0126 0127 012B 0129 0130 0131 0132 0133 0134 0135 0136 0137 013B 0139 0140 0141 0142 0143 0144 0145 0146 0147 014B 0149 0150 0151 0152 0153 0154 0155 0156 0157 015B 0159 0160 0161 0162 0163 0164 0165 0166 0167 016B 0169 0170 0171 13-May-19B7 12l53l32 ll-May-1987 1 3 i l l i 2 2 VAX FORTRAN V4.4-177 DUAO IC BUCHAL. OPT3F1NPAT. FOR 1451 Page 113 334 333 900 open! open! open! opent open! open! open! optnl open! open! open! open! I . f i l e -3 , f i l e -5 , f i l e -7 , f i l e -I I , f i l e ' 12, f i l e ' 13, f i l e ' 14, f i l e * 20, f i l e ' 21, f i l e -22, f l i e -23, f l i c fmax.dat' dmax.dat' l a a t . d a t ' d i e t . d a t ' 1 ' c o a l . d a t 'coa2.dat 'cos3.dat 'coa4.dat 'ang l .dat 'ang2.dat 'ang3.dat 'ang4.dat , s tatus*' , status*' , status*' , status*' ' , s ta tus ' ' , status' ' , s tatus' ' , status' ' , s ta tue ' ' , status' ' . s ta tus ' ' , s t a t u s 1 new') new' ) new') new' ) 'new') 'new') 'new') 'new') 'new') 'new') 'new') 'new') I t e r a t l v e l y Improve the t r a j e c t o r y u n t i l the coat f u n c t i o n a l i s minimized. I n i t i a l l y a very coarse t r a j e c t o r y with large steps between ajacent pos i t ions i s used. The t r a j e c t o r y i s progres s ive ly subdivided with each i t e r a t i o n u n t i l the step s ize l i n e a r i z a t i o n error i a wi th in a preselected to lerance . i n d l - 0 continue i f ( lndl .ge .nmax.or . s top) then goto 100 e l se indl - lndl+1 I terate along the t r a j e c t o r y , updating the jo in t values a t each p o s i t i o n us ing a quasi-Newton procedure to reduce the cost f u n c t i o n a l . Transform the i n i t i a l manipulator l i n k p o s i t i o n s . Transform the data base, then load the array of v e r t i c e s . c a l l trandb(thetal ) do 900 l l l n k * l , n l l n k s c a l l g v e r t s ( 2 , l , i l i n k , l b o x , n v l l n k ) do 333 l - l , n v l l n k do 334 J * l , 2 b o x 2 ( j , i , i l i n k ) * l b o x ( J , i ) continue continue continue Modify the Intermediate t ra jec tory po in t s . nadd*0 nsub*0 dmax*0. fnax*0. l s t a r t - 2 FIST AT 13-May-1987 12IS3I32 VAX FORTRAN V4.4-177 Page 4 ll-May-19B7 1 3 i l l i 2 2 DUAO: [BUCHAL. 0PT3FINPAT.F0R>451 0172 iend-last+1 0173 0174 do 200 i n d 2 - l a t a r t , l e n d 0173 i n f l a g ( i n d 2 ) • . f a l s e . 0176 iprev=ind2- l 0177 i n d i a t ( I p r e v ) « 0 . 017B 0179 do 201 1-1,ndof 0180 theta(1) -values(1 , lnd2) 0181 201 continue 0182 01B3 c Transform the l i n k v e r t i c e s In world coordinates . 0184 0183 c a l l trandbttheta) 0186 01B7 c Specify the l i n k pos i t ions to be tested against the obstac les 018B 01B9 do 1B0 i l i n k - 1 , n l i n k s 0190 c a l l g v e r t a t 2 , 1 , i l i n k , l b o x , n v l l n k ) 0191 0192 do 560 i - l , n v l i n k 0193 do 561 j - 1 , 2 0194 boxlt j , i ) - b o x 2 ( j , i , i l i n k ) 0193 b o x 2 ( j , i , i l i n k ) - l b o x t J , i ) 0196 361 continue 0197 560 continue 0198 0199 0200 c Ca lcu la te the convex h u l l s representing the swept volume 0201 c between sequent ia l p o s i t i o n s . 0202 0203 c If the two pos i t ions are i d e n t i c a l , then the h u l l Is jus t 0204 c b o x l . 0203 0206 i f <ind2.eq. last+l) then 0207 0208 n v e r t « n v l l n k 0209 do 135 1*1,nvert 0210 do 156 j -1 ,2 0211 h u l l ! j , i ) - b o x l ( J , i ) 0212 156 continue 0213 i v e r t s t l , i ) - l 0214 i v e r t s ( 2 , l ) - i 0213 153 continue 0216 0217 e lse 0218 0219 c a l l h u l l 2 d ( b o x l , l b o x , n v l i n k , n v l i n k , h u l l , n v e r t , 1 v e r t s ) 0220 0221 endif 0222 0223 c Perform interference tes ts between the h u l l s and the 0224 c obstac les . 0223 0226 do 1B1 lobst -1 ,nobst 0227 c a l l gverts t1 ,2 , iobs t ,obox ,nvobst ) 0228 FINPAT 13-May-19B7 12i53i32 VAX FORTRAN V4.4-177 Page 5 ll-May-19B7 1 3 i l l l 2 2 DUA0iCBUCHAL.OPT3FINPAT.FOR»451 0229 c a l l i n t e r f ( h u l l , o b o x , n v e r t , n v o b i t , l l i n k , i o b s t , 0230 l v e r t a , l n t f l g ) 0231 0232 ln f lag( ind2) - in f lag< i n d 2 ) . o r . i n t f l g 0233 0234 181 continue 0239 1B0 continue 0236 0237 c If a t r a j e c t o r y poa l t l on r e s u l t s i n interference with the 023B c •wept volume, aodify the t r a j e c t o r y to avoid the in ter ference 0239 0240 i f ( l n f l a g ( i n d 2 ) . o r . i n f l a g ( i p r e v ) ) then 0241 0242 do 209 i - l , n d o f 0243 t h e t a ( 1 ) • v a l u e * ( i . i p r e v ) 0244 209 continue 0243 0246 c a l l c o r e c t l t h e d o t , t h d o t l , i n d i » t ( l p r e v ) ) 0247 024B l f < ind i s t l lprev ) . g t .daax ) then 0249 dmax*indlst( lprev) 0230 endif 0251 0232 do 312 i » l , n d o f 0233 thesav!1 , lprev)"thedot( i ) 0234 p n u l t ( i ) » p f a c t 0255 312 continue 0236 0257 e l i e 023B 0259 do 313 1-1,ndof 0260 t h e s a v ( i , l p r e v ) " 0 . 0261 pmul t ( i ) -0 . 0262 313 continue 0263 0264 endif 0265 0266 c Ca lcu la te the jo in t H a l t penalty funct ion c o e f f i c i e n t s . 0267 026B do 119 1-1,ndof 0269 i f ( va luea t i , lprev ) . g t . vmax l1 ) ) then 0270 f t emp-abs !va lues ! i , lprev ) -vaax( i ) ) 0271 i f (ftemp.gt.fmax) faax-ftemp 0272 j m u l t ( i ) - j f a c t 0273 v l l m ( i ) « v m a x ! i ) 0274 e l s e l f ( v a l u e s ! ! , l p r e v ) . I t . v m i n ( i ) ) then 0273 f temp*abs(va lues ! i , lprev) -vmin( i ) ) 0276 i f (ftemp.gt.fmax) f m a x « f t e m p 0277 j a u l t ( i ) = j f a c t 027B v l i m ( i ) - v m i n ( l ) 0279 e lse 02B0 j m u l t ( i ) « 0 . 0281 endif 02B2 119 continue 0283 0284 0285 c Calculate the acce l era t ion a in imlxat ion weighting matrix. FINPAT 13-May-19B7 12•53•32 VAX FORTRAN V4.4-177 Page 6 l l-Hay-19B7 1 3 i l l s 2 2 DUAO i C BUCHAL. 0PT3FINPAT. FOR » 451 02B6 do 262 i - l , n d o f 02B7 02BB 02B9 l f ( i p r e v . e q . l ) then 0290 i i u l t l i l ' O . 0291 else 0292 a m u l t ( i ) » a f a c t 0293 end i f 0294 0295 262 continue 0296 0297 0298 0299 c Update the j o i n t v a r i a b l e s , us ing a quasl-Newton 0300 c search d i r e c t i o n . 0301 c The step s ize i s v a r i a b l e . 0302 0303 i c c l i p r e v ) - 0 . 0304 0305 dp 260 i - l , n d o f 0306 0307 c Ca lcu la te the elements of the Hesalan matrix . In 0308 c t h i s case, the Hessian 0309 c matrix la a l inear combination of the diagonal weighting 0310 c matr icea , and i t s inverse i s found by inver t ing the 0311 c diagonal elements. 0312 0313 denom-jmult(1)+pmult(1)+amult(i)A6. 0314 0315 denoml-jmult(1)+pmult(1)+anult(i)A3. 0316 0317 c Ca lcu la te the acce l era t ion term for d i f f e r e n t cases. 031B 0319 l f ( I p r e v . e q . l ) then 0320 a term( l ) -0 . 0321 e l s e i f ( iprev .eq .2 ) then 0322 a t e r a ( i ) - - 3 . A v a l u e s ( 1 , i p r e v - 1 ) - 4 . A v a l u e s ( i , i p r e v + 1 ) + 0323 A values! i , iprev+2) +6. Avalues! i , lprev) 0324 e l s e i f ( i p r e v . e q . l a s t - 1 ) then 0325 atera!1)-1 v a l u e s ( i , i p r e v - 2 ) - 4 . A v a l u e s ! i , i p r e v - 1 ) -0326 A 3 .Ava lues ! i , iprev+1) )+6 .Ava lues ! i , iprev) 0327 e l s e i f ( i p r e v . e q . l a a t ) then 0328 a t e r m ! i ) - v a l u e s ! i , i p r e v - 2 ) - 4 . A v a l u e s ! 1 , i p r e v - 1 ) + 0329 A 3 . A v a l u e s ! i , l p r e v ) 0330 else 0331 a t e r a ! 1 ) - v a l u e s ! i , l p r e v - 2 ) - 4 . A v a l u e s ( 1 , i p r e v - 1 ) -0332 A 4 .Avalues! i , iprev+1) lvalues!1 , iprev+2)+ 0333 A 6 . A v a l u e s ( i , i p r e v ) 0334 endlf 0335 0336 c Calcu la te and save the norm of the a c c e l e r a t i o n . 0337 c This i s necessary only i f the acce lerat ions are to 0338 c be saved i n a f i l e . 0339 0340 i f ( i p r e v . e q . l a s t ) then 0341 a t e m p - ! v a l u e s ! 1 , i p r e v - 1 ) - v a l u e s ! i , i p r e v ) ) 0342 e l s e i f ( i p r e v . e q . l ) then FINPAT 13-May-1987 12i53i32 VAX FORTRAN V*.4-177 Page 7 l l -Kay-19B7 1 3 : l l i 2 2 DUAOi CBUCHAL.OPT3FINPAT.FOR1431 0343 a t e a p " ! v a l u e s ! i , i p r e v + 1 ) - v a l u e s ! i ,iprev)) 0344 e lse 0343 a t e a p " ( v a l u e s ! i , i p r e v + 1 ) - 2 .Avalues ( i , i p r e v ) + 0346 * va lues (1 , iprev-1 ) ) 0347 e n d i f 0348 0349 a c c ! i p r e v ) » a c c ( i p r e v ) + a t e a p * A 2 0330 0331 0332 •c Ca lcu la te the quas i -Newton search d i r e c t i o n 0333 c of t h e coat f u n c t i o n a l . 0354 0335 i f ( i p r e v . q t . 1 . a n d . i p r e v . I t . l a s t ) then 0356 0357 i f (denoa.eq.0.) then 0358 0359 d l a s t ( i ) - 0 . 0360 0361 e lse 0362 0363 i f ( iprev .eq .2 ) then 0364 d l a s t ( l ) - - ! j a u l t ! i ) A ( v a l u e s ( i , i p r e v ) - v l i a ( i ) ) -0365 A p a u l t ( i ) A t h e d o t ( i ) + a n u l t < i ) * ( - 3 . A v a l u e s ( i , i p r e v 0366 A 6 .Avalues ( 1 , i p r e v ) - 4 .Avalues 1 1 , i p r e v + 1 ) + 0367 A values ) i , iprev+2) ) ) /denoa 0368 e l s e i f ( i p r e v . e q . l a s t - 1 ) then 0369 d l a s t ( i ) - - ! j a u l t l l ) A ( v a l u e s ( l , i p r e v ) - v l l n ( l ) ) -0370 A p a u l t ( l)Athedot ( l ) + a a u l t ( l)A ( v a l u a s ( l , l p r e v - 2 ) -0371 A 4 .Avalues ( 1 , i p r e v - 1 ) + 6 .Avalues ( 1 , i p r e v ) -0372 A 3.Avalues!1 , iprev+1) ) ) /denoa 0373 e lse 0374 d l a s t ( 1 ) • - ( j a u l t ( i)A ( v a l u e s ( 1 , i p r e v ) - v l i a ! I ) ) -0375 A p a u l t ! l)Athedot ( i ) + a a u l t ( i)A ( v a l u e s ( 1 , i p r e v - 2 ) -0376 A 4 . A v a l u e s ! i , i p r e v - 1 ) + 6 . A v a l u e s ( i , i p r e v ) -0377 A 4 .Avalues ( 1 , i p r e v + 1)+values(1 , i p r e v + 2 ) ) ) / d e n o a 0378 endif 0379 03B0 endif 0381 03B2 e l s e i f ( i p r e v . e q . l a s t ) then 03B3 03B4 l f (denoal .eq .0 . ) then 0385 d laat ( i )>0 . 0386 e lse 0387 d l a s t ( 1 ) • - ( j a u l t ( 1)A ( v a l u e s ( 1 , i p r e v ) - v l l a ! i ) ) -038B A pault ( l)Athedot ( i ) + a m u l t ( l>A ( v a l u e s ( i , l p r e v - 2 ) -03B9 A 2 .Avalues ( 1 , i p r e v - 1 ) + v a l u e s ( i ,iprev) ) ) / d e n o n l 0390 endlf 0391 0392 endlf 0393 0394 260 continue 0395 0396 a c c ! i p r e v ) » s q r t ( a c c ( i p r e v ) ) 0397 0398 c Ca lcu la te the step s i z e . 0399 FINPAT 13-May-19B7 12153132 VAX FORTRAN V4.4-177 Page B ll-May-19B7 13 1111 22 DUAO iCBUCHAL.0PT3FINPAT.F0R»451 0400 dmult-tau 0401 0402 c For th* l a s t p o a i t l o n , project the dealred jo in t change onto 0403 c the n u l l apace o f J B O that the too l coordinate ! are not 0404 c changed. 0405 0406 i f ( i p r e v . e q . l a s t ) then 0407 0408 c Ca lcu la te the jacobian for the end conf igurat ion . 0409 0410 c a l l J a c o b ! I d e n t , i d e n t , l e n , a l p h a , d i e t , t h e t a 2 , j t y p e . 0411 A n d o f , t n e t , j a c ) 0412 0413 do 25 i - l , n d o f 0414 a a t ( l , i ) - j a c ( 4 , l > 0415 mat ( 2 , i ) - j a c ( 5 , i ) 0416 n a t l ( l , i ) « j a c ( 4 , l ) 0417 n a t l (2,1) • jac (5,1) 0418 25 continue 0419 0420 do 411 i - 1 , 2 0421 x l a B t ( i ) - t n e t ( i , 4 ) 0422 x a c t ( i , l a a t ) - x l a a t ( 1 ) 0423 411 continue 0424 0425 c F i n d the hoaogeneous part of t h e inverse s o l u t i o n . 0426 0427 c a l l s o l v e l(mat,ndof , n d o f , 2 , 4 , h o a o g ) 042B 0429 c a l l gx iu l t (hoaog ,d las t ,de l l ,ndof ,ndof ,1 ,4 ,4 ,4 ) 0430 0431 do 419 1-1,ndof 0432 d a n g l e ( 1 )-daultAdell( 1 )Ad 2 r a d 0433 419 continue 0434 0435 c C a l c u l a t e the l i n e a r i z e d estimate of the updated gr ipper p o a i t l o n 0436 0437 c a l l gault(mat 1 .dang le ,de lx l , 2 ,ndof ,1 ,4 ,4 ,2 ) 0438 0439 do 415 1-1,2 0440 x l a s t ( l ) - x l a s t ( i ) + d e l x l ( l ) 0441 415 continue 0442 0443 c Ca lcu la te t h e updated jo in t values and the corresponding gr ipper 0444 c p o s i t i o n . 0445 0446 do 412 l> l ,ndof 0447 theta3(1)-theta2(1)+dmultAdel1(1) 0448 412 continue 0449 0450 c a l l J a c o b ( i d e n t , i d e n t,len,a l p h a , d i s t , t h a t a 3 , j t y p e , n d o f.tnet,jac) 0451 0452 c Ca lcu la te the l i n e a r i z e d formula e r r o r , and scale the j o i n t 0453 c change. 0454 0455 do 413 1-1,2 0456 d e l x l ( l ) » t n e t ( i , 4 ) - x l a a t ( l > FINPAT 13-May-1987 12i53•32 VAX FORTRAN V4.4-177 Page 9 l l-May-19B7 1 3 i l l i 2 2 __DUAO t CBUCHAL. OFT3FINPAT.FOR »451 0457 045B 0459 0460 0461 0462 0463 0464 0465 0466 0467 0468 0469 0470 0471 0472 0473 0474 0475 0476 0477 0478 0479 04B0 0481 04B2 04B3 04B4 0485 04B6 0487 04B8 0489 0490 0491 0492 0493 0494 0495 0496 0497 0498 0499 0500 0501 0502 0503 0504 0505 0506 0507 050B 0509 0510 0511 0512 0513 413 160 104 c 111 12 11 161 105 continue dtempl-sqrt<delxl(1)A*2+delxl(2)**2) l f (dtempl .gt .atepal ) than dault-dmultAatepal/dtempl •ndif Update the f i n a l conf igurat ion . do 160 i - l,ndof n v a l u e d , l a a t >-valuea( i , laat )+dmult*dell( i) t h e t a 2 ( i ) » n v a l u e ( i , l a s t ) continue continue Calcu la te the change i n the t o o l poa i t l on due to l i n e a r i z a t i o n error. c a l l J a c o b ( i d e n t , i d e n t , l e n , a l p h a , d i a t , t h e t a 2 , j t y p e , ndof ,tnet,Jac) delen"0. do 111 i - 1 , 2 d e l z ( i ) - t n e t ( l , 4 ) - z g o a l ( l ) d e l e n » d e l e n + d e l z ( i ) * * 2 continue delen-aqrt(delen) i f ( d e l e n . l t . t o l e r ) then goto 105 e lse Remove the e r r o r . do 12 1*1,ndof m a t ( l , i ) - j a c ( 4 , i ) mat (2 , i ) - jac (5 ,1 ) continue do 11 i -1 ,2 mat(1 ,ndof+1)-delz( i ) a a t l ( 1 , n d o f + 1 ) « d e l z < 1 ) continue c a l l aolve2(2,mat,4,4,nxdof,ndof,homog,thedot) do 161 1-1,ndof nva lue (1 , laa t ) -nva lue (1 , laa t ) - thedot (1 ) /d2rad t h e t a 2(i)-nvalue(i , l a a t ) continue goto 104 endif continue FIHPAT 13-Ma.y-1.9B7 12 • 53132 VAX FORTRAN V4.4-177 Pag* 10 l l-May-1987 13tll>22 DUAOiCBUCHAL.OPT3FINPAT.FOR»451 0514 alse 0515 0516 c Eat iaa ta th* l o c a l l i n e a r i t y of the Jacobian, and determine a 0517 c su i tab le jo in t step s i z e . 051B 0519 do 310 i - l , n d o f 0520 t h e t a 3 ( i ) - v a l u e s ( i , i p r e v ) 0521 theta4(1)-theta3(i)+dmult*dlast(1) 0522 dangle(1)-dmultAdlaat(1)*d2rad 0523 310 continue 0524 0525 c Ca lcu la te the Jacobian at the current conf igura t ion . 0526 0527 c a l l J a c o b ( i d e n t , i d e n t , l e n , a l p h a , d i s t , t h e t a 3 , j t y p e , n d o f , t n e t , j a c ) 0528 0529 do 315 i - l , 2 0530 x l a s t ( l ) - t n e t ( l , 4 ) 0531 xact d , i p r e v ) - x l a s t ( 1 ) 0532 315 continue 0533 0534 do 311 1-1,2 0535 do 317 j - l , n d o f 0536 • a t d , j ) - j a c ( i + 3 , j ) 0537 317 continue 0538 311 continue 0539 0540 c Ca lcu la te the change i n the gr ipper p o s i t i o n based on a l i n e a r 0541 c Jacobian and a des ired j o i n t change. 0542 0543 c a l l gmul t (mat ,dang le ,de lz l ,2 ,ndof ,1 ,4 ,4 ,2 ) 0544 0545 c Ca lcu la te the t r a j e c t o r y step s i ze e r r o r . 0546 0547 do 331 1-1,ndof 0548 d a n g l e d ) « ( v a l u e a ( i , i p r e v + 1 ) - v a l u e s ( 1 , i p r e v ) ) * d 2 r a d 0549 331 continue 0550 0551 c a l l gmult lmat ,dangle ,de lz2 ,2 ,ndof ,1 ,4 ,4 ,2 ) 0552 0553 do 332 i - l , 2 0554 xestt i , i p r e v + 1 ) - z l a a t ( i >+delz2(i) 0555 332 continue 0556 0557 c Ca lcu la te the Jacobian for the updated jo in t c o n f i g u r a t i o n , and 0558 c f i n d the actual new gr ipper p o s i t i o n . 0559 c A measure of the n o n - l i n e a r i t y of the Jacobian , or the formula 0560 c error of the l i n e a r approximation, l a given by the r a t i o of 0561 c the d i f ference between the l i n e a r i z e d and exact 0562 c gr ipper p o s i t i o n , and the magnitude of the l i n e a r i z e d p o a l t l o n 0563 c change. The step s ize i s chosen to keep t h i s r a t i o below some 0564 c spec i f i ed to lerance . 0565 0566 c a l l J a c o b ) i d e n t , i d e n t , l e n , a l p h a , d i a t , t h e t a 4 , j t y p e , n d o f , t n e t , j a c ) 0567 0568 do 316 1-1,2 0569 x l a s t ( l ) - x l a s t ( l ) + d e l x l ( l ) 0570 x c u r r ( i ) * t n e t ( i , 4 ) FINPAT 13-May-19B7 12I53I32 VAX FORTRAN V*.4-177 Page 11 ll-May-19B7 1 3 i l l l 2 2 DUAOICBUCHAL.0PT3FINPAT.FOR»451 0571 de lx2(1) -xcurr(1) -x las t (1 ) 0572 316 continue 0573 0574 dtemp2-sqrt(delx2(1)**2+delx2(2)**2) 0575 0576 c If the forau la error exceeds the thresho ld , acala the step s i z e . 0577 057B l f (dteap2.gt .s teps!) then 0579 dmult-dmultAstepsi/dtemp2 0580 endif 05B1 0582 c Update the j o i n t angles . 0583 0584 l f ( i p r a v . n e . l ) then 05B5 do 318 1-1,ndof 05B6 n v a l u e ! 1 , i p r e v ) • v a l u e s ! i , l p r e v ) + d m u l t * d l a s t ( i ) 05B7 318 continue 05BB endif 05B9 0590 endif 0591 0592 200 continue 0593 0594 c Update the t r a j e c t o r y . 0595 0596 do 444 l « 2 , l a s t 0597 do 446 j - l , n d o f 0598 v a l u e s ! j , i ) - n v a l u e ( J , i ) 0599 446 continue 0600 444 continue 0601 0602 c Ca lcu la te the l i n e a r i z a t i o n a r r o r f o r the d i s c r e t e approximation to 0603 c the t r a j e c t o r y . 0604 0605 do 101 i n d 4 » 2 , l a s t 0606 e r r o r - 0 . 0607 do 271 1-1,2 060B e r r o r * e r r o r + ( x e s t ( i , i n d 4 ) - x a c t ! i , i n d 4 ) ) * * 2 0609 271 continue 0610 e r r o r - s q r t l e r r o r ) 0611 0612 c This sec t ion i s necessary only l f vnora i s to be saved. 0613 0614 vnorm-0. 0615 do 277 i - l , n d o f 0616 v e l o c * ! v a l u e s ! 1 , i n d 4 ) - v a l u e s ! 1 , l n d 4 - l ) ) 0,617 vnorm"vnorm+veloc**2 061B 277 continue 0619 v n o r a » s q r t ( v n o r a ) 0620 0621 0622 c If the l i n e a r i s a t i o n error i a greater than the to l erance . 0623 c i n s e r t an in terpo la ted poa i t lon to reduce the step s i z e . 0624 c Conversely , l f the l i n e a r i z a t i o n error i s very smal l , a 0625 c p o s i t i o n can be removed to increase the step s i z e . 0626 0627 l f (error.gt .cmax) then FINPAT 13-May-19B7 12153132 VAX FORTRAN V4.4-177 Page 12 ll-May-19B7 1 3 l l l l 2 2 _ D U A 0 ICBUCHAL. 0PT3FINPAT.F0R|451 to +> to 062B 0629 0630 0631 0632 0633 0634 0635 0636 0637 063B 0639 0640 0641 0642 0643 0644 0645 0646 0647 064 B 0649 0650 0651 0652 0653 0654 0655 0656 0657 065B 0659 0660 0661 0662 0663 0664 0665 0666 0667 066B 0669 0670 0671 0672 0673 0674 0675 0676 0677 067B 0679 06B0 0681 0682 06B3 06B4 nadd-nadd+1 a d l i s t ( n a d d ) - l n d 4 - l e l s e i f (ind4 .ne. 1 .and. terror+eprev) . I t .cnax/cmln) then nsub-nsub+1 sublst(nsub)- lnd4 endlf • p r i v i r r o r 101 continue c Integrate coat . c a l l co s t t (va lues ,va in ,vmax . thesav , l a s t , A 11 .12 ,13 ,14 ,cos t ( indl ) ) w r i t e l S , * ) l a s t c Insert or remove d i scre te pos i t ions as prev ious ly determined. l f (nadd .g t .O.or .nsub .g t .O) then w r i t e ! * , * ) 'new pos i t ions added at i t e r a t i o n ' , l n d l w r i t » ( * , * > ' a d l l s t * , ( a d l i a t ( i ) , i - l , n a d d ) write**,*) 'aublat ' , ( s u b l s t l 1 ) , 1 - 1 , n s u b ) c a l l i n s a r t ( n a d d , n s u b , 2,adliBt , a u b l s t , v a l u e s ) endif c Stop l f the penetrat ion and j o i n t v i o l a t i o n to lerances are aet and c i f the p o s i t i o n step s ize error i s wi th in to lerance . i f (daaz . l t . aaxpen .and . fmax . I t . j to l er .and .nadd .eq .0 ) then s t o p - . t r u e . wr i t e !* ,* ) 'A*** Interference and- jo in t l i m i t s s a t i s f i e d AAA*' endlf wri te(3 ,*) dmax-maxpen write ( 1 ,A) fmax-j to ler write)*,*) 'last,cost,dmax,fmax ' , i n d l , l a s t , c o s t ( i n d l ) , d a a x , f a a x wri te(A,A) goto 115 endlf 100 continue do 500 i - l , l a s t wrlte(20,A) values(1,1) wrlte(21,A) v a l u a s ( 2 , i ) write(22,A) v a l u e s ) 3 , i ) write(23,A) v a l u e s ( 4 , i ) w r i t e ( 7 ,A) ind i s t(i) -maxpen 500 continue c l o s e d ) FINPAT 13-Kay-19B7 12i53i32 ll-May-19B7 1 3 l l l l 2 2 VAX FORTRAN V4.4-177 Page 13 DUA0i[BUCHAL.OPT3FINPAT.FOR»451 06B5 cloae(2) 06B6 close(3) 06B7 close!5) 0£BB close(6) 0689 c l o s e < 7 » 0690 close(B) 0691 close(9) 0692 c lose( lO) 0693 c l o s e ( l l ) 0694 close(12) 0695 close(13) 0696 close(14) 0697 close(20) 069B close(21) 0699 close(22) 0700 close(23) 0701 0702 return 0703 end PROGRAM SECTIONS ro •P- Naae Bytes A t t r i b u t e s 0 flCODE 376B PIC CON REL LCL SHR EXE RD NOMRT LONG 1 4PDATA 252 PIC CON REL LCL SHR NOEXE RD NOHRT LONG 2 (LOCAL 16140 PIC CON REL LCL NOSHR NOEXE RD HRT LONG 3 ROBOT 96 PIC OVR REL GBL SHR NOEXE RD HRT LONG 4 PARAM 1 a PIC OVR REL CBL SHR NOEXE RD HRT LONG 5 PARAM3 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 6 NMAX 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 7 TAO 4 PIC OVR REL GBL SHR NOEXE RO HRT LONG B GOAL 16 PIC OVR REL GBL SHR NOEXE RD HRT LONG 9 IDENT 64 PIC OVR REL GBL SHR NOEXE RD HRT LONG 10 LAST 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 11 LIMITS 32 PIC OVR REL GBL SHR NOEXE RD HRT LONG 12 R0BJ9 8 PIC OVR REL GBL SHR NOEXE RD HRT LONG 13 CMAX 4 PIC OVR REL CBL SHR NOEXE RD HRT LONG 14 STEPSI 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 15 LIMTOL B PIC OVR REL GBL SHR NOEXE RD HRT LONG 16 NRDOF B PIC OVR REL GBL SHR NOEXE RD HRT LONG T o t a l Space Al loca ted 20424 ENTRY POINTS Address Type Naae 0-00000000 FINPAT FINPAT VARIABLES Addreaa Type Nana Addreaa Type Nane 5 -00000000 RA4 AFACT B -0000000C R*4 ANCOAL AA RA4 CHIN AA R*4 D2RAD AA RA4 DENOH1 2 -00003A44 R*4 DMAX AA RA4 DTEMP2 2 -00003A74 RA4 EPREV AA RA4 FTEMP 2 -00003A2B 1*4 I 2 -00003A2C 1*4 IND1 2 -00003A4C 1*4 IND2 2 -00003A60 IA4 IOBST 2 -00003A54 1*4 IPREV 4 -00000000 1*4 JFACT 15 -00000004 R*4 JTOLER 2 -00003A3C IA4 NADO 16 -00000000 1*4 NDOF 12 -00000004 IA4 NOBST 2 -00003A40 1*4 NSUB 2 -00003A6B IA4 NVOBST 16 -00000004 1*4 NXDOF 2 -00003A24 L*4 STOP 7 -00000000 RA4 TAU AA R*4 VELOC AA RA4 VNORH ARRAYS Addreaa Type Nane Bytea Dlneni lona 2-00002EC0 R*4 ACC 400 (100) 2-00002BA0 1*4 ADLIST 400 (100) 3-00000020 R*4 ALPHA 16 (4) 2-00002B18 R*4 AMULT 16 (4) 2-0O0O3A10 RA4 ATERM 16 (4) 2-0000242B RA4 BOX1 80 (2, 10) 2-0000247B RA4 BOX2 800 (2, 10, 10) 2-00001DEB RA4 BOXES 800 (2, 10, 10) 2-00000760 RA4 COST 4000 (1000) 2-O00033C0 RA4 DANGLE 16 (4) 2-00000740 RA4 DELI 16 (4) 2-00001720 RA4 DELX 8 (2) 2-000033B0 RA4 DELX1 B (2) 2-000033BB RA4 DELX2 B (2) 3-00000050 RA4 DIST 16 (4) 2-00000750 RA4 DLAST 16 (4) 2-00001738 R*4 HOMOG 64 (4, 4) 2-000027AB RA4 HULL 160 (2, 20) 9-00000000 R*4 IDENT 64 <«. 4) 2-0OOO31E0 R*4 INDIST 400 (100) 2-00002848 L*4 INFLAC 400 (100) 2-000029DB 1*4 IVERTS 160 (2, 20) 2-00002B2B R*4 JAC 120 (6, 5) 2-000017BB R*4 JMULT 16 (4) 3-00000010 1*4 JTYPE 16 (4) 2-00002A7B R*4 LBOX 80 (2, 10) 3-00000030 R*4 LEN 16 (4) 3-00000000 1*4 LINK 16 (4) 2-00000020 R*4 MAT BO ( « . 5) 2-00000070 R*4 MAT1 BO (4, 5) 2-00000100 R*4 NVALUE 1600 <«, 100) 2-00002ACB R*4 OBOX BO (2, 10) 2-00001798 R*4 PMULT 16 (4) 2-000027AO R*4 POINT B (2) 13-May-1987 12153132 VAX FORTRAN V4.4-177 Page 14 ll-May-19B7 1 3 i l l i 2 2 _DUAO : CBUCHAL. OPT3FINPAT. FOR > 451 Addreaa Type Nane Addreaa Type Name A A RA4 ATEMP 13 -00000000 R*4 CMAX AA RA4 DELEN A* R*4 DENOM AA RA4 DMULT AA RA4 DTEMP1 AA RA4 ERROR 2 -00003A4B RA4 FMAX AA IA4 I END 2 -O0O03A30 IA4 ILINK AA IA4 IND4 2 -00O03A20 LA4 INTFLG AA 1*4 ISTART AA 1*4 J 10 -00000000 1*4 LAST 15 -00000000 RA4 MAXPEN 12 -00000000 1*4 NLINKS 6 -00000000 1*4 NMAX 2 -00003A5C IA4 NVERT 2 -00003A38 1*4 NVLINK 4 -00000004 R*4 PFACT 14 -00000000 R*4 STEPSI AA R*4 TOLER AA R*4 TOLER1 FINPAT 13-May-19B7 12•53132 VAX FORTRAN V4.4-177 Page ll-May-19fl7 1 3 i l l i 2 2 _DUA0 :CBUCHAL. OPT3FINPAT.FOR»451 2-00002D30 1*4 8UBL3T 400 < 100) 2-00002108 R*4 TBOXES BOO ( I, 10, 10) 2-00001728 RA4 TEMP 16 ( \1 2-00003370 R*4 THDOT1 16 ( 1) 2-00001700 R*4 THEDOT 16 ( ' () 2-000017A8 R*4 THESAV 1600 ( 1, 100) 3-00000040 R*4 THETA 16 ( 1) 2-00000000 RA4 THETA1 16 I 1) 2-00000010 R*4 THETA2 16 H t) 2-00003390 RA4 THETA3 16 << 1) 2-000033AO RA4 THETA4 16 l ) 2-00001710 RA4 THETOT 16 « < 1) 2-000000C0 RA4 TNET 64 H I, 4) AP-00000004O RA4 VALUES 1600 << I, 100) 2-0000279B RA4 VECT 8 C >) 2-00003050 RA4 VEL 400 ( ] LOO) 2-00001778 RA4 VLIM 16 (4 1) 11-00000010 RA4 VMAX 16 (4 1) 11-00000000 RA4 VMIN 16 (< t) 2-000036F0 RA4 XACT BOO <; t, 100) 2-000033B8 RA4 XCURR B ( • !) 2-000033D0 RA4 XEST 800 <: 1, 100) B-00000000 RA4 XGOAL 12 ( . 1) 2-000033BO RA4 XLAST B <: 1) LABELS Addreaa Labal Addraaa Labal Address Labal Addraaa Labal AA AA AA AA AA AA AA AA 11 109 160 209 310 317 411 446 AA AA AA AA AA AA AA AA 12 111 161 222 311 31B 412 500 AA O-OOOOOOFB AA AA AA AA AA AA 25 115 180 260 312 331 413 560 AA AA AA AA AA AA AA A* 100 119 1B1 262 313 332 415 561 Addraaa AA AA AA AA AA AA AA AA Laba l 101 155 200 271 315 333 419 900 Addrasa Labal 0-00000995 AA AA AA AA AA AA 104 156 201 277 316 334 444 FUNCTIONS AND SUBROUTINES REFERENCED Typa Naaa Type Naae Type Naaa Type Name Type Naae Type Naae CORECT COSTT F0R»CL0SE FOR J OPEN GMULT .GVERTS HULL2D INSERT INTERF JACOB RA4 KTH$SQRT SOLVEl SOLVE2 TRANDB 13-May-1987 12i57iSB VAX FORTRAN V*.4-177 Page 1 26-Feb-19B5 11|3B|10 DUAO i C BUCHAL. OPT3CPUTIME. FOR 11 OOOl C AAAA GET CPU TINE AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA 0002 c 0003 c + • 0004 c 1 1 0005 r e a l funct ion get_cpu_time ( baae_tiae ) 0006 c | | 0007 c j Programmer! Gary Lepp Nov. 5, 1984 j 000B c j I 0009 c j D e s c r i p t i o n ! j 0010 c j | 0011 c j This rout ine r e t u r n ! Elapsed CPU t i a e s ince the I n i t i a l I 0012 c j c a l l to t h i s rout ine . The CPU t i a e i s returned as a real*4 j 0013 c | value ln seconds. The r e s o l u t i o n of the t iae i s 0.01 seconds | 0014 c | j 0015 c j Before the CPU t iae Bay be re turned, the systea t i a e r aust ba j 0016 c | I n i t i a l i z e d . Th i s i s dona by c a l l i n g GET_CPU_TIME with an I 0017 c j argueaent equal to 0. On r e t u r n , CPU_TIME w i l l contain the j 0018 c j address of the storage block i n i t i a l i z e d by the system | 0019 c j routine LIB$INIT_TIMER. I 0020 c | | 0021 c | On subsequent c a l l s to CPU_TIME, the cpu t i a e elapsed s ince I 0022 c j the I n i t i a l i z a t i o n c a l l i a returned by passing the address of | 0023 c j the storage block as the argument. The cpu time w i l l ba j 0024 c j contained i n the v a r i a b l e CPU_TIKE. j 0025 c j j 0026 c | Inputi I 0027 c j | 002B c j Base_tlme - A realA4 v a r i a b l e which has the va lue i | 0029 c | 1) 0 i f the t i a e r i a to be I n i t i a l i z e d . | 0030 c | i i ) the address of the t i a e r storage b lock I 0031 c | returned by a previous c a l l to CPUJTIME. I 0032 c 1 1 0033 c | Output• 1 0034 c j | 0035 c j Cpu_tlae - A realA4 v a r i a b l e conta in ing! I 0036 c j i ) The address of the t i a e r storage block i f j 0037 c j CPU_TIHE was c a l l e d with a parameter of 0. | 003B c | 11) The elapsed CPU time since the timer was j 0039 c | i n i t i a l i z e d i f CPUJTIME was c a l l e d with a | 0040 c | timer Btorage block as the parameter. | 0041 c j | 0042 c + 0043 c 0044 equivalence ( i t l ae_atorage , t iae_storage ) 0045 c 0046 iget_cpu_time • 2 004 7 c 0048 c I n i t i a l i z e the timer - return the t i a e r storage block 0049 c 0050 i f ( base_tlae . eq . 0. ) then 0051 itime storage « 0 0052 c a l l l l b 9 i n i t _ t l a e r ( l t iae_atorage ) 0053 get_cpu_time = time_atorage 0054 c 0055 c Return the elapsed CPU time l n uni t s of seconds 0056 c 00.57 else GET CPU TIKE 0038 0059 0080 0061 0062 0063 0064 13-H»y-19B7 12I57I5B 26-Feb-19B5 H i 3 8 : 1 0 tine_atorage * baae_tine c a l l l i b $ s t a t _ t i u e r ( i g e t _ c p u _ t i n e , l c p u _ t l n e , l t l a e _ i t o r a g « ) get_cpu_tinie • 0.01 * i c p u _ t i i » e end i f " return end VAX FORTRAN V4 . 4-177 DUAO s C BUCHAL.0PT3CPUTIKE. FOR 11 Page PROGRAM SECTIONS Naae 0 (CODE 2 (LOCAL T o t a l Space Al located Bytea A t t r l b u t e a 67 40 107 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE RD NOHRT LONG RD HRT LONG ENTRY POINTS Addreaa Type Nane 0-00000000 R*4 GET CPU TIME VARIABLES Addreaa Type Nane AP-00000004B R*4 2-00000000 R*4 BASE_TIME TIME STORAGE Addreaa Type Nana 2-0000000C 1*4 ICPU TIME Addreaa Type Nane Addreaa Type Nane 2-00000008 1*4 IGET_CPU TIME 2-00000000 1*4 ITIME STORAGE FUNCTIONS AND SUBROUTINES REFERENCED Type Name LIB4INIT_TIMER Type Nane LIB«STAT_TIMER COMMAND QUALIFIERS FOR/LIST OPT.FOR,RDFILE.FOR.INPUT.FOR.GROH.FOH,FINPAT.FOR,S0LVE1.FOR,INT2D.FOR,HULL2D.F0R.GMULT.FOR,JACOB.FOR,COSTT.FOR, / CHECK • (NOBOUNDS, OVERFLOH , NOUNDERFLOH) /DEBUG- (NOSYMBOLS.TRACE BACK) / STANDARD* (NOSYNTAX, N0S0URCE_F0RM) /SH0H=(NOPREPROCESSOR,NOINCLUDE,MAP,NODICTIONARY.SINGLE) / WARNINGS = (GENERAL, NODECLARATIONS) /CONTINUATIONS'* 19 /NOCROSS_REFERENCE /NOD LINES /NOEXTEND_SOURCE /F77 /N0G_FL0ATING /I4 /N0MACHINE_C0DE /OPTIMIZE 13-May-19B7 12t55iS3 VAX FORTRAN V4.4-177 20-NOV-19B6 16i26i26 _DUAO : C BUCHAL. OPT3GMULT. FOR j 3 Pag* 1 0001 subroutine g n u I t ( a , b , c , a , n , 1 , n d l a a , n d i m b , n d i « c ) 0002 0003 c M u l t i p l y two matrices a(a x n ) , b in x 1) to get the product 0004 c c (a x 1) . 0005 0006 c Parameters• 0007 c a , b - the a a t r l c a s to be m u l t i p l i e d . 0008 c c- the product 0009 c m , n , l - the dimensions of the matr ices . 0010 c ndima- f i r s t dimension of f o r t r a n array ' a ' 0011 c ndimb- f i r s t dimension of f o r t r a n array 'b ' 0012 c ndlmc- f i r s t dimension of f o r t r a n array 'c* 0013 0014 r e a l a lndiaa,*) ,b(ndimb,*>,c(ndiac,A) 0015 0016 do 10 i n d l - 1 , 1 0017 001B do 20 i n d 2 - l , a 0019 0020 temp-0. 0021 do 30 l n d 3 ' l , n 0022 0023 temp-temp+a(ind2,1nd3 > *b( ind3 , ind l ) 0024 0025 30 continue 0026 0027 c ( ind2 , ind l ) - t emp 0028 0029 20 continue 0030 0031 10 continue 0032 0033 return 0034 end GHULT 13-May-19B7 12155153 VAX FORTRAN V4.4-177 Page 2 20-NOV-19B6 16:26126 _DUA0 : CBUCHAL. OPT3GMULT. FOR (3 PROGRAM SECTIONS Nane 0 (CODE 2 «LOCAL T o t a l Space A l l o c a t e d Bytes A t t r i b u t e s 192 16B 360 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE RO NOHRT LONG RO HRT LONG ENTRY POINTS Addreaa Type Naae 0-00000000 GHULT VARIABLES Addreaa Type Naae 2-00000000 1*4 IND1 AP-00000O10O 1*4 M AP-000000248 1*4 NDIMC Address Type Naae A* 1*4 IND2 AP-00000014O 1*4 N ** R*4 TEMP Addreaa Type Naae AA 1*4 IND3 AP-0000001CO 1*4 NDIMA Address Type Naae AF-00000018B 1*4 L AP-00000020B 1*4 NDIMB ARRAYS Address Type Naae Bytes Dimensions AP-00000004O R*4 A ** (*, *} AP-000000088 R*4 B ** (A, *) AP-00O00OOCB R*4 C AA (A, A) LABELS Address Label Address Label Address Label A* 10 A* 20 A* 30 COMMAND QUALIFIERS FOR/LIST OPT. FOR,RDFILE. FOR, INPUT. FOR,GROH. FOR, FINPAT. FOR, SOLVE1.FOR, INT2D. FOR,HUIX2D. FOR,GMULT. FOR, JACOB. FOR,COSTT. FOR, / CHECK * ( NO BOUNDS , OVERFLOW, NOUNDERFLOH) /DEBUG* < NOSYMBOLS ,TRACEBACK) / STANDARD" (NOSYNTAX, NOSOURCE_FORM) / SH0H= (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONARY, SINGLE) /HARNINGS* (GENERAL,NODECLARATIONS ) /CONTINUATI0NS = 19 /NOCROSS_REFERENCE /N0D_LINES / NOEXTENDSOURCE /F77 /NOG_FLOATING /I4 /NOMACHINE CODE /OPTIMIZE 13-Kay-19B7 12I53I16 27-Nov-1986 17I23I13 0001 subroutine growl de l ta) 0002 0003 c Grow tha obataclaa by an amount ' d a l t a ' . 0004 c T h i s compensates for the penetrations r e s u l t i n g f r o a us ing an 0005 c ex ter ior penalty func t ion for in ter ference . 0006 c There i s no inter ference l f the largest penetrat ion Is smaller 0007 c than ' d e l t a ' . OO0B 0009 r e a l sverts (2 ,100) , tverts !2 ,100) 0010 integer boxes(2,10) 0011 r e a l tempi 2,100),navel 2) ,norm12),nlast12) ,nf irst12) .edge12) 0012 0013 common / o b j e / b o x e s , s v e r t s , t v e r t s 0014 common / n o b j s / n l l n k s , n o b s t 0015 0016 i s tar t -n l lnks+1 0017 iend-nlinks+nobst 0018 0019 do 10 i n d - i s t a r t , i e n d 0020 0021 i f lrat -boxea11 ,Ind) 0022 l l a s t - l f i r s t + b o x e s 1 2 , i n d ) - 1 0023 do 20 i n d 2 » i f i r s t , i l a s t 0024 0025 l f I i n d 2 . e q . i l a s t ) then 0026 i n e x t - i f i r s t 0027 e l se 002B inext- lnd2+l 0029 endif 0030 0031 e len-0 . 0032 do 30 i - l , 2 0033 edge 11 ) - sver t s (1 , lnax t ) -aver t s (1 , ind2) 0034 elen-elen+edgel1)A*2 0035 30 continue 0036 e l e n - s q r t l e l e n ) 0037 0038 i f I i n d 2 . n e . i f i r s t ) then 0039 do 31 i -1 ,2 0040 n l a s t 1 1 ) > n o r « l 1 ) 0041 31 continue 0042 endif 0043 0044 norm!1)--edge!2)/elen 0045 norm12)-edge 11)/elen 0046 0047 l f ( l n d 2 . e q . l f i r s t ) then 0048 do 32 i - 1 , 2 0049 n f i r s t ( i ) - n o r a l l ) 0050 32 continue 0051 endlf 0052 0053 i f ( l n d 2 . n e . i f i r s t ) then 0054 rlen=0. 0055 do 33 i - 1 , 2 0056 navel i ) -nlaat11)+norn( i ) 0057 rlen-rlen+naveti )**2 VAX FORTRAN V4. 4-177 _DUA0 i C BUCHAL. 0PT3GR0H. FOR» to GROW 0038 0039 0060 0061 0062 0063 0064 0063 0066 0067 006B 0069 0070 0071 0072 0073 0074 0075 0076 0077 0078 0079 00B0 0081 0082 0083 0084 0085 0086 0087 0088 0089 0090 0091 0092 0093 0094 0095 0096 0097 009B 0099 0100 0101 0102 13-May-19B7 12I53I16 27-NOV-19B6 17i23t l3 VAX FORTRAN V4.4-177 DUA0: C BUCHAL. 0PT3GR0H. FOR) 5 Page 33 34 35 40 41 45 20 10 51 50 continue r l e n - s q r t l r l e n ) do 3 4 i ' 1 , 2 navel 1)"navel 1 )/rlen continue C O B -nave11)Anorn11)+nave12)Anora12) dist-delta/cos do 35 i - 1 , 2 teapli,indl)-avert* ( 1 , l n d 2 )+nave Ii)Adist continue endif i f (lnd2.eq.ila»t) then r l e n - 0 . do 40 i - 1 , 2 navel 1 )-noral1 )+nf I r s t Ii) rlen-rlen+naveli ) * * 2 continue r l e n - s q r t l r l e n ) do 41 i - 1 , 2 navel 1)-navel 1 )/rlen continue coa-naveI1)AnoraI1)•nave12)Anora12) dist-delta/coa do 43 1-1,2 teapl i , ifirat)-»verta(i,lfirit)+nave(i)AdiBt continue endif continue continue i f i r s t - b o x e s ( l , l s t a r t > ilast-boxes!1 ,iendl+boxeal 2 ,lend) - 1 do 30 l n d - l f l r s t , i l a a t do 51 i - 1 , 2 averts 1 1 ,ind)-teapli,lnd) continue continue return end GROW 13-May-19B7 12 i53 i16 VAX FORTRAN V*.4-177 27-NOV-1986 17I23I13 DUAO i CBUCHAL. 0PT3GR0W. FOR) 5 Page 3 PROGRAM SECTIONS Naae 0 (CODE 2 (LOCAL 3 OBJS 4 NOBJS T o t a l Bpace Al loca ted Bytea A t t r i b u t e ! 544 S6B 16B0 B 3100 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE PIC OVR REL GBL SHR NOEXE PIC OVR REL GBL SHR NOEXE RO NOMRT LONG RD HRT LONG RO HRT LONG RD HRT LONG ENTRY POINTS Addreaa Type Naae 0-00000000 GROH VARIABLES Addreaa Type Naae Addreaa Type Naae Address Type Naae Address Type Nana AA RA4 COS AP-00000004B R*4 DELTA AA R*4 DIST AA R*4 ELEN AA 1*4 I 2-0000034C 1*4 I END AA 1*4 IFIRST 2-0000035B 1*4 I LAST 2-00000350 IA4 IND AA 1*4 IND2 AA 1*4 I NEXT 2-00000348 1*4 I START 4-00000000 IA4 NLINKS 4-00000004 1*4 NOBST AA RA4 RLEN ARRAYS Address Type Nane Bytes Dlaensions 00000000 1*4 BOXES 80 (2, 10) 00000340 R*4 EDGE 8 I2> 00000320 R*4 NAVE a (2) 00000338 R*4 NFIR3T B (2) 00000330 H*4 NLAST B (2) 0000032B R*4 NORM 8 <2) 00000050 R*4 SVERTS BOO (2, 100) 00000000 R*4 TEMP 800 (2, 100) 00000370 R*4 TVERTS BOO (2, 100) LABELS Address Label Addreas Label Addreaa Label Address Label Addreaa Labe l Addreaa Label *A 10 AA 20 AA 30 AA 31 AA 32 AA 33 AA 34 AA 35 A* 40 AA 41 AA 45 AA 50 AA 51 GROH FUNCTIONS AND SUBROUTINES REFERENCED Type Haas R M HTHaSQRT 13-May-19B7 12tS3i l6 27-NOV-1986 17 i23 i l3 VAX FORTRAN V4.4-177 DUAO1C BUCHAL. OFT3GROH. FOR > 5 Page COMMAND QUALIFIERS FOR/LIST OPT.FOR,RTJFILE.FOR,INPUT.FOR,GROW.FOR,FINPAT.FOR,SOLVEl.FOR,INT2D.FOR,HULL2D.FOR ,GMULT.FOR,JACOB.FOR,COSTT.FOR, /CHECK-1NOBOUNDS,OVERFLOW,NOUNDERFLOH) /DEBUG- (NOSYMBOLS .TRACEBACK) / STANDARD- (NOSYNTAX, NOSOURCE_FORM) / SHOW- (NOPREPROCESSOR, NO INCLUDE , MAP , NODICTIONARY , SINGLE) /WARNINGS- (GENERAL, NODECLARATIONS) /CONTINUATI0NS=19 /NOCROSS_REFERENCE /NOD LINES /NOEXTEND_SOURCE /F77 / NOG_FLOATI NG /I4 /NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run T l a e i Elapsed T l a e i Page F a u l t a i Dynamic Memoryi 5.01 aeconda 15.25 aeconda 60B 462 pages 13-May-19B7 12.57.18 VAX FORTRAN V4.4-177 Page 1 l l-Hay-1987 17.21.24 DUAO!CBUCHAL.OPT3GVERTS.FOR»8 0001 subroutine gverta l iaode , l type .number ,verts ,nverts ) 0002 0003 c Access the geoaetry database f o r a given object . 0004 0005 c Paraaeters I 0006 c laode- 1- get s t a t i c vertex coordinates for an object 0007 c 2- get transformed vertex coordinates for an object 0008 c 3- s t o r e n e w transformed vertex coordinates for an 0009 c object 0010 c l t y p e - 1- the object i s a aoving l i n k 0011 c 2- the object i s a a tat ionary obstacle 0012 c number- the object id number 0013 c v e r t a - the array of vertex coordinatea. 0014 c nver t s - the number of v e r t i c e s 0015 0016 c Geometry database s t r u c t u r e ! 0017 c boxes- contains pointers to the vertex arrays ooia c b o x e s ( l , D - s t a r t i n g vertex of l i n k 1 0019 c boxes(2 ,1)- number of v e r t i c e s of l i n k 1 0020 c boxes (1 ,n l lnks+l ) - s t a r t i n g vertex of obstacle 1 0021 c boxes! 2 , n l i n k s + l ) - number of v e r t i c e s of obstacle 1 0022 c e v e r t s ! l , i ) - array of f ixed vertex coordinates 0023 c t v e r t s ! i , j ) - array of t r a n s f o r a e d vertex coordinatea 0024 c 0025 0026 Integer ltype,number 0027 r e a l verts(2 ,10) 002B 0029 Integer boxes(2,10) 0030 r e a l sverts !2 ,100) , tverts (2 ,100) 0031 0032 common / o b j s / b o x e a , a v e r t s , t v e r t s 0033 common /nobja /n l inka ,nobs t 0034 0035 c Calcu la te the object po inter . 0036 0037 i f ( i t y p e . e q . l ) then 003B ipoint-number 0039 e l s e i f l l t y p e . e q . 2 ) then 0040 ipoint-nl inks+nuaber 0041 endif 0042 0043 c F ind the s t a r t i n g vertex and the number of v e r t i c e s . 0044 0045 i s tar t"boxes (1 , ipo in t ) 0046 i n u m b - b o x e B ( 2 , i p o i n t ) 0047 004B i f ( lmode.eq. l) then 0049 0050 c Load t h e s t a t i c vertex array for the requested object . 0051 0052 icount-0 0053 d o 10 i n d - i s t a r t , l s t a r t + i n u m b - l 0054 icount-icount+1 0055 do 20 i - 1 , 2 0056 v e r t s ( 1 , i c o u n t ) - s v e r t s ( i , i n d ) 0057 20 continue to GVERTS 0058 0059 0080 0061 0062 0063 0064 0065 0066 0067 0068 0069 0070 0071 0072 0073 0074 0075 0076 0077 0078 0079 O0B0 O0B1 00B2 0083 0084 00B5 0086 0087 OOBB 00B9 0090 0091 13-Hay-19B7 12)57il8 ll-May-19B7 17:21:24 VAX FORTRAN V4.4-177 DUAO: C BUCHAL. OPT3GVERTS. FOR; 8 Page 10 21 11 22 12 continue nverts-inumb e l a e l f (imode.eq.2) then Load the transformed vertex array for the requeated object. 1count"0 do 11 ind" i s t a r t,iatart+lnuBb-l icount-icount+1 do 21 i - 1 , 2 verta(1,icount)«tverta(1,Ind) continue continue nverts - l n u a b e l a e l f (iaode . e q . 3 ) then 8tore the transformed v e r t i c e s for the s p e c i f i e d object. icount - 0 do 12 i n d - i s t a r t , i s t a r t + i n u * b - l i count "icount-t-1 do 22 i - 1 , 2 t v e r t s ( i , i n d ) - v e r t s ( i , i c o u n t ) continue continue endif return end G VERTS 13-May-1987 12i57i l8 VAX FORTRAN V4.4-177 Page 3 ll-May-1987 17i21i24 __DUAO t C BUCHAL. 0PT3GVERTS. FOR) 8 PROGRAM SECTIONS Nana 0 (CODE 2 (LOCAL 3 OBJS 4 NOBJS T o t a l Space Al loca ted Bytea A t t r i b u t e * 292 28 1680 B 2008 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE PIC OVR REL GBL SHR NOEXE PIC OVR REL GBL SHR NOEXE RD NOHRT LONG RD HRT LONG HRT LONG HRT LONG RD RD ENTRY POINTS Addreaa Type Naae 0-00000000 GVERTS tsi Cn VARIABLES Addreaa Type Naae AA 1*4 I A* 1*4 INUMB 4-00000000 1*4 NLINKS Addreaa Type Naae AA IA4 ICOUNT 2-00000000 IA4 IPOINT 4-00000004 1*4 NOBST Addreaa Type Naae AP-000000040 1*4 IMODE 2-00000004 1*4 ISTART AP-O0000OOC8 IA4 NUMBER Addreaa Type Naae AA 1*4 IND AP-00OO00OB8 1*4 ITYPE AP-00000014B 1*4 NVERTS ARRAYS Addreaa Type Naae 3-00000000 1*4 BOXES 3-00000050 R*4 SVERTS 3-00000370 R*4 TVERTS AP-000000100 R*4 VERTS Bytea Dlaenelona BO 800 BOO 80 (2, 10) (2, 100) (2, 100) (2, 10) LABELS Addreaa Label *A 10 Addrei AA Label 11 Addreaa Label AA 12 Addreaa Label AA 20 Addreaa Label AA 21 Addreaa Label AA 22 13-May-1987 12155108 VAX FORTRAN V4.4-177 Page 1 ll-May-1987 13l24i34 DUAO:CBUCHAL.OPT3HULL2D. FOR)34 0001 subroutine h u l l 2 d ( b o x l , b o x 2 , n v l , n v 2 , h u l l , n v e r t s , i v e r t s ) 0002 0003 c Find the convex h u l l enclosing two convex polygons i n 2-D. 0004 0005 c Parameterst 0006 0007 c boxl ,box2- arrays of v e r t i c e s d e f i n i n g two polygons. 0008 c n v l , n v 2 - the number of v e r t i c e s l n boxl,box2 0009 c h u l l - an array of v e r t i c e s de f in ing the ninie.ua 0010 c enc los ing convex h u l l 0011 c nver t s - the number of ver t i ce s l n h u l l 0012 c i v e r t a - an array l i n k i n g the v e r t i c e s of h u l l to the 0013 c v e r t i c e s of boxl,box2 0014 c i v e r t s ( l , i ) - the box conta in ing h u l l vertex 1 0015 c l v e r t s ( 2 , i > - the index of the corresponding vertex 0016 c i n box 0017 001B c V a r i a b l e s i 0019 0020 c aore- . t r u e , i f there are more edges to be found 0021 c search- . t r u e , i f the next edge of the h u l l l i n k s 0022 c the two polygons 0023 c i f l a g - . t r u e , u n t i l a stopping condi t ion i s reached 0024 0025 0026 r e a l boxl (2 ,10) ,box2(2 ,10) ,boxes(2 ,10 ,2) ,hul l (2 ,20) 0027 integer iver t s (2 ,20) 0028 r e a l v l ( 2 ) , v l a s t ( 2 ) 0029 integer nverts O030 r e a l vnorm(2) 0031 r e a l thull(2,20),ivtemp<2,20> 0032 r e a l v d i f f ( 2 ) 0033 0034 l o g i c a l more , search , i f l ag , s top 0035 0036 common / s i z e / s i z e 0037 O03B c I n i t i a l i z e f l a g s . 0039 0040 more*.true. 0041 i f l a g - . t r u e . 0042 s e a r c h - . f a l a e . 0043 a t o p - . f a l s e . 0044 ntemp-0 0045 0046 c Scale the tolerance according to the alze of the world. 0047 004B to l er - s i ze /1000 . 0049 0050 0051 c Load vertex array . 0052 0053 do 1 i « l , n v l 0054 do 2 j -1 ,2 0055 boxes (J ,1 ,1 ) -box l (J , i> 0056 2 continue 0057 1 continue HTJLL2D 13-Hay-19B7 12i33iOB VAX FORTRAN V4.4-177 Page 2 ll-May-19B7 13i24l34 _DUAO iCBUCHAL. OPT3HULL2D.F0R» 34 0038 do 3 i»l,nv2 0039 do 4 j«l,2 0060 boxes!j,i , 2 ) - b o x 2 (j,i) 0061 4 continue 0062 3 continue 0063 0064 c F ind a s t a r t i n g edge of the h u l l . • 0063 c F i r s t , tes t a l l the edges of box 1. 0066 c I f no h u l l edge i a found, then test a l l the edges of box 2. 0067 0068 do 200 ibox-1,2 0069 i f ( l f l a g ) then 0070 i f ( ibox .eq.l) then 0071 iother=2 0072 inumbl-nvl 0073 Inumb2-nv2 0074 e l se 0073 iother-1 0076 inumhl»nv2 0077 inumb2*nvl 0078 endif 0079 0080 do 10 i n d « l , l n u x t b l to O0B1 i f ( l f l a g ) then <-n 0082 l f ( i n d . e q . i n u a b l ) then 0 0 0083 lnext-1 0084 e lse 0085 inext-ind+1 0086 endif 00B7 do 120 i - 1 , 2 O0BB v l (1 ) -boxes (1 , lnext , lbox) -boxes (1 , ind , lbox) O0B9 120 continue 0090 0091 c Ca lcu la te the outward normal for a clockwise ordering of v e r t i c e s . 0092 0093 v l e n » s q r t < v l(l ) A A 2 + v l ( 2 ) A A 2 ) 0094 vnorat 1)— v l ( 2 ) / v l e n 0095 v n o r e ( 2 ) » v l ( l ) / v l e n 0096 0097 c Ca lcu la te the constant of the l i n e equation. 009B 0099 const-boxes(1 , ind, lbox)Avnoral1)+boxes(2 , ind, lbox)Avnora (2) 0100 0101 0102 c Test l f a l l the v e r t i c e s of box2 are on the aaae s ide . 0103 0104 daln2—10000. 0105 do 25 Ind2=l,nv2 0106 0107 t e s t 2-boxes(l , i n d 2,lother)Avnora(l)+ 0108 A boxes(2, lnd2,lother)Avnorm(2) 0109 l f ( test2.gt .daln2) then 0110 daln2-test2 0111 endif 0112 0113 25 continue 0114 HULL2D 13-Key-19B7 12l35lOB ll-May-19B7 13:24i34 0119 c It the teat edge l a an edge of the convex h u l l , 0116 0117 it (d j t ln2-conat . l e . to ler ) then 011B i f l a g - . f a l s e . 0119 do 30 i - 1 , 2 0120 t h u l l ( 1 , 1 ) ' b o x e s ( i , I n d , l b o x ) 0121 thu l l (1 ,2 ) -boxes (1 , lnex t , lbox ) 0122 lv tenp(1 ,1) - lbox 0123 l v t emp(2 , l ) - lnd 0124 lvtempl1,2)- lbox 0129 i v t e n p l 2 , 2 ) - l n e x t 0126 i f l r s t - i n d 0127 i l s s t - i n e x t 0128 ntenp-2 0129 i b o x l - l b o x 0130 30 continue 0131 endif 0132 0133 andlf 0134 0139 10 continue 0136 endif 0137 200 continue ro 0138 ui 0139 c If no edge of e i ther box i a an edge of the h u l l , then f i n d an *° 0140 c edge which connecta the f i r s t vertex of boxl to a vertex of box2. 0141 0142 l f ( i f l a g ) than 0143 ibox l -1 0144 do 11 i n d » l , n v 2 0143 l f ( l f l a g ) than 0146 0147 c Generate the edge vec tor . 014B 0149 do 121 1-1,2 0130 v l ( l ) - h o x e a ( l , l , l ) - b o x e s ( i , i n d , 2 ) 0131 121 continue 0132 0133 c Calcu la te the outward n o r a a l . 0134 0135 v l e n - s q r t ( v l ( l ) * * 2 + v l ( 2 ) A * 2 ) 0136 i f ( v l e n . g t . t o l e r ) then 0157 0138 v n o r n ( l ) — v l ( 2 ) / v l e n 0159 v n o r n ( 2 ) - v l ( l ) / v l e n 0160 0161 c Ca lcu la te the conatant of the l i n e equation. 0162 0163 const-boxes (1, l , l)*vnorn(l)'t-boxes( 2, l , l ) * v n o r n ( 2) 0164 0165 c Test l f a l l the v e r t i c e s of box2 are on tha sane s ide . 0166 0167 dnin2--10000. 0168 itemp-ind 0169 do 26 i n d 2 - l , n v 2 - l 0170 i teap*itenp+l 0171 i f ( l tenp.gt .nv2) ltenp-1 VAX FORTRAH V4.4-177 DUAOiC BUCHAL.0PT3HULL2D.FOR134 HULL2D 13-May-19B7 12iS5lOB VAX FORTRAN V4.4-177 Page 4 ll-May-19B7 13l24i34 DUAO:CBUCHAL.OPT3HULL2D.FOR|34 0172 0173 teat2'bozea( 1,iteap,2)*vnorm(l)-(-boxes (2, itemp, 2) Avnom 0174 l f ( test2.gt .dmln2) then 0173 ' dmin2*test2 0176 endlf 0177 0178 26 continue 0179 01BO i f (dmln2-cons t . l e . to l er ) then 0181 i f l a g - . f a l a e . 0182 do 31 i « l , 2 01B3 t h u l l ( i , l ) ' b o x e s ( 1 , 1 , 1 ) 0184 t h u l l ( l , 2 ) > b o x e s ( l , i n d , 2 ) 0183 l v t e a p ( l , l ) ' l 01B6 i v t e m p ( 2 , l ) « l 0187 i v t e m p ( l , 2 ) » 2 01BB l v t e B p ( 2 , 2 ) ' l n d 0189 i f i r a t - 1 0190 i l a s t » i n d 0191 ntenp'2 0192 31 continue 0193 endif 0194 endif 0193 endlf 0196 11 continue 0197 endif 019B 0199 c S t a r t i n g from the f i r s t edge of the h u l l , f i n d the edges 0200 c of the enclos ing h u l l i n a clockwise d i r e c t i o n . 0201 0202 c To s t a r t , the current box i s b o x l . 0203 0204 i b o x ' i b o x l 0203 0206 100 continue 0207 0208 l f ( i b o x . e q . l ) then 0209 i o t h e r » 2 0210 i n u m h l « n v l 0211 Inumb2*nv2 0212 else 0213 io ther-1 0214 i n u m b l » n v 2 0213 inumb2*nvl 0216 endif 0217 021B c Test the next vertex . 0219 0220 c If the next edge of the current box i s not on the h u l l , 0221 c test a l l poss ib le edges connecting 0222 c. t h e l a s t vertex to v e r t i c e s of the other box. 0223 c Select t h e one with the smallest angle to the previous edge. 0224 0225 l f (search) then 0226 0227 c Search for an edge connecting the current box to the other 022B HULL2D 13-May-1987 12:55i08 VAX FORTRAN V4.4-177 Page ll-May-1987 13:24:34 DUAO:CBUCHAL.OPT3HULL2D.FOR;34 0229 i flag= . t r u e . 0230 0231 c Calculate and normalize the previous edge for 0232 0233 do 71 i-1,2 0234 vlast( i)=thull(i,ntemp)-thull(i,ntemp-l) 0235 71 continue 0236 vlenl=sqrt(vlast(l)**2+vlast(2)**2) 0237 vlast<1)-vlaBt ( l ) /v lenl 023B vlast(2)=vlast(2) /vlenl 0239 0240 c Test a l l connecting edges and select the best 0241 0242 dtest=-l. 0243 ntemp=nterap+l 0244 do 60 ind3=l,inumb2 0245 i f (iflag) then 0246 do 70 i-1,2 v l (i) "boxes(1,ind3,iother)-thull (1,ntemp-l) continue vlen2=sqrt(vl(l)*A2+vl(2)**2) i f (vlen2.gt.toler) then vl(l)=vl(l)/vlen2 vl(2)-vl(2)/vlen2 test2=vl(l)*vlaBt(1)+vl(2)*vlast(2) else test2--10. endif 025B 0259 i f ( t e B t 2 . g t . d t e B t ) then 0260 dtest-test2 0261 do 130 i= l ,2 0262 t h u l l ( i , n t e m p > » b o x e B < i , ind3,iother) 0263 130 continue 0264 ivtemptl,ntemp)=iother 0265 ivtempl2,ntemp)=ind3 0266 ilaBt=ind3 0267 ibox=iother 026B endif 0269 endif 0270 60 continue 0271 0272 c If the next edge 1B nearly colllnear to the current e d g e , m e r g e 0273 c two edges into one b y removing the intermediate vertex from the 0274 c hul l . 0275 0276 e r r o r » ( v l e n l + v l e n 2 ) * s q r t ( 1 . - t e s t A * 2 ) / 4 . 0277 i f (error.It . toler) then 0278 do 135 1=1,2 0279 thull(i,ntemp-1)-thull(i,ntemp) 0280 ivtempli,ntemp-l)=ivtemp(i,ntemp) 02B1 135 continue 0282 ntemp=ntemp-l 02B3 endif 0284 0285 c If the last vertex is the same as the f i r s t , or i f the last K 0 2 5 0 f-4 0251 0252 0253 0254 0255 0256 0257 0247 024B 0249 70 HULL2D 13-May-19B7 121551 OB VAX FORTRAN V4.4-177 Pag* 6 ll-May-19B7 13I24I34 _DUAOiC BUCHAL. OPT3HULL2D. FOR » 34 0286 c vertex l e very close to the f i r s t , remove i t from the 02B7 c h u l l l i s t and q u i t . 02BB 0289 s e a r c h ' . f a l s e . 0290 l f ( l l a s t . e q . l f i r s t . a n d . l b o x . e q . i b o x l ) then 0291 more* . fa l sa . 0292 n t e m p « n t e m p - l 0293 else 0294 0295 v l e n « 0 . 0296 do 137 1-1,2 0297 v d i f f ( i ) - t h u l l ( 1 , i l a s t ) - t h u l l ( 1 , 1 ) 029B v len-v len+vdi f f ( i )**2 0299 137 continue 0300 v l en - sqr t (v l en ) 0301 0302 l f ( v l e n . I t . t o l e r ) then 0303 more - . fa l se . 0304 ntemp-ntemp-1 0305 endif 0306 endif 0307 0308 else 0309 to 0310 c Test the next edge of the current box. ON 0311 ^ 0312 i f ( i l a s t . a q . i n u m b l ) then 0313 inext-1 0314 e l se 0315 inext - i las t+1 0316 endlf 0317 031B c Test i f the next vertex Is the f i r s t ver tex , or l f i t l a 0319 c very close to the f i r s t vertex . If so, stop. 0320 0321 l f ( l n e x t . e q . l f i r s t . a n d . l b o x . e q . i b o x l ) then 0322 more*. fa lse . 0323 e lse 0324 v len-0 . 0325 do 147 i - 1 , 2 0326 vd l f f (1 ) -boxes (1 , lnex t , lbox ) - thu l1 (1 ,1 ) 0327 v len-v len+vdi f f ( i )**2 0328 147 continue 0329 i f ( v l e n . I t . t o l e r ) then 0330 more- . fa l se . 0331 e l se 0332 0333 do 20 i - 1 , 2 0334 v l ( l ) - b o x e s ( i , i n e x t , i b o x ) - b o x e s ( l , l l a a t , i b o x ) 0335 20 continue 0336 0337 c Calculate the outward normal. 0338 0339 v l e n « s q r t ( v l ( l ) * A 2 + v l ( 2 ) * * 2 ) 0340 v n o r m l 1 ) « - v l ( 2 ) / v l e n 0341 vnorm(2) -v l ( l ) / v l en 0342 HULL2D 13-May-1987 12tSSiOa VAX FORTRAN V4.4-177 Page 7 ll-May-1987 13t24t34 DUAO!CBUCHAL.OPT3HULL2D.FOR»34 0343 c Ca lcu la te the conatant of the l i n e equation. 0344 0345 conat"boxes( 1, lnex t , lbox )Avnoral 1) +boxea( 2, lnex t , lbox)Avnora(2) 0346 0347 c Teat l f a l l the v e r t i c e s of the other box are on the aaae a ide . 0348 0349 dain2--10000. 0350 do 145 i n d 2 » l , i n u a b 2 0351 0352 t e a t 2 » b o x e » ( 1 , l n d 2 , i o t h e r ) A v n o r a l 1 ) + 0353 A boxea(2 , ind2, iother)Avnorn (2) 0354 l f (teat2.gt.dmin2> then 0355 d a i n 2 » t e s t 2 0356 endif 0357 0358 145 continue 0359 0360 l f ( d n i n 2-const . l e .toler) then 0361 i f l a g - . f a l a e . 0362 nteap-nteap+1 0363 i l aa t" lnex t 0364 do 230 i - 1 , 2 0365 t h u l l ( i , n t e a p ) - b o x e s ( i , i n e x t , l b o x ) 0366 230 continue 0367 lv teapl1 ,nteap) - lbox OJ 0368 lvteap(2 ,nteap)- lnext 0369 e l se 0370 s e a r c h - . t r u e . 0371 endif 0372 0373 endi f 0374 endif 0375 endif 0376 0377 l f (aore) then 0378 goto 100 0379 endif 0380 0381 c Remove redundant v e r t i c e s which are c lose to other v e r t i c e s . 0382 0383 nverts-0 03B4 do 400 i n d - l , n t e a p 0385 i f ( i n d . e q . l ) then 03B6 l l a s t - n t e a p 0387 e lse 0388 i l a a t - l n d - 1 0389 endif 0390 v l e n - 0 . 0391 do 401 i - 1 , 2 0392 v l ( 1 ) - t h u l l ( 1 , i n d ) - t h u l l ( i , i l a s t ) 0393 vlen-vlen+vl( i)AA2 0394 401 continue 0395 v l e n - s q r t l v l e n ) 0396 i f ( v l e n . g t . t o l e r ) then 0397 nverts=nverts+l 0398 do 402 1-1,2 0399 h u l l ( i , n v e r t s ) « t h u l l ( i , i n d ) HULL2D 13-May-19B7 12i55t08 VAX FORTRAN V4.4-177 Page 8 l l-Hay-19B7 13I24I34 __DUAO : C BUCHAL. 0PT3HULL2D. FOR ) 34 0400 i v e r t s l i , n v e r t s ) " i v t e a p l 1 , l n d ) 0401 402 continue 0402 endif 0403 400 continue 0404 0405 return 0406 end PROGRAM SECTIONS Naae 0 (CODE 2 (LOCAL 3 SIZE T o t a l Space A l l o c a t e d Bytea A t t r i b u t e s 1B06 652 4 2462 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE PIC OVR REL GBL SHR NOEXE RD NOHRT LONG RD HRT LONG RO HRT LONG cr. ENTRT POINTS Address Type Naae 0-00000000 HULL2D VARIABLES Addreaa Type Naae Address Type Naae Addreaa Type Naae Addresa Type Naae AA RA4 CONST AA RA4 DMIN2 AA R*4 DTE3T AA R*4 ERROR AA IA4 I 2 -00000214 IA4 I BOX 2--00000230 IA4 IBOXl 2--00000228 IA4 IFIRST 2-00000208 LA4 I FLAG 2 -0000022C 1*4 I LAST 2 -00000224 1*4 IND AA 1*4 IND2 AA IA4 IND3 AA IA4 I NEXT 2 -0000021C IA4 INUMB1 2--00000220 IA4 INUMB2 2-0000021B IA4 IOTHER AA 1*4 ITEMP AA 1*4 J 2--00000200 L*4 MORE 2-0000020C IA4 NTEMP AP -0000000CO 1*4 NV1 AP--000000100 1*4 NV2 AP -0000001BO 1*4 NVERTS 2-00000204 LA4 SEARCH 3 -00000000 RA4 SIZE AA L*4 STOP 2--0000023B RA4 TEST AA RA4 TEST2 2 -00000210 R*4 TOLER AA R*4 VLEN AA R*4 VLEN1 2-00000234 RA4 VLEN 2 ARRAYS Addreaa Type Naae Bytes Dimensions AP-000000040 RA4 BOX1 BO (2, 10) AP-0000000BB RA4 BOX2 BO (2, 10) 2-00000000 R*4 BOXES 160 (2, 10, 2) AP-000000140 RA4 HULL 160 (2, 20) AP-0000001CO IA4 IVERTS 160 (2, 20) 2-0000015B RA4 IVTEMP 160 (2, 20) 2-000000BB RA4 THULL 160 (2, 20) 2-O00O0OA0 RA4 VI B (2) 2-000001FB RA4 VDIFF B (2) HULL2D 2-000000AB 2-OOOOOOBO 13-May-19B7 12l55lOB l l -Hay-19B7 13i24:34 R*4 RA4 VLAST VNORM B (2) B (2) VAX FORTRAN V4.4-177 DUAO : CBUCHAL. 0PT3HULL2D. FOR134 Page LABELS Addreaa Label AA AA AA AA AA 1 20 70 133 400 Addreaa Label AA AA AA AA AA 2 23 71 137 401 Address Label AA 3 AA 26 0-00000350 100 AA 145 AA 402 Addreaa Label AA AA AA AA 4 30 120 147 Address Label AA AA AA AA 10 31 121 200 Address Labe l AA AA AA AA 11 60 130 230 FUNCTIONS AND SUBROUTINES REFERENCED Type Naaa RA4 KTHflSQRT COMMAND QUALIFIERS FOR/LIST OPT.FOR,RDFILE.FOR,INPUT.FOR,GROW.FOR,FINPAT.FOR,B0LVE1.FOR,INT2D.FOR,HULL2D.FOR,CMULT.FOR,JACOB.FOR,COSTT.FOR, / CHECK • (NOBOUNDS, OVERFLOW , ROUNDER FLOW I /DEBUG" (NOSYMBOLS ,TRACEBACK) / STANDARD' (NOSYNTAX , NOSOURCE_FORM) / SHOW* (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONAR Y , S INGLE) /WARNINGS'(GENERAL,NODECLARATIONS) /CONTINUATIONS'!9 /NOCROSS REFERENCE /NOD LINES /NOEXTEND_S0URCE /F77 / NOG_FLOATI NG /I4 / NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run T i a e i Elapsed T l a e i Page F a u l t s i Dynaalc Memoryi 14.07 seconds 44.56 seconds 8B2 B45 pages 13-Kay-19B7 12I52I47 10-Apr-19B7 13l46l30 VAX FORTRAN V4.4-177 Page 1 DUAO ! CBUCHAL. OPT]INPUT.FOR 146 0001 subroutine Input 0002 0003 c Input the paraaetera for the path-planning a lgor i thm. 0004 c The paraaetera can be read f roa a f i l e , or they can be input 0005 c i n t e r a c t i v e l y by the user . 0006 c The paraaetera are passed through coaaon b locks . 0007 0008 r e a l theta(4) 0009 r e a l j f a c t , t a u , p f a c t 0010 integer n a i l 0011 integer l i n k ( 4 ) , j t y p e ( 4 ) 0012 r e a l l e n t 4 ) , d i s t ( 4 ) , a l p h a ! 4 ) 0013 r e a l vain!4),vmax(4) 0014 r e a l maxpen,jtoler 0015 l o g i c a l ans 0016 0017 coaaon / r o b o t / l i n k , J t y p e , a l p h a , l e n . t h e t a , d i e t 001B coaaon / p a r a a l / J f a c t , p f a c t 0019 coaaon /paraa3/afact 0020 coaaon / t a u / t a u 0021 common /caax/cmax 0022 common /naax/naax 0023 coaaon / l i B i t B / v a i n , v a a x , 0024 common / a t e p a i / s t e p s l 0^  0025 common / l i a t o l / m a x p e n , j t o l e r 0> 0026 common /nrdof /ndof ,nxdof 0027 common / f r a c t i / f r e c t i 002B 0029 c User can i n t e r a c t i v e l y enter paraaetera, or they can be read f r o a a 0030 c f i l e . 0031 0032 wr i t e !* ,* ) 'Do you wish to enter new paraaetera? ( T / F ) ' 0033 read!*,*) ana 0034 0035 i f (ans) then 0036 0037 o p e n ( 1 , f l l e « ' p a r a m . d a t ' . s t a t u s - ' o l d ' ) 0038 0039 c User Input of parametera. 0040 0041 r e a d d , * ) J fac t 0042 wr i te !* ,* ) ' J o i n t H a l t weighting f a c t o r ' , j f ac t 0043 wri te !* ,* ) 'Change? ( t / f ) ' 0044 read!*,*) ana 0045 l f (ana) then 0046 wr i t e !* ,* ) 'Enter new value ' 0047 read!*,*) j f a c t 004B endif 0049 0050 r e a d l l , * ) pfact 0051 wri te !* ,*) ' Interference weighting f a c t o r ' , p f a c t 0052 wri te !* ,*) 'Change? ( t / f ) ' 0053 r e a d ! * , * ) a n s 0054 i f ( a n s ) then 0055 wri te )* ,*) 'Enter new value ' 0056 read!A,A) pfact 0057 endif INPUT 13-May-19B7 12I32I47 VAX FORTRAN V4.4-177 Page 2 10-Apr-1987 13I46I30 _DUA0)CBUCHAL.OPT3INPUT.FORj46 ro ON —I 005B 0059 0060 0061 0062 0063 0064 0065 0066 0067 0068 0069 0070 0071 0072 0073 0074 0075 0076 0077 0078 0079 O0B0 00B1 0082 0083 0084 0085 00B6 0087 0088 0089 0090 0091 0092 0093 0094 0095 0096 0097 009B 0099 0100 0101 0102 0103 0104 0105 0106 0107 0108 0109 0110 0111 0112 0113 0114 r » a d ( l , * > f r a c t l wri te (* ,A) 'Penetrat ion f r a c t i o n ' , f r a c t l wr i te !A,A) 'Change? ( t / f ) ' read!*,*) ana l f (ana) then write(A,A) 'Enter new value' read(A,A) f r a c t l endif read(1,A) afact wr i te !* ,A) ' J o i n t a c c e l e r a t i o n weighting f a c t o r ' , afact wr i te !* ,* ) 'Change? ( t / f ) ' read!*,*) ana i f (ana) then wrl te (* ,A) 'Enter new value' read(A,A) afact endif r e a d l l , * ) nmax wri te (* ,A) ' M a i l B U M nunber of Iterat ions' ,nmax wri te !* ,* ) 'Change? ( t / f ) ' read(A,A) ana l f (ana) then write(A,A) 'Enter new value ' read!*,*) nnax andlf r e a d l l , * ) tau wri te !* ,* ) ' I t e r a t i o n fac tor (between 0 and l ) ' , t a u wr i t e !* ,A) 'Change? ( t / f ) ' read!*,*) ans i f (ana) then wr i te !* ,* ) 'Enter new value' read!*,*) tau andlf r e a d l l , A ) ateps l wri te !* ,*) 'Maximum I t e r a t i o n l i n e a r i s a t i o n e r r o r ' , s t e p s l writelA.A) 'Change7 l t / f ) ' read!*,*) ans i f (ans) then wri te !* ,* ) 'Enter new value' read!*,*) s teps i endif r e a d l l , * ) cmax wri te !* ,*) 'Maximum t r a j e c t o r y l i n e a r i z a t i o n e r r o r ' , c a a x wri te !* ,A) 'Change? ( t / f )* read(A,*) ana l f (ans) then wri te !* ,* ) 'Enter new value ' read(*,*) cnax endif r e a d l l , * ) maxpen,Jtoler INPUT 13-May-19B7 12i32)47 VAX FORTRAN V4.4-177 Pag* 3 10-Apr-1987 13|46|30 DUAO s C BUCHAL. OPT3 INPUT. FOR 146 0113 wri te !* ,* ) 'Maximum penetrat ion and jo in t U n i t t o l e r a n c e ' , 0116 * maxpen,jtoler 0117 wr i te !* ,* ) 'Change? ( t / f ) ' 0118 read!*,*) ana 0119 i f (ana) then 0120 wr i t e !* ,* ) 'Enter new value' 0121 read!* ,*) naxpen, j to ler 0122 endif 0123 0124 r e a d t l , * ) ( v n i n ! i ) , i - l , n d o f ) 0125 wr i te !* ,* ) 'Minimum jo in t a n g l e s ' , ( v m i n ! i ) , i « l , n d o f ) 0126 wr i te !* ,* ) 'Change? ( t / f ) ' 0127 read!*,*) ana 0128 l f (ana) then 0129 wr i t e !* ,A) 'Enter new value*' 0130 read(A,A) ( v m i n ! i ) , i » l , n d o f ) 0131 endlf 0132 0133 read(l,A) ( vmaxl1 ) , i - l ,ndof ) 0134 wrlte(A,A) 'Maximum j o i n t a n g l e s ' , ( v n a x ( i ) , i » l , n d o f ) 0135 write!A,A) 'Change? ( t / f ) ' 0136 read(*,A) ans 0137 l f (ans) then ^ 013B wr i t e !* ,* ) 'Enter new values ' 0\ 0139 read!*,A) ( v m a x ! i ) , i « l , n d o f ) co 0140 endlf 0141 0142 c Write the parameters back in to the f i l e . 0143 0144 rewind(l) 0145 0146 wrlte( l,A) j f ac t 0147 write( l,A) pfact 0148 w r i t e d.A) f r a c t i 0149 wrlte( l,A) afact 0150 w r i t e ! ! , * ) nmax 0151 w r i t e d , * ) tau 0152 w r i t e d , * ) ateps i 0153 w r i t e d , * ) cmax 0154 w r i t e d , * ) maxpen,jtoler 0155 w r i t e d , * ) <vmin(i>,i-l ,ndof> 0156 w r i t e d , * ) (vmax(1) ,1-1,ndof) 0157 0158 e lse 0159 0160 c read the parameters from a f i l e . 0161 0162 o p e n ! 1 , f l i e - ' p a r a n . d a t ' , s t a t u s - ' o l d ' ) 0163 0164 read!1,A) j f a c t 0165 read!1,A) pfact 0166 readd.A) f r a c t i 0167 read(l,A) afact 0168 read(l,A) nmax 0169 readd.A) tau 0170 readd.A) s teps i 0171 read!1,A) cmax INPUT 13-May-19B7 12i52i47 VAX FORTRAN V4.4-177 Page 4 10-Apr-19B7 13i4Bi30 DUAO I CBUCHAL. 0PT3INPUT. FOR 146 0172 r e a d d , * ) maxpen,jtoler 0173 r e a d d , * ) (v« i in( 1) , i » l , n d o f ) 0174 r e a d d , * ) (vmaxd ) , 1 » 1 ,ndof) 0175 0176 write**,*) ' j o i n t H a l t weighting f a c t o r ' , j fac t 0177 wri te )* ,* ) ' in ter ference weighting f a c t o r ' , p f a c t 0178 wri te )* ,*) 'Penetrat ion f r a c t i o n ' , f r a c t i 0179 wri te )* ,* ) ' j o i n t acce l era t ion weighting f a c t o r ' , a f a c t 01B0 wri te )* ,* ) 'maximum number of i t e r a t i o n s ' , n a a x 0181 w r i t e ) A , A ) ' i t e r a t i o n factor (between 0 and l ) ' , t a u 01B2 w r i t e ( A , A ) 'Maxiaua i t e r a t i o n l i n e a r i z a t i o n e r r o r ' , s t e p s ! 01B3 w r i t e ( A , A ) 'aaxiaua t r a j e c t o r y l i n e a r i z a t i o n e r r o r ' , c a a x 0184 wri te )* ,*) 'aaxiaua penetrat ion and j o i n t H a l t t o l e r a n c e ' , 0183 A aaxpen, j to ler 01B6 w r i t e ( A , A ) ' M i n i H U B jo int a n g l e s ' , ( v a i n ) 1 ) , i " l , n d o f ) 01B7 wri te )* ,* ) 'Maxiaua jo int a n g l e s ' , ( v a a x l 1 > , i » l , n d o f ) 01BB 01B9 endif 0190 0191 c l o s e d ) 0192 0193 re turn 0194 end NO o> PROGRAM SECTIONS Naae Bytea A t t r i b u t e s 0 (CODE 3363 PIC CON REL LCL SHR E X E RD NOHRT LONG 1 (PDATA 668 PIC CON REL LCL SHR NOEXE RD N0HRT LONG 2 (LOCAL 260 PIC CON REL LCL N0SHR NOEXE RD HRT LONG 3 ROBOT 96 PIC OVR REL GBL SHR NOEXE RD HRT LONG 4 PARAM1 IR i T l t T UVR7 HIST. U1RT. IBHR N t n t H F HIT) w r r r U U T W . 5 PARAM3 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 6 TAU 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 7 CMAX 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG B NMAX 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 9 LIMITS 32 PIC OVR REL GBL SHR NOEXE RD HRT LONG 10 STEPSI 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG 11 LIMTOL 8 PIC OVR REL GBL SHR NOEXE RD HRT LONG 12 NRDOF 8 PIC OVR REL GBL SHR NOEXE RD HRT LONG 13 FRACTI 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG T o t a l Space Al loca ted 4467 ENTRY POINTS Address Type Nane 0-00000000 INPUT INPUT 13-May-19B7 12I52I47 VAX FORTRAN V4.4-177 Page 5 10-Apr-19B7 13l46l30 OUAO 1CBUCHAL.OPT3INPUT.FOR|46 VARIABLES Addreaa Type H u e Address Type Naae Addreaa Type Name Addreaa Type Naae 5--00000000 RA4 AFACT 2 -00000000 LA4 ANS 7 -00000000 RA4 CMAX 13 -00000000 R*4 FRACTI A* IA4 I 4--00000000 RA4 JFACT 11 -00000004 RA4 JTOLER 11 -00000000 R*4 MAXPEN 12 -00000000 1*4 NDOF B -00000000 IA4 NMAX 12 -00000004 IA4 NXDOF 4 -00000004 RA4 PFACT 10 -00000000 RA4 STEPS I 6 -00000000 RA4 TAU ARRAYS Addreaa Type Naae Byte I Diaenaiona 3-00000020 RA4 ALPHA 16 (4) 3-00000050 RA4 DIST 16 (4) u-wruwmw TJW4 .vrmvv IA. (w»> 3-00000030 RA4 LEN 16 (4) 3-00000000 IA4 LINK 16 (4) 3-00000040 RA4 THETA 16 (4) 9-00000010 RA4 VMAX 16 (4) 9-00000000 RA4 VMIN 16 (4) FUNCTIONS AND SUBROUTINES REFERENCED O Type Naae Type Naae FORECLOSE FORttOPEN COMMAND QUALIFIERS FOR/LIST 0FT.F0R,RDFILE.FOR,INPUT.FOR,GROW.FOR,FINPAT.FOR,S0LVE1.FOR,INT2D.FOR,HOLL2D.FOR,GMULT.FOR,JACOB.FOR,COSTT.FOR, /CHECK- < NOBOUNDS , OVERFLOW , NOUNDERFLOH) /DEBUG-(NOSYMBOLS.TRACEBACK) / STANDARD- { NO SYNTAX, NOSOURCE_FORM) / SHOW- (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONARY, SINGLE) /WARNINGS-(GENERAL .NODECLARATIONS) /CONTINUATIONS-19 / NOCROSS_REFERENCE /NOD_LINES / NOEXTEND_SOURCE /F77 /NOG_FLOATING /I4 /NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run T i a e • Elapsed T l a e i Page Faul t s I Dynamic Memory• 9.69 seconds 29.22 seconds 727 462 pages 13-May-1987 12I57I37 VAX FORTRAN V4.4-177 Page 7-May-1987 17I44I33 DUAO:CBUCHAL.0PT3INSERT.FOR;20 0001 subroutine i n s e r t ! n a d d , n s u b , i m o d e , a d l i i t , s u b l s t , v a l u e s ) 0002 0003 c Insert and renove d iacre te pos i t ions along the t r a j e c t o r y to 0004 c maintain a des ired l i n e a r i s a t i o n error i n the d i s c r e t e OOOS c approximation of the t r a j e c t o r y . 0006 0007 c Parameterai 000B c nadd- number of pos i t ions to be added 0009 c nsub- number of pos i t ions to be removed 0010 c imode- current i t e r a t i o n d i r e c t i o n , forward or backward 0011 c a d l i s t - l i s t of pos i t ions to be added 0012 c suble t - H a t of pos i t ions to be removed 0013 c va lues - the updated array of jo int valuea 0014 0015 c Common blocka changedi 0016 c l a s t - the number of pos i t ions 0017 001B rea l valuea(4,100) 0019 Integer adl lat (100) , subls t (100) 0020 r e a l tempi4,100) 0021 0022 common / l a a t / l a a t 0023 0024 l f ( l aode . eq . l ) then 0025 l a t a r t ' l a a t 0026 i e n d ' l 0027 i n c r » - l 002B else 0029 i a t a r t - 1 0030 i e n d ' l a a t 0031 i n c r ' l 0032 endif 0033 0034 index'0 0035 i a d d ' l 0036 isub-1 0037 do 10 i n d ' i s t a r t , i e n d , i n c r 003B index'index+1 0039 do 20 1-1,4 0040 tempt 1 , index ) -va luea ! i , ind ) 0041 20 continue 0042 l f I l s u b . l e . n s u b . a n d . s u b l s t I l s u b ) . a q . i n d ) then 0043 isub=isub+l 0044 index-index-1 0045 endif 0046 i f l i a d d . l e . n a d d . a n d . a d l i s t l i a d d ) . e q . i n d ) then 0047 iadd-iadd+1 0048 index-index+1 0049 do 11 1-1,4 0050 tempi 1 , index)- !valuea(1 , lnd+incr)+valuea(1,ind))12. 0051 11 continue 0052 endif 0053 0054 10 continue 0055 0056 las t - index 0057 INSERT 13-May-19B7 12157137 VAX FORTRAN V4.4-177 Page 2 7-May-1987 17i44i33 _DUA0 :CBUCHAL. 0PT3INSERT.FOR> 20 005B 0059 O0B0 00B1 00B2 0063 0064 0065 0066 0067 006B 0069 0070 0071 0072 0073 0074 0075 0076 0077 0078 0079 0080 O0B1 35 50 60 i f ( lnode .eq . l ) then l s t a r t ' l a s t iend=l incr=- l e l se l s t a r t ' l i end ' laa t i n c r ' l endif ind2-0 do 50 i n d ' l a t a r t , i e n d , l n c r I n d 2 » i n d 2 + 1 do 55 j » l , 4 v a l u e s ! j , l n d 2 ) > t e a p (j , l n d ) continue continue do 60 i - 1 , 4 v a l u e a ( 1 , l a » t + l ) ' v a l u e a ( i , l a a t ) continue return end to - J ro PROGRAM SECTIONS Nana 0 (CODE 2 (LOCAL 3 LAST Bytea A t t r i b u t e s 398 PIC COR REL LCL SHR EXE 1692 PIC CON REL LCL NOSHR NOEXE 4 PIC OVR REL GBL SHR NOEXE RD NOKRT LONG RD HRT LONG RD HRT LONG T o t a l Space Al located 2094 ENTRY POINTS Address Type Nane 0-00000000 INSERT VARIABLES Address Type Nane * * 1*4 I 2-00000640 1*4 INCR ** 1*4 I START AP-000000048 1*4 NADD Addreaa Type Nana ** 1*4 IADD 2-0000064C 1*4 IND 2-0000064B 1*4 ISUB AP-O000OO0B8 1*4 NSUB Addreaa Type Nane Address Type Nane ** 1*4 I END ** 1*4 IND2 AA 1*4 J AP-0000000C8 1*4 IMODE 2- 00000644 1*4 INDEX 3- 00000000 1*4 LAST INSERT 13-May-19B7 12157137 VAX FORTRAN V4.4-177 Page 3 7-May-19B7 17i44>33 DUAOiCBUCHAL.OPT3 INSERT. FOR » 20 ARRAYS Addreaa Type Naae AP-00000010B 1*4 AOL1ST AP-00000014B 1*4 SUBLST 2-00000000 R*4 TEMP AP-00000018B R*4 VALUES Bytea Diaenalona 400 400 1600 1600 (100) (100) (4, 100) (4, 100) LABELS Addreaa A* Label 10 Addreai ** Label 11 Addreaa Label A* 20 Addreaa Label AA 50 Addraaa Labe l AA 55 AddreBB Label AA 60 COMMAND QUALIFIERS FOR/LIST OPT. FOR, RDFILE. FOR, INPUT. FOR, GROW. FOR, FINPAT. FOR , SOLVE1. FOR , INT2D. FOR ,HULL 2D. FOR, GMULT. FOR , JACOB. FOR , COSTT. FOR , /CHECK" (NOBOUNDS, OVERFLOW, NOUNDERFLOM) ro /DEBUG" (NO SYMBOLS .TRACEBACK) >J / STANDARD* (NOSYNTAX, N0SOURCE_F0RM) W / SHOW* (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONARY, SINGLE) /WARNINGS"! GENERAL, NODECLARATIONS) /CONTINUATIONS = 19 /NOCROSS_REFERENCE /NOD_LINES / NOEXTEND_SOURCE /F77 / NOG_FLOAT I NG /I4 /NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run T i a e i 3.51 aeconds Elapsed Tiae t B.96 seconds Page F a u l t s ! 581 Dynamic Memory! 845 pages 13-Kay-19B7 12136138 VAX FORTRAN V4.4-177 Page 1 ll-May-1987 14)14)10 DUAO i CBUCHAL. 0PT3INTERF. FOR) 24 ts3 0001 0002 0003 0004 0005 0006 0007 OOOB 0009 0010 0011 0012 0013 0014 0015 0016 0017 0018 0019 0020 0021 0022 0023 0024 0025 0026 0027 0028 0029 0030 0031 0032 0033 0034 0035 0036 0037 003B 0039 0040 0041 0042 0043 0044 0045 0046 0047 004B 0049 0050 0051 0052 0053 0054 0055 0056 0057 11 subroutine i n t e r f ( h u l l , o b s t , n v e r t l , n v e r t 2 , i l i n k , i o b s t , i v e r t s , i n f l a g ) Test for interference between a awept volume and an obs tac le , and f i n d the necessary c o r r e c t i o n vectors to reduce the in t er f erence . Paraaetera• h u l l - array of convex h u l l v e r t i c e a obat- array of obstacle v e r t i c e s n v e r t l , n v e r t 2 - number of v e r t i c e s In h u l l , o b a t l l i n k - moving l i n k id i obs t - s tat ionary obstacle id i v e r t a - array l i n k i n g h u l l v e r t i c e a to corresponding polygon v e r t i c e s i n f l a g - . t r u e , l f Interference l a detected r e a l hul l (2 ,20) ,obat (2 ,10) r e a l v e c t l ( 2 ) , v e c t 2 ( 2 ) , p o l n t l ( 2 ) , p o i n t 2 ( 2 ) r e a l vec t (2 ) ,po int (2 ) r e a l v l ens (2 ) ,vec t s (2 ,2 ) ,po in t s (2 ,2 ) Integer l v e r t s ( 2 , 2 0 ) , l p a l r ( 2 ) l o g i c a l i n f l a g , l e d g e , i f l a g 2 do 11 i - 1 , 2 v e c t l ( i ) - 0 . v e c t 2 ( i ) - 0 . p o i n t K D ' O . po int2( i )*0 . continue Test for interference between the convex h u l l representing a swept volume and a convex obstac le . c a l l l n t 2 d ( h u l l , o b a t , n v e r t l , n v a r t 2 , l n f l a g , d i a t a n , v e c t , p o i n t , i v e r t , l e d g e ) l f l a g 2 " . f a l s e . If Interference i s d e t e c t e d . . . i f ( in f lag) then If the Interference vector in tersec t s an edge of the convex h u l l . . i f (ledge) then i t e s t l - i v e r t s ( l , i v e r t Y i p a i r ( l ) - i v e r t F ind the endpoints of the edge, and the objects to which they belong. i f ( i v e r t . e q . n v e r t l ) then i t e s t 2 - i v e r t s ( l , l ) ipa i r (2 )= l e lse i t e s t 2 - i v e r t s ( l , i v e r t + l ) INTERF 13-Hey-19B7 12156138 VAX FORTRAN V4.4-177 Page 2 ll-May-1987 14|14|10 __DUAO : CBUCHAL. 0PT3INTERF. FOR) 24 0058 i p a i r ( 2 ) * i v e r t + l 0059 endif 0060 0061 c If the endpoint* l i e on d i f f e r e n t ob jec t s , then the edge i s 0062 c a swept edge. 0063 0064 i f ( i t e s t l . n e . i t e s t 2 ) then 0065 l f l a g 2 * . t r u e . 0066 i f ( l t e a t l . e q . 2 ) then 0067 i p a i r ( l ) « i p a i r ( 2 > 0068 1 p a i r !2 ) " i v e r t 0069 endif 0070 endif 0071 endif 0072 0073 c If the penetrat ion vector in tersec t s a awept edge, then 0074 c both the i n i t i a l and f i n a l poai t ions should be modif ied. 0075 c The r e l a t i v e notions of the end pos i t ions are weighted ao 0076 c that when combined the swept volume w i l l move away from 0077 c the penetrat ion i n the d i r e c t i o n of the penetrat ion 007B c vector . 0079 0080 l f ( l f lag2) then 0081 r l e n - 0 . 0082 r len2*0. 0083 do 1001 1*1,2 0084 r l e n - r l e n + ( h u l l ( i , i p a i r ( l ) ) - h u l l ( i , i p a i r ( 2 ) ) ) * * 2 0085 r l e n 2 T l e n 2 + < p o i n t ( i ) - h u l l ( i , l p a i r (2)))**2 0086 1001 continue 0087 rlamb-sqrtt r l e n 2 / r l e n ) 0088 a l p h « r l a m b / ( l . - r l a m b + r l a m b * * 2 ) 0089 be ta* l . - r lamb*a lph 0090 temp-0. 0091 do 1003 1*1,2 0092 v e c t K i ) - a l p h * v e c t ( i ) 0093 p o i n t l ( i ) * h u l l ( i , i p a l r ( 2 ) ) 0094 temp'temp+vectll1)**2 0095 1003 continue 0096 0097 else 009B 0099 c I f the penetrat ion vector in tersec t s the f i n a l p o a i t l o n . 0100 c then only the f i n a l pos i t i on i s modif ied. 0101 0102 i f ( i v e r t . g t . 0 . and. i v e r t . I t . 9. and. i v e r t a d , i v e r t ) . eq. 2) then 0103 beta '0 . 0104 temp*0. 0105 do 1010 1*1,2 0106 v e c t K i ) - v e c t ( i ) 0107 p o i n t l ( i ) * p o i n t ( i ) 0108 1010 continue 0109 elBe 0110 0111 c Otherwise, only the i n i t i a l p o s i t i o n i s modified 0112 0113 beta-1 . 0114 do 1011 i * l , 2 IBTERF 13-May-19B7 12i56i3B ll-May-1987 14i l4:10 VAX FORTRAN V4. 4-177 DUAO iCBUCHAL. OPT3INTERF.FOR;24 Page to 0115 one 0117 011B 0119 0120 0121 0122 0123 0124 0125 0126 0127 0128 0129 0130 0131 0132 0133 0134 0135 0136 0137 0138 0139 0140 0141 0142 0143 0144 0145 0146 0147 0148 0149 0150 0151 0152 0153 0154 1011 1100 920 99 v e c t l ( i ) - 0 . p o l n t l ( l ) « 0 . continue endif endif If there l a no penetrat ion , aat the penetrat ion vector and po int to zero. e l se do 1100 1-1,2 v e c t t i ) - 0 . p o i n t ( i ) - 0 . continue endlf Update the c o r r e c t i o n to be saved for the next i t e r a t i o n . do 920 1-1,2 vect2( i ) -beta*vect(1) i f ( i f lag2) then p o i n t 2 ( 1 ) - h u l l ( 1 , i p a i r ( 1 ) ) e l se p o i n t 2 ( l ) - p o i n t ! 1 ) endif continue Save the c o r r e c t i o n vectors and points l n the database. do 99 i - 1 , 2 v e c t a ( l , l ) - v e c t l ( l ) v e c t a ( l , 2 ) - v e c t 2 ( i ) p o i n t a l i . D - p o i n t K l ) p o i n t s ( 1 , 2 ) « p o i n t 2 ( i ) continue cal1 c v e c t ( 2 , i l i n k , i o b s t , v l e n s , v e c t s . p o i n t s ) return end INTERF 13-May-19B7 12i56i3B VAX FORTRAN V4.4-177 Page 4 l l-May-1987 1 4 l l 4 i l 0 __DUA0 1C BUCHAL. OPT3INTERF. FOR j 24 PROGRAM SECTIONS Naaa 0 (CODE 1 «PDATA 2 (LOCAL Bytes A t t r i b u t e s 528 PIC CON REL LCL SHR EXE 4 PIC CON REL LCL SHR NOEXE 24B PIC CON REL LCL NOSHR NOEXE RD NOHRT LONG RD NOHRT LONG RD WRT LONG T o t a l Space Al located 780 ENTRY POINTS Addreaa Type Nane 0-00000000 INTERF ro VARIABLES Addreaa Type Nane A A R*4 ALPH 2-00000060 L*4 IEDGE AP-000000180 1*4 IOBST AP-0000000CB 1*4 NVERT1 ** R*4 RLEN 2 Addreaa Type Nana 2-00000070 R*4 BETA 2-00000064 L*4 IFLAG2 ** 1*4 ITEST1 AP-00000010O 1*4 NVERT2 ** R*4 TEMP Addreaa Type Naae 2-00000068 R*4 DI STAN AP-000O0014H 1*4 ILINK * * 1*4 ITEST2 * * R*4 RLAMB Addreaa Type Name * * 1*4 I AP-00000020O L*4 INFLAG 2-0000006C 1*4 IVERT ** R*4 RLEN ARRAYS Addreaa Type Naae Bytes Olnensli AP-000000040 R*4 HULL 160 (2, 20) 2-000O005B 1*4 IPAIR a (2) AP-0000001CO 1*4 I VERTS 160 <2, 20) AP-0000000BB R*4 OBST BO (2, 10) 2-0000002B R*4 POINT 8 (2) 2-00000010 R*4 POINT1 a (2) 2-00000018 R*4 POINT2 a (2) 2-00O0004B R*4 POINTS 16 (2, 2) 2-00000020 R*4 VECT 8 (2) 2-00000000 R*4 VECT1 B (2) 2-O000000B R*4 VECT2 a (2) 2-00000038 R*4 VECTS 16 12, 2) 2-00000030 R*4 VLENS B (2) LABELS Address A * * * Label 11 1011 Address A * A A Label 99 1100 Addreas Label A A 920 Addreaa Label A A 1001 Address Label A A 1003 Addreas Label A A 1010 INTERF 13-May-19B7 12l56i38 ll-May-19B7 1 4 i l 4 i l 0 VAX FORTRAN V4. 4-177 DUAO:C BUCHAL.OPT]INTERF.FOR124 Page 5 FUNCTIONS AND SUBROUTINES REFERENCED Type Naae Type Nane Type Naae CVECT INT2D R*4 MTHfiSQRT COMMAND QUALIFIERS FOR/LIST OPT. FOR, RDFILE. FOR, INPUT. FOR, GROW. FOR, FINPAT. FOR, SOLVE1. FOR, INT2D. FOR, HULL2D. FOR, CMULT. FOR, JACOB. FOR, COSTT. FOR, / CHECK • (NOBOUNDS, OVERFLOW, NOUNDERFLOH) / DEBUG" (NOSYMBOLS, TRACEBACK) / STANDARD" (NOSYNTAX, NOSOURCE_FORM) / SHOW" (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONARY, B INGLE) /WARNINGS" (GENERAL,NODECLARATIONS > /C0NTINUATI0NS = 19 /NOCROSS_REFERENCE /NODLINES /NOEXTEHDBOURCE /F77 /NOG FLOATING /I4 /NOMACHINE CODE /OPTIMIZE COMPILATION STATISTICS Run T l a e i 5.16 seconds Elapsed T l a e i 14.92 seconds Page F a u l t s i 623 Dynamic MemoryI B45 pages 13-May-1987 13 i l0 i35 VAX FORTRAN V4.4-177 13-May-19B7 13I09I15 DUAO i CBUCHAL. OPT3INT2D.FOR»4 Page 1 OOOl subroutine i n t 2 d ( p o l y l , p o l y 2 , n v e r t l , n v e r t 2 , i f l a g , d i s t , v e c t , p o i n t , i v e r t , 0002 * ledge) 0003 0004 c Test two 2-D polygons for i n t e r s e c t i o n , and f i n d the penetrat ion vector 0005 c and penetrat ion point r e l a t i v e to the f i r a t polygon. 0006 0007 c Parameters• OOOB c p o l y l , p o l y 2 - arraya of v e r t i c e s representing two 2-D polygons. 0009 c n v e r t l , n v e r t 2 - the number of v e r t i c e s l n p o l y l , p o l y 2 0010 c l f l a g - returns . t r u e , i f the polygons i n t e r s e c t . 0011 c d i e t - returns the interference d i s tance . 0012 c vec t - returns the minimum interference vec tor , d i r e c t e d 0013 c from polygon 1 boundary to polygon 2 boundary. 0014 c po in t - returns the head of vec tor , attached to polygon 1. 0015 c i v e r t - Index of the c loses t vertex or f i r s t vertex of 0016 c c loses t edge of polygon 1 i f ' l e d g e ' • . t r u e . 0017 c ledge- . t r u e , l f the minimum distance vector i n t e r s e c t s an ooia c edge of the f i r s t polygon 0019 0020 c Data s t ruc tures ! 0021 c boxes- combines the two polygons i n a s ing le array 0022 c i p a i r - i p a l r l l ) ' index of c loses t vertex of current polygon 0023 c i p a l r ( 2 ) « Id of other polygon 0024 c i p a i r ( 3 ) • index of c loses t vertex of other polygon 0025 0026 r e a l point!2) 0027 r e a l p o l y l ( 2 , n v e r t l ) , p o l y 2 ( 2 , n v e r t 2 ) 0028 r e a l boxes(2,10,2) 0029 l o g i c a l l f l a g 0030 r e a l vect(2) 0031 r e a l vnorm(2) 0032 r e a l v l (2) ,v2(2) 0033 integer ipa ir*3) 0034 integer nvert l2) 0035 l o g i c a l ledge 0036 0037 l f l a g - . t r u e . 0038 dist - -10000. 0039 i v e r t - 1 0040 thresh-.00001 0041 0042 c Combine the polygons in to a s ingle a r r a y . 0043 0044 do 1 i » l , 2 0045 do 2 j « l , n v e r t l 0046 b o x e a l i , j , l ) - p o l y l l i , J ) 0047 2 continue 004B do 3 j>l ,nvert2 0049 boxea1 i , j ,2 ) -po ly211 , j) 0050 3 continue 0051 1 continue 0052 0053 n v e r t l 1 ) ' n v e r t l 0054 n v e r t l 2 ) » n v e r t 2 0055 0056 c Test the f i r s t polygon against the second polygon. 0057 IHT2D 13-Hay-19B7 13l lOi3B VAX FORTRAN V4.4-177 Page 2 13-May-19B7 13 i09 i l5 DUAO!tBUCHAL.0PT31NT2D.F0R;4 OOSB do 5 i n d 3 - l , 2 0059 If ( l f l a g ) then 0060 0061 l o t h e r « 2 0062 l f ( ind3.eq.2) io ther -1 0063 0064 c Find the edge which maximises the separating d i s tance . 0065 0066 do 10 i n d « l , n v e r t ( i n d 3 ) 0067 006B l f ( l f l a g ) then 0069 l f (ind . eq .nvert ( ind3)> then 0070 do IS j » l , 2 0071 v l ( j ) * b o x e s ( j , l , i n d 3 ) - b o x e s ( j , n v e r t ( i n d 3 ) , l n d 3 ) 0072 IS continue 0073 e lse 0074 do 20 j - 1 , 2 0075 v l ( j ) -boxes ( j , lnd+1 , ind3 >-boxes(j , ind, ind3) 0076 20 continue 0077 endi f 007B 0079 c Ca lcu la te the outward normal. 0080 00B1 v l e n » s q r t ( v l ( l ) * * 2 + v l ( 2 ) * * 2 ) 00B2 v n o n s t l ) » - v l ( 2 > / v l e n 0083 v n o r m ( 2 ) - v l ( l ) / v l e n 0084 0085 c Calculate constant of l i n e equation. 0086 00B7 const-boxes!1,Ind, lnd3)*vnora(l)+boxes(2,Ind, lnd3)*vnora(2) 0088 0089 c Plug i n an adjacent vertex of b o x l . 0090 0091 l f ( l n d . e q . l ) then 0092 itemp-nvert(ind3) 0093 e lse 0094 itemp-ind-1 0095 endif 0096 0097 t e s t l « b o x e s ( 1 , 1 t e m p , l n d 3 ) * v n o r n ( l ) 0098 A +boxes(2,itemp,ind3)*vnorn(2)-const 0099 0100 c Test l f a l l the v e r t i c e s of box2 are on the opposite side and sava 0101 c the c loses t vertex . 0102 0103 dain2-10000. 0104 do 25 l n d 2 - l , n v e r t ( l o t h e r ) 0105 0106 I f ( l f l a g ) then 0107 test2-boxes( l , ind2,iothsr)Avnorm<l>+ 0108 * boxes!2, ind2,iother>Avnormt2)-const 0109 l f (test2.1t .dmln2) then 0110 dmin2-test2 0111 l p a l r ( l ) - i n d 0112 i p a i r ( 2 ) - i o t h e r 0113 ipair ( 3 ) - i n d 2 0114 endif INT2D 13-May-19B7 13110139 VAX FORTRAN V4.4-177 Page 3 13-May-19B7 13l09i l5 ^DUA0iCBUCHAL.OPT31NT2D.F0R|4 00 0119 0116 endif 0117 23 continue 011B 0119 c If B O , the boxes are d i s j o i n t . 0120 0121 i f ( t ea t lAds i ln2 . l t . thresh) then 0122 i f l a g * . f a l s e . 0123 endif 0124 0129 c I f the boxes are not d i s j o i n t , update the in ter ference data . 0126 c Record the vertex an the f i r s t polygon which i s e i ther the c lo se s t 0127 c vertex , or the f i r s t vertex of the c loses t edge. 0128 0129 0130 l f ( i f l a g . a n d . d « i n 2 . g t . d i e t ) than 0131 dls t*daln2 0132 l f ( ind3.eq. l> than 0133 do 55 1*1,2 0134 v e c t ( 1 ) » d i s t A v n o r a l i ) 0135 p o l n t ( i ) ' b o x e s ( i , i p a i r ( 3 ) , i p e i r < 2 ) ) - d i s t A v n o r a l i ) 0136 55 continue 0137 i v e r t - i p a i r ( l ) 0138 ledge* . true . 0139 e l se 0140 do 60 1*1,2 0141 v e c t ( i ) * - d l s t * v n o r a ( l ) 0142 p o i n t ( i ) * b o x e s ( i , i p a l r ( 3 ) , l p a i r ( 2 ) ) 0143 60 continue 0144 l v e r t * l p a l r ( 3 ) 0145 l edge* . fa l se . 0146 endif 0147 andlf 014B endif 0149 10 continue 0150 endif 0151 0152 5 continue 0153 0154 0155 c If the boxea are d i s j o i n t , reset the output paraaeters 0156 0157 l f l . n o t . l f l a g ) then 0158 l e d g e - . f a l s e . 0159 do 150 i - 1 , 2 0160 v e c t ( i ) « 0 . 0161 p o i n t ( i ) * 0 . 0162 190 continue 0163 endif 0164 0165 return 0166 end INT2D 13-May-19B7 13:10l33 VAX FORTRAN V4.4-177 Page 4 13-Hay-19B7 13I09I15 DUAO :CBUCHAL.OPT3INT2D.FOR>4 PROGRAM SECTIONS Naaa 0 (CODE 2 (LOCAL Bytea A t t r i b u t e s 7B5 364 PIC CON REL LCL SHR EXE PIC CON REL LCL NOSHR NOEXE RO NOHRT LONG RO HRT LONG T o t a l Space A l l o c a t e d 1149 ENTRY POINTS Addreaa Type Naae 0-00000000 INT2D VARIABLES Addreaa Type Naae Addreaa Type Naae Addreaa Type Naae Addreaa Type Naae ho oo A* R*4 CONST AP-000000280 L M I EDGE AA 1*4 IND3 AA 1*4 J * * R*4 TEST2 AP-OOOOOOlBfJ R*4 DIST AP-000000148 L*4 IFLAG 2-000000CC 1*4 IOTHER AP-O0O00O0CO 1*4 NVERT1 * * R*4 THRESH * * R*4 DMIN2 2-00000ODO 1*4 IND * * 1*4 ITEMP AP-00000010B 1*4 NVERT2 * * R*4 VLEN * * 1*4 I * * 1*4 IHD2 AP-000000240 1*4 IVERT * * R*4 TEST1 ARRAYS Addreaa Type Naaa Bytea Dlaeni lona 00000000 R*4 BOXES 160 (2, 10, 2) 00O0O0BB 1*4 IPAIR 12 (3) O0OOO0C4 1*4 NVERT B (2) 00000020(1 R*4 POINT B (2) 000000043 RA4 P0LY1 A* (2, A) 000000083 R*4 POLY2 AA (2, A) 000000AB R*4 VI B (2) 000000B0 R*4 V2 B 12) ooooooica R*4 VECT B (2) 00O000A0 R*4 VNORM B (2) LABELS Addreaa Label Addreaa Label Addreaa Label Addreaa Label Addreaa Labe l Addreaa Label AA 1 AA 2 AA 3 AA S AA 10 AA 15 AA 20 AA 25 AA 55 AA 60 AA 150 INT2D FUNCTIONS AND SUBROUTINES REFERENCED Type Nane R M MTHOSORT 13-May-19B7 13 i l0 i35 13-May-19B7 13 i09 i l5 VAX FORTRAN V4.4-177 DUAO : C BUCHAL. OPT3INT2D. FOR > 4 Page oo 13-May-19B7 13l01i30 VAX FORTRAN V4.4-177 Page 1 ll-May-19B7 1B|38:56 DUAO s CBUCHAL.0PT3INVERS.FOR j 167 0001 subroutine i n v e r s ( l e n . a l p h a , d i s t , t h e t a , j t y p e , n d o f , n x d o f , i x d o t , 0002 * rotdee,xgoal , theta2) 0003 0004 c Find an optimal j o i n t conf igurat ion for a given too l p o s i t i o n . 0005 c An optimal inverse so lut ion i a found for a redundant manipulator 0006 c by i t e r a t i v e l y reducing the d i f ference between the des ired and 0007 c ac tua l gr ipper p o s i t i o n s , while simultaneously using redundancy to oooa c maximize jo in t a v a i l a b i l i t y . 0009 0010 c Parameters• 0011 0012 c l e n , a l p h a , d i s t - f ixed l i n k parameters 0013 c theta- jo in t angles 0014 c j type- array spec i fy ing jo in t types (prismatic or revolute) 0015 c ndof,nxdof- number of jo in t and task coordinates 0016 c ixdot - an array mapping a (1 X nxdof) task vector onto a 0017 c standard ( 1 X 6 ) vector containing three ro ta t ions 0018 c and three t r a n s l a t i o n a . 0019 c kgoal ,angoal - the des ired general ized ro ta t ion vector and angle 0020 c rotdes- des ired o r i e n t a t i o n matrix 0021 c xgoal- the des ired car tes ian gr ipper p o s i t i o n 0022 c theta2- the inverse jo int so lu t ion 0023 0024 0025 r e a l l en (* ) ,d ia t (* ) , a lpha(* ) , the ta (* ) , the ta2 (* ) 0026 integer jtype(4) 0027 r e a l S(10),UD(10,6),VD(10,10> 0028 r e a l xdot(6),xdot2(6) 0029 r e a l mat (10 , l l ) 0030 r e a l d last !10) ,danglet10) 0031 r e a l thedot!10) 0032 r e a l tsave(lO) 0033 r e a l tnet(4 ,4) 0034 r e a l xgoal!*) 0035 r e a l asave(lO) 0036 r e a l angles!10) 0037 r e a l k(3) ,kgoal(3) 0038 r e a l term!3) 0039 l o g i c a l ixdot(6) 0040 r e a l tempi(6) 0041 r e a l temp3(6) 0042 r e a l Jac(6 , 1 0 ) , i d e n t ( 4 , 4 ) 0043 r e a l rotdes(3,3) 0044 r e a l tmat (3 ,3 ) , t inv!3 ,3 ) 0045 r e a l rot(3) 0046 l o g i c a l r e p e a t , r o t f l g 0047 r e a l vdt(10,10) ,homog(10,10) , thdotl(10) ,matl(10,10) ,xdotl (6) 0048 r e a l tempt 6 ,6 ) , j ac inv l10 ,6 ) 0049 l o g i c a l stop2 0050 0051 data i d e n t / 1 . , 4 * 0 . , 1 . , 4 * 0 . , 1 . , 4 * 0 . , 1 . / 0052 0053 c Test f i l e B . 0054 0055 open)1 , f l i e - ' a l . d a t ' , s t a t u s - ' n e w ' ) 0056 o p e n ( 2 , f i l e - ' a 2 . d a t ' . s t a t u s - ' n e w ' ) 0057 open!3,f i l e - ' a 3 . d a t ' , s t a t u s - ' n e w ' ) INVERS 13-Kay-19B7 13I01I30 l l -May-19B7 1BI3B:56 0058 o p e n ( 4 , f l i e - ' a 4 . d a t ' , s t a t u s - ' n e w ' ) 0059 o p e n ! 5 , f i l e - ' e r r . d a t ' , s t a t u s • ' n e w ' ) 0060 o p e n ' 9 , f i l e = ' f o u l . d a t ' , s t a t u s - ' n e w ' ) 0061 o p e n ! 1 1 , f l i e - ' t l . d a t ' , s t a t u s - ' n e w ' ) 0062 o p e n ! 1 2 , f i l e = ' t 2 . d a t ' , s t a t u B - ' n e w ' ) 0063 open! 13,f H e - ' f a c t . d a t ' , s t a t u s - ' n e w ' ) 0064 o p e n ! 14 , f l i e - ' e r l n . d a t ' . s ta tus - ' new' ) 0065 o p e n f 1 5 . f i l e - ' t 3 . d a t ' , s t a t u B - ' n e w ' ) 0066 o p e n ! 1 6 . f i l e - ' a n o r . d a t ' , s t a t u s - ' n e w ' ) 0067 006B c Set maximum l i n e a r i z a t i o n e r r o r , j o i n t a v a i l a b i l i t y f a c t o r , maximum 0069 c number of i t e r a t i o n s . 0070 0071 wr i te !* ,* ) 'Enter ermaz, gamma ' 0072 read!*,*) ermaz,gamma 0073 0074 nmaz-100 0075 0076 s top2". fa l se . 0077 p i » 3 . 1 4 1 5 9 2 7 0078 t o l e r - . 0 1 0079 t o l e r 2 - . l 0080 toler4- .0001 00B1 d2rad=.0174533 0082 B t o l e r - , 0 0 0 1 0083 t h o m o g - 1 . 0084 0085 c Set a f l a g l f there are any r o t a t i o n a l task coordinates . 00B6 0087 r o t f l g » i z d o t ( l ) .or . ixdot l .2) . o r . i z d o t ( 3 ) 0088 0089 c Convert angles to radians . 0090 0091 do 1 1-1,ndof 0092 angles! i )=d2rad*theta(i) 0093 a s a v e ! i ) - a n g l e s ! i ) 0094 1 continue 0095 0096 c I t e r a t i v e l y converge towards an optimal j o i n t conf igurat ion . 0097 009B number=0 0099 0100 100 continue 0101 0102 i f !stop2.or.number.gt .nmaz) goto 200 0103 0104 number=number+l 0105 0106 do 2 l ' l . n d o f 0107 t h e t a ( i ) - a n g l e s ! l ) / d 2 r a d 010B 2 continue 0109 0110 d o 251 1 - 1 , n d o f 0111 thedot!i)=0. 0112 t h d o t l l i ) = 0 . 0113 251 continue 0114 VAX FORTRAN V4.4-177 Page 2 DUAO ! C BUCHAL. 0PT3 INVERS. FOR»16 7 INVERS 13-May-1987 13i01i30 VAX FORTRAN V4.4-177 Page 3 ll-May-1987 18i3Bs56 DUAO!CBUCHAL.OPT3 INVERS.FOR; 167 0115 c Calcu la te too l Jacobian. 0116 O i l 7 c a l l j a c o b ( i d e n t , i d e n t , l e n , a l p h a , d l a t , t h e t a , j t y p e , n d o f , t n e t , J a c ) 0118 0119 c Calculate the equivalent angle and axis of ro ta t ion for the 0120 c current frame o r i e n t a t i o n , (see Pau l , pp.25-33) 0121 0122 i f ( ro t f lg ) then 0123 0124 do 222 1-1,3 0125 do 333 j - 1 , 3 0126 t i n v d , j) = tnet( j , i ) 0127 333 continue 0128 222 continue 0129 0130 c a l l gmul t ( ro tdes , t inv , tmat ,3 ,3 ,3 ,3 ,3 ,3 ) 0131 0132 term!1>-(tmat(3,2)-tmat(2,3)) 0133 t e r m ( 2 ) = ( t m a t ( l , 3 ) - t m a t ( 3 , l ) ) 0134 term!3)*( tmat (2 ,1 ) - tmat (1 ,2 ) ) 0135 t e r m 4 ° t m a t ( 1,1 Ktmat ( 2,2) +tmat ( 3,3)-1 0136 0137 ang-atan2( sqrt(term( 1 >**2+term( 2>**2+term( 3)**2) , t em4) ^ 013B B l n a n g = B i n ( a n g ) oo 0139 cosang=cos(ang) a\ 0140 0141 i f (ang .g t . to ler4) then 0142 i f ( a n g . l t . 1 . 5 ) then 0143 denora-2.*sinang 0144 do 110 i - l , 3 0145 k( i ) - term(l) /denom 0146 110 continue 0147 e lse 0148 k m a x - 0 . 0149 do 111 1-1,3 0150 ttemp=tmat(1,i)-cosang 0151 i f ( t temp.gt .O.) then 0152 k( i>=sign( l . , term(1))Asqrt( t temp)/ (1 . -cosang) 0153 else 0154 k(i)=0. 0155 endif 0156 i f ( a b s ( k ( i ) ) . g t . k m a x ) then 0157 k m a x = a b B ( k ( i ) ) 015B l s a v e - i 0159 endif 0160 111 continue 0161 0162 i f ( i s a v e . e q . l ) then 0163 denom-2.*k(l>*(l.-cosang) 0164 k(2)=(tnet(2,1)+tnet(1,2))/denom 0165 k(3 ) - ( tne t (3 , l )* tne t ( l , 3 ) ) /denom 0166 e l s e i f ( l e a v e . e q . 2 ) then 0167 d e n o m = 2 . * k ( 2 ) M l . - c o s a n g ) 016B k(1)=(tnet(2 ,1)+tnet(1 ,2)) /denom 0169 k (3 )=( tne t (3 ,2 )+tne t (2 ,3 ) ) /denom 0170 e l B e 0171 denom-2.*k(3)*(1.-cosang) IRVERS 13-Kay-1987 13)01:30 VAX FORTRAN V4.4-177 Page 4 U-May-1987 18:38:56 DUAO:CBUCHAL.OPT3INVERS.FORt167 0172 It (1) = (tnet (3,1) +tnet (1,3)) /denom 0173 k ( 2) = (tnet(3,2)+tnet(2,31)/denom 0174 endif 0175 endif 0176 0177 e lse 017B 0179 ang=0. 0180 0181 endif 01B2 0183 do 223 1=1,3 0184 rot ( i )=k( i )*ang 0185 223 continue 0186 01B7 endif 0188 01B9 c The error l a the d i f f erence between the too l p o s i t i o n for the 0190 c current i t e r a t i o n and the desired too l p o s i t i o n . 0191 0192 c Save the previous p o s i t i o n e r r o r . 0193 0194 do 300 i « l , 6 0195 z d o t 2 ( i ) » z d o t ( i > 0196 300 continue 0197 0198 c Ca lcu la te the vector d i f ference between the des ired and ac tua l 0199 c o r i e n t a t i o n s . This i s not accurate i f the or i en ta t ions d i f f e r 0200 c s u b s t a n t i a l l y , but the accuracy improves as the d i f f erence i s 0201 c reduced. 0202 0203 e r r o r - 0 . 0204 0205 i f ( ro t f l g ) then 0206 0207 do 120 ind-1 ,3 0208 i f ( ixdot ( ind) ) then 0209 zdot(ind>=rotfind) 0210 error=error+rot(ind)**2 0211 endif 0212 120 continue 0213 0214 endif 0215 0216 c Calcu la te the p o s i t i o n e r r o r . 0217 0218 do 5 1-1,3 0219 zdot (i+3) - - tne t ( i , 4 ) +jtgoal ( i ) 0220 error-orror+zdot(i+3)**2 0221 5 continue 0222 0223 error=sqrt (error) 0224 0225 c Stop the in tera t ions when the ac tua l gr ipper pos i t i on i s w i th in 0226 c a t o l erab le distance from the des ired p o s i t i o n and the configurat: 0227 c i s optimized. 0228 INVERS 13-May-19B7 1 31O i l 30 VAX FORTRAN V4.4-177 Page 5 ll-May-1987 18(38:56 DUAO : CBUCHAL. 0FT3INVERS. FOR»167 0229 i f ( e r r o r . I t . t o l e r . a n d . t h o m o g . I t . t o l e r ) then 0230 Btop2= . t r u e . 0231 e lse 0232 0233 c Load the matrices for the s ingular value decomposition r o u t i n e . 0234 0235 i c o u n t « 0 0236 do 20 i - 1 , 6 0237 l f (ixdot<i>) then 0238 icount=icount+l 0239 do 21 j=l,ndof 0240 m a t ( i c o u n t , j ) • j a c t i , J ) 0241 matl < i count , j )* jac t1 , j ) 0242 21 continue 0243 matt icount,ndof+1)=xdot(1) 0244 endif 0245 20 continue 0246 0247 0248 c Perform a s ingular value decomposition. 0249 c Ca lcu la te the minimum norm jo in t change which w i l l reduce the e r r o r 0250 c to a spec i f i ed to lerance . 0251 0252 c a l l svdtmat,S,UD,VD,10,10,nxdof ,ndof ,1 ,0 ,nxdof) 0253 0254 0255 c If the matrix i a nearly rank d e f i c i e n t , approximate i t by a matrix 0256 c of reduced rank. P h y s i c a l l y t h i s means when two l i n k degrees of 0257 c freedom are nearly dependent, remove one of them. 025B c Otherwise, j o i n t displacements can become very large . 0259 c The number of non-zero s ingular values i s the rank of the matr ix . 0260 c The magnitude of the smallest s ingular value i s a measure of the 0261 c 'd i s tance ' to a matrix of reduced rank. 0262 0263 do 130 1=1,nxdof 0264 i f ( s t i l / e t D . g t . s t o l e r ) then 0265 i n a z - i 0266 endif 0267 130 continue 0268 0269 c Load the s ingular value matr i z . 0270 0271 do 131 i=l , imax 0272 do 132 j=l,imax 0273 i f t i . e q . j ) then 0274 tempt j , i ) = l . / • ( j ) 0275 e lse 0276 temp(j, i)=0. 0277 endif 027B 132 continue 0279 131 continue 02BO 0281 0282 do 140 l = l , l a a x 02B3 temp3(i)=mat(i,ndof+1) 02B4 140 continue 0285 INVERS 13-May-1987 13I01I30 VAX FORTRAN V4.4-177 Page 6 ll-May-19B7 18:3B:56 DUAO : CBUCHAL. 0PT3 INVERS .FOR »167 0286 c Calcu la te the minimum norm s o l u t i o n . 0287 02B8 c a l l gnult(temp,temp3,tempi, imax,imax.1,6,6,6) 0289 c a l l gmult(vd,tempi , thedot ,ndof , imax,1 ,10,6 ,10) 0290 0291 c Calcu la te the homogeneous s o l u t i o n . 0292 0293 do BOO 1*1,ndof 0294 do 801 j=l ,ndof 0295 v d t d , j ) « - v d < j , i > 0296 801 continue 0297 BOO continue 0298 0299 c a l l gmult(vd,vdt,homog,ndof, imax,ndof,10,10,10) 0300 0301 do 39 1-1,ndof 0302 homog(i , i ) -homog(i , i )+1. 0303 39 continue 0304 0303 c a l l gmu1t(homog,angles,thdot1,ndof,ndof,1,10,10,10) 0306 0307 c Adjust the step s i ze to insure rapid convergence. 0308 c The step s i ze i s chosen to give a steepest descent i t e r a t i o n when 0309 c the error i s l a r g e , and a r a p i d l y converging Newton-Raphson : i t e r a t i o n 0310 c when the error i s smal l . 0311 0312 0313 c Optimize the so lu t ion to maximize j o i n t a v a i l a b i l i t y . 0314 0315 0316 c Calcu la te the l i n e a r i z a t i o n error and c a l c u l a t e the new step s i ze 0317 c f a c t o r . 0318 0319 e r l i n - 0 . 0320 i f (number.eq.1) then 0321 do 310 i - 1 , 6 0322 er l in=erl in+xdot( i )**2 0323 310 continue 0324 e r l i n - s q r t ( e r l i n ) 0325 fmul t -amin l ( ( ermax/er l in ) ,1 . ) 0326 e lse 0327 do 302 1=1,6 032B er l in*er l in+(xdot ( i ) -< l . - fmul t )*xdot2 ( i ) )**2 0329 302 continue 0330 e r l i n = s q r t ( e r l i n ) 0331 fmult- l fmult+aminK (fmult*ermaz/erl in 1,1. )) 12 0332 endif 0333 0334 i f ( er l ln .g t .2 .*ermax) then 0335 do 303 i - l , n d o f 0336 angles(1)-asave( i ) 0337 thedotli)=tsave<i> 033B 303 continue 0339 repeat=.true. 0340 else 0341 r e p e a t - . f a l s e . 0342 write(14,*) e r l i n INVERS 13-May-19B7 13:01i30 ll-May-1987 18i38:56 VAX FORTRAN V4 . 4-177 DUAO : C BUCHAL. OFT3 INVERS. FOR j 1G7 Page VO O 0343 0344 0345 0346 0347 0348 0349 0350 0351 0352 0353 0354 0355 0356 0357 035B 0359 0360 0361 0362 0363 0364 0365 0366 0367 0368 0369 0370 0371 0372 0373 0374 0375 0376 0377 0378 0379 0380 0381 03B2 0383 03B4 03B5 03B6 0387 03BB 0389 0390 0391 0392 0393 0394 0395 0396 0397 039B 0399 305 351 w r i t e d , * ) angles! l ) /d2rad wr i t e (2 ,* l angles(2) /d2rad write(3,*) angles(3) /d2rad w r i t e d , * ) angles(4) /d2rad endi f Ca lcu la te the displacement r e s u l t i n g from maximizing the j o i n t a v a i l a b i l i t y . c a l l gmult(mat1,angles ,xdot l ,nxdof ,ndof ,1 ,10,10,10) Ca lcu la te the weight of the j o i n t change to increase a v a i l a b i l i t y . The factor i s chosen so that the jo int change increas ing a v a i l a b i l i t y i s some f ixed f r a c t i o n of the minimum norm jo in t change reducing the e r r o r . a n o r m « 0 . do 305 i = l , 6 anorm-anorm+xdotl(1)**2 continue anorm-sqrt(anora) t l e n - 0 . do 351 i - l , 6 t len-tlen+xdot!i>**2 continue t l e n - s q r t ( t l e n ) i f ( a n o r a . n e . 0 . . a n d . t e r r . g t . t o l e r 2 ) then fact*gamma*tlen/anorm e lse f a c t - 0 . endif Update the j o i n t v a r i a b l e s . tnora-0 . t h o m o g » 0 . t l e n l * 0 . t l en2-0 . t e r r - 0 . do 41 1=1,ndof thomog-thomog+thdotl!1)**2 t lenl=t lenl+thedot( i )**2 tlen2-tlen2+fact*angles<1)**2 a s a v e l i ) - a n g l e s ( i ) t s a v e ( i ) - t h e d o t d ) d l a s t t i ) - d a n g l e ( i ) d a n g l e ( i ) - t h e d o t ( i ) - t h d o t l ( i ) - f a c t * a n g l e s ( 1 ) angles! i ) -angle B (1)+fmult*dangle(1) t e r r - t e r r + ( d a n g l e ( i ) - d l a s t ( i ) ) * * 2 tnorm=tnorm+angles(i)**2 Map the angles in to the range -180 deg. to 1B0 deg. i f ( a n g l e s ! i ) . g t . p i ) then S 3 VO INVERS 0400 0401 0402 0403 0404 0405 0406 0407 0408 0409 0410 0411 0412 0413 0414 0415 0416 0417 0418 0419 0420 0421 0422 0423 0424 0425 0426 0427 0428 0429 0430 0431 0432 0433 0434 0435 0436 0437 0438 0439 0440 0441 0442 0443 0444 0445 0446 0447 0448 0449 0450 0451 0452 0453 0454 0455 04 56 13-May-19B7 13i01:30 ll-May-1987 18:3Br56 VAX FORTRAN V4.4-177 DUAO:C BUCHAL.0PT3INVERS.FOR;16 7 Page 41 200 c 900 angles(1) -angles(1) -2 .*pi e l s e i f ( a n g l e s ! i ) . I t . - p i ) then ang le s ! i ) -ang le s ! i )+2 .*p i endif continue tnorm-sqrt(tnorm) t e r r-Bqrt! t e r r ) t l e n l = f a u l t * s q r t ( t l e n l ) t len2=fmultAsqrt(t len2) thomog=fmult*sqrt(thomog) t len3=tlenl+tlen2 endif i f ( .not .repeat) then wr i te !* ,* ) error wri te(5 ,*) error wri te(9 ,*) f a u l t w r i t e l l l , * ) t l e n l write!12,*) t len2 write!15,*) thomog write!16,*) tnorn write!13,*) fac t endif goto 100 continue Convert the angles to degrees do 900 i - l , n d o f the ta2 ( i ) -ang le s ( i ) /d2rad continue wr i te !* ,* ) 'number- '.number c l o s e d ) close!2) close!3) close!4) close!5) close!6) close I 7) close(8) c lose!9) close!10) c l o s e d l ) c l o B e l 1 2 ) close!13) return e n d INVERS 13-May-1987 13I01 I30 ll-May-19B7 1BI3B:56 VAX FORTRAN V4.4-177 Page 9 DUAO:CBUCHAL.0PT3INVERS.FORi167 PROGRAM SECTIONS Nana Bytes 0 8C0DE 2B00 1 SPDATA 144 2 $LOCAL 470B T o t a l Space Al loca ted 7652 ENTRY POINTS Address Type Nane 0-00000000 INVERS A t t r i b u t e s PIC CON REL LCL SHR EXE RD NOHRT LONG PIC CON REL LCL SHR NOEXE RD NOHRT LONG PIC CON REL LCL NOSHR NOEXE RD HRT LONG VARIABLES Address Type Nane Address Type Nana Address Type Nane Addresa Type Name AA RA4 ANG AA RA4 ANORM AA RA4 COSANG AA RA4 D2RAD AA RA4 DENOM AA RA4 ERLIN 2--O0OOOE14 RA4 ERMAX 2--00000E30 RA4 ERROR 2- 00000E40 RA4 FACT 2 -00000E38 RA4 FMULT 2--00000E18 RA4 GAMMA AA IA4 I AA IA4 ICOUNT 2 -0O000E34 IA4 IMAX AA IA4 IND 2--OO00OE2C 1*4 I SAVE AA IA4 J A A IA4 KMAX AP--000000180 IA4 NDOF A A IA4 NMAX 2- 00000E20 IA4 NUMBER AP -0000001CO IA4 NXDOF AA RA4 PI 2--00000E08 LA 4 REPEAT 2-•OOOOOEOC LA4 ROTFLG AA RA4 SINANG AA RA4 STOLER 2--0O0O0E10 LA4 STOP2 2- OO00OE28 RA4 TERM4 2 -00OO0E3C RA4 TERR 2--O0O0OE1C RA4 THOMOG A A RA4 TLEN 2- OOOO0E4B RA4 TLEN1 2 -00O00E4C RA4 TLEN2 2--00000E44 RA4 TNORM AA R*4 TOLER A A RA4 TOLER2 A A RA4 T0LER4 AA R*4 TTEMP ARRAYS Address Type Name Bytes Dimensions AP-OOOOOOOBB R*4 ALPHA AA (*) 2-00000598 R*4 ANGLES 40 (10) 2-00000570 R*4 ASAVE 40 (10) 2-000004BB R*4 DANGLE 40 (10) AP-O0000O0C8 R*4 DIST A A (*) 2-00000490 R*4 DLAST 40 (10) 2-0000092B R*4 HOMOG 400 (10, 10) 2-00000704 R*4 IDENT 64 (4, 4) AP-000000200 L*4 IXDOT 24 (6) 2-00000614 R*4 JAC 240 (6, 10) 2-0O0OOD1B R*4 JACINV 240 (10, 6) AP-00000014B 1*4 JTYPE 16 (4) 2-000005CO R*4 K 12 (3) 2-000005CC R*4 KGOAL 12 (3) AP-000000048 R*4 LEN A* (*) 2-000002D8 R*4 MAT 440 (10, 11) 2-00000AE0 R*4 MAT1 400 (10, 10) 2-000007BC R*4 ROT 12 (3) AP-00000024B R*4 ROTDES 36 (3, 3) INVERS 2-00000000 RA4 S 40 (10) 2-00000C8B RA4 TEMP 144 (6, 6) 2-000005E4 RA4 TEMPI 24 (6) 2-000005FC RA4 TEMP3 24 (6) 2-000005D8 RA4 TERM 12 (3) 2-00000AB8 RA4 THDOT1 40 (10) 2-000004EO RA4 THEDOT 40 (10) AP-000000100 RA4 THETA AA (*) AP-0000002C8 RA4 THETA2 AA (*> 2-00000768 RA4 TINV 36 (3, 3) 2-00000744 RA4 TMAT 36 (3, 3) 2-00000530 RA4 TNET 64 (4, 4) 2-0000050B RA4 TSAVE • 40 (10) 2-0000002B RA4 UD 240 (10, 6) 2-00000118 RA4 VD 400 (10, 10) 2-0000079B RA4 VDT 400 (10, 10) 2-000002A8 RA4 XDOT 24 (6) 2-OO0OOC70 RA4 XDOT1 24 (6) 2-000002C0 RA4 XD0T2 24 (6) AP-00000O28O RA4 XGOAL AA (A) 13-May-19B7 13i01i30 VAX FORTRAN V4.4-177 ll-May-1987 18i3B!56 DUAO:CBUCHAL.OPT3INVERS.FOR)167 Page 10 LABELS Addreaa Label Addreas Label Address Label Address Label AddresB Label Address Label AA 1 AA 2 AA 5 AA 20 AA 21 AA 39 AA 41 0-00000178 100 AA 110 AA 111 AA 120 AA 130 AA 131 AA 132 AA 140 0-OO00OA2C 200 AA 222 AA 223 AA 251 AA 300 AA 302 AA 303 AA 305 AA 310 AA 333 AA 351 AA 800 AA 801 AA 900 FUNCTIONS AND SUBROUTINES REFERENCED Type Name Type Nane Type Nane Type Nana Type Name Type Name F0R$CL0SE RA4 MTH$SICN FOR$OPEN RA4 MTH$SIN CMULT RA4 MTH$SQRT JACOB SVD RA4 MTHSATAN2 RA4 MTH$C0S COMMAND QUALIFIERS FOR /LIST OPT. FOR , RDFILE. FOR, INPUT. FOR ,GR0H. FOR, FINPAT. FOR, S0LVE1. FOR , INT2D. FOR , HULL 2D. FOR ,GMULT. FOR, JACOB. FOR, COSTT. FOR, / CHECK * (NOBOUNDS , OVERFLOW, NOUNDERFLOH) /DEBUG-(NOSYMBOLS,TRACEBACK) /STANDARD-(NOSYNTAX,NOSOURCE_FORM) / SHOW- (NOPREPR0CESS0R , NOINCLUDE, MAP, NODICTIONARY, S INGLE) /WARNINGS - ( GENERAL,NODECLARATIONS) /CONTINUATIONS-19 /N0CROSS_REFERENCE /N0D_LINES / NOEXTEND_SOURCE /F77 / NOG_FLO AT I NG /I4 /NOMACHINE_C0DE /OPTIMIZE 13-May-1987 12l55l59 VAX FORTRAN V4.4-177 Page 1 ll-May-1987 13i2Bi56 DUAOlCBUCHAL.OPT]JACOB.FOR|13 0001 aubroutlne Jacob( tpre , tpoat , l a n , a l p h a , d i s t , d t h e t a , J t y p e , n r d o f , t n e t , j a c ) 0002 0003 c Calculate the Jacobian a a t r l x for a given manipulator c o n f i g u r a t i o n . 0004 0005 c Parametersi 0006 c t p r e - world to robot base coordinate transformation 0007 c tpost - transformation from l a s t l i n k frame to a point 0008 c of in t eres t attached to the l a s t l i n k 0009 c l e n , a l p h a , d i s t - Denavit-Hartenberg l i n k parameters 0010 c dtheta- jo in t values 0011 c j type- array spec i fy ing j o i n t types 0012 c 0- revo lute , 1- pr i smat ic 0013 c nrdof- number of l i n k s 0014 c tnet - the net transformation matrix r e l a t i n g the frame of 0015 c in teres t on the l a s t l i n k to the world frame 0016 c jac - the Jacobian matrix 0017 0018 0019 r e a l tpre !4 ,4 ) , tpos t (4 ,4 ) 0020 r e a l amat(4,4) 0021 r e a l lent*),alpha!.*) , d i s t t * ) .dtheta!*) 0022 r e a l jac!6,*> 0023 integer j type!*) 0024 0025 r e a l tnet!4 , 4 ) , T ( 4 , 4 ) , p ( 3 , 1 0 ) , r d i s t ( 3 ) 0026 r e a l tempt4,4) 0027 0028 c Load transformation r e l a t i n g robot base coordinates to world 0029 c coordinates . 0030 0031 do 200 i - l , 4 0032 do 201 j - 1 , 4 0033 Tt j , i ) - t p r e ( j . i ) 0034 201 continue 0035 200 continue 0036 0037 d2rad-.0174533 0038 0039 c Recursively c a l c u l a t e elements of Jacobian matrix. 0040 0041 do 10 ind-1 ,nrdof 0042 0043 the ta -d the ta 11 nd) *d 2 rad 0044 0045 amat!1,1)-cos!theta) 0046 amat!2,1)-sin!theta) 0047 amat(1 ,2 ) - - s in ! the ta)Acos (a lpha 1ind)) 0048 amat(2 , 2 )-coat t h e t a )ACOB(a l p h a ! i n d ) ) 0049 amat !3 ,2 ) -a in la lphat ind) ) 0050 amat(1,3)-s int the ta)*sin (a lpha t ind)) 0051 amat(2,3)- -cos(theta)A s in(alpha(ind)1 0052 •mat!3 , 3 ) - C O B(alpha!ind)) 0053 amat (1 ,4 )-lentind)Acos ( theta) 0054 amat (2 ,4 )-lentind)Asin( the ta ) 0055 amat (3 ,4 ) -d i s t(ind) 0056 amat (4 ,4)-l. 0057 JACOB 13-Hay-1987 12l55l59 ll-May-19B7 13.2Bi56 VAX FORTRAN V4. 4-177 DUAO : C BUCHAL. 0PT3JACOB. FOR; 13 Page K5 003B c a l l gmult(T,amat,Temp,4,4 ,4 ,4 ,4 ,4) 0059 0060 do 300 i ' 1 , 4 0061 do 301 j - 1 , 4 0062 t< j , i ) - t emp<j , i ) 0063 301 continue 0064 300 continue 0065 0066 l f ( j type( ind) .eq .0) then 0067 0068 c Revolute j o i n t . 0069 0070 do 20 i n d 2 - l , 3 0071 jac< ind2, ind1-T<ind2,3) 0072 P ( i n d 2 , i n d ) « T ( i n d 2 , 4 ) 0073 20 continue 0074 0075 elaa 0076 0077 c P r l a a a t i c j o i n t . 0078 0079 do 30 i n d 2 - l , 3 O0B0 jac( ind2, ind>-0 . 00B1 jac(ind2>3,ind)-T<ind2,3) 00B2 30 continue O0B3 O0B4 andlf 0085 00B6 10 continue 0087 00B8 c Poatmult lply by f ixed transformation r e l a 1 0089 c t o o l frame. 0090 0091 0092 0093 c The net transformation ' tnet ' re la tea the 0094 c reference frame. In other words, ' tnet ' i 0095 c and o r i e n t a t i o n . 0096 0097 c Calcu la te Jacobian elements which depend i 0098 c frame. 0099 0100 do 40 i n d « l , n r d o f 0101 0102 i f ( j type( ind) . eq .0 ) then 0103 0104 i f ( i n d . e q . l ) then 0105 do 50 1-1,3 0106 r d i a t ( i ) - - t n e t ( i , 4 ) 0107 50 continue 0108 else 0109 do 55 1-1,3 0110 r d i s t t i ) - P ( l , i n d - l ) - t n e t ( i , 4 ) 0111 55 continue 0112 endif 0113 0114 jac (4 , ind) — jac (2 , ind)*rd ia t (3 ) + jac JACOB 13-May-1987 12i55139 VAX FORTRAN V4.4-177 Page 3 ll-May-19B7 13i28i36 __DUAO ) CBUCHAL. 0PT3 JACOB. FOR 113 O H 5 jac(3,ind)« ja c ( 1 , i n d ) A r d i s t ( 3 ) - j a c ( 3 , i n d ) * r d i a t ( 1 ) 0 H 6 jac (6, ind) — jac( 1 ,ind)Ardist<2)+jac<2,ind)Ardist<1) 0117 0118 andlf 0119 0120 40 continue 0121 0122 return 0123 end PROGRAM SECTIONS Naae 0 (CODE 1 (PDATA 2 (LOCAL T o t a l Space Al loca ted Bytea A t t r i b u t e s 632 4 392 124B PIC CON REL LCL SHR EXE PIC CON REL LCL SHR NOEXE PIC CON REL LCL NOSHR NOEXE RD NOHRT LONG RD NOHRT LONG RD HRT LONG ro vo ENTRY POINTS Address Type Nane 0-00000000 JACOB VARIABLES Addreaa AA AA Type Naae RA4 D2RAD IA4 J Addreaa Type Naaa A A IA4 AP-00000020B IA4 I NRDOF Address Type Naae AA AA 1*4 IND RA4 THETA Address Type Naae A A IA4 IND2 ARRAYS Address Type Naae Bytes Dimensions AP-ooooooioa RA4 ALPHA AA (A) 2-00000000 R*4 AMAT 64 (*, 4) AP-000000148 RA4 DIST AA <A) AP-0000001Ba R*4 DTHETA AA (A) AP-000000288 RA4 JAC AA (6, A) AP-0000001CB 1*4 JTYPE AA (A) AP-oooooooca RA4 LEN AA (A) 2-00000080 R*4 P 120 (3, 10) 2-000000FB RA4 RDIST 12 (3) 2-00000040 R*4 T 64 M , 4) 2-00000104 RA4 TEMP 64 (4, 4) AP-00000024a R*4 TNET 64 (4, 4) A P - 0 0 0 0 0 0 0 B 3 RA4 TPOST 64 <4, 4) AP-000000040 R*4 TPRE 64 (4, 4) JACOB 13-May-19B7 12155159 VAX FORTRAN V4.4-177 Page 4 ll-May-1987 13i2Bi56 DUAOsCBUCHAL.0PT3JACOB.FOR»13 LABELS Addreaa AA AA Label 10 200 Address Label AA AA 20 201 Address AA AA Label 30 300 Address Label AA AA 40 301 Address Labe l AA go Addreas Labe l AA 55 FUNCTIONS AND SUBROUTINES REFERENCED Type Nane Type Nana GMULT RA4 MTH'COS Type Naaa R*4 MTH«SIN COMMAND QUALIFIERS FOR/LIST OPT. FOR, RDFILE. FOR, INPUT. FOR,GROW. FOR, FINPAT. FOR, SOLVE1. FOR fINT2D.FOR,HUXi2D.FOR .GMULT.FOR, JACOB.FOR,COSTT.FOR, /CHECK" (ROBOUNDS ,OVERFLOW,NOUNDERFLON) /DEBUG" (NO SYMBOLS .TRACEBACK) K> /STANDARD" (NOSYNTAX.NOSOURCE FORM) 5) / SHOW" (NOPREPROCESSOR, NOINCLUDE, MAP, NODICTIONARY .SINGLE) /WARNINGS" (GENERAL,NODECLARATIONS) /C0NTINUATIONS = 19 /NOCROSS_REFERENCE /NODLINES /NOEXTEND_SOURCE /F77 /NOG FLOATING /I4 /N0MACHINE_C0DE /OPTIMIZE COMPILATION STATISTICS Run Tlaat Elapsed T l a e i Page F a u l t s i Dynamic Memoryi 5.00 seconds 14.14 seconds 6B7 845 pages 13-May-19B7 13i00i40 l l-Hay-19B7 15>43>46 00 0001 0002 0003 0004 0005 0006 0007 000B 0009 0010 0011 0012 0013 0014 0015 0016 0017 001B 0019 0020 0021 0022 0023 0024 0025 0026 0027 0028 0029 0030 0031 0032 0033 0034 0035 0036 0037 0038 0039 0040 0041 0042 0043 0044 0045 0046 0047 0048 0049 0050 0051 0052 0053 0054 0055 0056 0057 program minver c Perform I t e r a t i v e Inverse kinematic ao lut ion and show the r e s u l t s c g r a p h i c a l l y . r e a l theta(4,10) r e a l boxl<2 ,10,10),box2(2,10,10) integer links',4) l o g i c a l i erase , s top r e a l xdot(3) ,xgoal (3) ,xcurr(31 r e a l a l p h a ) 4 ) , l e n l 4 ) , d i s t ( 4 ) Integer j t y p e t 4 ) , l i n k f 4 ) l o g i c a l lxdot(6) r e a l kgoal(3) r e a l thetal (4) r e a l theta2(4> r e a l rotdes(3,3) r e a l xpos(3) l o g i c a l s a v e , f i r a t , f i n a l data k g o a l / 0 . , 0 . , 1 / common / r o b o t I I i n k , j t y p e , a l p h a , l e n , t h e t a , d i s t common /nrdof /ndof ,nxdof c a l l r d f i l e l l a s t ' l wr i t e '* ,* ) 'Enter the number of task coordinates (2 or 3)' read(A,A> nxdof l f (nxdof.eq.3) i x d o t ( 3 ) » . t r u e . ixdot(4)=. true . i x d o t ( 5 ) - . t r u e . d2rad>.0174533 c Read the geometry of the l i n k s and obstacles from a f i l e . o p e n ( 1 , f i l e * ' b o x . d a t ' , s t a t u s * ' o l d ' ) r e a d d ,A) n l inks do 29 k » l , n l i n k s r e a d d ,A) n v e r t , l i n k s ( k ) do 13 1*1,nvert r e a d l l ,A) ( b o x l ( j , 1 , k ) , j * l , 2 ) 13 continue 29 continue c l o s e d ) npos*l write(A,A) 'Enter i n i t i a l conf igurat ion angles' read<*,*> ( theta( i ,npos) ,i"=l ,ndof ) do 25 1*1,ndof thetal( i )=theta(1,npos) VAX FORTRAN V4.4-177 Page 1 DUAO: C BUCHAL. 0PT3MINVER. FOR»8 MINVER 13-May-19B7 13i00l40 ll-May-19B7 15i43i46 vO VO 005B 0059 0060 0061 0062 0063 0064 0065 0066 0067 0068 0069 0070 0071 0072 0073 0074 0075 0076 0077 007B 0079 0080 0081 0082 0083 O0B4 0085 00B6 0087 0088 00B9 0090 0091 0092 0093 0094 0095 0096 0097 0098 0099 0100 0101 0102 0103 0104 0105 0106 0107 0108 0109 0110 0111 0112 0113 0114 25 c 100 23 continue I n i t i a l i z e the graphics environment. c a l l g r s t r t ( 4 0 2 7 , l ) c a l l newpag c a l l w i n d o w ! - 4 . , 4 . , - 4 . , 4 . ) c a l l vwporMO. ,100. ,0 . ,100. > f i r s t * . t r u e , i e r a s e ' . t r u e , f i n a l * . t r u e . continue i f ( . n o t . f i r s t ) then s t o p ' . t r u e , c a l l caclos wr i te !* ,* ) 'Enter goal p o s i t i o n and o r i e n t a t i o n ' read!*,*) xpos!1) ,xpos(2) ,angle angle-angle*d2rad F ind an optimal inverse so lu t ion for the des i red p o s i t i o n . c a l l ro taat !kgoa l ,ang le , ro tdes ) c a l l i n v e r s ! l e n , a l p h a , d i s t , t h e t a 1 , j t y p e , n d o f , n x d o f , i x d o t , r o t d e s , xpos,theta2) wr i t e !* ,* ) ' X , Y pos, angle ' , ! x p o s ( 1 ) , l ' l , 2 ) . a n g l e do 23 i - l , n d o f t h e t a l ( i ) - t h e t a 2 ( l ) continue c a l l cmopen endlf c a l l t r i d n t ! . f a l s e . ) Draw the l i n k positions. do 15 ipos'l,npos c a l l t r i d n t t . f a l s e . ) i l i n k = l do 10 ind'l,ndof angle'thetal(ind) c a l l rotate(angle.angle) c a l l transl(len!ind), 0 . ) c a l l move(0. ,0 . ) c a l l daBhpt(l) c a l l draw!-len!ind) , 0 . ) VAX FORTRAN V4.4-177 DUAO : CBUCHAL. 0PT3MINVER. FOR 18 MINVER 0115 c a l l dashpt(O) one 0117 l f ( ind . eq.ndof) then 0118 c a l l move(0. ,0.) 0119 c a l l d r a w ) . 2 , . 2 ) 0120 c a l l move(0. ,0.) 0121 c a l l draw<.2,- .2) 0122 and i f 0123 0124 c i f ( ind . e q . l l n k s ( i l i n k ) ) then 0125 c c a l l m o v e ( b o x l ( l , l , i l i n k ) , b o x l ( 2 ,1 , i l i n k ) ) 0126 c c a l l d r a w l b o x l l 1 , 2 , i l i n k ) , b o x l I 2 ,2 , l l l n k ) ) 0127 c c a l l d r a w ( b o x l ( 1 , 3 , i l i n k ) .boxl12 ,3 . i l i n k ) ) 0128 c c a l l d r a w ( b o x l ( l , 4 , i l i n k ) ,boxl<2 ,* . i l i n k ) ) 0129 c c a l l d r a w l b o x l ( 1 , 1 , i l i n k ) , b o x l ( 2 ,1 . i l i n k ) ) 0130 c i l l n k = i l i n k + l 0131 C" endif 0132 0133 10 continue 0134 15 continue 0135 0136 f i r s t 3 . f a l s e . 0137 0138 i f ( .not . s top) goto 100 0139 0140 101 continue 0141 c a l l g r B t o p -0142 0143 atop 0144 end 13-May-19B7 13i00i40 ll-May-1987 15i43:46 VAX FORTRAN V4.4-177 Page 3 DUAO!C BUCHAL.0PT3MINVER.FOR»8 MINVER 13-May-19B7 13:00:40 ll-May-19B7 15:43:46 VAX FORTRAN V4. 4-177 DUAO: C BUCHAL. OPT3MINVER. FOR»B P a g e 4 PROGRAM SECTIONS Naae Bytes 0 (CODE 811 1 fiPDATA 177 2 (LOCAL 2096 3 ROBOT 240 4 NRDOF B T o t a l Space Al loca ted 3332 At tr ibute s PIC CON REL LCL SHR EXE RD NOHRT LONG PIC CON REL LCL SHR NOEXE RD NOHRT LONG PIC CON REL LCL NOSHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG ENTRY POINTS Address 0-00000000 Type Nane MINVER VARIABLES OJ O Addreaa Type Nane Address Type Nane Address Type Nane Address Type Nane 2-O00006F4 RA4 ANGLE AA RA4 D2RAD AA L*4 FINAL AA L*4 FIRST AA IA4 I A* LA4 I ERASE AA IA4 I LA ST A A IA4 ILINK AA IA4 IND AA 1*4 IPOS AA 1*4 J A A 1*4 K 4-00000000 1*4 NDOF 2-000006EC 1*4 NLINKS AA IA4 NPOS 2-000006F0 1*4 NVERT 4-00000004 IA4 NXDOF AA L*4 SAVE 2-000006EB L*4 STOP ARRAYS Address Type Nane Bytes Dimensions 3-00000020 R*4 ALPHA 16 (4) 2-00000000 R*4 B0X1 800 <2, 10, 10) 2-00000320 RA4 B0X2 BOO <2, 10, 10) 3-OO000OE0 R*4 DIST 16 (4) 2-00000674 LA4 IXDOT 24 (6) 3-00000010 1*4 JTYPE 16 (4) 2-000006BC R*4 KGOAL 12 (3) 3-00000030 R*4 LEN 16 (4) 3-00000000 1*4 LINK 16 (4) 2-00000640 1*4 LINKS 16 (4) 2-000006BB R*4 ROTDES 36 '3 , 3) 3-00000040 R*4 THETA 160 <4, 10) 2-0000069B R*4 THETA1 16 <4> 2-000006AB R*4 THETA 2 16 14) 2-0000066B R*4 XCURR 12 (3) 2-00000650 R*4 XDOT 12 (3) 2-0000065C R*4 XGOAL 12 (3) 2-000006DC R*4 XPOS 12 (3) MINVER LABELS AddreBB Label AddreBB Label A A 10 AA 13 0-00000190 100 A A 101 FUNCTIONS AND SUBROUTINES REFERENCED Type Nane Type Nane CMCLOB CHOPEN GRSTOP GRSTRT ROTATE ROTMAT 13-Hay-19B7 13I00I40 VAX FORTRAN V4.4-177 Page ll-May-19B7 15t43:46 DUAO:CBUCHAL.OPT3MINVER.FOR)B Address Label AddreBB Label Address Labe l Address Label A A IB A A 23 A A 25 A A 29 Type Nana Type Nana Type Name Type Name DASHPT DRAM FOR$CLOSE FORfJOFEN INVERS HOVE NEWPAG RDFILE TRANSL TRIDNT VWPORT WINDOW 13-Mey-19B7 12)S2i26 VAX F0RTRA1J V4.4-177 ll-May-1987 17i21>52 DUAO1CBUCHAL.OPT30PT.FOR;1 Page 1 0001 program opt 0002 0003 c T h i s program f inds an optimal robot t r a j e c t o r y by minimizing a 0004 c cost f u n c t i o n a l . Interference and jo in t l i m i t constra ints are OOOS c represented by penalty funct ions . 0006 0007 c The current implementation appl ies to 2-D planar manipulators 0008 c where objects are represented by convex polygons. The convex 0009 c h u l l enclosing two adjacent l i n k pos i t ions i s used for the 0010 c interference c a l c u l a t i o n s . 0011 0012 c The i n i t i a l conf igurat ion i s g i v e n , and the f i n a l conf igurat ion 0013 c (the goal pos i t ion) must s a t i s f y the f i n a l p o s i t i o n c o n s t r a i n t . 0014 c 0013 c The cost func t iona l l a the i n t e g r a l of the j o i n t v e l o c i t i e s and 0016 c acce lerat ions over the path, plus a penalty funct ion for v i o l a t i n g 0017 c j o i n t and penetration l i m i t s . 0018 0019 0020 r e a l len(4),dist<4),alpha<4>,theta(4) 0021 integer j type(4>, l ink!4) 0022 r e a l xgoal<3) 0023 r e a l values(4,100) 0024 integer boxes(2,10) 0023 r e a l averts(2 ,100) , tverts(2 ,100) 0026 l o g i c a l i n t r a j 0027 integer l indof(2 ,20) 002B r e a l naxpen, j to ler 0029 r e a l kgoal(3) 0030 r e a l rotdes(3,3) 0031 l o g i c a l ixdot(6) 0032 0033 common / r o b o t / l i n k , j t y p e , a l p h a , l e n , t h e t a , d i s t 0034 common /nmax/nmax 0035 common /goal /xgoal ,angoal 0036 common / l a s t / l a s t 0037 common / o b j s / b o x e s , s v e r t s , t v e r t s 0038 common /nobj s /n l inks ,nobs t 0039 common / l i n d o f / l i n d o f 0040 common / l lmto l /maxpen , j to l er 0041 common /nrdof /ndof ,nxdof 0042 0043 0044 0045 c S t a r t the elapsed CPU timer. 0046 004 7 b a s e _ t l m e « g e t _ c p u _ t i m e ( 0 . ) 0048 0049 0050 c For the current Implementation, 2-0 r o t a t i o n about x-axls on ly . 0051 0052 k g o a l ( l > « 0 . 0053 k g o a l ( 2 ) « 0 . 0054 k g o a l ( 3 ) - l . 0055 0056 c Degrees of freedom are r o t a t i o n about x, t r a n s l a t i o n along x , y . 0057 13-May-1987 12i52i26 ll-May-1987 17i21i52 VAX FORTRAN V4.4-177 DUAO i C BUCHAL. OPT30PT. FOR »1 Page 2 0058 0059 00S0 0061 0062 C 0063 0064 0065 0066 c 0067 0068 0069 0070 e 0071 0072 0073 0074 0075 0076 0077 100 0078 0079 0080 c 00B1 OJ 0082 9 0083 00B4 00B5 0086 00B7 O0B8 0089 0090 0091 0092 0093 0094 0095 6 0096 0097 5 0098 0099 0100 0101 0102 0103 0104 0105 0106 B 0107 0108 7 0109 0110 0111 0112 c 0113 0114 i x d o t ( 3 ) - . t r u e . i x d o t ( 4 ) - . t r u e , i x d o t ( 5 ) - . t r u e . Read kinematic parameters f i l e , c a l l r d f l l e User se lects numerical opt imisat ion parameters, c a l l input Read the goal p o s i t i o n and i n i t i a l t r a j e c t o r y from a f i l e . o p e n ( 1 , f l i e " ' t r a j . d a t ' . s t a t u s - ' o l d ' ) r e a d d ,A) (xgoal(1) ,1 -1 ,3 ) ,angoal r e a d d ,A) l a s t do 100 i - l , l a s t r e a d d , * ) (values( j , 1 ) , j -1 ,ndof) continue c l o s e d ) Read the 2-D obstacle geometry data from a f i l e . o p e n d . f i l e " ' box.dat' , s t a t u s - ' o l d ' ) readd.A) n l i n k s l s t a r t - 1 do 5 i - l , n l i n k s readd.A) n v e r t s , i d o f l f ( l i n d o f ( 1 , i d o f ) . e q . 0 ) than l i n d o f I I , i d o f ) - i endif 1 indof12, idof)•1 indof(2 , idof)+1 b o x e s ( l , l ) - l s t a r t boxes(2,1)-nverts do 6 j - i s t a r t , i s t a r t + n v e r t s - l readd.A) ( sverts (k, j ) , k-1,2) continue i s t a r t - i s t a r t + n v e r t s • continue r e a d d ,A) nobet do 7 i - l , n o b e t readd.A) nverts b o x e s ( 1 , n l i n k s + i ) - l s t a r t boxes(2 ,nl inks+l)-nverts do B j - i s t a r t , i a t a r t + n v e r t s - 1 r e a d d . A) ( sverts (k , j) , k - l , 2 ) continue l s t a r t - i s t a r t + n v a r t a continue c l o s e d ) Grow the obstacles by the penetrat ion threshold . c a l l growlnaxpen) OPT 0115 one 0117 011B 0119 0120 0121 0122 0123 0124 0125 0126 0127 13-May-19B7 12I32I26 ll-May-19B7 17I21I52 VAX FORTRAN V4.4-177 _DUAO IC BUCHAL. 0PT30PT. FOR 11 Page Cenerate a path from the given i n i t i a l t r a j e c t o r y . c a l l f inpattvalues) Get the t o t a l elapsed CPU t l a e . et lae*get_cpu_tiae(base_tlae) wr i t e !* ,* ) 'Elapsed CPU t i a e - ' , e t i a e •top end PR0CRAH SECTIONS Naae Bytes 0 (CODE 644 1 (PDATA 39 £ 2 (LOCAL 1B44 Oi 3 ROBOT 96 4 UMAX 4 5 GOAL 16 6 LAST 4 7 OBJS 16B0 B NOBJS 8 9 LINDOF 160 10 LIMTOL 8 11 NRDOF B T o t a l Space Al loca ted 4511 ENTRY POINTS Address Type Naae 0-00000000 OPT A t t r i b u t e s PIC CON REL LCL SHR EXE RD NOHRT LONG PIC CON REL LCL SHR NOEXE RD NOHRT LONG PIC CON REL LCL NOSHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RO HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RO HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG VARIABLES Address Type Naae Address Type Naae Address Type Naae Address Type Naaa 5 -OOOOOOOC R*4 ANGOAL 2--000006B8 R*4 BASEjriKE AA RA4 ETIME AA IA4 I 2 -00000690 1*4 IDOF A* L*4 INTRAJ AA IA4 1START AA IA4 J 10 -00000004 R*4 JTOLER AA 1*4 K 6 -00000000 IA4 LAST 10-00000000 R*4 MAXPEN 11 -00000000 1*4 NDOF B--00000000 1*4 NLINKS 4 -00000000 IA4 NHAX B-00000004 1*4 NOBST 2 -0000068C 1*4 NVERTS 11--00000004 1*4 NXDOF OPT 13-May-19B7 12l52i26 ll-May-19B7 17i21i52 VAX FORTRAN V4.4-177 DUAO!C BUCHAL.OPT30FT.FOR»1 Page 4 ARRAYS Addreaa Type Name Bytea Dimensions 3-00000020 7-00000000 3-00000050 2- 00000670 3- 00000010 2- 00000640 3- 00000030 9-00000000 3-00000000 2- 0000064C 7-00000050 3- 00000040 7-00000370 2-00000000 5-00000000 R*4 1*4 R*4 L*4 1*4 R*4 R*4 1*4 1*4 R*4 R*4 R*4 R*4 R*4 R*4 ALPHA BOXES DIST IXDOT JTYPE KGOAL LEN LINDOF LINK ROTDES SVERTS THETA TVERTS VALUES XGOAL 16 80 16 24 16 12 16 160 16 36 800 16 BOO 1600 12 (4) (2, 10) (4) (6) (4) (3) (4) (2, 20) (4) (3, 3) (2, 100) (4) (2, 100) (4, 100) (3) co O LABELS Address Label Address Label ** 5 ** 6 Addreaa Label ** 7 Address Label * * B Address Label ** 100 FUNCTIONS AND SUBROUTINES REFERENCED Type Name Type Name FINPAT FORECLOSE RDFILE Type Name FOR8OPEN Type Name R*4, GET_CPU_TIKE Type Name GROW Type Name INPUT 0001 0002 0003 0004 0009 0006 0007 000B 0009 0010 0011 0012 0013 0014 0013 0016 0017 13-May-19B7 12|52|26 ll-May-1987 17i2l!52 VAX FORTRAN V4.4-177 DUAO i C BUCHAL. OPT30PT. FOR) 1 Page block data r e a l Ident(4,4) integer l lndof (2 ,10) r e a l d2rad coaaon / i d e n t / i d e n t coaaon / l i n d o f / l l n d o f common /d2rad/d2rad data i d e n t / l . , 4 A O . , 1 . , 4 A 0 . , 1 . , 4 A 0 . , 1 . / data l indof /20A0/ data d2rad/.0174533/ end c o O PROGRAM SECTIONS Name 0 IDENT 1 LINDOF 2 D2RAD T o t a l Space Al located Bytea A t t r i b u t e ! 64 PIC OVR REL GBL 80 PIC OVR REL GBL 4 PIC OVR REL GBL 148 SHR NOEXE SHR NOEXE SHR NOEXE RD HRT LONG RD HRT LONG RD HRT LONG VARIABLES Addreia Type Name 2-00000000 RA4 D2RAD ARRAYS Addreae Type Name Bytea Dlmenalons 0- 00000000 RA4 IDENT 1- 00000000 IA4 LINDOF 64 (4, 4) B0 (2, 10) 13-May-1987 I2i52i41 7-May-19B7 1 7 i l l i 3 2 VAX FORTRAN V4.4-177 DUAO i C BUCHAL. 0PT3RDFILE. FOR 14 Page 1 0001 0002 0003 c 0004 c 0005 oooe 0007 O008 0009 0010 0011 0012 0013 c 0014 O015 0016 0017 c 0018 0019 0020 0021 0022 c 0023 v>3 0024 O 0025 CO 0026 0027 c O02B 0029 0030 0031 0032 C 0033 0034 0035 0036 0037 c 003B 0039 0040 0041 0042 c 0043 0044 0045 0046 0047 004B 004 9 0050 subroutine r d f i l e Read the contenta of the setup f i l e s and i n i t i a l i z e the v a r i a b l e s . r e a l alpha(4>,len(4>,dist(4) , theta(4> integer j type(4) ,1 ink(4) connon / r o b o t / l i n k , j t y p e , a l p h a , l e n , t h e t a , d i e t connon /nrdof /ndof ,rutdof common / s i z e / s i z e Read the contents of the k inenat ics paraaeters f i l e . o p e n ( 1 , f i l a - ' p a r a n l . d a t ' . s t a t u s - ' o l d ' ) Read tha number of j o i n t and task coordinates . r e a d d , * ) r e a d d , * ) ndof,nxdof Read tha workstation a l sa ( u n i t s ) . r e a d d , * ) read(1,*) s i z e Read the robot j o i n t type array , read(1,A) r e a d d , * ) ( j t y p e d ) , i - l , n d o f ) Read the l i n k twi s t s , r e a d d , * ) r e a d d ,A) ( a l p h a d ) ,1*1,ndof) Read the l i n k lengths. r e a d d.A) r e a d d.A) ( l e n d ) ,1'1 .ndof > Read the l i n k j o i n t o f f s e t s . r e a d d.A) r e a d d.A) (d i s t ( i ) , i - l , n d o f ) c l o s e d ) return end RDFILE PROGRAM SECTIONS 13-May-19B7 12i92i41 7-May-1987 1 7 i l l i 3 2 VAX FORTRAN V4.4-177 DUAO : C BUCHAL. OPT3RDFILE. FOR 14 Page Nana Bytea A t t r i b u t e s 0 (CODE 361 PIC CON REL LCL SHR EXE RD NOHRT LONG 1 (PDATA 11 PIC CON REL LCL SHR NOEXE RD NOHRT LONG 2 (LOCAL 72 PIC CON REL LCL NOSHR NOEXE RD HRT LONG 3 ROBOT 96 PIC OVR REL GBL SHR NOEXE RD WRT LONG 4 NRDOF 8 PIC OVR REL GBL SHR NOEXE RD HRT LONG 5 SIZE 4 PIC OVR REL GBL SHR NOEXE RD HRT LONG T o t a l Space A l l o c a t e d 952 ENTRY POINTS Address Type Nana 0-00000000 RDFILE OJ O vo VARIABLES Address Type Nana * * 1*4 I Address Type Naaa 4-00000000 1*4 NDOF Address Type Nana 4-00000004 1*4 NXDOF Address Type Nana 9-00000000 R*4 SIZE ARRAYS Address Type Nana Bytes 3-00000020 R*4 ALPHA 16 3-00000050 R*4 DIST 16 3-00000010 1*4 JTYPE 16 3-00000030 R*4 LEN 16 3-00000000 1*4 LINK 16 3-00000040 R*4 THETA 16 FUNCTIONS AND SUBROUTINES REFERENCED Type Nane FORECLOSE Type Naae FOR?OPEN 13-May-19B7 13102:24 VAX FORTRAN V4.4-177 Page 1 10-Nov-19B6 15:30:44 DUAO :tBUCHAL. 0PT3ROTMAT.FOR; 1 0001 subroutine rotnat ( k a x l s,theta,rotat) 0002 0003 c Transforn a generalized rotation axis and angle into the equivalent 0004 c rotation matrix. 0005 0006 c See Paul p.30 for equations. 0007 OOOB r e a l kaxis( 3 ),theta,rotat ( 3 , 3 ) 0009 0010 tempi - 1 .-cos(theta) 0011 temp2-cos(theta) 0012 temp3=sin(theta) 0013 0014 rotat ( 1 , 1 )«kaxis ( 1 )Akaxi s(1)*templ+temp2 0015 rotat ( 2 , 1 )=kaxi s (1 )Akaxis (2 )Atempl+kaxi s(3)*temp3 0016 rotat ( 3 rl ) = k a x i B(l)AkaxiB ( 3)*templ-kaxis ( 2 ) A t e m p 3 0017 rotat(l , 2>=kaxis ( 2)Akaxis(l)Atempl-kaxis ( 3 > A t e m p 3 0018 rotat ( 2 , 2 > =kaxis< 2)Akaxis(2)Atempl+temp2 0019 r o t a t ( 3 , 2 ) = k a x i s (2 )Akaxis (3 )Atempl+kaxi s(1)Atemp3 0020 rotat(l , 3)=kaxis ( 3)Akaxis(l ) A t e m p l+kaxia ( 2 ) A t e m p 3 0021 rotat (2 ,3)=kaxis<3)*kaxis (2)Atempl-kaxis<l)Atemp3 0022 ro tat (3 ,3 )= kax i s ( 3 ) A kax i s < 3)A temp1+1 emp2 0023 0024 return 0025 end PROGRAM SECTIONS Name 0 (CODE 2 (LOCAL T o t a l Space Al loca ted Bytes A t t r i b u t e s 210 40 250 PIC CON REL LCL SHR EXE PIC CON REL LCL N0SHR NOEXE RD NOHRT LONG RD WRT LONG ENTRY POINTS Address Type Name 0-00000000 ROTMAT VARIABLES Address Type Name A A RA4 TEMPI Address Type Name A A RA4 TEMP 2 Address Type Name A A RA4 TEMP3 AddreBS Type Name AP-0000000BO R*4 THETA ROTMAT 13-May-19B7 13102124 VAX FORTRAN V4.4-177 Page 2 lO-Nov-1986 15:30:44 DUAO:CBUCHAL.OPT3ROTMAT.FOR:1 ARRAYS Address Type Name Bytes Dimensions AP-00000004O R*4 KAXIS 12 (3) AP-0000000CB R*4 ROTAT 36 (3, 3) FUNCTIONS AND SUBROUTINES REFERENCED Type Name Type Name R*4 MTH6C0S R*4 MTHSSIN COMMAND QUALIFIERS FOR / LI ST OPT. FOR , RDFILE. FOR, INPUT. FOR, GROW. FOR , FINPAT .FOR, S0LVE1. FOR, INT2D. FOR, HULL2D. FOR , GMULT. FOR, JACOB. FOR. COSTT. FOR, / CHECK = (NOBOUNDS, OVERFLOW, NOUNDERFLOW) / DEBUG = (NOSYMBOLS, TRACEBACK) / STANDARD" (NOSYNTAX, NOSOURCE_FORM) /SHOW" (NOPREPROCESSOR , NOINCLUDE,MAP,NODICTIONARY,SINGLE) /WARNINGS"(GENERAL,NODECLARATIONS) /CONTINUATIONS" 19 /NOCROSS_REFERENCE /NOD_LINES /NOEXTENDSOURCE /F77 /NOG_FLOATING /I4 / NOMACHI NE_CODE /OPTIMIZE COMPILATION STATISTICS Run Time: 2.42 seconds Elapsed Time: 6.12 s e c o n d B Page F a u l t s : 525 Dynamic Memory: B45 pages 13-Key-19B7 12i34i53 7-Nov-19B6 15i20t35 0001 subroutine solvel(mat,mdlaa r ndia,B ,n,homog) 0002 0003 c Find the pro jec t ion onto the nullapace of a matrix using the 0004 c s ingular value decomposition. 0003 0006 c Parametersi 0007 OOOB c • a t - the (si x n) matrix 0009 c mdima- the f i r s t dimension of array 'mat' 0010 c ndim- the dimension of 'homog' 0011 c a , n - the dimensions of the matrix 0012 c hoaog- the pro jec t ion onto the nul lapace of 'mat' 0013 0014 r e a l aat ladiaa,*) ,s (10) ,vd<10,10) O013 r e a l vdt(10,10) 0016 r e a l hoaog(ndia,ndia) 0017 001B aln<>ain0(a,n) 0019 0020 c Find the s ingular value decoaposlt ion of the matrix. 0021 0022 c a l l Bvd ( a a t , s,ud , v d,Bdlaa , 1 0 ,B , n , 0 , 0 , a l n ) 0023 0024 c Ca lcu la te the p r o j e c t i o n matrix which projec t s any vector 0025 c the n u l l space of the Jacobian. 0026 c T 0027 c The projec t ion i s ( I - vd vd ). 002B 0029 do BB5 1*1,n 0030 do B86 j * l , n 0031 vdt( j , i > — v d ( i , j ) 0032 886 continue 0033 BBS continue 0034 0033 c a l l gault(vd,vdt ,homog,n,m,n,10,10,ndla) 0036 0037 do 39 i - l , n 003B homog(i,1)"homog'1,1)+!. 0039 39 continue 0040 0041 return 0042 end VAX FORTRAN V4. 4-177 DUAO IC BUCHAL. 0PT3S0LVE1. FOR»9 S0LVE1 PROGRAM SECTIONS Naaa 0 ( C O D E 1 flPDATA 2 (LOCAL 13-May-19B7 12i54iS5 7-Nov-19B6 15i20l35 VAX FORTRAN V4.4-177 DUAO:C BUCHAL.OPT3SOLVE1.FOR»9 Page Bytea At t r ibute s 239 PIC CON REL LCL SHR EXE B PIC CON REL LCL SHR NOEXE 1044 PIC CON REL LCL NOSHR NOEXE RO NOHRT LONG RO NOHRT LONG RD HRT LONG T o t a l Space Al loca ted 1291 ENTRY POINTS Address Type Naae 0-00000000 S0LVE1 OJ VARIABLES Address Type Naae AA IA4 I 2-0000034B 1*4 MIN ARRAYS Addreaa Type Naae AA 1*4 J AF-000000143 1*4 N Addreas Type Naae AP-00000010C1 IA4 M AP-000O00OCO 1*4 NDIM Addreaa Type Naae AP-oooooooaa 1*4 M D I M A 2-0000034C R*4 U D Address Type Naaa Bytes Dimensions AP-000000180 R*4 HOMOG A* (A, *> AP-00000004B R*4 MAT AA (A, A) 2-00000000 R*4 S 40 (10) 2-00000028 R*4 VD 400 (10, 10) 2-000001B8 R*4 VDT 400 (10, 10) LABELS Addreaa Label Addreaa Label Address Label AA 39 AA 8B3 AA BBB FUNCTIONS AND SUBROUTINES REFERENCED Type Naae GMULT Type Naaa S V D 13-May-19B7 1 2 i 5 7 i l l 7-Nov-1986 15:18)54 0001 aubroutine solve2( laoda f mat,adiaa,ndia ,a ,n ,homog,thedot) 0002 0003 c F i n d the a in iaua nora inverse kinematic s o l u t i o n . 0004 c Solve the aystem Ax-b where A (m z n ) , z (n z 1) , b (a z 0005 0006 c 0007 0008 c imode- l f . t r u e , re turn the p r o j e c t i o n 'homog'. 0009 c mat- the matrix (A) to be so lved. 0010 c The l a s t column contains ' b ' . 0011 c ad iaa- The f i r s t dimension of array 'mat' 0012 c ndim- The dimension of the s o l u t i o n 'thedot' (1 0013 c m.n- The dimensions of A 0014 c homog- The p r o j e c t i o n onto the n u l l space of A 0015 c thedot- The a in iaua nora a o l u t i o n . ooie 0017 r e a l mattadima,*),thedotIndia) 0018 r e a l vd(10,10) ,s(10) ,vdt(10,10) 0019 r e a l hoaog(ndia,ndia) 0020 0021 •toler*.00001 0022 0023 a ln -a ln0(a ,n ) 0024 0025 c Perform s ingular value decomposition. 0026 0027 c a l l avd(mat ,a ,ud,vd,adlma,10 ,m,n, l ,0 ,mln) 0028 0029 c Derive the so lu t ion f r o a the s ingular valuea . 0030 0031 do 10 i » l , n 0032 t h e d o t ( l ) ' O . 0033 do 15 j » l , a 0034 i f ( s ( j ) / s ( l ) . g t . s t o l e r ) then 0035 thedot! i ) - thedot ! i ) + (vd( i , j ) / s t j))*aat( j , i 0036 endif 0037 15 continue 0038 10 continue 0039 0040 i f ( imode.eq. l ) then 0041 0042 c Calcu la te the pro jec t ion onto the nul lapace of 'mat'. 0043 0044 do 35 1-1,n 0045 do 36 j - l , n 0046 v d t ( i , j ) » - v d ( j , i ) 0047 36 continue 004B 35 continue 0049 0050 c a l l gmult(vd,vdt,homog,n,m,n,10,10,ndim) 0051 0052 do 100 1-1,n 0053 homog(i,i)-1.+homog(1,1) 0054 100 continue 0055 0056 endif 0057 VAX FORTRAN V4. 4-177 DUAO i C BUCHAL. OPT3SOLVE2. FOR) S0LVE2 005B 0059 13-May-1987 1 2 t 3 7 t l l 7-Nov-1986 15•IBi54 VAX FORTRAN V4.4-177 __DUA0 : C BUCHAL. OPT3SOLVE2. FOR} 13 Page return end PROGRAM SECTIONS Naae 0 (CODE 1 (PDATA 2 (LOCAL T o t a l Space A l l o c a t e d Bytea A t t r i b u t e s 3B3 12 1092 1487 PIC CON REL LCL SHR EXE PIC CON REL LCL SHR NOEXE PIC CON REL LCL NOSHR NOEXE RD NOHRT LONG RD NOHRT LONG RD HRT LONG ENTRY POINTS Address Type Naae 0-00000000 S0LVE2 OJ Ln VARIABLES Address Type AA 1*4 AP-00000OOCfJ 1*4 * * R*4 Naae I MDIMA STOLER Address Type Naae AP-000000048 1*4 IMODE 2-00000348 1*4 MIN 2-0000034C R*4 UD Address Type Naaa A* 1*4 AP-000000180) 1*4 Addreas Type Naae AP-O0000014O 1*4 M AP-OOOOOOIOB 1*4 NDIM ARRAYS Address Type AP-0000001CO R*4 AP-00000008Q R*4 2-00000190 R*4 AP-0O000020O R*4 2-00000000 R*4 2-000001B8 R*4 Name HOMOG MAT S THEDOT VD VDT Bytes Dimensions AA AA 40 AA 400 400 (A, A) (A, A) (10) (A) (10, 10) (10, 10) LABELS Address Label Address Label 10 AA 15 Address Label Address Label Address Label AA 35 AA 36 AA 100 S0LVE2 13-May-19B7 12)37)11 VAX FORTRAN V4.4-177 Page 3 7-Nov-19BG 13i lBi54 DUAO!CBUCHAL.0PT3S0LVE2.FOR)13 FUNCTIONS AND SUBROUTINES REFERENCED Type Nane Type Nane GMULT SVD COMMAND QUALIFIERS FOR/LIST OPT. FOR,RDFILE. FOR, INPUT. FOR, GROW. FOR,FINPAT. FOR, SOLVE1 .FOR, INT2D. F0R.HULL2D.F0R ,GMULT. FOR .JACOB. FOR,COSTT. FOR, / CHECK • (NOBOUNDS, OVERFLOW, NOUNDERFLOH) /DEBUG" (NOSYMBOLS, TRACEBACK) / STANDARD" (NOSYNTAX, NOSOURCE_FORM) / SHOW" (NOPREPROCESSOR, NO INCLUDE, MAP, NODICTIONARY, S INGLE) /WARNINGS* (GENERAL ,NODECLARATIONS) /CONTINUATIONS-19 /NOCROSS_REFERENCE /NOD LINES /NOEXTEND SOURCE /F77 /NOG_FLOATING /I4 / NOMACHI NE_CODE /OPTIMIZE COMPILATION STATISTICS Run T i n e i 2.93 aeconda Elapsed T l n e i 7.43 aeconda Page F a u l t a i 390 Dynanic Menory) B45 pages 13-M«y-1987 12l57i46 VAX FORTRAN V4.4-177 22-Jan-19B7 13i43i27 DUAO: C BUCHAL. OPT3SORT. FOR»13 Page 1 0001 subroutine s o r t t f r a c t , t n a x l , t n a x 2 , v l i e t , f t a b l e ) 0002 0003 c Teat a l l the penetrat ion vactora and f l a g thoae which are 0004 c larger than T r a c t ' t inea the l arges t . 0005 0006 c Paranetera• 0007 c f r a c t - the f r a c t i o n of the larges t penetrat ion to 0008 c be used as a thresho ld . 0009 c tmaxl,tmax2- the magnitudes of the two largeat penetrat ions 0010 c v l i s t d . J ) - array po int ing to the two largest penetrat ions 0011 c v l i s t < l , j ) - l i n k id 0012 c v l i s t ( 2 , j > - obstacle i d 0013 c v l i s t ( 3 , j > - current or previous penetrat ion 0014 c vector 0015 c v l i s t ( 4 , J ) - number of degrees of freedom of 0016 c l i n k 0017 c f t a b l e d , ] ) - tab le of penetrat ion vectors larger than tha 001B c threshold 0019 c 1-1 ink i d , j -obs t i d 0020 c values• 0021 c 1- current penetrat ion only 0022 c 2- previous penetrat ion only 0023 c 3- both penetrations 0024 c 0025 0026 r e a l f r a c t 0027 integer f table!10,10) 0028 r e a l v lenl (10,10) ,v len2!10,10) 0029 integer l indof(2 ,10) 0030 r e a l v l ena(2 ) ,vec t s (2 ,2 ) .po in t s (2 ,2 ) 0031 integer v l l s t ( 4 , 2 ) 0032 0033 common / l i n d o f / l i n d o f 0034 common /nobj s /n l inks ,nobs t 0035 common /nrdof /ndof ,nxdof 0036 0037 c F ind the largest and second larges t penetrat ion vec tors , and 0038 c f i n d the lengtha of a l l the penetrat ion vec tors . 0039 O040 tmaxl-0. 0041 tmax2-0. 0042 do 10 ind-1,ndof 0043 i s t a r t - 1 i n d o f ( 1 , i n d ) 0044 inumb-l indof(2 , ind) 0045 i f ( i s t a r t . n e . 0 ) then 0046 do 20 i l l n k - l s t a r t , l s t a r t + l n u m b - l 0047 do 25 I o b s t = 1 , n o b s t 0048 c a l l c v e c t ( 1 , i l i n k , i o b s t , v l e n s , v e c t s , p o i n t s ) 0049 v l e n l ( i l i n k , i o b s t l - v l e n s ( l ) 0050 v l e n 2 ( l l l n k , i o b B t ) - v l e n s ( 2 ) 0051 do 50 i n d 2 - l , 2 0052 l f ( v l ens ( lnd2) .g t . tmaxl ) then 0053 tmax2-tmaxl 0054 tmaxl=vlens(ind2) 0055 d o 55 1=1,4 0056 v l i s t ( i , 2 ) » v l i s t ( i , l ) 0057 55 continue OJ M 00 SORT 13-May-19B7 12.57:46 22-Jan-19B7 13:43:27 005B v l i s t ( l , l ) - i l i n k 0059 v l i s t ( 2 , l ) - i o b s t 0060 v l i B t ( 3 , l ) - i n d 2 0061 v l l « t ( 4 , l ) - i n d 0062 e l s e i f <vlens(ind2>.gt.taax2> then 0063 taax2=vlena(ind2) 0064 v l i s t ( l , 2 ) - i l i n k 0065 v l i s t ( 2 , 2 ) « i o b s t 0066 v l l s t ( 3 , 2 ) - i n d 2 0067 endlf 006B 50 continue 0069 25 continue 0070 20 continue 0071 endif 0072 10 continue 0073 0074 t e s t - f r a c t * t a a x l 0075 0076 c Enter a l l penetrat ion vectora larger than the threshold in to tab le 0077 c • f t a b l e ' . 007B 0079 do 100 ind-1 ,ndof 0080 l a t a r t ' l i n d o f ( 1 , i n d ) 0081 inumb-lindof < 2, lnd) 0082 do 120 i l ink«iBtart , i s tart+inuab- l 0083 do 125 i o b s t ' l , n o b s t 00B4 l f ( t e s t . g t . 0 . ) then 0085 t e a p l - v l e n l(ilink, i o b s t ) 00B6 t e a p 2 » v l e n 2 ( i l i n k , i o b s t ) 0087 i f ( t e a p l . g e .test . a n d . t e a p 2 . g e . t e a t ) than 00B8 f t a b l e d l i n k , iobst ) -3 0089 e l s e i f (teapl.ge.test) then 0090 ftable(11 i n k , i o b s t ) - l 0091 e l s e i f (tenp2.ge .test) then 0092 f t a b l e ! i l i n k , i o b s t ) « 2 0093 else 0094 f t a b l e ! i l i n k , i o b s t ) - 0 0095 endif 0096 - e l s e 0097 f t a b l e ( i l i n k , i o b s t ) - 0 0098 endif 0099 125 continue 0100 120 continue 0101 100 continue 0102 0103 return 0104 end VAX FORTRAN V4. 4-177 DUAO : C BUCHAL. 0FT3S0RT. FOR 113 Page SORT 13-May-1987 12i57i46 VAX FORTRAN V4.4-177 Page 22-Jan-19B7 13I43I27 DUAO > t BUCHAL.OPT3 SORT.FOR 113 PROGRAM SECTIONS Naae Bytea 0 8COOE 447 1 8PDATA 4 2 8LOCAL 936 3 LINDOF BO 4 NOBJS B 5 NRDOF B T o t a l Space Al located 1483 A t t r i b u t e s PIC CON REL LCL SHR EXE RD NOHRT LONG PIC CON REL LCL SHR NOEXE RD NOHRT LONG PIC CON REL LCL NOSHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG ENTRY POINTS Address Type Naae 0-00000000 SORT VARIABLES Address Type Naae AP-00000004& R*4 FRACT AA 1*4 IND2 5-00000000 1*4 NDOF * * R*4 TEMPI AP-OOOOOOOCd R*4 TMAX2 Addreas Type Naae * * 1*4 I A* 1*4 I NUMB 4-00000000 1*4 NLINKS ** R*4 TEMP2 Address Type Naae Address Type Naae 2-00000350 1*4 ILINK 2-00000354 1*4 IOBST 4-00000004 1*4 NOBST * * R*4 TEST 2-00000348 1*4 IND * * 1*4 ISTART 5-00000004 1*4 NXDOF AP-0000000B8 R*4 TMAX1 ARRAYS Addreas Type Naae Bytes Dimensions AP-O0000014d 1*4 FTABLE 400 (10, 10) 3-00000000 1*4 LINDOF BO (2, 10) 2-00000338 R*4 POINTS 16 (2, 2) 2-00000328 R*4 VECTS 16 (2, 2) 2-00000000 R*4 VLEN1 400 (10, 10) 2-00000190 R*4 VLEN 2 400 (10, 10) 2-00000320 R*4 VLENS B (2) AP-O0O00010O 1*4 VLIST 32 (4, 2) LABELS Address Label Address Label Address Label Address Label Address Labe l Address Labe * * 10 ** 20 ** 25 * * 50 * * 55 * * 100 AA 120 ** 125 SORT 13-May-19B7 12i57i46 VAX FORTRAN V4.4-177 22-Jan-19B7 13)43i27 DUAO : CBUCHAL. 0PT3S0RT. FOR 113 Page 4 FUNCTIONS AND SUBROUTINES REFERENCED Type Naae CVECT COMMAND QUALIFIERS FOR/LIST OPT. FOR , RDFILE. FOR, INPUT. FOR, GROM. FOR, FINPAT. FOR , SOLVE1. FOR, INT2D. FOR, HULL 2D. FOR , GHULT. FOR, JACOB. FOR, COSTT. FOR , /CHECK* < NOBOUNDS, OVERFLOW, NOUNDERFLOH) /DEBUG* (NOSYMBOLS ,TRACEBACK) / STANDARD* (NOSYNTAX, NOSOURCE_FORM) / SHOW* (NOPREPROCESSOR , NOINCLUDE, MAP, NODICTIONAR Y, S INGLE) /WARNINGS* (GENERAL .NODECLARATIONS) /C0NTINUATI0NS = 19 /NOCROSS_REFERENCE /NOD_LINES /NOEXTEND_SOURCE /F77 /NOG_FLOATING /I4 /NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run T l a e i Elapsed Tiae t Page F a u l t s ! Dynaalc Memoryi 4.23 seconds 10.89 seconds 624 845 pages 13-May-1987 13I00I5B ll-May-1987 IB1OB1IB 0001 program s t r a j 0002 0003 c I n t e r a c t i v e l y generate a proposed sequence of intermediate p o s i t i o n s 0004 c de f in ing a s t a r t i n g guess for the opt imizat ion a lgori thm. 0005 0006 r e a l theta(4,10) 0007 r e a l boxl(2,10,10) ,box2(2,10,10) 0008 integer l l n k B ( 4 ) 0009 l o g i c a l i e rase . s top 0010 r e a l xdot (3) ,xgoal (3) ,xcurr(3) 0011 r e a l a l p h a ! 4 ) , l e n t 4 ) , d i s t ( 4 ) 0012- integer j t y p e ( 4 ) , l i n k ! 4 ) 0013 l o g i c a l ixdot(6) 0014 r e a l kgoal(3) 0015 r e a l thetal (4) 0016 r e a l theta2<4) 0017 r e a l rotdes(3,3) 0018 r e a l xpos(3) 0019 l o g i c a l s a v e , f i r s t , f l n a l 0020 0021 data k g o a l / 0 . , 0 . , 1 / 0022 OJ 0023 common / r o b o t / l i n k , j t y p e , a l p h a , l e n , t h e t a , d i s t 1 0 0024 common /nrdof /ndof ,nxdof 0025 0026 c Read the kinematic parameters from a f i l e . 0027 0028 c a l l r d f i l e 0029 0030 i l a s t - 1 0031 0032 wr i t e !* ,* ) 'Enter the number of task coordinates (2 or 3)' 0033 read!*,*) nxdof 0034 0035 i f (nxdof.eq.3) ixdot!3)> . true . 0036 i x d o t ( 4 ) - . t r u e . 0037 i x d o t ! 5 ) - . t r u e . 0038 0039 d2rad-.0174533 0040 0041 c Read the geometry of the l i n k s and obstacles from a f i l e . 0042 0043 o p e n ! 1 , f l i e - ' b o x . d a t ' , s t a t u s - ' o l d ' ) 0044 r e a d d , * ) n l i n k s 0045 do 29 k - 1 , n l i n k s 0046 r e a d d , * ) n v e r t , l i n k s ! k ) 0047 do 13 i - l , n v e r t 0048 r e a d d , * ) (boxl( j , 1 , k ) , j -1 ,2) 0049 13 continue 0050 29 continue 0051 r e a d d , * ) nobst 0052 do 19 k=l,nobst 0053 r e a d d , * ) nvert 0054 do 14 i - l , n v e r t 0055 r e a d d , * ) !box2 ( j , i , k ) , j -1 ,2) 0056 14 continue 0057 19 continue VAX FORTRAN V4.4-177 Page 1 DUAO : C BUCHAL. 0PT3STRAJ. FOR»1 STRAJ 13-May-19B7 13i00:5B ll-May-1987 IBSOB'IIB 0058 c l o s e d ) 0059 0060 npos=l 0061 wr i te !* ,* ) 'Enter I n i t i a l conf igurat ion angles' 0062 r e a d ! * , A ) ( t h e t a ( i , n p o s ) , i - l , n d o f ) 0063 0064 do 25 i - l , n d o f 0065 t h e t a l l i l - t h e t a l i . n p o s ) 0066 25 continue 0067 006B c I n i t i a l i z e the graphics environment. 0069 0070 c a l l g r s t r t ( 4 0 2 7 , l ) 0071 c a l l newpag 0072 c a l l w indow)-4 . ,4 . , -4 . ,4 . } 0073 c a l l vwport lO . ,100 . , 0 . , 100 . ) 0074 0075 f i r s t * . t r u e . 0076 i e r a s e - . t r u e . 0077 f i n a l * . t r u e . 0078 0079 100 continue 0080 0081 i f l . n o t . f i r a t ) then 00B2 0083 c a l l cue1os 00B4 wr i te !* ,* ) 'Save poa i t ion? ( T / F ) ' O0B5 r e a d ( A , A ) save O0B6 i f ( .not.save) then 00B7 npos-maxOd ,npos-1) 0088 e lse 0089 f i n a l - . f a l s e . 0090 endif 0091 0092 w r i t e ( A , A ) 'Quit? ( T / F ) ' 0093 read)*,*) stop 0094 0095 i f ( f ina l ) then 0096 wri te !* ,* ) 'Enter goal p o s i t i o n and o r i e n t a t i o n ' 0097 r e a d ! * , * ) xgoal (1) ,xgoal (2) ,angoal 0098 e l se 0099 0100 l f (stop) then 0101 xpos( l ) -xgoal(1) 0102 xpos(2)-xgoal(2) 0103 angle-angoal*d2rad .0104 e lse 0105 wri te !* ,* ) 'Enter intermediate poa i t ion and o r i e n t a t i o n ' 0106 read!*,*) xpos(1) ,xpost2) ,angle 0107 angle-angle*d2rad 010B endif 0109 0110 c Find an optimal Inverse so lut ion for the des ired p o s i t i o n . 0111 0112 c a l l rotmat(kgoal ,angle ,rotdes) 0113 c a l l i n v e r s ( l e n , a l p h a , d i s t , t h e t a 1 , j t y p e , n d o f , n x d o f , i x d o t , r o t d e s , 0114 A x p o s , t h e t a 2 ) VAX FORTRAN V4. 4-177 DUAO:C BUCHAL.0PT3STRAJ.FOR > STRAJ 13-May-1987 13:O0i5B ll-May-1987 18:08:18 VAX FORTRAN V4. 4-177 DUAO;C BUCHAL.0PT3STRAJ.FOR;1 Page LO U> 0115 0116 0117 011B 0119 0120 0121 0122 0123 0124 0125 0126 0127 0128 0129 0130 0131 0132 0133 0134 0135 0136 0137 0138 0139 . 0140 0141 0142 0143 0144 0145 0146 0147 0148 0149 0150 0151 0152 0153 0154 0155 0156 0157 0158 0159 0160 0161 0162 0163 0164 0165 0166 0167 0168 0169 0170 0171 write!*,*> ' X , Y pos, angle (xpos(1) ,1-1 ,2) ,angle n p o s » n p o s + l do 23 1=1,ndof t h e t a l ( i ) « t h e t a 2 ( i > thetal 1, npOB) =theta2( 1) 23 continue e n d i f w r i t e ' * , * ) 'Erase screen? ( T / F ) ' read!*,*) ierase c a l l cnopen endif c a l l t r i d n t l . f a l s e . ) i f ( i e r a B e ) then c a l l newpag c Draw the obstacles . do 22 It=1,nobst c a l l novelbo>2(1,1 ,k) ,box2(2, l ,k)) c a l l drawlbox2(1,2 ,k) ,box2(2,2 ,k)) c a l l drawlbox2(1,3 ,k) ,box2(2,3 ,k)) c a l l draw(box2( l ,4 ,k) ,box2(2 ,4 ,k)) c a l l draw(box2(l , l ,k) ,box21 2 ,1 ,k)) 22 continue endif c Draw f i n a l p o s i t i o n . c a l l t r l d n t l . f a l s e . ) c a l l t r a n B l I x g o a l l 1 ) , x g o a l I 2)) c a l l rotate langoal .angoal ) c a l l n o v e l b o x l 1 1 , 1 , i l a s t ) , b o x l ( 2 , 1 , i l a s t ) ) c a l l d r a w l b o x l 1 1 , 2 ,i l a s t ) , b o x l ( 2 , 2 , i l a s t ) ) c a l l d r a w l b o x l 1 1 , 3 , i l a s t ) , b o x l I 2 , 3 , i l a s t ) ) c a l l d r a w l b o x l 1 1 , 4 , i l a s t ) , b o x l 1 2 , 4 , 1 l a s t ) ) c a l l d r a w l b o x l 1 1 , 1 , i l a B t ) , b o x l 1 2 , 1 , 1 l a s t ) ) c Draw the l i n k p o s i t i o n s . do 15 i p o s « l , n p o s c a l l t r i d n t l.falBe.) i l l n k = l do 10 ind"l ,ndof angle"thetaI ind. ipos) c a l l r o t a t e ( a n g l e , a n g l e I c a l l t r a n e l I l e n t i n d ) , 0 . ) c a l l n o v e l 0 . , 0 . ) STRAJ 0172 c a l l d a s h p t d ) 0173 c a l l draw(-len<Ind),0.) 0174 c a l l dashptlO) 0175 0176 l f ( i n d . e q . l i n k s ( i l i n k ) ) than 0177 c a l l move lboxld , 1 , i l i n k ) , boxl (2,1,11 ink > > 017B c a l l d r a w ( b o x l d , 2 , i l i n k ) ,boxl ( 2 .2, i 1 ink)) 0179 c a l l d r a w ( b o x l ( l , 3 , i l i n k ) , b o x l ( 2 , 3 , i l i n k ) ) 01B0 c a l l d r a w ( b o x l d , 4 , i l i n k ) ,boxl (2,4 , i l ink)) 01B1 c a l l d r a w l b o x l d , 1 , i l i n k ) , b o x l ( 2 , l , l l i n k ) ) 01B2 i l i n k - i l i n k + 1 01B3 endif 0184 01B5 10 continue 01B6 15 continue 01B7 0188 f i r s t * . f a l a e . 01B9 0190 i f ( .not .atop) goto 100 0191 0192 101 continue 0193 c a l l grs top co 0194 to 0195 c Save the pos i t ions i n a f i l e . 0196 0197 opend , f i l e » ' t r a j .dat ' . s tatus* 'nev' ) 019B w r i t e d , * ) xgoal (1) , xgoal (2) , xgoal ( 3) ,angoal 0199 w r i t e d.A) npos 0200 do 500 i* l ,npos 0201 w r i t e d , * ) (theta ( j , 1 ) , j * l ,4) 0202 500 continue 0203 c l o s e d ) 0204 0205 atop 0206 end 13-May-1987 13i00i58 ll-May-1987 1BI08I1B VAX FORTRAN V4.4-177 Page 4 DUAO:C BUCHAL.OPT3STRAJ.FOR >1 STRAJ 13-May-19B7 13:00i58 ll-May-1987 18:08:18 VAX FORTRAN V4.4-177 DUAO : t BUCHAL. OPT3STRAJ . FOR > 1 Page 5 PROGRAM SECTIONS Name Bytes 0 (CODE 1B90 1 SPDATA 271 2 (LOCAL 220B 3 ROBOT 240 4 NRDOF 8 T o t a l Space Al located 4817 A t t r i b u t e s PIC COR REL LCL SHR EXE RD NOHRT LONG PIC CON REL LCL SHR NOEXE RD NOHRT LONG PIC CON REL LCL NOSHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG PIC OVR REL GBL SHR NOEXE RD HRT LONG ENTRY POINTS Address Type Nane 0-00000000 STRAJ VARIABLES OJ Address Type Nane Address Type Naaa Addrasa Type Nane Address Type Nane ro Ul 2-0000070C RA4 ANGLE 2-00000708 RA4 ANGOAL AA RA4 D2RAD 2-000006F8 LA4 FINAL 2-000006F4 LA4 FIRST AA IA4 I 2-000006E8 LA4 IERASE AA IA4 ILAST A* IA4 ILINK AA IA4 IND AA IA4 IPOS AA IA4 J AA IA4 K 4-00000000 IA4 NDOF 2-000006FC IA4 NLINKS 2-00000704 IA4 NOBST AA 1*4 NPOS 2-00000700 IA4 NVERT 4-00000004 IA4 NXDOF 2-000006FO LA4 SAVE 2-000006EC LA4 STOP ARRAYS Addreaa Type Nane Bytea Dimensions 3-00000020 RA4 ALPHA 16 (4) 2-00000000 RA4 BOX1 800 <2, 10, 10 2-00000320 RA4 BOX2 800 (2, 10, 10 3-000000E0 RA4 DIST 16 (4) 2-00000674 LA4 IXDOT 24 (6) 3-00000010 IA4 JTYPE 16 (4) 2-0000068C R*4 KGOAL 12 13) 3-00000030 RA4 LEN 16 (4) 3-00000000 IA4 LINK 16 (4) 2-00000640 IA4 LINKS 16 (4) 2-000006B8 RA4 ROTDES 36 13, 3) 3-00000040 RA4 THETA 160 (4, 10) -2-00000698 RA4 THETA 1 16 (4) 2-000006A8 RA4 THETA 2 16 14) 2-00000668 RA4 XCURH 12 13) 2-00000650 R*4 XDOT 12 (3) 2-0000065C RA4 XGOAL 12 13) 2-000006DC R*4 XPOS 12 (3) STRAJ 13-May-19B7 13 i00 i58 VAX FORTRAN V4.4-177 Page ll-May-1987 18tOB:18 DUAO:CBUCHAL.OPT3STRAJ.FOR;1 LABELS Address Label AA AA 10 23 Address AA AA Label 13 25 Address Label A A AA 14 29 Address Label AA 15 0-0000022C 100 Address AA AA Labe l 19 101 Address Label AA AA 22 500 FUNCTIONS AND SUBROUTINES REFERENCED Type Nane Type Nane CMCL08 GRSTOP ROTATE CMOPEN GRSTRT ROTMAT Type Nane DASHPT INVERS TRANSL Type Nane DRAM MOVE TRIDNT Type Nane FDRSCLOSE NEHFAG VWPORT Type Nana F0R60PEN RDFILE HINDOW 13-May-1987 12I5BI 24-Jul-1986 11I16I 0001 SUBROUTINE SVD( A,Q,U,V,ND,NDV,M,N,P,NU,NV) 0002 DIMENSION A(ND, 1) ,Q( N) ,U(ND,1) ,V(NDV,1) ,E( 100) 0003 REALA8 SS,DA,DB.DSORT 0004 INTEGER P 0005 DATA T O L , E P S / 2 . E - 3 0 , 5 . E - 7 / 0006 C THIS ROUTINE IS CODED FROM THE ALGOL PROCEDURES BY GOLUB AND REINSCH 0007 C IN CONTRIBUTION 1/10, PAGE 134, OF 'HANDBOOK FOR AUTOMATIC 0008 C COMPUTATION', EDITED BY WILKINSON AND REINSCH. 0009 C THIS ROUTINE COMBINES PROCEDURES SVD AND MINFIT. M>N IF MATRIX 0010 C U REQUIRED. OTHERWISE NO RESTRICTION ON M. IF SOLN. OF AX»B 0011 C REQUIRED AND M<N, THEN RANK I A)*M. 0012 CO TO 10 0013 ENTRY SIRG(A,Q,ND,M,N) 0014 P ° 0 0015 NU-0 0016 NV-0 0017 GO TO 10 0018 ENTRY SOLSVD(A,Q,V,ND,NDV,M,N,P) 0019 NU«0 0020 NV'N 0021 10 NP-N+P 0022 NPl'N+1 U> 0023 IER«0 M 0024 C ^ 0025 C REDUCE U TO BI DIAGONAL FORM BY HOUSEHOLDER TRANSFORMATIONS 0026 IF (M.GE.N) GO TO 20 0027 MP1«M+1 002B DO 15 I 'MPl .N 0029 DO 15 J - 1 , N 0030 15 A ( I , J ) » 0 . 0031 20 CONTINUE 0032 G-0.0 0033 X » 0 . 0 0034 DO 130 I « 1 , N 0035 E ( I ) « G 0036 SS-O.DO 0037 L-I+ l 0038 IF(I.GT.M)CO TO 35 0039 DO 30 J « I , M 0040 DA»A(J , I> 0041 30 SS-SS+DAADA 0042 35 G=0.0 0043 IF(SS.LT.TOL) GO TO 60 0044 F » A ( I , I ) 0045 G'DSQRT(SS) 0046 IF(F.GE.O.O) G«-G 0047 H=FAG-SS 0048 A(I , I )=F-G 0049 IF(L.GT.NP) GO TO 60 0050 DO 55 J - L . N P 0051 SS-O.DO 0052 DO 45 K»I,M 0053 DA«A(K,I) 0054 DB=A(K,J) 0055 45 SS=SS+DAADB 0056 F ' S S / H 0057 DO 50 K»I,M VAX FORTRAN V4.4-177 page 1 DUAO : C BUCHAL. 0PT3SVDWS. FOR 14 SVD 13-May-19B7 12:58:02 24-Jul-1986 11:16:53 VAX FORTRAN V4. 4-177 DUAO:CBUCHAL.0PT3SVDHS.FOR14 Page to co oo 005B 50 A ( K , J ) » A < K , J ) + F * A ( K , I 1 0059 55 CONTINUE 0060 60 0(D=G 0061 SS-O.DO 0062 IF(L.GT.N) GO TO 120 0063 IF(I.GT.M)G0 TO 75 0064 DO 70 J - L . N 0065 DA=A(I,J> 0066 70 SS=SS+DA*DA 0067 75 G-0.0 006B IF(SS.LT.TOL) GO TO 120 0069 F=A(I,L> 0070 G-DSQRT(SS) 0071 IF(F .GE.O.O) G - - G 0072 H*F*G-SS 0073 H1-1.D0/H 0074 A(I ,L)=F-G 0075 DO B5 J - L , N 0076 B5 E<J1-A(I,J)*H1 0077 DO 100 J - L , M 007B SS-O.DO 0079 DO 90 K - L . N O0BO DA=A<J,K) O0B1 DB=A(I,K> 0082 90 SS-SS+DAADB 0083 DO 95 K«L,N 00B4 95 A ( J , K ) » A ( J , K ) + 3 S * E ( K ) O0B5 100 CONTINUE 0086 120 T-ABS(Q(I))+ABS(E(I)) 0087 IF(Y.GT.X) X=T 008B 130 CONTINUE 00B9 C 0090 c ACCUMULATE RIGHT HAND TRAN 0091 IF(NV.EQ.O) GO TO 210 0092 V(N,N)=1.0 0093 G-E(N) 0094 IF (N.EQ. l ) GO TO 210 0095 DO 200 11*2,N 0096 I-NP1-II 0097 L - I + l 0098 IF(G.EQ.O.O.OR.I .GT.M) i 0099 H=A(I,L)*G 0100 DO 140 J«L,N 0101 140 V ( J , I ) - A ( I , J ) / H 0102 DO 170 J - L , N 0103 SS-O.DO 0104 DO 150 K - L . N 0105 DA-A(I,K) 0106 DB=V(K,J) 0107 150 SS-SS+DA*DB 0108 DO 160 K - L , N 0109 160 V(K,J)=V(K,J)+SS*V(K,I) 0110 170 CONTINUE 0111 ISO DO 1B5 J=L,N 0112 V ( I , J ) - 0 . 0 0113 185 V( J ,I)=0.0 0114 190 V(I,11=1.0 180 SVD 0115 G-E(I) 0116 200 CONTINUE 0117 C 0118 C ACCUMULATE LEFT HAND TRANSFORMATIONS 0119 210 IF(NU.EQ.O) GO TO 310 0120 IF(NU.LE.N) GO TO 215 0121 DO 214 I«NP1,NU 0122 DO 213 J=NP1,NU 0123 213 U ( J , I ) » 0 . 0 0124 214 U ( I , I ) « 1 . 0 0125 215 DO 300 11 = 1,N 0126 I=NP1-II 0127 L - I + l 0128 G'Q(I) 0129 IF(L.GT.NU) GO TO 223 0130 DO 220 J = L,NU 0131 220 U ( I , J ) « 0 . 0 0132 225 IF(G.EO.O.O.OR.I .GT.M) GO TO 280 0133 H=A<I,I)*G 0134 IF(L.GT.NU) GO TO 255 0135 DO 250 J=L,NU 0136 SS-O.DO |5 0137 DO 240 K»L,M vo 013B DA=A(K,I) 0139 DB«U(K,J) 0140 240 SS=SS+DA*DB 0141 F=SS/H 0142 DO 245 K - I . M 0143 245 U(K,J)=U(K,J)+F*A(K,I) 0144 250 CONTINUE 0145 255 G1=1.D0/G 0146 DO 260 J=I,M 0147 260 U(J,I)=A(J,I)*G1 0148 GO TO 290 0149 C CODE FOR C=0.0 0150 2B0 DO 285 J = I ,M 0151 285 U(J,I)=0.0 0152 290 U(I,I)=U(I,I)+1.0 0153 300 CONTINUE 0154 C 0155 C DIAGONALISE THE BIDIAGONAL FORM 0156 310 CONTINUE 0157 EPSX=EPS*X 0158 IF(M.GE.N) CO TO 315 0159 IF (NP.EQ.N) GO TO 315 0160 MP1=M+1 0161 DO 312 J=NP1,NP 0162 DO 312 I=MP1,N 0163 312 A(I ,J)=0.0 0164 315 DO 1000 KK=1,N 0165 NITER = 0 0166 K=NP1-KK 0167 330 KP1=K+1 016B DO 340 LL=1,K 0169 L=KP1-LL 0170 IF(ABS(E(L)) .LE.EPSX) GO TO 450 0171 I F C L . E Q . l ) GO TO 450 13-May-19B7 12:5B:02 24-Jul-1986 11:16:53 VAX FORTRAN V4.4-177 Page 3 DUAO : C BUCHAL. 0PT3SVDHS. FOR 14 SVD 0172 IF<ABS(0(L-1)).LE.EPSX) CO TO 400 0173 340 CONTINUE 0174 C 0173 C CANCEL E(L) - ONLY IF L>1 0176 400 O O . O 0177 S = 1.0 0178 LM1»L-1 0179 DO 440 I - L , K 0180 F=S*E(I) 0181 E(I)=C*E(I) 0182 IF(ABS(F) .LE.EPSX) GO TO 450 0183 G-Q(I) 0184 IF(ABS(F).GT.ABS(G)) GO TO 410 0185 H » 2 . 0 * A B S ( G ) * S Q R T ( . 2 5 + 1 . 5 * F / G ) * * 2 ) 0186 CO TO 415 0187 410 H » 2 . 0 * A B S ( F ) * S O R T < . 2 5 + ) . 5 * G / F ) * * 2 ) 0188 415 Q(I)*H 0189 C - G / H 0190 S » - F / H 0191 IF(NU.EQ.O) GO TO 430 0192 DO 420 J«1 ,M 0193 Y=U(J,LH1> 0194 Z=U(J,I) 0195 U(J,LM1)"Y*C+Z*3 0196 U(J,I)=-Y*S+Z*C 0197 420 CONTINUE 0198 430 IF(NP.Eg.N) GO TO 440 0199 DO 435 J ' N P l , N F 0200 Y=A(LM1,J) 0201 Z*A(I ,J ) 0202 A(LM1,J)«C*Y+S*Z 0203 435 A ( I , J ) « - S * Y + C * Z 0204 440 CONTINUE 0205 C 0206 C TEST F CONVERGENCE 0207 450 Z-O(K) 0208 I F ( L . E O . K ) GO TO 900 0209 NITER-NITER+1 0210 IFINITER.LT.40) GO TO 455 0211 IER=IER+1 0212 GO TO 1000 0213 455 CONTINUE 0214 C 0215 C SHIFT FROM BOTTOM 2 BY 2 MINOR 0216 X=Q(L) 0217 Y -O (K-l ) 021B G=E(K-1) 0219 H=E(K) 0220 F=0.5*( (Y-Z)*(Y+Z)-MG-H)>MG+H) )/(H*Y) 0221 G=SQRT(F*F+1.0) 0222 TD=F+G 0223 I F ( F . L T . O . O ) TD=F-C 0224 F=( <X-Z)*<X+Z)+HMY/TD-H> ) /X 0225 C 0226 C NOW DO THE QR TRANSFORMATION 0227 C=1.0 0228 S=1.0 13-May-1987 12)58i02 VAX FORTRAN V4.4-177 ' 24-Jul-19B6 H : 1 6 i 5 3 DUAO : C BUCHAL. 0PT3SVDWS. FOR; 4 SVD L O L O 0229 1 :,P1«L+1 0230 [F(LPl .GT.K) GO TO 510 0231 1 30 500 I=LP1,K 0232 IM1-I-1 0233 ( >E(I> 0234 Y=Q(I) 0235 1 H=S*G 0236 1 G ' O G 0237 IF(ABS(F) .GT.ABS(H) ) GO ' TO 456 023B Z-2.0*ABS(H)*SQRT(.25+(. 5*F/H)AA2) 0239 i GO TO 457 0240 456 Z»2 .0*ABS<F)*SQRT<.25+1 . 5*H/F)**2) 0241 457 E(IM1)<=Z 0242 C ' F / Z 0243 S=H/Z 0244 F»X*C+G*S 0245 G«-XAS+C*C 0246 H«Y*S 0247 Y=Y*C 0248 IF'NV.EQ.0) GO TO 465 0249 DO 460 J » 1 , N 0250 X=V(J,IM1> 0251 Z « V ( J , I ) 0252 V(J,1M1)-X*C+Z*S 0253 V( J , D — X*S+Z*C 0254 460 CONTINUE 0255 465 IF(ABS(F) .GT.ABS(H)) GO TO 470 0256 IF (ABS(H) . E Q . 0 . ) GO TO 472 0257 Z=2.0*ABS(H)*SQRT( .25+( . 5*F/H)**2) 0258 GO TO 475 0259 470 Z=2.0*ABS(F)*SQRT(.25+1. ,5*H/F)**2) 0260 GO TO 475 0261 472 Z « 0 . 0262 C=1./SQRT(2.) 0263 S-C 0264 Q(I-1)"0. 0265 GO TO 476 0266 475 Q(I-1)-Z 0267 C - F / Z 026B S«H/Z 0269 476 F»C*G+S*Y 0270 X«-S*G+C*Y 0271 IF(NU.EQ.O) GO TO 490 0272 IF (IM1.GT.NU .OR.I .GT.NU) GO TO 490 0273 DO 480 J - 1 , M 0274 Y-U(J,IM1) 0275 Z-UIJ , I ) 0276 U(J,IM1)«Y*C+Z*S 0277 U ( J , I ) « - Y * S + Z * C 0278 480 I CONTINUE 0279 490 1 IF(N.EQ.NP) GO TO 500 0280 DO 495 J=>NP1,NP 0281 Y=A(IM1,J) 0282 Z=A(I,J) 0283 A(IM1,J)-C*Y+S*Z 0284 49! S A(I,J)-=-S*Y+CAZ 02B5 50( ) CONTINUE 13-H.Y-19B7 12158102 VAX FORTRAN V4.4-177 P « 9 « 24-Jul-19B6 11.16:53 __DUA0:C BUCHAL.OPT3SVDHS.FOR14 SVD 13-May-19B7 12.5B.02 24-Jul-19B6 11)26.33 VAX FORTRAN V4. 4-177 _DUAO > C BUCHAL. OPT3SVDHS. FOR»4 Page 8 AA 420 0-OOOOOC60 456 0-OOOOODAD 475 O-OOOOOECF 510 0-0000104D 650 0-00000F33 1000 0-00000AAC 430 0-00000C7F 457 0-O0OOODB9 476 0-000010A7 600 AA 660 AA 1100 AA 435 AA 460 AA 4B0 0-0OOOOFA8 610 0-OOOOOF64 700 0-00000B1F 440 0-OOOOOD20 465 0- O00DDE5O 490 AA 620 1- 00000000 710' 0-0O000B27 450 0-OOOOODBO 470 AA 495 0-00001005 630 0-00000EF0 900 0-00000B50 455 0-00000D5C 472 0-0OO00EC7 500 AA 640 AA 950 FUNCTIONS AND SUBROUTINES REFERENCED Type Name Type Name RAB MTH«DSQRT RA4 MTHBSQRT COMMAND QUALIFIERS FOR/LIST OPT.FOR, RDFILE. FOR , INPUT.FOR,GROH. FOR, FINPAT. FOR ,S0LVE1. FOR , INT2D. FOR ,HULL2D. FOR ,CMULT.FOR ,JACOB.FOR,COSTT.FOR, /CHECK* (NOBOUNDS , OVERFLOW, NOUNDERFLOH) /DEBUG-(N05TMB0LS,TRACEBACK) /STANDARD* (NOSYNTAX.NOSOURCE FORM) OJ / SHOW* I NOPREPROCESSOR , NO INCLUDE, MAP, NODICTIONART , S INGLE > W /WARNINGS* (GENERAL, NODECLARATIONS) ^ /CONTINUATIONS = 19 /NOCROSS_REFERENCE /H0D_LINES /NOEXTEND SOURCE /F77 / NOG_FLOATI NG /I4 /NOMACHINE_CODE /OPTIMIZE COMPILATION STATISTICS Run Timei 24.50 aeconda Elapaed Timet 81.99 aeconda Page Faults> 2828 Dynamic Memoryi B45 pages OJ OJ OJ SVD 0286 02B7 028B 0289 0290 0291 0292 0293 0294 0295 0296 0297 029B 0299 0300 0301 0302 0303 0304 0305 0306 0307 030B 0309 0310 0311 0312 0313 0314 0315 0316 0317 031B 0319 0320 0321 0322 0323 0324 0325 0326 0327 0328 0329 0330 0331 0332 0333 0334 0335 0336 13-May-19B7 12i5Bi02 24-Jul-1986 H t l 6 i 5 3 VAX FORTRAN V4.4-177 DUAOiCBUCHAL.0FT3SVDHS.FOR)4 Page 510 E I D - 0 . 0 EIK1-F Q<K)-X CO BACK AND TEST F SPLITTING GO TO 330 CONVERGENCE ATTAINED 900 IF(Z.GE.O.O) GO TO 1000 0(K>— Z IF'NV.EQ.O) GO TO 1000 DO 950 J - l . N 930 V < J , K ) » - V ( J , K > 1000 CONTINUE ACCURACY IN 40 C CHECK IF ANY SING. VALUES NOT FOUND TO REQD. C ITERATIONS. IER>0 IF SO. IF(IER.EO.O) GO TO 700 HRITE(6,710) IER 710 FORMAT ( ' 0 ' , 1 5 , 3 X , ' SINGULAR VALUES NOT FOUND IN 40 ITERATIONS') RETURN C C SORT SINGULAR VALUES 700 DO 600 K=1,N G » - 1 . 0 J»K DO 610 I - K . N IF<Q(I>.LE.G) CO TO 610 G=g<I) J - I 610 CONTINUE I F(J . E Q . K ) GO TO 600 OtJ ) - O ( K ) 0<K)-G IF(NV.EQ.O) GO TO 630 DO 620 I -1 ,N H - V ( I,J) V ( I,J ) - V ( I , K ) 620 V ( I , K ) - H 630 IF(NU.EO.O) GO TO 650 DO 640 1=1,M H=U(I,J) U ( I,J ) - U ( I , K ) 640 U(I,K)=H 650 IF'N.EQ.NF) GO TO 600 DO 660 I-NPl.NP H=A(J ,I) A (J , I ) - A ( K , I ) 660 A ( K , I ) - H 600 CONTINUE 1100 RETURN END SVD PROGRAM SECTIONS Has* 0 (CODE 1 $PDATA 2 (LOCAL T o t a l Space Al loca ted 4369 52 86B 5289 13-May-19B7 12I58I02 24-Jul-19B6 11I16I53 VAX FORTRAN V4.4-177 DUAOiC BUCHAL.OPT3SVDHS.FOR;4 Page Bytes A t t r i b u t e s PIC CON REL LCL SHR EXE PIC CON REL LCL SHR NOEXE PIC CON REL LCL NOSHR NOEXE RD NOHRT LONG RD NOHRT LONG RD HRT QUAD ENTRY POINTS Address Type Nane 0-0000003E SING Address Type Naae 0-0000006E SOLSVD Address Type Name 0-00000000 SVD VARIABLES Address Type Name Address Type Naae Address Type Name Address Type Name 2-00000244 R*4 C 2-00000198 R*B DA ** R*8 DB ** RA4 EPS 2-0000022B R M EPSX 2-000001DO R M F 2-O0OOO1C0 R*4 G ** RA4 Gl 2-000001D4 R M H ** R M HI 2-000001B0 1*4 I 2-000001AC IA4 IER 2-O0O001F0 I M II 2-00000264 1*4 IM1 2-000001B8 1*4 J 2-000001DC IA4 K 2-00000234 1*4 KX ** 1*4 KP1 2-000001CC 1*4 L AA IA4 LL 2-0000024C 1*4 LM1 ** 1*4 LP1 2-000002BCO 1*4 M AA IA4 MP1 2-000002900) 1*4 N 2-000002BBO 1*4 ND 2-0000029B9 1*4 NOV 2-0000023C 1*4 NITER 2-000001A4 1*4 HP 2-000001AB 1*4 NP1 AP-0000002BO 1*4 NU AP-0000002C8 IA4 NV 2-0000029C8 1*4 P 2-0000024B R*4 S 2-00000190 R*B SS AA R*4 TD 2-000001A0 R M TOL 2-000001C4 R M X 2-000001EC R M Y 2-0000025B RA4 Z ARRAYS Address Type Naae Bytes Dlaensions AP-000000048 RA4 A AA (A, 1) 2-00000000 R M E 400 (100) AP-000000088 RA4 Q AA (A) AP-0000000CB RA4 U AA (A, 1) 2-000002948 RA4 V AA (A, 1) LABELS Address Label Address Label Address Label Address Label Address Label Address Labi 0 -000000B0 10 AA 15 0--0000010D 20 AA 30 0-00000171 35 AA 45 AA 50 AA 55 0 -00000277 60 A* 70 0-000002CC 75 AA B5 AA 90 AA 95 AA 100 0--00000418 120 AA 130 AA 140 AA 150 AA 160 AA 170 0--000005CD 1B0 AA 185 AA 190 AA 200 0-0000062C 210 AA 213 AA 214 0-000006B9 215 AA 220 0--00000729 225 AA 240 AA 245 AA 250 0-00000803 255 AA 260 0--00000840 2B0 AA 2B5 0--00000B7D 290 AA 300 0-00000B91 310 AA 312 0--00000909 315 0-00000934 330 AA 340 0--000009BC 400 0-00000A14 410 0-00000A32 415 13-May-19B7 12i56i54 ll-May-1987 14i l7t22 VAX FORTRAN V4.4-177 Page 1 DUAO: C BUCHAL. 0PT3TRANDB. FOR » 2 0 OJ OJ On 0001 0002 0003 0004 0005 0006 0007 000B 0009 0010 0011 0012 0013 0014 0015 0016 0017 001B 0019 0020 0021 0022 0023 0024 0025 0026 0027 0028 0029 0030 0031 0032 0033 0034 0035 0036 0037 0038 0039 0040 0041 0042 0043 0044 0045 0046 0047 0048 0049 0050 0051 0052 0053 0054 0055 0056 0057 subroutine trandb(values) Transform the geometric database spec i fy ing kinematic chains of r i g i d l i n k s by performing a chain of transformations on each l i n k . The transformations are i n a standard form, and are funct ions of given j o i n t v a r i a b l e s . PARAMETERS I valuea- the valuea of the j o i n t v a r i a b l e s (degrees). SUBROUTINES CALLED) gmult C a l l e d fromi Var iab les I l e n . d i s t , a l p h a , t h e t a - Denavit-Hartenberg s p e c i f i c a t l o n s for the kinematic chains j type- array spec i fy ing each j o i n t aa pr ismat ic or revo lu ta ndof- the number of degreea of freedom for each kinematic chain integer ndof,nxdof Integer jtype!4) integer link<4) r e a l l en (4 ) ,d i s t (4 ) . a lpha(4 ) , the ta (4 ) r e a l valuea!4) r e a l ident(4,4} r e a l tempi4,4) r e a l pretra(4) ,poatra(4) r e a l tmat(4 ,4) , tprod(4,4) r e a l tempi(4),temp2(4),temp3(4) integer l indof(2 ,10) integer boxes(2,10) r e a l sverts (2 ,100) , tverts (2 ,100) common / r o b o t / l i n k , j t y p e ; a l p h a , l e n , t h e t a , d i a t common / i d e n t / i d e n t common /nrdof /ndof ,nxdof common / o b j s / b o x e s , s v e r t s , t v e r t s common /nobj s /n l inks ,nobs t common / l i n d o f / l i n d o f d 2 r a d « . 0 1 7 4 5 3 3 do 500 i - 1 ,4 do 501 j -1 ,4 t p r o d t j , i ) « i d e n t < j , i ) 501 continue 500 continue i c o u n t « 1 T R A N D B 13-May-1987 12156154 VAX FORTRAN V4.4-177 Page 2 ll-May-19B7 14I17I22 DUAO t C BUCHAL.OPT3TRANDB.FOR;20 t o t o 0058 0059 0060 0061 0062 0063 0064 0065 0066 0067 006B 0069 0070 0071 0072 0073 0074 0075 0076 0077 007B 0079 OOBO 00B1 0082 0083 0084 0085 0086 0087 OOBB 0089 0090 0091 0092 0093 0094 0095 0096 0097 009B 0099 0100 0101 Transform the part c l u s t e r s c o n s t i t u t i n g the l i n k s of a l l the open kinematic chains l n the system. Each l i n k i s re la ted to the previous l i n k by the Denavit-Hartenberg transformation. The net transformation i s the product of these transformations. t m a t ! 3 , l ) » 0 . tmat (4 , l ) -0 . t m a t ( 4 , 2 ) « 0 . tmat(4,3)-0. t m a t ( 4 , 4 ) » l . Transform each l i n k of tha kinematic chain by the Denavit-Hartenberg transformation. do 20 lnd2- l ,ndof tempi(lnd2}-alpha(ind2)Ad2rad temp2tind2)-theta(ind2)*d2rad temp3(ind2)-valuea(ind2)Ad2rad l f ( jtype(lnd2).eq.0> then Revolute j o i n t . tmat( tmat( tmatl tmatl tmatl tmatl tmatl tmat ( tmatl tmatl tmatl slse 1.1) 2,1) 1,2) 2,2) 3,2) 1,3) 2,3) 3,3) 1,4) 2,4) 3,4) •costtemp3tind2)) -S ln ( t emp3( lnd2) ) —Sin ( temp3( ind2))*cost tempi( ind2)) >cos(temp3llnd2))Acosttempitind2)) • s ln l tempi ( lnd2) ) •s in(temp3(ind2))Asinltempi Iind2>) --cos(temp3(ind2))*sinI tempiIind2)) -cos!tempi( ind2)) » l e n ( i n d 2 ) * c o s ( t e m p 3 ( i n d 2 > ) -Ien(ind2)*sinltemp3lind2)) • lent lnd2) Prismat ic j o i n t . 0102 t m a t ( 1 ,1) • c o s ( t e m p 2 l l n d 2 ) ) 0103 tmat 1 2 ,1) «sln(temp2(lnd2)) 0104 tmat 1 1 .2) - - s i n ! t e m p 2 ( i n d 2 ) ) A c o s I tempi I i n d 2 ) ) 0 1 0 5 t m a t I 2 ,2) • c o s I t e m p 2 1 i n d 2 ) ) A c o s I t e m p i I l n d 2 ) ) 0106 t m a t l 3 ,2) - s i n l t e m p l ( l n d 2 ) ) 0 1 0 7 t m a t l 1 ,3) - s i n ( t e r a p 2 l i n d 2 ) ) A s i n l t e m p i I i n d 2 ) ) 0 1 0 8 t m a t ( 2 ,3) » - c o s ( t e m p 2 l i n d 2 ) ) A s i n l t e m p i I i n d 2 ) ) 0 1 0 9 t m a t l 3 ,3) - c o s I t e m p i I i n d 2 ) ) 0110 t m a t l 1 ,4) - l e n ( l n d 2 ) A c o s ( t e m p 2 ( i n d 2 ) ) 0111 t m a t I 2 .4) - l e n ( i n d 2 ) A s i n ( t e m p 2 ( i n d 2 ) ) 0 1 1 2 t m a t l 3 .4) • t e m p 3 ( i n d 2 ) 0113 0114 endif TRANDB 13-M-.T-1987 12136• 94 VAX FORTRAN V4.4-177 Page 3 l l-May-19B7 14il7:22 DOAO 1C BUCHAL. 0PT3TRANDB. FOR 120 0119 0116 c a l l g n u l t ( t p r o d,tBat , t a a p , 4 , 4 , 4 , 4 , 4 , 4 ) 0117 0118 do 300 i - l , 4 0119 do 301 j -1 ,4 0120 t p r o d l j , l ) - t a a p ( j r l ) 0121 301 continue 0122 300 continue 0123 0124 c If the l i n k i a modelled, tranafora l t a v e r t i c e s l n tha 0129 0126 i f ( l i n d o f ( l , i n d 2 ) . n e . 0 ) then 0127 0128 do 111 i n d - l i n d o f ( l , i n d 2 ) , l i n d o f ( l , i n d 2 ) + l i n d o f ( 2 , i n d 2 ) - l 0129 do 101 j-boxes( 1 , ind) .boxes(1, Ind)-t-boxes ( 2 , i n d ) - l 0130 do 102 k-1,2 0131 p r e t r a l k ) - a v e r t s ( k , j ) 0132 102 continue 0133 pre tra (3 ) -0 . 0134 p r e t r a ( 4 ) - l . 0135 c a l l g m u l t ( t p r o d , p r e t r a , p o s t r a , 4 , 4 , 1 , 4 , 4 , 4 ) 0136 do 103 k-1,2 0137 t v e r t s ( k , j ) - n o s t r a ! k ) 0138 103 continue 0139 101 continue 0140 111 continue 0141 0142 endif 0143 0144 20 continue 0145 0146 return 0147 end TRANDB 13-Hey-1987 12I56IS4 ll-May-1987 14)17)22 VAX FORTRAN V4.4-177 Page 4 DUAO i C BUCHAL. OPT II TRANDB. FOR 12 0 PROGRAM SECTIONS Naaa Bytea A t t r i b u t e s 0 BCODE 563 PIC CON REL LCL SHR EXE RD NOHRT LONG 1 tPDATA 8 PIC CON REL LCL SHR NOEXE RD NOHRT LONG 2 6LOCAL 376 PIC CON REL LCL NOSHR NOEXE RD HRT LONG 3 ROBOT 96 PIC OVR REL CBL SHR NOEXE RD HRT LONG 4 IDENT 64 PIC OVR REL GBL SHR NOEXE RD HRT LONG 5 NRDOF B PIC OVR REL GBL SHR NOEXE RD HRT LONG e OBJS 1680 PIC OVR REL GBL SHR NOEXE RD HRT LONG 7 NOBJS B PIC OVR REL GBL SHR NOEXE RD HRT LONG 8 LINDOF 80 PIC OVR REL CBL SHR NOEXE RD - HRT LONG T o t a l Spaca A l l o c a t e d 2883 CO CO CO ENTRY POINTS Addreaa Type Naae 0-00000000 TRANDB VARIABLES Addreaa Type Naae A A A A 7-00000000 R M D2RAD I M IND2 I M NLINKS Addreaa Type Naae A A I M I A A 1*4 J 7-00000004 1*4 NOBST Addreaa Type Naae A A A A 5-00000004 IA4 ICOUNT IA4 K 1*4 NXDOF Addreaa Type Naae A A 5-00000000 IA4 IA4 IND NDOF ARRAYS Addreaa Type Naae Bytea Dlaenalona 3-00000020 RM ALPHA 16 (4) 6-00000000 IA4 BOXES BO (2, 10) 3-00000050 RM DIST 16 (4) 4-00000000 RM IDENT 64 (4, 4) 3-00000010 1*4 JTYPE 16 (4) 3-00000030 RM LEN 16 (4) 8-00000000 1*4 LINDOF 80 (2, 10) 3-00000000 IA4 LINK 16 (4) 2-00000050 RM POSTRA 16 (4) 2-00000040 RM PHETRA 16 (4) 6-00000050 RM SVERTS 800 (2, 100) 2-00000000 RM TEMP 64 (4, 4) 2-O0000OEO RM TEMPI 16 (4) 2-000000F0 RM TEMP2 16 (4) 2-00000100 RM TEMP 3 16 (4) 3-00000040 RM THETA 16 (4) 2-00000060 RM TMAT 64 (4, 4) 2-000000A0 RM TPROD 64 (4, 4) 6-00000370 RM TVERTS BOO <2, 100) AP-000000040 RM VALUES 16 (4) TRANDB LABELS Addreaa AA AA Label 20 301 Addreaa AA AA Label 101 500 13-May-19B7 12I56I54 VAX FORTRAN V4.4-177 ll-May-19B7 14tl7i22 DUAO:CBUCHAL.OPT3TRANDB.FOR»20 Page Addreaa Label AA AA 102 501 Addreaa AA Label 103 Addreaa Labe l AA i n Address Label AA 300 FUNCTIONS AND SUBROUTINES REFERENCED Type Nana Type Naaa GMULT RA4 MTHflCOB Type Naaa RA4 MTHASIN COMMAND QUALIFIERS FOR / LIST OPT. FOR, RDFILE. FOR, INPUT. FOR, GROH. FOR, FINPAT. FOR, SOLVE1. FOR, INT2D. FOR, HULL2D. FOR,GMULT. FOR, JACOB. FOR, COSTT. FOR, /CHECK - ( NO BOUNDS , OVERFLOW , ROUNDERFLOH) /DEBUG* (NOSTMBOLS .TRACE BACK) / STANDARD* (NOSYNTAX, N0S0URCE_FORM) / SHOW* (NOPREPROCESSOR, NOINCLUDE, MAP .NODICTIONART .SINGLE) w /WARNINGS* (GENERAL,NODECLARATIONS) LO /CONTINUATIONS* 19 /NOCROSS_REFERENCE /NOD_LINES / N0EXTEND_80URCE /F77 *° /NOG_FLOATING /I4 /N0MACHINE_C0DE /OPTIMIZE COMPILATION STATISTICS Run T i a e • Elapsed T l a e i Page F a u l t s ! Dynaa1c Meaory• 5.5S saconda 16.63 seconds 690 B45 pagea 

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

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

Comment

Related Items