f'_ r _

Evaluation of

Automated

Decisionmaking

Methodologies and

Development of an

Integrated Robotic

System Simulation

Appendix B--ROBSIM

Programmers Guide

Martin Marietta Aerospace

Denver Aerospace

P.O. Box 179

Denver, Colorado 80201

Contract NAS1-16759

• ' :" .4

March 1986

rUA_R_

NdEiOiidl Aeror_dU{iU5 died

RnaRp Administr_tinn

Langley Research Center

- - Hampton, Virqinia 23665

Data for general release September 1986
EVALUATION OF AUTOMATED

DECISlONMAKING METHODOLOGIES

AND DEVELOPMENT OF AN

INTEGRATED ROBOTIC SYSTEM

SIMULATION

Appendix B--ROBSIM Programmer's

Guide

Prepared by:

Dennis C. Haley

Bonni J. Almand

Mark M. Thomas

Linda D. Krauze

Keith D. Gremban

James C. Sanborn

Joy H. Kelly

Thomas M. Depkovich

William J. Wolfe

Thuy Nguyen

This work was performed for NASA

Langley Research Center under

Contract NAS1-16759.

The use of specific equipment or

company brand names in this report

does not in any way constitute endorsement

of those products or companies.

HCR-85-665

MARTIN MARIETTA

DENVER AEROSPACE

P.O. Box 179

Denver, Colorado 80201
Rev A, October 1985

APPENDIX B CONTENTS

INTRODUCTION ..........................

VCLR Format ..........................

Subroutine Indexing ......................

In-Code Documentation .....................

B-I

B-I

B-3

B-4

IMPLEMENTATION NOTES ......................

Executive-Level Command File ..................

Linking the Programs ......................

Fortran Files and Object Files .................

Fortran COMMON Blocks .....................

Interactive Help Utility ....................

Hardcopy Utility ........................

File "Type" Conventions ....................

B-6

B-6

B-6

B-9

B-9

B-10a

B-12

B-12

SYSTEM DEFINITION FUNCTION ...................

Modules :

INITDRVR ............................

SETLU .............................

LBR KELP ............................

BLDARM .............................

BLDENV .............................

BLDLOD .............................

BLDSYS .............................

BLDTRG .............................

EERCOM .............................

DEFUNIT ............................

CREATARM ............................

RD ARM .............................

BLDDAT .............................

WRTARM .............................

ENVIR .............................

RDENVS .............................

RDARMS .............................

BASES .............................

SETUP .............................

SETUP2 ..................... : .......

HARD COP ............................

SYSGRAF .............................

RDLOD S .............................

LOCMOD .............................

WRT SY S .............................

RDENV .............................

DRWENV .............................

WRTENV .............................

RDLOAD .............................

LOAD ..............................

TLDMAS .............................

DRWLOD .............................

B-17

B-20

B-21

B-22

B-23

B-24

B-25

B-26

B-26b

B-27

B-28

B-29

B-30

B-31

B-32

B-33

B-34

B-35

B-36

B-37

B-38

B-39

B-40

B-41

B-42

B-43

B-44

B-45

B-46

B-47

B-48

B-49

B-50

iii

PRECEDING PAGE BLANK i_OT FiL_Eb
Rev A, October 1985

WRTLOD .............................

RDTRGS .............................

LOCTMOD ............................

RDTARG .............................

TARGET .............................

DRWTRG ..............................

WRTTRG .............................

GRINIT .............................

SPAN ..............................

BASE ..............................

OBJECT .............................

GRAPH .............................

DIALS .............................

JOINT ..............................

ACTUATOR ............................

LINK ..............................

DEFSPJT ............................

TOOLJT .............................

TOOLLK .............................

TOTMAS .............................

ADDMAS .............................

ADDMAS2 ........................... +,

RCICR .............................

GRTERM .............................

BASPUT .............................

JACOB .............................

DATOUT .............................

FORM ..............................

CYL • • • • • • • • • • • • • • • • • • • • • • • • • • • • • •

RECT ..............................

TRISTR .............................

DATATAB ............................

FILLET .............................

OBSTCL .................... . ........

ORIENT .............................

_TV_C .............................

DRAW ..............................

ESMAT .............................

DBAS ..............................

BASELK .............................

RCT STR .............................

CADOBJ .............................

TARG ..............................

CVTUNIT ............................

MATMPY .............................

ERRMSG .............................

ROTMAT .............................

CETM ..............................

LOGO ..............................

CRPD ..............................

B-51

B-51a

B-51c

B-51e

B-51g

B-51i

B-51k

B-52

B-53

B-54

B-55

B-56

B-57

B-58

B-59

B-60

B-61

B-62

B-63

B-64

B-65

B-66

B-67

B-68

B-69

B-70

B-71

B-72

B-73

B-74

B-75

B-76

B-77

B-78

B-79

B-80

B-81

B-82

B-83

B-84

B-84b

B-84d

B-84f

B-84h

B-85

B-86

B-87

B-88

B-89

B-90

B-91

iv
Rev A, October 1985

ANALYSISTOOLFUNCTION .....................

Modules :

SIMDRVR ............................

RDSYS .............................

SGMNT .............................

REQUIR .............................

RESPON .............................

TASK ..............................

BASGMNT ............................

FTIN ..............................

REQOPT .............................

PRTARM .............................

GRAFIX .............................

CNTRLR .............................

SPRGINC ............................

CHKLMT ..............................

DYNAM .............................

VOLTAGE ............................

OUTREQ .............................

E SPAUS .............................

ENDR_Q .............................

SIMOPT .............................

INITCO .............................

DEFCNST ............................

PIDINIT ............................

OUTSIM .............................

CNTRSIG ............................

CONTROL ............................

SETCNST ............................

DERIV .............................

INTGRT .............................

END SIM .............................

SEGTASK ............................

BCNTRLR ............................

SEGINIT .............................

GRASP .............................

RELEAS .............................

ESCNTRL ............................

POSSPJT ............................

RCNTRL .............................

RATEPRO ............................

PCNTRL .............................

CABSM .............................

FORCE .............................

TORQUE .............................

ACTORQ .............................

REQPRT ..............................

REQSOF .............................

REQTRQ .............................

REQPLT .............................

POSGRDJT ............................

JTPOS .............................

CVTIN .............................

B-93

B-97

B-98

B-99

B-100

B-101

B-101b

B-101d

B-102

B-103

B-104

B-105

B-106

B-107

B-108

B-109

B-I I0

B-Ill

B-II2

B-II3

B-114

B-II5

B-II6

B-II7

B-II8

B-II9

B-120

B-121

B-122

B-123

B-124

B-124a

B-124c

B-125

B-126

B-127

B-128

B-129

B-130

B-131

B-132

B-133

B-134

B-135

B-136

B-137

B-138

B-139

B-140

B-141

B-142

B-143

V
Rev A, October 1985

SPRGFOR ............................

CNSTFOR ............................

PTACC .............................

POSSENS ............................

SIMPRT .............................

SIMPLT .............................

FORTOR .............................

FORREF .............................

CMPCTRL ............................

PIDCON .............................

PIDFOR .............................

EFINRT2 .............................

NLINK .............................

SIMLMT .............................

STOPFR .............................

ACTIVPIH ............................

DRTORQ .............................

EFINRT ...................... .......

SLVTHDD ............................

LININT .............................

LDVOLT .............................

CALCI .............................

SOLVE .............................

GAUSS .............................

BASINIT ............................

BRCNTRL ............................

BPCNTRL ............................

TRACKING ............................

IN ITTAR ............................

ANGLE S .............................

NEWFRAME ............................

PERSPECT ............................

HARALICKR ...........................

BDRTORQ ............................

SLVMBAS ............................

SLVLIN2 ............................

REPCOL .............................

ORERR .............................

OUTUN .............................

ICVTATD ............................

BORERR .............................

B-144

B-145

B-146

B-147

B-148

B-149

B-150

B-151

B-152

B-153

B-154

B-155

B-156

B-157

B-158

B-159

B-160

B-161

B-162

B-163

B-164

B-165

B-166

B-167

B-167a

B-167c

B-167e

B-167g

B-167i

B-167k

B-167m

B-167o

B-167q

B-167s

B-167u

B-168

B-169

B-170

B-171

B-172

B-172a

POSTPROCESSING FUNCTION ....................

Modules:

POSTDRVR ............................

SIMOTION ............................

HDWMOTIN ............................

ROBPLT .............................

LDTHET .............................

POSTGRA_ ............................

MINMAX .............................

A_ESoooeoeeoeoeooeeeoeeeoeeoeeeeee

SCAL ..............................

B-173

B-175

B-176

B-177

B-178

B-179

B-180

B-181

B-182

B-183

vi
RevA, October 1985

TICMRK............................. B-184

PREPROCESSOR FUNCTION..................... B-185

Modules:

PREPDRVR............................ B-187

BLDCAD............................. B-188

TRANSF............................. B-189

LINE .............................. B-190

POINT ............................. B-191

CURVE ............................. B-192

SPLINE ............................. B-193

GRAFCAD ............................ B-194

Figure

B-I Example Of In-Code Documentation ............. B-4

B-2 The ROBSIM Executive Command File ............. B-15

B-3 ROBSlM Executive Command File ............... B-6

B-4 ROBSlM Linker Command Files ................ B-16

B-5 Listing of MXPRMS.TXT ................... B-10

B-6 Listing of Help Documentation Extracted from POSTDRVR . . . B-II

B-7 Functional BLock Diagram for INITDRVR ........... B-IS

B-8 Functional BLock Diagram for SIMDRVR ........... B-93

B-9 Functional BLock Diagram for POSTDRVR ........... B-173

B-10 Functional Block Diagram for PRERDRVR ........... B-185

Table

B-I Modules in ROBSIM Object Libraries ............. B-7

B-If Device-Directory Specifications in ROBSIM ......... B-9

B-Ill Files Used for Interactive Help Utility .......... B-II

B-IV Filename Conventions Used in ROBSIM ......... • • • B-i3

B-V Programs Employed in INITDRVR ............... B-19

B-VI Programs Employed in SIMDRVR ............... B-96

B-VII Programs Employed in POSTDRVR ............... B-174

B-VIII Programs Employed in PREPDRVR ............... B-186

vii
Rev A, October 1985

The following is a list of pages modified or added to document the work

done under this phase of Contract NAS1-16759for Langley Research Center.

B-I B-511 B-103

B-6 B-52 B-103a

B-7 B-52a B-105

B-8 B-71 B-105a

B-Sa B-71a B-106

B-IO B-72 B-106a

B-lOa B-72a B-Ill

B-12 B-73 B-llla

B-13 B-73a B-If4

B-14 B-74 B-ll4a

B-l_a B-74a B-II5

B-15 B-75 B-115a

B-15a B-75a B-122

B-16 B-76 B-122a

B-16a B-76a B-124a

B-17 B-77 B-124b

B-18 B-77a B-124c

B-18a B-78 B-124d

B-19 B-78a B-125

B-20 B-79 B-125a

B-20a B-79a B-133

B-24 B-82 B-133a

B-24a. B-82a B-134

B-25 B-84 B-134a

B-25a B-84a B-135

B-26 B-84b B-135a

B-26a B-84c B-138

B-26b B-84d B-138a

B-26c B-84e B-156

B-27 B-84_ B-156a

B-27a B-84g B-158

B-29 B-84h B-158a

B-29a B-84£ B-161

B-31 B-93 B-161a

B-31a B-94 B-167a

B-40 B-95 B-167b

B-40a B-96 B-167c

B-43 B-97 B-167d

B-43a B-97a B-167e

B-48 B-98 B-167f

B-58a B-98a B-167g

B-51a B-99 B-167h

B-51b B-99a B-167i

B-51c B-100 B-167j

B-51d B-100a B-167k

B-51e B-101a B-1671

B-51f B-101b B-167m

B-51g B-101c B-167n

B-51h B-101d B-167o

B-51i B-101e B-167p

B-51j

B-51k

B-167q

B-167r

B-167s

B-167t

B-167u

B-167v

B-172a

B-172b

B-176

B-176a

B-177

B-177a

B-180

B-180a

B-185

B-186

B-187

B-187a

B-188

B-188a

B-189

B-189a

B-190

B-190a

B-191

B-191a

B-192

B-192a

B-193

B-194

B-194a

viii
1) The counter or other item to be "incremented" is initialized before

entering the loop;

2) The test is performed at the end of the loop. The conditions that

•must exist to exlC from the loop are those that appear in the DOUNTIL

test;

3) The item to be executed must be a standard construct or a single

statement;

4) The counter is incremented or other increment-like action is generally

taken (e.g., another line is read) at the beginning of the loop.

If "CI" is the condition that must exist to exit from the loop and "PI" is

a standard construct or single statement, the DOWHILE would be written as

i DOUNTIL L .lCI

DOCASE: The DOCASE construct is for executing a different set of statements for each of several different values of a variable. If "CI" is the variable being tested and if "CI" may have values i, 2, or 3, the construct appears

\ / ci. /F

PI P2 P3 1 P4 PI P2 P3 I P4

Example A Example B

Example A is equivalent to the nested IFTHENELSE form shown in B.

Subroutine indexing. - The subroutine descriptions and VCLRs are arranged

according to the number assigned in the block diagram. This label consists of

three parts (nl.n2.n3). The first part (nl) indicates with which executable--

(I) INITDRVR, (2) SIMDRVR, or (3) POSTDRVR--che module is associated. While

some routines are used in more than one executable, each is documented only

once and labeling number nl tells which section includes that documentation.

PIII OEDINCiPAGEBI.ANK

B-3
The next number(n2) indicates the level in the program hierarchy at which the

routine occurs. There are three main levels under each executive driver and a

fourth level =hat is assigned to the utility functions used by a variety of

routines. The final number (n3) in the routine label distinguishes the modules

within each level of one executable program.

In-code documentation. - Although the information contained in this programmer's guide provides an understanding of the overall program logic and

function of the individual subroutines, the bulk of the program documentation

is included in the Fortran program modules. This enhances the accessibility of

the documentation and allows it to be updated as modifications are made. Each

Fortran module contains a preamble _hat lists the routine's:

i) Purpose;

2) Input (calling arguments and terminal inputs);

3) Output (calling arguments and terminal outputs);

4) Common variables ;

5) Internal variables;

6) External references;

7) Functional description;

8) Assumptions and limitations;

9) Special comments;

I0) References.

Figure B-I illustrates an example of this in-code documentation. The file

SKLTN. FOR contains a skeleton of the preamble for use in writing new programs.

B-4
pIOGi.4Ul t NZfOINI

CDO

COO an_TtCS SI/q_ATION (i_0MIM) pROGPJU4

