NASA Contractor Report--178050

Evaluation of

Automated

Decisionmaking

Methodologies and

Development of an

Integrated Robotic

System Simulation

Martin Marietta Aerospace

Denver Aerospace

P.O. Box 179

Denver, Colorado 80201

Contract NAS1-16759

March 1986

(NASA-CE-178050) EVALUATION CP AUTOMATED

DECISION MAKIKG METHODOLOGIES AND

DEVELOPMENT OF AN INTEGBATED ROEOTIC SYSTEM

SIMULATION: STUDY RESULTS (Martin Marietta

Aerospace) 3S p FC A:3/M* AJ1 CSCL 09B G3/63

N86-24287

Unclas

16081

National Aeronautics and

Space Administration

Langley Research Center

Hampton, Virginia 23665
EVALUATION OF AUTOMATED

DECISIONMAKING METHODOLOGIES

AND DEVELOPMENT OF AN

INTEGRATED ROBOTIC SYSTEM

SIMULATION

Prepared by:

Dennis C. Haley

Bonni J. Almand

Mark M. Thomas

Linda D. Krauze

Keith D. Gremban

James C. San born

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.

MCR-85-665

MARTIN MARIETTA

DENVER AEROSPACE

P.O. Box 179

Denver, Colorado 80201
CONTENTS

Page

SUMMARY .............................. 1

INTRODUCTION ............................ 2

Background ........................... 2

Contract Objectives ....................... 2

Report Organization ....................... 3

TASK 21, USE OF CAD/CAM DATA

TASK 22, MULTIPLE ARMS WITH MOVING BASE .............. 9

Kinematic Analysis ....................... 9

Requirement Analysis ...................... 12

Response Simulation ....................... 14

TASK 23, TASK COMMAND MODULE .................... 17

TASK 24, RANGING SYSTEM SIMULATION ................. 22

CONCLUDING REMARKS ......................... 33

REFERENCES ............................. 35

Figure

1 CAD/CAM generated representation of T3 manipulator 5

2 Base link modeled with CAD/CAM data 5

3 Base and link 1 modeled with CAD/CAM data 6

4 Base, link 1, and link 2 modeled with CAD/CAM data 6

5 Product database evolution 8

6 Kinematic representation of a serial manipulator 11

7 Multiple arms on a single movable base 13

8 Manipulator system with target 17

9 Definition of the perspective projection 22

10 Sensor coordinate frame rotated relative to the reference

coordinate frame 23

11 Coordinates of the corners of the target 27

12 Perspective projection of the rectangle in the sensor frame . . 27

Table

I Primitive level commands ....

iii

PAGE BLANK NOT FILMED
SUMMARY

The computer simulation called ROBSIM, developed under the four

phases of this contract provides the capability to perform kinematic and

dynamic analyses of user defined, rigid link manipulators. The kinematic

analysis provides positions, velocities, and accelerations of all parts

of the manipulator for a prescribed motion. The dynamic analysis

includes requirements analysis which calculates system loads for specific

motions, and response simulation which calculates the motion resulting

from a prescribed set of driving torques or feedback control law. This

report documents the fourth phase of the contract which built on the

basic capabilities developed in the phases one through three.

The ability to utilize CAD/CAM generated data to model system

components was added. This feature allows the user to access a file of

CAD/CAM data written according to the Initial Graphics Exchange

Specification, version 2.0. The ROBSIM preprocessor function can read

this file and then write it in another format that can be read in and

used for the detailed graphics modeling of system components. A graphics

display is available during the reformatting procedure.

The capability to simulate multiple arms with movable bases was also

added for this phase of the contract. A system may be modeled with all

of the manipulators attached to a single movable base, or each robotic

arm may be attached to its own independent base. With this capability,

the user defines motion trajectories for both the manipulator joints or

end effectors and the movable bases. Position and/or force calculations

are then carried out for the bases as well as for the joints and links of

the arms. This addition has great utility in light of the emphasis being

placed on telerobotics and space station and satellite servicing.

To make the motion trajectory specification process easier, a task

oriented motion specification module (or task command module) was added.

This software allows the user to define a manipulator's motion by

choosing command from a list or menu. Each command in the menu is a

combination of some of the lower level commands currently in place in

ROBSIM and is separated into these commands when actually implemented to

control motion.

The fourth addition gives the user another option for controlling a

manipulator's motion. This option simulates a video camera mounted on

the end-effector of an arm tracking a target. Currently, only stationary

targets are modeled.

-1-
INTRODUCTION

Background

This report documents the results of work performed in Tasks 21

through 24 of contract NAS1-16759, Evaluation of Automated Decisionmaking

Methodologies and Development of an Integrated Robotic System Simulation.

It was prepared by Martin Marietta Denver Aerospace for the NASA Langley

Research Center in accordance with the contract statement of work. These

tasks constitute Phase IV of an ongoing activity addressing technologies

relevant to the design and operation of advanced manipulator systems.

Phase I concentrated on the identifying and evaluating applicable

artificial intelligence techniques and on developing a framework and

mathematical models for the computer simulation. These results (Phase 1)

were documented in 1982 in NASA contractor reports 165975, 165976, and

165977. Phases II and III developed and implemented the software

necessary to model a complete manipulator system and then perform

