4.
写出两杆之间的位姿矩阵从坐标系 Oi-1-xi-1yi-1zi-1 到坐标系 Oi-xiyizi 的变换矩阵为：
Aii 1 R ( xi 1 , a i 1 )Trans ( ai 1 , 0, 0) R( zi , qi )Trans (0, 0, d i ) 0 1 0 cos a i 1 0 sin a i 1 0 0 cos qi cos a sin q i 1 i sin a i 1 sin qi 0 0 sin a i 1 cos a i 1 0 0 1 0 0 0 0 1 0 0 0 ai 1 cos qi 1 0 0 sin qi 0 1 0 0 0 0 1 0 0 sin a i 1 cos a i 1 0 ai 1 sin qi cos qi 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 1 0 0
0 1 di 0 0 1 0 0
sin qi cos a i 1 cos qi sin a i 1 cos qi 0
di sin a i 1 di cos a i 1 1