C00 SYST_q hi[lIMIT1 rm In_lCTl0_ i_JTINi

COO

CDI PE_POSE

(2)1

COL "_he purees of the I_lN tt'olrmn is to pro_i4a a

CDI broad r_mle of com?ucer capabiliaiae Co ameia¢ in the

COl desip, verification, s;--.iacioa, and study of

_Ol robotic SysteM.

COl _ne pcolrm I_ITDEVI is the Eyetm _mfinitiom F_nctiou

(_L dr/_er, tC operates in _1 _lrerec¢ive mode, p_xlp¢ia8 the

COL uer _o¢ the eyerem def_iCiCm opeloa desired. Velid opci_u

COL are: create o¢ mmlify am arm dmce file, crimes or

COl m_di_ a detailed ear,roamer file, c_eate or modify io_d

CO1 Objects, create • lysElm dkl¢l file, o¢ cenli_l&e DII_M

CO1 prolrmm executes.

COl

C......................... ;;-;-;-;-;-_-;._ ........ ;;-; -_-;- :- -: - ._-_-._ -;:-:

CO2

¢D2 rWFINZTZ011 0_ I_UT

CO2

CO2 CALLING AJGtMI_qTS

CO2

CO2 SYM/_L TT_ DIXq 0_FIJI_TXCO

CO2

CD2 _/A

CO2

CO2 T£_qXNAL INPUTS

CO2

(:D2 SYlq$OL

CD2

CD2 IMOD£ I*_

o2

CDZ

CO2

CO2

C.D2

CO2

CO2 L_JU( I_k

CO2

CO2

CO2

CO2

CO2

CO2

_, .. ___ :._:: __.: ± :.:..: _._,._:._,................................

CO3

CO3 0K_IMITICi OF OUTPUT

C03

C0_ C_LLX_ _IrL1

CDI

C03 SYMBOL TYP_ O_q 0_'XNXTXCO

CO3

CO3 N/A

CD3 TE_q I K_ OUTPUTS

CD3

CO3 s_q_OL TYPE Dl/q DErXNXTZON

CD3

CO3 Noue

Co1

c___ __ ___±__ ___±_. _._._._ ......... _________ .........

Co&

c_ CONIqON VAKIA/_S

CO_

¢OA _PUT

CD_ LUL, LU2

CO4

CD_ OUTPUT

C0_

C_ NON_

Cn_

TYPE OZ]q _HXTIGI

L Select fLal for mode of opera¢io=

o l, truce/medley m am dac_t file

- 2, CruEe/medi_/ detailed

miz_mmsuc flie

- 3, Creete/medi_y locd Objects data file

- _. C_uce sye¢ol d_¢e file

- 5, Easd ROil_q INI_I e_e=uEive

1 Pr_mm term_nm£/_m fl._,

requested follw_ fltai

emr

" 1, laiaS_l prolm m

alecEiol pcmp¢

• I_/I, Temimscl pz_q_rnm ".

(::05

COS INT£R/_L 7A_ IA/_S

¢o$

COS SYmbOL T_PE DIN _FINITION

co_

COS IERROIt I_ L Error _ndicacor _Lel

CD_ -o, No errors encountered

CD_ .Nsr* 0, Error hls occurred,

CD_ conrad,s appropriate

COS error number required

CO5 by rml¢zae EItJU_G

COS _LP _•4 L The _nceler 911 input frc_ terminal co

¢D$ siiniiy user Vilhel _:o iCClll blip fill

CD} NODE [_ | _EXH prOl¢ll _ode fLal, set co L

CO5 co indicate program IXICUI_iOQ Ll

CO5 currently vichin the Sysce_ Oe_inic_on

COS Function

ORIGINAL P_- IS

OF POOR QUALIFY

Co_ _ wuc_

c_

c_ Z/O FILSS

_6

¢06 LOL - Lolical unit aeeiit_d for input from the teminel

CD@ LU2 - Lollcai ,mAt aesi_ed for _Cp_¢ Co the terBio_i

CO6

C0G S_IATCH FILLS

CO_

C_ Nl&

CD6

C0_ rTT_]I_AL _OU_XHE_

C_ BLDA/_q - Create/modify am data file

¢_ BL_N - Creete/Imdify detailed enviroummut flle

DLDLOD - Creace/iodify Loed Objects data file

I_1_ - Create syStml dace file

C_ _ -- hilCl effOL" Billies for 4_y e_o¢ o¢¢_r¢i_ I

CD6 durAa| exerutio_

C_J _ _ -- Gala e 4(:1=411118tO Eke _IiLeS[H Map libre_

C_ _T_ - D_fauit Is|loll u_its r_Ci.q_

C06

CO7

CO7 YUNCTIONAL OF_EX F_O_

CO7

CO7 _e _O1_4 clad file p_mpts the user for the

CO7 prosrm fu_ccio_ desired. The three ROLql/q proirlm

CD7 functions are Syltel _finlti_. AIrLeiylis TooLs, lUd

CO7 _IOIC _ocelein|. _e ueer lily clio request pro$cal

CO7 te_ae¢ie_. Upou receiving a valid fu_cciou request,

CO7 the ROBSI_ coiuud file crm_s£ers control co dud executes

CO7 _e appropriate f_Bctio_ driver.

CD_ _ Sylcell _lfiuiti_ eXeruEiq_B Celil _, _I_,

CO7 0LOIS, or BLOSYS co create or mdiEy an arm, envi_mmeat.

CO7 Lo_d objects or am system.

CO7

C08

CO8 ASSUII_TIONS _ LI_ITATXON_

C04_t

CD8 1. ROBSXN is prolrmmmd in FORTRAN 77 for use on

C0_ V_X 111780 c_q_cers under the V_ sparer=hi

c_ Syl¢lqle

CD8 Z. IIOIISXI4 uses Evans and SutherLand _iCi Picture

CM System jrap_ice usini _$ F_[Ti_k_ calleble

CM Iranise routines. U_I of the iraphics

CD8 cap_bilicLee in IOII_XM is optional, however

CD8 full utiliaat£on of the prolrem capabilities

CD_ i8 $reitly I_lited virho_¢ =he lraphicl*

CO8

,"O9

CD9 SpEcial CO_eSNTS

CD9

C99 I. _f irapbics _s desired, the lrap_ics _,_¢k scaciml

C_ ,mac be eesiKned _ein_ individual faciLAcy

C_9 procedures.

CD9 2. The necessary arm data files must exist prior ¢o

CD9 buLldin_ a system.

CD9

COl0

CO10 UlqULE_ S

COl0

COL0 Nmm

COl0

C _ YOLLOMING ClWJ.TI_ A _JELP LIBiUUtY

C'--' '--'-' -' ..... :-....... :--;: :-; :-:::: :-: :-'; :-....... ":'-:'- ";';: :-';......... :

COxl I_XTDM

cox Fills _ IS EXECUTED BY THE SYST_q OEFI$ITZON P_0_ESSOR

cox

COX2

CDX

COX

COX3

C4)X

COZ

CD¢

COX

COX

CDX

COX

CO][

COX2

COX

cn_

COX

CDX

PA/b_TEKS

_iifiers:

/I_OH

TYPE DIN DEPINITXON

l Select flee for mode of operation

- i, Create/modify an aru data file

- Z. Creete/moeify deteiled

e_viremmn c _ite

• 3, CreacelBmdi_y load objects dace file

u _, Create syStOle data file

" 5, End fOISt4 INXTDRV$ _ecu¢ive

Ftq_C_lO$

The P.OBSIN cOiind _ile pr_Ce the user for the

proiri lunacies desired. The chris _ESIN prolr_a

ftmc¢im_ ere Sylcell OefinicAo_, _mlLysie TOOLI, mad

Post P_oceeein$. The user may SilO requost prolram

CDX cerm_ulcio_. Upo_ receivin$ a vliid funccio_ request,

CDX the ItOE_IN c_ file transfers control Co _nd executes

C9_ the appropriate function driver.

C_ The Syscal _l_inicio_ execut£ve calls _LDA&H. _LDEHV.

CDX II_D_)O 0¢ BInSY$ CO truce or _odif7 an am, _viroml_lc,

COX Load o_jlmts or am sys¢l_.

CDX

Figure B-I.- Example of in-code documentation.

B-5
Implementation Notes

Rev A, October 1985

This section describes the programming conventions used in implementing

ROBSIM on the VAX-11 computer architecture under the mS operating system.

The program consists of a large number of FORTRAN routines and their compiled

object rmedules, alor_ with a limited number of executable images and VMS command files.

Executive-level command file. - The executive level of ROBSIM is handled

by an interactive command file ROBSIM.COM. Figure B-2* shows this co-,,and

file. This file runs one of the ROBSIM executable images (Fig. B-3) selected

by the user. PREPDRVR.EXE is designed as a driver for preprocessing data that

may be used by the other program executables. INITDRVR.EXE is the executable

containing the system definition routines and SIMDRVR.EXE contains the analysis tools image. The postprocessor functions reside in two executable files:

(1) POSTDRVT.EXE for video-terminal display of results, and (2) POSTDRHP.EXE

for hardcopy plotting. This is because the display software requires linking

of different modules for terminal vs hardcopy displays.

ROImlM.COM

PREPDRVR.EXE INITDRVR.EXE SIMORVR.EXE

Figure B-3.- ROBSL_ executive command file.

POSTDRVT.EXE

Linking the programs. - Each executable image contains an executive routine having the same name as the executable (POSTDRVR for the postprocessor

images) and a large number of supporting routines. To facilitate linking, the

compiled object modules are included in object libraries. The programs are

linked by executing command files that reference these libraries. Figure B-4

shows the linker command files and Table B-I lists the programs in the object

libraries.

The bulk of the libraries are contained in the main ROBSIM directory

(ROBSIM DIR:) but some reside in different directories or devices. This is

especially convenient for implementations on multiple-disk systems. Table

B-II lists the alternate device specifications; these logicals must be assigned to appropriate physical devices, possibly during the log-in procedure.

Full-page figures can be found at the en_ of this section.

B-6
Rev A, October 1985

TABLE B-I. MODULES IN ROBSIM OBJECT LIBRARIES

CNTLLIB.OLB OBJECT LIBRARY MODULES:

CMPCTRL CNTRS IG CONTROL

FORREF FORTOR ICVTATD

PIDCON PIDFOR PIDINIT

POSSENS

CRLIB.OLB OBJECT LIBRARY MODULES:

BLDARM BLDDAT BLDENV

BLDLOD BLDTRG BLDSYS

CREATARM DEFSPJT ENVIR

LOAD RDARM RDARMS

RDENV RDENVS RDLOAD

RDLODS RDTARG RDTRGS

TARGET TOTMAS WRTARM

WRTENV WRTLOD WRTSYS

WRTTRG

EANDSLIB.OLB OBJECT LIBRARY MODULES:

DATOUT DIALS

ESPAUS FORM

ISBIT1 LOGO

ROTMAT

ESCNTRL

GRAFIX

POSTGRAF

GEOMLIB.OLB OBJECT LIBRARY MODULES:

ACTUATOR BASE BASELK

BASES JOINT LINK

LOCMOD LOCTMOD OBJECT

SPAN TLDMAS TOOLJT

TOOLLK

GRAFLIB.OLB OBJECT LIBRARY MODULES:

CADOBJ CYL DATATAB

DBAS DRAW DRWENV

DRWLOD DRWTRG ESMAT

FILLET GRAPH GRTERM

MAT MATVEC OBSTCL

ORIENT RCTSTR RECT

SYSGRAF TARG TRISTR

MATHLIB.OLB OBJECT LIBRARY MODULES:

CETM CRPD DOT2

GAUSS MATMPY REPCOL

SLVLIN SLVLIN2 SOLVE

B-7
TABLEB-I. (cont)

ESCADLIB.OLB OBJECT LIBRARY MODULES:

GRAFCAD

POSTLIB.0LB OBJECT LIBRARY MODULES:

AXES HDWMOTIN

MINMAX ROBPLT

SIMOTION TICMRK

LDTHET

SCAL

REQLIB.OLB OBJECT LIBRARY MODULES:

ACTORQ BASGMNT BASINIT

BCNTRLR BORERR BPCNTRL

BRCNTRL CABSM CHKLMT

CNSTFOR CVTIN DEBUG

DYNAM ENDREQ FORCE

FTIN JTPOS LODMOV

0RERR OUTREQ PCNTRL

POSGRDJT POSSPJT PTACC

PVASPJT RATEPRO RCNTRL

REQOPT REQPLT REQPRT

REQSOF REQTRQ SPRGFOR

SPRG INC TORQUE VAGRDJT

SETLIB.0LB OBJECT LIBRARY MODULES:

ADDMAS ADDMAS2 BASPUT

GRASP RCICR RDSYS

RELEAS SETUP SETUP2

SIMLIB. 0LB OBJECT LIBRARY MODULES:

ACTIVPIH BDRTORQ CALCI

DEFCNST DERIV DOT

DRTORQ EFINRT EFINRT2

ENDS IM INITCO INTGRT

LDVOLT NL INK OUTS IM

PRTARM RESPON SETCNST

SIMLMT S IMOPT SIMPLT

SIMPRT SLVMBAS SLVTHDD

SLV2ARM STOPFR

UTILLIB.OLB OBJECT LIBRARY MODULES:

CVTUNIT DEFUNIT

JACOB LININT

SETLU ZERCOM

ERRMSG

0UTUN

B-8

Rev A, October 1985
TABLEB-I. (concl)

PREPLIB.OLB OBJECT LIBRARY MODULES:

BLDCAD

CADLIB.OLB OBJECT LIBRARY MODULES:

CURVE LINE POINT

TRANSF SPLINE

TASKLIB.OLB OBJECT LIBRARY MODULES:

ANGLES CNTRLR HARALICKR

INITTAR NEWFRAME PERSPECT

REQUIR SEGINIT SEGTASK

SGMNT TASK TRACKING

Rev A, October 1985

B-Sa
TABLE B-II. - DEVICE-DIRECTORY SPECIFICATIONS IN ROBSIM

Logical

ROBSIM DIR:

MPS DIR:

HZL DIR:

DI3000 DIR:

HARDCOPY DIR:

Modules contained in directory

Basic ROBSIM modules

Evans and Sutherland graphics modules

Help utility modules

Display modules (video-terminal and

hardcopy)

Modules for creating meta-files from

picture files

Fortran files and object files. - Each Fortran routine is included in its

own file; the name of the file is that of the routine it contains and the file

type is ".FOR". An object file with the same name and type ".OBJ" holds the

compiled version of each Fortran file. After a Fortran module is modified, an

executable image containing the updated version can be obtained by issuing the

following commands:

FORTRAN MODULENAME

LIBRARY/REPLACE LIBNAME MODULENAME

@LNKNAME

Connnand files FORROB.COM and REPLIB.COM exist for compiling the entire

set of routines in the main directory and updating the object libraries.

PRTROB.COM provides for printing all of the Fortran modules. Each of these command files should be updated when routines are added to or deleted from the

program.

Fortran COMMON blocks. - The variables used by several routines are arranged into COMMON blocks. The text file ROBCOM.DOC lists and briefly describes the variables included in each COMMON block. A Fortran COMMON statement for each block resides in a file of type ".CMN" that has the same name as

the COMMON block. The COMMON blocks are included during compilation of the

Fortran modules using the INCLUDE statement. This allows a block to be modified

by changing only the ".CMN" file, instead of all the Fortran modules that use

this block.

During compilation of the Fortran modules, maximum values must be specified for some of the array dimensions. These maximum dimensions are often defined by PARAMETER statements, and most of these statements are included in

the file MXPRMS.TXT. Figure B-5 shows a listing of this file. To change the

maximum dimension of some variables (e.g., to increase the number of arms possible in a system), the programmer must only change the appropriate parameter

(MXARMS) and recompile the programs.

B-9
Rev A, October 1985

MXPRMS. TXT

THEPARAMETERSIN THIS INCLUDE FILE ARE CONSTANT VALUES REQUIRED

BY THE PROGRAM.

INTEGER*2 MAXSPN, IFACTCAD, ISEGCAD

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

PARAMETER

(MXLNKS=I0,MXARMS=5)

(MAXORD=3,MAXSEG=20,MXPLTS=31)

(MXENCMPS=30,MXGRCMP=40,MXLDCMPS=I0)

(MXPTS=I0,MXLODS=I0)

(MXTRGCMPS=I0,MXTRGS=I0)

(MXGRAFPT=S000)

(MAXTRAN=50,MAXLIN=20000,MAXPNT=40)

(MAXARC=300,MAXSPL=200)

(IFLGEND=5557)

(IDIV=I0,ISEGCAD=I,IFACTCAD=I0)

(ITLNGTH=4,MAXFLD=30000,MAXSPN=500,MAXBRK=4)

DEFINITIONS

SYMBOL TYPE DIMENSION DEFINITION

MXARMS 1"4 I

MXLNKS 1"4 i

MXPLTS 1"4 I

MAXORD 1"4 I

MAXSEG 1"4 I

MXPTS 1"4 I

MXGRCMP 1"4 I

MXENCMPS 1"4 i

MXLDCMPS 1"4 I

MXLODS 1"4 I

MXTTRCMPS 1"4 I

MXTRGS 1"4 I

MXGRAFPT 1-4 I

ISEGCAD 1-2 I

IFACTCAD I*2 I

MAXSPN 1"2 I

MAXIMUM NUMBER OF ARMS

MAXIMUM NUMBER LINKS ALLOWABLE

MAXIMUM NUMBER OF Y ARRAY DATA

PARAMETERS WHICH MAY BE

WRITTEN TO PLOT FILE

ORDER OF THE POLYNOMIAL DESCRIBING

THE MOTION TIME HISTORY

MAXIMUM NUMBER OF TIME SEGMENTS

ALLOWED TO DESCRIBE THE MOTION

TIME HISTORY

MAXIMUM NUMBER OF POINT MASSES

IN EACH LINK OR LOAD

MAXIMUM NUMBER OF GRAPHICS COMPONENTS

ALLOWED PER LINK

MAXIMUM NUMBER OF GRAPHICS COMPONENTS

IN ENVIRONMENT

MAXIMUM NUMBER OF GRAPHICS COMPONENTS

IN EACH LOAD OBJECT

MAXIMUM NUMBER OF LOAD OBJECTS ALLOWED

MAXIMUM NUMBER OF GRAPHICS COMPONENTS

IN EACH TARGET OBJECT

MAXIMUM NUMBER OF TARGET OBJECTS

ALLOWED

MAXIMUM NUMBER OF GRAFIX POINTS ALLOWED

IN EACH COMPONENT

NUMBER OF SEGMENTS FOR THE CAD/CAM

GRAPHICS

SCALE FACTOR FOR THE CAD/CAM GRAPHICS

MAXIMUM SPAN OF THE ARM DURING CAD/CAM

GRAPHICS

B-IO
Rev A, October 1985

ITLNGTH 1-4 i

IFLGEND 1-4 i

MAXFLD 1-4 i

MAXARC 1-4 1

IDIV 1-4 1

MAXLIN 1-4 I

MAXPNT 1-4 I

MAXSPL 1-4 i

MAXBRK 1-4 I

MAXTRAN 1-4 1

LENGTHOFTHECAD/CAMFILE TITLE

CAD/CAMFILE FLAGSPECIFYING END OF

DIRECTORY DATA SECTION IN FILE

MAXIMUM NUMBER OF LINES IN THE CAD/CAM

DATA FILE

MAXIMUM NUMBER ALLOWABLE CAD/CAM

ARC ENTITIES

NUMBER OF STRAIGHT LINE SEGMENTS

INT0 WHICH CURVE ENTITY IS DIVIDED

FOR CAD/CAM GRAPHICS DISPLAY

MAXIMUM NUMBER ALLOWABLE CAD/CAM

LINE ENTITIES

MAXIMUM NUMBER ALLOWABLE CAD/CAM

POINT ENTITIES

MAXIMUM NUMBER ALLOWABLE CAD/CAM

B-SPLINE ENTITIES

MAXIMUM ORDER ALLOWED FOR B-SPLINE

DEFINING EQUATION DURING CAD/CAM

MAXIMUM NUMBER ALLOWABLE CAD/CAM

TRANSFORMATION ENTITIES

Figure B-5. - Listing of MXPRMS.TXT.

Interactive help utility. - An interactive help utility is implemented in

ROBSIM to provide online assistance to the user for answering some o£ the program prompts. The utility provides the user with information on the function

and form of the routine and its arguments. The utility is implemented using a

mixture of custom software and the VMS help utility. Information for the help

library is included in the in-code documentation in the FORTRAN modules under

the heading "CDX". The command file MAINHLP.COM is executed to set up a help

library ROBLIB.HLB from this documentation. The executable image MNEXTRACT.EXE

is run to extract the help library information from the FORTRAN modules; it

selects all program lines beginning with "CDX" and deletes the "CDX" headings.

The formatted file MAINHLP.DOC lists the FORTRAN modules (type ".FOR" implied)

to be searched for help library information. All segments that are extracted

must follow the conventions for creating help libraries as described in VAX/VMS

Volume 4A Program Development Tools Utilities Reference Manual, Section 10.3.2.

They are temporarily stored in a file of type ".HLP".

B-10a
(Warning - all ".HLP" routines are deleted by the command file execution')

As an example, Figure B-6 shows the help documentation extracted from program POSTDRVR. The help utility is accessed within the ROBSLM program modules

by a call to the subroutine LBR HLP. This module is included in the object

library HELP DIR:QESTLIB.OLB along with the other routines needed for the help

utility. Table B-III summarizes the main files employed by the help utility.

I POSTDRVR

THIS MODULE IS EXECUTED BY THE POST PROCESSOR

2 PARAMETERS

Qualifiers:

/IMODE

3 /IMODE

TYPE

1"4

2 FUNCTION

DIM DEFINITION

1 Select flag for mode of operation

s i, Replay simulation graphic motion

only

" 2, Replay simulation versus hardware

motion

= 3, Parameter versus parameter plots

= 4, Return to ROBSIM executive

The result of executing option I, is to call subroutine

SIMOTION which provides a replay of the robotic system motion

produced from a simulation run. Option 2 provides a

comparison of motion resultant from direct hardware theta value

read and motion resultant from simulation execution, through

subroutine HDWMOTIN.

If option 3 is selected, subroutine ROBPLT is called to

provide parameter versus parameter plots of any of the

data computed and written to a plot file during the

Requirements Analysis Tools Function.

Option 4 returns execution to the primary ROBSIM level.

Figure B-6. - Listing of help documentation extracted from POSTDRVR.

TABLE B-Ill. - FILES USED FOR INTERACTIVE HELP UTILITY

MAINHI2.COM

MAINHLP.DOC

MODULENAME.HLP

ROBLIB.HLB

LIB HELP.FOR

HEL MAC.MAR

Executive command file for extracting help

library documentation from FORTRAN code

Names of FORTRAN modules containing help information

Temporary file of help documentation extracted

from routine MODULENAME.FOR

Data file used for help utility

Module containing program which reads ROBLIB.HLB

Macro routine used to access help facility

B-II
Rev A, October 1985

Hardcopy utility. - The ROBSIM program provides the capability for interactive display of the manipulator system on an Evans and Sutherland graphics

workstation during system creation or analysis. In addition, plots o£ this

display can be generated on a hardcopy plotter for future reference. Generation o9 the hardcopy plot entails three steps: (I) creation during program

execution of a picture file, (2) conversion of this file into a graphics

meta-file, and (3) translation of this meta-file into a display or plot.

The first step is initiated by a call to the routine HARD COP at the

points in the program where a hardcopy of the E&S display may be desired. I£

the user selects to keep a hardcopy of the current display, a picture file

named by the user is created. The routines for this procedure are in object

libraries HCPIC.0LB and HCMFL.0LB in directory HARDCOPY_DIR: and are linked

with the ROBSIM executable images (Fig. B-4). Program execution continues

after the meta-file is completed.

After the ROBSIM run terminates, the user can activate programs that convert the picture files into meta-files by executing HCMFL.EXE. The resulting

picture file can be translated into a display on the video-terminal or hardcopy plotter using DI3000 software. The images TRANSLATE.EXE and then

VTMETTRNS.EXE or HPM_S.EXE in device-directory DI3000_DIR: perform this

translation. Activating these images is made easier by assignments in the

log-in command file:

HCMFL := RUN HARDCOPY DIR:HCMFL.EXE

TRANSLATE := RUN DI3000 DIR:TRANSLATE.EXE

HPMETTRNS := RUN DI3000 DIR:HPMETIRNS.EXE

VTMETTRNS := RUN DI3000 DIR:VTMETTRNS.EXE

The user need only type the keyword to start execution of these programs.

File "type" conventions. - The different types of files used in creating

the ROBSIM program are designated by individual file-type suffixes in their

file specifications. It is recommended that the programmer and user maintain

these conventions in the files they create. Table B-IV lists the suggested

type specifications.

B-12
TABLEB-IV. - FILENAMECONVENTIONS USED IN ROBSIM

APPENDAGE

FOR FILENAME

.ARM

•SYS

•LOD

.ENV

•OBS

.ACT

•CON

.THT

.VLT

.THP

•DAT

.CMN

°OLB

.LIS

•FOR

DEFINITION

ARM GEOMETRY FILE CREATED

DURING SYSTEM DEFINITION

SYSTEM GEOMETRY FILE CREATED

DURING SYSTEM DEFINITION

LOAD GEOMETRY FILE CREATED

DURING SYSTEM DEFINITION

ENVIRONMENT GEOMETRY FILE CREATED

DURING SYSTEM DEFINITION

OBSTACLE FILE (NONPLANARX,Y,Z COORDINATES)

READ FOR DETAILED GEOMETRY INPUT

ACTUATOR DEFINITION INPUT FILE

READ BY MODULE ACTUATOR DURING

SYSTEM DEFINITION

RESPONSE ANALYSIS CONTROL OPTIONS

INPUT FILE READ BY CONTRL MODULE

HARDWARE THETA ANGLE INPUT FILE

CREATED FROM HARDWARE CONVERSION

ROUTINE, AND INPUT DURING POST PROCESSING

HARDWARE VOLTAGE CONTROL SIGNAL INPUT

FILE CREATED FROM HARDWARE CONVERSION

ROUTINE, AND INPUT DURING RESPONSE

ANALYSIS EXECUTION

TIME HISTORY PROFILE FILE CREATED BY

AN INPUT TO REQUIREMENTS ANALYSIS

REQUIRMENTS OR RESPONSE SIMULATION

OPTIONS INPUT FILES; ALSO SOME OUTPUT FILES

COMMON BLOCKS INCLUDED THROUGHOUT PROGRAM

PROGR_ LIBRARY FILES

LISTINGS OF SUBROUTINES IN EACH LIBRARY

FORTRAN CODE

B-13
TABLEB-IV (cont)

•OBJ

.EXE

.COM

.TXT

.DOC

.HLP

•PRT

•PLT

.AGF

.PIC

•SOF

.AVT

.TRQ

.OUT

•BMP

Rev A, October 1985

FORTRAN OBJECT MODULES

ROBSIM PROGRAM AND UTILITIES EXECUTABLES

COMMAND FILES FOR COMPILING, REPLACING

MODULES IN APPROPRIATE LIBRARIES,

LINKING THE DRIVERS, PRINTING ALL

MODULES, AND RUNNING THE PROGRAMS

PARAMETER FILES INCLUDED IN MODULES

THROUGHOUT PROGRAM

DOCUMENTATION FILES

USER HELP FILES GENERATED WITH THE

MNEXTRACT UTILITY IN HELPER DIRECTORY

ACCESSIBLE WITH THE LBR HELP UTILITY

SIMULATION PRINT OUTPUT FILES

PLOT OUTPUT FILES FOR HEWLETT PACKARD

X-Y PLOTTER OR VTI25 GRAPHICS TERMINALS

ARM GEOMETRY PRINT OUTPUT FILES CREATED

DURING SYSTEM DEFINITION

PICTURE FILES OF EVANS AND SUTHERLAND

DISPLAYS, GENERATED WITH HARD COP

ROUTINE; MAY BE REPRODUCED ON THE

HEWLETT PACKARD PLOTTER AFTER

CONVERSION TO META-FILE FORMAT

SIMULATION OUTPUT FILE FOR POST PROCESSING

ACCELERATION-VELOCITY-THETA OUTPUT FILE

TORQUE OUTPUT FILE

ACTUAL HARDWARE OUTPUT FILES FOR

VOLTAGE CONTROL SIGNALS AND CORRESPONDING

THETA ANGLE VALUES

BASE MOTION PROFILE CREATED BY

AN INPUT TO REQUIREMENTS ANALYSIS

BASE REACTION TORQUES AND FORCES OUTPUT FILE

PRINTOUT FILE OF DETAILED GEOMETRY DATA

INPUT BY USER

B-14
TABLEB-IV (conci)

Rev A, October 1985

.CAD

.TRG

•SGF

DETAILED GRAPHICS COMPONENT DATA GENERATED

FROM CAD/CAM FILE INTERFACE IN PREPROCESSOR

TARGET GEOMETRY FILE CREATED DURING

SYSTEM DEFINITION

SYSTEM GEOMETRY PRINT OUTPUT FILES CREATED

WITH UTILITY RDWRTSYS

B-14a
Rev A, October 1985

!ROBSIM.COM

$SETNOVERIFY

$SETTERM/NOBROAD

$COUNT=O

SLOOPS:

$COUNT=COUNT+I

$1F COUNT.GT.I THEN GOTO ASKNEXT

SASKWICH:

SWRITE SYS$OUTPUT "INPUT (PREPDRVR)-- TO RUN ROBSIM PREPROCESSOR

FUNCTION"

SWRITE SYS$OUTPUT "INPUT (INITDRVR)-- TO RUN ROBSIM SYSTEM

DEFINITION FUNCTION"

SWRITE SYS$OUTPUT "

SWRITE SYS$OUTPUT "

SPROMPT:=WHICH:

(SIMDRVR) -- TO RUN ROBSIM

SIMULATION ANALYSIS

TOOLS FUNCTION"

(POSTDRVR)-- TO RUN ROBSIM POST

PROCESSOR FUNCTION"

SREAD/PROMPT="''PROMPT'" SYSSCOMMAND WHICH

$1F WHICH.EQS."INITDRVR" THEN GOTO INIT

$IF WHICH.EQS."SIMDRVR" THEN GOTO SIN

$IF WHICH.EQS."POSTDRVR" THEN GOTO POST

$PREP

SASSIGN TT SYS$INPUT

SASSIGN TT SYS$OUTPUT

SRUN [ROBSIM.WORK]PREPDRVR

SDEASSIGN SYSSINPUT

SDEASSIGN SYS$OUTPUT

$INQUIRR

SPROMPT::INPUT(Y) TO RUN PREPROCESSOR FUNCTION AGAIN,-

(OTHERWISE, RETURN)

SREAD/PROMPT="''PROMPT'" SYSSCOMMANDWHICH

$1F WHICH.EQS."Y" THEN GOTO PeEP

SGOTO LOOPS

$1NIT:

$ASSIGN TT SYS$1NPUT

SASSIGN TT SYS$OUTPUT

SRUN [ROBSIM.WORK]INITDRVR

SDEASSIGN SYS$1NPUT

$DEASSIGN SYS$OUTPUT

$INQUIRI:

SPROMPT:=INPUT (Y) TO RUN SYSTEM DEFINITION FUNCTION AGAIN, -

(OTHERWISE, RETURN)

$READ/PROMPT="''PROMPT'" SYS$COMMANDWHICH

$IF WHICH.EQS."Y" THEN GOTO INIT

$GOTO LOOPS

$SIM:

SASSIGN TT SYS$1NPUT

SASSIGN TT SYS$OUTPUT

Figure B-2. - The ROBSIM executive command file.

B-15
Rev A, October 1985

SRUN[ROBSIM.WORK]SIMDRVR

SDEASSIGNSYS$1NPUT

SDEASSIGNSYS$OUTPUT

$1NQUIRS:

$PROMPT:=INPUT(Y) TORUNSIMULATIONANALYSISTOOLSFUNCTIONAGAIN, (OTHERWISE, RETURN)

SREAD/PROMPT="''PROMPT'" SYS$COMMAND WHICH

$IFWHICH.EQS."Y"THENGOTOSIM

SGOT0LOOPS

$POST:

$WRITESYS$OUTPUT"DOYOUWISH(i) TERMINALOR

(2) KARDCOPYPLOTTING?"

SPROMPT:=ENTERINTEGER:

$READ/PROMPT="''PROMPT'" SYS$COMMAND WHICH

SASSIGNTT SYS$1NPUT

SASSIGNTT SYS$OUTPUT

$1FWHICH.EQS."I"THENRUN[ROBSIM.WORK]POSTDRVT

$1FWHICH.EQS."2"THENRUN[ROBSIM.WORK]POSTDRHP

SDEASSIGNSYS$1NPUT

$DEASSIGNSYS$OUTPUT

$INQUIRP:

SPROMPT:=INPUT(Y) TORUNPOSTPROCESSORFUNCTIONAGAIN,-

(OTHERWISE, RETURN)

$READ/PROMPT="''PROMPT'" SYS$COMMAND WHICH

$IFWHICH.EQS."Y"THENSOTOPOST

$GOT0LOOPS

$ASKNEXT:

SPROMPT:=INPUT (Q) IF YOU WISH TO EXIT THE PROGRAM

(OTHERWISE, RETURN)

SREAD/PROMPT="''PROMPT'" SYS$COMMAND QUIT

$1F QUIT.EQS.'"' THEN GOTO ASKWICH

SEXIT

$STOP

Figure B-2. (concl)

B-15a
_=v A, October 1985

LNKPREP.COM

SLINK/EXECUTABLE=PREPDRVR PREPDRFR,-

PREPLIB/LIB,-

CADLIB/LIB,-

ESCADLIB/LIB,-

UTILLIB/LIB,-

MATHLIB/LIB,-

HCPIC/LIB,-

HCMFL/LIB,-

DISKSUSERI:[ROBSIM.HELPER]QESTLIB/LIB,-

SYS$SYSDEVICE:[MPSGSP]MPLIB/L

LNKINIT.COM

$LINK/EXECUTABLE=INITDRVRINITDRVR,-

CRLIB/LIB,-

GEOMLIB/LIB,-

GRAFLIB/LIB,-

SETLIB/LIB,-

EANDSLIB/LIB,-

UTILLIB/LIB,-

MATHLIB/LIB,-

HCPIC/LIB,-

HCMFL/LIB,-

DISK$USERI:[ROBSIM.HELPER]QESTLIB/LIB,-

SYS$SYSDEVICE:[MPSGSP]MPLIB/L

LNKSIM.C0M

SLINK/EXECUTABLE=SIMDRVRSIMDRVR,-

SIMLIB/LIB,-

CNTLLIB/LIB,-

TASKLIB/LIB,-

REQLIB/LIB,-

EANDSLIB/LIB,-

UTILLIB/LIB,-

SETLIB/LIB,-

MATHLIB/LIB,-

HCPIC/LIB,-

HCMFL/LIB,-

DISK$USERI:[ROBSIM.HELPER]QESTLIB/LIB,-

SYS$SYSDEVICE:[MPSGSP]MPLIB/L

Figure B-4 - ROBSIM linker command 9iles.

B-16
Rev A, October 1985

LNKPOSTVT.COM

SLINK/EXECUTABLE=POSTDRVT POSTDRVR,-

POSTLIB/LIB,-

SETLIB/LIB,-

UTILLIB/LIB,-

MATHLIB/LIB,-

EANDSLIB/LIB,-

HCPIC/LIB,-

HCMFL/LIB,-

DISKSUSERI:[ROBSIM.HELPER]QESTLIB/LIB,-

SYS$SYSDEVICE:[MPSGSP]MPLIB/L

di3 Iink:DILIB/LIB,MFNODE/OPT, LVLC/OPT,-

DI3 DDR:DDRI25,-

DI3-LINK:UTILLIB/LIB

LNKPOSTHP.COM

SLINK/EXECUTABLE=POSTDRHP POSTDRVR,-

POSTLIB/LIB,-

SETLIB/LIB,-

UTILLIB/LIB,-

MATHLIB/LIB,-

EANDSLIB/LIB,-

HCPIC/LIB,-

HCMFL/LIB,-

DISKSUSERI:[ROBSIM.HELPER]QESTLIB/LIB

SYS$SYSDEVICE:[MPSGSP]MPLIB/L

DI3 CS:MGRAIN,Q3ATOC,-

di3_Iink:DILIB/LIB,MFNODE/OPT,LVLC/OPT,-

DI3 DDR:DDR72I,-

DI3-LINK:UTILLIB/LIB

Figure B-4. (Concl)

B-16a
The program INITDRVR is the system

shows the program modules employed in i

modules. The set of functional descrip

following pages describe these routines

Rev A, October 1985

1.1.1 I 1.1.2 I

[ SETLU I! LBR_HI

I 1.2.2 I 1

ZERCOM I1 DEFUNIT 1 [

-- ERRMSG

1.3.1 I

I GRINIT

-- E&S "Routines

1.3.2 I

!1-

1.3.3 !

BASE

- ERRMSG

- ERRMSG .4.4 I

1.4.6 ROTMAT

-- -- MA:rMPY 1.3.34 1.4.5 I

I

-- ERRING

1.2.13 I 1.3.17

I

LOGO

-- E&S Routines

RMSG 1.3.14 J

I -oo-I

1.3.15 I

1.1.7 - I

........... i

1.1.5

-- E&S Routines

1.2.7 !

l ENV|R

-- ERRMSG

1.2.2 I 1.4.1

DEFUNIT

ERRMSG- E.*_

I 1.4.1I

i ! cv_oN_I cV_ONI1.2.26 I

l

I RDTRGS

I

-- ERRMSG

1.4.1 I

I CVTUNIT I L CVTUNIT

LOCTMOD ]

-- MATMPY !- ERRMSG

1.4.4 ]

-- M;TMPY

1.4.5 I

I CETM I

FOLDOUT FRAME

B-17
ORIGINAL PA_E _:_

OF POOR QUALITY

Rev A, October 1985

1.2.5

- E&S Routi_

1.1,4

1.4.4

1.3.14

1.3.15

1.1.8

__._i_ II_ I_

-- Elks Routlm - E&S

RoutJn4s

Figure B-7. (cont)

B-18
Rev A, October 1985

1.1.7

BLDTRG

1.2.1 1.2.2

ZERCOM J [ OEFUN'T

-- ERRMSG

RDTARG

- ERRMSG

144

ROTMAT

1.41'1

1.3.22 1.3.24

TRISTR

1.3.35

RCTSTR

FILLET

!

I MATVEC1

1.3.1,1 !

! GRTERM i

1°2.32

IWR'R_1

Figure B-7. (concl)

B-18a
Rev A, October 1985

TABLE B-V. - PROGRAMS EMPLOYED IN INITDRVR

1.0

I.I.i

1.1.2

1.1.3

1.1.4

1.1.5

1.1.6

1.1.7

1.2.1

1.2.2

1.2.3

1.2.4

1.2.5

1.2.6

1.2.7

1.2.8

1.2.9

1.2.10

1.2.11

1.2.12

1.2.13

1.2.14

1.2.15

1.2.16

1.2.17

1.2.18

1.2.19

1.2.20

1.2.21

1.2.22

1.2.23

1.2.24

1.2.25

1.2.26

1.2.27

1.2.28

1.2.29

i.2.30

1.2.31

INITDRVR

SETLU

LBR HELP

BLD-ARM

BLDENV

BLDLOD

BLDSYS

BLDTRG

ZERCOM

DEFUNIT

CREATARM

RDARM

BLDDAT

WRTARM

ENVIR

RDm_

RDARMS

BASES

SETUP

SET_

HARD COP

SYS_

RDLODS

LO(_)D

WRTSYS

RDm_'V

DRWENV

WRTENV

RDLOAD

LOAD

TLDMAS

DRWLOD

WRTLOD

RDTRGS

LOCTMOD

RDTARG

TARGET

DRWTRG

WRTTRG

1.3.1 GRINIT

1.3.2 SPAN

1.3.3 BASE

I. 3.4 OBJECT

1.3.5 GRAPH

1.3.6 DIALS

1.3.7 JOINT

1.3.8 ACTUATOR

1.3.9 LINK

1.3. I0 DEFSPJT

I. 3.11 TOOLJT

1.3.12 TOOLLK

1.3.13 TOTMAS

1.3.14 ADDMAS

1.3.15 ADDMAS2

1.3.16 RCICR

1.3.17 GRTERM

1.3.18 BASPUT

1.3.19 JACOB

1.3.20 DATOUT

1.3.21 FORM

1.3.22 CYL

1.3.23 RECT

I. 3.24 TRISTR

1 .3.25 DATATAB

1.3.26 FILLET

1.3.27 0BSTCL

1.3.28 ORIENT

1.3.29 MAT

1.3.30 MATVEC

1.3.31 DRAW

1.3.32 ESMAT

1.3.33 DBAS

1.3.34 BASELK

1.3.35 RCTSTR

1.3.36 CADOBJ

1.3.37 TARG

1.4.1 CVTUNIT

1.4.2 MATMPY

1.4.3 ERRMSG

1.4.4 ROTMAT

1.4.5 CETM

1.4.6 LOGO

1.4.7 CRPD

B-19
1.0

Rev A, October 1985

INITDRVR

The program INITDRVRis the system definition function driver. It operates in an interactive mode, prompting the user for the system definition option desired--create or modify an arm data file, create or

modify a detailed environment file, create or modify a target objects

file, create or modify a load objects file, create a system data file,

or terminate INITDRVR execution. Subroutine SETLU is called to set the

Fortran logical units. The necessary simple cylinder or detailed single arm file must exist prior to building a system. A detailed graphics save file is opened if requested.

B-20
E

>

n_

O

tH

Z

H

E

O

0_

H

H

F

h

h

C

\ll

Rev A, October 1985

#'_D|#_,lln w _..

v_,_a,_i_L PAGE 18

OF POOQ QUALITY

W

01

E

U

O

0

300HI

m

300HI

300HI

3_USS3_

311UM

371J

UIUOH_IgISw

31U3U301 _:

gig078 77U3

s133r80_077H

AJI00Nl]lU_d0 H

01 3Kiln0# n

007078]7B3

_13]rgo i_i HT

A_0N/31_3_3

01 ]_IUl0_

_10]g 1_0 E

IH3HNOE^N3 0

037IB130 Id

13100N/]1B3_0

01 3NIle0# E

AN3078]7_0

37£3UlUO_Uln

013NIlnOa!_

N_070 ll_OL_

SS_OOUIUUUSI7d13HU3SAU03d13H-UO777U3

T

I

I

Z

0

U '

x

O_

>

0

P

H

Z

H

W

P

I

w Z

P N

H

n

N

m

H

W

n

W

0

0

3"

Z

0

H

H

rr

Ig

O.

_1

N

Z

0

0

B-20a
i.i.i SETLU

SETLU is called from the various executive drivers to set the Fortran

logical unit number to be stored in COMMON block LUNITBK for reference

by the rest of the ROBSIM program. After assigning the variables to

consecutive unit numbers, the "unit open" flags are reset to indicate

the units are not open (except the terminal read and write units).

SUBROUTINE SETLLI

RSSIGN LUI THRU LU20 SUCCESSIVE

LOGICAL NUMBERS STARTING NITN 5

[LUII5 .... )

SET FLAGS FOR LWI RNO LU2

INOICRTING UNITS OPeN

RESET FLAGS FOR REMRINING UNITS

INOICATING UNITS NOT OPENEO

DISPLRY LOGICRL ASSIGNMENTS TO

USER ANO PROMPT FOR FLAG TO

CONTINUE

B-21
1.1.2 LBRHELP

Subroutine LBR HELP is called to execute the help utility during a

ROBSIM run. I_ uses the object file created from the macro HELPMAC.MAR

and runs the system help utilities as required.

(VCLR for LBR HELP not available.)

B-22
1.1.3 BLDARM OF POOR QUALITY

BLDARM is met when a selection of i, to create or modify an arm data

file, is entered from INITDEVR. The user choices for mode of operation

are (i) create a simple cylinder arm data file, (2) modify existing arm

data file, (3) enter detailed graphics data for arm (a simple cylinder

file must already exist), or (4) terminate arm definition and return to

the INITDEVR. For initial creation, option (I), subroutine ZERCOM is

called to zero the COMMON locations and then CREATARM is called to build

the new data file. For modification, RDARM and CREATARM are called when

option (2) is requested. BLDDAT is responsible for the invention of detailed ar_ geometry. In all cases, WRTARM will be called to write the

arm data COMMON information.

SUBROUTINE BLOFqRM

SET I_I_OCESSOI_ MrmDl=" == I. FOR SYSTEM O_F'rN'rTION

PROMPT U$1=R F"OR OP_I_I::ITTON MOOF. "r'MOO_

REFqR I_ISmR RI='SI_ONSI="

r_oC_SE

"rMOO_ . I_=. :3 |_ = -

.J

OOCASE t/I

=Moo_ . -E. 2 / _=_ - _ / I

OOCASE //! _ I_

, / I_- _ E I_

=MooE - _ I_oo_ - _ / I=__-_ _ I_

¢_. _ TO _ _-4 == _-

•'to C_='JCNZ :r,/O I I ---J I_ _ ¢..._ I _.-. I

--_-=_-l_.____ i _ I_ I _ I _ =.__ =m_r_m_ ! -_-_ m I _ I I --J I

mou_-_ _ =p_ INom_,,, _ _ ___ I_ _ I __ I .-2__

' __ _ = _ I "--" I

USER "INCLUOING /1 @ _ _ I_

m"'_, OETR'rLED GEOMETmY /FI _ _ _ I_

F_L.._NIqME

RETURN

i ENO

B-23
1.1.4.

Rev A, October 1985

BLDENV

The user has the capability with routine BLDENV to specify a detailed

physical representation £or the robotic environment. Components £or

the environment are de£ined as basic geometric shapes (cylinders,

cones, rectangular solids, symmetric or nons3nmetric trapezoidal £igures, triangular cross-sectional beams, rectangular beams, fillet comDonents, data tablet-de£ined entities, obstacles, and CAD/CAM objects.

The component type is written to the detailed graphics save £ile if

requested.

B-24
ORIGINAL F_ rS

OF POOR QUALITY

Rev A, October 1985

>

Z

HI

0

J

W

Z

H

O

O

E

o7

O

tO

\ _L_

w mL1_

E_

_1 _

H TaD

I

lmImBTHI LTHB

I '-

Id

-'- II

ILl.='-=-m

#N H

v#

w_ 1

" °-W_ Ua'

__ --

|" iI

i m n"_ll

k, imml_

i Ill .I.,mTim

ld

Id

i IH "B.1Tlm

B-24a
Rev A, October 1985

I.I.5 BLDLOD

Through routine BLDLOD, the user has the capability to specify a detailed physical representation for the robotic load objects to be

used. Components for the load objects are defined as basic geometric

shapes (cylinders, cones, rectangular solids, symmetric or nonsymmetric

trapezoidal figures, triangular cross-sectional beams, rectangular

beams, fillet components, data tablet-defined entities, nonplanar entities, and CAD/CAM objects). This subroutine creates a new file, or

modifies an existing file of load objects, and includes the capability

to specify the detail at the first creation session for the load objects. The component type is written to the detailed graphics save

file if requested.

B-25
ORiGiNAL _ r_

POOR QUALITY

Rev A, October 1985

nlm

i! i I _ MnNI

i _:=-=="

(I) I I_lll

B-25a
Rev A, October 1985

1.1.6 BLDSYS

BLDSYS prompts the user for the moving base information and then reads

individual arm data files into the appropriate system COMMON blocks.

It desired, the locations of the fixed bases can be modified. BLDSYS

then sets up the environment, load objects, target data, and stores the

system file.

B-26
ORIGINAL PA,$_Z :":,

OF POOR QUALITY

Rev A, October 1985

>-

{00I

00

III ZHD0

if)

1ItI

-II

-_=,

31Z_

JlOZ@

rri_

El, 0

U.O"H

b>

U

i

I

.!

I

oH

u_

HO

EH

U

l-r

wEE

_t H

IIz,

°i IL2

t-3

rr_

u. r'l_ c r_

,I- II>

11JH

11

I1E

Ill

#lE>

l!

m_

O:

tLC

P

Iii

L_

rr

rrh

iii DDJUZHO

IIII O0

3:ZOH

I-HZH

I,

III OE0

IIEW

@03bC

(I

(I

!Iii

I-W

0'

hl,

Eh ,-

h@-

I,I D

=

zo

HW ,-

O 0C J h U H Z Ol H .I,II

n'0:l

h OH_ HE 0111:

'Z O!-

P (]: E0 _l F d o

III o_oH

n" HE:

EEN_:

FI-_(9 !

oZoE

I,I ,I--

rri

(_1_13 '

I-jOj I ii n H iiii :I _ I _I_ i I II

"ZZ _!

- 13:: ri - I

II O'r ,.

_: oluV,-ll

>-

rr

I1"

iii o

Iii Z

IIHUH

DTn

(I

n"

oiL0 1 Wh

/lI

JZ

IZH

U,5"

rr

iNF0F

U.

.J

..J

C]: U

hi Z

I,I

II0H

11. Ho

zZ

Hrr

"3

HW

"IT

I[h@

>- @OomOEH

n,0h@

>-,

n', =!

J[ JE

;u

0Z

hi

B-26a
1.1.7

Rev A, October 1985

BLDTKG

Through routine BLDTRG, the user can specify a detailed physical representation £or the robotic target objects to be used. Components £or

the target objects are defined as basic geometric shapes (cylinders,

cones, rectangular solids, symmetric or nonsymmetric trapezoidal figures, triangular cross-sectional beams, rectangular beams, fillet components, data tablet-defined entities, obstacles, CAD/CAM objects and

four-dot target patterns). This subroutine creates a new file or modifies an existing file of target objects, and includes the capability to

specify the detail at the first creation session for the target objects.

B-26b
ORIGINAL F.,',..- /

OF POOR QUAL_q_

Rev A, October 1985

@

E

H

O

I

a]

W

Z

H

H

fr

I -.i__-=--_ _ __,

,,,__o_o_ Zyff

o__ _1. _o_o_-

.,_._,,, _o_ z_

I "*_= [_Z!O U rrH i

""-q'T_°>""_"__ _ !

LL LD ldH

"_ = ,ilWh HE H 0E

o-71_z,,o_I°_oo

-- ZW

I ._--,_'_z _ _>_- H ZII_Z n

t I_,0

iO ff_-_J

•' =%"'

._ w=_r O UE

B-26c
io2oi

Rev A, October 1985

ZERCOM

Subroutine ZERCOM is called from BLDARM, BLDLOD, and BLDTRG to initialize the arm, load, and target data COMMON blocks prior to creating

new system files. The COMMON blocks initialized to zero include:

1) BLDGBK - arm geometric properties;

2) BLDMBK - arm mass properties;

3) GRAFBK - arm graphics data;

4) BLDABK - arm actuator parameters;

5) BLDSJBK - special joint flags;

6) LOADBK - load geometry and mass properties;

