梁政:机器人工程师进阶之路(五)雅可比矩阵和微分运动

空间雅可比(Space Jacobian)

已知末端位姿为

[公式]

对末端位置求时间导数

[公式]

[公式]

空间运动旋量

[公式]

[公式]

那么可以写为矩阵形式

[公式]

其中 [公式] 

物体雅可比(Body Jacobian)

和空间雅可比相同,已知末端位姿矩阵

[公式]

[公式]

[公式]

那么可以写为矩阵形式

[公式]

其中 [公式] 

两者之间的转换关系

[公式]

H1=105.03;W1=80.09;L1=174.42;W2=84.51;L2=174.42;W3=80.09;H2=80.09;W4=44.36;
M=[-1 0 0 L1+L2;0 0 1 W1-W2+W3+W4;0 1 0 H1-H2;0 0 0 1]  % Home Configuration of End-Effector 
%% Spatial Twist
Slist = [[0; 0; 1; 0; 0; 0], ...
        [0; 1; 0; -H1; 0; 0],...
        [0; 1; 0; -H1; 0; L1],...
        [0; 1; 0; -H1; 0; L1+L2],...
        [0; 0; -1; -W1+W2-W3; L1+L2;0],...
        [0; 1; 0; H2-H1; 0; L1+L2]];   %The joint screw axes in the space frame
thetalist = [pi/6; pi/6; pi/6; pi/6; pi/6; pi/6];

Ts=FKinSpace(M,Slist,thetalist);
Js=JacobianSpace(Slist,thetalist)
%% Body Twist
Blist = [[0; 1; 0; W1-W2+W3+W4; 0; L1+L2], ...
        [0; 0; 1; H2; -L1-L2; 0],...
        [0; 0; 1; H2; -L2; 0],...
        [0 ;0; 1; H2; 0; 0],...
        [0; -1; 0; -W4; 0;0],...
        [0; 0; 1; 0; 0; 0]];   %The joint screw axes in the end-effector frame
thetalist = [pi/6; pi/6; pi/6; pi/6; pi/6; pi/6];

Tb=FKinBody(M,Blist,thetalist);
Jb=JacobianBody(Blist,thetalist)
%%
Jss=Ajoint(Ts)*Jb
Jbb=Ajoint(Tb)*Js
%%
function T = FKinSpace(M, Slist, thetalist)
T = M;
for i = size(thetalist): -1: 1
    T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
end
end
%%
function T = FKinBody(M, Blist, thetalist)
T = M;
for i = 1: size(thetalist)
    T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i)));
end
end
%%
function Js = JacobianSpace(Slist, thetalist)
Js = Slist;
T = eye(4);
for i = 2: length(thetalist)
    T = T * MatrixExp6(VecTose3(Slist(:, i - 1) * thetalist(i - 1)));
	Js(:, i) = Adjoint(T) * Slist(:, i);
end
end
%%
function Jb = JacobianBody(Blist, thetalist)
Jb = Blist;
T = eye(4);
for i = length(thetalist) - 1: -1: 1   
    T = T * MatrixExp6(VecTose3(-1 * Blist(:, i + 1) * thetalist(i + 1)));
	Jb(:, i) = Adjoint(T) * Blist(:, i);
end
end
%%
function T = MatrixExp6(se3mat)
omgtheta = so3ToVec(se3mat(1: 3, 1: 3));
if NearZero(norm(omgtheta))
    T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1];
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = se3mat(1: 3, 1: 3) / theta; 
    T = [MatrixExp3(se3mat(1: 3, 1: 3)), ...
         (eye(3) * theta + (1 - cos(theta)) * omgmat ...
          + (theta - sin(theta)) * omgmat * omgmat) ...
            * se3mat(1: 3, 4) / theta;
         0, 0, 0, 1];
end
end
%%
function se3mat = VecTose3(V)
se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0];
end
%%
function so3mat = VecToso3(omg)
so3mat = [0, -omg(3), omg(2); omg(3), 0, -omg(1); -omg(2), omg(1), 0];
end
%%
function omg = so3ToVec(so3mat)
omg = [so3mat(3, 2); so3mat(1, 3); so3mat(2, 1)];
end
%%
function judge = NearZero(near)
judge = norm(near) < 1e-6;
end
%%
function [omghat, theta] = AxisAng3(expc3)
theta = norm(expc3);
omghat = expc3 / theta;
end
%%
function  R = MatrixExp3(so3mat)
omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
    R = eye(3);
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = so3mat / theta;
    R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end
%%
function AdT = Adjoint(T)
[R, p] = TransToRp(T);
AdT = [R, zeros(3); VecToso3(p) * R, R];
end
%%
function [R, p] = TransToRp(T)
R = T(1: 3, 1: 3);
p = T(1: 3, 4);
end

matlab源码请参考