%% --------------------------- JOINT 1 ---------------------------------
%% JOINT 1: POSITION
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qSet_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qFB_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5),grid minor
    end
end
legend('qSet','qFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: JOINT VELOCITY
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmRef_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmFB_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5), grid minor
    end
end
legend('qdmRef','qdmFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Base','FontSize',24')

%% JOINT 1: CURRENT
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iRef_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iFB_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5), grid minor
    end
end
legend('iRef','iFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: ELECTROMAGNETIC TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tem_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5), grid minor
    end
end
legend('Tem','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: BACKLASH TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'C_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Base','FontSize',24)

%% JOINT 1: UTIL TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tu_j1');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Base','FontSize',24)

%% --------------------------- JOINT 2 ---------------------------------
%% JOINT 2: JOINT POSITION
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qSet_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qFB_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5),grid minor
    end
end
legend('qSet','qFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint angular position [°]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: MOTOR POSITION
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qm_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5),grid minor
    end
end
xlabel('Time [s]','FontSize',20)
ylabel('Motor angular position [°]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: JOINT VELOCITY
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmRef_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmFB_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5),grid minor
    end
end
legend('qdRef','qdFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Shoulder','FontSize',24')

%% JOINT 2: CURRENT [dq0]
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iRef_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iq_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'id_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'i0_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('iRef','iq','id','i0','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: ELECTROMAGNETIC TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tem_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5), grid minor
    end
end
legend('Tem','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: BACKLASH TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'C_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% JOINT 2: UTIL TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tu_j2');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Shoulder','FontSize',24)

%% --------------------------- JOINT 3 ---------------------------------
%% JOINT 3: POSITION
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qSet_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qFB_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5), grid minor
    end
end
legend('qSet','qFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: JOINT VELOCITY
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmRef_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmFB_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5), grid minor
    end
end
legend('qdRef','qdFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Elbow','FontSize',24')

%% JOINT 3: CURRENT
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iRef_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iFB_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5), grid minor
        hold on
    end
end
legend('iRef','iFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: ELECTROMAGNETIC TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tem_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
        hold on
    end
end
legend('Tem','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: BACKLASH TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'C_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% JOINT 3: UTIL TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tu_j3');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Elbow','FontSize',24)

%% --------------------------- JOINT 4 ---------------------------------
%% JOINT 4: POSITION
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qSet_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qFB_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5),grid minor
    end
end
legend('qSet','qFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: JOINT VELOCITY
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmRef_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmFB_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5),grid minor
    end
end
legend('qdRef','qdFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Wrist 1','FontSize',24')

%% JOINT 4: CURRENT
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iRef_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iFB_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('iRef','iFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: ELECTROMAGNETIC TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tem_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tem','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: BACKLASH TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'C_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% JOINT 4: UTIL TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tu_j4');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 1','FontSize',24)

%% --------------------------- JOINT 5 ---------------------------------
%% JOINT 5: POSITION
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qSet_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qFB_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5),grid minor
        hold on
    end
end
legend('qSet','qFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: JOINT VELOCITY
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmRef_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmFB_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5),grid minor
    end
end
legend('qdRef','qdFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Wrist 2','FontSize',24')

%% JOINT 5: CURRENT
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iRef_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iFB_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('iRef','iFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: ELECTROMAGNETIC TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tem_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tem','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: BACKLASH TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'C_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% JOINT 5: UTIL TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tu_j5');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 2','FontSize',24)

%% --------------------------- JOINT 6 ---------------------------------
%% JOINT 6: POSITION
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qSet_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qFB_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi,'LineWidth',1.5),grid minor
    end
end
legend('qSet','qFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Angular position [°]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: JOINT VELOCITY
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmRef_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'qdmFB_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data*180/pi/101,'LineWidth',1.5), grid minor
    end
end
legend('qdRef','qdFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Joint velocity [°/s]','FontSize',20)
title('Joint: Wrist 3','FontSize',24')

%% JOINT 6: CURRENT
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iRef_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'iFB_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end

legend('iRef','iFB','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Current [A]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: ELECTOMAGNETIC TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tem_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tem','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: BACKLASH TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'C_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('C','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% JOINT 6: UTIL TORQUE
figure() 
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'Tu_j6');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('Tu','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('Torque [Nm]','FontSize',20)
title('Joint: Wrist 3','FontSize',24)

%% TCP
figure()
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'TCP_x');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'TCP_y');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5)
        hold on
    end
end
for i=1:length(out_c.logsout)
    check = strcmp(out_c.logsout{1,i}.Name,'TCP_z');
    if check ==1
        plot(out_c.Time,out_c.logsout{1,i}.Values.Data,'LineWidth',1.5),grid minor
    end
end
legend('x','y','z','FontSize',18)
xlabel('Time [s]','FontSize',20)
ylabel('TCP coordinates [m]','FontSize',20)
title('Tool Center Point','FontSize',24)

%% TCP 3D plot
figure()
for i=1:length(out_c.logsout)
    check_x = strcmp(out_c.logsout{1,i}.Name,'TCP_x');
    check_y = strcmp(out_c.logsout{1,i}.Name,'TCP_y');
    check_z = strcmp(out_c.logsout{1,i}.Name,'TCP_z');
    if check_x == 1
        index_x = i;
    end
     if check_y == 1
        index_y = i;
    end
     if check_z == 1
        index_z = i;
     end
end
plot3(out_c.logsout{1,index_x}.Values.Data,out_c.logsout{1,index_y}.Values.Data,...
        out_c.logsout{1,index_z}.Values.Data,'LineWidth',1.5)
grid minor
axis equal
xlabel('x(t) [m]')
ylabel('y(t) [m]')
zlabel('z(t) [m]')
title('Tool Center Point','FontSize',24)