%% Cinemática diferencial

%% Full Kinematics - ZZ

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_zz

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 123 * 1e-3;
L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 pi
        th4 0 0 -pi/2
        th5 L4 0 0];
R7 = [0 0 1 0
      0 -1 0 0
      1 0 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*A5*A6*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
jacobian(1,4) = diff(rT6(1,4),th4);
jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(2,4) = diff(rT6(2,4),th4);
jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
jacobian(3,4) = diff(rT6(3,4),th4);
jacobian(3,5) = diff(rT6(3,4),th5);

time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
    th5 = humanoid_state(a+1,7);
    th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    th5 = humanoid_state(a,7);
    th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th5 = humanoid_state(n,7);
    th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n) speed_joint_15(n) speed_joint_16(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n) accel_joint_15(n) accel_joint_16(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,1,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,3,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,2,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)


%% Full Kinematics - XX

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_xx

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 123 * 1e-3;
L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 pi
        th4 0 0 -pi/2
        th5 L4 0 0];
R7 = [0 0 1 0
      0 -1 0 0
      1 0 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*A5*A6*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
jacobian(1,4) = diff(rT6(1,4),th4);
jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(2,4) = diff(rT6(2,4),th4);
jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
jacobian(3,4) = diff(rT6(3,4),th4);
jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
    th5 = humanoid_state(a+1,7);
    th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    th5 = humanoid_state(a,7);
    th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th5 = humanoid_state(n,7);
    th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n) speed_joint_15(n) speed_joint_16(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n) accel_joint_15(n) accel_joint_16(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Flexão joelho - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,1,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Flexão joelho - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,3,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -3 3])

figure
hold on , grid on
title('Aceleração linear ZZ - Flexão joelho - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,2,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

%% Full Kinematics - YY

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_yy

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 123 * 1e-3;
L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 pi
        th4 0 0 -pi/2
        th5 L4 0 0];
R7 = [0 0 1 0
      0 -1 0 0
      1 0 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*A5*A6*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
jacobian(1,4) = diff(rT6(1,4),th4);
jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(2,4) = diff(rT6(2,4),th4);
jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
jacobian(3,4) = diff(rT6(3,4),th4);
jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
    th5 = humanoid_state(a+1,7);
    th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    th5 = humanoid_state(a,7);
    th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th5 = humanoid_state(n,7);
    th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n) speed_joint_15(n) speed_joint_16(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n) accel_joint_15(n) accel_joint_16(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Movimento Lateral - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,1,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Movimento Lateral - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,3,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -3 3])

figure
hold on , grid on
title('Aceleração linear ZZ - Movimento Lateral - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,2,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% NEXT KINEMATICS 6 %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Sensor 6 Kinematics - ZZ

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_zz

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 22 * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 0];

R7 = [0 0 1 0
      1 0 0 0
      0 1 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
rT6 = A1*A2*A3*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,2,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,3,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Agac hamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,1,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% NEXT KINEMATICS %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% Sensor 6 Kinematics - XX

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_xx

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 22 * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 0];
R7 = [0 0 1 0
      1 0 0 0
      0 1 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
rT6 = A1*A2*A3*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
%     th3 = humanoid_state(a+1,5);
%     th5 = humanoid_state(a+1,7);
%     th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
%     th3 = humanoid_state(a,5);
%     th5 = humanoid_state(a,7);
%     th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
% speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
% speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
% speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
% accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
% accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
% accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
%     th3 = humanoid_state(n,5);
%     th5 = humanoid_state(n,7);
%     th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Flexão joelho - cinemática & sensor 6','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,2,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Flexão joelho - cinemática & sensor 6','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,3,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Flexão joelho - cinemática & sensor 6','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,1,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)



