%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% rpy2jac and eul2jac verification
%
clear
syms phi
syms theta
syms psi
assume(phi,'real')
assume(theta,'real')
assume(psi,'real')
vphi=0.1
vtheta=0.2
vpsi=0.3


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ZYZ-Euler angles
%R =rz(phi)*ry(theta)*rx(psi)
%rotation of (phi, theta,psi) about mobile axes ZYZ.

%Elementary rotations
rzphi=[cos(phi) -sin(phi) 0; sin(phi) cos(phi) 0; 0 0 1]
rytheta = [cos(theta) 0 sin(theta);0 1 0;-sin(theta) 0 cos(theta)]
rzpsi=[cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]

% The rotation matrix becomes:
eul1 = rzphi*rytheta*rzpsi

% Verification using the eul2r function
eul2 = eul2r(phi,theta,psi)

% Check with numerical values:
double(subs(eul1,{phi,theta,psi},{vphi,vtheta,vpsi}))
eul2r(vphi,vtheta,vpsi)

%verification of the T matrix that relates the angular velocity w.r.t. the
%derivative of the rpy angles
Teul1 = [[0 0 1]' rzphi*[0 1 0]' rzphi*rytheta*[0 0 1]']
Teul2 = eul2jac(phi, theta, psi)

% Check with numerical values:
double(subs(Teul1,{phi,theta,psi},{vphi,vtheta,vpsi}))
eul2jac(0.1,0.2,0.3)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ROLL-PITCH-YAW
% Rotation about fixed axis: first x, then y, finally z, i.e.:
%        R = rz(phi)*ry(theta)*rx(psi)
%
%   Siciliano nomenclature:     R = rz(roll)*ry(pitch)*rx(yaw)
%   Corke nomenclature:         R = rz(yaw)*ry(pitch)*rx(roll)
%
%Elementary rotations.
%Using Sicilano book nomenclature:
% Rotation about x an angle psi (yaw)
% Rotation about y an angle theta (pitch)
% Rotation about z an angle phi (roll)
rxpsi=[1 0 0; 0 cos(psi) -sin(psi);0 sin(psi) cos(psi)]
rytheta = [cos(theta) 0 sin(theta);0 1 0;-sin(theta) 0 cos(theta)]
rzphi=[cos(phi) -sin(phi) 0; sin(phi) cos(phi) 0; 0 0 1]

% The rotation matrix becomes:
rpy1 = rzphi*rytheta*rxpsi

% Verification using the rpy2r function.
% This function has as parameters the roll, pitch and yaw angles using
% Corke nomenclature. 
% To fit Siciliano nomenclature we interchange the order:
rpy2 = rpy2r(psi,theta,phi)

% Check with numerical values:
double(subs(rpy1,{phi,theta,psi},{vphi,vtheta,vpsi}))
rpy2r(vpsi,vtheta,vphi)

%verification of the T matrix that relates the angular velocity w.r.t. the
%derivative of the rpy angles
Trpy1 = [rzphi*rytheta*[1 0 0]' rzphi*[0 1 0]' [0 0 1]']
Trpy2 = rpy2jac(psi,theta,phi)
% Check with numerical values:
double(subs(Trpy1,{phi,theta,psi},{vphi,vtheta,vpsi}))
rpy2jac(vpsi,vtheta,vphi)

