Table 5.1. Link parameters for the orange-picking robot.

 Axis of Joint
 Link Motion Variable a d a a
 1 Zo 81 81 0 0 -900
 2 Z12 4 0 0 900
 3 Z2 d3 0 d3 0 0



 01000
 1 0 0 0
 2A3 0 1 0 0
 0 0 1 da
 0 0 0 1 (5-3)

where Si = sin Oi, Ci = cos Oi, and d3 is the distance along Z2 from the axis of rotation Z1 to the

origin of frame 3. Multiplying these A matrices, the position and orientation of the image

plane of the manipulator's vision system with respect to the base was established as T3:

 C1 C2 -S, C1S2 C1S2d3
 T 1 2 SlC2 C1 S1S2 S1S2d3
 T3= A2 A3 -S2 0 C2 C2d3

 0 0 0 1 (5-4)

The orientation of Z3 with respect to the robot base frame (frame 0) was determined by the 3 x 3

matrix in the upper left-hand corer of T3. The three column vectors of the 3 x 3 matrix, from

left to right, defined the direction of the X3, Y3, and Z3 axes, respectively. The fourth column

vector of the T3 matrix defined the position of the origin of the camera frame (frame 3). Due to

the configuration of the manipulator, this position vector, [ C1S2d3 S1S2d3 C2d3 1 ]T, was

always colinear with the Z3 = [ C1S2 S1S2 C2 1 T vector of the camera frame.

 Due to the importance of these position vectors in establishing the imaging and vision-

servoing kinematics, it was also important to understand the configuration of the robot for the

possible values of the joint variables. First, let all of the joint variables be equal to 0. In this

case, all of the coordinate frames of the robot had a common point as their origins. Frames 0, 2,

and 3 were completely coincident with common X, Y, and Z vectors. Frame 1, on the other hand,

was positioned so that X1 and Xo, Y1 and -Zo, and Z1 and Yo were coincident. As a result, if all of