7) LGRAFBK - load graphics data;

8) TARGBK - target geometry and mass properties;

9) TGRAFBK - target graphics data.

B-27
Rev A, October 1985

[ II

I !

I

O0 _ lV u Z _

7 q W

,/z!

wI II

wEv

,,,10131 w Z

o:H

t-t Jt Q.

HZ o m

OZ,_

ry 3 ,,,BE

' _E hl

vJ!X _ 3

C:Em[z

ZI> H

I 0

i o

i

I

_" _lm

oJ(]: I_ / I(1" / i

rn , trr m I

tr-iBizllvlZl _o!Z

Mlwiv olW

__!_l_,-_l_o, o!O Izi

_: Z ! zO u°_ ZH

°!Htzl_

m _l O w ,

I-

_IHIOI_

_IIIH_ 0

0 Q. •

IU/0:lo 0

m/ tc_ _-

_Irr_ I_ :_:lW/Z_ or.

iz rnln" _ w

m

Iz:Z_ • :z

H D!Z z 3

m Z HI M Z

"'J I,,Im

II_1 III

[_:i Z Jig H! _lU'; z

tw _ I H

_'..JIHI_ i_1

!;' HllIi_ iH

IN t:]:_ 9__,l>toiH ,, ,4.1 IlZ IZ

!w 3 w 13

Owl 0

I oI_I o

O

0

Z

0

Z

5-

0

0

.J

H

0

N

n

Z

H

rn

H

i

ro

im

jN

i

Z

n"

Q:

bZ

hi

UI

Zl

H_

Z

H

0

"3

.J

H

W

0

N

rn

Z

Z

II

iz

..1

H

P

Z

3

0

0

0

II

Z

! ti",

rr H I_ _.

h rr H

olo o1,,, [

u ml ol v

mln z • LO oO'

: _ ' _. _,/

, O o o H _-

IzR_>r-_l I

°IHI"IPI_I_! 77"

Z m Ill 0 O

" _lu!_l,',l.I Iii

> O_IH alO:::lzl l lil l

w_J:O:t I rrt

ZI IT r I'- h (_

O' _ 0

ill Ii @

rr rr _- N n"

z ZIz 3

H :::)IHII_ Z ,Z,I

,,, !Z i il_ ni

_1_ m HI/-..

i H I" I#1_ I I]:1

;01:' _" 0:' O:

>_...]I_1>,4,i_

_Hi_l I,m_ rr

o I.- i _ / g IE j _o:

ill: / _ /_i>

i---- In'_ ,-,In"

I 0 w W _

i °lml u

B-27a
1.2.2 DEFUNIT

Subroutine DEFUNIT is called during system definition to set up input

and output units specified by the user. _f these I/O units are not metric, the routine establishes conversion factors between the I/O units

and internal (metric) units. The conversion factors are stored in variable CONUNIT_ and LISUNIT contains a character string listing the I/O

units employed.

SUBROUTINE OEFUNIT

\

I

PROMPT FOR DESIRED "I"/0 UNITS I

(METRIC OR ENGLISM) I

UNITS ARE METRIC /

__J

OEFINE ENGLISM TO

METRIC CONVERSIONS

OISPLRY ENGLISN UNITS

FOR VARIABLES

RETURN

END

B-28
Rev A, October 1985

I. 2.3 CREATARM

Subroutine CREATARM is called within the system definition function to

provide control of the creation or modification modes for the simple

cylinder arm data file. The basic routines called for either option

are SPAN (define arm span), BASE (define base geometric properties),

BASELK (define base mass properties), JOINT (define joint), ACTUATOR

(optional, to define motor properties), LINK (define link properties),

and DEFSPJT (optional, to define special joints). If the user opts for

an end-effector, TOOLJT (define tool-joint properties) and TOOLLK

(define tool-link properties) are called. Graphics may be requested

during CREATARM.

B-29
ORIGINAL

"=vr POOR

Rev A_ October 1985

ZE

F-EWUWZH30

n"

_73

@

rr

HO

XP

i'

.J

W

11.

gln'

_0

Nil.

Qn'

..IN

.J_U

I-I

%J

_III:JJ

JtHmlFO)

o

rrlE I- '

Hid

[Z tr i

[HO

>t, I,@

(ft

Jl_jZJ

J

(In0hO

HHHO

.__ -'F

Z_" I.- 8@

.!=7 z_. -_-z

@ILI_

H

,_I_ "' ['_"Oil.>"

HQ_H=ZO

[E n.OO

(.,3 HZ

dleeHe¢¢ I-.

•" j@ JtL@

i_i(_H ). U,OH _) on

_u OH _-O_-

un $'H

Fj JE -_

H(.9, rrOE

b>. ol OlLJIL Z H Z I"1111"I H o H (_0 Wn _&W 0 @_

..1E

.J&

O0

bl- Iii" 3J L) (_ JJ _1

O0 "3U

oo_

JIJJ

JIJ

UI(J0 /_-

Zo

i"

.J8@WZH1EN@7

.JH

I-Z0

r3

"1

B-29a
1.2.4 RDARM

Subroutine RDARM is called from BLDARM to read from an unformatted arm

data file the data describing a single arm. Routines called by BLDARM

can then modify the data. The user is prompted for the name of the arm

data file to be modified. The following data are read from it:

1) Input/output units;

2) Geometric properties;

3) Mass properties;

4) Graphics data;

5) Actuator data;

6) Information on special Joints.

The user has the option of saving or deleting the old data file.

SUBROUTINE ROARM

PROMPT FOR NI=IME OF ARM OATA FILE

OPEN ARM OATI=I FILE

RI_'AO INPUT/OUTPUT I,_IN-rT8 DATA

REg_O GEOMETRIC PROPERTIES O;=_TA

REAO MI_SS I='ROI_ERTIES OI_TA

l_El=iO GI_I_PI-IICS OI=ITA

REI_D ACTUATOR DICTA

REI_O SPEC'I-RL ,_IOTNT OQTI=I

DISPLI:::IY MESSI::::I(_E THI:::IT I=ILL IDF=ITI=IHI::::ISBI=I=N RI='.I=:IO

I..1_1 Z

"N

DELETE

1_1._

SAVe" RI_M OI:=ITI:::I FTIE

OELETE ARM

I

SRVE _!LE

RETURN

ENI3 J

B-30
1.2.5

Rev A, October 1985

BLDDAT

Subroutine BLDDAT provides the user the capability to specify a more

detailed physical representation for the links of the robotic arm.

Components o£ the robotic arm system are defined by combinations of

geometric primitives. A number o£ detailed components can be included

for the base, each link extension and the tool definitions. The components are simple three-dimensional shapes: the cylinders, cones, rectangular solids, s3mnetric trapezoids, nonsymmetric trapezoids, triangular cross-sectional beams, rectangular beams, data tablet structures,

fillet components, nonplanar entities, and CAD/CAM objects. Unique

subroutines are called to handle loading the graphics object data for

the shapes chosen to represent a detailed arm. Additional shapes can

be added as required. The component type is written to the detailed

graphics save file if requested.

B-31
OF POOR QUALFFY

Rev A, October 1985

H

E

O

O

J

O0

W

Z

H

H

D

0

E

0]

D

B

5

U.

H

H

L_

E

/

h

H

H

W

C0

i

[]

WH

_zz-_Z

_O

H•oO

0 oz

b. _

-rN

O

_-J

fla.

N n.U.l

.J o or

H

TJ

0

0

E_Z_ H

o 7

_- 0

0

G

J

J i

O

I

!,

II'll

tl

Sl till

B-31a
i. 2.6 WRTARM

Subroutine WRTARM is called from BLDARM to save, in a user-specified

file, the data generated when creating or modifying an arm description.

The user is prompted for the name of this file and is also given the option of storing a formatted file containing the arm description for

later printing.

SUBROUTINE 61R TI:::IRM

F=F:_OMPT FOR NIClME OF" F"I'LE TO 1,4R'rTE F::II_M OF::ITI¢I TO

OPEN FTL_E

T_WR'r'TE FORMFITTEO IqRM GEOMETRY F'rI_E FOR PR'I'NTZNG

PROMPT ?OR _ OF _RM _OMETRY _INTOUT FZL.£

OPEN WI_E

_R_TE _RM GEOHETRY DESCRIPTION TO F_LE =

Z

CLOSE AND SAVE FILE

_RITE _NPUT/OUTPUT UNITS O_TA TO FILE

NRITE RRM GEOMETRY OATR

NRITE ARM MRSS PROPERTIES

NRITE RRH GRRPHICS DATA

NRITE RCTUATOR DRTR

NRITE SPECIRL JOINT DATA

OISPLAY MESSAGE THAT FILE WAS NRITT_N

CLOSE ANO SAVE FILE

RETURN

ENO

B-32
1.2.7 ENVlR

Subroutine ENVIR interactively establishes the basic properties of the

system environment during system definition. This includes the input/

output units, the gravity vector and the system span.

SUBROUTINE ENV'ri:_

PROMPT USER FOR GRI=IV'rTy FLF:IG. STF:=INI:3ARO

OR INPUT VALUE

READ USER RESPONSE

T_ USER REOLJESTEI::3GRAVITY STF:INDI=IRE! _F ,,

6 PROMPT USER FI:3A TNPUT ACCELERATION

'-- d DUE TO GRI=iV'rTy ::EN 1:3EFI=iULT UN::ETS

S REI=ID USEI=_ "r'NPUT {_RI=IVTTY VEE:TOR "rNTO

_! Gi=_FIV PFIRFIMETER

_ CI::tLL CVTUNTT GRI::IV TO CONVERT

. TO "rNTERNI=IL MATH LIN'rTS

PROMPT USER FOR SYSTEM SPF:::IN TN

DEFi::IULT UNITS

REI::::I[3 USER "rNPUT TNTO SYSSi:::'N '=_-

P I::::tR I::::IME T Ei:::_ -'_

CI:=ILL CVTUN'rT TO CONVERT SYSSPN TO

TNTERNI:::IL MI:::tTI--I UNTTS

_ETURN

END

B-33
1.2.8 RDENVS

The subroutine RDENVS is called from BLDSYS if the user wishes to include an environment in the system being created. This routine reads

the unformatted environment data file created by the system definition

function for the multiarm system. The user is prompted for the name of

the data file under which the environment data have been stored. The

file is opened and COMMON block ENVTBK loaded from the data file during

system creation. The file is closed and saved.

SUBROUTINE ROENVS

PROMPT FOR NRME OF FILE CONTRINING

ENVIRONMENT 0ATA

OPEN ENVIRONMENT DATA FILE

RERD INPUT/OUTPUT UNITS DATA

[ MEND NRNPNIC$ ORTN FOR E@CN

COMPONENT

O0 UNTIL a = NUMBER OF COMPONENTS

IN ENVZRONNENT

CLOSE FILE AND SRVE

OSSPLAY MESSAGE TN_T FILE N#S BEEN

RERO AND SRVEO

RETURN

END

B-34
1.2.9 RDARI_

OF POOR QUALr£_'

Routine RDARMS is called during the total robotic system creation for

each of the arms desired for inclusion in the system setup. The subroutine RDARMS reads the unformatted data file created by the system

definition function containing any one arm file. The user is prompted

for the name of the data file under which the arm data have been stored.

The file is opened and read into the following COMMON blocks: GEOMBK,

AMASBK, IOBJBK, TOOLBK, FORCBK, MOTORBK and SPJTBK.

SUBROUTINE ROI:::IRM$

SET I::_I:_OCESSSOI:_ MOOE m I. FOI_ SYSTEM OEW'rN'I"TION

PI:_OMI_T USI=I::_ I=OR I="I"I..m'NF=IMI=" Of=" S'I"NGLI=" I:=II=_M _"1"I,=_" TO F_I='I-'IO

OPI:'N F:::IRM DF::ITI::::IF-I-L_"

STORI:" SYSTF:'M UNT'TS -rN TI:'MPORF::IRY F::IRRI::::IYS

RERO UN'I"TS COMMON BLOCK -r'NTO SYSTEM COMMON

_ _--_OM_'rF&'Y' _MON 5t.._CI_ F_ _M=I_. _l'r_, RNO 1"001_ ZN'r_ _'Y_rrEM

_MMON

REI=IIO _ I_:_I:_.RI"_ES _ 9t_O_.K F'Olq _. ..ITS. _ND 1"OO1-- _J4TO

CONV_J:rr _ 1"0 _ UN_'r_ _ _ _

R_JI=IO ORI:=IF===I:]:CS Ol:lTl:l _MON BLOCK ".]_NTO SY_TB COMMON

CONVE_T l:::l_M _l=:l=:::IN TO ZNT_'_NI:::II_ WN'I"T_

_C_::IL _" I_:::tl:::ll:_'r_ OgJmmc'r OF:ITI:I BY (IRRM _Pf=lN / _Y_r_M _l:_l:lN)

R[I::IO I:=ICTURTOR DI=ITI::I C_MMON BLOCK "rNTO _Y_TI:'M COMMON

CONV_'RT F::tCTURTOR BRTR TO "I"NTERNI:=IL UN'I"T_

I:t_1:::1_ BI:=_I_'rI::IL ,.JOINT ol=rrl::l COMMON _I_OC:K _ITO _YBT[M COMMON

CLOSE S-I'NGLE RRM OATA F7-LE

I:_EbJI_'I"TE _YSTEM UN"rTS "r'NTO _YSTEM LJN'r'TS COMMON BLOCK

RF'TI_I_N

_'ND

B-35
1.2.10 BASES

BASES modifies the base location or orientation when including an arm

in a system; it is called from BLDSYS.

SLJJSROLJTINE 81:=:1S ES

WR=TE CURRENT BASE _IENTION VALUE TQ TERMINAL

PROMPT FO_ BASE" O_IENTATION MODIFICATION FLAG

T -- 0

00 WHILE T . LT. :3

I -- I _- I

P_m_.T p'_ = TH .OT_TmON _mUmNCZ _X=S o_ _OTR'r=ON

=,

REIqD ROTRT:Z::ON _I_I.JENCE RX'I'_ OP" ROTRT'J:ON "_N'RO =ROT

PNI3hdPT FOR "1" 'TI--I NOTNT'roN t::INGLE i -"3

, _._1

=

z

,R[R0 _TR'r'-J.J:_N RNQI_I[ _'biTO R_TRNQ _OR = T_ gI[OU[I_I[ _MmE]q

IPROMPT FOR 'LJSER INPLJT TERM!NNT_ON

D@ UNT=L USeR TERMIN_TE_ ROTRT_N 5E@UENEE =NRUT 1

_ET _NT_E_ M_TR:::r.X ELEMENT BR_O UPON =ROT MRTR=X EL._MENT_ J

i

L_D TEM_ MRTR=X N=TN _WRRENT _R=ENTRT=@N MRTR=X /

_LL ROTM_T TO =OM_UT[ R_TRT=_N MRTR=X, TEMR_

1:_3N_l'rENl_"rl[ _ RI:II" MR'rR:I::X ¢_L} I.,4:='TI.4 CURt_IENT FRO1" Hl=f'rPfcX ('I"EMP)

L_:::_R t''l ROTI=IT"_'PIN Ml::rrl::t_X. OR. t,.,l:Z"_ I¢:,IESULT::2:N_ TRI¢IN_PC)RM_II'O MRTR:Z:X

RETURN

END

B-36
1.2.11 SETUP

Subroutine SETUP calls SETUP2 for each arm in the manipulator system to

calculate the positions of all arm components in terms of world coordinates.

SUBROUTINE SETUI m

CALL SETUP2 TO CALCULRTE ALL

POSITIONS IN NORLD COORDINATES

O0 UNTIL KARM -- NUMBER OF RRMS IN

THE SYSTEM

RETURN

ENID

B-37
1.2.12 SETUP2

SETUP2 works every increment. It calculates the positions of all links

(including base, tool, and any held loads) and transforms link and centroid vectors to world coordinates. The recursive positioning method

described in the main text is used. Finally, subroutine JACOB is called

to compute the Jacobian for the current position.

SUBROUTINE SETLIP2

CF_-L 8ASI_.JT TO LOFIDBI=I_[ LOC. _NO_e-t_=_l_l_ON _r_P'm_

VRR_RSLE5

CALL MATMPY TO FIND JOINT N TO JOINT N_I VECTOR

CALL HRTMPY TO FZND JOINT N TO LINK N C.G.

VECTOR tHCG)

T'_ N . LT. NUMBER OF JOINT5 EFFECTOR) *I (NOT RT END

L JOTNT TS NINGE OR SNIVEl

CFILL CETH RND HFITHPY TO FOR $1m_.O'I"NI; JO'I"N "1" _T

GET JO"t'NT N TO N--I R..L. t ORTR I"N RRRIclY OR

TRF:INS. MRTR'rX (R,JL)

CRLL MRTMPY TO GET JOINT N TO

6JORL_D TRI:=INSFORMRT"I-ON MRTRIX

UI:_DaT I=" H-I-J FOR 5LTDTNI3 JO'T'NT5

__1

._J

Z

LIPDaTE POSITION VRR-rFtBLE POS

DO UNT-I-L N -- NUNBER OF" JOINTS * 1

CFILL HF=ITNPY TO GET HCG FOR ENID EFF'I='CTOIq

CRLL HRTHPY TO Ir'ZN(_ LOCFIT'rON Off ENO EFF. REF. FIT. IN NORI_D COOR.

I::IRM "I"S HOLDING F:I LORD OBJECT

UPDATE LOCATION OF HELD LOAO OBJECT [NULL)

CaLL JACOB TO CaLCULaTE TNE JaCOBIaN

RETURN

ENO

B-38
1.2.13 HARD COP

Subroutine HARD COP is executed when a hardeopy record of the current

Evans and Sutherland display may be desired. This routine queries the

user to determine if a hardcopy is desired and runs the appropriate routines to create a picture file for later translation into a hardcopy

plot.

(VCLR for HARD COP is not available.)

B-39
Rev A, October 1985

1.2.14 SYSGRAF

Subroutine SYSGRAF provides the system definition graphics capability

in the system definition function. SYSGRAF displays the environment,

target, load and robotic arm choices for building a robotic system

scenario. It takes as input through the calling sequence, the number

o9 arms in the system, a flag indicating the existence of an environment file for the system, a target file, and a load objects file inclusion indicator. It uses the system span input by the user to scale the

graphics picture. IFLAG controls the logical flow in the subroutine.

If IFLAG=I, the graphics system is initialized and displayed in the

initial condition; if IFLAG =2, the robotic system, targets, loads and

environment are displayed; if IFLAG=3, the graphics display is terminated. In the update mode, the environment is constant and therefore

not updated. As before, the Evans and Sutherland graphics routines are

used to provide all graphic capabilities.

B-40
Rev A, October 1985

ORiG_.!_,AL _:'.,:,_.7 _i"

DE POORQUAL_

_ (77nN)

i i i (7lnN)

z i z i z z

i

It-"oi_ I,o _ _ "

i°_ iliC_" 'Z g Z ' 9 ZO

°." !I

17 i>

B--_Oa
1.2.15 RDLODS

RDLODS reads in load data if the user requests that loads be included

in the robotic system under construction. The file read is the unformatted load file created by the system definition function. Subroutine

RDLODS prompts the user for the name of the file containing load data,

and then reads those data into COMMON blocks LGRAFBK and LOADBK during

system creation. The load file is scaled from load units to internal

system units, closed, and saved.

SUBROUTINE RDLODS

PROMPT FOR NAME OF FILE CONTAINING LOADS

DATA

LOADS ORTA FILE EXISTS //_F

OPEN LOADS OATR FILE

STORE SYSTEM I/O UNITS IN DUMMY

VARIABLES

RERD LORD OBJECTS INPUT/OUTPUT UNITS

DRTR

I=IEI " MI=ISS PROPERT'T'ES OF:iTI=I

ORO O'_J----E-C_'S METI=t'I-C I/O UNITS NOT

I

REI=ID LOAD OBJECTS GFtI:=II='I-IICS Ol::lTi:::::l

CLOSE I=INO SF::tVE F'rLE

RESTORE SYSTEM I/O UNITS

RETURN

END

A

.iJ

.i.l

z

w

B-41
1.2.16 LOCMOD

Subroutine LOCMOD is called from BLDSYS to allow the user to modify the

locations and orientations of load objects when building a system. The

current location is displayed and then the user is prompted for a new

location. The subroutine also displays the transformation matrix for

the current orientation of the load object and prompts the user for a

sequence of rotation axes and angles that define a change in orientation. ROTMAT is called to calculate the transformation matrix from the

user input, and MATMPY combines this new transformation matrix with the

old one.

SUBROUTINE LOCMOO

OISPLRY CURRENT LOCRTION OF LORD

OBJECT

PROMPT USER FOR RND RERO mN NEM

LOAO OBJECT LOCRTION

0mSPLRY TRRNSFORMRTION MRTRIX FOR

AXIS

USER INPUT OF ROTRTZON

RNGLE

DO UNTIL USER STOP FLAG OR 3

ROTATIONS

CALL ROTMAT TO CALCULATE NEN

TRRNSFORMATION MATRIX

CALL MATMPY TO COMBINE OLD AND

NEN TRANSFORMATION MATRICES

.._J

._J

Z

RETURN

END

B-42
Rev A, October 1985

1.2.17 WRTSYS

Subroutine WRTSYSis called from BLDSYS to write an unformatted file

containing the data needed to describe a system. These date include

user units, moving base £1ags, arm geometry, mass and actuator properties, special joint data, arm graphics, environment, load object and

target properties, tool data, and load object and target graphics

information.

B-A3
Rev A, October 1985

W

Z

H

b

3

0

OE

rn

_J_.

• II _-Z

"_ 0 _)H

>-

X

Wz_ZF

nwwH

Ojn Z

_HO D

W

@0 H

_w

CS

pie

_L

8

@W

>nO

HJ

(J

@Z

IIH

J

h

W Im>

if,

t_

Z

>

il.

P

Irr

Oh

9

n- n. _i

zx

v

I

u

(I

v

!1.

3

2

C

\

i

I

i

|

I

Z

¥

zzi 

]]o 8

88,,,

¢,,, _

b II _ I1:

u o. _-

OE,. utr

N 0 I1.

_ O_°_O • b

n Hi@ _-

_o

I- hi_ w W

H HI- _- h

i-_

_r

o

E

h

E

O

@

O

H

I

ee

®

@

O

"3

0

hl

H

nr

"t

z

7

_w

,,>.

b.

oW

U

NO

-rj &

_O

@

U

@

E

w

h

H

n,

3

i

I

I

II

_UI

,=b!

fi-_3a
1.2.18 RDENV

.... 5.v_.,_ RDE_g reads an unformatted environment data file durin E

the system definition function. The conTenT of the file is the pertinent COMMON block definin E an environment for The robotic system. The

user is prompted for the file name from which the file is to be read.

SUBROUTINE RD_NV

PROMPT USER FOR FILENAME OF

ENVIRONMENT FILE TO RERD

OpEN ENVIRONMENT DATA FILE

READ UNITS COMMON BLOCK

READ ENVIRONMENT GRAPNICS DATA

COMMON BLOCK

CLOSE ENVIRONMENT OATA FILE

RETURN

END

B-44
1.2.19 DRWENV OF' POOR QUALITY

DRWENV is called within the system definition function from BLDENV to

provide graphics display during the generation of detailed environment

graphics representations. It is called to display each successive environment component as it is defined.

SLIBI:::t OUT'r NE OIRIAENV

SET _ROCESSOR MOOE = I. FOR SYSTEM OEFINITION

ZNITZALZ_ING

m_T _ _oR _v_ _ {NULL}

SET SCALE FACTOR, _FACT -- IOOO./ENVIR. SPRN

TN-r T-I'F::IL T :_T NG o-r SPLI::IY

I_llO D_N "rli_ulLlrl-_ON IIINI_ Iqgl'lli'l-_@N _ll.l.llll @llrl- _ ll11_cz_{lll -rll@lll, i_ "1"1_ zD_'-_--I-Y

SET W'rNOOW BOUNDRR'rE5 =,w_ _ m = _ = ==q.s

DRONING OR REPLRCING COMPONENT /_

SET N_NDON BOUNDRRZES

SET CURRENT "rNTE_ER TRRN@LRT'rON RNO ROT. VRLUES

SET NUMBER OII P--OMI:_ONENTS =N ENV'I'R. ImRRRMETER

DO FOR I=ILL COMPONENTB BEFORE CURRENT COMI ='.

..J CI=ILL 03ClFITI=I TO EI'I'31=_LFIY COMPONENT ..--I

=

_ _ _ _" _ _ __ _ Z

CI::=ILL 03DF::ITF:I TO o'rSPLF::IY COMPONENT

C,_UN"rER FOR LR_rr IENV. I:I_,JIE¢T I¢IAI_Ry /(:_---'_TZ{3N IJ_EI_

CLOSE AND I::::_EPLRCE SEGMENT

RETUI_N

END

B-45
1.2.20 WETENV

Subroutine WKTENV writes an unformatted environment data file durin8

the system definition function. The content of the file is the pertinent COMMON block defining an environment.

SUBROUTINE I,-IR TENV

PROMPT USER FOR FILENAME OF

ENVIRONMENT FILE TO WRITE

OPEN ENVIRONMENT DRTR FILE

WRITE UNITS COMMON BLOCK

NRITE ENVIRONMENT GRRPNICS DRTA

COMMON BLOCK

I .

i

t

CLOSE ENVIRONMENT DATA FILE

RETURN

END

B-46
1.2.21 RDLOAD

The subroutine KDLOAD reads an unformatted load objects data file during

the system definition function. The contents of the file are the pertinent COMMON blocks defining a load file for the robotic system. The

user is prompted for the file name from which the file is to be read.

SUB_OUT'rNE RC)I._OACl

PROMPT USER FOR FILENAME OF LOAD

OBJECTS FILE TO REAO

OPEN LOAO OBJECTS DATA FILE

READ UNITS COMMON BLOCK

REAO LORD OBJECTS MASS PROPERTIES

COMMON BLOCK

RERO LOAO OBJECTS G_APHICS OATA

COMMON BLOCK

CLOSE LOAO OBJECTS DATA _ILE

RETURN

ENID

B-47
Rev A, October 1985

1.2.22 LOAD

Subroutine LOADis called during the BLDLOD option of INITDRVR. It

allows the user to create and define the mass properties of one or more

load objects. If a file of load object data already exists, this subroutine may be used to modify portions of those data. The load parameters for which the user is prompted are listed:

1) Load object name (up to 8 characters);

2) Location and orientation with respect to the world coordinate system;

3) Length and radius;

4) Center of mass;

5) Mass;

6) Inertia distribution;

7) Grasp point and approach vector in load local coordinates;

8) Mass and location of any point masses included.

B-48
..... ,,,-L P_ 13

OF POOR QUALITY

Rev A, October 1985

(77nN)

0

E

0

J

n 8

ITII1NI

30l m'm m _md/m_ I ,,,,,

O,Ol_ d01S am'rim

!U3_ _m_

:]HUN13"-]i"8)@07UO_U:iOllIdHOUd

B-4.Sa
1.2 • 23 TLDtO.S

Subroutine TLDMAS is called from BLDLOD to add the effects of point

masses included in a load object. Variables for total mass, centroid

location, and rotary inertia are initialized with the values for the

simple load object. If point masses are included, ADDMAS is called to

calculate new values for these variables that include the point mass

effects.

SUBROUTINE TLOMA$

• NITIALIZE TOTAL LOAO MASS. CG,

ANO INERTIA _ITH SIMPLE LOAO ORTR

NUMBER OF POINT MRSSES FOR

CT .GT. 0

MASS ORTR

DO UNTIL IP -- NUMBER

OF POINT MRSSES FOR

CURRENT LORD OBJECT

---I

.--J

Z

w

RETURN

END

B-6,9
1.2 •24 DRWLOD

ORIGINAL PAGE 13

POORQUALITY

DRWLOD is called within the system definition function from BLDLOD to

provide graphics display during the generation of a detailed load objects file. It is called to display each successive load object compohen= as it is defined.

SUBROUTINe- DRWLO0

SET _ROCES_OR MOOE = I- FOR SYSTEM OEFINITION

INITIR_I_ING '

SET SCALE F_CTOR. IFRCT = 1000: /LOAO SPAN

INITIALI_ING OISPLRY '

I"RRN@LRTm_ON RNO CRI.J_ O'J:RL@ TO _FI'RTLI_ RNRLO@ CONTROl= OZ=IL.@

I=i0"1"1==1"1"_..¢_NVl=ll_t._ _ CJ.IRI=_NT ZNTIESlEF_ TR_R'I'3:¢_I I=IND RIOT. VI=It.U_8

ORAWING OR REPLRCING COMPONENT "" /_

_" mmm_CS_Mmm.'ON _Z_ ON _ m_ Om:_.NTm'C'ZON O_ t.OmO

SEIT _INDOW 'BOUNDRRIE$

_ET CUmREN7 mNT_ER 7RF_N_RT'VON RNO ROT. VF_t__JE_

'=_w _ c_o,_'_-,.m ,_xcs sYS_'_ _._-×. _,_'-Y. __u-_

SET NUMBER O_ COMPONENTS IN LORO _::_RRF::IMETER

_O FOR FILL COMPONENTS BEFC)FIE CURREb_T COMP. ]

sL---r_-,-cs FLRr-_ _Om _=UENTZRt. RNO _=¢T,'RNmrZN_ PT_.

CI:=II--L 1_30RTI=I TO O'rSPL_:_Y C:OMPOI_EI'_IT ::_z

Cl0UNTER Fl_ I..R_T _ O_I__.T RRRRY _--RT_N _'

'C_LL. 'O:_DF:::ITF:::ITO O_SPl 'RY COMI_ONENT

SET COUNTER FOR _ LORO OBJECT RRR_Y _T'I_ON USED

CLOSI=" _NO RI_PLRCl=" SI='GMI_NT

_E'rU_N

ENO

B-50
i. 2.25 WRTLOD

Subroutine WRTLOD writes the unformatted load objects data file during

the system definition function. The contents of the file are the pertinent COMMON blocks defining the mass properties and graphics of the

loads for a system.

SUBROUTINE WRTLOD

PROMPT USER FOR FILENAME OF LOAD

OBJECTS FILE TO WRITE

OPEN LOAD OBJECTS DATA FILE

WRITE UNZTS COMMON BLOCK

WRITE LOAO OBJECTS MASS PROPERTIES

COMMON BLOCK

I .

WRITE LOAO OBJECTS GRAPMICS OATA

COMMON BLOCK

CLOSE LOAD OBJECTS OATR F_LE

RETURN

F'NO

B-51
Rev A, October 1985

1.2.26 RDTRGS

RDTRGSreads in target data if the user requests that targets be included in the robotic system under construction. The file read is the

unformatted target file created by the system definition function.

Subroutine RDTRGS prompts the user for the name of the file containing

target data, and then reads those data into common blocks TGRAFBK and

TARGBK during system creation. The target file is scaled from target

units to internal system units, closed, and saved.

B-51a
Rev A, October 1985

(77nN)

0

B-51b
Rev A, October 1985

1.2.27 LOCTMOD

Subroutine LOCTMOD is called from BLDSYS to allow the user to modify

the locations and orientations of target objects when building a system. The current location is displayed, and then the user is prompted

for a new location. The subroutine also displays the transformation

matrix for the current orientation of the target object and prompts the

user for a sequence of rotation axes and angles that define a change in

orientation. ROTMAT is called to calculate the transformation matrix

from the user input, and MATMPY combines this new transformation matrix

with the old one.

B-51c
Rev A, October 1985

(771N)

Z

__x£ 7

oO_Oo__z z :z = _z _I