kinematic and/or dynamic analyses. The results of work done under Phases

II and II were documented in 1984 in NASA contractor reports 172401,

172402, and 172403.

This project is motivated by the realization that NASA advanced

missions require the increasing use of automation technologies for both

economic and performance reasons. Development of these complex

technologies in a timely and cost efficient manner requires the extensive

use of computer simulation tools that allow options to be evaluated and

compared before building hardware prototypes.

Contract Objectives

The objective of this phase of the contract was to build on the

simulation capabilities developed in Phases I through III, specifically

by adding the following enhancements:

1) The ability to utilize geometry data from a CAD/CAM database to

model manipulator system components;

\

2) The capability to simulate multiple arms on a single movable

base or multiple movable bases;

3) The addition of a task command module to make manipulator motion

specification easier. Each task command is broken down into a

series of lower level primitives previously used to define a

motion;

4) The ability to control a manipulator's motion by simulating a

video camera mounted on the end-effector tracking a stationary

target.

-2-
Report Organization

This report consists of three volumes—the task or study results and

two appendices. The study result volume documents the technical aspects

of the software developed and implemented for Tasks 21 through 24 of

contract NAS1-16759. Each task is reviewed in a separate section of the

report, along with any impact it might have had on other parts of the

program. Appendix A is the updated users guide, which reflects any

changes caused by implementation of Tasks 21 through 24. Any page that

was changed will be marked Rev A, October 1985 on the upper outside

corner. Appendix B is the programmers guide and contains Visual Control

Logic Representations (VCLRs) of all the subroutines as well as an

outline of the program structure. As in Appendix A, any pages that were

changed or added as a result of incorporation of the of the four new

tasks are marked Rev A, October 1985.

-3-
TASK 21, USE OF CAD/CAM DATA

Completion of Task 21 required the design of an interface between ROBSIMand CAD/CAM-generated models. This interface enables the ROBSIM user to

model the detailed graphics of a system more easily by accessing and

using data converted from a CAD/CAM model file. This can be used for the

detailed graphics modeling of manipulator components as well as for

modeling the system environment, target objects, and load objects.

The interface designed for this task reads CAD/CAM data formatted

according to the Initial Graphics Exchange Specification (IGES) version

2.0. This specification (IGES) is supported by several vendors of

CAD/CAM systems to facilitate the exchange of information between

different systems. IGES version 2.0 supports a number of geometric

entities; however, for this implementation of ROBSIM the following

entities are translated:

1) Points;

2) Lines;

3) Arc Segments;

4) Transformations.

These entities are decomposed into individual (x, y, z) points that

represent their polynomial curves, and are written to files that may be

read in as obstacle entities for detailed graphics modeling during system

definition or simply displayed on a graphics terminal. A line is defined

by its two endpoints, whereas a circular arc segment is divided into 10

linear segments. Transformations include both linear and rotational

components and may be applied to any of the other geometric entities.

Figure 1 shows a display of a manipulator generated on a Computer-Vision

CAD/CAM system, and converted using ROBSIM for display on an Evans and

Sutherland graphics system. Figures 2 through 4 show the use of the

interface software to create the detailed representations of the first

three links of the manipulator.

The work done for this task demonstrates the usefulness of this

effort, and the need for work in this area to continue. However, certain

concerns should be noted before extensive development is conducted.

During development of the software for this task, files were

generated on three different CAD/CAM systems that all wrote files to the

IGES version 2.0 specification. In all of the files the data needed to

define the geometric entities were there, but the formats varied. This

indicates a need for more work in the area of standardization. In

addition, CAD/CAM systems are still closer to automated drafting tools,

than a tool for parts definition. Geometric features such as flanges,

holes or fillets are not defined, only low-level entities such as arcs,

splines, and lines are. To accomplish feature definition solids

definition, and the definition of other intrinsic properties, further

revisions to IGES and evolution in the product definition database format

are needed.

-4-
Figure 1. - CAD/CAM-generated representation ofT3 manipulator.

Figure 2. - Base link modeled with CAD/CAM data.

-5-
Figure 3. - Base and link I modeled with CAD/CAM data.

Figure 4 - Base, link 1 and link 2 modeled with CAD/CAM data

-6-
In an effort to address some of the shortcomings of the IGES

specification, the McDonnell Aircraft Company has been working on a

Product Definition Data Interface (PDDI) under contract to the Air Force

Wright Aeronautical Laboratories (AFWAL). PDDI attempts to fill in some

gaps and provide another step in the evolution to a more universal

standard that would be an interface between all engineering, design,

development, and manufacturing functions. Some of the milestones of this

evolution are shown in Figure 5. More information on PDDI may be

obtained from the ICAM CM Library, Wright-Patterson AFB, OH 45433-6533.

-7-
in

co

O)

Tf

00

CO

CO

O)

Figure 5. - Product database evolution.

-8-
TASK 22, MULTIPLE ARMS WITH MOVING BASE

Task 22 extended the analysis capabilities of the ROBSIM program to

include multiple manipulator systems with moving bases. The ability to

determine forces and moments at the base of each arm in the system is

included with the moving base capability. This section of the report is

divided into the following three subsections:

1) Kinematic analysis

2) Requirement analysis

3) Response simulation

