% CINEMATICA INVERSA

% COORDENADAS
p=[0 0 0];
l1=0.5; l2=0.35; l3=0.2; l4=0.4; l5=0.25;

% POSICION INICIAL DEL BRAZO

q0 = [0 pi/2 0.3 pi/2];

% ECUACIONES
R=sqrt(p(1)^2+p(2)^2);
cq2=(R^2-l2^2-l4^2)/(2*l2*l3);
sq21=sqrt(1-cq2);
sq22=-sqrt(1-cq2);
q2i1=atan2(sq21,cq2);
q2i2=atan2(sq22,cq2);
beta=atan2(p(2),p(1));
alphay1=l4*sq21;
alphay2=l4*sq22;
alphax=l2+(l3*cq2);
alpha1=atan2(alphay1,alphax);
alpha2=atan2(alphay2,alphax);
q1i1=beta-alpha1;
q1i2=beta-alpha2;
q3i=l1+l3-l5-p(3)-q0(3);
q4i1=q0(4)+q1i1+q2i1;
q4i2=q0(4)+q1i2+q2i2;

% RESULTADO
qi1=[q1i1*180/pi q2i1 q3i q4i1*180/pi]
qi1=[q1i2*180/pi q2i2 q3i q4i2*180/pi]

% CINEMATICA DIRECTA
% ANGULOS
q = [0 0 0.3 pi/2];
q1=q(1);
q2=q(2);
q3=q(3);
q4=q(4);

% a alpha d theta
PD=[l2 0 l1 q1
l4 pi l3 q2
0 0 q3 0
0 0 l5 q4];

% MATRIZ DE TRANSFORMACION HOMOGENEA

% MATRIZ DE 0 A 1
B011=[cos(PD(1,4)) -sin(PD(1,4))*cos(PD(1,2)) sin(PD(1,4))*sin(PD(1,2))
cos(PD(1,4))*(PD(1,1))];
B012=[sin(PD(1,4)) cos(PD(1,4))*cos(PD(1,2)) -cos(PD(1,4))*sin(PD(1,2))
sin(PD(1,4))*(PD(1,1))];
B013=[0 sin(PD(1,2)) cos(PD(1,2)) (PD(1,3))];
B014=[0 0 0 1];
B01=[B011;B012;B013;B014];

% MATRIZ DE 1 A 2
B121=[cos(PD(2,4)) -sin(PD(2,4))*cos(PD(2,2)) sin(PD(2,4))*sin(PD(2,2))
cos(PD(2,4))*(PD(2,1))];
B122=[sin(PD(2,4)) cos(PD(2,4))*cos(PD(2,2)) -cos(PD(2,4))*sin(PD(2,2))
sin(PD(2,4))*(PD(2,1))];
B123=[0 sin(PD(2,2)) cos(PD(2,2)) (PD(2,3))];
B124=[0 0 0 1];
B12=[B121;B122;B123;B124];

% MATRIZ DE 2 A 3
B231=[cos(PD(3,4)) -sin(PD(3,4))*cos(PD(3,2)) sin(PD(3,4))*sin(PD(3,2))
cos(PD(3,4))*(PD(3,1))];
B232=[sin(PD(3,4)) cos(PD(3,4))*cos(PD(3,2)) -cos(PD(3,4))*sin(PD(3,2))
sin(PD(3,4))*(PD(3,1))];
B233=[0 sin(PD(3,2)) cos(PD(3,2)) (PD(3,3))];
B234=[0 0 0 1];
B23=[B231;B232;B233;B234];

% MATRIZ DE 3 A 4
B341=[cos(PD(4,4)) -sin(PD(4,4))*cos(PD(4,2)) sin(PD(4,4))*sin(PD(4,2))
cos(PD(4,4))*(PD(4,1))];
B342=[sin(PD(4,4)) cos(PD(4,4))*cos(PD(4,2)) -cos(PD(4,4))*sin(PD(4,2))
sin(PD(4,4))*(PD(4,1))];
B343=[0 sin(PD(4,2)) cos(PD(4,2)) (PD(4,3))];
B344=[0 0 0 1];
B34=[B341;B342;B343;B344];

% RESULTADO
S4=B01*B12*B23*B34