T1 = se2(1, 2, 30*pi/180)

clf

axis([0 5 0 5]);
trplot(T1, 'frame', '1', 'color', 'b')

T2 = se2(2, 1, 0)

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

T3 = T1*T2

trplot2(T3, 'frame', '3', 'color', 'g');

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

P = [3 ; 2 ];

plot_point(P, '*');

iprint('eg_frames_2d');
clf

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

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

homtrans( inv(T1), P)

P2 = homtrans( inv(T2), P)

R = rotx(pi/2)

clf

trplot(R)

iprint('rx_p2');

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

iprint('rx_p2_ry_p2');

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)'