%% Sensor 6 Kinematics - YY

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_yy

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 22 * 1e-3;
% L3 = 123 * 1e-3;
% L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 0];
%         th3 L3 0 pi
%         th4 0 0 -pi/2
%         th5 L4 0 0];
R7 = [0 0 1 0
      1 0 0 0
      0 1 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
% A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
% A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
% A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
% jacobian(1,3) = diff(rT6(1,4),th3);
% jacobian(1,4) = diff(rT6(1,4),th4);
% jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
% jacobian(2,3) = diff(rT6(2,4),th3);
% jacobian(2,4) = diff(rT6(2,4),th4);
% jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
% jacobian(3,3) = diff(rT6(3,4),th3);
% jacobian(3,4) = diff(rT6(3,4),th4);
% jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
%     th3 = humanoid_state(a+1,5);
%     th5 = humanoid_state(a+1,7);
%     th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
%     th3 = humanoid_state(a,5);
%     th5 = humanoid_state(a,7);
%     th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
% speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
% speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
% speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
% accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
% accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
% accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
%     th3 = humanoid_state(n,5);
%     th5 = humanoid_state(n,7);
%     th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Movimento lateral - cinemática & sensor 6','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,2,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Movimento lateral - cinemática & sensor 6','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,3,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Movimento lateral - cinemática & sensor 6','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,6),sensors_data(:,1,6),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

%% Sensor 5 Kinematics - ZZ

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_zz

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 42 * 1e-3;
% L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 0];
%         th4 0 0 -pi/2
%         th5 L4 0 0];
R7 = [0 0 1 0
      -1 0 0 0
      0 -1 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
% A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
% A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*A4*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
% jacobian(1,4) = diff(rT6(1,4),th4);
% jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
% jacobian(2,4) = diff(rT6(2,4),th4);
% jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
% jacobian(3,4) = diff(rT6(3,4),th4);
% jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
%     th5 = humanoid_state(a+1,7);
%     th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
%     th5 = humanoid_state(a,7);
%     th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
% speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
% speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
% accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
% accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
%     th5 = humanoid_state(n,7);
%     th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Agachamento - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,2,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Agachamento - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,3,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Agachamento - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,1,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)


%% Sensor 5 Kinematics - XX

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_xx

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 42 * 1e-3;
% L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 0];
%         th4 0 0 -pi/2
%         th5 L4 0 0];
R7 = [0 0 1 0
      -1 0 0 0
      0 -1 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
% A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
% A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*A4*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
% jacobian(1,4) = diff(rT6(1,4),th4);
% jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
% jacobian(2,4) = diff(rT6(2,4),th4);
% jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
% jacobian(3,4) = diff(rT6(3,4),th4);
% jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
%     th5 = humanoid_state(a+1,7);
%     th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
%     th5 = humanoid_state(a,7);
%     th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
% speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
% speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
% accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
% accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
%     th5 = humanoid_state(n,7);
%     th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Flexão joelho - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,2,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Flexão joelho - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,3,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Flexão joelho - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,1,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

%% Sensor 5 Kinematics - YY

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_yy

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 42 * 1e-3;
% L4 = 20  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 0];
%         th4 0 0 -pi/2
%         th5 L4 0 0];
R7 = [0 0 1 0
      -1 0 0 0
      0 -1 0 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
% A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
% A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
rT6 = A1*A2*A3*A4*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
% jacobian(1,4) = diff(rT6(1,4),th4);
% jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
% jacobian(2,4) = diff(rT6(2,4),th4);
% jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
% jacobian(3,4) = diff(rT6(3,4),th4);
% jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
%     th5 = humanoid_state(a+1,7);
%     th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
%     th5 = humanoid_state(a,7);
%     th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
% speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
% speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
% accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
% accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
%     th5 = humanoid_state(n,7);
%     th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Movimento lateral - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,2,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Movimento lateral - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,3,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Movimento lateral - cinemática & sensor 5','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,5),sensors_data(:,1,5),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

















%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




%% Full Kinematics - ZZ

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_zz

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 123 * 1e-3;
L4 = 60  * 1e-3;
L5 = 105  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 pi
        th4 0 0 -pi/2
        th5+pi/2 L4 0 pi/2
        0 0 L5 0];
