%QUEST�O 1:

disp('RESPOSTA QUEST�O 1:')

T = [0.8001 -0.3314 0.5 0.6;
0.5811 0.635 -0.509 0.3;
-0.1488 0.6978 0.7006 1;
0 0 0 0];
%Cria��o dos simb�licos:
psi = sym('psi') ;
phi = sym ('phi');
theta = sym('theta');
%Sequencia de Rota��o determinada:
RPY = trotx(psi)*troty(theta)*trotz(phi);
%A partir dos valores obtidos encontrei os angulos:

theta = asin(0.5) %Theta

phi = asin(-0.3314/(-cos(theta)))%Phi
psi = asin(-0.509/(-cos(theta)))%Psi
disp('------------------------------------')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%QUEST�O 2:
disp('RESPOSTA QUEST�O 2:')
PX = 1.21;
PY = 1.411;
PZ = 0;
Lx=1;
Ly=1;
L(1) = Link([0 0 Lx 0]); %Cria��o da primeira eq
L(2) = Link([0 0 Ly 0]); %cria��o da segunda eq

Robot = SerialLink(L);
Robot.name='planar 2 link';
T = [1 0 0 PX; 0 1 0 PY; 0 0 1 PZ;0 0 0 1]; %Matriz de transforma��o
q0 = [0 0]; %posicao inicial
M = [1 1 0 0 0 0];
J = Robot.ikine(T,q0,M)%formula cinematica inversa

Q1= J(1) %junta

Q2= J(2) %junta
Robot.plot(J*pi/180); %cria grafico 3D