function y=RPY(M)
phi=M(1);
theta=M(2);
psi=M(3);

y=rot3z(phi)*rot3y(theta)*rot3x(psi);




