puma 560 的压=雅可比矩阵

PUMA 560是一种六轴机器人，其雅可比矩阵可以表示为：

J = [ -sin(q1)*(l2*cos(q2)+l3*cos(q2+q3)+l4*cos(q2+q3+q4))+cos(q1)*(l2*sin(q2)+l3*sin(q2+q3)+l4*sin(q2+q3+q4)) cos(q1)*(l3*cos(q2+q3)+l4*cos(q2+q3+q4)) -sin(q1)*(l3*sin(q2+q3)+l4*sin(q2+q3+q4)) -sin(q1)*(l4*cos(q2+q3+q4))+cos(q1)*l4*sin(q2+q3+q4) 0 0 0;
cos(q1)*(l2*cos(q2)+l3*cos(q2+q3)+l4*cos(q2+q3+q4))+sin(q1)*(l2*sin(q2)+l3*sin(q2+q3)+l4*sin(q2+q3+q4)) sin(q1)*(l3*cos(q2+q3)+l4*cos(q2+q3+q4)) cos(q1)*(l3*sin(q2+q3)+l4*sin(q2+q3+q4)) cos(q1)*(l4*cos(q2+q3+q4))+sin(q1)*l4*sin(q2+q3+q4) 0 0 0;
0 -l2*sin(q2)-l3*sin(q2+q3)-l4*sin(q2+q3+q4) -l3*sin(q2+q3)-l4*sin(q2+q3+q4) -l4*sin(q2+q3+q4) 0 0 0;
0 cos(q2)+cos(q2+q3)+cos(q2+q3+q4) cos(q2+q3)+cos(q2+q3+q4) cos(q2+q3+q4) 0 0 0;
0 sin(q2)+sin(q2+q3)+sin(q2+q3+q4) sin(q2+q3)+sin(q2+q3+q4) sin(q2+q3+q4) 0 0 0;
1 1 1 1 0 0 0]

其中，q1-q4表示机器人的关节角度，l2-l4表示机器人的臂长。