Kinematic analysis

The forward kinematic solution for a manipulator with a moving base

is identical to that described in NASA contractor report 172401. That

is, starting with the specified velocities and accelerations for the

base, the velocities and accelerations of each link are obtained by

successive application of the following recursive relations:

u>i+1 = UK + 9i+1 S_i+1 (rotating joint i + 1)

a) = oj. (sliding joint i + 1)

Vi+1 = Vt + UK x (R1+1 - R±) (rotating joint i + 1)

Ii+1 - 1± + J-i * (Ri+1 - *i> + 81+1 it+i (sliding joint i + 1)

«i + "i * 81+1 S.+1 + 9.+1 Si+1 (rotating joint i + 1)

£ (sliding joint i + 1)

+ x x " ^^ +

 -

 x ^ ~ -) (rotating joint i + 1)

ai+1 - a. + .. x [<,. x (R .+1 - R .)]+« . x (R.+1 - R^

+ 2^ x 9i+1 li+1 + 9*i+1 S_i+1 (sliding joint i + 1)

-9-
where:

—i+l: angular velocity of link i + 1

UK: angular velocity of link i

e : joint i + 1 relative angular velocity

s^ : unit vector along joint i + 1 rotational axis

\r : translational velocity of joint i + 1

v.: translational velocity of joint i

—i

—i+l: vector locating joint i + 1 in the world frame

R.: vector locating joint i in the world frame

o_ : angular acceleration of link i + 1

a : angular acceleration of link i

—i

9 : joint i + 1 relative angular acceleration

.a.,,: translational acceleration of joint i + 1 in the world

1+1 frame

_a.: translational acceleration of joint i in the world frame

All vectors used in the preceeding equations are expressed in terms of

the world reference frame, and the origin of the [X-j] frame is placed

at joint i (Fig. 6).

To accommodate base motion analysis, an option was added to allow

the user to define a base time history profile. This new option is very

similar to end-effector control of a manipulator using the time history

profile specification. The two options available for defining base

motion within each time segment are:

1) Kate control;

i) Position control.

Tnese options and their implementations are discussed in NASA

contractor report 172401. The outputs generated from this option are the

oase position, velocities and accelerations that are used as inputs to

the recursive equations previously stated to determine the link

velocities ana accelerations.

-10-
Figure 6. •

Kinematic representation of a serial manipulator

-11-
Requirement analysis.

The reaction forces and torques at the manipulator joints are

evaluated as discussed in NASA contractor report 172401. The forces and

torques are successively computed starting from the terminal link of the

arm ana proceeding back to the base. Assuming the only external load

applied to the intermediate links are the gravity forces acting through

their centroids, the recursive equations for the joint reactions are

x m.

+ [I.] «. + 10. x [I ] u.

i i — i i — i

where :

f_ : force at joint i

-i+l: force at Joint i + 1

n^: mass of link i

^ .; acceleration of centroid of link i

JL: acceleration of vector due to gravity

I : inertia matrix of link i in world frame

h..: vector from origin of link i to the centroid of that link

_t.: reaction torque at joint i

ti+l: reaction torque at joint i + 1

All of the vector quantities in these equations are expressed in the

world coordinate system. Another capability added for this task, is the

ability to put multiple arms on one moving base (see Fig 7). In this

case, the reaction forces and torques at the base are computed as a sum

of all the individual base reaction forces and torques contributed by

eacn arm that is attached to the moving base.

-VI-
ORIGINAL

OF POOR

ROBOTIC SYSTEM SIMULATION PROGRRM CROBSIM) K^^TCQ^^QgZ^^MlBM

CURRENT TZME f!SFTCI - O. OOO JOINT TRflVEL STOTUS

1 1

J

U

 i

) ff"^^^|*" '

RRM1

JNT1

JNT2

JNT3

JNT4

JNT5

JNTS

RRM2

JNT1

JNT2

JNT3

JNT4

JNT5

JNT6

VRLOJE

O. OO

0. 00

0. 00

0. OO

0. OO

0.00

VRLUE

0. 00

0. 00

0.00

0. OO

0. 00

O. OO

X MRX

O

-97

98

0

O

0

i

X MRX

0

-97

98

1

0

0

0 |

1

j

Figure 7 - Multiple arms on a single movable base.

-13-
A preferred method for modeling a system containing a moving base

with multiple arms attached is to model one of the arms with the moving

base geometric and mass properties, and the other arms with zero base

properties. The locations of all first joints will then be referenced to

a common base frame.

Response simulation

The difference between the new modification and the simulation

discussed in NASA contractor report 172401 is the inclusion of the base

motion in the dynamic equations of motion. These equations are solved

for the base accelerations and the joint accelerations at each processing

time step. To calculate the base and joint accelerations, the equations

of motion are reformulated to add the six rigid-body base accelerations

(three rotational, three translational). The controlling equation for an

N-joint manipulator with a moving base is

[1]

I. - [A (£) ] _e_ + _b 03, i)

where

r_ : (6 + N, 1) generalized torque vector containing 3 base

reaction torques, 3 base reaction forces and N driving

torques at N joints

_6_: (6 + N, 1) acceleration vector containing 3 base angular

~~ accelerations, 3 base translational accelerations and N

joint accelerations

