%% Simulacion Hexapod 2021
%Rodrigo Figueroa 
%Gonzalo Palarea

%% Matriz DH
d = [-1.72, 0];
a = [4.53, 6.47];
cfg = [0, 0]; % configuracion de prueba
%syms  q1 q2 % Creamos las variables para los angulos
q1 = cfg(1);
q2 = cfg(2);
theta = [q1, q2];
alfa = [pi/2,0]; 
A_1 = genDHmatrix(theta(1),d(1),a(1),alfa(1)); %Matriz DH del Primer eslabon
A_2= genDHmatrix(theta(2),d(2),a(2),alfa(2)); %Matriz DH del Segundo Eslabon
DH = A_1*A_2;
DH_2 = getFK(DH);
%Transformaciones de base por pierna
T_B1 = trotz(45)*transl(4.96,7.69,0)*trotx(0);
T_B2 = trotz(-45)*transl(-4.96,7.69,0)*trotx(0);
T_B3 = trotz(0)*transl(4.96,0,0)*trotx(0);
T_B4 = trotz(90)*transl(4.96,0,0)*trotx(0);
T_B5 = trotz(135)*transl(4.96,-7.69,0)*trotx(0);
T_B6 = trotz(-135)*transl(-4.96,-7.69,0)*trotx(0);
%% Cinematica Directa
P1 = getFK(T_B1*DH_2);
P2 = getFK(T_B2*DH_2);
P3 = getFK(T_B3*DH_2);
P4 = getFK(T_B4*DH_2);
P5 = getFK(T_B5*DH_2);
P6 = getFK(T_B6*DH_2);

%% Cinematica diferencial
q = [0;0];
L1 = Revolute('d',d(1),'a',a(1),'alpha',pi/2,'offset',0);
L2 = Revolute('d',d(2),'a',a(2),'alpha',0,'offset',0);
Hexapod=SerialLink([L1,L2],'name','Hexapod');
Jacobiano_hexapod= Hexapod.jacob0(q);  
%% Cinematica inversa:
q_deseada = [pi;1.2];
T1 = ur5FK(q_deseada);
[q,Q] = ur5IK(T1,[0; 0], 'levenberg',Jacobiano_hexapod);
plot(Q);
%% Cinematica Directa:
%K = [B_T_E(1,4); B_T_E(2,4)] % Cinematica Directa
%% Animacion
%Dimensiones del robot
L = 10;
W = 10;
L1 = Revolute('d',d(1),'a',a(1),'alpha',pi/2,'offset',0);
L2 = Revolute('d',d(2),'a',a(2),'alpha',0,'offset',0);
patas(1)=SerialLink([L1,L2],'name','pata1','base',SE3(0,0,0)*SE3.Rz(pi/2));
q0 = zeros([1 2]);
patas(2)=SerialLink([L1,L2],'name','pata2','base',SE3(-L,0,0)*SE3.Rz(pi/2));
patas(3)=SerialLink([L1,L2],'name','pata3','base',SE3(-L/2,0,0)*SE3.Rz(pi/2));
patas(4)=SerialLink([L1,L2],'name','pata4','base',SE3(-L,-W,0)*SE3.Rz(-pi/2));
patas(5)=SerialLink([L1,L2],'name','pata5','base',SE3(-L/2,-W,0)*SE3.Rz(-pi/2));
patas(6)=SerialLink([L1,L2],'name','pata6','base',SE3(0,-W,0)*SE3.Rz(-pi/2));
patas(1).teach()
patas(1).plot(q0);
hold on;
patas(2).plot(q0);
patas(3).plot(q0);
patas(4).plot(q0);
patas(5).plot(q0);
patas(6).plot(q0);
xlim([-20 20])
ylim([-20 20])
patas(1).teach();
patas(2).teach();
