Archives - Bharath Institute of Higher Education and Research

Archives - Bharath Institute of Higher Education and Research
COMPUTER SIMULATED
VISUAL AND TACTILE FEEDBACK
AS AN AID TO
MANIPULATOR AND VEHICLE
CONTROL
by
Calvin McCoy W.ney III
S.B. MASSACHUSETTS INSTITUTE OF TECHNOLOGY
SUBMITTED IN PARTIAL FULFILMENT
OF THE REQUIREMENTS FOR THE
DEGREE OF
MASTER OF SCIENCE IN
MECHANICAL ENGINEERING
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
June 1981
Massachusetts
Institute of Technology 1981
Signature of Author
Department of Mechlnical Engineering
May
8,1981
C
Certified by
---
Thomas B. Sheridan
Thesis Supervisor
Accepted by
Archives
MASSACHUSETTS INSTITUTE
OF TECHNOLOGY
JUL 3 1 1981
L\BRARIES
./
Warren M. Rohsenow
Chairman, Department Committee
COMPUTER SIMULATED
VISUAL AND TACTILE FEEDBACK
AS AN AID TO
MANIPULATOR AND VEHICLE
CONTROL
by
CALVIN McCOY WINEY III
Submitted to the Department of Mechanical Engineering
on May 8, 1981 in partial fulfilment of the
requirements for the Degree of Master of Science in
Mechanical Engineering
ABSTRACT
A computer graphic
simulation of a seven degree-offreedom slave manipulator controlled by an actual master was
manipulator had
An electronically coupled
E-2
developed.
previously been interfaced to a PDP-11/34 by K. Tani, allowing the computer to sense and control each degree of freedom
independently. The
simulated
manipulator was
capable of
sensing a force in
arbitrarly shaped
object and
moving an
an arbitrary direction with no actual object or force existing.
The simulated manipulator could also be attached to a
simulated vehicle capable of motion with six
degrees-offreedom. The vehicle simulation is currently being used in
conjunction with dynamic simulations
developed by H. Kazerooni to
test
different
types of
dynamic controllers for
submarines. Shadows, multiple views and proximity indicators
were
evaluated to
determine their effectiveness in giving
depth information. The results indicated that these aids are
useful.
Subjects felt that shadows gave the best perception
of the environment, but found isometric views easiest to use
on the tasks performed.
This type of simulation appears to
be realistic and adaptable to a multitude of applications.
Thesis Supervisor:
Title:
Thomas Sheridan
Professor of Engineering and Applied
Psychology
-2-
ACKNOWLEDGEMENTS
I wish to thank
Professor Thomas Sheridan
for
his
many words of advice and encouragement. I appreciate the opportunity to work in the Man-Machine
Systems Laboratory,
it
has been a most interesting and educational experience.
I
would
like to
thank my many friends at the Man-
Machine Systems Lab for making it an enjoyable place to work.
Special thanks must go to my friend and co-reseacher,
yoon Kazerooni,
who developed the vehicle dynamics
tion and helped me interface it with the graphic
I would also like to thank
improving
the
understand the
simulation,
simulation.
Don Fyler for his suggestions on
Dana Yoeger for
the computer.
all the people who put time into
at a very busy time in the
Finally, I wish to
for her help on the
proofreading
understanding
simula-
helping
me to
computer system and Ahmet Buharali for help-
ing to maintain
for
Homa-
A word of
thanks must go
performing my
experiments
semester.
thank my fiancee, Deborah Darago,
statistical analysis of the experments,
this
during the
document and
many hours
for her
patience and
spent working on this
project.
This work was supported
by the Office of Naval
Research, Contract N00014-17-C-0256, monitored
by Mr.
to
G Malecki, Engineering Psychology Programs.
-3-
TABLE OF CONTENTS
ABSTRACT .............................................
.....
2
ACKNOWLEDGEMENTS ...........................................
LIST OF FIGURES AND TABLES
................
...............
6
INTRODUCTION AND PROBLEM STATEMENT.........................8
Artificial Intellegence Versus Supervisory Control....... 8
Computer Generation of Operator Feedback.................9
Simulation of Master-Slave Manipulators.................10
Depth Perception From Two-dimensional Images............11
APPLICATIONS OF COMPUTER GRAPHIC MANIPULATOR SIMULATIONS..13
Simulation of Various Operating Conditions..............13
Testing of Control Systems..............................13
Supplementation of Operator Feedback....................14
Rehearsal........
.......................................
15
Humanizing Man-Computer Interactions....................16
DISPLAY THEORY AND DEVELOPMENT...........................17
Data Storage
............
............
.........
e..... ......
Translations and Rotations........
Shadow Generation.........................c....e
Moments of Inertia....
..............
.........
.....
17
..... 19
e....24
.
...................
26
Body and Global Coordinates............................27
Generation of Force Feedback......
......................
28
Resolved Motion Rate Control. ...........................
34
Control of the Manipulator..............................
35
Object Movement...............
-4-
.........
.................
37
Touching Cconditions ....................................
38
EQUIPMENT...
PDP11/34 C omputer ....................................... 43
E-2 Master -Slave Manipulator..............................45
.....
Megatek Di splay Processor........................
. 45
EXPERIMENTAL DESIGN .......................................
47
Programmin g Considerations..........................
....47
Manipulato r and Vehicle Simulation......................
50
Depth Indi cators
53
........................................
Experiment al Measurement of Operator Performance........54
RESULTS AND CONCLUSIONS..
Master-Sla ve Simulation
..............
....
55
................................
55
Simulated Force-Feedback....... ........
Depth Indi cators.•......
............
. .
............
..
•...............................
Conclusion s and Recommendations
.
64
66
..70
.......................
REFERENCES..
APPENDICES..
I Depth In dicator Experiment
Statistics.................73
II Simulat ion Transformation Matrices ...................
80
III Force- Feedback Jacobian Calculation.................86
IV Auxilla ry Software ..........
.... .
..................
88
V Manipula tor Simulation Software......................
90
VI Submari ne Simulation Software.......................107
VII Manipu lator Control Software........................126
VIII Progr am Data Bases
.
........................
-5-
....
........
128
LIST OF FIGURES AND TABLES
Figure 1
Manipulator and Vehicle Angles.................18
Figure 2
Effects of Order of Rotation.......... .........
22
Figure 3
Equivalent Body and Global Rotations.. .........
29
Figure 4
Master-Slave
Manipulator Block Diagram.........
30
Figure 5
Computer Controlled Master Block Diagr am. ...... 30
Figure 6
Multiple Spherical Touching Conditions .........
Figure 7
Multiple
Figure 8
Rectangular
Figure 9
Man-Machine Systems Laboratory........
......... 44
Figure 10
E-2 Slave Manipulator.................
......... 46
Figure 11
Display Type
Figure 12
Display Type 2 - Front and Sid e Isometrics. .... 56
Figure 13
Display Type 3 - Front View wi th Proximity
Spherical Touching
Touching Conditions.......
-
Shadow with
Indicator.....................
Figure 14
Figure
......... 42
Walls........
..... 56
................. 57
Proximity
Indicator
Alone.... .57
Manipulator Simulation with Shadows. ........... 60
17 Detail of Tongs Gripping a Rectangul ar Peg ..... 61
Figure 18
Figure
Display Type 4 -
......... 40
15 Manipulator Simulation with Shadows. ........... 59
Figure 16
Figure
Conditions
40
Simulated
Surface...................
........... 62
19 Deflection of Simulated Surface ..... ........... 62
Figure 20
Deflection of a Soft Surface........ ........... 63
Figure
21 Deflection of a Hard Surface........ ........... 63
Figure
22
Figure
23 Evaluation of Depth Indicators...... ........... 68
Submarine
Simulation................
-6-
........... 65
Table 1
Average Time to Locate Object -
Subject I ... ..73
Table 2
Average Time to Locate Object -
Subject
Figure 24
Learning Curves -
Subject I...
..74
Figure 25
Learning Curves -
Subject
2...
.. 75
Figure 26
Learning
Subject
3...
..76
Figure
Learning Curves
Subject
4...
.. 77
27
Curves -
2.... .. 73
Table 3
Average Time to Locate Object
-
Subject
3.... ..78
Table 4
Average Time to Loacte Object -
Subject
4.... 6.78
Table 5
Average Time
Table 6
Three-Way Analysis of Variance
Table 7
Control Interface I/O Channels
to Locate Object - All Subjects. ..78
-7-
.. 79
...............
.126
INTRODUCTION
In the past several years, develpoments in the electronics
industry have made
powerful and inexpensive.
mini-computers extremely small,
Microprocessors are now being in-
corporated in machinery ranging from
large scale production
equipment to household dishwashers. As this trend continues,
more effort will
be put into the use of the computer to aid
the human operator.
which
and
use
Automobiles
are already
microprocessors to control
relay failure
on the market
automobile
information to the driver.
become more common,
function
As computers
the question will arise as
to how they
can best serve the operator.
Artificial Intelligence Versus Supervisory Control
The use of computers to aid
divided
into two
supervisory
human
catagories: artificial
control.
operators can be
intelligence
The major difference
between the two
approaches is in the manner in which the computer
with the human
attempts to give
operator.
the
and
interacts
Artificial intelligence ( A. I. )
computer maximum
intelligence and to
replace all operator functions by the computer.
Supervisory
control acknowledges that the operator has certain abilities.
It attempts to use those talents and
supplement those which
are lacking.
The
task of removing a bolt from an undersea struc-
ture emphasizes the differences between these two approaches.
-8-
The A. I.
approach might require the computer
tiate between the pilings and the
to differen-
surroundings,
to be able
tell a bolt from a barnacle and be able to select the proper
bolt.
It must also be able to determine the proper angle at
which to turn the nut,
be able to select the proper tool to
fit that nut and cope with the possability of the
nut being
damaged.
The supervisory control approach would rely on the
operator
to find the bolt, while possibly aiding with image
enhancement.
and
The operator would
condition of the bolt.
If
determine the proper tools
the vehicle were moving, the
computer might aid the operator by compensating for that motion.
The
computer
would then
operator showed it the proper
remove the
bolt after the
orientation and described the
desired motion.
The
stances
make
A. I. approach
operator
drawback
is
order to
cope with
that
it
interaction
requires
all
possible
variable the environment;
gramming becomes.
becomes
necessary when circumimpossable.
extensive
Its major
programming
contingencies.
in
The more
the more complex the required pro-
The supervisory approach achieves economy
by taking advantage of the operator's abilities ( experience
and
intuitive skills )
while the
computer supplies memory,
accurate position control, speed and repeatability.
Computer Generation of Operator Feedback
Since supervisory control relys on
-9-
operator
inter-
action,
one
important way
operator is to improve the
in which a
operator's
computer can aid an
knowledge of his en-
vironment. Feedback can take many forms,
though humans rely
most heavily on their tactile, visual and
Improved feedback is important for
auditory
several reasons. Supply-
ing the operator with more processed
information leaves the
operator more time to dedicate to the task.
Any information
can be related to the operator by a set of numbers,
the some types of feedback are more easily
others.
The
described
orientation of a
by a
set of
vehicle
angles,
senses.
assimulated than
can
however a
however
be
completely
display
of the
vehicle is more informative even though it is less accurate.
Feedback may exist,
case,
the
but it may be of poor quality.
computer
could be used to
improve
In this
rather than
create feedback.
Several forms of feedback can be used simultaniously
to reinforce the operator's
perception of the
environment.
For example, combining tactile with visual feedback may be a
better aid than either
tactile or visual feedback alone.
A
single type of feedback may not be best suited for all tasks.
The use of several
different types
of feedback could allow
the operator to select the type of feedback he preferred for
each type of task.
Simulation of Master-Slave Manipulators
The basic part of this
-10-
research was the development
of a computer
lator.
pulator
simulation of a master-slave manipu-
graphic
The simulation was controlled by an E-2 master maniwhich had
previously been interfaced to a PDP11/34
computer by K. Tani (7).
This interface allowed the computer
to sense and control each of the seven degrees of freedom of
the
Part of the simulation in-
independently.
manipulator
cluded the development of an environment within the computer
which the simulated manipulator could interact with. The simulated
was
manipulator
shaped object about in
capable of
moving an
arbitrarly
three-dimensional space and simulat-
ing force-feedback in an arbitrary direction. Force was felt
when the
manipulator
fined surface.
The
grasped an object or touched a predesimulated manipulator could also be at-
tached to a vehicle capable of
freedom.
motion
with six degrees-of-
The vehicle simulation is currently
H. Kazerooni to test
various
types of
being used by
dynamic controllers
for small underwater vehicles.
Depth Perception From Two-Dimensional Images
One of the
graphics
of a
manipulation
difficulties
three-dimensional
via closed-circuit
common to both
generating
system and to
performing
television
is the lack of
ability to perceive three dimensions. The method of displaying depth would appear to be particularly
cations such as
critical in
manipulation which require
appli-
physical motion
to be coordinated to visual input. Tasks such as grasping an
-11-
ability to quickly
object from a moving vehicle require the
integrate the two-dimensional image plus the depth cues into
three-dimensional
If the information cannot be as-
motion.
similate quickly, the object will have moved relative to the
position has been
the time its
operator by
A
determined.
close spatial relationship between the display and the
real
world would appear to make interpreting the display easier.
Shadows,
views and
multiple
were tested to determine their
effectiveness as depth cues.
chosen for
Each depth indicator was
indicators
proximity
exemplifying a partic-
ular attribute. The shadow was chosen because it is the most
familiar depth cue and
has a strong
spatial
relationship.
Its drawbacks are that it compresses picture information and
the e is the
possablity
that several
shadows near one an-
other may make interpretation difficult.
isometric
projections normally used in mechanical
give depth
information more
determination of
coordination
quire
depth
problems.
extensive
proximity
The front and side
prior
clearly than the
from the
shadow,
side view may
Both of these depth
knowledge of the
drawings
but
cause some
indicators re-
environment.
The
indicator which measures the distance between the
manipulator
tongs and the desired destination could be dis-
played with
little prior
practice,
knowledge of the environment.
proximity could be
on the tongs.
The
proximity
In
determined by a sonar mounted
indicator was displayed along
-12-
with a front view of the manipulator.
indi-
proximity
The
cator, with no other display, was used as a control.
APPLICATIONS
OF COMPUTER GRAPHIC MANIPULATOR SIMULATIONS
Simulation of Various Operating
simulating envi-
and for operator training by
permentation
The
for ex-
realistic simulation can be of value both
A
ronments
Conditions
created in a
which can not be easily
viscosity and the
change in the
laboratory.
between
relationship
weight and mass associated with underwater work can be simumanipu-
undersea
lated without requiring tanks of water or
lators. The zero-gravity conditions associated work in space
such as the space shuttle can be easily
simulated by a com-
puter, but would be very difficult to obtain in a laboratory
by any other means.
it is
By using a computer to control a manipulator,
possable to vary the properties of that
it can be used
ments of
inertia can
be
massive
can
be
of
locked
a
to
industrial
seven
simulate
Mo-
light hot-room
changed to make a
degrees-of-freedom
manipulator
so that
simulate many types of manipulators.
to
manipulator behave like a
Several
manipulator
manipulator.
degree-of-freedom
a less
flexible
manipulator.
Testing of Control
Building
a
Systems
control
expensive and time consuming
system for a vehicle can be an
undertaking.
-13-
Propulsion units
need to be
modified and sensors need to be installed.
controllor there is always the risk of instablity and
a new
When creat-
failure which could result in damaged hardware.
one-of-kind vehicle,
control system for a
ing a
and
With
time lost in a
the money
failure could make improvement or addi-
tion of a control system prohibitive. Using a computer simulation would
allow the
and easily.
quickly
could be
tested
Failure of a
Propulsion and
an instability
Therefore,
to collect
sensor
need for
without the
computer simulation
tially no risk.
to be changed
prototype controllor
configurations
expensive hardware.
generally involves essen-
a simulation can
additional
be run during
data on the
failure
with out the risk of hardware damage.
Supplemention of Operator Feedback
Due to the high cost of using
manned
humans directly,
un-
submersibles are used to inspect and repair offshore
oil rigs in the North Sea. To avoid tethering problems, communicatons to the operator on the surface can be via accoustic link.
One difficulty with an
is only capable of low bit rate
is the
product
of the
accoustic link is that it
transmission.
frame rate,
number of bits of gray
scale and the number of pixels ( resolution ).
pixel
picture
flicker
3 million
The bit rate
with 5 bits of grey and a
A 200 by 200
frame rate at the
limit of 15 frames per second requires transmitting
bits per second.
To the operator who is watching
-14-
the work via a television picture
link,
this means he
sent through an accoustic
receives a very degraded
Since the manipulator position can be
knowing
bits,
each
angle of the seven
these 118 bits
picture (3).
completely defined by
degrees-of-freedom
to 16
can be transmitted freqently allowing
the simulation to be updated
frequently. By superimposing a
rapidly
updated
updated
but high resolution television picture, data trans-
mission
can
simulation of the
be optimized
the display is refreshed
manipulator on a slowly
such that
the moving portion of
frequently while the static visual
background is of good quality.
In the case of poor
visability,
a simulation could
be used to generate or enhance the view of the surroundings.
If part of the
environment was known in
advance and stored
in the computer, it could be displayed on the
soon as the operator established enough
locate and orient the environment
Objects could
simulation as
reference points to
relative to the operator.
also be inputted into
the display by feeling
about and recording points of contact. The points of contact
could then be analyzed to determine the location of surfaces
and edges.
Rehearsal
When an
operator is required perform a dangerous or
delicate task where a
mistake might harm the operator,
equipment or the task, it might be
-15-
desirable for the
the
oper-
ator to be able to
actually
practice the task first before
performing it.
If a realistic simulation was available, the
operator could
practice the
task on the simulation until he
felt confident to actually execute the task.
lator were
fectly.
simulation until he performed the
task perWhen
The computer could moniter each practice run.
operator was
the
the operator could perform
computer controlled,
the task on the
If the manipu-
satisfied with a run,
he could tell
the
computer execute that run. The computer would then duplicate
the
previous motion
(6).
Humanizing Man-Computer
Interaction
It has been suggested that a standard master manipulator or a
version be used as a means of
smaller table-top
communicating with a computer. The manipulator used in these
degrees-of-freedom
experiments had seven
tongs
range.
it to
to be
moved to any
It was also
communicate
One of the
position and
which allowed the
orientation within
capable of force-feedback which allowed
contact with an object
to the operator.
CAD systems
is developing the
difficulties with
ability to input three-dimensional data. A manipulator could
be used as a three dimensional digitizer.
ity of the manipulator might help in
The spatial qual-
inputting points which
were three-dimensional in nature. The manipulator could send
force-feedback when the
order
to aid the
operator touched a point or line in
operator in
locating references.
-16-
If
the
desired points were all on a
could be used to restrict
desired object
ined by
given plane,
computer control
movement to that plane.
had been inputted,
When the
it could be easily exam-
grasping it and rotating it
as if it were in one's
hand.
The
stract way.
physical
manipulator
could
be used in a more ab-
also
Data could be encoded with spatial,
properties.
A
particular
tactile or
point might be hard or
soft, heavy or light,
or sticky or slippery.
helpful in aiding the
operator to
This might be
select a particular type
of data while searching through a data space.
It might also
help him to notice small differences between attributes.
he were looking for a particular type of data,
could use force-feedback to
"push"
If
the computer
him in the right direc-
tion (6).
DISPLAY THEORY
Data Storage
A
the
schematic of the
definitions
of
the
manipulator
and
degrees-of-freedom
vehicle
is
shown
with
in
figure 1. The arm, submarine and object were stored in standard
point-connectivity data form.
three distinct portions,
the shoulder,
Each section of display was
The
data base
reference
for each
frame with the
The arm was broken into
treated as a
forearm
and tongs.
separate
entity.
section was stored in an unrotated
center of rotation located at the
-17-
A~
I
f
VVl
'3
Figure 1
Manipulator and Vehicle Angles
-18-
origin. The vehicle was stored so that forward motion of the
unrotated vehicle was in the negative z direction. Each section of the arm extended from the origin along the
z-axis.
This
reference
display terminal.
frame was the same as
In this
right-handed
of the
screen.
Each
The origin
matrix
containing the
element from the
element had a
that of the
coordinate system,
the x-axis was to the right, the y-axis was
z-axis was out of the screen.
negative
upwards and the
was at the center
corresponding rotation
transformations required to move the
reference
frame to its
desired location.
Objects which could be manipulated also had a set of
touch-
ing conditions which were defined in the reference frame.
Translations and Rotations
The multiple
calculated in matrix form. The matrix ele-
were most easily
ments were fed
ware matrix
coordinates.
required to display the arm
rotations
directly into the
multiplier.
display processor's hard-
All rotations were based on global
A translation
Tx in the
X direction is given
by:
= X + Tx
X'
In three dimensions this can be expressed in matrix form as:
1
-
Z' -
1
1 0 0 Tx
X
0OY' 1
0
Ty
Y
0
1
Tz
0
0 0 0 1
-19-
Z
1
where the ones in the coordinate matrices satisfy the matrix
algebra needed to add the constant Tn.
This matrix form can
be abbreviated as:
X' = TX
T is called the translation matrix.
Transformation matrices
can also be formed for rotations in the same manner. The rotation
Rx for a
matrix
rotation of an
A about the
angle
x-axis is:
Rx
1
0
0
0
0 cosA -sinA 0
0 sinA cosA 0
0
0
0
1
Rotation matrices about the y and z-axis are given by:
cosA 0 sinA 0
Ry
0
1 0
0
0
0
Rz-
cosA -sinA 0
-sinA 0 cosA 0
These
rotation
0
0
sinA cosA O 0
1
0
0
1 0
0
0
0
1
matrixes are for rotation about the origin.
To rotate about an
arbitrary point,
desired
center of
rotation to the origin,
tation,
then translate the center of
one must translate the
perform the ro-
rotation
back to its
original position (4,5).
A series of transformations can be reduced to a single
transformation
matrix
matrix by matrix
multiplication
is
associative
-20-
multiplication. Since
but is not commuta-
tive,
the order in which
important.
axis is
Rotation
x-axis.
performed is
about the x-axis and then about the z-
not the same as
about the
transformatioms are
rotation about the z-axis and then
This is most
easily seen by the example
illustrated in figure 2. If one holds one's right arm out to
the side with the palm facing downward and a coordinate system is
established
so that
the y-axis
is upward
x-axis extends to the right parallel
to ones arm,
z-axis
forearm is
extends
out behind.
degrees about the x-axis,
If the
and the
then the
rotated 90
then 90 degrees about the y-axis,
the forearm is pointed forward and the palm is facing to the
left.
If the
y-axis,
forearm is first
then 90
rotated 90 degrees about the
degrees about
facing upward and the
palm is
the x-axis,
facing
the forearm is
foreward.
Transfor-
mation matrices occur before the coordinate matrices and are
sequenced
in the order
in which
they occur
from right to
left as follows:
X' A'XI An ...
In this
A3 A2 Al X
simulation it was
necessary
to be able to
perform the reverse coordinate transformation. This requires
taking the inverse of the transformation matrix. The inverse
of A is found by:
A-l=adj(transpose(A) )/det()
For a 4x4 matrix,
computing the
-21-
adjoint
requires
finding
n
C
Figure
2
Effects
of
-
Order
of
Rotation
-22-
determinants
additions and
tions.
of 16
3x3 matrices,
6 multiplications,
each of which requires 5
for a total of 176 opera-
Since the bottom row of the transformation matrix is
only used to satisfy
the matrix algebra and contains no in-
formation, the transformation matrix can be partitioned into
a 3x3 rotation matrix and a 1x3 translation matrix:
R
0
The coordinate transformation is then given by:
X' =RX+T
This can be solved for X to give:
Computing the adjoint of a
3x3 matrix requires
finding the
determinants of 9 2x2 matrices, each of which require 1 add-
ition and
tions.
2 multiplications
for a total of only
The new transformation
matrix can be
27 opera-
reconstructed
by rejoining the new rotation and translation matrices as:
1110
R T1
For the general rotation matrix,
-23-
XX XY XZ
XT
YX
ZX
YY
ZY
YZ
ZZ
YT
ZT
0
0
0
1
the components of the inverse matrix,
RXX RXY RXZ RXT
RYX RYY RYZ RYT
RZX RZY RZZ RZT
0
0
0
1
are fould to be:
RXY= (ZYXZ-XYZZ)/Det
RXT -RXXXT-RXYYT-RXZZT
RYY=(XXZZ-XZZX)/Det
RYT -RYXXT-RYYYT-RYZZT
RZY=(XYZX-ZYXX)/Det
RZT=-RZXXT-RZYYT-RZZZT
RXX-(YYZZ-ZYYZ)/Det
RXZ=(XYYZ-YYXZ)/Det
RYX-(YZZX-YXZZ)/Det
RYZ- (YXXZ-YZXX)/Det
RZX-(YXZY-YYZX)/Det
RZZ-(XXYY-XYYX)/Det
where
det is the
determinant of the rotation matrix and is
given by:
De t=XXYYZZ+XYYZZX+XZYXZY-ZXYYXZ-ZYYZXX-ZZYXXY
The determinate of a
rotation
ever it was calculated to
matrix is formally one, how-
compensate for
roundoff error by
the computer.
Shadow Generation
If
object
the source of
that rays passing
considered parallel,
matrix notation.
overhead,
then
illumination is
through
far enough from an
different
then its shadow can be
points can be
represented by
If the source of illumination is
for a shadow
cast on a
-24-
directly
horizantal surface
the
y-coordinate of
every
point in the
shadow
is at the
height of the surface Ys and x and z values remain unchanged.
The transformation matrix for this shadow is:
0 0 0
1
0
If the surface
0 0 Ys
0
0
1
0
0
0
0
1
expressed as a
can be
linear function of x
and z as given by:
y=Ax+Bz+C
then
the transformation
matrix for a
shadow cast
on this
surface is given by:
1 0 0 0
A
0
0
It is also
0
0
0
B
1
0
possable to
C
0
1
accomodate light
coming in from an
arbitrary angle. Suppose the light is from an angle (a) from
the y-axis,
as measured about the z-axis and the surface is
horizantal at Ys.
The x component of the shadow remains the
same as that of the object,
face value Ys.
the
The z value is
y component is at the surdetermined from the distance
between the surface and the object and the angle of
nation.
X'=X
Y'=Ys
Z'=Z-(Y-Ys)Tan(a)
-25-
illumi-
The transformation matrix is then:
1
0
0 0
0
0
0
Ys
0
0
0
1
0 -Tan(a)
1 YsTan(a)
Shadows on different surfaces and
illumination from differ-
ent directions can be handled in a similar manner.
Inertia
It is
hoped future vehicle simulations will include
manipulator-vehicle
interactions such as
when manipulating a massive object
those which occur
from a small,
light ve-
hicle.
This will require determining
the moment of inertia
of the
manipulator for any position.
If the centroidal mo-
inertia Ic is
ment of
the manipulator,
lator
known for each
unrotated section of
then the moment of inertia for any manipu-
configuration can be determined (2).
tance must be calculated for the
segment of the
rotation
manipulator.
matrix R,
First the iner-
proper orientation of each
If a segment is described by a
were R is
the
rotation
portion of the
transformation matrix, then the inertance Ir for the rotated
segment is given by:
YIrRIc
=
The final moment of inertia I' about the shoulder can the be
found by applying the
Zt
are
the
distances
parallel axis theorem.
from
the
-26-
If Xt, Yt and
shoulder to the centroid
of the segment in question and M is the mass of the section,
then the
inertance
is:
I'=RIcRrT +M
-XtYt
Xt2+Zt2
-YtZt
-XtZt
-YtZt
Xt2+Yt2
If Xc is the vector describing the
in the
unrotated
location of the centroid
reference frame,
then
the
vector Xt is
found to be:
Xt=RXc
Since the
manipulator is oriented along the z-axis,
manipulator
is
Yc are zero,
relatively
symmetric
about the z-axis,
if the
Xc and
in which case the elements of Xt are simply:
Xt-XZXc+XT
Yt=YZYc+YT
Zt-ZZZc+ZT
Global and Body Coordinates
The orientation of a
vehicle is
normally expressed
in the body-referenced Euler angles yaw, pitch and roll. The
order of rotation is yaw, followed by pitch and finally roll.
The reason for this
generally
convention is that forces on a body are
invarient with
when they are
respect to changes in orientation
described by
body
coordinates.
The dynamic
programs developed by H. Kazerooni are based on body centered
coordinates.
Since
global coordinates,
the
display
processor is based on
a method of transforming from body cen-
tered to global coordinates was needed.
-27-
Examination
of motions
of the vehicle reveals that
the rotations yaw,
pitch and roll in
the
followed by pitch and then yaw in global
same as roll,
coordinates.
nates are
Before
any rotation,
equivalant ( Figure 3 ).
body axis is therefore the same.
about
global
coordinates
body
coordinates are
global and body coordiRoll
about
global and
After this rotation, pitch
performs the same function as it
did in body coordinates. Final yaw performed in global coordinates after these transformations is still the the same as
yaw performed first in body coordinates.
Generation of Force-Feedback
In normal
tion
transfer
manipulator operation,
between our
positional information.
tional
Since there
manipulator.
force
master and slave manipulator is
The control
information to be
interface allows posi-
transferred between
means of
is no
information to the manipulator,
feedback
involves
the only informa-
encoding force
computer and
directly sending
generation of force-
information into a posi-
tional signal.
To
understand
computer
generated
force-feedback,
one should first understand the operation of the manipulator
in master-slave mode.
Figure 4
shows the
configuration of
the manipulator in this mode. Each manipulator is made up of
a set of servomotors, each of which is directly coupled to a
potentiometer.
Each
servomotor is
-28-
driven by an
amplifier
~i)
L~-x
K
I
I ww
a4
Roll
4,
1
f"-x
A
-AL
I
P; re h
1~
Y&/
nui
xI
r
10
nI
I
·X
Figure
3
Equivalent
Body and Global
-29-
Rotations
Mas ter
5 ave
Figure 4
Master-Slave Manipulator Block Diagram
M aster
m
Figure
5
Computer Controlled Master Block Diagram
-30-
that outputs a current which is proportional to the
ence between the
master and slave positions.
If Xm
are the master and slave positions respectively,
Gs are the gains of the amplifiers
ter and slave,
then the
differand Xs
and Gm and
associated with the mas-
currents input to the
servomotors
are given by:
is=Gs(.Xm-Xs)
im=Gm(Xs-Xm)
If Q represents
the gain of the
current into force,
servomotors in
converting
then:
Fs-QGs(Xm-Xs)
Fm=QGm(Xs-Xm)
(1)
(2)
Combining equations 1 and 2 yeilds:
Fs=-FmGs/Gm
(3)
or force on the master is directly proportional to the force
on the slave.
Since
between the
only
information is
positional
master and slave,
transferred
how that information relates
to forces on master and slave needs to be considered. If the
slave is pressing on a linear elastic surface,
the slave is
given by:
Fs=KAX
which is the
the force on
(4)
conventional spring law where
AX is the
dis-
placement of the surface and K is the spring constant.
Com-
bining
felt
equations
3 and 4
gives the
through the master:
-31-
resulting force
Fm--KAXGm/Gs
Combining
and solving for (Xs-Xm) yields
equations 1 and 4
the offset between master and slave
position dX for a given
displacement of the surface AX
dX=(Xs-Xm)=KdX/(QGs)
to the master mani-
the input
Using the above information,
pulator required for
(5)
feedback can
computer generated force
be determined.
Figure 5 shows the configuration of the master manipulator under computer control. It is the same as the masterslave
configuration
except that the
placed by a computer input.
slave position is re-
If the slave position Xs is re-
placed by computer input Xc, equation 5 becomes:
dX=(Xc-Xm) -KtX/(QGs)
In a computer simulation,
is given by the
the
(6)
penetration into the surface
difference between a reference point at the
surface and the position of the master (eq.
7)
X-Xref-Xm
(7)
Equation 6 can be rewritten using equation 7 as:
Xc=Xm+K(Xref-Xm)/(QGs)
(8)
This equation gives the computer input Xc required to generate force feedback proportional
to the
stiffness of the sur-
face and the penetration into the surface. If
infinitely
deformable
(K=O),
then Xc=Xm
allows the manipulator to move freely. If
-32-
the surface is
and the computer
the surface is com-
then Xc-Xref. Hence the computer
rigid (K/(QGs-=),
pletely
will not
allow the
manipulator
to
penetrate the reference
plane.
Since
manipulator
simulating
to
move to a
gles corresponding
cept
Our
current
trial of the con-
interpolation method.
first touched
a surface,
saved as a reference.
within the
directing the
position other than its
to that position.
used a linear
lator
requires
force
arises to calculate the manipulator an-
the need
position,
a
surface,
When the manipu-
its angular
As long as the
the desired
position
manipulator
manipulator
was
remained
position was
calculated to be a weighted mean of the current position and
the reference position. This approach proved the concept but
had two problems.
First,
since the program
cycled in dis-
crete time steps,
it was
possable
manipulator to
surface
have penetrated well into the
realized
reference
that
penetration
for the
before the
had occurred.
position which is
computer
The result was a
within the surface rather than
on the surface. When the manipulator was withdrawn, the surface seemed tacky because the computer attempted to pull the
the manipulator
back to the reference rather than releasing
the manipulator.
The second problem concerned the direction
of the force-feedback.
The force was generated was
vector defined by the reference
pulator
position,
instead of
along a
position and current
normal to the
-33-
surface.
maniThe
result was
similar to that of a
rubberband
being attached
between the manipulator and the reference position.
Resolved Motion Rate
Both these
Control
problems were
overcome by
developing a
transformation to go from cartesian coordinates to the manipulator's multiple angle coordinate system. There are several problems associated
with
directly solving the transfor-
mation equations in term of manipulator angles. First, there
are three transformation equations and six unknown angles. A
value for
three angles must
be assumed to define a
solution for the remaining three angles.
unique
Careful inspection
of the function of the six angles ( figure 1 ) reveals that,
due to the difference
in lever arm associated with rotation
of the tongs about the shoulder and rotation about the wrist,
angles Al, A2 and A3
small effect
hand
Since
control hand position and
on hand
orientation
differences
orientation.
and have
between
A4,
have only a
A5, and A6 control
little effect on hand position.
manipulator
position
and com-
puter input position are small, A4, A5 and A6 can be assumed
to be constant at their
current values while changes in Al,
A2 and A3 are used to position the manipulator.
The second
problem is in solving the equations des-
cribing manipulator position for input angles, since arcsine
and arccosine are not monotonic functions. Although an exact
solution is possable, the simplest method is Resolved Motion
-34-
Rate Control
fully
(RMRC) proposed by D. Whitney
implimented
in our
manipulator
(8),
and success-
by K. Tani (7).
If
X=f(A)
where X is the position vector of the tip of the manipulator
tongs and A is a vector made up of manipulator joint angles,
then the differential
of X is:
dX=JdA
where J(O) is the Jacobian of X given by:
Therefore,
dX/dAl dX/dA2
dX/dA3
dY/dAl dY/dA2
dY/dA3
dZ/dAl dZ/dA2
dZ/dA3
for incremental
the incre-
change in angle AA,
mental change in position AX is given by:
d X=JAA
(9)
equation 9 can be
For a given angular position,
the
incremental
change in angles
solved for
associated with a change
of position:
AA-J~ A X
(10)
The new angular position A' of the manipulator can be determined by adding the
10 )
incremental change in angle
to the current manipulator
(
equation
position:
A' =A+J'-A X
Control of the Manipulator
To avoid striking objects with the slave while watching the display monitor,
the slave arm was
-35-
held in a fixed
position when the
manipulator
was used under control.
The
computer needed to allow only the master manipulator to move
freely,
slave
fixed.
master
face.
or generate a force on the master while holding the
Under computer control,
and slave
the positions of the
manipulator are set by the control inter-
Deviations from
these positions require applying ap-
propriate forces to the manipulators.
ulator to move freely
For the master manip-
under computer control,
must sense t he current manipulator position,
position to
the computer
then feed that
the control interface. Since the control program
runs with a discrete cycle time,
there is a period when the
manipulator must be deflected from the position specified by
the control interface.
During this period,
applied to t:he manipulator
waiting for
and
update
manipulator
forward loop
the
the
to maintain the deflection while
computer to realize the
control
a force must be
interface.
being quite stiff.
change in position
This
results
Simulating the
in the
tachometer-
used in master-slave operation helps alleviate
the problem. If the current manipulator position is given by
Xm
and the last
position is given by Xl,
then the velocity
of the manipulator is proportional to the difference between
these two positions:
V~Xm-X1
In simulating tach-forward loop signal,
a position correct-
ion proportional to the velocity of the manipulator is added
-36-
to the new manipulator position.
ward is F,
then the
If
the degree of tach for-
signal sent to the
control
interface
worked well.
A larger
Xc is:
Xc=Xm+F(Xm-Xl)
A tach forward coefficient of .6-.7
value generally lead to
instability.
inclusion of an acceleration term
K. Tani (7) found the
slightly beneficial, how-
ever it seemed to be of little value in this application.
ation.
times
A short cycle time was
important for
20 to 30
worked quite
of up to
milliseconds
60
milliseconds
the
program
separated from the
which
controlled the
oper-
well and cycle
were tolerable.
cycle time was also important. With these
mind,
smooth
Constant
considerations in
manipulator
was
program which performed the graphics and
simulation. The program was slaved to a clock for even cycle
time.
Object Motion
The simplest form of
position
objects
with
no
which are
rotation.
object
This
incapable of
switches,
or with
axis such
as spheres.
sphere's
profile does
motion is a
can only
rotation
objects which are
The latter
be used
such as
sliding
holds since a
orientation and hence
it may be displayed knowing only positional information.
move
such an object
requires only
-37-
with
symmetric about every
assertion
not vary with
change of
that the
To
center of the
object
translates along with the tongs.
Motion of
more complex
objects requires both posi-
tional and rotational information. Since the
matrix for the tongs is
already known,
transformation
the most
efficient
means of moving an object would be to use the same transformation
matrix.
This
requires that
tongs and the object be
in the same
the tongs grip the object,
reference frame as the
tong
transformation.
transformation
base for the
reference
frame.
When
the object is moved to the
same
tongs by applying the inverse of the
The
treated as a single entity.
tong
the data
tongs
and
object
can then
be
They can then be moved with the
until such time
as tongs
release the
object.
Touching Conditions
Determining
whether an object has
be broken down into two problems.
between the tongs.
to hold the object.
tongs are
been grasped can
First, the object must be
Secondly the tongs must be closed enough
This second condition requires that the
closed to a size which is
smaller than the width
of the object measured in the direction of the normal to the
jaws of the tongs.
The simplest method of determining whether an object
has been gripped is to use
This
spherical
touching
conditions.
requires that a spherical region of sensitivity
tact point
)
( con-
be defined corresponding to the size and posi-
-38-
tion of the object.
the required
closed to less
If a point
between the tongs is within
raduis of the contact
than that radius,
point and the tongs are
then the
object has been
gripped.
Objects of arbitrary shape can be grasped by using
a series
of spherical
approximated
radii.
by a
touching
group of contact
Difficulties arise
sufficeintly closed
points with
in determining if
within the tongs.
approximated by
grasped such that the tongs
object can be
The object is
different
the tongs are
if there is a possibility
one contact point being
rectangular block
conditions.
of more than
Figure 6 shows a
two spheres.
If
it
close in along the y-axis,
grasped if the
center of the
where in the
shaded region.
to be closed
to the radius of one
However,
is
the
tongs are any-
the tongs would have
sphere or half the width
of the block.
The block could be approximated by one sphere
( figure 7 ),
which would result in space which is not part
of the block
and
the tongs
being an acceptable
would not have
place to
grip the object
to be fully
closed when the
block is gripped along its small side.
Nonsymmetric
touching conditions allow for more ac-
curate and realistic grasping of objects.
quire the ability to
However, they re-
translate the touching
conditions and
the position of the tongs to the same reference frame. Since
the tongs are
easily
described by points and lines,
transformed into the object reference.
-39-
they are most
This is done
|
Km.
Figure
6
Multiple Spherical
\
Touching
Conditions
-
N
~\
?i
~``~
Figure 7
Multiple Spherical
Touching Conditions
-40-
-
Once the tongs and object are in
the object transformation.
the
same
tions.
First
it must be
make
Secondly,
possable to
the scaled
object.
possable to
touching condi-
determine if
a point
given a unit direction vector,
determine the
direction vector
The second
conditions
touching
Two requirements are placed on
satisfies them.
must be
appropriate
reference frame,
are needed.
in the tongs by the inverse of
points
by premultiplying the
condition is
it
scaling factor needed to
reach the surface of the
needed
to
determine the
closure of the tongs required to grip the object.
The most basic
nonsymmetric touching conditions are
rectangular conditions. Figure 8 shows two-dimensional rectangular conditions
for the rectangle described by length 2A,
height 2B and center (Xc,Yc).
given by
(Xt,Yt)
in the
If the center of the tongs is
object reference frame,
then the
object is within the tongs if:
IXt-XcJ <A
JYt-Yc <B
and
If the object is within the tongs,
check to
see if the
vector (Xn,Yn) be
rectangle,
necessary to
tongs are sufficiently closed.
Let the
the normal vector to the face of the jaws.
If origin for the normal vector is
the
it is then
it can be
scaled
taken to be the center of
to a new vector
(Xs,Ys)
which intersects the surface of the intersects the rectangle.
If this vector were to intersect the line Y=B,
then by pro-
portional triangles, its coordinates are found to be:
-41-
X= A+xe
Y_-8tYo
Figure 8
Rectangular
Touching Conditions
-42-
(Xs,Ys)=(A,YnA/Xn)
If the vector were to intersect the line X=A, its coordinates are given by:
(Xs,Ys)=(XnB/Ys,B)
The shorter of the two vectors is the one which stops at the
surface
of the
rectangle
width of the rectangle
normal vector.
ject if the
and its magnitude corresponds to
as measured in the
direction of the
The tongs are closed enough to grasp the ob-
closure D is less the the magnitude of the vec-
tor (Xs,Ys).
EQUIPMENT
The major equipment used in this project is shown in
figure 9.
The
simulation was
run and
controlled from the
computer terminal in the center of the picture.
The display
terminal is
right.
master
setting
on the
table
to the far
The
manipulator is directly in front of the display ter-
minal. The slave is in the background on the left. The large
rack of
electronics
behind
the
display
servo-amplifiers and control interface.
terminal
is the
The computer is not
shown.
PDP11/34 Computer
The simulation was performed on a PDP11/34 running a
RSX-11M operating system.
This is a multiuser,
timesharing
system. A PDP11/34 is not a particularly fast computer, however
it
was sufficient.
It
would be more desirable
-43-
to run on
-
-44-
·- LiPUIIU-·i-
a machine solely dedicated to this task.
E-2 Master-Slave Manipulator
The manipulator was a rebuilt Argonne National LaborThis is a light hot-
itory E2 master-slave manipulator (2).
with seven
manipulator
room style
full force reflection.
It
electronically
is
interfaced to the
it to be
degrees-of-freedom
PDP11/34
coupled allowing
AN5400 A/D
through a
converter using an interface built by K. Tani (7).
trol interface
allows
independent
and
control of
sensing and
each degree-of-freedom. Figure 10 show the E-2
The con-
slave manip-
ulator simulated in this project.
Megatek Display Processor
The graphics were performed on a Megatek 7000 vector
graphics terminal with three dimensional hardware rotate and
a resolution
ability
of 4095 lines.
from its last
to move the beam
location on the screen with
beam
can also be
The display processor
location
has the
to a new
the beam either on or off.
moved a given
The
displacement from its last
position. With appropriate manipulation, text strings can be
displayed in a variety of sizes and
orientations.
of commands can be defined to be a subpicture.
can then be executed by
referencing the
This allows a series of strokes
be defined
only once,
The commands
subpicture number.
which is used many times to
then accessed
by a single
This saves time in loading the display processor.
-45-
A series
command.
I\
IC
I
The hardware rotate is a 4x2 matrix multiplier which
is loaded
into the display
list and affects all succeeding
vectors. It is capable of rotation, translation, scaling and
clipping at screen boundaries.
of
perspective.
The origin
It is not,
for all transformations is as-
sumed to be the center of the .screen.
rotate
the
are not cumulative,
preceeding call.
each call overrides
Relative vectors
are affected by the
to
t he first
absolute vector pre-
regardless
ceeding the relative vectors
multiplier
Calls to the hardware
but rather
matrix applied
rotation
however, capable
of when the matrix
This caused some problems in moving
was called.
the sphere which was made up solely of relative strokes.
The above commands are placed in an installed common
block in the
transferred to the Megatek's
PDP11/34 and are
display list en masse by a DMA transfer. The Megatek sequentially
scans the display list and
puts the desired vectors
on the display monitor. There are also commands to determine
the location of a particular
list and
commands to
commands can be
be replaced
change
instruction in the the display
that location.
Any number of
erased from the end of the display list and
by others.
EXPERIMENTAL DESIGN
Programming Considerations
There are
tors in the
two
important,
simulation:
competing programming
speed and size.
-47-
fac-
The RSX11M system
which was operating on our machine restricts program size to
32K words. A 4K address window is
led
common
needed to
block used
communicate
common block was
DMA transfer.
required for each instal-
in a program.
with the
One common block was
A/D and
needed to hold
Megatek,
Megatek commands
and one
prior to
This means the maximum effective program size
was only 24K. Speed was a factor since it was desirable that
the
when
program run in real time.
the program
dynamics
ran
program,
generated
simultaniously
with
H. Kazerooni's
which required a cycle time of less than
230 milliseconds to
puter
It was particularly critical
retain stability.
force-feedback
Secondly,
was desired,
needed to send signals to the manipulator
when com-
the computer
control interface
at time intervals on the order of 20 milliseconds.
Most of the graphics program was involved in calculating rotation matrices and
manipulating objects.
Only a
small portion of the program was involved in initialization.
This eliminated the possiblity of using overlays or multiple
tasks to reduce program size.
promises
between
subroutines
arrays
It was necessary to make com-
versus
explicit
variables,
versus inline program segments.
ables and inline programming
program space.
are faster,
When there are many
but
and
Explicit varirequire more
zero terms in a program
section, such as those which occur when preforming multiplications of rotation matrices, explicit programming may be as
-48-
short as subroutines and obviously much faster.
explicit
variables
speed and ease of
The
placing
and
inline
In general,
programming were
used for
interpretation.
amount
of data
all arrays
in the
space required
unused
portions of the
block associated with the display processor.
for the display was
placed
was reduced by
common
All point data
in direct access files,
elimi-
nating the need to bring it into the program. This also made
program size independent of display complexity.
hardware rotation and
Using the display processor's
subpictures reduced cycle time by a factor of three over the
equivalent program without these options. During the initialization segment of the program,
the display
form.
In
created
into the
calling
lieved
display
was drawn
the main
each independent element of
into a
body of
by loading the
subpicture
the
appropriate
display processor's
the need to individually
transformation
matrix
followed by
The use of subpictures re-
from the task
each program cycle.
the display was
hardware rotate,
the desired subpicture.
the computer
program,
in its unrotated
of reloading the entire
The hardware rotate eliminated
calculate the
coordinates of each
point in the display.
When the
manipulator was allowed to move freely un-
der computer control it was critical that the manipulator be
controlled at regular time intervals.
-49-
Since these intervals
were
shorter
than the
control of the
cycle time of the
display program,
manipulator during free motion was performed
by a separate program. When the manipulator was touching the
simulated
surface and
the cycle
time was not as
the
force-feedback was
being generated,
critical because the velocity of
manipulator was small.
Since in this
case the display
angular position of the
program needed to calculate the new
manipulator required to generate force-feedback, the display
program
controlled the arm
while the
control
program re-
mained dormant.
Syncronization between the two programs was
achieved by the
use of flags
passed
through an
installed
common block.
Manipulator and Vehicle Simulation
A design for the manipulator was chosen which simply
but accurately represented the shape of the E-2 manipulator.
Since no hidden line removal
the
cycle time of the
was attempted in order to keep
program down,
too many
lines would
have made the display difficult to view. The tongs which are
the most
important
most detail.
portion of
the display were
The forearm and shoulder had
given the
a square, rather
than circular cross-section. Cylinders are difficult to display correctly unless complex
mine the
also gave
algorithms are used to deter-
location of their edges.
a better
perception of
cular cross-section.
-50-
The square cross-section
rotation
of than a cir-
Two types
of objects
and touching
conditions were
implemented. The first object implemented was a sphere using
spherical touching conditions.
a
rectangular peg
rectangular
When this proved successful,
was installed.
The peg used two sets of
One for the
touching conditions.
main body of
the peg and one for the stem.
dynamic
Various
and
static
could
properties
bb
inputted for the objects allowing a simulated environment to
be built up
of collision,
the simulation.
which could be entered into
The manipulator
rectangular room.
with a wall of the room,
from demonstrating
pose of the walls
elasticity
drag,
and the hardness of
conservation of momentum
the objects were all parameters
within a
Gravity,
within the computer.
and objects
When a moving
it rebounded
object
collided
off the wall.
conservation of momentum,
was to keep the
were enclosed
Aside
the main pur-
objects within the reach
of the manipulator.
Two applications of
When an object was gripped
force-feedback were introduced.
by the manipulator,
force-feed-
back was sent to keep the tongs open to the width of the objects.
The resulting sensation was that of an actual object
within the tongs.
The second application
force-feedback to the full manipulator.
involved applying
A three-dimensional
surface was defined. The surface was assumed to be relatively flat
eliminating the need to
-51-
calculate the surface nor-
mal,
instead the surface normal was assumed to be vertical.
assigned to various locations on
hardnesses were
Different
the surface. Force-feedback was implemented so that the surwas touched
face could be felt when it
As visual feedback,
by the manipulator.
a gridwork approximation of the surface
was displayed on the
graphics terminal. To aid the operator
in perceiving depth,
the contour directly below the manipu-
lator was displayed
in darker linework.
As
the manipulator
penetrated the surface, the contour deflected. The extent of
the deflection was dependent on the stiffness of the surface
If the surface was soft,
and depth of penetration.
neighborhood of the penetration.
occurred in the
tion only
deflec-
If the surface was stiff, the whole surface deflected.
The simulation could be displayed from any viewpoint.
stationary or moving. It has
could be either
The viewpoint
that a moving
been suggested
giving the operator a better
sional nature of the
viewpoint
might be useful in
perception of the three-dimen-
environment
(6).
The display could be
scrolled and zoomed so that any portion of the display could
be observed
in detail.
The viewpoint
could be
controlled
from the keyboard without interrupting the program by use of
a request
for IO0
(QIO).
The vehicle simulation was capable of the same functions
mounted
as the
manipulator
on a vehicle
simulation.
which was
-52-
The manipulator
capable of
was
six degree-of-
freedom
motion.
The position of the
vehicle was
inputted
through an installed common block, which allowed the vehicle
to be controlled by a secondary program.
Depth Indicators
A
major difficulty associated with a vidio terminal
is the lack of depth perception. Normally, when one is looking at a three-dimensional scene,
peception of
between the
three-dimensions from
scene veiwed
one's brain generates the
differences in parallax
through the
right and left eyes.
The differences in parallax are associated with the distance
between the two eyes
and the distance
between
objects and
the eyes. When a scene is displayed on a vidio terminal, all
points are at the same distance from the eyes and the parallax information is lost.
When the
simulation was first developed,
depth in-
formation was transmitted using the traditional orthographic
projections.
This approach
ation problems
and the
appeared to cause some coordin-
operator seemed
to become confused
over which view was the front and which was the side. Experienced manipulator
operators often
rely heavily on shadows
for depth cues. The second depth indicator was a shadow with
the source
of
illumination
directly overhead.
The shadow
was cast on a imaginary horizantal floor 50 inches below the
manipulator's shoulder.
helpful in
Walls displayed
orienting the shadow.
-53-
on the screen were
Both of these
indicators
require
extensive prior
knowledge of the environment.
third depth indicator was a
proximity indicator.
The
The indi-
cator showed the absolute distance between the tongs and the
object
as a line on the
display terminal.
The display was
designed such that the length of the line was
as the
display when the object was
tongs.
The indicator was
ranges.
The
proximity
the same scale
within 24 inches of the
ten times less sensitive at longer
detector
could be
implemented
by
placing a sonar device on the tongs of a manipulator.
Experimental Measurement of Operator Performance
The
three
depth
tested on five subjects.
The first
indicators
Two types of
involved reaching
stationary sphere.
plus a
out and
however the
In the
were
tasks were designed.
grabbing a two
inch,
The time required for a subject to grasp
the sphere after the display was flashed
recorded.
control
second experiment,
sphere was moving.
on the
screen was
the task was the same
The path
of the sphere was
an orbit about the surface of an ellipsoid described by:
X=Xc+10sin( t/3)cos(t/12)
Y=Yc+5sin(t/3)sin(t/12)
Z=Zc+7cos(t/3)
where
(Xc,Yc,Zc)
ellipsoid,
t was the
measured in inches.
bring
was a vector describing the center of the
out any
time in
seconds
and
distances were
It was hoped that the moving task would
coordination
problems
-54-
associated
with the
depth indicators.
All three display types were shown at the same scale.
The shadow was
azimuth and
display
with reference
displayed along
zenith of 15 degrees ( figure 11 ).
type was a
displayed vertically
The third display
front view with a
above the
proximity
The second
proximity indicator
manipulator
( figure 12 ).
front and a side orthographic
type was a
projection shown side by side (figure 13 ).
the
walls at an
indicator with
no
The control was
view of the
manipulator
( figure 14 ).
The subjects were run
through a
series of
display
types with both moving and stationary objects. The positions
of the objects had to be selected
mained
within
the
reach
such that the objects re-
of the
manipulator and
did not
coincide with any real object which would obstruct the manipulator. Display types were mixed because subjects tended to
become bored with repetitions of the same display type.
order of
the display
subjects
knew which display type to expect next.
types was
kept constant
The
so that the
The posi-
tions of the sphere were arranged such that the average distance between
successive
positions
was the same
for each
display type.
RESULTS AND CONCLUSIONS
Master-Slave Simulation
As the simulation developed,
-55-
it became increasingly
Figure
-
11
Shadow
Display Type 1
with
Walls
Figure 12
Display Type 2
- Front and Side Isometrics
·
0
0
0
S
0
·
Figure 13
Display Type 3
- Front View with Proximity
Indicator
Figure 14
Display Type 4
- Proximity Indicator
Alone
0
0
apparrent that that the cycle time of the program was an important issue.
By using the techniques mentioned under Pro-
gramming Considerations,
it was possable
to keep the cycle
time of the program to 60-100 milliseconds. This seems to be
quite tolerable.
Figures 15 and 16
show
the
simulated
manipulator
and its shadow from
various viewpoints.
object can
seen in these illustrations.
also be
are needed as a reference for
The sphere and the
The walls
interpreting the shadow.
The
shadow is cast on the floor of the cube. Figure 17 shows the
tongs
grasping a
orientations.
rectangular
It is
peg from
important to
the jaws is dependent on the
several
different
note that the closure of
orientation of the object. The
object is centerred between the jaws when it is gripped.
Figures 18 and 19 show
dark contour is the
the simulated
profile of the surface
surface.
directly
The
below
the manipulator. Figure 19 shows the surface being deflected
by pressure from the simulated manipulator.
Figures 20
21
a soft
show the
differences in
deflection of
( figure 20 ) and a hard surface ( figure 21 ).
ion of a
soft surface
and
surface
The deflect-
is much more localized when compared
to the deflection of a hard surface.
The
submarine
simulation was
capable
of the same
functions as the manipulator simulation. It was also capable
of motion in six degrees-of-freedom
-58-
under control
of a se-
Figure
15
Manipulator
Simulation with
-59-
Shadows
Figure 16
Manipulator
Simulation with
-60-
Shadows
Figure 17
Detail of Tongs
Gripping a
-61-
Rectangular Peg
0
Figure
18
Simulated
Surface
0
Figure 19
Surface
Deflection of Simulated
Figure 20
Deflection of a Soft
Figure 21
Deflection of a Hard Surface
Surface
-63-
condary
lated
program.
Figure 22 shows the
details of the simu-
submarine.
Modifications
of the
structure of both the manipu-
lator and the submarine could be made easily by changing the
data bases. This allowed the displays to
point-connectivity
be easily adapted
the submarine or manipulator
to represent
being simulated at the time.
Simulated Force-Feedback
One
simulation which was quite suc-
feature of the
cessful but was not formally
feedback.
evaluated was
Force-feedback generated when
simulated force
grasping an object
gave the simulation a strong feeling of reality. It could be
used at two levels.
The first
level was conformational.
On
this level,
the force-feedback was either on 100 percent or
it was off.
This was highly
because it gave tactile
the tongs.
conformation of the object being in
happened to slip
If the object
the loss of tactile feedback
Though the
operator
several seconds.
by looking at
was no longer
with other
from the tongs,
gave an immediate
could tell
display that the object
was preoccupied
useful when grasping an object
tasks,
The change in
indication.
the graphic
in the tongs,
he might not notice for
tactile feedback was impos-
sable to ignore. The second level was quantitative.
level, force-feedback could contain
ness or weight etc..
On this
information about hard-
Surfaces varying
-64-
if he
between
very
rigid
Figure
22
Submarine
Simulation
-65-
and the
foam
consistancy of
simulating a
difficult with
greatest
were
rubber
simulated.
variety
of types of
simulation.
time of the
surfaces came from the cycle
The
As
a
surface becomes softer,
deeper penetration was possable for
applied force.
This larger range of motion implied
a given
required to be
manipulator was
that the
moved larger dis-
The simulation
tances each program cycle.
program had dif-
ficultly updating the manipulator quickly enough. The result
was that the manipulator
in discrete steps when feel-
moved
ing a soft surface. Despite this difficulty, it was possable
to
simulate a large range of hardnesses. Although the manipperfect
ulator response was not
sponse was tolerable.
back seems quite
for soft surfaces,
Overall the
its re-
simulation of force-feed-
promising.
Depth Indicators
Each of four subjects preformed 80 to 90 repetitions
of each the four display types and two tasks. Subjects 1 and
2 were experienced
graphic simulation.
with both
the E-2
manipulator
Subject 4 was experenced with the mani-
pulator and subject 3 had no prior experience.
learned to
curves were
use the displays
in one to
recorded for each subject
these curves
showed no significant
performed an
additional 32
data from these
and the
All subjects
two hours.
Learning
( Appendix I ).
learning,
repetitions of
When
the subjects
each task.
The
final trials was analysised. The overall re-66-
figure 23.
summarized in
sults are
found in appendix I.
are
with no addi-
The proximity indicator
tional display clearly gave very poor results.
this would not overwelm the other
results
Complete
In order that
three display types,
the
proximity indicator was left out of the statistical analysis.
analysis-of-varients
A three-way
three
maining
showed
display
significant
types and the
was performed
types ( Appendix I ).
differences between
The
results
subjects,
display
two tasks. There were significant interactions
and between subjects and tasks.
subjects and display types,
no significant effects of tasks on display type.
There were
The front and side
orthographic
performance on both tasks.
the best
on the re-
projections showed
The subjects felt that
it presented only slight coordination problems. Three of the
display because
they felt it
shadow showed the next best times.
All subjects
four subjects
gave
preferred this
the clearest
The
felt that
the
detail.
shadow gave
perception of the
them the best
position of objects in the environment. The main reason they
found it
difficult to use
was that the
shadow of the hand
tended to obscure the shadow of the object when the two were
in close
proximity.
If the subject
object and needed to know which way
away from
shadows.
the
object to
the
was very
to move,
get a clear
Although the results were not
-67-
close to the
he had to back
picture
of the
statisticly
signi-
1 SUA Dow
-q
I
2 FoNT
4
3 AoT
o NS
oX Oi L
A PQ
J'J•
-'.(
r .1
/I
-0J
-8
ST"A TIoN A R Y
Figure
/4 oV X A/
Evaluation of Depth Indicators
-68-
OVE
ALL
the shadow was less affected by the motion of the
ficant,
object than the front and side views. Larger or faster momore significant
may show
tions
differences.
might be improved by reducing the intensity of the
of the
shadow
in the
shadow
The
linework
reducing the
manipulator or
number
of lines. The shadow may show better performance on a larger
object.
One subject preferred the
front view with the prox-
because of its simplicity.
imity indicator
proximity indicator were close to
those
of the shadow.
information from the
order to obtain depth
Times using the
In
proximity indi-
cator the operator had to move the manipulator and watch the
response
of the
indicator.
mistake.
in
the
Although
implemented,
wrong
direction
the proximity
it gives
little
to cause
though he would occasionally end up
trouble to the operator
searching
seemed
This
before
realizing
his
indicator could be easily
only limited
depth
information and
probably would not be suitable for a complicated task.
The proximity indicator by itself was much more difficult to use than
learn a method of
any of the other displays.
moving the manipulator in order to deter-
mine the proper direction of motion.
was extremely difficult
ject
would tend to
The proximity indicator
to use on a moving object.
change direction of
subject was getting near to the object.
-69-
0i
Subjects did
The ob-
motion just as the
Although
indicators,
time required
there were
these
to
differences
differences
perform the
between the depth
were small
task.
compared to the
It is
difficult to say
with certainty which is best since each has its good and bad
points.
The best depth
bination of the
indicator would appear to be a com-
front and side views
shadow could be used to obtain an
with the shadow.
The
overall perception of the
environment while the front and side views could be used for
detail work.
With a computer
the operator
to select
graphic simulation,
the
best
view
allowing
would be
a simple
matter.
Conclusions and Recommendations
The manipulator simulation has proven to be a highly
flexible
and valuable tool.
It is
presently being adapted
to perform several types of function.
H. Kazerooni is using
the vehicle simulation
to test and evaluate different types
of control systems for
small submersibles.
jects is to design a fuel-optimizing,
One of the pro-
bottom-following rou-
tine. The vehicle simulation allows the operator to know the
position and orientation of the vehicle in real time instead
of needing to interpret
visual
feedback
also
under manual control.
interactions can be
many pages of computer output.
allows
the
vehicle
It is hoped that
integrated into
near future.
-70-
the
to
be
The
driven
manipulator-vehicle
simulation in the
onment
D. Fyler is
examaning methods of defining the envir-
by feeling
about with
a manipulator
and recording
any contact with an object using a variation of this program.
and a polyhedron is fit-
The data obtained is then analyzed
ted to the data points. This could prove
in
situations
similar
of poor
tests in which a
visablity.
to be highly useful
B. Wood
is
predefined object is
performing
inserted into
the display by feeling for reference points which orient the
object.
The concept of a manipulator interacting with a simulated environment has proved to be very interesting. It may
be fruitful to improve the reality of the simulated environment.
Areas of
conservation of
improvement
might
angular momentum of
At the present time
include
the
addition of the
simulated objects.
interaction between the object
the only
and the manipulator
occurs when the object is gripped.
simulation could be
improved by
The
considering collisions be-
tween the object and other portions of the manipulator. This
could allow the object to be pushed by the manipulator without being gripped.
The use of
force-feedback
could be ex-
panded to allow the object to feel heavy when picked up,
to
allow the
operator
to feel a
reaction force
manipulator was struck by a moving object.
-71-
or
when the
REFERENCES
1.
Brooks,
Thurston L.,"Superman: A System for Supervisory
Manipulation
and the Study of Human/Computer Interactions",
Masters Thesis, Department of Mechanical Engineering, M.I.T.,
May 1979, MIT Sea Grant Report MITSG 79-20.
Stephen H.,
2.
Crandall,
F.
and Pridmore-Brown,
Dean C.,
Edward
1968.
Deghuee, Bradley J.,"Operator-Ajustable Frame Rate, Res-
olution,
Tradeoff in Fixed-Bandwidth Remote
and Gray Scale
Department of Mechan-
Manipulator Control", Masters Thesis,
ical Engineering, M.I.T.,
4.
Kurtz Jr.,
Dynamics of Mechanical and
David C.,
McGraw-Hill,
Electrical Systems,
3.
Karnopp,
Sept. 1980.
Faux, I.D. and Pratt, M.J.,
Computational
Geometry for
Design and Manufacture, Ellis Horwood, 1979.
5.
Meyfarth, Philip F.,
notes,
M.I.T.,
2.157
Computer-Aided
Design class
1979.
M.I.T., personal communication,1981.
6.
Sheridan, Thomas B.,
7.
Tani, Kazuo, "Supervisory Control of Remote Manipulation
with
Compensation for
Laboratory Report,
8.
Whitney,
D.E.,
lators and Human
Machine Systems,
Moving Target",
Man-Machine Systems
M.I.T.,1980.
"Resolved Motion Rate Control of ManipuProstheses",
Vol.
IEEE
MMS-10, No.
-72-
Transactions
2, June 1969.
on
Man-
APPENDIX I
DEPTH INDICATOR EXPERIMENT STATISTICS
Four
the
subjects
various depth
shown
performmed
indicators.
in figures
24, 25, 26
the
Their
and 27.
curves became relatively flat,
experiments testing
learning
When
curves are
these learning
indicating that the subjects
each subject performmed 32
had learned to use the displays,
additional repetitions of each display type. The means
( X )
and the standard deviations ( S ) for these tests are compiled
in
tables
1-5.
TABLE i AVERAGE
TASK
ISPLAY
shadow
front & prox
front & side
prox only
all
displays
TIME TO LOCATE OBJECT
SUBJECT #1
stationary
X
S
2. 87
1.00
3.83
1.76
3.08
1.14
13.54
10.74
5. 83
7.08
moving
X
S
3.06
1.35
1.57
3.95
3.60
1.30
32.45
24.40
10. 77
17.55
(seconds)
both tasks
X
S
2.97
1.19
3.89
1.67
3.34
1.28
23.00
21.03
8.30
13.59
TABLE 2 AVERAGE TIME TO LOCATE OBJECT (seconds)
SUBJECT #2
TASK
DISPLAY
shadow
front & prox
front & side
prox only
all displays
stationary
X
3.89
3.77
2.34
12.55
5.50
moving
S
.95
1.07
.68
8.16
5.79
both tasks
S
3.09
3.54
2.41
23.65
8.12
-73-
1.47
1.26
.76
18.77
13.00
X
3.24
3.66
2.37
18.10
6.84
S
1.30
1.17
.72
15.50
18.17
SHAIM
POI
Or
rOvIN
f
a0
s
I
I
TRIALS
TRI•L
g
STATIONARY
*
s
I8
TRIALS
TRIALS
.PIM OLr
STAT1O•IAr
wolU 4 5101
S1TATIImY
STATIOrM
5
I
t~~l~
°.
•
•
~
%
-, ".
. ...
•..~~
*
a
0
SO
Figure 24
TRIALS
0
..
1.
SO
a
TRIALS
Learning Curves -
a
0
Subject 1
-74-
SO
TRIALS
0
50
TRIALS
I
w
row &Po /e
*me
mOVINW
in
flot I sMo
Iqgll6
PON= OIr
g
w.
yie
r
,r
·'.
.
*.
***..
.*
*..
I
-;.~rYS~Y
m
U
0
TRIALS
a
5
0o
50
TRtIA.
I
MS
TRIM•)
0""T 4 POe
SIATISMIY
%."
tr)lIA
o
I
tqr.*
r*%,
SO
Figure 25
___~~_
TRI ALS
0
S
TRIALS
Learning Curves - Subject
-75-
TRIALS
0
SO
TRIAL)
pPox
SHADOW
roMT
MOVINS
PovinS
FRONm 6 SIDE
S
MOvlN
PNo
oM.v
nOv11'
Y
in-
I·
p
*
~
*
*
. . - :
.
'.'
1..
..
a I
a
SO
So
0
TRIALS
TRIALS
s
aO
TRIALS
'
0
5o
C6
0
STATIU
STAT ICO y
STATIONARY
STATIONAr
1
STATIOmrY
TRI ALS
'· '
'
~
·· ·
~·.
.
·: ··.
'
'····
~
· r_...
·. ·
.r~;
a
a
Figure
so
26
TRIALS
0
50
·
· J.·' .
TRIALS
Learning Curves -
r
·..
·
-·
~
0
Subject 3
-76-
SO
Q
TRIALS
0
50
TRIALS
---
w
SHADOW
fOVIoT
POIVn
e0,,
Mo•' m
row
nOVIN
aR
Sim
!PROx
OM
ove
'·
··
* * ....
." ,.
•
"• """"•7.-•"
"
.·s
"'
TRIA.L 0u
5e
.. .
S.
··
· ~l~.·~·····;C~~
AU
II
.
,.. •' .
. *. .
5
YSlA· a
· r·rrr
..
,"
.
,
,.*
..
_
TRAL
g
u
dPU
58
8
s
a
• 4
TRIALS
· •o ·
TRIES
wl
PROo ONLY
mel
Iml
SIATOSVD
STATIONAR
i
I
,.
92
,.
.1
I
j.~..
.r.. .
a
50
Figure 27
TRIALS
0
SO
TRIALS
0
Learning Curves - Subject 4
-77-
50
TRIALS
a
•
50
TRIALS
TABLE 3 AVERAGE TIME TO LOCATE OBJECT (seconds)
SUBJECT #3
both tasks
moving
stationary
X
S
TASK
DISPLAY
shadow
front & prox
front & side
prox only
3.74
3.73
2.83
14.07
2.05
1.26
1.11
11.21
4.74
5.56
3.77
35.36
S
2.93
5.73
1.67
28.51
X
4.24
4.65
3.31
24.72
S
2.58
4.43
1.49
24.14
all displays
6.09
7.38
12.36
22.37
10.15
17.26
TABLE 4 AVERAGE TIME TO LOCATE OBJECT (seconds)
SUBJECT #4
stationary
X
S
4.93
1.83
3.91
2.90
TASK
DISPLAY
shadow
front & prox
moving
X
5.12
4.43
S
4.00
2.51
both tasks
X
S
5.03
3.11
2.74
4.17
front & side
3.26
.99
4.28
2.39
3.77
1.90
prox only
all displays
20.90
8.24
11.21
9.40
34.44
12.07
36.14
22.37
27.67
10.15
27.60
17.26
TABLE 5 AVERAGE TIME TO LOCATE OBJECT (seconds)
ALL SUBJECTS
TASK
DI SPLAY
stationary
X
S
moving
X
shadow
3.73
1.54
4.00
2.67
3.87
2.1ý
front & prox
4.06
1.89
4.37
3.29
4.22
2.69
front & side
prox only
2.88
15.26
.99
14.33
3.50
31.47
1.65
27.69
3.19
23.36
1.40
23.49
all displays
6.62
8.88
10.8
18.41
8.94
14.62
S
both tasks
X
S
From looking at the results of the experiment it
clear that the
proximity
was
indicator alone was a much poorer
depth indicator than the other three displays. In order that
the poor results from the
proximity indicator not
overwelm
the other indicators, it was left out from further analysis.
A
three-way
analysis of
remaining three
variance
displays using
was
performed
on
the
IBM's Scientific Subroutine
-78-
Package
( ANOVA
variation and
computes the
interaction.
was determined
of the
) which
by a
variation
mean square for each
The signifacance of the results
F statistic defined as the mean square
in question
divided
by the
mean square
the residuals. The residuals were taken to be all interactions involving the number of trails.
The following is a
condensation of the results.
TABLE 6
THREE-WAY ANALYSIS OF VARIANCE
S=SUBJECTS
E-EXPERIMENT TYPE (MOVING OR STATIONARY)
D-DISPLAY TYPE (SHADOW, FRONT & SIDE, FRONT & PROX)
R=REPETITIONS
SOURCE OF
VARIATION
F(fl,f2)
S
E
13.26(3,744)
9.84(1,744)
.0005
.001
D
11.70(2,744)
.0005
LEVEL OF
CONFIDENCE
(nul)
SE
3.58(3,744)
.125
SD
ED
2.98(6,744)
.50(2,744)
.01
.95
SED
jects,
These results
show clear
display types
and
significant
differences
spect to their
types
and on
.95
.36(6,744)
the two
differences
between sub-
experiments.
There are
among individual subjects with re-
performance
experiments.
both on the
different
display
The experiment type had no sig-
nifacant effect on differences between the display types.
-79-
APPENDIX II
TRANSFORMATION MATRICES
SIMULATION
divided
The arm was
into
three
sections:
the hand,
the forearm and the upper arm. The coordinates of the center
of
rotation of each
eliminated the
need
to
translate the
the origin, perform the rotation,
This
origin.
section were set at the
center of rotation to
then translate the center
of rotation back to the proper position. Instead the section
rotated, then
in question needed only to be
the proper location.
translated
The points of rotation and the
nation of the angles are shown in figure 12.
At
to
desig-
each joint,
the order of rotation was torsion first, followed by flexure.
Rotations were performed first at the extremities. First the
hand was rotated,
then it was translated so that its center
of rotation was located at the end of the forearm.
Next the
hand and the
forearm were rotated about the elbow,
then the
hand and the
forearm were translated so that the
elbow was
Lastly the
whole arm
located at the
end of the upper arm.
was rotated about the shoulder. For simplicity,
sin(An) were abbreviated
mation
the
Cn and Sn respectively.
cos(An) and
Transfor-
matrices were found for each section of the arm. For
convenience of the reader,
that used in the
the notation is the same as
simulation software listed in Appendices V
and VI.
The rotation
matrix
of the
-80-
upper
arm
around the
shoulder is given by:
1
0
0
0
C2 -S2
0
0
0
0
Cl
Sl
-S1
CI
0
0
S2
0
0
1
0
0
0 0
0
0
C2
0
0
0
0
0
Which reduces to:
C2
-S2
CIS2
S1S2
C1C2
SIC2
0
0
This was abbreviated
0
0
-Si
Ci
0
0
0
1
as:
XX
XY 0 0
The
YX
ZX
YY
ZY
YZ
ZZ
0
0
0
0
0
1
transformation
matrix for rotation about
the elbow and
translation to the end of the upper arm is found from:
1000
1
0
1 0 0 0 C4 -S4 0 0
0
0
0 0
1 ZFT
0
0
C3 -S3
S3
C3
0
0
S4
0
C4
0
0
1
0
0
0
0
0
0
0
1
0
0
0
1
1
0
Which reduces to:
C4
-S4
C3C4
-S3
0
0
S3S4
0
S3C4
0
C3
0
ZFT
1
C3S4
This
transformatiom
0
matrix multiplied
-81-
with the matrix for
calculated above gives the fol-
rotation about the shoulder
lowing transformation matrix for rotation of the forearm:
-XXS4+XYC3C4
XXC4+XYC3S4
YXC4+YYC3S4+YZS3S4
-YXS4+YYC3C4+YZS3C4
ZXC4+ZYC3S4+ZZS3S4
-ZXS4+ZYC3C4+ZZS3C4
0
0
-XYS3
-YYS3+YZC3
0
YZZFT
-ZYS3+ZZC3
ZZZFT
0
1
For simplicity this was abbreviated as:
XY1
XZl
0
YX1
YY1
YZl
YT1
ZXI
ZY1
ZZ1
ZT1
0
0
0
1
XX1
The transformation matrix for rotation of the hand about the
wrist and translation of the wrist to the end of the hand is
determined in the
for the forearm.
same manner as the
It
is
given by:
C6 -S6
0 0
S5S6 S5C6
C5 ZST
C5S6 C5C6 -S5
0
0
transformation matrix
0
0
1
This is multiplied by the transformation matrix for rotation
about the elbow and shoulder to give:
-82-
XX1C6+XY1C5S6+XZIS5S6
-XX1S6+XY1C5C6+XZlS5C6
YX1C6+YY1C 5S6+YZ S 5S6
ZX1C6+ZY1C5S6+ZZlS5S6
0
-YX1S6+YY 1C 5C6+YZS 5C6
-ZXlS6+ZY1C5C6+ZZlS5C6
0
-XY1S5+XZ1C5
-YYS 5+YZ1C5
-ZYiS5+ZZlC5
XZ1ZST
YZlZST+YT1
ZZlZST+ZTI
0
1
For simplicity, this was abbreviated as:
XX2
YX2
ZX2
0
XY2
YY2
ZY2
0
XZ2
YZ2
ZZ2
0
XT2
YT2
ZT2
1
The translation matrix for the closure of the tongs
for a given closure coefficient D is
1 0 0 0
0
0
0
The
1
0
0
0
1
0
1.9D
-. 8D
1
motion of the
about the z-axis,
submarine is
described by
roll
followed by pitching about the x-axis and
finally yaw about the y-axis as expressed by:
CV2 0 SV2 0
0
1
-SV2 0
0
0
The vehicle is
1
0
0
0
CV2 0
0
0
0
0 0
1
0
0
CV1 -SV1 0
SV1 CV1 0
0
then moved into
1
CV3 -SV3 0 0
SV3
0
CV3 0
0
1
0
0
0
0
1
0
position by the translation:
-83-
1
0
0
0
0
1
0
0
0
0
1
0
v11
V12+YSUBT
V10+ZSUBT
1
The resulting transformation matrix is:
-SVICV2+CVISV2SV3
CV1CV3
SV1SV2+CV1CV2SV3
0
CVICV2+SVISV2SV3
SVICV3
-CVISV2+SVlCV2SV3
0
SV2CV3
-SV3
V1l
V12+YSUBT
0
1
CV2CV3
V1O+ZSUBT
The submarine transformation matrix was abbreviated by:
With the above
SXX
SXY
SXZ
SXT
SYX
SZX
SYY
SZY
SYZ
SZZ
SYT
SZT
0
0
0
1
transformations,
the arm can be
described in
three-space with seven
the
motion can be
vehicle
described
completely
degrees-of-freedom and
in
six
degrees-of-
freedom.
The azimuth and zenith of the viewpoint were selectable by the operator. The order of operations was an azimuth
rotation
mation
about the y-axis, followed by a
about the
x-axis,
lation incorporating a
zenith
transfor-
and finally a positioning trans-
scaling factor
mations are given by:
-84-
S.
These
transfor-
S 0 0 STX
1 0
0
0
0
0 0 S 0
S
0
STY
0
0
WC2 -WS2
WS2 WC2
0
0
0
0
S
0
0
1
0
0
WC1 0 WS1 0
0
1
0
0
0
0
0
1
-WS1 0 WC1 0
Since the graphic terminal only displays X and Y, only the
first two rows needed to be computed yielding:
SWC1
0
0
0
0
0
SWS1WS2
SWC2
SWS1
STX
1
0
0
1
-SWClWS2
These transformations were abbreviated by:
WXX
WXY
WXZ
WXT
WYX
0
WYY
0
WYZ
1
WYT
0O
0
0
0
1
-85-
STY
APPENDIX III
FORCE-FEEDBACK JACOBIAN CALCULATIONS
In order to implement simulated force-feedback,
the
jacobian matrix for the tip of the tongs needed to be calculated.
The position of the tip of the tongs is
XX2
XY2
XZ2
XT2
0
YX2
ZX2
YY2
ZY2
YZ2
ZZ2
YT2
ZT2
0
ZHT
0
0
0
1
given by:
1
This reduces to the position vector for the tip of the
tongs given by:
XZ2ZHT+XT2
YZ2ZHT+YT2
ZZ2ZHT+ZT2
1
The derivatives
matrix.
of this
position
vector
In the simulation software,
are the jacobian
this matrix was repre-
sented by:
DXD1
DXD2
DXD3
DZD1
DZD2
DZD3
DYDI
DXD2 DYD3
The terms of this matrix are given by:
DXD1=0
DYD1=(-S1S2S4+S1C2C3C4+C1S3C4)S5HTR
+(S1C2S3-C1C3)(C5HTR+ZFT)-C1ZST
DZD1=(CIS2S4-C1C2C3C4+S1S3C4)S5HTR
+(-CIC2S3-S1C3)(C5HTR+ZFT)-S1ZST
DXD2=(-S2S4+C2C3C4)S5HTR+C2S3(C5HTR+ZFT)
DYD2=(CIC2S4+CIS2C3C4)S5HTR+C S2S3(C5HTR+ZFT)
DZD2=(S1C2S4+S1S2C3C4)S5HTR+S1S2S3(C5HTR+ZFT)
-86-
DXD3=-S2S3C4S5HTR+S2C3(C5HTR+ZFT)
DYD3=(CIC2S3C4+S1C3C4)S5HTR+(-C1C2C3+S1S3)(C5HTR+ZFT)
DZD3=(SIC2S3C4-CIC3C4)S5HTR+(-SlC2C3-CIS3)(C5HTR+ZFT)
The jacobian matrix for the manipulator attached to a vehicle
is found by premultiplying the jacobian matrix determined above by the rotation matrix for the vehicle.
DXD1'
DYD1'
DZD1'
DXD2'
DYD2'
DZD2'
DXD3'
DYD3'
DZD3'
SXX
L SYX
SXY
SXz
SYY
SYZ
SZX
SZY
SZZ
-87-
ILi
DXD1
DXD2
DXD3
DYD1
DZD1
DYD2
DZD2
DYD3
DZD3
APPENDIX IV
AUXILLARY SOFTWARE
This program
calls several
subroutines involved in
performing graghics on the Megatek graphics terminal.
MGINIT initializes the graphics terminal.
NPOINT(loc) reads the address of the next entry into
the display.
MOVI3(ix,iy,iz)
iy, iz
moves the beam to the
position ix,
without drawing a line.
DRWI3(ix,iy,iz)
draws a line from the previous posi-
tion of the beam to ix, iy, iz.
RMOVI3(ix,iy,iz) moves the beam an increment ix, iy,
iz
from its last position.
RDRWI3(ix,iy,iz)
iz
draws a line an
increment ix, iy,
from the last beam position.
MGSEND sends the picture to the Megatek display.
MODIFY(loc)
replaces
the command at address loc of
the display list by the next Megatek command executed.
LDTRN3(xx,xy,xz,xt,yx,yy,yz,yt)
loads the first two
rows of a transformation matrix into the hardware rotate.
SUBDEF(i) defines beginning of subpicture i.
SUBEND(i) defines the end of subpicture i.
SUBCAL(i) calls subpicture i.
The following programs operate the A/D converter:
ANINIT initializes the A/D converter.
-88-
AINSQ(ichanl,ichan2,iarray)
input
channels ichanl to ichan2 of
reads data from
the A/D
analog
converter into
the array iarray.
AOUTSQ(ichanl,ichan2,iarray)
writes
from
array
iarray to analog output channels ichanl to ichan2.
DIN(ichan,ivar)
reads
digital input
channel ichan
into ivar.
DOUT(ichan,ivar) writes ivar to digital output channel ichan.
PATH, NPATH are arbitrary contours which are used by
H. Kazerooni's dynamic programs for
bottom following exper-
iments.
GETADR, READEF AND QIO allow the keyboard to be read
with out interrupting the program.
-89-
APPENDIX V
MANIPULATOR SIMULATION SOFTWARE
C INITIALIZE PROGRAM
DIMENSION IA(16),A(7),IPARAM(6),IOSB(2),IBUF(12)
BYTE ACON(34,2)
COMMON /DMABUF/IDUM(3610),ADAT(22,3),PNT(22,2),
1
TCH(3,6),SCL(7,2),II(7),Il(7),12(7)
,3(7),IS(7),Fl,
1
IT1,IT2,ICTL
DATA OXX,OXY,OXZ/1.,O.,O./
DATA OYX,OYY,OYZ/O.,1.,0./
DATA OZX,OZY,OZZ/O.,O.,1./
ICTL=O
WRITE(5,1)
FORMAT(' ENTER FUNCTION'/'
100
1
1
C
C
C
2
1=SPHERE'/' 2=OBJECT'/
' 4=SURFACE')
READ(5,*)IFUNC
ISPH=O
IOBJ=O
ISURF=O
IF((IFUNC.AND."1).NE.O)ISPH=1
IF((IFUNC.AND."2).NE.O)IOBJ=1
IF((IFUNC.AND."4).NE.O)ISURF=1
INITIALIZE THE MEGATEK
CALL MGINIT
CALL SETINT(13)
INITALIZE THE A/D
CALL ANINIT
CALL AINSQ(16,29,IA)
CALL AOUTSQ(4,17,IA)
READ MANIPULATOR CONTROL PARAMETERS
WRITE (5,2)
FORMAT(' FEEDFORWARD ACTIVE AND PASSIVE CYCLE
1
(.7,30,150)')
READ(5,*)F1,IT1,IT2
ICTL=1
CALL DOUT(24,"77)
C INITIALIZE THE KEYBOARD MONITER ROUTINE
CALL GETADR(IPARAM(1),ICMD)
IPARAM(2)=1
C SET FUNCTION FLAGS
ISHAD=-1
!SHADOW
IWALL=-1
!WALLS
ITCHPT=-1
!TOUCHPOINTS
ICOOR=-1
!COORDINATES
IPROX=-l
!PROXIMITY INDICATOR
C SET POSITION OF ARM PARTS
ZHC=-4.4
!CENTER OF HAND
-90-
TIMES
ZHT=-5.6
!END OF HAND
ZFT=-40.3
!END OF FOREARM
ZST=-18.4
!END OF SHOULDER
SF-40.
!DISPLAY SCALE
NHL=34
I# LINES IN HAND
NSL=16
!# LINES IN SHOULDER
NFL=20
l# LINES IN FORARM
C SET POSITION OF OBJECTS
XO=O
YO--40
ZO=-24
OXT=-10
OYT=-35
OZT=O
IFLG=O
IFLGO-O
IFLGS=0O
C SET INITIAL VELOCITY TO ZERO
VX=0.
VY=O.
VZ-O.
OVX=O
OVY=o
OVZ=O
TIM=SECNDS (0.)
C SET VIEWPOINT AND ZOOM
TX=O.
TY=O.
S=1
WAO=0
WA2=0
DWA1=0
DWA2=0
DTX=0O
DTY=O
DS=O
C READ SCALING FACTORS FOR A/D OUTPUT OF ANGLES
OPEN(UNIT-4,NAME='DL1: [200,125]SCALE.DAT',TYPE='OLD')
READ(4,*)((SCL(I,J),J=1 ,2)I=1,7)
CLOSE(UNIT=4,DISPOSE='SAVE')
C READ ARM POINT DATA
OPEN(UNIT=4,NAME='DL1: [200,125]ARMDAT.DAT',TYPE='OLD')
READ(4,*)((ADAT(I,J),J=1,3),I=1,22)
CLOSE(UNIT=4,DISPOSE='SAVE')
C READ ARM CONNECTIVITY DATA
OPEN(UNIT-4,NAME='DL1:[200,125]ARMCON.DAT',TYPE='OLD')
READ(4,*)((ACON(I,J),J=1,2),I=1,NHL)
CLOSE(UNIT-4,DISPOSE='SAVE')
C READ OBJECT TOUCHING CONDITIONS
-91-
(,£1Vac
(,sXII
/,SqIVM
(,NIrl
051
OaI
XO~IHd
O1
XO"0
-Z6-
9Z*
9L
T
t* S= I
98
9L
IS='X
9L "ZT9*S=P
9LZI*IS=I
•L'•,•8=I
(T)1 gacgns riivo
I ailS NI SYIvM
'
61
z
oO
,..Z,
.acu
..
a., 'S1NIOaHDOOO
..
M.. 'MOIVHS IdOi
lSVrl RSV'==a
MVdGI=
SH'HfI&DI aqflS
,)IVklOl
,S., Ha7,
•I•O
*..
.,,
,).VNlIoa
(•I'
aAON=N
(IQVOI
D
civor D
PI
I
CT
•) •I~M
) LVNwIOd
zI
(ZT '1)Ia1
(,:Rd7& SNYNI
(,NMOG
NOOZ=
MVHI O
I)IVNUO
IJ
(I'& )a1Irmi
difi
OOZ=6
)IVNI•A
01T
(o0' q)HI'
IHDIII=]
(,:Haxl NOOZ O .)VIKiOj
(6 " 9) 211•IIg
( ,aHN~aD=o0
laq=
NMO0=i'Vc,
loLnbA
(,:aaxl
6
I
8
afl=&&=.. I)IVNX~O
L70.. (8 ')aI IlIm
aIVISNVTI
01
L
,)IVNIOd
(L ' g) HIuM
(,•(aIS=L
dO1S=9
IHDIII=9
•1l•q=
NMOU=Z
(,:aa•I
INOHI=T
I
aifl=8 ,)IVN•1a
(962)•I'am
9
.IVIOx Oi ,)Ivx0oa
(' )21•Im
/0OT
SNOI•DflUISNI qOH'INOD XV1dSIQ INIcIH
(,(T>>o) XIIDIiSV'L
'SSaNaIJS 'X1IAVdD 'NOIIVWI~1DD•a ,)VNWIOd
3
I
t
WPS)Hlrum
OT
t01 OIOD((I'aN'HaSI)'GNV'(I'N*N''OI))aI
'XIIAVED 'NOIIVH-dIalDD9G CiiISci aVHI 0
XIlDII.SV'I
((ooC00'ooz)
ova~ins ao Za'Ia S1aaVYIVXa aINZS
)IVN'Oa
0'03TTa(cgS)
uit
oIoD(t'aN'a~nSI)ai
COT Ol
uv.aa
(,
du
D
AVS,=SSOaSI
TI ) g• (
(9
£
I ) H DI )
bdaL4VOdVd
UV~d
I' =1INfl)SOID
nNIINOD3
*i'7
(ZOT=aNH
) (
I
0
Z01
TO1
I
I-I-aON
TOT1 O
'TI=I
:Ifla=aNVNgV=IINf)NaO
( ,fOliO,=aB'lVGHDlFqO[SZITOOZ]
I
9fnS OINI 1Larf0
(")aNaans rnva
(0'<I)GIMEI•
(04I'-)cIMaGN
(06Igf-)£cIm•dG
(06f-4I-)cImHGIT
V0o1 D
qqIVD
qqvD
qqvD
qqvD
(0gI-gf-)CIM"Hal qqvo
(0' I-6)C1MUGN qqvD
(04r- gI) c1mG• qqVo
(06 (•S*9L')XIdI'0)IAOWII qqVD
(')asIans qqvD
i flS NI aIIHdS (IVOq D
(,aAVS, =aSOaSIa' Z=INn)aSOqD
(,aAVS 4 =aSOdSIa'I=IINn)aSOqD
(O)aNaans qqvD
(JS"ISN)M1vac qqva
(O)dcrans qqvD
£ flS NI aa'qnOHS GVOq D
(Z)aNaans qqvo
(S'lqN)MV-dG rqnv
()•)cIgns
rqqvo
Z UfS NI WNYvaOIa
flNIINOD
NN'W('I)
ZI'00z]
:IqG=
(avoq
9OT
avai
IHN'IT=I 0T OCI
VIVQ NKlVOIIA0 01
IAHII
HDIflO1HI
(c=azIsa•OOa•
Da•Iag=ss.DDv'
1c•
cI O =aaxgi' .VaVc'GIRV[ ZI'o00Z] :TIQ ai=VN' Z=LINfl)NnadO
(£aqnO=aadX'$g'iVNODNwiV[gZT
OOdS D
I
N'I=IINfl)NadO
(M)INaBgnS qqV
(f614-)
I
cIMuci ririo
('1-'I'x-)CIAOW TIVD
(f-'4-'1-)£IAON
rIrVD
(n-'W- '")CIAO TIVO
rlvo
rlrVD
(n-'w-'X-)£imu'a
(u- 6N- 6,x) citii
rivo
(n-'I'Ix)£Imacl
(n-'I'x)£1Mc-d
'VD
(r'I'x-)£IMNcI TIVo
(fg I 4x-) c1m'da rirvD
(f'w-'x-))£ImHa rvo
(v'I')0CIAO
TWOVD
auio =aaijx
((IaO=dXa'
NOI.ISOd WIyV qVIIINI
(9)aNians l1vo
ZflNINOD
(04040) EIAON
IqVD
0W
i-I zil
(vori
0
D
ZTI
O(G
(SGVHSq)INIOaN
qIVD
•fnNI.NOD
(0'0'0)'CIAOXrqqjVD
0T11
O''T=I ITT OG
(SaNI'q)lNIOaN qqVD
OTT
SHNI' ONIlilfaNI HOA oDVaS LNOWUN RAVS D
(UI).INI.I..S qqvo
afnNIINO0
601
GN•SSW qqVD
HflNIINOD
801
(W LVIl) E1M'H
qqVO
(za'I a'is/z'as/x)alns2as=LI
7=Z
O0Z'OOOZ4OOOZ-= 801o OG
(O00Z-'4I'
I)CIAON 17M
(Za'T a'as/Z 'as/X)ansI*as=I
OOOZ-=Z
I=X
OOZ'OOOZ'OOOZ-=I 601 OG
•flNI.NOD
UNHSDN qIVO
flNIINOD
qriVD
()I40c£)IM•ga
(Za'I
LO1
901
'4S/Z 'AS/X)
IxnsLas=LI
f=X
OOZ'OOOZ'OOOZ-=F 901 OG
(l',Ilg O0z-) EIAON4 11Vo
(za'Ta'as/z'as/x).•insxas=).I
"OOOZ-=x
I=Z
ooZ'OOOZ'oooz-=I LOT Oa
(OI)INIlaS q1VD
O1OD(T'aN'"JdnSI)•I
OTT1
aDVAdflS
(9) aqcnns qrvo
9 Uns OINI XvqaasIa
NVNVW1Ia
(iAVS,=aSOJSIGgZ=1INn)HSOrID
(,CaAVS,=SOaSIG'-T=INfn)aSOqD
(9)(INSlS qq1VD
QVO'1 D
8
(E=azIscaoDaI' £liauiQ& =ssaxOv'
LAVG'•QVaqO[•ZTgOOZ] :TI'Q 4=aWVN'Z=lINn)NaaO
£sVc'NODfrO[SZT'OOZ] :T'1QI,=aVN'T=IINfl)NaaO
DO 113 I=1,22
PNT(I,1)=ADAT(I,2)
PNT(I,2)=ADAT(I,3)
113
CONTINUE
CALL NPOINT(LOC)
C READ ARM POSITION FROM A/D CONVERTER
114
CALL AINSQ(23,29,IS)
DO 115 I=1,7
13(I)=IS(I)
A(I)=FLOAT(IS(I))/3276.2
115
CONTINUE
C INCREMENT VIEWPOINT
WA1=WA1+DWAI
WA2=WA2+DWA2
AND
CONVERT TO VOLTAGE
S=S+DS
TX=TX+DTX
TY=TY+DTY
C
SCALE A/D OUTPUT AND CALCULATE SINES & COSINES
AH=SCL (1,1
)*(A(5)+SCL(1, 2))
Sl=SIN(AH)
C1=COS(AH)
AH=SCL(2 , 1 )*(A(7)+SCL(2,2))
S2=SIN(AH)
C2=COS(AH)
AH=SCL(3,
)*(A(6)+SCL(3,2))-SCL(1, 1)*(A(5)+SCL(1,2))
S3=SIN(AH)
C3=COS(AH)
AH=SCL(4, 1 )*(A(2)+SCL(4,2))
S4=SIN(AH)
C4=COS(AH)
AH=SCL(5, 1 )*((A(3)-A(4) )/2.+SCL(5,2))
S5=SIN(AH)
C5=COS(AH)
AH=SCL(6, 1 )*((A(3)+A(4) )/2 .+SCL(6,2))
S6=SIN(AH)
C6=COS(AH)
D=SCL(7,1),•(A(1 )+SCL(7,2))
WS1=SIN(WA]
WC1=COS(WA]
WS2=SIN(WA'
WC2=COS(WAg
HCR=ZHC-.8*D
HTR=ZHT-.8*D
CALCALATE TRANSFORMATIONS
CALCULATE VIEWPOINT TRANSFORMATIONS
WXX=WC1*S
WXY=0O
WXZ=WSl*S
WXT=TX*S
-95-
C
C
C
C
C
WYX=WS1*WS2*S
WYY=WC2*S
WYZ=-WC1*WS2*S
WYT=TY*S
CALCULATE dX/dAl
YY=S1*S2*S4-S1*C2*C3*C4-C1*S3*C4
YZ=S1*C2*S3-C1*C3
YT=-CL*ZST
ZY=-C1*S2*S4+C1*C*2C3*C4-S1*S3*C4
ZZ=-Cl*C2*S3-S1*C3
ZT=-SI*ZST
DYD1=(-YY*S5+YZ*C5)*HTR+YZ*ZFT+YT
DZD1=(-ZY*S5+ZZ*C5)*HTR+ZZ*ZFT+ZT
CALCULATE dX/dA2
XY=S2*S4-C2*C3*C4
XZ=C2*S3
YY=-C1*C2*S4-C1*S2*C3*C4
YZ=C1*S2*S3
ZY=-S1*C2*S4-S1*S2*C3*C4
ZZ=S1*S2*S3
DXD2=(-XY*S5+XZ*C5)*HTR+XZ*ZFT
DYD2= (-YY*S5+YZ*C5) *HTR+YZ*ZFT
DZD2=(-ZY*S5+ZZ*C5)*HTR+ZZ*ZFT
CALCULATE dX/dA3
XY=S2*S3*C4
XZ=S2*C3
YY=-C1*C2*S3*C4-S1*C3*C4
YZ=-CI*C2*C
3+S1*S3
ZY=-S1*C2*S3*C4+C1*C3*C4
ZZ=-S1*C2*C3-C1*S3
DXD3=(-XY*S5+XZ*C5)*HTR+XZ*ZFT
DYD3=(-YY*S5+YZ*C5) *HTR+YZ*ZFT
DZD3=(-ZY*S5+ZZ*C5)*HTR+ZZ*ZFT
CALCULATE INVERSE
DET=DXD2*DYD3*DZD1+DXD3*DYD1*DZD2
1
-DZD1*DYD2*DXD3-DYD1*DXD2*DZD3
R11=(DYD2*DZD3-DYD3*DZD2)/DET
R12=(DZD2*DXD3-DXD2*DZD3)/DET
R13=(DXD2*DYD3-DYD2*DXD3)/DET
R21=(DYD3*DZD1-DYD1*DZD3)/DET
R22=-DXD3*DZD1/DET
R23=DYD1*DXD3/DET
R31=(DYD1*DZD2-DYD2*DZD1)/DET
R32=(DXD2*DZD1-DZD2*DXD1)/DET
R33=-DXD2*DYD1/DET
SHOULDER TRANSFORMATIONS
XX=C2
XY=-S2
YX=C1*S2
-96-
••o"(
LTI
GNVH
dO
0LfO((9*
-L601HA 01
IHdS
0d 7IDOlHA IRS D
aIaHaS DNIG1OH SI GNVH al D
'"60"("1L"o" (OZ-DZ)Sgv)
I
q'C)
".o(o .)(oX-o,)sqv)*-oo. ( "I- (ox-ox)sqv))JI
911 oIoo( (9 "aDG) "NV
(i• baosi) )i
Giaso tI lqIS SaIUD
[NV
IqDXD ISV
GaadJII)
d D
811 OOO(' aN'HaSI)dI
THHaS ONIG'IOH SI GNVH al HINIIaia•3
ZIZ+ITLH*ZZZ=IZ
Z I+E•*•Z ZX=IX
Z X+'IH* ZZX= IX
Z Z+II DH* Z Z=D Z
Z I++IDH
NVEH
dO SMVf
ZZ=DAO
ZIX+H3H*ZZX=DX
NRM14q XkVMGIN INIOd UNIN4HIiaa
D
141 ic 0-wNI I=141
a(I
•l
(O) SQNDaS=KII
NI L=KI4(Irlc I
aWIL Q2SJVqa
TlZ+LIZ*IZZ=LZI
1*DZZ+SJISXZ-=ZZZ
9D*Su1ZZ+9D0*0*IXZ+9S*XZ-=ZAZ
9S•gS* 1ZZ+9S*90IXZ+9D0TXZ=ZXZ
1$A+$dZ•t1 ZA=Z$X
DIZX+IS• *A-=ZZX
9D•S•1 ZX+9D1ulzxi+9S•1X-=Z l
9*9*STZXA+9S*9D1
X+9D*XX=ZXX
IdZI ZX=ZLX
D*ITZX+gS* ITX-=ZZX
9D0SS
ZX+9D0
1
IXX+9S •TXX-=ZX
9Ss~S1ZX+9S•AD*TX+9D*1XX=ZXX
SNOIIVNIIOdSNVIL
aNId 0
QNVH D
LSZ*ZZI=TZ
ED*ZZ+ES*XZ-=-ZZ
tD3cScZZ+thDX*c
Z+tS*XZ-=-~XZ
tS~cS~ZZ+PS*+4AZ+tzDXZ=TXZ
+SZ~Z•X=IX
ED* ZX+CS*7,X-=T
ZX
tD3cSSzx~tD*CD*)LX+tS*XX-=TXX
t S*CS *U
ZX~+ S*ED* XX t7 D*XX= X
ESILX-=TZX
tD ED
X+t'S*XX-=1AX0
ES*DC4XX+7DXX= TXX
SNOIIVN4ITOdSNVI$ NV•VTOd
ID=ZZ
ZD*IS=XZ
ZS*TS=XZ
TS-=Z0
ZD;ID=XL
D
IFS=1
VX=(XC-XO)/DELTIM
VY=(YC-YO)/DELTIM
VZ=(ZC-ZO)/DELTIM
C SET CENTER OF SPHERE TO POSITION OF HAND
XO=XC
YO=YC
ZO=ZC
C HOLD HAND OPEN AROUND SPHERE
D=.6
C SET FORCE FEEDBACK FLAG
IFORCE=1
GOTO 118
C IF HAND IS NOT HOLDING SPHERE
117
IFORCE=O
IFS=O
C KEEP SPHERE WITHIN BOUNDARIES
IF(ABS(XO).GT.26.)VX=-VX*ELAST
IF(ABS(YO+24.).GT.24.)VY=-VY*ELAST
IF(ABS(ZO+12.).GT.24.)VZ=-VZ
*ELAST
C ACCELERATE/DECCELERATE SPHERE
XO=XO+VX*DELTIM
YO=YO+VY*DELTIM
ZO=ZO+VZ*DELTIM
116
IF(YO.GT.-48)VY=VY-G*DELTIM
VX=VX*(1-DRAG)
VY=VY*(1-DRAG)
VZ=VZ* (1-DRAG)
PROX=SQRT((XO-XC)**2+(YO-YC)**2+(ZO-ZC)**2)
C DETERMINE IF HAND IS HOLDING OBJECT
118
IF(IOBJ.NE.1)GOTO 123
IF((IFO.EQ.1) .AND.(DD.LE.D))GOTO 121
C FIND HAND REFERENCE TO OBJECT REFERENCE TRANSFORMATION
DET=OXX*OYY*OZZ+OXY*OYZ*OZX+OXZ*OYX*OZY
1 -OZX*OYY*OXZ-OZY*OYZ*OXX-OZZ*OYX*OXY
RXX=(OYY*OZZ-OZY*OYZ)/DET
RXY=(OZY*OXZ-OXY*OZZ)/DET
RXZ=(OXY*OYZ-OYY*OXZ)/DET
RXT=-RXX*OXT-RXY*OYT-RXZ*OZT
RYX=(OYZ*OZX-OYX*OZZ)/DET
RYY=(OXX*OZZ-OXZ*OZX)/DET
RYZ=(OYX*OXZ-OYZ*OXX)/DET
RYT=-RYX*OXT-RYY*OYT-RYZ*OZT
RZX=(OYX*OZY-OYY*OZX)/DET
RZY=(OXY*OZX-OZY*OXX)/DET
RZZ=(OXX*OYY-OXY*OYX)/DET
RZT=-RZX*OXT-RZY*OYT-RZZ*OZT
C MOVE HAND TO OBJECT REFERENCE FRAME
X=RXX*XC+RXY*YC+RXZ*ZC+RXT
-98-
Y=RYX*XC+RYY*YC+RYZ*ZC+RYT
Z-=RZX*XC+RZY*YC+RZZ*ZC+RZT
XN=ABS(RXX*XY2+RXY*YY2+RXZ*ZY2)
YN=ABS(RYX*XY2+RYY*YY2+RYZ*ZY2)
ZN=ABS(RZX*XY2+RZY*YY2+RZZ*ZY2)
C DETERMINE IF HAND IF HOLDING OBJECT
DO 119 I=1,NOP
C DETERMINE IF CENTER OF HAND IS WITHIN OBJECT
IF(ABS(X-TCH(I,1)).GT.TCH(I,4))GOTO 119
IF(ABS(Y-TCH(I,2)).GT.TCH(I,5))GOTO 119
IF(ABS(Z-TCH(I, 3)).GT.TCH(I,6))GOTO 119
C DETERMINE IF HAND IS CLOSED ENOUGH TO GRASP OBJECT
IF(XN.NE.O)RS=TCH(I,4)/XN
IF(YN.NE.O)RH=TCH(I,5)/YN
IF(RH.LT.RS)RS=RH
IF(ZN.NE.O)RH=TCH(I,6)/ZN
IF(RH.LT.RS)RS=RH
R=RS*SQRT(XN**2+YN**2+ZN**2)
IF((1-R/1.9).GT.D)GOTO 119
C HOLD HAND OPEN TO WIDTH OF OBJECT
DD=1-R/1. 9
NCENT=I
GOTO 120
119
CONTINUE
GOTO 122
120
IFO=1
C CALCULATE OBJECT TO DATA BASE TRANSFORMATION
DET=XX2*YY2*ZZ2+XY2*YZ2*ZX2+XZ2*YX2*ZY2
1
-ZX2*YY2*XZ2-ZY2*YZ2*XX2-ZZ2*YX2*XY2
RXX=(YY2*ZZ2-ZY2*YZ2)/DET
RXY=(ZY2*XZ2-XY2*ZZ2)/DET
RXZ=(XY2*YZ2-YY2*XZ2)/DET
RXT=-RXX*XT2-RXY*YT2-RXZ*ZT2
RYX=(YZ2*ZX2-YX2*ZZ2)/DET
RYY=(XX2*ZZ2-XZ2*ZX2)/DET
RYZ=(YX2*XZ2-YZ2*XX2)/DET
RYT=-RYX*XT2-RYY*YT2-RYZ*ZT2
RZX=(YX2*ZY2-YY2*ZX2)/DET
RZY=(XY2*ZX2-ZY2*XX2)/DET
RZZ=(XX2*YY2-XY2*YX2)/DET
RZT=-RZX*XT2-RZY*YT2-RZZ*ZT2
C MOVE OBJECT TO DATABASE
HXX=RXX*OXX+RXY*OYX+RXZ*OZX
HXY=RXX*OXY+RXY*OYY+RXZ*OZY
HXZ=RXX*OXZ+RXY*OYZ+RXZ*OZZ
HXT=RXX*OXT+RXY*OYT+RXZ*OZT+RXT
HYX=RYX*OXX+RYY*OYX+RYZ*OZX
HYY=RYX*OXY+RYY*OYY+RYZ*OZY
HYZ=RYX*OXZ+RYY*OYZ+RYZ*OZZ
-99-
C
HYT=RYX*OXT+RYY*OYT+RYZ*OZT+RYT !UNCENTERED
C CENTER OBJECT
HYT=-HYX*TCH(NCENT,1)-HYY*TCH(NCENT,2)
1 -HYZ*TCH(NCENT,3)
HZX=RZX*OXX+RZY*OYX+RZZ*OZX
HZY=RZX*OXY+RZY*OYY+RZZ*OZY
HZZ=RZX*OXZ+RZY*OYZ+RZZ*OZZ
HZT=RZX*OXT+RZY*OYT+RZZ*OZT+RZT
C DETERMINE OBJECT POSITION
121
OXX=XX2*HXX+XY2*HYX+XZ2*HZX
OXY=XX2*HXY+XY2*HYY+XZ2*HZY
OXZ=XX2*HXZ+XY2*HYZ+XZ2*HZZ
OXT=XX2*HXT+XY2*HYT+XZ2*HZT+XT2
OYX=YX2*HXX+YY2*HYX+YZ2*HZX
OYY=YX2*HXY+YY2*HYY+YZ2*HZY
OYZ =YX2*HXZ+YY2*HYZ+YZ2*HZ Z
OYT=YX2*HXT+YY2*HYT+YZ2*HZT+YT2
OZX=ZX2*HXX+ZY2*HYX+ZZ2*HZX
OZY=ZX2*HXY+ZY2*HYY+ZZ2*HZY
OZZ=ZX2*HXZ+ZY2*HYZ+ZZ2*HZZ
OZT=ZX2*HXT+ZY2*HYT+ZZ2*HZT+ZT2
C SET VELOCITY OF OBJECT TO VELOCTY OF HAND
OVX=(XC-OXO)/DELTIM
OVY=(YC-OYO)/DELTIM
OVZ=(ZC-OZO)/DELTIM
C SAVE HAND POSITION FOR VELOCITY DETERMINATION
OXO=XC
OYO=YC
OZO=ZC
C SET FORCE FEEDBACK FLAG
IFORCO=1
D=DD
GOTO 123
C IF HAND IS NOT HOLDING OBJECT
122
IFORCO=O
IFO=0O
C KEEP OBJECT WITHIN BOUNDARIES
IF(ABS(OXT).GT.26.)OVX=-OVX*ELAST
IF(ABS(OYT+24.).GT.24.)OVY=-OVY*ELAST
IF(ABS(OZT+12.).GT.24.)OVZ=-OVZ*ELAST
C ACCELERATE/DECCELERATE OBJECT
OXT=OXT+OVX*DELTIM
OYT=OYT+OVY*DELTIM
OZT=OZT+OVZ*DELTIM
IF(OYT.GT.-48)OVY=OVY-G*DELTIM
OVX=OVX* (1-DRAG)
OVY=OVY*(1 -DRAG)
OVZ=OVZ*(1-DRAG)
C DETERMINE IF HAND IS TOUCHING SURFACE
-100-
3D0OA
-TOT-
QI6'"+(Z 'VT+I)IVQV=(TI 'T+I)LNa
a•8 *-( C6171) IV(IV=( Z'17 1+) IN
QI6 1+(Z '1+1)
V(IV=
(T
'1+I) N
I*69 "-c( 'oT+I)) vv=(I
z'o
0T+I)lN
fl*R6*
6
-(( C01+
1) IVCV=(
Z0I+)
*6 *T
I-(TZ 1),I)VV=(TI
'I)
INJ
N
9'Z=I 6ZV OQ
8ZT
NOIIISOJ (INVH
V1flD3'1VD 3
SNOIIVNIIOSNV"l TUflS0OID NVH 3
(SI'Ll'TT)bsanov '1vo
OrLL3I
UnNINOD3
LZT
((I)£I-(I)sIivorIaia+z"9L zz(I)v=(i sI)
L' =I LZT O(I
(T')rIDS/
(CV+V)+( 9)V=(9)V
('z)'los/Izv+(L)v=(L)v
(T6Trrl~s/Tv+(s)v=(s)v
CC'H*ZJ+ZC'd*2ý+TC'H*Xa=CV
£1IZZ+ZTI•IXA+T
IT11X.=TV
8qT oioo(0oba'Ntvi)ai
9ZT
•V o01 •o3o• lxaav 3
(LLTI,,'Z)nlO
rI3IVD
(XINDI-'6)Ilnov TIV
(iILNDI'Tt)lnov rqqV
((8)VI)IVO'I•, (qV-T
I
+((Z'L)r0S-(
L)rIOS/(I)
Z"9L ZiV=BISNOI
9Zt1 0ooD(0obSON0OI)aI
SfNOI 01 a3D106
laaV 3
X3DV8IAa
(LL., 'tZ)Inoc
HDU0d NO Nlfll 3
T='IDI (Co
0b "wauvi ) ai
IqVo(O
"S'bsDNOII)aI
XDVqcaaa aD3HO
aAO Nlfll 3
S D311 .tI=NIlVI
O3IO.O
a
x•I"0"•d0Zl=SDNOI
I3a fPO 11O THHaHS DNIH3DO1 dl )3DVaHSA
a~31Oa
O=S3DOOI
=ZT OIOD
SZT
INHS 3
1ZI
T=S3DI0Ia
O=ZA
D~d3Q=XA
O=Xa
aIss
avAISISSI
'IflDV
'ROVaH11S
DNIHDflOI
Sl
NVH dI
3
/ Z1 01s3 (D/XGID)N
lx
((OI/IZ)SOD*(S/IX)NIS)SffV=Dq
( Z a 4 1' I zX41X)Anx=
SZT OIOD(I"aN•N
nlS I) I
PNT(I+14, 2)=ADAT(I+14, 3)-. 8*D
129
CONTINUE
C ERASE PREVIOUS DISPLAY
CALL LDPTRO(LOC)
C LOAD DISPLAY
C LOAD HAND INTO SUB 7
CALL SUBDEF(7)
DO 131 I-1,NHL
MS=MM
M=ACON(I,1)
MM=ACON(I,2)
IF(MS.EQ.M)GOTO 130
IX=SF*ADAT(M,1)
IY=SF*PNT(M,1)
IZ=SF*PNT(M,2)
CALL MOVI3(IX,IY,IZ)
IX=SF*ADAT(MM,1)
130
IY=SF*PNT(MM,1)
IZ=SF*PNT(MM,2)
CALL DRWI3(IX,IY,IZ)
131
CONTINUE
CALL SUBEND(7)
C LOAD SHOULDER
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1 XX,XY,O. ,O.YX,YY,YZ,O.,ZX,ZY,ZZ,O.,SF)
CALL SUBCAL(3)
C LOAD FOREARM
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1 XX1,XY1,XZ1,XT1,YX1,YY1,YZ1,YT1,ZX1,ZY1,ZZl,ZT, SF)
CALL SUBCAL(2)
C LOAD HAND
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1 XX2,XY2,XZ2,XT2,YX2,YY2,YZ2,YT2,ZX2,ZY2,ZZ2,ZT2,SF)
CALL SUBCAL(7)
C LOAD OBJECT
IF(IOBJ.NE.1)GOTO 132
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1 OXX,OXY,OXZ,OXT,OYX,OYY,OYZ,OYT,OZX,OZY,OZZ,•ZT,SF)
CALL SUBCAL(5)
C LOAD SHADOWS
IF(ISHAD.NE.1)GOTO 133
132
C LOAD SHOULDER SHADOW
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1 XXXY,O. ,O.,O.,O.,O.,-48.75,ZX,ZY,ZZ,0.,SF)
CALL SUBCAL(3)
C LOAD FOREARM SHADOW
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1 XX1,XY1,XZ1,XT1,0. ,O.,0O.,-48.75,ZX1,ZY1,ZZ1,ZT1,SF)
CALL SUBCAL(2)
-102-
C LOAD HAND SHADOW
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1
XX2,XY2,XZ2,XT2,0. ,0.,0.
,-48.75,ZX2,ZY2,ZZ2,ZT2,SF)
CALL SUBCAL(7)
C LOAD OBJECT SHADOW
IF(IOBJ.NE.1)GOTO 133
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1
OXX,OXY,OXZ,OXT,0.,O.,O.,-48.75,OZX,OZY,OZZ,OZT,SF)
CALL SUBCAL(5)
133
CALL LDTRN3(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT)
C DISPLAY CENTER OF HAND
IF(ITCHPT.NE.1)GOTO 134
IX=SF*XC
IY=SF*YC
IZ=SF*ZC
CALL MOVI3(IX,IY,IZ)
CALL DRWI3(IX,IY,IZ)
C DISPLAY SHADOW
IF(ISHAD.NE.1)GOTO 134
IY=-1950
CALL MOVI3(IX,IY,IZ)
CALL DRWI3(IX,IY,IZ)
C DRAW WALLS
134
IF(IWALL.NE.1)GOTO 135
CALL SUBCAL(1)
C LOAD PERMANANT DISPLAY
135
CALL SUBCAL(6)
C DISPLAY CROSS-SECTION OF SURFACE UNDER HAND
IF(ISURF.NE.1)GOTO 137
CALL SETINT(15)
IY=SF*YSURF(-2000./SF, ZT,P ,P2)
CALL MOVI3(-2000,IY,IFIX(SF*ZT))
DO 136 I=-2000,2000,80
X=I
IY=SF*YSURF(X/SF,ZT,P1,P2)
IF(DEP.GT.O)IY=FLOAT(IY)-DEP*SF*
1
EXP(-.004*(1.-BC)*(X/SF-XT)**2.)
CALL DRWI3(I,IY,IFIX(SF*ZT))
136
CONTINUE
C DISPLAY SPHERE
137
IF(ISPH.NE.1)GOTO 138
IX=SF/S*(WXX*XO+WXZ*ZO)
IY=SF/S*(WYX*XO+WYY*YO+WYZ*ZO)
IZ=SF/S*(WZX*XO+WZY*YO+WZZ*ZO)
CALL LDTRN3(S,O.,O.,WXT,O.,S,O.,WYT)
CALL MOVI3(IX,IY,IZ)
CALL SUBCAL(4)
C DISPLAY SPHERE SHADOW
IF(ISHAD.NE.1)GOTO 138
-103-
b
Io'+zvMa=zvma(oLo..b('a
(vi7' "+i
1o0.1
'4
wdI) aI
,fiaNI sioaaD
a aI
) Lvwoa
0a
666
o., "'7 TI .. 4ECo.- (666 6 9) HJIB M
A1~LNB u•q'Ia
T1 oloD(Z"aN'nnflI)aI
(nnI'4)acaval q'qv
(SalI1vEVal'gSOl"c'
'£'ooToT,)OIb
cu iIvgx
lrddl
rl1
VD(l'IN'Jal)aI
CUMO Uax
QNSSON4 IrVD
o
HIfDIJ
l
iuaaH
fli IvIaSIo01
(0'000T'01)cIMlia
a
via
I T•)I
01T
INaS a
laVo
'vIVa
(0'000oT'o0oT-)IAO
(o'(xoiad'o0)xIaI'o)cImlcata IIVa
*oI/(" z-xotad)+-z=xoxtaO(z"o •xoat)aI
(0'00o'0)EIAON q1VO
O7T oloo(IaN'NXO'aI)aI
6ET
lOLVaIGNI lIINIXOXa xvuIasIa a
(0o'Z'II'anqI)iVHO VIVO
(0'00T'ooOZ-)AIAOW nqVD
(9'6a' a=-I,)1VHHOa
79
KlIr&I ZG( anEiI '179 ' T 1) MOODNS
(O'Z' T'.n•nl)6VHD 'liVO
(0'O09T1'O00oZ-)EIAOW q'vVD
(9"6a'•a=x•)I VKoaE
iONa
D9(angl'I) 9 g 1) •
(O''gllgaflqI)dVHO
9
TV
(0'OOLI'OOOZ-)CIAON q1VO
(t'8
a'
=ZI )IVNWIOa
IZ(dansi 'Z9 'iI) laONN
vivO
(O'Z'IT'anfI)q1VHO
(O'g0081'000Z-)CIAOW q'Vi
19
GODONaH
IX(aanslI '19 'II)
(o'Z' II 'algI)VHD q'VD
(0'0061T'OOOZ-)IAON 'IVv
( '7
'a'g-aX, ) IVw4uoa
IX(andI '09 ' I I) HOOaN
6CI 010('N'0100aI)al
I "0 '"'"0''"0'"'T)N1GI
'iVD
( 00' "'
09
8 EY
a
SHIVNIGI9OOD XVIJSI(
(+)t
DsfnS qqVD
17,M 6XXM' ZXIM'XXkM
(ZI'XI'XI)AIAON qVDv
XM'xXM'ZXM'XXM)CN•GLLI 'IIVD
O961-=ZI
OZxaS=xI
OXoaS=XI
-S01VýI
OOD(I"aN"IVHSI)iI
(o'o'0o)CIAOX Ivio
(saNII)xaiaow lavo
Z-SaNIq=SaNIq
ýýl 0-10D(90T..-aN*GNDIdd
Z+SGVSHSI=SIVH Sq
(ZI'0961-'XI)CIAON 1IVO
(SaIVRSq)XAIG(OI4 qqvD
(suvns'r>i~Iaow
'YIV
ir oioo(i'Oba•'vHsi)ai
Z+SaNIT=SaNIq
(ZI'XI'XI)CIAON qrFVO
(saNI')7.)I(oII
qqvD
lZ 1 J.S=ZI
IX.*S=XI
.TX,•S=XI
EVI 01 f(ODu.,HN ca*DI ) I
Z+SaVHSr= S(IVHSI
(zI'o06T-'x
I)CIMN( 'VIVD
(saVHSq)1i{Iaow
rInv
ZTi oio(
Z171
"oba'IVRSI)lI
Z+SNIq=-SNIq
(ZI'sI'XI)cIMIw 'IvD
(SaNIq)xdII(Ol laVo
IZ*JS=ZI
.&~SI•SII
.izXas=zI
IX•aS=XI
Zil OOO(o01..
001 00(ZZI. "baN'aIl)al
X0HaI-=XOIaaI(0zl.,
,00ODI-=I00DI (O.
l$HDII-=IJHDII (ZI..
vi"-="IvI"i (L z I.
OVHSI-=IVHSI (Z I.. "ba -ciwi)i
aois (( ZL90,,
I..
"ba- ciwi)i
10-SI=S(I
To*-sl=sa(cqo. 'ba'cRwni) ai
TO +Sa=Sa(TLO.. 'ba ciwai)ai
I 'T
O=x•cI
0=SQ
O=ZVMQ
T'T OIOD(990., baN'waDI)aI
"ba'c•ai)ai
-ot-xlGaxlGaccl..
"o$ix
$GXQX(LL
0..
ba•'•Noi)ai
o07+XOi=Xi(7Lc I.. .ba'cai)ai
oo0+xia=ila(g L0.
-ba-aRoi)ai
*+TVMLI= TVMQ (990..
-IVMCI=TVMG0(990., -ba-a-NW)ai
-zvMcI= zvMc ( 90..
.bs.aNDI)6I
LSHADS=LSHADS-2
CALL MODIFY(LSHADS)
CALL RMOVI3(0,O,0)
144
IF(ICMD.NE."067)GOTO 145
WA1=1.57
WA2=0.
145
IF(ICMD.NE."061)GOTO 146
WA =0
WA2=-0
146
IF(ICMD.NE."060)GOTO 114
TX=O
TY=O
GOTO 114
END
FUNCTION YSURF(X,Z,P1,P2)
YSURF=(SIN(.1256*Z)+COS(.02472*Z))*Pl*X/(P2+X**2)-35.
RETURN
END
SUBROUTINE DRAW(IPNTS,SCL)
DO 200 I=1,IPNTS
MS=MM
C READ CONNECTIVITY DATA
READ(1,*,END=300)M,MM
C IF
STARTING POINT WAS LAST LOCATION SKIP MOVE
IF(M.EQ.MS)GOTO 100
C READ COORDINATES AND DRAW
READ(2 'M)X,Y, Z
IX=SCL*X
IY=SCL*Y
IZ=SCL*Z
CALL MOVI3(IX,IY,IZ)
100
READ(2'MM)X,Y,Z
IX=SCL*X
IY=SCL*Y
IZ=SCL*Z
CALL DRWI3(IX,IY,IZ)
200
CONTINUE
300
RETURN
END
SUBROUTINE LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
1
XX,XY,XZ, XT,YX,YY,YZ,YT,ZX, ZY, ZZ,ZT, SF)
CALL LDTRN3(WXX*XX+WXY*YX+WXZ*ZX,WXX*XY+WXY*YY+WXZ*ZY,
1
WXX*XZ+WXY*YZ+WXZ*ZZ,SF*(WXX*XT+WXY*YT+WXZ*ZT)+WXT,
1
WYX*XX+WYY*YX+WYZ*ZX,WYX*XY+WYY*YY+WYZ*ZY,WYX*XZ
1
+WYY*YZ+WYZ*ZZ,SF*(WYX*XT+WYY*YT+WYZ*ZT)+WYT)
RETURN
END
-106-
APPENDIX VI
SUBMARINE
SIMULATION SOFTWARE
C INITIALIZE PROGRAM
DIMENSION IA(16),A(7),IPARAM(6),I OSB(2),IBUF(12)
BYTE ACON(34,2)
COMMON /DMABUF/IDUM(3610),ADAT(22 ,3),PNT(22,2),
1
TCH(3,6),SCL(7,2) ,II(7) ,I
(7),12( 7),I3(7),IS(7),F1,
1
IT1,IT2,ICTL,VEC(18),ALNGTH,AMP,I NDX
DATA OXX,OXY,OXZ/I.,O.,O./
DATA OYX, OYY,OYZ/O.,t1.,O./
DATA OZX,OZY,OZZ/O.,0,1./
ICTL=O
100
WRITE(5,50)
50
FORMAT(' ENTER FUNCTION'/' 1=SPHE RE'/' 2=OBJECT'/
1
' 4=SURFACE'/' 8=PATH'/' 16=NPATH ')
READ(5,*)IFUNC
ISPH=O
IOBJ=O
ISURF=0O
IPATH=O
INPATH=0O
IF((IFUNC.AND."l).NE.0)ISPH=1
IF((IFUNC.AND."2).NE.0)IOBJ=1
IF((IFUNC.AND."4).NE.O)ISURF=1
IF((IFUNC.AND."10).NE.O)IPATH=1
IF((IFUNC.AND."20).NE.O)INPATH=1
C INITIALIZE THE MEGATEK
CALL MGINIT
CALL SETINT(13)
C INITALIZE THE A/D
CALL ANINIT
CALL AINSQ(16,29,IA)
CALL AOUTSQ(4,17,IA)
WRITE (5,51)
51
FORMAT(' FEEDFORWARD ACTIVE AND PASSIVE CYCLE TIMES
1
(.7,30,150)')
READ(5, *)F1, IT1,I, T2
ICTL=1
CALL DOUT(24,"77)
C INITIALIZE THE KEYBOARD MONITER ROUTINE
CALL GETADR(IPARAM(1),ICMD)
IPARAM(2)=1
C SET FUNCTION FLAGS
ISHAD=-1
!SHADOW
IWALL=-1
!WALLS
ITCHPT=-1
!TOUCHPOUNTS
IPROX=-1
!PROXIMITY INDICA TOR
-107-
C SET POSITION OF ARM PARTS
ZHC=-5.
!CENTER OF HAND
ZHT=-5.6
!END OF HAND
ZFT=-40.3
!END OF FOREARM
ZST=-l 8.4
!END OF SHOULDER
YSUBT=-7.5
!CENTER OF SUBMARINE ROTATION
ZSUBT=-15.
!CENTER OF SUBMARINE ROTATION
SF=40.
!DISPLAY SCALE
NSUBL=40
!# LINES IN SUBMARINE
NHL=34
!# LINES IN HAND
NSL=16
!# LINES IN SHOULDER
NFL=20
!# LINES IN FORARM
C SET POSITION OF OBJECTS
XO=0
YO=-40
ZO=-24
OXT=-10
OYT=-40
OZT=-20
IFLG=O
IFLGO=O
IFLGS=O
C SET INITIAL VELOCITY TO ZERO
VX=O.
VY=O.
VZ=0.
OVX=O
OVY=O
OVZ=o
TIM=SECNDS(0.)
C SET VIEWPOINT AND ZOOM
WRITE(5,45)
45
FORMAT(' ENTER DISPLAY SCALE FACTOR
READ(5,*)S
(<1)')
SF=SF*S
TX=O.
TY=O.
S=1
WAl=O
WA2=0
DWA1=0
DWA2=O
DTX=0O
DTY=O
DS=O
C READ SCALING FACTORS FOR A/D OUTPUT OF ANGLES
OPEN(UNIT=4,NAME='DL1:[200,125]SCALE.DAT',TYPE='OLD
READ(4,*)((SCL(I,J),J=1,2),I=1,7)
CLOSE(UNIT=4,DISPOSE='SAVE')
-108-
)
(i.Vadal
( SINIOaHIDflOT
O
-601.,, 'TIxH Ol ,z,,,
o0. ,,I",,
'rMO(aVHS aIO.
.,S,,
iaai
saxi 1)TVIWNOf
(ZI6)TI-aM
,)XLOwlIOd
(OT') alI'M
( :Sadl SaNI'I MVIIG 01 )IVHWOd
(6 ' ) TI'HM
(iNMOG NOOZ=E adn HOOZ=6 s)IVOUod
(801
Z 2)JIv
(•:•XT~]00Z OT, ()TVH•Od
I.d11=®
=
a0TS=S
L
(LaIN•)
=oIM I
( i •TNaD:0
TROIa=] NMOU= 1iV'4
aftl=,=,, I)IVxWaO
a'onb i L O.,. (9 '9) aiTIIM
(,:aall HTVaSNV'd. O1 )IVN'dOa
1a'=v
THDIiI=9
(aa
(.aGIS=L INOUd=T
NMOIG=Z anfl=
)IVNOJ
0 7 0 Hiram•t
X1 a:•.O• O
)IVwHoa
,
c
(*
•)
aiTu'
802
IOXTNOD xVwISIG ININd D
SNOITD.TISNI
(,(S>>o) X1'o•a)Iva
'gIIAVID 'NOI.IV•aqaDDSG s)1Vw•aO
'SSHNA.IIS
I
zo
(Zgs)a;iTa•
0o O TOO(('aN'HdaSI)'NV'(v'aN'r•OI))AI
XIIDITSVqHS?
6IAVTD3
'NOITV~~'a aDD(G G aISva
( (00oo
'0 OZ) [DVnsfl
LOZ
aIi
V
:
ao0 Zd'ad SIIHTawVaVad
TNa *)TIVNXHO
(CT
') TIIum
L0Z 010D(1'"N"dnSI)i.I
90
-oDVa1flS JO SI-Iia$•aVTV QiVa1
(CHAVS4 =aSOdSI"'•=IINfl)aSOD
90
170
HfNIINOD
(9'T=r'C(f'I)nDI)C(oZ=aN'•'')avaa
I-I=dON
9 9 =I V0Z OU
TvI'DTr~0I[O[gZT'00Z] :Tq'iU=aVN'17=.INfl)NadO
(Cnao,=a daI'
5
SNOIIIGNOD ONIHDflO aVax D
( ,AVS==aSOaSIa4$=IINfln)aSOqD
aVHx D
(qHN'II' (Z'l=f ' ( fg I )N O OV ) ) ( `v
gt ) aTV
ac1'iTVa'NODNaV[gZT'OOZ] :TI'Jai-=aVN'=IINn)NadO
VTVa 7.TIAITDNNOD (IV
TH
(C AVS =aSOaSIa'V=IINni)aSOqD
(Zz'I=I ' (cI=r'(f'I)TVaV))(*'•)avza•
Tl'Va[TIIV[WVRZT00Z] :TtIi=•aVN$7=IINn)NadO
VITVG INIOJ
C ciaa01= aXI'
((IaOs-=ax1'aiva'NODfrO[
(
-Oil-AVS,=
1
SOSIl'aILINfl)aSO'ID
(M)aNans I'IVD
(as OOI)MV•,H
""Vo
Mazagns
IqVD~
lI:
(CmazIsc-xoD )j'
iaa=ssHD vI'
I
ZI'00Z] :(I'iQ=ZNVN'I=JINfl)NdaO
Sg~nS oiNI iDoarO avoi D
(V)aNzqns lavo
(0O'I)£-)EIMcIi¶
(o
(r~ •
lVD
'i-)aacins ?I'Ivo
cim•imu rirv
aivDD
((0gr-g 1-)
')1si•
cm1AO]
V
gnS
NI ZlIHJS
SZZ" •,•S=£
aVOI
D
gzs "as=l
(C AVSS=HSOaSIa'Z-IINn)aSOID
(,ZAVS,-ZSOd~asg'I-INn)ZSO'ID0
aivo
()aN•ns
(as"ISN)tvlIM
TIVD
(M)azagns qIqv
c gfS NI H'IOnOHS cvoi D
(z)aNaans lYvo
(as'IAN)',dva
[voy
(z)aaaacns TIvo
Z afS NI
NxVzxIOa UVOq
fnNINOD
D
6
WKW(~I)Gvall
THN'I-I EE6 Oa
ViVIa WXVHXOa 01 .Il• HOfIOHHlqOOdS D
(=•ZIsauxooD
.oI0a&DIaIQ-SSIDDV'
I
(aqo=-aa=x) i'NO]av
(laOr
Pdax< , V(IaV(IotT4va[xlvzi'00]
5ZT ' OO Z::E'lq.=•jVN'z-=INfl)N~aO
T?(Ia atTN
Z
l= IfNn )Nac O
(&ZAVS,=•SO•aSIaZ=1IN)•ZSOIO
( ,-AVS, =zsoasia' I=iINfl)aSOriD
(M)aNOns qqvo
(US'00I)MVBa
qqVo
()aaafns
qqvo
(cC-zISscloo' ,1DllIG4,-SSaD0V' I
(Ir'lOi=•aJxi
v(I'v•aqgnS[ZIo00Z] :Ilqa,=
Z=iINn)Nzao
gvN'
(Culqo,=alax$'.vcvNODans[lz'oo0z] :lqI.=aNVN' I=IINf)NaaO
I alS NI HNIXVWQflS
V(IO' D
SaalDIdfnS aVOl D
CLOSE(UNIT=2,DISPOSE='SAVE')
C LOAD
105
106
PERMANANT DISPLAY INTO SUB 6
CALL SUBDEF(6)
IF(ISURF.NE.1)GOTO 759
CALL SETINT(10)
DO 106 I=-2000,2000,200
Z=I
X=-2000.
IY=SF*YSURF(X/SF,Z/SF,P1,P2)
CALL MOVI3(-2000,IY,I)
DO 105 J=-2000,2000,200
X=J
IY=SF*YSURF(X/SF,Z/SF,P1,P2)
CALL DRWI3(J,IY,I)
CONTINUE
CALL MGSEND
CONTINUE
DO 108 I=-2000,2000,200
X=I
Z=-2000.
IY=SF*YSURF(X/SF,Z/SF,P1,P2)
CALL MOVI3(I,IY,-2000)
DO 107 J=-2000,2000,200
Z=J
IY=SF*YSURF(X/SF,Z/SF,P1, P2)
CALL DRWI3(I,IY,J)
107
CONTINUE
CALL MGSEND
108
CONTINUE
CALL SETINT(13)
C LOAD PATH FOLLOWING PATHS
759
IF(INPATH.NE.1)GOTO 561
CALL MOVI3(-2000,-000,-1000,0)
DO 560 I=-2000,2000,10
PX=FLOAT(I)/SF/12.
CALL NPATH(PX,Z,DL,DR,HEIGT,X1,X3,PY,INDX)
CALL DRWI3(I,-IFIX(12.*PY*SF),O)
560
CONTINUE
561
IF(IPATH.NE.1)GOTO 109
CALL MOVI3(-2000,-1000,0)
DO 562 I=-2000,2000,10
PX=FLOAT(I)/SF/12.
CALL PATH(AMP,ALNGTH,PX,PEL)
CALL DRWI3(I,-IFIX(12.*PEL*SF)-1300,O)
562
CONTINUE
109
CALL NPOINT(LINES)
C SAVE MEMORY SPACE FOR INPUTTING LINES
DO 110 1=1,40
CALL MOVI3(0,0,0)
-111-
110
CONTINUE
CALL NPOINT(LSHADS)
DO 111 I=1,40
CALL MOVI3(0,0,0)
111
CONTINUE
CALL SUBEND(6)
C LOAD INITIAL ARM POSITION
DO 834 I=1,22
PNT(I,1)=ADAT(I,2)
PNT(I,2)-ADAT(I,3)
834
CONTINUE
CALL NPOINT(LOC)
C READ ARM POSITION FROM A/D CONVERTER AND CONVERT TO VOLTAGE
112
CALL AINSQ(23,29,IS)
DO 113 I=1,7
I3(I)=IS(I)
A(I)=FLOAT(IS(I))/3276.2
113
CONTINUE
C INCREMENT VIEWPOINT
WA1=WA1+DWAl
WA2 =WA2+DWA2
S=S+DS
TX=TX+DTX
TY=TY+DTY
C SCALE A/D OUTPUT AND CALCULATE SINES & COSINES
914
AH=SCL(1,1)*(A(5)+SCL(1, 2))
Sl=SIN(AR)
Ci=COS(AH)
AH=SCL(2,1)*(A(7)+SCL(2,2))
S2=SIN(AH)
C2=COS(AH)
AH=SCL(3,1)*(A(6)+SCL(3,2))-SCL(1,1)*(A(5)+SCL(, 2))
S3=SIN(AH)
C3=COS(AH)
AH=SCL(4,1)*(A(2 )+SCL(4,2))
S4=SIN(AH)
C4=COS(AH)
AH=SCL(5,1)*((A(3)-A(4))/2.+SCL(5,2))
SS=SIN(AH)
C5=COS(AH)
AH=SCL(6,1)*((A(3)+A(4))/2.+SCL(6,2))
$6=SIN(AH)
C6=COS(AH)
D=SCL(7, 1)*(A(1)+SCL(7,2))
SV1=SIN(VEC(7))
CV1-COS(VEC(7))
SV2=SIN(VEC(9))
CV2=COS(VEC(9))
SV3=SIN(VEC(8))
-112-
CV3=COS(VEC(8))
WS1-SIN(WA1)
WC1=COS(WAl)
C
C
C
C
C
WS2=SIN(WA2)
WC2-COS(WA2)
HCR=ZHC-. 8*D
HTR=ZHT-. 8*D
CALCALATE TRANSFORMATIONS
CALCULATE VIEWPOINT TRANSFORMATIONS
WXX=WC1 *S
WXY=0
WXZ-WS1*S
WXT=TX*S/SF
WYX=WS1*WS2*S
WYY=WC2*S
WYZ=-WC1*WS2*S
WYT=TY*S/SF
CALCULATE SUBMARINE TRANSFORMATIONS
SXX=CV1*CV2+SV1*SV2*SV3
SXY=-SV1*CV2+CV1*SV2*SV3
SXZ=SV2*CV3
SXT=VEC(11)*12.
SYX=SV1*CV3
SYY=CV1*CV3
SYZ=-SV3
SYT=VEC(12)*12. +YSUBT
SZX=-CV1*SV2+SV1*CV2*SV3
SZY=SV1*SV2+CV1*CV2*SV3
SZZ=CV2*CV3
SZT=VEC(10) *12.+ZSUBT
CALCULATE dX/dAl
YY=Sl*S2*S4-S1*C2*C3*C4-C1*S3*C4
YZ=S1*C2*S3-C1*C3
YT=-Cl*ZST
ZY=-C1*S2*S4+C1*C2*C3*C4-S1*S3*C4
ZZ=-C1*C2*S3-S1*C3
ZT=-Sl*ZST
Y ( -YY*S5+YZ*C5) *HTR+YZ*ZFT+YT
Z=(-ZY*S5+ZZ*C5)*HTR+ZZ*ZFT+ZT
DXD1=SXY*Y+SXZ*Z
DYD1=SYY*Y+SYZ*Z
DZD1=SZY*Y+SZZ*Z
CALCULATE dX/dA2
XY=S2*S4-C2*C3*C4
XZ=C2*S3
YY=-C1*C2*S4-C1*S2*C3*C4
YZ=C1*S2*S3
ZY--S1*C2*S4-S1*S2*C3*C4
ZZ=S1*S2*S3
-113-
X=(-XY*S5+XZ*C5)*HTR+XZ*ZFT
Y=(-YY*S5+YZ*C5)*HTR+YZ*ZFT
Z=(-ZY*SS+ZZ*C5)*HTR+ZZ*ZFT
DXD2=SXX*X+SXY*Y+SXZ*Z
DYD2=SYX*X+SYY*Y+SYZ*Z
DZD2=SZX*X+SZY*Y+SZZ*Z
C CALCULATE dX/dA3
XY=S2*S3*C4
XZ=S2*C3
YY=-C1*C2*S3*C4-S1*C3*C4
YZ=-C1*C2*C3+S1*S3
ZY=-S1*C2*S3*C4+C1*C33*C4
ZZ=-S1*C2*C3-CI*S3
X=(-XY*S5+XZ*C5)*HTR+XZ*ZFT
Y=(-YY*S5+YZ*C5)*HTR+YZ*ZFT
Z=(-ZY*S5+ZZ*C5)*HTR+ZZ*ZFT
DXD3=SXX*X+SXY*Y+SXZ*Z
DYD3=SYX*X+SYY*Y+SYZ*Z
DZD3=SZX*X+SZY*Y+SZZ*Z
C CALCULATE INVERSE
1
DET=DXD1*DYD2*DZD3+DXD2*DYD3*DZD1+DXD3*DYD1*DZD2
-DZD1*DYD2*DXD3-DYD1*DXD2*DZD3-DXD *DYD3*DZD2
R11=(DYD2*DZD3-DYD3*DZD2)/DET
R12=(DZD2*DXD3-DXD2*DZD3)/DET
R13=(DXD2*DYD3-DYD2*DXD3)/DET
R21=(DYD3*DZD1-DYD1*DZD3)/DET
R22=(DXD1*DZD3-DXD3*DZD1)/DET
R23=(DYD1*DXD3-DXD1*DYD3)/DET
R31=(DYD1*DZD2-DYD2*DZDI)/DET
R32=(DXD2*DZD1-DZD2*DXD1)/DET
R33=(DXD1*DYD2-DXD2*DYD1)/DET
C SHOULDER TRANSFORMATIONS
XX=C2
XY=-S2
YX=CI*S2
YY=C1*C2
YZ=-Sl
ZX=S1*S2
ZY=S1*C2
ZZ=C1
C FOREARM TRANSFORMATIONS
XX =XX*C4+XY*C3*S4
XY.1=-XX*S4+XY*C3*C4
XZl=-XY*S3
YX1=YX*C4+YY*C3*S4+YZ*S3*S4
YY1=-YX*S4+YY*C3*C4+YZ*S3*C4
YZl=-YY*S3+YZ*C3
YT1=YZ*ZST
ZX1=ZX*C4+ZY*C3*S4+ZZ*S3*S4
-114-
ZY1=-ZX*S4+ZY*C3*C4+ZZ*S3*C4
ZZ1=-ZY*S3+ZZ*C3
ZT1=ZZ*ZST
C HAND TRANSFORMATIONS
XX2=XX1*C6+XY1*C5*S6+XZl*s5*S6
XY2=-XX1*S6+XY1*C5*C6+XZ1*S5*C6
XZ2=-XY1*S5+XZ *C5
XT2=XZI*ZFT
YX2=YX1*C6+YY1*C5*S6+YZ 1*S5*S6
YY2=-YX1*S6+YY1*C5*C6+YZl*S5*C6
YZ2=-YY1*S5+YZ 1*C5
YT2=YZ1*ZFT+YT1
ZX2=ZX1*C6+ZY1*C5*S6+ZZl*S5*S6
ZY2=-ZX1*S6+ZY1*C5*C6+ZZ1*S5*C6
ZZ2=-ZY1*S5+ZZ1*C5
C
C
C
C
C
ZT2=ZZ1*ZFT+ZT1
COMBINED WORLD-SUB TRANSFORMATIONS
WSXX=WXX*SXX+WXY*SYX+WXZ*SZX
WSXY=WXX*SXY+WXY*SYY+WXZ*SZY
WSXZ=WXX*SXZ+WXY*SYZ+WXZ*SZZ
WSXT=WXX*SXT+WXY*SYT+WXZ*SZT+WXT
WSYX=WYX*SXX+WYY*SYX+WYZ*SZX
WSYY=WYX*SXY+WYY*SYY+WYZ*SZY
WSYZ=WYX*SXZ+WYY*SYZ+WYZ*SZZ
WSYT=WYX*SXT+WYY*SYT+WYZ*SZT+WYT
WORLD-SUB TRANSFORMATION OF SHADOW
SWXX=WXX*SXX+WXZ*SZX
SWXY=WXX*SXY+WXZ*SZY
SWXZ=WXX*SXZ+WXZ*SZZ
SWXT=WXX*SXT-WXY*48.75+WXZ*SZT+WXT
SWYX=WYX*SXX+WYZ*SZX
SWYY=WYX*SXY+WYZ*SZY
SWYZ=WYX*SXZ+WYZ*SZZ
SWYT=WYX*SXT-WYY*48.75+WYZ*SZT+WYT
DETERMINE POINT MIDWAY BETWEEN JAWS OF HAND
FIND ELAPSED TIME
OLDTIM=TIM
TIM=SECNDS(0.)
DELTIM=TIM-OLDTIM
DETERMINE POINT MIDWAY BETWEEN JAWS OF HAND
X=XZ 2 *HCR+XT2
Y=YZ2*HCR+YT2
Z=ZZ2*HCR+ZT2
XC=SXX*X+SXY*Y+SXZ*Z+SXT
YC=SYX*X+SYY*Y+SYZ*Z+SYT
ZC=SZX*X+SZY*Y+SZZ*Z+SZT
X=XZ2*HTR+XT2
Y=YZ2*HTR+YT2
Z=ZZ2*HTR+ZT2
-115-
XT=SXX*X+SXY*Y+SXZ*Z+SXT
YT=SYX*X+SYY*Y+SYZ* Z+SYT
ZT=SZX*X+SZY*Y+SZZ*Z+SZT
C DETERMINE IF HAND IS HOLDING SPHERE
IF(ISPH.NE.1)GOTO 121
C SKIP IF OBJECT GRIPPED LAST CYCLE AND JAWS STILL CLOSED
IF((IFS.EQ.1) .AND. (D.GE..6))GOTO 333
IF((ABS(XC-XO).GT.1.).OR.(ABS(YC-YO).GT.1.).OR.
1 (ABS(ZC-ZO).GT.1.).OR.(D.LT..6))GOTO 117
C IF HAND IS HOLDING SPHERE
C SET VELOCITY OF SPHERE TO VELOCTY OF HAND
IFS-I
VX=(XC-XO)/DELTIM
333
VY-(YC-YO)/DELTIM
VZ=(ZC-ZO)/DELTIM
C SET CENTER OF SPHERE TO POSITION OF HAND
XO-XC
YO-YC
ZO-ZC
C HOLD HAND OPEN AROUND SPHERE
D=.6
C SET FORCE FEEDBACK FLAG
IFORCE=1
GOTO 121
C IF HAND IS NOT HOLDING SPHERE
117
IFORCE=O
IFS=O
C KEEP SPHERE WITHIN BOUNDARIES
IF(ABS(XO).GT.26.)VX=-VX*ELAST
IF(ABS(YO+24.) .GT.24.)VY=-VY*ELAST
IF(ABS(ZO+12.).GT.24.)VZ=-VZ*ELAST
C ACCELERATE/DECCELERATE SPHERE
XO =XO+VX*DELT IM
YO=YO+VY*DELTIM
ZO=ZO+VZ*DELTIM
IF(YO. GT.-4 8)VY=VY-G*DELTIM
VX=VX* (1-DRAG)
VY=VY*(1-DRAG)
VZ-VZ*(1-DRAG)
PROX=SQRT((XO-XC)**2+(YO-YC)**2+(ZO-ZC)**2)
C DETERMINE IF HAND IS HOLDING OBJECT
IF(IOBJ.NE.1)GOTO 321
121
IF((IFO.EQ.1).AND.(DD.LE.D))GOTO 315
C FIND HAND REFERENCE TO OBJECT REFERENCE TRANSFORMATION
DET=OXX*OYY*OZZ+OXY*OYZ*OZX+OXZ*OYX*OZY
1 -OZX*OYY*OXZ-OZY*OYZ*OXX-OZZ*OYX*OXY
RXX = (OYY*OZZ-OZY*OYZ)/DET
RXY=(OZY*OXZ-OXY*OZZ) /DET
RXZ=(OXY*OYZ-OYY*OXZ)/DET
-116-
RXT=-RXX*OXT-RXY*OYT-RXZ*OZT
RYX=(OYZ*OZX-OYX*OZZ)/DET
RYY=(OXX*OZZ-OXZ*OZX)/DET
RYZ =(OYX*OXZ-OYZ*OXX) /DET
RYT=-RYX*OXT-RYY*OYT-RYZ*OZT
RZX=(OYX*OZY-OYY*OZX)/DET
RZY=(OXY*OZX-OZY*OXX)/DET
RZZ=(OXX*OYY-OXY*OYX)/DET
RZT=-RZX*OXT-RZY*OYT-RZZ*OZT
C MOVE HAND TO OBJECT REFERENCE FRAME
X=RXX*XC+RXY*YC+RXZ* ZC+RXT
Y=RYX*XC+RYY*YC+RYZ*ZC+RYT
Z=RZX*XC+RZY*YC+RZZ*ZC+RZT
XN=ABS(RXX*XY2+RXY*YY2+RXZ*ZY2)
YN=ABS(RYX*XY2+RYY*YY2+RYZ*ZY2)
ZN=ABS(RZX*XY2+RZY*YY2+RZZ*ZY2)
C DETERMINE IF HAND IF HOLDING OBJECT
DO 323 I=1,NOP
C DETERMINE IF CENTER OF HAND IS WITHIN OBJECT
IF(ABS(X-TCH(I,1)).GT.TCH(I,4))GOTO 323
IF(ABS(Y-TCH(I,2)).GT.TCH(I,5))GOTO 323
IF(ABS(Z-TCH(I, 3)).GT.TCH(I, 6))GOTO 323
C DETERMINE IF HAND IS CLOSED ENOUGH TO GRASP OBJECT
IF(XN.NE.O)RS=TCH(I,4)/XN
IF(YN.NE.0)RH=TCH(I, 5)/YN
IF(RH.LT.RS)RS=RH
IF(ZN.NE.O)RH=TCH(I, 6)/ZN
IF(RH.LT.RS)RS=RH
R=RS*SQRT(XN**2+YN**2+ZN**2)
IF((1-R/1.9).GT.D)GOTO 323
C HOLD HAND OPEN TO WIDTH OF OBJECT
DD=1-R/1. 9
NCENT=I
GOTO 320
323
CONTINUE
GOTO 317
320
IFO=1
C CALCULATE OBJECT TO DATA BASE TRANSFORMATION
HXX=SXX*XX2+SXY*YX2+SXZ*ZX2
HXY=SXX*XY2+SXY*YY2+SXZ*ZY2
HXZ=SXX*XZ2+SXY*YZ2+SXZ*ZZ2
HXT=SXX*XT2+SXY*YT2+SXZ*ZT2+SXT
HYX=SYX*XX2+SYY*YX2+SYZ*ZX2
HYY=SYX*XY2+SYY*YY2+SYZ*ZY2
HYZ=SYX*XZ2+SYY*YZ2+SYZ*ZZ2
HYT=SYX*XT2+SYY*YT2+SYZ*ZT2+SYT
HZX=SZX*XX2+SZY*YX2+SZZ*ZX2
HZY=SZX*XY2+SZY*YY2+SZZ*ZY2
HZZ=SZX*XZ2+SZY*YZ2+SZZ*ZZ2
-117-
HZT=SZX*XT2+SZY*YT2+SZZ*ZT2+SZT
DET=HXX*HYY*HZZ+HXY*HYZ*HZX+HXZ*HYX*HZY
1
-HZX*HYY*HXZ-HZY*HYZ*HXX-HZZ*HYX*HXY
RXX=(HYY*HZZ-HZY*HYZ)/DET
RXY=(HZY*HXZ-HXY*HZZ)/DET
RXZ=(HXY*HYZ-HYY*HXZ)/DET
RXT=-RXX*HXT-RXY*HYT-RXZ*HZT
RYX=(HYZ*HZX-HYX*HZZ)/DET
RYY=(HXX*HZZ-HXZ*HZX)/DET
RYZ=(HYX*HXZ-HYZ*HXX)/DET
RYT=-RYX*HXT-RYY*HYT-RYZ*HZT
RZX=(HYX*HZY-HYY*HZX)/DET
RZY=(HXY*HZX-HZY*HXX)/DET
RZZ=(HXX*HYY-HXY*HYX)/DET
RZT=-RZX*HXT-RZY*HYT-RZZ*HZT
C MOVE OBJECT TO DATABASE
HXX=RXX*OXX+RXY*OYX+RXZ*OZX
HXY=RXX*OXY+RXY*OYY+RXZ*OZY
HXZ=RXX*OXZ+RXY*OYZ+RXZ*OZZ
HXT=RXX*OXT+RXY*OYT+RXZ*OZT+RXT
HYX=RYX*OXX+RYY*OYX+RYZ*OZX
HYY=RYX*OXY+RYY*OYY+RYZ*OZY
HYZ=RYX*OXZ+RYY*OYZ+RYZ*OZZ
C
HYT=RYX*OXT+RYY*OYT+RYZ*OZT+RYT !UNCENTERED
C CENTER OBJECT WITHIN GRIPS
HYT=-HYX*TCH(NCENT,1)-HYY*TCH(NCENT,2)
1
-HYZ*TCH(NCENT,3)
HZX=RZX*OXX+RZY*OYX+RZZ*OZX
HZY=RZX*OXY+RZY*OYY+RZZ*OZY
HZZ=RZX*OXZ+RZY*OYZ+RZZ*OZZ
HZT=RZX*OXT+RZY*OYT+RZZ*OZT+RZT
C DETERMINE OBJECT POSITION
315
RXX=SXX*XX2+SXY*YX2+SXZ*ZX2
RXY=SXX*XY2+SXY*YY2+SXZ*ZY2
RXZ=SXX*XZ2+SXY*YZ2+SXZ*ZZ2
RXT=SXX*XT2+SXY*YT2+SXZ*ZT2+SXT
RYX=SYX*XX2+SYY*YX2+SYZ*ZX2
RYY=SYX*XY2+SYY*YY2+SYZ*ZY2
RYZ=SYX*XZ2+SYY*YZ2+SYZ*ZZ2
RYT=SYX*XT2+SYY*YT2+SYZ*ZT2+SYT
RZX=SZX*XX2+SZY*YX2+SZZ*ZX2
RZY=SZX*XY2+SZY*YY2+SZZ*ZY2
RZZ=SZX*XZ2+SZY*YZ2+SZZ*ZZ2
RZT=SZX*XT2+SZY*YT2+SZZ*ZT2+SZT
OXX=RXX*HXX+RXY*HYX+RXZ*HZX
OXY=RXX*HXY+RXY*HYY+RXZ*HZY
OXZ=RXX*HXZ+RXY*HYZ+RXZ*HZZ
OXT=RXX*HXT+RXY*HYT+RXZ*HZT+RXT
OYX=RYX*HXX+RYY*HYX+RYZ*HZX
-118-
OYY=RYX*HXY+RYY *HYY+RYZ *HZY
OYZ=RYX*HXZ+RYY*HYZ+RYZ*HZZ
OYT=RYX*HXT+RYY *HYT+RYZ*HZT+RYT
OZX=RZX*HXX+RZY*HYX+RZZ*HZX
OZY=RZX*HXY+RZY*HYY+RZZ*HZY
OZZ=RZX*HXZ+RZY*HYZ+RZZ*HZZ
OZT=RZX*HXT+RZY*HYT+RZZ*HZT+RZT
C SET VELOCITY OF OBJECT TO VELOCTY OF HAND
OVX= (XC-OXO)/DELTIM
OVY=(YC-OYO)/DELTIM
OVZ=(ZC-OZO)/DELTIM
C SAVE HAND POSITION FOR VELOCITY DETERMINATION
OXO=XC
OYO=YC
OZO=ZC
C SET FORCE FEEDBACK FLAG
IFORCO=1
D=DD
GOTO 321
C IF HAND IS NOT HOLDING OBJECT
317
IFORCO=O
IFO=0O
C KEEP OBJECT WITHIN BOUNDARIES
IF(ABS(OXT).GT.26.)OVX==-VX*ELAST
IF(ABS(OYT+24.).GT.24.)OVY=-OVY*ELAST
IF(ABS(OZT+12.).GT.24.)OVZ=-OVZ*ELAST
C ACCELERATE/DECCELERATE OBJECT
OXT=OXT+OVX*DELTIM
OYT=OYT+OVY*DELTIM
OZT=OZT+OVZ*DELTIM
IF (OYT.GT. -4 8)OVY=OVY-G*DELTIM
OVX=OVX*(1-DRAG)
OVY=OVY*(1-DRAG)
OVZ=OVZ*(1-DRAG)
C DETERMINE IF HAND IS TOUCHING SURFACE
321
IF(ISURF.NE.1)GOTO 123
Y=YSURF(XT,ZT,P1,P2)
BC=ABS(SIN(XT/5)*COS(ZT/10))
DEP=Y-YT
IF(YT.GT.Y)GOTO 413
C IF HAND IS TOUCHING SURFACE, CALCULATE RESISTIVE FORCE
FX=O
FY=DEP*BC
FZ=O
IFORCS=1
GOTO 123
413
IFORCS=O
C SEND FORCE FEEDBACK
123
ITONGS=IFORCE.OR.IFORCO
-119-
-OZi1-
CT'N)INaddS=7xI
(T'N)VTIaVJaS=XI
861 OTOf(N'ba'SN)AI
(Z ' I) NODV=
(1 'I)NODV=N
ww=sw
HH S~
IHN'I= I 66t OG
(L).aans
•flS
aV
L rnS OINI QNVH cvoI a
xvrlSias
uvo'i a
(ao'I)oiuacrn 'via
XVIaSIG SflOIAU'd HSV1Id D
fnNIINOD
17Z1
aI8
C-(
6'1?+I) VV=ZVI+I)T+I) Na
a*66T+(Z'VI)+I)VLV= (1V'I+I)INa
CI*8 "-( C'l7+I) &aV=(Z 'l+I) Ncl
*6 "+(Z
+I)VlVaV=
(4+I)
•Na
C*8R"-(g'0I+I)
Na
6lV=(Zg0I+I)
*6"'T-(Z'OT+I)•VV=(T'01o+I)INa
a8 "-(C'gl)lVaV=(Z6I) SNa
G*6"1-(Z'I)IVGV=(I'I)LNa
s'Z=I VZT Oa
69C
NOIIISOJ QNVH alVqnDqVD a
SNOIVNHUOaSNVXL
TnSO70 cNVH a
(S I'LI' TI) bslnov rirVD
O=rlaoaI
HflNIINOD
99L
((I)£I-(I)
SI)*+Z"9L
Z
(I)V=(I) SI
!'g=I 99L OG
(IT'c)s/t(v+Tv)+(9)v=f(9)v
(1'Z)qDSIZV+(L)V=(L)V
(
'g)qoslvv+(s)v=(g)v
CZII*ZilZZ'H*Xa+I Z'H* a= B
£ I'UZa+Z EXA+I Ixxa=IV
E£dZZad+ZTa'X+ITTHXa=TV
69£ Oi•O(o''ba'wavi)I
LOL
va o1 aoaoa xaIdav a
(LLI..''Z)$IflOQI 'IVD
(TnILNDI-'6)nlOV qVDa
(aINDI'I1)lnov
qVDv
((8)VI)iVOqa(RV- I)+
I
((Z'Z)rIDS-('T'L) DS/al)*Z'9!ZEoBV-•INDI
LOL OiOD(O"ba'SDNOII)•I
SONOI 01 adaOd XlIaaV a
'XDYaHv
a HaDIO NO Nlfll a
T=oai (oo '~wavi)ai
(LL..'"Z).noa 'rivo(o "b"sDNoOI)ai
V9a
•crta a••Oa adO Ndnl a
Sa1O i[I=xvBI
0
g888 o1OD(l'aN'roI)aI
MOQVHS
(~6'Z ZZ'ZZ'gXZ'xz'gX'
iZ•xzxJ'
x' /x '
'XMszxmsxSXXS'ZXmS'M'zxmS'XxSx'X
S'XXSms'XXS
aDrtfO aVO'
(L)zMOSM lavo
x'gz' xzxxgxx
)N11
VD
3
I
MOaVHS QNVH aVOq 0
(z)MavsN
rqVO
(as 'IiTZ'IZZ'IXZ'Z'Z'IJXJ'IZ2X'Ix'IX'ITX'IZX'IxX'IXX
'IxMS'ZlMS'M4MS'XXMS''XmS'lSzXMS'XXMSxxs)w•a'I
qqvD
(as' .o0'ZZ'z'XZ' "0' Z'
I
MOaVHS WXV•Oxa avol D
(E)'ivgns qqvo
X'Xg' "0' "0'X'XX I
'TiXsiMS'zMS'XMS'XXMsXMSZXMSXxxmXS'XMSXS')Nu
fqVD
MOaVHS 1aaqnoHs avo1 0
(•)qvoDns qHvo
(Ixl SaS
'ZXxMSsXgMS'iXMS'•XMSaS'ZXMSgxxmsgXXMS)
I
£N-iaq qqvD
MOaVHS aNlIVKNnlS aVOq
888 00("(IaN"aSVHSI)aI
998
mOavHs GvOq D
(C)'Ivons qqvD
(aS'iZO'ZZO'xZO'XZO'
xO'ZiO'xXO'XxO'IXO'ZXO'&XO'XXO
'1XM'ZxM'M'Xg'x~'lXLM'ZXM'iXMgXX?)NaIn
I
qIrlVD
9g8 ooo(i •N"•rOI)I
iDarco avo- o
0
(L)IVODns IrIVD
(aS' ZZ'ZZZCZxZ'ZXZ' ZXZ'
Xl'ZZX' gzxxg•XgZXgxx'
XX
ZXSM'XXSM'XXSM'gXSM'ZXSM XXSMb XXSM)Nlllq 'qIV3
g 'SM'
I
aNVH aVOq 0
( )Ivouns
(aS'iz'izz'uIZ'XZ
XZ'I1X'IZ'IUx'IXX'lx'
rrivD
IZX'lXX'IXX
0
I
'xxsm)Nl'I I'IVD
wxva~doa avol 0
( )ivoDns 'rivD
' "0'0o'xx'xx
I
'glXSM'ZS4SM'XKSM'XSM'TXSm'ZXSMXSm'X
(as' "o'zz'xz'xz' oz
0'gx'xc
'TxSM'ZXSg'XASMXXSM'XXSzXSM'XXSM'XXSM)Ni
'I
rVD0
URGROHS
aVOI 3
(I)qvoans qqVD
( AXSM*.AS
gZXSMgX •SM'XXSMg'XSMiaS'ZXSM'XXSM'XXSM)ENala(Iq
aIvD
I
HNIIIVN•HnS
VOq D
(L)aNa~ns qqVD
anNIINOD0
661
(zI9'1'XI)EIMda qqvo
( I ' H)tNd*AS=XI
(I'Ww)ivav as=xi
(ZI'gI'XlI) IAOX qVD
(Z' ) .NaS= ZI
8 67
CALL LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
OXX,OXY,OXZ,OXT,O. ,O. ,0.,-48.75,0ZX,OZY,OZZ,OZT,SF)
CALL SUBCAL(5)
888
CALL LDTRN3(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT)
C DISPLAY CENTER OF HAND
IF(ITCHPT.NE.1)GOTO 130
IX-SF*XC
IY=SF*YC
IZ=SF*ZC
CALL MOVI3(IX,IY,IZ)
CALL DRWI3(IX,IY,IZ)
C DISPLAY SHADOW
IF(ISHAD.NE.1)GOTO 130
IX=SF*XC
IY=-1950
IZ=SF*ZC
CALL MOVI3(IX,IY,IZ)
CALL DRWI3(IX,IY,IZ)
C LOAD PERMANANT DISPLAY
130
CALL SUBCAL(6)
C DISPLAY CROSS-SECTION OF SURFACE UNDER HAND
IF(ISURF.NE.1)GOTO 843
CALL SETINT(15)
IY=SF*YSURF(2000./SF,ZT,P1,P2)
CALL MOVI3(IFIX(X),IY,IFIX(SF*ZT))
DO 808 I=-2000,2000,80
X=I
IY=SF*YSURF(X/SF,ZT,P1,P2)
IF(DEP.GT.O)IY=FLOAT(IY)-SF*DEP*
1 EXP(-.O04*(1.-BC)*(X/SF-XT)**2.)
CALL DRWI3(I,IY,IFIX(SF*ZT))
808
CONTINUE
C DISPLAY PROXIMITY INDICATOR
843
IF(IPROX.NE.1)GOTO 699
CALL LDTRN3(SF,0.0.,.,O.,O.,SF,O.,O.)
CALL MOVI3(0,1000,0)
1
IF(PROX.GT.24)PROX=24.+(PROX-24.)/10.
CALL RDRWI3(0,IFIX(40.*PROX),O)
CALL MOVI3(-100,1000,0)
CALL DRWI3(100,1000,0)
C DISPLAY SPHERE
699
IF(ISPH.NE.1)GOTO 340
IX=SF/S*(WXX*XO+WXZ*ZO)
IY=SF/S*(WYX*XO+WYY*YO+WYZ*ZO)
IZ=SF/S*(WZX*XO+WZY*YO+WZZ*ZO)
CALL LDTRN3(S,O.,0.,WXT,0.,S,O.,WYT)
CALL MOVI3(IX,IY,IZ)
CALL SUBCAL(4)
C DISPLAY SPHERE SHADOW
-122-
009 O1OD(T"ba"GVHSI)al
Z+SaNI q=SaNI"
(zI'•x'xI) ClM•G 1rvo
(saNI'i)xaiiow qrivD
IZ*..S=ZI
ILJIS=LI
IX*JS=XI
009 OIO07(OT.,''aNGDI) dI
001 0o0o(zzT..'
XOIJI I-=X
awoi)aI
'a
[HD•I(LZl,.'b3'a•DI)•I
rlIVMI-=q
GVHSI-=(I[VHSI(Zi.. o'b'awoI)ai
joisazci.,oba~awoi)ji
crsc(c90.. ba'cwoi).i
10"-S a=sa(T9o,.ba'awo)i)
a*I a
10 +S i=sci(iL 0.. "bN' ili
IaL
TTL
0=•1l(I
0=11XL
O=S(I
0=ITVML
IlL
0O1O(9o0.." aN'DI)aI
*o1-xlc
*Ot7-Ia =xQa( c i,..
b a*aoi)ai
*O+XIG =xia(LiLo.,
*baauKri~j
*Q17+L IG
"+IVM(I [iva(990O.. "ba'aoi)ai
ba
- ZVMG= [v=Q(79o,.. "baNDI).aI
[v=4(Z90o.
'ba'c•o)ai
ZtrM(I (990.
O, "b'RD
r~
) al
"+ZVMUI= zvmci(oLo.. "bac'Di)ai
IflaNI Q0DaI
3
0=a&•I
(Vs,'+)•+•)
Oa
O
TOT..'9
00 0 1,,)TT..' 9CE .. (666 49) &I•IdM
LUINH
666
dValD
D
0o1o(zoaNenni)ai
(nni'g)a
vaa qqvD
zi1
SQ 'KV Va 69S 01
C 9 40 0 70 .. oib aavD(o"
•N" •I)a
UGXOK
lav
UN•S0N
"l1VD
Nada.ng
lvqasia 01 .•aniold
(17)Ivqans qlvo
(ZI'XI'XI)CIAOW
($AM'X
hIVD
M'ZXM'XXM'$XM'XXMZXM'XXM)LN•$lU
TIVD
0O61-=ZI
OZ*AS=XI
OXrdS=XI
o7C OOO(v'aN'c[VHSI)aI
GV1
017 D
GN2S
3
600
601
602
603
604
CALL MODIFY(LSHADS)
CALL DRWI3(IX,-1950,IZ)
LSHADS LSHADS+2
IF(ICMD.NE."115)GOTO 601
IX=SF*XT
IY=SF*YT
IZ=SF*ZT
CALL MODIFY(LINES)
CALL MOVI3(IX,IY,IZ)
LINES=LINES+2
IF(ISHAD.EQ.1)GOTO 601
CALL MODIFY(LSHADS)
CALL MOVI3(IX,-1950,IZ)
LSHADS=LS HADS+2
IF(ICMD.NE."105)GOTO 602
LINES=LINES-2
CALL MODIFY(LINES)
CALL RMOVI3(0,0,0)
IF(ISHAD.NE.1)GOTO 602
LSHADS=LSHADS-2
CALL MODIFY(LSHADS)
CALL RMOVI3(0,0,0)
IF(ICMD.NE."067)GOTO 603
WA1-1.57
WA2=O.
IF(ICMD.NE."061)GOTO 604
WA1=0
WA2=0
IF(ICMD.NE."060)GOTO 112
TX-O
TY=0O
GOTO 112
END
FUNCTION YSURF(X,Z,P1,P2)
YSURF=(SIN(.1256*Z)+COS(.02472*Z))*Pl*X/(P2+X**2)-35.
RETURN
END
SUBROUTINE DRAW(IPNTS,SCL)
DO 200 I-1,IPNTS
MS=MM
C READ CONECTIVITY DATA
READ(1,*,END=300)M,MM
C IF STARTING POINT WAS LAST LOCATION SKIP MOVE
IF(M.EQ.MS)GOTO 100
C READ COORDINATES AND DRAW LINE
READ(2 'M)X,Y, Z
IX=SCL*X
IY=SCL*Y
IZ=SCL*Z
-124-
100
200
300
1
1
1
1
CALL MOVI3(IX,IY,IZ)
READ(2'MM)X,Y,Z
IX=SCL*X
IY=SCL*Y
IZ=SCL*Z
CALL DRWI3(IX, IY,IZ)
CONTINUE
RETURN
END
SUBROUTINE LTRN(WXX,WXY,WXZ,WXT,WYX,WYY,WYZ,WYT,
XX,XY,XZ, XT,YX,YY,YZ,YT, ZX, ZY,ZZ,ZT, SF)
CALL LDTRN3(WXX*XX+WXY*YX+WXZ*ZX,WXX*XY+WXY*YY+WXZ*Zy,
WXX*XZ+WXY*YZ+WXZ*ZZ,SF*(WXX*XT+WXY*YT+WXZ*ZT+WXT),
WYX*XX+WYY*YX+WYZ*ZX,WYX*XY+WYY*YY+WYZ*ZY,WYX*XZ
+WYY*YZ+WYZ*ZZ,SF*(WYX*XT+WYY*YT+WYZ*ZT+WYT))
RETURN
END
-125-
APPENDIX VII
MANIPULATOR CONTROL SOFTWARE
The control
interface is able to
independently de-
signate each degree of freedom of the manipulator to be comThis is done by sending a 16
puter control or master-slave.
bit
control word
to the
The bits correspond
port 24.
through
to the interface
digital output
degrees-of-freedom as
follows:
O/TAR/LZY/XOT/ARL/ZYX
H---SLAVE -- +- MASTER---i
The bits are set to one for
each
degree-of-freedom
controlled through
computer control.
is read
through
The.value of
analog input
and
via the channels shown in
analog output
table 7.
TABLE 7 CONTROL INTERFACE I/O CHANNELS
DEGREE OF
FREEDOM
ANALOG
INPUT
CHANNEL
ANALOG
OUTOUT
CHANNEL
T
16
4
S
A
17
5
L
R
18
6
A
L
19
7
V
Z
20
8
E
Y
21
9
X
22
10
M
A
S
T
A
R
23
24
25
11
12
13
T
L
26
14
E
Z
27
15
R
Y
X
28
29
16
17
The
manipulator angles
used in this simulation are
-126-
linear combinations of
the above degrees-of-freedom. The re-
lationships between the two
are given
by:
A1=Z
A2=X
A3=Y-Z
A4=A
A5=(R-L)/2
A6=(R+L)/2
D=T
The
following program was
used to. control the mani-
pulator:
CNTRL.FTN
C THIS PROGRAM ALLOWS THE MASTER MANIPULATOR TO MOVE
C FREELY UNDER COMPUTER CONTROL WHEN ICTL=1, NO
C ACTION IS TAKEN WHEN ICTL=O. THE CYCLE TIMES (IT1,IT2),
C
DEGREE OF TACH FORWARD (Fl) AND CONTROL WORD (ICTRL)
C
ARE INPUT THROUGH DMABUF.
COMMON /DMABUF/IDUM(3894),II(7),Il(7),I2(7),
3(7),
1
IS(7),F1,IT1,IT2,ICTL
ICTL=O
C CHECK TO DETERMINE IF CONTROL DESIRED
100
IF(ICTL.EQ.O)GOTO 800
C SAVE LAST POSITION FOR TACH FORWARD
DO 200 I=1,6
I2(I)=Il(I)
200
CONTINUE
C READ MANIPULATOR POSITION
CALL AINSQ(24,29,I1)
C ADD IN TACH FORWARD
DO 300 I=1,6
II(I)=FLOAT(Il(I))+Fl*FLOAT(Il(I)-I2(I))
300
CONTINUE
C SEND NEW MANIPULATOR POSITION
CALL AOUTSQ(12,17,II)
C WAIT FOR NEXT CYCLE
700
CALL WAIT(IT1,1,IDS)
GOTO 100
800
CALL WAIT(IT2,1,IDS)
GOTO 100
END
-127-
APPENDIX VIII
PROGRAM DATA BASES
SCALE .DAT
This file contains scaling factors for scaling A/D output
to manipulator angles.
SCALE FACTOR
.082
.082
.196
-. 314
-.314
-.32
.33
OFFSET
.5
-. 0
-8.1575
0.
-2.
5.
-.9865
ARMDAT.DAT
This file contains manipulator point data.
The point number
is given for reference only, it is not part of the file.
POINT # X
1
.5
2
.5
3
.5
4
.5
5
.5
6
.5
7
.5
8
.5
Y
1.3
3.3
2.4
1.9
1.9
-1.9
-1.9
-2.4
Z
-1.3
-2.5
-5.5
-5.5
-3.5
-3.5
-5.5
-5.5
9
.5
-3.3
-2.5
10
11
12
.5
-. 5
-. 5
-1.3
1.3
3.3
-1.3
-1.3
-2.5
13
-.5
2.4
-5.5
14
15
16
17
18
19
20
21
22
23
-. 5
-. 5
-. 5
-. 5
-. 5
-. 5
-. 5
0.
0
1.
1.9
1.9
-1.9
-1.9
-2.4
-3.3
-1.3
0.
0
1
-5.5
-3.5
-3.5
-5.5
-5.5
-2.5
-1.3
0.
-40.3
-1.3
-128-
POINT # X
24
1.
25
1.
26
1.
27
-1.
28
-1.
29
-1.
30
-1.
31
0.
32
0
33
1.
1.
34
1.
35
36
1.
i.
37
-1.
38
-1.
39
-1.
40
-1.
Y
X
1
-39
-39
-1.3
-1.3
-39
-39
-1.3
0.
-18.4
0
-1
-1
1
1
-1
-1
O.
0
1
1
-1
-1
1
1
-1
-17.1
-17.1
0
0
-17.1
-17.1
0
-1
-i
-i
ARMCON.DAT
This file contains manipulator connectivity data. Lines are
draw between starting point (SP) and endpoint (EP) defined
in ARMDAT.DAT
SP
1
4
7
10
13
16
19
2
5
8
21
21
22
24
27
30
25
31
32
32
35
EP
2
5
8
1
14
17
20
12
15
18
1
20
25
25
28
27
29
27
34
39
36
SP
2
5
8
11
14
17
20
3
6
9
21
22
22
25
28
23
26
31
32
33
36
EP
3
6
9
12
15
18
11
13
16
19
11
24
29
26
29
27
30
26
38
34
33
-129-
SP
3
6
9
12
15
18
1
4
7
10
21
22
23
26
29
24
31
31
32
34
37
EP
4
7
10
13
16
19
11
14
17
20
10
28
24
23
30
28
23
30
35
35
38
SP
38
33
36
EP
39
37
40
SP
39
34
EP
40
38
SP
40
35
SUBDAT.DAT
This file contains submarine point data.
POINT # X
1
0
2
0
3
5
4
7.5
5
5
6
0
7
-5
8
-7.5
9
-5
10
0
11
5
12
7.5
13
5
14
0
15
-5
16
-7.5
17
-5
18
0
19
0
20
0
21
0
22
0
23
5
24
5
25
-5
26
-5
27
5
28
5
29
0
30
-5
31
-5
32
0
33
3.5
34
3.5
35
0
36
-3.5
37
-3.5
38
0
Y
12.5
15
12.5
7.5
2.5
0
2.5
7.5
12.5
15
12.5
7.5
2.5
0
2.5
7.5
12.5
7.5
12.5
12.5
2.5
2.5
7.5
7.5
7.5
7.5
12.5
12.5
15
12.5
12.5
20
20
20
20
20
20
7.5
Z
-10
0
0
0
0
0
0
0
0
25
25
25
25
25
25
25
25
40
42
45
45
42
42
45
45
42
5
10
15
10
5
2
5
10
13
10
5
44
-130-
EP
37
39
SUBCON.DAT
This file contains submarine connectivity data.
SP
1I
I
1
3
6
9
12
15
2
5
8
11
14
17
20
22
24
26
27
30
33
36
27
30
EP
2
5
8
11
14
17
18
18
3
6
9
12
15
10
38
18
38
18
28
31
34
37
33
36
SP
1
1
1
4
7
10
13
16
3
6
9
12
15
18
38
18
38
18
28
31
34
37
28
31
EP
3
6
9
12
15
18
18
18
4
7
2
13
16
19
21
23
25
38
29
2
35
32
34
37
SP
1
1
29
5
8
11
14
17
4
7
10
13
16
19
21
23
25
2
29
32
35
2
29
EP
4
7
10
13
16
18
18
18
5
8
11
14
17
20
22
24
26
27
30
33
36
32
35
OBJDAT.DAT
This file contains object point data for a rectangular peg.
POINT #
1
2
3
4
5
6
7
8
9
10
X
-1
1
1
-1
-1
1
1
-1
0
0
Y
.5
.5
-.5
-. 5
.5
.5
-. 5
-.5
0
0
o
o
Z
-1
-1
-1
-1
1
1
1
1
1
3
OBJCON.DAT
This file contains connectivity data for the rectangular peg.
-131-
SP
1
4
7
2
9
EP
2
1
8
6
10
SP
2
5
8
3
EP
3
6
5
7
SP
3
6
1
4
EP
4
7
5
8
OBJTCH.DAT
This file contains touching conditions for the rectangular
peg.
Zc)
These conditions are rectanqular with center (Xc,Yc,
and dimensions
Xc
0
0
Yc
0
0
(A,B,C).
Zc
0
2
A
1
.1
-132-
B
.5
.1
C
1
1
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement