reduced system of equations (6.15)-(6.18) must be solved. By substituting the expressions of uz, qz, u.q, and q.q from Eqs. (6.11)-(6.14) into Eqs. (6.19)-(6.22) and rearranging, we get


rI = (-alt y aid2tx) S1 + (-altx + d21t y) C1

 + t Px typy rld2tz T3d3r4 T4d4, (6.39)'


r2 = -a1r2tx S1 + 1 T2ty C1 Tr12tz + T3T4, (6.40)


r3 -a1r2Px S1 + 1 T2Py C1

 r2Pz + r 2d2 + d3 + T3d4, (6.41)

and

r4= (-alpy aid2Px) S1 + (-alPx + d2alPy) C1

 -Tld2Pz + 73d3d4

 + (p.p +a12+a22+d22-a32-d32-a42-d42). (6.42)


These last four equations prove that the terms rl, r2, r3, and r4 are all of the form


 ri = ril S1 + ri2 C1 + ri3, i=l, . 4,


where the constants rij are fully determined by the arm parameters and the end-effector pose elements.

 Another choice is to use Eq. (6.30). The reduced

 system of equations is given by Eqs. (5.13)-(5.16) with all elements of pose matrix P replaced by corresponding elements of matrix Q of Eq. (6.31). The ri quantities become linear expressions in S5 and C5 and have the form

..