R7 = [0 1 0 0
      -1 0 0 0
      0 0 1 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
A7 = rotz(DH(7,1))*trans(DH(7,2),0,DH(7,3))*rotx(DH(7,4));
rT6 = A1*A2*A3*A5*A6*A7*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
jacobian(1,4) = diff(rT6(1,4),th4);
jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(2,4) = diff(rT6(2,4),th4);
jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
jacobian(3,4) = diff(rT6(3,4),th4);
jacobian(3,5) = diff(rT6(3,4),th5);

time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
    th5 = humanoid_state(a+1,7);
    th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    th5 = humanoid_state(a,7);
    th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th5 = humanoid_state(n,7);
    th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n) speed_joint_15(n) speed_joint_16(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n) accel_joint_15(n) accel_joint_16(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,1,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,3,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -5 5])

figure
hold on , grid on
title('Aceleração linear ZZ - Agachamento - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,2,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)


%% Full Kinematics - XX

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_xx

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 123 * 1e-3;
L4 = 60  * 1e-3;
L5 = 105  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 pi
        th4 0 0 -pi/2
        th5+pi/2 L4 0 pi/2
        0 0 L5 0];
R8 = [0 1 0 0
      -1 0 0 0
      0 0 1 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
A7 = rotz(DH(7,1))*trans(DH(7,2),0,DH(7,3))*rotx(DH(7,4));
rT6 = A1*A2*A3*A5*A6*A7*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
jacobian(1,4) = diff(rT6(1,4),th4);
jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(2,4) = diff(rT6(2,4),th4);
jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
jacobian(3,4) = diff(rT6(3,4),th4);
jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
    th5 = humanoid_state(a+1,7);
    th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    th5 = humanoid_state(a,7);
    th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th5 = humanoid_state(n,7);
    th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n) speed_joint_15(n) speed_joint_16(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n) accel_joint_15(n) accel_joint_16(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Flexão joelho - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,1,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Flexão joelho - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,3,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -3 3])

