Figure 5.4. Coordinate frame representation of the camera frame and the fruit position with
respect to the base frame.
where
C1C2 S1C2 -S2 0
-1 -S Ci 0 0
C1S2 S1S2 C2 -d3
0 0 0 and (5-12)
po- the position of the fruit in the base frame coordinates.
The derivation of the vision gains began by assuming the manipulator was initially
aligned with a fruit which was located at some position po in the base frame coordinates and
some distance z from the camera. The joints of the manipulator were located at some position
81, e2, and d3 when the fruit was aligned. This arrangement caused the fruit to lie along Zc at a
radial distance (d3 + z) from the origin of the base frame. Therefore, po was expressed as
p (d3+z)C1 S2
P (d3+z)S1S2
(d3+ z)C2
1 (5-13)
Substituting equation 5-13 into equation 5-11 and solving for pi,