梁政:机器人工程师进阶之路(五)雅可比矩阵和微分运动
空间雅可比(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源码请参考
评论(0)
您还未登录,请登录后发表或查看评论