figure
hold on , grid on
title('Aceleração linear ZZ - Flexão joelho - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,2,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

%% Full Kinematics - YY

clc, clear all , close all

gyro_sensivity = 0.061035156; %Converte ADC em º/s
accel_sensivity = (1/256)*9.81; %COnverte ADC em m/s^2

%getting the servos_data & sensors_data
move_yy

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end

sensors_data(:,4,:) = sensors_data(:,4,:)*gyro_sensivity;
sensors_data(:,5,:) = sensors_data(:,5,:)*gyro_sensivity;
sensors_data(:,6,:) = sensors_data(:,6,:)*gyro_sensivity;
sensors_data(:,1,:) = sensors_data(:,1,:)*accel_sensivity;
sensors_data(:,2,:) = sensors_data(:,2,:)*accel_sensivity;
sensors_data(:,3,:) = sensors_data(:,3,:)*accel_sensivity;


L1 = 45  * 1e-3;
L2 = 132 * 1e-3;
L3 = 123 * 1e-3;
L4 = 60  * 1e-3;
L5 = 105  * 1e-3;

syms th1 th2 th3 th4 th5;
% theta i | L i | d i | alpha i |
DH = [pi/2 0 L1 pi/2
        th1+pi/2 0 0 pi/2
        th2 L2 0 pi
        th3 L3 0 pi
        th4 0 0 -pi/2
        th5+pi/2 L4 0 pi/2
        0 0 L5 0];
R8 = [0 1 0 0
      -1 0 0 0
      0 0 1 0
      0 0 0 1];
% AA = Tlink(DH(:,1),DH(:,2),DH(:,3),DH(:,4))
A1 = rotz(DH(1,1))*trans(DH(1,2),0,DH(1,3))*rotx(DH(1,4));
A2 = rotz(DH(2,1))*trans(DH(2,2),0,DH(2,3))*rotx(DH(2,4));
A3 = rotz(DH(3,1))*trans(DH(3,2),0,DH(3,3))*rotx(DH(3,4));
A4 = rotz(DH(4,1))*trans(DH(4,2),0,DH(4,3))*rotx(DH(4,4));
A5 = rotz(DH(5,1))*trans(DH(5,2),0,DH(5,3))*rotx(DH(5,4));
A6 = rotz(DH(6,1))*trans(DH(6,2),0,DH(6,3))*rotx(DH(6,4));
A7 = rotz(DH(7,1))*trans(DH(7,2),0,DH(7,3))*rotx(DH(7,4));
rT6 = A1*A2*A3*A5*A6*A7*R7;

jacobian(1,1) = diff(rT6(1,4),th1);
jacobian(1,2) = diff(rT6(1,4),th2);
jacobian(1,3) = diff(rT6(1,4),th3);
jacobian(1,4) = diff(rT6(1,4),th4);
jacobian(1,5) = diff(rT6(1,4),th5);
jacobian(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(2,4) = diff(rT6(2,4),th4);
jacobian(2,5) = diff(rT6(2,4),th5);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);
jacobian(3,4) = diff(rT6(3,4),th4);
jacobian(3,5) = diff(rT6(3,4),th5);


time_d = diff(humanoid_time);
jacobian_d = [];
tempos_d = diff(humanoid_time);

% diff(times)
for r = 1:(size(humanoid_time,1)-1)
    tempo_d(r) = humanoid_time(r)+(tempos_d(r)/2);
end

tempos_dd = diff(tempo_d);
tempo_dd = [];
for r = 1:(size(tempo_d,2)-1)
    tempo_dd(r) = tempo_d(r)+(tempos_dd(r)/2);
end
% getting the diff(Jacobian)
for a = 1:size(time_d,1)
    th1 = humanoid_state(a+1,1);
    th2 = humanoid_state(a+1,3);
    th3 = humanoid_state(a+1,5);
    th5 = humanoid_state(a+1,7);
    th4 = humanoid_state(a+1,9);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    th5 = humanoid_state(a,7);
    th4 = humanoid_state(a,9);
    jac1 = eval(jacobian);
    jacobian_d(:,:,a) = (jac2-jac1)./time_d(a);
end
% angular speed & angular accel
speed_joint_11 = diff(humanoid_state(:,1))./diff(humanoid_time);
speed_joint_12 = diff(humanoid_state(:,3))./diff(humanoid_time);
speed_joint_13 = diff(humanoid_state(:,5))./diff(humanoid_time);
speed_joint_15 = diff(humanoid_state(:,9))./diff(humanoid_time);
speed_joint_16 = diff(humanoid_state(:,7))./diff(humanoid_time);

accel_joint_11 = diff(speed_joint_11)./diff(tempo_d)';
accel_joint_12 = diff(speed_joint_12)./diff(tempo_d)';
accel_joint_13 = diff(speed_joint_13)./diff(tempo_d)';
accel_joint_15 = diff(speed_joint_15)./diff(tempo_d)';
accel_joint_16 = diff(speed_joint_16)./diff(tempo_d)';

linear_speed = [];
linear_accel = [];
for n = 1:size(accel_joint_11,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th5 = humanoid_state(n,7);
    th4 = humanoid_state(n,9);
%     subs(jacobian,{th1,th2,th3,th4,th5},{humanoid_state(n,1),humanoid_state(n,3),humanoid_state(n,5),humanoid_state(n,7),humanoid_state(n,9)});
    angular_speed = [speed_joint_11(n) speed_joint_12(n) speed_joint_13(n) speed_joint_15(n) speed_joint_16(n)]';
    linear_speed = [ linear_speed eval(jacobian)*angular_speed];
    angular_accel = [accel_joint_11(n) accel_joint_12(n) accel_joint_13(n) accel_joint_15(n) accel_joint_16(n)]';
    linear_accel = [ linear_accel ((eval(jacobian)*angular_accel)+(jacobian_d(:,:,n)*angular_speed))];
end

figure
hold on , grid on
title('Aceleração linear XX - Movimento Lateral - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(1,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,1,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)

figure
hold on , grid on
title('Aceleração linear YY - Movimento Lateral - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(2,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,3,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
axis([0 humanoid_time(end) -3 3])

figure
hold on , grid on
title('Aceleração linear ZZ - Movimento Lateral - cinemática & sensores 1 e 2','FontSize',14,'FontWeight','bold')
plot(tempo_dd,linear_accel(3,:),'r','LineWidth',2)
plot(sensors_time(:,1),-sensors_data(:,2,1),'b','LineWidth',2)
legend('cinemática','sensor 1')
xlabel('Tempo [seg]','FontSize',14,'FontWeight','bold')
ylabel('Aceleração [m/s²]','FontSize',14,'FontWeight','bold')
set(gca,'FontSize',14)
