%% accel_inclinometer

%% full kinematics zz
clc, clear all , close all
save_id = 1;
move_zz

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

%adding the accel_offset
accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end


L1 = 45*1e-3;
L2 = 132*1e-3;
L3 = 123*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 0 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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,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);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];


end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);

% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 3]; 
roll_axis = [3 2 2 3 2];
for q = 1:size(sensors_time(:,1),1)
    for numero = 1:5
        % if A_magnitude outside the threshold
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
           if numero <= 2
                pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),numero),256);
                roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),numero),256);
           elseif numero >2 && numero <5
                pitch_sensor(numero,q) = atan2(-sensors_data(q,pitch_axis(numero),numero),256);
                roll_sensor(numero,q) = atan2(-sensors_data(q,roll_axis(numero),numero),256);
           else
               pitch_sensor(numero,q) = atan2(-sensors_data(q,pitch_axis(numero),9),256);
                roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),9),256);
           end
        else
            
            if q == 1 % for 1st iteration 
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

% images
figure
hold on, grid on
title('Pitch no agachamento - anca direita & acel1_x','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',3)
plot(sensors_time(:,1),pitch_sensor(1,:),'b','LineWidth',2)
legend('cinematica','acel1_x')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_pitch_1','jpg')
end
disp('anca, sensor 1')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,1),'nearest');
erro_quad = (servo_interp - pitch_sensor(1,:)').^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Pitch no agachamento - anca direita & acel9_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',3)
plot(sensors_time(:,9),pitch_sensor(5,:)+5,'b','LineWidth',2)
legend('cinematica','acel9_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_pitch_9','jpg')
end
disp('anca, sensor 9')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,9),'nearest');
erro_quad = (servo_interp - (pitch_sensor(5,:)'+5)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = pitch_sensor(5,:)'+5;
[r,A,B]=regression(x',y')
figure
hold on, grid on
title('Roll no agachamento - anca direita & acel1_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',3)
plot(sensors_time(:,1),roll_sensor(1,:),'b','LineWidth',2)
legend('cinematica','acel1_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
axis([0 humanoid_time(end) -10 10])
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_roll_1','jpg')
end
disp('anca, sensor 1')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (roll_sensor(1,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Roll no agachamento - anca direita & acel9_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',3)
plot(sensors_time(:,9),roll_sensor(5,:)-1,'b','LineWidth',2)
legend('cinematica','acel9_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
axis([0 humanoid_time(end) -10 10])
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_roll_9','jpg')
end
disp('anca, sensor 9')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,9),'nearest');
erro_quad = (servo_interp - (roll_sensor(5,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(5,:)';
[r,A,B]=regression(x',y')
%%
% full kinematics xx
clc, clear all
save_id = 1;
move_xx

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

%adding the accel_offset
accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end


L1 = 45*1e-3;
L2 = 132*1e-3;
L3 = 123*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 0 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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,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);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);

% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 3]; 
roll_axis = [3 2 2 3 2];
for q = 1:size(sensors_time(:,1),1)
    for numero = 1:5
        % if A_magnitude outside the threshold
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            if numero <= 2
                pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),numero),256);
                roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),numero),256);
            elseif numero >2 && numero <5
                pitch_sensor(numero,q) = -atan2(sensors_data(q,pitch_axis(numero),numero),256);
                roll_sensor(numero,q) = -atan2(sensors_data(q,roll_axis(numero),numero),256);
            else
                pitch_sensor(numero,q) = atan2(-sensors_data(q,pitch_axis(numero),9),256);
                roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),9),256);
            end
        else
            
            if q == 1 % for 1st iteration 
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