[A(6)J: (6 + N, 6 + N) effective inertia matrix referenced to

the base and joint coordinates, accordingly

b(9, 9): (6 + N, 1) vector of position- and velocity-related

~~ effective torques, including external loads, gravity,

velocity-related inertia terms and friction

The calculation of these terms is described in NASA contractor

report 172401, except for the new modifications to the calculations of I.

and [A (9)] .

Generalized torque, JL - Besides the N-joint actuator torques this

generalized torque also includes the three reaction torques and 3

reaction forces at the base. These base reaction torques and forces can

be generated by two methods:

1) Read a file of base reaction torques and forces versus time;

2) Read a base motion profile (not currently implemented).

-14-
The base reaction torques and forces file which can be generated

during a requirements analysis run could be used along with the joint

actuator torque file to drive the response simulation » thereby

validating that the requirements analysis and response simulation.

If a base motion is specified, part of the equations [l] will be

solved for the N-joint accelerations using the known base motion profile

and the N-joint torques. Then the backward recursive formulas are solved

next for the base reaction forces and torques.

Effective inertia matrix, [A1] - As discussed in NASA contractor

report 172401, the effective inertia matrix [A1] due to the link's mass

distribution is given by

[A1] = [J1

T

[M±]l [0]

[bT~j7i~F

T

[J 1

where

[M ]: (3, 3) diagonal matrix with link mass [M^] along the

diagonal

[I.]: (3, 3) link i inertia matrix about the link i centroid,

. referenced to the world frame

[A ]: (6 + N, 6 + N) link i effective inertia matrix referenced to

. the base and joint coordinates

[J ]: (6, 6 + N) Jacobian matrix relating the motion of link i to

the joints. Because the base is modeled as a 6-DOF rigid

body, the expressions:

Rotating joint j _< i

and

J. = 0 ( Sliding joint j < i

1° i * 0 I

can be applied to the base degrees of freedom 1 _< j _< 6 with

.R. = vector locating the origin of the base frame, ,

referenced to the world system, for 1 < j < 6

vector locating the base eg in world system,

_S. = unit vector along base axes, referenced to the world system.

Sc - f i S - i

-5 (o)

 x 15 (o) L

( ? (

rotating base joints

1°)

}°( sliding base joints

(i) 4^ j < 6

-15-
Therefore, for i = base

- V

0

0 1

0

1

0

R :l£

1 110 ' 0 I 0 I 0 I 0 I I I

I I I

'ololo'o'o o

I 0 I 0 |0 I 0 I 0 I 0

I 0 I 0 I 0 I 0 I 0 I 0

Similarly, for joints i from 1 to N, a component vector of

can be written

i lL

 x (£TPK - R.)

r

A =/T^J _(l£ _ ~"3

S.

~3

Rotating joint j < i

J

1

-

—i

Sliding joint j _< i

where: 1 _< i _< N and 1 _< j _< 6 + N

Ti

For j > i the components of —j are zero because a displacement at

joint j has no effect on the absolute motion of link i.

The total inertia matrix is calculated (as described in NASA contractor

report 172401) using the symmetry of the inertia matrix, and a recursive

procedure to complete the mass properties of the composite system of link

i through N.

In the full simulation case where a torque file is input, the equations

of motion are then solved for the base accelerations and joint

accelerations in terms of the state (base positions and velocities, joint

positions and velocities), and the generalized torque J. .

-16-
TASK 23, TASK COMMAND MODULE

Task 23 added a task command module to the motion specification

section of ROBSIM. The intent of this task was to simplify the motion

specification process by implementing a set of task commands, in which

each task command is a combination of some of the motion commands

previously used.

The commands for motion specification previously in place and

described in NASA contractor report 172401 are now called primitives or

primitive-level commands. The task commands added for this task are

essentially just groupings of primitives. Both command levels are now in

place and the user may define a motion specification file using either

level. The primitive-level commands are grouped into two sections—

motion and nonmotion. Table I lists these commands.

TABLE I. - PRIMITIVE-LEVEL COMMANDS

Motion Nonmotion

joint rate controljoint position control

end effector rate control

end effector position control

grasp a load object

release a load object

change tool reference point

time delay or wait

force/torque control on/off

active compliance control on/off

The task level commands added in this phase of the contract were:

1) Pick up an object;

2) Place an object;

3) Normal or guarded arm movement;

4) Hold current position;

5) Change the end effector reference point;

6) Use operator control;

7) Set response simulation control mode;

8) Sensor control of end effector position.

When implemented, each of these commands is separated into

primitives that are then written to a motion history file and used for

motion control the same way as before. Sensor control of the endeffector position was also added to the primitive command level to keep

consistancy between primitive and task level commands.

-17-
Picking up an object combines position control of the end-effector

to move the arm to the load object and the nonmotion primitive grasp to

combine the load object's mass properties with those of the end effector

for subsequent motion. The position to which the end-effector moves is

defined with the load object as the grasp point. This point is

transformed to desired end-effector position at the end of the move by

multiplying the grasp point by the load-to-world transformation matrix

and adding this to the load object location vector

where

r is the grasp point vector in world coordinates

tf is the time at the end of the move

[P.] is the load to world transformation matrix

•jT is the grasp point vector in the load coordinates

