%% Cinemática diferencial

%% Full Kinematics - ZZ

clc, clear all , close all
save_id = 0;


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('Agachamento - anca dir. - acel. linear X - cinemática & acel1_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,1),-sensors_data(:,1,1))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(2),'YTick',(-8:2:8),'ycolor','b','FontSize',17)
set(ax(1),'YTick',(-1:0.5:1.5),'ycolor','r','FontSize',17)
legend('cinemática','acel1_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_x_1','jpg')
end
disp('accel x - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,1,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)
x = servo_interp;
y = -sensors_data(:,1,1);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Agachamento - anca dir. - acel. linear Y - cinemática & acel1_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,1),-sensors_data(:,3,1))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel1_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_y_1','jpg')
end
disp('accel y - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,3,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,3,1);
[r,A,B]=regression(x',y')



figure
hold on , grid on
title('Agachamento - anca dir. - acel. linear Z - cinemática & acel1_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,1),-sensors_data(:,2,1))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[6 14],'YTick',(6:1:14),'ycolor','b','FontSize',17)
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
legend('cinemática','acel1_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_z_1','jpg')
end
disp('accel z - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,2,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,2,1);
[r,A,B]=regression(x',y')


figure
hold on, grid on
plot(sensors_time(:,1),ones(size(sensors_data,1),1)*(256-10)*accel_sensivity,'r','LineWidth',3)
plot(sensors_time(:,1),ones(size(sensors_data,1),1)*(256+10)*accel_sensivity,'r','LineWidth',3)
plot(sensors_time(:,1),sqrt(sensors_data(:,1,1).^2+sensors_data(:,2,1).^2+sensors_data(:,3,1).^2),'LineWidth',2)


%% Full Kinematics - XX

clc, clear all , close all
save_id = 0;
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('Flexão do joelho - anca dir. - acel. linear X - cinemática & acel1_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,1),-sensors_data(:,1,1))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(2),'ycolor','b','FontSize',17)
set(ax(1),'ycolor','r','FontSize',17)
legend('cinemática','acel1_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_x_1','jpg')
end
disp('accel x - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),1,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),1,1);
[r,A,B]=regression(x',y')

figure
hold on , grid on
title('Flexão do joelho - anca dir. - acel. linear Y - cinemática & acel1_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,1),-sensors_data(:,3,1))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','b','FontSize',17)
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.1:0.5),'ycolor','r','FontSize',17)
legend('cinemática','acel1_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_y_1','jpg')
end
disp('accel y - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),3,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),3,1);
[r,A,B]=regression(x',y')



figure
hold on , grid on
title('Flexão do joelho - anca dir. - acel. linear Z - cinemática & acel1_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,1),-sensors_data(:,2,1))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ycolor','b','FontSize',17)
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
legend('cinemática','acel1_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_z_1','jpg')
end
disp('accel z - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),2,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),2,1);
[r,A,B]=regression(x',y')


%% Full Kinematics - YY

clc, clear all , close all
save_id = 0;
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('Movimento lateral - anca dir. - acel. linear X - cinemática & acel1_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,1),-sensors_data(:,1,1))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ylim',[-0.5 1],'YTick',(-0.5:0.2:1),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-1 2],'YTick',(-1:0.2:2),'ycolor','b','FontSize',17)
legend('cinemática','acel1_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_x_1','jpg')
end
disp('accel x - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,1,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,1,1);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Movimento lateral - anca dir. - acel. linear Y - cinemática & acel1_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,1),-sensors_data(:,3,1))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','b','FontSize',17)
legend('cinemática','acel1_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_y_1','jpg')
end
disp('accel y - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,3,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,3,1);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Movimento lateral - anca dir. - acel. linear Z - cinemática & acel1_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,1),-sensors_data(:,2,1))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[8 11],'YTick',(8:0.4:11),'ycolor','b','FontSize',17)
legend('cinemática','acel1_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_z_1','jpg')
end
disp('accel z - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,2,1))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,2,1);
[r,A,B]=regression(x',y')


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

%% Sensor 6 Kinematics - ZZ

clc, clear all , close all
save_id = 0;
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('Agachamento - tíbia dir. - acel. linear X - cinemática & acel6_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,6),-sensors_data(:,2,6))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel6_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_x_6','jpg')
end
disp('accel x - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),2,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),2,6);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Agachamento - tíbia dir. - acel. linear Y - cinemática & acel6_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,6),-sensors_data(:,3,6))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel6_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_y_6','jpg')
end
disp('accel y - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),3,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),3,6);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Agachamento - tíbia dir. - acel. linear Z - cinemática & acel6_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,6),-sensors_data(:,1,6))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.05 0.05],'YTick',(-0.05:0.02:0.05),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-19 -4],'YTick',(-19:1:-4),'ycolor','b','FontSize',17)
legend('cinemática','acel6_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_z_6','jpg')
end
disp('accel z - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),1,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),1,6);
[r,A,B]=regression(x',y')
%% Sensor 6 Kinematics - XX

clc, clear all , close all
save_id = 0;
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);
    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('Flexão do joelho - tíbia dir. - acel. linear X - cinemática & acel6_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,6),-sensors_data(:,2,6))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel6_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_x_6','jpg')
end
disp('accel x - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,2,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,2,6);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Flexão do joelho - tíbia dir. - acel. linear Y - cinemática & acel6_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,6),-sensors_data(:,3,6))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel6_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_y_6','jpg')
end
disp('accel y - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,3,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,3,6);
[r,A,B]=regression(x',y')

figure
hold on , grid on
title('Flexão do joelho - tíbia dir. - acel. linear Z - cinemática & acel6_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,6),-sensors_data(:,1,6))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel6_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_z_6','jpg')
end
disp('accel z - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (-sensors_data(:,1,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = -sensors_data(:,1,6);
[r,A,B]=regression(x',y')

%% Sensor 6 Kinematics - YY

clc, clear all , close all
save_id = 0;
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;

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('Movimento lateral - tíbia dir. - acel. linear X - cinemática & acel6_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,6),-sensors_data(:,2,6))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','b','FontSize',17)
legend('cinemática','acel6_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_x_6','jpg')
end
disp('accel x - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),2,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),2,6);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Movimento lateral - tíbia dir. - acel. linear Y - cinemática & acel6_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,6),-sensors_data(:,3,6))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-5 5],'YTick',(-5:1:5),'ycolor','b','FontSize',17)
legend('cinemática','acel6_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_y_6','jpg')
end
disp('accel y - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),3,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),3,6);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Movimento lateral - tíbia dir. - acel. linear Z - cinemática & acel6_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,6),-sensors_data(:,1,6))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-10 -8],'YTick',(-10:0.4:-8),'ycolor','b','FontSize',17)
legend('cinemática','acel6_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_z_6','jpg')
end
disp('accel z - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,6),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),1,6))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),1,6);
[r,A,B]=regression(x',y')

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% NEXT KINEMATICS 5 %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Sensor 5 Kinematics - ZZ

clc, clear all , close all
save_id = 0;
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;

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];
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));
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(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);


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);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    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);

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)';

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);
%     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('Agachamento - fémur dir. - acel. linear X - cinemática & acel5_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,5),-sensors_data(:,2,5))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_x_5','jpg')
end

figure
hold on , grid on
title('Agachamento - fémur dir. - acel. linear Y - cinemática & acel5_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,5),-sensors_data(:,3,5))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-2 2],'YTick',(-2:0.4:2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_y_5','jpg')
end

figure
hold on , grid on
title('Agachamento - fémur dir. - acel. linear Z - cinemática & acel5_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,5),-sensors_data(:,1,5))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_z_5','jpg')
end
%% Sensor 5 Kinematics - XX

clc, clear all , close all
save_id = 0;
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;

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];
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));
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(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);


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);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    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);

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)';

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);
%     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('Flexão do joelho - fémur dir. - acel. linear X - cinemática & acel5_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,5),-sensors_data(:,2,5))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-10 3],'YTick',(-10:1:3),'ycolor','b','FontSize',17)
legend('cinemática','acel5_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_x_5','jpg')
end

figure
hold on , grid on
title('Flexão do joelho - fémur dir. - acel. linear Y - cinemática & acel5_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,5),-sensors_data(:,3,5))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_y_5','jpg')
end

figure
hold on , grid on
title('Flexão do joelho - fémur dir. - acel. linear Z - cinemática & acel5_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,5),-sensors_data(:,1,5))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.4 0.4],'YTick',(-0.4:0.2:0.4),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_z_5','jpg')
end

%% Sensor 5 Kinematics - YY

clc, clear all , close all
save_id = 0;
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;


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];

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));
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(2,1) = diff(rT6(2,4),th1);
jacobian(2,2) = diff(rT6(2,4),th2);
jacobian(2,3) = diff(rT6(2,4),th3);
jacobian(3,1) = diff(rT6(3,4),th1);
jacobian(3,2) = diff(rT6(3,4),th2);
jacobian(3,3) = diff(rT6(3,4),th3);


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);
    jac2 = eval(jacobian);
    th1 = humanoid_state(a,1);
    th2 = humanoid_state(a,3);
    th3 = humanoid_state(a,5);
    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);

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)';

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);