% images
figure
hold on, grid on
title('Pitch na flexão do joelho - anca direita & acel1_x','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',3)
plot(sensors_time(:,1),pitch_sensor(1,:),'b','LineWidth',2)
legend('cinematica','acel1_x','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_pitch_1','jpg')
end
disp('anca, sensor 1')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (pitch_sensor(1,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Pitch na flexão do joelho - anca direita & acel9_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',3)
plot(sensors_time(:,9),pitch_sensor(5,:)+5,'b','LineWidth',2)
legend('cinematica','acel9_z','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_pitch_9','jpg')
end
disp('anca, sensor 9')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (pitch_sensor(5,1:(end-1))'+5)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Roll na flexão do joelho - anca direita & acel1_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',3)
plot(sensors_time(:,1),roll_sensor(1,:),'b','LineWidth',2)
legend('cinematica','acel1_z','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_roll_1','jpg')
end
disp('anca, sensor 1')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (roll_sensor(1,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Roll na flexão do joelho - anca direita & acel9_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',3)
plot(sensors_time(:,9),roll_sensor(5,:)-1,'b','LineWidth',2)
legend('cinematica','acel9_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_roll_9','jpg')
end

disp('anca, sensor 9')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,9),'nearest');
erro_quad = (servo_interp(1:(end-1)) - (roll_sensor(5,1:(end-1))'-1)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp(1:(end-1));
y = roll_sensor(5,1:(end-1))'-1;
[r,A,B]=regression(x',y')
%%
% full kinematics yy
clc, clear all 
save_id = 1;
move_yy

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

%adding the accel_offset
accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end


L1 = 45*1e-3;
L2 = 132*1e-3;
L3 = 123*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 0 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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,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);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);

% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));

pitch_axis = [1 1 1 1 3]; 
roll_axis = [3 2 2 3 2];
for q = 1:size(sensors_time(:,1),1)
    for numero = 1:5
        % if A_magnitude outside the threshold
         if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            if numero <= 2
                pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),numero),256);
                roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),numero),256);
            elseif numero >2 && numero <5
                pitch_sensor(numero,q) = -atan2(sensors_data(q,pitch_axis(numero),numero),256);
                roll_sensor(numero,q) = -atan2(sensors_data(q,roll_axis(numero),numero),256);
            else
                pitch_sensor(numero,q) = atan2(-sensors_data(q,pitch_axis(numero),9),256);
                roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),9),256);
            end
        else
            
            if q == 1 % for 1st iteration 
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end                                
        end
    end
end
pitch_sensor = rad2deg(-pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

% images
figure
hold on, grid on
title('Pitch no movimento lateral - anca direita & acel1_x','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',3)
plot(sensors_time(:,1),pitch_sensor(1,:),'b','LineWidth',2)
legend('cinematica','acel1_x')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_pitch_1','jpg')
end
disp('anca, sensor 1')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (pitch_sensor(1,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Pitch no movimento lateral - anca direita & acel9_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',3)
plot(sensors_time(:,9),pitch_sensor(5,:),'b','LineWidth',2)
legend('cinematica','acel9_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_pitch_9','jpg')
end
disp('anca, sensor 9')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,9),'nearest');
erro_quad = (servo_interp - (pitch_sensor(5,:)'-11)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = pitch_sensor(5,:)'-11;
[r,A,B]=regression(x',y')

figure
hold on, grid on
title('Roll no movimento lateral - anca direita & acel1_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',3)
plot(sensors_time(:,1),roll_sensor(1,:),'b','LineWidth',2)
legend('cinematica','acel1_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_roll_1','jpg')
end
disp('anca, sensor 1')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,1),'nearest');
erro_quad = (servo_interp - (roll_sensor(1,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Roll no movimento lateral - anca direita & acel9_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',3)
plot(sensors_time(:,9),roll_sensor(5,:)-1,'b','LineWidth',2)
legend('cinematica','acel9_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_roll_9','jpg')
end
disp('anca, sensor 9')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,9),'nearest');
erro_quad = (servo_interp - (roll_sensor(5,:)'-1)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(5,:)'-1;
[r,A,B]=regression(x',y')
%% partial kinematics (sensor 6) - zz

clc, clear all , close all

save_id = 1;
move_zz

% humanoid_data_zz
for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end
humanoid_state(:,5) = zeros(size(humanoid_time,1),1); % servo 13
humanoid_state(:,6) = zeros(size(humanoid_time,1),1); % servo 23
humanoid_state(:,7) = zeros(size(humanoid_time,1),1); % servo 15
humanoid_state(:,8) = zeros(size(humanoid_time,1),1); % servo 25
humanoid_state(:,9) = zeros(size(humanoid_time,1),1); % servo 16
humanoid_state(:,10) = zeros(size(humanoid_time,1),1); % servo 26
humanoid_state(:,11) = zeros(size(humanoid_time,1),1); % servo 31
humanoid_state(:,12) = zeros(size(humanoid_time,1),1); % servo 32
humanoid_state(:,13) = zeros(size(humanoid_time,1),1); % servo (rotacao_tronco)


% sensors_data_zz
%adding the accel_offset
accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end

L1 = 45*1e-3;
L2 = 132*1e-3;
L3 = 123*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 0 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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,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);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);

% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
yaw_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 2 2 2 2]; 
roll_axis = [3 2 2 3 3 3 3 3];

for q = 1:size(sensors_time(:,1),1)
    for numero = 6:6
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),6),256);
            roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),6),256);

        else
            if q == 1
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