_L is the location of the load object in world coordinates

The desired orientation of the end-effector at the end of the move is

found from the approach vector and the end effector y-axis orientation

[Tc] -

where

Xg is the load object approach vector (unit vector) in the

world coordinate system (also a vector defining the

direction of the end-effector x-axis in world coordinates)

Y. is a unit vector defining the direction of the end-effector

y-axis in world coordinates

Z is a unit vector defining the direction of the end-effector

z-axis in world coordinates

[T ] is the end-effector-to-world transformation matrix

Placing an object combines position control of the end-effector to

move the object to a desired location and the release command to return

the end-effector mass properties to those it had before grasping the

object. Implementation of this option uses the algorithms currently in

place.

-18-
The move arm task command allows the user to specify either a normal

or guarded move. A normal move is simply position control of the endeffector. A guarded move is also position control of the end-effector

but is used when in close proximity to another object to avoid

collision. The time allowed for the move is doubled (slowing down the

motion) and force/torque control is turned on to detect if any collisions

occur. No sensed force would indicate that the move went smoothly and a

sensed force or torque would mean a collision had occurred and the path

of motion must be altered.

The wait or time delay command holds the arm in its current position

for a length of time specified by the user.

Changing the end-effector reference point changes the point for

which motion is defined when the user controls the end effector instead

of the individual joints.

The operator control command has not been implemented yet.

Setting the control mode allows the user to choose a control method

other than proportional-integral-derivative control of each joint to be

used in the response simulation mode of ROBSIM. The control options

currently implemented include:

a) Hybrid force/torque control;

b) Active compliance control;

c) Dual arm coordinated control.

The hybrid force/torque control and active compliance control are

discussed in the previous ROBSIM report, NASA contractor report 172401.

Although dual-arm control was not a requirement of this phase of the

contract, initial work in this area was done in support of the ITA

program. When analyzing a single manipulator arm, the ROBSIM program

solves the dynamic equations of motion for the joint accelerations at

each time step and numerically integrates these accelerations using a

fourth-order Runge-Kutta integration algorithm. The dynamic motion

equation solved has the form

where

1. JL

 are tne vectors

 °f joint displacements, velocities,

and accelerations

T_ is the vector o"£ actuator drive -torques and is

calculated using a feedback control law

b^ is the vector of position- and velocity-related torques

and is solved for using the recursive Newton-Euler

equations with joint accelerations set to zero

[A] is the instantaneous effective inertia matrix for the

manipulator and is calculated using methods described by

Thomas [1982] and Walker [1982] .

-19-
As mentioned earlier, this equation is solved for

integration algorithm obtains Q. and 8.

J8. and a numerical

For this initial implementation of the dual-arm control simulation,

an attempt was made to couple the dynamic solution equations and restrict

the translational and rotational velocities of the end-effector to be the

same.

Sensor control of the end-effector position simulates a video-type

sensor mounted on the end effector that can locate a target in the

manipulator workspace and then move the arm toward it.

The target is created during the system definition portion of ROBSIM

in a manner very similar to creating a load object. The target, however,

is drawn as four dots within the system (see Fig 8).

Figure 8. - Manipulator system with target.

-20-
The addition of the task command module necessitated a slight

modification of the load object creation/modification process. The

process has changed minimally from that described in the previous ROBSIM

report (NASA contractor report 172401). The same options and parameters

still need to be defined by the user with the addition of three more

pieces of data. This additional information requested of the user during

the object creation process has been added in conjunction with the

creation of a set of task level commands.

The additional data needed for each load object are a name for the

object, a grasp point on the object, and an approach vector for grasping

the load object.

The load name is a unique eight-character alphanumeric string

identifying the load object.

The grasp point is the x, y, z coordinates in the load local

coordinate system that define where a manipulator end-effector must grasp

the object to move or lift it.

The approach vector is used to define the direction or side of the

object that the end effector must be coming from to grasp the object.

This vector should be defined in the load local coordinate system and

corresponds to the direction of end-effector x-axis.

-21-
TASK 24, RANGING SYSTEM SIMULATION

Task 24 added the capability to simulate a manipulator-mounted

sensor system that provides accurate relative position and orientation

determination between the manipulator end-effector and a specified

target. This information is then used to control the motion of the

manipulator. The algorithm implemented is based on a paper by Robert M.

Haralick [Haralick, 1980]. This algorithm calculates the camera (sensor)

pointing angles and distances to a target based on the perspective

projection of the four corner dots of the target.

Given an x-y-z coordinate frame, the perspective projection of the

point (XQ, yo, zo) is defined in Figure 9.

Note that by convention the y-axis is along the line of sight (LOS)