%     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('Movimento lateral - fémur dir. - acel. linear X - cinemática & acel5_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,5),-sensors_data(:,2,5))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(1),'ycolor','r','FontSize',17)
set(ax(2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_x_5','jpg')
end

figure
hold on , grid on
title('Movimento lateral - fémur dir. - acel. linear Y - cinemática & acel5_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,5),-sensors_data(:,3,5))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-2 2],'YTick',(-2:0.5:2),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-2 2],'YTick',(-2:0.5:2),'ycolor','b','FontSize',17)
legend('cinemática','acel5_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_y_5','jpg')
end

figure
hold on , grid on
title('Movimento lateral - fémur dir. - acel. linear Z - cinemática & acel5_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,5),-sensors_data(:,1,5))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.2:0.5),'ycolor','r','FontSize',17)
set(ax(2),'ylim',[-10.5 -9],'YTick',(-10.5:0.4:-9),'ycolor','b','FontSize',17)
legend('cinemática','acel5_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_z_5','jpg')
end
% accelerometer's length
figure
hold on, grid on
plot(sensors_time(:,1),ones(size(sensors_data,1),1)*(256-10)*accel_sensivity,'r','LineWidth',3)
plot(sensors_time(:,1),ones(size(sensors_data,1),1)*(256+10)*accel_sensivity,'r','LineWidth',3)
plot(sensors_time(:,1),sqrt(sensors_data(:,1,1).^2+sensors_data(:,2,1).^2+sensors_data(:,3,1).^2),'LineWidth',2)

%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%KINEMATICS FOR 9th SENSOR%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%

%% Full Kinematics - ZZ

clc, clear all , close all
save_id = 0;


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('Agachamento - anca dir. - acel. linear X - cinemática & acel9_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,9),sensors_data(:,3,9))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(2),'ylim',[-15 15],'YTick',(-15:3:15),'ycolor','b','FontSize',17)
set(ax(1),'ylim',[-1.5 1.5],'YTick',(-1.5:0.3:1.5),'ycolor','r','FontSize',17)
legend('cinemática','acel9_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_x_9','jpg')
end
disp('accel x - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (sensors_data(1:(end-1),3,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = sensors_data(1:(end-1),3,9);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Agachamento - anca dir. - aceler. linear Y - cinemática & acel9_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,9),-sensors_data(:,2,9))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-5 5],'YTick',(-5:1:5),'ycolor','b')
set(ax(1),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','r')
legend('cinemática','acel9_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_y_9','jpg')
end
disp('accel y - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),2,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),2,9);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Agachamento - anca dir. - acel. linear Z - cinemática & acel9_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,9),sensors_data(:,1,9))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-15 -5],'YTick',(-15:1:-5),'ycolor','b','FontSize',17)
set(ax(1),'ylim',[-0.3 0.3],'YTick',(-0.3:0.1:0.3),'ycolor','r','FontSize',17)
legend('cinemática','acel9_x','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_ZZ_z_9','jpg')
end
disp('accel z - agachamento')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (sensors_data(1:(end-1),1,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = sensors_data(1:(end-1),1,9);
[r,A,B]=regression(x',y')

%% Full Kinematics - XX

clc, clear all , close all
save_id = 0;
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];
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('Flexão do joelho - anca dir. - acel. linear X - cinemática & acel9_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,9),sensors_data(:,3,9))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(2),'ylim',[-10 10],'YTick',(-10:2:10),'ycolor','b')
set(ax(1),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','r')
legend('cinemática','acel9_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_x_9','jpg')
end
disp('accel x - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-2)) - (sensors_data(1:(end-2),3,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-2));
y = sensors_data(1:(end-2),3,9);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Flexão do joelho - anca dir. - acel. linear X - cinemática & acel9_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,9),-sensors_data(:,2,9)+0.3)
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-1.5 1.5],'YTick',(-1.5:0.3:1.5),'ycolor','b')
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.1:0.5),'ycolor','r')
legend('cinemática','acel9_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_y_9','jpg')
end
disp('accel y - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-2)) - (-sensors_data(1:(end-2),2,9)+0.3)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-2));
y = -sensors_data(1:(end-2),2,9)+0.3;
[r,A,B]=regression(x',y')

figure
hold on , grid on
title('Flexão do joelho - anca dir. - acel. linear Z - cinemática & acel9_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,9),sensors_data(:,1,9))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-15 -5],'YTick',(-15:1:-5),'ycolor','b')
set(ax(1),'ylim',[-0.5 0.5],'YTick',(-0.5:0.1:0.5),'ycolor','r')
legend('cinemática','acel9_x','Location','Best')
xlabel('Tempo [seg]','FontWeight','bold')
ylabel(ax(1),'Aceleração [m/s²]','FontWeight','bold','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_XX_z_9','jpg')
end
disp('accel z - flexao_joelho')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-2)) - (sensors_data(1:(end-2),1,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-2));
y = sensors_data(1:(end-2),1,9);
[r,A,B]=regression(x',y')

%% Full Kinematics - YY

clc, clear all , close all
save_id = 0;
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];
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('Movimento lateral - anca dir. - acel. linear X - cinemática & acel9_z','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(1,:),sensors_time(:,9),sensors_data(:,3,9))
set(y1,'LineWidth',2,'Color','r')
set(y2,'LineWidth',2,'Color','b')
set(ax(2),'ylim',[-2 2],'YTick',(-2:0.4:2),'ycolor','b')
set(ax(1),'ylim',[-1 1],'YTick',(-1:0.2:1),'ycolor','r')
legend('cinemática','acel9_z','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_x_9','jpg')
end
disp('accel x - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(1,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (sensors_data(1:(end-1),3,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = sensors_data(1:(end-1),3,9);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Movimento lateral - anca dir. - acel. linear X - cinemática & acel9_y','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(2,:),sensors_time(:,9),-sensors_data(:,2,9))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-2 2],'YTick',(-2:0.4:2),'ycolor','b')
set(ax(1),'ylim',[-2 2],'YTick',(-2:0.4:2),'ycolor','r')
legend('cinemática','acel9_y','Location','Best')
xlabel('Tempo [seg]')
ylabel(ax(1),'Aceleração [m/s²]','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_y_9','jpg')
end
disp('accel y - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(2,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (-sensors_data(1:(end-1),2,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = -sensors_data(1:(end-1),2,9);
[r,A,B]=regression(x',y')


figure
hold on , grid on
title('Movimento lateral - anca dir. - acel. linear X - cinemática & acel9_x','FontSize',17,'FontWeight','bold')
[ax y1 y2] = plotyy(tempo_dd,linear_accel(3,:),sensors_time(:,9),sensors_data(:,1,9))
set(y1,'LineWidth',2,'Color',[1 0 0])
set(y2,'LineWidth',2,'Color',[0 0 1])
set(ax(2),'ylim',[-11 -9],'YTick',(-11:0.4:-9),'ycolor','b')
set(ax(1),'ylim',[-2 2],'YTick',(-2:0.4:2),'ycolor','r')
legend('cinemática','acel9_x','Location','Best')
xlabel('Tempo [seg]','FontWeight','bold')
ylabel(ax(1),'Aceleração [m/s²]','FontWeight','bold','Color','k')
set(ax,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/accel_YY_z_9','jpg')
end
disp('accel z - mov_lateral')
servo_interp = interp1(tempo_dd,linear_accel(3,:),sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (sensors_data(1:(end-1),1,9))).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = sensors_data(1:(end-1),1,9);
[r,A,B]=regression(x',y')