% graphs
figure
hold on, grid on
title('Pitch no agachamento - tíbia direita & acel6_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',2)
plot(sensors_time(:,6),-pitch_sensor(6,:)-2,'b','LineWidth',2)
legend('cinemática','acel6_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_pitch_6','jpg')
end
disp('pitch tibia, sensor 6')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (-pitch_sensor(6,:)'-2)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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

figure
hold on, grid on
title('Roll no agachamento - tíbia direita & acel6_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',2)
plot(sensors_time(:,6),roll_sensor(6,:),'b','LineWidth',2)
legend('cinemática','acel6_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
axis([0 humanoid_time(end) -10 10])
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_roll_6','jpg')
end
disp('roll tibia, sensor 6')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (roll_sensor(6,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(6,:)';
[r,A,B]=regression(x',y')
%%
% partial kinematics (sensor 6) - xx

clc, clear all
save_id = 1;
move_xx


for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end
humanoid_state(:,5) = zeros(size(humanoid_time,1),1); % servo 13
humanoid_state(:,6) = zeros(size(humanoid_time,1),1); % servo 23
humanoid_state(:,7) = zeros(size(humanoid_time,1),1); % servo 15
humanoid_state(:,8) = zeros(size(humanoid_time,1),1); % servo 25
humanoid_state(:,9) = zeros(size(humanoid_time,1),1); % servo 16
humanoid_state(:,10) = zeros(size(humanoid_time,1),1); % servo 26
humanoid_state(:,11) = zeros(size(humanoid_time,1),1); % servo 31
humanoid_state(:,12) = zeros(size(humanoid_time,1),1); % servo 32
humanoid_state(:,13) = zeros(size(humanoid_time,1),1); % servo (rotacao_tronco)


%adding the accel_offset
accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end

L1 = 45*1e-3;
L2 = 132*1e-3;
L3 = 123*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 0 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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,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);

    pitch_servo =[pitch_servo asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);

% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
yaw_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 2 2 2 2]; 
roll_axis = [3 2 2 3 3 3 3 3];

for q = 1:size(sensors_time(:,1),1)
    for numero = 6:6
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),6),256);
            roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),6),256);

        else
            if q == 1
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

