% x(1) = sin Phi
% x(2) = sin Theta
% x(3) = sin Psi
% x(4) = cos Phi
% x(5) = cos Theta
% x(6) = cos Psi

function F = Calc_Trans_RB_R2(x, leg_v2, l_2)
F = [x(6) * x(4) - x(3) * x(5) * x(1)- (leg_v2(1,1)/ l_2);
     -x(6) * x(1) - x(3) * x(5) * x(4) - (leg_v2(2,1)/ l_2);
     x(3) * x(2) - (leg_v2(3,1)/ l_2);
     x(1)^2 + x(4)^2 - 1;
     x(2)^2 + x(5)^2 - 1;
     x(3)^2 + x(6)^2 - 1];