It is useful to check that the multiplication of the four transformation matrices matches for a given
finger and at least one hand configuration, such as the zero position. The computed homogeneous
transformation matrix from the wrist to tool frame for finger 1 is:
Equation 3 - Forward Kinematics Matrix for Finger F1
Where:
2
2
Φ
+
Θ
=
k
a
3
3
Φ
+
Θ
=
k
b
)
cos(
b
a
c
ab
+
=
)
sin(
b
a
s
ab
+
=
)
*
)
2
/
(
*
r
cos(
1
1
j
c
k
π
−
Θ
=
)
*
)
2
/
(
*
r
sin(
1
1
j
s
k
π
−
Θ
=
1
=
k
The hand configuration is determined using position feedback from the encoders. The number of
encoder ticks and availability of inner link joint encoders depends on the model number of the
hand. The following are the hand positions in units of radians
before
the TorqueSwitch™ is
activated:
Θ
k1
=
(ENC(k) / CNT(k)) * π
for k = 1, 2
0
for k = 3
Θ
k2
=
(ENC(k) / CNT(k)) * 140 * (π/180)
for k = 1, 2, 3
Θ
k3
=
(ENC(k) / CNT(k)) * (140/3) * (π/180)
for k = 1, 2, 3
where,
CNT(k) = total number of quadrature encoder counts of finger k
ENC(k) = present encoder position of finger k
Notice that Θ
k3
will generally move 1/3 the amount of Θ
k2
but after the TorqueSwitch™ has been
activated the inner link stops moving and all the joint torque is applied to the outer link. Users
that have inner link joint position sensors will be able to determine finger joint positions at all
times. For earlier hands without inner link position sensors it may be possible to estimate joint
positions after detecting breakaway. This section is concerned with equations for forward
kinematics and does not attempt keeping track of finger joint positions all the time. Once the
outer finger link stops on hands without inner link position sensors after TorqueSwitch™
activation, the joint positions and end tip position cannot be accurately determined until the
TorqueSwitch™ mechanism is reset. It may be reset by opening the finger. Refer to Appendix B
for information on how to detect TorqueSwitch™ activation.
The finger end tip positions are found in the
W
T
T
matrices in the last column. Discarding the 1 in
the last row, you have the Cartesian X end tip coordinate in the first row, the Y coordinate in the
2
nd
row, and the Z coordinate in the 3
rd
row.
Page 76 of 89
+
+
+
+
+
−
−
+
+
+
−
−
−
=
1
0
0
0
0
2
3
3
1
1
1
2
1
3
1
3
1
1
1
1
1
1
2
1
3
1
3
1
1
1
w
a
ab
ab
ab
ab
a
ab
ab
ab
ab
w
a
ab
ab
ab
ab
w
T
D
A
s
c
D
s
A
c
s
s
A
c
s
A
s
s
D
c
s
A
s
s
c
c
s
jA
c
A
c
c
A
s
c
D
c
c
A
s
c
s
c
c
T