of the sensor and the image plane. Where starred coordinates (X«, Z-'O

exist, the LOS is placed one focal length, fT in front of the camera

lens, which is at the origin.

f = camera focal length

I (o,f,o)

<VYo'Zo>

p.0 = 0 M

 lz\

x*0 =

 fJk

Y

o

f 7

Figure 9. -Definition of the perspective projection.

-22-
The camera pointing angles (note that on the camera line of sight

points in type positive y-axis direction) are defined as:

Counterclockwise rotation about the z-axis. (theta).

-90° < 9 <_ 90°

Counterclockwise rotation about the x-axis (phi).

-90° < <j> <_ 90°

Clockwise rotation about the y-axis (xi).

-180° < 5 <_ 180°

Given a reference frame to start from, a sequence of pointing

angles can arbitrarily orient a sensor (Fig. 10).

A direction cosine matrix (sometimes called the "attitude" matrix)

is a 3x3 matrix that provides the mathematical connection between the

coordinates of a given point in the reference frame and the coordinates

of the same point in a rotated frame.

Figure 10. •

Sensor coordinate frame rotated

relative to the reference coordinate

frame.

-23-
The entries in the 3x3 matrix [A] depend on the 9 <J>

of the particular rotation.

angles

For a counterclockwise rotation e about the z-axis, the direction

cosine matrix is given by

[A J - z

cos6

-sin6

0

sine

cose

0

0

0

1

For a counterclockwise rotation

cosine matrix is given by

about the x-axis, the direction

[Al = X

1

0

0

0

cos<}>

~sin<J>

0

sin4>

cos<|>

For a clockwise rotation E; about the y-axis, the direction

cosine matrix is given by

[A 1 - y

cosC

0

-sin?

0

1

0

sin?

0

cosC

Now, given an arbitrary orientation, the direction cosine matrix is

obtained by multiplying the sequence of matrices'for 9 <j> ? with

the e matrix on the right and the 5 matrix on the left. Therefore

[A] = [Ay] [Ax]

cos£cos9 + sin£Jsin<j>sin9

-cos<j>sin9

-sinCcose + cos£sin<j>sin9

cos§sin6 - sin£sin<j>cos6

coscjicosQ

- cosCsin<j)Cos9

sin£costj>

sinq>

cos£cos<p

To calculate the perspective projection in a rotated sensor frame of

a point P given reference frame coordinates ( xo, yo, zo), we first

convert to sensor frame coordinates. Given the sensor pointing

angles 6 <|> 5 we have

[A]P , = P

—ref —sen

(x )

o sen

o sen

-24.-
Now applying the definition of perspective projection, the

perspective projection of P in the sensor frame is given by

x* = f (xo)sen

(yo) sen

(ZQ) sen

(yo) sen

and .substituting, we get

xo(cosgcos6 + singsin(j)sin6) + yo(cosgsin6 - zo (sinScos'i')

xo (-cos<(>sin6) + yo (cos<j>cos9) + zo(sin<|>)

xo(-singcos6 + cosgsin(j)sin9) + yo(-singsin6 - cosgsin$cos6) + 2o(cos£cos6)

XQ C-cos4>sin9) + y0(cos<j)cos9) + z 0 (sin<j))

Furthermore, these equations can be neatly expressed as

cos? sin?

ypsin9

(-cosqisinS) (cos<j>cos0)

xpsinosinS - + zpcos(j>

xo(-cosq>sin6) + y0 (cos<pcos6) + z 0 (sin4>)

-25-
This result is obtained by factoring out cos C and sin £ from the

first equation for x* and z*. The advantage to these expressions is that

the dependence on 5 has been neatly separated from e and 4> .

Now we attempt to go in the reverse direction. That is, assuming

knowledge of the perspective projection, what can one say about the

possible points in three-dimensional space that produced it? The set of

points in three-dimensional space that could produce the perspective

projection (x*, z*) are given by the ray of reference frame coordinate

defined by

= X

x'cos9 - fsin9cos<j> + z"sin9sin<j>

+ fcosdcos* - z'cos9sin<j>

fsini}) +

for some \ and where

sin? cosCC

This result comes from the observation that any point on the ray has

coordinates (x*, f, z*) in the sensor frame. To express this in the

reference frame we multiply by [A]"1

X [ A ]-1 f

z*

cos9 -sin6cos<j> sin6sin<j>

sin6 cos9cos<j> -sin<j>cos9

0 sincj) cosij)

Note that [A]"1 is obtained by doing the negative of the original

rotation angles in the reverse sequence.

The perspective projection of a target can then be accomplished by

finding the projection of each of the four corners.

Similarly, the reverse calculations may also be^carried out (i.,e.,

given the perspective projections of the corners px" p2" p3" p^"

find the pointing angles 8 <i> and 5 ) (see Fig. 11 and 12).

-26-
ref

'ref

C Lx

Figure 11. -

Toe coordinates of the corners of the target

rectangle in the reference frame parallel to the

• rectangle sides.

•ref

sen

r

ref

Figure 12. - Perspective projection of the rectangle in the sensor frame

-27-
Establish the notation

Pi*

XI

yi

_Z1

Pz =

X2 = XI + W

y2 = yi

Z2 = Zl

P3 =

X3 = XI

ys = yi

Z3 Z1+ L

p«* =

XI* = XI + W

yi» = yi

zit zi + L

We can apply the results of the previous section to each of the

corners. For K=l, 2, 3, 4

P *

1C

cosC

-sin? cos?

x

where

x cos9 4- y sin9

K \fr>*

x (-coscfisine) + y (cos<jicos8) + z (sin<j>)

+ _ f »-sin<j>sin9 - y sin<j>cos9 + z cos<j>

Z

 — I K 1C 1C

ic —

x (-cos<f>sin9) + y (cos<j>cos9) + z

IN 1C 1C