II "'_ _ ! __ _o_ z

EH _.9___1_ o x4

hCl:i _ U

= = _--" I= I

B-51d
Rev A, October 1985

1.2.28 RDTARG

The subroutine RDTARG reads an unformatted target object data file

during the system definition function. The contents of the file are

the pertinent common blocks defining a target file for the robotic systea. The user is prompted for the file name from which the file is to

be read.

B-Sle
Rev A, October 1985

D

_z

_o

O

L

Z

O

o

m

_9

_z

o8

H

il

_Z

FO

--10

W

rr

F

W

[9

n"

W

d

z

2

zo

W

S-51f
Rev A, October 1985

I. 2.29 TARGET

Subroutine TARGET is called during the BLI)TRG option of INITDRVR. It

allows the user to create and define the properties of one or more target objects. If a file of target object data already exists, this subroutine may be used to modify portions of these data. The target parameters for which the user is prompted are the location and orientation

with respect to the world coordinates.

B-51g
Rev A, October 1985

h

W

@

E

E

h

hl

Z

H

H

3

0

E

rn

hl

_ o °_

IIIIlZIll _oHZ _ _ _x

_W

u)O _-Z

H

H

n _oOz _ F__H3n" i_

3HuED _rr_ HEO OOOE

E 0 FOH lhE

pWO0 E_ E_

& 8 nOH Z b U_

0 Ob

_O. ,, o_o O_IOC _ _

z

(llnN) 5

-]E @

[O_ 0I "'

_ o , x_,• i_-,_

_z

•"o [_zZ W II

,,,

z _-_- I_0:z

,_:oo,,o,_ __i_o_ mH°',_- __H°

In'J(]:

&E z- 1 E_"

gH u iz_ _o_ I,o_ '00 i

_-m In'n"iZ • u.

__ __o O @

In D_

o__ _ _o°°_

B-51h
Rev A, October 1985

1.2.30 DRWTRG

DRWTRG is called within the system de£inition £unction £rom BLDTRG to

provide graphics display during the generation o£ a detailed target object £ile. It is called to display each successive target object component as it is de£ined.

B-51i
Rev A, October 1985

o_

o•

Z:_

H

!-

0

O: I

rn

_o

o•

O

o

I1

(717N)

(3 _.

5

ill 0

'_ ,u

o

o

_o _z

_o

_a a

A
Rev A, October 1985

1.2.31 WRTTRG

Subroutine WRTTRG writes the unformatted target object data file during

the system definition function. The contents o£ the file are the pertinent common blocks defining the mass properties and graphics of the

targets for the system.

B-51k
Rev A, October 1985

B-511
1.3.1

Rev A, October 1985

GRINIT

For building an environment, simple arm, detailed arm, targets or loads

with graphics, routine GRINIT initializes the E&S display, extended

switches/lights, and analog control dials, and draws the graphics display border. Working £rom the input £1ag IFLAG, the type heading of

the general display is chosen, either simple cylinder, detailed geometry, environment, target or load. The graphics segments are opened and

the title for the system de£inition £unction driver currently under

execution is output.

B-52
Rev A, October 1985

I

_H

'I!i

22

2 1

B-52a
1.3.2 SPAN

The manipulator arm span is requested as input from the user during initial creation of simple cylinder arm data; modification of the ARMSPN

value is also allowed through a call to SPAN during the CREATARMmodification mode.

SUBROUTINE SPAN

ERROR CODE ---- 0

T_ SYSTEM MODIFICRTION FLRG IS SET

PROMPT FOR SPRN

VRLWE MODIFiCRTION (NULL)

FLAG

I::_AOMI::_T FOR NEW SPAN.

OR REACH° OF RRM

INPUT MOOIFY FLAG . NE. I

READ S_N _NTO RRMSPN

PRRRMETER

(NULL)

RETURN

END

B-53
1.3.3 BASE

ORIGINAL PAGE E-S

OF POOR QUALITY

Subroutine BASE is called within the system definition function during

definition or modification of the simple cylinder arm or detailed arm

geometry file. The purpose of subroutine BASE is to provide the input

of the robotic base position, orientation, and physical dimensions

(radius of base, endpoints and number of sides), and to load these

values into COMMON blocks.

SUBROUTINE BI:qSE

_ET MO_E _L_G _ i

_T _RROR C_Q_ _Q m O

RERO MOOIF"_CRTION CRTEGORY OESZRE_

B-54
1.3.4 OBJECT

ORIGINAL PAGEtS

OF POOR QUALITY

Subroutine OBJECT creates simple cylinder graphics data used by the graphics package to draw the robotic arm during the system definition function. The data created in OBJECT are stored in COMMON block IARMOBJ,

and represent a right circular cylinder of the specified size for each

system component (the base, each link and the tool). It is called for

generation of each of these components in turn.

SUBROUTINE OBJECT

I SET _RROR COnE FL_G -- O

SCF_--_NG FRCTORo _FRCT. = IOOO. /RRH SPRN VRLIIE

l OOCRSE

INPUT JOINT/LINK _OUNT_R. _N. = 1 ' @_,_._

@L_gF _ @P @OP_wOe'_rFS _ (_Jq_:4ff _ M_@qY MJD4=qT _ L

@Rg" @B,.I_"? W _ @l'Imm@'r_ @mjDgg4rG _ _ 1

CRLCULRTE NUMBER OF LINEs TO ORRW

It'T 6_II M HaDE R.MOe tn'@_ CDI,PgmZT=Re _

_ _ AMSqmWo :FAItNaBJ(t,e _ NNJktlq _

QlUU_J

=,

INCREMENT ICNTRRM BY 3

_ mml.,llUl gUlqll..IE lm_ul _P_N U @p' _ _4mL.I _

SET QNGLE OFFSET PQRAMETER

----'1 : _il / ,,---,,.

_ Im'm"_ _. _

-] _____,._ _

[t._M_NTI F_

SCRLE L.INK RRDIUS BY IFRCT

O0 FOR EI=ICH CYL_NOEN END c'rRcLE

SET X -- ENDPTS SCRLED BY IFRCT I

_4pnq'R_ i _RLJJ_ _ tm_=_sT Pq8 _ Plqa'JE t t

CY_

CON_ECTZNG

;ILI"E;_NI=rI'J:NG

po'?'Nl"$

INCREMENT

' I_NTRRH QNO

• NOEX VRLUE FOR

L_. OF NEXT

QRRRY :OMPONENT

! _ETURN ,

B-55
1.3.5 GRAPH

ORIGINAL PAGE f3

OF POOR QUALIFY

If the graphics are initiated during the simple single arm creation or

modification, subroutine GRAPH provides the graphics capability for the

simple cylinder representation of the robotic manipulator. GRAPH displays each base, joint/link combination and tool as they are defined.

Graphics during the modification mode is handled with calling arguments

input to GRAPH; appropriate deletions, additions and changes to the

links are visually depicted. GRAPH provides only the simplified robotic

arm definition display.

S U E_P:_Ct__ T__T.NF: @ RQPl-I

sE'r F='rcTuRI[ _qO(:_SllOA TRIqN_ 3"0 :£DI[NT_"I'Y

Sl['r ._NI'EOI[R 3_=II=INaI=.A'rZON _ ROTRI_J_N VI_..UES

S_['I" _¢)_ BOUNO_q_

_q_. / _ _mm_aw m

i 5CRLE ,JT. L.OCRT'_ON BY CFRC'T RN r_ LOR r'_ ZNTO "I"NTE_ER RRRRY

CRI..t- TTI_RN FOR P_'CTUR Er PROCF_OR T_RN_FORMRTION MRTR_X

EXTRRCT OFF"_ET JT. RN_. FROM JOTNT VRR'I"RBLE F_RY

CI=ILL. TROTX t_R --Y t,,,I'I"TH JT. l=k"M_. TO mOTI=ITE TI=;q=IN_FORMRT'_r_N

I_X_m¢T RO=rRT_N mX[_ U_[_I _ OR_'_NI--_'N_. FROM JO'rN'r _E_UENCE I=_RRY

CRLL. TROTX.--Y OR --_ N_Y't'H ZNTE_----_ZR RNr--_. TO ROTRTE TRRN_P"OF:_tR'r-/._N

_ET NUMRR _F COMF:_ONENT_ ZN L.'T'NK F_RRRMSmTER

_Q _O_ ERCH Ce'_M_='t'_P,_NT IN L'rNK

/

LORD RF_I _S_. RRRRY FOR _=IR_H_CS FL,c_GS

L.ORr9 F:_M OB_J. RRRRY FOR _Et_UENTZRL. ;::_O'_NT_

LORO RRM _,.J. RRRRY FOR Rt.T_RNRT't'N@ F=M_'NT@

r_.FILL D_RTR TO r_ZSPt.RY L_'NK

i

SET COUNTER FOR LI=IST RRM OBJECT RRRRY LO{=R'T'_N U=_E_-, I

B-56
1.3.6 DIALS

DIALS is called to scale the Evans and Sutherland analog control dials

values read during camera perspective changes via the extended E&S

dials. The values are scaled to integers between -32767 and +32767.

SUBROUT3_NE I_IALS

I TRRNSFER GRRPNIC$ RNRLOG CONTROL

DIALS DEVICE RECORD _ROM QUEUE

DO

!

FOR ERCN CONTROL DZRL VRLUE RERD

VRLUE .GT.

I SET VALUE TO

--6_53'_I - _- VALUE

_2767.

[NULL)

SET VRLUE TO

655S_ _ VALUE

--32767.

(NULL::;

RETURN

I

END

B-57
1.3.7 JOINT

ORIGINAL PAGE 13

OF POOR QUALITY

For JOINT, the user inputs joint type, joint location as Cartesian coordinates in Cerms of the coordinate system of the previous joint (or

base, if the current joint is joint I), joint orientation as a rotation

sequence of axes and corresponding angles with respect to the previous

joint coordinate system (or base, if joint i), and initial joint state

(initial angle for hinge or swivel, or initial length for sliding

joints). The x-axis of a joint coordinate system is directed along the

centerline of the link between joint i and joint i+l (or end-effector

if the current joint is the final joint in the system). JOINT is called

by CREATARM during initial creation or modification of arm data.

SUBI::_OUT'rNE JOINT

B-58
i •3.8 ACTUATOR

ACTUATOR allows the user to define or modify the COMMON blocks defining

actuator properties for the am by interactively prompting for actuator

parameter values or by reading a previously constructed file of actuator

parameter values. The user can opt for no actuator definition if

desired.

m

i

__..,,.

I I

'II

i

!i

B-59
1.3.9 LINK

ORIGINAL PAGE IS

OF POOR QUALITY

In the create mode of LINK, the user is prompted for link endpoints in

coordinates along the x-axis, link radius, the location of center of

mass as the Cartesian coordinates of the center of gravity in the coordinate system of the joint at the "base" end of the link, link mass and

inertia matrix relative to the centroid and the number of sides for the

desired simple cylinder.

SUBROUTINE LTNK

|

B-60
1.3.10 DEFSPJT

This routine interactively establishes the number, type and location of

"special joints." These are joints for which a constraint is placed on

the relative joint displacements.

SUBROUT_N_ O_FSPJT

PROMPT FOR NSPJT -- TRE NUMBER

OF SPECIAL JOINTS IN THE ARM

OO FOR EACH SPECIAL JOINT

PROMPT FOR TYPE OF SPECIRL

JOINT RNO SET ISPTYP

PROMPT FOR WHICH JOINT OF

ARM THE SPECIAL JOINT I5 RNO

SET NJTSP

B-61
1.3.11 TOOLJT

Subroutine TOOLJT is called by CREATARM to interactively define or modify the geometry properties of the manipulator end-effector. The data for

which the user is prompted include:

I) Location of tool with respect to final llnk;

2) Orientation of tool with respect to final llnk.

The orientation data are input as a sequence of rotations about coordinate axes and ROTMAT is called to compute the corresponding rotation

matrix.

SUBROUTINE TOOLJT

INTT'_AL=_E AOTAT=ON SEQUENCE C'I"I:qOT)

TO _E_:_O TNOTCF:ITTNG NO ROTATIONS

T_ MODIFYING PREV'TOU_ OATI=t /_F

SUI_C_'_55"rv_"LY I_II_OMI:::_T I='O1::_ X, Yo F=INO

LOCF:IT'T'ON OF TOOL L,J-rTH RESPF'CT TO

I::mI_ECEnTNG L'rN_

"I-R _ 0

z _

_ ?-,--,

1.1_ e-, I.i.J

,--,m,, _ CE

'--"" _ I_Z

I,J.m -'rI,.6. _ I----

I.I.J

,.,,,

?",-" e-r"

e",

?we-;

i,i

._.J I,----

IR = IR_I

ENTER _ROT CIR] -- COO_O_NATE RXIS

FOR ROTRTION

ENTER AJTANG (IR) -- RNGLE OF

ROTATION

ABOUT T_IS AXIS

PROMPT FOR USEM TERMINRTION

DO UNTIL USE_ TE_M_NMTES O_ _ --

SET JNTSEQ -- _OT (1) I_OT (2_ _OT (_)

C_LL ROTM_T TO COM_UTE _OT_T=ON M_T_X

C_R_E_POND=NG TO TH=S _TQT=ON SEQUENCE

F--_ C=

Z

z

I.,z.J r-r"

B-62
ORIGINAL p_=

.O_ POOR QUALITy

i • 3. i2 TOOL_

Subroutine TOOLLK is called by C?J_ATAI_ to tnteracttvely define or modify the mass and graphics properties of the manipulator end-effector.

The data for which the user is prompted include:

I) Endpolnts for cylinder representation;

2) Radius of cyllnder;

3) Number of sides of cylinder;

4) Center of mass of end-effector;

5) Mass;

6) Inertia distribution;

7) Location and mass of point masses.

When modlfylng _xlstlng data, the user has the option of whlch categorles to modlfy.

=! I"

_......_I "

_ a,. I

EN=

B-63
1.3.13 TOTMAS

Routine TOTMAS combines individual components of each link's contributions (e.g., link mass, point masses) to obtain a total mass distribution for the joint/link combinations and tool during robot arm creation.

Variables for the total mass, centroid location, and inertia distribution are initialized with the values from the simple link. If point

masses are included, ADDMAS is called to add the effects of these additional terms.

SUBROUTINE TOTMRS

INITIALI_E TOTAL MASS. C.G.. AND

INERTIA VARIRBLE$ WITH LINK ORTA

......... i

NUMBER OF POINT MASSES I

FOR CURRENT LINK . GT. O

CALL ROOMRS TO

INCORPORATE POINT

MASS OATA

DO UNTIL IP = NUMBER

OF POINT MASSES FOR

CURRENT LINK

__I

__J

Z

w

DO UNTIL N = NUMBER OF LINKS IN ARM

B-64
1.3.14 ADDMAS

Subroutine ADDMAS combines the mass properties of two objects to obtain

composite values for the mass, centroid location, and inertia distribution. ADDMAS calls ADDMAS2 to perform the computations. ADDMAS then

loads the results into the first object's mass property variables.

I=UT MI:I$S I_I_$ULT$ INTO MI_SS Ol _" SOOY

I

PUT C.G. _ESULTS INTO C.G.

I

O_ SOOY

PUT ZN[RTZR MRTRZX ZNTO IN[RTI_

MRTRIX OR 800Y 1

RETURN

ENCl

B-65
1.3.15 ADDMAS2

ADDMAS2 calculates the composite mass properties of two rigid bodies

joined together. The mass, centroid location, and inertia matrix for

the composite body are returned as results.

SUBROUTINE AOOMRS2

TOTQL NRSS = MF:_SS OF BODY I ÷ MRSS

OF BODY 2

CONPOS!TE CENTROID = (MASS i * CG

1 _ MASS 2 * 013 23 / TOTAL MI=ISS

R 1 = CG 1 -- COMPOSITE CENTROZD

R2 = CG 2 -- COMPOSITE CENTROID

CALL RC:EOF_ FOR R1 SQUFaRED MATRIX (F_ISQ)

USED TO F'['NE) COMPOSITE :ENEI:_T:EF:::I

CF:ILL RC:ECR FOR 1_2 SQUF:II:::tEE3 Mf=ITi_3:::X (Ft2SEI)

USED TO I=:ENE} COMPOSITE :_NEF:_TII=I

COMPOS-r'TE "TNERT-r_:a = R-EN 1 + f:1-rN2 _-

(MASS 1 _ RlSQ _ MFaSS 2 _ R2SQ:}

RETUF_N

END

B-66
1.3.16 RCICR

Subroutine RCICR is called by ADDMAS2 to set up the inertia matrix

corresponding to a point mass displaced from the body centroid. This

inertia matrix forms one component of the inertia distribution for the

composite body.

SUBROUTINE _C!C_

TR = O.O

I INCREMENT Z

I I INCREMENT J

8QUARIZI2 MI=ITRTX ELEMENT, CRSQ (T, j) )

I = --_ CT) _ R (,..I)

OO UNTTL_ J = 3

T_ ---- TR -- RSC_ ('1"o "1")

CIO LINTTL I-., 3 "

i "I'NCRIZMZNT T

12TI=IGONI=IL _'I_EMI=NT, .RSQ (T, "I-) m _SI_ (T. T)

I "*" TR

O0 UNTTL "T" = 3

B-67
1.3.17 GRTERM

GRTERM is called to terminate the Evans and Sutherland device processor

display unit. It calls MPSTOP to terminate the multi-picture processor

display unit graphics.

SUBROUTINE GRTERM

CALL MPSTOP TO TERMINRTE GRRPMICS

SYSTEM

RETURN

END

B-68
1.3.18 BASPUT

Subroutine BASPUT is called from subroutine SETUP2 during position

calculations for the manipulator. This subroutinetakes the position

and orientation of the base of each arm (with respect to the world

coordinate system) and loads these data into the arrays POS and ROT.

SUBROUTINE BI::ISPUT

PUT ARM BASE LOCI:::ITTON TN ARRAY

POS

PUT ARM BASE ORIENTATION IN

ARAAY ROT

O0 UNTIL J -- 3

O0 UNTIL I -- 3

DO UNTIL KARM -- NUMBER OF ARMS

RETURN

END

B-69
1.3.19 JACOB

Subroutine JACOB sets up the Jacobian matrix that will later be used to

solve for individual joint velocities for each arm given the end effector velocity. This subroutine uses end-effector position and joint to

world transformation matrices to determine the entries of the Jacobian

as described in a previous section. The result is a 6xN matrix for each

arm.

SUSROIJT'I'NE JACOB

CRLL. MATHPY TO I¢I_T REF I_O_'NT VECTOR "f'N WORLD COOI=_)'rI_RTES

ROD VECTOR P'ROM WORLD OR'rG't'N TO IINI-I wlmlB'ECTOR OR"I'G'TN

F-l-NO ,..IOENT F::IX'I'S OF ROTF:::ITEON

T_ JOINT IS NINGE

F"rND O:ZRECT'J:_N C@S'rNE5 _ ,,JOZNT

Rx'rs b4. R.T. _I..D COOR. (R (3))

CRLJ- CRPO TO I'_ND RM w Iq X (VECTOR

ENO EP1 r. I:=tEF. PT. TO _JO:T.N_

F_JlclCOB (1. JT) I RIll (1)

RJF:ICOB (3. JT) -- RL4 (3)

RJF=tCOB ('q-. JT) ----" g:l (1)

RJF:=ICOI_ (5. ,_IT) -- A (:23

RJRCOB C8. JT) = F_ (3)

OR Sl,,4"'r VIEI_

RJRCOB ( 1 • JT_ I

ROT (1. 1, JT. KRRM)

RJI:ICOB (2, JT) --

ROT (2. I, JT. KF::IRM)

RJi:ICOB (3. JT)

FtOT (:9. 1- JT. KARM)

RJF:ICOt3 ('q-. JT_I

_- O.O

RJACOB (5. JT)

m O.O

I:_JI::tCOB [8. JT)

= O.O

rio IJNTTL JT = NUMBI=R OF JO"I'NT$ -r'N F:=II_M

F:_I=TUI::_N

_-NO

B-70
Rev A, October 1985

1.3.20 DATOUT

DATOUT is responsible for the data output in the columns set up by subroutine FORM of the E&S robotic simulation display. It includes the

current simulation time, joint travel angles, percent of the maximum

traveled for each joint and task commands. DATOUT has provisions for

only two arms.

B-71
Rev A, October 1985

X (771qN)

D

0

0

W

Z

H

D

0

nO3

O

i

H

n"

51

2_

sl

U.

btu

>_

I1.

O_

U

O

H

.J

a.

N

N

'IH

O

= £

_=_ o _: o: _:_ u

n _-IL I LI

1 _:_ _z_ _£ _ _ _ ,,,_ o

o

R x: XI §_/,I

B-71a
Rev A, October 1985

1.3.21 FORM

The FORM routine sets up the borders and the text output locations £or

the manipulator display on the E&S graphics unit. It sets up the Evans

and Sutherland graphics display borders and outputs the robotic simulation title, current simulation time text title, joint travel status

data column and task command headings. FORM has provisions £or only

two arms.

B-72
Rev A, October 1985

0 _0

LD rr

2_ g

U___ _-_

o E

coo _

>____E

0

_°_S s -]

Q D

Q 13..

I-

' Z)

i

I

L Q

Z

O]

0

(_

LO

S

8

S

S

d

S

S

H

D

n

H

D

0

I

•o•

U.

o

H

Z

-)

hi

\IL

O]

8

,/Ii

I

q

_-0

_J_-

I,I.Ix

>bJ

I-if)

I-Z

Zl-t

11121

O_

-r

I..-

(_9

Z

-1-

S

o_

¢,

S

S

0

D

D

0

I

i

i

LO

LO

o

L)

H

3

O.

H

0

Lfl

B-72a
Rev A, October 1985

1.3.22 CYL

Subroutine CYL is called within the system definition function during

detailed graphic representation generation for the robotic system constituents (environment, arms, targets, loads). I£ the requested component is a cylinder or cone, it is called to compute data points for

the graphic routines. The controlling argument in the call, ISHAPE,

determines which geometric shape has been chosen in calling routine

BLDENV, BLDDAT, BLDTRG or BLDENV. The detailed graphic component dimensions are written to a print/save file for archiving the program

interaction.

B-73
Z

H

P

D

0

g

0

ORtGtNA_ pAGE., IS

of poor qu_rPe_

B-73a

i
1.3.23 RECT

Rev A, October 1985

Subroutine RECT is called within the system definition function during

generation of detailed graphic representations for environment, arm,

target, or load objects file. It is called if the requested component

is a rectangular solid (ISHAPE = 3), a symmetric trapezoidal solid

(ISHAPE = 4), or a nonsymmetric trapezoidal solid (ISHAPE = 5) to compute data points for the graphic routines. The detailed graphic component dimensions are written to a print/save file for archiving the

program interaction.

B-74
_p_m

Rev A, October 1985

F

U

W

ill

Z

H

D

O

n"

0O

D

!81 '

Ii17

il

l-

 !li i

I ild '.-_

i|

_'.

L _1

i!,

I I

I

I

II

I

!|

'1

ll"

i|

I--t

rrl

HI

i

1

'i

• I

i,i

I,

Id t

@,_

,,,.i 't

v

I

i i

I

t

> ''i I 3i

i..I

w iJ Ji

13

Z , d

"r I1:_

r} _-i i

Il ui

"_ ul HI

.rml :E!

H_i •

"_B} Z

O

_'_ Z

_-_._l_

w_ Iz mi_

[ i_wH,-

i I_ II

I, ,_ IIII

I I}1 !_

I I--

\t _,IL.

I

I

i

i

u _

E

> 3 3

hl J J

@ "I :3

Z Z

I

Oz. ,:

Ell. E

.rE E

Nit /1"

"' 1

-r _E__

H> Z "_ 0

N; ,

¢ !|

i

ui

]i

E

>,

I

13

Z

Ir

H

I

7

¢

Id

W

>

n

_1

t

I

I : I

,I

]

rr

,., ., > ,-] ,.,,

-, _ ,," ._ _!

I- iO H I" LI

!H "_U H

t- IU,l 11" _1

2

n

g Z E Z

oi, E (o. rr

._Oii '0'

H "r_.

/.!o J/I j

hi

D

J

E

>

h

.i

IL

IL

0

F

z

b,I

J

I1.

f

11)

t_

¢

I

rr

_1111 I

_Ii1_II

|111111

il_lll I

ll|lt! I

|lil_l I

Zlillll

llellll

il9111_

IIIlll I

_11111 I

_111111

_lllll I

i]l l_

l i ill

it Ili

IIIll

11118

B-7_a
L

:.. _ _m_ __

Rev A, October 1985

1.3.24 TRISTR

Subroutine TRISTR is called within the system definition function

during detailed graphics representation generation for the environment,

manipulator, target, or load objects. If the requested component is a

triangular cross-section beam (ISHAPE = 6), it is called to compute

data points for the graphics routine. The detailed graphic component

dimensions are written to a print/save file for archivin_ the program

interaction.

B-75
ORIGINAL PAGE IS

OF POOR QUALITY

Rev A, October 1985

B-75a
Rev A, October 1985

1.3.23 DATATAB

Subroutine DATATAB is called within the system definition £unction

during detailed graphics representation generation for the environment,

the robotic arm, target, or load. If the requested component is a data

tablet structure, it is called to compute data points £or the graphic

routines when the input ISHAPE flag = 8. The detailed graphic component dimensions are written to a print/save file £or archiving the

program interaction.

B-76
Rev A, October 1985

!

i

i

Z

H

0

&

J@

_Z

-H

ZO:

IZ

J

rr

"t

OH

IOU

_Z

WO

ZU

H E

Jo

J

111Ii.

Z

o

!:oIz

_[rr

!,=

i;

H ÷

oUlH

I- I-

_. :_ _- ZD z

1,131,10

I,-. Z :E O

IIIH

_an.

EiI-u I

1]:, Z

a:i_ H H

& Z

_-iO 0

U.'

C

E

I" iii. .!

iu

= III_ I

, El

,I

H

IZ

hl

Z

I1.1

J

I1.1

).

p_

iN

iX

iim

lu

IH

iim

w

@

÷

H

Zl

I£

_1

E

÷I

I..Ez

H=i

H

H

3

0

0

H

tZ

N

'I _1

B-76a
Rev A, October 1985

1.3.26 FILLET

Subroutine FILLET is called within the system definition function

during detailed graphics representation generation £or the environment,

the robotic arm, target, or load. If the requested component is a fillet part, it is called to compute the data points for the graphic routines. For a concave fillet, the input ISHAPE flag is 9. The detailed

graphic component dimensions are written to a print/save file for archiving the program interaction.

B-77
Rev A, October 1985

I I

b

hi

1

J

H

Ii

hl

Z

H

b

3

0

rr

'1 ̧:

I I

,' II 1

'l I

' zi wJ :

il a_

_!1_' _ _"_.'_-'°°

LLIZ -"

- zl :::) M ,_ i-] X

_,, _ I"'_: z_a: , , -.

, u.,.,j P i

i -_imPnl._ _ Oio',_ :__ H

-_zi_" _1_ . Z-, ,.i ,, ",_ _1_,o. +

a ,_ Ul (I: I- II b I

. z_ _1 3 _Iolo W 3 _o

"° "'! u]_:U" ._1_1 __1 O"'

.= I"I'

" °= _'_ !i wml'-ol=i_ Wi >

I - °O:P z z¢O JI Z UII_I H H

:F'!Z_ HiD o o hl

J >1 '_'_,3 _,',rr

' z lit J W_" , _:_>rr 1:[ i -_i _,, , ....

' ' '_ i_ _'"' _'i ..... o

I ! @1 _. l"_.;; _.1 .... .a

II I o IUIU u .... J

3

[ I

: I

I

,I

I

I

I

I '

I!

I-II hi

@1,_1

=1 1;I

•,.1 I I

I- b, ,.,

I=i

'_i"'

iit"li

i| i

,i"

i '

I'

I

i

I

I

i I

i

B-77a
Rev A, October 1985

1.3.27 OBSTCL

Subroutine 0BSTCL is called within the system definition function

during graphics representation generation with option ISHAPE equal 10

for the environment, detailed robotic arm, target, or load. It is

called if a requested component is an obstacle entity (a choice option

from BLDENV) or nonplanar structure (for BLDLOD, BLDTRG or BLDDAT). It

computes data points for the graphic routines. The detailed graphic

component dimensions are written to a print/save file for archiving the

program interaction.

B-78
ORIGINAL PAGE 13

OF POOR QUALITY

Rev &, October 1985

,,u.I

o D

t- H

_11 =, j "'_ i.!

o_1H

_-i rJSIH

J _ F..=IQ

U "_j HU" I

m _ J __t z

rn,,, Hu_i

0 d EL-.'_1 =

I1. ILl

W_E0_ ow

Zi,,Z,l Z iz H

HZHi_ m

_'_I0 o

_I ','_ z

l_E z H

LII

U

H --

rr z

_ NBII

oc LOZI il 3.2

H_'U

: nD_

i zut.

_-I orrn- u

_V_ _o

I

I

lz HI

_ Z

W

_¢ Z

II _o 3 w

--0 O:

b_v z

E m

3 " II'P:z

_ Z

-n

y m

J

H

Z

I °

O

H

Z

Zlllhl

IIIZI

EOH

Onll

E

0

U

I

@ @

°,,, zz

J H H

_ Hr. H z

+ x [L u u

.i _ _ o H

n

_-Z Z-O Io:I _ _:_ _=IZ _u

l¢W, I03

, IZM o,, IL_ l"zo,,_1_io :o"-_. z .

I'-IWII IL O .-IHI.-I 1= I=_

IZ iu,o H "o "m/ z

:,,-,"- w E! _/o w=,.

•_M n ___i_!_

E _ IT @PZI

"'

mrrlLl ziH Il'l'lx,

3 H_IH P, 8. )'E

-_MI3#I >-Iv"I-

, i_. iI_ 0_-_:_I_'-iZJI"E _t_ tu-_l_- °

, I,T,_

Zlj a. j

0t_ o _

z Im _ ,,, _ o,,,Io

I

I

Id I'

"o_.'_.o°_ "Zl/I u- I _,,, "-z

_z_" °'z"oi 0:0 W LULU

LLIH Z I_I"HJ _._ mo z,¢¢b JZE Z_I

_._: IO_'>_ H [U O OH

,_:_z emrr -InLL z_:_-i_ u_( ZP- U--;-

.& 3ZZ "-- bhl

EE 3

ooj x HW 0

_' OOW O

Z

E 0

;3Z

Hill

In"

I

5-78a
1.3.28 ORIENT

ORIENT is called from most o£ the build options in INITDRVR, allowing

the user to reposition components. The user can input a rotation sequence consisting of rotation axes and angles, and a translation vector

to position the origin of the component within the reference coordinate

system. MAT is called to compute the total rotation transformation matrix, and MATVEC to transform vectors from the new coordinate system to

the reference system. The translation vector is then added to each set

of coordinates. The translation and orientation of detailed graphic

components are written to a print/save file for archivir_ the program

interaction.

B-79
Rev A, October 1985

\U_

Z

0

HZ

HO

EH

Jb

ZJ_ Ld H

t-E(_ ITIX

H

n'ob iii 0

oz nE>-

W J Ld

Z00Z fl"

HZO E

_0

3Hn"

OHO 7_

EE 0

O]H[] H

3OW H

tOEE E

H H

I,3

HC] 0

W E

gEE

IW

3ff

v _

E /F

H

H

E

H

0

E

E

0

Ii

E

W

O]

3

H

n

E

0

E

n

E

(]]FIN)

WX

HH

Z EE

0 JH_

H DE,

