Codes: MODERN ROBOTICS Ch.3_Expo. Coods.基础代码实现

%%1 Transform omega to so3 matrix
% W for skew-symmetirc matirx
% w for omega, angular velocity
function [so3mat]=vecToso3r(omg)
so3mat=[[0, -omg(3), omg(2)];
        [omg(3), 0, -omg(1)];
        [-omg(2), omg(1), 0]];
end
%------------EOF-------------

%%2 输入6-vector，输出se3
function S_mat = VecTose3r(S)
w = S(1:3);
v = S(4:6);
w_mat = vecToso3r(w);
S_mat = [w_mat, v; 0, 0, 0, 0];
end
%------------EOF-------------

%%3 so3 to Angular velocity
function [omg]=so3ToVecr(so3mat)
omg(1)=so3mat(3,2);
omg(2)=so3mat(1,3);
omg(3)=so3mat(2,1);
end
%------------EOF-------------

%%4 根据se3矩阵求6-vector
function s=se3ToVecr(S)
W = S(1:3, 1:3);
v = S(1:3, 4);
w = so3ToVecr(W);
s = [w'; v];
end
%------------EOF-------------

%%5 从wtheta中分离出w与theta （开始忘了讨论了！）
function [w, theta] = AxisAng3r(wtheta)
% |w|=1必然成立，故若wtheta=0，则theta=0，w无法求解
if NearZero(norm(wtheta))
    theta = 0;
    w = 'cannot find';
% theta!=0，可以作为分母除
else
    theta = norm(wtheta);
    w = wtheta./theta;
end
%------------EOF-------------

%%6 从Stheta中分离出S与theta（也是忘了讨论！）
function [S, theta] = AxisAng6r(Stheta)
wtheta = Stheta(1:3);
vtheta = Stheta(4:6);
% 如果|wtheta|=0,可能由于w=0或theta=0，需继续判断
if NearZero(norm(wtheta))
    % 假设theta!=0，那么w=0，则|v|=1，则|vtheta|将求出theta值
    theta = norm(vtheta);
    % 如果求出的theta值为0，表明wtheta=0是由theta=0引起的，这时螺旋轴S无法找到
    if NearZero(theta)
        theta = 0;
        S = 'cannot find';
    % 如果theta值不为0，表明w=0，这时theta！=0即可作为分母除
    else
        w = zeros(3,1);% 这句没有作用，只是表明这种情况下是w=0
        S = Stheta./theta;
    end
% 如果|wtheta|！=0，则说明|w|=1且theta!=0，直接作分母除theta即可    
else
    theta = norm(wtheta);
    S = Stheta./theta;
end
%------------EOF-------------

%%7 Matrix exponential for so3
% 根据wtheta_mat，求取旋转矩阵
function [R]=MatExp3r(wtheta_mat)
    % 求出w与theta
    wtheta = so3ToVecr(wtheta_mat);
    theta = norm(wtheta);
    w = wtheta./theta;
    % 求w的矩阵表示w_mat
    w_mat = vecToso3r(w);
    % 求矩阵指数映射
    R= eye(3)+sin(theta).* w_mat+(1-cos(theta)).* w_mat^2;
end
%------------EOF-------------

%%8 根据指数坐标的矩阵表示Stheta_mat，求变换矩阵T（忘了分类w=0的情况！）
function T = MatExp6r(Stheta_mat)
Stheta = se3ToVecr(Stheta_mat);
wtheta = Stheta(1:3);
vtheta = Stheta(4:6);
if NearZero(norm(wtheta))
    T=[eye(3), vtheta;
        zeros(1,3), 1];
else
    % 计算角速度指数坐标wtheta_mat的指数映射
    wtheta_mat = vecToso3r(wtheta);
    R = MatExp3r(wtheta_mat);
    % 分离螺旋轴指数坐标Stheta，求v向量，并求w向量的矩阵表示w_mat，并用于计算Gv
    [S,theta] = AxisAng6r(Stheta);
    w = S(1:3);
    v = S(4:6);
    w_mat = vecToso3r(w);
    Gv = (eye(3).*theta+(1-cos(theta)).*w_mat+(theta-sin(theta)).*w_mat^2)*v;
    % 将R、Gv、eye(1,3)、1拼接成变换矩阵T
    T=[R,       Gv;
        eye(1,3), 1];
end
end
%------------EOF-------------

%%9 矩阵对数算法，根据旋转矩阵R，求旋转指数坐标的矩阵表示wtheta_mat
% R to so3mat, namely, so3omghat.*theta
function wtheta_mat = MatLog3r(R)
    if isequal(R,eye(3))
        theta = 0;
        fprintf('omghat is undefined');
        wtheta_mat = zeros(3);
    elseif trace(R)==-1
        theta = pi;
        if ~NearZero(1+R(3,3))
            w =(1/sqrt(2*(1+R(3,3))))...
                    .*[R(1,3);R(2,3);1+R(3,3)]
        elseif ~NearZero(1+R(2,2))
            w =(1/sqrt(2*(1+R(2,2))))...
                    .*[R(1,2);1+R(2,2);R(3,2)]
        elseif ~NearZero(1+R(3,3))
            w =(1/sqrt(2*(1+R(1,1))))...
                    .*[1+R(1,1);R(2,1);R(3,1)]
        end
        w_mat = vecToso3r(w);
        wtheta_mat = w_mat.*theta;
    else
        theta = acos((trace(R)-1)/2);
        w_mat = (1/(2*sin(theta))).*(R-R');
        wtheta_mat = w_mat.*theta;
    end
end
%------------EOF-------------

%%10 根据变换矩阵T,求螺旋指数坐标stheta_mat
function stheta_mat = MatLog6r(T)
    [R,p] = TransToRpr(T);
    if isequal(eye(3), R)
        w = zeros(3,1);
        theta = norm(p);
        v = p./theta;
    else
        wtheta_mat = MatLog3r(R);
        wtheta = so3ToVecr(wtheta_mat);
        theta = norm(wtheta);
        w = (wtheta)'./theta;
        w_mat = vecToso3r(w);
        v = ((1/theta).*eye(3)-1/2.*w_mat+(1/theta-1/2*cot(theta/2)).*w_mat^2)*p;
    end
    s = [w; v];
    stheta = s.*theta;
    stheta_mat = VecTose3r(stheta);
end
%------------EOF-------------

%%11 qsh螺旋轴变换为单位螺旋轴
function S = ScrewToAxisr(q,s,h)
% Takes q: a point lying on the screw axis, column vector
%       s: a unit vector in the direction of the screw axis, column vector
%       h: the pitch of the screw axis.
S = [s; cross(s,-q)+h.*s];
end
%------------EOF-------------

%%12 计算变换矩阵T的伴随映射
function AdT=Adjointr(T)
[R,p] = TransToRpr(T);
P = vecToso3r(p);
AdT=[R, eye(3);
    P*R,    R];
end
%------------EOF-------------

　　All codes are written in function format, and pay attention to codes #5、6、8、9、11  !