and going in the reverse direction we know that there are four constants

AI \z ^3 A.i» such that if

and

~x -

z

K

'

K

a

cos5 -sin?

sin? cos?

~x *"

z

K

*

K

x cos6 - fsinBcos<j) + z sin9sin<j>

1C Is

> f

x sin9 4- fcos9cos<j) - z cos9sin6

i^ ic

fsllKJ) Z COStj)

then (x, y, z) are the reference coordinates of a point on the ray

emanating from the origin through the point P* . This^gives us a

starting point to go from the perspective projections PI* p* P3* Pt,'!

to the sensor pointing angles 9 d> ? . Now we use the fact that these

perspective projections are of a rectangle.

-28-
XI

yi

- fsinScos* + ^

sine + fcos6cos4» - zi

fsin<j> + z

[1]

J

XI

fsin<J> + zz'coscj)

[2]

J

X3

X3*cos8 - fsin6cos<j>

f fcos8cos<j>

fsin<j> + z;

[3]

+ W

yi

+ L

- fsinecoscj. +

+ fcos6cos4> -

fsin<|) + zi

-29-
These equations give enough information to solve for 9 4> 5

without knowing L, W, x^ y^ z^ Xj, X2, X3, X^.. To begin, notice that

the first and second coordinates of equations [l] and [3] are the same

(KI and yj)

Al(xj/cos6 - fsin6cos<j) + zi"sin9sin<{>)

= X3(x3"cos9 - fsin9cos<|> + z3"sin9sin(J>)

XxCxj^sinS + fcos6cos<j) - z1'*cos9sin(j>)