( OE

F J

( EZ

[ n oo

o H

( H OH

( (/]HE

3 JZ

JE

_L IHUH EE

7

Cr 3

&

0

0

0

E

U_ o

(I

wz

ZOI

HH

a:zl E

ONi

(_N)2

7m

HU U_

H

UZ

wa. _

>El

HE

y

_1

-J 5i

u lj

-rm,

0

B-79a

L
Subroutine HAT is called during the system definition function to compute the total rotation transformation matrix defined by the input rotation sequence and angles. MAT is called from subroutine ORIENT. The

rotation sequence passed to it determines the transformation matrix

from the component system to the reference system it calculates. The

transpose (inverse) of the normal X,Y and Z-axis rotation matrices are

used. For each rotation in the input sequence, the axis rotation matrix

is loaded and premultiplied with the current total transformation

matrix.

SUBROUTINE MI=IT

I -- O

I -- I ÷ 1

_m'RO RMAT INT_RM_'nI_7_ ROTRT"rON MATRIX

T'_ NOTF::ITTON I=:IX'I"_5IS X--F:IX'I-S ._

RMI_IF _ _D -- _ [Iqlq_

NMPrT _l. _ 1 __

.-J

Z

w

ROTFIT'I_'ON F_×I$ IS Y--F:tX'r$ /_

z

I:_HI_IT_ _ _ _

ROTI=IT'rON I=lXT$ IS -_--RX'1"$ /'_

RMF:IT ( 1, 1 ) 1 COS (RNG]

I_MRT (1. m) -- --SIN (I_ING)

RMAT [2, i) -- STN [I_iNG)

I::_MI=:IT(2. 2) = COS (ANI_)

C._NC.ClTENFITE: Ti::_qNS MR'r_'rX I-_TI-,I RMRT RNO I_OF:I[_ "rNTO TF:_qN_

B-80
1.3.3O MATVEC

Subroutine MATVEC is called during the system definition function to

provide matrix/vector multiplication. The routine is called from

ORIENT. The vector A is multiplied by the matrix TRANS to produce output vector B. Note that this matrix/vector multiplication is 3-D only.

SUBROUTINE MI:::ITVEC

I -- O

I = I + 1

J -- O

= PROI::IUCT OF" M!RTRIX TRI::INS,

I:I, J} + .VECTOR F:I, ELMT. I:J)

1:30 UNTIL d . El3. 3

i:30 UNTIL "1" . EO.

I

3

RETURN

END i

B-81
Rev A, October 1985

1.3.31 DRAW

Subroutine DRAW is called within the system definition function to provide the graphics display during the generation of arm detailed representations. It is called to display each successive component as it is

defined. The routine logic is controlled by flag inputs specifying

initialization (at which time base/link/tool transformation matrix concatenations to the system are performed), component drawing, or component modification world.

B-82
Rev A, October 1985

W

Z

H

D

0

0

D

(77nN)
1.3.32 ESMAT

ORIGINAL PAGE _'3

OF POOR QUALITY

Subroutine ESMAT uses Evans and Sutherland graphics routines to construct the required transformation matrices from each system section

coordinate system to the graphics coordinate system. Input argument K

specifies which system section is under consideration. It is called

during execution of the system definition function. It is called from

subroutine DRAW to compute the required transformation matrices for each

system section. The robotic system has section coordinate systems for

the base, each joint/link, and the end-effector. An input value of K=I

indicates the robotic system base. The transformation matrix is composed of a translation matrix based on the base location and rotation

matrices constructed using the base orientation parameters. A value of

K from 2 to the number of links plus i (NJ+I) indicates the (K-l)th

joint/link. All transformation matrices from each of the sections to

the previous joint (or base, if the current joint is the first joint)

are concatenated to form the total transformation matrix to the graphics

coordinate system. Each joint transformation matrix is composed of a

translation matrix based on the joint position, a rotation matrix based

on the initial joint angular displacement, and rotation matrices for

joint orientation. A value of KaNJ+2 indicates the end-effector system.

The transformation matrix for the end-effector is composed of a translation matrix, and rotation matrices for end-effector orientation. The

end-effector location and orientation are specified relative to the

coordinate system of the final joint in the system.

SUBROUTINE ESII:::IT

RRM BASE

ONE OF THE RRM JOTNTS

_r_2:_l_[ JT. L_.I:rI'?_.._N lily IFRI:T _ L{)qO INTO INT[rI[R I_:tl_;IY

P_RLJ.. TTRIqN FOl:t P'I'I--'I-URII I=II_I3r_E_l_,O1R TRRNSI='PIRMRTTJ3N MRTR-rX

EXTI=_I::IOT OFP'_ET JT. RNeo FROM JtY_NT VRRTRBLE RRRRY

---J

TROTX _ --Y NZTI-I JT. FIN@. TO ROTRTE TRRN_P'_RMRT'rJ_'41 ._j

RRH TOOL

@r----qLE TO_L. _T_N _ :_=I¢'I" I=INO _ INT0 :T.N'I'Er_IER

$:II_IRY

EX_ RO'I'Icrl"ZON I¢IXI[_ _ :=N OIq_EN'I-J:I_. _ ,.ll_L'N'r

_'1" X. y. ii ROT. _lN_tJ[@ USl[D %N 0R'_J[N'I"ZN_ FROH ,,.IT.

r---_LL TROTX. --Y OR _im N-rTH ZNTE@ER RN P-- TEl I_DTRTE

i TRR_DRHF_"_O_

END

.--J

.-J

z

w

T_ET TO LORD NII;ITRIX Rt:_RRY 61"I'TH OU_RENT P=CTURE

PR_ESSI3R TRRNS.

RETURN

B-83
Rev A, October 1985

1.3.33 DBAS

Subroutine DBAS is called within the system definition function during

detailed graphics representation generation. For the subroutine, input

calling argument IMAN specifies environment, robotic system component,

target, or load objects file consideration. Graphics object data

IOBJBK are loaded for robotic system components, TGRAFBK is loaded for

target components, LGRAFBK is loaded for load components, and ENVTBK is

loaded for environment components. The manner in which the data are

stored in the COMMON blocks is dictated by the data format used in

Evans and Sutherland graphics routine D3DATA.

B-84

\

\
Rev k, October 1985

CO

O_

O0

0

LU

Z -

H[_z

4-

O_z

n'_z

31u

_LG

! I

L.tSllltl/"000[

: ltOl3B-I

]IB3S13S

M

o NUdS

- OUO'll'O00I

': HOIOU_

i #, 3"IU35£3S

I

U313NUBUd

"gHUINI

SIN]NOdN03

_0

_3OWnNL:IS

U313WU_Ud

OUOINI

SIN3NOdN03

JO

_3BW_ £IS

iN]NOdW03

13]r80_@I

NINOIW@I

I@iSI]S

IN3NOdN03

IN3_UA3

133P80OBOl

NINOIIU30I _°

i_ISI]S

'Sld S_lll_

'l_l]LllJ

o]sn

NOIi_30I

133P80O_Oq

iSUl_0_

_]INB03lqS

mImimi

B-84a
Rev A, October 1985

1.3.34 BASELK

In the create mode of BASELK, the user is prompted for the base mass,

the location of the base center o£ mass in the base coordinate system,

the base inertia matrix relative to the centroid, the point mass values

and locations i£ desired.

B-84b

\

\

\
OF POOR QUALITY

Rev A, 0¢tober 1985

J

!11

hl

Z

H

H

3

O

n"

D

HIrlHA_ HI;U8H'JU]HO:l_ tJkl

(llnN)

(1711N)

'lil_l OIBZ

B-84,C
Rev A, October 1985

1.3.35 RCTSTR

Subroutine RCTSTR is called within the system de£inition £unction

during detailed graphics representation generation for the environment,

manipulator, load, or target objects. If the requested component is a

rectangular cross-section beam (ISHAPE=7), it is called to compute data

points _or the graphics routine.

B-84d
Rev A, October lg85

o_

° !! I

_o

a M _

z _>_

+| +!

o_ o__

B-8_._
Rev A, October 1985

1.3.36 CADOBJ

Subroutine CADOBJ is called within the system definition function

during detailed graphics representation generation for the environment,

manipulator, load, or target objects. If the requested component is a

CAD/CAM object (ISHAPE=ll), it is called to compute data points for the

graphics routine.

B184f
Rev A, October 1985

o

3

B-84g
Rev A, October 1985

1.3.37 TARG

Subroutine TARG is called within the system definition function during

detailed graphics representation generation for target objects. If the

requested component is a 4-dot target object (ISHAPE=12), it is called

to compute data points for the graphics routine.

B-84h
ORIGINAL PA_I_, |5

OF POOR QUALm-'(

Rev A, October 1985

2

B-8_i
Subroutine CVTUNIT is responsible for the conversion of input data from

• I/0 units to internal mathematical units. Each data value VAL is multiplied by CONUNIT(IDIM) and replaced in VAL.

SUBROUTINE CVTIJNTT

DO .FOR EACN VALUE TO BE CONVERTED

VALWE = VALUE TINES APPROPRIATE

COMPONENT OF CONUNIT

i

k

B-85
1.4.2 MAT_PY

Subroutine MATMPY performs the multiplication of two matrices, ABfC,

where A has _ rows and J columns, the dimension of B is JxK and C is

IxK. The matrices and their sizes are passed co subroutine as calling

arguments.

SUBROUTINE MRTMPY

C (I_OW° ICOL) -- O. O

R = R [IROW. ICNT) *B (ICNT. ICOL)

!

C (IROW. ICOL) -- C (IROW, ICOL_ _R

O0 UNTIL fONT = NUMBER OF

COLUMNS IN MATRIX R [ROWS IN B)

DO UNTIL ICOL -- NUMBER OF

,COLUMNS IN SECONO MRTRIX B

O0 UNTIL IROW = NUMBER OF ROWS IN

FIRST MATRIX R

RETURN

END

B-86
1.4.3 EP_RMSG

Subroutine ERRMSG is called when certain errors occur during ROBSIM

execution. The routine first displays the current operating mode

(i.e., system definition, analysis, or postprocessing). The routine

searches the file ERROR.DAT for an error message corresponding to the

error number passed to it. The message is typed at the terminal and

execution returns to the calling routine, from which it continues or

terminates depending on whether the error is fatal.

CLOSE ERROR MESSAGE FILE

RETURN

END

B-87
i. 4.4 ROT_IAT

ROTMAT computes a rotation matrix from a sequence of up to three rotations about coordinate axes. It decomposes the input calling argument

JSEQ.into three successive rotation axes, computes each corresponding

rotation matrix from the specified angles of rotation and combines these

successively to find the overall rotation matrix.

SUBROUTINE ROTMAT

INITIALI_E RRRAY T2 [3. 3) TO AN

IDENTITY MATRIX

DETERMINE INOIVIDUAL ROTRTION AXES

(IJOC3)}

0 T_ TJO In') . NE.

%.

C@LL C_'TM TO FINO THE

TRaNSFORMaTION MATRTX

i T 1 (So CALL31 FOI:_ MATMPYI::ISINGLETO COMBINE _OTATION

NE_ MATF:_'rx T1 NTTH OLO

i O0 UNTIL T=3 MATR'I"X T2

[NULL:}

PUT RESULT IN AARAY RM [3. 3)

RETURN

ENO

B-88
I. 4.5 CE_.I

Subroutine CETM calculates a transformation matrix for a specific input

axis of rotation and rotation angle by the use of appropriate direction

cosines matrix. The calling argument input is:

Symbol Type Dim. Definition

IAXIS 1"4

TH R*4

i Rotation axis for joint orientation

= I, Rotation about x-axis

- 2, Rotation about y-axis

- 3, Rotation about z-axis

l Rotation angle for joint orientation

SUBROUTENE CETM

TN"I-TTRLT'-_E T i TO -_ERO MI:::::ITRTX

DOCR5E -FRX-F5

ECIURL5

1

_ Z x l---

e--I '-_- x _

---I

2

....I

3

.___1

___1

Z

B-89
1.4.6 LOGO

The LOGO routine calculates data points required to output the Martin

Marietta logo, and displays it on the robotic simulation E&S graphics

display. It extracts from the data points file LOGO.DAT, the Martin

Marietta company logo, scales and displays the loEo for the robotic

graphics simulation.

SUBI::tOWT_NE LOGO

OPEN LOGO OI::ITA FILE

_E_Q _LL OF LOGO Ol:::ITl:::l TNTO _NTEG_'R Ol:::ITlq_R_AY

CLOSF LOGO Oi:=lTl:::! FII_I_

,SET TNTEI_I=I::t STI=I_ VI=ILUi_ TO 80/INPUT $T;_E

DO FOR EACH ORTA HORI_ONTRL P2×EL

L = 0

INCREMENT I

i

: INTEGER _ VALUE = INITIAL

!

L____ _ = L _ 1

lSm'T OUTPUT LO:O _Y X. Y. :J. EL_'MF'NI" L. TO X. Y. :m

SET STF;_T lqNID STOP POINT_ L-OOP F_F:_RRM_T_RS

L __ J = 0

lifo WH'T'L.E I_, NUMBI=R OP" i_O'I'NTB _ bO_O, . I.,,.._--. 500

om-r-_ZN- _ To us,"ZN ou'mu'rrZN_ _.o_o_---._rrs

I I,_E_m','-rE "I"NTEtSER X VI::II--UE-- TNTEmEm X * mR'l'ZO

! I

' I

iSm'T L'rNE (_I_'NERt:rI'@R Im'Ll:Ir___@ FOR @i:IRF_'I':$ -'_Z@_t..I:IY I:lOI,JT_:NE

t RETURN ENO

I

!

' i

I

I

I

' l

B-90
1.4.7 CRPD

Subroutine CRPD computes the cross-product of two vectors A and B, each

containing three components. The result is put into the vector C.

SUBROUTINE CRPD

C (1) -- A (2) _B 1::3) --A (:3) _B (2)

C (2) -- A (3) -.-B (1) --A (1) ---B (3)

C (33 -- I=1 (13 -.-8 (2) --I=1 (2) ---8 (1)

RETURN

END

B-91
B-92
The Analysis Tools Function

Rev A, October 1985

The program SIMDRVRis the analysis tools function driver. The following

set of routine functional descriptions and VCLRs(visual control logic representations) are the modules found in the analysis tools function of ROBSIM.

2.0 i

I

1.1.111.1.21 2.1.112.1.sI

- ERRSMG

2.2.241 2.1.2J

i SEGTASK I 1 SEGMNT I

I I

i.,.iI1.2.1111.4.31i.,.,]2.2.11 2.,.,I

I - CETM - ERRMSG " _-_'_' -' - _'_ _1"_'°' I

I L ---- -I 1.4 .11 1"4"41 I 2.4.41 1.4.31

j _._.-I _.,._1 _.,.,I _._._,1 ,

, I_,_,_ll_ll-_,Jl_:o_! ,,

I - _x l

I - c_D I

I

I I

I_ J

I *Used often, so referred to by * in subsequent flowcharts. I

I

Figure B-8. - Functional block diagram for SIMDRVR.

B-93

PRECEDING PAGE BLANK I_OT F|_,_ED
ORIGINAL PAGE I_

=_m'OOR QUALITY

2.2.212.2.3112.111

I '

1.1.21 1.4.3 ̧1

2.3.44 i 2.3.451 2.3.71 2.3.461

[' BASINIT I I BRCNTRL ] [ RATEPRO ] [ BPCNTRL II

2.4.6 I

2.1.3 l

E REQUIR I

i

_i I_l _61 _I

I0_x III_ II_o_._II

- E&S ROUTINES

I

2.2.25 i

[ BCNTRLR ]

]

1.4.3 I

ERRMS G [

i

]

I

- MATMPY

- CRPD

- ERRMSG

1.3.19[

JACOB

I - MATMPY

CRPD

1.2.11

I SETUP2

I

1.2.111

I SETUP2

!

2.4.3[

- DOT [

- CRPD ORERR

- CRPD

2.2.8[

CHKLMT ] [ DYNAM

2.2.9[ 2.2.10] 2.2.11[ 2.2.12 i

i i VOLTAGE ] [ OUTREQ I [ ESPAUS ][ ENDREQ ]

(Not yet - E&S ROUTINES

implemented)

2.3.13[ 2.3.14 2.3.15[ 2.3.16[

i IL II Ii i

, I

TRACKING

I[2.3.4; 1.4.2, [2.3.51[ [2.3.52[ 2"3"191

- I_TI_Y

2.3.49 l I

t ANGLES A _----2.3.20 I 2.3.21[ 2.3.22 i

2 •3.50[ SPRGFOR CNSTFOR

1.3.19

- _TNPY

- CRPD

1.2.11]

- CRPD

SETUP2 ]

- CRPD

- MATMPY

- CRPD

2.4.3

[°_i

- CRPD

Rev A, October 1985

1.4.71.4.21

*Defined in previous flowchart [SIMDRVR ].

Vigure B-8. - (cont)

FOLDOUT F_E

B-94
Rev A, October 1985

2.2.13 1 2.2.14[ 2.2.15 1 2.2.161 2.2.31

I SIMOPT 11 INITCO I I DEFCNST I i PIDINT i n

_ ERRMSG

ORiGi_L _"_'_ "_

POOR QUALITY

2.1.4 1

I RESPON ]

1.1.21225i 1.4.31

!

*2.3.91 ** 2.3.11

- CRPD I

2.3.12

ACTORQ I

2.3.23 i

I POSSENS I

2.4.512.2si 2.4.312.3.261

I - CRPD ]

I

- CRPD - MATMPY 2.4.2 ]

- CRPD

2.3.24[ 2.3.25_

Is_ i["_ I __ ][ s_w_I

- RCICR 2.4.2]

- MATMPY

- CRPD i REPCOL ]

iPoss Nsi

2.3.29|

_I _I I _o. ]

li_o_]i__i

I_°_IL_,_o_ I _i

- MATMPY - MATMPY

- CRPD - CRPD

2.2.23 I

O_TV11T_=_ii_"_ilE_o_Mi

- E&S ROUTINES

2.3.34 [ 2.2.21

I_o,_i[o_TvI

]

2.2.81 2.3.36] 2.3. [1.3.

I_w_ll °_ I °_°_II_ [Is_w_°°l__

I - MATMPY ]

2.3.53] 2.2.251 2.3.39 i 2.3.40 2.3.411 2.3.42] 2.4.1]

REPCOL I [ BDRTORQ I [ BCNTRLR] [ LININT ] [ LDVOLT I I CALCI I I SOLVE ] ] SLVLIN2 I

*Defined in flowchart [SIMDRVR].

**These subroutines defined in previous flowchart under [REQUIR] .

-_..J FOLDOUT FE;:_;'_E

Figure B-8. - (concl)

B-95

/ IFOLDOUT FR_,_E
Rev A, October 1985

2.0 S IMDRVR

2.1.1 RDSYS

2.1.2 SGMNT

2.1,3 REQUIR

2.1.4 RESPON

2.1.5 TASK

2.1.6 BASGMNT

2.2.1 FTIN

2.2.2 REQOPT

2.2.3 PRTARM

2.2.4 GRAF IX

2.2.5 CNTRLR

2.2.6 SPRG INC

2.2.7 CHKLHT

2.2.8 DYNAM

2.2.9 VOLTAGE

2.2.10 OUTREQ

2.2.11 ESPAUS

2.2.12 ENDREQ

2.2.13 SIMOPT

2.2.14 INITCO

2.2.15 DEFCNST

2.2.16 PIDINIT

2.2.17 OUTSIM

2.2.18 CNTRS IG

2.2.19 CONTROL

2.2.20 SETCNST

2.2.21 DERIV

2.2.22 INTGRT

2.2.23 ENDSIN

2.2.24 SEGTASK

2.2.25 BCNTRLR

2.3.1 SEGINIT

2.3 •2 GRASP

2.3.3 RELEAS

2.3.4 ESCNTRL

2.3.5 POSSPJT

2.3.6 RCNTRL

TABLE B-VI. - PROGRAMS EMPLOYED IN SIMDRVR

2.3.7 RATEPR0

2.3.8 PCNTRL

2.3.9 CABS IM

2.3.10 FORCE

2.3.11 TORQUE

2.3.12 ACTORQ

2.3.13 REQPRT

2.3.14 REQSOF

2.3.15 REQTRQ

2.3.16 REQPLT

2.3.17 POSGRDJT

2.3.18 2TPOS

2.3.19 Cv'TI N

2.3.20 SPRGFOR

2.3.21 CNSTFOR

2.3.22 PTACC

2.3.23 POSSENS

2.3.24 SIMPRT

2.3.25 SIMPLT

2.3.26 FORTOR

2.3.27 FORREF

2.3.28 CMPCTRL

2.3.29 PIDCON

2.3.30 PIDFOR

2.3,31 EFINRT2

2.3.32 NLINK

2.3.33 SIMLMT

2.3.34 STOPFR

2.3.35 ACTIVPIH

2.3.36 DRTORQ

2.3.37 EFINRT

2.3.38 SLVrHDD

2.3.39 LININT

2.3.40 LDVOLT

2.3.41 CALCI

2.3.42 SOLVE

2.3.43 GAUSS

2.3.44 BASINIT

2.3.45 BRCNTRL

2.3.46 BPCNTRL

2.3.47 TRACKING

2.3.48

2.3.49

2.3.50

2.3.51

2.3.52

2.3.53

2.3.54

2.4.1

2.4.2

2.4.3

2.4.4

2.4.5

2.4.6

INITTAR

ANGLES

NEWFRAME

PERSPECT

HARALICKR

BDRTORQ

SLVMBAS

SLVLIN2

REPCOL

ORERR

0UTt_

ICVrATD

BORERR

B-96
Rev A, October 1985

2.0 SIMDRVR

The program SIMDRVR is the analysis tools function driver. It operates

in an interactive mode, prompting the user for the analysis option desired: requirements analysis without graphics, requirements analysis

with graphics (a display o£ system motion during program execution), response simulation analysis without graphics, response simulation analysis

with graphics, option to set up a base or arm motion program or terminate

SIMDRVR execution.

B-97
OF POOR _'_ ll,_t,/t"_m _'_v i •

Rev A, October 1985

B-97a
2.1.1

Rev A, October 1985

RDSYS

Subroutine RDSYS is called from SIMDRVR to read in the manipulator system definition data needed to run any of the SIMDRVR analysis options.

The routine first prompts the user for the name of the file containing

the system's data and then opens that file. If the system includes

moving bases it reads the number of bases. Moving base numbers, geometric properties, mass properties, actuator properties and special

joint data for each arm are read in, as well as system graphics data

and the definition o£ gravity for the system. If the system contains

an environment, the data describing it are read in. If load objects

are also to be included, the data describing them are read in. Endeffector data and target data are to be read next. After that, the

file is closed and saved.

B-98
OF.. POOR QUALITY

Rev A, October 1985

Ul

}-

Ul

O

E

W

Z

H

H

D

0

rr

m

D

@

B-98a
2.1.2

Rev A, October 1985

SGMNT

Subroutine SGMNT allows the user to set up the desired motion profile

for a requirements analysis or response simulation run. It is called

from SIMDRVR. An existing motion profile file may be read in and modified or the profile may be defined interactively. Motion is specified

in one of £our ways:

1) Desired position of end-effector;

2) Desired position of each joint;

3) Rate of end-effector movement;

4) Rate of each joint.

In addition, motion may be specified by having an end-effector-mounted

sensor move toward a target. Several nonmotion-type operations such as

grasp a load object, release object and wait a given length of time may

also be speci£ied.

B-99
Rev &, October 1985

D

39WSS3HUOU_3IU7dSIO

_ $379NWINIOP

_H 7WIIINI311WM

o_o__ _. (77nN)

z_ _,,9

_z__

37I_OUdIUOISIH

3HIlIN3UUfl3UOJ

SIN3H93S77U31IUM

• a Z

__ _ o_•0 HIll x_ZlIZI

,,,_ -

r. z J> EIo ow OF

z om O(,g

B-99a
Rev A, October 1985

2.1.3 REQUIR

Subroutine REQUIR is called from SIMDRVR and is the routine that controls the execution of any requirements analysis run. It first calls

REQOPT to set up program run time options. I£ requested, PRTARM is

called to write a description o£ the system to an output file. SETUP

is called to calculate initial positions. GRAFIX is called if the run

is to include graphic displays. The subroutines CNTRLR, SPRGINC,

CHEIMT, DYNAM and 0UTREQ are called at every increment of a user-defined

time loop to calculate the manipulator system's motion, forces and

torques, and write these data to an output file. ESPAUS is called when

motion is temporarily halted during execution. When the stop time is

reached, ENDREQ is called to close any open files.

B-100
Rev A, October 1985

I

0

H

OE

o•

J

n

Z

3=I

!

I

!

I

I

i

i

i

!

I

!

!

!

I

i

!

!

B-100a
Rev A, October 1985

2.1.4 RESPON

Subroutine RESPON is called from SIMDRVR to control the execution of a

response simulation run. Run time options and program variables are

first initialized. A user-defined time loop is executed to call routines to carry out all the control functions. After execution is completed, ENDSIM is called to close the files.

B-I01
Rev A, October 1985

Z

0

n

Ul

n"

Iii

Z

H

b

3

0

n"

3

g)

t i

li

tl,

, i !

t

,o! 3; ' I_1=,

,lil-'

H°IL ;_

wt-IIIti ;--

| _l.Z;._ , ,=

,,pzt•

t-, ,oIDJn

.Z.toIZ' i_-

.UlZl&,

t-iH

41_ --

!_ _!z oEO,, II1_ _![

L)

I -

I i _ .

/_-I ,_

I

F

I

I

b

h

F

H'

1-'

i

o_

[!

kL

J J_

J ID

3 D z

_Z Z H

H _,", W

i_ai 1

4111

hill: (J

HZ_ H

lillE] H

11., × 0 .,zH

•_ 1-010 _

i

I- wIH..I-E E

z _;wz

H _'.H E

-in'

J HiO

j D

E !W:E

-_ ,_!_

I@,W I,

iWi-.l

n ,_i (..)

;Imll

! i

i

! !

i

'_llE ,LI,.,

031-,4

HOE ,

I-EO@

0 ILl

E I'-I- i

Jz_ m

il

_ _'_L °

U O1_

_ NiN I

(J U l- .

HEH 0 D

Z

)" 0,4,1,

g

I1 ..,_ @!_ H

"']D

Z m_0 _ 11.

>"--'__I_'I_ I0 Bi..I HII_ Ill

__1 lUl_i_' U @

u e _ ,

, _H•

IF

ej

: H

I o

I

41

I

|

I

i

i

I

i

B-101a
2.1.5

Rev A, October 1985

TASK

Subroutine TASK is the preliminary routine called when defining manipulator motion. The user has the choice of modifying an existing time

history (motion) profile, creating a new profile or writing a user

readable, formatted file from an existing time history profile. The

subroutine opens the appropriate files and then calls subroutine SGMNT

if an existing £ile is being modified, a formatted file is to be written, or a new file is to be created using just lower level motion commands. I£ task level commands are used, TASK calls subroutine

SEGTASK. For other options task calls SGMNT.

B-101b
Rev A, Octobe_ 1985

(77nN)

o

8#

_J

X

W

EW

w_o

0

_o

_oo_

O_@H

_¢o_

_ZP±

O_W

_Oj

OH_

_H

_oz

OWO

Z

H

Z

Z

0

Z

_.WO. "' 0 W

ObT _ _.

HI-,-! ..I H

_._..._- H __,,,"

z..au /-I--

O_ ILl II _.H 0 zo_ _

i- I- fl:l Z m-_.

OC E 0 E

_-_n, O. J oc

ZHO 0 _ 0

"'"" I "

h J UII_0dill

_@NI_NN_I INH_3_"171:10 _ Z0

13kal )iSt_I_I UlUl] _, _C,W

/_ )ISUI]NI_30

/ -I q7u3-

!1110. ,,, 0

_o:jz u-I _-z_ =

OHI- _I W._ !_IT

_LIL H _Z_ .Y.

_oo" z _z__

gwm W __ I--

__z n H_w-/

u._o _ #u 3

u

0

0

B--lOlc
2.1.6

Rev A, October 1985

BASGMNT

Subroutine BASGMNT allows the user to set up the desired base motion

profile for a requirements analysis or response simulation run. It is

called from SIMDRVR. An existing base motion profile file may be read

in and modified or the profile may be defined interactively. Base motion is specified in one o£ two ways:

1) Desired position and orientation of the base;

2) Rate of base motion.

B-lOld
Rev A, October 1985

D

0

D

3E)OSS3H _IO_U3kW7dSIO

3NUN37IJ

_0_-i ldHO_d

o

o

77nN

OD

B-101e
2.2.1 FTIN

Subroutine FTIN is called from SGMNT if force/torque or active compliance control was specified by the user. If force/torque control was

specified, the user is prompted for the number of force and torque components to be controlled, the unit vectors in the directions to be controlled and the magnitude of the control force or torque. If active

compliance control was specified, the user is prompted for the stiffness

matrix at the end-effector reference point.

SUBROUTINE FTIN

IOP i EQ. 5

PROMPT USER FOR FORCE/TORQUE CONTROL ON/OFF

i

FLRG

T_ TURN FORCE/TORQUE CONTROL ON //_

PROMPT USER FOR NL.IMS_'R OF FORCE

COMPONENTS TO CONTROL

PROMI:_T USER FOR CONTROL D'I"RECT'I'ON m , .

IPROMI='I" USER FOR MI=d___I_"rU_E _Ir CONTROL. FORCE:

DO UNT'rL N . EQ. NUMBER OF CONTROLLED _-1

FORCE COMPONENTS _..I

...--1

PROMPT USER FOR NUMI=IER OF TORr'JUE =

Z

/#

COMPONENTS TO CONTROL

P_0L_.._T USER FOR CONTROL DIRECTION

IPROMPT _ _ _ _

TORQUE

DO UNT_:L N . EQ. NLIMeER OF CONTROLLED

LTORC|UE COMPONENTS

"r'oP . Er_. 6

I "

_--------_TURN CI::3MPLTi:::INCE CONTi:_EIL ON

I_ _ m_,'c=N._,e_nm mF I

_ I (NULL)

°1

RETURN

END

B-I02
2.2.2

Rev A, October 1985

REQOPT

Subroutine REQOPT is called £rom REQUIR to define requirements analysis

run time options. The user may list currently defined options and use

them or input a new set of options. Options the user may set include

run time data file write, torque file write, control method to be used,

control of robot base, dynamic calculations, playback file write, plot

file write, and simulation start time, stop times and processing step

size.

B-I03
Rev A, October 1985

ORIGINAL ........ " "

POOR QUALITY'

I | I R

il

I !

II

II

1

I

II

II

Ii

II

I

i

il

i[

,I

I

I

I

I

I

Ill

\

II

il|

_II II,

_t

F

>

i jm

I #f

Ii

F

O

L

C

C

b

C

z

F

f_

@I

RI

i

(77nN)

Jll,,

]lid

/AU'

I]]H

1I

B-lO3a
2_2.3 PRTARH

Subroutine PRTARM is called form either REQUIR or RESPON when the flag

for printed output of that analysis is set. This routine prints a description of the manipulator system that includes the following variables: current arm number and number of Joints per arm, type and mass

of each joint, initial angular positions and velocities of each joint,

joint travel and rate limits, Joint/llnk centroid locatlons, Joint location relative to previous joint, inertia matrix for each Joint, orientation matrix for each joint relative to previous joint, span of the

whole system, and the acceleration attributable to gravity.

SUBROUTINE ImRTRRM

NRITE CURRENT RRM NUMBER lIND THE

NUMB_'R OF JO'I"NTS PER I:::IRM

6QR-I"T_" TI-I_" TYPe" F:INO MF:ISS OF" _'I::ICI-I JO'I"NT

CONVERT DATR TO BE 64R'f'TTEN F'ROM

I"NTERNI==IL TO "I'NI:mUT/OUTPUT UN-I'TS

64R'rTE JO'f'NT "f'N'T'T'I'I==IL6:_NGULI=:IR F:_QS'I"T'I"ONS

AND V_'l_OC'rT'r m'S

64RITE JOINT TI::_RVE/ I:::IND RI:::ITIE I_INTTS

64RTTE JOINT/LINK CENTROID I_OCI:::ITZONS

64R-rTE JoT'aT LOCRT'rONS RELRTTVE TO

PREV-I"OUS JOI"NT

64RI"TE -rN_'I=:_T'f'R MI:=ITR'I"C_'S FOR JOTNT/L_TNK

COMB'I"NI::IT'I'ONS

64R'I'T_" OI=:_'rENTI:=ITTON MI==IT_I"CES FOI::_ EI::_CN

JOINT I::_I_I_I:::ITIVI: TO I::mREVIOUS JOTNT

DO UNTIL KRRM == NUMBER OF RRM8 "t'N THE SY_STEM

NRITE TOTRL SYSTEM SPF:::IN

64RITE ACC_'L_'RF=IT'rON OlJ_" TO GRF=IV'I"Ty

RETURN

ENI_

B-I04
2.2.4

Rev A, October 1985

GRAFIX

Subroutine GRAFIX provides the motion graphics capability in the response simulation, requirements analysis and postprocessing functions. GRAFIX displays the environment, target, load and robotic system motion within the environment. If IFLAG=I, the graphics system is

