Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
SAND86-2972 UC-13
Unlimited Release
Printed November 1987
Prepared by
Sandia National Laboratories
Albuquerque, New Mexico 87185 and Livermore, California 94550
for the United States Department of Ener9Y
under Contract DEAC0476DP00789
SF2900Q(SS1 )
Distribution
Category UC-13
SAND 86-2972
Unlimited Release
Printed November 1987
Victor J. Johnson
Switching Devices Division
Sandia National Laboratories
Albuquerque, NM 87185
Gregory P. Starr, Professor
Department of Mechanical Engineering
University of New Mexico
Albuquerque, NM 87131
Abstract
This report develops the kinematic and dynamic equations for
finger of the
three-fingered
Stanford/JPL robot hand
documents the physical parameters
needed to implement
equations. The equations can be used in control schemes
position and force control of the Stanford/JPL robot hand.
one
and
the
for
Acknowledgments
This work was supported
by Technology Based Research and
Development funds. A debt of gratitude is owed Dr. J. Kenneth
Salisbury, Massachusetts Institute of Technology, for permission
to reprint from his papers the physical parameters of the
Stanford/JPL robot hand. Debts are also owed to Jon H. Barnette,
Switching Devices Division, Gary N. Beeler, Electromechanical
Components Department, and
Patrick
J. Eicker, Intelligent
Computing Department for their continuing support of the Sandia
dexterous hand project.
The authors are also indebted to Mark A. Powell and Clifford S.
Loucks, 1414 for reviewing and editing this report.
Contents
..............................................
.....
..
..........................
....................................
....................................
.......................
................................
.....................................
........
......................
.............................
.................
.
...............................
................................................
7
11
11
13
17
18
22
23
25
29
29
31
34
40
44
60
8
12
13
21
26
33
Tables
1 Denavit-Hartenberg Parameters
14
Introduction
1.0 Homogeneous Transformations and Frame Assignments
1.1 Coordinate Frame Conventions and Link Parameters
1.2 Finger Coordinate Frames
2.0 Forward Kinematics
3.0 Inverse Kinematics
4.0 Iterative Newton-Euler Dynamics
4.1 Velocity Equations
4.2 Static Forces
4.3
Iterative Newton-Euler Dynamics Equations
4.3.1 Acceleration llPropagationll
4.3.2 Force llPropagationll
4.3.3 Iterative Algorithm Application
APPENDIX A - Physical Parameters of the Stanford/JPL Hand
APPENDIX B - MACSYMA Output
References
Figures
1
2
3
4
...........................
Introduction
robot hand
be
found
and
also
the
developed equations.
in
Salisbury
[1,2],
The latter
but
it
is
The Stanford/JPL
Salisbury Jr., is a
joints (Figure 1).
three-fingered
Each
finger
by
Dr.
J. Kenneth
Thus, an
three fingers.
analysis of
as
one
3.
The
on
fingertip position.
This
the
ability
known
joint
angles,
are
finger
called
finger
location
that
of
kinematics".
The
tip
Kinematics is the
study
of
motion
and
position;
derived
technique because
in this
report using
derivation process.
The Jacobian
to
is
The dynamic
the Newton-Euler
Jacobian
in the
matrix
that relates
joint velocities
inertial
However, the
controller.
and
positional
friction
driver
of nonlinearity decoupling
modeled.
satisfactorily
accomplished.
in
nonlinearities
of the
Stanford/JPL hand is a
the
unless the
mathematically
This
friction
feat
has
Another
loading can be
not
problem
yet been
that
needs
Due to the
low
9
is
currently controlled by
is
system.
Description
report.
The
first
section
homogeneous
of
this
transformations
simplify the
analyses.
report
and
The
introduces
frame
assignments
conventions
that
finger is covered.
equations.
the
velocity,
equations are
acceleration,
derived.
The
were
Appendix A describes
Craig
implemented
to
used are
the
[3],
Section three
to
dynamic
develop the
the
and
references
were
used
the
how
briefly
in
Itmotorttspace.
10
Major
portions of the
is
MACSYMAl*
Appendix
symbolic manipulation
are
the
a reprint of material
output file from the
program
that
defines
the
manipulator
links
relative to
how
each
other.
The
are
orientations in
desirable
space.
The
for
Therefore, standardized
representing positions
conventions used
detailed in Craig [ 3 ] .
and
Link
parameters
notation.
This
are
defined
allows
unique
using
definition
between t w o j o i n t s and
two
links
l i n k length,
the
link
(a),
(2)
the
Denavit-Hartenberg
of
the relationship
twist,
Figure 2.
See Figure 2.
(Figure 3 )
Frame two
is placed with
z2 along the
Now,
frame
is placed
where theta1 is zero, and the last frame (frame 3 ) is placed such
that the x3 axis aligns with x2 axis and d3 is zero.
The z3 axis
An additional
frame, called
tool
frame, is conventionally
end of the last link.
The
13
_..________I
--
__
ai-l
0
90
0
0
thetai
di
thetal
L1
thetaa
theta3
L2
L3
position
frame
in
frame
i to the same
14
where i-lT
the
and
transformation
orientation
position
and
in
relating
frame
a
to
orientation described
position
the
same
in
terms
of frame i-1.
s(thi)
c(thi)
s(ali-l) = the sine of the link i-1 twist angle (alphai- l).
c(ali-l) = the cosine of the
1)
ai-l
di
Using Equation
(1) and
transformations to
the
needed
to
=I
C
S
-S
:j
IT2
-S
0
C
s2
2
T3
c3
-s3
s3
Tt
-1
L3
-0
all
that
are
necessary
to find the
16
The
forward
kinematic
orientation of a
known.
equations
fingertip when
define
the
the position
and
(5)
together.
c1c23
-c1s 23
LICl
s1c23
- s 1s 23
-C1
Llsl
L2s2
23
23
+ L2c1c2 + L3c1c23
+
i-
L2s1c2
L3s1c23]
L3s23
given set of joint angles by putting the joint angles into matrix
Equation (6). The upper
left
are the components of the position vector: px, py, and p,.
17
forward
is, as would
be expected, the
kinematics problem.
Given an (x,y,z)
necessary
to
place
the
equations
requires
finger
in
forward
kinematic
solution difficult.
For some
For
the
three-degree-of-freedom
To grasp an
contact of the
fingers
is a solution.
orientation, or
coordinates.
finger there
combination
of
three positional
is required.
fingertip
parameters of interest.
the
OTt
the
transform:
18
and
pY
They are
(6).
Px
pY
L1Cl
LISl + L2s1c2
L 2 ~ 1 ~+ 2L 3 ~ 1 ~ 2 3
+ L3s1c23
Equation
and
by
cl,
and (9).
(7)
(8),
divide the
results.
The result is
equation (10)
be
solved
for thetal by
using a two-sided
thetal
ATAN2(p
p )
Y' x
c1
and
Equation
(8)
by s1 and
a1 = (P,
2 0,5
Py 1
Squaring equations
(9)
L1 = L3c23
and
(13),
+ L2c2
adding,
even
function
Equation
solutions are
flexing the
(15).
finger
or
f(-x)], and is
Because
attainable.
[f(x)
of
These
physical
two
hyperextending
constraints,
solutions correspond to
it:
theta3 negative, or
theta3 = ACOS[(al 2 + p, 2
L32
L22 ) / ( ~ L ~ L ~ ) ]
20
two
7'
\
FLEX
$ HYPER
EXT N S S O N
\
\
\
alc2
pzs2 = L3c3
L2
rearranged
(16)
to
obtain
theta2 :
-(L3c3
L2) = -p 2 s2
(-alc2)
(17)
theta2 = ATAN2(-al,p,)
but
ATAN2[(L3c3
multiple
one
is
not
positive
value
in
the
La),
solutions.
attainable
argument
of the second
This
extended
22
an
of
each
successive
link
can
be
calculated by
I1propagatingt1
the velocity of the previous link; Craig [3].
velocity of each link
has
The
i+l
- i+lR,io
1
i
Oi+l
i+lz
tdi+l
i+l
relating
describing
joint i+l.
It
where
is usually
axis
the
of rotation of
vector
[ 0 0 13T
the
transformation to
23
Therefore, a
descriptions
from
one
~ .is
Only i + l 1
needed to change the reference frame description, and not i+lTi'
reference frame to
the next
i+lv
- i+lRi( ivi
i+l
where i+lvi+l
the
is not
required.
i
i
0.x
1 Pi+l)
linear
velocity
of
the
origin
of link
iVi
01
. x Pi+l = the c r w s product
of
the
angular velocity of
reference
tot
pd2+td3
24
See
be
rewritten as
shown
relationship between
joint velocities.
The
3x3
matrix
the
on
in
Equation (23) to
the
right-hand side of
L2s3
the
locally
The structure of a
defined
link
frames.
Then, the
shows a generalized
link.
where ifi
the
i
fi+l = o
force
exerted
on
link
by
link
i-1
referenced to frame i.
i
"i
i
"i+l
i
i
'i+lX fi+l = o
26
(25)
where ini
on
link
by link i-1
referenced to frame i.
i
i
'i+lX fi+l
of
link
frame i.
If Equations (24) and (25) are rewritten to find ifi and ini, and
i+l
using the known values of i+l
"i+l and the appropriate
i+l and
rotation matrices, Equations (26) and (27) are obtained.
fi
i+l
lRi+l
i+l
i+l
in = i
Ri+l
ni+l
i
The form of these
i
i
'i+lX fi
equations allows
the
are
tlpropagationtt
of the forces
base link.
balanced
by
mechanism, with the exception of the torqu'e about the joint axis.
Thus, the joint torque
required
the
joint
Taui
in TiZ
i
i
27
(27),
and
(29)
Tau =
'(L
L2s3
L3+L2c2
c +L c +L1)
323 2 2
0
0
L3
where fx, f
and fZ = the x, y, and z direction of
Y'
applied to the tip of a finger.
the
The
a force
inverse transformation.
28
4.3
Derivation of
the
dynamic
equations
of
manipulator can be
it
is
an
intuitive approach
it was selected
mechanical background.
Before
calculating
the
inertial
forces on
each
link, the
In subsection 4.1
the
last
be
used
(31)
and
(32)
are
- i+lR.io
i+lo
1
i
i+l
i+lz
tdi+l
i+l
29
i+l
Odi+1
1
i+lRiiod.
i+lz
i+lRiioi x tdi+l
i+l
+ tddi+li+lzi+l
where
i+l
Odi+1
the
rotational
acceleration
of
link
i+l
tddi+l
i+la
i
i
i
i
i
i+l - i+lRi[iodi x Pi+l + oi x ( oi x Pi+l) + ail
(32)
needed
iaCi
30
(33)
of
the
center
of mass of
the
position
of
the
center
of
link
frame
referenced to frame i
Equations (31), (32), and (33) have been written to propagate the
accelerations from link 0 to the end link, since the acceleration
and velocity of link 0
effects on
the
links
is known.
are
accounted
for
by
letting
the
4.3.2
Force 'IPropagationl@
acting
link can be found if the mass, the mass moments of inertia, and
the linear acceleration of
acceleration is
found
the
using
center of
equations
and
(32).
The
The
(34)
where Fi
mi
where Ni
Ici
.t
the
center of link i in
i
oi x Iciioi
the moment of
inertia
(35)
tensor
center of link i
The
forces and
a generalized link.
The free-body
32
Figure
6.
+ iFi
where ifi
i
ni
i
i
i+ln
i
Ni + Ri.+l
i+l + 'Ci
i'i+l
x ''Ri
x iFi
(37)
+ i+l
i+l
the
component
(38).
33
of
Taui
in TiZ
i
i
4.3.3
iterating forward from the base link to the end link with
Equations (30), (31),
from the end link to
(32),
the
(33),
first
(34)
and (38).
program
used.
The complete
The
resulting
Tau(t,td,tdd) = M(t)tdd
where Tau
V(t,td)
G(t)
that is a function of
first
(39)
vector
of
velocity
terms,
containing
(54) give
the
terms
generated in the
analyses.
Mll
c2 *Ix3
+
+
+
* s ~ ~ * -s c
3
*(L2/2)2 *m3*s2*s3
23
c *L *(L2/2)*m3*s2*s - L1*(L2/2)*m3*s2*s3
2 2
3
c *I *s *s3 + c3*Ix3*s2*s
+ Ix2*s22
23 y3 2
23
c *c * c ~ ~ * ( L 2~*m3
/ ~ +) c * c ~ ~ * L ~ * ( L ~ / ~ ) * ~ ~
2
2 3
c 2 2 * c 3 *2~*(~,/2) *m3 + c 2 3 * ~ 1 * ( ~ 2 / 2 ) * m 3
c2*c3*L1*(L2/2)*m3
+ c22*L22*m3 + 2 * c 2 * ~ 1 * ~ 2 * m 3
( ~ ~ / *m3+c2
2 )
2 * ( ~ ~ / 2 ) ~+* 2m*~c 2 * ~ 1 * ( ~ 2 / *m2
2)
2
+ (L1/2)*m2 + (L1/2) *ml + Izl
+
MI2
= 0
c2 2*1y2
c *c3*c *I
2
23 y3
MI3
= 0
M21 = 0
M31
M3 2 = (L2/2)2*rn3+C3*L2*(L2/2)*m3+1,3
36
V1
*s2*s
23
*S
*td *td3+c * ~ ~ ~ * I ~ ~ * s ~ * t d ~ * t d ~
1
*I * S *td *td3+C * ~ ~ ~ * I ~ ~ * ~ ~ * t d ~ * t d ~
y3 3
1
2
- 2 * c *c3*(L2/2)2*m3*s *tdl*td3
-c2*c
23
2
-2*c
23
*L2*(L2/2)*m3*s
23
-2*L1*(L2/2)*m3*s
23
*tdl*td3
*td *td3-c2*c3*I
1
23
*~~~*td~*td~
- ~ ~ * ~ ~ * I ~ ~ * ~ ~ ~ * t d ~ * t d ~ + ~ ~ * ~ ~ * I ~ ~ *
+c 3 *c23*Iz3
*s2*tdl*td3-C3*~23*Iy3*~2*tdl*td3
+~~*c~~*I~~*s~*td~*td~
+2*(L2/2) 2*m3*s2*s23*s3*tdl*td2
*s2*s
*s3*td *td2+I
+I23
23
1
Y3
-Ix3*s2*s
23*~3*tdl*td2
*s * ~ ~ ~ * s ~ * t d ~ * t d ~
2
+2*L2* (L2/2)*m3*s22*s3*tdl*td2
+C
* c ~ ~ * I ~ ~ * s ~ * ~*I
~ ~*s3*tdl*td2
* ~ ~ ~ - c ~ * c
23 y3
+~~*c~~*I~~*s~*td~*td~
-2*c2*c3*(L2/2) 2*m3*s23*tdl*td2
-2*~~*~~*(L~/2)*m~*s~~*td~*td~
-2*L1*(L2/2)*m3*~23*tdl*td2-c2*c3*1z3
*~~~*td~*td~
-c2*c *I * ~ ~ ~ * t d ~ * t d ~ *+~c~~~**~t ~d *~ I* ~t ~
d~
3 Y3
-2*c2*c3*L2*(L2/2)*m3*s2*td *td2
1
- ~ * c ~ *m3
* L*s~ *tdl*td2-2*L
~
*L2*m3*s2*tdl*td2
2
37
-2*c2*(L2/2) 2*m2*s2*tdl*td2
-2*L *(L2/2)*m2*s2*tdl*td2+c3*c23*Iz3*s2*tdl*td2
1
*I *s2*tdl*td2-2*c *I *s2*tdl*td2
y3
2 Y2
+c * ~ ~ ~ * I ~ ~ **td2+2*c2*Ix2*s2*tdl*td2
s
~
*
t
d
3
1
-c3*c23
V2
-L2*(L2/2)*m3*s3*td32-2*L2*(L2/2)*m3*s3*td2*td3
+c *L 2*m * s 2 * s 2*td12+L1*L2*m3*s2*s32*tdl2
2
2
3
3
2*~2*
(L2/2)*m3*s3*tdl +c22*L2*(L2/2) *m3*s3*tdl2
-c2 3
*I
-c
23
*td12+c2*c3*L2*(L2/2)*m3*s2*tdl2
x3*'23
v3
+c23*Iy3*~23*tdl- c ~ ~ * I 23*tdl
~~*s
+ c ~ * c ~ * (L2/2)
L ~ * *m3*s2*td12+c3*L1*(L2/2)*m3*s2*tdl2
G1
- -n
*s *s +f *L *s
4 y 2 3 42 3 2
+c *n4x*s +c *c3*n -c * ~ ~ * f ~ ~ * L ~ - c ~ * f ~ ~ * L ~
3
2 2
4Y 2
'f4z*L1
38
G2
(53)
*C * g + ( ~ ~ / 2 ) * m ~ + c ~ * c 2
~*m
~ *3 +c2*g*(L2/2)*m2
g*L
2 3
+f4y*L3+C3*f4y*L2
+C
G~
= - g * (2~/2)*m 3 *s2*s3+n4z+c2*c3*g*(L2/2)*m3+f4y*L3
are
39
(54)
APPENDIX A
4.
are
written
in
llmanipulatorllspace:
torques are
the
torques
required
joints, and
the
derived
joint
position
motors,
and
not
gear
velocity
the
train
angles
and
controls
joints,
the
manipulator
and
at the
by
the derived
and
must
thus
be
series
be
of
pulleys.
accomplished
the
calculated
The
at the
torques,
to the motors.
the
necessary to transform
PROCEDURE FOR
DETERMINING
TENDON
40
Tmin = 0.0;
: constants a and b
a = 0.147082081;
b = 0.227833027;
fl =
0.351757117*M1
f2 = -0.227083653*Ml
f3 = -0.227083653*Ml
f4 =
0.351757117*M1
+
-
0.4403178320*M2
0.415855728*M3
0.0233141675*M2
0.751094470*M3
0.0233141675*M2
0.707056610*M3
0.3680896100*M2
0.347640183*M3
T3 = b*y
T4 = a*y
+
+
+
+
fl;
f2;
f3;
f4;
SUMMARY OF MATRICES:
0.86763
R =
-0.6477
-0.6477
1.237
0.6477
-0.6477
-1.237
0.0
0.6858
-0.6858
0.0
1.0
1.54902
1.54902
1.1303
1.0
0.351757117
R-
0.440317832
-0.415855728
0.147082081
-0.227083653
-0.0233141676
0.75109447
0.227833027
-0.227083653
-0.0233141676
-0.70705661
0.227833027
1 0.351757117
-0.36808961
0.347640183
0.147082081
NOTES :
Dimensions are in cm.
For example if
T is in grams M
R.T will
negative tensions.
Any
elements in row
value
4
of
will
with
cause no
moment (Ml,M2,M3) to be
(vector of
tendon velocities)
42
and
If
(vector of joint
then V
Rt .W can be
rates.
tendon
space to motor
between
Capstan radius
= .375
Gearing reduction
in.
2000/rev.
is simply a
the motor
= 25/1
Encoder resolution
space
APPENDIX B
- MACSYMA Output
r ora, -11 ,
[s2 I c2 * 01 $
(c8) R23:matrix([c3,-~3,0],
[S3,C3,01,
[O,O,ll)$
(c9) R34:matrix([l,O,O],
r0,1,01 I
[O,
0,111 Q
44
(c22) P12:matrix([Ll],[o],[01)$
(c23) PlCl:matrix([Ll2],[0],[0])$
(c24) ~23:matrix([L2],[0],[Ol)$
(c25) ~ 2 ~ 2 : m a t r i x ( [ L 2 2 ][O],
,
[O])$
(c26) P34:rnatrix([L3],[0], [a])$
( ~ 2 7 )~ ~ ~ ~ : m a t r i x ( [ ~ ~ ~ ] , [ ~ 1 , [ 0 1 ~ $
(c28) c1I1:matrix([IX1,0,0],
[O,IY1,01,
r o t 0 ,IZ11) $
(c29) C212:matrix([IXZ,O,O],
[ O f IY2,OI I
[ O f 0 ,I221 1 $
(c30) C313:matrix([IX3,0,0],
[O,IY3,01,
[ O r 0 I Iz3 I) $
(c31) cross(a,b):=matri~([a[2]*b[3]-a[3]*b[2]]~
~ ~ ~ ~ l * ~ ~ ~ ~la [
- 1~1 *~ ~ ~~ ~l 1 *- a~ [ ~2 1~* ~
l ~l~ 1f 1 ~ ~
(c32) lf44:matrix([f4~],[f4y], [f4z])$
(c33) I n 4 4 : m a t r i x ( [ n 4 ~ ] , [ n 4 y l , [ n 4 z ] ) $
(c34) Oll:R10.000+TD1*Zll;
[ tdl
(c35) ODll:R10.000+cross(RlO.OOO,TDl*Zll)+TDDl*2ll:
45
VD1Cl:cross(OD11,P1Cl)+cross(Oll,cross(Oll,PlCl))+VDll;
[ s2 tdl
I] 3
[ [ [ - 112 m l tdl
[
[
[[112 m l tddl]]
1
3
1
1
OD22:R21.ODll+cross(R21.011,TD2*Z22)+TDD2*Z22;
[
[
[
~2 tdl td21 ]
[ tdd2 ]
46
3
1
1
1
(c42) VD22:R2l.(cross(OD11,P12)+cross(Oll,cross(Oll,Pl2))+VDll);
[
[
[[g ~2
2
1
~2 11 tdl ]] 3
2
[ [[ll s2 tdl + c2 g]]
1
1
3
1
11 tddl]]
[[-
(c43) VD2C2:cross(OD22,P2C2)+cross(O22,cross(O22,P2C2))+VD22:
[
[
[
[
[
(d43)
[[122 tdd2
2
c2 122 s2 tdl
[ [ - 122 ( ~ tddl
2
2
2
~2 122 tdl
[ [ - 122 td2
~2 tdl td2)
2
c2 11 tdl + g S2]]
1
1
1
1
+ 11 s2 tdl + c2 g]]
1
1
11 tddl + 122 ~2 tdl td2]] 3
2
2
2
~2 122 tdl
2
~2 11 tdl
1
1
g ~2)]]
(d44)
1
1
1
2
2
c2 122 s2 tdl + 11 s2 tdl + c2 g)]]
s2 tdl td2)
(c45) N22:C212.OD22+cross(O22,C212.022);
[
[
(d45) [
[
[
[
c2 tdl td2)
s2 tdl td2)
[iz2 tdd2
2
c2 iy2 s2 tdl
c2 1x2 s2 tdl ]
[ c2 s3 tdl
[
[ c2 ~3 tdl
c3 s2 tdl
~2 ~3 tdl ]
td3
td2
1
2
(c46) 033:R32.022+TD3*233;
3
1
1
1
1
1
1
(c47) 033:matrix([s23*TDl],
[co23*TD1],
[TD2+TD3]) $
(c48) OD33:R32.OD22+cross(R32.02~,~D3*~~~)+TDD3*233;
+ (c2 c3 tdl
c3 (c2 tddl
(d48) matrix([[c3
[ [ - 8 9 ( ~ tddl
2
s2 s3 tdl) td3]],
- ~2 tdl td2)
s3 (c2 tddl
( ~ ~3
2 tdl
s2 tdl td2)
c2 tdl td2)
~3 ~2 tdl) td3]],
[[tdd3
(c49) O D 3 3 : m a t r i x ( [ s 2 3 * T D D l + c o 2 3 * T D 1 * T D a + c 0 2 3 * T D l * T D 3 ] ~
[CO~~*TDD~-S~~*TD~*TD~-~~~*TR~*TD~],
[ TDD2+TDD3 ]) :
[ s23 tddl
[ C023 tddl
[
[
(d49)
tdd3
-+
1
3
1
1
~ 2 3
tdl td?
tdd2
2
(d50) matrix([[[s3
2
+ ~3 ( - 12 td2
[[E-
2
~3
(12 tdd2
+
2
2
~2
12 tdl
2
c2 12 s2 tdl
2
2
12 t d l
( - 12 td2
~2
12 ( ~ tddl
2
~2 tdl td2)
~2 11 tdl
11 s2 tdl
t 11 s2 tdl
c2 12 s2 td1
g sZ)]]],
c2 g)
c2 g)
2
2
+ g s2)]]],
~2 11 tdl
1 1 tddl
12 S2 tdl td2]]])
(12 tdd2
122 (td3
2
2
~ 0 2 3 122 tdl I]], [[[122 (tdd3 + tdd2)
td2)
c3 ( - 12 td2
48
11 92 tdl
2
c2 12 s2 tdl
~2
2
12 tdl
+ c2 g)
2
C2 11 tdl
+ g S2)
tdd2]])
c3 (12 tdd2
~3 ( - 12 td2
2
2
11 tddl
~2
td2)
~2 11 tdl
~ 2 3tdl td3
+ c2 g)
2
12 tdl
+ 11 s2 tdl
c2 12 s2 tdl
12 (c2 tddl
s2 tdl td2)
12 ~2 tdl td2]]])
122 (td3
co23
td2)
~3 ( - 12 td2
c2 12 s2 tdl
2
~2
2
122 tdl )]]Ir [[[m3 (122 (tdd3
2
s3 ( - 12 td2
~2
12 tdl
12 (c2 tddl
12 s2 tdl td2)]]])
2
1 1 s2 tdl
~2 11 tdl
(c53) N33:C313.0D33+cross(033,C313.033)
(d53) matrix([[ix3
(s23 tddl
td2)
2
co23 ix3 s23 tdl I])
g s2)
2
g ~ 2 )
+ C023 122 ~ 2 3tdl )]]I,
td2)
(c54) lf33:R34.lf44+F33;
c2 g)
122 ~ 2 3
tdl (td3
2
~2 11 tdl
11 tddl
~2 tdl td2)
c 2 g)
tdd2)
12 tdl
2
11 s2 tdl
td2)]lr
tdd2)
+ td2)
2
(d54) matrix([[[m3
122 (td3
c023
td2)
2
122 tdl )
~3 ( - 12 td2
f4x]]],
2
2
12 tdl
(-
12 (c2 tddl
~2 tdl td2)
11 s2 tdl
+ c2
12 s2 tdl td2)
~2
12 tdl
11 s2 tdl
~2 11 tdl
c2 g
g ~2
11 tddl
C2 11 tdl
9)
+ g S2)
td
2
~2
+ f4y]]], [[[m3
2
c2 12 s2 tdl
~3 ( - 12 td2
td2)
f4z]]J)
(c55) ln33:N33+R34.ln44+cross(P3C3,F33)+cross(P34,~34.lf44);
(d55) matrix([[[[ix3
(s23 tddl
[[[[-
+ td2)
12 ( ~ tddl
2
12 s2 tdl td2)
~2 tdl td2)
~ 2 3tdl td3
11 tddl
td2)
+ td2)
td2)
n4y
f4z 13]]]1,
2
[[[[122 m3 (122 (tdd3
tdd2)
~2 9)
2
C023 122 s23 tdl )
~3 ( - 1 2 td2
+
+
c3 (12 tdd2
2
c2
2
12 tdl
iZ3 (tdd3
c2 12 s2 tdl
+ 11 s2 tdl
2
c2 11 tdl
g S2)
2
tdd2)
50
(c56) TAU3:grind(ratexpand(transpo~e(ln33).233)):
[[[122A2*m3*tdd3+iz3*tdd3+122A2*m3*tdd2+c3*l2*l22*m3*tdd2+~z3*tdd2
+12*122*m3*s3*td2A2+c2A2*12*122*m3*s3*tdlA2
+c2*ll*122*m3*s3*tdlA2+co23*122A2*m3*s23*tdlA2
+co23*iy3*s23*td1A2-co23*ix3*s23*tdlA2
+c2*c3*12*122*m3*s2*tdlA2+c3*ll*l22*m3*s2*tdlA2-g*l22*m3*s2*s3
+n4z+c~*c3*g*122*m3+f4y*l3]]]$
(d56)
done
(c57) lf22:R23.lf33+F22:
(d57) matrix([[[-
c3 (12 tdd2
s3 ( - 12 td2
~2
12 tdl
tdd2)
2
1 1 s2 tdl
~2 1 1 tdl
c2 g)
2
g ~ 2 +
) C023 122 ~ 2 tdl
3
)
2
2
+ C3 ( - 12 td2
2
2
2
2
S3 ( - 12 td2
2
122 t d l
c2
tdd2)
2
~2
2
~3 ( - 12 td2
2
12 tdl
12 (c2 tddl
2
122 tdl )
+ f4x)
+ g s2)]]],
2
c2 12 s2 t d l
+
2
+
2
11 s2 tdl
1 1 tddl
+ c2 g)
1 1 s2 tdl
2
g ~ 2 +
) C023 122 S23 tdl )
2
1 1 s2 t d l
~2 1 1 tdl
~2 tdl td2)
c2 g)
f4y)
~2 11 tdl
+ g ~ 2 ) C023
c2 1 1 tdl
c3 (12 tdd2
12 tdl
~2
1 1 s2 tdl
2
c2 1 1 t d l
c2 12 s2 tdl
2
12 t d l
~2
+ m2 ( - 122 td2
c2 g)
g ~ 2 ) C023
f4y)
2
2
122 tdl )
+ c2 g)]]],
+ td2)
+ f4x)
+ 12 s2 tdl td2) + m2
122 ( 0 2 tddl
(-
s2 tdl td2)
11 tddl
(d58) matrix([[[[-
12 ( ~ tddl
2
~2 tdl td2)
(9
11 tdQ1
- co23
+
(-
td2)
12 (c2 tddl
n4x)
12 s2 tdl td2)
12 (c2 tddl
~2 tdl td2)
td2)
+
+
td2)
.t
f4z)
11 tddl
f4z 13)
+ td2)
c2 tdl td2)
S23 t d l td3
1 1 tddl
s2 t d l td2)
td2)
s23 t d l td2)
+ td2) + n4y
f4z 13)
+ td2)
co23 t d l td3
~2 tdl td2)
S23 t d l td2)
td2)
122 ~ 2 3
tdl (td3
~2 tdl td2)
12 s2 t d l td2)
122 ~ 2 t
3 d l (td3
td2)
[[[[c3
11 tddl
td2)
+ co23 t d l td2)
+ n4x)
+ 122 S2 t d l td2)
~2 9)
2
~3 ( - 12 td2
~2
2
12 tdl
2
c2 11 tdl
52
g s2)
2
+ 11 s2 tdl
2
(12 tdd2
~3 ( - 12 td2
~3 ( -
iz3 (tdd3
iz2 tdd2
2
c2 ix2 s2 t d l
~2
1 1 ~2 tdl
2
12 t d l
tdd2)
~2 g )
2
122 (td3
g s2)
td2)
2
122 tdl )
C023
tdd2)
n4z
+ c2 g)
11 s2 tdl
g ~ 2 +
) C023 122 S23 tdl )
f4x))
~2 1 1 tdl
2
c2 12 s2 tdl
12 tdl
c3 (12 tdd2
~2
2
~2 1 1 tdl
12 td2
~2 12 ~2 tdl
c2 122 s2 tdl
1 1 s2 tdl
2
C023 ix3 s23 tdl
c2 g )
c2 iy2 s2 tdl
f4y 13]]]])
(c59) TAU2:qrind(ratexpand(transpose(ln22).222));
[[[122A2*m3*tdd3+c3*l2*m3*tdd3+iz3*tdd3+iz3*tdd3+l2A2*m3*s3A2*tdd2+l22A2*m3*tdd2
+2*~3*12*122*m3*tdd2+~3~2*12~2*m3*tdd2+122~2*m2*tdd2+~~3*~dd2
+iz2*tdd2-12*122*m3*s3*td32-2*12*12*122*m3*s3*td2*td3
+c2*12A2*m3*s2*s3A2*tdlA2+ll*12*m3*s2*s3A2*tdlA2
-co23A2*12*122*m3*s3*td1A2+c2A2*12*122*m3*s3*td1A2
+c2*ll*122*m3*s3*tdlA2+co23*122A2*m3*s23*tdlA2
+c3*co23*12*122*m3*s23*tdlA2+co23*iy3*s23*tdlA2
-co23*ix3*s23*td1A2+c2*c3*12*m3*s2*rn3*s2*td1A2
+c3*ll*122*m3*s2*tdlA2+c2*c3A2*12A2*m3*s2*tdlA2
+c3A2*ll*12*m3*s2*td12+c2*1222*m2*s2*tdlA2
+ll*122*m2*s2*td1A2+c2*iy2*s2*tdlA2-c2*~x2*s2*tdlA2
+c2*g*12*m3*s3A2-g*122*m3*s2*s3+f4x*l2*s3+n4z+c2*c3*g*l22*m3
+c~*c~A~*g*1~*m~+c~*g*122*m~+f~y*l~+c3*f~y*l2]]]$
done
(d59)
(c60) lfll:R12.lf22+Fll;
(d60) matrix([[[c2
(-
c2 12 s2 tdl
c3 (12 tdd2
~3 ( - 12 t d 2
~2
2
12 tdl
tdd2)
2
1 1 s2 tdl
~2 11 tdl
c2 g)
g s2)
2
C023 122 S23 tdl ) + f 4 y )
C3 ( - 12 td2
m2 ( - 122 td2
c2 9)
2
C023 122 S23 tdl )
~2
c2
122 tdl
12 tdl
2
~3 ( - 12 td2
tdd2)
~2
+ c3
c3 (12 tdd2
2
[ [ [ - m3
(-
12 ( ~ tddl
2
12 ~2 tdl td2)
~2 9)
g s2)
11 s2 tdl
c2 9))
112 ml tddl
tdd2)
2
122 tdl )
f4x)
I]],
td2)
11 tddl
c3
2
2
12 tdl
+ c2 9)
2
2
12 tdl
~2 11 tdl
c2 12 s 2 tdl
2
~2
f4z]]],
11 s2 tdl
td2)
112 ml tdl
~2 tdl td2)
~2
11 s2 tdl
C023
2
122 (td3
m2 ( - 122 ( ~ tddl
2
f4x)
+ g ~ 2 )
2
~2 1 1 tdl
c3 ( - 12 td2
2
c2 12 s2 tdl
2
td2)
g s2))
1 1 tddl
2
c2 12 s2 t d l
s3 ( - 12 td2
~2 1 1 tdl
~2 tdl td2)
c2 122 s2 tdl
2
122 tdl )
S3 (m3 (S3
12 tdl
122 (td3
m2 (122 tdd2
~2
1 1 ~2 tdl
( - 12 td2
c2 12 ~2 tdl
c2 g)
g ~ 2 ) C023
c2 1 1 tdl
12 tdl
f4y)
1 1 s2 tdl
~2 1 1 tdl
2
(12 tdd2
c2 12 s2 tdl
g ~ 2 +
) C023 122 S23 tdl ) + f 4 y )
2
1 1 s2 tdl
2
~2 1 1 tdl
54
g S2)
c2 g)
C023
2
122 (td3
2
122 tdl )
td2)
f4X)
2
122 t d l
m2 ( - 122 td2
~2 9)
2
co23 122 s23 tdl )
c2
tdd2)
~2
2
~3 ( - 12 td2
c2 1 1 tdl
c3 (12 tdd2
f4y)
2
12 tdl
~2 12 ~2 tdl
2
C3 ( - 12 td2
m2 (122 tdd2
~2
11 ~2 tdl
12 tdl
2
c2 12 s2 t d l
2
~2 1 1 tdl
g s2))
11 s2 tdl
g s2)
s3 (m3 (s3
2
(12 tdd2
2
~2 11 t d l
+ c2 122 s2 t d l
c2 9)
122 (td3
+ td2)
2
g ~ 2 ) C023
2
122 tdl )
+ f4X)
+ 1 1 s2 tdl
c2 9 ) ) + g m 1 J l l )
(c61) lnll:Nll+R12.ln22+cross(PlCl,Fll)+cross(Pl2,Rl2.lf22):
( - s3 ( -
(d61) matrix([[[[c2
(C023 tddl
~ 2 t
3 d l td3
c2 iz2 t d l t d 2
td2)
+
+
n4y
n4x)
12 ( ~ tddl
2
12 s2 tdl td2)
~2 t d l td2)
td2)
~2 t d l td2)
11 tddl
+ td2)
~ 2 tdl
3
td3
+ c2 t d l td2)
s2
1 1 tddl
f4z 13)
12 ( ~ tddl
2
12023 t d l td3
td2)
12 s2 tdl td2)
s23 t d l td3
td2)
122 m3 ( - 122
+ td2)
s23 t d l td2)
td2)
n4y
f4z 13)
+ td2)
12 (c2 tddl
+ 12 s2 tdl td2)
f4z)
iy2 (c2 t d d l
11 tddl
td2)
c2 12 s2 tdl
s3 ( - 12 td2
C3 ( - 12 td2
m2 ( - 122 td2
tdd2)
~2 9)
c2
2
co23 122 s23 tdl )
~2
12 tdl
td2)
+ n4x)
td2)
2
2
c2 g)
g s2)
m2 (122 tdd2
12 tdl
1 1 s2 tdl
2
~2 1 1 t;d1
+
2
g S2)
c2 11 tdl
c3 (12 tdd2
2
C023 122 ~ 2 3
tdl )
c2 g)
C023
+ f4y)
2
122 (td3
2
122 tdl )
+ td2)
+ f4X)
g s2))
2
c2 12 s2 tdl
11 s2 tdl
2
c2 11 tdl
g S2)
s3 (m3 (s3
2
11 62 tdl
2
~3 ( - 12 td2
2
=+
12 tdl
c2
2
11 t d l
f4y)
2
~2 12 ~2 t d l
2
122 t41
2
11 s2 tdl
- ~2
12 tdl
c2
tdd2)
c2 12 s2 tdl
2
~2
~3 ( - 12 td2
c3 (12 tdd2
(12 tdd2
$3 (ix3 (s23 f d d l
+ 123
[ [ [ [ - 11 (s2 ( -
s 2 tdl td2)
c2 122 s2 tdl
c2 9)
2
~2 1 1 tdl
122 (td3
td2)
2
122 tdl )
g ~ 2 ) C02 3
f4x)
1 1 s2 tdl
c3 (12 tdd2
c2 9)))
2
tdd2)
56
+ c2 12 s 2 tdl
1 1 s2 tdl
~2 9)
2
co23 122 s23 tdl )
(12 tdd2
S3 ( - 12 td2
iz3 (tdd3
iz2 tdd2
c2 ix2 s2 tdl
tdd2)
tdd2)
~2 9)
2
~2 11 tdl
c3 ( 1 2 tdd2
n4z
~2 11 tdl
12 (c2 tddl
1 2 s2 tdl td2)
1x23
tdl td3
td2)
td2)
12 ( ~ tddl
2
12 s2 tdl td2)
n4x)
td2)
2
122 tdl )
f4x))
2
11 s2 tdl + c2 g)
2
g ~ 2 )
+ C023 122 S23 tdl )
c2 122 s2 tdl
2
11 s2 tdl
c2 g )
2
c2 iy2 s2 tdl
td2)
td2)
n4y
f4z 13)
c2 tdl td2)
c2
122 ~ 2 tdl
3
(td3
122 ~ 2 3
tdl (td3 + td2)
57
td2)
2
c2 12 s2 tdl
g ~ 2 ) C023
11 tddl
122 (td3
~2 tdl td2)
g s2)
f4y 131111,
11 tddl
2
C023 ix3 s23 tdl
g 112 ml
~2 tdl td2)
c2 11 tdl
s3 (m3 (s3
2
11 s2 tdl
2
2
~2 12 tdl
2
(-
2
2
~2 12 tdl
c3 ( - 12 td2
2
1 2 tdl
~2
f4y)
2
[[[[s2
~2 12 s2 tdl
2
~3 ( - 1 2 td2
td2)
n4y
f4z 13)
12 (C2 tddl
S2 tdl td2)
1 1 tddl
~ 2 tdl
3
td2)
td2)
+ 12 s2 tdl td2) + f4z) + s3 (ix3 (s23 tddl + co23 tdl td3 + co23 tdl td2)
+ c023 iz3 tdl (td3 + td2)
- co23
~2 tdl td2)
11 tddl
s2 tdl td2)
12 (c2 tddl
td2)
n4x)
~ 2 3
tdl td2)
11 tddl
122 ~ 2 3
tdl (td3
s2 tdl td2)
s2 tdl td2)
+ 12 s2 tdl td2)
f4z)
2
112
ml tddl
td2)
11 tddl
izl tddl]]]])
(c62) TAU1:grind(ratexpand(transpose(lnll).Zll)):
[[[c2*~x3*s23*s3*tdd1-co23*l22A2*m3*s2*s3*tddl-c2*l2*l22*m3*s2*s3*tddl
-ll*122*m3*s2*s3*tddl-~o23*iy3*s2*s3*tddl
+~3*ix3*s2*s23*tddl+ix2*s2^2*tddl
+c2*c3*co23*122A2*m3*tddl+c2*co23*12*122*m3*tddl
+c2A2*c3*12*122*m3*tddl+co23*ll*122*m3*tddl
+c2*c3*ll*122*m3*tddl+c2A2*12A2*m3*tddl
+2*c2*ll*12*m3*tddl+llA2*m3*tddl+c22*122A2*m2*tddl
+2*c2*ll*122*m2*tddl+llA2*m2*tddl+ll2A2*ml*tddl+~zl*tddl
+~2*~3*~023*iy3*tddl+c2~2*iy2*tddl
+2*122A2*m3*s2*s23*s3*tdl*td3+iz3*s2*s23*s3*tdl*td3
+iy3*s2*s23*s3*tdl*td3-i~3*s2*s23*s3*tdl*td3
+~2*~023*iz3*s3*tdl*td3-~2*~023*iy3*~3*tdl*td3
+c2*co23*ix3*s3*tdl*td3-2*c2*c3*l22A2*m3*s23*tdl*td3
-2*~2*12*122*m3*s23*tdl*td3-2*ll*122*m3*~23*tdl*td3
-~2*~3*iz3*s23*tdl*td3-~2*~3*iy3*s23*tdl*td3
+c2*c3*ix3*s23*tdl*td3+c3*co23*iz3*s2*tdl*td3
-c3*co23*iy3*s2*tdl*td3+c3*co23*ix3*s2*tdl*td3
+2*122A2*m3*s2*s23*s3*tdl*td2+iz3*s2*s23*s3*tdl*td2
+iy3*s2*s23*s3*tdl*td2-i~3*s2*s23*s3*tdl*td2
+2*12*122*m3*s2A2*s3*tdl*td2+c2*co23*iz3*s3*tdl*td2
-c2*co23*iy3*s3*tdl*td2+c2*co23*ix3*s3*tdl*td2
-2*c2*c3*122A2*m3*s23*tdl*td2-2*c2*12*122*m3*s23*td1*td2
-2*ll*122*m3*s23*tdl*td2-~2*~3*iz3*s23*tdl*td2
-~2*~3*iy3*s23*tdl*td2+~2*~3*ix3*s23*tdl*td2
-2*c2*c3*12*122*m3*s2*tdl*td2-2*c2*12A2*m3*s2*td1*td2
-2*ll*12*m3*s2*tdl*td2-2*c2*1222*m2*m2*~2*tdl*td2
58
References
Salisbury,
The
Stanford/JPL
Hand:
Mechanical
Specifications1#,(Salisbury Robotics, Inc., Palo Alto, CA,
[l] K.
1984. )
[2] K. Salisbury, IDesign and
1984. )
[3]
[4]
60
DISTRIBUTION:
2540 G. N. Beeler
2545 J. H. Barnette
2545 V. J. Johnson (25)
2545 D. W. Plummer
2545 M. A. Powell
1414 C. S. Loucks
1414 R. W. Harrigan
3141 S. A. Landenberger (5)
3151 W. L. Garner (3)
3154-1 C. H. Dalin (28) for DOE/OSTI
8024 P. W. Dean
UNM Dr. G. P. Starr