= X3(x3""sin9 + fcos9cos<f> - Z3"*cos6sin<|>)

Cross multiplying, canceling Xj.'X3 and combining like terms

(xi* - x3')fcos<J> + (x3*zi* - xi'z3-)sin(j» = 0

tancj) = f

Now, notice that the first and second coordinates of equations [2]

and [4] are the same. Apply the same steps as for [l] and [3] and get

0

This gives an alternate expression for tan <J>

tan<t>

- x2")

Thus <f> can be calculated two ways

= tan

-1

f (X3* -

= tan

-1 ' - x2*)

This gives us a way to calculate <j> once the primed coordinates are

known. Now to solve for 9 , notice that the second and third coordinates

of [l]and [2] are the same (y^ and

fcos6cos4> - zi"cos9sin<j>)

= X2(x2"*sin6 + fcos9cos4> -Zz^

Xi(fsin<j> + zj'coscj)) * X2(fsin<j> + z2"cos4)

Cross multiplying, cancel Xi*X2 and combine like terms

* - x2")fsin9sino - Zi')fcos9

-30-
and by symmetry (the second and third coordinates of [3] and [4] are the

same: y and z + L)

(x3" - x^Ofsinesincjj + (xa-'zit" - x», "z3")sinecos<j> + (zi,* - z3*)fcos6 = 0

Take these, two equations and eliminate the first term by multiplying

the first one by (x3* - xO and the second one by(xi* - x2")and then

subtracting

[(x3" - xi/) (xi*z2- - x^Z!*) - (xi* - x2") (x-j'z^' - x4*z3*)] sinGcoscp

+ [(x3*- x**) (z2- - z1*)-(x1* - x2*) (z,/ - z3')] fcose = 0

- x3-) - (x3* - x^*) (z2* - zi')

tane = f

[Cx3* - x^')

This gives a way to calculate 6 once 4> and the primed coordinates

are known.

Now lets go back to equation [5]. Because these are both equal to

tan <(>

(Xl*-X3')f _ (X2* - X,/)f

x2'zlt' -

From this and the definition of the primed coordinates we have

Note that the denominators result from the rotationally invariant

etC.

= (x2* -

Divide both sides by cos 5 and solve for tan 5

(X1*-X3*) (X2*Z1»* - X!t*Z2") - (X2* - Xl»") (X1*Z3* - X3^Zl*)

Z3* - X3*Zl*) - (Z3* - Zl*) (*2*Zlt* - Xi,*Z2*)

-31-
* A

Now we have a complete procedure for starting with PI p2 Pa

and arriving at 6 0 5

-32-
CONCLUDING REMARKS

This report has documented the work done in Phase IV of contract

NAS7-16759 Evaluation of Automated Decisionmaking Methodologies and

Development of an Integrated Robotic System Simulation. The tasks (21

through 24) added for this phase extend the capabilities and versatility

of the ROBSIM program as follows:

1) Access to CAD/CAM data - This software module addressed task 21

of the contract. It allows the user to access and display

CAD/CAM generated data that is written to a file in accordance

with the Initial Graphics Exchange Specification (IGES) version

2.0. The user may also use this data for the detailed graphics

modeling of system components.

2) Multiple arms with movable bases - This work addressed task 22

of the contract. The user may now model and do analyses on

systems that include up to five manipulator arms. In addition,

the arm bases may have motion specified for them. Each arm aray

be mounted on an independent movable base, or a single base may

be attached to more than one arm.

3) Task command module - This addresses task 23 of the contract and

was added to make the procedure for motion trajectory

specification of a manipulator easier. Each of the task

commands is a combination of one or more of the lower level

motion specification commands currently in place.

4) Ranging system simulation - This addresses task 24 of the

contract. This gives the user another way of controlling the

robot motion. The software developed simulates a video camera

mounted on the end-effector of a manipulator. The camera looks

for a user specified target and moves the end-effector towards

it.

The ROBSIM program is a useful tool for the analysis and design of

manipulator system components. There are, however, areas for future

enhancement than would make the program even more versatile and useful.

The areas for expansion could be directed toward the support of

teleoperator systems and other robotic systems capable of remote space

operations such as satellite servicing or space station construction.

The addition of hand controllers to drive the simulation would have

great utility in the area of telerobotics and teleoperator control. The

system would be useful for manipulator operator training and for use as a

predictive display in servicing operations having large time delays.

Interfacing the analysis software to multiple graphics systems to

try and make it somewhat device independent would increase interest in

using the program. Other interfaces of interest would be to existing

software packages in various areas like controls analysis, finite element

analysis, and flexibility.

-33-
Work in the area of trajectory planning should be continued. Also,

the hardware/software validation efforts should be carried on to better

define the strong points of the simulation and the areas requiring more

work. In order to validate the software running on different systems, a

library of test cases should be built and documented. Another useful

library would be one containing validated models of several popular

manipulators.

Lastly, the developments needed to implement real-time control could

be investigated.

-34-
REFERENCES

Haley, Almand, Thomas, Krauze, Gremban, Sanborn, Kelly, and Depkovich:

Evaluation of Automated Decisionmaking Methodologies and Development of

An Integrated Robotic System Simulation.NASA Contractor Report 172401,

September 1984.

R. M. Haralick: Determining Camera Parameters from the Perspective

Projection of a Rectangle. Manuscript, Virginia Polytechnic Institute

and State University, Blacksburg, Virginia, June 1980.

M. Thomas and D. Tesar: "Dynamic Modeling of Serial Manipulator Arms."

ASME Transactions, Journal of Dynamic Systems, Measurement, and Control,

Vol 104, 1982, pp 218-228.

M. W. Walker and D. E. Orin: "Efficient Dynamic Computer Simulation of

Robotic Mechanisms." ASME Transactions, Journal of Dynamic Systems,

Measurement, and Control, Vol 104, 1982, pp 205-211.

-35-
Standard Bibliographic Page

t Report No. 2 Government Accenton No

NASA CR- 178050

4 r.tie «xj subt.tie Evaluation of Automated Decisionmakinq

Methodologies and Development of an Integrated

Robotic System Simulation

7 Authorfo D. C. Haley, B. J. Almand, M. M. Thomas, L. D.

Krauze, K. D. Gremban, J. C. Sanborn, J. H. Kelley,

T. M. Depkovich, W. J. Wolfe, and T. Nguyen

9 Performing Organization Name and Address

Martin Marietta Aerospace

Denver Aerospace

P. 0. Box 179

Denver, CO 80201

12. Sponsoring Agency Name and Address

National Aeronautics and Space Administration

Washington, DC 20546

3. Recipient'i Ciutog No

5 Report Date

March 1986

6 Performing Orginuition Code

8 Performing Orgimzation Report No

MCR-85-665

10 Work Unit No

11. Contract or Grant No.

NAS1-16759

13. Type of Report and Period Covered

Contractor Report

14 Sponsoring Agency Code

506-45-21-01

15. Suooiemenurv Notes

This report consists of three volumes: Study Results, NASA CR-178050; Appendix A -

User's Guide, NASA CR-178051; and Appendix B - Programmer's Guide, NASA CR-178052.

Supersede **M|*W> *«%.*•*•• s NAS * iw »^rA • CR-172401 \*i> ± i t>~r\s A, f •« NAS >*"A CR-172402 * \**\ ^ i tw^wb, f an u*d i\J NAS iinwAn CR-172403 wr\— X / t~W w. •

is. Abstract The report describes the implementation of a generic computer simulation for

manipulator systems (ROBSIM). The program is written in FORTRAN, and allows the user to1. Interactively define a manipulator system consisting of multiple arms,

load objects, targets, and an environment.

2. Request graphic display or replay of manipulator motion.

3. Investigate and simulate various control methods including manual force/torque

and active compliance control.

4. Perform kinematic analysis, requirements analysis, and response simulation

of manipulator motion.

Previous reports (NASA CR 172401-172403) have described the algorithms and procedures

for using ROBSIM. This report supersedes these reports and describes additional features

which have been added:

1. The ability to define motion profiles and compute loads on a common base to

which manipulator arms are attached.

2. Capability to accept data describing manipulator geometry from a

Computer Aided Design (CAD) data base using the Initial Graphics

Exchange Specification (IGES) format.

3. A manipulator control algorithm derived from processing the TV image of

known reference points on a target.

4. A vocabulary of simple high-level task commands (e.g., move, grasp, approach)

which can be used to define task scenarios.

17. Key Words (Suggested by Author (i»)

ROBSIM

Robotics

Simulation

19 Security CUtsif (of this report)

Unclassified

18. Distribution Statement

Unclassified - Unlimited

Subject Category 63

20 Security CUoif (of thu pegel 21. No of Pages 22. Price

Unclassified 33

For sate by the National Technical Intonation Service. Springfield Virginia 22161