71 lines
1.9 KiB
Mathematica
71 lines
1.9 KiB
Mathematica
|
%% Compute position angle from R and Q
|
||
|
|
thetas_R = zeros(length(pos.Time), 3);
|
||
|
|
thetas_Q = zeros(length(pos.Time), 3);
|
||
|
|
|
||
|
|
for i = 1:length(pos.Time)
|
||
|
|
[thetax, thetay, thetaz] = RM2angle(R.Data(:, :, i));
|
||
|
|
thetas_R(i, 1) = thetax; thetas_R(i, 2) = thetay; thetas_R(i, 3) = thetaz;
|
||
|
|
|
||
|
|
[thetax, thetay, thetaz] = quaternionToEulerAngles(Q.Data(i, :));
|
||
|
|
thetas_Q(i, 1) = thetax; thetas_Q(i, 2) = thetay; thetas_Q(i, 3) = thetaz;
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Compute setpoint
|
||
|
|
setpoint_c = zeros(length(pos.Time), 6);
|
||
|
|
|
||
|
|
for i = 1:length(pos.Time)
|
||
|
|
setpoint_c(i, :) = computeSetpoint(ty.Data(i), ry.Data(i), rz.Data(i));
|
||
|
|
end
|
||
|
|
|
||
|
|
%%
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(pos.Time, pos.Data(:, 1), 'k-', 'DisplayName', 'position');
|
||
|
|
plot(pos.Time, setpoint_c(:, 1), '--', 'DisplayName', 'Computed setpoint');
|
||
|
|
hold off;
|
||
|
|
legend();
|
||
|
|
xlabel('Time (s)'); ylabel('Translation (m)');
|
||
|
|
|
||
|
|
%%
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(pos.Time, pos.Data(:, 2), 'k-', 'DisplayName', 'position');
|
||
|
|
plot(pos.Time, setpoint_c(:, 2), '--', 'DisplayName', 'Computed setpoint');
|
||
|
|
hold off;
|
||
|
|
legend();
|
||
|
|
xlabel('Time (s)'); ylabel('Translation (m)');
|
||
|
|
%%
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(pos.Time, pos.Data(:, 3), 'k-', 'DisplayName', 'position');
|
||
|
|
plot(pos.Time, setpoint_c(:, 3), '--', 'DisplayName', 'Computed setpoint');
|
||
|
|
hold off;
|
||
|
|
legend();
|
||
|
|
xlabel('Time (s)'); ylabel('Translation (m)');
|
||
|
|
|
||
|
|
%%
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(pos.Time, pos.Data(:, 4), 'k-', 'DisplayName', 'position');
|
||
|
|
plot(pos.Time, setpoint_c(:, 4), '--', 'DisplayName', 'Computed setpoint');
|
||
|
|
hold off;
|
||
|
|
legend();
|
||
|
|
xlabel('Time (s)'); ylabel('Rotation (rad)');
|
||
|
|
|
||
|
|
%%
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(pos.Time, pos.Data(:, 5), 'k-', 'DisplayName', 'position');
|
||
|
|
plot(pos.Time, setpoint_c(:, 5), '--', 'DisplayName', 'Computed setpoint');
|
||
|
|
hold off;
|
||
|
|
legend();
|
||
|
|
xlabel('Time (s)'); ylabel('Rotation (rad)');
|
||
|
|
|
||
|
|
%%
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(pos.Time, pos.Data(:, 6), 'k-', 'DisplayName', 'position');
|
||
|
|
plot(pos.Time, setpoint_c(:, 6), '--', 'DisplayName', 'Computed setpoint');
|
||
|
|
hold off;
|
||
|
|
legend();
|
||
|
|
xlabel('Time (s)'); ylabel('Rotation (rad)');
|