% graphs
figure
hold on, grid on
title('Pitch na flexão do joelho - tíbia direita & acel6_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',2)
plot(sensors_time(:,6),pitch_sensor(6,:)+2,'b','LineWidth',2)
legend('cinemática','acel6_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_pitch_6','jpg')
end
disp('pitch tibia, sensor 6')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (pitch_sensor(6,:)'+2)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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


figure
hold on, grid on
title('Roll na flexão do joelho - tíbia direita & acel6_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',2)
plot(sensors_time(:,6),roll_sensor(6,:),'b','LineWidth',2)
legend('cinemática','acel6_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
axis([0 humanoid_time(end) -10 10])
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_roll_6','jpg')
end
disp('roll tibia, sensor 6')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (roll_sensor(6,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(6,:)';
[r,A,B]=regression(x',y')
%%
% partial kinematics (sensor 6) - yy

clc, clear all

save_id = 1;
move_yy

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end
humanoid_state(:,5) = zeros(size(humanoid_time,1),1); % servo 13
humanoid_state(:,6) = zeros(size(humanoid_time,1),1); % servo 23
humanoid_state(:,7) = zeros(size(humanoid_time,1),1); % servo 15
humanoid_state(:,8) = zeros(size(humanoid_time,1),1); % servo 25
humanoid_state(:,9) = zeros(size(humanoid_time,1),1); % servo 16
humanoid_state(:,10) = zeros(size(humanoid_time,1),1); % servo 26
humanoid_state(:,11) = zeros(size(humanoid_time,1),1); % servo 31
humanoid_state(:,12) = zeros(size(humanoid_time,1),1); % servo 32
humanoid_state(:,13) = zeros(size(humanoid_time,1),1); % servo (rotacao_tronco)

%adding the accel_offset
accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end

L1 = 45*1e-3;
L2 = 132*1e-3;
L3 = 123*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 0 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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,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);

    pitch_servo =[pitch_servo asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);

% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
yaw_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 2 2 2 2]; 
roll_axis = [3 2 2 3 3 3 3 3];

for q = 1:size(sensors_time(:,1),1)
    for numero = 6:6
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),6),256);
            roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),6),256);

        else
            if q == 1
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

% graphs
figure
hold on, grid on
title('Pitch no movimento lateral - tíbia direita & acel6_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',2)
plot(sensors_time(:,6),pitch_sensor(6,:)+2,'b','LineWidth',2)
legend('cinemática','acel6_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
axis([0 humanoid_time(end) -10 10])
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_pitch_6','jpg')
end
disp('pitch femur, sensor 6')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (pitch_sensor(6,:)'+2)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

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


figure
hold on, grid on
title('Roll no movimento lateral - tíbia direita & acel6_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',2)
plot(sensors_time(:,6),roll_sensor(6,:),'b','LineWidth',2)
legend('cinemática','acel6_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_roll_6','jpg')
end
disp('roll tibia, sensor 6')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,6),'nearest');
erro_quad = (servo_interp - (roll_sensor(6,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(6,:)';
[r,A,B]=regression(x',y')
%% partial kinematics (sensor 5) - zz

clc, clear all , close all

save_id = 1;
move_zz

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end
humanoid_state(:,7) = zeros(size(humanoid_time,1),1); % servo 15
humanoid_state(:,8) = zeros(size(humanoid_time,1),1); % servo 25
humanoid_state(:,9) = zeros(size(humanoid_time,1),1); % servo 16
humanoid_state(:,10) = zeros(size(humanoid_time,1),1); % servo 26
humanoid_state(:,11) = zeros(size(humanoid_time,1),1); % servo 31
humanoid_state(:,12) = zeros(size(humanoid_time,1),1); % servo 32
humanoid_state(:,13) = zeros(size(humanoid_time,1),1); % servo (rotacao_tronco)

accel_offset
%adding the accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end

L1=45;
L2=132;
L3=42;
% seixos;
syms th1 th2 th3 th4 th5
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 0 0 0];
R7 = [0 0 1 0
      0 -1 0 0
      1 0 0 0
      0 0 0 1];
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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th4 = humanoid_state(n,9);
    th5 = humanoid_state(n,7);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];
end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);
% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
yaw_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 2 2 2 2]; 
roll_axis = [3 2 2 3 3 3 3 3];
for q = 1:size(sensors_time(:,1),1)
    for numero = 5:5
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),numero),256);
            roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),numero),256);
        else
            sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)
            if q == 1
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(-pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

figure
hold on, grid on
title('Pitch no agachamento - fémur direito & acel5_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',2)
plot(sensors_time(:,5),pitch_sensor(5,:)-1.5,'b','LineWidth',2)
legend('cinemática','acel5_y','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_pitch_5','jpg')
end
disp('pitch femur, sensor 5')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,5),'nearest');
erro_quad = (servo_interp - (pitch_sensor(5,:)'-1.5)).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = pitch_sensor(5,:)'-1.5;
[r,A,B]=regression(x',y')

figure
hold on, grid on
title('Roll no agachamento - fémur direito & acel5_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',2)
plot(sensors_time(:,5),roll_sensor(5,:)+1.5,'b','LineWidth',2)
legend('cinemática','acel5_z','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_ZZ_roll_5','jpg')
end
%%
% partial kinematics (sensor 5) - xx

clc, clear all 

save_id = 1;
move_xx

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end
humanoid_state(:,7) = zeros(size(humanoid_time,1),1); % servo 15
humanoid_state(:,8) = zeros(size(humanoid_time,1),1); % servo 25
humanoid_state(:,9) = zeros(size(humanoid_time,1),1); % servo 16
humanoid_state(:,10) = zeros(size(humanoid_time,1),1); % servo 26
humanoid_state(:,11) = zeros(size(humanoid_time,1),1); % servo 31
humanoid_state(:,12) = zeros(size(humanoid_time,1),1); % servo 32
humanoid_state(:,13) = zeros(size(humanoid_time,1),1); % servo (rotacao_tronco)

accel_offset
%adding the accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end

L1=45;
L2=132;
L3=42;
% seixos;
syms th1 th2 th3 th4 th5
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 0 0 0];
R7 = [0 0 1 0
      0 -1 0 0
      1 0 0 0
      0 0 0 1];
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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th4 = humanoid_state(n,9);
    th5 = humanoid_state(n,7);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);
% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
yaw_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 2 2 2 2]; 
roll_axis = [3 2 2 3 3 3 3 3];
for q = 1:size(sensors_time(:,1),1)
    for numero = 5:5
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),numero),256);
            roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),numero),256);
        else
            sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)
            if q == 1
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(-pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

figure
hold on, grid on
title('Pitch na flexão do joelho - fémur direito & acel5_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',2)
plot(sensors_time(:,5),pitch_sensor(5,:),'b','LineWidth',2)
legend('cinemática','acel5_y')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_pitch_5','jpg')
end
disp('pitch femur, sensor 5')
servo_interp = interp1(humanoid_time,pitch_servo,sensors_time(:,5),'nearest');
erro_quad = (servo_interp - (pitch_sensor(5,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = pitch_sensor(5,:)';
[r,A,B]=regression(x',y')

figure
hold on, grid on
title('Roll na flexão do joelho - fémur direito & acel5_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',2)
plot(sensors_time(:,5),roll_sensor(5,:),'b','LineWidth',2)
legend('cinemática','acel5_z')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_XX_roll_5','jpg')
end
disp('roll femur, sensor 5')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,5),'nearest');
erro_quad = (servo_interp - (roll_sensor(5,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(5,:)';
[r,A,B]=regression(x',y')

%%

% partial kinematics (sensor 5) - yy

clc, clear all 

save_id = 1;
move_yy

for q = 1:(size(humanoid_state,2)-1)
humanoid_state(:,q) = deg2rad(humanoid_state(:,q));
end
humanoid_state(:,7) = zeros(size(humanoid_time,1),1); % servo 15
humanoid_state(:,8) = zeros(size(humanoid_time,1),1); % servo 25
humanoid_state(:,9) = zeros(size(humanoid_time,1),1); % servo 16
humanoid_state(:,10) = zeros(size(humanoid_time,1),1); % servo 26
humanoid_state(:,11) = zeros(size(humanoid_time,1),1); % servo 31
humanoid_state(:,12) = zeros(size(humanoid_time,1),1); % servo 32
humanoid_state(:,13) = zeros(size(humanoid_time,1),1); % servo (rotacao_tronco)

accel_offset
%adding the accel_offset
for n = 1:total_number
    sensors_data(:,1,n) = sensors_data(:,1,n)-accel_offset_val(n,1);
    sensors_data(:,2,n) = sensors_data(:,2,n)-accel_offset_val(n,2);
    sensors_data(:,3,n) = sensors_data(:,3,n)-accel_offset_val(n,3);
end

L1=45;
L2=132;
L3=42;

syms th1 th2 th3 th4 th5
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 0 0 0];
R7 = [0 0 1 0
      0 -1 0 0
      1 0 0 0
      0 0 0 1];
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));
AAA_g = A1*A2*A3*A4*A5*A6*R7;

% orientação obtida pela cinemática completa e SERVOMOTORES
pitch_servo = [];
roll_servo =  [];
yaw_servo = [];
for n = 1:size(humanoid_state,1)
    th1 = humanoid_state(n,1);
    th2 = humanoid_state(n,3);
    th3 = humanoid_state(n,5);
    th4 = humanoid_state(n,9);
    th5 = humanoid_state(n,7);

    pitch_servo =[pitch_servo -asin(eval(AAA_g(3,1)))];
    roll_servo = [roll_servo atan2(eval(AAA_g(3,2)),eval(AAA_g(3,3)))];
    yaw_servo = [yaw_servo atan2(eval(AAA_g(2,1)),eval(AAA_g(1,1)))];

end
pitch_servo = rad2deg(pitch_servo);
roll_servo = rad2deg(roll_servo);
yaw_servo = rad2deg(yaw_servo);
% orientação obtida pela cinemática completa e sensores
pitch_sensor = zeros(total_number,size(sensors_data,1));
roll_sensor = zeros(total_number,size(sensors_data,1));
yaw_sensor = zeros(total_number,size(sensors_data,1));
pitch_axis = [1 1 1 1 2 2 2 2]; 
roll_axis = [3 2 2 3 3 3 3 3];
for q = 1:size(sensors_time(:,1),1)
    for numero = 5:5
        if sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)>=240 && sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)<=265
            pitch_sensor(numero,q) = atan2(sensors_data(q,pitch_axis(numero),numero),256);
            roll_sensor(numero,q) = atan2(sensors_data(q,roll_axis(numero),numero),256);
        else
            sqrt(sensors_data(q,1,numero)^2+sensors_data(q,2,numero)^2+sensors_data(q,3,numero)^2)
            if q == 1
                pitch_sensor(numero,q) = 0;
                roll_sensor(numero,q) = 0 ;
            else
                pitch_sensor(numero,q) = pitch_sensor(numero,q-1);
                roll_sensor(numero,q) =  roll_sensor(numero,q-1);
            end
        end
    end