initialized and displayed in the initial condition; if IFLAG=2, the

display is updated to the current time step condition; if IFLAG=3, the

motion is complete and the graphics are terminated.

B-105
Rev A, October 1985

X

H

h

n"

L_

W

Z

H

p3

0

n"

rn

3

4

{lliIN}

3 _3

_: - H__

u 0 ! _ ill :ioI- o_EZ , _ I _, ioz o &ILl I-:

_: !_° 0:'.=

,oz 

bJ E E !u

1 ,

_. -_ ,,,,,,,

H °|Nz

,4_,_,,o,w_,,

°(,,_o_ _H_ 8 u

oo 9

_H

(I E

_._,, _ _ P._.

I- FE

J

J

E

J

H

.IZ

-1

O

13

III

B-lOSa
2.2.5

Rev A, October 1985

CNTRLR

Subroutine CNTRLR is called £rom REQUIR to obtain the angular position,

velocity, and acceleration £or each joint of each arm at each processing

time step. If the variable IDATA was set to 1 earlier, the data are obtained by reading an existing £ile that contains just these data. If

IDATA equals 2, the values are calculated £rom the motion pro£iles.

Subroutine PCNTRL is called £or the position control calculations and

RCNTRL is called £or the rate control calculations and TRACKING is

called £or sensor control. IDATA equal to 3 allows the system motion

to be controlled by dials on the Evans and Sutherland.

B-106
2

Rev A, October 1985

H

dR19£

77U3

B-106a
2.2.6 SPRGINC

Subroutine SPRGINC is called from REQUIR to set the variables used when

the end-effector is to have compliance associated with it. The variables set includes spring reference position, orientation and the spring

constant.

SUBROUTINE SF:,I:_G'rNC {T I:M Im 01::_I:II::_Y)

SET SPRING VARIABLES SPRP (POS) .

SPROR (ORIENTRTION) , AND

SPRK (STIFFNESS M

N = NJ {KRRM) + 2

POS (1, N. KARM) . LE. . 68872

. AND. POS (3. N. KRRM) . GE.

.68872

ISPR {KARM) = 2

{NULL}

O0 UNTIL KARM . EQ. NUMBER OF ARMS

RETURN

ENID

B-I07
CHKLMTchecks Joint displacement and rate limits during requirements

analysis. It does not modify any values but prints a warning to the

terminal if any lim/ts are exceeded.

SUBROUTINE CMKLMT

00 FOR I_I=ICM ARM TN SYSTEM

DO FOR I='ACFI JOINT TN ARM

T_ MINIMUMDISPLRCEMENTOR MRXIMUM EXCEEDSVALUE

TYPE WRRNING TO

TERMINRL NITN [NULL)

PERTINENT DATR

T_ RRTE EXCEEDS MINIMUM OR MRXIMUM VRLUE

TYPE MRRNING TO

TERMINAL WITM [NULL)

PERTINENT OATR

.JOINT IRF:ITENOT EI_IUI=IT-I'ONS BE SOLVED COULD

TYI _" 1,41:IRN"rNG TO

TERMINRL. 64"rTM PERTINENT (NULL)

OI=ITI¢I

B-I08
2.2.8 DYNAM

Subroutine DYNAM is called from REQUIR to compute the manipulator system dynamics at each processing time step by calling the SETUP, CABSM,

FORCE, TORQUE, and ACTORQ subroutines.

SUBROUTINE DYNRH

CALL SETUP TO FIND ALL POSITIONS

IN WORLD COORDINATES

CRLL CABSM TO FIND ABSOLUTE VEL.

RND RGCEL. OF RLL LINKS

CRLL FORCE TO FIND JOINT RERCTiON

FORCES

CRLL TORQUE TO FIND JOINT REACTION

TORQUES

CRLL RCTORQ TO FIND JOINT RCTURTOR

TORQUES

RETURN

ENO

B-f09
2,2o9 VOLTAGE

(Not implemented yet.

B-110
Rev A, October 1985

2.2.10 O[Yl_q

Subroutine 0D_EQ is called from REQUIR to write output data to files

requested by the user. The files the user may elect to have data written to are:

1) Run time output data file;

2) Data file for subsequent replay of motion on a vector graphics system;

3) Actuator torque data file;

4) Run time data file for subsequent plotting;

5) Base torques and forces data file.

B-Ill
Rev A, October 1985

W

Z

H

D

0

E

Q]

D

z

H

B-111a
2.2.11 ESPAUS

Routine ESPAUS is responsible for polling the status of the E&S function

keys to determine the on/off status of the devices switch for playback

motion cessation. A light indicator in the function key is used to inform the user of the key status; when llghted, the perspective viewing

is in operation.

SUBROUTINE ESPRUS

STRTUS FUNCTION KEYS [EXTENDED

SWITCHES) EVENT QUEUE

T_ USER FUNCTION KEY FOR //] TERMINATING "rS ACTIVATED / FI

ISTOP OUTPUT CNULL) FLRG -- 0

USER FUNCTION KEY FOR /

CESSRTION OF MOTION IS /

RCTIVRTEO / F

ISTOP OUTPUT

FLAG m 1

CNULL)

RETURN

END

B-f12
2.2.12 ENDRKQ

Subroutine ENDREQ closes any files opened durln E runnlnE of the requiremen_s analysis portion of ROBSIM.

SLIBI:::tOUTTNE ENIDFtEO

FILE 3 IS OPEN

CLOSE LUS

T_ F'rLE 8 IS OPEN .....

CLOSE LU6

T_,,.

CLOSE LU13

FILE 14 IS OPEN

CLOSE LU14 I

FILE 18 IS OPEN

CLOSE LU16

FILE 17 {KARM)

(NULL)

{NULL)

{NULL)

{NULL)

S

{NULL} i

T_ IS OPEN

CLOSE LU17 (KARM] (NULL)

DO UNTTL KII:::tRM . E{:a. NUMBER OF ARMS

RETURN

,- -- u

END

B-If3
Rev A, October 1985

2.2.13 SIMOPT

Subroutine SIM0PT interactively prompts the user for the program start

time, stop time, processing time step, and several £1ags for control of

output and the selection o£ some computational capabilities. Among

these output options is a simulation output file that contains the data

required by the postprocessing function for further study. The user

also specifies the time frequency of the output of data to the file.

The user is also allowed to request printed output during the analysis

tools function execution.

The content and format of the data to be printed are provided for within

each of the analysis tools. The flag set within SIMOPT is used only to

turn the print routines on. The time frequency of the printed output

is also specified. Other options are for generation of an accelerationvelocity-theta file and/or a plot output data file that may be plotted

with the ROBSIM postprocessing plot utility with their associated output time steps. The user may also request use o£ a torque input file

or a control option to read a hardware input voltage file for computational capabilities.

B-114
ORIGIi_L "_

OF POOR QUALITY

Rev A, October 1985

H

(77nN)

II_ Lll

"I,III LI

TIII_

B-ll_a
Rev A, October 1985

2.2.14 INITCO

Subroutine INITCO prompts the user for the initial joint position (TH)

and velocity (THD) o£ each joint o£ each arm. If moving base is simulated, the routine prompts the user for the initial base positions,

orientations and velocities.

B-115
Rev A, October 1985

B-115a
DEFCNST reads a file containing the information needed to define a constraint (either planar or pes-in-hole type) on the end-effector motion

during dynamic simulatlon of the arm response. The user specifies the

name of the constraint file in response to interactive prompts.

SUBROUTINE 131='FCNST

QI..II_'F_y WHI='THI_I::_ t,l$1m'l::_ I.,JI:::INTS TO "rNCLU_ CONSTI:_I:::I'I"NT

KI:=:IRM ----"1 _-

I_I_OMPT FO_ F'rI_'NI_IM_" OF" OONSTI_I_INT FTI__"

OldEN CONSTRNINT F'-rLE _-

REND TOOL I_EI=ENENCE #OINT LOCNTION o

REND NUMSEI_ OF PLNNNR CONSTRNINTS m

DO FOR ENCN PLRNNR CONBTRRINT w_

RI_AO THE 4 COONO'rNATm'8 0I='FIN'rNG TH_ PLAN_"

REi=IO NUMB_'R OF I_EI_--IN--NOI--N CONSTIm_ImlINTS w_

DO FOR ERC_-4 PEG--IN--NOL I_ CONSTIm_RINT w_

REI=ID NOLE LOCRTION _-

RERD DIRECTION OF HOLE I_XT-S ,,_

I_EI=iD DEPTI-I OF HOLE

READ RI=IOILJS OI _ MOLE _=

RNI=iO FRTCTION COENFTC'I"ENT FOI_ I-IOLIE

b--

CLOSE CONSTNNINT FILE 'J

k NOLE CONSTRRINT INCLUDED /

ACTIVATE HOLE CONSTRRINT Ol_P----J_ HI_,._ CON_ZNT _NCTZVE

B-If6
2.2.16 PIDINIT

Subroutine PIDINT is called from RESPON to initialize variables used in

the program's control algorithms. POSSENS is called first to determine

the actual Joint positions. Initial values for some control variables

are set. The user is then asked to supply system gains for the methods

of control that will be used during program execution. These gains may

be supplied by either reading in a file of existing gains or by the user

interactively inputting the gains.

SUBROUTINE PIOINIT

CALL POSSEN$ TO 0@TAIN STN

ZN:ZT_ BT_ STI..I_O. OLJ::_S_ _R_T. _RZN'r. III¢IRZNT.

I¢I_PVE

USING P_O CONTROL

kREROING GF:r'rNS FROM A F-rLE /

PROMPT IJS_'R FOR F'FL_'NRM_" . PROMPT I_IS_'R

TO TNPIJT

RERO GF:=I-I'NS FROM F'I'LI_ GI=:I'I"N r'IRTF::I

(NULL)

/

 EAD .G FRO. A /

PROMPT IJSER FOR F'I'I_ENRME PROMPT USER

TO "r Ni::'U T

REF::ID I_F::I'T'NS FROM I="rL 1=" GR"rN OI=ITR

.--J

.--I

"--1

Z

/,

L OSING RCTIVE COMPLIRNCE CONTROL

REAOING GAINS FROM R /

FILE /F

PROMPT

USER TO

INPUT

GAIN OATA

PROMPT USER FOR

FILENAME

RERO GAINS FROM

FILE

i

Z

w

RETURN

END

/

B-II7
2.2.17 0UTSIM

Subroutine 0UTSIM is called from RESPON to write the appropriate output

data to the different types of files requested by the user. Types of

output files available are:

i) File of run time data for subsequent tabular printout;

2) File of joint positions, velocities and acceleratlons as functions

of time;

3) Data file for later motion replay on vector Eraphlcs machine;

4) File of data for subsequent x-y plottlnE.

SUBROUTINE OUTST"M

FIRST CRLL TO SUBROUTINE /_

sat _>_ 'puR_s To _ T_N¢ (NULl-)

CORR_CI" T_'M_ TO _,iR'rTt[ (DRTR _,_ (NULL)

•--.- ,aBly 'm = = B .=.= (NULl--)

'OO UNTT"L KARM . EQ. NUMBER OI = ARMS

T'_ CORRECT TZHE T'ORTR TO NR_TE . EO.ORTR

_R._'rE C_T _ TO OUTPUT

i.a_-rl[ l"l-b _ Tt-loo To ou'rlmuT _[1.m:

OO UNq-Z/ KRRM . _ NUMBER OF RF_B

(NULl--) (NULL)

r_ 7"STMO . EO. 1

m

CORRECT TIHE TO WRITE ORTR

[_RI'FE TI-_ :EL.D TO OUTI_U'F P'_I_E:

DO UNT'J:I_ KJRRM . I[_ I_I_ OF" RRM5

(NULL)

--J

Z

L "]'PLOT . EO. 1

=_ ,,,,:=_=¢."r "to _,==_"r,=" (NULL)

RETURN

I=NID

(NULl-)

B-II8
2.2.18 CNTRS IG

Subroutine CNTRSIG is called from the routine &EQUIR. Joint variables

are stored in dummy variables and CNTRLE is called to calculate joint

angular reference positions and velocities. The end-effector position

error is calculated and ORERR is called to determine the orientation

error. If force/torque control is being used, subroutines FORTOR and

FORREF are called to calculate Joint reference positions and reference

forces and torques. If active compliance control is being used, subroutine CMPCTRL is called to calculate amplifier input voltages.

SUBROUTINE CNTRSIG

SAVE VRLUES OF' TH, TND,' POS, ROT. IDATA,

AND TIME IN DUMMY VARIABLES

CALL CNTRLR TO CALCULATE REFTNT AND

REFTNDT

RETURN VALUES STORED "rN OUMMY VAIABLES

END EFFECTOR POSIT_ON ERROR. EPSE_ --

EPOS -- POS

CALL ORERR TO FIND THE END EFFECTOR

ORIENTATION ERROR

USING FOREC/TORQUE CONTROL

CRLL FORTOR TO CRLCUL_TE

REFERENCE JO=NT _OSITIONS

CALL FO_REF TO CALCULATE

REFERENCE FORCES ANO TORQUES

(NULL)

7_USING RCTIVE COMPLIANCE

CRLL CMPC_L. TO C_LCUI-RTE _OTOR

_MPLT_'TB'R TI_:_JT VOI..TRGIES

CONTROl

{NULL)

RETURN

END

B-II9
2 • 2 • 19 CONTROL

Subroutine CONTROL is called from RESPON at every processing.time step.

If a feedback control law is to be used, POSSENS is called to get the

actual joint data and PIDCON is called to get actuator voltages for PID

control. If force/torque control is being used, PIDFOE is also called

to get actuator voltages caused by the force-controlled components.

SUBROIJTINE CONTROl--

ISTF . EQ. 1 /

.--J

__.J

Z

CRLL POSSENS (KRRM) TO GET

RCTURL JOINT ORTR

CRLL PIOCON (KRRM). TO GET

ACTUATOR VOLTRGES FOR PIO

CONTROL

IFTCN .EQ. 1

CRLI- P_FOR (KI:II:_I TO

G_9" RC_TOII_ VOII-TR@E5

FOR I_)_'I'OR@IJE[ CONTROL

(NULL)

00 UNTIL KARM . EQ. NUMBER OF RRMS

RETURN

ENO

B-120
2.2.20 SETCNST

SETCNST checks planar constraints to see if they are violated or need

to be activated. If the current velocity violates the constraint, the

velocity impulse to satisfy the constraint is evaluated.

L NWMB_R GREATER OF PLANAR THAN CONSTRAINTS _ERO /

EVRLUATE OISTANCE TO CONSTRAINT

PLANE

T_ DISTI=INCE LI_SS TNAN _I_RO

m_T mcm_T ___Tmm (NULL) I,dRRN'rN@ TO 'I"_RM"I'NRL

DISTANCE LESS THAN TOLERANCE

SET UP FLAGS TO ACTIVATE

CONSTRAINT

EVALUATE VELOCITY OF POINT

TONAAD CONSTRAINT #LANE

T\VELOCITY GR_F:_TER THI:IN EERO

_'Vl=ll,__Ulml'l"_" JO'I"NT l_l::r1*m""rMl_IJl.._"

TO MIqK I_' VI_'LOC'I"TY "_I_'I:_0

PRINT |V_LOCITY IMPULSe|

N_NING TO T_RMINQL

UPORT_ JOINT VELOCITIES

--J

----I

z

w

----I

Z

w

A

---,,I

--J

z

w

B-121
Rev A, October 1985

2.2.21 DERIV

DERIV is used during response simulation to interface between INTGRT and

the dynamics module NLINK. This routine puts the state vector Z into

the appropriate common variables, calls NLINK and puts the results from

the common variable THDD into ZD.
Rev A, October 1985

iJ

I

,J

D

,Z

,_"1 I.,,,,..,_ _

I

=' i

OI !

!111 :

WI ,t :

H HHZ,I_:

I- ,,,_ IT _ ,,, w PINI_>

O I--'l_J'tl'l II1:

z:E _

H_l__:!E_

I- H

0 a

IT' m"' _" ,.,._ a

_ t-

--j oH

wlWlH_

iii: zo.

B-122a

Z

O

H

(/)

Z

I11

3:

H

[3

H
OF. POOR QUALITY

Subroutlne INTGRT Is called from RESPON and uses a fourth-order RungeKutta algorithm to integrate a state vector Z. State derivatives are

computed by the subroutine DERIV.

SUBROUTTNI=" INTGRT

SET T TO INITIRL TZME FOR INTEGRRTION

SET HOT TO TIHE STEP/2.

OO FOR ERCH COHPONENT _N STRTE

_qVI[_R_ _C'I'0RC0M_NIINT I

aL_l" D__I! _ STIBITI[ _,m4-1)T ¢=DI4BOI_[NT

CRLL STOPFR TO 5TOP MOTZON DUE TO DRY FRICT_0N

SET T TO INZTZAL TIME--HOT

CRLL OERIV TO CALCULATE 5TRTE OERIVATZVES

DO FOR ERCH COHPONENT IN STRTE

_T "]rO _irr'lmtgrz _ VI[L--'T_ ¢OM_'OINI[N"f'_'I"JCMI[ S'f'l[_ ]

I T

CRLL 5TOPFR

CALL OERIV

00 FOR ERCH COMPONENT IN STATE

_ O_-J-_ _ I_F_ _L_

C_LL STOPFR

SET T TO TN'rT_'FIL TTME ÷ TTHE STEP

CI:::iI_I_ O_'RTV

O0 FOR _'RCH COHPONENT IN STRTE

SET DEL_ TO LRST (_EL'_m"STRTE rlER'I"V. *HOT |

SET STRTE VECTOR TO SRVE STRTE VECTOR,.,-OEL_/). ]

CRLL 5TOPFR

CRLL O_'RIV

_ETW_N

ENE3

m

B-123
2.2.23 ENDSIM

Subroutine ENDSIM closes any files opened durlnE execution of the response simulation portion of ROBSIM.

5UeI::IOUTINE ENOS'I'M

(NULL)

(NULL)

,,4

(NULL)

/¢

(NULL)

(NULL)

CLO51= LUI@ (NULL)

RETURN

END

I

B-124
Rev A, October 1985

2.2.24 SEGTASK

Subroutine SEGTASK is called £rom TASK and allows the user to create a

file that specifies manipulator motion using the following task

commands:

I) Pick up object;

2) Place object at speci£ied location;

3) Move arm;

4) Hold current position;

5) Change end effector reference point;

6) Operator control (not implemented yet);

7) Set control mode for response simulation;

8) Sensor o£ end effector position.

The user is prompted for initial joint angles and other necessary data

after which a sequence o£ task commands may be implemented.

/

B-124a
01: POOR QUALn'Y

RevA_ October 1985

(77nN)

371JdHl

0181W0]IIUM

llINHHBfl3 ION]gH_HM31I_

S '_]" _ _13H_0O0

'_3'&6'IH0 _ H0H19NH7

SHE H0HHHSBldMOHd

(7717N)

z_" _" 37T3dill

_m_zan __ I Ul80UII_M

__l_s _ 371_dill

X_II["m m_rm6__ 01 UIU0 UII_M

H

B-12&b
Rev A_ October 1985

2.2.25 BCNTRLR

Subroutine BCNTRLR is called from REQUIR to obtain the position,

velocity and acceleration for each moving base at each processing time

step. I£ the variable IBDATA was set to 1 earlier, the base data are

obtained by reading an existing £ile that contains just these data. If

IBDATA equals 2, the base values are calculated £rom the base motion

pro£iles. Subroutines RATEPR0 and BPCNTRL are called £or the base position control calculations and BRCNTRL is called £or the base rate

control calculation.

B-124c
Rev A, October 1985

ill

nF

Z

O

rn

W

Z

H

b

3

0

BE

rn

3

I !",

H'll°N' LI

IL!II i i

I!,,,111,,,zl z !Zl

J H _ @

Ia:l_' e

i(]: "_ ,,, 0') J J

'1--_1o . I1 !L_ 13 j j

in t_ 0 _ W o: E z

H

>

, l, _h.! ", _- O

¢fl :o H _ _1 _-,Z H Ho ICE

H i_ IIE:i2G:F_ m

:wlw_ 3 z _x

i'

, U I . Z 'w

m "_ W' _ ILL (_

0 , z ,

"!_t;, ! I____ oo z

• I _z / ,H I

/dl31I_SOd-q_^-OOU_OU_UIUO3SU(](]U]UD I

_ l'-j I

o:,

Ol

z

iI,!

0

n;

B-12_d
2.3.1

Rev A, October 1985

SEGINIT

Subroutine SEGINIT is called from CNTRLR at the beginning of each new

motion profile segment. If the segment is to define motion of the

manipulator, the sensor and target data or the coefficients of the

polynomials defining the motion rates, the desired positions and orientations are read from the motion history file. If it is a nonmotion

segment, the appropriate subroutines are called or variables are defined to ensure these actions are carried out. If position control is

specified, the motion deltas for the current time segment are calculated.

B-125
Rev A, October 1985

39USS3HU0_3 311_M

9 "03 JlSl

dOl OU3U

S "03' NOUI

dOl OU3U

w

[I

U

0

"03'03SNII

dOl OU3U

"03',dU700

dOI OU3U

18730

3HI1135

SOd

]1U7AO7U3

z _" dOZ5U373U77U3

_]" dSUUO77U0

U

n

77nN

B-125a
Subroutine GRASP is called from SEGINIT when the option flag IOP equals

1 (this denotes that the arm is to grasp a load object). The subroutine

first checks to make sure the arm is not already holding an object and

that the desired object is not being held by another arm. The location

and orientation of the load object are then defined with respect to the

end-effector coordinate system, This ensures that the object's locatlon and orientation wlth respect to the world coordinate system _Iii

he updated correctly during a move and that the display shows the object

moving with the arm. The end-effector mass properties are modified to

include the load object to ensure the correct system response.

SUBROUT'rNIm" GRASP

T_ DESIGNATEO HOLDING ARM ANNOT OBJECT CURRENTLY

T_ DESIRED OBJECT ANOTHER NOT ARM HELO BY

CALL MATMPY TO GET OBJECT LOCAL

CG VECTOA IN WORLD COOR SYSTEM

CALCULATE TOOL ORT"G'rN TO OBJECT . _.j

CG VECTOR TN WORLD COOA o-_

I---

CALL MATMPY TO GET ABOV_-"

VECTOR 7"N TOOL COOR SYSTEM _ L,,

