%%
figure;

%% Tx
subplot(2, 3, 1);
hold on;
plot(pos.Time, pos.Data(:, 1), 'k-');
plot(setpoint.Time, setpoint.Data(:, 1), 'k--');
legend({'x - pos', 'x - setpoint'});
hold off;
xlabel('Time (s)'); ylabel('Position (m)');

%% Ty
subplot(2, 3, 2);
hold on;
plot(pos.Time, pos.Data(:, 2), 'k-');
plot(setpoint.Time, setpoint.Data(:, 2), 'k--');
legend({'y - pos', 'y - setpoint'});
hold off;
xlabel('Time (s)'); ylabel('Position (m)');

%% Tz
subplot(2, 3, 3);
hold on;
plot(pos.Time, pos.Data(:, 3), 'k-');
plot(setpoint.Time, setpoint.Data(:, 3), 'k--');
legend({'z - pos', 'z - setpoint'});
hold off;
xlabel('Time (s)'); ylabel('Position (m)');

%% Rx
subplot(2, 3, 4);
hold on;
plot(pos.Time, pos.Data(:, 4), 'k-');
plot(setpoint.Time, setpoint.Data(:, 4), 'k--');
legend({'$\theta_x$ - pos', '$\theta_x$ - setpoint'});
hold off;
xlabel('Time (s)'); ylabel('Rotation (rad)');

%% Ry
subplot(2, 3, 5);
hold on;
plot(pos.Time, pos.Data(:, 5), 'k-');
plot(setpoint.Time, setpoint.Data(:, 5), 'k--');
legend({'$\theta_y$ - pos', '$\theta_y$ - setpoint'});
hold off;
xlabel('Time (s)'); ylabel('Rotation (rad)');

%% Rz
subplot(2, 3, 6);
hold on;
plot(pos.Time, pos.Data(:, 6), 'k-');
plot(setpoint.Time, setpoint.Data(:, 6), 'k--');
legend({'$\theta_z$ - pos', '$\theta_z$ - setpoint'});
hold off;
xlabel('Time (s)'); ylabel('Rotation (rad)');