T1 = se2(1, 2, 30*pi/180)
axis([0 5 0 5]);
figure(1)
trplot(T1, 'frame', '1', 'color', 'b')

T2 = se2(2, 1, 0)

hold on
figure(2)
trplot2(T2, 'frame', '2', 'color', 'r');

T3 = T1*T2
figure(3)
trplot2(T3, 'frame', '3', 'color', 'g');

T4 = T2*T1;
figure(4)
trplot2(T4, 'frame', '4', 'color', 'c');

P = [3 ; 2 ];

figure(5)
plot_point(P, '*');


P1 = inv(T1) * [P; 1]

h2e( inv(T1) * e2h(P) )

homtrans( inv(T1), P)

P2 = homtrans( inv(T2), P)

R = rotx(pi/2)


figure(6)
trplot(R)




rotx(pi/2) * roty(pi/2)

trplot(ans)


roty(pi/2)*rotx(pi/2)

R = rotz(0.1) * roty(0.2) * rotz(0.3);

R = eul2r(0.1, 0.2, 0.3)

gamma = tr2eul(R)

R = eul2r(0.1 , -0.2, 0.3)

tr2eul(R)

eul2r(ans)

R = eul2r(0.1 , 0, 0.3)
tr2eul(R)

R = rpy2r(0.1, 0.2, 0.3)

gamma = tr2rpy(R)

a = [1 0 0]';
o = [0 1 0]';
oa2r(o, a)

R = rpy2r(0.1 , 0.2, 0.3);

[theta, v] = tr2angvec(R)

[v,lambda] = eig(R)

R = angvec2r(pi/2, [1 0 0])

q = Quaternion( rpy2tr(0.1, 0.2, 0.3)  )

q.norm

q = q * q;

q.inv()

q*q.inv()

q/q

% q.r

q.plot()

Quaternion([1 2 3])

q*[1 0 0]'

T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)

trplot(T)

t2r(T)

transl(T)'