end
pitch_sensor = rad2deg(-pitch_sensor);
roll_sensor = rad2deg(roll_sensor);

figure
hold on, grid on
title('Pitch no movimento lateral - fémur direito & acel5_y','FontSize',17,'FontWeight','bold')
plot(humanoid_time,pitch_servo,'r','LineWidth',2)
plot(sensors_time(:,5),pitch_sensor(5,:)+2.5,'b','LineWidth',2)
legend('cinemática','acel5_y','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_pitch_5','jpg')
end

figure
hold on, grid on
title('Roll no movimento lateral - fémur direito & acel5_z','FontSize',17,'FontWeight','bold')
plot(humanoid_time,roll_servo,'r','LineWidth',2)
plot(sensors_time(:,5),roll_sensor(5,:),'b','LineWidth',2)
legend('cinemática','acel5_z','Location','Best')
xlabel('Tempo [seg]','FontSize',17)
ylabel('Valor Angular [\circ]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/inclin_YY_roll_5','jpg')
end
disp('roll femur, sensor 5')
servo_interp = interp1(humanoid_time,roll_servo,sensors_time(:,5),'nearest');
erro_quad = (servo_interp - (roll_sensor(5,:)')).^2;
erro_quad_med = sum(erro_quad)/numel(erro_quad)

x = servo_interp;
y = roll_sensor(5,:)';
[r,A,B]=regression(x',y')
