Matlab Robotics Toolbox系列—使用篇（5）
Chapter 5 Jacobian
% 雅可比矩阵和差分运动，差分运动可以用一组向量表示[dx dy dz drx dry drz]，可用公式表示为transl(dx, dy, dz) * trotx(drx) * troty(dry) * trotz(drz)，当然更直接用 diff2tr() 函数
>> D = [.1 .2 0 -.2 .1 .1]';
>> delta2tr(D)
ans =
1.0000 -0.1000 0.1000 0.1000
0.1000 1.0000 0.2000 0.2000
-0.1000 -0.2000 1.0000 0
0 0 0 1.0000
% 在其他坐标系下表示方法上，先给出坐标系变换矩阵，再用叉乘方式
>> T = transl(100, 200, 300) * troty(pi/8) * trotz(-pi/4);
>> DT = tr2jac(T) * D;
% 以puma560为例进行描述
>> mdl_puma560
>> q = [0.1 0.75 -2.25 0 .75 0]
% 计算该坐标系下的Jacobian矩阵
>> J = p560.jacob0(q)
J =
0.0746 -0.3031 -0.0102 0 0 0
0.7593 -0.0304 -0.0010 0 0 0
-0.0000 0.7481 0.4322 0 0 0
-0.0000 0.0998 0.0998 0.9925 0.0998 0.6782
0.0000 -0.9950 -0.9950 0.0996 -0.9950 0.0681
1.0000 0.0000 0.0000 0.0707 0.0000 0.7317
% 以关节末端为坐标系计算Jacobian矩阵
>> J = p560.jacobn(q)
J =
0.1098 -0.7328 -0.3021 0 0 0
0.7481 0.0000 0.0000 0 0 0
0.1023 0.3397 0.3092 0 0 0
-0.6816 -0.0000 -0.0000 0.6816 0 0
-0.0000 -1.0000 -1.0000 -0.0000 -1.0000 0
0.7317 0.0000 0.0000 0.7317 0.0000 1.0000
% 右上角的3x3矩阵为0，表明了4-6关节的平移运动不会对末端产生影响
% 判断是否为奇异矩阵，可以用det()方法，不为0即可
>> det(J)
% 其逆矩阵为
>> Ji = inv(J)
Ji =
0.0000 1.3367 -0.0000 -0.0000 -0.0000 0
-2.4946 0.6993 -2.4374 0 0.0000 0
2.7410 -1.2106 5.9125 0.0000 0.0000 -0.0000
0.0000 1.3367 -0.0000 1.4671 -0.0000 0
-0.2464 0.5113 -3.4751 -0.0000 -1.0000 0
-0.0000 -1.9561 0.0000 -1.0734 0.0000 1.0000
% Whitney's resolved rate motion control dQ/dt = J(q)^-1 dX/dt
>> vel = [1 0 0 0 0 0]'; % translational motion in the X direction
>> qvel = Ji * vel;
>> qvel'
% 计算末端的变换矩阵
>> T6 = p560.fkine(q);
% 将世界坐标系下的速度转换为T6坐标系
>> d6X = tr2jac(T6) * vel;
% 计算关节速度
>> qvel = Ji *d6X;
% 可以通过Jacobian的秩来描述
>> rank( p560.jacobn(qr) )
% 对于特定的一组关节角，其奇异性可以衡量为：
>> svd( jacobn(p560, qr) )
% 两种用于描述可操作性的方法
>> p560.maniplty(q, 'yoshikawa')
>> p560.maniplty(q, 'asada') %计入了惯量