Modern_Robotics/packages/Matlab/mr/MatrixLog6.m

function expmat = MatrixLog6(T)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a transformation matrix T in SE(3).
% Returns the corresponding se(3) representation of exponential 
% coordinates.
% Example Input:
% 
% clear; clc;
% T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
% expmat = MatrixLog6(T)
% 
% Output:
% expc6 =
%         0         0         0         0
%         0         0   -1.5708    2.3562
%         0    1.5708         0    2.3562
%         0         0         0         0

[R, p] = TransToRp(T);
if NearZero(norm(R - eye(3)))
    expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
else
    acosinput = (trace(R) - 1) / 2;
    if acosinput > 1
        acosinput = 1;
    elseif acosinput < -1
        acosinput = -1;
    end
    theta = acos(acosinput);
    omgmat = MatrixLog3(R);
    expmat = [ omgmat, (eye(3) - omgmat / 2 ...
                        + (1 / theta - cot(theta / 2) / 2) ...
                          * omgmat * omgmat / theta) * p;
              0, 0, 0, 0];
end
end