(:3::

CI::ILL MRTHIPY TO I:_LJT TOOL 0R'rG:]:N TO (.D U-J

C=: O-_

:OE_.IECT OR'T'G'r'N VECTOR "rN TOOL CODA SYST O"1 t.l..I

DETERM'T'NE OBJECT TO TOOL "'

TRANSFORMATION MATR'rx P--

I,i P4

C:=:

CALL HF:ITMPY TO _ OBJECT LOCFIL _NERT'ItR _--I =

HRTR'I'X ZN TOOL COOR SYSTEM

CALL ADDMAS TO COMBTNE OBJECT

AND TOOL MASS PROPERTIES

RETURN

END

B-126
2.3.3 RELEAS

Subroutine RELEAS is called from SEGINIT when the arm is to let go of a

load object. The routine first makes sure the load object to be released is being held by the current arm. If it is being held, the endeffector mass properties are reset to the values held before the object

was picked up and MATMPY is called to obtain the location and orientation of the load object with respect to the world. If the object is not

being held, a message is displayed to the user.

SUBROUTINE RELEFIS

FIRM TS HOLDING OBJECT

BE RELEASED

RESET RTMAS, ATCG,

AINMAT TO CONTAIN TOOL

ONLY DRTR

CALL MATMPY TO FINO

LOCATION AND

ORIENTATION OF LOAD

WAT WORLD SYSTEM

TO

I----

(.D

(:3=

(._ w.J

=i: --_

i,I

I--.

I--I

RETURN

END

B-127
2.3.4 ESCNTRL

ESCNTRL allows control of system motion through use of the Evans and

Sutherland extended switches (function keys) and loads the coefficients

of the polynomial describing the motion, PCOEF. Options include Indlvldual Joint control or end-effector control. For end-effector control,

either the base coordinates or tool coordinates may be used as the reference frame. Also, the controlled motion may be either translation

or rotation. The manipulator motion is always rate-controlled rather

than jolnt-controlled. The user may select which arm and joint is to

move.

SUBROUTINE _SCNTRL

iplO _ plqOQrXLl[ #__ZN_B _ Elqo_ ImlqAM. OP I.ZNK_ PI[_ R_N

STRTUS FUNCT_[m'ON I<E_?S (EXTENOED Sb4"rTcI-IE$) EVENT QUEUE

_ bl.i:¢l_l[ _ I<Lr_S _ Imam ]_'rl_qA_'r'xvI i¢_frRol- olr J'rs.

I_O1__I = STgTLIS OF "I'NID"rv'r'IDUI=II-- I='UNC:T'rON KEYS

SE'I" PI:::IRRM_TI[]::_ P'OR JOgrNT OR I_NO--F-.FP'I=CTOR CONTROL

SET PRRRME'TER _¢ImEC'f'FYM-".NG POSZT Mr'ON OR RRTE C_NTROL,

I_:IRRMlt'I'I[R _ TRI_N_ OR _ Jgf'. MOI'JCON

T_ ENID--I=I='I='I=CTOR CONTROL //FR

T_ _ CONVI=RT

TR I=::la S / RTTONI=:II_ 90.

JT. MOTION Dm'G/Sm'C

V_"LOI_"t'TY

SET VELOC'rTY CONVERT _O. TO RRO/Sm'C

VF:_LUE TO SET OE_/_[C V_'LOC'rTY

VF::ILUE OF _.2. TO RFIO/_EC SET MOT'f'ON

"f'NCI-IES/SEC _ PROFZLE

SET HOT'IrON SET MOT'I'ON COEFF. ,, S

PROFZLE COEFI w. . PROFZL.E COEFF. • VECTOR

ELEMENTS 1. 2. S. EL.EMENTS '%. S. So EL.EMENTS,, TO

TO SET VEL.OC't'TY TO SET VELOC'r'TY SET VEL.OCZTY

STOP T:EME FOR S_".HULRTZON m CURRENT q_ME -- STEP S'r_E

B-128
2.3.5 POSSPJT

POSSPJT is the executive routine that calls handling routines for finding the position of special Joints within the manipulator. Currently

only one type of special Joint can be Included.

SUBROUTINE POSSPJT

OO FOR EACH R_M

SPECIAL JOINTS ARE

INCLUDED

DO F0R EACH SPECIAL JOINT

\

TYPE EQUAL

GOROY JOINT

CRLL

POSGROJT (NULL)

._J

._,J

--m

z

B-129
2.3.6 RCNTRL

Subroutine RCNTRL is called from CNTRLR when rate control of the Joints

or end-effector is specified. If Joint rate control was chosen, the

joint rates TKD are calculated from their polynomial definitions and the

accelerations TKDD by finite difference methods. For end-effector rate

control, the defining polynomials are evaluated for the current time.

JACOB and SLVLIN2 are then called co transform these end-effector races

to individual Joint rates. Accelerations (of each joint) are again calculated using finite difference methods.

p--

t_J z

P'- p--4 P..

_ w p..

O=

uJ

._J _

_ =- u_ z

r-_ --J _ _-4

SUBROUTINE RCNTRIJNTTUL . EQ. 1

C_LC ROOT RNO OMOOT (ENO

EFF. RRTES)

T'_ JNTTUL . EQ. 2

PUT RDOT _ ROOm RNO OMOOT

RNO OMOOT OT 610RLD C 0 OR SYS_

_N VECTOR _sr , .... -__ -. vw_mm_m_ _

ENDVEL ,_¢"m_m,:wm..

CRLL JACOB TO CALC

JACOBZAN

CRLL SLVLIN2 TO SOLVE FOR

_O_NT RATES

PUT _OINT RATES IN THD

ARRAY

CALCULATE THDD

RETURN

END

B-130
2.3.7 P.ATEPRO

RATEPRO is called from subroutine CNTRLR when position control of the

manipulator is desired. The time allowed for the move is divided into

six equal portions. The first portion is defined to be constant acceleration. The next four are constant velocity. The last is constant

deceleration equal in magnitude to the first portion. The distance

traveled in the whole time is set to 1 and the appropriate distance

traveled, velocity and acceleration for each portion are calculated.

SUBROUTINE RRTEPRO

OELTIM = TIMSEG/B. 0

RCCEL = i. O/ (5. O*OELTIM**2)

V_LOC = RCCEL*OELTIM

T . LT.

mELTIM

m

RO =

QCCEL*T

OOCRSE

1" . inF.. _ml__"J:H

• _ T . U.T.

R m

. 5.,,HRCC:EL._DI_--_2

R m

RO -- VELOC

RDID =- O. 0

T . GE.

(5. O*OELTTW)

am

R i

R-_.VL'mLOC_. (T--S. O*_OLq..'r'J:M)

I_IDID = --_CC_-L_

z

B-131
2.3.8 PCNTRL

PCNTRL is called from CNTRLR when position control of the manipulator Is

to be used. Joint position control uses the segment rate profile defined by subroutine RATEPRO to calculate the joint positions, velocities, and accelerations. End-effector position control uses the same

rate profile to get the end-effector rates. JTPOS is then called to get

joint positions, and JACOB and SLVLIN2 are called to get the joint velocities. Joint accelerations are calculated by finite difference methods.

SUBROUTINE #CNTRL

JNTTUL . EQ. 1 #

EPOS = RR_OELPOS_SEGPOS

TI-I --

RR.M.OELTIM_SEGTI-I

THO --

RO_OI='L_TI-I

T_O0 =

RrlO.OELTI-I

CALL SETUP

TO CRLC ALL

PO5ITIONS

IN _ORLO

COORDINATES

CI::ILCULATI=" TIIE ANGLI=" OF

ROTATION

CI¢IL.L MRTMI_Y TO F'I"NO TIlE

Oi::t'I'ENTI:IT'I'ON TRII:::INSFORIMII_T'I'ON MI:=ITI::I'I'X

T_USING COMPLIRNCEFOACE/TORQUE CONTROL OR

SET POSREF (NULl..)

AND ORREF

CALL JTPOS TO GET JOINT

POSITIONS

CRLCULRTE ENOVEL, END

EFFECTOR VELOCITY

CALL JACOB AND SLVLIN2 TO

GET JOINT VELOCITIES

TIlO0 --

(TIIO--OLOTIIO) /$TPPRO

B-132
Rev A, October 1985

2.3.9 CABSM

CABSM uses a recursive technique to compute the absolute angular and

translational velocity and acceleration of each joint/link combination

in the system.

B-133
Rev A, October 1985

Z

O

0

W

Z

H

F3

0

n"

1

(7717N)

\ll_

\

!

W II

J

0

>

flH

H

II

+

÷

II

ff

÷

'11

B-133a
Rev A, October 1985

2.3.10 FORCE

Subroutine FORCE is called from DYNAM to calculate the force exerted on

each joint. The force at the end-effector is determined first. PTACC

is called to find link centroid accelerations, and the forces caused by

these accelerations are added to the end-effector forces to find the

force at each joint. If the system includes multiple arms on a moving

base, the reaction force at the base is a sum o£ the individual base

reaction forces from each arm that is attached to the base.

B-134
Rev A, October 1985

(77nN)

>n_ zmz w >i,-, ,,-I,_ z o ._o_ _,_i

u: H .__ , _ z Pi_'o_ _ ,

i i i-- t.,1 i_.r 1

_o o o o_ ",lz .-_

z_°7_ _.

_0 uz ,_ Is.!pz !_ ,,, ,,,._,o__,__.o_

4 _ 0i,-, _ _:_ I

(/) H Jm o :H. m leE, , _ !

_: ,,oz _o o_ _ _ _,_| .

"'ii'- _- !''' z .IF,-_._l_li_

,H J /_-:_

--0 ----0 /do o

B-13_a
Rev A, October 1985

2.3.11 TORQUE

Subroutine TORQUE is called from DYNAH to calculate individual joint

torques. The torques at the end-e_fector are determined first.

Torques at the preceding joint are then calculated by adding the

torques attributable to link inertias and centroid forces to the endeffector torques. The routine works back toward the base of the

manipulators adding the torques caused by inertias and centroid forces

to the cumulative torques thus far to obtain the current joint

torques. If the system includes multiple arms on a moving baser the

total reaction torque at the base is the sum of all the individual

reaction torques at the base from each arm that is attached to the base.

B-135
Rev A, October 1985

B-135a
2.3.12 ACTORQ

Subroutine ACTORQ calculates actuator drive torques for each joint. It

is called from REQUIR when running requirements analysis and from NLINK

when running response slmulatlon. This routine first solves for the

free axis of each Joint and the component of joint reaction torque about

this axis. The torque needed to overcome inertia and viscous and dry

friction are added to the joint reaction torques to obtain a total actuator drive torque for each Joint.

SUBROUTINE ACTORO

INCREMENT KRRM

INCREMENT N

SET NRXIS TO RXIS OF ROTRTION

BASED ON JOINT TYPE

NRXIS . NE. 0.

SOLVE FOR TERM OF

RCTURTOR TORQUE OUE

TO LINK N OF REVOLUTE

JO=NT

SOLVE FOR TERM OF

RCTURTOR TO_E DUE

TO LINK N OF SL=DZNG

JO=NT

ROO LINK N MOTOR INERTIR RNO

FRICTION TERMS TO RCTURTOR

TORQUE

00'-- UNTIL N -- NUMBER OF JOINTS IN

KARM

DO UNTIL KRRM = NRRM

RETURN

END

B-136
2.3.13 REQPRT

Subroutine REQPRT is called from OUTREQ to write run data to an output

file if this option was requested by the user. Data written to this

file includes time, anEular position, velocity and acceleration, translational position, velocity and acceleration, and Joint force and torque

vectors •

SUBROUTINE REQPRT

IDO FOR EF::::ICI-I ROBOTIC I:=:IRM

NRtTE TIME, RRM NUMBER TO PRINT FTLE

CONVERT THETI=:I VRLUES TO OUTPUT UNITS

I_IRITE I::::::ING. POSITION, VEL. • _CC. •

I::::lCT. TOR. FOR EF::ICI-I JT. TO FILE

130 FOR EI:::ICN JT. I:=INO ENO--EFF.

IMRITE TRI:=INS. I:=lr'ls. , VEL. . F:::ICC. TO F-rLE

T_ NOT END--EFF.

NR'rTE I:::IBSOLUTE F::ING. VEL. ,

RNG. ACC.

WR'I'T I_ ROT. MF:rr. FROM JT. TO "rNERT_.FII_. "rNERT_:I:::I

H_l"l'. FOR /'I'NK _'N I_(_R/PI

NR'rTE JT. FORCE VECT. , FORCE

VECT. RT JT. /LTNK CENTRO'rO

IAI:_-t-TE JT. TORQUE VECTOR

z

B-137
Rev A, October 1985

2.3.14 REQSOF

REQSOF is called from OUTRE_ to write a simulation playback £ile if

this option was requested by the user. The simulation playback file

contains joint angular positions, task commands and load objects flags

as a function of time and is used to replay the motion that occurred

during a requirements analysis run without doing the calculations normally associated with that run.

B-138
Rev A, October 1985

II0

Ol

C_

hl

rrWZH

b-D0

n"

O0D

Ol

rF

o:

C) H

k-O

rn0

rrI0C

_J

rr0

EL00

II00V

_0EZOH

O:

rr

Iii

O_O

LdH

rr

-4

Ii0

_00WD

_J>W

-rZH0

-]

0

O:0

_J

Ii

LLo

0_0

rro

rn

LL

rr IIII

O0

LZ

I..d L.9

6: i__. m

w F- _ H H cr

crX

-4

Z

-]

_J

n"

[3Z

Iii

B-138a
2 •3 •15 P,EQTP.Q

REQTRQ is called from OUTREQ to write a file of actuator torques as a

function of time if this option was chosen by the user. These data may

then be used to run a response simulation run.

SUBROUT'rN_" REQTI::_Q

WRITE TZME TO UNFORMRTTED TOROUE

OUTPUT F_LE

O0 FOR ERCH ROBOTIC ARM

NRITE JOINT ACTUATOR TOROUE

VRLUE5 TO FILE

RETURN

ENO

B-139
2 • 3 • 16 REQPLT

OF POOR QU._%,II"_.

Subroutine REQPLT is called from OUTREQ to write a file of various manipulator parameters as a function of time during a requirements analysis

run. This file may then be used to create x-y plots of these parameters

as a function of time.

SUBROUTINE AEQPL_T

IOR'1"R PLOT F'rL[ HERr1ER "rNFORMRTTON FOR fRCH RLOT FCLE TYI mlm"

TZM[ TOL.ERRNCE FOR CHECK NHEN BEGTNN'rNG FI"L[ 61R'I'TE

FIT STFIRT T'rM[

_P_Ol,_'r WOA WHZ_ OP TI-I Ir_/[ Pt.ffT" P'_J[ TYl_r.s TO _=_FIE

._ [NO--EFFECTORR'r s'rRR'r TZME PLOT PRCKRGE TYPE

_-" I (NUIJ-J

JOTNT POSTT'rON PLOT P!qcKIqGF Typl="

RT 5T.RR.r T._ME

mumm _ :Nqmmmm Kmm.Ml_ _ _ "m J (NULL|

WR'I'TE T't'ME. P_S'rTZnN OF ,..InZNT ¢_

_I='F::ICTTON FORCI='$ PlOT PIqCKI:IG[ TYPe"

RT STRRT T_ME

(:NULL)

[NULL)

[NULL)

[NULL)

-- -- -- "---",'J" "= "" " I CNu_.U [NULL)

MR'r.rE TTME,, ,,.IT. FORCE VEC.rORS. TORQUE VECTORS

_'_'_-'_Cr_MB'rNFITTON OF FIBOVE FOUR PLOT P_qCKRGE TYPES ,_

RT @TRRT TIME

(NULL)

_%1"I[ _ ,,.I"T._ I_DI.,. _. _ VKI... _ _. Ira'@It.. TON..

ImlCT. "r@R.

WR'r.rE T'I'Mf. _NI:_-EFF. RN{;. F=OS. • POS. . FORC f, T.0R@UE

Rm'TURN

m"NO

.--J

Z

w

B-140
2:3.17 __0$@DJT

POSGRDJT computes the position of the intermediate Joint in a special

Joint combination called a "Gordy Joint." This position is selected to

satisfy a constraint on the three Joints in this combination.

SUBROUTINE POSGROJT

SETUP X2, .X3, Y3e RNO EO W'91"TH COORD_J_:::ITII' Rx"rs

VECTORS

COMPUT[ CO_'FF'rC'r_'NTS F:Io B. C Rh_ O US'rNG THI_'SE

VI='CTORS

T_ O LESS THAN _ERO

PRZNT IIO'f'SCRZH'rNRNT EmRORI

WIRRN-rNG (NULL)

DENOMINATOR A = _FI_O

OTI-I = I:_I DTH -- 2 T"rME5

RTRN ((D--B) /1=1}

l:::IO0 OTH TO D'f'SPLRCEMENT OF "I'NTERMED"rATE JO'rNT

COMPUTE NEW POSTTION

.

T_ OIRECTION OF RESULT IS WRONG

SUeT_CT DTH B_CK OFF OF JOINT D_SPL_EEMENT

T_ DENOMTNATOR A ----- _ERO /F

DTH = 2 TIMES

OTH = 0.0

ATAN { {--0--8) /A)

ADO OTH TO JOINT DISPLACEMENT

COMPUTE NEW POSITION

_J

._.J

Z

w

B-141
2.3-.18 JTPOS

JTPOS is an iterative routine for finding a set of Joint angles corresponding to a desired hand position and orientation. The error DPOS in

position is calculated and then ORERR is called to find the orientation

error and transform it into a rotation vector. This rotation vector is

combined with DPOS, giving DP. The Jacobian relating hand motion to

Joint motion is computed and the set of six llnear equations [J](DTG) -

(DP) is solved for the Joint updates DTH. This procedure is repeated

until the desired position is obtained.

SUBROUTINE JTPO$

INITIRLI_E TOLERRNCES. LIMITS ANO

SCALING FACTORS

COMPUT_ POSITION ERROR OR

COMPUTE ORIENTATION ERROR OPWI

TOTRL POSITION ERROR NOT

LESS THRN TOLERANCE

CALL JACOB TO EVALURTE

JRCOBIRN

CALL SLVLIN2 TO COMPUTE

JOINT UPORTES OTH

ADO OTH TO CURRENT JOINT

POSITIONS

EVALUATE NEN ENO--EFFECTOR

POSITION

/

__1

_J

z

w

00 UNTIL TOTAL POSITION ERROR LESS

THAN TOLERANCE

B-142
2.3.19 CVTIN

CVTIN transforms llnk inertia matrices from local coordinates into their

equivalent representation in world coordinates for use in dynamic analysis.

SUBROUTINE CV T't"N

[30 _'0_ ERCH ARM

DO FOR EI::ICI,-4 JO-rNT

PT = TRRNSPOSE OF ROTRTION

MRTRIX (ROT) FOR JOINT

I:=IINN -- ROT TIMES I:=IINMRT TINES PT •

B-143
2 •3 •20 SPRGFOR

Subroutine 8PRGFOR is called from FORCE when the manipulator endeffector is modeled as a compliant entity. This routine calculates the

forces and torques at the end-effector reference point caused by its

having compliance.

SUBROUTINE SPRGWOA

CAI.I. MATMPY TO GET SPRING REF

POINT IN WORLD COOR

AO0 WORI.O ORIGIN TO ENO EFFEOTOR

.c oo 

CAI.I. ORERA TO CAI.C OEI.TA

ORIENTATION

(NULL)

CRLI- MI::ITMPY TO CRILC RESULTING

F-'OI::ICE$ I::INI "_ l'Ol::lOUl----$

CAI--C FORCe'S F:::INO TORI_ILJI=S OUI= TO

1.7"NEAA C_7"SI_I.AO_'MI=N7"S

FT"NO FORC_"S I:::INO TORQLI_'S I=:IT Till=" _'NO

EFFECTOR REF POINT

AF'TUAN

END

B-144
2.3.21 CNSTFOR

CNSTFOE is called from subroutine FORCE to compute the force on the endeffector and the torque about the end-effector reference point attributable to external constraints. These values are then added to the variables FEND and TEND.

SUBROUTINE C::NS7"FOR

COMPUTE F. FORCE ON END EFFECTOR

OWE TO CONSTRAINT

COMPUTE T. TOROU_ ABOUT END

EFFECTOR REF POINT OWE TO

CONSTRAINT

ROB F ANO T TO FENO RNO TENO

RETURN

I:"NI3

8-145
2.3.22 PTACC

PTACCcomputes the acceleration of any point in any link of either arm.

It uses the angular velocity and acceleration of the llnk to find the

acceleration of the point relative to the acceleration of the llnk's

origin and adds this to the acceleration of this llnk origin.

SUBROUTINE PTACC

OMEGA -- /TNK I=I=INGU/IqR VELOCITY

VEC = VECTOR FROM LINK ORIGIN TO

POINT

A/PI-II=I = LINK ANGU/I=IR AAACCE/ERATION

14CV -- OMEGR CROSS VEC

14ClMCV ---- OMEGI=I CROSS 14CV

AL_CV -- AlPhA CROSS VEC

RESUIT -- RICV _ _C_CV ÷

RCCELEIRRTION OF LINK ORIGIN

B-146
2•3.23 POSSENS

POSSENS is called from subroutine CONTROL when one of the feedback control laws is being used to drive a response simulation run. This routine obtains the discrete representation of the actual Joint positions

and also determines the actual joint velocities and accelerations.

SUBROWTIN_ I::'OSSENS

OISCRETI_E JOINT POSITION ORTA.

STH

JOINT VELOCITIES. STND =

[STN--OLDSTN1) / (TIME--SIGLRST)

JOINT RCCEL.. ST_O0 =

(STN--2. O*OLDSTNI+OLDSTN2)

(TIME--SIGLRST) *--2

/

OLOSTN2 = OLDSTNI

OLDSTNI = STN

DO UNTIL NJ . EO. NUMBER OF JOINTS

IN CURRENT RRM

RETURN

ENO

B-147
2.3.24 S IMPRT

SIMP&T outputs the condensed or full data printout to file. It prints

the position, velocity, and acceleration data for the arm at the time

when called. If input flag IPEINT equals 2, a limited amount of information is printed (only TH, THD, THDDj and TDR).

SUBROUTINE $1MPRT

DO FOR ERCH RRM OF ROBOTIC SYSTEM

MRITE CURRENT SIM. TIME

CONVERT TMETA TERMS TO I/O UNITS

FOR RLL JOINTS OF RRM

WRITE JT. RNGLES, VEL., RCCEL.

RNO ORIVE TORO.

PZO CONTROL

WRITE PID [NULL)

VRRIABLES

FULL PRINTOUT OPTED

WRITE EFFECTIVE

INERTIA MATRIX

NRITE JOINT/LINK

PRRRMETERS

[NULL]

RETURN

END

B-148
2.3.25 SI_LT OF POOR QU_L_

SII_LT allows the user to wTite a plot file for output. The user is

asked to choose from amo_ several dlfferent plot package options. The

chosen package determines which response simulation parameters are

written to the plot file.

Option i, the BRIEF PLOT PACKA_, writes Joint angular displacements,

joint angular velocities, Joint angular accelerations and drive torques.

Option 2, the END-EFFECTOR PLOT PACKAGE, writes end-effector translational position, force vector at the end-effector and torque vector at

the end-effector.

Option 3, the JOINT POSITIONS PLOT PACKAGE, writes translational Joint

positions.

Option 4, the REACTION FORCES PLOT PACKAGE, writes force Joint vectors

and torque joint vectors.

Option 5, the COMBINATION PLOT PACKAGE, writes all of the above--jolnt

angular displacements, translational joint positions, joint angular

velocities, joint angular accelerations, force joint vectors, torque

joint vectors, drive torques, end-effector translational position, force

vector at the end-effector and torque vector at the end-effector.

Option 6, the PID CONTROL PLOT PACKAGE, writes amplifier voltages, joint

reference positions, Joint position errors, end-effector reference position and end-effector position error.

Option 7, the FORGE/TORQUE PLOT PACKAGE, writes amplifier voltages, reference position, reference force, end-effector translational position,

force vector at the end-effector, torque vector at the end-effector,

error in position and error in force/torque.

SUf:_ROUTTNE $1MPLT

SET PLOT HEROER ORTR RECORO$ FOR ERCH _LOT TY_E

SIM. _T START TIME

BI=t'rEF PI:ICKI=IGE CHOSEN

WERO_R RE_ _ _frRR'r "r_ME

"I"_'- _ VEI-. II=lCr_, . I:INO OR'I'_ T_ _-

ENO--EFFECTOR _RCKRGE CHOSEN

i,,Illz'rlm I..I_JIInER R_ORO ZI m" @TI:R'r l'J_[

_ IDOL. I'ORC., ImlNO _ lmrl" lllwo-tl-ll_Ic'r01ml

JOINT POSITION PRCKRGE CHOSEN

WR'r'rt., H_'F_D_R R[CORO "rF _'RR'r -r%M_,

_RI'Tm" _m.. RNO _JOzN'r PO_. Vm'CTOR_

[NULL]

[NULL]

[NULL]

I [NULL]

I::tEI:=ICT'rON FORCES I:='F::ICKI:::II_E CHOSEN

I,,,111:!"I"1"1= MERn¢'R R[r_,_R n "rim" ,IITRRT "1"_M1¢

WR!"r _" "r_M f. FORC. RN_ "roR(_.l[_ lilt Jo'rNT_

COMBINaTiON _CK_GE CHOSEN

WR-r-rP" H_m'_lO_.ii Rp'CORO MSp" _-'rIIIIRT _1_

&4_:'I'_ T%I-_. R_ _&__&__ _ Till[ II:IBOVE PRRRIw_m'r_mR_

R_ _ONTROL _CKBGE CHOSEN

[NULL]

[NULL]

[NULL:)

MI:t'I'TE HEI:IIOER _ECOR n IF STIIIIT T'I'ME

1,4R%-r[ _ _ FI(P'. _. lmO@. _ ROT. IIFIFI.

.,.4

FORCE/TOROUE CONTROL PF:ICKRGE CHOSEN

IAR-I'TE: lIE:FIBER RECORI:) TF STI:::IRT TIME

IAR-i-TE T'I-ME, F::iNI::} JO-I'NT VOLTS. [NULL]

RETURN

B-149
2.3.26 FORTOR

FORTOR is called from subroutine CNTRSIG if manual force/torque control

is used to drive a response simulation. This routine calculates the

joint position error vectors caused by the error in the positioncontrolled components of end-effector motion.

SUBROUTINE FORTOR

POSITION ERROR VECTOR. ERPOS --

POSREF--POS

REMOVE FORCE CONTROLLEO COMPONENTS

FROM VECTOR ERPOS

T_ OOF .GE. 9

CRLL ORERR TO 0ETERNINE THE

ORIENTATION ERROR VECTOR

_MOV_ TORQUE CONTrOLLeD COMPONENTS

FROM ORIENTRTION ERROR VECTOR

COMBINE _OS. RNO OR. ERROR VECTORS

INTO TNE V_CTOR OELP

CRLL JACOB TO CALCULATE THE

JRCOBIAN

__J

Z

w

CALL SLVLIN2 TO SOLVE FOR DELTA

JOINT POSITIONS

SET REFERENCE JOINT POSITIONS

RETURN

END

B-150
2.3.27 FO_F

FORREF is called from subroutine CNTRSIG when manual force/torque control is used to drive a response simulation. Individual Joint torque

error vectors are calculated from the end-effector force error and torque error vectors.

SUBROUTINE FORREF

CRLCULRTE END EFFECTOR FORCE ERROR

COMPONENTS

CRLCULRTE END EFFECTOR TOROUE

ERROR COMPONENTS

STORE ERROR COMPONENTS IN VECTOR

DELFT

CALCULATE REFERENCE FORCE/TORQUE

VECTOR

CALL JACOB TO DETERMINE TWE

JRCOBIRN, RJRCOB

CRLCULRTE JOINT TOROUES. TORJNT

TORJNT--DELFT*RJRCOB

RETURN

ENO

B-151
2.3.28 CMPCTRL

CMPCTRL is called from CNTRSIG when active compliance control is used

in a response simulation run. This subroutine first calculates end

position deltas (ref-actual), joint control torques, and joint torque

deltas (control-sensed). The thetas are put through a derivative control block to get Joint torques. The Joint torque deltas are put

through a lead-lag filter in parallel with an integrating control block.

The Joint control torques are summed with the other processed signals

to get a total joint torque. This is then converted to motor amplifier

input voltages.

SUBROUTINE CMPCTRL

SET TVCVT. JOINT TORQUE TO VOLTS

CONVERSEON FRCTOR

CALL JRCOB TO CRLCWLATE T_E JACOBIAN.

RJROOB

OETERM_NE RJTRANS, TNE TRANSPOSE OF TNE

JRCOBIRN

CRLL MATMPY TO FINO TOR. TME INPUT

TORQUES

CALL MATMPY TO FIND TSIAS. THE @IRS

TORQUES

JOINT CONTROL TORQUES. TCTRL -- TOR +

TBIAS

DETERMINE TSENS, SENSEO FORCES ANO

TORQUES

ITORQUE OELTAS. DELTOR = TCTRL + TSENS

CALCULATE RJTORQ. JOINT ACTUATOR DRIVE

TORQUES

CONVERT JOINT TORQUES TO INPUT VOLTAGES

RETURN

ENO

B-152
2.3.29 PIDCON

Subroutine PIDCON is called from CONTROL when a control law is used to

drive a response simulation run. The routine takes the vector Of Joint

position errors and, simulating a PID control loop, calculates Joint

actuator voltages.

SUBROUTINe" P'r'OCON

SET JOINT ACTUATOR TOROUE TO VOLTS

CONVERSION FACTOR

CALCULRTE JOINT POSITION ERROR.

OELTN = TNREF -- STN

CRLCULATE ERRINT, THE ERROR

INTEGRAL

CALCULATE RJTORQ. JOINT RCTUATOR

TORQUES

CONVERT ACTUATOR TORQUES TO

VOLTAGES

RETURN

ENO

B-153
2,3.30 PIDFOR

Subroutine PIDFORis called from CONTROL when force/torque control is

being used to drive a response simulation run. This routine calculates

the joint actuator voltages caused by the force-controlled components

of manipulator motion.

SUBROUTINE PIO_OR

COMPUTE TVCVT. JOINT ACTURTOR

TORQUE TO VOLT5 CONVERSION FRCTOR

COMPUTE FERRINT. FORCE ERROR

INTEGRRL

CRLCULRTE RJTORQ, JOINT RCTURTOR

TORQUES

CONVERT JOINT ACTUATOR TORQUES TO

VOLTRGE5

RETURN

ENO

B-154
2.3.31 EF INRT2

EFINRT2 computes the effective inertia matrix (in Joint coordinates) for

a manipulator. The effective inertia matrix is an NxN matrix that gives

the Joint torques attributable to Joint accelerations. The (m,n) term

corresponds to Joints m and n and depends on the mass of the arm from

llnk n to the end-effector so the program evaluates composite masses,

centroids and inertia distributions for these "composite masses." Each

term of the effective inertia matrix is then evaluated as a combination

of dot products and cross-products among the joint axis directions and

locations and the mass parameters of the composite links (see Study Results volume).

F_NO TAANSLATZONRL CONTA_BUTZON TO

EFFECTIVE _NERT_R COMFONENT (M, N) ANO

EFFECTIVE INERTIA TERM (N.M) I TERM (M.N)

B-155
Rev A, October 1985

2.3.32 NLINK

Subroutine NLINK is called from DERIV during response simulation to

compute the base accelerations BASACC, BASOMD and joint accelerations

THDD. It sets the joint accelerations to zero, the base accelerations

to zero if moving base is simulated. The requirement analysis is used

to compute effective joint torques. If moving base is simulated,

BDRTORQ is called to input base driving torques and the base acceleration torques are calculated and DRTORQ is called to find the joint

driving torques. EFINRT is called to compute the effective inertias.

If moving base is simulated, SLVMBAS is called to solve for base and

joint accelerations. If dual arm control, SLV2ARM is called to compute

joint accelerations. Otherwise SLVTHDD is called to solve for joint

accelerations.

B-156
Rev A, October 1985

(7-nl)

1.

_ _!,

i- Z I

_ 0

Z H

VO_-tlU

z,,,_

Hd_ I

/ _1-_1

z_zdl

HE'I

Z ._m_b

H - Z_l_

I-- ._ (Jgl_

Ozz I[

D_ '

>0 "

U:N I

IFM/.

I _l.'

I ! '

! I

' !I ."] n":E ,-t_

_(H)01_ -- Z H !."

_mm Tm ,,; ,,,n_ !_z

X

'='_ H_ E 7" H' _ Ill '-'-- J-_:l_,oj

"°I°° _) O N u J i

>,-:E (f) rr W _'H

I I _ l-I>@ u • 03 "IL o "'"H:_-'|>"

i ,,_ I-I-w @ Z

W,,,-Em i L 3E u

[L!_E]"' '0 0 _z _ J

,,,_z° Z i "

_ ¢:_

Zu dla

__°°"'_ _ [

Zo _ 0:7- I'T

"_:_ _tz

o_o ,_®,_,_!_!_1 > _ I_ ff _-z_-°°H_

J ,,,ai_l_lOIW !WO: EE

_:u"J_: =_ H n" IEZ _

-il " lg'it - ' _ 0 O_ • J

/_ I

8 i/_l _ /,-IO_:In_}._,"

n'lO

3

__z

I

I

I

I

I

I

i

B-156a
2.3.33 SIMLMT

SIMLMTis called by DERIV and first checks the Joint displacements

against their limits. If any limits are exceeded, the Joint position

is set to that limit and the Joint rate and acceleration are limited to

zero. Similarly, the rate limits are checked and if any are exceeded,

the corresponding rate is set to that limit and the acceleration is

bounded by zero. IMOD is set if any positions or rates are modified.

SUBROU'I'IN_" $'rMLMT

IMOO = 0

O0 FOR EACH RRM

O0 FOR ERCH JOINT N

SET DISPLACEMENT TO UPPER BOUND

IMO0 -- 1

(NULL)

JOINT OISPLQCEMENT EXCEEDS LOWER BOUND

SET O_S_L_CEMENT TO LO_ER BOUND

_______ (NULL) m

IMOO -- I

JOINT _TE EXCEEDS U_ BOUND

SET RRTE TO UPPER _OWNO

• _OO = 1

JO_NT _T_ EXCEEDS LO_ER BOUND

SI_T I_I::::tTI_ TO I_01_11_ BOUNO

TI--IF=IN OR EI:UI:I/ TO -_ERO

_MOO -- l

__J

_.J

z

w

_-15 7
Rev A, October 1985

2.3.34 STOPFR

Subroutine STOPFR is called £rom INTGRT to simulate static friction in

the joints during a response simulation run. If the joint velocity at

the previous time step is not equal to zero and the sign is the opposite

o£ the sign o£ the current time step, the current velocity and position

delta are set to zero. If moving base is simulated, assume no £riction

at the base joints.

B-158
Rev A, October 1985

d

H

H

_ d_

_1- _.I4HoH

_0 0

_OzO

, ILIHW

U_-wb

zZ Z_

.E,H H _H

.wO_W

"' W

/FI

hF _

u !d •

w_[dl_W _ I_,r_ _7,

.z,o !F-o WF-IHFZ

o

!

o_

_. _ "'

I

|

O'

al '

B-15@a
2.3.35 ACTIVPIH

ACTIVPIH sets up the flags and variables activating a peg-ln-hole constraint if such a constraint is included. It sets up four point constralnts--two each (in orthogonal directions) at the top of the hole

and at the tip of the peg.

PUT TOOL REFERENCE _OZNT LOCRT_0N INTO PEGL0C

PUT (HINUS_ NOLE--RXIS 0IRECTION INT0 PEGOIR

DELl EQURLS UNIT VECTOR RLONG X--RXIS CROSS

PEGDIR

7\MAGNITUD_" OF Om'L I N_'AR ;zERO

[DELl -- UNIT VECTOR NORMF:::ILI:_I=" O_'L1

I:=ILONG Y--I=IXIS

OEL2 -- UNIT VECTOR OEL2 I DELl CROSS

I=ILI3NG "_--I=IX'I'S PEGO'I'I=I -...I

.--I

INITIALIZE FLAGS FOA 4

DOUBLE--SIDED POINT CONSTRAINTS

POINT 1 AT PEG TIP PLUS ROLE

AAOIUS ALONG DELl

POINT 1 AT PEG TIP PLUS _OLE

RAOIUS ALONG OEL2

POINT 1 RT HOLE ENTRANCE-PLUS

HOLE RROIWS RLONG DELl

POINT I RT NOLE ENTRANCE PLUS

HOLE RADIUS RLONG OEL2

z

B-15 9
2 • 3.36 DRTORQ

The DRTORQ routine calculates the torque output from each joint motor

by using a control algorithm strategy or reading them from a file. The

calculations are based on the torque constant for each Joint and the

armature current.

SUBROUTT"N I=" C)RTORO

_m'AD TOROUE FLAG. TTORO. _ I

Rmm'RD T'ffM_" I_ROM TOR_U[ ff_..E ZNTO TN_XT _ _

' KARM - o =,

INCREMENT KARM _ _-

O0 UNTIL KARM -- NARM c_

CALl-- L"rN_'NT TO _--__HPUTE NL_,.I TORI_L,IE C.J_NTROL P-" _ _.-

sZr=NRI-- 7"_HE COEFF$. I-,-I P--

KF:IRM -- 0

"rNCRI_M_'NT KARM

N = 0

TNCRP'MI:'NT N

TDR CN. KRRM) • OR"f'VZNG TOR:LIE =.,

"rNTERPOLRTT"ON BETWEEN TORL FIN n TORN

00 UNTT"L. N _ NUMBER OF _IOTNT8 IN KARM

mm

DO UNTIL KAI:_M = NI:::I_M

B-160
Rev A, October 1985

2.3.37 EFINRT

EFINRT computes the effective inertia matrix for a system containing

one or more arms on fixed or moving bases. The effective inertia matrix PHI gives the base torques, forces and joint torques attributable

to base and joints accelerations. The dimension of PHI is (ND, ND)

NARM

where ND = (6 * NBAS) + l[ NJ(1)

f=-i

B-161
Rev A, October 1985

b

G:

Z

H

h

I11

W

Z

H

3

0

nr

3

(7717N)

..I

H

B-161a
2.3.38 SLVT_DD ORiGI_4_L p;__c

OF POOR

SLVTHDD solves for unknown Joint acceleratlonsq__tralnt reaction

forces for a given arm state and Joint drlvlng forces. All zerovelocity joints are assumed to have zero acceleration. If the friction

forces needed to produce zero acceleration are greater than the static

friction force, the acceleration is assumed finite and the equations are

re-solved. Similarly, the constraints are assumed active and if the resultlng constraint force is in the wrong direction the constraint becomes inactive and the equations are re-solved. This process is repeated until all conditions on the friction forces and constraints are

satisfied.

SUBROUTINE St.VTHOO

DO FOR ERCH JOINT

_Im_O_NT RRTE I _ERO RNO _TRTZC FRZCTZON NONEERO /pp

FLEV -- TRUE I FLEV -- FRLSE

l

ZNZ'T_RL_tE FLER l FL_V

Tk_,NO CONSTRRINT$ OR EERO--VELOCZTY JOINTS EXIST /_

CALL JRCOB TO EVRLUATE THE JRCOBIRN Z

TTEMP = TEFF

_ CONSTRRINT$ EXIST /_

00 FOR ERC_ CONSTRRINT FORCE

¢.J

I---

CONIITI:IlIqZN7 RCTIV[ _ FC_II_-'I[ MRS _I'MP_R $_I'IIN

MRI<E CONISTR1KT.NT "I'NIIqCTZ'VE t {NULL_

T_ ¢_N_rlRIRZNT _ IqND RCIDE_ _ ZHImROImEFI

MRK[ CON-_ITRR_J_IT RCT'ZVE I tNULIJ

U.. Tk_I:ILL CONSTRR'I'NT CONIDIT'I'ONS SRTTSFIEID

--.J O0 FOR I=ILL JO'I'NTS FOR l_ICl-I FL;'V 1 TRUE

oe L RCCELERI::IT]_ON TS NON_ERO _/F

---J SET FRLSE ca-)

FI--_I=I -- (NULL) m _ ,= a --_

•to _c_l _WJ_llZON Z

._ TRUE c_=mcr --

O0 UNTIL RLL FORCE RNID CONSTRRZNT CONO_'TI"ONS $1::IT'I''_F'I'ED

---J

--J

Z

B-162
2.3.39 LININT

Subroutine LININT is called to set up the coefficient for performing

linear interpolation between two vectors.

SUSAOWTIN£ LININT

FIRST VALUE OF

INDEPENDENT

VARIABLE EQWALS

SECONB VALUE

CFI= (COEFFICIENT

FOR FIRST

VECTOR) EQUALS

_ERO)

CF2 EQUALS ONE

CF2 = (CURRENT

VRLUE OF IND.

VAR. -- FIRST

VALUE) / (SECOND

VALUE -- FIRST)

CFI = 1.0 -- CF2

B-163
2.3.40 LDVOLT

Subroutine LDVOLT is called from DRTORQ when a file of actuator voltages

is to be read in and used to drive a response simulation run. At the

correct time the routine reads time and voltaEe from an existing file.

LININT is called to interpolate the best voltage for the current simulation time. The control voltaEe is then calculated from this.

sueRouT'I"N_" _OVOl.T

"rROCNTL . EO. 1

T_ CORRECT TIME OATATO REAO NEW

REAO _N TIME

l:

i

I REAO _AROWARE

L VOLTAGES

I

iOO UNTIL N . EO. NUMBER

!OF JOINTS

O0 UNTIL KARM . EQ.

NUMBER OF ARMS

CALl.. LININT TO 7"NT_IqPOLIqTI:" OI:ITI:I FOI_

CURRENT B'rMULI:ITT'ON T'rMI:"

CAI.CUI.ATE STMBI.ATION CONTROlVOI-TI=I(BES

RETI-IRN

_NO

z

w

z

B- 164
2.3.41 CALCI

The CALCI subroutine calculates the amplifier current values for each

of the joints in the system giveu the motor parameter values and the

state velocity.

SUe_OUTINE CALCl

KRRM. RAM COUNTEA = O

INCREMENT KI:II_M

N. JOINT COUNTER ---- 0

ARMATURE I {N. KARM) = (AMP

GAIN_VOLTAGE -- BACK

EMF*JT. VEL. ) /RRM RE$.

!

i

BO UNTIL N = NUMBER OF JOINTS

FOR KARM

DO UNTIL KARM = NARM. TOTAL NUMBER

OF ARMS

RETURN

END

B-165
2.3.42 SOLVE

SOLVE is used to solve a set of N linear equations in N unknowns. It

sets up an identity-ausmenting matrix, calls GAUSS to invert the original matrix and then multiplies this inverse times the right-hand side

of the equations to obtain the resulting solution.

SUBROUTINE SOLVE

PUT ORIGINAL MRTRIX INTO C

FORM IDENTITy AUGMENTING MRTRIX IN

AUG

CRLL GRUSS TO PERFORM ELIMINRTION.

PUTTING INVERSE OF C INTO RUG

MULTIPLY RUG BY RIGHT--RRNO--SIOE OF

ORIGINAL _ EOUATIONS TO GET X

B-166
2.3.43 GAUSS

GAUSS performs Gauss-Jordan elimination with partial pivoting on an augmented matrix system to reduce the system to row-echelon form during the

matrix inversion process. The largest value remaining in a column is

used as the pivot value for that column during reduction.

SUBROUTINE GRUSS

STRRT WITN ORIGINAL MRTRIX IN A AND •o__ _oo_=_o_o_x _

oo_o__c_ _v_oo_ _oo_o _o_

_oo_ _o__ • o_ _ _o _o

REDUCE COLUMN I OF A T0 _ERO

PERFORM SRME RO_ OPERATIONS ON B

REDUCE PIVOT ROW OF A TO

START WITH 1

PERFORM SAME OPERATION ON B

MOVE PIVOT RO_ UP TO RO_ I

B-167
Rev A, October 1985

2.3.44 BASINIT

Subroutine BASINIT is called from BCNTRLR at the beginning of each new

base motion people segment. The coefficients of the polynomials defining the base rates or the desired positions and orientations are read

from the base motion profile.

B-167a
Rev A, October 1985

D

@

0

E

W

rr

d

h

m

d_

_H

_H

7OO

H

OR

'_H

_H

2_

H

E

0

E

_H

E

U

300N70UIN0331UU

]SWOUOJ SIN]I31JJ]O3

J]O3UOOU]_

zo

W

B-167b
Rev A, October 1985

2.3. _5 BRCNTRL

Subroutine BRCNTRL is called from BCNTRLR when rate control of the base

is specified. The rate polynomials are used to calculate the base

translational and angular velocities. The base positions, orientations

and accelerations are calculated from the velocities.

B-167c
Rev A, October 1985

3

3

\L

J

i

I

o

_> _oo

11.11

m

i

II

L.

!_. Ll

H CI

'_0 (iC"

,0 tl

n

O0

0

.I

LI

C"

/_- tl

i

o,, \,,I

I

I

II

liI'

to_l,..,

.1olo_

Otmlj

[ tl.

i ld

I

I

!

I

/I,-I

r i

'"I'

r(/1

_,,iH

_X

'' [0:

|

O

It'

Fo

I°

• Q

a Io z

o::

trl _:

" 0

I

r

nxI

_'z

F

o_

(,3 ILl

0 (f)

J O]

0o

a.l

.J

d

8

m

o

&

o_

9

?

o_

o_

O3

I

I

o

ILl

B-167d
Rev A, October 1985

2.3.46 BPCNTRL

Subroutine BPCNTRL is called from BCNTRLR when position control of the

base is to be used. The rate pro£ile is used to calculate the base positions, velocities and accelerations.

B-167e
Rev A, October 1985

J

r_

H

Z

O

O.

W

Z

H

b

3

0

n"

_0

3

U)

0

(0

o

03

3

W

Z

W

H

(I

J

3

U

!

(I

U

z_

WO:

_z_

U 3

W o

H

n"

, 0

E

0

W

a3

3

W

Z

Iii

F

(I

/

3

U

J

E

U

o•

o_

rn

J

hl

>

m

W

O

J

Q:

O

o

U

U

E

(I

m

W

rn

U

/

E

U

Z

O_

3

tW

nO

Z

hl

B-167f
Rev A, October 1985

2.3. _7 TRACKING

Subroutine TRACKING £s called from CNTRLR when sensor control of endeffector motion is chosen by the user. This simulates tracking of a

target by a video device mounted on a manipulator end-effector.

B-167g
Rev A, October 1985

hl

Z

H

o_

U

H

H

H

Z

H

I

W

0111

-Jo

39ugg3NUOUU3INIUd

Iii

Z

39Ugg3NUOUU3INIUd

z

irr

II

I'

o

III

B-167h
Rev A, October 1985

2.3.48 INITTAR

Subroutine INITTAR is called £rom TRACKING to obtain the coordinates o£

the target corner points.

B-167i
Rev A, October 1985

8E

E

IH

H

Z

H

hi

Z

H

H

3

0

rr

rn

D

II

O

fl"

Iii

Z

IT

O

U

b

n"

H

Ii

Ii

O

I!1

H

E

Z

H

o

n"

O

O

U

O

N

J

I

,_111

,X

HZ

"'1

IT

¢

b

8.

W

E

FW

rr

E

b

I

W

0

u_

oo11

,,H

on,

FUL

E

H

hk

O_

E

,,,6

Z

IT.

0CI

U\

J

H II

-r",.,

tl.H

0"

rr

file

WF

E

7W

Ha:

OE

cr

OH

ON

rr

E

Fn"

rr

rr II

E

0

0

6

g?

H

IT

E

h

O.

3

b

W

IT

0

Z

hl

s-16?j
Rev A, October 1985

2.3.49 ANGLES

Subroutine ANGLES is called from TRACKING to calculate the Euler angles

given a direction cosine matrix.

B-167k
Rev A, October 1985

W

d

E

h

0

HO

W_

Z 'i

EX

2

H

Z

(3

C[

3"

/Hi

,., _b_: W

3 E,,,.

z u4_E

III

- IJ.

0_ww

/U

E1

_o._-_- j_ _H

I-,_ H_ HE

z --

I -

W

EE NO H

WO _u_ E

i-,j u_ j

W = c[

"-I _--,,

I.%1 u

:Iii

I

'H

E

O

E

,h

H_

U]H

H

Iii

H

E

I

D

U

I

E

O

\Ii

O

E

!

E

/F

r

\

J

J

D

Z

O

E

-!

,-I

z_ "_

HZ

U]E

WZ

BE

ZE

E

T

O

J

J

3

Z

Ii

I

' (3

/I

/FI|

0

Z

III

B-1671
Rev A, October 1985

2.3.50 _E

Subroutine NENFRAME is called _rom TRACKING to obtain the coordinates

o£ a three-dimensional vector in a new coordinate system.

B-167m
Rev A, October 1985

I,i

Ii

3

III (3

Z W

III '

Z III

H 0

H

0

0 Z

i

' A

/

_o

n

X

U ii

J

E

U

÷

Hj

!10

P

0

.J

0

X

0

.J

Z

il 0J

0

11

;'1

W

F

H

.J

H

F

Z

3

0

0

E(]:

UO

H

_J

H

F

Z

3

0

0

"z

z

(3,-,

_W

11F7

x

Z

3

tIii

n,

0

Z

III

B-16Sn
Rev A, October 1985

2.3.51PERSPECT

Subroutine PERSPECT is called £rom TRACKING to calculate the perspective projection of a three-dimensional vector. The result is a twodimensional vector.

B-167o
Rev A, October 1985

H

U

W

n

W_

n_

W

Z

H

H

0

n_

D

r_

X

\

X

b_

E

I--

03

X

X

\

n

03

X

L

IT

(I

H

X

Z

D

H

W

0

Z

W

B-167p
Rev A, October 1985

2.3.52 HARALICER

Subroutine HARALICKER is called £rom TRACKING and calculates the pointir_ angles of a camera relative to a rectangular target, based on the

perspective projection o£ the four corners of the rectangle.

B-167q
Rev A, October 1985

nI

w

z

H

H

O

0

n"

3

\h

\

Z )-

(I J

W W

brt"

(]:H

jf-I

rr

W_ H

E

U

W

n"

Z.

H 1

W i

cr _

I

t

I

i

I

I

!

!

I

I

B-167r
Rev A, October 1985

2.3.53 BDRTORQ

Subroutine BDRTORQ reads base torques and forces from a file and computes new torques and forces by linear interpolation.

B-167s
Rev A, October 1985

(77nN)

O0

D

0

n"

D

0

D

z

_,_

rn

z

D

0

D

O

Z

W

B-167t
Rev A, October 1985

2.3.54 SLVMBAS

Subroutine SLVMBAS solves £or base and joint accelerations £or a given

arm state and driving _orces.

B-167u
M,,,,., o_F p,I _ a .t'3__, _.. *.-- _'_

UKII.111_/"_L- li r" .....

OF POOR QUALITY

Rev A, October 1985

(77RN]

[7717N]

[7717N1

B-167v
2.4.1 SLVLIN2

SLVLIN2 finds an optimal solution X to a linear set of equations AX = B

where the magnitude of each component of X is bounded -XLIM(N)< = X(N)

<" XLIM(N). The program first sets up matrices D, which forms an orthogonal basis for the reachable space of A, and C, which provides the conversion from D space to AH space. The matrix AN is also set up; it conrains vectors in the null space of H along with the initial solution VH

and is used as the tableau for linear programming. Once an initial solution is found, linear programming by a modification of the Simplex method is performed; the magnitude of the result is maximized subject to

the constraints on X. This solution is then scaled to give the final

solution.

SUBROUTINE SLVLIN2

SCPlL._ CC3L.L..JIv_ Or" PD (6:L'V2"JqG Iql-I) SO THAT L-_M_'TS ON X _mE _--O_

PUT FIRST COLUMN OF Rl-I -rNTO O

OO FOR EFICIM REMI::_-rN-rNG COLUMN OF FIN

SUImlTRII_CT (_OMP,0NI='NT_i OF" CI_II.JMN I:::II.,I_NG CO/I.IMN5 OI _ 0

AESULTING COLUMN VECTO_ IS

IFORM NEXT COLUMN OF C M_T_IX

NOT _E_O /

PUT COLUMN

INT0 NULL

SP_CE M_TR_X _N

USE D lIND C TO SOLVE LI"NEI:::IR EI_UI:::IT-rONS

T_ EOIJI:::IT'FON:S SOLVED EXF:ICTLY /

"r'F::IPR = 0 "T'A_I_ = 1

"_NULL Slml:::lCE EX'I"STS F:::INID TI-II:::::ITsImF::ICE B NOT "rN /_

GENE_I=ITE T'N-rT-rI:::IL Bl_S-rC {==

FEaSiBLE SOLUTION

O0 _LE FU_T_E_ OPT_M_RT_ON

I_S POSSIBLE

C_LL REPCOL TO FURTNER

OPT_MZ_E

SCI::ILE

z

_ESULTS {USING XLIM] TO GET W_NSL

SOLUTION

B-168
2.4.2 REPCOL

REPCOL replaces column ICOL of matrix A where A represents the tableau

for a linear programming problem, X represents the variables, and the

limits on each variable are plus or minus one. REPCOL first finds the

largest allowable change in the free variable (the variable that corresponds to the column being replaced), and the new constraint variable

that limits this change. The solution is updated and tableau A is modified to reflect this change of constraint variables.

SUeI_OUTINE l_m'ImCOL

FINO NNETNER TO INCREASE OR OEOREASE

VARIABLE OORRESPONOING TO fOOL

FINO _OW CIRON) NNION ALLOWS

SMALLEST CN_NG_ IN TNQT V_AIABLE

WPORTE X VECTOR TO REFLECT TNAT

CN_NGE OF VRLWE

OO FOR ERCN ROW N OF R

R (N, "t'COl_) I

I=I I:N. 1"1:O1.__) /R l:'rROl,,l, "r'COL)

OO FOR ERCN COLUMN I EXCEPT ICOL

OO _OR ERCN ROW N

R(N,I) I R[N.I) --

R (N. ICOL) *R (IRON- I)

B-169
2.4.3 0Z_

Subroutine ORERR is used to find the chanEe in orientation between two

coordinate systems. The error in orientation is computed and then

transformed into a rotation (maEnitude less than pi) about a unique

rocation axis. This axis is computed as the cross-product of two of

the columns of DOR, and the rotation anEle PH is computed by (COS(PH) =

I÷.5*(X.DX+Y.DY+Z.DZ), where X.DX is the dot product of the X column of

ROR with the X column of DOR, etc.

SUBROUTINE ORE_R

TN_ _NGL_ OF _OT_TON

RETURN

END

B-170
2.4.4 0UTUN

OUTUN is a functiou =hat conver=s a value from in=ernal (metric) units

=o =he user-specified inpu_/outpu= units by dividing by a couversion

fat=or.

F'IJNCTTON OIJTUN

CONVERT A VALUE FROM INTERNAL TO

I/O UNITS USING OUTUN =VAL/CONUNIT

RETURN

END

B-171
2.4.5 ICVTATD

ICVTATD is a function that returns a digitized number when given a real

value. If the value is outside an allowable minimum or maximum, it is

set equal to the appropriate limit. The digitized number is then computed using the following relation:

B = (VALMAx(VAL- VALMIN). VALMIN) * (2MBITS - 2(NBITS-I))

where

NBITS = number of bits available for digitized value,

7ALMIN = minimum allowable value for VAL,

VALMAX - maximum allowable value for 7AL,

VAL - output value.

ICVTATD, the digitized number, is set equal to the closest integer to B.

VAL

FU CT=O 

(ANALOG VAI._UI =') . aT. " //7

VALMIN (MINIMUM

VAL _ VALMIN

T_ VAL .GT.

(NULL)

VALMAX ALLO_EQ) (MAXIMUM /F

VAL = VALMAX (NULL)

A = (VAL--VALMIN) / (VALMAX--VALMIN)

B = A_2_NBITS--2_ (NBITS--I)

ICVTATD = CLOSEST INTEGE_ TO 8

RETURN

ENO

B-172
2.4.6

Rev A, October 1985

BORERR

Subroutine BORERR is used to find the change in orientation between two

coordinate systems. The error in orientation is computed and then

transformed into a rotation (magnitude less than pi) about a rotation

axis referenced to the base coordinate system.

B-172a
Rev A, October 1985

B-172b
oRIGINAL PAGE IS

OF poOR QUALITY The Postprocessing Function

The program POSTDRVR is the postprocessing function driver. The following set of routine functional descriptions and VCLRs (visual control logic

representations) are the modules found in the postprocessor function of ROBSIM.

1.1.1 I 1.1.2 I

[ ' SETLU ] LBR-HELP ]

2.1.1 ! 1.2.11 i 2.2.4 i

[ RDSYS l [ SETUP l 1 GRAF'x

-- ERRMSG

1.3.18 ] 1.4.5 I

[ BASPUT i [ CETM

1.2.12

SETUP2 1

-- MATMPY

1.3.19 I

[ CRPOJ

2.3.2

I GRASP

1.3.14 !

I ADDMAS

1.3.15 I

ADDMAS2

1.3.16 [

RCICR

1.3.6 I- 1.3.21

I II

E&S Routines

- E&S Routines .._1--a. 6 i

I LOGO

- E&S Routines

3.1.1 I

I SIMOTION I

RELEAS

- MATMPY 2,2.11

-- E&S Routines

I

-- E&S Routines

132° _ ED:l"_:tiTnes I ] l ROutinesI

1.2.13

HARD--COP 1

1

Figure B-9.- Functional block diagram for POSTDR VR.

3.0 -

POSTDRVR I

I

2.1.1 J

R DSYS

- ERRMSG

1.2.11

I

I

I 2.2.11 1 3.2.3 I 3.2.4 sETuP ] [ ES"AUS] i "'"'AX ]1

3.2.2 7_"E"_ _--_tin_

[ I

POSTGRAF

li ill DIALS 1 DATOUT E&S Graphics Routines

-- E&_ Routines " E&S Routines

1.3.21 1.2.13

! FORM ] [ HARD--COP 1

3.2.1

LDTHET

2.3.39 ]

LININT ]

.3.18 I

BASPUT ] [ -_ETM

E&S Routines

1.4.6 I

LOGO 1

- E&S Routines

1.2.12

J SETUP2 I

1.4.5 ]-- MATMPY 1.__3.19 ]

[[ i m

I- MATMPY 1.4.5 I

I CRPO 1

I

AXES

-- E&S Routines

3.1.3 I

I ROBPLT

3.2.5 i

I I SCAL

1

3.2.6 I

II,c_ II

-- E&S Routines

DI3000

Plot Routines

FGLOOUTF;_:_

B-173
TAble B-VII. - PROGRAMS EMPLOYED IN POSTDRVR

3.0 PO STDRVR

3.1.1 SIMOTION

3.1.2 ILDWMOTIN

3.1.3 ROBPLT

3.2.1 LDTHET

3.2.2 POSTGRAF

3.2.3 MINMAX

3.2.4 AXES

3.2.5 SCAL

3.2.6 TICFLRK

B-174
3.0

ORIGINAL m-,,,:- _

OF POOR QUALITY

POSTDRVR

The program POSTDRVR is the postprocessln8 function driver. It operates

in an interactive mode, promptlnE the user for the postprocesslng option

desired: replay robotic system simulation motion, replay simulation

versus hardware motion, parameter versus parameter plots, or terminate

POSTDRVR execution. For simulation replay, option i, subroutine

SIHOTION, is called. Option 2 provides a comparison of hardware and the

corresponding simulation motion through subroutine HDWMOTIN. If option

3 is selected, ROBPLT plots any of the data computed and written to one

of the seven types of plot file packages during the requirements or

simulation analysis tools functions.

F_ROGRAM POS"I'ORVR

SET F:F:_OCES$O;:_ NO0 _" = 3, FOR POST PF_OCESS'I'N$

5_'T ERROA COOE -- 0

CRL/ S--ETI_I.J 3"0 Sl="r PROGRI=IM I_I_F'I=IUI--T L.O@'I'CI=II_ UNI"r NUM61='RS

I:::ROMImT US£F_ FOR OI=I='RI=IT'rON MOO1='. "I"M01:31=

AEAO LJS_"_ RESPONSE

M_SS_G_

B-175
Rev A, October 1985

3.1.1 SIMOTION

SIMOTION is called during the postprocessing function to provide a replay of the robotic system motion produced during a previous run of the

requirements or simulation phase of the analysis tools function. It

opens the chosen robotic system geometry file and simulation output file

for each graphics replay, and calls GRAFIX with the displacements at

each time step to update the system motion display.

B-176
o

ORIGINAL r:_,,_,;--

OF POCR QU_,LITY

N

I

i°

Rev A, October 1985

B-176a
3.1.2

Rev A, October 1985

HDWMOTIN

HDWMOTIN is called during the postprocessing £unction to provide a replay of the robotic system motion produced during the requirements/simulation analysis tools functions versus the actual motion that occurred

during the corresponding hardware run. It opens the chosen system geometry file, simulation output file for graphics replay and hardware file

containing recorded joint theta values. It calls POSTGRAF with the

hardware and simulation displacements at each time step to update the

system motion display.

B-177
Rev A, October 1985

ZH0

nlIZH

Fn"m@

m

D

i3C

nJ .4

-__

"2Z

£

.4

O

o_

lk

ZH_

Io_

.J

O

Ii N|5\

"t

InUH

nn- @O

O. NH

Ip. ZNEEU

.JnmH

IU.

(1;

n,

I-m

Cl

0.

-1

B-177a
3.1.3 KOBPLT

The ROBPLT subroutine plots the contents of one of several choices for

plot package formats on a Hewlett-Packard X-Y plotter or a VAX VT125

graphics terminal. It uses exclusively the DI3000 plot package.

ROBPLT requests the user to select the appropriate one of seven plot

file types that was written at the user's discretion during the requirements or simulation analysis tools functions: the brief package, the

end-effector package, the Joint positions package, the reaction forces

package, a combination of the above four packages, the P_D control package or the force/torque control package.

5U_OUT'rNm" _08F::'t_'r

: ,_nlml auul lumo Im_l __ -_- ',L_ nLdm' --- m

_m_ imm_ _mm v Im ,m_ m my imi4 MmB ',_mmm_mmm

_ ,v,nmm ._ me 'v_ _mmm

i_ _mmm _mm.

W ,_immmmm i.qll.

--,,mm --" --.----'''=" ,---.------

.m,

B-178
3.2.1

OF POOR QUALrFY

The LDTHET rou=tne loads the =beta values for each Joint from dlrect

read of the hardware control theta values £11e, It is called from

HDWHOTL_/during the postprocesslng function for each simuJ.atton tlme

step, There is a limit of one theta signal value for each joint that

can be read,

SUBROUT=NE LOTHET

_E_ r_T_ _L_. I' _OT_Er. "NE" _

dr "

-- ,,,

I1--

INCmEMENT KA_M

Ill

KARM : 0

_-_ _NCREMENT KARM

N _ O

ZNCREMENT N

DO UNTIL N -- NUMBER OF _O_NTS _N K_RM

_O UNTIL K_RM -- N_RM

_ET_N

END

,-,7, C_H
Rev A, October 1985

3.2.2 POSTG_"

Subroutine POSTGRAF provides the motion graphics capability in the postprocessing £unction for HD_IOTIN, a replay of the simulation motion versus actual hardware motion. The value of the difference in the simulation and hardware thetas is displayedt along with the environment, robotic arms, targets and load objects.

A

B-180
Rev A, October 1985

m

3

_: IzZ z

• _ _ _--

i-_._ _

_ou oOH Hz

°i Ii

Itl

Z_

L H E

H ,._

I-.j" >"

0_ _

_._i _i

J_

,

!_ i.,

77 !

i (77nN) _,

' _-

z '- _7

-- jD

-_!.__ ' ._i

,,_, _o

u'¢O !i

_E_" J

_,,,_,_i ocmlo _ _ ©

Z_m 1 J

o/_.j O H

If"

i_

J!:II_

B-180a
3.2.3 HDtH_

HIb_AX searches the postprocessor plot file for the maximum and minimum

values to be used in scalln8 the axes of the plot. The x and y ILtnimums

and maximums are found for all parameters the user chooses for plotting.

SUBI_OUT-rNI:" MTNMF:IX

I"(_. I::IRM P--J3LINTEi::I. " 0

N,.INT ,,, _Tt:IHT RE_._IR II'l P'OR l"ypIr OF I_..o-r lW']_L_

= _ "I"NCFtEMENT NPLT

t_j_ _ ____. \ NPLT -- I::l_=tl=lMLr'l'lS'l_l CHO_IF--_..N TO I_LI:_'I"

130 LIN_j.[_ N.,JNT --, NIJMglEI=I OF REC.,_F:tI_S FOR KF:II=IM

I30 LINrJ.J_ ENI= OF I_L13"1" F_ILE

B-181
3.2.4 AXES

Subroutine AXES draws the x and y axes for a plot during the x-y

plottlng option of the postprocessor.

SUBROUTINE RXES

MZN QNO HRX EERO CROSSING CHE_K FLRG$ -- O

XMZN VRLUE --'O

CNULL3

XMRX VRLUE I 0 , I

X _X _ CROSSING FL._G = I I

T_,,, ' yM-rN VI_ILU E I 0

Y M.T.N _.ERO CRO_S:ENG _ -- I {NUL.L.1

T_ YMRX VRLUE I 0

CNULLJ

Y _ _ CROSS3:N_ _ -- I {NULL]

Y M::EN :;=ERO GROSSING FLI=iG I 1

SET Y =_F-{:K = Y _ v_-u= / Y _ v_u.¢

CNECK IS NEGF::IT'r'VE ORIGIN

O. SET Y ORIGIN I O. Y OII_=_N l y MIN VI:II_UE

CALL JMOVE TO HOVE PEN TO (XMIN. YORG} COOROINRTE

CRLL _RRN TO ORRW X--_XZS FROM OR=_=N TO CXM_X, YORG)

__ x Mz. ERo CROSS NGFLA =

!.-..- ,% /F

U-'l CE tl X ORIGIN I X MIN

_ SET X ORIGIN -- O. V_LUE

CRLL JMOVE TO HOVE PEN TO CXORG. YMIN] COOROINRTE

{:RLIm JORI=IW TO ORRIA Y--RXI"_ FROM OR'rQTN TO {XORG. YMRX3

RETUI:qN

ENO

B-182
3.2.5 S_

For both user-selected automatic or specified scaling of the postprocessor plot file, routine SCAL is called from the ROBPLT option. It

chooses the most appropriate scale for the x- and y-axls tic marks. It

finds the exponent of the scale base, the tic mark spacing and the xinimum tic mark value. The _Inlmum value, XI, to be used for the scale,

and DX, the scale increment between tic marks, are chosen to satisfy

specific constraints.

SUBrOUTiNE SCAL

RO0 A SMALL TOLERANCE TO THE MINIMUM

VALUE, FOR T_UNCATION

SUBTRACT A SMALL TO&ERANCE FROM TNE

MAXIMUM VRLUEo FOR TRUNCATION

RI_ST TRIAL VALU_ SCALE INCAEMENT. OX. --

(MAX--MIX) /NUMB_ P_RTITIONS

SET EXPONENT -- LOG OF DX

T--_EXPO IS NESATIV_ AND NOT AN INT_ER

EXPO I EXPO -- _l . (NULL)

SET BRSE = IO. --,,'rNTIEI_ER PRRT OF" _:-'XPO

MlqT-rC $CRL'rNG I-S

I (NULL)

SET MZNZMUM TZC M_K VSLUE I ZNTEG_L ._

MULTIPLE OF STEP _E

_TU_N

B-183
3.2.6 TI_[Rg.

Routine TICMRK actually draws and labels the tic marks for a plot during

the postprocessor function.

SUBROUTINE TICM_K

I COMPUTE X .,I:iNO Y I:tXT$ LI:mNI3TI-I$

OV--R RRN m_" Or" X--RX:E_ OR:X:_-_CN TO

I DRR_ T_C M_RKS XMRX VRt.t_[

1::30 AT X--AXIS TIC Mi:::IAK WI::tI='I::I:IUENC::Y

IOETERH:ENE LENGTH CF TIlE rM.III:tlI:t_NT "l--J:(:: MRRK _Im'IL.F::IB_'L.T6.1_" X--l::Ix"rs T'TC MI:::IRKS 6J'rT6.1 Vf::II_UI=S

OETERMZNE: I_ENGTH OP" TILE: EXI¢_ONENT P'OI_ TH ImmX--4clX:T.S

SCRI..E

OUTPUT I='X_ONI=NT _'Of:_ Till=" X--i::::iX'I'S SC:i:::ILI="

i r_RRI.I T'ItC MI:I_KS OVE]:_ RRNGE O# m" Y--RX'I"_ OR:ZG:ZN TO

YMRX VRLUE

DO F::tT Y--i:::IX'I'S TT"C:: MAF::_K I=RI=I:IIUENCY

I

OETERH't'NE I_ENGTI-I CF TIlE CURRENT "t--J:C HRRK LFIBEL I

I

Lm_IBE:L THe" Y--I:::IX_'S TIC: MI::II_K_5 M'rTH VI:_LU_'S

C:_:Tmm'RM3".NEL.[NGTH OP" THE EXPONENT IL'OR THE Y-'4=IXIS

OUTPUT I='XPON_mNT FC3I::_ THe" Y--AX"rS SCF::_L_"

_m'TU_N

_'N[D

B-184
THE PREPROCESSING FUNCTION

The program PREPDRVR iS the preprocessor function driver. It operates in an

interactive mode, prompting the user to for the preprocessor option desired.

Valid options are, create or modify a CAD/CAM object file or terminate

PREPDRVR program execution. Figure B-tO shows the functional diagram for

PREPDRVR and Table B-VIII lists the subroutine employed.

4.0

i

1.1.1I 1.1o2I 4.1oiI 1.4.3I

I

42._I 4.2oiI 4°2°2I 4=.3I 4.2.4I 4.2_I

PREPDRVR !

i [ 1.4.2

Figure B-10. Functional Block Diagram for PREPDRVR

B-185
Rev A, October 1985

TABLE B-VIII.-PROGRAMS EMPLOYED IN PREPDRVR

4.0 PREPDRVR

4.1.1 BLDCAD

4.2.1 TRANSF

4.2.2 LINE

4.2.3 POINT

4.2.4 CURVE

4.2.5 SPLINE

4.2.6 GRAFCAD

B-186
Rev A, October 1985

4.0 PREPDRVR

The program PREPDRVR is the Preprocessor function driver. It operates

in an interactive mode, prompting the user for the preprocessor option

desired. Valid options are, currently: create or modify a CAD/CAM object file or terminate FREPDRVR program execution.

B-187
Rev A, October 1985

(9 _ W

Z H O

H Z O

M D E

01 j H

U U •

O H I,l [d

Oom O 01

_ J o z

> W II E 0

II O. b O.

0 [U j Z Ol Ol

W n" II

EO O.

E U nWm H

e H

I,I

O

0

2

Z

O , ,-4

H

H II

E

E W

W 0

& O

O E

H

W

E

0

EW w

wu_ _

t-- :W

H E iH

E O

Z

W

H

_,,,8

_n_I,I _ O 0 o

JEOff w

m n

Lg_ E 0 @ 0

o

([D

O O • _Eu

H D

I,i D F

U J

0 F &

E W I

& In 0

E

J &

b j

W E

O

SS_33UXUUUBI7

dq_HU_SRU0J

dq_H-USq77U3

'1,11,1

&X

OW

jrr

H>

HE

ZO

On

Ill

OE

O0,.

O. []

Z o,,,

O]

B-187a
4.1.1 BLDCAD

BLDCADis called in the preprocessor driver. It reads a CAD/CAM

initial graphics exchange specification (IGES) - formatted file, calls

the appropriate entity routine to fill the real number and integer data

arrays (for graphics routine interaction), and i£ graphics display is

opted by the user, calls a graphics routine to display the entities or

an Evans and Sutherland device. The file of IGES data may be saved for

input during the system definition function detailed graphics

generation for arms, loads, environment or targets. The format read

from the IGES database follows the documentation for version 2.0

published by the National Bureau of Standards.

B-188
;.o.

OF POOR QU_

Rev A_ October 1985

/

_J

D

Z

D

E

U

D

W

Z

H

P

D

0

n"

rn

D

U1

B-188a
Rev A, October 1985

.2. i TRANSF

TRANSF CAD/CAM IGES read entity-associated transformation data and

loads the transformation array for the rotations and translations to be

applied to the entity before graphics display.

B-189
Rev A, October 1985

I!

U1 ZE

n" kWZHb30

n"

fn

-m

wZ

30

jH

EF

>E

J

n'LO

wZ

mE

En"

31-

jO

EH

WH

rrEY W ff

jO

HU-Z

nE

EE

OH

DE

EO

WU- E

EL0 0 W

U_HH

>_H n_

EWE

O

ZH

0

EO

EH

n_H 0 •

LF-

@0Z n" EH H 0 >-

EJO n

jE

ZE3HWE

QZW

B-189a
4.2.2

Rev A, October 1985

LINE

LINE reads CAD/CAM IGES line endpoint entity data and loads the line

array for the graphics display.

B-190
Rev A, October 1985

WzHWZHH0E

003

0_

01W

3M

JH

EZ

>H

0

EE

W[]

00Z

2W3

ZW

ZHJ

/EW

rrWJH

I!

r-1

CI: UOEWE

Ii0N

>-XE0

II

E0h

>-EEEEEWWHZH

Iii ZH/OE0/

UEEE

ZE3PWE

0ZW

B-190a
4.2.3

Rev A, October 1985

POINT

POINT reads CAD/CAM IGES point entity data and loads the points array

for the graphics display.

B-191
Rev A, October 1985

ZH0

0. WZHbD0

r_

--m

WDJE>

n, wmEDZJW

n" WJH

U. OEO0EWE

bZH0

ELh0¢

cr0

u.

0

u_

)-EEEE

0_WUbZH

F- ZH0nD

(E0

.J

UHI

R. E

IT

L_

Z

ITDbWE

0ZW

B-191a
4..2.4.

Rev A, October 1985

CURVE

CURVE reads CAD/CAM IGES circular arc data and loads the arc points array £or the graphics display.

B-192
Rev A, October 1985

>

3

0

B-192a
4.2.5 SPLINE

(not implemented yet)

Rev A, October 1985

B-193
_.2.6

Rev A, October 1985

GRAFCAD

GRAFCAD displays the CAD/CAM IGES entity data on an Evans and

Sutherland graphics device.

B-19_,