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')    %计入了惯量