TMvenaty Microfilms International - OhioLINK ETD

TMvenaty Microfilms

International

M ICROCOPY RESOLUTION TEST CHART NA TIO N AL BUREAU OF STANDARDS

STANDARD REFERENCE M ATERIAL 1010a (ANSI and ISO TEST CHART No. 2)

i.i i

t

University Microfilms Inc.300 N. Zecb Road, Ann Arbor, MI 48106

INFORMATION TO USERS

This reproduction was made from a copy of a manuscript sent to us for publication and m icrofilm ing. While the most advanced technology has been used to photograph and reproduce this manuscript, the quality of the reproduction is heavily dependent upon the quality of the material submitted. Pages in any manuscript may have indistinct print. In all cases the best available copy has been filmed.

The following explanation of techniques is provided to help clarify notations which may appear on this reproduction.

1. Manuscripts may not always be complete. When it is not possible to obtain missing pages, a note appears to indicate this.

2. When copyrighted materials are removed from the manuscript, a note appears to indicate this.

3. Oversize materials (maps, drawings, and charts) are photographed by sectioning the original, beginning at the upper left hand corner and continuing from left to right in equal sections with small overlaps. Each oversize page is also filmed as one exposure and is available, for an additional charge, as a standard 35m m slide or in black and white paper format.*

4. Most photographs reproduce acceptably on positive microfilm or microfiche but lack clarity on xerographic copies made from the microfilm. For an additional charge, all photographs are available in black and w hite standard 35m m slide format.*

♦For more in form ation about black and w hite slides or enlarged paper reproductions, please contact the D issertations Custom er Services Departm ent.

UMI Dissertation Information Service

University Microfilms InternationalA Bell & Howell Information Company300 N Zeeb Road, Ann Arbor, Michigan 48106

8618802

Langer, F ran z D ie te r

KINEMATICS AND ROTATIONAL DYNAMICS OF MULTI-LINKAGE SYSTEMS AND THE CONTROL OF A PLANAR TWO-LINK SYSTEM IN THE AIR

The Ohio State Univers i ty Ph.D. 1986

UniversityMicrofilms

International 300 N. Zeeb Road, Ann Arbor, Ml 48106

Copyright 1986

by

Langer, Franz Dieter

All Rights Reserved

PLEASE NOTE:

In all cases this material has been filmed in the best possible way from the available copy. Problems encountered with this document have been identified herewith a check mark -J

1. Glossy photographs or pages______

2. Colored illustrations, paper or prin t_______

3. Photographs with dark background_____

4. Illustrations are poor copy_______

5. Pages with black marks, not original copy______

6. Print shows through as there is text on both sides of p a g e ________

7. Indistinct, broken or small print on several pages

8. Print exceeds margin requirements______

9. Tightly bound copy with print lost in spine _

10. Computer printout pages with indistinct print_______

11. Page(s)_____________lacking when material received, and not available from school orauthor.

12. Page(s)_____________seem to be missing in numbering only as text follows.

13. Two pages num bered . Text follows.

14. Curling and wrinkled pages______

15. Dissertation contains pages with print at a slant, filmed as received__________

16. Other______________________________________________________________________________

UniversityMicrofilms

International

KINEMATICS AND ROTATIONAL DYNAMICS OF MULTI-LINKAGE SYSTEMS AND

THE CONTROL OF A PLANAR TWO-LINK SYSTEM IN THE AIR

DISSERTATION

Presented in Par t ia l Fu l f i l lm en t of the Requirements for the Degree Doctor of Philosophy in the Graduate School

of The Ohio State University

by

Franz D ieter Langer

★ ★ ★ ★ ★

The Ohio State University 1986

Reading Committee:

Professor H. Hemami Professor R. MosesProfessor S. Yurkovich Approved by

Department of E le c t r ic a l Engineering

©1986

FRANZ DIETER LANGER

All Rights Reserved

Dedicated to my parents,

Franz and Elisabeth Langer.

ACKNOWLEDGMENTS

I would l i k e to express my appreciation to r\y adviser, Professor Hooshang Hemami, for his continuous guidance, encouragement and support. I am especia lly grateful to him for his invaluable assistance and constructive c r i t ic is m without which this research project could not have been completed.

I would l i k e to express my grat i tude to Professor Randy Moses, to Professor Steve Yurkovich, and to Professor Don W. M i l l e r for the careful reading and valuable comments on th is d isser ta t ion .

I also would l i k e to thank Todd Pienkowski for the coding of the algorithms of the graphics package, which perform database generation fo r animation.

I would also l i k e to thank Ms. Emily Baird and LuCinda Flores for the excel lent typing of th is manuscript.

This work was supported in part by the National Science Foundation under Grant Number ECS-820-1240, and in part by the Department of E le c t r ic a l Engineering, The Ohio State U nivers i ty , Columbus, Ohio 43210.

VITA

June 1, 1956 Born: Leverkusen, Federal Republic of Germany(FRG).

June 1977 - Jan. 1979 Teaching Assistant, Department of Nachrichtentechnik, Fachhochschule Cologne, FRG.

January 1979

March 1979 - June 1980

July 1980 - June 1981

August 1981 - June 1982

August 1982 - June 1986

December 1983

Ingenieur (Grad.) Nachrichtentechnik, Fachhochschule Cologne, FRG.

Systems Engineer for Avionic Systems at Messerschmitt - Boelkow-Blohm, Munich, FRG.

Systems Engineer for Avionic Systems at Elektronik System GeselIschaft , Munich, FRG.

Recipient of a Fulbright Scholarship for Graduate Studies at The Ohio State Universi ty , Columbus, Ohio, USA.

Research and Teaching Assistant, Departments of E le c t r ic a l Engineering and Mathematics, The Ohio State Universi ty .

M.S. , Department of E lec t r ica l Engineering,The Ohio State Universi ty .

PUBLICATIONS

1. D.F. Langer and H. Hemami, "Dynamics, Control and Simulation of theJump of a Quadruped," Math Bioscience, 75, May 1985, 257-27.

2. D.F. Langer, H. Hemami, and D. Brown, "Applicat ion, Derivat ion, andComputation of Holonomic Constraint Forces in Mechanical Systems." Proceedings fo the 19th Midwestern Mechanics Conference, The Ohio State Univers i ty , Columbus, Ohio, September 9-11, 1985.

iv

FIELDS OF STUDY

Department: E lec tr ica l Engineering, The Ohio State Universi ty

Major F ie ld : Studies in Control: Professor H. Hemami

Professor R. Scnerzinger

Minor Fields: Studies in Mathematics: Professor T. Scheick

Studies in S ta t is t ic s : Professor T. Notz

Studies in Antennas: Professor R. Rudduck

v

TABLE OF CONTENTS

ACKNOWLEDGMENTS .................................................................................................... i i

V I T A ............................................................................................................................... i i i

LIST OF FIGURES ....................................................................................................

LIST OF TABLES ....................................................................................................

CHAPTER PAGE

I . INTRODUCTION

1.1 Scope of the D i s s e r t a t i o n ....................................... 1

1.2 Organization of the Dissertat ion ................................... 3

I I . SURVEYS OF PREVIOUS WORK

2.1 Introduction .......................................................................... 4

2.2 Computer Graphics Applied to Movement Studies . . 4

2 .3 The Dynamics of Multi-Body Systems with Emphasison Rotational Motion .............................................................. 8

2 .3 .1 The Formulation of the Dynamics ofMulti-Body Systems ................................................. 8 .

2 .3 .2 Methods for the Projection of theEquations of Motion of Multi-Body System . 12

2 .3 .3 The Dynamics of Multi-Body Systems in the A ir 14

2 .3 .4 The Symbolic Generation of the Equations of Motion of Multi-Body Systems ........................... 15

2.4 Numerical In s t a b i l i t i e s Induced by the Impositionof Holonomic and Simple Nonholonomic Constraints 15

2.5 The A tt i tude Control of Multi-Body Systems . . . . 15

vi

Page

2.6 Nonlinear C o n t r o l la b i l i t y Concepts .................................... 17

2.7 The Automation of the Nonlinear C o n t r o l la b i l i t yAnalysis ....................................................................................... 23

2.8 Summary ............................................................................................. 24

I I I . A GRAPHICS SOFTWARE PACKAGE FOR KINEMATIC STUDIES

3.1 Introduction . ............................................................................ 25

3.2 The Hardware Arrangement and Software Structure . . 26

3 .2 .1 Hardware Arrangement ................................................. 26

3 .2 .2 Software Structure ...................................................... 25

3.3 Basic Graphics Software .......................................................... 29

3 .3 .1 The Three-Dimensional Viewing Process . . . . 29

3 .3 .1 .1 . Normalizing Transformation . . . . 29

3 .3 .1 .2 . Three Dimensional Clipping . . . . 36

3.4 Hidden Surface Removal ............................................................... 39

3 .4 .1 Hidden Surface Removal forConvex Polyhedra ......................................................... 41

3 .4 .2 Hidden Surface Removal forConcave Polyhedra ......................................................... 41

3 .4 .2 .1 Decision Tests ........................................ 42

3 .4 . 2 . 2 . Warnock Algorithm .................................... 43

3 .4 . 2 . 3 . Weiler Atherton A lg o r i th m ..... 47

3.5 Object Manipulation and Data Structure ........................... 54

3 .5 .1 Data Structure for Kinematic Manipulation . 54

3 .5 .2 Implementation of the Hidden Surface RemovalAlgorithms and Their Data Structure .................. 55

3 .5 .3 Data Structure for Animation Studies...... 57

3.6 Convex Polyhedra ........................................................................... 57

v i i

Page

3.7 Concave Polyhedra ....................................................................... 59

3.7 .1 Humanoid: A D A M ............................................................ 59

3 .7 .1 .1 Program and Data Structures . . . . 59

3 .7 .1 .2 Kinematics ................................................ 64

3 .7 .1 .3 Animation ..................................................... 68

3.8 Summary ................................................ 68IV. THE GENERATION AND THE NONLINEAR CONTROLLABILITY ANALYSIS

OF MECHANICAL SYSTEMS USING THE SYMBOLIC ALGEBRAIC MANIPULATION (SAM) LANGUAGE MACSYMA

4.1 I n t r o d u c t i o n ................................................................................ 69

4.2 The Symbolic Generation of the Rotational Equationsof Motion and the Computation of Their F i r s t In tegrals 70

4.2 .1 The Rotational Equations of Motion forN Rigid L i n k s ................................... 70

4 .2 .2 The Derivat ion of the Rotational Equations of Motion Using the Newton Euler Method anda Decoupling Transformation ............................... 72

4 .2 .3 The Derivation of the Rotational Equationsof Motion from the Interconnection Diagram 73

4 .2 .4 F i rs t In tegrals of the Rotational Equationsof M o t i o n ...................................................................... 77

4.3 The Automation of the Nonlinear C o n t r o l la b i l i t yAnalysis ................................................................................... 79

4.4 The Control of the Rotational Equations of PlanarMotion ......................................................... . . . . . . . . 86

4 .4 .1 The C o n t r o l la b i l i t y Analysis of Two Planar Rigid Links Which are Subject to a Constant Angular Momentum ..................................................... 86

4 .4 .2 A Solution to the Inverse A tt i tude Control Problem of Two Planar, Rigid Links Whichare Subject to a Constant Angular Momentum 92

4.5 Summary ........................................................................................ 118

v i i i

Page

V. APPLICATIONS, DERIVATIONS AND COMPUTATIONS OFHOLONOMIC CONSTRAINT FORCES IN MECHANICAL SYSTEMS

5.1 I n t r o d u c t i o n .............................................................................. 119

5.2 Derivation of Constraint Forces ....................................... 120

5.3 Computation of Constraint Forces ................................... 125

5.3 .1 Computational Complexity ................................... 126

5 .3 .2 Estimates of Round-Off Errors ........................... 127

5.4 I l l u s t r a t i v e Examples ......................................................... 133

5 .4 .1 Planar One L i n k ......................................................... 133

5 .4 .2 Planar Double Pendulum ............................................ 136

5 .4 .3 One Link in Three Dimensions............................... 143

5.5 Conclusions .............................................................................. 150

V I . DISCUSSIONS AND CONCLUSIONS

6.1 Summary........................................................................................... 151

6.2 Recommendations for Future Research .............................. 153

ix

PageAPPENDIX

A Computer Graphics Procedures .............................................................. 154

A . l The Modules of the Basic Graphics P a c k a g e ..................................... 154

A .2 The Modules for Generating the Humanoid, A D A M .............................176

A .3 The Modules for A n i m a t i o n ........................................................................ 194

A .4 Help Documents for Using the Graphics P a c k a g e .............................216

B. The Symbolic Generation of the Rotational Dynamics ofN Rigid Links in Thrpe-Dimensional Space and the Computation of Their F i rs t In tegra ls ........................................ 21D

B . l The Symbolic Projection of the Newton EulerEquations of Motion Using MACSYMA ............................................ 210

B.2 A General Expression for the Rotational Equationsof Motion and the Symbolic Evaluation for the Case of Three Links (N=3) ............................................................................... 224

B.3 The Derivation of the Hamiltonian Form of theRotational Equations of Motion ..................................................... 230

B.4 The Derivation of the Angular Momentum Expressionfor N Arbitrary Rigid Bodies ..........................................................233

B.5 The Derivat ion of the Angular Momentum Expressions for N Rigid Bodies Subject to Holonomic (Connection)Constraints .................................................................................................238

C. The Symbolic Generation and the C o n t r o l la b i l i t y Analysisof the Rotational Dynamics of Two Rigid Planar Links . . . 245

REFERENCES .................................................................................................................. 255

x

LIST OF FIGURES

Fig . Page

2.1 ADAM: A twenty-three Segment Humanoid ....................................... 62.2 The Assembly Robot Consisting of a Robot Arm and

a Conveyor B e l t ............................................................................................ 7

2 .3 The i ' t h Body of a Kinematic Chain and the Bodyand the In e r t ia l Coordinate System ................................................ 9

2.4 Simulation Diagram of a Constrained Mechanical System . . . 13

2.5 I l l u s t r a t i o n of the Lie Bracket O p e r a t io n ..................................19

3.1 Graphics Hardware Arrangement ............................................................... 27

3.2 Dependence Tree of the Basic GraphicsSoftware Package: GRAPH ...................................................................... 28

3.3 Three-dimensional Viewing Process ...................................................... 30

3 .4 . World and Eye Space Coordinate Systems ......................................... 31

3.5 Visual Realism by Perspective Project ion ..................................... 34

3.6 Eye Space Coordinate System with Truncated Viewing Pyramid 38

3.7 Window Edge C o o rd in a te s ........................................................................ 38

3.8 Clipping of a Polygonal M e s h ........................................................... 40(a) Open Polygon Clipping(b) Closed Polygon Clipping

3.9 Test for Containment of One Polygon in Another Polygon . . 44

3.10 Degenerate Case of Edge I n t e r s e c t i o n .......................................... 44

3.11 Flowchart of the Warnock A l g o r i t h m ..................................................... 45

3.12 Flowchart of the Weiler Atherton Algorithm ............................... 48

3.13 Flowchart of the Polygon Clipping Algorithm ............................... 49

3.14a Polygon Clipping of a Clip Polygon Against a Subject Polygon 52

3.14b Generation of the Inside L is t from the Modified Vertex L is tCl ip and Subject L i s t .................................................................................... 52

xi

LIST OF FIGURES(conti nued)

F ig . Page

3.14c Generation of the Outside L is t from the Modified VertexL is t of Slip and Subject P o l y g o n ..........................................................53

3.15 Viewing Pipelines and Object Data Formats ..................................... 56

3.16 Convex Polyhedra: ......................................................................................... 5R(a) Generalized, Truncated Cone(b) Sphere(c) Generalized Bar

3.17 Pointer Structures for Body Segment Description ........................ 60

3.18 Diagram of the Fall Structure of ADAM's S e g m e n ts .........................61

3.19 Coordinate Systems of Body Segments andD ef in i t io n of Position Vectors ......................................................... 62

3.20 Program Dependencies in Software Model ADAM ................................. 63

3.21 Kinematic Study of a Human Long J u m p ...................................................65

4.1 A Collection of N Rigid, Interconnected Links and Their Respective Body Coordinate Systems and theIn e r t i a l Coordinate System .................................................................. 71

4 .2 A Three Link Interconnected System with Centers of Mass CM-j( i = 1, 2 , 3) and Connection-Barycenters B-j ( i = 1, 2 , 3) 74

4 .3 A Planar, One Link System, Which is Hinged to the Origin 84

4.4 Two R ig id , Planar Links and the Associated ConnectionBarycenters and Centers of M a s s .............................................................. 88

4.5 Simulation Diagram for the A tt i tude Control of thePlanar, Reduced Two Link System.............................................................. 95

4.6 The F l igh t Parabola of a Point Mass which Startsup at the Origin and Lands rn an Incl ined Plane R .................. 96

4.7 Simulation Results for an Under-Rotated Dive, H0 = 2(kg * m2 * r a d / s e c ) ........................................................................................ 99

4.8 Simulation Results for an Optimal Dive, H0 = 24(kg * m2 * r a d / s e c ) ...................................................................................... 103

x i i

LIST OF FIGURES(conti nued)

F ig . Page

4.9 Simulation Results for an Over-Rotated Dive,H0 = 35 (kg * m2 * r a d / s e c ) ....................................................................... 107

4.10 Simulation Diagram for the Att i tude Control ofthe Planar, Unreduced Two-Link System ............................................ 112

4.11 Simulation Diagram for the A tt i tude Control of thePlanar, Unreduced Two-Link System ..................................................... 114

4.12 Trajector ies for the Optimal Dive Using as

State Z = (e , 02 , 0 i , 02) T ...................................................................115

5.1 The Simple, Planar Pendulum Subject to Two HolonomicContact Constraints ................................................................................... 121

5.2 The Pendulum Embedded in 0 , x, y S p a c e .............................................. 124

5.3 Comparison of the Operation Count of Smith's, Kane'sand Hemami's Method for Computing Constraint Force . . . . 131

5.4 Double Pendulum Subject to Holonomic Constraints ................... 137

5.5 Double Pendulum Embedded in a Larger State Space ................... 140

5.6 Three-Dimensional Pendulum with Body and In e r t ia l Coordinate Systems Corresponding to Axes (X^r, X2R, X3R)and X^f, X 2 x» X 3 j ) .........................................................................................145

8.1 System of n Rigid Bodies with a Discrete Mass D is tr ibut ionin an In e r t i a l Coordinate System Which Originates in the Center of Mass C .............................................................................................234

8 .2 Body i with I t s Own Body Coordinate System B-jCS in anIn e r t ia l Frame ............................................................................................ 236

x i i i

LIST OF TABLES

Table Page

4.1 Body Segment Parameters for the Planar, Two-Link System . . 97

5.1 Computational Complexity of Calculating an Inverse Matrixand Solving a System of Simultaneous Equations ............................. 128

5.2 Computational Complexity of Calculating Constraint ForcesAssuming the In e r t ia Matrix is Diagonal .......................................... 129

5.3 Computational Complexity of Calculat ing Constraint ForcesAssuming the In e r t ia Matrix is Nondiagonal ...................................... 130

5.4 Operation Count for the Methods of Calculating ConstraintForces for the Planar One L i n k ...................................................................134

5.5 Upper Bounds for Round-Off Errors for Computed ConstraintForces of the Planar One L i n k ................................................................... 135

5.6 Operation Count for the Methods of Calculating ConstraintForces for the Planar Two L i n k ...................................................................138

5.7 Upper Bounds for Round-Off Errors for Computed ConstraintForces of the Planar Double Pendulum ................................................... 144

5.8 Operation Count for Computing Holonomic Constraint Forcesof the Three-Dimensional Pendulum ........................................................ 148

5.9 Operation Count for Computing Holonomic and SimpleNonholonomic Constraint Forces of the Three-Dimensional Pendulum................................................................................................................... 149

7.1 Help Document for P lo t t ing a Sphere Using theGraphics Package ............................................................................................ 207

7.2 Help Document for P lo t t ing A Cylinder Using theGraphics Package ............................................................................................... 208

7.3 Help Document for the Use of the Animation Routines . . . . 209

8.1 Elements of the Coupled Newton Euler Equations of Motion(Equ. 4 .1 ) fo r N Rigid L i n k s ....................................................................... 211

8 .2 Elements of the Decoupling Transformation (Equ. 4 .2 ) . . . . 213

8 .3 MACSYMA Program L ist ing for the Symbolic Project ion of anUnconstrained Three-Link System Onto a Reduced State Space . 215

xiv

LIST OF TABLES(continued)

Table Page

8.4 The Unconstrained Equations of Motion and the DecouplingTransform for Three Links in Three-Dimensional Space . . . . 218

8.5 The Projected Equations of Motion for Three Links inThree-Dimensional Space .......................................................................... 221

8.6 A General Expression for the Decoupled Equations of Motionfo r N Rigid Interconnected Bodies (Equ. 4 .3 ) ............................... 225

8.7 MACSYMA Program L ist ing for the Derivation of the Rotational Dynamics for Three Links in Three-DimensionSpace from the Interconnection D iagram ................................................. 226

8.8 The Projected Equations of Rotational Motion for Three Links in Three-Dimensional Space Which Are Derived fromthe Interconnection Diagram .................................................................. 228

8.9 The General Expression for the Angular Momentum M of NLinks Which Are Subject to Holonomic Constraints ...................... 240

8.10 List ing of the Program Which Symbolically Calculates theAngular Momentum of Three Links ........................................ . . . . 242

8.11 The Angular Momentum H3 and Related Quantit ies of ThreeHolonomic Links ............................................................................................ 243

9.1 The Symbolic Generation of the Rotational Dynamics of Two Planar, Rigid Links from the Dynamics of the SameSystem in Three-Dimensional Space ..................................................... 246

9.2 Linear and Nonlinear Symbolic C o n t r o l l a b i l i t y Analysis ofTwo Planar L i n k s ................................................................................................. 248

9.3 The Invo lut ive Basis Set of Vector Fields for theDetermination of the Nonlinear C o n t r o l l a b i l i t y of the Rotational Dynamics of Two Planar Links ........................................ 249

9.4 The C o n t r o l la b i l i t y Matrix of the Linearized RotationalDynamics of Two Planar Links and Test for C o n t r o l l a b i l i t y D e f e c t s ...................................................................................................................252

xv

Chapter 1

INTRODUCTION

1.1 Scope of the Dissertation

Considerable at tent ion has recently been given to the study of m u lt i - l in ka g e , r ig id body systems. These systems can be employed to both describe the dynamics of robotic mechanisms and to approximate some mechanical a t t r ibu tes of human and animal locomotion.

This dissertat ion pr imarily deals with the a t t i tu d e control problem of the above mentioned systems. I t is divided in to the major areas of the rotat ional dynamics of mult i - l inkage systems, the c o n t r o l la b i l i t y analysis of the equations of rotat ional motion, and the display of the kinematics of motion of m ult i - l inkage systems. The studies in the above mentioned major areas are done on d i f fe re n t types of multi-body systems. The kinematics are studied on the complex model of a human body. The rotat ional dynamics are derived for the general case of an a rb i t ra ry number of r ig id l inks which are connected in an open kinematic chain, and move in three-dimensional space. The c o n t r o l la b i l i t y of m ult i - l inkage systems is analyzed via a planar, two-l ink system. F in a l ly , the reduction by projection and the imbedding of multi body systems, which are important concepts in dynamics, are i l l u s t r a te d by one and two link systems which move in two-and three-dimensional space.

The human body is represented by twenty-three body segments, which are made up of wire-basket solids of revolution and are produced by a general purpose graphics program package for the display of multi-body systems. The graphics program accepts the kinematic information on the m u l t i - 1inkage system as input parameters, solves the associated hidden surface problem and does a transformation from the three-dimensional world space to the two-dimensional image space. The user can input the data, which are the orientat ions of the individual l in k s , by means of an input f i l e or during an in te ra c t iv e dialogue. The major emphasis of this study is placed on the implementation and optimization of computer graphic related algorithms, on software organization and on data base generation for animation.

1

The determination and systematic symbolic generation of the dynamics of the m u l t i - 1inkage systems in the a i r , and on the ground, two important issues in dynamics, are investigated. The dynamics in the a i r are decoupled into the t rans la t iona l dynamics of the center of mass (COM) of the composite system and the rotational dynamics of the individual l inks about the center of mass. Constraints that are due to the angular momentum being constant are included in the formulation.

The c o n t r o l la b i l i t y of the rota t ional dynamics of a planar, two l inksystem are analyzed using methods of l in e a r and nonlinear control theory. I t is shown that the above mentioned two approaches produce s imilar resu l ts . The inverse a t t i tu d e control problem is defined for the planar, two-l ink system and is solved by transforming the corresponding i l l -posedboundary value problem in to a well-posed one. D ig i ta l computersimulations are performed to demonstrate that l in e a r state feedback ensures that the nonlinear two-l ink system is capable fo tracking a prespecified reference t ra je c to ry . I t is also shown that the above mentioned, p a r t i a l l y control lable system does not exhibi t numerical i n s t a b i l i t i e s during simulations i f the constra int , to which the system is subjected, is a l in e a r function of the s ta te variables of the system. The complexity of the c o n t r o l l a b i l i t y of the m ult i - l inkage systems increases with the r is ing number of l inks . Although there i s , in p r in c ip le , no d i f f i c u l t y in performing the nonlinear c o n t r o l la b i l i t y analysis by hand, the lengthy, time consuming computations, which are involved, become increasingly prone to human erro r . For this reason algorithms for an automation of the nonlinear control 1a b i l t i y analysis are discussed. The study comprises properties of the Lie algebra under a change of the frames of references and under a t r ia n g u la r iza t io n of elements of the Lie algebra.

The mechanism of projecting a multi -1inkage system to a reduced s ta te space is investigated. This space is characterized by the imposition of holonomic and simple nonholonomic constraint forces or torques. In the multi -1inkage systems, which are studied here, holonomic constraints have to be enforced, i f the individual bodies of the composite system are jo inted together in a kinematic chain. I f these connections are such that rotat ional degrees of freedom are lo s t , constraint torques are employed, to model these p a r t ic u la r jo in t ch a rac te r is t ic s . The analysis comprises a comparison of three d i f fe re n t algorithms of numerically computing the above type of constraint forces and torques. Several examples demonstrate merits of these d i f fe re n t techniques and compare them.

2

1.2 Organization of the D issertation

This d issertat ion consists of six chapters and three appendices.In chapter two the l i t e r a t u r e is surveyed pertaining to:

- computer graphics applied to movement studies

- dynamics of mult i - l inkage systems in the a i r and the symbolic generation of the equations of motion

- mathematical concepts in nonlinear control theory and the automation of the nonlinear c o n t r o l l a b i l i t y analysis

- the mechanism of project ion of the m ult i - l inkage system and associated i n s t a b i l i t i e s .

In chapter three a number of essential components of a computer graphics package are reviewed. Two algorithms for the solution of the hidden surface problem are outl ined. Animation studies of a human-like model, denoted ADAM, are shown. In chapter four the rotat ional dynamics of m u lt i - l in ka g e systems are derived by using two d i f fe re n t approaches. As an example, the c o n t r o l l a b i l i t y of a constrained planar two l ink system is analyzed using concepts of l in e a r and nonlinear control theory. The a t t i tu d e control problem of the above system is defined and a control strategy is discussed. In chapter f iv e the mechanism of project ion of the m ult i - l inkage system is addressed. Three algorithms are discussed, which can be employed to perform th is project ion numerically. The merits of these algorithms are i l lu s t r a te d by several examples. F in a l ly , a summary is presented in Chapter six and direct ion of future research is di scussed.

3

Chapter 2

SURVEY OF PREVIOUS WORK

2.1 Introduction

In th is chapter the l i t e r a t u r e relevant to the subject matter of th is d issertat ion is surveyed. In section 2 .2 , some fundamentals of computer graphics and the graphical representation of the kinematics of multi-body systems are discussed. In section 2.3 the dynamics of the above mentioned systems are presented and the mechanism of projection by the imposition of holonomic and simple nonholonomic constraint forces or torques is reviewed. In section 2.4 the in s t a b i l i t i e s which are caused by confining the system to a reduced state space are described. In section 2.5 the a t t i tu d e control problem of multi-body systems is defined. F in a l ly , nonlinear c o n t r o l l a b i l i t y concepts are presented in section 2 . 6 .

2 .2 Computer Graphics Applied to Movement Studies

An essential part of the study of dynamics of mechanical systems is the graphical representation of simulation resu lts , i . e . , the transformation and projection of t ra je c to r ie s of the object onto a viewing screen. This provides for a visual v e r i f ic a t io n of simulation r e su l ts , which can, in tu rn , be shown to a wider, not necessarily techn ica l ly inc l ined , audience. Furthermore, computer graphics provides fo r means of data reduction or image enhancement, i f necessary. In f a c t , in movement studies i t is sometimes desired to see the object in the top, side, or f ronta l plane instead of having a three-dimensional image of i t [ 1 ] , In gait analysis , on the other hand, i t might be important to focus on the motion of the hip, for example, to id en t i fy asymmetries of g a i t . There is also an associated inverse problem; i . e , the generation of three-dimensional objects from a two-dimensional image. The inverse problem is of in te r e s t , for example, in the reconstruction of the three-dimensional t r a je c to r ie s of dummies in a crash t e s t , which is recorded on two-dimensional photographic material [2].

Computer graphics is extensively applied in robotics, especially in CAD/CAM systems [3 - 14]. I t i s , for example, sometimes necessary to v is u a l ize the working envelope of an assembly robot T i l ] . On the other hand, computer graphics can be applied to ve r i fy application programs of robots by displaying the motion of the robotic mechanism [ 121.

4

In th is d issertat ion a graphics software package is presented which displays the model of a humanoid and an assembly robot as a set of wire frame segments. This program provides in te ra c t iv e and f i l e -d r iv e n input. The corresponding input parameters are the or ientat ions of the indiv idual segments of the composite system. The software package supports animation cap a b i l i t ie s as w e l l . In the present implementation the user is able to display a cycl ic motion of the multi-body system at various speeds on the screen. An introduction to computer graphics and suggestions for the structure of such a software package are given in [15 - 17].

A number of the essential components of the basic graphic package are reviewed below. This package is p r im ar i ly designed for the use of a vector display , but the objects may be shown on a raster scan display as well [ 1 6 ] . I t generates a database of standard geometric objects, such as a sphere, a cone, a cy l in der , and a bar, which are represented as wire frame baskets and stored in polygon l i s t format [15 - 18]. The indiv idual polygons of these objects are subject to program modules, which do e i th e r open polygon c l ipping [15 - 17] or closed polygon cl ipping using the Sutherland Hodgeman re -en trant clipping algorithm [ 1 9 ] , The hidden surfaces of the object can be removed upon request. There are e s sen t ia l ly two types of algorithms [ 2 0 ] , which accomplish th is task. I f the objects are convex polyhedra the above types of algorithms take a p a r t ic u la r simple form and operate in object space, which is the space that describes the object before the clipping and projection operation. However, i f the objects are concave polyhedra, the corresponding hidden-surface removal algorithms are more elaborate. Two d i f fe r e n t image space algorithms, which operate on the description of the object in image (or screen) space coordinates, are implemented in the graphics software package and are optimized for the present configura t io n . These are the Warnock [21 ] and Weiler-Atherton [22] algorithms. The former algorithm exploits area coherence of the objects, i . e . , the tendency for small areas to be contained in at most a single polygon.I t , consequently, subdivides the i n i t i a l l y large display area into smaller but equal subareas un t i l a correct decision on the v i s i b i l i t y or i n v i s i b i l i t y of polygons is reached. The Wei 1er-Atherton algorithm uses object area coherence. I t recursively subdivides polygons along boundaries of other polygons unt i l a conclusive decision on v i s i b i l i t y or i n v i s i b i l i t y is reached. A f ter the c l ipping and hidden surface removal operat ion, the object is displayed perspectively on the screen [15 -1 7 ] .

The modules of the basic graphics software package are used to display the more complex objects, such as the assembly robot and the humanoid, ca l led ADAM. Some researchers concentrated on the simulation of human movement [23 , 24 ] . The design of the humanoid is taken from [ 2 5 ] , see Fig. 2 .1 . The humanoid is a 23 segment model, where the indiv idual segments are represented as wire-basket solids of revolution.

5

ADAM : A Twenty-three Segment Humanoid

Figure 2 .2 : The Assembly Robot Consisting of

a Robot Arm and a Conveyor Belt

7

Each of the legs consists of four segments: th igh, lower leg, fo o t , andtoes. S im i la r ly , the arms consist of four segments each: the upperarm, lower arm, the palm and the f ingers . The main body consists of the head and a six-segmented torso; the individual segments are denoted: the lower torso, r ib 4 to r ib 1, and the upper torso. Each body segment has at most 6 degrees of freedom: three angles specifying i tsor ien ta t ion and three corresponding angular v e lo c i t ie s . The assembly robot, see Fig. 2 .2 , which is studied here, consists of a robot, which is modelled according to the specif icat ion of the Rhino robot arm [2 6 ] , and a conveyor b e l t . In the animation studies, involving e i ther the humanoid or the assembly-robot, a database is created, which consists of intermediate positions of the whole motion cycle. These intermediate picture frames are displayed on the vector graphics display to create a movie- l ike scene.

2.3 The Dynamics of Multi-Body Systems with Emphasis on RotationalM o t io n

In th is section the dynamics of multi-body systems are reviewed with emphasis on rotat ional motion. In section 2 .3 .1 the general formulation of the above mentioned systems is presented using Bryant angles and Euler parameters as frames of reference. In section 2 .3 .2 several methods are introduced which serve to project the equations of motion of mechanical systems to a reduced subspace. The derivat ion of the rotat ional dynamics of multi-body systems is reviewed in section2 .3 .3 . F i n a l ly , in section 2 .3 .4 the symbolic generation of the dynamics of mechanical systems is discussed.

2 .3 .1 The Formulation of the Dynamics of Multi-Body System

In th is dissertat ion mechanical systems , which are a co l lect ion of r ig id bodies hinged together at ideal bal l-and-socket jo in t s , and which impose holonomic or simple nonholonomic connection constra ints , are studied. The above mentioned systems are called multi-body systems in the sequel. These systems are used to represent bipeds [2 7 -3 1 ] , quadrupeds [ 3 2 ] , and robotic mechanisms [3 3 ] . The dynamics of multi-body systems can be derived using the Hamiltonian formalism, or the Newton Euler or Lagrangian mechanics [34 - 3 8 ] . As the complexity of the multi body system increases, i t is advantageous to resort to graph theory in specifying the mutual interconnection of bodies [ 3 9 ] . In what follows the construction of rotat ional and t rans la t iona l dynamics of the multi-body system are reviewed. Consider Fig. 2 .3 , which shows the i^h body of a kinematic chain in a body-j coordinate system (B-jCS) with axes Xi -j, X? -j, X3 J , r e la t iv e to a world (or i n e r t i a l ) coordinate system ( l i S ) wi£h axes Xy, Yy, Zy. The or ig in of the i^h body is denoted 0-j. The posit ion vectors K-j and L-j point to the hinge point with the ( i + l ) ^

8

body (i+1)

body ( i )

body ( i - 1 )

Figure 2 .3 . The Body o f a Kinematic Chain and the Body and

the In e r t i a l Coordinate System

9

body and the body re la t iv e to Oj. Other qu an t i t ie s , which arenot shown in Fig. 2 .1 , are vectors r j and N-j of reaction forces acting on body i and body i -1 and vector of external torques acting on the respective j o i n t . The or ientat ion and angular ve locity of the i ^ body are specified in minimal state space form by maps A< and Bj using Bryantangles [39"l. Then the equations of motion for the i th l ink are [39 ,40]:

m X-j = -m.j g Yw + Tj - r - j t i

01 = B-j W-j (2 .1 )

J j Wj = -WWjJjWj + KK-j A j - l r j - LL-j A j " l r i+1

with

xi = ( xl , i » x2 , i » x3 , i

0 i = ( 0 l , i » 02 , i » e3 ,i

« 2 .1 . « 3 ,1 )T

0 -w3 . i W2 , i

WWj = +w3 ,i 0 -W l, i

"w2 , i « l . i 0

c2 c 3 "C2S3 s2

Ai = c l s3 + S1S2C3 cl c 3 " S1S2 S3 “ S1C2

s l s3 " C1S2C3 S1C3+C1S2S3 ClC2

C3/ C2 - s 3/ c 2 0

Bi = s3 c3 0

- c 3s2/ c 2 S2S3 /C2 1

C l = cos(0 l f i ) ; C2 = cos(e2 > i ) ; C3 = cos(e3 > i )

S1 = s in ( e 1 # i ) ; S2 = s in (e 2 > i ) ; S3 = s in ( e 3 f i )

10

mi: mass of the boydg: gravity constant

Yw: unit vector in v e r t ica l d irect ion

A l te r n a t iv e ly , the or ientat ion and angular ve loc ity can be specified in nonminimal s tate space form by maps Aj * and B-j* using Euler parameters.The equations of motion for the i^h l ink are then given by:

m-jXj = -m-jg Yw + r-j - r-j+j

Qj - Bj* Wj* ( 2 . 2 )

JjWj = -WW-j J-jW-j + KKj (Ai * ) - 1r i - LLj (Ai * ) “ l r i + i + Nj - Ni+1

with Wj* = (0 , Wl t j , W2 >j , W3 f i )T

2 ( l 0 2 + 1 1 2 ) - 1 2 (1 1 1 2 - 1 o l3 ) 2 (1 1 1 3 + 1 o l 2 )

A i * = 2 ( q 1I 2 + 1 o1 3 ) 2 ( l 02 + 1 2 2 ) - 1 2 (1 2 1 3 " l o l l )

2 ( q i q 3 - I 0I 2 ) 2 (1 2 1 3 + l o l l ) 2 ( l 0 2 + 1 3 2 ) - 1

1o -1 1 - 1 2 ‘■13

B j * = 2 1 l 1o - 1 3 12

12 13 1o '■11

13 "1 2 11 1o

10 10 , i

11 i i , i

12 + 12 , i

13 -*• 1 3 , i

There is a re la t ion between the Euler Parameters and the Bryant angles [39 ] :

t r Aj - f l 4

(2 .3 )

= ~ 120 , i ( i = 2 , 3)

11

Nonminimal state space formulations are advantageous for computational reasons [37 , 39 ] , redundant .computer simulations and d ig i ta l computer simulations of redundant systems.

2 .3 .2 Methods for the Projection of the Equations of Motion of Multi-Body SysteTn?

The mechanism of projections is an in tegral part in the study of the dynamics of multi-body systems. The projection is accomplished by the imposition of holonomic or simple nonholonomic constraint forces or torques. Holonomic constraints are, by d e f in i t io n , represented by algebraic equations, in the generalized coordintes and the time. All the other constra ints , which do not f a l l in the above category, are denoted as non-holonomic constraints [34 - 36]. The term simple, non-holonomic constraints refers to those non-holonomic constraints tha t are l in e a r in the generalized v e lo c i t ie s . In mechanics, fur ther subgroups of constraints are defined [ 3 9 ] , which are not mentioned here.In the multi-body systems, which are studied in th is d isser ta t ion ,holonomic constraints have to be enforced, i f the individual bodies of the composite system, are jo in ted together in a kinematic chain [ 4 0 ] .I f these connections are such that rotat ional degrees of freedom are l o s t , constraint torques are engaged, which model these par t icu la r jo in t character is t ics [4 0 ] .

The multi-body systems and the corresponding constraints can be modelled in the fol lowing way. Consider Fig. ? .4 , which shows a simulation diagram of a constrained mechanical system. The vectors of s ta te , input, and constraints are denoted X, I), and r . The dynamics of the individual bodies of the composite system, which are specified byEq. ( 2 . 1 ) , or ( 2 . 2 ) , are represented by the block:

X = X(X, U, r )

I t is shown [14] tha t the constraint forces or torques can be represented as a function of the state and input of the system:

r = r ( X , U)

The constraint forces serve d i f fe re n t purposes depending upon the appl icat ion considered. In robotics [ 4 1 ] , locomotion [ 3 3 ] , or in biomechanical systems [ 4 2 ] , the constraint forces are controlled such that kinematic constraints are maintained or de l ib e ra te ly v io la ted . Hence, for the synthesis of control laws, i t is desired to derive expressions fo r the constraint forces as functions of the state and the input of the mechanical system, and to derive the s e n s i t iv i ty of these forces with respect to the state and the input [ 4 3 ] . The holonomic constraints can be grouped into m odel- in tr ins ic and model-environment constra ints . The former group re f le c ts the interconnection of

12

of constraintForces

stateInput

Fig. 2.4 Simulation Diagram of a Constrained Mechanical System.

13

individual r ig id l inks in a system [4 4 ] , The above constraints are permanently engaged. They can be implemented into the system's dynamic formulation by a method which projects an unconstrained system to a lower dimensional, constrained subspace [4 0 ] . I f the interconnection between l inks does not allow any movement at a l l ; the corresponding l inks are locked [4 5 ] .

The other group of holonomic constraints describe the in teract ion of the mechanical model with i ts environment, such as ground contact of legs [1 6 -4 9 ] , skiing [5 0 ] , s l id ing [4 2 ] , o r t a c t i l e sensing [51-53] where the sensors detect the p a r t ia l shape, cha rac te r is t ic s , and r e la t iv e motion of an unknown object by surface contact.

The computation of constraint forces as proposed by Smith [54 ] is suitable pr imar i ly for analysis. In th is method, the n dynamic and m constraint equations are solved simultaneously for the unknown accelerations and forces of constraint.

In the second method [ 5 5 ] , suitable for control and simulation, the dynamic equations are substituted back in the constraint equations to el iminate the accelerat ions. The remaining system of m equations is then solved for the forces as a function of the state and input (F ig . 2 .2).

In the th i rd method, proposed by Kane [ 3 4 ] , the mechanical system is embedded in a la rger s ta te space. This space is chosen here in such a way that the constraint equations, corresponding to the unknown constraint forces are l in e a r functions of the new state of the system. The imposition of these simpler constraints reduces this larger dimensional system to the or ig inal one and renders appropriate expressions for the constraint forces. I t is shown on several examples that the calculat ion of these forces results in some computational savings over the f i r s t and second method.

The computational complexity of each method is assessed by provided a count of ar ithmetic operation involved. This can fu r ther be used to estimate the computation time on various computers in a Fortran environment [5 6 ] , The accuracy of the individual method is inferred from the condition number of the respective co e f f ic ie n t matrix of the constraint forces. This provides an upper bound of the re la t iv e residual and the r e la t iv e error [5 7 -6 3 ] ,

2 .3 .3 The Dynamics of Multi-Body Systems in the A ir

In the study of a t t i tu d e control of multi-body systems, see section 2. 6 , the equations of motion of the multi-body system are conveniently described by the t rans la t iona l motion of the center of mass of the composite system and by the rotat ional equations of motion.

14

Those multi-body systems are of in teres t in the study of spacecraft dynamics [64-71] or in analyzing the motion of human T717! or animals under free f a l l conditions. The cat , for example a t tracted considerable a t te n t io n , since i t always lands safely on a l l four legs [73-801.Several studies focus on astronauts, who are maneuvering in outer space [ 8 1 -8 7 ] , or on various disc ipl ines in sport , where the athletes are subject to free f a l l conditions, for example diving [3 0 ] , hurdle running, or pole vault ing.

The derivat ion of the rotat ional equations of motion is considered next. Many researchers have addressed th is problem in the past [39 , 64, 72] , In order to ensure correct resu lts , two independent methods of deriv ing the rotat ional equations of motion are used. In the f i r s t method [32] a free body analysis is done on the system and a decoupling transform is used to extract the rotat ional dynamics. In the second method [39 , 64] the rotat ional equations of motion are derived based upon the system's interconnection structure.

2 .3 .4 The Symbolic Generation of the Equations of Motion of Multi-Body Systems

The symbolic generation of the equations of motion of multi-body systems has attracted many researchers [88 -101 ] . In th is dissertat ion the rotat ional equations of motion are generated by two d i f fe re n t methods using the symbolic language MACSYMA [1 0 0 ] . Thus, the unreduced expressions, which arise through the imposition of constraints [401 can be reduced to t h e i r minimal form by symbolically applying suitable trigonometric id e n t i t ie s and by cancel l ing of common factors , for example. The symbolic manipulation eliminates truncation errors which are caused by the f i n i t e number representation of intermediate results . The symbolic computations o f fe r additional advantages over numeric computations [ 9 9 ] , which are, however, at the expense of the speed of computation. The l a t t e r problem can be overcome by t ran s la t ing the Lisp code, which is the language used to code MACSYMA, into FORTRAN code [ 101] fo r the purpose of i t e r a t i v e l y solving the dynamic equations.

2.4 Numerical I n s t a b i l i t i e s Induced by the Imposition of Holonomic andSimple Nonholonomic Constraints

The imposition of holonomic and simple, nonholonomic constraints on a multi-body system changes i ts s t a b i l i t y properties [102-105] . These i n s t a b i l i t i e s can be compensated by numerical s ta b i l i z a t io n methods [102-110] . These methods, which are o r ig in a l l y used to s ta b i l i z e autonomous systems, can be applied to nonautonomous systems as w e l l , but with relaxed requirements, i . e . , i t suffices that the l a t t e r systems are made s ta b i1iza b le . S ta b i l i za t io n methods are suggested by Baumgarte and others [102-115] , and agree very well with concepts in control theory.

15

In the l a t t e r formulation the numerical s ta b i l i z a t io n methods are equivalent to contro l l ing the unconstrained system, subject to holonomic and simple nonholonomic constraints . The system is acted on by addit ional hypothetical inputs, whose only function is to maintain the constraints and assure s t a b i l i t y .

2.5 The A tt i tude Control of Multi-Body Systems

In th is section the a t t i tu d e control of multi-body systems, which are subject to a constant angular momentum, is discussed.

The angular momentum pr inc ip le very often serves as an analyt ica l tool to bear on questions in the f i e ld of sports and space explorat ion. As an i l l u s t r a t i o n of this pr inc ip le consider the fol lowing question. Assume that a man is f ree ly f a l l i n g a f te r losing ground contact. How can he move various parts of his body r e la t iv e to each other to change his or ienta t ion in space? A solution to th is problem is proposed below. Assuming that the man has no means of producing any external forces, he is subject to the gravity force only, while the angular momentum about his center of gravity remains constant. By v i r tue of the angular momentum pr inc ip le he can apply in ternal forces to change the r e la t iv e position of body parts , so that his or ientat ion is changed fo r the purpose of keeping the angular momentum constant.

Trampolinists, trapeze performers and divers are some of the athletes who explo i t th is p r in c ip le . In f a c t , a diver [80] who was tra ined to perform the same r ight ing movements that a cat does when i t is released from an a rb i t ra ry position above ground. The physical phenomenon, which was analyzed in th is way, is that a cat always lands safely on i t s feet and never on the back, as one might suspect. Other examples which i l l u s t r a t e the use of th is pr inc ip le are hurdle runners, long jumpers, and astronauts maneuvering in f ree space. Astronauts have to be able to change the or ienta t ion of t h e i r bodies in a state ofweightlessnes to perform certa in tasks in connection with manned spaceef l i g h t [ 8 5 -8 7 ] . I f they are not in d i rect contact with the space vehicle they have the choice of using e i th e r a gas gun to produce the - desired change or moving various parts of t h e i r body to accomplish thesame task. In the former case the angular momentum does not remainconstant while in the l a t t e r case i t does. The l a t t e r case is a t t r a c t iv e because i t can be applied in case of a mechanical f a i lu r e of the force producing device. I t also eliminates the need for astronauts to carry excessive amounts of supply f u e l , since cert ian maneuvers can be done by exploi t ing the angular momentum p r in c ip le .

In the study of the a t t i tu d e control of multi-body systems a direct and an inverse a t t i tu d e control problem are defined as follows [8 2 ] .The variables which specify the or ienta t ion of the composite system r e la t iv e to an in e r t i a l system, are denoted external var iab les , while those, which specify the re la t iv e position of body parts to each other,

16

are called in ternal variab les . In the d i rec t a t t i tu d e control problem the in ternal variables are completely specif ied , while the external ones are unknown. This leads to three coupled, f i r s t - o r d e r d i f f e r e n t ia l equations with three unknowns in three dimensions, which can be solved by numerical in tegra t ion . In the inverse a t t i tu d e control problem, the s i tua t io n is reversed, i . e . , the external variables are known and the in terna l ones are unknown. This problem occurs in manned spacecraft, fo r example, where i t is necessary for the astronaut to know, how to move his limbs to bring about a change of his o r ien ta t io n . The inverse a t t i tu d e control problem also leads to three coupled, d i f f e r e n t ia l equations but in the unknown external var iab les , which usually exceed the number of three. Hence, th is is an i l l -po sed boundary value problem, which requires considerable e f fo r t to f ind a solution [8 1 ] .

2.6 Nonlinear C o n t r o l la b i l i t y Concepts

The c o n t r o l l a b i l i t y analysis of a system determines whether the inputs to the system are able to drive the respective state vectors to any a rb i t ra ry point in state space. For l in e a r systems one of several standard tests [116] can be used to determine t h e i r c o n t r o l l a b i l i t y .The same analysis is more elaborate for nonlinear systems. However, the standard tests for the c o n t r o l l a b i l i t y of l in e a r systems can be applied to nonlinear systems as w e l l , when the l a t t e r ones are approximated by a l i n e a r system. This can be done by expanding the nonlinear system in a Taylor series and by reta in ing only the f i r s t order terms. The l a t t e r te s t sometimes Tails to determine the exact c o n t r o l l a b i l i t y properties of the system [117] . Therefore, nonlinear c o n t r o l l a b i l i t y tests are designed to overcome the above mentioned d i f f i c u l t y . The Lie bracket operation is an integral part of the nonlinear c o n t r o l l a b i l i t y analysis Their der ivat ion is given below for a scalar system, but without loss of g enera l i ty . Consider the system

mx = I u-j ( t ) f - j [ x ( t ) ] , x0 e Rn ( 2 . 1)

i = l

which evolves on an n-dimensional state space and has controls u-j(i = 1 , . . . , m) e ( 0 , 1} , which can be switched on and o f f .A solution to Eq. (2 .1 ) is denoted

x ( t ) = e x p [ t f ] x 0 ( 2. 2)

regardless of whether Eq. ( 2 . 1) represents a l ine a r or nonlinear system. The question of c o n t r o l l a b i l i t y of the system, given by Eq.( 2. 1) , can now be reformulated as to whether the terms f-j ( i = 1..............m)in Eq. (2 .1 ) span the space in Rn, which is tangent to x ( t ) , specified by Eq. ( 2 . 2 ) . Hence, i t is desired to systematical ly id e n t i fy basis

17

vector f ie ld s for th is tangent space. At the end of th is process i t has to be determined, whether these basis vector f ie lds, span the whole tangent space. These basis vector f ie ld s can be found as explained below. Consider Fig. 2 .1 , which shows the t ra je c to ry of the state vector in the respective state space. The t ra je c to ry starts at x0 , which is the i n i t i a l value of x ( t ) , Eq. ( 2 . 2 ) . I t is assumed that a ll but the second control , Eq. ( 2 . 1 ) , are switched o f f , so that x ( t ) moves along the vector f i e ld f 2 for t units of time in forward direct ion unti l point x i is reached. At th is point a l l but the f i r s t control are switched o f f and the sta te vector moves along the vector f i e ld f j in forward direct ion up to point X2 . Now, the same vector f ie ld s are traversed in opposite directions unti l point X4 is reached, see Fig.2 .1 . I f point X4 foes not coincide with the s tar t ing point x^, a new d irec t ion in state space is created by the above sequence of traversal of vector f ie ld s f j and f 2- However, i f points xj and X4 coincide, no new direct ion in state space is generated.

A Taylor series expansion for the system

x ( t ) = f i [ x ( t )3 x(o) = x0

about x0 , assuming that i t ex is ts , is:

x ( t ) = f i ( xQ) + t + \ -d-? f i-^ n ) t ? + 0(t 3) (2 .3 )UL c dt2

The in tegrat ion of Eq. (2 .3 ) y ie lds :

x ( t ) = x0 * f i ( x 0) Jdt + 4f j t dt 1 / t 2dt + 0(t4)Ul>u

x ( t ) = x0 + f , ( x 0 ) t + + 0Ct3)

Hence, a second order expression for x ( t ) of Eq. (2 .3 ) is :

x ( t ) = x0 + t f i ( x 0 ) + Y - f i ( x 0 ) + 0 ( t 3 ) <2 -4 )

where f i ( x 0 ) = ^

The above second order expansion is used to f ind an expression for x l» x2» x3» and x4 as a function of x0 , Fig. 2 .1 . The value of x j i s , using Eq. (2 .4 ) :

18

exp( t f , )

exp(t f 9 )

exp( - t f j )

exp( - t f 2 )

Figure 2 .5 . I l l u s t r a t i o n of the Lie Bracket Operation

19

x i ( t ) = e x p ( t f 2)x0= x0 + t f 2 (x0 ) + ® M ^ n l f 2(x0 ) + 0 ( t3 )

S im i la r ly , the value of x2 is :

x2( t ) = e x p f t f ^ x j = expCtf j ) e x p ( t f 2) x0

= X! + t f i f x i ) + 4 f ^ X i ) + 0 ( t3 ) ( 2 . 5 )

Si nee

f l ( * l ) “ f l ( x 0 ) + ^ S n l (x i -xo ) + i ^ g j ^ l ) ( x i - x 0 ) 2 + 0 (x r xo)3

x2( t ) = x0 + t f 2(x0 ) + -5 f ^ )- f 2 ( x 0 ) + 0( t 3)

+ t { f 1(x0) + a f^ x-o) (x i -x0) + \ a2fax2Q (x i~x° )2 + ° [ (x i ' x° ^ 3}

+ 0( t 3)

The value of X3( t ) , Fig. 2 .1 , is found to be:

X3( t ) = e x p ( - t f 2)x 2

= x2 - t f 2(x2 ) + y ~ f 2(x2) + 0( t 3)

= *0 + t f ! ( x 0) + f l ( * o ) + f 2<*o)

- 2af r & >) f i t x 0 ) ] + 0 ( t3 ) + 0 [ ( x i - x o)3 ] t 0 [ ( x j - x o)3]

S im i l a r l y ,

x4 ( t ) = e x p ( - t f 1)x 2

= x3 - t f j ( x 3) + £ a^ fx 3) f i ( x 3) + 0 ( t3 )

- *o + t? '2<*o) - ^ § j j r A f l<xo H *

0 ( t 3) + 0 (x i - x0 )3 + 0 (x2 - x0 )3 + 0(X3 - x0 ) 3

20

WUh t x 1 f 2 ■ f ! - t f l * f 2^ : L i e b r a c k e t o f f l * f 2

X4( t ) = x0 + t 2[ f j , + higher order terms ( 2 . 6)

Under the assumptions that the higher order terms in Eq. (2 .6 ) vanish, i t follows that points x0 and X4 in F ig. 2.1 coincide i f the Lie bracket of f ie ld s f j and f^ , denoted [ f i , f o ] , vanishes. Hence, the Lie bracket operation serves as a tool to id e n t i fy basis vector f ie ld s which span the tangent space for x ( t ) , Eq. ( 2 . 2 ) . Further references on d i f f e r e n t i a l geometry and concepts of nonlinear control theory are found in [118-124] .

In th is d issertat ion the above concepts are applied to mechanical systems. Crouch [125] used geometric control theory to analyze a r ig id one l ink system in three-dimensional space. Khosravi [4 ] analyzed the c o n t r o l l a b i l i t y of a planar l inkage system in the a i r and on the ground.

In what follows the notation of Marino and Cesareo [126] is used, who considered a nonlinear system

mx = f ( x ) + G(x)u = f ( x ) + I g-j (x) Ui (2 .7 )

i =1y = h(x)

where x e Rn u e Rm, and y e RT denote the s ta te , input, and output ofthe system. They define two basic d is t r ibut ions for the analysis ofstructural properties of Eq. (2 .7 ) :

" - the strongly accessible d is t r ib u t io n L0 , which is the smallest in vo lu t ive d is t r ib u t io n which contains

span { ad | g-j : 1 > 0 , 1 < i < m) "

where

fld g-j = g-j , adf g-j = [f-j g j ]

ad k+1 gi = adf (ad k g-j)

" - the accessible d is t r ib u t io n L which is the smallest invo lut ive d is t r ib u t io n which contains

span { f 1 g i gm) "

where L(x) = span { L0 ( x ) i f ( x ) ) .

Further de f in i t ions are:

21

G° = span { g i gm}

GJ = span {GJ-1 , [Gf , GJ'1] ) : Gf = f + G°

[G f , GJ-1 ] = span { [ x , y l : x e Gf, .y e GJ-1 }

M°(x) = G°(x)

MJ(x) = spanfMJ-^x) [ f , MJ-1] ^ ) }

The fol lowing proposition [126] states the in v o lu t i v i t y property ofsets G^(x) and Mk ( x ) :

"a. I f for some integer k, Gk(x) = Gk+1(x) fo r every x e V,

open subset in Rn, then G^(x) = G ^ ( x ) fo r any integer

1 > 0 and any x e V.

"b. I f , for some integer k, M^(x) = M^+1(x) fo r every x e V, open

subset in Rn, then M^fx) = M +^(x) fo r any integer 1 > 0 and

any x e V."

The next proposition [126] states the re la t ionship between Gk (x)and L0 ( x ) , which are given above:

L0 (x) = Gn_1(x) fo r every x e N, open and dense submanifold in Rn.

Let

na = sup dim L(x)

x e Rn

nsa = sup dim L0 (x)

x e Rn

nx = sup dim Mmax(x)

x e Rn

n = dim(x)

The the tuple (n, na , nsa, n^) gives a preliminary c la s s i f ic a t io n of the sytem, Eq. (2 .7 ) [126]:

I f n = na , the system is accessible.

I f n = na = nsa, the system is strongly accessible.

I f n > nsa, the quantity (n - nsa) gives the number of (strongly)nonaccessible d irect ions . I f n = n j , the system is feedback equivalent to a l in e a r contro l lab le system whose state space has dimension nsa.

22

Next some theorems of [127] are stated which allow the determination of the c o n t r o l l a b i l i t y properties of the mechanical systems considered here:

Theorem 1 [127, pg. 27]: Access ib i l i ty is a necessary but nots u f f ic ie n t condition for the c o n t r o l l a b i l i t y of the system, Eq. (2 .7 )

Theorem 2 [127, p. 61]: Assume the system, Eq. (2 .7 ) is accessiblewith unconstrained controls and f ( x ) is a Poisson stable vector f i e l d , i . e . , div ( f ( x ) ) = 0 [126, pg. 60 ] , Then the system is contro l lab le .

Theorem 3 [127, pg. 27]: The accessible system given by Eq. (2 .7 ) is strongly accessible i f there exists some time T, such that R ( t , x0 ) c Sr , and R(T, x0 ) is not contained in an analyt ic surface S$ of dimension s < r , where

R ( t , x0 ): time T reachable set from the i n i t i a l state x0Sr : ana ly t ic surface of dimension r

The existence of a reachable set R(T, x0 ) c Sx is interchangeably used with the c o n t r o l l a b i l i t y of the sytem on an r-dimensional manifold.

2.7 The Automation of the Nonlinear C o n t r o l l a b i l i t y Analysis

The concepts of nonlinear control theory are la rge ly applied to e i th e r scalar systems or to non-scalar systems of low dimensionali ty, which merely serve to i l l u s t r a t e the underlying pr inc ip les . The nonlinear c o n t r o l l a b i l i t y analysis of systems with many s ta tes , for example, rapid ly increases in complexity due to the character is t ics of the matrices involved. Even though there is no d i f f i c u l t y in p r in c ip le , the p robab i l i ty of making mistakes while performing tedious hand computations which require a large amount of time is very high. This problem can only be overcome with the help of a d ig i ta l computer which is able to execute a symbolic manipulation program.

The above mentioned programs, which can be used to symbolically derive the equations of motion of a system, see section 2 .3 .2 , are also applied for the evaluation of nonlinear c o n t r o l l a b i l i t y or observabi l i ty propert ies [9 6 , 126, 128]. Claude and Dufresne [128] use MACSYMA [84] to obtain decoupling of nonlinear systems by means of state-feeback. Marino and Cesareo [126] use REDUCE [81 ] to check feedback (or s tate) equivalence to l in e a r contro l lab le systems and to analyze the accessi b i l i t y and strong a c ce s s ib i l i ty properties of nonlinear systems, which are l in e a r in the controls. They provide some algorithms, which are w rit ten in pseudocode, and can, there fo re , be coded in any of the symbolic manipulation systems ava i lab le .

23

2.8 Summary

In th is chapter a number of previous works in the areas of kinematics, dynamics and control of m ult i - l inkage system are surveyed.A twenty-three segment human model for kinematic studies is introduced. The dynamics of m u l t i -1 inkage systems in the a i r and the symbolic generation of the equations of motion are discussed. Concepts in nonlinear control theory are reviewed. The mechanism of projection of m ult i - l inkage systems and the associated i n s t a b i l i t i e s are addressed.

24

Chapter 3

A GRAPHICS SOFTWARE PACKAGE FOR MOVEMENT STUDIES

3.1 Introduction

An important part of the study of dynamics of mechancal systems is the graphical representation of simulation resu lts , i . e . , the transformation and projection of the object onto a viewing screen.This provides for a visual v e r f ica t io n of simulation resu lts , which can, in tu rn , be presented to a wider, not necessarily technica l ly in c l ined , audience. Computer graphics provides the option of data reduction or even image enhancement for the purpose of focussing on features of the object being invest igated. In the study of animal or human movement [1 1 5 ] , i t might be desired to display a f r o n t , side, or top view of the object . The spec if ica t ion of an a rb i t ra ry viewing plane, however, provides the experimenter with more f l e x i b i l i t y . Computer graphics is extensively employed in CAD/CAM appl icat ion [3 - 141. I t can be used, for example, to display the working envelope of an assembly robot T i l l . Furthermore, i t can be employed to v e r i fy appl icat ion programs for robots by displaying the motion of the robotic mechanism [ 121.

In th is chapter a graphics software package is introduced which displays the model of a humanoid (ADAM) and an assembly robot as a set of wire frame segments. This package provides in te ra c t iv e and f i l e - driven input. The corresponding input parameters are the orientat ions of the individual segments of the composite system. This software package supports animation c a p a b i l i t i e s , as w e l l . In the present implementa t io n the user is able to display a cycl ic motion of the multi-body system at various speeds on the screen. The graphics software package consists of three main modules. The basic graphics module contains a number of elementary graphics operations, including c l ipp ing , hidden surface removal, and perspective transformation [1D3-1051. The second main module contains a set of procedures which compute the two- dimensional image of a 23-segment humanoid, see Fig. 2 .3 . I t uses the procedures of the former module as software p r im it ives . F in a l ly , the th i rd module contains a number of subprograms which ca lculate the two- dimensional image of the assembly robot, see Fig. 2 .4 . S im i la r ly , i t uses the routines of the basic graphics modules as software pr im it ives . The three major grapics software modules are l is te d in Appendix A l , A2, and A3. They are implemented on an IBM-PC (personal computer) using the language PASCAL. Some important information on how to use th is graphics package is provided in Appendix A4, which shows the HELP-document that is in s ta l le d in the computer system.

25

I t is pointed out that some of the algorithms in th is section were designed as projects for independent studies in the Department of E lectr ic a l Engineering, and were supervised by the author. These algorithms include the hidden surface removal routines and the procedures for animat io n .

The organization of th is chapter is as fol lows. In section 3 .2 , the structure of the software and functional blocks of the hardware are described. In section 3.3 and 3 .4 , software elements, which perform elementary graphical functions, are discussed. Section 3.5 describes kinematic manipulations and object data structues. Sections 3.6 and 3.7 focus on p lo t t in g of polyhedra, which are wire-basket solids representing the object 's segments, and on the implementation of animation capabi1i t i e s .

3.2 The Hardware Arrangement and Software Structure

3 .2 .1 Hardware Arrangement

The hardware arrangement for the graphics system is shown in Fig.3 .1 . The graphics software is general enough, so that the output can be displayed on both a raster-scan and a random-scan display. Consequently, the IBM-PC, which is the main processor in the present design, is in terfaced to both types of output devices. The random scan display is the vector graphics system, which consists of a graphics generator and a graphcis display. The graphics generator, which accepts input data in ser ia l form, is connected to the p ara l le l output of the IBM-PC via a special purpose in te r fa c e . This in ter face transmits ser ia l data at a high baud rate in order to achieve the transmission speed of a para l le l i nterface.

The raster-scan display in the above configurat ion is the control display of the IBM, which can be switched between normal mode, i . e . , te x t mode, and graphics mode. F in a l ly , the IBM-PC is connected to a PDP11, which drives an X-Y p l o t t e r . This serves to get a hard copy of the f in a l p icture in the fol lowing way: A plot f i l e which contains thedisplay code for the f ina l image, is sent to the PDP 11. The PDP 11 act ivates a module which transla tes the display code in equivalent commands for the X-Y p lo t t e r .

3 .2 .2 . Software Structure

An overview of the basic graphics software is provided in Fig.3 . 2 . , which indicates program dependencies as w e l l . The function of the modules, whose names are largely se l f -exp lanatory , is explained below.

26

M o n i I o r

I B M - P C

PDP 1 1 / 2 3

D i s p l a yG e n e r a t o r

Graphics

Plo I t e r

HV

I n t e r l a c e

Se r i a lI B M - P C

Figure 3 .1 . Hardware Arrangement for Graphics

27

GRAPH

InitPerspectiue

ClipEdgesin3Da4 0pPo!yC!ip3D

ClipEdêsin3DbCIPolyClip3D

UJarnock UJeiler-Rtherton

Utilities

Init^Graphics MoueTo LineTo

OpenSegmentCloseSegmentDeleteSegmentFindSegmentAppendSegmentPostSegmentUnPostSegment

GenTruncCone

Sphere

Pyramid

GenBar

AnimationCubSplinelnt

Figure 3 .2 . Dependence Tree of the Basic Graphics

Software Package: GRAPH

28

The branch of programs, which are dependent on In1t Perspective, does the c l ipping operations, removes hidden surfaces, and accomplishes the project ion onto the display area. The block, denoted U t i l i t y , provides the essentia l mathematical operations, l i k e matrix and f i l e operation and i n i t i a l i z a t i o n of kinematic transformation matrices. The above mentioned elementary routines are not l is te d by t h e i r indiv idual names. The branch of programs, which depends on the program I n i t Graphic, consists of a l l graphic pr im it ives [1 5 , 16}, which generate display code. The programs Gen TruncCone (generalized, truncated cone), sphere, pyramid, and GenRar (generalized bar) create a database of elementary geometric objects, represented by wire-basket so l ids . F i n a l l y , the programs dependent on animation contain a l l modules, which support the animation cap a b i l i t ie s of the graphics package. All programs shown in Fig. 3 .2 are l is te d in Appendix A.

3.3 Basic Graphics Software

3.3 .1 Three-dimensional Viewing Process

A block diagram i l l u s t r a t i n g the three-dimensional viewing process is shown in Fig. 3 .3 . An object which is to be displayed on the screen enters the viewing process at the top of Fig. 3 .3 . I t is ty p ic a l ly described by a mesh of polygons, in which each individual polygon is represented by a set of vert ices which are specif ied in world (or absolute) coordinates. These world coordinates are subject to a normalizing transformation, which consists of a world to eye space transformation, an aspect r a t io adjustment, and a transformation to window edge coordinates. The normalizing transformation is described in section 3 .3 . 1 . 1 . The transformed object is then clipped in three dimensional space, which is discussed in section 3 . 3 . 1 . 2 . The object , which is now in canonical view descrip t ion , is transformed in to screen space and i t s hidden l ines are removed. This is described in section 3 .4 . F i n a l ly , the transformed segment is subject to two dimensional image space ca lcu la t ions , a f t e r which i t is displayed on the screen [15 , 16}.

3 .3 .1 .1 Normalizing Transformation

The normalizing transformation consists of a world to eye space transformation, an aspect r a t io adjustment, and a transformation to window edge coordinates. The world to eye space transformation is explained below.

Consider Fig. 3.4a which shows a right-handed world coordinate system with axes Xw, Yw, and Zw and or ig in 0W. An observer is specif ied by a local vector EP (eye point) r e la t iv e to 0W. A center of in teres t (or center of project ion) is specif ied by a local vector COI r e la t i v e to

29

Output Prlm ltluet in Three-Dim. Coord.

Transformation To IJUindoui-Edge Coordinates

Rspect Ratio Rdjustm ent

World To Eye Space

Transform ation

Display

Clipping Rgalnst Canonical llleuf

Screen Space Transform ation

Hidden Line Removal

Screen Space To Uleuiport Transform ation

Norm alizingTransform ation

Two-Dimensional Image Calculations

Figure 3 .3 . Three-Dimensional Viewing Process

30

EP

en

COI

(a) World Space Coordinate System

ow

eu

E P

OOIcn

-► Z .

(b) Eye Space Coordinate System

Figure 3 .4 . World and Eye Space Coordinate Systems

31

Ow. The direct ion of view is given by the d i f ference vector COI-EP, i ts unit normal is denoted by Pen, the d i rec t ion pointing upwards r e la t iv e to the direct ion of view is cal led Peu. The above world space coordinate system is mapped into the eye coordinate system, which is shown in Fig. 3.4b, with axes Xe , Ye , and Ze . I t is a left -handed system since the projection of i t s axes X? and Ye onto an imaginary viewing plane is supposed to represent horizontal and v er t ica l extension of the image, while the coordinate Ze , which is pointing in to the plane, transmits the depth perception. In the eye coordinate system the observer position EP is at the o r ig in . The Ye axis is aligned with the vector Peu. S im i la r ly , the Ze axis is aligned with the vector Pen. The transformation from the world to the eye coordinate system consists of a sequence of mappings [15 - 1R], which are concatenated into a single matrix

(Xe \ Ye \ Ze \ 1 )T = N(XW, YW, ZW, 1) (3 .1 )

with ( Xe 1, Ye ' , Zg, 1) T : homogeneous coordinates of the eyecoordinate system, without aspect ra t io adjustment

(XW, Yy, Zy, 1)^: homogeneous world coordinates

C0 0 -S0 -C0 Ix C q + COI^ S q

“sc0 c o ix - c o i y c , + Colz s , c 0"c4)Se - s * "C<J>C0 COIxC^Sg + COIyS^ + C o ^ C ^ C q + d

0 0 0 1

COI - EP = (a , b, c )T : d i f fe rence vector of center of in terest

and eye point

d = a?- + b? + c^ : length of Col - EP

f = a^ +

C q = COSG - J

S q = s i n0 = j

C<t» = C0S<J> = 1

C bS<j) - sini}) - g-

32

The aspect r a t io adjustment compensates for the e f fec t that d is t in c t iv e v e r t ic a l and horizontal l ine resolutions have on the f in a l image. I t consists of a scaling which is represented by a matrix S s . t . :

with

> ze* 1) T S(xe

1 0 0 00 AR 0 00 0 1 n0 0 0 0

= S(xe 1, y ^ 1, ze , 1 )T (3 .2 )

AR: aspect ra t io = horizontal l ines v er t ica l l ines

The perspective transformation, which is considered next, is done in two steps. I n i t i a l l y , the vert ices of the segment, which are now in eye space coordinates are transformed into coordinates of a canonical view. In the l a t t e r form, the subsequent clipping algorithm is p a r t ic u la r ly stra ightforward to implement. In the second and f in a l step the vert ices of the segment are transformed into the screen space coordinates. The perspective transformation has to reach two goals. F i r s t l y , the f in a l image has to be v isua l ly r e a l i s t i c , i . e . , edges or polygons, which are f a r away from the observer have to appear smaller on the screen. Secondly, points in eye space are not simply projected onto a viewing plane but are transformed to i t . This transformation is such that the z depth information is preserved and that planes in eye space remain planes in screen space. This implies that 2 d is t in c t points in eye space, which possess the same ver t ica l and horizontal screen space coordinates, can be distinguished by t h e i r z-depth. Furthermore, the z-depth of an a rb i t ra ry point on a known polygon can be found by l inear in te rp o la t io n . The expressions for the perspective transformation are given below. F i r s t , the perspective projection is discussed, see Fig. 3.5a. In th is f igure a screen is shown, with height and width equal to a quanti ty S, which is located a distance D r e la t iv e to the or ig in of the eye coordinate system. The screen is para l le l to the Xe-Ye plane and has i t s own coordinate system with axes Xs and Ys. A point P with eye coordiantes (xe , yp, ze ) is projected onto the screen to point P ' , which has coordinates (xs , y s , D) in the eye coordinate system. The projection equations are found from Fig. 3.5b which shows the Xe-Ze plane of Fig. 3 .5a. A s im i la r f igure for the Ye-Ze plane can be obtained from Figure 3.5b by replacing Xe by Ye . I t is assumed that theobserver has a viewing angle dentoed by 0 i . e . , the observer cannot see

0 ftobjects which are beyond the l in e Xe = ± Ze tan ^ and Ye ± Ze tan ^

in Xe - Ze plane and Ye - Ze plane. From Fig. 3.5b:

33

a) eye coordinate system

b) Xe , Ze plane of the eye coordinate system

Figure 3 .5 . Visual Realism by Perspective Project ion

34

Normalizing the above equation with Xsmax = Dtan

(3 .3 )

xsmax zetan -p(3 .4 )

S im i la r ly , for the Ye - Ze plane

*s = Ye ysmax zetan |

The property that planes in the eye coordinate system, denoted by,

transform in to planes in the screen coordinate system, denoted by

is s a t is f ie d i f :

a' = a

b * = b

r , _ d(H - Y)c " HY tarie/2

_ c - d/H tan e /2

„ _ Y H Yz s - y ~T1T + (H - Y)ze

where

a, b, c, d: coe f f ic ien ts of the equation of a plane in thecoordinate system.

a ' , b ' , c ‘ , d *: coe f f ic ien ts of the equation of a plane in thescreen coordinate system.

(xs, ys» *s ) : screen coordinates.

H: location of the front clipping plane r e la t iv e to 0^.

Y: location of the back clipping plane r e la t iv e to 0^.

The front (h i th e r ) c l ipping plane is due to the fact that objects wich are too close to the observer cannot be displayed on the screen due to

axe - bye + cze + d = 0 (3 .6a)

a ' x s + b ' y s + c ' z s + d' = 0 (3 .6b)

35

i t s f i n i t e resolut ion. For the same argument a back (yon) c lipping plane is introduced, which eliminates a l l points which are too fa r away from the observer.

The coef f ic ien ts a, b, c, and d in (3 .6a) can be found using elementary l in e a r algebra [ 1 5 ] . They are related to the coeff ic ients a ' , b ' , c ' , and d1 of Eq. (3 .6b) [1 5 ] . Equations (3 .4 ) and (3 .6 ) can w r i t ten in to a single matrix P, such that:

(xs \ ys \ zs \ w's )T = p(xe , ye , ze , 1 )T ( 3. 7)

where (xs ' , y s ' , zs , w's )T: intermediate screen coordinates

1 0 0 00 1 0 00 0 06 P60

Y

0 6

P =

0

Y 11 HY - H 9 H - Y

The matrices N, S, and P of eq. ( 3 . 1 ) , Eq. ( 3 . 2 ) , and Eq. (3 .7 ) are concatenated into a single matrix Q, s . t . :

( xs '» ys'» zs'» ws ' ) T = Q(xw> yw» zw» ! ) T

with

Ce 0 -Se -COIx C0 + COIZS0-ARS^Sq C<j) ar "^CgAR (coixS^Se - coiyC^ + COIzS4)S0 )AR

Q = -C^SqciS - V 6 - c4,c0«6 (COIxC Sg + COIyS + COIzC(j)C0+d)a6 + p6-C*S06 - v -C*C06 (COlxC(j)S0 + COIyS + COI zC(j)C0+d) 6

3 .3 .1 .2 Three Dimensional Clipping

There are two issues of primary in teres t in clipping in three dimensional space. The f i r s t issue deals with the clipping of edges against the respective clipping planes. The second one deals with the appl icat ion of the above mentioned algorithm to the problem of clipping a closed polygon against a set of clipping planes.

36

3-D Edge Clipping

The three dimensional edge clipping is explained below.

Consider Fig. 3 .6 , which shows the truncated viewing pyramid in the eye coordinate system. From the discussion in the previous section i t follows that objects which are v is ib le to the observer are bounded by the planes

0 0- z e tan j < xe < ze tan j

- z e tan j < y e < z e tan -| ( 3 .R)

H < ze < y

where 0 is the viewing angle and H, Y specify the coordinates of theh i th e r and yon c l ipping plane.

The in tersect ion of a clipping plane with an edge can be calculatedusing elementary vector analysis as follows: consider a plane, which isspecif ied by

N • P + d = 0

where N is the plane's normal, d is the shortest distance of the plane to the o r ig in , and P is an a rb i t ra ry point. An edge is specified as

P = ? i + t ( P 2 - Pi) (3 .9 )

where Pi and P2 are i n i t i a l and terminal points of the l i n e , and P is again an a rb i t ra ry point on the l i n e , and t is a scalar var iab le . The value of t , which corresponds to the in tersect ion of the l ine with the plane is

1 = W \ (P2-P 1) ( 3 * ln)

The implementation of Eq. (3 .10) and the corresponding tests for edge tocl ipping plane in tersect ion [”17] produce an i n e f f i c i e n t algorithm. Theuse of homogeneous coordinate provides for more e f f i c i e n t c lipping routine [1 5 ] . Performing the m u lt ip l ica t io n in Eq. (3 .7 ) produces intermediate (homogeneous) screen coordinates.

37

Ye

Hither Plane

X Yon PlaneViewing Plane

Figure 3 .6 . Eye Space Coordinate System with Truncated

Viewing Pyramid

w .

- w

Z . - W ,

Figure 3 .7 . Window Eye Coordinates

38

xe

ye

y i f l tan | (ze - H)

ze tan |-

(3.11)

I t follows that the bounds specified in (3 .8 ) can be wri t ten as:

-ws < xs ' < ws

-ws ' < ys ' < ws (3.12)

0 < zs ' < ws

Consequently, an edge which v io lates the above inequa l i t ies is clipped. Using the quantit ies xs ' , y s ' , z ^ ' , and w^' as new coordinates introduces the window edge coordinates, Fig. 3 .7 . In th is new space the clipping planes have p a r t ic u la r ly simple equations. A clipping algorithm based on window edge coordinates is given in (151 and is implemented in the present software package.

Three Dimensional Polygon Clipping

The three-dimensional polygon clipping deals with the implementation of a clipping algorithm to c l ip the individual edges of a (closed) polygon. Consider Fig. 3 .8 , which shows a polygon mesh representing the front faces of a sphere. In F ig. 3.8a the individual edges of the mesh are subjected to a clipping algorithm, i . e . , edges which in tersect clipping planes are concsequently clipped and edges which are beyond those planes are discarded. I t is obvious that this method produces open polygons as output polygons i f a c lipping is performed. The e f fec t of th is clipping method is seen on the poles of the sphere in F ig .3 .8 a , i . e . , several edges of the polygon mesh appear to be missing. Consider now Fig. 3 .8b, which shows the same object as in Fig. 12a, using an algorithm which produces closed output polygons [ 1 9 ] . The edges, which are missing in Fig. 3 .8a , are indeed displayed. I t can be seen that the use of the l a t t e r algorithm produces a more r e a l i s t i c display image.

3.4 Hidden Surface Removal Algorithms

In the following section hidden surface removal algorithms are discussed. I t is distinguished between algorithm fo r convex polyhedra, i . e . , the corresponding in t e r io r angles are smaller than or equal to n, and

39

(a

Fig. 3 .8 . Clipping of a Polygonal Mesh

a) Open Polygon Clipping

b) Closed Polygon Clipping

40

concave polyhedra, i . e . , those, which do not f a l l in the above category. The hidden surface removal algorithm for convex polyhedra, which are described in section 3 .4 .1 are f a i r l y straightforward tc implement. The corresponding algorithm for concave polyhedra, which are discussed in section 3 .4 .2 are more complex ones.

3 .4 .1 Hidden Surface Removal Algorithm for Convex Polyhedra

I t is assumed in the following section that polyhedra have the data structure of the polygon l i s t format [16 , 18] . In th is format polyhedra are redundantly described by a set of ve r t ices , v j , . . . , vn, which consist of the coordinates of the vert ices in three-dimensional space, and a set of polygons p j , . . . , pm, where p-j ( i = 1 , . . . , m) consists of a l i s t of pointers to those vert ices which form the polygon's edges. The redundancy is introduced by the fact that edges which belong to several polygons are l i s te d more than once. (Hidden surfaces, i . e . , surfaces which are in v is ib le to the observer, can be removed by forming the dot product between the surface normal and the vector of the viewing d i rec t ion . The dot products of f ront and back face can be distinguished by t h e i r respective signs. These faces, for which the dot product vanishes, are back faces as well [ 1 5 ] . )

3 .4 .2 Hidden Surface Removal for Concave Polyhedra

The algorithms for hidden surface removal of concave polyhedra are described more in d e t a i l . In section 3 .4 .2 .1 standard te s ts , which are used in these algorithms are l i s t e d . In section 3 .4 .2 .2 a hidden l in e removal algorithm is described which is based on area subdivision [ 21] and operates in image space, i . e . , i t uses the object 's description in image (or screen) space as input data. The so-called Warnock algorithm explo i ts area coherence, i . e . , the tendency for small areas to be contained in at most a single polygon. Consequently, the algorithm recurs ive ly subdivides the i n i t i a l large display area into smaller subareas un t i l a correct decision on v i s i b i l i t y of i n v i s i b i l i t y of a polygon is reached. Another area subdivision algorithm is described in section 3 .4 . 2 . 3 . The so-called Weiler Atherton [22 ] algorithm explo its object area coherence, i . e . , i t recursively subdivides the or ig inal area along polygon boundaries un t i l a correct v i s i b i l i t y or i n v i s i b i l i t y decision is reached. The current implementation of th is algorithm dramatica l ly decreases the number of subdivisions as compared to the former algorithm. However, at each i te ra t io n stage, decisions are conducted which are more complex than the corresponding ones of the Warnock algorithm.

41

3 .4 . 2 . 1 . Decision Tests

In th is section decision tests [17] are l i s t e d , which are used in the currrent implementation of the Warnock r21] and Weiler Atherton [ 22] algorithm.

Intersect ion of two edges:

Two edges E and E ' , each described by two points

Pi = (x-j, y i ) , e E [ i = 1, 2]and

Pi ' = ( x - j \ y i ' ) e E' [ i = 1 , 2]

in tersect at point ( x j , y j ) i f :

min[min(xi , X2) , m in ( x i ' , X21) ] < xj < max[max(xi, X2) » m axfx j ' , X21) ]

and (3.13)

min[min(yl t y 2) , m in f y j ' , y2 ' ) ] < y i < maxCmaxfyj, y 2) , maxCyj', y 2 ' ) ]

Containment of a polygon in a rectangular area

A polygon P, which is specified by a tuple of i ts vertices P = (V j , V2» . . . Vm) with Vj = ( x j , y j ) , j e [ 1 , m] is contained in the rectangular area A with

A = ( *1 y I xmin < x < xmax and y min < y < ymax )

x i e Cxmin* xmax^ V jV (3.14)

y j e ^min* ^max J y j

Test for non overlap of two closed convex polygons in image space

Two polygons Pj and P2 , each specified by a tuple of i t s ver t ices ,s . t .

P1 = ( vl , l » v l ,2 • • •> vl,m)

p2 = ( v21 , l» v2 , 2 .............. v2»n)

Vi . j = ( xi * j i y i , j ) i e [ 1 , 2] j e [ 1 max(m, n ) ]are d is jo in t in image space i f the following minimax tests hold:

42

or

or

or

max[x it1l < min[x2f j1

max[x2>j ] < m i n [ x ^ j ]

maxHy i , i D < min[y2 j ]

max[y2 , j ] < m i n C y j j ]

(3.15)

i e [ 1 ,

j e [ 1 ,

m]

n l

Containment of one polygon in another polygon

Consider Fig. 3 .9 . A polygon Pj completely surrounds another polygon P2 , where Pj and P2 are as specif ied before, i f :

where V l , j is the local vector from the vertex V2 J to vertex

The implementation of the above tests in the hidden surface algorithm has to be such that decision tests for cases with a high p ro b ab i l i ty of occurrence have to be done pr ior to those tests for cases with a lower prob ab i l i ty of occurrence. For example, the tes t fo r complete containment, which is f a i r l y expensive, has to be done only, i f the the tests for d is jo in t polygons has fa i l e d . Furthermore, in computing edge in tersect ion , care has to be taken to avoid degenerate cases, which are shown in Fig. 3 .10 .

3 . 4 . 2 . 2 . Warnock Algorithm

A flowchart of the Warnock [211 algorithm is shown in Fig. 3 .11 .The algorithm, which is coded in PASCAL, recursively subdivides the o r ig ina l rectangular display area into four subareas of equal dimensions u n t i l a conclusion is reached on the f in a l display of the area element. The subareas can in the l i m i t be such that they e i the r represent a l ine segment or an individual pixel of the f in a l image. The recursion is terminated i f one of the four cases, which are shown at the bottom of Fig. 3 .11 , occurs. From l e f t to r ig h t , in the f i r s t case a l l polygons are d is jo in t with the area element. Consequently, only the background value of i t is displayed. In the second and th i rd case, only one polygon intersects or is completely contained in the area element.

m-1

J=i

43

m

U1,4

Figure 3 .9 . Test for Containment of One Polygon in Another Polygon

I n f i n i t e l y

P o in ts

D o u b l e P o i n t

M a n y

Figure 3 .10 . Degenerate Cases of Edge In tersection

44

( ^ S t o r t

Push I Init ial ly I

PushI recursively IPop

' ' Mo r e Than I J o I y .7,

I t l a a « T v>

Surrounde,Z - D e p t h

O v er la p?

Discard Polygoni Behind Surround.I n t e r s e c t i o n

u rroun da In Front,

Clip Polygon Rgalntt Bound.

“X o m pi a t a^ o n t a I n m a n S u r r o u n d e r

Uleiuport MapVi ewport Map Ulewport MapVi ewpor t Map

Dl tplay Backgr. * Contained

P ol ygo n

S 3

DltplayB a c k g r o u n d

Only

£ 3

Display Back- j r o u n d ' C o n t o l n e d

Part OF Polygon

Y M

DisplayS u r r o u n d e r

Only

Stack Cmpty ?

^^~”~0one

Figure 3 .11 . Flowchart of the Warnock Algorithm

45

( ^ ^ R e t u r n ^ ^ ) ( ^ ^ e t u r n ^ ^

Pop

CenterH, CenterV SideK, SideV,

Polygons : P(i)

Push ( initially )

CenterH, CenterV SideH, SideV,

Polygons : P(i)

P u s h :CenterH}; CenterV3; SldeHNeut; SldeVNeut

Polygont

Push:CenterH4; CenterV4; SldeHNeut; SldeVNeui

Polygont

P u s h :CenterH2; CenterV2; SldeHNeut; SldeVNeut

Polygont

Push ( recursively )

P u th :CenterH l; CenterV I; SldeHNeut,SldeVNeut

Polygont

C an ta rH 3 -C an ta rH 1 ;C a n t e r m - C a n te r H 2 C a n ta rV 3 -C a n ta rV I «Can ta rV ;C a n ta rV 4-C a n ta rV 3

C a n ta rH I» C a n ta rH /2 C a n l a r V I - C a n ta r V / 2

S ldaH Naut-S ldaH /2

;C a n ta rH 2 > C an ta r t i l» C a n ta rH;C a n ta rV 2 -C a n ta rV l

;S id a V N a iv S ld a V /2

Figure 3.11 (cont 'd)

46

Hence, the contained part of the polygon is displayed together with the background value of the rest of the area element. The fourth case is character ized by the fact that one polygon completely surrounds the area element. Consequently, only the color of the surrounder polygon is displayed. The decision te s ts , which are discussed in section 3 .4 .2 .1 are implemented according to the re la t iv e probab i l i ty of occurrence of the respective cas§. In the case of recursive subdivision the area element description and the p o te n t ia l ly contained or in tersecting polygons are pushed on the stack. The algorithm is terminated i f the stack is empty.

3 .4 .2 .3 Weiler-Atherton Algorithm

The discussion of the Weiler Atherton algorithm [223 consists of a description of the actual hidden surface algorithm and a descriptionof the special case of polygon clipping.

Hidden surface removal a lgori thm:

Consider the flowchart of the Weiler Atherton algorithm in Fig.3 .12 . I n i t i a l l y a l l polygons are roughly presorted according to t h e i r z-depth, to increase algorithm e f f ic ie n c y . For example, the polygons can be sorted according to the average z-depth of th e i r corresponding v ert ices . The polygon on top of the l i s t , which has the lowest approximate z-depth, is taken to be the c l ip polygon, see next section. The remaining polygons are put on the input l i s t , i . e . , they are subject polygons which are clipped agains the c l ip polygon. In what follows an inside l i s t and an outside l i s t are created. The inside l i s t contains a l l polygons or a l l fragments of polygons, which are inside the c l ip polygon. The outside l i s t contains a l l the remaining polygons or fragmented polygons. In case that the actual c l ip polygon is not the foremost polygon, the algorithm recursively subdivides the polygons of the inside l i s t into smaller fragments un t i l a z-depth test conclusively determines v i s i b i l i t y or i n v i s i b i l i t y of polygon fragments. Af ter th is the inside l i s t is displayed and the whole process is repeated for the. outside l i s t , i . e . , the polygon on top of the outside l i s t is taken to be the c l ip polygon and the rest of the outside l i s t is copied into the input l i s t . The algorithm stops i f the outside l i s t is exhausted.

Polygon clipping algorithm

A flowchart of the algorithm, which c l ips a subject polygon against a c l ip polygon, is shown in Fig. 3 .13 . In the clipping process the c l ip polygon remains unchanged while the subject polygon might be fragmented in to smaller polygons. An example, which i l lu s t r a te s the above clipping algori thm, is presented in Fig. 3 .14a, 3.14b, and 3.14c. Fig. 3.14a shows the c l ip polygon, specified by a tuple of vert ices Cj, C£, . . . ,Cg, and a subject polygon specified by vertices S^, S2 .............. Sg. The

47

( l a r i

o l g g o n t D l i j o l n l

- J i m h Clip P - J - " 'P u l On OUTIIOC L ie t

Taka Tha P o lyg o n On Top 01 Tha

l i t ! H i CLIP P o ly g o n . P u l The R a i l On Tha INPUT l i l t .

R ou g h ly ( o r l Tha l l t l 01 In p u t

P o ly g o n ! R eco rd ing To T ho lr

2 - D e p t h

POLYGON CLIPPING :

Clip INPUT L i l t Rg. CLIP P o ly g o n

Sol Up INSIDE*0UTSI0E L I I I I .

n

o l y g o n OTTond lng

■ ^ z - O a p l h L i —^

D l t p l a y INSIDE Or OUTSIDE l i l t

R am oua Rll P o l y g o n ! l a h l n d

CLIP P o ly g o n F rom Tha

INSIDE L i l t

U lo i u p o r t T r a n i T o r m a t l o nR e c u r i l o n

Clip INSIOE l i s t N g a ln i t O l l e n d e r . R e tu rn The

D e l u d i n g INSIOE And

OUTSIOE l i e to f l i M o d i f i e d

INSIOE L i l t .

N Take OUTSIDE Lie I Hi

The Neu) I n p u t L i l t .OUTSIOE

l l t l E m p ty

Figure 3 .12. Flowchart of the Weiler-Atherton Algorithm

48

k

Figure 3

P01V60N Ct IPPINf i

C om pu t e i r i l i r i i d i n n i • e t u ue e n

Sub jec t And Cl ip Polygon

*--------------------------

( m a r t i n l a n d l i o n * Po in t * Ai t a l i * U a r t l t a i I n t o Contour

List Of Cl ip And Sub jec t Polygon

Sal Up t u t 1 And t i n 7

S a l a d S t a r t i ng Point

Ge ne r a t e INS 101III!

N

t aka l t ( Point i n n o t 7 f t*

S t a r t i n g Po in t

S a n aOUISIO

r a t ei t u t

l a t u r n

13. Flowchart of the Polygon Clipping Algorithm

l l t l I Im p ly ?

'■ 'I e g a n a r a t f r ' Ca i a Of I d g a

i n t o r i B c l i o n j

49

f l a r i l o g I n l a r i a c l i o n

B a a c h a d T

B a l a r n

r o l l o u t Cl i p P o l y g o n Uar l OM C o n l o u i U n l i t

No m ! I n t a r i a c l l o n P o l n l

J u m p Back To S u b j a i t P o l y g o n

l a I N I I B I l l t l

r o l l oau S a b j a t l P o l y g o n U a r l au

C o n l o a r Un t i l N b m I I n l a r i a c l i o n P o l n l

Fig. 3 .13 . (cont 'd)

50

S t a r l i n g I n t a r c a c l i o n

Ra a c h a d 7

l a l a r n

B a n a r a l a t U T S I B I t i l l

f o l l o w S u b j a d P o l y g o n Uar l aM C o n t o u r t / n f If Man l

I n l a n a c l i o n P o l n l

f o l l o w Cl i p P o l y g o n U a r l a a C o n t o u r I n B a c k w a r d l i r a t l i o n Un t i l

N a n i I n l a r a a d i o n P o l n l

J u m p To Cl i p P o l y g o n

Fig . 3 .13 . (cont 'd)

51

s

Clip Polygon

Subject Polygon

Figure 3.14a. Polygon Clipping of a C l ip Polygon

Against a Subject Polygon

itohianPnlymeCtwnuTS>

k

Stop

h

Figure 3.14b. Generation of the Inside L is t from the Modified

Vertex L is t of C l ip and Subject Polygon

52

Clip Polygon Contour Subject Polygon Contour

Figure

Stan

•14c. Generation of the Outside L is t from the Modified

Vertex L is t of C l ip and Subject Polygon

53

c l ip and subject polygon intersect at points I j , . . . , I 4 . The clipping algorithm modifies the or ig inal vertex l i s t s of c l ip and subject polygon by insert ing the in tersection points as fa lse vert ices in the appropriate places in the two l i s t s . The algorithm generates two addit ional l i s t s . The f i r s t l i s t contains a l l those in tersect ion points at which the c l ip polygon border passes to the l e f t of the subject

polygon border. I f S = S-j, S-j+j and C = C j , Cj+i denote edges of subject and c l ip polygon, which are directed in clockwise sense while t ravers ing the corresponding polygon, then the f i r s t l i s t is characterized by: S x C > 0.

The remaining in tersect ion points, which do not s a t is fy the above condit ion, are put on the second l i s t . After th is the inside and outside l i s t are created as i l lu s t r a t e d in Fig. 3.14b and Fig. 3 .14c.

3.5 Object Manipulation and Data Structures

In th is section the data formats at the d i f fe r e n t stages of object manipulation are described. Consider the viewing pipelines in F ig. 3.15 through which objects have to pass in order to be displayed on the screen. The kinemtic operations which are applied to the object's master f i l e s to produce the corresponding world f i l e s are described in section 3 .5 .1 . The data structures of the objects which pass through the hidden surface removal algorithms are discussed in section 3 .5 .2 .The data structure for performing animation is described in section 3 .5 .3 .

3 .5 .1 Kinematic Operations and Data Structures

In the present graphic software package a body segment, which is modelled by a convex polyhedron, is completely specified by i ts kinematic parameters and by a l i s t of the vert ices of the polyhedron.

The kinematic parameters are considered below. Consider Fig. 2 .3 which shows the i^h body segment in a body coordinte system with axes Xi i , X2 J . X j j , r e la t iv e to a world coordinate system with axes X^, Y^, Zyj.' The’ or ig in of the i^h body is denoted by 0-j. The position vectors K-j and Lj point to the hinge point with the ( i + l ) th body and the ( i - l th body r e la t iv e to 0^. The or ienta t ion of the i^h body is specif ied by Bryant angles [3 9 ] .

In the current implementation the kinematic information of the i t h body, which consists of i ts o r ie n ta t io n , the posit ion vectors to the hinge points with adjacent segments and the locat ion of the or ig in 0-j, is stored as a pointer structure ( in PASCAL).

54

The vert ices of the polyhedron are stored in polygon l i s t format [103, 106]. In th is format each polygonal face of the polyhedron is redundantly described by a set of vertex pointers, which point to the coordinates of the respective ver t ices . The above mentioned polygon l i s t of a body segment is denoted a master f i l e , see Fig. 3 .15 . I t is transformed to a world f i l e by applying the respective kinematic transformation to which i t is subjected.

3 .5 .2 Implementation of the Hidden Surface Removal Algorithms and Their Data Structures

In the current implementation of the hidden surface algorithms d i f fe r e n t data structures are used to optimize algorithm e f f ic ie n c y .The hidden surface algorithm for convex polyhedra is applied to the world (or object) space description of the respective polyhedron, which is the polygon l i s t format, since th is algorithm operates in object space, see Fig. 3 .15 . The concave polyhedra, which are studied in th is th e s is , are composed of a set of convex polyhedra. Hence, the above object space algorithm is applied to the indiv idual convex polyhedra to e l im inate a l l t o t a l l y in v is ib le surfaces in a presorting phase. The indiv idual polyhedra are then entered to the clipping algorithm in three-dimensional space and transformed in to image space. In the f in a l phase e i th e r the Warnock or the Weiler Atherton algorithm can be appl ied, since both procedures operate in image space. I f the Warnock algorithm is used a l l individual polyhedra, which are stored in d i f fe r e n t polygon l i s t s , are copied in to a single polygon l i s t and entered to the hidden surface algorithm. Hence, the data structures e s s e n t ia l ly remain unchanged.

I f the Weiler-Atherton algorithm is used the description of the indiv idual polyhedra is changed from polygon l i s t format to a matrix interconnection structures, in which the matrix elements represent vert ices of the polyhedra's polygon mesh. The l a t t e r form is a non-redundant surface descript ion. I t increases the object 's area coherence, since the boundary of polygon mesh serves as e i th e r c l ip or. subject polygon, while in t e r io r polygons render the surface's tex ture .In f a c t , the change from polygon l i s t to matrix interconnection format reduces the number of subdivisions, which are necessary to compute the f in a l d isplay. However, during each i t e r a t io n stage, more intermediate calcula t ions are necessary since a possible fragmentation of the subject polygon includes a recalculat ion of the surface 's texture as w e l l .

The implementation of the Warnock and Weiler-Atherton algorithm as a recursive procedure requires a considerable amount of memory space, i f the f ina l image is very complex. There are two ways of reducing the actual memory space of the algorithm. I f the f in a l image contains many objects which are evenly scattered on the screen area, then the screen is subdivided in to smaller subareas, each of which is treated as a

55

The object ii in matter coordinates in polygon lit! format

The object it in world coordinates in polygon list format

Removal of Potentially Invisible Surfaces

the object it in > image coordinates

in polypm list in nutria imbtfonncclion format \

the object is in screen coordinates in segmented display files

Open Polygon Clipping

Redundant Display Code Generation

Hidden Surface Removal Weilcr - Atherton Algorithm

Closed Polygon Clipping (Suthcrland-Hodgcman a lg .)

Efficient Display Code Generation

Hidden Surface Removal Wamock Algorithm

Generation of M u te r Hies

Kinematic Operations

Body Segment Description

Display o f the Object

Figure 3 .15 . Viewing Pipelines and Object Data Formats

56

separate hidden surface problem. However, i f the objects are concentrated on a pa r t ic u la r area of the screen and are evenly scattered in ( z - ) depth, the above approach is not necessarily e f fe c t iv e . In th is case the rectangular image volume is subdivided in to several slabs along the z -ax is . Each of the slabs is now treated as a d i f fe re n t hidden surface problem. After th is , the solutions of the intermediate hidden surface problems are used as inputs to the actual hidden surface problem.

3 .5 .3 The Data Structure for Animation Studies

Consider Fig. 3 .15 . At the end of the viewing pipeline segmented display f i l e s are created which contain the display code for the ob ject 's image. In the present graphics environment, see section 2 .1 , a to t a l of 64 display f i l e s are permitted. A rough estimate of the data transmission speed from the computer to the graphics generator suggests tha t for animation purposes a l l display f i l e s have to be loaded into the memory of the graphics generator, i . e . , i t is not feas ib le to in termediate ly store data on the hard disk of the computer and t ransfer i t to the graphics generator while performing animation. Therefore, the organization of these display f i l e s is such that a l l s tationary segments of an object are stored in a single display f i l e . This leaves a to ta l of 63 display f i l e s for specifying intermediate positions of a movement cycle. The e f fec t of a continuous, per iodic motion is created by sequent ia l ly blanking one display f i l e while unblanking the remaining nonstationary ones.

3.6 Convex Polyhedra

Generalized Truncated Cone:Consider Fig. 3 .16 , which shows a generalized, truncated cone

in a body reference system with axes X, Y, Z. The top and bottom surface of i t are e l l ipses with major axes atop, btop, and abottom, bbottom. The program allows surface d e ta i ls of the side of the cone to be represented by a polygonal mesh with an a rb i t r a ry number of horizontal and ver t ica l subdivisions.

Sphere:Consider Fig. 3.16b, which shows a sphere. I t is specified by i ts

radius r . I t s surface de ta i ls are represented by a polygonal mesh with an a rb i t ra ry number of ve r t ica l and horizontal subdivisions.

Generalized Bar:Consider Fig. 3 .16c, which shows the generalized bar.

57

(b)

Figure 3 .16 . Convex Polyhedra: a) Generalized, Truncated Cone;

b) Sphere; c) Generalized Bar

58

3.7 Concave Polyhedra

In what follows concave polyhedra are shown, which are composed of a set of convex polyhedra. A model of a humanoid, called ADAM, is described in section 3 .7 .1 . The model of an assembly robot is presented in section 3 .7 .2 .

3 .7 .1 Humanoid: ADAM

3 .7 .1 .1 Program and Data Structures

The data, which is necessary to display the humanoid as a wire gridmodel, see Fig. 2 .1 , is separated into kinematic information andbody segment descript ion.

The kinematic information is stored in pointer structures which contain a record of the following form in PASCAL:

Type pointer = * node; node = record

name: s t r ing ;a-abs, a - r e l : array;1 , k, 1- r i g h t , k - r ig h t ,1-abs, k-abs, 1- r ig h t -a b s , 1- le f t - a b s : array;theta: arrayup, down, l e f t , r igh t : pointerend;

where

name: name of the body segment

a-abs, a - r e l : absolute and r e la t iv e or ienta t ion of the segment

1 , . . . , l . l e f t - j b s : absolute and r e la t iv e position vectors tohinge points with adjacent segments

theta : or ientat ion of the body segment

up r igh t : points to the upper, lower, l e f t and r ight record

A fter i n i t i a l i z a t i o n the pointer structures have the configurat ion, which is shown in Fig. 3 .17 . The lower torso (L0T0RS0), i . e . , the hip, is chosen to be the reference segment. Then the individual segments exh ib i t the nested t ree structure , which is shown in Fig. 3 .18 . Note, however, that the branch from L0T0RS0 to head is traversed in the opposite d irect ion as compared to the remaining branches. Consider now Fig. 3 .19 , which shows an exploded view of ADAM. I t shows the body (or master) coordinate systems of the individual segments and the respective posit ion vectors which point to the hinge points with adjacent l inks .

59

Urarm Rlb1 Ularm

Lrarm LlarmRlb2

Rpalm Rlb3 Lpal

Rlb4

Lolorao

Ullag

LfootRfoot

Rloas

Record Structure :

Figure 3 .17. Pointer Structures fo r Body Segment Description

Nome of the RecordRight

PointerUp

PointerDown

PointerLott

Pointer

60

Lotorso Ulleg

Llleg ^— Lfoot

4L_Ltoes

Urleg4l_ Lrleg

^— RfootRtoes

Rib4t - Rib3

±-RIb2Ribl

Uptorso — Ularm

^— H armLpalm

L fingers

— Urarmt — Lrarm

t —Rpalm^— Rfingers

— Head

Figure 3 .18 . Diagram of the Tree Structure od ADAM's Segments

61

1

URARM

LRARM

RPALM

RFINGERS

URLEG

LRLEG

RFOOT

RTOES

HEAD

UPTORSO

RIB-j

r i b 2

RIB3

r i b 4

LOTORSO

ULLEG

LLLEG

LFOOT

LTOES

URARM

LRARM

LPALM

LFINGERS

Figure 3 .19 . Coordinate Systems of Body Segments and

Defin i t ions of Position Vectors

62

In terflctiuelnp*--------- fldam lnit

MasterFiles

Readflnglest ____

SetflnglesRecTreeUJalker

* UJorldFilesDisplayFiles

-PrintRecords

FindPointer

Figure 3 .20 . Program Dependencies in Software Module ADAM

63

All segments except LOTORSO and UPTORSO are connected to at most two adjacent segments; i t su f f ices , therefore , to specify the position vectors L-j, K-j or both and , see section 3 .4 .2 . The segments LOTORSO and UPTORSO are connected to more than two adjacent segments; hence, i t is necessary to introduce additional position vectors, denoted L-LEFT, AND L-RIGHT, which specify the segments' interconnections.

As pointed out e a r l i e r , the body segment descript ion, which contains the boundary polygon and the surface de ta i ls of the polygonal mesh, which represents the body segments, are stored in e i the r polygon l i s t format or in matrix interconnection form, section 3 .4 .3 .

Various programs that are necessary to compute the wire grid model of ADAM, Fig. 2 .1 , are shown in F ig. 3 .20. Note that the basic graphic routines are not l is te d here.

3 .7 .1 .2 Kinematics

The kinematics of the humanoid are explained below. The absolute locations of the respective origins are calculated f i r s t . The or ig in of the reference segment is set to zero:

°L0T0RS0 = (°» °» 0)

In the branch from LOTORSO to HEAD the or ig in 0-j of the i^h segment r e la t iv e to the previous segment, which id pointed out by pointer DOWN is :

° i = °D0WN + KqOWN “ Li 1 e [LOTORSO, RR1B4, . . . , P1B1, UPTORSO,HEAD] (3 .18)

S im i la r ly in the remaining branches, the or ig in 0 j of the j t h segment r e la t iv e to the previous segment which is pointed at by pointer UP is :

Oj = 0UP + LUP - Kj j * (3.19)

The spec if icat ion of the or ienta t ion of the individual body segment is considered next. The re la t iv e or ientat ion specif ies the or ientat ion of the actual segment re la t iv e to the adjacent segment which is closer to the LOTORSO, while t raversing the interconnection t r e e . The absolute or ien ta t io n of a body segment is the or ienta t ion of i t r e la t iv e to the LOTORSO. Referring to Fig. 3.19 the absolute or ienta t ion A-ABS-j of a segment is

A _ AbS-j A-Relk

K e P

(3 .20)

64

Figure 3 .21. Kinematic Study of a Human Long Jump.

65

&

Figure 3 .21 . ( c o n t . )

66

where the set P contains a l l body segments which are encountered while t ravers ing the interconnection t ree from the actual segment to the LOTORSO.'

The above formulas are implemented in a program called World F i le s , see Fig. 3 .2 .1 . This program is called by a routine denoted recursive t r e e walker (RecTree Walker), which traverses the directed graph,Fig. 3.18 in the correct d i rec t ion .

3 .7 .1 .3 Animation

The graphics package supports animation c a p a b i l i t ie s as w e l l . This is done in the fol lowing way. The program produces a database of up to 64 in term idia te positions of the object under consideration. The display f i l e s , which contain the above mentioned posit ions, are loaded into the graphics generator, see section 3 .2 .1 , and are sequentia l ly blanked or unblanked to create a movie- l ike image. The user has the choice of creating the animation database by e i th e r l in e a r in terpo la t ion or by cubic-spl ine in te rpo la t ion . In the former one the user has to input the i n i t i a l and terminal posture of the object . In the l a t t e r type of in te rpo la t ion the user has to specify several intermediate (or p ivot) postures of the object . The user can inf luence the speed of the moviel i k e image with the speed of blanking and unblanking of the display f i l e s . An animation study with the humanoid, which does a long jump, is shown in Fig. 3 .21 . The postures of humanoid shown in th is f igure represent the pivot positions for the cubic spline in terpo la t ion . Information on how to operate the program is provided in Appendix A3.

3 .8 Summa ry

In th is chapter a p a r t ic u la r hardware configuration and a software structure for the kinematic analysis of m ult i - l inkage system are described. For that purpose a computer graphics program is developed, which accepts the kinematic information of the m u l t i - 1inkage system as input parameters, solves the associated hidden surface problem, and does a transformation from three-dimensional world space to a two-dimensional image space. Although the graphics package consists of a set of elementary computer graphics functions, such as perspective transformations, open and closed polygon c l ip p in g , i t contains advanced algorithms, which perform hidden surface removal, as w e l l . The graphics package is structured such that the objects are displayed on a vector display but can also be shown on a raster scan display in case of a f a i lu r e of the f i r s t mentioned output device. Major contr ibutions of th is study in the area of computer graphics are the implementation and optimization of the above mentioned algorithms in a minicomputer environment and the d e f in i t io n of p a r t ic u la r data structure which accomplishes th is task. F in a l ly , an animation study for the analysis of the human long jump is shown.

68

CHAPTER IV

CONTROLLABILITY ANALYSIS OF MECHANICAL SYSTEMS

4.1 Introduction

This chapter is focused on the generation of the equations of motion and the nonlinear c o n t r o l l a b i l i t y analysis of mechanical systems using symbolic algebraic manipulation (SAM) language, which is called MACSYMA. Special emphasis is placed on the der ivat ion of the rotat ional equations of motion of systems of connected r ig id bodies. Advantages and disadvantages of SAM systems are discussed below:

The use of symbolic manipulation systems for the generation of the dynamics of mechanical systems [89-951 and for the subsequent appl icat ion to nonlinear c o n t r o l l a b i l i t y studies [126, 128] is well understood. High level symbolic manipulation programs l i k e MACSYMA [100] or REDUCE [97 ] are especia l ly suited for the above mentioned problems. Their s u i t a b i l i t y re l ies on d irec t commands for the fol lowing operations:

- a r i thmet ic matrix operations l i k e matrix addit ion , m u l t i p l i cat ion , computation of the inverse and the determinant, rank te s t and orthogonalization methods, where the matrix entries are allowed to be rat ional polynomials,

- d i f f e r e n t ia l calculus,

- pattern matching c a p a b i l i t ie s for the s im p l i f ica t ion of tr igonometric expressions, for example,

- d e f in i t io n of user defined algebraic ru les , which are useful, for example, for mult ip ly ing matrices whose entr ies are themselves matrices.

The potential advantage of symbolic manipulation programs is th e i r c a p a b i l i ty of performing large numbers of algebraic operations in f i n i t e t ime. These operations can also be done by hand but the p robab i l i ty of making mistakes while performing these ca lcu la t ions , which also require a large amount of t ime, is f a i r l y high. However, i t should be pointed out here that cert ian checks have to be performed to ve r i fy the correc t ness of the results which are obtained from a SAM system. I f , for

69

example, the dynamics of a mechanical system are derived symbolically [89 -96 ] such a check can be done using both a Newton-Euler approach and a Lagrangian mechanics method, or i f the c o n t r o l l a b i l i t y of a constrained system is analyzed, an upper bound could be provided for the dimension of the control lable subspace.

The special type of mechanical system, whose rotat ional dynamics are investigated here, are described next. The movements of these multi-body systems are specified by the dynamics of the center of mass of the composite system and by a set of rotat ional equations of motion [3 2 , 39] . The decoupled form of a system description is useful in the study of spacecraft dynamics [64-71] or in analyzing the motion of animals or humans under free f a l l conditions [7 2 -7 9 ] . In these studies the motion of the system's center of mass is influenced by gravity forces alone so that the result ing t ra je c to ry is a function of the corresponding i n i t i a l conditions only. Hence, the dynamics of the center of mass are those of the point mass, which is an elementary problem in physics and is not fur ther pursued here.

The inverse a t t i tu d e control problem is defined for a planar, two- l i n k system and is solved by transforming the corresponding i l l -posed boundary value problem into a well-posed one under suitable assumptions. D ig i ta l computer simulations are performed to demonstrate that l in e a r s ta te feedback ensures that the nonlinear two l ink system is capable of tracking a prespecif ied reference t r a je c to ry . I t is also shown that the above mentioned, p a r t i a l l y contro l lab le system does not exhib i t numerical in s t a b i l i t i e s during simulations i f the constra int , to which the system is subjected is a l in e a r function of the sta te variables of the system.

The organization of th is chapter is as fo l lows. In section 4.2 the symbolic generation of the rotat ional equations of motion are discussed using two d i f fe re n t approaches. In section 4.3 nonlinear control concepts, which are applicable to nonlinear systems with l in e a r inputs, are outl ined. The automation fo the nonlinear c o n t r o l la b i l i t y analysis is treated in section 4 .4 .

4 .2 The Symbolic Generation of the Rotational Equations of Motion and the Computation of Their f i r s t In tegrals

4 .2 .1 The Rotation Equations of Motion for N Rigid Links

In the fol lowing sections two methods are presented for deriving the rotat ional equations of motion for a system of N r ig id bodies (F ig . 4 . 1 ) . Fig . 4.1 shows N r ig id bodies connected together at ideal ball -and-socket jo in t s , which impose holonomic or simple nonholonomic connection constraints. Each body i ( i = 1, . . . , N) has associated with i t a body i coordinate system B-jCS, which is aligned with the principal axes of body i and or ig inates in the center of mass ( CM-j) of body i .The local vectors Kj and Li describe i n i t i a l and terminal connection

70

COM

ICS

YX

Figure 4 .1 . A Collect ion of N Rigid, Interconnected Links and Their

Respective Body Coordinate Systems and the In e r t ia l

Coordinate System.

71

point of l ink i re la t iv e to CM-j. An in e r t ia l coordinate system ICS, r e la t iv e to which the N bodies move, is assumed. The axes of B-jCS are denoted { X-j, Y^, Z-j} and those of the ICS are (X, Y, Z ) . The maps A-j

Af : B-jCS ■*> ICS

and B-j,

B-j : Bn'CS ■> ICS

map position and angular ve loc ity vectors from the respective body to the in e r t i a l coordinate system. Bryant angles [ 3 9 ] are chosen for the maps A-j and B-j. Sections 3 . 2 . 1 and 3 . 2 . 2 describe the two method of deriv ing the rotat ional equations of motion for the system, shown in Fig. 4 . 1 .

4 .2 .2 The Derivat ion of the Rotation Equations of Motion Using the Newton-Euler Method and the Decoupling Transformation

In the f i r s t method of deriving the rotat ion equations of motion,a free body analysis is done separately for each of the N r ig id bodies[34] to a r r ive at Eq. ( 2 . 1 ) . Eq. (2 .1 ) can be compactly wr i t ten in vector-matrix form as:

Ml • Z = U2 T + U3 + U4 M (4 .1 )

where

Z = ( X j , . . . , Xp, Wj, . . . , Wp)

r - ( r j , . . . , Tp_ i )

M = (Mi, . . . , Mn_ i)

The other quanti t ies of Eq. (4 .1 ) are specif ied in Appendix B l , TableBl . Constraint equations, which express the holonomic connection constraints of the N-bodies of Fig. 4 .1 , are d i f fe r e n t ia te d t ia te d twice to a r r ive at:

Z = U5 Y + U6 (4 .2 )

with

Y = (X, Wi Wn)

A general expression for the matrices U5 and Ug is given in Appendix B l , Table 8 .2 . Eq. (4 .2 ) is used to project Eq. (4 .1 ) with base vector Z to a reduced space with base vector Y and to decouple rotat ional and t rans la t iona l motion [3 2 ] , The equations of the new, reduced system are:

72

Vj Y = V2 + v3 + V4 + v5 Mwith

(4 .3)

Vi = U5t Ui U5

V2 = Uj UgV3 = U5t U3 = 0 !!

v4 = u5t u2v 5 - u5t u4

where t denotes the transpose operation. A general expression fo r the elements of equ. (4 .3 ) is given in Appendix B2, Table 8 . 6 . Equations ( 4 . 1 ) , ( 4 . 2 ) , and (4 .3 ) are programmed in the symbolic language MACSYMA and evaluated for the case of three l inks in three-dimensional space.The corresponding program l i s t i n g is shown in Appendix B l , Table 8.3 A fter the execution of the program the resu lts , which consist of the unconstrained (Equ. ( 4 . 1 ) , the projected equations of motion (equ.( 4 . . 3 ) ) , and the decoupling transformation (equ. ( 4 . 2 ) ) are displayed in Appendix B l , Tables 8.4 and 8 .5 .

4 .2 .3 The Derivation of the Rotational Equation of Motion from the Interconnection Diagram

In the second method of deriving the rotat ional equations ofmotion the resu lts , which were published by Braume [72] in 1906 andrediscovered independently by Hooker [ 6 4 ] , and Roberson and Wittenburg [ 3 9 ] , are d i re c t ly applied to the system of Fig. 4 .1 . This method f i r s t proves the v a l id i t y of the general expression of Eq. (4 .1 ) and of the transformed Eq. ( 4 . 3 ) . Second, the method provides a straightforward way to derive the elements of Eq. (4 .3 ) without performing the p ro jec t io n , as outlined before. In what follows the notation of [5 1 ] , is used, which is i l lu s t r a te d below on the example of a three l ink system, see Fig. 4 .2 . Fig. 4 .2 shows a th re e - l in k interconnected system with centers of mass CM-j ( i = 1 ,2 ,3 ) and the corresponding connection .barycenters CB-j ( i = 1 ,2 ,3 ) [3 9 ,6 4 ] . The barycenter CBj is the center ofmass of the augmented body i , which is formed by assuming that the masses of the bodies connected to i t are concentrated in the respectiveinterconnection jo in t s . Hooker [64] fu r th e r defines a set S whichcontains a l l unique labels of the bodies and a set F^ which contains a ll unique lables of jo in t s , which are connected to body X ( \ = 1, 2 , 3 ) . F in a l ly ?Xj .1? the set of labels of those bodies which are connected to body X via jo in t j e i th e r d i re c t ly or via a chain of intermediate bodies. For the three l ink system of Fig. 4 .2 . S, F and S y ( x = 1 ,2 ,3 ; j = 2, 3) are

S = ( 1 , 2 , 3}

73

body 2

body 1

Figure 4 .2 . A Three-Link Interconnected System with Centers of

Mass CM-j ( i = 1 ,2 ,3 ) and Connection-Barycenters

CB-j (i = 1 ,2 ,3 ) .

74

Fi = {2} , F2 = {2 , 3} , F3 = {3}

S12 = ( 2 , 3} , S13 = {3}

s22 = U> • S23 = {3}

s32 = { ! ) . S33 = (1 . 2}

The vectors OL^j are vectors from CMj to jo in t j . Furthermore,

01 j = 0L\p for any p e S^j , i . e . ,

0 l 12 = 0 l 13 ; o l 32 = o l 31

The vectors and describe the center of mass of body \ , CM , and the hinge point of body \ to j o i n t j r e la t iv e to the barycenter of body X.

‘ - S' I ^ 0LX|1 (4 .4 )P*X

° \P = h + 0L\p n

with m = I mp (4 .5)p=l

The main result of [64 ] is the set of rotat ional equations of motion:

io\ + I D\p x DpX x m x p#X

O)P

with

a) x ^ x m x (ay x D^\)p*X

+ TX + l r TX j H + M F ^ I x F^ (4 .6 )je F X ^ \ = 1, . . . , n

♦ \ * = 4>X + (i"x D \2 + X iH|i ° 2\ p ) Ip*X

" K D\ DX + I mP DXp Dxp) p*X

moment of in e r t ia of body \

4>X*: moment of in e r t ia of augmented body \

o) : angular ve loc ity of body \

75

T \ : external torque on body \ (T^ = 0)

F^: external force on body \ (F^ = 0)

j H ; constraint torque on body \

N: number of l inks

The rotat ional equations of motion, which are specif ied for each in d iv i dual l ink \ e [1 , n] in Eq. ( 4 . 6 ) , can be more compactly w r i t ten in vector matrix form, see Eq. ( 4 . 3 ) . The elements of Eq. (4 .3 ) are specif ie d in Appendix B2, Table 8 .6 . Equation (4 .3 ) is programmed in MACSYMA fo r the case of three (N=3) l in k s . The program l i s t i n g is shown in Appendix B2, Table 8.7 and the resu lts , which are obtained by executing the above program on a DEC 20 computer, are shown in Appendix B2, Table 8 .8 . I t can be shown that the two approaches of computing -trie rotat ional dynamics for three l inks in three-dimensional space produce the same resu l ts , see Appendix B2. This strongly supports the v a l id i t y of the rotat ional equations of motion for N = 3.

76

4 .2 .4 F i rs t In tegrals of the Rotational Equations of Motion

In this section i t is shown that the angular momentum constraint is im p l ic i t in the rotat ional equations of motion, Eq. ( 4 . 6 ) . Consider Eq. (4 .3 ) whose rotational part is rewrit ten as a set of f i r s t orderd i f f e r e n t ia l equations in a common reference frame, the ICS:

vjW = V2 + V4 + V5 M (4 .7 )

where

W = (W! Wn) t

Vj ( i = 1, . . . , 5 ) : the portion of the augmented matrices V-j ( i = 1, . . . , 5 ) , which correspond to rotat ional motion.

A f i r s t integral of Eq. (4 .7 ) is a scalar function h(e , w, t ) ,such that whenever the quanti t ies 0 with e = ( 0j 0n)t and Ws a t is fy Eq. ( 4 . 7 ) , then h (0 , W, t ) is equal to a constant.

Lemma: The f i r s t integral of Eq. (4 .3 ) is a vector function H(0, W, t )in the in e r t ia l coordinate system ICS.

N NH(0, w, t ) = I {A j I-j-j Wj + I (A-j DD-j j AjT Aj DD j -j W j ) }

1=1 j = l

Nwith I-j-j = J-j - m jDD j2 J mj DD2j j (4 .8 )

j = l

Proof: I t is shown below tha t the time der iva t ive of H(0, w, t ) isequal to the expression, which is obtained by adding the individual

d i f f e r e n t ia l equations of Eq. (4 .7 ) in the ICS. The quantity H i s :

NH(0, u , T) = I (Aj WW-j I-j-j W-j + Aj I j j W-j)

i = l

N N+ I I m {A j WWj DD-j j A jTA jD D j j W j - A j DDj j WW-j Aj T AjDD j j Wj

i = l j = lj *1

- Aj DDjjAjTAjWWj2Dj j + Aj DDj j Aj T DDj j Wj >

77

The sum of the equations of motion, Eq. (4 .7 ) is :

NH(0, w, T) = I (Ai WW-j I i i Wi + Ai I i i Wi)

i =1

N N+ m I I (Ai DD -j j AiTAj DDji Wj - AiDDi j Ai TAj wwj 2D j i )

i = l j = l

An inspection of the above equations reveals that H (9 ,w , t ) = H i (e ,w , t ) i f th e i r d i f ference vanishes

N NA = ft - H i = I I m(Ai(WWi DD-j j - DDijWWj)AiTAjDDji Wj} = 0

i =1 j = lj *1 (4 .9 )

For the purpose of s impli fy ing the above proof l e t

Aj WWj = WWj*

Ai DDj j = DD*j j

Ai Wj = Wi* ( i , j = l , . . . N)

such that :

N NA = & - Hi = J I m {WWi* DD*ij - DD*ij WWn**)DD*j i Wj*}

i = l j = l

(4 .10)

Furthermore, the crossproduct operators appearing in Eq. (4 .10) are designated by the "X" operator and the superscripts are dropped from now on, so that

WWij* + x Wijx

DDij* -*• x DijX

N NHence A = H - Hj = £ £ m {Wi x [ D i j x ( D j i x W j ) ] -

i = l j = l

Dij[Wi x (Dji x W j)] (4 .11)

78

Using elementary vector calculus Eq. (4 .10) reduces to

N NA = I I (Di j • Wj) W-j x Dj-j - (D-j j • Dj - j ) Wj x Wj

i =1 j = l

( wi * W j)D i j x Ojj + (W-j • Dj-j) D-jj x Wj

By symmetry

N i -1A I £ ( D-j j • Wj) Wj x Dj-j - (D-j j • Dj-j) Wj x Wj

1=1 j = l

- (Wi • Wj ) D-j j x Dj i + (Wi • D j i ) Di j x Wj

+ (Dj i • Wi)Wj x D i j - (Dj i • Di j )Wj x Wi

- (Wj • Wi) Dj i x D i j + (Wj • D i j ) D j i x Wi = 0

Hence A = 0, since l i k e terms in the above equation cancel each other.

q .e .d .

Next, i t is shown that Eq. (4 .8 ) is indeed the expression for the angular momentum. In Appendix A4, the angular momentum expression is derived from i ts d e f in i t io n [8 ] fo r N a rb i t ra ry r ig id bodies. This expression is specified in Appendix B5 for N holonomic r ig id bodies.In Appendix B6, the above mentioned expression is programmed in the symbolic language MACSYMA [100] and is evaluated for N = 2 and N = 3, see Appendix B, Tables 8.10 and 8 .11 . The angular momenta for N = 2 and N = 3 are the same expressions, which are obtained by summing up the columns of the vector v, w for N = 2 and N = 3, see Appendix B l , Tables 8 .4 , and 8 .6 . This proves the v a l id i t y of Eq. (4 .8 ) fo r N = 2 and N = 3.

4 .3 The Automation of the Nonlinear C o n t r o l la b i l i t y Analysis

I t is shown, see section 2 .8 , that in the nonlinear c o n t r o l l a b i l i t y analysis , one has, in general, to deal with a large number of a lgebraic manipulations due to the evaluation of Lie brackets and due to a set of rank tests in order to f ind the dimension of the control led subspace. The theorems and de f in i t io ns of nonlinear c o n t r o l la b i l i t y concepts, which are stated in section 2 .8 , are already in a suitable form to be programmed in the SAM language MACSYMA. Two additional theorems are stated here, which prove to be very powerful in the automated nonlinear c o n t r o l la b i l i t y analysis; they are denoted theorem 4 and theorem 5.

79

Theorem 4 [121]

The Lie bracket of two vector f ie ld s f and g, denoted [ f , g ] , has tensor characters, i . e . , i t is invar iant under a change of the local coordinate system. The or ig inal proof appears in [1 2 1 ] . An easy proof is provided here.

Proof: Let f = f ( x ) = f ( y ) and g = g(x) = g(y) be two vector f ie ld s onan n-dimensional manifold which are expressed in two d i f fe re n t frames of references, which are

x = ( x j , . . . x^) e

and y = ( y i , . . . , y n ) e Rn

Let the coordinates of the second frame be expressible in those of the f i r s t frame, s . t .

X = P ( y )fk y

Then the Jacobi an, denoted p* (y ) = , which is assumed to be a

nonsingular transformation is the fol lowing map:

p*( Y ) : X -i- Y

The Lie bracket of vector f ie ld s f and g, denoted [ f , g ] , is invar iant under a change of the frame of reference i f :

[ f ( Y ) , g (Y ) ] = [ P* f ( x ) , p *g (x ) ] = p * [ f ( x ) , g ( x ) ]

By d e f in i t io n [ f ( x ) , g (x ) ] = g(x) - f ( x )

p * [ f ( x ) , g ( x ) ] = ( M i l gtx) . M > 0 f ( x ) )

_ ax af(x) . v ax ag(x) ," aY “ ax ' " ay ~ W ~ t{x)

= [px f ( x ) . p* 9 ( x ) ]

- ^ % ^ - g ( P(Y)) f (p (Y))

= g(Y) - f ( Y)

= [ f ( Y ) , g( Y)] q .e .d .

80

The next theorem deals with the span and the Lie brackets of an or ig ina l and a t r iangu lar ized set of vector f i e l d s , which is given in [126] without a proof.

Theorem 5

Consider a col lect ion S, of vector f ie ld s

SX = ( f 1 .............. f™}

on an n-dimensional manifold and le t the set S£

S2 = { ^ l 1 . . . Gf m)

denote the set which is obtained by t r ia n g u la r iz in g the set S j . Then

span { f l , . . . f™} = span ( Gf 1 , . . . , Gf m} .

Furthermore, i f

[ f 1 , f j ] e span { f 1 , . . . , f ”1} i , j e [ 0 , tn]

[ Gf i , Gf j ] e span { Gf l , . . . , Gf m} = span { f l , . . . f™}

Proof: Assume, without loss of genera l i ty , that the set Sj does notneed to be pivoted so that i t can be t r iangu la r ized by premult ip licat ion of a lower t r ia n g u la r matrix A(x) as fol lows, see [126] and section 5.3:

A(x)

f l G f l

f 2

•—

Gf2

•

0

0

f m

0

0

Gfm

where A(x) =

al l ( x )

a2 l (>0

an l ( x ) • • ann(x )

81

x: state of the systemFrom Eq. (4 .11) i t follows that

span { f 1 ........... f™} = span { Gf ^ 1 . . . Gf m}

Let an a rb i t ra ry tr iangu lar ized vector of Eq. (4.110 be expressed as:

mGf ' * I a ik (*) fk(x)

k=lThen the Lie brackets of vectors Gf> and GfJ , denoted [ Gf , Gf J ] ( x ) is :

m m[Gf1 , GfJ ](x) = [ I a1k( x ) f k ( x ) , I a j i ( x ) f 1 ( x ) ]

k=l 1=1m m

= I I C a ik (x ) fk ( x ) , a j i f ] ( x ) ] (by l i n e a r i t y ) k=l 1=1

From [122, pg. 283]

m m[ G f i , G f j ] ( x ) = I I { ai k (x) a j , ( x ) [ f k ( x ) , f ' ( x ) l

k = l 1 =1

+ ai k (x) (L fk (x ) a j i ( x ) ) • f 1 (x)

- aj l ( x ) ( L f * ( * ) aj k ( x ) ) • f k (x) }

with ( L f l ( x ) ai k ( x ) ) = < grad a j i ( x ) , f k (x) >

Let ai k (x) a j i ( x ) = b j j k ] (x)

ai k (x) (L fk(x) a j i ( x ) ) = C j j ki ( x )

- a j i ( x ) ( L f i ( x ) ai k ( x ) ) = di j k i ( x )

Then, i t follows that

m m[ Gf i , Gf J ] ( x ) = I I { bi j k i ( x ) [ f k ( x ) , f l ( x ) ]

k=l 1=1

+ c i j k l ( * ) * f 1 ( * ) + di j k 1 (x ) f k (x)> (4.12)

82

Hence i t follows that i f

[ f * , f J ] ( x ) e span ( f 1 , . . . , f™} then

[ Gf i , Gf J ] ( x ) e span { Gf 1 , Gf m> = span { f 1, . . . f™}.

Theorem 4 implies that one can construct a state space representationfo r a system, in which the Lie brackets take a p a r t ic u la r simple form.Theorem 5 implies that in v o lu t i v i t y properties of a set of vector f ie lds are invar iant under a t r ia n g u la r iz a t io n of the set.

The use of theorems 4 and 5 is i l l u s t r a te d below via an example of a planar, one l ink system, which is hinged to the or ig in see Fig. 4.When using Euler parameters i t s normalized equations of motion are:

X = f + gu (4.13)

with X = (q0 , q j , w)T

. 0q0 = Sln 2

eqi = cos 2

w = 0

f - < q o £ . . 2 q „ q i ) T

g - (0 , 0 , - 1)T

The above system is contro l lab le i f i t s s tructura l parameters, see section 2 .8 , are:

(n, na , nsa, n^) = (3 , 2 , 2 , 2) (4 .14)

Equation (4 .14 ) implies that the maximal dimension of the contro l lab lesubspace is the number of states (n = 3) less the number of constraints,which is one in Eq. (4 .1 3 ) .

A subset S c { f , g} is :

S = { f i , g i , [ f , g ] , [ [ f , g ] , f ] , [ [ f . g i l , g ] )

1 „ 0 1 0 0? qi w 1 qlf i 0 1 0 0{ ■ j q0w f 9 ■ f qo 9 » }

2 q0 qi -1 0 1 0

( 4 . IB)

83

Y

Figure 4 .3 . A Planar, One Link System Which is Hinged to the

Orig in .

84

By applying theorem 4, for example, the set S, Eq. (4 .15) can betransformed to a set S*, which has as states (e , e ) . Let <j>*(q0 , q j , w) be the map frokm the old state to the new state of the system:

<{) ★

qi. w): (q0» qi»

50 ' 50 ^ 50a<io | aqi | 5w

50 | 50 I 50»q0 a q i j 5w

2qi -2q0 0

0 I 0 I 1

Hence

•

09

0•

19

09

0

sin© -1 A 1 0

(4.16)

The set S*, which exhibits the same c o n t r o l l a b i l i t y properties as set S, allows an e f f i c ie n t determination of the structura l parameters, since i t contains two independent, constant vector f i e ld s . The set S, on the other hand, contains two independent, nonconstant vector f i e l d s , which implies that one has to check th is for possible c o n t r o l la b i l i t y defects. The structura l parameters of the set S*, denoted (n* , ns* , nsa , n*) = ( 2 , 2 , 2 , 2 ) , i . e . , the system, Eq. (4 .13 ) is contro l lab le . A l te rn a t iv e ly , one can apply theorem 5 to Eq. (4 .15) in order to produce a set GS, which is formed from set S by t r ia n g u la r i z a t io n . This is done by premultip lying S by a lower t r ia n g u la r matrix as follows:

1 0 0 0 [ f , g ] G[ f , g]W -1 0 0 f , Gf0 0 1 0 g Gg0 0 - 1 0 [ C f , g L f ] G[ [ f , g l , f ]

q j /2 -q0/2 0

0 0 -2qoqi

0 0 - 10 0 0

85

An inspection of Eq. (4 .17) reveals that sup(ns) = sup(nsa) = 2. There are no c o n t r o l l a b i l i t y defects, however, since q0 and qj can each vanish, but they do not do so simultaneously. Hence,

(n, ns , nsa, n ^ = (3 , 2, 2, 2)

and the system is c ontro l lab le . Note that the checks for the determination of in v o lu t iv i ty are less complex for the t r iangu lar ized set , Eq. ( 4 .1 7 ) , than for the or ig inal set , Eq. (4 .1 5 ) .

4 .4 The Control of the Rotational Equations of Motion of Two Planar, Rigid Links

In what follows the methods which are described in sections 4.2 and 4 .3 are applied to a planar, two-link system. I t s rotat ional dynamics are derived from the interconnection diagram, section 4 .2 .3 .The motion of the system is restr ic ted to t rans la t ions in the Y-Z plane and to rotat ions about the X-axis , i . e . , entr ies in the system matrices, which do not correspond to the above type of motion are set to zero.

I t is shown that the constant angular momentum constraint has quasi-hoionomic character, i . e , i t can be used to e liminate one state var iab le of the or ig ina l system which corresponds to the loss of ha l f a degree of freedom.

The c o n t r o l l a b i l i t y of the system is analyzed using l in e a r and nonlinear c o n t r o l l a b i l i t y concepts. I t is then shown that a simple, t ime-varying state feedback can be implemented to control the system about a nominal t r a je c to ry . The organization of section 4 .4 is as fo l lows. In section 4 .4 .1 the equations of motion of the system are specif ied and t h e i r c o n t r o l l a b i l i t y is analyzed. In section 4 .4 .2 a solution to the d i rec t a t t i tu d e control problem is proposed for two planar, r ig id l in k s , which are subject to a constant angular momentum. Numerical i n s t a b i l i t i e s , which occur during the simulation of the unreduced, p a r t i a l l y contro l lab le system are discussed.

4 .4 .1 The Rotational Equations of Motion and the C o n t r o l l a b i l i t y Analysis of Two Planar, Rigid LiriFs

In th is section the rotat ional equations of motion are specified fo r a planar two l ink system. I t s c o n t r o l l a b i l i t y is analyzed using concepts of l in e a r and nonlinear control theory. I t is shown in Appendix B.3 that the rotat ional equations of motion of a r ig id body system, with ideal torque actuators as inputs, can be wr it ten in the fo l lowing form:

86

X = f ( X) + gU (4.18)

where X and U are the state and the input to the system and f (X) and g are respectively the nonconstant d r i f t term and the constant input m atr ix . The elements of Eq. (4 .18) are derived for two planar, r ig id l inks in Appendix C, s ta r t ing from the equivalent system in three-dimensional space. Consider Fig. 4 .4 , which shows the above mentioned system consisting of two l in k s , joined together at a common hinge point. The state of the system is

X = (0j_, 02, P i , P2)T0 -j: angle between l ink i and the ver t ica l ( i = 1, 2 )

P-i: angular momentum of l ink i r e la t iv e to the center ofmass of the composite system ( i = 1, 2)

The d r i f t term f (X ) and the input matrix g are given below:

^ (J2 PX + D C2i P2)

■git *J1 p2 + n C21 pl ) f (X ) = nc;

- [Pj ?2 (J 1J2 + D2C2i 2 ) + DC2 i ( J i P 22 + J2 pl 2) ]

+ det^ ^Pl P2 JlJ2 + ° 2c212) + DC2 l ( Jl p22 + J2 pl 2)]

g = (0 , 0 , 1, - l ) T

det = J j J2 - C212 D2

C21 = c o s (©2 - 0 j )

S21 = sin(02 - 0i)

D = M o, 2 D21

m-j: mass of l ink i ( i = 1, 2 )

J-j: in e r t ia matrix of l ink i re la t iv e to the connectionbarycenter of l ink i ( i = 1, 2 )

U: torque between l inks 1 and 2.

87

Figure 4 .4 . Two Rig id, Planar Links and the Associated Connection

Barycenters and Centers of Mass.

88

The q u a n t i t ie s , which appear in Fig. 4 .3 , are already described in section 4 .2 .3 and are l is te d below:

CM^: center of mass of l ink i

CB-j: connection barycenter of l ink i

D-j: position vector to CM-j r e la t iv e to CB-j

D-jj: position vector of jo in t i r e la t iv e to CBj

01 -j j : position vector of jo in t i r e la t iv e to CMj

J-j: in e r t ia matrix of body i r e la t iv e to CB-j( i , j = 1, 2 , i j )

Hence, the new, reduced system is:

Y = f ! ( Y) + g i U (4 .21)

with Y = ( e i 02 P) ;

P = P!

f l ( Y ) =

J?Pt + DC?i( Hn - P)

J1J2 " c212°2

J l (H n - P) + PDCpi

J1J2 “ c212°2-DS?ifP(Hn - P ) (J1J?+D2c?i2) + DC?!CJt( Hn- P + J?P2 i 1

(J lJ2 - C2i^B^)^

91 = (0 , 0 , 1)T

Note that is a re lat ionship between the angular moments P-j and the angular velocites 0-j ( i = 1, 2 ) :

Pl = J l -DC2 1 e i

P2 - 0C2l J 2 02

I t is assumed that the two l in k s , see Fig. 4 .3 are not subject to any external force besides the gravity force. Hence, the to ta l angular momentum of the system, denoted H0 , remains constant, so that

Pl + P2 = H0 = constant (4 .20)

89

The nature of th is constraint renders the rotat ional dynamics p a r t ia l l y c ontro l la b le . The maximum dimension of the contro l lab le subspace, sup(dim C), i s , due to the presence of the constraint:

sup (dim C) = 4 (s ta tes ) - 1 (constra in t ) = 3

The angular momentum constra int , Eq. (4.2(1) can be implemented into the dynamic system, Eq. (4 .18) by replacing P2 , f ° r example, by:

After i t s implementation the constraint renders the th i rd and fourth equation of Eq. (4 .18) id e n t ic a l , so that one of them can be dropped from the dynamic formulation, see Appendix C.

The above mentioned constraint has quasi-holonomic character, since i t can be employed to e liminate only one of the state variables instead of two [ 3 4 ] , corresponding to the loss of ha lf a degree of freedom [3 4 ] . The c o n t r o l l a b i l i t y analysis of Eq. (4 .21) is considered below. In l in e a r c o n t r o l l a b i l i t y analysis the system, Eq. ( 4 .2 1 ) , is approximated by the f i r s t terms of a Taylor series:

6Y: incremental state

6U: incremental input (torque)

After th a t , the c o n t r o l l a b i l i t y matrix C is formed, which is analyzed fo r possible c o n t r o l l a b i l i t y defects.

The la rges t , p o te n t ia l ly nonzero, ( 3 x 3 ) determinant of C is , see Appendix C:

P2 - H0 - Pi

6Y = A 6Y + B6U (4.22)

with

Cj = {B, AB, A2B) (4.23)

“s21{ [ (D J22 - 2C21 D2 )J2 - J j 2 + 2C21 D2J j ]d e t2 - ---------------------------

( J lJ 2 - C2i 2D2) 2

+ H0[ (D J1 + C21 D2 )J2 + DJ! 2 - C21 D2J! - 2C21 D3]}

(4.24)

90

wtiich vanishes for the fol lowing cases:

a) The angles and 02 are equal: 0? - 01 = 0b) The two l ink system degenerates into a one l ink system.

c) At the points where P = H0 = 0

d) The remaining cases where det Cj = 0

I t follows that the l inear ized system is completely contro l lab le on a three-dimensional subspace with the exception of some isolated points. The results of the nonlinear analysis are considered next. I t is shown in Appendix C that the set

has structura l parameters, see section 2 . 6 ,

( na» nsa» n» n) = ( 3 » 3 » 3 » 3 )i . e . , i t is ( lo c a l l y ) c on tro l la b le . The largest p o te n t ia l ly nonzero determinant of L0 , denoted det, is :

C o n t r o l la b i l i t y defects occur at the points where C3 vanishes, which are at the points where:

a) the angular momentum H0 vanishes: H0 = 0b) the angles 03 and 0 are equal: 03 - ©i = 0c) the two l ink system degenerates in to a one l ink system: D = 0

Hence the nonlinear system is ( lo c a l l y ) contro l lab le in thethree-dimensional manifold with the exception of some isolated points. The determinants d e t j , Eq. ( 4 .2 5 ) , and det2 , Eq. ( 4 .2 3 ) , which re f le c t the c o n t r o l l a b i l i t y defects of the nonlinear and the approximated l in e a r systems have, in f a c t , very terms in common. The reason for th is is given below. The c o n t r o l l a b i l i t y matrix of the approximated l in e a r system is:

Lo = (g , [ f , g ] , [ [ f , g ] , f]> (4.25)

d e t l = ~2D s?. l------( J 1J2 - 02^ 2 )^

(4.26)

C = {B, AB, A2B} = (g, g , > Y = Y0

S im i l a r l y , L0 is :

l-o = ( g , C f , g ] , [ [ f . g ] , f j )

91

Hence, both sets d i f f e r by the term

a ra[f , g]5Y >- aY

which, in tu rn , y ie lds d is t in c t iv e values for the corresponding determi nants.

Conclusion: The l in e a r and nonlinear c o n t r o l l a b i l i t y analysis provideiden t ica l results with respect to the maximum dimension of the c ontro l lab le subspace. The results are d i f f e r e n t , however, with respect to c o n t r o l l a b i l i t y defects. Hence, the nonlinear c o n t r o l l a b i l i t y does not provide much more information than the l in e a r analysis.

4 .4 .2 A Solution to the A tt i tude Control Problem of Two Planar, Rigid Links, Whose Angular Momentum is Constant-

In th is section i t is shown that a c o n tro l le r can be devised for the planar, two-body system, whose sole purpose is to make the system fo l low some prespecified nominal t r a j e c t o r i e s . This is a d irect consequence of the fact that the above system is completely control lable on a constrained subspace, see section 4 .4 .2 . Hence, the computation of the reference t r a je c to r ie s and the implementation of the contro l le r are discussed below. F i n a l ly , d i g i t a l computer simulations are presented to v e r i fy the functioning of the c o n t ro l le r .

Computation of Reference T ra jec tor ies

The a t t i tu d e control problem of the planar two-body system can be reformulated as the fol lowing two-point boudnary value problem by using the notation of the previous section: Find © i ( t ) and 02( t ) , whichs a t is fy the angular momentum constra int:

H0 = (J i - DC2i)0i + (J2 - C21 D)©2 (4 .26)

subject to the fol lowing boundary conditions:

9 l ( t 0) = e 10 9l ( t f ) = el f02( t o) = 02O 02( t f ) = e2f

where t 0 and t f are the time at the l e f t and r igh t boundary.

A solut ion to the d i rec t a t t i tu d e control problem is considered next. The form of the solut ion is that of [82 - 8 4 ] , with the exceptionthat a c o n tro l le r is proposed and implemented here. Let the angle 0j ( t )

92

denote external variable of the two-body system, which specifies i ts or ien ta t ion re la t iv e to an in e r t ia l (or reference) frame. Furthermore,l e t 02i ( t ) = 02( t ) - 0 j ( t ) . The d irec t a t t i tu d e control problemconsists of prespecifying the internal variables and solving for the external ones by using the constraint equation, Eq. (4 .2 6 ) . In this process the i l l -posed boundary value problem, Eq. ( 4 .2 6 ) , is , thus, converted to a well-posed one.

The in ternal variable 021( t ) is assumed to be of the form

e 2 l ( t ) = ao cos (a j t ) + a2 (4.27)

where the constants a-j ( i = 0, 1, 2) are specified l a t e r . By subst i tu t ing Eq. (4 .27) in to Eq. ( 4 .2 6 ) , the l a t t e r one can be solved

for the quantity 0 j ( t ) :

5 i ‘ t> - <«.2»>

Hence

e l<t) - / f ei ( t ) dt + 01O = / f j? ; Jg?l DgCplr> dt + 910*0 t 0

(4.29)Choosing the quantity P = Pj to be:

P = J10i - DC21 ©2 = (J i ” 0^21)©! - DC21 021 (4 .30)

eliminates the need of computing bias torques, i . e . , an inverse p lant , as suggested in [ 3 2 ] , is not necessary.

C ontro l le r Implementation

The c o n tro l le r is implemented as fol lows. Starting from the l inear ized system, Eq. ( 4 .2 2 ) , a closed loop system, denoted

6 Y = (A + B K ( t ) )6Y (4 .31)

is formed which has eigenvalues at ( - 2 0 , -2 5 , - 3 0 ) . This is accomplished by transforming Eq. (4 .22) in to the contro l lab le canonical form, computing the transformed feedback gains K * ( t ) , and byapplying an inverse transform to obtain the actual feedback gains K ( t ) .

93

D ig i ta l Computer Simulations

A simulation diagram, i l l u s t r a t i n g the above mentioned design, is shown in F ig. 4 .5 . The body segment parameters are taken from [301 andare l is te d in Table 4 .1 , The l e f t and r ight boundary values are [30 ] :

0 j (O) = - 0.2 rad 02( ° ) = 0*12 rad0 j (1 .5 ) = 7i 02(1.5) = 7i

with t 0 = 0 sec and t f = 1.5 sec.

The above boundary values determine the constants a ( i = 0, 1, 2) the in ternal variable 02i ( t ) , Eq. (4.27)

02 i ( t ) = a0 cos (a j t + a2)

with a0 = | { [ 02( 0 ) - 0 j ( o ) ] - [ 02( 1. 5) - 0X( 1.5)]> = 0.16

a j = 2.0944 ; T = 1.5 sec

a2 = \ ( [ 0 2(o) - 0 i ( o ) ] - [ 02( 1. 5) - 0! ( 1. 5 ) ] } = 0.16

The simulations, which are done here, are used to represent the diving motion of a human being [30] which is modelled as a planar, two-l ink system. The diver jumps from a nine meter elevated platform, t rav e ls two meters in a horizontal d irect ion and lands in the water.The above assumptions determine the shape of the f l i g h t parabola of the center of mass of the composite system. Consider Fig. 4 .6 , which shows the f l i g h t parabola of a point mass. The point mass has an i n i t i a l v e loc i ty vector v0 , which makes an angle 41 with the horizontal and lands at point R, which l ie s on an incl ined plane B, which makes and angle a with the horizonta l . Recall that the range R of the point mass is

R = 2- l V-aI— sin(p - a) cos p . |Vq| . magnitude of v0 (4 .32)g cos2a

The maximum range Rmax is reached at p = ^ ^

1 vr> 1Rmax = gTI + si net) (4,33)

A position vector r ( t ) which shows the location of the point mass at time t re la t iv e to the or ig in is :

94

dU

State Feedback

dU - K(t) dY

Eq. (4.31)

®1<0

*|Mfc,.(407)(Oa»

Figure 4 .5 . Simulation Diagram fo r the A tt i tude Control of the Reduced, Planar

Two Link System

Point mass at time t

Point mass at time t = 0 sec

Range RPlane B

Figure 4 .6 . F l ig h t Parabola of a Point Mass Which Starts a t the

Origin and Lands on an Incl ined Plane R.

96

Table 4 .1 . Body Segment Parameters for The Two-Link System

Link No. 1 Link No. 2

I/Kg-m? 1.30 2.35

J/kg-m2 4.0567 3.65671 /meter 0.4 0.32

k/meter J 0.6 0.28

D -1.8667 -1.8667

97

r ( t ) = ( |v0 |°cosp)t y + { ( | v 0 |s inp ) t - gt?} z (4.34)A A

where y and z are unit vectors in the horizontal and vert ica l d ire c t ion s . The time t f , at which r ( t ) = p is

t f = 2 1VQI s ip (P. ~ - ) (4 .35)1 g cosa

Using equations (4 .33) - (4 .35) the position r ( t ) corresponding toA A A A

r (0 ) = 0*y + 0*z and r (1 .5 ) = -gy + 2z is:

r ( t ) = 1.3329t y + (0.1464 - 4 .0965)t z (4.36)

In the simulations, which are shown next, i t is assumed that the only v a r iab le , which the diver can inf luence, is the to ta l angular momentum H0 . Since the quantity H0 is determined by the corresponding i n i t i a l conditions, the diver has to t ry to generate the pa r t ic u la r value of H0 , denoted H0pf , to achieve a correct dive. The diver over-rotates i f

Ho > H0pt

and underrotates i f

Ho K Hopt

The t r a je c to r ie s for the three cases of under-rotat ion, correct ro ta t io n , and over-ro ta t ion are shown in Fig. ( 4 . 7 ) , Fig. ( 4 . 8 ) , and Fig. ( 4 . 9 ) . Fig (4 .7 ) shows that the angles ej ( t ) and 02( t ) , the corresponding angular v e lo c i t i e s , and the angular momentum are tracked very well with the c ontro l le r shown in Fig ( 4 .5 ) . However, the time

d e r iva t ive of the angular momentum, denoted P, d i f fe r s considerably from i t s reference value and so does the input torque U whose reference value is equal to zero. The above mentioned d i f ference and the peak values of

U and P at the l e f t and r ight hand sides of the respective graphs are due to the fact that at time t = 0 sec. the two l inks s tar t moving r e la t iv e to each other while at t = 1.5 sec. they stop to do so. Fig.(4 .8 ) and (4 .9 ) show s im i lar charac te r is t ic s . However, only the angles 0 l ( t ) and 02( t ) are tracked very w e l l . The corresponding angular v e lo c i t ie s and the angular momentum P show large errors between actual and reference value at l e f t and r ight boundaries while the tracking in between the boundaries is very w e l l . A close inspection of the input torques reveals that they exhib i t the same charac te r is t ic as the one in

98

METE

RS

mmDiving Board

-5 .0

( i * 0.25 sec)

- 7 .8

Water Level- 10.6 “

- 3 .0 - 1 .4 0.2 1.8 3.4 5.0

METERS

Figure 4 .7 . Simulation Results fo r an Under-Rotated Dive,

HQ = 2 (KG * METER2 * RAD/ SEC)

99

1.IC

r e f . value0.03

O 0.63■=tCd

act . value

;—1—mbs—1—idot—L tJdt TIME, SEC

O.flK

1.1br e f . value

0.9C

act . value

O.QK

TIME, SEC

Or e f . value

O

CXI

act . valueCD^ S.K

O.dDC

TIME, SEC

Figure 1 .7 . (co n t.)

100

r e f . valueoLU

□«=ccc

act . value

tris—1—ides—1—irir TIME, SEC

r e f . value

O.OIXE

act . value

1 B.k

TIME, SEC

r e f . valueC\J

UJ 3331,1 00

act . value«fcO2

-D .rr^ ts

Trie 1 ids 1 idnrO.dBT

TIME, SEC

Figure 4 .7 . (c o n t . )

101

Figure 4.

o.err

to O.OIC. cc

■\----- 1----- 1----- 1----- 1----- r

iz:o

- 0 . 18* -

-iL«Ufc

■ D .I IT -

0.®C D7

r e f . value

act . value

TIME, SEC

7. (cont . )

102

METE

RS

. 6

Diving Board. 2

-5 .0

-7 .8

- 10 . 6 Water Level

-3 .0 1.4 0 . 2 1.8 3.4 5.0

METERS

Figure 4 .8 . Simulation Results fo r an Optimal Dive,

HQ = 24 (KG * METER2 * RAD / SEC)

103

r e f . value

Qo:

u.w

— act. value

Trice—1—iricc—1—irte—1—rinro.flbr

TIME, SEC

r e f . value

U .TZac t . value

t t& k — 1— jd o r

TIME, SEC

It .TOU J

r e f . valueQ<Ca :*ooa iU J t— U J2:*CD act . value

o.dnc

TIME, SEC

Figure 4 .8 . ( c o n t . )

104

r e f . value

<_2U J

St. I to*xDC

act . value

r e f . valueC.07

a a-raU~1act . value

I.K

TIME, SEC

r e f . valueC\J

oU JU1ttT 0.03 U J►— U J2: -0.06

ac t . value

o.ar

TIME, SEC

Figure 4 .8 . (c o n t . )

105

0.15 r e f . value

UJ f— UJs:

-0.17-

zroH-3UJ3

-1.11

act. value

Trie—1—vioi—1—c TIME, SEC

dtr

Figure 4 .8 . ( c o n t . )

106

0.6

.2Diving Board

-5 .0

-7.8

- 10.6 Water Level

- 3 . 0 1.4 0 . 2 1.8 3.4 5.0

METERS

Figure 4 .9 . Simulation Results fo r an Over-Rotated Dive,

HQ = 35 (KG * METER2 *RAD / SEC).

107

r e f . value

c.u.

t.?l

act . value

TIME, SEC

•t.SII

r e f . value3 .U

2.00

I K )

l.UIact . value

*- fl.k

TIME, SEC

O 16.3? r e f . valueLU

CNJ16.21LU i—

U J 21act . value

TIME, SEC

Figure 4 .9 . (c o n t . )

108

I ^ > 'T

s.eir e f . value

3 .1 7

LUCO 5.13

act. value

■bt4de—1—ir&js—1—rrir TIME, SEC

O.lK

r e f . value

OLUCO

act . value

n.ids “Sla

TIME, SEC

C\J

o r e f . valueLU

a:LU l— LUz:

2.00T

O.R

act . value

TIME, SEC

Figure 4 .9 . (c o n t . )

109

e.eire f . value

LU I— UJs: -i.t? -i

ac t . value

5— 1—mloc— 1— rrtr

TIME, SEC

Figure 4 .9 . (cont . )

110

Fig. 4 .7 with the exception that the peak values at the respective boundaries are more pronounced in Fig. (4 .8 ) and Fig. ( 4 . 9 ) . Therefore, the cause of these discrepancies is the same as discussed above. The r e la t i v e ly bad tracking properties of the c o n t r o l le r , Eq. (4 .3 1 ) , suggests that i t might be too slow, so that new eigenvalues, which are fu r th e r to the l e f t than the present ones, w i l l probably improve the performance of the c ontro l le r .

Simulation of the Unconstrained System

The simulation of the reduced system, Eq. (4 .21) is shwon in Fig. 4.7 - 4 .9 . I t is known from the physics of the problem involved that the unreduced system, Eq. ( 4 .1 8 ) , behaves in exactly the same way as the reduced one. However, the simulation of the unreduced system, might pose numerical problems, see Section 2 .4 , since i t is only p a r t i a l l y c ontro l la b le . I t is shown below that the constraint has to be a r t i f i c i a l l y s ta b i l i ze d during the simulation of the unreduced system, i f i t is being v io lated in th is process.

Consider Fig. 4 .1 0 , which shows a diagram for the simulation of the unreduced system, Eq. (4 .1 8 ) . This configurat ion is used to re-s imulate the optimal d ive, which is shown in Fig. 4 .8 with the t r a je c to r ie s for the reduced system. Note, from Fig. 4 .10 , that the feedback law is derived from the reduced state Y = ( f l j , e^, P i ) T.The simulation of th is p a r t ic u la r configurat ion does not pose any numerical problems, since the angular momentum constra in t , Eq. (4 .20) is a l in e a r function of the state and, consequently, is not v io lated during a numerical in tegrat ion process. Hence, for th is case the t r a je c to r ie s fo r the optimal dive as ident ica l fo r the reduced and the unreduced system. As a consequence for the fact that the angular momentumconstraint is not v io lated at a l l , the value of the angular momentum ofthe second l ink can be recovered from the knowledge of the angular momentum of the f i r s t l ink and the to ta l angular momentum:

P 2 ( t ) = H0 - Pj ( t )

P 2 ( t ) = - P l ( t )

Next, i t is shown that i f the constraint is a nonlinear function of the s ta te , numerical problems are encountered, so that the constraint

111

PLANT

X - f ( X ) + g U

Eq. (4.18)

dU

State Feedback

dU - K(t) dY

Eq. (4.31)

W O - » !» ♦ •* ,«

Pl« .

Figure 4 .10. Simulation Diagram fo r the A t t i tude Control of the Unreduced, Planar

Two Link System

has to he a r t i f i c i a l l y s ta b i l i z e d . For that purpose a new state Z is defined as follows:

Z = ( 0 i 02 0 i 02) T

so that the new system is

1 = f 2 (Z) + g2 (Z) U

®1

f z W ■=e

L j l - ° c 12-1 .21

D S12 021

-DC12 J 2 -D S12 01

(4 .37)

92(Z) = J l -DCi J -1

J DC12 J2 _ L - ' _

The constraint is now a nonlinear function of the state

H0 = (J i - D C12) 0 i + (J2 ~ D c12) ‘q 2 (4 .38)

A simulation diagram for the above system is shown in Fig. 4 .11 . Note that the to ta l angular momentum is that of the optimal dive, so that

H0 = 24 (kg * rad * m2/sec ) .

Furthermore, the input torque U for the new unreduced system, Eq. ( 4 .3 7 ) , is taken to be the same as that for the optimal dive, Fig.( 4 . 8 ) . The simulation results for the above mentioned system are shown

113

PLANTdU

equ. (4.37)

Figure 4 .11 . Simulation Diagram fo r the Att i tude Control of the Unreduced, Planar

Two Link System with State Z = (0^, 0^, 0^, 0^)^ .

r e f . value

44. VM

act . value

0. 1)

-trine—1—trierTIME, SEC

• n

r e f . valueC.CI -

LUCO ft. 16

ac t . value

-d dc—1—irtr TIME, SEC

0.109

C\JOtn

act . value

o.dsr

TIME, SEC

Fiqure 4 .12 . Tra jector ies fo r the Optimal Dive Usinq as. T

State Z = (O , 0 , 0 , 0 ) .1 2 1 2

115

oe£CC

act . value

u.«re f . value

L I I I J

TIME, SEC

r e f . value£.00

OLULO

act . value*.00

.a :

TIME, SEC

act . valueCM

tn

TIME, SEC

Figure 4 .12 . (c o n t.)

116

TXl*t -

r e f . valueU JHU J51

h-U J act . value

doc—1—d x

TIME, SEC

oLU ir>o

*CVJcc

*CD

D . j r -

0.31 -

Z3.«B -

\r e f . value

•J.TJ’O.flDT

act . value

J—doc—1—due—1—doc—1—ivinc 4' rrtt

TIME, SEC

Figure 4 .12 . (cont . )

117

in Fig . 4 .12 . A close inspection of Fig. 4.1? reveals that the tracking of the angles 9 , ( t ) and G ^ t ) is f a i r l y good, since deviations of the corresponding actual values from the reference t ra je c to r ie s are compensated by the much larger errors in actual and reference values, which occur in the respective angular v e lo c i t ie s . Note also that the constraint H0 is severely v io la ted . I t follows that th is simulation can be of fu r ther use since the numerical in s t a b i l i t i e s which are introduced in to the system through the process of numerical in teg ra t io n , do not cause a s ign i f ic a n t divergence of the angles G i ( t ) and 02( t ) from the respective reference values. This is due to the fact that the dive has a f i n i t e duration which is specified by the time i t takes to dive from a nine meter elevated platform in to the water. I f the set up shown in Fig. 4.11 were used to simulate the motion of a s a t e l l i t e in outer space corresponding to a not necessarily f i n i t e amount of time for simulation, the i n s t a b i l i t i e s introduced by the above mentioned mechanism had a more severe e f f e c t . In the l a t t e r case add i t ion a l , hypothetical inputs are necessary to maintain the constraint and assure s t a b i l i t y of the system.

4 .5 Summa ry

In th is chapter the rotat ional equations of motion and t h e i r f i r s t in tegra ls are derived for multi-1 inkage systems in three-dimensional space. I t is shown that the above task can be considerably s impli f ied through the use of a general-purpose, symbolic manipulation program.The rotat ional equations of motion are specified for a planar, two l ink system. I t s c o n t r o l la b i l i t y properties are analyzed using methods of l in e a r and nonlinear control theory. I t is shown that in the above nonlinear system the nonlinear c o n t r o l l a b i l i t y analysis does not provide much more information as the corresponding l in e a r analysis. The automation of the nonlinear c o n t r o l l a b i l i t y analysis is discussed. The inverse a t t i tu d e control problem is i l lu s t r a t e d via a diving motion. A solution to th is problem is implemented for the planar, two-l ink system. D ig i ta l computer simulations are performed for the unreduced system and fo r the reduced system, where the constant angular momentum constraint is implemented as a quasi-hoionomic constra int . The unreduced system, which is only p a r t i a l l y c on tro l la b le , is not subject to numerical i n s t a b i l i t i e s during d ig i ta l computer simulation, i f the constraint equation is a l inear function of the state of the system. However, i t is shown, that for nonlinear constraint equations in the s ta te , hypothetical controls have to be devised whose sole purpose is to maintain the constraints and assure s t a b i l i t y .

118

Chapter V

COMPUTATIONS OF HOLONOMIC AND SIMPLE NONHOLONOMIC CONSTRAINT FORCES OR TORQUES IN MULTI-BODY SYSTEMS

5.1 Introduction

Constraint forces or torques are an in tegral part of the study of multi-body systems. The terms simple nonholonomic constraints refers to those nonholonomic constraints whose corresponding constraint equations have the same structure as holonomic ones i f both types of equations are d i f fe re n t ia te d twice with respect to time. In the multi-body systems, which are studied in th is d iss e r ta t ion , holonomic constraints have to be enforced, i f the individual bodies of the composite system are jo inted together in a kinematic chain T40]. I f these connections are such that rotat ional degrees of freedom are lo s t , constraint torques are engaged, which model these p a r t ic u la r jo in t character is t ics [ 4 0 j .

The constraint forces of a multi-body system serve d i f fe re n t purposes. In robotic and locomotion systems, implementing, maintaining, and de l ib e ra te ly v io la t in g contact and support conditions require e i ther on- l ine sensing of constraint forces or ind irec t computation of them.In th is chapter three d i f fe re n t methods of ca lculat ing constraint forces are compared with respect to computational complexity and accuracy, and computation time. In the f i r s t two methods, the nonlinear constraint equations and the system dynamic equations are combined such that the resu lt ing set of equations is uniquely solvable for the constraint forces. In the th i rd method, the mechanical system is embedded in a higher dimensional state space, chosen in such a way that the constraint equations are l in e a r in the new state of the system. Consequently the computation of the constraint force s im p l i f ie s . The complexity of the three methods and computation time for a p a r t ic u la r computer can be in ferred from an operation count. The accuracy of the individual method is estimated by providing an upper bound of the r e la t iv e error and the r e la t iv e residual of the computed constraint forces.

The organization of th is chapter is given below. In section 5.2 and 5.3 the derivat ion and compuation of constraint forces or torques, via the above mentioned methods, is outl ined. I l l u s t r a t i v e examples are given in section 5 .4 . F in a l ly , the results of th is study are discussed in section 5 .5 .

119

5.2 Derivation of Constraint Forces

The above three methods are applied in th is section to a simple inverted pendulum for comparison purposes. Other examples can be found in references [16 , 17, and 18]. Consider a simple planar pendulum (Fig. 5 . 1 ) . I ts center of mass is at point 0 with coordinates (x , y) and i ts contact point with the ground is P. The system has an input IJ and is subject to holonomic contact forces r x and Ty. Let X = [x , y , 0 ] , and l e t m and I be respectively the mass and moment of in e r t ia of the pendulum about i ts center of grav ity .The equations of motion are:

with

u X = +u2r + U3 + U4U

u = diag(m, m, I )

(5 .1 )

u2 =

1

0

-k cos 0

0

1

k sin 0

u3 = ( 0 , -mg, 0 ) T

u4 = (0 , 0 , 1)T

The two holonomic constraints are:

x- k sin 0 = 0

y - k cos 0 = 0

The are d i f fe re n t ia te d twice to a r r ive at:

uJ X = U5 (5 .2 )

120

( x , y )

mgr

Figure 5 .1 . The Simple, Planar Pendulum Subject to Two

Holonomic Contact Constraints

121

with U5 = - k ( s 1n 0 , cos 9)" 0 .

The f i r s t method consists of solving Eqs. ( 5 .1 , 5 .2 ) simultaneously for the unknowns. In matrix form

AxYx = Bi (5 .3 )

with

M ■ C o 2 Uu^T)

Bl = ( u3 + U4Uj

u5

Yi = (r, X )T

The above system is solved by forming an augmented matrix (Aj; B i ) ,and transforming i t to ( I , A j^ lB j) by elementary row operations [5 7 , 581where the quantity I represents the id e n t i ty matr ix . This method requires less ar ithmetic operations than computing the inverse matr ix (A i ) _1 for solution [57 , 5 8 ] .

In the second method the accelerat ions are eliminated. The new system of equations is:

A2Y2 = B2 (5 .4 )with

A2 = u2T ( u i ) " 1 u2

B2 = U5 - u2t ( u i ) " 1 (U3 + U4U)

y2 = r

In the above der iva t ion , the matrix A2 is posi t ive d e f in i t e [ 4 0 ] , and therefore in v e r t ib le . The dimension of A2 is equal to the number of constraints m.

In the th i rd method, the or ig ina l system is changed to the system depicted in F ig . 5 .2 . The state vector of the system is

W = (X]_, Y i , 9, Xl f Y i , 0 )^ , where Xj and Yj are the coordinates of thecontact point P. The equations of the new system are:

viZ = v2r + V3 + V4U + V5 (5 .5 )

122

with

'1 =

v2 =

m

0

mkC

1 0

0 10 0

0 mkC

m -mkS-mkS I + mk?

v3 =

V4 = U4

v5 = ( ks e2 , kce2 , o )T

i = U i , Y i . e )T

S = sin 0; C = cos 0

The constraints are:

X i = 0

Y! = 0

The constraint forces can be calculated very e f f i c i e n t l y , from

A3 Y3 = B3

(5 .6 )

(5 .7 )

with

A3 =

B3 =

•v2 v i) - v2T

V3 + V4U + V5

0

Eq. (5 .7 ) is s im i lar to Eq. (5 .3 ) but offers considerable computational

123

y

Figure 5 .2 . The Pendulum Embedded in 0 , x , y , Space.

124

advantages. Matrix A3 is already of t r iangu la r form, so that i ts inverse can be computed without using the forward i t e r a t io n . The lower portion of the vector B3 is equal to zero, such that during back subst i tu t ion , additional computational savings occur. Eqs. (5.1 and 5.5 ) are re lated by the nonsingular transformation [61:

X = D ^ + D2 (5 .8 )

with

Di =

1

0

0

0

10

kC

-kS

1

D2 =

-k s e 2 - k c e 2

0

Using Eq. (5 .8 ) the matrices of Eq. (5 .5 ) are:

n = DiT ui

v2 = Di t u2

v3 = Di t U3

v4 = Di t u4

v5 = -D i t d2

Dl

5.3 Computation of Constraint Forces

The la s t step in each of the above three derivat ions is to solve aset of l in e a r equations in the forces of constraint r . The methods to beused for th is purpose are Gaussian elimination with p a r t ia l pivot ing [57 -63] - a series of row operations consisting of two parts: theforward e limination and the back subst i tu t ion. Consider the equation

Ay = B (5 .9 )

During forward i t e r a t io n , the matrix A is transformed to an upper t r ia n g u la r matrix R, according to:

R = L - l PA , (5 .10)

125

The m u l t ip l ie r s , which perform th is transformation, form a lower t r ia n g u la r matrix L, which has l ' s along i t s diagonal. The matrix P is :

p = pn - l pn-2 P1 »

where the P-j (1 = 1, . . . , n-1) are Frobenius matrices [ 3 1 ] , which accomplish a row permutation of matrix A in the 1th i t e r a t io n step. During back substitut ion the diagonal elements of R, which are r-j-j ( i = 1.............. p ) , are normalized by premult ip l icat ion of R by 0:

0 = diag (-1—) , . . . , - J L ) . The solution to Eq. (5 .9 ) is : r n r pp

Y = (0R)-1 (0 L '1)B (5.11)

Due to the form of matrices (OR) and L, t h e i r inverses are found by changing the sign of the off -diagonal elements. The operations involved in ca lculat ing Eq. (5 .11) are l is te d in Table 5 .1 . The searches for a maximum, which are l is te d in Table 5 .1 , are done during pivoting.

5 .3 .1 Computational Complexity

The number of additions and m ult ip l ica t ions for a l l three methods as a function of the n dynamic and m constraint equations of the system are derived here. These formulas are based on the Gauss elimination technique and are shown in Tables 5.2 and 5.3 for two cases: diagonaland nondiagonal in e r t i a matrices. These formulas are based on Eqs. ( 5 . 3 ) , ( 5 . 4 ) , and (5 .7 ) fo r the general case. The expressions in Tables5.2 and 5.3 fo r the th i rd method contain an operation for solving Eq. ( 5 . 7 ) , provided that the corresponding matrices Vj ( i = 1, . . . , 5) are known. However, i f the matrices of the old system, Eq. ( 5 . 1 ) , are known only, they can be transformed to those of the new one, Eq. ( 5 . 5 ) , using the transformation Eq. ( 5 . 8 ) . Consequently, a separate operation count is provided for the above transformation.

The computational savings, due to the blocks of zero elements in matrices Aj of Eq. (5 .3 ) and A3 , B3 , in Eq. ( 5 . 7 ) , are accounted for in Table 5 .3 . Additional computational savings due to a diagonal in e r t ia matrix u j , in Eq. ( 5 . 3 ) , ( 5 . 4 ) , and (5 . 8 ) are re f lected in Table 5 .2 .The remaining matrices, however, are assumed to contain only nonzero e n t r ie s .

The computational complexity of the three methods is summarized in F1g. 5 .3 . In F ig . 5 .3 the number of additions and m ult ip l ica t ions of the three methods is shown as a function of the n dynamic and m constra int equations of the system. F ig . 5.3a and b represent a system with few constra in ts , where m = 0.2 * n. F ig . 4c and d show a system with a medium number of constraints , s . t . ,

126

in = 0.5 * n

Figs. 5 .3 e and f represent a system with many constraints, where

m = 0.9 * n

Figs. 5 .3a , c, and e are graphed based on the expressions of Table 5 .2 , Figs. 5 b, d, and f are based on Table 5 .3 . An in terpre ta t ion of Fig.5.3 reveals the a p p l ic a b i l i t y of the three methods:

Method I (Smith):

This method is e f f i c ie n t i f the system is known in the form specif ie d by Eq. (5 .1 ) and i f the in e r t ia matrix uj is non-diagonal.

Method I I ( Hemami)

This method is e f f i c ie n t i f the system is known in the form specif ied by Eq. (5 .1 ) but with a diagonal in e r t ia matrix.

Method I I I (Kane):

This method requires the least amount of computations, i f the system is known in the form specif ied by Eq. ( 5 . 5 ) . I f the old system, Eq. ( 5 . 1 ) , is known only, th is method is s t i l l e f f i c ie n t fo r a system with many constraints.

5 .3 .2 Estimates of Round-Off Errors

The fol lowing expressions are upper bounds of the round-off errors [57 -63] which occur by applying the method of Gaussian elimination for solving a system of l in e a r equations. These round-off errors are due to the representation of real numbers as f lo a t in g point numbers with f i n i t e mantissa in the memory of a computer.

Consider a l in e a r system, Eq. ( 5 . 9 ) , with a t rue solution Y and a computed solution Y*, which is obtained by applying the method of Gaussian e l im inat ion . I t is assumed that the matrices A and B in Eq.( 5 . 9 ) , are expressed in exact f lo a t in g point numbers and that no overflow nor underflow occurs during the solution of ( 5 . 9 ) . Let

nI I C I I = I |c i | (5 .12)

i =1

denote the norm of a vector c with elements c-j ( i = 1 , . . . , n). Then the r e la t iv e residual is :

(5.13)

127

Table 5 .1 . Computational Complexity of Calculating an Inverse Matrix

and Solving a System of Simultaneous Equations.

r ■ ------ — ' -----------1 Number of j Additions

Number of M ult ip l icat ions

Otheroperati ons

solving a system of p simultaneous equations via Eq. (5 .11)

. 3 o 5F + 7 p 7 p \ (P2 + P)

searches for a maximum

computing the |2 3 _ 2 + 1 p pxp inverse [3 p ” p "3 matrix via 1 A-l = (OR)- 1( QL- 1P) 1

2 . 2 . 1 1 , 2 , D\^ P + P + - j P -^ (P + P)

searches for a maximum

'

128

Table 5 .2 . Computational Complexity of Calculat ing Constraint Forces

Assuming the In e r t ia Matrix is f liagonal.

Calculation of Constrai nt Forces

Number of Additions

Number of M ult ip l icat ions

Otheroperati ons

Via Method I

Eq. (5 .3 )

(Smith)

m n + n m m^n + r \ c m

+ i (n+tn)2 + i n2 x \L+ -p (n+m)2 + yp

2 - n - m

•i[(n+m)2 - n-Hn]

-m^ -1

- -£•[(n-m)2_n+ml

searches

Via Method I I

Eq. (5 .4 )

(Hemami)

1 3 2 m2■j m + m n — ^— m3 + m2n + m2

11 m + mn + 2 mn - 7* m5*

-y (m3 + m)

searches- 1

Via Method I I I

Eq. (5 .7 )

( Kane)

(n-m) 3 + (n2-n)

+ (n-m)(n-m-l)m

(n-m)

(n-m)3 + (n-m)2 [ (n-m)2+n+m[l

+(n-m)(n-m-l)m3 o o 5 + — tn "g- m

- i

searches

Transformation

of s tate vector

Eq. (5 .8 )

2 - 2n 3n2

129

Table 5 .3 . Computational Complexity of Calculating Constraint Forces

Assuming the In e r t ia Matrix is Nondiagonal.

Calculation of Constrai nt Forces

Number of Additions

Number of M ult ip l ica t ions

Otheroperations

Via Method I

Eq. (5 .3 )

(Smith)

•g- (n-+™)3 - | m ^

+ (n+ni)2 - -jr n -y

(n+m)2 _ .1 m3

+ | (n+m)2 - | n

m" 7

i [ (n + m ) 2 - n-m]

-m2 -1

searches

Via Method I I

Eq. (5 .4 )

( Hemami)

(m+n)2 + n2- n 2

m2 X 1 n 11 m ' T 1 n ~ ~ E m

- j (m+n)2 + n2+n2

3 o n + Y m + mn + -g-

(m2 + m) - 1

searches

m

[Via Method I I I (n -m )2 + ^ (n2-n) j (n-m)2 + (n-m)2 [ (n -m )2+n+m]

I Eq. (5 .7 )

| (Kane)

+ (n-m)(n-m-l)m1 (n-m)

-j 'r -j

+(n-m)(n-m-l)m

+ - n2 _ m2 + m

11 n

7

-1

searches

^Transformation

of state vector 2n2 - 2n 2n2 + 2n2

130

HO m H U . • H

(a)

H O

100

m • m

(c)

• M O

» H

NO W H U . • H

( b )BUM v n r u

m H Um • H

(d)

131

i m m d t m m r .

mo

m m mm mm. • m

(e)

+ - + - + Method I

• •1X1XkX Method I I

Method I I I

0 - 0 - 0 - : Method I I I

. man *tm tm x ri* mtr.

too

MO m mu. • m

(Smith)

(Hemami)

(Kane)

Including a Transformation

of the Basis

a) Few Constraints (M = 0.2 N), Diagonal In e r t i a Matrix

b) Few Constraints, Non-diagonal In e r t i a Matrix

c) Medium Number of Constraints (M = 0.5 N), Diagonal

I n e r t i a Matrix

d) Medium Number of Constraints, Non-diagonal In e r t ia

Matrix

e) Many Constraints (M = 0.9 N), Diagonal In e r t ia Matrix

f ) Many Constraints, Non-diagonal In e r t i a Matrix

Figure 5 .3 . Comparison of the Operation Count o f Smith's, Kane's,

and Hemami's Method of Computing Constraint Forces.

132

and the r e la t iv e error is:

(5.14)

where:

p: base of the f lo a t ing point number system

t : number of s ign i f icant d ig i ts in the mantissa of thef lo a t in g point number

p: 0 < p < p in most cases [24]

and

cond (A): condition number of A

5.4 I l l u s t r a t i v e Examples

In th is section the three methods are compared for three i l l u s t r a t i o n examples.

5 .4 .1 . Planar One Link

The f i r s t example is the planar one l ink of F ig . 5 .1 . The constraint forces, in analyt ical form, are

r x = aj Se2 + a2 SC + a3 CU

(5.15)

Iy = a^Ce2 + a4 + a2C2 - 83 SU(5.16)

with

al = -mk

133

Table 5 .4 . Operation Count for the Methods of Calculating ConstraintForces for the Planar One Link.

Total Number of

Calculation of Constraint

Forces Additions M ult ip l ica t ions Searches

Analyt ic Method Eq. (5 .16) 5 12 -

I n i t i a l i z i n g U2 , U5 [Eq. (5.1 , 5 . 2 ) ] - 5

Method I (Smith)(m = 2, n = 3) 43 68 9

Method I I (Hemami): 15 31 1Method I I I (Kane) + 3 + 3 0 (transformation of base vector)

8 + 54 0

I n i t i a l i z i n g v j , V5 [Eq. ( 5 . 5 ) ] 3

and B3 [Eq. ( 5 . 7 ) ]

5

Method I I I (Kane) 3 8 0

134

Table 5 .5 . Upper Bounds for Round-Off Errors for Computed ConstraintForces of the Planar One Link

Simulation: The numerical values of the parameters for th is example are

m = 49 kg; I = 2.35 kg m ; 1 = 0.312 m; 9 = 0° , 45°; p = 2.

Calculation of

Constraint Forces

via the Method of

Single Precision (t=24) Double Precision (t=5&)

Relat iveError

Relat iveResidual

< 12x 10-8

< 12xl0-8

RelativeError

Relat iveResidual

Smith (Eq. ( 5 . 3 ) ) < 11574x10-7 < 12x10-8 <

Hemami, (Eq. ( 5 . 4 ) ) < 408x10-7 < 12x10-8 <

Kane, (Eq. ( 5 . 7 ) ) < 8x10-7 < 12x 10-8 <

2694x10-16 < 11x10-11

95x10-16 < 11x10-172x10-16 < 11x10-17

135

where S and C respectively mean sin e and cos 0 . The operations, which are necessary to calculate r * and Fy via Eq. (5 .1 6 ) , are counted assuming that the constants a-j ( i = 1, . . . , 4) are known and that repeated terms are computed only once, i . e . , they are stored in memory. An operation count for ca lculat ing constraint forces using the numerical methods, section 5 .3 .1 is performed based on the expressions of Table 5 .2 . The results are shown in Table 5 .4 . The round o f f errors are given in Table 5 .5 ; i t is assumed here that the numbers which are used to compute the constraint forces are represented e i th e r in single (24 b i t mantissa) or double (54 b i t mantissa) precision format in a d ig i ta l computer.

5 .4 .2 Planar Double Pendulum

A planar double pendulum is considered next (F ig . 5 . 4 ) . The corresponding centers of mass are at points Oi ( i = 1, 2) with coordinates (x- j , y-j) (i = 1, 2 ) . The hinge points are denoted by P-j and P-j * ( i = 1, 2 ) . The distances PjQi and 0iP-j ' are called k-j and 1-j ( i = 1, 2 ) . The angle 9-j (i = 1, 2) is measured between the l in e P0-j and the v e r t i c a l . The system has inputs u and u? and is subject to holonomic forces = ( r i x r i y ) T and r 2 = [T2x» r;?y)T » which constrain the t rans la t iona l motion of the system. The matrices of the dynamic and constraint equations, Eq. (5 .1 ) and Eq. (5 .2 ) which are denoted u-j ( i = 1, . . . , 5 ) ; are given below:

ui = diag(mi, mi, m2» m2» h » h )

+1 0 -1 0

0 +1 0 -1

* 1 0 0 +1 0u2 =

0 0 0 +1

- k l cl + k isi - 1 1C1 +1l sl0 0 - k 2C2 +k2s2

C-j = cos e-j; Si = sin e-j (1 =

U3 = (0 , -mjg 0, -^29» °» 0)T

T0 0 0 0 -1 o_U4 =0 0 0 0 1 -1

136

Figure 5.4 Double Pendulum Subject to Holonomic Constraints.

137

Table 5 .6 . Operation Count for the Methods of Calculat ing ConstraintForces for the Planar Two Link.

Calculation of Constraint

Forces Additions

Total Number of

M ult ip l ica t ions Searches

Analyt ic Expression Eq. (5 .21) 16 28 -

I n i t i a l i z i n g U£, U5 [Eq. ( 5 .1 , 5 . 2 ) ] 2 18 0

Method I (Smith)(m = 4, n = 6) 355 455 27

Method I I (Hemami) 356 488 9

Method I I I (Kane) + ( t rans

formation of base vector)

25 + 420 46 + 504 1

I n i t i a l i z i n g A3 and B3 [Eq. ( 5 . 7 ) H 6 19 -

Method I I I (Kane) 25 46 1

138

u 5 = ~k i s i ® i 2 ; - k i c I © i 2 ; ( - i i s i Q i 2 -><2S2 0 2 2 ) ;

( - 1 i C ^ i 2 - k2c2®22)

(5.17)

The base vector X of the system is:

X = (x j y j x2 y ? 01 ©2)T •

An operation count for computing the constraint forces r i and r£ numeric a l l y , using the formulas of Table 5 .2 , is given in Table 5 .6 , row 2,3, and 4. The computation of the above forces via the th i rd method, see section 5 .2 .2 is shown below. F i r s t , the or ig ina l system, which is shown in Fig. 5.4 is embedded in a larger state space, see Fig. 5 .5 . In that space the or ig inal system has four addit ional t rans la t iona l degrees of freedom which correspond to the variables ( x n , y i j , X21, y2 l)« The local vector ( x ^ , y n r specif ies the lower hinge point Pi of the f i r s t l in k r e la t iv e to the o r ig in . The vector (X21, Y2l) specifies the lower hinge point Po of the second l ink r e la t iv e to the upper hinge point Pi of the f i r s t l in k . From Fig. 5 .5 i t can be seen that the imposition of constrai nts

reduces th is larger dimensional system to the or ig inal one. The matrices of the dynamic equations of the new system, Eq. (5 .5 ) are denoted by v-j (i = 1, . . . , 5) and are given below:

X u = y n = x 2 i = y 2 l = 0 (5 .18)

Ci2 = cos(9i - 02)

1 0 -1

0 1 0

0 0 1

0 -1

0

0

V2 = 0 0 0 1

0 00 0

0 0 0 0

139

Figure 5.5

pi

- i --------------------------------------------------------------------- ► *

xn

Double Pendulum Embedded in a Larger State Space.

140

0 " k l s l 0 l 2-mig " k l c l 0 l 2

0 - ( 1 i + k 1) Si©!^ - k2S2022

-m2g ; v5 = - ( 1 i + k i ) C j 0 j ^ - k2C2 022

- k l S i m j g - ( 1 x + k ^ S ^ g 0

" k2s2m29 0

V4 = U4 (5.19)

The basic vector Z of the new system is

1 = ( x l l * y i l . X2 1 * Y 21 . e l » e 2 ) T

The constraint forces r in Eq. ( 5 . 5 ) , are:

r = ( r i x r l y r 2x r 2Y) T

The constraint forces r are found in the fol lowing way. The equations of the new system, Eq. ( 5 .1 9 ) , and the constraint equation, Eq. ( 5 .1 8 ) , which are d i f fe r e n t ia te d twice, are w r i t ten in the form of Eq. ( 5 . 7 ) . A permutation of columns and a reduction of the system produces the fol lowing almost upper t r ia n g u la r system of equations:

1 0 -1 0 a l 0 r lx *>10 1 0 - 1 a2 0 r ! y b2

0 0 1 0 a 3 a 7 T2X — b30 0 0 1 a4 a8 r 2y b40 0 0 0 a 5 a 6 0 1 b50 0 0 0 a 6 a 9 0 2 b6

( 5 . 2 0 )

with

a^ = m^k^C^

a2 = -mxk1S1

a 3 = m2 0 l + k l ) c l

141

a4 -m2 ( 11 + k l ) s l

a5 = I I + m i k i 2 + m i ( l i + k i ) 2

a6 s m2 ( 1 l + k l ) k2c 12

a7 = m2k2^2

a8 = "m2k2 s2

a9 = 12 + m2k22

bl = - k l S i e i 2

- b 2 = - k l C i 0 i 2 - mig

b3 = - ( l l + k i ) S i 0 i 2 - k2S2 022

b4 c - ( l l + k i ) C i 0 i 2 - k2C2©22 - ni2g

b5 = - k l S i m i g - ( l i + kl)Sim2g - ui + U2

b6 = - k2s2m29 - u2

An operation count for doing th is is given in Table 5 .6 , row 6 and 7, where i t is assumed that the equations of the new system, Eq. ( 5 . 5 ) , are known. However, i f the equations of the old system are known only, they can be transformed, v ia Eq. ( 5 . 8 ) , to those of the new system as stated e a r l i e r . The operation count for the l a t t e r transformation is shown in Table 5 .6 , row 5.

Elementary row operations are done on Eq. (5 .20) to f ind ana na ly t ic expression for the unknown constraint forces:

rlx = U Sl9 i2 + k2S2022 - - ^ --[klSimig - (li+ki)Simsg - ui + u2 ]

rly s h c1012 + k2C2022 " “a5‘-*[kl s l ml9 - Ol+kl)Simsg - ui + u2 ]

+ (-mi + m2)g

T2x = 63 + — + 8587----- — —---—13 a5 a5a9 a6

_ , , a4bc be - be (5 .21)2y “ 6,» + + a5a8 8 5 4 -- -s |e '

142

An operation count for computing the constraint forces via Eq. (5 .21) is performed assuming that qu an t i t ies , which are needed repeatedly, are stored in the memory of a computer. The results are shown in Table 5.6 row 1.

Upper bounds for the round-off error for the computed constraint forces are provided in Table 5 .7 .

5 .4 .3 One Link in Three Dimensions

The th i rd example is a one l ink system in three dimensions (Fig. 5 .1 ) . The one l ink system has i ts own body fixed coordinate system BCS with axes (X jg , X£B> *3b) and which moves r e la t iv e to an in e r t ia l coordinate system ICS with axes ( X j i , X 21 * X 3 j ) . The center of mass of th is system is located at the or ig in of the body coordinate system, which is aligned with the principal axes of the body [4 0 ] . I t is assumed that the one l ink is subject to three holonomic constra ints , i . e . , i t is hinged to the o r ig in . Furthermore, two addit ional simple nonholonomic constraints [14] are imposed on i t , i . e . , the rotat ional motion of the system is confined to rotations about the X j i axis . I t should be noted th a t , for the computation of constraint forces, the simple nonholonomic constraints can be treated as holonomic ones [4 0 ] .

I t is shown here that the computation of holonomic constraint forces is very e f f i c ie n t i f the th i rd method is used. Furthermore, the computation of the additional nonholonomic constraint forces does not s ig n i f ic a n t ly increase the number of operations involved, i f the th i rd method is used for th is purpose. The equations of motion of th is system are:

0 = BW• •X = X

(5 .17)JW = WW FW + M + At Ra + KK ATr

MX = G + r

where

9: angular ve locity vector in the ICS

W: angular velocity vector in the BCS

WW: matrix representation of cross product operator [14]

X: location of the center of mass of the body in the ICS.

A: map of vectors from BCS to ICS

Table 5 .7 . Upper Bounds for Round-Off Errors for Computed Constraint

Forces of the Planar Double Pendulum.

The parameters of the d ig i ta l computer simulation are:

mi

h

h

1 2

49 kg

2.35 kg m

0.312 m

0.2476 m

m2

h

kl

k2

7.63 kg

0.089 kg m

0.312 m

0.184 m

Calculation of

Constraint Forces

via the Method of

Single Precision (t=24) Double Precision ( t =54)

Relat iveError

RelativeResidual

Relat iveError

RelativeResidual

Smith (Eq. ( 5 .3 ) )

Hemami, (Eq. 5 .4 ) )

Kane, (Eq. 5 .7 ) )

< 340x10-6

< 40x10-6

< 2x10-6

< 12x l0 -8

< 12x10-8

< 12x10-8

< 3 1 0 x 1 0 - 1 6

< 4x10-16

< 2x10-16

< l l x l O - H

< l lx lO -1 7

< l lx lO -1 7

144

Figure 5 .6 . Three-Dimensional Pendulum with Body and In e r t ia l

Coordinate Systems Corresponding to

Axes (^10* x2B’ x3b ^^d ^21* ^31^ *

145

B: map of the vector W to the vector e

J: In e r t i a dyadic of the system

r: constraint forces due to holonomic constraints

A: constraint forces due to simple nonholonomic constraints

G: (0 , 0 , -mg)T: gravity vector

M: vector of external torques

The holonomic constraints of the system X + AK = 0 are d i f fe r e n t ia te d twice to a r r ive at:

X + A(WW)2k - AKKW = 0 (5 .18)

The two holonomic constra ints , which imply that the vector W is bothorthogonal to X21 and X31 , are formulated as

RTAW = 0 , with RT = [ q I 1 1 *

and are d i f fe r e n t ia te d once:

RTA(WW)W + RTAW = 0 (5.19)

The appl icat ion of the f i r s t method of ca lcula t ing constraint forces,Eq. ( 5 . 3 ) , gives

- I 0 M 0 r G

- kkaT - aTr 0 J A WW JW + M

0 0 I -A KK X -A(WW)2k

0 0 0 RTA w 0

(5.20)

The appl icat ion of the second method, as specif ied by Eq. ( 4 ) , renders

146

J - l - MA(KK)2AT - m akk at r

rt m a KK At M rT r

r

A

A(WW)2k + J -1 g - AKK(WWJW + M)

RTa M-^WWJW + M)(5.21)

For the appl icat ion of the th i rd method, Eq. ( 5 . 7 ) , a vector Z is defined as:

X = Z - AK

which introduces three addit ional degrees of freedom in to the constrained system. Then the new, la rger dimensional system reduces to the or ig ina l one by sett ing

Z = 0

The formulation of the th i rd method of ca lcu la t ing constraint forces

is :

- I 0 MA(KK)

0 -ATr J-M(KK)2

0 0 -RTa

0 0 0

MA(KK)2k2

-MKK (WW)2 K + WW JW + M

0

0

M r

- mkkaT A

0 w- I 1

(5.22)

I f the holonomic constraints of the pendulum are to be computed only, the rows and columns corresponding to the nonholonomic constraints are deleted from Eq. ( 5 .2 0 ) , ( 5 .2 1 ) , and Eq. ( 5 .2 2 ) . An operation count of

147

Table 5 .8 . Operation Count for Computing Holonomic Constraint Forces

of the Three Dimensional Pendulum

Total Number ofMethod ofComputation Additions M u l t ip l ica t ions Searches

Smith (Eq. (5 .3 ) ) 268 349 23

Hemami (Eq. ( 5 .4 ) ) 271 382 5

Kane (Eq. ( 5 .7 ) ) old system known (Eq. ( 5 . D )

461 435 5

new system known (Eq. ( 5 .5 ) )

41 15 5

148

Table 5 .9 . Operation Count for Computing Holonomic and Simple

Nonholonomic Constraint Forces of the Three Dimensional Pendulum

Total Number ofMethod of

Computation Additions M ul t ip l ica t ions Searches

Smith (Eq. ( 5 .3 ) ) 455 576 29

Hemami (Eq. ( 5 .4 ) ) 460 617 29

Kane (Eq. ( 5 .7 ) ) old system known (Eq. ( 5 .1 ) )

575 530 0

new system known (Eq. ( 5 .3 ) )

71 26 0

149

computing the holonomic forces is shown in Table R. An operation count for computing both the holonomic and simple nonholonomic constraint forces is given in Table 5 .9 . In Tables 5.8 and 5.9 the number of operations, which are necessary to i n i t i a l i z e the respective matrices are not l i s te d since both tables only demonstrate the additional computational e f fo r t of calcula t ing non-holonomic constraint forces.

5.5 Conclusi on

In th is paper three d i f fe re n t methods of computing holonomic constraint forces are discussed. The f i r s t method, as suggested by Smith, is suitable pr imarily for the analysis of linkage systems, since the unknown accelerations of the mechanical system are solved simultaneously with the unknown constraint forces. An analysis of the round o f f e rrors , which are due to the number representation in a d i g i t a l computer, reveals that the r e la t iv e error in computing the above forces is f a i r l y high for the two examples considered. I t is , there fore , appropriate to use a double precision format for programming the f i r s t method on a d ig i ta l computer.

The second method of computing constraint forces is suitable primari ly for control purposes, since the constraint forces are expressed as functions of the state and the input of the mechanical system. An operation count for th is method shows, that the computation of constraint forces is r e la t iv e ly e f f i c ie n t i f the in e r t ia matrix of the system is diagonal. Furthermore, i f the mechanical system s a t is f ie s the above condition and is , in addit ion , subject to few constraints, th is method is the most e f f i c ie n t one. The round o f f e r ro r , inherent in th is method, is considerably lower than the one of the f i r s t method.

The th i rd method of computing holonomic constraint forces is suitable for the analysis of control systems, since the system of equations is essent ia l ly solved for the unknown constraint forces. This method is computationally least costly for most of types of mechanical systems considered. The above method also has the smallest upper bound fo r round-off errors. I t is suitable for deriving analyt ical expressions for constraint forces, as w e l l . I t is computationally e f f i c i e n t i f simple nonholonomic constraint forces are computed simultaneously with holonomic forces.

The high cost of computing the constraint forces via methods I , I I , and I I I (provided that the system is known in the old formulation, Eq. ( 5 . 1 ) ) is due to the size of the matrices involved. I t is evident, however, that the computation of constraint forces via analyt ical expressions requires a minimal computational e f f o r t . The reason for th is is , that symbolic s im p l i f ica t ion is done when an analy t ical expression for constraint forces is derived, i . e . , many terms involving tr igonometric expressions are combined to a single expression.

150

Chapter 6

Discussions and Conclusions

6.1 Summa ry

In th is d isser ta t ion , the a t t i tu d e control of multi-1inkage systems is analyzed. In th is framework, the rotat ional dynamics of multi-body systems are derived, the c o n t r o l l a b i l i t y properties of these systems are studied, and th e i r graphical representation is investigated by means of a kinematic model. The studies in the above mentioned areas are done on d i f fe r e n t types of multi-body systems.

The kinematics of the above mentioned systems are explored on the model of a human-being represented by twenty-three body segments which are made up of wire-basket solids of revolution. For that purpose a computer graphics program is developed, which accepts the kinematic information of the humanoid as input parameters, solves the associated hidden surface problem, and does a transformation from three-dimensional world space to a two-dimensional image space. Although the graphics package consists of a set of elementary computer graphics functions, such as perspective transformations, open and closed polygon c l ipping, i t contains advanced algorithms, which perform hidden surface removal as w e l l . The graphics package is structured such that the objects are displayed on a vector display but can also be shown on a raster scan display in case of a f a i lu r e of the f i r s t mentioned output device.Major contributions of th is study in the area of computer graphics are the implementation and optimization of the above mentioned algorithms in a minicomputer environment and the d e f in i t io n of pa r t ic u la r data structure which accomplishes th is task. I t is shown that the humanoid can be represented by a set of convex polyhedra whose respective polygons are stored in polygon l i s t format. However, for processing the object through the Weiler Atherton hidden surface algorithm i t is advantageous to change the data structure to matrix interconnection format to speed up the computation of the f in a l display.

Two important issues in dynamics, the determination and systematic symbolic generation of the rotat ional equations of motion are invest igated. I t is obvious that the der ivat ion of the dynamics of multi-body systems is prone to human errors , espec ia l ly , i f the number of bodies involved are high. For that reason a general purpose symbolic manipulation program is employed which derives the above mentioned

151

dynamics automatically . I t is shown that the symbolic manipulation program can automatically check i t s e l f by deriv ing the dynamics via two d is t in c t approaches and comparing the resu lts . F i rs t in tegra ls of motion can be derived in a s imilar way.

The c o n t r o l l a b i l i t y of the rotat ional dynamics of a planar, two l in k system are analyzed using methods of l in e a r and nonlinear control theory. I t is shown that the above mentioned two approaches produce s im i la r resu l ts . I t is pointed out that the constant angular momentum constra in t , to which the above mentioned system is subjected, has quasi-holonomic character. This stems from the fact the quasi-holonomic constra int can be used to eliminate only one state var iab le of the system instead of two, which is the c h arac te r is t ic of a holonomic constraint in position state variab les . This corresponds to the loss of only ha l f a degree of freedom instead of the loss of one. The above mentioned quasi-holonomic constraint can be used to project the planar two-l ink system, which is p a r t i a l l y contro l lab le in the or ig inal space, to a reduced space, in which i t is completely contro l lab le .

The inverse a t t i tu d e control problem is defined for the planar two l ink system. I t is solved by transforming the corresponding i l l -posed boundary value problem into a well-posed one using a well-known method. D ig i ta l computer simulations are performed to demonstrate that l in e a r sta te feedback ensures that the nonlinear two l ink system is capable of t racking a prespecified t r a je c to ry . I t is shown that the above mentioned, p a r t i a l l y c on tro l la b le , unreduced dynamic system does not exh ib i t numerical in s t a b i l i t i e s which are induced during a numerical simulation i f the quasi-holonomic constraint is a l in e a r function of the s ta te variables of the system. In case of nonlinear constraints, addit ional hypothetical inputs have to be defined whose only purpose is to maintain the constraint and assure s t a b i l i t y .

The der ivat ion and computation of holonomic and simple nonholonomic constraint forces or torques in imbedded multi-body systems is studied. In fast and e f f i c ie n t computation of the above mentioned quant i t ies is an important issue in robotics applications under r e a l t ime constra ints . The analysis comprises a comparison of three d i f fe r e n t algorithms of computing the above type of constraint forces or torques. Several examples demonstrate merits of these d i f fe re n t techniques and compare them.

152

6.2 Recommendations for Future Research

The work outlined in th is d issertat ion can be extended in the fol lowing major areas:

1) Symbolic generation of the equations of motion:

Symbolic algebraic manipulation (SAM) programs can be e f fe c t iv e ly employed to derive the dynamics of multi-body systems. However, major shortcomings in th is appl icat ion are due to the f i n i t e amount of data space for the storage of intermediate resu lts . Hence, some research can be focussed in reducing the amount of th is space and, thereby, making the program more useful. This can be accomplished by devising additional rules which are c h arac te r is t ic of the system under in ves t igat ion , for example for r ig i d , pin-connected multi-body systems a charac te r is t ic re la t ionship is :

A = AWW (See Eqn. 5.18)

2) Automated c o n t r o l l a b i l i t y analysis:

The problem of a f i n i t e amount of data space occurs in the automated c o n t r o l l a b i l i t y analysis as w e l l . Hence, some research e f fo r t can be directed in making the analysis more e f f i c i e n t . Further research can be directed in developing an expert system for the nonlinear c o n t r o l l a b i l i t y analysis of mechanical systems in which a l l appl ic able c o n t r o l l a b i l i t y theorems for the above mentioned systems are implemented.

3) Graphical representation of multi-body system:

Movie l i k e images of multi-body systems can be produced by computing a set of postures o f f - l i n e by loading them in to the memory of the graphics generator, and by sequentia l ly blanking and unblanking the display f i l e s which contain the respective postures. Hence, some research can be focussed on devising a dedicated computer arch i tec ture for a real - t ime display of the motion of multi-body systems.

4) Numerical i n s t a b i l i t i e s during the simulation of constrained system:

Simulation of the unreduced, p a r t i a l l y contro l lab le planar two l ink system does not produce nor indicate numerical in s t a b i l i t i e s i f the respective constraints of the system are l in e a r functions of the s ta te variables of the system. Hence, future research can be directed in invest igat ing whether these findings hold for other types of constrained systems as wel l .

153

APPENDIX A

COMPUTER GRAPHICS PROCEDURES

In Appendix A program l is t in g s are provided for the procedures of the Graphics Package which is described in Chapter 3. The organization of th is appendix is as fol lows. In Appendix A . l , A.2, and A .3 program l i s t ings are provided for the basic graphics package, see Fig. 3 .2 , the modules which generate the humanoid, see Fig. 3 .20 , and the modules which accomplish the animation. F in a l ly , in Appendix A4 some help documents are provided for using the graphics program package.

A . l The Modules of the Basic Graphics Package

A program l i s t i n g of the basic graphics package, see Fig. 3 .2 , is given below:

< VERSION OF GRAPHMOD.PAS ! VERSION « 2 , JUNE 4 . 1966

{ * * * * * » * » » * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * ? * * * * * * * * * * * * * * * * * » » » * » :< GRAFHMOD. FAS c o n t a i n s e l e m e n t a r y g r a p h i c s s u b r o u t i n e s , wh i c h a r e w r i t t e n< i n PASCAL.< USAGE ! GRAF'HMOD. FAS i s i n c l u d e d i n t h e main p r o g r a m u s i n g t h e{ c o m p i l e r d i r e c t i v e * i d : g r a p h m o d . f a s■i COMPILATION ! MAIN p r o g r a m : name • pas{ WORN p r o g r a m : GRAPHMOD.F’AS<J- t i n s p a c k a g e c o n t a i n s t h e f o l l o w i n g p r o c e d u r e s\< PROCEDURE NAME , VERSION No. , DATE , MODIF ICATIONS

G l o b a l 1 J a n - 7 - B 6< I n i t F e r s p e c t i v e 1 F e b - 1 5 - 8 6< I m t G r a p h i c s 1 F e b - 1 5 - 0 6< G e n C y l i n d e r 1 J a n - 2 0 - 6 6■C Ge n S p he r e 1 J a n - 2 1 - B 6

■c MoveTo 1< L i n e T o 1\ O p m S e g m e n t 1< C l o s e S e g m e n t 1■C P o s t S e g m e n t 1{I

U n P os t Se gm e nt 1

< C o n c a t F i l e 1 F e b - 1 0 - 6 6\ T r u n _ R o t 1 F e b - 0 7 - 8 6< P r M a t V e c 1

P r M u t M a t 1< UmaL3 1t CupyMat 1< CopyVec 1\ V e tA ddV ec Sub Ve c 1<

154

{ p r o g r a m s i e r } d i t l t r l a n q e r{ d o t e 7 - J a n - 8 6{ f u n c t i o n ! t h i s f i l e p r o v i d e * t h e g l o b a l v a r i a b l e s , wh i ch< can be c o m p i l e d w i t h a l l p r o c e d u r e s{ d e f i n i t i o n s I

CONS'.p i = 3 . 1 4 1 5 9 2 6 5 4 } e p s i l o n * l E - 5 }

7YFEa r r 3 i * A R R A Y 1 1 • . o r r 3 r e A R R A Y t l . , a r r 4 r«ARRAYC1 . . a r r 4 i - A R R A Y C l . . u r r 5 i “ ARRAYC1 . . a r r 6 r “ A R R A Y t l . . u r r e c = A R R A Y C l . . a r r l O i “ ARRAYC1 • a r r 5 0 r “ ARRAYt1 • a r r 2 k 4 i “ ARRAYCl . a r r 3 K 3 r * A R R A Y C l . a r r 2 h 4 r “ ARRAYCl . a r r 4 k 4 r “ ARRAYCl . o r r 4 k 4 i » A R R A Y C 1 . . 4 , l a r r 2 0 K 5 r - A R R A Y C l . o u t c o d e “ ARRAYC1 • • s t r “ s t r i n g C 2 0 3 } s t r 2 “ s t r i n g C 2 3 } i t r f ' s t n n g [ 4 1 } s t r 0 “ s t r i n g C B 3 } s t r l 4 “ s t n n g E 1 4 3 }

33 OF i n t e g e r }33 OF r e a l !43 OF r e a l }43 OF i n t e g e r }33 OF i n t e g e r }63 OF r e a l !63 OF c h a r }• 1 0 3 OF i n t e g e r }. 5 0 3 OF r e a l }. . 2 , 1 . . 4 3 OF i n t e g e r }

33 OF r e a l }43 OF r e a l }43 OF r e a l }43 OF i n t e g e r }

. 2 0 , 1 . . 5 3 o f r e a l } 43 OF b o o l e a n }

. 3 , 1 .

. 2 , 1 .

. 4 , 1 .

OARc o i , e o } u r r 3 r } { c e n t e r o f i n t e r e s t , e y e p o s i t i o n >v a d e g , a r , h , y o : r e a l } { v i e w i n g a n g l e t d e g . ) , a s p e c t r a t i o , h i t h e r , yon> v i e w o o r t } a r r 4 1 }K i p , y i p , >;2p , y2p 1 i n t e g e r } f r o n t o i t /ucKf u c e l c h a r }t e s t p r o y r a m . p a r m i n i t , m a p W l d t o S c r r e a d y I b o o l e a n } { t e s t o f t h e w h o l e p r o g r a m >

{ s e t by S Y I N I T i f p e r s p ’ { m e t e r s a r e i n i t i a l i z e d

p l o t G t o r F i l e o r F c } c h a r } { used i n a l l p l o t r o u t i n e s >i n p , o u t } t e n t }d i i v e _ s i s t , d r i v e _ w i d , d r i v e _ p l t , d r i v e _ d a t : s t r 2 }

{ d i s k d r i v e s f o r s i s t e r , w o r l d , p l o t and d a t a f i l e s > w * ! a r r 4 k 4 r } { sap ULB — .> SCR >

v p o r t : a r r 4 i }u u s t X F l o t t e d , L a s t Y F l o t t e d I i n t e g e r } { used i n MoveTo, L i n e T o > ok .‘ b o o l e a n 1

i p r o g ra m me r { d a t e { f u n c t i o n

d i e t e r l o n g e r >1 5 - f e b - B o J

i n i t i a l i z a t i o n o f p a r a m e t e r s f o r p e r s p e c t i v e t r a n s f o rmat l on>i anc o f c o n t r o l s w i t c h e s f o r t h e s ys t em

? RCCEIiUF.E I i n t F e r s p e c t i ve }OAF

. n , cinticn . cna r } r a o i u s , t n e t a , p h i , : ; ; r e . , ;

155

r EG I N p i 1 - 3 . 14 1C 927 ;w i t t l n ' ' S u b r o u t i n e I n i t f p r s p ^ c t i v e ' ) {u r l t e l n < ' ' ) ;w r i t e ! ' v i e w i n g a n g l e i d e g . ) = 7 ’ ) { r e a a In ! vadeg ) :w r i t e ! ' e y e p o s i t i o n ! r o d i u c « ? ' ) J r ead I n ! raci i u s

R e p e a tu n t e l n ;u n t j l n l ’ i n p u t o f ! t h e t a . p h i > o r ' ) {u n U ( ' ( t o p r s i d e . f r o n t v i a u ) CT. V3 * T ' ) }r e a d 1n ! t h ) {

U n t i l cl, i n C ' T ' , ' V ' 3 {Cube ch o f

' T ' ! b e g i nREPEAT

w n t e ( ' e l e v a t i o n a n g l e CO. . 9 0 1 * ? ' ) {r e ad I n ( t h e t a > >

UNTIL ! ! t h e t a : > 0 . 0 ) A N t i ! t h e t a < 9 0 . 0 ) > {

REPEATw r i t e ! ' a z i m u t h a n g l e C O . . 3 6 0 3 • T ' ) »r e ad I n ( p h i >

UNTIL ( < p h i 1 0 . 0 ) ANCK p h i < 3 6 0 . 0 ) ) e n d ;

' O ' ! b e g i nR e p e a t

w r i t e ! ’ F r o n t . Top* o r S i d e v i e w C F . T . S 3 “ ? ' ) {r e a d I n ( c n d c h ) 1

U n t i l cedch i n C ' F ' . ' T ' . ' S ' 3{

C a t e cmdch o f ' F ' : b e g i n

t h e t a : - B 9 . ? ? { p h i ; « 0 . 1 end i

' T ' I b e g i nt h e t a : « 0 . 1 i p h i : « 0 . 1

end !' S ’ ! beg i n

t h e t a ! * 8 9 . 9 9 J p h i : - 8 9 . 9 9 end

end { c a t e s t a t e m e n t > end 

e n d ; { c a t e s t a t e m e n t >

t h e t a : « t h e t a * p i / l 801 p h i ! - p h i * p i / I B O t

e p C 1 3 : = r a d i u s * s i n ( t h e t a ) * s i n ;e p C 2 3 : w r a d i u s * c o s ( t h e t a > ;e p i 3 3 : « r o d i u s * 6 i n ! t h e t a > * c o s ! p h i > ;

REPEATw r i t e ! ' m i t . o f o t h e r p e r s p . p a r a m e t . CY/N3 7 ' ) ; r e a d l n <cmocn) j

U NTI L cmdch IN C ' Y ' , ' N ' 3!CASE cmdch OF

' Y ' :£<EGIN

w r i t e ! ' a s p e c t r a t i o C G T I 1 . 0 { F C I 1 . 2 A 3 = 7 ' ) { r e a d l n ,w r i t e ! ' c e n t e r o f i n t e r e s t I >;-comp . =1 ' ) ! r e ad 1 r, * c o i C :w r i t e ! ' c e n t e r o f i n t e r e s t I y - c o m p . = 7 ' >» r ead 1 n ! c o i Ll

156

w r i t e ! ' w r i t e ! ' w r i t e ! '

c * n t » r o f i n t e r e s t * 1 - c o u p . * ’ ' ) i r e a d I n ! c o i I ~h i t h e r p l a n e « T ' ) i r t o o i n ( h ) ;y o n - p l a n e « ? ' ) ( r e a d l r , < y o ) ;

t e s t p r o g r a m ! « f a l s e i

REPEATu n t i l ' p l o t on G t f o r F c . o r w r i t e t o F i l e C G / F / F 3 ? ' ) i r e o d l

UNTIL ch I N I ' G ' , 'F ' , ' F ' 3 I F l o t G t o r F i l e o r F c ! “ ch

ENI ' ;N ' t

REGINa r: *1 : coici3!'0;coiC23:=0;coiC33:=0;h:!=0.5;yo'.- 50C1;

t e s t p r o g r o n , : » f o l s e ! F l o t G t o r F i l e o r F c : i= ' F 'ENI'

e n g ;

w r i t e l n 1REF EAT

w r i t e ! ' f ron t o r b o c K f oc e CF / B / A ! ( “ F + B) 3 «? ' ) { r e a d l n ( f r o n t o r tUNTI L f r o n t o r b a c k f a c e I N I ' F '» ' B ' , ' A ' 3 J

\ o t h e r f i x e d p a r a m e t e r o f t h e g r a p h i c s t e r m i n a l > i f ( ( F l o t G t o r F l l e O r F c * ' F ' ) o r ) t h en

b e g i nv i e w p o r t C 1 3 ! * 0 ; v i e w p o r t C 2 3 ! “ 6 4 0 } v i e w p o r t C 3 3 ! “ 0 ! v i e w p o r t 1 4 3 ! « 2 0 0 (

end ELSE

b e g i nv i e w p o r t I 1 3 ! m0 ; v i e w p o r t C 2 3 ! “ 1 0 2 0 1 v i e w p o r t C 3 3 J » 1 0 2 0 } v i e w p o r t C 4 3 ! = 0 ;

end ipa rmi i i i t ! * t r u e » m a p U l d t o S c r r e a d y ! “ f a l s e < map I ULD - - 3 SCR must be i n i t i a l i z e d >

ENl ' i 

< f u n c t i o n : i n i t i a l i z a t i o n o f g r a p h i c s t e r m i n a l >< d a t e : 1 5 - f eb-B6 >

PROCEBURE In i t G r a p h l c s i

VARe t x , d c 4 , c r f l f : c h a r ;

BEGINe t x : = c h r < 3 ) } d c 4 : « c h r < 2 0 )» c r ! » c h r ( 1 3 ) i l f : « c h r ( 1 0 ) ;c a s e P l o t G t o r F i l e o r p c o f

C ' i b e g i nw r i t e l n ! l s t . e t ; : . d c 4 , c r t l f > ; w n t e l n ( 1 s t . ' en.: en ! e x ! sr. ! s x ! u s ! ' >

end i ' F ' : bea in

w r i t e l n ( o u t , e t ; ; . d c 4 , c r i l f > ) w n t e l n ( o u t , ' en.1 i n ! ex! s n! si : ! uni! ' )

end •' F ' ! b e g i n

111 r es i h i r e s c o l o r ( y e l l o w ) < 6 4 0 * 2 0 0 d o t s b l a c K + one c o l o r > end

end < c a s e >e n d ;

157



PROCEDURE e l l i p s e t x a x i s l e n . z a x i s l e n l r e a l i n d i v l i n t e g e r ; V h R x c o o r d . y c o o r d l a r r C C

p i l r e a i ; i l i n t e g e r ;

BEGINp i ! = 3 . 1 4 1 5 9 2 7 t FOR i I = 0 TO n d i v - 1 DO

DEGIN< c o o r d t i + l ! i : = x a : . i s l e n * c o f s ( 2 * n i / n d i v * i ) ; y c o o r d C i + l T : = z a x i * l e n » s i n < 2 » P i / n d i v » i ;

ENDEND; < e l l i p s e >

PROCEDURE g e n c y l i n d e r C x a x i s t o p . x a x i s b o t t o m . z a x i s t o p , z a x i s b o t t o m . h e i g h t . ' r e a l ;n d i v h o r i z o n t a l . n d i v v e r t i c a l ! i n t e g e r ! d r i y e : s t r 2 ; o u t f i l e ; * t r B ;

e x t e n s i o n : s t r 4 >;

n p o l y , n e d g e , n v e r t » i , o > K : i n t e g e r ( x c o o r d , y c o o r d i a r r S O r i x a x i s a c t u a l . z a x i s a c t u a l , yac t u a 1 ! r e a 1 i v t ! a r r 3 r ; p n : a r r 5 i ; o u t p u t f i l e ; * t r ;

DEGIN { g e n c y l i n d e r >

n p o l y : = n d i v v e r t i c a l * n d i v h o r i z o n t o l ; n v e r t : = ( n d i v v * r t i c a l + l ) * n d i w h o r i z o n t a i ; n e d g e .' *A ;

o u t p u t f i l e : = c on c a t ( d n v e , o u t f i l e , e x t e n s i o n ) J a s s i g n t o u t . o u t p u t f i l e ) ; r e w r i t e ( o u t ) ;u r i t e ( o u t , n v e r f . 6 ) ; w n t e ( o u t , n p o l y : 6 > ; w r i t e l n ( o u t ) ;

FOR i ! = 0 TO n d i v v e r t i c a l DO DEGIN

x a x i s a c t u a l ! = x a x i s t o p + ( x a x i s b o t t o m - x a x i s t o p ) / n d i v v e r t i c a l » i j z a x i s a c t u a l I = z a x i s t o p + ( z a x i s b o t t o m - z a x i s t o p ) / n d i v v e r t l c a l * l l y a c t u a l ; « h e i g h t / 2 - h e i g h t / n d l v v e r t l c a 1 * i (

{ c o m p u t a t i o n o f v e r t i c e s >

e l l i p s e ( x a x i s a c t u a l , z a x i s a c t u a l , n d i v h o r i z o n t a l , x c o o r p , y c o o r d ) ;

FOR .j I = 1 TO n a l v n o r l zon t a l DO DEGIN

v t C l i : = x c o o r d C j 3 ; v t C 2 3 i “ y a c t u a l ; v t C 3 l : = y c o o r d [ j l ;w r i t e ( o u t , v t t l 3 : 1 2 1 3 ) ; w r i t e ( o u t , ' ' >;

w r i t e ( o u t , v t C 2 3 : i 2 ; 3 ) ; w r i t e ( o u t , ' )J w r i t e ( o u t , v t C 3 3 ! 1 2 : 3 ) i u r i t e l n ( o u t ) ;

UhR

VhR

ENDe n d ;

158

\ c o m p u t a t i o n o f p o l y g o n f ^ c t « >

FOD i : “ 1 TO n d i v v e r t i c a l DO DEGIN

FOG J ! = l TO n d i v h o n z o n t o i DO DEGIN

K : * ( i - l ) * n d i v h o r i z o n t o l + J ! p n C 1 3 : *>nedge;p n C 2 3 : = K ; p n C 3 3 : * K + n d i v h o r i z o n t o l i I F ( j ” n d l v h o r l s o n t a 1)

THENDEGIN

p n C 4 3 : * = i * n d i v h o n z o n t Q l + i ; p n C G 3 ; « ( i - l > * n d i v h o n z o n t Q l + l

END ELSE

DEGINp n C 4 3 ! * p n C 3 3 + l ! p n C 5 3 ! “ p n C 2 3 + l

e n d ;u r i t r < o u t i p n E 1 3 : 6 ) i u r i t « ( o u t r p n E 2 3 ; 6 > ;

w n t e ( o u t » p n C 3 3 : 6 > ; u r i t e < o u t » p n C 4 3 : & > ; u n t f i o u t , p n [ j ] ; 6 ) ! u n t * l n ( o u t ) I

ENDe n d ;

c i o s e ( o u t ) < c l o s e f i l e OUT >

END; < g e n c y l i n d e r >

159

{ p r o g r a e e e r : d i e t e r l o n g e r >( d o t e : 2 1 - J o n - B 6 >{ f u n c t i o n : g e n e r a t e e a s t e r c o o r d i n a t e s f o r p l o t t i n g a s p h e r e >

PROCEDURE g e n s p h e r e ( nd i v h o r i z . nd i v v e r t d i v 2 ! i n t e g e r ) r a d i u * ! r e a i ; d r i v e : s t r 2 ; o u t f 1 l e J » t r 6 l e x t e n s i on : s t r 4 ) va r ou t ! t e x t > i

VARx . y . z . t h . p h i l r e a l ; n v e r t . n p o l y l i n t e g e r !1 . J » k : i n t e g e r ; p ! a r r S i > p i ! r e a l Ii n p u t f i l e . o u t p u t f i l e t s t r ;

PROCEDURE s p h e r e ( r a d i u s f t h , p h i : r e a l ; VAR >: , y , z : r e a l ) ;DEGIN

x ; = r a d i u s * s i n ( t h ) * s i n ( p h i > ;z ! = r a d i u s » s i n ( t h ) * c o s ( p h i > ;y : « r o d i u s * c o s ( t h )

e n d ;

PEGINp i ; « 3 . 1 4 1 5 9 2 6 5 4 Jn v e r t ! ■ ( n d i v v e r t d i v 2 - l l i n d i v h o n z t i ; n p o l y : » n d l v v e r t d i v 2 * n d i v h o r i z >

o u t p u t f i l e : = c o n c a t ( d r i v e , o u t f i l e . e x t e n s i o n > ; a s * i g n ( o u t . o u t p u t f i l e > ; r e w n t e ( o u t ) ;

w r i t e ( o u t » n v e r t ! 6 ) ; w r i t e ( o u t . n p o l y ! 6 > ; w r i t e l n ( o u t > ;

x : - o ; y : » r o d i u s ! z : - o ; u r i t e ( o u t . x : 1 2 ! 3 ) J u n t e ( o u t . ' ' ) ;w r i t e ( o u t . y : 1 2 1 3 ) j u r i t e l o u t . ' ' ) } u r i t e ( o u t . z : i 2 : 3 ) i w r i t e l n ( o u t > ;

FOR i S « l TO n d i v v e r t d i v 2 - l DO PEGIN

t h : = p i / n d i v w e r t d i v 2 * i ;FOR j t c O TO n d i v h o r i z - 1 DO

PEGINp i n : e 2 * p i / n d i v h o n z * J . s p h e r e ( r Q d i u s , t h , p h i , x , y , z ) ; w r i t e ( o u t , x : i 2 : 3 > ; w r i t e ( o u t t ' ' ) ;

w r i t e ( o u t . y : i 2 : 3 > . w r i t e ( o u t . ' ' ) ; u r i t e ( o u t . z ! 1 2 : 3 ) i u n t e l n t o u t ) i

ENDe n d ;

> : ! = 0 ; y ; = - r o d i u s ! z ! e 0 ; w r i t e l n ( o u t , x : i 2 : 3 i y : i 2 : 3 , z : i 2 : 3 > ;

{ c o m p u t a t i o n o f p o l y g o n l i s t >

FOR l : = 1 TO n d i v v e r t d i v 2 DO PEGIN

FOR j ; = l TO n d i v h o n z DO PEGIN

I F ( i = l )THEN

PEGIN { f i r s t s e t o f t r i a n g . f a c e s > p : i 3 ; « 3 { p C 2 3 ! « i ; p C 3 3 ; » j + i ;

160

I F <J “ n d l v h o r i z )THEN

PEGIN pC43 i «2

ENP ELSE

PEGINp [ 4 3 : = .j + 2

e n p ;w r i t e ( o u t r p E 1 3 ! 6 ) i w r i t e ( o u t . p C 2 3 : 6 ) i

w r i t e ( o u t » p C 3 3 ; 6 > i u r i t e < o u t . p C 4 3 : 4 > i i j r i t e l n ( o u t > (ENP < f i r s t s e t o f t r i o n g . f a c e s >

ELSEPEGIN

I F < a»nd 1 w e r t d 1 v 2 )THEN

PEGIN < t e r m i n a l s e t o f t n a n g . f a c e sp [ 1 3 i ” 3 ( p C 2 3 1 “ < nd i v v e r t d i v 2 - 2 ) 4 n d i v h o n z + l + J i p C 3 3 ! “ n v e r t i I F ( j “ n d i v h o n z >

THENBEGIN

p [ 43 i “ (nd i v v e r t d i v 2 - 2 ) * n d i v h o r i z + 2 ENP

ELSE BEGIN

p C 4 3 : « p C 2 3 + lENPJ

w r i t e ( o u t f P t l 3 : 6 ) > w r i t e ( o u t ' p C 2 3 : 6 ) iw r i t e ( o u t ' p [ 3 3 : 6 ) ; w r i t e ( o u t f p [ 4 3 : 6 ) > u r i t e l n ( o u t ) ;

ENP < t e r m i n a l s e t o f t r i o n g . f a c e sELSE

PEGIN p C 1 3 : “ 4 ; p C 2 3 ! “ ( i - 2 ) * n d i v h o r i z + l + j ; p [ 3 3 ! “ p C 2 3 + n d i v h o r i z {I F ( j = n d l v h o r i z )

THENBEGIN

p [ 5 3 : “ ( i - 2 > * n d i v h o n z + 2 !p [ 4 3 : c p C 5 3 + n d i v h o r i z

ENPELSE

PEGINp C 5 3 ! “ p C 2 3 + l i p [ 4 3 ! “ p C3 3 + l

e n p ;w r i t e ( o u t r p E 1 3 : 6 ) i u r i t e ( o u t , p E 2 3 : 6 ) >

u r i t e ( o u t . p C 3 3 : 6 ) ; u r i t e ( o u t . p C 4 3 : & ) ;w r i t e ( o u t . p C 5 3 : 6 ) i u r i t e l n ( o u t ) i

ENP

ENPENP

ENPi c i o s e ( o u t >

ENPi

161

 r o g r a e e e r ! d i e t e r l o n g e rf d a t v ! 1 6 - j a n - 8 &f f u n c t i o n : g r a p h i c p r i m i t i v e *

>>>

FROCEI'UFiE m o v e t o ( x » y ! i n t e g e r >5E-EGIN

c a s e F I o t G t o r F i l e o r F c o f' G ' I H r i t e ( L S T t ' p e 0 i ! p i i ' i x M i ' ( ' i y ! 4 i , | : p e l i ! ' ) i ' F ' : w r i t e C OUT, ' p e O i ! p o ' i n ! 4 i ' i ' » r M > ' ( ! p * l t • ' ) ! ' F ' : beg i n

L a s t X F l o t t e d : = x } L a s t Y p l o t t e d : » y end

e n d f l oBe >ENU;

PROCEDUFE 1 i n e t o ( x , y I i n t e g e r ) J KEGIN

c o s e P l o t G t o r F i l e o r P c o f' G ' ! beg i n

w r 1 te < L S T » ' pa ' » x J 4 . ' , ' »y ! 4 » ' > ' > S w n t e l n ( L S T ) e n d ;

'F ' I b e g i nw r 1 t e ( OUT > ' p a ' » x S 4» ' » '» y S 4 r ' S ' > S w r i t e l n ( O U T )

end i' P ' S b e g i n

d r a w ( L a s t X P l o t t e d » L a * t Y F l o t t e d , x , y r l ) ;L a s t X F l o t t e d ! * X S L a s t Y F l o t t e d : « Y

ende n d f c o se >

e n d ;

p r o c e d u r e O p e n S e g m e n t t l ! i n t e g e r ) , v a r o u t f i l e I s t r l 4 S

v o l u e _ o f _ i : * t r 2 ( beg i n

f open o u t p u t f i l e S S EG Mi .F L T >

s t r ( i j v a l u e _ o f _ i ) !u u t f i l e ! * c o n c a t ( d r i v e _ p H i ' s egm ' , v a l u e _ o f _ i , ' . p i t ' > S a t s i g n l o u t r o u t f l i e ) i r e u r i t e ( o u t ) i

c ose F l o t G t o r F i l e o r P c o f ' G ' : b e g i n

u n t i l 1 s t . ' n f ' > i 14 . ' . ! ' ) S w r i t e l n ( l s t ) end i

' F ' S b e g i nu r i t e l o u t i ' n f ' f i ; 4 . ' . I ' ) l u n t e l n ( o u t )

e n d ;'F ' : b e g i n end < no s e g m e n t a t i o n > end-C c a s e >

eno ;

p r o c e d u r e C l os eS egmen tS b e g i n

c a s e F l o t G t o r F i l e o r F c o f ' G ' ! b e g i n

u n t e l l i t . ' sn I ' ) S w n t e l n ( l s t ) end i

162

F ' : b e g i nw r i t e ( o u t • ' * n : ' ) { w r i t e l n < o u t )

e n d ;' F ' : b e g i n end < no s e g m e n t a t i o n >

e n d i < c a s e >

f c l o s e OUTFUT f i l e > ‘c l o s e ( o u t )

end ;

p r o c e d u r e P o s t S e g m e n t ( i ! i n t e g e r > { beg i i i

ca s e F l o t G t o r F i l e o r F c o f ' G ' ! beg i n

w r i t e ( l s t r ' u f ' . i : 4 f ' . ! ' ) } u r i t e l n ( l s t ) end i

' F ' : b e g i nu r i t * ( o u t r ’ u f ' i i ! 4 i '■ ! ' ) i u n t e l n ( o u t )

e n d ;F ' : b e g i n end f no s e g m e n t a t i o n >

e n d f c a se >e n d ;

p i o c e a u r e Unh o s t S e g m e n t < i 1 1n t e g e r ) J b e g i n

c a s e F l o t G t o r F i l e o r F c o f ' G ' ! beg i n

u r i t i ( l s t i ' b f ' i i ! 4 i M ' K u n t e l n d s t ) e n d ;

'F ' ! b e g i nw r i t e ( o u t » ' b f ' , i : 4 , ' , I ' ) { w r i t e l n ( o u t )

end {' P ' S b e g i n end < no s e g m e n t a t i o n >

e n d f c a s e >ena {

p r o c e d u r e I i e l e teSeg' . - . - . - t < 1 ! i n t e g e r ) { beg i n

c a s e F l o t G ' . r : r i l e o r F c o f ' G ' { beg i n

w r i t e ( 1 s t > ' e f ' » iS 4 f ' , S ' ) { w r i t e l n ( l s t ) end {

' F ' I b e g i nw n t e ( o u t r ' e f ' . 1 14 , ' , : ' ) { w r i t e l n ( o u t )

ena !' P ' S b e g i n end < no s e g m e n t a t i o n >

e n d f c a s e >end {

\ p r og ra mm er f u a t e f f u n c t i o n

d i e t e r l o n g e r 1 0 - f e b - 8 6r e ad f i l e n a m e _ i n l , ad d o f f s e t l t o t h e v e r t i c e s r e ad f i l e n a m e _ i n 2 , add o f f s e t s t o t h e v e r t i c e s copy b o t h f i l e s i n t o f i l e na me_out

p r o c e d u r e c o n c a t f i l e s ( d n v e . i n l : s t r 2 { n a m e _ i n l S s t r 8 ; e : : t _ i n i : s t r 4 { d r i v e _ i r i 2 i s t r 2 ;n a m e _ i n 2 S s t r B S e x t _ i n 2 S s t r 4 ; d r i v e _ o u t S s t r 2 { n a m e _ o u t i s t r B 5 e : - : t _ o u t S s t r 4 ! o f f s e t l i o f f s e t 2 ! a r r 3 r ) {

163

v o r i n p 1 , 1n p 2 r o u t ! t e x t !i n p l f i l e r i n p 2 f i l e « o u t f i l e • s t r ii f . j » n v * r t l . i w * r t 2 » n p o l y l t n p o l y 2 » n v e r t o u t . n p o l y o u t l i n t e g e r } v t ! a r r 3 r } p n ! a r r l 0 i ;

b e g i ni n p l f i l e : « c o n c a t < d r i v e _ i n l i n o i n e _ i n l , e > : t _ i n l > { a s s i g n ii n p 2 f i l e . ' « c o n c a t < d r i v e _ i n 2 r n a e e _ i n 2 > e x t _ i n 2 >} a * s i g n ( i n p 2 » i n p 2 f i l e > ! r e s e t }

r e a d l r e a d { r e a d I n }r e a d ( i n p 2 t n v e r t 2 ) ! r e a d ( i n p 2 f n p o l y 2 ) } r e a d l n ( i n p 2 > ;n v e r t o u t : “ n v e r t l + n v e r t 2 ! n p o l y o u t : = n p o l y l + n p o l y 2 ;w r i t e ( o u t , n v e r t o u t 1 6 ) ( w r i t e ( o u t , n p o l y o u t ! 6 > ; w r i t e l n ( o u t > (

< copy v t r t i c e i o f l e t f i l e + o f f s e t l i n t o o u t f i l e >

f o r i ! * l t o n v e r t l do b e g i nr e a d ( i n p l » v t C 1 3 ) i r e a d ( i n p l r v t C 2 3 > ! r e a d ( i n p l » v t C 3 3 ) t r e a d l n < i n p l ) iv t C n : « v t C 1 2 + o f f s e t l C l D ; v t C 2 3 : = v t C 2 D + o f f * e t l C 2 3 }v t C 3 D : = v t C 3 2 + o f f » e t i r 3 i ;w r i t e ( o u t » v t C l l ! B : 3 ) ; w r i t e < o u t » v t C 2 D ! B ! 3 ) ; w r i t e ( o u t i v t t 3 3 : 8 : 3 ) ; u n t e l n ( o u t end i

{ copy v e r t i c e s o f 2nd f i l e + o f f s e t 2 i n t o o u t f i l e >

f o r x :=1 t o n v e r t 2 do b e g i nr e a d ( i n p 2 » v t t l 3 ) { r e a d ; r e a d ; r e a d l n ! v t C l i : - v t C 1 3 + o f f s e t 2 i : i 2 ; v t t 2 a : « v t C 2 3 + o f f s e t 2 C 2 D ; v t L 3 3 : = v t [ 3 3 + o f f s e t 2 C 3 3 {« r i t e < o u t * v t E 1 3 1 B 1 3 > * u r i t e < o u t ? v t I 2 3 1 8 : 3 ) { * i r i t e < o u t . v t [ 3 3 : 8 : 3 > ; w r i t e l n ( o u tend

t copy p o l y g o n s o f 1 s t f i l e i n t o o u t f i l e >

f o r i i = l t o n p o l y l do beg i nr e a d ( i n p l f p n C 1 3 > r w n t e ( o u t > p n C 1 3 : 6 > > f o r J ! = 2 t o p n C 1 3 f l do

b e g i nr e a d i i n p l i p n C j l l S w r i t e f o u t r p n C J l i d ) end i r e ad I n ( i n p l > ; w r i t e l n ( o u t >

end ;

< copy p o l y g o n s o f 2nd f i l e i n t o o u t f i l e >

f o r i ! « l t o n p o l y 2 do b e g i nr e a d ; f o r u ! t 2 t o p n [ 1 3 f l do

beg i nr e u O < i n p 2 , p n C . j 3 ) i p n C J 3 ! « p n C u 3 + n v e r t i ; u r i t e ( o u t » p n C : u 3 : 6 ) e n d j r e a d l n ( i n p 2 ) i u n t e l n ( o u t )

end 1

164

c 2 o s e ( i n p l ) { c l o s e ( i n p 2 > < c l o s e ( o u t >• n d 1

p rog ra mm er ! d i e t e r l o n g e rd o t e ! 7 - f e b - 0 £ >

f u n c t i o n ! t h e body i s r o t a t e d u s i n g m a t r i x r o t m a tond t r a n s l a t e d by o r i g t h e body i s d e s c r i b e d by a f i l e

d r i v e l i n p f i l e . m s t t h e t r a n s f o r m e d body i s s t o r e d i n t h e f i l e

d r i v e ! i n p f l l e . w l d

p r o c e d u r e t r a n _ r o t ( o r i g ! a r r 3 r 1v a r r o t m a t ! a r r 3 K 3 r ) !

i i J . K , n p o l y i n v e r t ! i n t e g e r ( v t , v t n e u ! or r 3 r ! p n ! a r r 3 i 1

BEGIN

< r e ad i n v e r t i c e s >

r e a d ( i n p , n v e r t ) t r e a d ( i n p , n p o l y ) ! r e a d I n i w r i t e ( o u t » n v e r t ! 6 > } w r i t e ( o u t , n p o l y ! 6 > S w r i t e l n < o u t > ;

f o r 1 ! * 1 t o n v e r t do b e g i nr e a d » r e a d ( i n p » v t [ 2 3 ) } r e o d ( i n p , v t C 3 3 > ; r e a d l n ( i n p ) i f o r j ! «1 t o 3 do

b e g i nvt newEd 3 ! * 0 J f o r K ! « l t o 3 do

beg i nv t n e u C j l ! « v t n e w C J l t r o t i n a t C u , k l S v t C K l

end i< add v e c t o r r l t o t r a n s f o r m e d v e r t i c e s >v t n e u C J l i ^ v t n e u C J D t o n g C J le n d ,

w r l t e < o u t , v t n e w E 1 3 ! 1 2 ! 3 ) l w r i t e ( o u t , ' ' ) J w n t e ( o u t , v t n e w E 2 1 ! 1 2 1 3 ) !w r i t e ( o u t , ' ' > ; u n t e ( o u t , v t n e w C 3 2 ! 1 2 ! 3 ) J w n t e l n ( o u t ) end !

< copy p o l y g o n i n d e c e s i n t o o u t p u t f i l e >

f o r i ! » l t o n p o l y do beg in

r e a d ( i n p , p n C 1 3 ) J u r i t f ( o u t , p n [ l ] ; 6 ) |f o r K ! “ 2 t o p n C l l + 1 oob e g i n

r e a d » w r i t e < o u t , p n [ K 3 ! 6 >• n d !r e a o l n ( i n p ) } w r i t e l n < o u t )

end

< d a t e ! 1 0 - f e b - B 6 >{ f u n c t i o n ! m a t r i k s u b r o u t i n e s >

165

p r o c e d u r e p r e o t v e c ( e a t ! a r r 3 K 3 r i v e c • o r r 3 r ! v o r r t » ! o r r 3 r ) I

< function ! product of a ( 3 x 3 ) Matrix with o ( 3 x 1) vector >

v a r r o w , c d I ! i n t e g e r ;

beg i nf o r r o x ! * l t o 3 do

beg inr e s t r o w ] I « 0 ( f o r c o l l ' l t o 3 do

b e g i nresC r o w ! « r e s C r o w 3 + e a t C r o w , c o l 3 * v e c Ccol 3 end

endend i

p r o c e d u r e p r e a t e a t <M a t f i r s t . m a t s e c ! a r r 3 K 3 r i v a r e a t r e * : a r r 3 K 3 r ) !

< func t i o n ! p r o d u c t of a ( 3 x 3 ) Mat r i x with a ( 3 x 3 ) M a t r i x >

v a r r o w , c o l . i ! i n t e g e r ;

b e g i nf u r r o w : = l t o 3 do

beg inf o r c o l l ' l t o 3 do

beg inM a t r e s C r o w r C O l D i - O t f o r i t ■= 1 t o 3 do

b e g i n» u t r e * [ r o w , c o l 3 ! » m a t r e s [ r o w » c o l 3 + i i i a t f i r s t C r o w , i 3 * i i i a t s e c [ : i r C o l 3end

endend

end t

p r o c e d u r e u«iat3< v a r » a t : a r r 3 k 3 r >;

< f u n c t i o n : * e t t i n g up o u n i t y M a t r i x >

v a r r o w r c o l : i n t e g e r >

b e g i nf o r r o w ! * l t o 3 do

beg i nf o r c o l : = 1 t o 3 do

beg ini f < r o w= co l > t h e n

beg inn a tC row t c o l 3 : = 1 end

e l s e b e g i ne a t L row > c o l 3 i * 0 end

endend

e n d ;

166

p r o c e d u r e c o p y e o t <e o t i n ! o r r 3 k 3 r I v a r e a t c p y : a r r 3 k 3 r >i

v a r r o M i C o l ! i n t e g e r }

beg i nf o r r o u l ' l t o 3 do

beg inf o r c o l ! * l t o 3 do

beg i n» a t c p y [ r o w , c o l 3 ! « m o t i n C r o w , c o l 3• nd

• n d• n d r

p r o c e d u r e c o p y v e c ( v e c i n ! o r r 3 r ! v o r v e c c p y } a r r 3 r > J

v a r r o w l i n t e g e r i

b»g i nf o r r o u ! = l t o 3 do

beg inv e c c p y£ r o «3 . ' »vec i n t row3• nd

• n d }

p r o c e d u r e v e c a d d v i c t u b v e c ( v * c 1 , v e c 2 t v » c 3 ! a r r 3 r } v a r v e e r

< f u n c t i o n : v » c H v » c 2 - v » c 3 » v » c r » »

v a r r o w l i n t e g e r }

b e g i nf o r r o u l ' l t o 3 do

beg inv * c r e s t r o w } » v e c l C r o w 3 + vec2C r o w3 - v e c 3 C row3• n d

• n d }

{ f u n c t i o n : s e t u p a r o t a t i o n e a t r i x f o r b r y a n t a n g l e s

p r o c e d u r e b r y _ a n g ( t h l r t h 2 r t h 3 ! r e a l ! v a r r o t m a t ! a r r 3 K 3 r ) ;v a r c i . c 2 »c 3 . s 1 , s 2 . s 3 ! r e a l ;b e g i n

c i : » c o s < t h l > } c 2 : « lc o s ( t h 2 > ! c 3 : * c o s < t h 3 > ;• I ! “ s i n < t h l ) } s 2 ! “ s i n < t h 2 > ! s 3 : “ i i n ( t h 3 ) }

r o t m a t C 1 . 1 3 : « c 2 * c 3 } r o t ma t [ 1 . 2 3 : » - c 2 * s 3 } r o t m a t C 1 , 3 1 1»s2 J

r o t m a t C 2 . 13'. = c l » s 3 + s l * s 2 * c 3 } r o t m a t C 2 » 2 3 : = c l * c 3 - s l * s 2 * s 3 } r o t m a t C 2 , 3 3 : « - s l * c 2 ;

r o t m a t C 3 » 1 3 : = s l * s 3 - c l * s 2 * c 3 ! r o t m a t C 3 r 2 3 ! « s l * c 3 + c l » » 2 * s 3 i r o t m a t C 3 f 3 3 : « = c l * c 2

• n d i

! a r r 3 r )}

>

>

167

{ p r o g r a s s e r ! d i e t e r l a n g e r >{ d a t e ! 8 - j o n - 8 6 >{ f u n c t i o n ! e a p p i n g o f s t r a i g h t l i n e s i n 3d w o r l d space >{ i n t o s c r e e n space >{ i n p u t parast* ! x l i y l . r l , > :2 <y2 . z2 e n d p o i n t s o f l i n e >{ o u t p u t paran. . ! x l p » y l p , x 2 p . y 2 p e n d p o i n t s o f l i n e on t h e{ g l o b a l v a r . ! v i e u p o r t C 4 1 = ( v l r v r » v t f V b ) o f v i e w p o r t >{ c o i C 3 D , e p C 3 1 . o r . v a t h » y o ! p e r s p e c t i v e t r a n s f . >

PROCEDURE c l i p e d g e s i n 3 d < x l , y l , z l , x 2 . y 2 r z 2 ! r e a l ! VAR x l p , y i p »x2p * y 2 p ! i n t e g e r ( v a r M a p U l d t o S c r r e a d y ! b o o l e a n ( v a r w s ! a r r 4 k 4 r > !

VARp l w » p 2 w » p l s . p 2 s ! a r r 4 r } i r j I i n t e g e r !

FROCEDURE perspeCVAR w s ! a r r 4 K 4 r > !{ f u n c t i o n ! e a p p i n g f r o e w o r l d{ t h e e a t r i x US >see{ d e f i n i t i o n s : a s p e c t r a t i o !{ h i t h e r yon p l a n e !{ v i e w i n g a n g l e !{ c e n t e r o f i n t e r . !{ e y e p n t ( o b s e r v ) !

VARurb,crd.fralpharbetardeltarCth'Sth'Cphrsph'va:reai;

BEGIN { p e r s p e >a ! « c o i [ l l - e p C l l » b ! « c o i C 2 1 - e p E 2 3 } c i « c o i [ 3 3 - e p [ 3 i ;d ! « s q r t ( a * a + b * b + c * c > {f ! “ s q r t ( a * a + c * c > »v a ! » v a d e g * 3 . 1 4 1 5 / 1 8 0 !d e l t o ! * s i n i v a ) / c o s < v a ) lalpha!*delta*yo/<yo-h)!beta!«delta*yo*h/(h-yo);

{ c t h = c o s ( t h e t a 1 ! s t h « s i n <t h e t a ) ! c p h - c o s ( p h i ) ; s p h * s i n ( p h i ) >

c t h ! « c / f { s t h l « a / f > cph i « f / d ! s p h ! « b / d !

wsC1 . 1 1 ! - c t h ; w s t 2 f l l ! “ - s t h * s p h * o r } w s [ 3 » l l ! “ - s t h * c p h * a l p h a ; w s C 4 , l l ! “ - s t h * c p h * d e l t a t

w s t l , 2 1 1 - 0 . 0 ! wsC2 »21! - c p h * a r ! w s C 3 r 2 1 ! = - s p h * o l p h a ! w s C 4 f 2 1 ! “ - s p h » d e l t a !

wsC1 . 3 3 ! “ - s t h ; w s C 2 . 3 1 ! - - c t h * s p h » a r ! w s [ 3 , 3 1 ! - - c t h * c p h * a l p h a ! w s C 4 , 3 ] : - - c t h * c p h S d e l t a !

w s C l , 4 3 ! » < - c o i C 1 3 * c t h + c o i C 3 3 * s t h > ;u s C 2 . 4 i : = ( c o i C l l * s t h * s p h - c o i C 2 3 * c p h + c o i C 3 1 * c t h » s p h ) * a r !w s [ 3 , 4 1 ! - < c o i [ l l * s t h * c p h + c o i C 2 1 * s p h + c o i C 3 1 * c t h * c p h + d > * a l p h a + b e t a iw s [ 4 i 4 l : = < c o i C l l * & t h * c p h + c o i C 2 1 * s p h + c o i [ 3 1 * c t h * c p h + d > * d e l t a

s p a c e t o s c r e e n sp ac e u s i n g t h e s i s e a c i e j e w s k i p g . 5 4

ARH , YO VADEG COI EF

168

e n d ;

PROCEDURE c l i p ( x l » y l i Z l » w l , x 2 i y 2 ( Z 2 . w 2 { r e o l { V A R x l p , y i p . x 2 p » y 2 p { i n t e g e r ) ;TYPE

e d g e “ l • . 6 ;o u t c o d e “ SET OP e d g e i f e a g e t a k e s v a l u e s 1 , 2 , 3 , 4 , 0 , 6 and o u l c c a e i s

a s e t w i t h a t n o t t 6 e l e m e n t sVAR

w c { ARRAYC1. . 2 , 1 . . 6 3 OP r e a l ; c 1 »c 2 ! o u t c o d e ; d x , d y , d z , d w , t , t l , t 2 ! r e a l { i i i n t e g e r )

PROCEDURE s h o w l i n e < x x l , y y l » z z l » w l , x x 2 » y y 2 , z z 2 » w 2 { r e a l { V A R x l p , y l p , x 2 p , y 2 p { j VAR

x l , y l , z l » x 2 , y 2 , z 2 ; r e a l { v l , v r , v t , v b ! i n t e g e r {

DEGIN < s h o w l i n e >

v l ! “ v i e w p o r t ! 1 3 { v r { “ v i e w p o r t C 2 3 { v t J“ v i e w p o r t E 3 3 { v b I “ v l e w p o r t t 43 {

< W R I T E L N ( X X l , y y l , z z l , w l , x x 2 , y y 2 , z z 2 , w 2 >1>

i f ( a b s ( w l ) < e p s i l o n ) t h e n beg inx l { “ ( v r + v l ) / 2 { y l ! “ ( v t + v b ) / 2 {end

ELSEb e g i n

x l ! “ x x l / w l * ( v r - v l ) / 2 + ( v r + v l ) / 2 { y l { “ y y l / w l * ( v t - v b > / 2 + < v t + v b > / 2 { end {

i f ( o b s ( w 2 ) < e p s i l o n ) t h e n beg i nx 2 ! “ < v r + v l > / 2 { y 2 { “ ( v t + v b ) / 2 {end

ELSEb e g i nx 2 { “ > :x 2 / w 2 * ( v r - v l ) / 2 + ( v r + v l ) / 2 { y 2 { « y y 2 / w 2 * ( v t —v b ) / 2 + ( v t + v b ) / 2 { end {

x l p { “ r o u n d ( x l ) { y i p { “ r o u n d ( y l ) { x 2 p { “ r o u n d <x 2 ) { y 2 p { “ r o u n d ( y 2 )

e n d ;< s h o w l i n e >

PROCEDURE e a k e w i n d o w c o o r d s ( p { i n t e g e r { > : , y » z , w : r e a i ; V A R c l o u t c o d e l i

VARi { i n t e g e r {

DEGIN < e a Ke w i n d o w c o o r d >

w c E p , 1 3 { “ w + x { w c C p , 2 3 : = w - : : { w c C p , 3 3 ! “ W + y ; w c C p , 4 3 : = w - y { w c C p , 5 3 ! “ z { w c C p i 6 3 {“ w - z ! c , “ C3{ < c “ enipty s e t >POR l ! “ 1 TO 6 DO I F wc Cp• 1 3 ; 0

169

THEN c ! « c + t 1 3<c i s t h e s e t o f e l e e e n t s o f w c . w h i c h o r * o u t s i d e c l i p l i » i t s >

e n d ;< makewindowcoor d >

BEGIN { c l i p >

i i i a k e w i n d o w c o o r d s ( l » ; : l i y l » z l > w l t c l > ; i * a K * w i n d o w c o o r d s < 2 » : : 2 » y 2 r z 2 j w 2 t c 2 ) i I F ( c 1 Sc2 ) - [ 3

THEh

■C i f b o t h e n d p o i n t s o r * o u t s i d e c l i p p i n g p l o n e t h r o w edge o u t >

BEGIN < c l * c 2 ; s e t c l i n t e r s e c t w i t h s e t c2 > t l ! » 0 { t 2 : « i ;FOR i l - l TO 6 DOI F < w c C l , i 3 < 0 ) OR <wcC2 f i 3 < 0 )

THENBEGIN

t : « w c C l , i 3 / ( w c C l Pi 3 - w c C 2 f i 3 > ;I F wc 1 1> i 3 < 0

THEN BEGIN

I F t > t l THEN t l ! * t

ENDELSE

BEGINI F t < t 2

THEN t 2 ! « tEND



e n d ;

I F t 2 >= t l THEN

BEGINd x : « x 2 - x i ; d y ! « y 2 - y i ;d r : “ 2 2 - z i ; d w : » w 2 - w i ;I F t 2 <> 1

THEN

< i f p o i n t 2 i s o u t s i d e c l i p p i n g p l a n e . c l i p p o i r

BEGINx 2 : “ x l + t 2 * d x ; y 2 : “ y l + t 2 * d y ; z 2 ! * = z l + t 2 * d z ; w 2 : « u l + t 2 * d w

e n d ;I F t l <> 0

THEN

< i f p o i n t 1 i s o u t s i d e c l i p p i n g p l a n e • c l i p po:

BEGINx l : =;! l + t l * d : : ; y l : « y l + t l * d y ; z i ; * = z i + t i * d z ; w i ; “ w i + t i » d w

e n d ;* h o w l i n e ( x l f y l r Z l r W l » x 2 r y 2 . z 2 , u 2 . x l p . y l p . x 2 p . y 2 p >

170

ENB« t l >

ENB

ENB i { c l i p >

BEGIN < c 1 i p 3 d >I F (NOT p a r m i n i t )

t h e nBEGIN

w r i t e l n ( c o n , ’ * * * * p e r s p e c t i v e p a r a m e t e r s a r e n o t i n i t i a l i z e d * ♦» w r i t e l n ( c o n » ' * * * * c a l l p r o c e d u r e I n i t P e r s p e c t i v e * * »

ENBELSE

BEGIN

i f ( n o t M a p U l d t o S c r r e a d y > t h e n b e g i n p e r s p e ( w s ) end;

< now map ! WLD - - > SCR i s i n i t i a l i z e d >M a p W l d t o S c r r e a d y ! = t r u e ;



p i u c n : « x i ; p i u C 2 i : - y i ; p i w i : 3 3 : - z i ( p i w C 4 : : - = i ; p 2 w C 1 3 : « ; s 2 ; p 2 w C 2 3 : « y 2 ; p 2 w E 3 3 : = z 2 ; p 2 g C 4 3 : = i ;

< ma pp i ng f r o m w o r l d t o s c r e e n s p a c e >

FOR i i ' l TO 4 DO BEGIN

p i s C i 3 : - o ; p 2 s C i i : - o ;FOR j : - l TO A BO

BEGINp l s C i 3 : « p l s C i 3 + p l w C j 3 * w s C i » j 3 ; p 2 s C i 3 : = p 2 s C i 3 + p 2 w C j 3 * w s L i r j 3

ENB e n b ;

■C c l i p p i n g s c r e e n s p ac e c o o r d i n a t e s >

c l i p ( p l s [ 1 3 , p l s t 2 3 , p l s E 3 3 , p l s C 4 3 , p 2 s E 1 3 , p 2 s t 2 3 , p 2 s t 3 3 , p 2 s C 4 3 , : : l p , y i p , x 2 p . y2p ) i

I F t e s t p r o g r a m THEN

BEGINw r i t e l n ;w r i t e l n ( ' ----------------- p r o c e d u r e c l i p e d g e s i n 3 d --------------------------------------------w n t e l n ( ' p l s c r e e n ; ; , y , Z r w p 2 s c r e e n ; . , y ( z , ww r i t e l n ( p l s C 1 3 , p l s C 2 3 * p l s t 3 3 » p l s C 4 3 » p 2 s C 1 3 , p 2 s C 2 3 F P - s C 3 3 f p 2 i w r i t e l n C ' p l w o r l d >:( y , z t w p 2 w o r l d x . y >w r i t e l n ( p l w C 1 3 > p l w C 2 3 > p l w C 3 3 f P l w C 4 3 > p 2 w C 1 3 t p 2 w C 2 3 > p 2 u C 3 j r p 2 k w r i t e l n ( ' x l p . y i p » >;2pt y2p ' , x l p : 6 • y I p : 6 , x2p 16 , > 2p 1 6 ) wr l t e 1

ENB

ENBENB! { c 1 i p 3 d >

171

<p rog r a n * ! r { d o t *< f u n c t i o n {{{{{

PROCEDURE c 1 i p p o l y i n 3 d i

TYREp t r = Av e r t e : ; i v e r t ex - RE COR D

v t l A R R A Y C l . . 1 0 0 , 1 . . 3 3 OF r e a l ; n x t : p t r ; bcK : p t r

e n d ;

VARn p o l y , n v * r t , n e d g , i , j , K , m , n , l a s t x , l a s t y ! i n t e g e r ; p l e q p 2 » p 2 e q p 3 , f a c e ! b o o l e a n ; e p s : r e a l ( p n l a r r l O i ; p , q : p t r ;v t : < l , v t y l , v t z l , v t x 2 » v t y 2 , v t z 2 ! r e a l ; n r e c o r d s i i n t e g e r ; l n p u t f i l e . o u t p u t f i l e l s t r ! a b s _ o f _ d ! r e a l ;

PROCEDURE f i n d v t r t r x ( VAR p I p t r J V A R v t x , v t y , v t z t r e a l ; i n d e x ! i n t e g e r ) ;

VARn r e c o r d , a c t i n d e x l i n t e g e r ; i : i n t e g e r i

D EGINf r e s e t p o i n t e r t o f i r s t r e c o r d >WHILE p A .bcK <:• N I L DO

DEGINp ! « p A .bcK

e n d ;

{ c a l c u l a t e a c t u a l r e c o r d >

n rec o r d : * t r u n c ( i n d e x / 1 0 0 ) }

FOR i S « l TO n r e c o r d DO DEGIN

p ! “ p A . n x te n d ;

I F ( ( i n d e x - n r e c o r d * 1 0 0 ) = 0 >THEN

DEGINa c t i n d e x ; * i n d e i ; - ( n r e c o r d - l ) * 1 0 0

END ELSE

DEGINa c t i n d e x ! = i n d e x - n r e c o r d * 1 0 0

END!v t x : * p A . v t C a c t i n d e x , 1 3 ; v t y ! = p A . v t C a c t i n d e x , 2 3 ; v t z : = p A . v t C a c t i n d e ; ; , 3 3

END!< f i n d v e r t e x >

d i e t e r l a n g e r >2 6 - J a n - B 6 3r e a d s f i l e I N PF I LE . WL D >and g e n e r a t e s f i l e I N F F I L E . F L T wh i c h c o n t a i n s >p l o t c o t s o n d t f o r t h e g r a p h i c s g e n e r a t o r >t h e i n p u t and o u t p u t f i l e s a r e c o n t r o l l e d by >p r o g r a m s 0PENSEGHENT and CL0SESEGHENT and >by F' LOTFILE. >

172

FUNCTION b a c k f o c e ( p n 2 . p n 3 . p n 4 : i n t e g * r ; v o r a b s _ o f _ d ! r e o l ) . ' b o o l e a n .YikR

« f b ( c ! o r r 4 r i j ! i n t # 5 * r » d ! r # o ] ! v t J A R R A Y C l . . 3 , 1 . . 3 3 OF r * Q l )

BEGINf i n d v e r t e x ( p . v t C 1 . 1 3 , v t C l , 2 3 , v t [ l , 3 3 , p n 2 ) .f i n d v e r t e ! : >f i n d v e r t e x ( p , v t C 3 , 1 3 » v t [ 3 , 2 3 » v t C 3 , 3 3 , p n 4 > ;FOR i ! » l TO 3 00

REDINa C i 3 i « v t C 3 f i 3 - v t t 2 , i 3 ;b C i 3 : « v « 2 , i 3 - v t C l , i 3 {c C i 3 ! * e p C i 3 - c o i C i 3

e n h ;< a = v3 - v 2 { b « v 2 - v l { c “ ep - c o i >< F o r * d « ( o c r o s s b ) d o t c > >< f r o n t f o e * 1 d > 0 >< bock f o e * * d < 0 >

d : « ( a C 2 3 * b C 3 3 - b C 2 3 * a C 3 3 >* c t 1 3 F < a C 3 3 * b C 1 3 - a C 1 3 * b C3 3 >*cC2 3 i ( o C 1 3 * b C 2 3 - o C 2 3 * b C 1 3 ) » c C 3 3 ;

abs o f d l « a b s ( d > I ' I F d > 0

THEN REGIN

b o c k f o c * : “ t r u #END

ELSEREGIN

b o c k f a c e : « f a l s eEND

ENRi < f u n c t i o n b a c k f o c e >

REGIN

r e v . d r * o d ( i n p , n p o l y ) ! r e o d l n ( i n p ) , n e w ( p ) ; < c r e o l e new r e c o r d >p A . b e k : « N I L . < EOR t o bek f i e l d o f 1 s t r e c o r d >n r e c o r d s 1“ t r une < n v e r t / 100 > + 11 FOR i :=1 TO n r e c o r d s DO

REGINI F < ( i > “ l ) f k N D > >

THENREGIN

f o r j : « a t o i o o doBEGIN

r e a d ( i n p , p A . v t C j , 1 3 ) » r eod ( i n p » p A . v t [ j , 2 3 ) ; r e a d ( i n p , p A. v t [ . j , 3 3 ) ; r e o d I n ( i n p )

ENDEND

ELSEBEGIN

FOR J ! = 1 TO <n v e r t - ( n r e c o r d s - 1 > * 1 0 0 ) DO BEGIN

r e a d ( i n p , p A . v t C j , 1 3 ) >r e o d ( i n p , p A . v t C j , 2 3 ) » r e a d ( i n p , p A . v t C . j , 3 3 > ; r e o d l n ( i n p )

ENDEND!

173

I f i <» n r t c o r d c THEN

PEGINn e w ( q ) ; < open new r e c o r d >p A . n x t . ' “ q ; q ' . b c k : * p ; { p o i n t e r t o p r e v . r e c o r d )F ! “ q < r e s e t p o i n t e r >

ENI'e n d ;

< r e a d i n g v e r t i c e s >p A. n x t : = N I L i C EOR t o n e x t f i e l d o f l o s t r e c o r d >< s t a r t p l o t t i n g p o l y g o n by p o l y g o n > e p s : «=o. 0 0 0 1 ;FOR i ! « l TO n p o l y DO

PEGINr e a d ( i n p r p n C 13 ) Jf o r n ! * 2 t o p n C 1 3 + l dobeg in

r e a d e n d ;

r e a d l n ( i n p ) !

p n t p n C 1 3 + 2 3 : - p n E 2 3 ;

k :«o;r e p e a t

< check f o r d e g e n e r a t e p o l yg o n s ? w h i l e b a c k f a c i n g >< d e g . p o l y g . • v e r t e x [ i 3 « v e r t e x C i + 1 3 M > k :«k + i ;

f a c e : « b a c k f a c e ( p n C k + 1 3 t p n C k + 2 3 f p n C k + 3 3 r a b s _ o f _ d ) ;u n t i l

<<abs o f _ d > 0 . 0 0 0 0 1 ) o r ( k « p n C 1 3 - l >)»I F ( a b s o f " d x O . 0 0 0 0 1 )

THENPEGINe n d ;

I F ( ( f r o n t o r b a c k f a c e * ' f t ' ) OR( ( f r o n t o r b a c k f a c e » ' P ' >ftND f a c e ) OR ( < f r o n t o r b a c k f a c e « ' F ' ) f t N D ( N 0 T f a c e ) ) )

THENPEGIN

l a s t x : » 0 ; i a s t y ! « 0 !FOR j : * 2 TO p n C 1 3 + l DO

PEGIN < s i n g l e p o l y g o n s >k : =pnC. j3 ; i i . : c p n t j + i i ; f i n d v e r t e x ( p » v t x l » v t y l f v t z l . k ) ; f i n d v e r t e x ( p » v t x 2 f v t y 2 » v t z 2 » n i ) ; c l i p e d g e s i n 3 d ( v t x l ? v t y l » v t z l f v t ; : 2 , v t y 2 i v t z 2 ,

x l p » y l p r x 2 p f y 2 p » n i a p W l d t o S c r r e a d y > w s ) ; p l e q p 2 ; = ( ( a b s ( l a s t x - : : l p ) - . e p s ) f t N D ( a b s ( l a s t y - y l p ) - . e p s ) ) ( p 2 e q p 3 : “ ( ( a b s ( x 2 p - x l p ) x e p s > f t N D ( a b s ( y 2 p - y l p ) . e p s ) > ;

I F ( N O T p 2 e q p 3)THEN

PEGIN < d r aw a l i n e f ron i p2 — . p3 >I F (NOT p l e q p 2 >

THENPEGIN

sioveto ( x l p > y i p )e n d ;

l i n e t o ( x 2 p > y 2 p )

174

ENP!< droM a l i n e f r o m p 2 — > p3 >

END< m i n g l e p o l y g o n * >

ENP

e n d ;

{ d e l e t e v e r t e x a r r o v s >

UHILE p A. b c k O N I L HO PEGIN

p ! ■pA . beke n d :

UHILE p A• n x t <> N I L DO PEGIN

p t « p A. n x t ! d i s p o s e < p A. b e k )

e n d ;d i m p o s e ( p )

e n d ;

17 5

A-2 The Modules for Generating the Humanoid, ADAM

A program l i s t i n g of the modules which generate the humanoid, see Fig. 3 .20 , is given below:

v e r s i o n * 1 . 1 MAY 1 3 , 1966

USERS GUIDE wo i n f i l e work f i l e GRAF'HMOD . F AS

\ g r a p h \ m i s c e l l a

\ g r o p h \ c o n c a v p o

\ g r a p h \ p r i m i t i v

\ g r o p h \ f i l e o p

\ g r a p h \ m a t o p

\ g r a p h \ b a s i c s

c o m p i l e r d i r e c t i v e { * 1 D ! GRAF'HMOD. F'ASJ :t<: ADAM. FAS: b : g r a f h m o d . f a s

c o n t a i n s t h e f o l l o w i n g p r o c e d u r e s ! I n i t f e r s p e c t i v e

I n i t G r o p h i c s : GENCYLINDER

GENSFHERE I MOVETO

LI NETO OpenSegment C l o s e S e g m e n t P os t S e g m e nt UnF o s t S e g m e n t

: P l o t F i l e C o n c o t F l i e 7 r a n R ot

: F r Ma t Ue c F r M a t M o t Umat3 C o p yh o t CopyVecU e c AddV e c SubV e c B ry _on g

: C l i p E d g e s i n 3 d C l i o F o l y i n 3 d

p rog r o t i e rd a t ef u n c t i o n

d i e t e r l o n g e r >1 0 - f e b - B 6 >m a m pr og r a m f o r p l o t t i n g ADAM >

t y p e p o i n t e r “ An o d e i node = r e c o r d

n a m e t s t r B j < none o f t h e r e c o r d >o _ ab s , o _ r e l ! a r r 3 K 3 r i < a b s . ond r e l . o r i e n t a t i o n » i a t . >o r l g i n _ a b s I a r r 3 r J < o r i g i n o f a c t u a l node >l , k , l _ n g h t , l _ l e f t , l _ a b s , k _ a b s ,l _ r i g l i t _ o b s , l _ l e f t _ o b s ! a r r 3 r i

< l o c a l v e c t o r s t o i n i t i a l and t e r e i n a l j o i n t > t h ! a r r 3 r } < r e l . o r a b s . o r i e n t a t i o n a n g l e s >u p , do wn , l e f t , r i g h t ! p o i n t e r i end ;

e * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * » * * * » * * * * * * * * * * » * * * * * » * * * * * >u s e r ' s g u i d e ! ADAM >

ADAMINIT I i n i t i a l i z a t i o n o f p o i n t e r s t r u c t u r e s ond p o s i t i o n v e c t . >M a s t e r F i l e s I c r e a t i o n o f e a s i e r f i l e s >R e a d A n g l e s ! r e a d a n g l e s f r o m d i s k C l , wh i c h d e s c r i b e t h e body s e g m e n t s '

o r i e n t a t i o n U o r l d F i l e • c r e a t i o n o f w o r l d f i l e sR e c T r e e U a l k e r : r e c u r s i v e l y c omp ut es new w o r l d f i l e s F i n d F o i n t e r ! d e t e r m i n e s t h e p o i n t e r c o r r e s p o n d i n g t o a body s e g

m e n t s ' name

176

< P r i n t R eco rd ! p r i n t s o u t o i l t h * i n f o r m a t i o n i n t h e n o d * r t c o r d i o f< " Mifth< P l o t F i l * : g e n e r a t e s p l o t commands< D i s p l a y F i l e s ! g e n e r a t e s segment ed d i s p l a y f i l e s{ I n t e r r t c t i v e l n p ! s t a r t e s i n t e r a c t i v * o p e r a t i o n{ * * * * * * * * * * * * * * * * * * * * * * * * * » * * * » »

v a r l o t o r s o p t r f P r q ' S e g m e n t p t r i p o i n t e r ! i : i n t e g e r !r * l o r a b s _ a n g l * s ! b o o l * o n ! c * d c h i c h o r ! se gm en t no i i n t e g e r ! done i b o o l e a n ! s e g m e n t N a m e i s t r B !

- (programmer i d i e t e r l o n g e r< d a t e ! 1 0 - f e b - B 6■Cf unct ion ! t h i s p r o g r a m r e o d s f i l e _ b o d y _ p o r » ond s e t s up t h e m a s t e r< f i l e s f o r i t i n i t i a l i z e s t h e p o i n t e r s t r u c t u r e s r< w h i c h d e s c r i b e body se gment s

p r o c e d u r e o d o m i n i t ( f i l e _ b o d y _ p o r m ! s t r l 4 ! v o r l o t o r s o p t r i p o i n t e r l i

i n p . o u t i t e x t !o u t f l l e i s t r B i e x t e n s i o n i s t M !K r 1 1 a r r 3 r !i >n d i v h r n d i v v i i n t e g e r !u t o p > a b o t t o m . b t o p »b b o t t o m i h e i g h t ! r e a l ! b o d y _ s * g m _ p a r m i a r r 2 0 k 5 r ! d i s t » d i * t _ r i b ! r * a l S u p t o r s o p t r r p > q ! p o i n t e r ! o f f s e t l f o f f s e t 2 I a r r 3 r !

p r o c e d u r e M a * t e r F i l e s ( v a r b o d y _ s * g m _ p a r m ! a r r 2 0 k 5 r ) !

b e g i n < M a s t e r F i l e s >< open f i l e _ b o d y _ p a r s >

e > : t e n s i o n i “ ' . m s t ' 5 o s s i g n ( i n p f f i l e _ b o d y _ p a r i i i > i

< ch ec k w h e t h e r f i l e _ b o d y _ p o r m e x i s t s , see p g . 1 1 6 P a s c a l Manua l > { ♦ ! - > r e s e t ( i n p ) { ! + > ! o k ! = ( i o r e s u l t * 0 1 ! i f n o t ok t h e n

beg i nw n t e l n ! w r i t e l n !w r i t e l n ( ' * » * * * » * » co py t h e f i l e ' »f i l e _ b o d y _ p a r » i , ' t o d r i v e

d r i v e _ d a t , ' * » * * * * * * * ' ) !h a l t

end !{ end I / O ch ec k >

r e a d l n ( i n p ) ! < n e g l e c t f i r s t ( c om men t ! l i n e i n f i l e _ b o d y _ p o r m >

f o r i i = 1 t o 19 do beg in

r e a d ( i n p • o u t f i l e > ! r e a d l n i { segment name >

177

r e o d ( i n p , h e i g h t ) ; r e o d ( i n p . a t o p > f r e a d ( i n p . b t o p > ; r e a d ( i n p , a b o t t o » > ;r e a d ( i n p , b b o t t o * > ; r e a d ( i n p , n d i v h ) l r e a d ( i n p . n d i v v > ; r e a d l n i i n p ) ;b o d y _ » » g n _ p o r * i t i t l ] : ‘s h * » g h t !body_segm_pa r m t i , 2 3 ; * a t o p »b o d y _ s e g m _ p a r m C i . 3 3 : ' b t o p ;b o d y _ s e g r i _ p a r m C i i 4 3 : ' a b o t t o m ;b o d y _ s e g m _ p a r m C i . 5 3 ! * b b o t t o m (

i f ( ( o u t f i l e <> ' h e a d ' ) ond ( o u t f i l e <> ' H E A t ' ' ) ) t h e n beg ing e n c y l i n d e r ( o t o p . a b o t t o m . b t o p . b b o t t o m . h e i g h t . n d i v h » n d i v v . d r i v e _ m s t »

o u t f i l e . e x t e n s i o n ) ;end

e l s eb e g i ny e n s p h e r e ( n d i v h . n d i v v , h e i g h t , d r i v e _ m s t , o u t f i l e . e x t e n s i o n , o u t ) end

e n d ;c l o s e t i n p )>

< c o i i b i n e f i l e s L 0 T0 R5 0 1 + L 0 T 0 R S 0 2 — > LOTORSO >

o f f s e t l C 1 3 : = 0 ; o f f s e t l C 2 i : « = 0 ; o f f s e t l C 3 i : « 0 ;o f f s e t 2 C 13 I = 0 ; o f f s e t 2 C 2 3 S “ - ( b o d y segm p a r » t 1 1 » 13 + bo d y seg»_ par mn 12 , 13 ) / 2 i o f f s e t 2 C 3 D! “ 0 ;c o n c a t f i l e s ( d r i v e _ * s t » ' l o t o r s o l ' , ' . e i s t ' , d r i v e _ » s t , ' l o t o r s o 2 ' , ' . m s t ' .

d r i v e _ » s t > ' l o t o r s o ' , ' . m s t ' , o f f s e t l , o f f s e t 2 >!

{ c o mb ine f i l e s UFT0RS01+UFT0RS02 — > UFT0RS0 >

o f f s e t 2 C 2 D ! - 0 ;o f f s e t l C 2 3 ! ' ( b o d y _ s e g m _ p o r B i C 6 » 1 3 + b o d y _ s e g m _ p o r m C 7 , 1 3 > / 2 ; c o n c a t f i l e s ( d n v e _ m s t , ' u p t o r s o l ' , ' . m s t ' , d r i v e _ m s t , ' u p t o r s o 2 ' > ' . m s t ' ,

d r i v e _ m s t » ' u p t o r s o ' , ' . * s t / , o f f s e t l , o f f s e t 2 > ;

{ c o mb i ne f i l e s LARM1+LARM2 — > LARM >

o f f s e t l C 2 3 : = b o d y _ s e g m _ p a r m C 1 4 , 1 3 / 2 ;o f f s e t 2 C 2 3 : “ - b o d y _ s e g m _ p o r n i C 1 5 r l 3 / 2 ;c o n c o t f i l e s ( d n v e _ » s t f ' l a r m l ' » ' . e i s t ' t d r i v e _ m s t f ' l a r m 2 '» ' . m s t ' .

d r i v e _ m s t > ' l a r m ' , ' . m s t ' * o f f s e t 1 , o f f s e t 2 ) >

{ c omb ine f i l e s LLEG1+LLEG2 — > LLEG >

o f f s e t l C 2 3 ! “ b o d y _ s e g m _ p a r m t 2 , 1 3 / 2 ; o f f s e t 2 C 2 3 ! “ - b o d y _ s e g m _ p o r a t 3 , 1 3 / 2 !c o n c a t f i l e s ( d r i v e _ a s t , ' l l e g l ' , ' . a s t ' , d r i v e _ m s t , , l l e g 2 ' , ' . * s t ' »

d r i v e _ m s t , ' 1 l e g ' , ' . m s t ' , o f f s e t l . o f f s e t 2 ) e n d ! { H o s t e r F i l e s >

b e g i n < o o o m i n i t B io in p r o c e d u r e body >

h a s t e r F i l e s ( b o d y _ s e g m _ p a r m ) ;



< LOTORSO >d i s t :=0 .2*R0 I ' Y_SEGM_F' ARMCB,13;

d i s t _ r i b i * 0 . 2 * b o d y _ s e g n . _ p a r m t B . 1 3 ;

178

n e w ( p ) | l o t o r s o p t r t » p $ w i t h p A do

b e g i nd o w n ! « n i l ! n a m e ! “ ' l o t o r s o ' !k [ 1 3 ! “ O i k [ 2 3 ; “ b o d y _ s e g m _ p a r m [ l l , 1 3 / 2 + d i s t _ r i b i k [ 3 3 ! “ Oi

1 l e f t [ 1 3 ! “ - b o d y segm p o r m [ 1 1 , 4 3+ bod y segm p a r m [ 1 . 2 3 !1 l e f t [ 2 3 ! = - ! body s e g m _ p o r * [ 1 1 , 1 3 > / 2 i l _ l e f t [ 3 3 ! * 0 !

1 r i g h t C 1 3 ! = - L l e f t C 1 3 !1 r i g h t [ 2 3 ! “ l I e f t C 2 3 ! l _ n g h t [ 3 3 ! “ l _ l e f t C 3 3 end !

i f !b o dy _s e gm _p o rm [ 1 1 , 43 < 2 » bod y_s egm_p or mC 1 . 2 3 ) t h e n

beg in w r i t e l nw r i t e l n ( ' --------------------FROCEMJRE A I ' A h l N I T ------------------------------ ' > !WRITELN( ' l e g s do n o t f i t u n d e r LOWER TORSO ' ) !WR ITE LN <' b o d y _ s e g C l l , 4 3 > 2 * b o d y _ s e g [ 1 , 2 3 ' ) !w r i t e l n ! ' 1 / 2 w i d t h o f 1 / 2 w i d t h o f ' ) !w r i t e l n ! ' l o w e r t o r s o u p p e r l e g ' ) !w r i t e l n end !

 l e g >

n e w ! p > ! l o t o r s o p t r A . l e f t : “ p i w i t h p A do

beg i nr i g n t : « n i l j l e f t : « n i l ! n a m e ! “ ' u l l e g ' ! u p ! “ l o t o r s o p t r !n e u < q ) ! l o t o r s o p t r A . r i g h t ! “ q ! q A . r i g h t ! “ n i l ! q A . l e f t ! “ n i l ! q A . n a m e ! “ ' u r l e g ' ! q A . u p ! “ l o t o r s o p t r (k [ 1 3 ! “ O i k [ 2 3 ! “ body segm p a r m [ 1 , 1 3 / 2 + d i s t r i b ! k [ 3 3 : * 0 ! q A. k [ 1 3 : “ k [ 1 3 i q A. k [ 2 3 : “ k C 2 3 i q A . k [ 3 3 ! “ k C 3 3 i

l C 1 3 ! “ 0 ! l [ 2 3 ! “ - b o d y _ s e g m p a r m [ l , 1 3 / 2 ! l [ 3 3 ! “ 0 ! q A < l [ 1 3 ! “ l [ 1 3 ! q A. l C 2 3 ! “ l [ 2 3 i q A . l [ 3 3 ! “ l [ 3 3 end i

{ l o w e r l e f t ! p > and r i g h t i q ) l e g >

n e w ! p ) J l o t o r s o p t r A. l e f t A . d o w n ! “ p ! w i t h p A do

beg i nr i g h t : “ n i l ! l e f t : “ n i i ;name S“ ' l l l e g ' f u p ! = l o t o r s o p t r A . l e f t !n e u i q ) ; i o t o r s o p t r A . r i g h t A . d o w n ! “ q ! q A . r i g h t : = n i l ! q A . l e f t : = n i l ; q A . nan.e : = ' l r l e g ' i q A . u p ! = l o t o r s op t r A . r i g h t ; k [ 1 3 ! “ 0 ! k [ 2 3 ! “ body segm p a r m [ 2 , 1 3 + d l s t i k [ 3 3 ! “ 01 q A . k [ l 3 ; = k [ 1 3 ; q A. k [ 2 3 ! = k [ 2 3 ; q A . k [ 3 3 ! “ k [ 3 3 ;

1 [ 1 3 ; = 0 ; 1 [ 2 3 ! “ - b o d y segm p a r m [ 3 , 1 3 ! 1 [ 3 3 I “ 0 ! q A . l t l 3 ; = l [ 1 3 i q A. l [ 2 3 ! “ l [ 2 3 ; q A. l [ 3 3 ; = l [ 3 3 end i

< l e f t !p> and n g h t ! q ) f o o t >

179

n e w ( p ) l l o t o r s o p t r * . l e f t * . d o w n * . d o w n ! « p I w i t h p * do

b e g i nn o n e ! ■ ' l f o o t ' !u p : = l o t o r s o p t r * . l e f t * . d o w n ;r i g h t : « n i i ; i e f t : ^ n i l tn e w ( q > ; i o t o r s op t r * . r i g h t * , down* .down * =q I q * . na me ! “ ' r f o o t ' ! q * . u p : * l o t o r c o p t r * . r i g h t * . d o w n ! q * . r i g h t ! “ n i H q * . l e f t ! “ n i l !

kC13 : - 0 ! k n 2 3 : « b o d y _ s e g m _ p a r m C 4 , 1 3 / 2 + d i s t ! k C 3 3 : «0 ! q * . k [ n : - K C l D ; q * . k C 2 3 : - k C 2 3 ; q * . k C 3 i : « k C 3 2 (

1 [ 1 3 ! - 0 ! 1 £ 2 3 ! “ - b o d y segm p o r m £ 4 , 1 3 / 2 ! 1 £ 3 3 ! “ 0 ! q * . l C 1 3 : - l C 1 3 ! q * . l C 2 3 : - l C 2 3 i q * . U 3 3 : « l C 3 3 end i

< l e f t (p> ond r i g h t ( q ) t o e s >

new< p ) ! l o t o r s o p t r * . l e f t * . d o w n * . d o w n * . d o w n : « p ! w i t h p * do

beg i nna me : * ' l t o e s ' !u p ! “ l o t o r s o p t r * . l e f t * . d o w n * . down! r i g h t : " n i l i l e f t ! “ n i i ; d o w n ! « n i l !n e w ( q > ! l o t o r s o p t r * . r i g h t * . d o w n * • d o w n * • d o w n ! “ q ! q * . n o m e ! * ' r t o e s ' ! q * . u p ! « l o t o r * o p t r * . r i g h t * . d o w n * . d o w n ! q * . r i g h t ; « n i l ! q * . l e f t : “ n i l ! q * . d o w n : “ n i 1!

k C 1 3 ; > 0 ! k C 2 3 : > b o d y s e g m _p a rm £ 1 9 . 1 3 / 2 + d i s t ! k £ 3 3 : “ 0 !q * . k £ 1 3 : - K £ 1 3 ! q * . K E 2 3 : “ k E 2 3 ; q * . K £ 3 3 : « k £ 3 3e n d !

{ R I M >

n e w ( p ) ; l o t o r s o p t r * . u p : “ p ! w i t h p * do

beg inname I * ' r i b 4 ' !r i g h t : “ n i l i l e f t : “ n i l idown 1“ l o t o r s o p t r !

k C 1 3 ! » 0 ! k C 2 3 ! =body_segm parmC I B , 1 3 / 2 + d i s t r i b ! k C33 : «=o; 1 C 1 3 ! « 0 ! 1 C 2 3 : — b o d y _ s e g m _ p a r m C l B , 1 3 / 2 ! l C 3 3 : = 0 e n d !

< RI£>3 }

n e w ( p > ; i o t o r s o p t r * . u p * . u p ! = p ! w i t h p * do

beg i nna me! = ' r i b 3 ' !r i g h t : “ n i i ; i e f t : “ n i l ; d o w n : = l o t o r s o p t r * . u p !

k C 1 3 : “ 0 ; k [ 2 3 : “ b o d y _ s eg m _p ar m C1 0 I 1 3 / 2 + d i s t _ r i b ! k C 3 3 : » 0 !1 C1 3 ; = 0 ; 1 C 2 3 ; = - b o d y _ s e g m _ p a r m £ 1 0 » 1 3 / 2 ! 1 E 3 3 : = 0 e n d !

< R I B 2 >

n e w ( p ) ( l o t o r s o p t r * . u p * . u p * . u p : = p ! w i t h p * do

180

b e g i nn o » t : « ' r i b 2 ' ;r i g h t : « n i i ; i e f t : ' n i l ; d o w n ! = l o t o r s o p t r A. u p A. u p ;

K[ 1 3 ! ' 0 >k[ 2 3 ! ' b o d y segm p a r m [ ? , 1 3 / 2 + d i s t _ r i b ! k [ 3 3 : = 0 (1C13 : « 0 ; 1 [ 2 3 : ' - b o d y _ s e g m _ p o r m [ 9 , 1 3 / 2 ; U 3 3 :«=0 e n d ;

< RIB1 >

n e w ( p ) ; i o t o r s o p t r A . u p A . u p A . u p A . u p : * p ; w i t h p A do

beg i nn a me ! • ' r i b l ' ;r i g h t : = n i i ; i e f t : = n i i ; d o w n ! « l o t o r s o p t r A . u p A. u p A . u p !

k [ 1 3 i « 0 ; k [ 2 3 ! =body segm pa rm[ B . 1 3 / 2 + d i s t r i b ! K C 3 3 ! * O i l [ 1 3 : * 0 ; i [ 2 3 : — b o d y _ s e g m _ p a r m [ B , 1 3 / 2 ; i C 3 3 : « 0 e n d ;



new J l o t o r s o p t r A . u p A. u p A . u p A . u p A. u p ! ” p ; w i t h p A do

beg i nname : = ' u p t o r s o ' ;d o w n ! “ l o t o r s o p t r A . u p A . u p A . u p A .up ; u p t o r s o p t r ! * p }K [ 13 ! = 0 ; K [ 2 3 : =body segm p a r m [ 6 t 13+body s e g e p o r i t i [ 7 , 1 3 / 2 + d i s t ;

K [ 3 3 i = 0;1 [ 1 3 : « 0 5 1 [ 2 3 : “ - b o d y _ s e g m _ p a r n i [ 7 , 1 3 / 2 ; 1 [ 3 3 ! = 0 ;

l _ l e f t [ 1 3 ! » - b o d y _ s e g m _ p a r m [ 7 f 43 +b od y _s e gm _p a rm[ 13 »23 ;1 l e f t [ 2 3 ! « - b od y _ s e g m p a r m [ 7 , 1 3 / 2 ; l ” l e f t [ 3 3 ! “ 0!

l _ r i g h t [ 1 3 : - - l l e f t [ 1 3 5 l _ r i g h t [ 2 3 !=1 I e f t [ 2 3 J l _ r i g h t [ 3 3 : = l l l e f t [ 3 3 e n d ;

i f < b o dy _ se g m_ p ar i i i [ 7 , 4 3 < b o d y _ s e g m _ p a r m [ B . 2 3 +b od y _s e gm _ pa r m[ 1 3 . 2 3 ) t h e n

b e g i nw n t e l n ( w n t e l n < ' -------------------------- PROCEDURE A D A M I N I T -------------------------------w r i t e l n t ' ARMS do n o t f i t u n d e r UPPER TORSO ' > ;U R I t e l n C ' i n c r e a s e b o d y _ s e g [ 7 » 4 3 1 / 2 w i d t h o f u p p e r t o r s o w r i t e l n ( ' o rw r i t e l n ( ' d e c r e a s e b o d y _ s e g [ B > 2 3 I 1 / 2 w i d t h o f r i b l w r i t e l n ( ' b o d y _ s e g [ 7 . 4 3 > b o d y _ s e g [ 8 t 23 + b o d y _ s e g [ 1 3 » 2 3 w n t e l ne n d ;

{ HEAD >

n e w ( p ) ; u p t o r s o p t r A . u p : * p ; w i t h p A do

beg i nn a me : = ' h e a d ' ;d o w n : = u p t o r s o p t r ; u p ; * n i l ; l e f t i = n i i ; r i g h t : = n i i ; l C 1 3 : « 0 ; i [ 2 3 ! « - b o d y _ s e g m _ p a r m [ 5 . 1 3 ; i [ 3 3 ; = 0 e n d ;

181

< UPPER LEFT 

n e w i p > ! u p t o r s o p t r A . l e f t ! “ p! w i t h p A do

beg inr i g h t ! “ n i i ; i e f t : “ n i l ! n a » e ! * ' u l a r * ' t u p ! “ up t o r s o p t r !n e w ( q ) ! u p t o r * o p t r A . r i g h t ! “ q i q A. T i g h t ! * n i l ! q A . l e f t ! “ n i l ! q A .nanie! = ' u r a r i i i ' | q A. u p ! “ u p t o r * o p t r !

k t l 3 ! “ 0 ! K t 2 3 ! “ body *egm pa r»C 1 3 , 1 3 / 2 + d i s t { K133 : “ 0{ q A . k t l 3 ! “ k C 1 3 ! q A. k [ 2 3 ! “ k [ 2 3 { q A. K [ 3 3 ! “ KC33!

K l ] : ' 0 ; K 2 ] : * - b o d y Sega, po r e t 1 3 , 1 3 / 2 } 1 E33 ! “ 0 ! q A. l C 1 3 : “ l C 1 3 ! q A. l C 2 3 ! - l t 2 3 i q A . l C 3 3 ! “ l t 3 3 e n d {

< LOUER L E F T ( p ) ond R I G H T <q ) ARM >

n e w ( p ) ! u p t o r » o p t r A. l e f t A . d o w n ! “ p ! w i t h p A do

beg inr i g h t ! “ m l l l e f t : “ n i l i n a e e l - ' l l a r e ' I u p ! “ u p t o r * o p t r A . l e f t !n e u < q ) ! u p t o r * o p t r A . r i g h t A . d o w n ! “ q ! q A . - i g h t ! “ n i l ! q A . l e f t : » n i l ! q A. n a e e ! “ ' l r a r » ' ; q A. u p ! “ u p t o r * o p t r A . r i g h t !

k C 1 3 ! “ 0 ! K [ 2 1 ! “ body segiti p a r e ! 1 4 . 1 1 + d i s t ! kC33 ! “ 0 > q A . k C 1 3 ! “ k C 1 3 ! q A . k C 2 3 i “ k C 2 3 ! q A. k C 3 3 i “ kC33 !

1 t 1 3 ! “ 0 ! 1 E 2 3 ! “ - b o d y *eg»> p o r e t 1 5 , 1 3 ! 1 1 33 ! “ 0 ! q A. l C 1 3 ! “ l C 1 3 ! q A . l [ 2 3 ! “ l C 2 3 ; q A. l C 3 3 ! “ l C 33 end !

< LEFT 

n e w < p ) ! u p t o r » o p t r A . l e f t A . downA . down! “ p ! w i t h p A do

b e g i nnan.e ! “ ' 1 pa l « i ' !up ! “ u p t o r » o p t r A . l e f t A . d o w n !r i g h t ! “ n i l ( l e f t ! “ n i l in e w ( q ) i u p t o r * o p t r A . r i g h t A . d ownA . d o w n ! “ q ! q A . n a e e ! “ ' r p a l e ' !q A . u p ! “ u p t o r * o p t r A . n g h t A . down!q A. r i g h t ! “ n i l ! q A . l e f t ! “ n i l !

KCI 3 ! “ 0 ! K C 2 3 ! “ body t egf t p a r e C 1 6 > 1 3 / 2 + d l * t ! KC3 3 ! “ 0! q A. k C 1 3 ! “ k C 1 3 i q A . k C 2 3 ! “ k C 2 3 ! q A. k C 3 3 ! “ KE33!

l C 1 3 ! “ 0 ! l C 2 3 ! “ - b o d y *egn. parn.C 1 6 , 1 3 / 2 ! 1 C33 ! “ 0! q A. l C 1 3 : = l C 1 3 ! q A . l t 2 3 ! = l C 2 3 ! q A . l C 3 3 ! “ l E 3 3 end i

< L E F T ( p ) and R I G H T <q ) FINGERS >

n e w i p ) ! u p t o r » o p t r A . l e f t A . downA. downA. down! “ p ! w i t h p A do

beg innanie: = ' l f i n g e r s ' iu p ! * = u p t o r * o p t r A . l e f t A . downA . d o w n ! n g h t : “ n i l ! l e f t ! “ n i l { d o w n ! = n 1 1!n e w ( q ) ! u p t o r » o p t r A . r i g h t A . d o w n A . downA, d o w n ! “ q 5 q A .naBiei = ' r f i n g e r s ' ! q A. u p ! “ u p t o r * o p t r A . r i g h t A . downA . down!

182

q A. r i g h t ! « n i l ! q A . l e f t ! “ n i l ! q A. do wn ! - n i 1!

K[ 1 3 ! * 0 ! K C 23 ! “ body segm pormC1 7 . 1 3 / 2 + d i s t S K C 3 3 ! ” 0!q A. k C 1 3 ! « K C 1 3 ; q A . k f 2 3 : - k t 2 3 ! q A. k C 3 3 ! “ ki :33end

e n d! < o d a m i n i t >

< p r o g rammer ! d i e t e r l o n g e r< d a t e I 1 0 - f e b ~ 8 6{ f u n c t i o n ! r e a d a n g l e * o f body s e g m e n t * f r o m f i l e< i n p u t _ a n g l e s » wh i c h a r e s t o r e d i n t h e f o l l o w i n g{ o r d e r !< 1 s t l i n e : L 0 T 0 R S 0 / R I M / R I E < 3 / R I B 2 / R I B 1 / U F T 0 R S 0 /< HEiM i / U R l EG/LRLEG/ RFOOT/RTOES/ ULLEG/< LLLEG/LFOOT/LTOES/ULARM/LLARM/LHRNB/< LF INGER/UR6RM/LRARM/RH6ND/RFINGER< 5 t h l i n e ! e m p t y ------------------{ 6 t h l i n e : t h 1 , t h 2 , t h 3 , . . .< l i n e ! . . .<< n r e c o r d * i n d i c a t e how many l i n e * a r e r e a d f r o m■C t h e i n p u t f i l e

p r o c e d u r e r e a d a n g l e * ( i n p u t _ a n g l e s ! * t r M { l o t o r s o p t r .’ p o i n t e r (n r e c o r d s I i n t e g e r >!

va ri ! i n t e g e r !

p r o c e d u r e r e a d i n p u t <l o t o r s o p t r ! p o i n t e r >;

v a r p , up t o n o p t r I p o i n t e r !

beg i np ! “ l o t o r s o p t r !

{ r e a d f i r s t t h r e e v a l u e s as c e n t e r o f i n t e r e s t v a l u e s >

r e a d ( i n p . c o i t l 3 ) ! r e a d 

w h i l e p <> n i l do b e g i nr e a d ( i n p Fp A . t h C 1 3 ) ! r e a d ! r e a d » p ! »p A . up e n d !

f r e ad a n g l e s f o r r i g h t l e g >

p ! = l o t o r s o p t r A . r i g h t ! w h i l e p v,- n i l do

b e g i nr e a d ( i n p r p A . t h E 1 3 > ! r e a d ( i n p » p A. t h C 2 3 ) ! r e a d ( i n p » p A . t h C 3 3 > ! p ! “ p A . down e n d !

< r e a d a n g l e s f o r l e f t l e g >

p ! = l o t o r s o p t r A . l e f t !

183

w h i l e p <> n i l do b e g i nr e a d ( i n p , p A . t h [ i : i > ( r * a d ; r * a d ( i n p , p A. t h t 3 3 > ; p ! c p A• down end !

u p t o r s o p t r ! « l o t o r * o p t r A. u p A. u p A. u p A . u p A . u p ;

< r e a d a n g l e * f o r l e f t a r e >

p : « u p t o r * o p t r A. l e f t ; w h i l e p <> n i l do

beg i nr e a d ( i n p » p A . t h t l ] ) ; r * a d ( i n p , p A . t h [ 2 3 > ! r * a d < l n p , p A . t h C 3 3 > ; p : = p A .down end >

< r e a d a n g l e * f o r r i g h t o r e >

p ! « u p t o r * o p t r A . r i g h t i

w h i l e p <> n i l do b e g i nr e a d ( i n p . p A . t h C l ] ) S r e a d ( i n p , p A . t h [ 2 3 ) J r e Q d ; p I « p A . down end >r e a d l n ( i n p >

e n d ; < r e a d i n p u t >

b e g i n < r e a d a n g l e * >a s i i g n ( i n p > i n p u t _ a n g l e s > ;

< c h e ck w h e t h e r f i l e _ b o d y _ p a r e e x i * t * > s e e p g . 1 1 6 P a s c a l h a n u a l >< » I - > r e * e t ;o k ! = ( l o r e s u l t « 0 >; i f n o t ok t h e n

b e g i nw r i t e l n i u n t e l n ;w r i t e l n t ' * * * * * * * * F i l e i n p u t a n g l e * , ' do es n o t e x i s t * * * * * * » * * ' > ; h a l t

e n d ;< end 1 / 0 ch ec k >

r e a d l n i i n p > ; r e a d l n ( i n p > ; r e a d l n ( i n p > ; r e a d l n ( i n p > ; r e a d l n ( i n p > ;•C n e g l e c t 5 c o n s e n t l i n e * i n f i l e i n p u t _ a n g l e s >

f o r i ! “ l t o n r e c o r d s do beg inr e a d i n p u t ( l o t o r s o p t r ) end fc l o s e ( i n p )

e n d ! < r e a d a n g l e s >

< p r o g r a m m e r< d a t e< f unc t i o n<<<p r o c e d u r e U o r l d F i l e C p i p o i n t e r ) ;

d i e t e r l a n g e r 0 4 - » a r c h - 1 9 8 6r e s e t f i l e N a e e . h s t f o r i n p u t open f i l e Name.Wld f o r o u t p u t co mp ut e w o r l d f i l e s c l o s e i n p u t and o u t p u t c h a n n e l

184

v a r don# : b o o l e a n }n a me . ms t F i l e N a m e ! s t rB} i n p f i l e . o u t f i l e l s t r l * }

p r o c e d u r e w o r l d _ f i l e ( p 1p o i n t e r ) } v a r

d ! o r r 3 r } l p t r . u ( p o i n t e r } n i s t r B } beg in

w i t h p * doo e g i n < r e c o r d l o o p >

n ! * d q * » )< co mpute o r i e n t a t i o n m a t r i x > b r y _ a n g < t h C 1 3 . t h C 2 3 r t h C 3 3 » a _ r e l ) ;l p t r } “ l o t o r s o p t r } u i “ l p t r * . u p * . u p * . u p * . u p * . u p } { p o i n t e r t o u p t o r s o >

i f ( n “ l p t r * . na me ) t h e n beg i n

< LOWER TORSO > c o p y m a t ( a _ r e l , a _ a b s ) } p r m o t v e c < a _ a b s . K , k _ a b s > } p r m a t v e c ( a _ a b s > l ' l _ a b s > !p r m o t v e c < o _ a b S r l _ r i g h t » l _ r i g h t _ a b s > } p r m o t v e c < a _ a b s > l _ l e f t » l _ l e f t _ a b s ) }< o r i g i n o f LOWER T0R50 i * i n t h e o r i g i n o f w o r l d c o o r d . > o n g i n _ a b s C 1 3 } “ 0 ! o r i g i n _ a b s [ 2 3 : “ 0 } o r i g i n _ a b s t 3 3 : “ 0 } t r a n _ r o t ( o r i g i n _ a b s , a _ a b s >

end ELSE

b e g i nI F <<n * l p t r * . u p * . n a m e ) o r <n « l p t r * . u p * . u p * . na me ) o r <n c u * . d o w n * . n a me >

<n “ u * . d o w n * . d o w n * . n a m e ) o r < n« u * . na m e> o r ( n « u * . u p * . n a me )>THEN

b e g i n■C R I B 4 , R I B 3 , R I B 2 , R I R 1 , UFTORSO, and HEAP >

i f r e l o r a b s _ o n g l e s t h e nb e g i n p r m a t m a t < a _ r e l >d o w n * . o _ a b s » a _ a b s > end

e l s eb e g i n c o p y m u t <a _ r e l . a _ a b s ) en d}

p r m o t v e c C a_ a bs . 1 > l _ o b s >} p r m o t v e c ( a _ a b s » K , K _ a b s ) } i f (name ■= u * . n a m e ) t h e n

b e g i np r m o t v e c ( o _ o b s . l _ l e f t . l _ l e f t _ o b s ) ! p r m o tv ec ( o _ o b s j l _ r i g h t » l _ n g h t _ o b s )

e n d }< o r i g i n C * d o w n 3 + K_obsC* down3 - l _ o b s C i 3 “ o n g i n C i l >vec a d d v e c s u b v e c ( d o w n * . o r i g i n _ o b s . d o w n * . K _ o b s > l _ o b s t o r i g i n _ a b s ) }t r o n _ r o t < o r i g i n _ o b s . o _ a b 6 )

endEcSE

REDIN< URLED, LRLEG, RFOOT , RTOES. URhRM, L R h R h . RHhNR. RF INGEF.S >< ULLEG, L L L E G , LFOGT »LTOES t U vhR h , l-l-ARM, LHhNG , uFINGERS > i f r e 1o r a b s a n g 1 es t h e n

b e g i n p i miotmot < o_ r e l > u p * . a _ o b s »o _ a b s ) end e l se

b e g i n c o p y m a t ( o _ r e l t a _ a b s ) end} p r m o t v e c ( a _ a b s >K . k _ o b s > i p r m a t v e c ( o _ O D S t l r 3 _ a b s ) }

I F ( ( n « u * . l e f t * . na me > o r ( n = l p t r * . l e f t * . n a m e ) ) t h e n

185

b e g i n c o p y v t c ( u p 1' . X l e f t a b S r d ) end ■ I s *

beg inI F ( (n = u ' , . r i g h t A . n o » p ) o r <n “ l p t r A . r i g h t A • na me>) t h en

b e g i n c o p y v e c ( u p A . 1 _ r i g h t _ a b s >a > end e l s e

b e g i n c o p y v e c < d t Au p l : 1~ l e f t _ o b 6 t AupD i f ULLEG o r ULMRH >< ! l _ r i g h t _ a b s C Au p l i f URLEG o r URhRM>< ! l _ o b s C Au p l o . w . >v e c o d d v e c s u b v e c ( u p A. o r i g i n _ a b s » d . K _ a b s . o r i g i n _ o b e >{

{ B e t t i n g up BidBter f i l e s 1 tran_rot<origin_QbBFd_obs>

endend

end < r e c o r d l o o p > end I < w o r l d _ f i l e s >

b e g i n f e a r n p r o c e d u r e body U o r l d F i l e >< r e s e t M a s t e r f i l e s and open W o r l d F i l e s >

m s t F i l e N a m e I ' ' ' { na me t « p A. name Iif ((Name''lotorso'> or <name*'rib4 ' ) or (name*'rib3') or

< name = ' r ib2 ' ) or <name* ' rib 1 ' ) or <name''uptorso') or(nanie" ' head ' > >THEN b e g i n m s t F i l e N a m e ! “ name end

ELSE b e g i n

i f < ( n a m e * ' u r 1 e g ' ) o r (name“ ' u l l e g ' ) ) t h e n b e g i n m s t F i l e N a m e ! « ' u l e g ' end

ELSE b e g i n

i f < ( n a m e ' ' 1 r l e g '> o r ( n a m e ' ' 1 1 l e g ' ) > t h e n b e g i n m s t F i l e N a m e 1 l e g ' e n d !

i f <<n a m e ' ' 1 f o o t '> o r <name» ' r f o o t ' ) > t h e n b e g i n ms t F i 1 eName ! “ ' f o o t ' e n d!

i f C ( n a m e - ' r t o e s ' ) o r ( n a m e ' ' 1 t o e s ' ) ) t h e n b e g i n m s t F i l e N a m e t o e s ' e n d;

i f <<n a me = ' u r a r m ' ) o r ( na me = ' u l a r m ' ) ) t h e n b e g i n m s t F i l e N a m e w a r m ' e n d ;

i f ( ( n a m e = ' 1 r a r m ' ) o r ( n a m e ' ' 1 1 a r m ' ) > t h e n b e g i n m s t F i l e N a m e ' l a r m ' e n d j

i f < <name“ ' l p a l m ' ) o r <n o m e ' ' r p a l m ' ) > t h e n b e g i n m s t F i l eName •*=' p a l m ' e n d ;

i f ( <name= ' I f i n g e r s ' ) o r ( n a m e ' ' r f l n g e r s ' ) ) t h e n b e g i n m s t F i l e N a m e f i n g e r s ' end

endend i

i f < l e n g t h ( m s t F i l ename > 0 ) t h e nb e g i n < t h e name i s an a c t u a l Segment Name >

i n p f i l e ! “ c o n c a t ( d n v e _ m s t r m s t F l l en a me f ' • ms t ' ) }

a s s i g n ( i n p ' i n p f i l e ) ! r e s e t C i n p ) ; o u t f i l e . ' = c o n c a t ( d n v e _ w l d »na»ie , " . w l d ' ) ; a s s i g n ( o u t t o u t f i l e ) i r e u r i t e ( o u t ) i u o r l d _ f i l e ' . p > ; c l o s e ( i n p ) ; c l o s e ( o u t >

end ELSE

186

b e g i n < t h e name i * n o t a Segment Nome >u r i t e l n ( ' -------------------------------------------------W O R L D F I L E ------------------------------------------- ' > iu n t e l n < ' you d i d i t a g o i n J— > ' » FA . n a m e » ' < - - i n p u t t e d t o w o r l d f i l e ' ) w r i t e l n ( ' i s n o t t h e name o f o bo dy s eg me n t ' )

ende n d! < W o r l d F i l e >< >

<-------------------------------------------------------------------------------------------■C d o t e ! 3 - m a r c h - 1 9 8 6< f u n c t i o n ! i n p u t o f segment nomet t h e p r o g r a m t h e n c a l c u l a t e s■C o i l w o r l d f i l e s r wh i ch a r e a f f e c t e d i f t h e a n g l e s{ i n t h e a c t u a l s e gm e nt a r e changed

p r o c e d u r e R e c T r e e U a l k e r ! v a r l i u i p o i n t e r }

f u n c t i o n S t o p R e c u r s i o n l q ! p o i n t e r ) ! b o o l e a n ! beg in

s t o p R e c u r s i o n ! « f a l s e > w i t h q A do

b e g i ni f ( t l e f t = n i l > and ( r i g h t « n i l ) and

( o r <down“ n i l > ) > t h e n b e g i n s t o p R e c u r s i o n ! " t r u e end

ende n d ! < s t o p R e c u r s i o n >

b e g i n { main p r o c e d u r e body R e c T r e e U a l k e r > i f s t o p R e c u r s i o n < h e a d t t o e s , f i n g e r s >

THENb e g i n w o r l d F i l e ( p ) end

ELSEb e g i n < r e c u r s i o n >

i : * l o t o r s o p t r ! u ! c l A. u p A . u p A . u p A . u p A . up! i f < (p = l > o r o r ( p = l A . u p A . u p > o r o r

 o r ( p = u > )THEN

b e g i n < upwar d d i r e c t e d s e gm e nt s >i f ( ( p = l ) < l o w e r t o r s o > o r <pBu> i R e c T r e e U a l k e r w o r l d F i l e ( p ) { R e c T r e e U a l k e r <pA. u p )

endend -C upward d i r e c t e d s e gm en t s >

ELSEb e g i n { downward d i r e c t e d s e qm en t s >

w o r l d F i l e ( p ) ! R e c T r e e U a l k e r ( p A. do wn ) end { downward d i r e c t e d s egm ent s >

end < r e c u r s i o n > e n d ! < R e c T r e e U a l k e r >

< >

>>>>>

187

< ---------------------------------------------------------------------------------------------------------------------------------------------------------- >{ f u n c t i o n ! used f o r i n t e r a c t i v e o p e r a t i o n >< t h e p r o g ra m f i n d * t h e s egm ent p o i n t e r g i v e n t h e >{ s e g m e n t ' s name >

p r o c e d u r e F i n d F o i n t e r ( s e g m e n t N a m e ! s t r B } v a r p l p o i n t e r ) } v a r h e a d p t r ! p o i n t e r }

p r o c e d u r e F i e c F i n d F o i n t e r ( q ! p o i n t e r ) } beg in

i f < ( p * n i l ) and (q <> n i l ) ) < t e r m i n a t i o n c o n d i t i o n > t h e n b e g i n

i f <qA. n a m e « * e g * e n t N a m e ) t h e n b e g i n p ! » q end

ELSEb e g i n { r e c u r s i o n >

F i e c F i n d F o i n t e r ( q A . down > 1 R e c F i n d P o i n t e r ( q A . l e f t > }R e c F i n d F o i n t e r ( q A . r i g h t )

end < r e c u r s i o n >end

e n d! < R e c F i n d P o i n t e r >

b e g i n { main p r o c e d u r e body F i n d F o i n t e r >p : * > n i i ; h e a d p t r l » l o t o r s o p t r A . u p A. u p A . u p A . u p A . u p A . u p } R e c F i n d F o i n t e r t h e a d p t r )

e n d} < F i n d F o i n t e r > < >

{FUNCTION i t e s t i n g o f t h e c o n t e n t s o f t h e r e c o r d s c o n t a i n i n g t h e >{ body s egm ent d e s c r i p t i o n >

p r o c e d u r e p r i n t _ r e c o r d s }

p r o c e d u r e p r n t r e c o r d ( p I p o i n t e r ) }

v a r r o w i c o l ! i n t e g e r }

beg i nw i t h p A do

beg i nw r i t e l n < l s t , ' * * * * * * * * * * * * * * * * * * r e c o r d } n a m e , ' * * * * » * » * * * * * * » ' > } u r i t e l n ( 1 s t ) j w n t e l n ( 1 s t , ' A_ABS ' ) }f o r r o u ! = l t o 3 do

beg i nf o r c o l ! “ l t o 3 do

b e g i nw r i t e ( 1 s t , a _ a b s t r o w , c o l 3 : 1 2 ! 3 > e n d } w r i t e l n ( 1 s t )

e n d } w r i t e l n ( l s t ) } w r i t e l n < 1 s t ) j w n t e l n < 1 s t , ' A_FiEL ' ) }f o r r o w ! « l t o 3 do

beg i nf o r c o l ! « 1 t o 3 do

beg inw r i t e ( 1 s t . a _ r e l [ r o w , c o l i : i 2 ! 3 ) end } w n t e l n ( 1 s t >

188

e n d } w r i t e l n ( l » t ) l w r i t e l n ( l s t l !w r i t e l n d s t , ' L K L_RIGHT L . L E F T OR IG I NFOR r o n ! * l t o 3 do

b e g i nw r i t e l n < l s t , l i : r o w 3 : i 2 ! 3 » K C r o w 3 : i 2 : 3 »

l _ r i g h t C r o w 3 : i 2 ! 3 , l _ l e f t C r o w 3 ; i 2 ! 3 , o r i g i n _ a b s C r o w 3 ! 1 2 : 3 >e n d } w n t e l n ( l s t > }

w r i t e l n d s t , ' L_ABS K_ABS L_RIGHT_ABS L_ LEFT _ABS' ) }FOR r o w I = l t o 3 do

b e g i nw r i t e l n d s t , l _ h R S [ r o u 3 : i 2 : 3 , K _ a b s C r o u 3 : 1 2 1 3 ,

l _ r i g h t _ a b s t r ow3 ! 1 2 1 3 . 1 _ 1 e f t _ o b * C r o w 3 } 1 2 1 3 ) end } w r i t e l n <1s t >!

WRITELN < LST >}w r i t e l n ( 1 s t . ' a n g l e s t h l . t h 2 . t h 3 ' ) »w r i t e l n ( l s t , t h [ 1 3 , t h C 2 3 , t h C 3 3 > }I F ( u p = n i l > t h e n BEGIN w r i t e l n d s t . ' p o i n t e r UP S N I L ' ) END

e l s e b e g i n w r i t e l n ( 1 s t . ' p o i n t e r up — > ' , u p * . n a m e ) END}I F ( P O W N = n i l ) t h e n BEGIN w r i t e l n ( , l s t . ' p o i n t e r DOUN } N I L ' ) END

e l s e b e g i n w r i t e l n ( 1 s t . ' p o i n t e r DOUN — > ' . d ow n* .name) END)I F ( L E F T - n i l ) t h e n BEGIN w r i t e l n d s t . ' p o i n t e r LEFT ! N I L ' ) END

e l s e b e g i n w r i t e l n d s t , ' p o i n t e r LEFT — > ' . l e f t A. n a m e ) END}I F ( R I G H T = n i 1 > t h e n BEGIN w r i t e l n d s t . ' p o i n t e r RIGHT : N I L ' ) END

e l s e b e g i n w r i t e l n d s t , ' p o i n t e r RIGHT — > ' , r i g h t * . n o n e ) ENDEND

e n d }

b e g i n 

p ! “ LOTORSOPTR p r n t r e c o r d < p ) p r n t r e c o r d ( p * p r n t r e c o r d <p* p r n t r e c o r d < p * p r n t r e c o r d < p * p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d <p* p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d < p * p r n t r e c o r d ( p * p r n t r e c o r d <p* p r n t r e c o r d < p * p i c l o t o r s o p t r p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d < p * p r n t r e c o r d ( p * p r n t r e c o r d ( p * p r n t r e c o r d ( p *

up)}u p * . u p )» u p * . u p * . u p ) } up * . u p * . u p * . up * . u p * . u p * . up * ■ u p * «up * . l e f t ) }l e f . * .down ) } l e f t * . d o w n * . l e f t * . d o w n * , r i g h t > } r i g h t * . d o w n > r i g h t * . d o w n * r i g h t * . d o w n * • u p * . u p * . u p * l e f t ) }l e f t * . d o w n ) } l e f t * . d o w n * . l e f t * . d o w n * . r i g h t ) } r i g h t * . d o w n > r i g h t * .down* r i g h t * . d o w n *

u p ) }u p * .up > } u p * . u p * . u p ) }

down) }d o w n * . do wn ) }

1 down) } . d o w n * . d o w n ) ! . u p * . u p }

down) }d o w n * . down) }

. d o w n ) } • d o w n * . down)

en d} 

{ f u n c t i o n i g e n e r a t i o n o f a p l o t f i l e o r o f p l o t c o e a a n d s , w h i c h a r e >< t h e i n p u t f i l e i s c o n t r o l l e d by R LOT F IL E , >

189

< t h e o u t p u t f i l e i s opened by OFENSEGMENT and c l o s e d by >< CLOSESEGMENT . >

p r o c e d u r e p 1o t f i 1e ( p ! p o > n t e r >( v or i n p f 1 1 e ! B t r l 4 !

b e g i n < F l o t E i l e >< open i n p u t f i l e >

i n p f i l e : “ c o n c a t < d r i v e _ u l d r p n , n a e e » ' . w l d ' ) } o * » i g n c 1o s e ( i n p )

end I < F l o t E i l e >

p r o c e d u r e r ' i s p l a y F i l e s < s e g m e n t n o ! i n t e g e r ) ! beg i n

< g e n e r a t e se gm en t ed p l o t f i l e s >

< 1 s t s e gm e n t 1 TOFiSO and HEAO >O p e n S e g » e n t ( * e g » e n t n o ) !

 i n i t g r a p h l e s I

p i -LOT ORSOFTR!p l o t f l l e ( p ) Ip l o t f i l e 1p l o t f i l e }p l o t f i l e 

< 2nd s e gm e nt ! LEFT LEG >< O p e n S e g m e n t ( s e g m e r t n o + 1 ) ; >

p l o t f i l e ( p A F l e f t ) l p l o t f i l e < O p e n S e g m e n t <s e g m e n t n o + 2 >! >

p l o t f i l e ( p A .right)! p l o t f i l e ( p A .r i 9 h t A. d o u n ) i p l o t f i l e ( p A F r i g h t A . downa f d o u n ) ! p l o t f i l e ( p A .r i g h t A f dounA f d o unA f do wn)!


{ 4 t h s e gm e nt ! LEFT ARM >{ O p e n S e g m e n t ( s e g m e n t n o t 3 ) ! >

p : * = l o t o r s o p t r A FupA F u p r . u p AFupA Fup!

190

p l o t f i l e ( p A. l e f t ) | p l o t f i l e 

< 5 t h s e gm e nt ! RIGHT ARM >< O p e n S e g m e n t ( s e g m e n t n o t 4 > ; >

p l o t f i l e ! p l o t f i l e { p l o t f i l e ( p A. r i g h t A. downA . down >! p l o t f i l e ( p A. r i g h t A. d ow nA . downA. do wn ) !

C1 os eS egmen t end? < t ' i s p l a y F i l e s >

p r o c e d u r e S e t A n g l e s < v a r segmentname ! s t r B } va r s e g s e n t p t r I p o i n t e r ) i v a r i : i n t e g e r . d o n e i b o o l e a n !

b e g i n < s e t A n g l e s > d o n e ! « f a l s e ; w h i l e n o t done do

b e g i n w r i t e l n <' wr i t e l n <' w n t e l n < ' w r i t e l n ( ' w r i t e l n <' w r l t e l n ( ' w r i t e l n <' w r i t e i ' r e p e a t

b u f l e n : «e ;r e a d 1 n ( se gment Na me)>

u n t i l l e n g t h ( segment Nome> > 0 ! < segmentName c o n t a i n s a t l e a s t 1 c h a r >

i f se gment name <> ' ' t h e n d o n e ! “ t r u e e n d ; < w h i l e l o o p >F i n d F o i n t e r ( s e g m e n t N a m e . s e g m e n t p t r ) ; w i t h s e g m e n t p t r A do

beg i nf o r i ! «1 t o 3 do

b e g i nu n t i l ' t h e t a C ' . l . ' 3 « ' ) ; r e a d l n ( thC i 3 ) ;t h C i 3 ! * > t h C l 3 * p l / l B O

endend

e n d ; < S e t A n g l e s >

p r o c e d u r e I n t e r A c t l v e l n p !v a r f l n a m e I pacKed a r r a y C l . . 643 o f s t r l 4 ;

name I s t r 8 ; n o f l l e s ! i n t e g e r ;

b e g i n { I n t e r A c t l v e I n p > w h i l e n o t done do

beg in

S e t Segment A n g l e s i n t e r a c t i v e l y ; se gment s a r e named ' ) ! as shown b e l o w '> i

I o t o r s o i r i b 4 ; r i b 3 ; r i b 2 ; r i b l ! u p t o r s o ; h e a d ; ' ) ; u r l e g ; 1 r l e g ! r f o o t ! r t o e s ( ' ) ;u l l e g ; 1 1 l e g ; 1 f o o t ;1 t o e s i ' ) } u l a r m ! 1 l a r m t I p a l m ! I f i n g e r s ; ' ) !u r a r i i j l r o r s j r p o l s j r f i n g e r s ) ' ) ;

Segment name • ? ' ) i

191

r e p e a tw r i t e l n ! ' B eg i n ( S e t A n g l e s ( Ch a ng e F e r s p . , Read F i l e ' ,

' ( P r i n t r e c o r d * . Q u i t ' ) ! u r i t K ' C B / 5 / C / R / F / Q a * ? ' ) !r e a d I n (cmdch ) i

u n t i l cmdch i n C ' E<' r ' S ' . ' C ' . ' G ' . ' R ' . ' F ' 11 c a s e cmdch o f ' F ' l p r i n t _ r e c o r d s (' B ' ! beg in

< m i t . o f p e r s p e c t i v e and s y s t e m p a r a m e t e r s >I n i t P e r s p e c t i v e !

{ i n i t i a l i z a t i o n o f body s egm ent p a r a m e t e r s and c o m p u t a t i o n >< o f n i as t e r f i l e s >a d a m i n 11 < ' c l s e g m n t . d a t ' , l o t o r s o p t r ) !

w r i t e ( ' E n t e r f i l e name t o r e a d d a t a f r o m C f i l e n a m e . d a t ] : ' ) !r e a d l n ( n a m e >;f l n a m e C l l i * c o n c a t ( n a m e r ' . d a t ' > !

■C r e a d one r e c o r d o f f i l e b l s e g _ a n g . d a t > r e a d a n g l e s < f l n a « i e C l l ( l o t o r s o p t r r l ) (

RecT r e e U a l k e r ( l o t o r s o p t r >;

■C PRINT.RECORHS! > r e p e a t

w r i t e ! ' s egm ent number C l . . 6 4 ] « T ' ) !r e a d l n <s e g m e n t n o > I

u n t i l se gment no i n C l . . 6 4 3 } d i s p l a y f i l e s ( s e g m e n t n o >

end !

' S ' ! b e g i n < c ha ng e a n g l e s o f body s e gm e nt s i n t e r a c t i v e l y >< and co mp ut e new w o r l d f i l e o f t h o s e s e gm en t s >< o n l y t wh i c h a r e a f f e c t e d by t h e ch ang e o f a n g l e s >

S e t A n g l e s ( se gment Na mer s e g m e n t p t r ) !R e c T r e e U a l k e r ( s e g m e n t p t r ) ! r e p e a t

w r i t e ! ' s egm ent number C l . . 641 « ? ' ) !r e a d l n ( se gment no >!

u n t i l se gment no i n C l . . 6 4 3 ! d i s p l a y f l i e s ! s e g m e n t n o )

e n d !

' C ' ! beg i nI n i t F e r s p e c t i v e !{ FRINT_RECORCSi >R e c T r e e U a l k e r ( l o t o r s o p t r > ! < c a l c u l a t e a l l w o r l d f i l e s > r e p e a t

w r i t e ( ' segment number C l . . 6 4 ] = ? ' ) !r e a d l n ( s e g m e n t n o ) !

u n t i l s egment no i n C l . . 6 4 1 ! d i s p l a y f i l e s < s e g m e nt n o )

end !' O ' ! beg i n

d o n e ! = t r u e e n d !

192

' R ' : b e g i nr e p e o t

w r i t e ( ' E n t e r number o f f i l e you wo ul d l i k e r e a d i n C l . . 641 : ' > !r e a d I n < n o f i l e t )

u n t i l ( n o f i l e e >■** 1 ) and ( n o f i l e s <■ 6 4 ) !

f o r i ! * 1 t o n o f i l e s do b e g i n

w r i t e < ' E n t e r f i l e name t o r e a d d a t a f r om t f i l e n a m e . d a t ! ! ' ) )r e ad I n ( n a m e > if l n a m e C i l ! * c o n c a t ( n a m e t ' . d o t ' )

e n d !

f o r i ! « 1 t o n o f i l e s do b e g i n

{ r e a d one r e c o r d o f f i l e > r e a d a n g l e s ( f l n a m e t i l r l o t o r s o p t r r D !• a p U l d t o S c r r e a d y ! • f a l s e !

R e c T r e e U a l k e r ( l o t o r s o p t r ) !

< PRINT_RECORI |S ! > s e g me nt n o ! « i i d i s p l a y f i l e t ( s e g m e n t n o ) end

endend { c a s e s t a t e m e n t >

end < w h i l e l o o p > e n d ) 

b e g i n < AIukM main p r o g r a m body >

d n v e _ d a t !■= ' c ! ' ! d n v « . s » t i « ' c ! ' ! d r i v e _ w l d !■ ' c ! ' ! d r i v e _ p 1 1 ! = ' d : ' !

{ r e l a t i v e a n g l e s a r e i n p u t t e d > r e l o r a b s _ a n g l e s ! = t r u e ! d o n e ! = f a l s e !I n t e r a c t i v e l n p t t e K t c o l o r ( 9 )

e n d . { adam ma i n p r o g r a m body >< >

193

A.3 The Modules for Animation

A program l i s t i n g of the subroutines which accomplish the animation of the humanoid are given below:

VERSION OF I N T E R . PAS : VERSION • 1 . 0 . MAY 3 1 , 1966

I NTE R. FAS i s an i n t e r p o l a t i o n r o u t i n e w h i c h u t i l i z e s an i n t e r p o l a t i o n method Know as c u b i c s p l i n e s

r e a d _ i n _ d o t a 5 r e a d s d a t a f r o m i n p u t f i l e and s t o r e s i n p o i n t e rs t r u c t u r e s

g e t _ i n f o ! p r om ts u s e r f o r i n f o r m a t i o n needed t o g a t h e r d a t ac r e a t e f i l e s ! c r e a t e s number o f f i l e s ne ede d t o s t o r e i n t e r p o l a t e d

d a t ar e a d _ d a t a ! r e a d s d a t a f r o m f i l e t o d e t e r m i n e t y p e o f i n t e r p o l a t i o n

and number t r a c K s , s t e p s , and p i v o t s o f d a t a f o r m _ 4 _ x _ 4 ! c r e a t e s ne ede d f o u r by f o u r m a t r i x f o r c a l c u l a t i o nm u l t i p l y ! m u l t i p l i e s a m a t r i x by a v e c t o rf o r m _ m a t r i x I c r e a t e s a v a r i a b l e s i z e d m a t r i x d e p e n d a n t on t h e number

p i v o t s t h e d a t a h a s f o r m _ v e c ' c r e a t e s a f o u r by on e m a t r i x f r o m d a t a i n p o i n t e r

s t r u c t u r e sc a l c a l l d a t a ! c a l c u l a t e s and s t o r e d a t a c a l c u l a t e d f r o m a s i n g l e t r a c kp r o c e s s I c a l l s c a l c a l l d a t a f o r e a c h t r a c k

< * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * » * >

<p rog rammer< d a t e< f u n c t i o n

Todd M. F e n k o w s k i 3 1 - m a y - 8 6t o i n t e r p o l a t e a n g l e s f o r p r o g r a m adam

p r o g r a m i n t e r p o l a t i o n ( i n p u t , o u t p u t , i n p , o u t ) ;

c o n s tm a x p i v o t s ■= 20? p i ■= 3 . 1 4 1 5 9 2 6 5 4 ;

t y p es t r = s t r i n g C 2 0 3 J i o _ t y p e « t e x t ; p o i n t e r t y p e « An o de t y p e J n o d e t y p e » r e c o r d

v a l u e : r e a l ; r i g h t ! p o i n t e r t y p e ; down I p o i n t e r t y p e e n d !

v e c _ t y p e « a r r a y E 1 . . m a x p i v o t s 3 o f r e a l ; m a t _ t y p e * a r r a y C l . . m a x p i v o t s ! o f v e c _ t y p e !

i n p ! t e x t ; o u t t t e x t ; i n p f i l e ; s t r ;

194

f i l e s o u t S a r r a y C 1 . . A 4 1 o f s t r )d a t a p n t r , d p n t r , t r c p n t r , t r c 2 p n t r , p i v p n t r ! p o i n t e r t y p e )n t r a c K i t n s t e p s , n p i v o t s , v a l _ n u m b e r ! i n t e g e r )f o u r x 4 ! m a t _ t y p e )m a t r i x i m a t _ t y p e )v e c t o r , v e c ' ) ! v e c _ t y p e )t a n g e n t ! v e c _ t y p e )i n t e r _ t y p e ! c h a r )i n p u t _ d e v ) c h a r )d i s K ) s t r i n g C23)i n f i l e , o u t f i l e s t r i n g C8D)

< h e ad s d a t a i n f r o m a p r e v i o u s l y a s s i g n e d f i l e >< o r f r o m t h e Ke yb o ar d >

p r o c e d u r e r e a d _ i n _ d a t a » v a r

p o i n t e r , p o i n t e r l , p o i n t e r 2 ) i n t e g e r ) teiup ) i n t e g e r )

b e g i n

{ l o o p t o r e a d i n e ach p i v o t o f d a t a

f o r p o i n t e r 2 ) “ 1 t o n p i v o t s do beg i ntemp )■ r o u n d ( n t r a c K s / 1 8 + 0 , 5 ) ) p o i n t e r ) « 1)

•C l o o p t o r e a d i n d a t a p o i n t s f o r e a c h p i v o t >< r e a d s 18 v a l u e s o f f each l i n e >

f o r p o i n t e r l ) » 1 t o temp do b e g i n

w h i l e ( p o i n t e r < * ( 1 8 * p o i n t e r l )> and n o t ( p o i n t e r > n t r a c K s ) do b e g i n

n e u ( d a t a p n t r ) )



i f p o i n t e r « 1 t h e n b e g i n

i f ( p o i n t e r 2 « 1 ) t h e n b e g i n

d p n t r i « d a t a p n t r ) t r c 2 p n t r i « d a t a p n t r )

end e l s e

t r c 2 p n t r != p i v p n t r ) p i v p n t r )■= d a t a p n t r ) t r c p n t r )= d a t a p n t r

e n d )

< r e ad i n a d a t a v a l u e >

i f i n p u t _ d e v «= ' F ' t h e nr e a d d n p , d a t a p n t r A . v a l u e )

e l s e b e g i n

w r i t e ( ' e n t e r n e x t d a t a v a l u e ) ' ) ) r e a d ( d a t a p n t r A . v a l u e ) )

195

w r i t e l n • n d t

t i n c r e m e n t v a r i a b l e p o i n t e r ond p o i n t e r s t r u c t u r e s >

p o i n t e r ! ■ p o i n t e r + 1$ i f ( p o i n t e r ^ O 1 ) t h e n i f p o i n t e r 2 <> n p i v o t s t h e n

b e g i nt r c S p n t r * . r i g h t ! « d a t a p n t r ! t r c 2 p n t r ! « t r c 2 p n t r * . down end

e l s e b e g i nt r c 2 p n t r * . r i g h t !*= d a t a p n t r ! t r c 2 p n t r * . r i g h t * . r i g h t n i l !t r c 2 p n t r ! • t r c 2 p n t r * . down e n d !

t r c p n t r A .down ! “ d a t a p n t r ! t r c p n t r ! = d a t a p n t r

e n d !

■C s e t l a s t p o i n t e r i n e a c h p i v o t t o n i l >

t r c p n t r * . down ! « n i lend

ende n d !

t r e a d s i n t e r p o l a t i o n t y p e , n u a b e r o f t r a c k s , number o f s t e p , and >< number o f p i v o t s i f n o n l i n e a r , f r o m a f i l e p r e v i u o s l y d e f i n e d >

p r o c e d u r e r e a d _ i n f o !

v a r o n e _ c h a r ! c h a r !

b e g i nr e s e t ( i n p ) ! r e a d ( i n p , o n e _ c h a r ) ! w n t e l n ( ' r e a d i n g i n f i l e ' ) !w h i l e n o t < ( o n e . c h a r « ' L ' > o r ( o n e _ c h a r = ' N ' ) > do

b e g i nr e a d l n ( i n p ) ! r e a d ( i n p , o n e . c h a r )

e n d !

i n t e r _ t y p e ! “ o n e . c h a r ! i f i n t e r . t y p e = ' N ' t h e n b e g i n

r e a d ( i n p , n t r a c K s , n s t e p s , n p i v o t s ) !w r i t e l n ( ' number o f t r a c k s = ' ! 1 9 , n t r a c k s ! 3 , ' nu mb e r o f s t e p s = ' ! 2 5 ,

n s t e p s ! 3 , ' nu mb er o f p i v o t s *= ' ! 2 5 , n p l v o t s ! 3 >ende l s e

b e g i nr e a d ( i n p , n t r a c k s , n s t e p s ) !w n t e l n ( ' nu mb er o f t r a c k s = ' ! 2 5 , n t r a c k s ! 3 , ' n u m b e r o f s t e p s = ' ! 2 5 ,

n s t e p s ! 2 >! n p i v o t s ! * 2

e n d !

196

r e o d _ i n _ d o t a end ;

< c r e a t e s a l l f i l e s needed f o r o u t p u t >{ t h e number o f f i l e s need i s >< ( n um b er o f p i v o t s - 1 ) t i d i e s number o f s t e p s + 1 >

p r o c e d u r e c r e a t e _ f 1 l e s i

t y p eo d d _ t y p e ■ s t r i n g C2D{

v a rI : i n t e g e r ? se r_n um ! a d d _ t y p e {

< r e c i e v e s a number ' i n n u m b e r ' and c o n v e r t s i s t o an a l p h a n u m e r i c >< s t r i n g >

p r o c e d u r e c h a r a c t e r i z e ( number ! i n t e g e r ! v a r a d d i t i o n I o d d _ t y p e ) { va r

t i m e s , l o o p , num ! i n t e g e r ; c h a r l I c h a r ;

beg i na d d i t i o n I ' ' ' {i f number > 9 t h e n l o o p 2 e l s e l o o p 1= 1! f o r t i m e s : « 1 t o l o o p do

b e g i nnum number - t r u n e ( n u m b e r / 1 0 ) * 1 0 ic a s e num o f

1 c h a r l • K ' l ' {o c h a r l J K ' 2 ' i3 c h a r l | m ' 3 ' ;A c h a r l • e ' A ' \5 c h a r l J s * 5 ' ;6 c h a r l J B ' 6 ' ;7 c h a r l » B ' 7 ' ;B ch a r l { B ' B ' !9 c h a r l :«« ' 9 ' ;0 c h a r l J B ' 0 '

end ia d d i t i o n 1= c o n c a t ( c h a r l , a d d i t i o n ) ; number : « t r u n c ( n u m b e r / 1 0 >

ende n d ;

b e g i nf o r I 1= 1 t o n s t e p s * ( n p l v o t s - 1) + 1 do

beg l n< g e t a l p h a n u m e r i c s t r i n g o f I ' s v a l u e >

c h a r a c t e r i z e d , s e r _ n u m ) !f l l e s o u t [ 1 1 ; = c o n c a t ( d i sh., o u t f i l e , s e r _ n u m , ' . d o t ' ) ; a s s i g n ( o u t , f i l e s o u t C I l ) ;

r e u r i t e ( o u t ) ; < c r e a t e s f i l e and c l e a r s c o n t e n t s >w r i t e l n ( o u t , ' ' ) l u n t e l n ( o u t , ' ' ) { w r i t e l n ( o u t , ' ' ) { w r i t e l n ( o u t , ' c l o s e ( d u t )

end

' >; wr 1 1(

197

end J

< g e t s i n f o r m a t i o n os t o wh er e d a t a i s coming f r o m ! >< K e y b o a r d or f i l e , and t h e i n t e r p o l a t i o n t y p e , t r a c K s >< s t e p s and p i v o t s i f i n p u t i s f r o m K e y b o a r d >

p r o c e d u r e g e t _ i n f o !

beg in r e p e a t

w r i t e ( ' I n p u t d e v i c e ! K eyb oa rd o f F i l e C K / F ] ! ' ) ! r e a d l n ( i n p u t _ d e v ) i

u n t i l f w r i t e l n ! i f i n p u t _ d e v ■ ' F ' t h en

b e g i nw r i t e ( ' I ' r i v e Ch! / b ! / C I / I i ! 3 * ' ) ! r e a d l n ( d i s K ) !w r i t e ( 'N am e o f i n p u t f i l e C i n f i l e . d o t ] , i n f i l e ■ ' ) }r e a d l n ( i n f i l e ) !w r i t e l n !i n p f i l e ! ■ c o n c a t ( d i » K , i n f i l e , ' . d o t ' ) ! a s s i g n ( i n p , i n p f i l e ) iw r i t e ( ' N a m e o f o u t p u t f i l e C o u t f l l e . d a t 3 , o u t f i l e ■= ' ) ! r e a d l n ( o u t f i l e ) ! w r i t e l n ! r e a d _ i n f o

ende l s e

b e g i nr e p e a t

w r i t e ( ' L i n e a r o r N o n - l i n e a r i n t e r p o l a t i o n C L / N ] ! ' ) !r e a d l n ( i n t e r _ t y p e ) i

u n t i l ( i n t e r _ t y p e » ' N ' ) o r ( i n t e r _ t y p e * ' L ' ) ! w r i t e l n i w r i t e ( ' N u m b e r o f t r a c K s C1 . . n t r a c K s ] , n t r a c K s «= ' ) ! r e a d 1n ( n t r a c K s ) ! w r i t e l n !w r i t e ( ' n u m b e r o f i n t e r p o l a t i o n s t e p s C l . . n s t e p s ] , n s t e p s = ' ) ! r e a d I n ( n s t e p s ) ! w n t e l n ! i f i n t e r _ t y p e • ' N ' t h e n

b e g i nw r i t e ( ' n u m b e r o f p i v o t p o i n t s C1 . . n p i v o t s ] , n p i v o t s = ' ) ! r e ad I n ( n p l v o t s ) ! w r i t e l n

ende l s e

n p i v o t s ! * 2 !w n t e C ' N a m e o f o u t p u t f i l e C o u t f i l e . d a t ] , o . u t f i l e = ' ) ! r e a d 1n ( o u t f i l e ) ! r e a d _ i n _ d a t a

enue n d ,

< as si gn s val ue s to matri;: (see c a l c u l a t i o n s on cubic splines) >

p r o c e d u r e f orn,_4_;:_4 ! vu r

Lount 1 , countZ' ! integer!

beginf ou rx4 C 1,1 ] != 2! f ou r C 1 ,2 j ! •= - 2 !

198

f ou r * 4 C 1 * 33 « i ;f ou t k 4 t 1*43 « 1 ;four ;:4 C 2*13 * - 3 ;f o u r * 4 C 2 * 2 3 * 3;f o u r : : 4 £ 2 * 3 34 ou r ;:4C 2 * 4 j • - i tf o u r x 4 C 3 * 1 3 * o ;f our;:4C3*23 « 0{four;:4C3*33 * l ;f our>:4C 3* 4 3 * 0}f ou rx4 C 4 * 13 * l *f ou r>:4 C 4*23 « o ;f o u r x 4 t 4 * 3 3 - o ;f ou rx4 C 4 * 4 3 e o ;for cou ntl J* 1 to

for c o u n t 2 J ■ 5 t o k O K P i V D l t do f o u r ; . '4CcountJ r c o u n t 2 J 0 !

f o r c o u n t l ! = 5 t o m a x p i v o t s do f o r c o u n t 2 !■ 1 t o f do

f o u r x A C c o u n t l t c o u n t 2 T ! “ Oi• n d ;

< m u l t i p l i e s o m a t r i x by a v e c t o r t o r e t u r n o v e c t o r >

p r o c e d u r e mu 1 1 i p 2y <s 1 r e : i n t e g e r ; e a t : m a t _ t y p e } v e c ! v e c _ t y p e j v a r t o n ! v e c _ t y p e ) i

rowc o l

i n t e g e r i n t e g e r !

b e g i n< c l e a r t a n v e c t o r >f o r row : « 1 t o m a x p i v o t s do

t a n C r o u ] ! * Oi< go t h r o u g h a l l rows o f m a t r i x > f o r row : « 1 t o s i z e do

beg i nt a n t r o w l ! “ Oi

f o r c o l !■ 1 t o s i r e dot a n t r o w l i * t a n t r o w l + m a t t r o w F c o l l * v e c t c o l l

endend i

< f o r m s a m a t r i x d e p e n d i n g on t h e number o f p i v o t s >

p r o c e d u r e f o r m _ m a t r i K ; v a r

c o u n t l i i n t e g e r ! c o u n t l i i n t e g e r }

(*»**************************************)< * SUBROUTINE: HAT I N U . FAS * )( a * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * )

( a t * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * /

( * FROGRAHMER! HELMUT BUCHNER, m e t e r l a n g e r * /( * BATE: JUNE 4 , 19 82 * /

199

( * • /( * f u n c t i o n : i n v e r t t h e i n e r t i a m a t r i x * /(* */<* u s e r g u i h e : m a t i n t <a , d > * /<* * /<* FROGRAHMER * /<* g u i d e : > p a s m a t i n v * m a t i n v * /<* >hAC H ATI NT“ MATINT * /< * * /<* PROCEDURES * /<* c a l l e d : n o n e * /<* */<* GLODAL VARIABLES * /<* r e f e r e n c e d : n o n e * /<* * /<* GLODAL VARIARLES * )c* m o d i f i e d : n o n e * ><* * )(*»*******»*(*t*t»»*tt»**(*t»»****»****»»***(**»*»*»»*********)

(»»»******************»*****•»***»*»*»*»********»»*»»*»*****»»/ /* * // * GAUSS - SEIDEL ALGORITHM I S EMPLOYED * // * * //**»**»***•*****»*»»*»***»»****»*****»***•»»•*»***»**»»»***•**)

FROCEDURE i n v f c o t r 1 >:(VAR A: B O t . t y p e ) ;

LADEL 1 0 0 , 1 1 0 ;

VAR I , I J , I K , I 2 , J , J I , J K , J F , J O , J R , K , K I , K J , K K , N , N K : i n t e g e r ;d i g a , h o l d : r e a l ;L , M : ARRA YC1 • . :

<* SEARCH FOR THE LARGEST ELEMENT *>

N :■ n p i v o t » - 2 !D : = 1 . 0 ; nk :■= - n ;FOR K : « 1 TO N DO

DEGINnk : « nk + n ; l ckd:>= k; h c k j : - k; k k : - nk + k ; d i g a : « a c k . k i ;

FOR J i = K TO N DO DEGINI Z N * ( J - l ) ;FOR I ! » K TO N DO

DEGINi j :*= i z + 1 ;I F ADS( DIGA) - ADS ( AC I , J3 ) -.0 • 0 THEN

200

BEGINBIGA A C I . J D ; LCK] := I f MCK] •■* J}e n d ;

e n d ;e n d ;

( * INTERCHANGE ROUS * )

j : « l c k ] ;I F J - K > 0 THEN

BEGINk i ; = k - n ;FOR I ! - \ TO N DO

BEGINk i :*= k : + n ; h o l d - a c n , i d ; j i :■= k i - k + j ; a c k . i j a c j , i 3; a c J r i ^ : « h o l d ; e n d ;

e n d ;

( * INTERCHANGE COLUMNS * )

I : « MCK] {I F I - K > 0 THEN

BEGINJR ! » N * ( 1 - 1 ) ;FOR J : - 1 TO N DO

BEGINJK : » NK + j ; j i : ■ j r + j ;HOLD :« - a c j . k i ; a c j , k 3 : « a c j . i d ; a c J r i 3 h o l d ; e n d ;

e n d ;

( * D I V I D E COLUMNS BY MINUS RIVOT (VALUE OF PIVOT ELEMENT * /

( * I S CONTAINED I N BIGA) * )

I F BIGA » 0 . 0 THEN BEGINd : = o . o ;GOTO n o ;END ELSE BEGINFOR i : * l TO N DO

BEGINI F I - K C- 0 THEN

BEGINI K : « NK + i ;AC 1 1K I A C I r K ] / ( - B I G A ) ;e n d ;

e n d ;

201

(t REDUCE MATRIX *>

FOR I l « l TO N DOBEGINIK !«= NK + I?HOLD J - A C I , K 3 ! i j : = I - N t FOR J ! * l t o n do

BEGINi j : * i j + n ;I F I - K <> 0 THEN

BEGINI F J - K <> 0 THEN

BEGINk j : « i j - i + k ;A C I . J I HOLD * A t K , J J + At 1 , J Ie n d ;

e n d ;e n d ;

e n d ;

( * D I V I D E ROU BY P I VOT * )

k j ; « k - n ;FOR j ; « l TO N DO

BEGINk j k j + n ;I F J - K <> 0 THEN A C K . J I !«= AC K . J I / BIGA;e n d ;

< * PRODUCT OF PIVOTS *>

d : « d * b i g a ;

( * REPLACE PIVOT BY RECIPROCAL * )

ACK.KD : « 1 . 0 / b i g a ; e n d ;

e n d ;

<* F I NA L ROU AND COLUMN INTERCHANGE *>

k n ; i o o : k : = k - i ;

I F K > 0 THEN BEGINi : ■ l c k i ;I F I - K > 0 THEN

BEGINj o := n * ( K - i >; j r : = n * ( i - i >;FOR j : « l TO N DO

BEGINj k : « j o + j ;HOLD :■= A C J . K 3 ; j i : « j r + j ; a c j . k i : « - A C j . n ; a c j . i d : « h o l d ; e n d ;

e n d ;

202

J i « MCK] ;I F J - K > 0 THEN

BEGINKI ! - K - N!FOR I l « l TO N no

BEGINK I ! = KI + N(h o l b := h C K . i i ! j i k i - K + j j ACK,n -ACJ,ia; f t C J , I D : « HOLD!e n h ;

e n d ;GOTO 100}ENH;

n o : e n o ;

b e g i n< Form t h e • a t m : ( s e e c u b i c s p l i n e c a l c u l a t i o n s ) > f o r c o u n t l } “ 1 t o n p i v o t s - 2 do

b e g i nf o r c o u n t s : = 1 t o n p i v o t s - 2 do

beg i ni f c o u n t l « c o u n t 2 t h e n

m a t r i x C c o u n t l » c o un t 2 1 ! ■ 4 e l s e i f ( c o u n t l “ c o u n t 2 - 1 ) o r ( c o u n t l ■ c o u n t 2 + 1 ) t h e n

• a t r i x C c o u n t l , c o u n t 2 1 :■= 1 e l s e m a t r i x C c o u n t l i C O u n t 2 D } “ 0 e n d }

< c l e a r a l l u nne ede d l o c a t i o n s > f o r c o u n t 2 i » n p i v o t s - 1 t o m a x p i v o t s do

m a t r i x C c o u n t l » c o u n t 2 1 !*= 0 e n d }

f o r c o u n t l ! « n p i v o t s - 1 t o m a x p i v o t s do f o r c o u n t 2 1 t o m a x p i v o t s do

m a t r i x C c o u n t l , c o u n t S l :«= 0 } l n v m a t r i x ( m a t r i x ) { t a K e i n v e r s e o f m a t r i x >

e n d }

p r o c e d u r e f o r e > _ v e c t o r } v a r

c o u n t : i n t e g e r ! d 2 p n t r : p o i n t e r t y p e }

begin{ s t a r t w i t h f i r s t t r a c k > d S p n t r := d p n t r }v e c t o r C l l }•= 3 * ( d 2 p n t r A • r i g h t A . r i g h t A . v a l u e - d 2 p n t r A . v a l u e ) }

f o r c o u n t 2 t o n p i v o t s - 1 do b e g i n

d S p n t r *= d 2 p n t r A . r i g h t !v e c t o r C c o u n t l := 3 * ( d 2 p n t r A . r i g h t A . r i g h t A. v a l u e - d 2 p n t r A . v a l u e ) }

e n d !e n d }

203

p r o c e d u r e c o l c _ a l l _ d a t a ! wq r

b e e s v e c _ t y p e ! t a n p n t r : i n t e g e r ! d a t a p n t r ! p o i n t e r t y p e ! c o u n t r f i l e _ n u » i r i ! i n t e g e r ! v o l ! r e a l !

p r o c e d u r e s t o r e _ v a l ! beg in

i f v a l _ n u m b e r > 3 t h e n v a l ! » ( v a 1* p i ) / 1 8 0 ! { f i r s t t h r e e v a l u e s a r ec e n t e r o f i n t e r e s t n o t a n g l e s

a s s i g n ( o u t » f i l e s o u t C c o u n t l ) ia p p e n d ( o u t ) ! < open f i l e and s a v e d a t a t o add more >

< w r i t e o n l y 16 v a l u e s on a l i n e >i i f t r u n c ( v a l _ n u m b e r / 1 8 > * 1 8 « v a l _ n u m b e r t h e n

w r i t e l n ( o u t , v a l ! 6 ! 2 > e l s e w r i t e C o u t r v a l ! 6 ! 2 ) ! c l o s e ( o u t )

< t w i l l s t o r e v a l u e s i n f i l e s * > e n d !

p r o c e d u r e c a l c _ s t e p s ( b ! v e c _ t y p e j s t e p s ! i n t e g e r ! p i v o t s ! i n t e g e r ) !

v a rnumber ! r e a l ! s t e p p e r ! i n t e g e r !

beg i nnumber !■ 0 ! s t e p p e r !■ 0 !

< c a l c u l a t e s n s t e p s v a l u e s b e t w e e n e a c h t wo v a l u e s o f d a t a > w h i l e ( s t e p p e r <> n s t e p s ) do

b e g i ns t e p p e r I ■ s t e p p e r + 1 !v a l ! • b t * 3 + n u m b e r * ( b C 3 3 + n u m b e r * ( b t 2 3 + n u m b e r S b l 1 3 ) ) ! s t o r e _ v a l t < s t o r e v a l u e i n a p p r o p r i a t e f i l e > c o u n t ! * c o u n t + 1 ! nu mb er !■ number + l / ( n s t e p s >

ende n d !

beg i nf o r c o u n t != 1 t o m a ; :p i v o t s do 

v e c 4 C c o u n t 3 ! “ 0 ! t a n p n t r ! “ 1 ! d a t a p n t r != d p n t r ! c o u n t != 1!r e p e a t < c a l c c c u b i c f u n c t i o n f o r ea c h t w o p o i n t s an a row i n one

t r a c k > v e c 4 C 1 3 ! « d a t a p n t r A . v a l u e ! v e c 4 C 2 3 ! ■> d a t a p n t r A. r i g h t A . v a l u e ! d a t a p n t r i « d a t a p n t r A . r i g h t ! v e c 4 C 3 3 t « t a n g e n t C t a n p n t r l ! v e c 4 C 4 3 != t a n g e n t C t a n p n t r + 1 3 !

204

t o n p n t r { • t o n p n t r + l ie u l t i p l y ( 4 , f o u r x 4 . v e c 4 , b e e s ) } < m u l t i p l y v e c 4 by f our v.4 t o g e t

c o n s t a n t s f o r c u b i c f u n c t i o n >c a l c _ s t e p s ( b e c s , n s t e p s , n p W n t t )

u n t i l ( d a t a p n t r A. r i g h t ■ n i l ) o r ( t o n p n t r » n p i v o t s ) } v a l !■ d a t a p n t r A . v a l u e }s t o r e _ v a l < s t o r e l a s t v a l u e i n f i l e >

e n d !

p r o c e d u r e p r o c e s s }

va rl o op ! i n t e g e r } t r e k } i n t e g e r }

beg inv a l _ n u » b * r 1} < t o e a k e s u r e o n l y 16 v a l u e s p e r l i n e >t r e k } • 1} < t e l l s wh i c h t r a c k o f d a t a p o i n t s b e i n g c a l c u l a t e d >f o r m _ 4 _ x _ 4 } < f o r e e a t r i x o n l y o n ce >f o r m _ m a t r i x ( <» w i l l fo rk , and t a k e i n v e r s e f )w r i t e l n } w r i t e l n < ' c a l c u l a t i n g t r a c k number ' ) }

< l o o p t o c a l c u l a t e a l l d a t a o n ce i t ha s been e n t e r e d o r r e a d i n > f o r l o o p 1" 1 t o ( n t r a c k s ) do

beg i nw n t e ( t r c k l 4 ) }f o r m _ v e c t o r } < c a l c u l a t e s d a t a f r o m p r e s e n t t r a c k >e u l t i p l y < n p i v o t s - 2 , e a t r i x , v e c t o r , t a n g e n t ) } < c a l c t a n g e n t v e c t o r >

c a l c _ a l l _ d a t a }d p n t r }» d p n t r A .down} <* n e x t t r a c k d a t a s e t *> t r e k ! * t r e k + 1} v a l _ n u m b e r !■ v a l _ n u m b e r + 1

ende n d }

beg inC l r s c r } g e t _ i n f o }w r i t e l n } w r i t e l n ( ' » c r e a t i n g f i l e s * ' ) } c r e a t e _ f l l e s }w n t e l n } w r i t e l n ( ' • • p r o c e s s i n g d a t a , p l e a s e w a i t * * ' ) } p r o c e s s }w r i t e l n } w r i t e l n } w r i t e l n ( ' * * * d one * * * ' )

e n d .

205

A.4 Help Documents fo r Using the Graphics Package

Several help documents fo r using the graphics package are l is te d in Tables 7.1 to 7 .3 . Tables 7.1 and 7.2 show a l i s t i n g of the program and the in te ra c t iv e dialogue fo r p lo t t ing a sphere and a cy l inder.

Table 7.3 i l lu s t r a t e s the use of the animation routines to generate a set of intermediate postures of the human-like model.

206

Table 7 .1 . Help Document fo r P lo tt ing a Sphere Using the

Graphics Package.

\ g r o ) r « M » r I P i e t e r L o n g e r >< d e l e I 2 1 - J a n - i A >< f u n c t i o n I d e e e n e t r e t i o n g r e g r a t f o r s l o t t i n g o f e e p h e r e >

< t h e f o l l o w i n g c o e p l l e r d i r e c t i v e l i n k * t h e p r o g r o * p e c k o g e >< O R h f H M O P . P A B t o t h o d e e o n e t r o l l o n p r o g r e o >

< • ! O A A P H h OD . P A B >

v o r r u d i u a l r e o l l n d | v h o r i s , n d l v v e r t d i v 2 ! i n t e g e r I o u l f i l e l e l r B i e > < t e r t e t o n l » l r 4 | i n p u t f H e l e l r l 4 |

b e g i n < e o t n p r o g r o * b o d y >

I n i t F e r e p e c l l v e |

r o d i u t t * 2 0 0 i u ' j t f 1 ) * 1 • ' e p h ' i w r i t e l n IM r j t f l n C ' S u b r o u t i n e f ' # * o _ e o h ' Mw r i t e l n < ' ' > jw r ) t a ( ' n d I v h o r I : o n t o 1 • T ’ ) | r e - i d I n ( nd I v h o r ] r ) «w r i l e i ' n d i v v e r l l c o l d l v b y 2 • T ' > I r o o d 1 n ( nd i w e r I d i v D i

d r l v e _ w l d l » ’ c l ' l d r i v e _ p l l l * ' d ! ' I O u l f i l e t « ' * p h ' | e x l e r > « l o n : > ' . w l d ' l

< y e n e i a l e a w o r l d f i l e w h i c h c o n l o i n e a i r h e r e i n p o l y g o n >< H a t f o r a . ' i t )

g e n * p h * r e < n d l v h o r l f , n d i v v e r l d l v ? , r o d l u a . d r i v * _ w l d . o u t f l l e .e x t e n t J r n , o u t >1

< o p e n i n p u t c h o n n e i f o r f u r t h e r p r o c e t t i n g o f t h e w o r l d f i l e o f t h e )< t d h a r a )i n p u t f i l * l * c o n c ' ) t < d r i v e _ w l d » o u l f l l e « e x l * n t l o n > |

o » * i g n ( l n p , i n p u l f i l e > l r * t e l < l n o ) {

< o p e n o u o u t p u t f i l e w h i c h c o n t i l n * t h e f i n u l d l t p l ' i r c o d e >< f o t t h e t o h e r e w i t h n i a t ' S t G M l . f L T > U p e n S w g a . e n t ( 1 ) 1I n 1 1 Cm u p h 1 c t I 1 l n i t i i l i : t t h e g r a p h i c s o u t p u t d e v i c e >C l i p F o l r i n 3 l ' l C 1 o t e S e g i t en t ) c l o a e ( i n p > l

e n d .

D > l * EMD. Sf HS u b r o u t i n e I n l U e r t p e c t i v e

v i e w i n g o n g l e t d e q . ) e y e p u t i t i o n I r o d i u t

i n p u t o f ( t h e t i . p h i ) o r( t o p . * l d e « f r o n t v i e w ) [ T . 0 3 • T V

F r o n t . T o p . o r S i d e v i e w ( F . T . S 3 ■ T F 1 n l t . o f o t h e t p e r t p . p q r -i * e t < C Y / N 3 T > o w p v c t r a t i o t G T : 1 . 0 I F C S 1 . 2 4 3 » T 1 . 2 4c e n t e r o f i n t e r e t t I K - c o « . p . , T 0c e n t e r o f l n t e i e t t 1 y - c o « p . “ T 0c e n t e r o f i n t e r e t t I i - c o » p . * ’ 0l i l t t i r r p i r n * * ' iy o r t * p I ' ) n » » ’ 1 0 0 0p l o t o n G t . o r f c . o r w r i t e t o F i l e C G / F / F 3 T F

f r o n t o r b u c K f u c e i f / h / A S t BF 4 M 3 F

Su b t ou t l n y l>ee>o_ t p h

nj i vhor i i ont i 1 • * 10n d i w e r t l e a l d i v b r 2 ■ T 10

- t 30* y *oo

207

Table 7 .2 . Help Document fo r P lo tt in g a Cylinder Using the

Graphics Package.

.p rug rumme r ■CcJate < f u n t 1 ion t

di f l e r longtr >2 0 - j a n - 8 6 >to plo t a g en e ra l c y l i n d e r wh ich has an >e l l i p s e a* a g r o u n d p l a n e >

< the f o l l o w i n g c o m p i l e r d i r e c t i v e links the p r o g r a m p a c k a g e >< GRAF'HMOti• F'AS to the d e m o n s t a r t i o n p r o g r a m >



vor o t o p * a b o t t o n i * b t o p * b b o t t o m , h e i a h t J r e a ] {nd i w e r t i c a l » n d i v h o r i r o n t a l * i * j I i n t e g e r i

o u t f i l e { s t r 8 » e n t e n s i o n i s t r 4 { i n p u t f 1 1 e : s t r 14 ?

b e g i n < main p r o g r a m body >

I m t Pe rs p e c t i v e *

a t o p J e 100* a b o t t o m 1=400* b t o p { = S 0 { b b o t t o m ! = 1 5 0 { h e i Q h t { = 5 0 0 *

d n v e , w l d ! « ' c ! ' * d n v e _ p l t J = 'dJ ' * e;:tension! = ' . w 1 d ' {outf lleJ- c y 1 ' *

w r i t e l n *w r i t e l n ( ' S u b r o u t i n e I«emo_cyl ')*w r i t e l n ( ' ------------------------------------------ ' ) {

w n t e ( ' n d i v h o r i z o n t a 1 = 7 ' ) * read 1 n (nd i vho r l r on ta 1 > Iwrite ( n d i w e r t i c a l - 7 ' ) « r e a d l n < n d i w e r t i c o l W

i g e n e r a t e a wo rld fi le wh ich c o n t a i n s a sp he r e in p o l y g o n )< l i s t f o r m a t >

y e n c y l i n d e r ( a t o p * a b o t t o m , b t o p * b b o t t o m * h e i g h t * n d i v h o r i z o n t a l » n d i w e r t i c a l » d r i v e _ w l d * o u If ile*e:: tension)*

t open input channe l for fu r t h e r p r o c e s s i n g > inputf i l e * s c o n c a t ( d n v e _ w l d f O u t f ile*e;:tensiori) * a s s i g n ( i n p * i n p u t f 1 1 e j * r e s et O p e n S e g m e n t ( 1 > {i n i t u i a p h i c s { C l ipF o l y i n 3 [ i {C l o s e S e y m e n t *c i o s e ( i n p ) * < c l o s e i n p u t c h a n n e l >

end .Sub r o u t i n e I n i t F e r s p e c t i v e

v i e w i n g a n g le < de g .) eye p o s i t i o n : radius

input of (theta*p hi) orttop *s l d e *front view) C T , V 3 * ? 0

Front* Top* or Side v i e w C F * T * S 3 = 7 F init . of oth er persp. p ar a m e t . C Y / N 3 ? i a s p e c t ra tio C G T I 1 . 0 * F C I 1 « 2 4 3 *= ? 1 * 2 4t e n t e r of int er es t I ::-comp. = 7 0

c e n t e r of in te re s t \ y - c o m p * - ? 0t e n t e r of int ere st ♦ z - c o m p . = ? 0hithe r pl ane "" 1y o n - p i a n e = ? 1 0 0 0pl o t on Gt* or Fc* or w r i t e to Fil e CG / F / F D 7 F

t r o n t o r b a c K f u c e £F / R / A i \ - F + R ) J F

S u b r o u t i n e Rem c^c y!

n U i v h o r i z o n t u 1 = 7 citujwer*. ical - 7 5

* 7 40« 7 5 0 0

208

Tabl

e 7.

3.

Help

Do

cum

ent

for

the

Use

of th

e A

nim

atio

n R

outi

nes.

• o«.* r »-

• — —

OOOO OOOO © O O © COO© OOOO OOOO © © © ©

OOOO OOOO OOOO OOOO OOOO OOOO OOOO

OOOO O O M o OOOO OOOO o © «• o © o -

OOOO

o o o ©

OOOO OOOO OOOO O O ^ O OOOO OOOO OOOO OOOO*

OOOO OOOO OOOO OOOO OOOO OOOO OOOO OOOO

OOOO OOOO OOOO OOOO OOOO OOOO OOOO OOOO

© © -» © OOOO

OOOO OOOO OOOO OOOO OOOO OOOO OOOO © o o o

OOOO OOOO OOOO OOOO OOOO OOOO OOOO OOOOo n

OOOO OOOO OI I OO o O- o rj o

OOOO

OOOO

OOOO

OOOOOOOO

o© o © M

OOOOOOOO

o o o oOOOO

o o o oo o o o

o o o oo o o o

n o ri o —

o o o oo o o o

o o o oo o o o

o o o o

a o e* • • • • c

t- © « —o • —— — oo a t. a nw r a — rs

o o o o

o o o o

0 o o o«n

1 o o o o o o o

o o o o

o o o o

o o o o

0 o o oin

1 o o o o o o o

o o o o

I o o o n• o o o o

mo o * o

o o o o0 o o oklW O1 o ► o ni o o o

rs.♦ O O O O

oo o ► o

o o o o

0 0 0 0

m o» © ©- ©1 io o o on

o o o o

o o o o0 o o oki•w i i1 o « o

inOo o o

r> o. © o O kl

n© © ~ ©

klO

X o o

o- © o

r> n

o o o

o o o o

o o o o

o o o o

in

o o o o o o o

o o o o

- 9

0

N'i

**

of

ou

tpu

t M

l#

tou

t f

11

• . d

ot

3 •

ou

tfll

fr

» L

ON

O.J

UH

F

r> kl

■oo-o

cnoCM

-< %"3 A■? m * o

Appendix B

THE SYMBOLIC GENERATION OF THE ROTATIONAL DYNAMICS OF THREE RIGID LINKS IN THREE-DIMENSIONAL SPACE AND THE

COMPUTATION OF THEIR FIRST INTEGRALS

In Appendix B the two methods of deriving the rotat ional dynamics of an open kinematic chain of r ig id bodies, see section 4 .2 , are i l l u s t r a t e d for a th re e - l in k system in three-dimensional space. I t is shown how the f i r s t in tegrals of the above mentioned systems are derived and computed. All the ana ly t ica l expressions for the rotat ional dynamics and the f i r s t in te g ra ls , with the exception of Appendix B3 and B4 are programmed in the symbolic manipulation language MACSYMA and are evaluated for the case of three (N=3) r ig id l in k s . Therefore, the indiv idual sections of th is appendix contain the corresponding (MACSYMA) program l i s t i n g as well the results which are obtained by executing these programs. The organization of th is appendix is as fol lows.In Appendix B1 the equations of motion of three r ig id l inks are projected onto a subspace, in which rotat ional and t rans la t iona l dynamics are decoupled, see section 4 .2 .2 . In Appendix B2 the rota t ional dynamics of the same system are derived from an interconnection diagram, see section 4 .2 .3 . In Appendix B3 a general expression for the rotat ional dynamics in Hamiltonian form is derived.In Appendix B4 a general expression is derived fro the angular momentum of N a r b i t r a r y , r ig id bodies, which is the f i r s t in tegra l of the above mentioned mechanical systems. In Appendix B5 a general expression is derived from an open chain composed of N r i g i d , holonomically constrained bodies.

B . l . The Symbolic Project ion of the Newton-Euler Equations of Motion Using MACSYMA

This appendix refers to the main section 4 .2 .2 . Consider Table 8 .1 , which shows the elements of Eq. (4 .1 ) fo r a system of N, holonomic, r ig id l in k s , which are interconnected in an open chain. Table 8.2 depicts the elements of the decoupling transformation, which are specif ied by Eq. ( 4 . 2 ) . I t is pointed out that the matrices F, G of Table 8 .3 are a function of N, the number of l inks of the composite system.

210

Equations (4 .1 ) and (4 .2 ) are programmed for the case of three (N = 3) l inks using MACSYMA. The corresponding program l i s t i n g is shown IN TABLE B.3. This program was executed on a DEC20 computer at The Ohio State Universi ty . The resu l ts , which consist of a l i s t i n g of the unconstrained and projected equations of motion for three l inks inthree-dimensional space are shown in Tables 8.4 and 8 .5 . The programl i s t i n g can easi ly be modified to mechanical systems with more than three l inks in three-dimensional space.

Table 8 .1 . Elements of the Coupled Newton Euler Equations of Motion (Eq. 4 .1 ) fo r N Rigid, Holonomic Bodies

Uj = block diag(mj E, m2E, . . . , mn E, J j , J 2 , . . . » Jn) '

[3n x 3n] with m-j ( i = 1, . . . , n): mass of body i

E: [3 x 3] unity matrix

Ji ( i = 1, . . . , n): [3 x 3] diagonal in e r t ia matrix ofbody i

U2 =

E 0 0-E E 0

0 -E . . • •

• n ••

•• • E

0 • -E

-LL i AjT 0 0

KK2A2t - l l2a2t ••0 kk3a3t

•

• 0•• • 0•

•-LUn-lAf,

_ 0 0 KKn ATn

: [3n x 3 ( n - l ) J

-migR3

-mngR3

^n(wn)

with R3 = (0 , 0 , 1 )T ; f i (wi ) -WWiJiWi

211

Table 8 .1 . (cont'd )

0 0 . . . 00

0 0 0

AlT 0 n

A2 - a2t •

0 a3•

• 0 •

•• • Ot0 •

• -An0

An

212

Table 8 .2 . Elements of the Decoupling Transformation, Eq. (4 .2 )

— — —

XI E F11 f 12 • • • Fnl•••

E•

f 21•

^n

••E Fnl Fnn

wi 0 1 0 0••

•• 0 i :

0wn 0 0 . . . 0 1

— —

G1

Ao

0

with E: [3 x 3] Id en t i ty matrix

Fj ^j = f" l , l (N ) -*■ FDI AG i n : [3 x 33 matrix

FI , J = FI , j ( N) FL0DIAGi >j > n : [3 x 3] matrixI>J I > J

FI , J = FI , j ( N) FUPDIAGI,J,N: [3 x 3] matrix

I<J I<J

Gj = Gj( N) ■* G ^ n I = 1» • • • » N: C3 x 1] matrix

213

Table 8 .2 . (cont 'd)

N I - I■■■■V \

A . LL > A ♦ A , A A > H

I I f A I I / K

A ■ I ♦ > A ■ 1FDI AG ■ ---------------------- . . . — . . . . . . . . . . . . . . . . . .

1 , N AA

VA . ( LL - A K ) > A - A . RA . A

J J J / A J J J

A • J ♦ 1FUPD1AG * - — . . . . . . . . . --- . . . — . . . . . . .

1 I J . N A

J - 1

\A . ( L L - AA ) > A ♦ A . A LL

J J i t A J J Jl l l lA « 1

r i / OD) AG ■ - - - — . . . . . . . . . _________ . . . . . . . . . . . . .1 . J . N A

( A

J ■ 1 ♦ 1

Nllll

< 2 > Vi . ( L - A ) >J J J /

<?»A - A

A JA A )

J J

J ♦ 1

1. NI - 1BASS

\> ( A

/ JBBSS

J s 1

< 2>

J - IBBSS

( L - A ) > A ♦ AJ J / A J

A b i

< 2>A L )

J J

<2>

NBBSS

\L >1 /

BBSS

A

< 2 >A ♦ A

A 1

1 ♦ 1

I - 1BBSS

VA >1 /

BBSS

A s J

214

Table 8 .3 . MACSYMA Program L ist ing for the Symbolic Project ion of a Three-Link System in Three-Dimensional Space

pr l ntr llt tnne».pujj.syr.");

» h o » t i r e : t r u e ; o y n a r a l i o c r t r o i ;

/ •/ */ •/*bloc* (

J i : 1 1 1 J , a 1 I d i ) J , 11:111), 1) 1 :1 i 1j j k :: k 1 1 j , k t-1: ►. > 11 j * 1 : n 1 l s ,

sirrplic for u 3 ■

t r o j e c t i o n ol U n i s y s i e f

r a u o t l o n s of not io n

of line tc nlo ri «nn "iiti letsoc t . l V B O

*/ • / • /»/

3 2 : J 12J , 33:3)3),a 2 : a I 2 J , o 1 j) ,l.:i 121 , 13:1 111 ,l l / : l 1(21. 113:iltji ,►.2 :x I 2 1 , x 3 : x I 3 J ,X x 2 : x * I 2 J , X X 3 : x x I 3 1 ,r 2 : n 111 , it 3 i t i 31 ,

o f t ld i ei ij l 331, nor, scalar),oec U r t i u 1 , j ,r ! , * • 1 , j ) , n o n s c « ) e r ), a c c i a r e ( l l l l , 1 1 2 ,x*.2 , x x ) , t i * l , l 2 » 2 , f j » 3 , dl , a 2 fd 3 , a l t fa 2 t , a 3 t)

.nonsc ala r 1 ,oe c 1 or e ( In ., r i , n i , b 3 , g 1 , s ce 1 a r ) ); / * pi.q ot ei ocs * /

-■»suin:tun.(r I x ) , X , 1, 3 1 ,

/ • d ? l l n t s l n ^ l l l l c d t l o n r u l t s » /t)l OCR (

a e f r u l e ( r u l e l , t r « n s p o s e ( l l l ) , - l l l ) , d e t r u l e ! i u l e 2 , l r a n s p o s r t i ) 2 ) , - 1 1 2 ) , d t i r u i e t r » . ) e 3 , i r a r , s p o s e ( x x 2 ) , - K X x ) , d t l r u i t t r c l e - l . t r a n s p o s e ! x i 3 ) , - x x 3 ) , o e i r J l e t r u l e O . t r a n s p o s e t a l 1, a l * ' ! - 1 11, d P t r j l e t r d l P p , t r a n s p o s e l a 2 ) , a 2 * * ( - l ) l ,1 e t r u l e t r d l e 7 , t r a n s p o s e ( a 3 ) , a 3 " * t - l ) ) i ; 2 * enj o l o l oc i . * /%

/• Plpr.pnts cl p r o j e c t i o n n.atrlces 00 ano Uo »/1 11 , 3 , n J : =

11 1 = 3 m e n ! 1 / n s of • r a t s 1 op t a 11 J . ks 11 1 »s jn. t n. I x 1 , x , 1 , 1 - 1 ) *a l i j . l l l U ' s o n U l « i , 2 , 1 0 , r ) ) 1

toftI l f l < 3 t r . m 1 / r s urr » r a t s ln-r ( - a I ) J . x x [ J ) . n I J i <a I 3 1 . ( - x x I 3 1 •* 11 I 31 ) <

s u is ( if ( x 1 , • , 3 ♦ 1 . ri 11 t L f t . - l / F s u n » r a t s ) r o l d l 3 ] . l - R K l 3 J 3 l i l 3 J ) * s u n ( . F l » ; j , K , l , 3 - l i

< a I 3 1 . 1 1 I ) 1 • n 13 11 1 ;

gll.nJ :=nlocx ! lexpr 1 , e x o r 2 , e x p r 3 1 ,e x p r l i 3 i: = l/ rsunl» r a t s l n p ( c l 3 ) . k » l 3 J * * 2 . lll3i-xlJJ)*fcuiFlnlxj,x,l,J'

a i 3 1 . I J )**/ . 1 ( J J »n l 3 J i ,e « i r l U , r . j : = - i / i . s L M i o t s l n H o l i J . m l i l “ 2 . x 11 ) * s u m ! f. I x 1 , x , 1 , 1 - 1 ) 3

all!.fc»lj)**2 . ll) )*SlHtrlXJ,x,l-tl,n)l, e x i r 3 i3 ,ni:=-i/rsjp»ratslr„ri-rtl 3 J . » » l j l " * 2 . x I 3 1 *n l j ) « a ( 3 J . * a l 3 J “ *i

t - x l ) ) « l l 3 J ) » S u f M n U J . x , j « l , n ) ) , ratsii ;lsu.ilexprlt 3 J , 3 , l . i - l ) » e x p r 2 li,nJ«sun,(expr 3 l3 »f'l,3 ,l*l,nll);

/* lrvor.e riles tor scalar pr o d u c t s »/ootscr dies : tr ie; not us If p : t r o e ; dot l s i n,p; ir oe ;t l O C x (

u 1 : i. a l r i x ( 11 l . I , C , U , 0 , C , 0 ] , I u , r 2 . 1 ,C,u,o,oj,!!/, 0 ,i. 3 . 1 ,0 ,0 ,0'J, to , o , 0 , 3 1 , ( ' , li 1 , I u , 0 , o , 0 , 3 2 , o 1 , 10 , o , i , 0 , u , 3 31 1 ,

o 2 : F. d ir i xt l- l,o j,l i,- ll, lu, li, !-i ll. tro ris ,o se! dll ,ui , u > . i . t r d n i r c s e ( m ) , - ) 1 2 .lranti,oseidiil,I u , x i. 3 . l r a n s p o s e ( d 3 J J ) ,

1 )♦

215

Table 8 .3 . (cont . )

u J : "at r i x ( I -» 1 .Q.rJJ , I-x 2.v.r 3) , i - m j .g .r 3 J , If i • 1J , I1 2 » 2 J , If 3»3 J ) , i i t M a u i m I o . o j , 10,u) , I u , 0 J , I-t ren sp o se l al ), 0 J ,

I t r a n s p o s e l a 2 ) , - t r a n s p o s e i a 2 ) J , l u . t r a n s p o s e l a i l ) ), u5:-nalrixill,ilj,l,.}J,til,2,3],rll,.>,3Jj,

Il , f l 2 , l , 3 ) , f l 2 , 2 , 3 j , t t 2 , 3 , 3 j j , ( l , f l l , l , 3 ) , l l 3 , 2 , j j , f l j , j , j j j ,I 0 , > , U , U J , I ( , 0 , 1 , 0 J , I 0 , 0 , 0 , 1 J ) ,

u o : 7i a L r i xl lg l l, 3 J ] , l q i 2 ,3 J J , l u I 3 , J) ] ,i o J , [ u J , l o ) jJ; 2* end of d I o c k */

2 » start »y2 » o la _ n : « of dy nam ic equ. re to re t r a n s f o r m a t i o n */2» ne»_r. : • ol Dy na m ic eou. after t r a n s f o r m a t i o n *2/ * ini _ r. : t of incut toi cu e v ec tor s Ctor 3 links ,inp _n= 2)* 22 * c o n s t r _ n : • of c o n s t r a i n t f orc e ve ct o r s */OlOCKI

oio_n:t>, ne» _n: 4, lnp_n:2 , co n st r_n :2,

u b t i i , 3 J : = r a t s i m p ( t r a n s D o s e C u b l j , i J J ) , ubt :dermati lx I ubt ,ne»_n,oiri_n , 1 , 1 ) ,ubt:a( p i y i l u b l , r u l c l , r u l e 2 , r u l e d , r u l - H , r u l e b , r u l e b , r u l e 7 ) , u b t d i u . j j : -I a t s lr [ ( sun C ob t [l,k] . u 1 Ik, } J ,k,1 , o i d . n );,

2 » v 1 : = u b t . u I . u b «/

vll i, i ): = raisimr(soi>i(ubtulll,r j.u oin ,iJ ,m, l,o la_ nJ ), vl : je n na tr ix( vl, r.e «_n ,nc »_n ),v l f o o m y l l v i . r u l e l , rule 2, ruieJ, rule-l, ruleb,ruiet,,rule7),v 1 : ex, ai c t y l ), v l i r a t s i . p l v l j ,

2 * v 2 := -ubt . ul . ub «/

v 2 i i , j j : = - r a t s i r p ( s u m ( u b t u l i r , K j . a t I k , 3 ) , K , 1 , n e « . n ) ) ,vi : jpnii a l r 1 x l v2 , ne*_ri, 1,1 . 1 J .v 2 : a„iiyl I v 2 ,r u l e 1,rule 2 , r u l e d , r u l e 4 , r u i e b , r u l e o , r u l e 7 ),v x f r a l s i m p i v x ) , v 2 : exf and I v 2 j , vx : rat si.i pt v 2 ),

/» vj := ubt . u 2 = 0 iff: * 2

v j I i , 3 J: = ral s im p Is u t' l ub t (l , Kj , u 2’l K , 3 J,r. ,l,olo-nji, v 3 : a e n m a t r i x ( v j , n e » _ n , c o n s t l _ N , 1,1),v 3 : a n p i y l i v j , r u l e ) , r u l e 2 , r u i e a , r u i e v , r u 2 fcb,iuxeo,ruie 7 ),V J : c x p a n j I v )), v3:rriisucstl:i,nl + rr2-tirj,v3), v j :raia i u p i v a j ,

2 * v 4 ubt . u 3 ♦/

v41 i,3 J: = ra t si n pI s ur i lu b tl l ,K j . u 3 l K , ] J , K , l , o l o _ n 3 J , vi: Jeniiiti i A( v«, nek .n, 1,1,1),va i ar i l y l i v 4 , r u i e J , r u l e 2 , r u l e 3 , r j J e 4 , r o i e b , r u i e b , r u l e 7 ) .V ‘, : ra t s l ,i M v 4 1 ,v 4 :raisuost(n,iii-*n.2 'tii3 ,v 4 ),v 4 : r a t s i r p u v y i ,v «: e xp a n c t v 4 j ,v4:rats i.i .KV kj, v4: rat suP stl n,x l»m x«. ,'3,vvl,

216


✓* vb := ubt . u*

vb ii, lj: = rsts)np(su'T( ubt li. K). u4l K,j ),K , vb: jt[,:o!H*;vb,nc«.r, ,1 np_n , 1 , 1 ) ); /• er>

/• iratricrs of or igi nal systen

dlsol?yiul ,ui ,u j,u<,ub,ut>) ;

/ • r e t ri c es ol t r a n s t o r r e c to tco jpleo)

d l s p l a y ( v ) , v 2 ) ; o l s o l v 3 ,v 4 ,vb> ;

c l o * e t l l e ( “n e » e ul J .t x t" ) ;

1 , o l o . n ) ) ,n o f o J o c k *

s y s t e n

Table 8 .4 . The Original Equations of Motion and the Decoupling Transform for 3 l inks in 3-D.

• r u • a c s y m e

t h i s 1 S * t L M 30 Si c ) L c u v r l y r . t l * 7 u , * a s s a c r »-s h 11 s I n s t i t u t e o i I e c n r . o l o y y

&« o a o J h - j f i x f i l e < » A C £ i * > 7 C P b 2 u . M X . 3 0 , i

I C 1 ) t d t d . l o a o ( " n t : * « e u i 3 . s y ' ' " ) ;W a t e r i n g t n e t i U * n e * " ^ . e u 1 j . s y n *•W a t c h i n g o o r e .1 J me- lboc/Jo f S P C.(Li) •nefc v.eulJ.svn''IC 2 J olSf-lc>(ul,u2,u3,u4,ub,ut>);

M 0 0 0

02

u) =

0 0 0

0 0

0 J 0 01

U 0 J

0 0 J

- 1

10

LL • 1 n A/, b P t I A ) 1 1

3 0

- 1 1u

► f • 7 * A i . SPuSt ( m J - LL • r*Mf»,Sh LSfc ( A ) 2 2 2 2

U h f • i r \Ai»bPLL L I A )3 3

U3 =

- » \ j h 31

• P* u t J2

• fc 0 hij

218

Table 8 .4 . (cont'd)

h 1 r 1 Y 2 * 2

F S* S

lU

(•00

- I h A K f - P u S L l A ) 1

ThANSPLSF( A ) - l H A N S P u J t C * )2 2

Jl'Ar»v>Pubt.(A ) J

U «• N ) (A . LI ) * IA . I L L - Mv ) ) • K (Aj 2 1 1 * 2 2 2 2 2

Ub - U I H a I U ,

1 1H I A . hr ) L (A . LL ) K I A , LL ) f h (A • KK )J 3% J 1 1 1 J 2 2 1 2 2

---------------------------- j # U , ------------------------------- , (

j / l 3 * ' 1 j / 1

K (A . I t ) H (A . LL )3 1 3 1 1 1

........... ..... J , 11 , ------------------ ,► l < t ►' * ♦ h ■* f3 2 1 3 2 1

« i A . ( LL - KK ) ) i X (A . LL ) C M < H ) I A . h K )1 2 2 2 2 2 2 2 1 3 3

- , ] , 10. 1

I L , C, , 1, Uj , lu, 0, 0, 11)

. K* )2

, 0, 0).

219

Table 8 . 4 . (con'd)

<2> </>Uo * A A l h l A l l C (A . • » . A ) - *> (A . »« . ( b - A ) )

3 3 3 i 3 2 / 2 2

< 7 > <2>* ** l* • »* . A ) 4 <- K - A ) (A . ■» . L ) ) / l * 4 M 4 A )],

2 2 2 2 3 2 1 1 1 3 2 1

<2> < 2 > ll* I" . »» . A ) - K IA . n . b ) - A (A . fc« . A )

* 3 J 3 3 2 / 2 1 2 2 /

< 2 >4 M (A . *■•> . L ) ) / ( * 4 A 4 “ )),

1 1 1 1 3 / 1

</> <2>[ - ( ( A 4 A ) (/ . . Hfc . r > - M CA . mm . l b - A ) )

2 1 3 3 3 1 2 2 / 2

< 2 > <2>- * (A • »* . L ) • A i & . »* . L ) ) / I A 4 A 4 A )), 103, 101,

2 2 2 2 1 1 1 1 3 / 110) )

220

Table 8 .5 . The Projected Equations of Motion for 3 Links in 3D.

IC3) cist 1 * y ( v 1 ,v / ,» J |V 4,v b.v nl;V I ■ > A I x 1 A ( I *• ♦ i * 3 " , 0 . u , Oi , l u ,

3 2 I

<23I X x < > X ) LL - J X - U “ * x J

1 3 1 2 1 1 3 1 2 1 1

x < x. < X3 2 1

< - 1 >- ( X x ( I L . A . A . L L ) 3 ( - X H - K X i

1 3 1 1 2 2 1 3 1 2

<- I 3x X ( L L . A . A . A A )

< - 1 3 1 J 1 1 3 3( L I , . A . A . A K 1 ) / ( X 3 X « x ) , ----------------------------------------------------------------------------------------------------------- ) ,

1 ) 2 2 3 4 1 X 4 X 4 X3 2 1

< - ) >l u , - ( - > I L L . X . A . L L 1 3 I - X X - X x )

1 3 2 2 1 1 1 3 1 2

<- 1 >( A X . A . A L I , 1 J / I X 3 ► 3 H J ,

2 1 1 3 2 1

< 2 >( X X ( L L . A X J <" 1 >( X • , . ) » ( l L . A . a • L K ) - I X ( AA • A . A . K A )

2 1 3 / 2 3 3 1 3 2 2 3 3- - - - - - - - - - - - - - - - - - - - - - - - - - - J ,

f l 3 X 3 X

3 2 1

< - 1 >X x ( AA . A . A . LL )1 3 3 3 1 1

10, ,X 3 M 3 X3 2 1

<- 13 < - 13( A 3 X 1 X ( X A . A . A . l L 1 - I x ( A A . A . A . A a )

2 1 3 3 3 2 2 1 3 3 3 2 2

4 «♦ ►3 2 1

< 2 3l x 3 X ) X A X 3 ( - 3 - X • M ) J

2 1 . 3 3 3 2 1 3 ------------------- J J

r X X 3 X3 2 1

221

Table 8 . S. (cont'd)

I 0 0 J

( 0 0 )J

( u 0 J I iI V u J

• G M k 3

f 1 * 1

► 2 A I

0- I r x A N & t ' L S L l A )

11KAh£>PUbLlA )

2

0

u0

• 1 K Ar»£>HJ£fc. I A ) 2

1 H A h a K j S t ( A )J

222


42>V2 s x A1 h 1 a ( I v/J , I - < ( x X ♦ I- x ) ( b L . a a . L 1

1 1 1 2 1 1 1

- X i ( L I . A 1 i 1

♦ X x ( ! . ! . A 1 1 1

- ) > 42>• A . V . . . K )

i j J

- 1> < 2 >• A . x . ■ L 1

2 * 2

4 - 1> <2>1 ( L L • A . A . A. . K ) 1/CX X X 4 X )J,

1 J 1 2 1 1 3 2 1

< 2> v2>t- ((' 4 X. ) X (LL . xW . L 1 - X x ( L L . . . . K )

2 1 3 2 2 2 1 3 2 a 2

<- 1> < 2 >4 (- x • x ) X (LL . A • A • a . . K J

2 1 3 2 2 3 3 3

4- 1> < 2 > <2>4 x x (LL . A . A . . a . L 1 - h X (KK . xx . L )

1 3 2 2 1 1 1 1 J 2 2 2

<2> 4 - 1> <2>4 (X X. 1 x K ) ( K 1 . a a . K ) 4 M M (Aa . A . A . A A . K )

I '• 3 1 2 2 2 2 1 3 4 ^ 3 3 3

<- S> <2>♦ ( - X X - X X ) ( KK . A . A . A . . L J 1 2 ( X 4 X 4 X 1 ) ,

1 3 1 2 2 2 1 1 1 3 2 1

< 2 >I - 1 I * 4 I- ) A ( K K . A A . A ) 4 ( x X - A I M

2 1 3 3 2 1 3

4 - 1> 42>I KK . A . A . XX

3 3 2 2

4 - 1> <2>L ) 4 M A, (KK . A . A . A A

2 1 3 3 3 2 2

4 - 1> 42>- * - (AA . A . A . AA . L ) ) / ( X 4 M 4 M 11)

1 3 3 3 1 1 1 3 2 1

223

Appendix B.2. A General Expression for the Rotational Equations of Motion and the Symbolic Evaluation for the Case~oT Three Links (N = 3)

This appendix refers to the main sections 4 .2 .2 and 4 .2 .3 .Consider Table 8 .6 , which specif ies the elements of of Eq. (4 .3 ) fo r a system of N, holonomic, r ig id bodies, which are interconnected in an open chain. Eq. (4 .3 ) is programmed in MACSYMA for the case of three (N = 3) l inks . A program l i s t i n g is shown in Table 8 .7 . The program was executed on a DEC-20 system at The Ohio State U nivers i ty . The corresponding results are shown in Table 8 .8 . Note that Table 8.5 describes both the t rans la t iona l and the rotat ional dynamics of three l inks in three-dimensional space. However, Table 8 .5 describes both the t rans la t iona l and the rotat ional dynamics of three l inks in three-dimensional space. However, Table 8.8 describes only the rotat ional dynamics of the same system. A comparison of the rotat ional dynamics of the system using the two d i f fe re n t approaches shows that the corresponding entries of the same matrices are equal, when i t is assumed that the mechanical parameters of the three l in k s , which are shown in Table 8 .5 , are specif ied in the in e r t ia l coordinate system, i . e . , the A-j ( i = 1, 2 , 3) in the unity matrices.

224

Table 8 .6 . A General Expression for the Decoupled Equations of Motion for N Rigid, Interconnected Bodies, Eq. (4 .3 )

Vi -

M 0

0 I U

0 m DD2iA2ÂjDDj2• •• •• •

0 m DDnjAnÂ]iDDjp,

mDD 1 2 ^ 1 ^ 2 ^ 2 1

122

. . m DinAÂnDDnj

lnn

I i i - J i i " m DDi^ ~ 1 mj DD^ i jj *1

V2 =

V5

0 -Mg

m I DD-ijAjAjWW 2 ° j l -WWj I n Wj

•••

> v4 =•••

m I OOnjAjAjMH/ Djn "WWn *nn ^n

— —

0 • • • 0

- A i l 0

a2t • • 0

•• a3t

0 ••

-A J Mn-1•

0A T

"

225

Table 8 .7 . MACSYMA Program List ing fo r the Derivation of the

Rotational Dynamics fo r Three Links in Three-Dimen

sional Space from the-Interconnection Diagram.

•noet lae: true; dynaaa] loc: t rue;

• r l t f n i f C u r y c t i . i . u f ) ; p n n t t i i e C o a r y c . s y * " i ;

/ • g e n e r a l r x k m i l o n f o r r o t a t i o n a l e q u a t i o n s o t a o t l o n •/ • I r o n : ana MKUi l i . l t : > * l r i t Pl i tAr t iCAi , A l I l T U D t */« t U O A U L h S 10k AN N - u U u l S A l L L U l l t ’ UA5, 1 S»b b , pp . 1 2 3 - 1 2 b •/ • o n . : 1 1 1 l a i t n o a , j J : v e c t o r s i r o i com l l ar t poa J t o j o i n t j *

/ * n _ l l n « : t o t r i g i d l i n n s *

/ • oe i in l t l ons ot funct ions • /

d d l l l a i coal :a- l / i i sup»(su' i. le(ir.ju)*oll l laBPda,i3U) , a j u . 1 ,n_l lnn )-». 1 latcda) • ol l l la»Dda,lanipaa]);

dd lu l i a i r i od , r j u l : = oal ( la n. odal«ol l 1 lan-oaa,« J a i l

dl l la»Doaj : = -l/o.tun.»iiuo. (plwjul*ol l lai»toa,ni3uJ, i t iu, l ,n_l lnr . ) - r l laapdal» o l l l aoLoa . lac roa l l ;

a lu l laTcoe, t r )u i :£d l ( le*Dda]«ol l la iDoa,r 3 uJ;

• *p l l laTtoa j : * ra is l»Cl j l la i rPoa)-» l laapaa l*aa l l la«Doal**2-su"toinjuJ»LiolulianDoo,m)ul**2,i«3u,l.n_llnn)«B.l lampQa)»dolui latrpoa, laopodl**2);

• ap2llaooaa,i i jul :aralsiB.p(>isu»*aolul ias.Dda,B]uJ.oaiul»3u.laabaaJ};

*2 l lennda , j j : = ratsl»p(:»sur»(*un(odiul lairDda,»3 u l . a . l » j u ) ‘, * 2 .alol... 3u, larnDda),Pi lu, l /n. l lnn)*oalul la»ooa, lar taal . a u i ] e i o d a l " 2 . d lu[ lar tuaa, laiDoa])) ;

va l ien nr.e , ] i : * ra ts l»p( .ae i la i rDaal .v l I lanooa, lanodaj .a l laacoal ) ;

a lsp lav lo l l lanpoaj .d lu l lancaa.njuJ.aolUanDuaJ, oalul lanuda, n j u J , expl l ldi . iLaai,exp2 l l a i roaa , r ju i , v 2 l l a m c ja , j l , a 4 l la iDaa, 3 1 );

f i l l : c l l 1 1 , 1 1 ; 1 1 2 1 :c 1 j 1 2 , 1 1 ; x i 31: c i l i J, l J ;

111 :ol l ) , H r 1 2 1 : o l i 2 , 1 j ;1 J 1: c l i 3 , 1 J ;

/• o e i .

o i l 1 2 , 1 1 :n» l 2 J;

• l :. 11) ;• a 1 : . . 111;j i s j iu ;■ 1 si 111;

111 2 : c 1111,21; \ 1113:o l l11, j ! ; o l l l l . l l : C;1122S0U 12 ,21 ; 1 1 2 3 : 0 1 1 (2,31; o i l i 2 , 2 i : u ;I i 3 x : c l l l 3 , 2 l ; l l 3 3 : o i i 13,31; 01x13,31i V i

112S0111, 21 ; 1 1 3 : 0 1 ( 1 ,31; 0 1 l l , 1 j : u ;1 2 2 : 0 1 1 2 , 2 1 ; 1 2 3 : 0 1 12,31; o l ( 2 , 2 1 :o;1 3 i : o l l 3 ,2 i ; l 3 3 :o i (3 ,3 l ; 0 1 13 ,31: 0 ;

hoGrEb ” -> del . Ot hfcKAe1

o l l ( i , 3 1 : l l l l l ; ol 11,21:1 111; 0111,31:1111o i l (2 ,3 ) :11121; 0 112,11: X 12 j ; 0112,31:1121o i l 1 3,2J:kx 131; o! ( 3 , i i : k (3j ; o l 13,21: k (31

a 2 : a 12 J; a 3 : a 131 ;ea2:ua121; aa 3 S aa131;3 2 : ) 1 21 ; J 3 : J 13 J ;»2 : r l 21; • 3: v 131 ;

*/

•ec iar t ' l i3 i ,32, )3J,ncnscdlarJ ;• • c i a i e u - 1 , a • 1 , 2 , t a i l , nor. scalar) ;a r c l a r e u i l l i , i l i x , 1 1 1 3 , 1121.11x2,1123, 1131,1132,l l J J i .nonscalar 1 ; deciarel 1111,112,113, 121 , 122, 123, 131 , 132,1331 , nonscalar1;• eclarei toi ,K2,*3,<asui2,scaier l ;

• otscruies : t rue; ootbsiap: true; do t i s lap ; t rue ;

226

Table 8 . 7 . (cont 'd )

e v a l u a t i o n o l l o c a l v f c t o i Jn - i l n » : Jj ■ iuirsaualtrlKj , k , l , r,_ 11 nk )

raii i « u(ooI 11)); r a t 41 * piocl I / ) ) ; r » l 4 l r p ( a a l 1 1 ) )r e t s 1 n u t o o l u 1 1 , x ) ) r « t » i » p l o o l L I x , 1 j ) r e t * 1 i n l o o i u [ 3 , 1 J ) r a t a m P i ' i a l i u i . i ) )

rat4ln.p(dcluli,)) );r a t s l n p ( o a i u l x . J i ) ; r a i t l i i p t a o l u D . x ) ) I r a t s l « p l a o l u i 3 , J i ) ;

r a t s l a p l d d l u l l . l ) )

/•/ •

c o n p u t a l i o n o l s y s t e m n t n c t s n a t r i x . v l i * * > u s t . u ) . ub )

v l : g r n " i a t r i x i v i , n _ l i n k ,n _ l i n K , i , l ) ;

tor iainooan t r ru n . l l nx dolo r a.)u:l tnru n . l l n x oo

l l lairDda*n)u men v 1 1 laml da , if 3uJ : exp 1 Iian Dua)c i te v l l laneoe.n ju j iexp i l la fcoaa.n ju j ;

v l : t u l l r a t s ln p t v l );

/ » » a t r l » v . ( e=> - o b t . u l . ub ) * /

v2 :ger».dtr)Mv2 , n_ l i n k , 1 , 1 , 1 ) ; v 2 : t u l i r a t i i k p ( v x ) ;

/ * l a t r n v4 ( «*> ubt . u3 ) • /

v 4 : g r r * a t r i A ( v 4 , i i - l l n k , l , l , l j ; v«: t l 1 l r a ta l ii p i \ 4) ;

/ * display ot lu rc t lons anc oe t im t lons • /

4 ! tp lev l v ), v x ,va );

c lo i f U ln * o a r y c e n t . t a t * ) j

227

Table 8 .8 . The Projected Equations of Motion for 3 Links in 3 D Derived from the Interconnected Diagram.

(CV2) / » Di s p l a y ot (unct ion s ano d e f in it i on s*/dl spj a y( v ) ,v2,vs);

LL A 4 LL A LL A1 3 1 2 < 2 > 1 1 < 2 >

V 1 = "AI HI A II - A (--------------------1 4 (- A - X )-1---------------- ) 4 J ,l a 4 a 4 a 3 2 a 4 a 4 a 1

3 2 1 3 2 1

ILL - AA ) a •< a a A aa a2 2 3 2 2 3 3

- LL a ------- — --------------j tL a _ - — ,l l » 4 a 4 a l 1 a 4 a 4 a

3 2 1 3 2 1

LL a 1 I

l u s t - LL ) a < (a A ) . ---------------- ,2 2 3 2 2 a 4 a, 4 A

3 2 1

ILL - aa ) a - as a LL a 4 a aa2 2 3 2 2 < 2 > 2 3 1 2 <2>

- A ( 1 - a I-------------------- )1 a • * . « ( ■ 2 * 4 A 4 a

3 . 1 3 2 1

LL a . ► LL - a aa2 2 1 2 1 2 < 2 >

- A ( ------------------------------------------------------) 4 J ,J a 4 a 4 A 2

3 2 1

la a 3 3

(Lp a 4 a LL - a aa ) .---------------- 1 ,2 2 1 2 1 2 a 4 a 4 a

3 2 1

LL a o l a 4 a L l - M aa1 1 2 2 1 2 1 2

laa * #-----------------, aa a . ----------------------- ,

3 3 a 4 a a a 3 3 a 4 a 4 a3 2 1 3 2 1

at a t a 4 a ) aa3 3 < 2> 2 1 3 <2>

t- . - J ( 1 - a I ) 4 J J)2 1 a 4 a 4 a 3 a 4 a 4 a 3

3 2 1 3 2 1

228


LL m k n1 1 < 2 3 3 3

* 2 * (’ 411 l A l l l l « “ ♦ « ) C------------------------------- . . --- --------------------------J 2 ) * 4 X. 4 X 3 M 4 X 4 M

j 2 1 3 2 1

LL X I L - K 1 H - K M1 1 < 2 > 2 2 3 2 2

. ( . x . f - X ) ( . . ) ) ,3 2 1 x 4 x 4 x 2 K 4 M 4 M

3 2 1 3 2 1

I LL - KK ) X - KK X L M2 2 3 2 2 < 2 > 1 1

l l - x - x - , j i - - - - - ------------------------------------------------ . _--- ------------ - - - - - - . j

J 2 1 X 4 M 4 H 1 «. 4 M 4 X3 2 1 3 2 1

l L m . m LL “ M k k K M2 2 1 2 1 2 < 2 > 3 3

♦ l » 4 X . x ) ( ---------------------------------------------------------------- . x . . — -------------------------) ) ,

J 2 1 M 4 M 4 M J M 4 X 4 M3 2 1 3 2 1

KK M L M 4 r 4 L “ M K3 3 < 2 2 2 2 1 2 1 2

t ( M 4 M 4 X 1 V . » * . ------------- )J 2 1 X 4 X 4 X 2 M 4 M 4 M

3 2 1 3 2 1

X. K X L M3 3 < 2 > 1 1

4 I K 4 4 4 M ) I . X . . - - - - - - - - - - - - J J )J "• 2 1 X 4 X 4 K 1 M 4 M 4 M

3 2 1 3 2 1X

LL X. 4 LL M LL M1 3 1 2 < 2 > 1 1

V 4 = X A l X l A U > > . I - * i --------------------------------- - - 1 4 I - M - M ) I ------------------------1 J K 4 M 4 X 3 2 X 4 X 4

3 2 1 3 2

I L L ■ K K 1 X - K K X

2 2 3 2 2 < 2 >4 J 1 . « 1 , l » « . I - X - J

1 1 2 1 x 4 X. 4 h3 2 1

L L I 4 K K K LL x. 4 X L L - M KK2 3 1 2 < 2 > 2 2 1 2 1 2 < 2 >

- » i ----------------------- --------------- ) - x. i - - - — - ) 4 J ) .2 A 4 K 4 M 3 . 4 X 4 X 2

3 2 1 3 2 1

X K M I X. 4 M ) K X3 3 < 2 > 2 1 3 < 2 >

I X . . 11- X. - X ) I - - - - - - - - - - - - ) - I - - - - - - - - - - - - - ) 4 J ) .3 / 1 X 4 M 4 X. 3 X 4 . 4 4 3

3 2 1 3 2 1

< 2 >- - )x1

» J .2

x 1 ) 3

229

Appendix B.3. Derivat ion of the Hamiltonian Form of the RotationalEquations of Motion.

In th is appendix the Hamiltonian form of the rotational equations of motion is derived, which is useful in the c o n t r o l l a b i l i t y analysis of the system, Eq. ( 4 . 3 ) , or Eq. ( 4 . 6 ) . The rotat ional equations of motion, Eq. ( 4 . 6 ) , Table 8.7 are given below in a common reference frame, the ICS system.

v^W = V2 + V4 + V5 M

with(B . l )

VI =

Aj I j j ro Aj DDj2 AjÂ2DD2j

m A2 DD21 A2Âj DDj2 A2 122

• •• •m a

m An DDni ApÂ^DDpj ^n *nn

m I AX DD12 AjTaj W W ^D jT -A1WU1I 11W1

v2 = •••

m 1 ^n D^nj AnÂj WWj2 Djnj * l

; v4 =•••

-AnWWn I nn Wn

-E 0 . . . 0

E -E :

v 5 = 0 E

0 -E•

0 0 E

The angular momentum vector P = (Pj

The time der iva t ive of P is

P = V} W + vg + v 4 + vg + v (B.2)

230

v6 = M

I3 * 1

h DDijAÂjDDj-jWj

•= M

Ij*l

Aj WW! DOuAÂjDDjjWj

•

I

••

An DDnj AnÂjDDjnWj I

••

An WWn DDpjAgTAjDDjpWjj*n j*"

V7 = M

l Aj DDu^TAjODjjWjj * l

I An DDnj AnTAjDDjnWj j * n

= M

J Aj MH, DDuAfAjDDjiMj J*1

I An W«n DDnjA nTAjD n jnWj j * n

v8 = M

1 A j n0 1j A 1Ti j D D j 1Wj

1 An DDnj AnÂ jD D jnWj j * n

= M

I h DOijAiTAjWWô^

1 An DDpjApTAjWWj^Djp .i#n

Vg =

A1 WW1 l n W1

An WWn Ipn Wn

Substi tut ing Eq. ( B . l ) in to Eq. (B.2) and re a l iz in g that vj> = - v r ; V4 = -vg gives:

P = vjo + v 5M (B.3)

with

231

v 10 = vg + Vg = M

I AjfWWÔu - DD1JWW1)A1TAjDDj1Wj

1 An(WWn ^ n j " ^ n j WWn)AntAjDDjnWjj * l

Also: 0 = BW = B(v^ P)

with B = block diag(B^, . . . , Bn)

(B.4)

Equations (B.3) and (B.4) are the Hamiltonian sta te equations for the rota t ional equations of motion.

232

Appendix B4. Derivation of the Angular Momentum Expression for n Arb itrary Rigid Bodies.

In the fol lowing section the expression for the angular momentumof n a r b i t r a ry r ig id bodies is derived, which is shown in [34] fo r thecase of a single r ig id body. For the sake of s im p l ic i ty i t is f i r s t assumed that each r ig id body has a discrete mass d is t r ib u t io n . I t is shown that the resu lts , obtained under these re s t r ic t io n s , apply to r ig id bodies with continuous mass d istr ibut ions as w e l l . Consider Figure 8 .1 . which shows n r ig id bodies with t h e i r respective centersof mass C-j(i = 1................. n) in an in e r t i a l frame RCS with basis{Rx , Ry, Rz} . The orig in of frame R coincides with the center of massC of tne compound system. Each r ig id body has a to ta l mass mj( i = 1, . . . , n) and consists of p par t ic les with mass m-jj( i * 1, . . . , nj ; j = 1, . . . , p) s . t . :

m-, m-, j , i - 1, . . . , nj = l

I t has associated with i t a local vector p-j to i t s own center of massc-j r e la t iv e to C. A vector r - j j points from c-j to the p a r t ic le m-jj.From Fig. B . l i t follows that

z i j = Pi + M j (B.5)

Since C-j is the center of mass of body i the fol lowing re lat ionship holds:

0=1

d_dt

mi j H j = 0 ( B* 6 )

P PI mi j r i j = I mi j r i j = 0

J=1 J=1

S im i la r ly , since C is the system's center of mass:

nI mij P i j = 0 (B.7)

i =1 J J

d n na t X mi Pi = X mi Pi = 0

1=1 i= l

The angular momentum of a single body is re la t iv e to C is [2 6 ] :

u ?i = I mi j zz i j z i j

J=1

233

X

Figure 8 .1 . System of n Rigid Bodies with Discrete Mass

D is tr ibut ion in an In e r t i a l Coordinate System which

Originates in the Center of Mass C.

234

where zz^j is the crossproduct operator. The angular momentum of the compound system re la t iv e to C is :

H = I Hi = I I m i j z z i j Zi j (B.R)1=1 1=1 j = l

The time der iva t ive of Eq. (B.5) is :

*1 j = Pi + (B.9)

Insert ing equations (B .5 ) , (B .6 ) , (B.7) and (B.9) in to (B.R) gives:

n P n pH = I I mi j PPi Pi + I I mi j r r i j (B. 10)

1=1 j = l 1=1 j = l

The expression for the angular momentum H consists of two terms. The f i r s t term on the r ight hand side of Eq. (B.10) represents theangular momentum of the points C-j ( i = 1, . . . , n) r e la t iv e to C assumingtha t the to ta l mass of body is is concentrated in C-j [ 2 6 ] . The secondterm in Eq. (B.10) represents the angular momenta of a l l i bodiesr e la t iv e to th e i r center of mass C-j ( i = 1, . . . , n ) . The l a t t e r term can be fu r ther s im pl i f ied . Consider, there fore , Fig. 8 .2 which shows the i ^ body ( i = 1 n) of Fig. B . l with an associated bodycoordinate system B-j CS with axes (X^, Y-j, Z-j) whose or ig in coincideswith CiR. The

Body term f-

i is assumed to rotate with angular ve locity w in frame •jj in Eq. (B.10) which is the time der iva t ive r e la t iv e to

frame R, can be expressed as:

i j = ( z i j - Pi ) = r i jB-iCS

+ OXO-j T j j

where r i j Bscsis the der iva t ive with respect to the frame B-jCS.

However, since body i is assumed to be a r ig id body, the location of m-j j ( i = 1 n; j = 1, . . . , p) i s f ixed in B-jCS. Hence, the

term r-j ,• vanishes:B-jcs

m p # m pI Z m1j r r i j H j = I I mij rr-jj(oxoi r i j )

i= l j= l i=l j= l

" J . J , (m1j r i j * r i j wi " r i j * wi r i j )i = l j = l

235

Rx

Figure 8 . 2 . Body i With I t s Own Body Coordinate System B-jCS in an

In e r t i a l Frame.

236

where the sign ( * ) symbolizes the scalar product between vectors. Introducing a unit dyadic U, s . t . ,

U*v = v*U = v

where v is an a rb i t ra ry vector gives

m p m pJ . J . mi j r r i j s . 1 I mi j ( r i j * r i j * u “ r i j r i j ) * ui1=1 J=1 1=1 0=1

n~ 1 l i “ i ( B.11)

1=1

The quantity I-j denotes the in e r t ia dyadic of body i re la t iv e to i ts center of mass C-j. F in a l ly , the angular momentum formula, Eq. (B.10) fo r n r ig id bodies with a d iscrete mass d is t r ib u t io n becomes:

nH = £ I-j u-j + m-j pp-j p-j (B.12)

i =1

n n pwith I mi ppi Pi = 1 1 mij ppi Pi

i =1 1=1 j = l

Equation (B.12) applies to a system of r ig id bodies with continuous mass d is t r ib u t io n as w e l l . To show t h i s , take the l i m i t of Eq. (B.12)

n nH' = l im H = I j pPi Pi dmi + J j r r i j r ^ dmi

r-vm i= i mi i = l mimi i->0

(B.13)

This s im pl i f ies for r ig id bodies to:

nH' = H = I mi ppi Pi + i a>i (B.14)

i =1

where I i = / ( r i j * r i j * u ” r i j r i j ) dmi * which is the in e r t ia dyadicmi

of body i r e la t iv e to i t s center of mass Ci.

237

Appendix B.5 . Derivaton of the Angular Momentum Expression for N Rigid Bodies Subject to Holonomic (Connection) Constrai nts.

In th is section the formula for the angular momentum of a co l lec t ion of N a rb i t ra ry r ig id bodies, Eq. (B .14 ) , is applied to N- interconnected r ig id bodies. The l a t t e r bodies and t h e i r associated body coordinate are shown in Fig. 8 .1 . The terminology is the same as outl ined in Appendix B4. A local vector p-j shows the center of mass ofbody i ( i = 1 N) r e la t iv e to the center of mass of the compoundsystem CM. Using the appropriate results from [32] the vectorPi = P i ( N) . where N represents the number of l in k s , can be w r i t ten as:

i 1-1 j - 1Pi ( N) = I { ( - Kj + Lj ) I mk + '-j mj)

j = l k=l

. i - 1 N" |i[ <Ki I mk + Li I mk )

n k=l k=i+l

1 N N

TT J . . { "KJ mJ + ( " KJ + Lj^ J . , mk}J=i+1 k=j+l

where

K i ( L i ) : local vector to the i n i t i a l ( terminal) hinge point of the i t h l ink r e la t iv e to the center of mass of the i ^ l ink

mi: mass of the i th l ink

I t should be noted that the time der iva t ive of vectors K-j with respect to the RCS is :

= KiRCS B-jCS

+ Wli-jKi

with Wi: angular ve loc ity of body i in B-jCS

WW-j: cross product operator of

However, the quantity is a constant in the BjCS s . t .B-jCS

vanishes. The vector K-j is conveniently [40] expressed in the B-jCS and transforms from the B-jCS to the RCS via the map A-j:

= Ai KiRCS BiCS

238

Taking the time der iva t ive on both sides and noting that A-j = A-jWW-j gives automatically the previously mentioned resu lt :

V1 RCS = Ai WW-j KI'M B-j CS

The above also applies to the vector L-j. With these de f in i t io ns the expressions for the angular momentum Hn of n r ig id , holonomic l inks can be evaluated, (see Table 8 , 8 ) . In Table 8.8 the expression for the angular momentum Hn and for vectors pp-j(n) and f»-j(n) = pdt-j(n) is given. For reasons of convenience the maps

A-j: B-jCS -► RCS

A-jT : BjCS -► B-jCS

were omitted from the expressions of Hn. A program l i s t i n g for computing Hn, see Table 8 . 8 . symbolically using MACSYMA [100] is given in Table 8 .9 . The angular momentum Hn is evaluated for H3 (n = 3) and the results are shown in Table 8 .10 .

The program, see Table 8 .9 , can easi ly be modified to derive the angular momentum expression of an a rb i t ra ry number of l inks . The number (N) of l inks is only l im ited by the implementation of MACSYMA on the computer system, i . e . , the amount of avai lab le space for data storage.

239

Table 8 .9 . The General Expression for the Angular Momentum Hn of N Links Which are Subject to Holonomic Constraints.

N

\ \H a > N ( P P . P D T ) ♦ > ( I . w )

H / J J , N J , ■ / J J'*«« n i tJ ■ 1 J a 1

M »t i t i »«“

> I L L > * " > a - M K K >

/ J / K J / K J J

NK i l l

■ ■ it acre

J a 1 ♦ 1 K * J t l K a J ♦ |P P a - . . . . . . .

I , n a

I - I J - I J - 1B8SS I I I : I K I I\ \ V.

> I L L > N - KK > M ♦ N L L )/ J / K J / K J JH i t l e t : » »J = 1 K * 1 K a 1► . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

a

N 1 - 1a a s a \ a a i t\ \

LL > a ♦ KK > a1 / K I / K

a a a a a a a cK a l « ] K a 1

240

Table 8 .9 . (cont 'd )

v v \P DT s - ( > ( - ( L L . * ) > K ♦ ( K K . « ) > M

I , H / J J / K J J / KKBIR BRBB I I S I

J s I 4 1 K « J 4 1 K r J 4 1

♦ N ( K K . M ) ) > / «J J J

I - 1 J - 1 J - 1B K II BIBS EBBS

\ V \> ( - ( LL . « ) > N » ( K K . M ) > K - A (LL . * ) )

/ J J / K J J / K J J J

J ■ 1 K s 1 K b 1

«N I - 1

BBBE EEBE\ \

L L . w > K 4 KK . ■ > KI I / K I I / K

BBEB BESSK s 1 ♦ 1 K E 1

241

Table 8 .10. L is t ing of the Program which Symbolically Calculates the Angular Momentum of Three Links.

/ • s y m b o l i c c o m p u t a t i o n o f t n e a n n u l a r a o a a n t u a o f n l i n k s/ • ( I n 3 0 ) , mhl cn a r a s u b j e c t t o n o l o n o m l : c o n s t r a i n t s ./ • O C t . 1985/ • n o t a t i o n : t n e l o t * . * I n d i c a t e s a a a t r l x p r o d u c t/ */ • d e f i n i t i o n s o f s c a l a r s and a a t r l c e s

11:111); 12:1(2); 13:1(3);e l | a ( l ) ; m 2 ! a ( 2 ) 1 a 3 l a l 3 ) ;

k k 2 : k k ( 2 ) ; k k 3 : k k ( 3 ) » 1 1 1 : 1 1 ( 1 ) ; 1 1 2 : 1 1 ( 2 ) ;■ l : a ( i ) ; s 2 : i ( 2 ) ; * 3 : « ( 3 ) ;d e c l a r e ( ( 1 1 , 1 2 , 1 3 , m l , « 2 , a 3 ) , n o n s c a l a r ) ;d e c l s r e ( ( k < 2 , K t 3 , 1 1 1 , 1 1 2 ) , n o n s c a l a r ) ;d e c l a r e t ( m , a l , a 2 , m ) 1 , s c a l a r ) ;

t * I n v o k e r u l e s f o r s c a l a r p r o d u c t s • /

d o t s c r u l e s : t r u e ; d o t O s l s p : t r u e ; d o t 1 s l a p : t r u e ;s i a p s u » : t r u e ;

e x p r l l l } : m ( - k k [ J J * l l t ) ) ) * s u m ( i » l k ) , k , l , J - l ) * l l ( J ] » m t ) ) fe x p r 2 1 1 , n ) : m - i / e * ( k k I l ) » s u m ( m [ k ) , x , l , l - l ) * l l l l J » s u » ( o [ k ) , k , l * l , n ) ) ;e x p r 3 l J , n ) : « - k k [ ) ) » m [ ) ) * ( - k k l 1 J * l l l ) ) ) » s u m ( a [ k ) , K , J * l , n } ;

p p ( l , n ) i m l / a m s u m ( e x p r l l ) ) , J , l , l - l > * e x p r 2 ( l , n ) “ l / a m s u B ( e x p r 3 ( ) , n ) , J , l * l , n ) ;

e x p r « ( j ) : m - ( ( - k k I ) ) . e ( J ) *11 ( } J . a t ) ) ) * s u * ( a [ k ) , x , l , J - l ) * l l ( J J . * [ J ] * a ( J ) ) ; e x p r 5 ( l , n ) : * * l / t > * ( k k l l ) . e t l ) » s u m ( i ( x ) , k , l , l - l ) * l l C l J . e ( l ) »

s u a ( s ( k ) , k , 1 ♦ 1 , n ) ) ; a i p r b ( J , n ) : m - ( - x x l j J . a ( j ) » m ( ) ) t ( - k i c ( j ) . » U J * l l l ) ) . B [ j j ) *

s u a ( m i x ] , k , J * 1 , n ) ) ;

p o t I l , n ) : « l / » » t u » ( e x p r « l J ) , ) , 1 , 1 - 1 ) * e x p r S 1 1 , n l - l / o * s u m ( e x p r 6 1 ) , n ) , 1 , 1 * 1 , n ) ;

r * a n g u l a r momentum H( N1 a / r t o c e n t e r o f aass • /

n l n ) : m s u « ( l ( ) ) . a l l ] , l , l , n ) * s u a ( B ( l J * p p I l , n ) . p d t ( l , n J , ) , l , n ) ;

h ? : e v ( n ( 2 1 ) ; t>2: expand ( n 2 ) ; n 2 : r a t s l « p ( h 2 ) ; n ) : e v ( n ( ) l 1; h 3 : e x p a n d ( h 3 ) ; n 3 : r a t » l » p ( n 3 ) ;

d l s p l a y ( p p ( l , 2 ) , p p ( 2 , 2 ) , p d t ( l , 2 J , p d t ( 2 , 2 ] ,p p ( l , 3 ) , p p ( 2 , 3 ) , p p ( 3 , 3 ) , p d t l l , 3 ) , p d t l 2 , 3 J , p d t l 3 , 3 } , e i p r l ( ) J , e x p r 2 ( l , n ) , e x p r 3 l j , n ) , e x p r 4 l ) J , e x p r H l . n l . e s a r H l , n l ,p p l l , n ) , p d t ( l , n J , n ( n ) , n 2 , n J ) ;

242

Table 8 .11 . The Angular Momentum H3 and Related Quantit ies of Three Holonomic Constraints.

- 1 KK ♦ ( LL - KK ) a * a KK LL ( * ♦ N >

PP1 » 3

PP

PP a3, 3

(M 4 « ) ( LL . W ) 3 2 1 1

1 3 2 3 2 2 1 1 2

a a

a KK LL 3 3 2

♦ a kk a l l 3 1 2 1 1

2 , 3 a n m

a ( LL - KK > ♦ H1 2 2 2

L ♦ a l l ( a ♦2 1 1 2

a ) kk 1 3

PO T a1, 3

a (KK . a ) - a ( LL . a - KK . a ) ♦ M (KK . a \3 3 3 3 2 2 2 2 2 2 2

H ( K K . « ) M ( LL . a ) ♦ 1 ( K K . ¥ ) M ( LL . a ) 3 3 3 3 2 2 1 2 2 1 1 1

PDT a ♦ --------------------------------------------------------------- ---------------------------------2 , 3 a k n

( a ♦ a ) ( k k . a ) 2 1 3 3

P O T a>, 3

- a ( LL . a - kk . a > - a ( LL . a ) - a ( LL . a )1 2 2 2 2 2 2 2 1 1 1

243


t 2 2 2 1 1 2 1 2

s 2 <2>« ( - M - 2 N N - M ) K ) (KK . K 1

2 1 2 1 2 2 2

2 2 2 <2>♦ { ( - N - K ) N ♦ c - « - 2 M M - k ;> K ) ( L U . » )

2 1 2 2 i :i i 2 2 2

2 2 2 2 <2>♦ ( . K H ♦ C- 2 » « — * ) * - K N - N K 1 (KK

1 2 1 2 1 2 1 2 1 2 2

2 2 2 2 <2>♦ ( - * M ♦ ( - 2 M « - K ) N - N N - K K ) ( LL

1 2 1 2 1 2 1 2 1 2 1

2 2 2

2 1 ) 2 1 2 1 2 2 2 2

2 2♦ C l - n M - K ) M • N N > (KK . KK . » )

1 2 1 1 1 2 2 2 2

2 2« 1

1 ) 1 2 1 2 2 1 1

2 2 2♦

2 1 3 2 1 2 1 2 2 2 2

2 2« )

1 2 1 2 v 1 3 2 2 2

2 2♦ ( C - N K - K ) n - n k ) ( LL . LL . M )

I 2 1 2 1 2 2 1 1

2 2♦ l l - n n - n j * — K * ) (KK . KK . « )

1 2 1 3 1 2 2 2 2

2 2♦ ■ >I » 1 2 1 ) 2 2 2

2 2 2 2

» » 1 2 * » » 2 1 22 2

♦ ( K K ♦ ( « N « N ) « ) ( L L . KK . tf ) 1 2 1 2 1 ) 1 2 )

Appendix C

THE GENERATION OF THE ROTATIONAL DYNAMICS OF TWO PLANAR LINKS AND THE CORRESPONDING, AUTOMATED CONTROLLABILITY ANALYSIS

This appendix is on the der ivat ion of the rota t ional dynamics of the planar, two l ink system, which is shown in Fig. 4 .3 , and on the corresponding c o n t r o l l a b i l i t y analysis. The appendix refers to the main section 4 .3 .

Consider Table 9 .1 , which shows a (MACSYMA) program l i s t i n g for the symbolic generation of the rotat ional dynamics of two l inks from t h e i r interconnection diagram, see section 4 .2 .3 . The notation used is ident ica l to that of the above mentioned section. The maps for the transformation of or ientat ion vectors and of angular ve loc i ty vectors are chosen according to the Bryant convention [3 9 ] . The motion of the system is re s t r ic te d to the Y-Z plane and to notations about the X-axis only, i . e . , matr ix entr ies which do not correspond to the above type of motion are set to zero. A f te r th is a data compression is done, i . e . , the system matrices of f and g of Eq. ( 4 .1 8 ) , which are 12 x 1 matrices fo r the three-dimensional system, are reduced to 4 x 1 matrices for planar motion. The result ing system matrices are shown in Eq. (4 .2 0 ) . Note that the angular momentum constra in t , see section 4 .3 . :

P2 = H0 - PX

is already implemented into the system matrices. This implies that the 3rd and 4th equation of the system are ident ica l so that one of the two equations can be dropped from now on. Furthermore

Pi = p

A program l i s t i n g for the automated c o n t r o l l a b i l i t y analysis is shown in Table 9 .2 . The nonlinear c o n t r o l l a b i l i t y analysis is done f i r s t . A basis set L0 c { f , g>LA is :

l 0 = ( g , U * g ] , [ [ f . g ] , f ] }and has structura l parameters, see section 2 .6 ,

( na» nsa> nl» n) = ( 3 > 3 * 3 , 3)

i . e . , i t is ( lo c a l l y ) c o n tro l la b le . The set L0 is shown in Table 9 .3 .

245

Table 9,1. The Symbolic Generation of the Rotational Dynamics of Two Limbs.

s h o t * l 1 : i i ■ j c ; i v i ' i i n j l o c : l r u e

pr 1 in i 11 e v ’’ 111 11/ • j n ' H r 11 i o r i i i ) 1 1 . 0 n y n i l " , i c s i t 2 1 I n i : s 1 i i * - i i

/ ♦ t r t t , I - d y n , i P i c s o t t i i 1 b a " e s y s t e m i l l i - P

2 ♦

2 » • i c i 1 n l t i o r i s o f m . c i i a n 1 c < ’ 1 p o r d . i c 1 1 - r s2 « <i< r i n „ S s i l l t h e s y s t d

2 * • 1 ) 2 : o o p . v o r t o i o t j o l n l . 2 1 1 1 . t o c t 1

2 * • i 2 1 : n o s . v e c t o r o f i o i i i U n 1 . t o c n 2

2 * l ) : I n e r t l d t e n s o r o t h c i y 1 i d . t o o a t

/ ♦ i / : l n e r t l i t o n s o r o t o o o y / r c l . t o u i i 2a r r . y ( L n , 2 , > ) ; d r r n y ( 1 d , ! • , h l n v ) , 2 ) ;

2 * o ' ’ I j h i L i o n o t d r y . i n t a n i l l b ♦ /

l> r , _ d n j ( 1 J I o c k ( I c I , c ^ , c ? , 5 l , b . ' , j j J ,r I : c (; j i t 11 l i , l J ) , c 2 : c n c 1111 i , 2 I 1 s I : s 1 n t t n t i , 1 j I , s 2 : s i i i ( l i > i l , 2 J ) o l d r ) x ( I c < ' c i , - r d s J ,

l c d : , J * s d i . / t c l , c l ' r J - s d s d d I M t s l - c d . ' . V ' c J , s l ' c d t c d s ^ t b j

f i l l : i M t r l » ( l r J / c ^ , - b i / c / , n J

c J : l ij s ( 1 111 i , J I } , s i : s 1 n t i n I 1 , i > ) ,

i> L i ,- i d c / i , c l ' c ; J ) ,

I' 1 IIv I 1 J

u l u c u l 2 *

tr. I,I ~C J*32/C2 ,

: n d t r 1 x ( l r ^ * c J , i - c 2 » s . J , I 2 ,

c i , u,r. J , < : i , u i

u) ,1 J J , "I ,ii I ,IJ 11,

s l i ' i l l f l c . j t l c n o t s y s t P i " n . a t r i c i - s t o i 2 - U mo L i o n * 2

L111 I , J j : 'J , ■)_ I 2 I j J : n , I I , J J : n ,

t i l l e d : n, d _ 2 1 ( l J : n i

I x , 2 J : v ,» l I , 2 ) : ' , v. i i . j j : n , » I x , 2 J : l ,. I : i n t r i x I I « I l , l I J . i * 11 , 2 J J , I . 11 , J J J ) , . 2 : ; i a t i l > l l . l 2 , i J ) . l . l 2 , 2 ) J , l , l 2 , l J J ) , l J : u d i i x ( I i n i , u , i> J , I o , u , u J , 11, , ' ->/ i ) J > ,1 / : n i l l J x ( I i n 2 , ll , ll J , I I I , l l , 0 J , I u , o , n J ) ,

e v CIII s - -II J I ] J , S 1 n- r , u r t O' J t , 1 - 1 I ,e v i i - r y _ -xii' i I 1 I , s 1 11 r , >n; L o' ) t , 1 =■ 2 1 ,d 1 s o l o , ( n i l j , d I 2 1 , H 1 I , n I 2 I , c 1 n v I t j , u I n v I 2 1 ) ,

L n 1 2 , JJ : <>, j - 2 1 i J J : ,* L 2 , J 1 : o ,

) J l :, . I 1 J ..12) .) 11 2 : l r i x

ixi t I i x l

•Jn 2 1 : ii 1 1 1 i x (

I 2 1 : JI / , 1J , > 2 2 : ) I 2 , 2 ), 2 1 1 , I * 1 1 , 1 J , d , l 1 , I ) J ,

‘ ' 1 ‘ I * I x , 3 J , ' J , - . I 2 , 1 J J , Ild_ 1 2 I J J , 0 , -u_l2 I 1 ) J ,) ,

I d _ / 1 I J ) , U , - o _ 21 I 1 J I ,

1 I I , 1 J ) I 2 , 2 ji l l , 21 ] I > , I J

: d I 1 ) . : 1 I I - 0 _ I 2 1 2 ) , d _ 1 2 I 1 J , n )1 i ) , - o _ I I 1 j , ' 1 - 2 ) ( 2 J J .

I - d _ 2 1 t 2.1 , i l _ 2 1 I 1 ) , U J J ) 1 ,1 2 ,e o n c e ( n » n 1 1 ) . d o 1 2 . 1 1 a n s p o : ; e t « 1 1 J ) . d I 2 J i d ) 21 1p i t i e d n • a 1 2 ) . I d 21 . t r . t n s p o s d a l 2 l ) . a I I J . a d ) / )

1)1 OCX ♦ 2

.11,21 , . I 1 , 1J, 0 ) ) , ■. I 2 , 2 J , . 1 2 , 1 J , 0 J ) ,

e r a n )

246

Table 9 .1 . (cont 'd )

/ ♦ ti<*n i t o n J o i ( o n o l s t . i U - e i i m l . t o n s * // » = I ) l U l ' V ) i p * // * i d t = t < i. I * t/ * l e i / . u i i- <it = t / . ’ i * i i / n i ' * /

b i o c u lJ J I t . U : =

I f I < = Jt H I '

( I t j <= i t l r i T. J ( l , l l l i , i J e l s e t r 1 ' j r e d u c e ( j 1 1 , / J I 1 , } - J J ) )e l s e

( 1 1 j <= J t h e I' t r l j r e d u c e ( j I i , 1 ) l 1 - J , j J ) e l s e l i — i . j — J J J ,J 1 : i n i ' i i d t r i x l J J » t. > t" J » t i l , I i : -

I f 1 <= S t h e n ( I I I J . * I J I 1 J I 1 J e J s e( i f 1 e - i. t h e n ( L > l ' / t i . W i ' ) t l - J J 1 1 J e l s e

( I t 1 <= v t h e n t r 1 j r e o u c e ( I «> * a I 1 1 . ( w • 1 1 J . . i d 1 1 - j o I I . * * I 1 J ) .I r a n s i o s e t o l l J J . a l ^ j . o i i i . r f i l l l - n J l l J ) e l s e

( f t 1 <= l i t h e n t r t o t e d u c e ( C oi * a 1 / I . ( / * I i J . J i / 1 - d a i 1 . « x 1 / J ) .L r n h s i ' O S C l t ) l / ! j ) . a t l l . J J W . » l J l l “ lt J | i J ) ) ) ) ,

f : a e n ' n a t r l x ( f , l i , 1 , 1 , 1 ) ,

/ ♦ J a l a c o Ri h i e s s i o n f o r i - d c a s e ♦ /

l 1 M : e a t r J x l I m 1 1 , 1 J , j J t 1 , 4 ) I , I 1 j I •*, 1 J , J J 1 4 , •» 1 J J ,1 j i ' i i i i v : j j / c " ( - i ) ,l j i j i n v : f u l l r a t s u b s t l d l i d i 1 n. , o _ 1 i I J J * J - i 1 I i J ’ .n, j j i ' J i n v ) , f i d h ; -11 r l x ( 1 1 1 1 , 1 1 J , t r I 1 , 1 J ) , 11 l 1 , 1 J ) , 11 t l o , 1 ) I ) , t i d : t i ) ) l r r t l s i i t s i l n l i d i ) ’n , o _ i i l , ! j » a _ i i l i J » i i ' , t i o t , a i a : "i 6 L T l x l I o J , l u j , ( 1 J , I — I J ) ,

t n d t : i " a t r 1 x ( I t h a t 1 1 , 1 j 1 , 11 l u l l I 7 , 1 1 J ) , p i u a t r l x ( l p l ) , J j J , l p l i , ) J J J , t h d t : j J i d 1 n v . u ,f i l : 3 u t . s t l U h J t l l , l J , w U , U , f i d ) , t i d : s I] i s t ( 1 11 'J 11 i , 1 J , * l i , 1 1 , t i n ) , t i 1 : t u J 1 r a t s 1 1" p I f i d ) , i l S D j d V ( t » f i d , ' l i ' l ) ,

K J 1 1 ( a I l n u t ( t i ' J j a j <l ) )) ; / » e n d o l h l o c K * /

/ ♦ s a v e s y s t e " f a t r i c e s

i d s s H v e l I M i d s y s t e u . o a t " J , >i i d , f i o ) ;

247

Table 9.2 Linear and Nonlinear Symbolic C o n t r o l l a b i l i t y Analysis of Two Planar Links.

cl y n. i ii I 1 c ( : l i o r ; s i . n . I 1 » i : t r u r ;

<■ r 1 1 M l l e t " U " ti < ■ c 1 . I > t " ) ;p r i ii t i i 1 ■> t " r e i . . . ' j . s y n " ) ■

/ * i i ' i c - . u d u o n o n l i n e a r c o r I I <> 1 1 <• i. i 1 I I v .'»iw» l y s I/ • l o r I 111 r i l i i X - I •

s i r - i y i | -i i i i i , n c i J , J , .1 j ; r - ^ v t l i > l l r i . u j , J , I J ;

1 o s i ( " X o s y s. 1 1/ i n. ' l (i l " ) ; f l i i u n i ;X * 1 u p 1 r i re n t d L 1 o n o l ci n j ij J * r ino: i.’ i i t inn c o n M r i/ * p i ■* p x s n o - c o n s t m l p x - t i ' i - n l

t : I •) 1 I r .i t r. oi D t ( n o - p l ) , I J , p I X , I I , I x >11 ;t : f o 11 r 11 b 11 c I r v i i , j n I = I , 1 n X = I , d ] x u x I r = - | , m> =• 1 j )t u n i l i s i l i b j n l t i i l ^ , l J - i h l I , 1 ) 1 , < ) ;5 u t S I ( C X I , C C S ( L I t x , l J - L t i l l , l l J , i ) ;6 U n S t C P , p I I , 1 I , v, j ;S i m * , c o l J : = i t c o l - l i n r n 1 1 1 I ( t I t o « , I J , I n I I , I J

M . o t ( l l c n l = x l i m n 'i 1 1 » 11 I r o» , ) iH i l ' i . ( I t C O l - 1 t l i t - n "j 1 1 I I 1 I I i r« , I

I L p I o l t I 1 1 I r n , , i j , l I x , 1 1 j ) ) ; a 11 r i : 1 u 11 r d t s 1 i p ( u p h h a l r j x ( o , J , 1 , 1 , 1 ) ) ; a 1 i n : s o p s I I s x 1 , s 1 n I i n 1 x , 1 1 - t n I 1 , 1 J ) , a 1 1 n J ;a 1 1 n : b u p s 1 1 c x 1 , c o s 1 1 n I x , l J - 1 n I i , 1 1 j , d i i n j ; a 1 1 n : s u v s I l p , j. I t , 1 J , d 1 i n l ; b 1 1 n : i i - i 11 1 x I 1 0 1 , l u j , I j j ) ;

/ * n o n l l i . e o t c o n t r o l l a b i l i t y t /1 1 e n r a c K I v c c 1 , v e c ? J : = n 1 o c k (

/ * j a c o i 1 a n ii*a t r 1 x ♦ /) a c o o 1 o n I i / c e l t i J : = : 1 o c x ( a r I n y ( ii d t , J , J J ,

!■<-=» t 1 1 , J 1 : = 1 1 j = t t ii n n o l I i I v e c t o i I 1 , 1 J , I ii I1.1,,' K ( 1 1 J = X t l i f *l l <J ) t I I V P C t n r I 1 , 1 1

t L o t ( l l l - l i n r n u l f t l v c c t o i i i .K l' . ' iK r t l f l l V P C t o r l l , l l , l l x , ) l ) J ) ,

I n 1 1 r o t s 1 mu I - i e rut a t r 1 x l n.-i t , J , J , 1 , l J J 1 ,/ s l v e c i , v e c X l = r i v e c l . v r c X - ' i v e c X . v e c l » / t u 1 1 r a t s i n > r ( J a c o n 1 a n I v e c 1 1 . v e c X - ) d c o d l a n I * e c

t - o : i i e o r d C K I t , u ) ; t _ u „ i : 11 p I r ac* : 11 _ g , 1 1 : c o i i i r o l l . n l . : i n o c u l m , t _ . j , t _ J _ _ i ) ; r a n* . _ n on 11 n_s y s t e r . : r puk i c o n t i c 1 1 _n at ) ; c : s obs c I s X 1 , s 1 n I t h I X , 1 I - 1 ii ( 1 , 1 I ) , c o m r o i l _ m a t J ; c : s ti c s 1 1 c X I , c o s 1 1 h I x , l j - t r, l 1 , l j j , c 1 ;C : s u b s t I u 1 , p I 1 , 1 I , c 1 ; c : s ii r s t ( i ■ x , i i x , i j , c ) ; c s s u i ' s t i n , j i x o X l n , c 1;c 1 : t n 1 1 r a t s l .up l ’H i p r p 1 nan i ( s ut na 1 1 J x C1 , c ) ) ) ;C X : f u 1 1 r a t s I up l op Le r n l n a n t ( s npnid 11 1 x l x , c ) J J ; c J : t i ) l l : d i b i i i p m e t e i i ' l i i a i i t l b i i i ' i i d L [ l x l J , c ) l j ; c4 : t j 11 r a t b i np m > i •?rm 1 n o n 1 1 si ipi i 'd t r 1 x i i , c J ) J ; K l l l l o l l h u t l a l l n , i > l l r J j ;

/ ♦ l i n e a r c o n l r o i 1 o c 1 1 i I y ♦ /

1 1 : I u 1 1 1 a t s i ir p ( a 1 1 n . M 1 n J ;f X : t u 1 1 r a t s 1 n p I d 1 1 n . t 1 ) :f l s l u l l r a t s i up i a i 1 n . i x J ;c o n t r o 1 I _ I a t : d o r) c u 1 1 b l i n , 1 1 . I X t ;c r s u n s t i s x i , s l n ( t n l x , i j - t n i i , i j i . c p n i r o i i . n a t i ;c . s u o s L l c X l , c o s 11 r IX , l J - t n l l , 1 J ) , c i ;c : s u l s t U 1 , t l l . l J . c ) ;c : s u n s t ( i x , i I X , l l , c l ;c : s a p s 1 1 c , 111 x ii x 1 1: , O ;c 1 : t u l 1 1 a t s i nip l U e V e r r 1 11. in t ( so lor. a t r i > I 1 , c ) > ) ; c X : i u l l r a t s i r ( i r j p t e r n . I n a n t l s u t ' i v i t r i x l / . O l i ; c 1 : t ii 1 I r a t s l inp m e t e r n 1 r i a n t l s ui n.a l r 1 x I 1 , c 1 JJ ; c l i t u l l r a t s i i . p m e l e r i . l n a i t l s u i n u n i - l , c ) ) ) ; r s i i k . l I n _s , s t f i r : r dux ( c u r i r o 1 i _n i t ) ; c o n t r o l l _ ' ' d t _ u r i : e v ( c o n t r u i l _ ' i d t , n u = u j , • r a i i X - l l n . s y s L e i i i - i i P ' K i d i i K i c o i i l i u l i . ' i i d t . o M j ) ;

/ ♦ s t a t i s t i L S ♦ /

s t d t u s t t r e e c o r e i ;

* / • x

i .11

, I n I x , I I ) J , o I 1 , 1 J )

I , I I ), Ln t X , 1 J 1I I , p l l , 1 1 J

1 . v e c 1 j i ;

248

Table 9 .3 . Invo lut ive Basis Set of Vectors for the Determination of Nonlinear C o n t r o l l a b i l i t y of the Rotational Dynamics of Two Planar Links.

11*2 - C 2 ) Ii(U2J) '4411:1/110, ---------------------------, - 11022 2

I N I 1 0 2 - C 21 0

2 2 2 J( l i l t ( I 0 2 1 I. no S?1 - 4 0 2 I It 1*1 *S 2 i ) * 4 0 2 I U H i 1 . 2 1

2 J 2 2- 2 ( . 2 1 I) I I I ; 6 2 1 ) ♦ 1 0 2 11 U I ' l l l - l j / 1 • 2 1 1 1 U H i : . 2 1 )

2 3 2 J J 4♦ i n i U 0 2 1 l> HI 0 21 - 2 0 21 H HO 6 2 1 ) - 2 0 2 1 U 11 6 2 1 )

3 ) 2 2 2 2 4 4 ». 02 ( 1 M 1 1. / - i C 2 I L J M 1 1 2 * 3 C 2 1 ul 1 0 1 1 12 - C 2 1 \, ) 1 ,

1 M - C 2 1 I) 2CO, - ---------------------------------------------, - ( 1 1< 2 ( I N I 12 Li 110 6 2 1 - 2 U H I 6 2 1 )

2 211. ) 1 - 2 - (.21 I)

2 2 3♦ 4 0 2 1 Ii l r . 1 HI 6 2 1 - 2 C2 1 I) H I 6 2 I )

2 3 2 3♦ I I . 1 ( 2 ( . 2 1 U HU 6 21 - 4 ( . 21 I) H I 6 2 1 )

2 2 2 3 4♦ l i . l ( 2 0 2 1 I. H I 6 2 1 - 2 C 2 I U 110 6 2 1 ) ♦ 2 0 2 1 I) 11 6 2 1

3 4 3 3 2 2 2 2 4 4- 2 0 2 1 I. 110 6 2 1 ) / ( l o l 1 0 2 - 3 C 21 ll 1 0 1 1 0 2 1 3 0 2 ) D 1 0 1 1 0 2

6 f 2- 0 21 0 ) ) , 1 1 , - ( 1 0 2 I 1 0 1 11) HO 6 2 1 - 2 0 H I 6 2 1 ) ♦ 2 0 2 1 l> H I S 2 1 )

2 2 2 3 2 3♦ 1 0 1 ( 2 0 2 1 0 H I S 2 1 - 2 0 2 1 L MO 6 2 1 ) - 2 0 / 1 0 11 6 2 1 ♦ 0 2 1 0 H 0 S 2 I )

2 2 2 2 4 t/ ( I N I 1 0 2 - 2 0 2 1 1/ 1 0 1 1 0 2 + 0 2 1 U ) ,

4 2 2 2 4 2- ( 1 0 2 ( 1 0 1 ( 6 ( 2 0 0 2 1 H I 6 2 1 ♦ 2 0 2 1 H I )

4 2 2 * 4 2 2 2 4♦ 11 HO ( - 2 0 0 2 1 HI 6 2 1 - 2 0 2 1 H I ) + it HO ( 3 0 2 1 6 2 1 - 0 2 1 ) )

2 J 3 * 2 2 3 2 3♦ 101 ( l> ( 2 0 2 1 H) - 12 0 2 1 H I S2 1 ) * It HO ( 2 0 C2 1 HI 6 2 1 - 2 0 2 1 H I )

3 2 2 3 2 2 2 2 2- 8 0 2 1 L ll'J 6 2 1 ) ♦ 1 0 1 ( U ( H I 6 2 1 - 0 2 1 H I )

2 2 2 2 2 2 2♦ Li HO ( 2 0 2 1 HI - 2 H I E 2 1 ) ♦ Li HO ( 6 2 1 - 0 2 1 ) )

6 1 2 2 6 2 6 3 2 6♦ Li ( - 1 , 0 2 1 HI 6 2 1 - 3 C2 1 H I ) ♦ I) Ho ( 0 O2 I H I 6 2 1 ♦ 2 C 2 1 H I ) )

2 3 3 2 2 24 I N 2 ( 1 0 1 ( U ( 2 0 2 1 H I - 12 0 2 1 H I 6 2 1 )

3 2 3 2♦ H HO 14 0 21 HI 6 2 1 - 2 0 2 1 HJ ) ) ♦ 1 1

2 2 2 2 2 2 2 2 ( I ) ( 4 HI 6 2 1 - 4 0 2 1 H I ) + 0 11O ( 4 0 2 1 H I - 4 V l 6 2 1 ) )

4 2 2 2 4 2 J♦ U 1 6 0 2 1 HI 6 21 ♦ 0 2 1 H I ) ♦ 1 0 1

2 2 ( 0 2 1 U HI - 2 0 2 1 L HO HI ♦ 0 21 D . l i U ) )

249

Table 9.3 (cont 'd )b 3 2 2 b ?.

4 r < l ( i) ( - u O l i i 5 21 - 3 c / 1 P i >

b i 2 b b / J 2 b4 U 11 ■'» l l n C 2 i l \ 6 2 1 4 4 1.2 1 1 1 ) 4 U i l l ) I - 4 C 2 1 5 2 1 - C 2 1 ) )

2 ■! 2 2 2 « 2 * 1 M 11- l b 1 2 1 P I .‘>2 1 l 1 7 1 P I )

4 2 2 4 ' 1 2 2 2 44 0 , | J ( - 1 0 1 7 ) H 6 2 1 - 2 l i l P I ) 4 U HU l i 1 2 1 6 2 1 4 1 . 2 1 ) )

3 / 2 2 2 2 2 24 11.2 ( I . I l l I P 1 6 21 - ( 2 1 U ) I l ' ) I I) l i l l P I )

d I 2 2 t> 2 o 4 2 b4 1) ( b ( . 21 P I 5 / 1 4 2 C 21 l l ) 4 I) MO ( - o C 21 P i 5 2 1 - 2 ( ' 21

6 / 4 2 b * » 4 2 2 3 .14 u ,10 ( 3 1. 2) . S 2 l 4 C 21 ) ) 2 ( i ; i l 1 0 2 - 4 ( 2 1 I) l i l l 1N2

4 - 1 2 2 b h H tf4 b ( . 21 I) 1 0 1 1 0 2 - 4 ( 2 1 V l l i l 1 0 2 4 ( 2 1 l> ) ) ,

2I - 1 , ( 1 0 2 ( 1 0 ) ( l i HO 5 2 1 - 2 6 1 1 6 2 1 ) 4 2 C 2 i u P I .> 2 1 )

2 2 2 3 2 34 1 .1 1 ( / 1.2 1 l l P ) 6 2 1 - 2 O i l 0 I ' O 6 2 1 ) - 2 (. ' 21 Ll l - l 6 2 1 4 ( . 21 0

2 2 2 i 4 4✓ ( 1 1.1 I o 2 - 2 ( . 2 1 Ii l o 1 1 0 2 4 C 21 I) ) ,

4 2 2 2 4 2( I k 2 I 101 10 ( t u ( 2 1 P I 6 2 1 4 2 ( . 21 P I )

4 2 2 I 1 2 2 2 44 ll rio ( - 2t . C2 1 11 5 2 1 • 2 ( 2 1 P I ) 4 u Hu ( 1 1 1 1 5 2 1 - 1 . 2 1 ) )

2 1 3 2 2 / 3 24 1 0 1 ( u ( / ( 2 1 P I - 1 2 1 2 1 P I 1 2 1 ) 4 p n J ( 2 u 1. 2) I ' l 5 2 1 - 2 .

1 2 2 3 2 2 2 2 2- H 1 2 1 I) HO 6 1 1 ) 4 I N I ( I ) ( P I j 2 I - 1 2 1 P I )

2 2 2 2 2 2 2 4 i) HU ( 2 C 21 P I - 2 P I 5 2 1 ) 4 H HO 1 5 2 1 - ( 2 1 ) )

b 3 2 2 b 2 b 3 24 U ( - 1 2 C 2 1 P I 6 2 1 - 3 ( . 21 P 1 ) 4 L) HO ( 0 C2 1 P I 5 2 1 4 2 C2 1

2 3 3 2 2 /4 1 0 2 ( J i l l ( I ' ( 2 ( 2 1 P I - 12 ( 2 1 P I 5 2 1 )

3 2 3 24 I) HU ( I ( . 2 1 11 5 2 1 - 2 C' 21 P I ) ) 4 l o 1

2 2 2 2 2 2 2 2 I U ( 1 P I 6 . 1 - I 1 2 1 P t ) 4 I,' I tu 14 C 21 P I - 4 P t 6 2 1 ) )

4 / 2 2 4 2 34 11 ( o 1 / 1 p ) 6 2 1 l 1 1 1 I ' l ) 4 M i

2 2 ( ( ' 2 1 U P ! - 2 < 2 1 0 HU P I 4 1 2 1 H Hu ) )

b 3 2 2 b 2 •4 l o l ( L i ( - 12 ( . 21 P I 6 2 1 - 3 ( . 2 1 11 )

b j 2 b b 2 3 2 ' b4 I) HU ( 1 0 C2 1 1 1 6 2 1 4 4 1 2 1 P I ) 4 0 HU I - 4 ( - 2 1 5 2 1 - ( 2 1 ) )

2 4 2 2 2 4 24 1 0 1 ( i i ( b C 21 P I 5 2 1 4 ( 2 1 P i )

■I 2 2 4 4 2 2 2 44 U HU ( - 1 0 C . l P I 6 2 ) - 2 ( 2 1 1 1 ) 4 P HO ( b ( 2 1 6 2 1 4 ( 2 1 ) )

3 2 2 2 2 2 2 24 1 0 2 I P 1 0 1 ( P I o 21 - ( 2 1 H ) 4 ( 2 1 0 1 0 1 P I )

h 1 2 2 6 2 0 4 / b4 U ( 6 ( 2 1 P I 5 2 1 4 2 ( 2 1 P I ) 4 li HU I - b ( 2 1 P I 5 2 1 - 2 ( 2 1

6 2 4 2 6 4 4 2 2 3 34 0 HO 13 ( / I 6 2 1 4 C 2 1 ) ) / I i H 1 11.2 - 4 ( 2 i U 1 0 1 1 0 2

4 4 / 2 b l i d o4 f c ( 2 1 6 1 0 1 i u 2 — 4 I z 1 P l o l l o / 4 ( 2 1 6 ) ) )

PI )

HU S 2 1 )

3C21 I ' l )

bP I ) )

P I )

250


( C2 1) c 1 :1 u ) 1 r r) l s 1 icp ( de t c ; m 1 i i . i i i 11 s u h r a t r 1 x ( 1 , c ) j j ;1 1 e = 12? n s r c .( 0 2 4 ) b

( C 2 b ) c 2 : ( u J 1 r a L b in p ( op ( f . T hi J I s ubmd t r 1 x i 2 , c ) ) ; ;I i t i» = 1 1 / m b e r .( 0 2 S ) 0( C 2 b ) c 1 : 1 u 1 i i a I s 1 u p ( <ie i c r m 1 n a n t I s uuii.a t r 1 x ( J , c ) ) j ;T 1 in e = l i s t * m s e c .

2 0 Hb L'21( 0 2 b ) -----------------------------------------------------------------------------------------------

2 '2 2 2 < * 4J . i l l»v2 - '2 C21 I) i i l l I t . * 4 1.2 1 li

( C2 ) ) C4 : 1 u i 1 r a t s J inp ( de t o r in i n a n t ( s ui j i i iat r 1 x ( 4 , c ) ) ) ;T 1 n e = 1 1 / 2 m s e c .

2 U l ib .S2l( 0 2 7 )----------------------------------- --------------- -------------- ---------------------------------------------------------------------

2 2 2 2 4 4I ’l l I ’12 - i 1 2 1 O H i ! l . u I L 2 I 0

351

Table 9 .4 . C o n t r o l l a b i l i t y Matrix of the Linearized RotationalDynamics of Two Planar Links and Test for C o n t r o l la b i l i t y Defects.

I N . - C / l b I lJ -* J ” A 1 M * ( I U , ----------------------------------------------------./ /

I M 1 0 / - C / l n

IJ M I L - H l . , / ! - i I ’ l . 0 / 1 ) -I I ' i f / M S / l 4 C / l 1/ I I I ) S / 1— - - - - — - — — — ] t

/ / 2 2 4 41 1 1 l i . / - / C / l H I I - I l i V 4 C / 1 b

I . 1 - C / l 'JlU, - ----------------------------,/ /

I I-I 1 ) . / - C / l 1)

J N 1 (I H C S / l - I) 1 1 I / 1 ) 4 I) 1 0 / 11 S / 1 1 C / l II HU S / 12 / 2 2. 4 4

I N I l U/ ; - 2 C / l I; 1 1.1 i n 2 4 C / 1 L'

l l , - l l ' . * i J U l I D HD S / 1 “ * 1’ I I . ' . / l l 2. L / l U H I s / 1 )

* 2 / 3 2 34 1 M ( . 2 L 2 1 D HI S / 1 - * C / l U HU S / l l - 2 C / 1 u HI S / 1 4 C / 1 D HO S / 1 )

2 t . 2 2 4 4/ i n u i n 2 - / c / i i; I M m i i i ' l l L i ,

/ / / / / / 2 2 ( 1 1 1 / l l . i l l l . 1 2 l l S / 1 4 4 C / l HI 1 4 I' HO I - / HI S / 1 - 4 C / l H I )

2 2 2 ) 3 21 C / t C l 'J 1 ♦ I ’ HO U ' Z l HI - / C / l 1 1 S / 1 J

/ 2 i i 21 1H1 l l / I ll ItO HI - C / l 1/ HI 1 - 3 C / l b HI J

2 4. 2 2 2 2 2 2 2 ♦ l i . 1 I I; I C / l HI - HI S / 1 1 4 b li u 1 / I 1 s / 1 - / C / l H I )

2 2 2 2 2 2 2 2 2 2 1 D HU I C / l - S / 1 J ) 4 111/ ( L I C / l Hi - HI S / 1 1

t l .« 1 I C / l b ill! HI - C ' / l V H 1 * ) ) 4 i ' l l

i 2 3 3 2 2 3111 li 0 1 / C / l H ) S / 1 4 b C / l H I ) 4 V HO I - 2 C / l S / 1 - / C / l )

3 3 2 2 4 / / 4 4 / 4 4- 1 C / l I HI ) - C / i V HO S / 1 4 / C / l H HI - / C / l b Ho H I )

j 3 2 2 2 2 l I t o/ ( l . l )■' . / “ 3 C / l t 1 N 1 l i , / 4 J C / l U l m I N / “ C / l | . ) j ,

/I - 1 , 1 ) 0 / 111. ) I s 110 S / i - / O i l S / 1 ) 4 / C / l u HI S / 1 )

2 / / 3 . 2 34 i 141 1 / C / l U HI S / 1 - / C / l 0 n o s / 1 ) - / C / l S HI S /1 4 C / l H fill 5 / 1 )

/ / / / 1 4/ ( l . i l I N / - 2 C / l I 1.11 I N / ♦ C / l U ) ,

252


- t l i < / ( l o l l l ' ( / HI S / 1 4 4 c / l HI ) 4 b HO l - / t ' l . S2I - 4 I V ) H

/ V: / i i JC 2 1 [■ I' D ) 4 I' h d ( C / l H) " I C / l HI S z l I

2 2 S S LI M ( C / l u 110 11 - C / l I- I ' l J - J C / l 0 11 J

/ / / / 2 z / 2 2I M 10 ( L / l I ’ l - I’ l S / l ) + 0 110 1 / I ' l S / l - / l / l H I )

2 2 / 2 t 2 2 2 / /I) I " j u / 1 - s / 1 ) > 4 1 0 / IL I C / l HI - HI S / 1 )

f

I M I L / l I. H' J HI - C / l L> HI J > ♦ I M

j 2 J 1 2 / JU H O 1 / C / l HI S / 1 * b C / l H 1 J 4 L> 110 I - z C / l s / 1 - / C / l 1

J i 2 2 1 / / 1 4 / 4 4- J C / I U HI ) - C / l 11 HO S / 1 4 / C / l Ii HI - / C / l I i I I I ) H I )

1 1 2 2 2 2 4 4 0 0/ ( 1 .11 1 0 / - J C / l I) 1 0 1 1 0 / 4 1 C / l Li I M H i 2 - C / l I) 1 1 )

( C l o ) c l : 1 u 1 1 r o t s 1 inn i. h e l e r m l i i d h i ( s u i o n d L r 1 n ( 1 , c ) ) ) ;I 1 n<e = l i d m s e c .( 0 1 0 ) o

( C I 1 ) c / : 1 o ) 1 1 a t s 1 mi ( d e t e r in I n a r . 1 1 s o o m d l r 1 x ( / , c ) ) ) ;I l ' i e = 4 4 n i c e .( 0 1 1 ) o

( C 1 / 1 c 1 : 1 o l i r n i s I m p ( u i ' t e i ni l n a n l ( s o h r a L i i x ( 1 , c ) J ) ;T 1 n c = 1 / Vu I ' s e c .

2 2 2 2( I ' l / ) - ( ( 0 1 0 2 - / C / l 1/ 1 0 / - 0 l l . 1 4 2 C / l 0 I M ) l l

z 2 2 2 34 l l ' H 0 I S l 4 C / l 0 H O ) H I / 4 h I I I ) I M - C / l O HU 1. 4) - 2 L / l L‘ H O )

1 J / 2 2. '2 4 4 0 1)/ I l l . l 1 0 / - i C / l H l i l l 1 0 / 4 J C / l 0 I M 1 0 / - C / i IJ )

I C 1 1 ) c 4 : i u l l r d L s l n i ' l o e t t ' i n i l i i r i r i t l s u L > i ; i d L r l x ( 4 , c ) ) ) ;I I me = b 11 i i s e c .

/ 2 2 2 ( O i l ) ( l l I i . / - 2 C ' / l 0 1 0 / - 0 1 0 1 4 / C / l 0 I M ) HI

/ 2 2 / J4 ( 0 IH) I M 4 C / l 0 h O ) 1 0 / 4 0 HO l l . l - C / l U n o M l - 2 C / l 0 HO)

3 1 2 2 2 2 4 4 O H/ l l O l 1 0 / - 1 C / l H J M 1 0 / 4 J C / l I) 11.1 111/ - L / l H )

( C l 4 J i ' 3 i i K _ l J i i _ s y s t e i i i : r a r n < . ( c o i i l r o l l _ n . d i ) ;1 1 ii. p = o 4 v c it s e c . *( 0 1 4 ) J

S / 1

S 2 1

253

Possible rank defects of L0 are found by sequentia l ly e liminating one of i t s four rows and be evaluating the determinant of the remaining square m atr ix . The p o te n t ia l ly nonzero determinant det, is:

de t ! = ----- 2D_H£L_SZ1_( J lJ 2 - C^D2)

The l in e a r c o n t r o l l a b i l i t y analysis is considered next. The c o n t r o l l a b i l i t y matrix C, see section 4 .4 ,

C = { B , AB, A2B } : 4 x 3

is shown in Table C.5. C o n t r o l la b i l i t y defects are found in a s imilar way as described before. The largest p o te n t ia l ly nonzero determinant d e t2 of d is :

det 2 = ^ 2 1 ---------- { ( D J 22 - 2C21D2 ) J 2 - D J ^ + 2C21D2J 1)P 1

( j 1j 2 - c212° 2 ) 3

+ H0[ ( D J 1 + C21D2 ) J 2 + D J j 2 - C 2 i0 2J ! - 2C212D3] }

254

REFERENCES

1. Muybridge, E . , Complete Human and Animal Locomotion, Dover Publications, I n c . , New York, 1979.

2. Ouimet, P .C . , Computerized Film Analysis for Three-DimensionalTrajectory Estimation, Master's Thesis, The Ohio State Univers i ty , F ecnS B '4 . --------------------

3. Heginbotham, W.B., Dooner, M . , and Kennedy, D .N . , "Computer Graphics Simulations of Industr ia l Robot In te rac t ions , Proc. of 7th In tern . Symp. on Industr ia l Robots, Tokyo, Oct. 1977, 169-176.

4. Popplestone, R .P . , Ambler, A .P . , and Bel los, I . , "RAPT, A Language fo r Describing Assemblies," The Industr ia l Robot, Sept. 1978, 131-137.

5. Soroka, B . I . , "Debugging Robot Programs with a Simulator."Proc. of CAD/CAM 8 Conf. Anaheim, Nov. 1980.

6. Queromes, J . , "Computer Aided Design and Robotics." Proc. 12th In te rn . Symp. on Industr ia l Robots, P ar is , June 1982, 1 A 5 - 1 9 S .

7. Lieberman, L . I . , Grossmann, D .D ., Lavin, M.A., Lozano-Perez, T . , and Wesley, M.A., "Three-Dimensional Modelling for Automated Mechanical Assembly Research." Report RC 7089, IBM Research Center, Yorktown Heights, 1978.

8. C o i f f e t , P . , Robot Technology, Prent ice-Hal1, London, 1985.

9. Bernard, F . , " In tegral Computer-Assisted Design and Manufacturing System." 12th ICAS Munich, Oct. 1980.

10. Parent, M. and Laurgeau, C . , Logic and Programming. Robot Technology, v o l . 5 , Kogan Page, London 1984.

11. Dombre, E . , B orre l , P. and Liegeois, A . , "A CAD System for Programming Robot Action," D ig i ta l Systems for Industr ia l Automation,Vol. 2 , Dane Russel 1, New York.

12. Meyer, J . , "An Emulation System for Programmable Sensory Robots." IBM Journal of Research and Development, 1981, 2 5 (6 ) , 955-962.

255

13. Sol, E . J . , Veldpaus, F .E . , and Janssen, J .D . , "Tools for Modelling the Kinematics of Multibody Systems," Proc. of Advanced Softwarein Robotics, May 1982, 1B.1 - IB . 12.

14. G r a i l l e , J . and Liegeois, A . , "Locomotion Model Using CAD Methods." IFAC Workshop in Human Gait Analysis and Appl icat ion, Montpel l ier ,

Mow. 1983.

15. Newman, W.M. and Sproul l , R .F . , Pr inciples of In te ra c t iv e ComputerGraphics, McGraw H i l l , Inc. New York, 1979.

16. Foley, J.D. and Van Daam, A . , Fundamentals of In te ra c t iv e ComputerGraphics. Addison-Wesley Publ. Comp., Reading, Ma., 1983.

17. G i lo i , W.K., In terac t ive Computer Graphics, Prentice Hall In c . , Englewood C l i f f 's , N .J . , 1978.

18. Maciejewski, A.A. and C.A. Kle in , "SAM-Animation Software for Simulation Art icu la ted Motion." Comp. & Graphics, v o l . 9 , no. 4 , 1985, 383-391.

19. Sutherland, J .E . and Hodgeman, G.W., "Reentrant Polygon Clipping." Communications of the ACM, 1 7 (1 ) , Jan. 1974, 32-42.

20. Sutherland, I . E . , R.F. Sproull , and R.A. Schumacker, "ACharacterization of Ten Hidden-Surface Algorithms." Computing Surveys, 6 ( 1 ) , March 1974, 1-55.

21. Warnock, J . , "A Hidden-Surface Algorithm for Computer Generated Half-Tone P ictures." Univ. Utah Comp. Science Dep. TR 4 -1 5 , 1969, NTIS AD-753671.

22. Weiler , K. and P. Atherton, "Hidden Surface Removal Using Polygon Area Sort ing." SIGGRAPH *77 Proceedings published as Computer Graphics 11 (2 ) , 1977, 214-222.

23. Badler, N . I . , O'Rourke, J . , and Kaufman, B . , "Special Problems in Human Movement Simulation," Computer Graphics 14, 3, (1980), 189-197.

24. Burtnyk, N. and M. Wein, " In te ra c t iv e Skeleton Techniques for Enhancing Motion Dynamics in Key Frame Animation," Communications of the ACM, 19, 10, (1976), 564-569.

25. O'Donnell , T .J . and A.J. Olson, "GRAMPS - A Graphics Language In te rp re te r for Real-Time, In te ra c t iv e , Three-Dimensional Picture Edit ing and Animation." Computer Graphics, vo l . 15, no. 3, August 1981, 133-142.

256

26. Sandhu, Inc. Specif icat ion Sheet Rhino XR-1.

27. Hemami, H . , "A Feedback ON-OFF Model of Biped Dynamics." IEEE Transactions on Systems, Man and Cybernetics, Vol . SMC 10, No. 7, July 1980, 376-383.

28. Zheng, Y .F . , "Modell ing, Control and Simulation of Three-Dimensional Robotic Systems with Appl icat ion to Biped Locomotion." Ph.D. D isser ta t ion , The Ohio State U nivers i ty ,March 1984.

29. Hatze, H . , "A Comprehensive Model for Human Motion Simulations and I t s Appl ication to the Take-Off Phase of the Long Jump." J. ofBiomechanics, vol . 14, 1981, 135-142.

30. Khosravi-Sichani , B . , "Control of Multi-Linkage Planr Systems in the A i r and on the Ground." Ph.D. D isser ta t ion , The Ohio State U nivers i ty , June 1985.

31. Ghosh, T.K. and Boykin, W.H., J r . , "Analytic Determination of an Optimal Human Motion," J. of Optimization Theory and A pp l icat ion ,vol . 19, no. 2, June 1976, 327-346.

32. Langer, F.D. and H. Hemami, "Dynamics, Control and Simulation of a Quadruped." Math. Bioscience, 75, 1985, 257-277.

33. K le in , C .A . , Olson, K.W. and Pugh, D .R . , "Use of Force and At t i tude Sensors for Locomotion of a Legged Vehicle over I r re g u la r Terra in ," The In ternat ional Journal of Robotics Research, vo l . 2 , no. 2, Summer 1983, 3-17^

34. Kane, T .R . , Dynamics, New York: Ho lt , Rhinehardt, and Winston, 1968.

35. Leimanis, E . , The General Problem of the Motion of Coupled Rigid Bodies About a Fixed Point . Ber l in : Springer-Verlag, 1965.

36. Whitaker, E .T . , A Treat ise on the Analyt ical Dynamics of Par t ic les and Rigid Bodies^ Cambridge Univ. Press, London, England, 1937.

37. Roberson, R .E . , "Kinematical Equations for Bodies Whose Rotation is Described by the Euler Rodrigues Parameters." AIAA-Journal, vol.6, no. 5 , 1968, 916-917.

38. Weinstock, R., Calculus of V a r ia t io n , New York: Dover Public.I n c . , 1974.

257

39. Wittenburg, F . , Dynamics of Systems of Rigid Bodies. S tu t tgar t : Teubner Verlag, 1977.

40. Hemami, H . , "A state Space Model for Interconnected Rigid Bodies." IEEE Trans, on Automatic Control, vo l . Ac-27, no. 2, April 1982, 576-382.

41. Mason, M.T. and Salisbury, J r . , J . K . , Robot Hands and the Mechanics of Manipulat ion, The M . I .T . Press, Cambridge, 1985, 199 pages.

42. Wongchaisuwat, C., Hemami, H . , and Buchner, H . , "Control of Sliding and Rol l ing at Natural Joints ." Journal of Biomedical Engineering, v o l . 106, Nov. 1984, 368-375.

43. Hemami, H. and Wyman, B .F . , " Ind i rec t Control of the Forces of Constraint in Dynamic Systems." J. of Dynamic Systems,Measurement, and Control, Dec. 1979, 355-360.

44. Wittenburg, J . , "The Dynamics of the Human Body." Proc. 5th World Congress on Theory of Machines and Mechanisms, 1979,843-846.

45. Alexander, R.McN., "The Gaits of Bipedal and Quadrupedal Animals." In ternat ional Journal of Robotics Research, vo l . 3 , no. 2 , 1984,

46. Winter, D .A . , "Energy Generation and Absorption at the Ankle and Knee During Fast, Natural , and Slow Cadences." C l in ica l Orthopedicas and Related Research, 1975, May 1983, 147-154.

47. Lar in , V.B. and Naumenko, K . I . , "Control of a Walking Robot withAlmost Weightless Legs: Continuous Version I , Izv An SSSRMekhanika Tverdogo T e la , Vol. 17, no. 3 , 198, 52-62.

48. Lar in , V.B. and Naumenko, K . I . , "Control of a Walking Robot with Almost Weightless Legs, Discrete Model, I I . " Izv An SSSR Mekhanika Tverdogo T e la , v o l . 17, no. 4 , 1982, 36-44.

49. Jambaryan, How Mammals Run. Wiley, New York, 1974.

50. Remizov, L .P . , "Biomechanics of Optimal F l ig h t in Ski Jumping."Journal of Biomechanics, vo l . 17, no. 3 , 1984, 167-171.

51. Kinoshita , G. and K. M a t to r i , " T a c t i le Sensor Design and T a c t i l e Sensing on 3D Objects." J. of Robotic Systems, 2 ( 1 ) , 1985, 41-52.

52. Geschke, C .C ., "A System for Programming and Control l ing Sensor Based Robot Manipulators." IEEE Trans. PAMI-5, 1983, 1-7.

258

53. S. Hackwood et a l . , "A Torque-Sensitive T a c t i le Array forRobotics." In t . J. Robotics Res. 2 ( 2 ) , 1983, 46-50.

54. Smith, D .A . , Chace, M.A., and Rubens, A .C ., "The AutomaticGeneration of a Mathematical Model for Machinery Systems."Trans. ASME, J . Eng. Industry , 1973, 629-635.

55. Hemami, H. and B. Wyman, "Modelling and Control of Constrained Dynamic Systems with Application to Biped Locomotion in the Frontal Plane, IEEE Trans, on Aut. Control, vo l . AC-24, no. 4 , August 1979, 526-535.

56. Dongarro, J . J . , "Performance of Various Computers Using Standard Linear Equations Software in a Fortran Environment." Technical Memorandum No. 23 (Rev. 2) Argonne National Laboratory, Aug. 1974.

57. Stoer, J . , Introduction to Numerical Mathematics I , Ber l in :Spri nger-Verlage 1972.

58. Stoer, J . and Buli rsch, R . , Introduction to Numerical Mathematics 11, B er l in , Springer Verlag 1973.

59. C o l la tz , L . , Funktionanalysis und Numerische Mathematik. Die Grund lehren der MaW. Wissenschattern in Einzerdarstel lungen. Bd. 120. B er l in : Springer Verlag 1968.

60. Householder, A .S . , The Theory of Matrices in Numerical Analysis,New York: Bla isdell 1964.

61. Householder, A .S . , Pr inciples of Numerical Analysis, New York: McGraw H i l l , 1953.

62. Forsythe, G.E. and M ol le r , C .B . , Computer Solutions of Linear Algebraic Systems, Series in Automatic Computations, Englewood C l i f f s : Prentice Hall 1967.

63. Wilkinson, J.M. and Reinsch, C .N ., Linear Algebra. Handbook for Automatic Computation, Vol. I I . Grundlehrender Math. Wissensch. in E in ze ld a rs te l1 ungen, ED. 165. Ber l in : Springer Verlag 1971.

64. Hooker, W.W. and Margulies, G., "The Dynamical A t t i tude Equations fo r an N-Body S a t e l l i t e . " J. of Astronaut. Science, 12, 1965, 123-128.

65. Roberson, R .E . , Wittenburg, J . , "A Dynamical Formalism for an A rb it ra ry Number of Interconnected Rigid Bodies." With Referenceto the Problem of S a t e l l i t e A t t i tude Control, 3rd IFAC Congr. 1966, Proc. London (1968), 46D.2-46D.9.

259

66. Robe, T.R. and Kane, R .R ., "Dynamics of an E las t ic S a t e l l i t e . "In t . J r n l . Solids and Structures 3 (1966) 333-352, 691-703,1031-1051.

67. Russel l , W.J. , "On the Formulation of Equations of Rotational Motion for an N-Body Spacecraft, Techn. Rep. 0200 (4133)-2 , Aerospace Corp. El Segundo C a l i f . (1969).

68. Hooker, W.W., A Set of R Dynamical A t t i tude Equations for an A rb it ra ry n Body S a t e l l i t e Having r Rotational Degrees of Freedom, AIAA J r n l . 8 (1970).

69. L ik ins , P.M., Dynamics and Control of F lex ib le Space Vehicles, Techn. Rept. 32-1320, Nov. 1 , JPL Pasadena, C a l i f . (1970).

70. Kane, T.R. and Wang, C .F . , "On the Perturbation of Equations ofMotion." In t . J r n l . Num. Meth. Eng. (1972) 837-216.

71. Roberson, R .E . , "A Form of the Transla t ional Dynamical Equations fo r Rela t ive Motion in Systems of Many Non-Rigid Bodies, Acta Mech. 14 (1972) , 297-308.

72. Fischer, P . , Einfuehrung in die Mechanik Lebender Mechanismen, Leipzig: Teubner Verlag, 1906.

73. Marey, M., "Des mouvements que certains animaux executent pour retomber sur leurs pieds, l o r s ' q u ' i l s sone precip ites d'un l ieu Eleve. Comptes Rendus des Sceances de V'Academie des Sciences, v o l . 119, 1 8 9 4 , 7 1 4 - 7 1 7 . ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

74. Levy, Maurice, "Observations sur le principe des a i re s ." Comptes Rendus des Sceances de l'Academie des Sciences, v o l . 119, 1894,ir r m r .----------------------------------------------------------------------

75. Magus, R . , "Wie Sich die fa llende Katze in der Luft umdreht." Archives Neerlandaises de Physiologie, vo l . 7, 1922, 218-222.

76. Radenmaker, G.G.J. and Ter Braar, J .W.G., "Das Umdrehen der fallenden Katze in der L u f t . ' Acta Oto-laryngologica, Stockholm, v o l . 23, 1953, 313-343.

77. McDonald, D .A ., "The Righting Movement of a Freely Fa l l ing Cat." American Journal of Physiology, v o l . 129, 1955, 34-35.

78. McDonald, D .A . , "How does a Fa l l ing Cat Turn over?" St. Bartholomew's Hospital Journal, v o l . 56, 1955, 254-25TT7

260

79. McDonald, D .A., "How Does a Cat Fall on I ts Feet?" New Sc ien t is t , v o l . 7, 1960, 1647-1649.

80. McDonald, D .A . , "How Does a Man Twist in the Air?" New Sc ie n t is t , v o l . 10, 1961, 501-503.

81. Smith, P .G . , and Kane, T .R . , "On the Dynamics of the Human Rody in Free F a l l . " Journal of Applied Mechanics, v o l . 1, 1968, 167-168.

82. Pasarello, C.E. and R.L. Huston, "Human A tt i tude Control." Journal of Biomechanics, v o l . 4 , 1971, 95-102.

83. Kane, T.R. and Scher, M.P. , "Human Self -Rotat ion by Means of Limb Movements." Journal of Biomechanics, vo l . 3 , 1970, 39-49.

84. Kulwick, P .V . , Schle i , E . J . , and Vergamini, P .L . , "Weightless Man: Self Rotation Techniques," AMRL, AM RL-TDR-62-129, Oct. 1962.

85. Stepantsov, V .Y . , Yeremin, A . , and Alekperov, S . , "Maneuvering in Free Space." NASA, Washington, D .C . , Techn. Report, No. TT-F 9883,1966.

86. Whitsett , C .E. , "Some Dynamic Response Characterist ics of the Weightless Man." AMRL, AMRL-TDR-63-18, Apr. 1963.

87. Tieber, J.A . and Lindemuth, R.W., "An Analysis of the In e r t ia l Properties and Performance of the Astronaut Maneuvering System." Aerospace Medical Research Lab. AMRL-TR-65-216, Dec. 1965.

88. Bourne, S.R. and Morton, J . R . , "The Design of the CambridgeAlgebra System." Proc. of the 2nd Symp. on Symbolic and AlgebraicManipul. New York: ACM, 1971, 134-143.

89. Schiehlen, W. and Kreuzer, E . , "Rechnergestuetztes Aufstellen der Bewegungsgleichungen Gewoehnlicher Mehrkoerpersysteme." Ing. Archiv. 46, 1977, 185-194.

90. Kreuzer, E . , "Dynamical Analysis of Mechanisms Using Symbolical Equation Manipulat ion." IFTOM V ASMEG 00748, 1979, 599-602.

91. D i l lo n , S.R . , "Computer Assisted Equation Generation in Linkage Systems." Ph.D. D is s e r ta t io n , The Ohio State U n ivers i ty , 1973.

92. D i l lo n , S.R. and H. Hemami, "Automated Equation-Generation and I tsAppl ication to Problems in Control." Proc. of 5th Joint Automatic Control Conf. Austin, Texas, 1974.

93. Hemami, H., "Some Aspects of Euler-Newton Equations of Motion."Ing. Archiv. 52, 1982, 167-176.

261

94. Liegeois, A. and N.J . Aldin, "Generation of Programmation Automatiques Des Equations De Lagrange Des Robots Et Manipulateurs." I . N .R .T .A . , Rapport de Recherche, 32, 1980.

95. Cesareo, G . , Nicole, F . , and Nicosia , S . , "D .K .M . I .R . , Generazione Automatica Di Modelli Dinamici Di Robot I n d u s t r i a l i ." Rapporto 81-03, Inst i tuo di Automatica, University of Rome, Sept. 1981.

96. Cesareo, G . , "Symbolic Algebraic Manipulation and Nonlinear Control Theory." Rep. 82-14, In s t i tu to di Automatica, University of Rome, 1982.

97. Hearn, A.C., "Reduce 2: A System for Algebraic Manipulation."Proc. of 2nd Symp. on Symb. and Algebraic Manip. New York: ACM1971, 128-132.

98. Mart in , W. and Tateman, R .J . , "The MACSYMA System: Proc. of 2ndSymp. on Symb. and Algebraic Manipulation, New York: ACM, 1971,59-75.

99. Pavel le , R. and Wang. P . , "MACSYMA from F to G." J. Symbolic Computation, vo l . 1 , 1985, 69-100.

100. The MACSYMA Reference Manual (version 10) , Massachusetts In s t i t u t e of Technology, 1984.

101. Wang, P . , "Automatic Derivat ion and Generation of Fortran Programs fo r F in i te Element Analysis." ISA, 1985, paper #85-0329.

102. Baumgarte, J . "S tab i l is ierung von Bindungen im Lagrangeschen Formalisinus." ZAMM 38, 1978, 360-361.

103. Baumgarte, J . , "Bereucksichtigung holonomer Nebenbedingungen in Hamiltonformalismus." ZAMM 59, 1979, 27-28.

104. Baumgarte, J . and E. S t i e f e l , " S ta b i l i z a t io n by Manipulation of the Hamiltonian." Celest ia l Mechanics 10, 1976, 71-85.

105. Baumgarte, J . and W. von Gruenhagen, "A S ta b i l i za t io n Procedure for the D i f f e r e n t ia l Equations of the Symmetric Gyroscope Including Perturbing Torques." Celest ia l Mechanics, 20, 1979, 173-178.

106. Von Gruenhagen, W., "Zur S ta b i l iz ie run g der numerischen Integration von Bewegungsgleichungen." I n s t i t u t fur Flugmechanik Report No. DFVLR-FB79-41, 1979.

107. Velez, C .E . , "Notions of Analytic vs. Numerical S t a b i l i t y as Applied to the Numerical Calculation of Orb i ts ." Celest ia l Mechanics, 10, 1974, 405-422.

262

108. Baumgarte, J . , "Numerical S ta b i l i z a t io n of the D i f f e r e n t ia l Equations of Keplerian Motion." Celest ia l Mechanics, 5, 1971, 109-501.

109. B e t t is , D . , "S tab i l i za t io n of F in i te Difference Methods of Numerical In teg ra t io n ." Celest ia l Mechanics, 2 , 1980, 282-295.

110. Burdet, C . , "Remarks on the Perturbed Two-Body Problem and the Harmonic O s c i l la to rs ." In ternal 1017 repor t .

111. S t i e f e l , E . , and B e t t is , D . , " S ta b i l i za t io n of Cowell 's Method." Numerical Mathematics, 13, 1969, 154-179.

112. S t i e f e l , E. and Scheife"!e, G . , Linear and Regular Celest ia l Mechanics. Springer Verlag, B e r l in , West Germany, 1971.

113. Baumgarte, J . "The General Theory of Conservative S ta b i l iz a t io n of the Keplerian Problem." K.E. Szebeler (ed ) , I n s t a b i l i t i e s in Dynamical Systems, Proc. of NATO Advanced Study I n s t i t u t e , Cortina d'Ampezzo, I t a l y , 1978, R l-93 .

114. S t i e f e l , E . , "Remarks on the Numerical In tegra iton of Keplerian Orbi ts ." Celest ia l Mechanics, 2, 1970, 174-281.

115. K a i la th , T . , Linear Systems, Prent ice-Hall I n c . , Englewood C l i f f s , 1980.

116. Brockett , R.W., "Nonlinear Systems and D i f f e r e n t ia l Geometry." Proceedings of the IEEE, v o l . 60, no. 1 , 1976, 62-72.

117. Spivak, M . , A Comprehensive Introduction to D i f f e r e n t ia l Geometry. Publish or Perish, Berkeley, C a . , 1979.

118. Hermann, R., D i f fe re n t ia l Geometry and the Calculus of V a r ia t io n . Academic Press, New York, 1960.

119. Cast i , J . L . , Nonlinear System Theory. Academic Press Inc. 1985.

120. Auslander, L . , D i f f e r e n t ia l Geometry. Harper & Row, New York,1967.

121. Boothby, W.M., An Introduction to D i f fe re n t ia b le Manifolds and Riemannian Geometry. Academic Press, New York 1975.

122. I s id o r i , A . , Nonlinear Control Systems: An Introduct ion,Springer-Verlag, B e r l in , 1985.

263

123. Brockett , R.W., Mil lman, R.S. and Sussmann, H .J . , eds. D i f fe re n t ia l Geometric Control Theory, Proceedings of the Conference, held at Michigan technological U nivers i ty , June 28-July 2 1982, Birkhauser, Boston, S tu t tg a r t , 1983.

124. Brockett, R.W., "Lie Algebras and Lie Groups in Control Theory." Geometric Methods in System Theory, D.A. Mayne and R.W, Brockett, eds . , t)ordrecht, Reidel, The Netherlands, 1973.

125. Crouch, P .E . , "Spacecraft A tt i tude Control and S ta b i l i z a t io n : Applications of Geometric Control Theory to Rigid Body Model."IEEE Transactions on Automatic Control, v o l . AC-29, No. 4 , April 1984, " 3 2 1 - 3 2 3 . ----------------------------------------------------------------------------------------------------------------------

126. Marino, R, and G. Cesareo, "Nonlinear Control Theory and Symbolic Algebraic Manipulation," Proceedings of the MTNS, In ternat ional Symp. Beer Sheva, I s r a e l , June 20-24, 1983.

127. Crouch, P.E. and B. Bonnard, "An Appraisal of Linear Analytic Systems Theory with Applications to A tt i tude Control." European Space Agency Contract Rep. , ESTEC Contract 377 /78/NL/AK(SC) ,May 1980; Contractor: Applied Systems Studies, Coventry, England.

128. Claude, D . , and P. Dufresne, "An Appl ication of MACSYMA to Nonlinear Systems Decoupling," Computer Algebra, Proceedings of EUROSAM 82, M arse i l le , France, April 1982.

264

Date post:	25-Mar-2023
Category:	Documents
Upload:	khangminh22
View:	0 times
Download:	0 times