In [11]:

    

function S = skew(v)
    S = [0, -v(3), v(2); v(3), 0, -v(1); -v(2), v(1), 0];
end
S = skew([1,2,3])


    

    




S =

   0  -3   2
   3   0  -1
  -2   1   0

In [12]:

    

function v = vex(S)
    v = [S(3,2), S(1,3), S(2,1)];
end
v = vex(S)


    

    




v =

   1   2   3

In [13]:

    

function R = exp(w)
    th = norm(w);
    K = skew(w / th);
    R = eye(3) + sin(th) * K + (1 - cos(th)) * K * K;
end


    

In [14]:

    

function R = rotz(th)
    ct = cos(th);
    st = sin(th);
    R = [ct, -st, 0; st, ct, 0; 0, 0, 1];
end


    

In [15]:

    

function R = roty(th)
    ct = cos(th);
    st = sin(th);
    R = [ct, 0, st; 0, 1, 0; -st, 0, ct];
end


    

In [16]:

    

function R = rotx(th)
    ct = cos(th);
    st = sin(th);
    R = [1, 0, 0; 0, ct, -st; 0, st, ct];
end


    

In [17]:

    

R = rotx(pi/6)


    

    




R =

   1.00000   0.00000   0.00000
   0.00000   0.86603  -0.50000
   0.00000   0.50000   0.86603

In [18]:

    

R = exp([pi/6,0,0])


    

    




R =

   1.00000   0.00000   0.00000
   0.00000   0.86603  -0.50000
   0.00000   0.50000   0.86603

In [19]:

    

function w = log(R)
    th = acos((trace(R) - 1) / 2);
    w = ([R(3,2) - R(2,3), R(1,3) - R(3,1), R(2,1) - R(1,2)] / (2 * sin(th))) * th;
end


    

In [21]:

    

w = [0, pi/6,0]
log(exp(w))


    

    




w =

   0.00000   0.52360   0.00000

ans =

   0.00000   0.52360   0.00000

In [ ]:

    




    

In [ ]: