diff --git a/backup.org b/backup.org index 61596e9..1df0048 100644 --- a/backup.org +++ b/backup.org @@ -10809,6 +10809,762 @@ data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm) | Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s) | 75 | 9 | 118 | | Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s) | 428 | 11 | 169 | +* TODO SRI Figures :noexport: +** Open Loop Figure / Effect of mass +#+begin_src matlab :exports none +%% Save Identified Plants +load('G_ol.mat', 'G_int_m0', 'G_int_m1', 'G_int_m2', 'G_int_m3', 'f'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +t = tiledlayout(6, 6, 'TileSpacing', 'compact', 'Padding', 'None'); + +for i = 1:6 + for j = 1:6 + nexttile(); + if i == j + plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 1]); + else + plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.5]); + end + set(gca, 'XTickLabel',[]); + set(gca, 'YTickLabel',[]); + if j == 1 + ylabel(sprintf('$dL_{%i}$', i)) + end + if i == 6 + xlabel(sprintf('$u_{%i}$', j)) + end + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-7, 2e-4]); + xlim([1, 1e3]); + end +end +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_6x6_plant.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_6x6_plant.png]] + +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ... + 'DisplayName', '$m = 0$ kg'); +for i = 2:6 + plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 1.0], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-4]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_plant_m0.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_plant_m0.png]] + +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ... + 'DisplayName', '$m = 0$ kg'); +for i = 2:6 + plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 1.0], ... + 'HandleVisibility', 'off'); +end +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_m1(:, i, j)), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_m1(:,1, 1)), 'color', [colors(2,:), 1.0], ... + 'DisplayName', '$m = 15$ kg'); +for i = 2:6 + plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 1.0], ... + 'HandleVisibility', 'off'); +end +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_m2(:, i, j)), 'color', [colors(3,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_m2(:,1, 1)), 'color', [colors(3,:), 1.0], ... + 'DisplayName', '$m = 30$ kg'); +for i = 2:6 + plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 1.0], ... + 'HandleVisibility', 'off'); +end +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_m3(:, i, j)), 'color', [colors(4,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_m3(:,1, 1)), 'color', [colors(4,:), 1.0], ... + 'DisplayName', '$m = 45$ kg'); +for i = 2:6 + plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(4,:), 1.0], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-4]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]); + plot(f, 180/pi*unwrapphase(angle(-G_int_m1(:,i, i)), f), 'color', [colors(2,:), 1.0]); + plot(f, 180/pi*unwrapphase(angle(-G_int_m2(:,i, i)), f), 'color', [colors(3,:), 1.0]); + plot(f, 180/pi*unwrapphase(angle(-G_int_m3(:,i, i)), f), 'color', [colors(4,:), 1.0]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_plant_m3.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_plant_m3.png]] + +** Effect of Active Damping +#+begin_src matlab :exports none +%% Save Identified Plants +load('G_hac.mat', 'G_hac_m0', 'G_hac_m1', 'G_hac_m2', 'G_hac_m3', 'f'); +#+end_src + +#+begin_src matlab :exports none :results none +%% description +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:)], ... + 'DisplayName', '$m = 0$ - OL'); +for i = 2:6 + plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:)], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(2,:)], ... + 'DisplayName', '$m = 0$ - Damped'); +for i = 2:6 + plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:)], ... + 'HandleVisibility', 'off') +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-4]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:)]); +end +for i =1:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(2,:)]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_plant_m0_comp_damped.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_plant_m0_comp_damped.png]] + +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_hac_m0(:,1, 1)), 'color', [colors(1,:), 1.0], ... + 'DisplayName', '$m = 0$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 1.0], ... + 'HandleVisibility', 'off'); +end +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_hac_m1(:, i, j)), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_hac_m1(:,1, 1)), 'color', [colors(2,:), 1.0], ... + 'DisplayName', '$m = 15$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 1.0], ... + 'HandleVisibility', 'off'); +end +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_hac_m2(:, i, j)), 'color', [colors(3,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_hac_m2(:,1, 1)), 'color', [colors(3,:), 1.0], ... + 'DisplayName', '$m = 30$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 1.0], ... + 'HandleVisibility', 'off'); +end +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_hac_m3(:, i, j)), 'color', [colors(4,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_hac_m3(:,1, 1)), 'color', [colors(4,:), 1.0], ... + 'DisplayName', '$m = 45$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 1.0], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-4]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(1,:), 1.0]); + plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,i, i)), f), 'color', [colors(2,:), 1.0]); + plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,i, i)), f), 'color', [colors(3,:), 1.0]); + plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,i, i)), f), 'color', [colors(4,:), 1.0]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_plant_m3_damped.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_plant_m3_damped.png]] + +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_int_m0(:,1,1)), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped'); +plot(f, abs(G_hac_m0(:,1,1)), 'color', [colors(2,:), 0.5], 'DisplayName', 'damped'); +for i = 1:6 + plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off'); +end +for i = 1:6 + plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); + plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); +ylim([1e-8, 2e-4]); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*unwrapphase(angle(-G_int_m0(:,i, i)), f), 'color', [colors(1,:), 0.5]); + plot(f, 180/pi*unwrapphase(angle(-G_int_m1(:,i, i)), f), 'color', [colors(1,:), 0.5]); + plot(f, 180/pi*unwrapphase(angle(-G_int_m2(:,i, i)), f), 'color', [colors(1,:), 0.5]); + plot(f, 180/pi*unwrapphase(angle(-G_int_m3(:,i, i)), f), 'color', [colors(1,:), 0.5]); +end +for i = 1:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(2,:), 0.5]); + plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,i, i)), f), 'color', [colors(2,:), 0.5]); + plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,i, i)), f), 'color', [colors(2,:), 0.5]); + plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,i, i)), f), 'color', [colors(2,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_plant_comp_damped_undamped.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_plant_comp_damped_undamped.png]] + +** Comparison with the Model +#+begin_src matlab +%% Save Damped Plants +load('Gm.mat', 'Gm_hac_m0', 'Gm_hac_m1', 'Gm_hac_m2', 'Gm_hac_m3'); +#+end_src + +All elements: +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +t = tiledlayout(6, 6, 'TileSpacing', 'compact', 'Padding', 'None'); + +for i = 1:6 + for j = 1:6 + nexttile(); + hold on; + if i == j + plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(i,:), 0.5]); + plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), '--', 'color', colors(i,:)); + else + plot(f, abs(G_hac_m0(:, i, j)), 'color', [0, 0, 0, 0.5]); + plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 1]); + end + hold off; + set(gca, 'XTickLabel',[]); + set(gca, 'YTickLabel',[]); + if j == 1 + ylabel(sprintf('$dL_{%i}$', i)) + end + if i == 6 + xlabel(sprintf('$u_{%i}$', j)) + end + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-7, 1e-4]); + xlim([1, 1e3]); + end +end +#+end_src +#+begin_src matlab :exports none :results none +%% Measured transfer function from generated voltages to measured voltage on the force sensors +figure; +t = tiledlayout(6, 6, 'TileSpacing', 'compact', 'Padding', 'None'); + +for i = 1:6 + for j = 1:6 + nexttile(); + hold on; + if i == j + plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(1,:), 1]); + plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'k-'); + else + plot(f, abs(G_hac_m0(:, i, j)), 'color', [colors(1,:), 0.5]); + plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), '-', 'color', [0, 0, 0, 0.5]); + end + hold off; + set(gca, 'XTickLabel',[]); + set(gca, 'YTickLabel',[]); + if j == 1 + ylabel(sprintf('$dL_{%i}$', i)) + end + if i == 6 + xlabel(sprintf('$u_{%i}$', j)) + end + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-7, 2e-4]); + xlim([1, 1e3]); + end +end +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_comp_plant_model_6x6.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_comp_plant_model_6x6.png]] + +Diagonal elements: +#+begin_src matlab :exports none :results none +%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$m = 0$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$m = 15$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$m = 30$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ... + 'DisplayName', '$m = 45$ kg'); +for i = 2:6 + plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ... + 'DisplayName', '$m = 0$ kg (model)'); +plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ... + 'DisplayName', '$m = 15$ kg (model)'); +plot(freqs, abs(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ... + 'DisplayName', '$m = 30$ kg (model)'); +plot(freqs, abs(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ... + 'DisplayName', '$m = 45$ kg (model)'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-4]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,1,1)), f), 'color', [colors(1,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m0(:,i, i)), f), 'color', [colors(1,:), 0.5]); +end +plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,1,1)), f), 'color', [colors(2,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m1(:,i, i)), f), 'color', [colors(2,:), 0.5]); +end +plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,1,1)), f), 'color', [colors(3,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m2(:,i, i)), f), 'color', [colors(3,:), 0.5]); +end +plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,1,1)), f), 'color', [colors(4,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*unwrapphase(angle(G_hac_m3(:,i, i)), f), 'color', [colors(4,:), 0.5]); +end +plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(1,:)); +plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(2,:)); +plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(3,:)); +plot(freqs, 180/pi*unwrapphase(angle(squeeze(freqresp(exp(-3e-4*s)*Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), f), '--', 'color', colors(4,:)); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-270, 20]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_comp_plant_model_damped_diag.pdf', 'width', 1000, 'height', 1000, 'transparent', false); +#+end_src + +#+RESULTS: +[[file:figs/sri2024_comp_plant_model_damped_diag.png]] + +** Tomography scans +#+begin_src matlab :exports none +data_tomo_30rpm_m0 = load(sprintf("%s/scans/2023-08-17_15-26_tomography_30rpm_m0_robust.mat", mat_dir)); +data_tomo_30rpm_m0.time = Ts*[0:length(data_tomo_30rpm_m0.Rz)-1]; +yztomographymoviesri('movies/sri2024_tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-1.5, 1.5]) +#+end_src + +** Dy scan +#+begin_src matlab +%% Slow Ty scan (10um/s) +data_ty_ol_slow = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat", mat_dir)); +data_ty_ol_slow.time = Ts*[0:length(data_ty_ol_slow.Dy_int)-1]; +data_ty_ol_slow.e_dy = detrend(data_ty_ol_slow.e_dy, 0); +data_ty_ol_slow.e_dz = detrend(data_ty_ol_slow.e_dz, 0); +#+end_src + +#+begin_src matlab +fig = figure; +tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(ax1, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dy, '-', 'color', [colors(1,:)],'LineWidth',1); +% plot(ax1, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dy, '-', 'color', [colors(2,:)],'LineWidth',1); +hold off; +set(gca, 'XTickLabel',[]); +ylabel("$D_y$ error [$\mu m$]"); +ax2 = nexttile; +hold on; +plot(ax2, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dz, '-', 'color', [colors(1,:)],'LineWidth',1); +% plot(ax2, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dz, '-', 'color', [colors(2,:)],'LineWidth',1); +hold off; +xlabel("$D_y$ setpoint [$\mu m$]"); +ylabel("Z motion [$\mu m$]"); +linkaxes([ax1,ax2],'x'); +xlim([-100, 100]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_ty_scan_ol.pdf', 'width', 600, 'height', 1000, 'transparent', false); +#+end_src + +#+name: fig:sri2024_ty_scan_ol +#+caption: description +#+RESULTS: +[[file:figs/sri2024_ty_scan_ol.png]] + +#+begin_src matlab +fig = figure; +tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(ax1, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dy, '-', 'color', [colors(1,:)],'LineWidth',1, 'DisplayName', 'OL'); +plot(ax1, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dy, '-', 'color', [colors(2,:)],'LineWidth',1, 'DisplayName', sprintf('CL $\\epsilon_y = %.1f$ nm', 1e9*rms(data_ty_cl_slow.e_dy))); +hold off; +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +set(gca, 'XTickLabel',[]); +ylabel("$D_y$ error [$\mu m$]"); +ax2 = nexttile; +hold on; +plot(ax2, 1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_dz, '-', 'color', [colors(1,:)],'LineWidth',1, 'DisplayName', 'OL'); +plot(ax2, 1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_dz, '-', 'color', [colors(2,:)],'LineWidth',1, 'DisplayName', sprintf('CL $\\epsilon_z = %.1f$ nm', 1e9*rms(data_ty_cl_slow.e_dz))); +hold off; +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +xlabel("$D_y$ setpoint [$\mu m$]"); +ylabel("Z motion [$\mu m$]"); +linkaxes([ax1,ax2],'x'); +xlim([-100, 100]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_ty_scan_cl.pdf', 'width', 600, 'height', 1000, 'transparent', false); +#+end_src + +#+name: fig:sri2024_ty_scan_cl +#+caption: description +#+RESULTS: +[[file:figs/sri2024_ty_scan_cl.png]] + +#+begin_src matlab +fig = figure; +tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile; +hold([ax1,ax2], 'on'); +plot(ax1, 1e6*data_ol.Ty, 1e6*data_ol.e_dy, '-', 'color', [colors(1,:)],'LineWidth',1); +plot(ax1, 1e6*data_cl.Ty, 1e6*data_cl.e_dy, '-', 'color', [colors(2,:)],'LineWidth',1); +set(gca, 'XTickLabel',[]); +ylabel("$D_y$ error [$\mu m$]"); +ax2 = nexttile; +plot(ax2, 1e6*data_cl.Ty, 1e6*data_cl.e_dz, '-', 'color', [colors(2,:)],'LineWidth',1); +plot(ax2, 1e6*data_ol.Ty, 1e6*data_ol.e_dz, '-', 'color', [colors(1,:)],'LineWidth',1); +xlabel("$D_y$ setpoint [$\mu m$]"); +ylabel("Z motion [$\mu m$]"); +#+end_src + +#+begin_src matlab +dyscanmoviesri('movies/sri2024_ty_ol_slow', data_ty_ol_slow, 'xlim_ax1', [-100, 100], 'ylim_ax1', [-1.5, 1.5], 'xlim_ax2', [-100, 100], 'ylim_ax2', [-1, 1]) +#+end_src + +#+begin_src matlab +%% Slow Ty scan (10um/s) - CL +data_ty_cl_slow = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat", mat_dir)); +data_ty_cl_slow.time = Ts*[0:length(data_ty_cl_slow.Dy_int)-1]; +#+end_src + +#+begin_src matlab +dyscanclmoviesri('movies/sri2024_ty_cl_slow', data_ty_ol_slow, data_ty_cl_slow, 'xlim_ax1', [-100, 100], 'ylim_ax1', [-1.5, 1.5], 'xlim_ax2', [-100, 100], 'ylim_ax2', [-0.5, 0.5]) +#+end_src + +** Ry (reflectivity) scan +#+begin_src matlab +%% Load data for the reflectivity scan +data_ry = load(sprintf("%s/scans/2023-08-18_15-24_first_reflectivity_m0.mat", mat_dir)); +data_ry.time = Ts*[0:length(data_ry.Ry_int)-1]; +#+end_src + +#+begin_src matlab +fig = figure; +tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(data_ry.time, 1e6*data_ry.Ry_int, 'color', colors(2,:), 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry))) +plot(data_ry.time, 1e6*data_ry.m_hexa_ry, 'k--', 'DisplayName', '$R_y$ setpoint') +hold off; +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +set(gca, 'XTickLabel',[]); +ylabel("$R_y$ motion [$\mu$rad]"); +ylim([-310, 310]) +ax2 = nexttile; +hold on; +plot(data_ry.time, 1e9*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy))) +plot(data_ry.time, 1e9*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz))) +hold off; +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +xlabel("Time [s]"); +ylabel("$D_y$, $D_z$ motion [nm]"); +ylim([-150, 150]) +linkaxes([ax1,ax2],'x'); +xlim([0, 6.2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_ry_scan_cl.pdf', 'width', 600, 'height', 1000, 'transparent', false); +#+end_src + +#+name: fig:sri2024_ry_scan_cl +#+caption: description +#+RESULTS: +[[file:figs/sri2024_ry_scan_cl.png]] + + +** Dz scans +#+begin_src matlab +data_dz_10ums = load(sprintf("%s/scans/2023-08-18_15-33_dirty_layer_m0_small.mat", mat_dir)); +data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1]; +#+end_src + +#+begin_src matlab +data_dz_100ums = load(sprintf("%s/scans/2023-08-18_15-32_dirty_layer_m0.mat", mat_dir)); +data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1]; +#+end_src + +#+begin_src matlab +fig = figure; +tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, ... + 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry))) +plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_dy, ... + 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy))) +plot(data_dz_10ums.time, 1e6*data_dz_10ums.Dz_int, ... + 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dz))) +plot(data_dz_10ums.time, 1e6*data_dz_10ums.m_hexa_dz, 'k--', ... + 'DisplayName', 'Setpoint, $10\mu$m/s') +hold off; +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +xlabel("Time [s]"); +ylabel("Measured motion [$\mu$m, $\mu$rad]"); +ylim([-11, 11]) + +ax2 = nexttile; +hold on; +plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, ... + 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry))) +plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_dy, ... + 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy))) +plot(data_dz_100ums.time, 1e6*data_dz_100ums.Dz_int, ... + 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dz))) +plot(data_dz_100ums.time, 1e6*data_dz_100ums.m_hexa_dz, 'k--', ... + 'DisplayName', 'Setpoint, $10\mu$m/s') +hold off; +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +xlabel("Time [s]"); +ylabel("Measured motion [$\mu$m, $\mu$rad]"); +ylim([-110, 110]) +linkaxes([ax1,ax2],'x'); +xlim([0, 2.2]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sri2024_tz_scan_cl.pdf', 'width', 600, 'height', 1000, 'transparent', false); +#+end_src + +#+name: fig:sri2024_tz_scan_cl +#+caption: description +#+RESULTS: +[[file:figs/sri2024_tz_scan_cl.png]] + * Helping Functions :noexport: ** =unwrapphase= :PROPERTIES: @@ -13135,6 +13891,7 @@ lion.J_int_to_X = [ 0 0 -0.787401574803149 -0.212 #+begin_src matlab save('./mat/stages.mat', 'lion', '-append'); #+end_src + ** Initialize Disturbances :PROPERTIES: :header-args:matlab+: :tangle ./matlab/src/initializeDisturbances.m diff --git a/figs/test_id31_Rz_align_correct.pdf b/figs/test_id31_Rz_align_correct.pdf new file mode 100644 index 0000000..eb19536 Binary files /dev/null and b/figs/test_id31_Rz_align_correct.pdf differ diff --git a/figs/test_id31_Rz_align_correct.png b/figs/test_id31_Rz_align_correct.png new file mode 100644 index 0000000..45c5ae7 Binary files /dev/null and b/figs/test_id31_Rz_align_correct.png differ diff --git a/figs/test_id31_Rz_align_error.pdf b/figs/test_id31_Rz_align_error.pdf new file mode 100644 index 0000000..a56c007 Binary files /dev/null and b/figs/test_id31_Rz_align_error.pdf differ diff --git a/figs/test_id31_Rz_align_error.png b/figs/test_id31_Rz_align_error.png new file mode 100644 index 0000000..772c6a7 Binary files /dev/null and b/figs/test_id31_Rz_align_error.png differ diff --git a/figs/test_id31_comp_simscape_iff_diag_masses.pdf b/figs/test_id31_comp_simscape_iff_diag_masses.pdf new file mode 100644 index 0000000..6a0815e Binary files /dev/null and b/figs/test_id31_comp_simscape_iff_diag_masses.pdf differ diff --git a/figs/test_id31_comp_simscape_iff_diag_masses.png b/figs/test_id31_comp_simscape_iff_diag_masses.png new file mode 100644 index 0000000..4e82bf5 Binary files /dev/null and b/figs/test_id31_comp_simscape_iff_diag_masses.png differ diff --git a/figs/test_id31_comp_simscape_int_diag_masses.pdf b/figs/test_id31_comp_simscape_int_diag_masses.pdf new file mode 100644 index 0000000..6480369 Binary files /dev/null and b/figs/test_id31_comp_simscape_int_diag_masses.pdf differ diff --git a/figs/test_id31_comp_simscape_int_diag_masses.png b/figs/test_id31_comp_simscape_int_diag_masses.png new file mode 100644 index 0000000..d2811e4 Binary files /dev/null and b/figs/test_id31_comp_simscape_int_diag_masses.png differ diff --git a/figs/test_id31_effect_rotation_coupling.pdf b/figs/test_id31_effect_rotation_coupling.pdf new file mode 100644 index 0000000..bbb451d Binary files /dev/null and b/figs/test_id31_effect_rotation_coupling.pdf differ diff --git a/figs/test_id31_effect_rotation_coupling.png b/figs/test_id31_effect_rotation_coupling.png new file mode 100644 index 0000000..3185823 Binary files /dev/null and b/figs/test_id31_effect_rotation_coupling.png differ diff --git a/figs/test_id31_effect_rotation_direct.pdf b/figs/test_id31_effect_rotation_direct.pdf new file mode 100644 index 0000000..e711782 Binary files /dev/null and b/figs/test_id31_effect_rotation_direct.pdf differ diff --git a/figs/test_id31_effect_rotation_direct.png b/figs/test_id31_effect_rotation_direct.png new file mode 100644 index 0000000..5c05bf8 Binary files /dev/null and b/figs/test_id31_effect_rotation_direct.png differ diff --git a/figs/test_id31_first_id_iff.pdf b/figs/test_id31_first_id_iff.pdf new file mode 100644 index 0000000..26a8e11 Binary files /dev/null and b/figs/test_id31_first_id_iff.pdf differ diff --git a/figs/test_id31_first_id_iff.png b/figs/test_id31_first_id_iff.png new file mode 100644 index 0000000..e7f1403 Binary files /dev/null and b/figs/test_id31_first_id_iff.png differ diff --git a/figs/test_id31_first_id_int.pdf b/figs/test_id31_first_id_int.pdf new file mode 100644 index 0000000..ba84424 Binary files /dev/null and b/figs/test_id31_first_id_int.pdf differ diff --git a/figs/test_id31_first_id_int.png b/figs/test_id31_first_id_int.png new file mode 100644 index 0000000..f233c07 Binary files /dev/null and b/figs/test_id31_first_id_int.png differ diff --git a/figs/test_id31_first_id_int_better_rz_align.pdf b/figs/test_id31_first_id_int_better_rz_align.pdf new file mode 100644 index 0000000..f81edae Binary files /dev/null and b/figs/test_id31_first_id_int_better_rz_align.pdf differ diff --git a/figs/test_id31_first_id_int_better_rz_align.png b/figs/test_id31_first_id_int_better_rz_align.png new file mode 100644 index 0000000..5e4de11 Binary files /dev/null and b/figs/test_id31_first_id_int_better_rz_align.png differ diff --git a/figs/test_id31_picture_mass_m0.jpg b/figs/test_id31_picture_mass_m0.jpg new file mode 100644 index 0000000..4175150 Binary files /dev/null and b/figs/test_id31_picture_mass_m0.jpg differ diff --git a/figs/test_id31_picture_mass_m1.jpg b/figs/test_id31_picture_mass_m1.jpg new file mode 100644 index 0000000..a120eca Binary files /dev/null and b/figs/test_id31_picture_mass_m1.jpg differ diff --git a/figs/test_id31_picture_mass_m2.jpg b/figs/test_id31_picture_mass_m2.jpg new file mode 100644 index 0000000..6d6aaba Binary files /dev/null and b/figs/test_id31_picture_mass_m2.jpg differ diff --git a/figs/test_id31_picture_mass_m3.jpg b/figs/test_id31_picture_mass_m3.jpg new file mode 100644 index 0000000..856476e Binary files /dev/null and b/figs/test_id31_picture_mass_m3.jpg differ diff --git a/preamble_extra.tex b/preamble_extra.tex new file mode 100644 index 0000000..98cfc04 --- /dev/null +++ b/preamble_extra.tex @@ -0,0 +1,134 @@ +\usepackage{float} +\usepackage{enumitem} + +\usepackage{caption,tabularx,booktabs} +\usepackage{bm} + +\usepackage{xpatch} % Recommanded for biblatex +\usepackage[ % use biblatex for bibliography + backend=biber, % use biber backend (bibtex replacement) or bibtex + style=ieee, % bib style + hyperref=true, % activate hyperref support + backref=true, % activate backrefs + isbn=false, % don't show isbn tags + url=false, % don't show url tags + doi=false, % don't show doi tags + urldate=long, % display type for dates + maxnames=3, % + minnames=1, % + maxbibnames=5, % + minbibnames=3, % + maxcitenames=2, % + mincitenames=1 % + ]{biblatex} + +\setlength\bibitemsep{1.1\itemsep} + +\usepackage{caption} +\usepackage{subcaption} + +\captionsetup[figure]{labelfont=bf} +\captionsetup[subfigure]{labelfont=bf} +\captionsetup[listing]{labelfont=bf} +\captionsetup[table]{labelfont=bf} + +\usepackage{xcolor} + +\definecolor{my-blue}{HTML}{6b7adb} +\definecolor{my-pale-blue}{HTML}{e6e9f9} +\definecolor{my-red}{HTML}{db6b6b} +\definecolor{my-pale-red}{HTML}{f9e6e6} +\definecolor{my-green}{HTML}{6bdbb6} +\definecolor{my-pale-green}{HTML}{e6f9f3} +\definecolor{my-yellow}{HTML}{dbd26b} +\definecolor{my-pale-yellow}{HTML}{f9f7e6} +\definecolor{my-orange}{HTML}{dba76b} +\definecolor{my-pale-orange}{HTML}{f9f0e6} +\definecolor{my-grey}{HTML}{a3a3a3} +\definecolor{my-pale-grey}{HTML}{f0f0f0} +\definecolor{my-turq}{HTML}{6bc7db} +\definecolor{my-pale-turq}{HTML}{e6f6f9} + +\usepackage{inconsolata} + +\usepackage[newfloat=true, chapter]{minted} +\usemintedstyle{autumn} + +\setminted{frame=lines,breaklines=true,tabsize=4,fontsize=\scriptsize,autogobble=true,labelposition=topline,bgcolor=my-pale-grey} +\setminted[matlab]{label=Matlab} +\setminted[latex]{label=LaTeX} +\setminted[bash]{label=Bash} +\setminted[python]{label=Python} +\setminted[text]{label=Results} +\setminted[md]{label=Org Mode} + +\setmintedinline{fontsize=\normalsize,bgcolor=my-pale-grey} + +\usepackage[most]{tcolorbox} + +\tcbuselibrary{minted} + +\newtcolorbox{seealso}{ enhanced,breakable,colback=my-pale-grey,colframe=my-grey,fonttitle=\bfseries,title=See Also} +\newtcolorbox{hint}{ enhanced,breakable,colback=my-pale-grey,colframe=my-grey,fonttitle=\bfseries,title=Hint} +\newtcolorbox{definition}{enhanced,breakable,colback=my-pale-red, colframe=my-red, fonttitle=\bfseries,title=Definition} +\newtcolorbox{important}{ enhanced,breakable,colback=my-pale-red, colframe=my-red, fonttitle=\bfseries,title=Important} +\newtcolorbox{exampl}[1][]{ enhanced,breakable,colback=my-pale-green,colframe=my-green,fonttitle=\bfseries,title=Example,#1} +\newtcolorbox{exercice}{ enhanced,breakable,colback=my-pale-yellow,colframe=my-yellow,fonttitle=\bfseries,title=Exercice} +\newtcolorbox{question}{ enhanced,breakable,colback=my-pale-yellow,colframe=my-yellow,fonttitle=\bfseries,title=Question} +\newtcolorbox{answer}{ enhanced,breakable,colback=my-pale-turq,colframe=my-turq,fonttitle=\bfseries,title=Answer} +\newtcolorbox{summary}{ enhanced,breakable,colback=my-pale-blue,colframe=my-blue,fonttitle=\bfseries,title=Summary} +\newtcolorbox{note}{ enhanced,breakable,colback=my-pale-blue,colframe=my-blue,fonttitle=\bfseries,title=Note} +\newtcolorbox{caution}{ enhanced,breakable,colback=my-pale-orange,colframe=my-orange,fonttitle=\bfseries,title=Caution} +\newtcolorbox{warning}{ enhanced,breakable,colback=my-pale-orange,colframe=my-orange,fonttitle=\bfseries,title=Warning} + +\newtcolorbox{my-quote}[1]{% + colback=my-pale-grey, + grow to right by=-10mm, + grow to left by=-10mm, + boxrule=0pt, + boxsep=0pt, + breakable, + enhanced jigsaw, + borderline west={4pt}{0pt}{my-grey}} + +\renewenvironment{quote}{\begin{my-quote}}{\end{my-quote}} + +\newtcolorbox{my-verse}[1]{% + colback=my-pale-grey, + grow to right by=-10mm, + grow to left by=-10mm, + boxrule=0pt, + boxsep=0pt, + breakable, + enhanced jigsaw, + borderline west={4pt}{0pt}{my-grey}} + +\renewenvironment{verse}{\begin{my-verse}}{\end{my-verse}} + +\usepackage{environ}% http://ctan.org/pkg/environ +\NewEnviron{aside}{% + \marginpar{\BODY} +} + +\renewenvironment{verbatim}{\VerbatimEnvironment\begin{minted}[]{text}}{\end{minted}} + +\usepackage{soul} +\sethlcolor{my-pale-grey} + +\let\OldTexttt\texttt +\renewcommand{\texttt}[1]{{\ttfamily\hl{\mbox{\,#1\,}}}} + +\makeatletter +\preto\Gin@extensions{png,} +\DeclareGraphicsRule{.png}{pdf}{.pdf}{\noexpand\Gin@base.pdf} +\preto\Gin@extensions{gif,} +\DeclareGraphicsRule{.gif}{png}{.png}{\noexpand\Gin@base.png} +\makeatother + +\usepackage{hyperref} +\hypersetup{ + colorlinks = true, + allcolors = my-blue +} + +\usepackage{hypcap} diff --git a/test-bench-id31.org b/test-bench-id31.org index 9ba7273..c7fd817 100644 --- a/test-bench-id31.org +++ b/test-bench-id31.org @@ -100,6 +100,9 @@ ** Notes Prefix is =test_id31= +data_dir = "/home/thomas/mnt/data_id31/id31nass/id31/20230801/RAW_DATA" +mat_dir = "/home/thomas/mnt/data_id31/nass" + *Goals*: - Short stroke metrology - Complete validation of the concept + nano-hexapod + instrumentation + control system @@ -130,6 +133,54 @@ Prefix is =test_id31= - Reflectivity - Diffraction tomography +** TODO [#A] Make a nice schematic with all the signal names + +- Force sensors +- Encoders +- Interferometers +- Command signal +- Estimated error from external metrology +- ... + +** DONE [#A] Resolve height issue in Simscape model +CLOSED: [2024-11-13 Wed 11:53] + +For the current (ID31) model, the beam is 175mm above the nano-hexapod top platform. +It should be 150mm (25mm offset). + + +Check the micro-station model. + + +Height granite <=> micro-heapod = 530mm (OK Model/Solidworks) +Height of nano-heaxapod = 95mm +Height granite <=> beam = 800 mm + +This means that height of nano-hexapod <=> beam is 800 - 530 - 95 = *175mm and not 150mm*. + + +- [X] *I need to know what was used during the experiments!* + *150mm* was used during the experiments +- [X] It should be compatible for the Jacobian used for the short stroke metrology + it seems 150mm was used for the metrology jacobian! +- [X] If something is change, update the previous Simscape models + +** TODO [#B] Should the micro-hexapod position be adjusted to match the experiment + +After alignment, the micro-hexapod position was *h1tz = -17.72101mm*. +I suppose compared to the initial height of 350mm + +** TODO [#A] Maybe just need one mass for the first identification + +First identification: +- compare with Simscape +- High coupling +- Check Rz alignment +- Correct Rz alignment +- New identification for all masses +- Better match with Simscape model! + +** QUES [#A] Why now we have minimum phase zero for IFF Plant? ** CANC [#C] Find identification where Rz was not taken into account CLOSED: [2024-11-12 Tue 16:03] @@ -177,6 +228,9 @@ High authority control is then applied #+end_figure * Short Stroke Metrology System +:PROPERTIES: +:header-args:matlab+: :tangle matlab/test_id31_1_metrology.m +:END: <> ** Introduction :ignore: @@ -629,6 +683,1822 @@ exportFig('figs/test_id31_xy_map_sphere.pdf', 'width', 'half', 'height', 'normal #+end_subfigure #+end_figure +* Identified Open Loop Plant +:PROPERTIES: +:header-args:matlab+: :tangle matlab/test_id31_2_open_loop_plant.m +:END: +<> +** Introduction :ignore: + +** Matlab Init :noexport:ignore: +#+begin_src matlab +%% test_id31_2_open_loop_plant.m +#+end_src + +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> +#+end_src + +#+begin_src matlab :noweb yes +<> +#+end_src + +#+begin_src matlab :noweb yes +<> +#+end_src + +** First Open-Loop Plant Identification +<> + +The plant dynamics is first identified for a fixed spindle angle (at $0\,\text{deg}$) and without any payload. +The model dynamics is also identified in the same conditions. + +A first comparison between the model and the measured dynamics is done in Figure ref:fig:test_id31_first_id. +A good match can be observed for the diagonal dynamics (except the high frequency modes which are not modeled). +However, the coupling for the transfer function from command signals $\mathbf{u}$ to estimated strut motion from the external metrology $e\mathbf{\mathcal{L}}$ is larger than expected (Figure ref:fig:test_id31_first_id_int). + +#+begin_src matlab +%% Identify the plant dynamics using the Simscape model + +% Initialize each Simscape model elements +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeNanoHexapod('flex_bot_type', '2dof', ... + 'flex_top_type', '3dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof'); +initializeSample('type', '0'); + +initializeSimscapeConfiguration('gravity', false); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); +initializeReferences(); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors + +% With no payload +Gm = linearize(mdl, io); +Gm.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; +#+end_src + +#+begin_src matlab +%% Identify the plant from experimental data + +% Load identification data +data = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat'); + +% Sampling Time [s] +Ts = 1e-4; + +% Hannning Windows +Nfft = floor(2.0/Ts); +win = hanning(Nfft); +Noverlap = floor(Nfft/2); + +% And we get the frequency vector +[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, Noverlap, Nfft, 1/Ts); + +% IFF Plant (transfer function from u to taum) +G_iff = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +% INT Plant (transfer function from u to eL) +G_int = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ... + 'DisplayName', '$e\mathcal{L}_i/u_i$ meas'); +plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ... + 'DisplayName', '$e\mathcal{L}_i/u_i$ model'); +for i = 2:6 + plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ... + 'HandleVisibility', 'off'); +end +plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$e\mathcal{L}_i/u_j$ meas'); +plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', '$e\mathcal{L}_i/u_j$ model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([2e-9, 2e-4]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_first_id_int.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+begin_src matlab :exports none :results none +%% Comparison between the measured dynamics and the model dynamics - Force Sensors +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_iff(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Fn%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_iff(:,1, 1)), 'color', [colors(1,:)], ... + 'DisplayName', '$\tau_{m,i}/u_i$ meas'); +plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ... + 'DisplayName', '$\tau_{m,i}/u_i$ model'); +for i = 2:6 + plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:)], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ... + 'HandleVisibility', 'off'); +end +plot(f, abs(G_iff(:, 1, 2)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$\tau_{m,i}/u_j$ meas'); +plot(freqs, abs(squeeze(freqresp(Gm(sprintf('Fn%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', '$\tau_{m,i}/u_j$ model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_iff(:,i, i)), 'color', [colors(1,:)]); +end +for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_first_id_iff.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+name: fig:test_id31_first_id +#+caption: Comparison between the measured dynamics and the multi-body model dynamics. Both for the external metrology (\subref{fig:test_id31_first_id_int}) and force sensors (\subref{fig:test_id31_first_id_iff}). +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_first_id_int}External Metrology} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/test_id31_first_id_int.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_first_id_iff}Force Sensors} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/test_id31_first_id_iff.png]] +#+end_subfigure +#+end_figure + +** Better Angular Alignment +<> + +One possible explanation of the increased coupling observed in Figure ref:fig:test_id31_first_id_int is the poor alignment between the external metrology axes (i.e. the interferometer supports) and the nano-hexapod axes. +To estimate this alignment, a decentralized low-bandwidth feedback controller based on the nano-hexapod encoders is implemented. +This allowed to perform two straight movements of the nano-hexapod along the $x$ and $y$ axes in the frame of the nano-hexapod. +During these two movements, the external metrology measurement is recorded and shown in Figure ref:fig:test_id31_Rz_align_error. +It was found that there is a misalignment of 2.7 degrees (rotation along the vertical axis) between the interferometer axes and nano-hexapod axes. +This was corrected by adding an offset to the spindle angle. +To check that the alignment has improved, the same movement was performed using the nano-hexapod while recording the signal of the external metrology. +Results shown in Figure ref:fig:test_id31_Rz_align_correct are indeed indicating much better alignment. + +#+begin_src matlab +%% Load Data +data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2); +data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3); +data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1); +data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2); +#+end_src + +#+begin_src matlab +% Estimation of Rz misalignment +p1 = polyfit(data_1_dx.Dx_int_filtered, data_1_dx.Dy_int_filtered, 1); +p2 = polyfit(data_1_dy.Dx_int_filtered, data_1_dy.Dy_int_filtered, 1); + +Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~3 degrees +#+end_src + +#+begin_src matlab +%% Estimation of the Rz misalignment +figure; +hold on; +plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'Measurement') +plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off') +plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$R_z = %.1f$ deg', Rz_error*180/pi)) +plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off') +hold off; +xlabel('Interf $D_x$ [$\mu$m]'); +ylabel('Interf $D_y$ [$\mu$m]'); +axis equal +xlim([-10, 10]); ylim([-10, 10]); +xticks([-10:5:10]); yticks([-10:5:10]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_Rz_align_error.pdf', 'width', 'half', 'height', 'normal'); +#+end_src + +#+begin_src matlab +% Estimation of Rz misalignment after correcting the Rz angle +p1 = polyfit(data_2_dx.Dx_int_filtered, data_2_dx.Dy_int_filtered, 1); +p2 = polyfit(data_2_dy.Dx_int_filtered, data_2_dy.Dy_int_filtered, 1); + +Rz_error = (atan(p1(1)) + atan(p2(1))-pi/2)/2; % ~0.2 degrees +#+end_src + +#+begin_src matlab +%% Estimation of the Rz misalignment after correcting the Rz offset +figure; +hold on; +plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(5,:), 'DisplayName', 'Measurement') +plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(5,:), 'HandleVisibility', 'off') +plot( 1e6*[-10:10]*cos(Rz_error), 1e6*[-10:10]*sin(Rz_error), 'k--', 'DisplayName', sprintf('$R_z = %.1f$ deg', Rz_error*180/pi)) +plot(-1e6*[-10:10]*sin(Rz_error), 1e6*[-10:10]*cos(Rz_error), 'k--', 'HandleVisibility', 'off') +hold off; +xlabel('Interf $D_x$ [$\mu$m]'); +ylabel('Interf $D_y$ [$\mu$m]'); +axis equal +xlim([-10, 10]); ylim([-10, 10]); +xticks([-10:5:10]); yticks([-10:5:10]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_Rz_align_correct.pdf', 'width', 'half', 'height', 'normal'); +#+end_src + +#+name: fig:test_id31_Rz_align_error +#+caption: Measurement of the Nano-Hexapod axes in the frame of the external metrology. Before alignment (\subref{fig:test_id31_Rz_align_error}) and after alignment (\subref{fig:test_id31_Rz_align_correct}). +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_error}Before alignment} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/test_id31_Rz_align_error.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_Rz_align_correct}After alignment} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/test_id31_Rz_align_correct.png]] +#+end_subfigure +#+end_figure + +** Open-Loop Identification after alignment +<> + +The plant dynamics is identified after the fine alignment and is compared with the model dynamics in Figure ref:fig:test_id31_first_id_int_better_rz_align. +Compared to the initial identification shown in Figure ref:fig:test_id31_first_id_int, the obtained coupling has decreased and is close to the coupling obtained with the multi-body model. + +#+begin_src matlab +%% Identification of the plant after Rz alignment +data_align = load('2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat'); + +G_int_align = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none :results none +%% Decrease of the coupling with better Rz alignment +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$e\mathcal{L}_i/u_j$'); +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ... + 'DisplayName', '$e\mathcal{L}_i/u_j$ - $R_z$ align'); +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ... + 'DisplayName', '$e\mathcal{L}_i/u_i$'); +for i = 2:6 + plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ... + 'HandleVisibility', 'off'); +end +plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ... + 'DisplayName', '$e\mathcal{L}_i/u_i$ - $R_z$ align'); +for i = 2:6 + plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-4]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]); +end +for i = 1:6 + plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_align(:, i, j)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(1,:)], ... + 'DisplayName', '$e\mathcal{L}_i/u_i$ meas'); +plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ... + 'DisplayName', '$e\mathcal{L}_i/u_i$ model'); +for i = 2:6 + plot(f, abs(G_int_align(:,i, i)), 'color', [colors(1,:)], ... + 'HandleVisibility', 'off'); + plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ... + 'HandleVisibility', 'off'); +end +plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$e\mathcal{L}_i/u_j$ meas'); +plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', '$e\mathcal{L}_i/u_j$ model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +xlim([1, 1e3]); ylim([2e-9, 2e-4]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/test_id31_first_id_int_better_rz_align.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_id31_first_id_int_better_rz_align +#+caption: Decrease of the coupling with better Rz alignment +#+RESULTS: +[[file:figs/test_id31_first_id_int_better_rz_align.png]] + +** Effect of Payload Mass +<> + +The system dynamics was identified with four payload conditions that are shown in Figure ref:fig:test_id31_picture_masses. +The obtained direct terms are compared with the model dynamics in Figure ref:fig:test_nhexa_comp_simscape_diag_masses. + +#+name: fig:test_id31_picture_masses +#+caption: The four tested payload conditions. (\subref{fig:test_id31_picture_mass_m0}) without payload. (\subref{fig:test_id31_picture_mass_m1}) with $13\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m2}) with $26\,\text{kg}$ payload. (\subref{fig:test_id31_picture_mass_m3}) with $39\,\text{kg}$ payload. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m0}$m=0\,\text{kg}$} +#+attr_latex: :options {0.24\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.99\linewidth +[[file:figs/test_id31_picture_mass_m0.jpg]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m1}$m=13\,\text{kg}$} +#+attr_latex: :options {0.24\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.99\linewidth +[[file:figs/test_id31_picture_mass_m1.jpg]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m2}$m=26\,\text{kg}$} +#+attr_latex: :options {0.24\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.99\linewidth +[[file:figs/test_id31_picture_mass_m2.jpg]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_picture_mass_m3}$m=39\,\text{kg}$} +#+attr_latex: :options {0.24\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.99\linewidth +[[file:figs/test_id31_picture_mass_m3.jpg]] +#+end_subfigure +#+end_figure + +#+begin_src matlab +%% Identify the plant from experimental data - All payloads + +% Load identification data +data_m0_Wz0 = load('2023-08-08_16-17_ol_plant_m0_Wz0.mat'); +data_m1_Wz0 = load('2023-08-08_18-57_ol_plant_m1_Wz0.mat'); +data_m2_Wz0 = load('2023-08-08_17-12_ol_plant_m2_Wz0.mat'); +data_m3_Wz0 = load('2023-08-08_18-20_ol_plant_m3_Wz0.mat'); + +% Sampling Time [s] +Ts = 1e-4; + +% Hannning Windows +Nfft = floor(2.0/Ts); +win = hanning(Nfft); +Noverlap = floor(Nfft/2); + +% And we get the frequency vector +[~, f] = tfestimate(data_m0_Wz0.uL1.id_plant, data_m0_Wz0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts); + +% No payload +G_iff_m0_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +G_int_m0_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz0.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_m0_Wz0(:,:,i_strut) = tfestimate(data_m0_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +% 1 "payload layer" +G_iff_m1_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +G_int_m1_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m1_Wz0.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_m1_Wz0(:,:,i_strut) = tfestimate(data_m1_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +% 2 "payload layers" +G_iff_m2_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +G_int_m2_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m2_Wz0.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_m2_Wz0(:,:,i_strut) = tfestimate(data_m2_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +% 3 "payload layers" +G_iff_m3_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +G_int_m3_Wz0 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L1 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L2 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L3 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L4 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L5 ; data_m3_Wz0.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_m3_Wz0(:,:,i_strut) = tfestimate(data_m3_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Identify the model dynamics for all payload conditions +% Initialize each Simscape model elements +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeNanoHexapod('flex_bot_type', '2dof', ... + 'flex_top_type', '3dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof'); +initializeSample('type', '0'); + +initializeSimscapeConfiguration('gravity', false); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); +initializeController('type', 'open-loop'); +initializeReferences(); + +% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors + +initializeSample('type', '0'); +Gm_m0_Wz0 = linearize(mdl, io); +Gm_m0_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm_m0_Wz0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; + +initializeSample('type', '1'); +Gm_m1_Wz0 = linearize(mdl, io); +Gm_m1_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm_m1_Wz0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; + +initializeSample('type', '2'); +Gm_m2_Wz0 = linearize(mdl, io); +Gm_m2_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm_m2_Wz0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; + +initializeSample('type', '3'); +Gm_m3_Wz0 = linearize(mdl, io); +Gm_m3_Wz0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm_m3_Wz0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', 'Meas (0kg)'); +for i = 2:6 + plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_int_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', 'Meas (13kg)'); +for i = 2:6 + plot(f, abs(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_int_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', 'Meas (26kg)'); +for i = 2:6 + plot(f, abs(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_int_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ... + 'DisplayName', 'Meas (39kg)'); +for i = 2:6 + plot(f, abs(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ... + 'DisplayName', 'Model (0kg)'); +plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ... + 'DisplayName', 'Model (13kg)'); +plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ... + 'DisplayName', 'Model (26kg)'); +plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ... + 'DisplayName', 'Model (39kg)'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 5e-4]); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_int_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_int_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_int_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]); +end +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:)) +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:)) +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:)) +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:)) +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([10, 5e2]); +xticks([10, 20, 50, 100, 200, 500]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_comp_simscape_int_diag_masses.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', 'Meas (0kg)'); +for i = 2:6 + plot(f, abs(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m1_Wz0(:, 1, 1)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', 'Meas (13kg)'); +for i = 2:6 + plot(f, abs(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m2_Wz0(:, 1, 1)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', 'Meas (26kg)'); +for i = 2:6 + plot(f, abs(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m3_Wz0(:, 1, 1)), 'color', [colors(4,:), 0.5], ... + 'DisplayName', 'Meas (39kg)'); +for i = 2:6 + plot(f, abs(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ... + 'DisplayName', 'Model (0kg)'); +plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ... + 'DisplayName', 'Model (13kg)'); +plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ... + 'DisplayName', 'Model (26kg)'); +plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ... + 'DisplayName', 'Model (39kg)'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(G_iff_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_m1_Wz0(:,i, i)), 'color', [colors(2,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_m2_Wz0(:,i, i)), 'color', [colors(3,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_m3_Wz0(:,i, i)), 'color', [colors(4,:), 0.5]); +end +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:)) +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:)) +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:)) +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3_Wz0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:)) +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([10, 5e2]); +xticks([10, 20, 50, 100, 200, 500]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_comp_simscape_iff_diag_masses.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+name: fig:test_nhexa_comp_simscape_diag_masses +#+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the dynamics identified from the Simscape model. Both for the dynamics from $u$ to $e\mathcal{L}$ (\subref{fig:test_id31_comp_simscape_int_diag_masses}) and from $u$ to $V_s$ (\subref{fig:test_id31_comp_simscape_iff_diag_masses}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_simscape_int_diag_masses}from $u$ to $d_e$} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/test_id31_comp_simscape_int_diag_masses.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_simscape_iff_diag_masses}from $u$ to $V_s$} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/test_id31_comp_simscape_iff_diag_masses.png]] +#+end_subfigure +#+end_figure + +** Effect of Spindle Rotation +<> + +The dynamics was then identified while the Spindle was rotating at constant velocity. +Three identification experiments were performed: no spindle rotation, spindle rotation at $36\,\text{deg}/s$ and at $180\,\text{deg}/s$. + +The comparison of the obtained dynamics from command signal $u$ to estimated strut error $e\mathcal{L}$ is done in Figure ref:fig:test_id31_effect_rotation. +Both direct terms (Figure ref:fig:test_id31_effect_rotation_direct) and coupling terms (Figure ref:fig:test_id31_effect_rotation_coupling) are unaffected by the rotation. +The same can be observed for the dynamics from the command signal to the encoders and to the force sensors. +This confirms that the rotation has no significant effect on the plant dynamics. +This also indicates that the metrology kinematics is correct and is working in real time. + +#+begin_src matlab +%% Identify the model dynamics with Spindle rotation +initializeSample('type', '0'); +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 360/36); +Gm_m0_Wz36 = linearize(mdl, io, 0.1); +Gm_m0_Wz36.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm_m0_Wz36.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; + +initializeReferences(... + 'Rz_type', 'rotating', ... + 'Rz_period', 360/180); +Gm_m0_Wz180 = linearize(mdl, io, 0.1); +Gm_m0_Wz180.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +Gm_m0_Wz180.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... + 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; +#+end_src + +#+begin_src matlab :exports none :tangle no +% The identified dynamics are then saved for further use. +save('./matlab/mat/test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m0_Wz180', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0'); +#+end_src + +#+begin_src matlab :eval no +% The identified dynamics are then saved for further use. +save('./mat/test_id31_simscape_open_loop_plants.mat', 'Gm_m0_Wz0', 'Gm_m0_Wz180', 'Gm_m1_Wz0', 'Gm_m2_Wz0', 'Gm_m3_Wz0'); +#+end_src + +#+begin_src matlab +%% Identify the plant from experimental data - Effect of rotation + +% Load identification data +data_m0_Wz36 = load('2023-08-08_16-28_ol_plant_m0_Wz36.mat'); +data_m0_Wz180 = load('2023-08-08_16-45_ol_plant_m0_Wz180.mat'); + +% Spindle Rotation at 36 deg/s +G_iff_m0_Wz36 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +G_int_m0_Wz36 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz36.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_m0_Wz36(:,:,i_strut) = tfestimate(data_m0_Wz36.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +% Spindle Rotation at 180 deg/s +G_iff_m0_Wz180 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end + +G_int_m0_Wz180 = zeros(length(f), 6, 6); +for i_strut = 1:6 + eL = [data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L1 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L2 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L3 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L4 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L5 ; data_m0_Wz180.(sprintf("uL%i", i_strut)).e_L6]'; + + G_int_m0_Wz180(:,:,i_strut) = tfestimate(data_m0_Wz180.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none :results none +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +nexttile(); +hold on; +plot(f, abs(G_int_m0_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$\Omega_z = 0$'); +plot(f, abs(G_int_m0_Wz36(:, 1, 1)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$\Omega_z = 36$ deg/s'); +plot(f, abs(G_int_m0_Wz180(:, 1, 1)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$\Omega_z = 180$ deg/s'); +for i = 2:6 + plot(f, abs(G_int_m0_Wz0(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off') + plot(f, abs(G_int_m0_Wz36(:,i, i)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off') + plot(f, abs(G_int_m0_Wz180(:,i, i)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off') +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([10, 1e3]); ylim([1e-8, 2e-4]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_effect_rotation_direct.pdf', 'width', 'half', 'height', 'short'); +#+end_src + +#+begin_src matlab :exports none :results none +figure; +tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +nexttile(); +hold on; +plot(f, abs(G_int_m0_Wz0(:, 1, 2)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$\Omega_z = 0$'); +plot(f, abs(G_int_m0_Wz36(:, 1, 2)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$\Omega_z = 36$ deg/s'); +plot(f, abs(G_int_m0_Wz180(:, 1, 2)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$\Omega_z = 180$ deg/s'); +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_int_m0_Wz0(:, i, j)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off'); + plot(f, abs(G_int_m0_Wz36(:, i, j)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off'); + plot(f, abs(G_int_m0_Wz180(:, i, j)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([10, 1e3]); ylim([1e-8, 2e-4]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/test_id31_effect_rotation_coupling.pdf', 'width', 'half', 'height', 'short'); +#+end_src + +#+name: fig:test_id31_effect_rotation +#+caption: Effect of the spindle rotation on the plant dynamics from $u$ to $e\mathcal{L}$. Three rotational velocities are tested ($0\,\text{deg}/s$, $36\,\text{deg}/s$ and $180\,\text{deg}/s$). Both direct terms (\subref{fig:test_id31_effect_rotation_direct}) and coupling terms (\subref{fig:test_id31_effect_rotation_coupling}) are displayed. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_effect_rotation_direct}Direct terms} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/test_id31_effect_rotation_direct.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:test_id31_effect_rotation_coupling}Coupling terms} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/test_id31_effect_rotation_coupling.png]] +#+end_subfigure +#+end_figure + +** Conclusion +:PROPERTIES: +:UNNUMBERED: t +:END: + +Thanks to the model, poor alignment between the nano-hexapod axes and the external metrology axes could be identified. +After alignment, the identified dynamics is well matching with the multi-body model. + +Also, the effects of the payload mass and of the spindle rotation are well capture in the model. + +* Decentralized Integral Force Feedback +:PROPERTIES: +:header-args:matlab+: :tangle matlab/test_id31_3_iff.m +:END: +<> +** Introduction :ignore: + +** Matlab Init :noexport:ignore: +#+begin_src matlab +%% test_id31_3_iff.m +#+end_src + +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> +#+end_src + +#+begin_src matlab :noweb yes +<> +#+end_src + +#+begin_src matlab :noweb yes +<> +#+end_src + +** IFF Plants +*** Introduction :ignore: + +*** 6x6 Plant +#+begin_src matlab +%% Load identification data +data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir)); +#+end_src + +#+begin_src matlab :exports none +% Hannning Windows +Nfft = floor(1.0/Ts); +win = hanning(Nfft); +Noverlap = floor(Nfft/2); +#+end_src + +#+begin_src matlab :exports none +% And we get the frequency vector +[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts); +#+end_src + +#+begin_src matlab :exports none +%% IFF Plant (transfer function from u to taum) +G_iff = zeros(length(f), 6, 6); + +for i_strut = 1:6 + eL = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 1:6 + plot(f, abs(G_iff(:,i, i)), 'color', colors(i,:), ... + 'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i)); +end +plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$\tau_{m,i}/u_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(G_iff(:,i, i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_Giff_plant.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:id31_Giff_plant +#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +#+RESULTS: +[[file:figs/id31_Giff_plant.png]] + +Compare with Model: +#+begin_src matlab +load('Gm_iff.mat'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(f, abs(G_iff(:, i, j)), 'color', [colors(3,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, j), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 2:6 + plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off'); +end +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off'); +end +% plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... +% 'DisplayName', '$\tau_{m,i}/u_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e2]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); + +% ax2 = nexttile; +% hold on; +% for i =1:6 +% plot(f, 180/pi*angle(G_iff(:,i, i))); +% end +% hold off; +% set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +% xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +% hold off; +% yticks(-360:90:360); +% ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +*** Effect of Rotation +#+begin_src matlab +%% Load identification data +data_Wz0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir)); +data_Wz1 = load(sprintf('%s/dynamics/2023-08-08_16-28_ol_plant_m0_Wz36.mat', mat_dir)); +data_Wz2 = load(sprintf('%s/dynamics/2023-08-08_16-45_ol_plant_m0_Wz180.mat', mat_dir)); +#+end_src + +#+begin_src matlab :exports none +% Sampling Time [s] +Ts = 1e-4; + +% Hannning Windows +Nfft = floor(1.0/Ts); +win = hanning(Nfft); +Noverlap = floor(Nfft/2); +#+end_src + +#+begin_src matlab :exports none +% And we get the frequency vector +[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts); +#+end_src + +#+begin_src matlab :exports none +%% IFF Plant (transfer function from u to taum) +G_iff_Wz0 = zeros(length(f), 6, 6); + +for i_strut = 1:6 + eL = [data_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_Wz0(:,:,i_strut) = tfestimate(data_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% IFF Plant (transfer function from u to taum) +G_iff_Wz1 = zeros(length(f), 6, 6); + +for i_strut = 1:6 + eL = [data_Wz1.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_Wz1(:,:,i_strut) = tfestimate(data_Wz1.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% IFF Plant (transfer function from u to taum) +G_iff_Wz2 = zeros(length(f), 6, 6); + +for i_strut = 1:6 + eL = [data_Wz2.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs6]'; + + G_iff_Wz2(:,:,i_strut) = tfestimate(data_Wz2.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Save Identified Plants +save('./mat/G_iff.mat', 'G_iff_Wz0', 'G_iff_Wz1', 'G_iff_Wz2', '-append'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ... + 'DisplayName', '$\Omega_z = 0$'); +for i = 2:6 + plot(f, abs(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_Wz1(:, 1, 1)), 'color', [colors(2,:), 0.2], ... + 'DisplayName', '$\Omega_z = 36$ deg/s'); +for i = 2:6 + plot(f, abs(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_Wz2(:, 1, 1)), 'color', [colors(3,:), 0.2], ... + 'DisplayName', '$\Omega_z = 180$ deg/s'); +for i = 2:6 + plot(f, abs(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2], ... + 'HandleVisibility', 'off') +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_Giff_effect_rotation.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:id31_Giff_effect_rotation +#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +#+RESULTS: +[[file:figs/id31_Giff_effect_rotation.png]] + +*** Effect of Mass +#+begin_src matlab +load('G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$m = 0$'); +for i = 2:6 + plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$m = 1$'); +for i = 2:6 + plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$m = 2$'); +for i = 2:6 + plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ... + 'DisplayName', '$m = 3$'); +for i = 2:6 + plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ... + 'HandleVisibility', 'off') +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]); +end +for i =1:6 + plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_Giff_effect_mass.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:id31_Giff_effect_mass +#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor +#+RESULTS: +[[file:figs/id31_Giff_effect_mass.png]] + +*** Compare with the model +#+begin_src matlab +load('Gm.mat') +#+end_src + +#+begin_src matlab :exports none :results none +%% Comparison of the identified IFF plant and the IFF plant extracted from the simscape model +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$m = 0$'); +for i = 2:6 + plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$m = 1$'); +for i = 2:6 + plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$m = 2$'); +for i = 2:6 + plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ... + 'DisplayName', '$m = 3$'); +for i = 2:6 + plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ... + 'HandleVisibility', 'off') +end +plot(freqs, abs(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ... + 'DisplayName', '$m = 0$ - Model'); +plot(freqs, abs(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ... + 'DisplayName', '$m = 1$ - Model'); +plot(freqs, abs(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ... + 'DisplayName', '$m = 2$ - Model'); +plot(freqs, abs(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ... + 'DisplayName', '$m = 3$ - Model'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e2]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +plot(f, 180/pi*angle(G_iff_m0(:,1,1)), 'color', [colors(1,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]); +end +plot(f, 180/pi*angle(G_iff_m1(:,1,1)), 'color', [colors(2,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]); +end +plot(f, 180/pi*angle(G_iff_m2(:,1,1)), 'color', [colors(3,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]); +end +plot(f, 180/pi*angle(G_iff_m3(:,1,1)), 'color', [colors(4,:), 0.5]); +for i = 2:6 + plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]); +end +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:)); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:)); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:)); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:)); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-90, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_Giff_plant_comp_model.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:id31_Giff_plant_comp_model +#+caption: Comparison of the identified IFF plant and the IFF plant extracted from the simscape model +#+RESULTS: +[[file:figs/id31_Giff_plant_comp_model.png]] + + +** IFF Controller +*** Controller Design +Test second order high pass filter: +#+begin_src matlab +wz = 2*pi*10; +xiz = 0.7; +Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1) + % s/(2*pi*1)/(1 + s/(2*pi*1)) * ... % HPF: reduce gain at low frequency +#+end_src + +We want integral action between 20Hz and 200Hz. +#+begin_src matlab +%% IFF Controller +Kiff = -1e2 * ... % Gain + 1/(0.01*2*pi + s) * ... % LPF: provides integral action + Ghpf * ... + eye(6); % Diagonal 6x6 controller +Kiff.InputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}; +Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; +#+end_src + +Loop Gain: +#+begin_src matlab :exports none :results none +%% IFF Loop gain of the diagonal terms +Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz')); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(f, abs(G_iff_m0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ... + 'DisplayName', '$m = 0$'); +plot(f, abs(G_iff_m1(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ... + 'DisplayName', '$m = 1$'); +plot(f, abs(G_iff_m2(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ... + 'DisplayName', '$m = 2$'); +plot(f, abs(G_iff_m3(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ... + 'DisplayName', '$m = 3$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-2, 1e1]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); + +ax2 = nexttile; +hold on; +plot(f, 180/pi*angle(-G_iff_m0(:,1,1).*Kiff_frf), 'color', colors(1,:)); +plot(f, 180/pi*angle(-G_iff_m1(:,1,1).*Kiff_frf), 'color', colors(2,:)); +plot(f, 180/pi*angle(-G_iff_m2(:,1,1).*Kiff_frf), 'color', colors(3,:)); +plot(f, 180/pi*angle(-G_iff_m3(:,1,1).*Kiff_frf), 'color', colors(4,:)); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_iff_loop_gain_diagonal_terms.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:id31_iff_loop_gain_diagonal_terms +#+caption: IFF Loop gain of the diagonal terms +#+RESULTS: +[[file:figs/id31_iff_loop_gain_diagonal_terms.png]] + +Root Locus to obtain optimal gain. +#+begin_src matlab :exports none :results none +%% Root Locus for IFF +gains = logspace(-2, 2, 100); +Gm_iff_m0 = Gm_m0({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}); +Gm_iff_m1 = Gm_m1({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}); +Gm_iff_m2 = Gm_m2({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}); +Gm_iff_m3 = Gm_m3({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}); + +figure; +tiledlayout(1, 4, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile(); +hold on; +plot(real(pole(Gm_iff_m0)), imag(pole(Gm_iff_m0)), 'x', 'color', colors(1,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(Gm_iff_m0, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ... + 'DisplayName', '$g_{opt}$'); +hold off; +axis equal; +xlim([-640, 0]); ylim([0, 1600]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +title('$m_0$'); + +ax2 = nexttile(); +hold on; +plot(real(pole(Gm_iff_m1)), imag(pole(Gm_iff_m1)), 'x', 'color', colors(2,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(Gm_iff_m1, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ... + 'DisplayName', '$g_{opt}$'); +hold off; +axis equal; +xlim([-320, 0]); ylim([0, 800]); +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +title('$m_1$'); + +ax3 = nexttile(); +hold on; +plot(real(pole(Gm_iff_m2)), imag(pole(Gm_iff_m2)), 'x', 'color', colors(3,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(Gm_iff_m2, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ... + 'DisplayName', '$g_{opt}$'); +hold off; +axis equal; +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlim([-240, 0]); ylim([0, 600]); +title('$m_2$'); + +ax4 = nexttile(); +hold on; +plot(real(pole(Gm_iff_m3)), imag(pole(Gm_iff_m3)), 'x', 'color', colors(4,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +clpoles = pole(feedback(Gm_iff_m3, Kiff, +1)); +plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ... + 'DisplayName', '$g_{opt}$'); +hold off; +axis equal; +set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); +xlim([-160, 0]); ylim([0, 400]); +title('$m_3$'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_iff_root_locus.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:id31_iff_root_locus +#+caption: Root Locus for IFF. Green crosses are closed-loop poles for the same choosen IFF gain. +#+RESULTS: +[[file:figs/id31_iff_root_locus.png]] + +*** TODO Verify Stability +Verify Stability with Nyquist plot: + +- Why bad stability margins? + +#+begin_src matlab :exports none +%% Compute the Eigenvalues of the loop gain +Ldet = zeros(4, 6, length(f)); + +% Loop gain +Lmimo = pagemtimes(permute(G_iff_m0, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz'))); +for i_f = 2:length(f) + Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); +end + +Lmimo = pagemtimes(permute(G_iff_m1, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz'))); +for i_f = 2:length(f) + Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); +end + +Lmimo = pagemtimes(permute(G_iff_m2, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz'))); +for i_f = 2:length(f) + Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); +end + +Lmimo = pagemtimes(permute(G_iff_m3, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz'))); +for i_f = 2:length(f) + Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); +end +#+end_src + +#+begin_src matlab :exports none +%% Plot of the eigenvalues of L in the complex plane +figure; +hold on; +for i_mass = 1:4 + plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass, :), ... + 'DisplayName', sprintf('%i masses', i_mass)); + plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass, :), ... + 'HandleVisibility', 'off'); + for i = 1:6 + plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass, :), ... + 'HandleVisibility', 'off'); + end +end +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +legend('location', 'southeast'); +xlim([-3, 1]); ylim([-2, 2]); +#+end_src + +*** Save Controller +#+begin_src matlab :exports none :tangle no +K_order = order(Kiff(1,1)); + +Kz = c2d(Kiff(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4); +[num, den] = tfdata(Kz, 'v'); + +formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; +fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_iff.dat', 'w'); +fprintf(fileID, formatSpec, [num; den]'); +fclose(fileID); +#+end_src + +#+begin_src matlab +save('./matlab/mat/K_iff.mat', 'Kiff') +#+end_src + +** Estimated Damped Plant +#+begin_src matlab +%% Damped plant from Simscape model +Gm_hac_m0 = -feedback(Gm_m0, Kiff, 'name', +1); +Gm_hac_m1 = -feedback(Gm_m1, Kiff, 'name', +1); +Gm_hac_m2 = -feedback(Gm_m2, Kiff, 'name', +1); +Gm_hac_m3 = -feedback(Gm_m3, Kiff, 'name', +1); +#+end_src + +#+begin_src matlab +%% Verify Stability +isstable(Gm_hac_m0) +isstable(Gm_hac_m1) +isstable(Gm_hac_m2) +isstable(Gm_hac_m3) +#+end_src + +#+begin_src matlab +%% Save Damped Plants +save('./matlab/mat/Gm.mat', 'Gm_hac_m0', 'Gm_hac_m1', 'Gm_hac_m2', 'Gm_hac_m3', '-append'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Estimated damped plant from the Simscape model +figure; +tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... + 'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$'); +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$'); +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... + 'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$'); +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... + 'HandleVisibility', 'off'); +end +plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ... + 'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$'); +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 1e-3]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]); +end +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]); +end +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]); +end +for i =1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]) + +linkaxes([ax1,ax2],'x'); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/id31_hac_damped_plant_estimated_simscape.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:id31_hac_damped_plant_estimated_simscape +#+caption: description +#+RESULTS: +[[file:figs/id31_hac_damped_plant_estimated_simscape.png]] + + + * Bibliography :ignore: #+latex: \printbibliography[heading=bibintoc,title={Bibliography}] @@ -660,19 +2530,31 @@ addpath('./subsystems/'); % Path for Subsystems Simulink files data_dir = './mat/' #+END_SRC +** Initialize Simscape Model +#+NAME: m-init-simscape +#+begin_src matlab +% Simulink Model name +mdl = 'nass_model_id31'; + +load('nass_model_conf_simulink.mat'); +#+end_src + ** Initialize other elements #+NAME: m-init-other #+BEGIN_SRC matlab %% Colors for the figures colors = colororder; -freqs = logspace(0, 3, 1000); -Ts = 1e-4; -% data_dir = "/home/thomas/mnt/data_id31/id31nass/id31/20230801/RAW_DATA" -% mat_dir = "/home/thomas/mnt/data_id31/nass" +%% Frequency Vector +freqs = logspace(log10(1), log10(2e3), 1000); + +%% Sampling Time +Ts = 1e-4; #+END_SRC -** =h5scan= - Easily load h5 files +* Matlab Functions :noexport: +** Utility Functions +*** =h5scan= - Easily load h5 files :PROPERTIES: :header-args:matlab+: :tangle matlab/src/h5scan.m :header-args:matlab+: :comments none :mkdirp yes :eval no @@ -782,7 +2664,7 @@ function A = vrtlds(f,nm,det) % H5F.close(fid); #+end_src -** =sphereFit= - Fit sphere from x,y,z points +*** =sphereFit= - Fit sphere from x,y,z points :PROPERTIES: :header-args:matlab+: :tangle matlab/src/sphereFit.m :header-args:matlab+: :comments none :mkdirp yes :eval no @@ -822,6 +2704,2536 @@ Center=(A\B).'; Radius=sqrt(mean(sum([X(:,1)-Center(1),X(:,2)-Center(2),X(:,3)-Center(3)].^2,2))); #+end_src +** Initialize Simscape Model +*** =initializeSimscapeConfiguration=: Simscape Configuration +#+begin_src matlab :tangle matlab/src/initializeSimscapeConfiguration.m :comments none :mkdirp yes :eval no +function [] = initializeSimscapeConfiguration(args) + + arguments + args.gravity logical {mustBeNumericOrLogical} = true + end + + conf_simscape = struct(); + + if args.gravity + conf_simscape.type = 1; + else + conf_simscape.type = 2; + end + + if exist('./mat', 'dir') + if exist('./mat/nass_model_conf_simscape.mat', 'file') + save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append'); + else + save('mat/nass_model_conf_simscape.mat', 'conf_simscape'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file') + save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append'); + else + save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape'); + end + end + +end +#+end_src + +*** =initializeLoggingConfiguration=: Logging Configuration +#+begin_src matlab :tangle matlab/src/initializeLoggingConfiguration.m :comments none :mkdirp yes :eval no +function [] = initializeLoggingConfiguration(args) + + arguments + args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none' + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 + end + + conf_log = struct(); + + switch args.log + case 'none' + conf_log.type = 0; + case 'all' + conf_log.type = 1; + case 'forces' + conf_log.type = 2; + end + + conf_log.Ts = args.Ts; + + if exist('./mat', 'dir') + if exist('./mat/nass_model_conf_log.mat', 'file') + save('mat/nass_model_conf_log.mat', 'conf_log', '-append'); + else + save('mat/nass_model_conf_log.mat', 'conf_log'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_conf_log.mat', 'file') + save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append'); + else + save('matlab/mat/nass_model_conf_log.mat', 'conf_log'); + end + end + +end +#+end_src + +*** =initializeReferences=: Generate Reference Signals +#+begin_src matlab :tangle matlab/src/initializeReferences.m :comments none :mkdirp yes :eval no +function [ref] = initializeReferences(args) + + arguments + % Sampling Frequency [s] + args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 + % Maximum simulation time [s] + args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100 + % Either "constant" / "triangular" / "sinusoidal" + args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant' + % Amplitude of the displacement [m] + args.Dy_amplitude (1,1) double {mustBeNumeric} = 0 + % Period of the displacement [s] + args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1 + % Either "constant" / "triangular" / "sinusoidal" + args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant' + % Amplitude [rad] + args.Ry_amplitude (1,1) double {mustBeNumeric} = 0 + % Period of the displacement [s] + args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1 + % Either "constant" / "rotating" + args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant' + % Initial angle [rad] + args.Rz_amplitude (1,1) double {mustBeNumeric} = 0 + % Period of the rotating [s] + args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1 + % For now, only constant is implemented + args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant' + % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles) + args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ... + % For now, only constant is implemented + args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant' + % Initial position of the two masses + args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi] + % For now, only constant is implemented + args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant' + % Initial position [m,m,m,rad,rad,rad] of the top platform + args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1) + end + + %% Set Sampling Time + Ts = args.Ts; + Tmax = args.Tmax; + + %% Low Pass Filter to filter out the references + s = zpk('s'); + w0 = 2*pi*10; + xi = 1; + H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2); + + %% Translation stage - Dy + t = 0:Ts:Tmax; % Time Vector [s] + Dy = zeros(length(t), 1); + Dyd = zeros(length(t), 1); + Dydd = zeros(length(t), 1); + switch args.Dy_type + case 'constant' + Dy(:) = args.Dy_amplitude; + Dyd(:) = 0; + Dydd(:) = 0; + case 'triangular' + % This is done to unsure that we start with no displacement + Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2); + i0 = find(t>=args.Dy_period/4,1); + Dy(1:end-i0+1) = Dy_raw(i0:end); + Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value + + % The signal is filtered out + Dy = lsim(H_lpf, Dy, t); + Dyd = lsim(H_lpf*s, Dy, t); + Dydd = lsim(H_lpf*s^2, Dy, t); + case 'sinusoidal' + Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t); + Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t); + Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t); + otherwise + warning('Dy_type is not set correctly'); + end + + Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd); + + %% Tilt Stage - Ry + t = 0:Ts:Tmax; % Time Vector [s] + Ry = zeros(length(t), 1); + Ryd = zeros(length(t), 1); + Rydd = zeros(length(t), 1); + + switch args.Ry_type + case 'constant' + Ry(:) = args.Ry_amplitude; + Ryd(:) = 0; + Rydd(:) = 0; + case 'triangular' + Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2); + i0 = find(t>=args.Ry_period/4,1); + Ry(1:end-i0+1) = Ry_raw(i0:end); + Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value + + % The signal is filtered out + Ry = lsim(H_lpf, Ry, t); + Ryd = lsim(H_lpf*s, Ry, t); + Rydd = lsim(H_lpf*s^2, Ry, t); + case 'sinusoidal' + Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t); + + Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t); + Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t); + otherwise + warning('Ry_type is not set correctly'); + end + + Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd); + + %% Spindle - Rz + t = 0:Ts:Tmax; % Time Vector [s] + Rz = zeros(length(t), 1); + Rzd = zeros(length(t), 1); + Rzdd = zeros(length(t), 1); + + switch args.Rz_type + case 'constant' + Rz(:) = args.Rz_amplitude; + Rzd(:) = 0; + Rzdd(:) = 0; + case 'rotating-not-filtered' + Rz(:) = 2*pi/args.Rz_period*t; + + % The signal is filtered out + Rz(:) = 2*pi/args.Rz_period*t; + Rzd(:) = 2*pi/args.Rz_period; + Rzdd(:) = 0; + + % We add the angle offset + Rz = Rz + args.Rz_amplitude; + + case 'rotating' + Rz(:) = 2*pi/args.Rz_period*t; + + % The signal is filtered out + Rz = lsim(H_lpf, Rz, t); + Rzd = lsim(H_lpf*s, Rz, t); + Rzdd = lsim(H_lpf*s^2, Rz, t); + + % We add the angle offset + Rz = Rz + args.Rz_amplitude; + otherwise + warning('Rz_type is not set correctly'); + end + + Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd); + + %% Micro-Hexapod + t = [0, Ts]; + Dh = zeros(length(t), 6); + Dhl = zeros(length(t), 6); + + switch args.Dh_type + case 'constant' + Dh = [args.Dh_pos, args.Dh_pos]; + + load('nass_model_stages.mat', 'micro_hexapod'); + + AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)]; + + tx = args.Dh_pos(4); + ty = args.Dh_pos(5); + tz = args.Dh_pos(6); + + ARB = [cos(tz) -sin(tz) 0; + sin(tz) cos(tz) 0; + 0 0 1]*... + [ cos(ty) 0 sin(ty); + 0 1 0; + -sin(ty) 0 cos(ty)]*... + [1 0 0; + 0 cos(tx) -sin(tx); + 0 sin(tx) cos(tx)]; + + [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB); + Dhl = [Dhl, Dhl]; + otherwise + warning('Dh_type is not set correctly'); + end + + Dh = struct('time', t, 'signals', struct('values', Dh)); + Dhl = struct('time', t, 'signals', struct('values', Dhl)); + + if exist('./mat', 'dir') + if exist('./mat/nass_model_references.mat', 'file') + save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append'); + else + save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_references.mat', 'file') + save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append'); + else + save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts'); + end + end + +end +#+end_src + +*** =initializeDisturbances=: Initialize Disturbances +#+begin_src matlab :tangle matlab/src/initializeDisturbances.m :comments none :mkdirp yes :eval no +function [] = initializeDisturbances(args) +% initializeDisturbances - Initialize the disturbances +% +% Syntax: [] = initializeDisturbances(args) +% +% Inputs: +% - args - + + + arguments + % Global parameter to enable or disable the disturbances + args.enable logical {mustBeNumericOrLogical} = true + % Ground Motion - X direction + args.Dw_x logical {mustBeNumericOrLogical} = true + % Ground Motion - Y direction + args.Dw_y logical {mustBeNumericOrLogical} = true + % Ground Motion - Z direction + args.Dw_z logical {mustBeNumericOrLogical} = true + % Translation Stage - X direction + args.Fdy_x logical {mustBeNumericOrLogical} = true + % Translation Stage - Z direction + args.Fdy_z logical {mustBeNumericOrLogical} = true + % Spindle - X direction + args.Frz_x logical {mustBeNumericOrLogical} = true + % Spindle - Y direction + args.Frz_y logical {mustBeNumericOrLogical} = true + % Spindle - Z direction + args.Frz_z logical {mustBeNumericOrLogical} = true + end + + % Initialization of random numbers + rng("shuffle"); + + %% Ground Motion + if args.enable + % Load the PSD of disturbance + load('ustation_disturbance_psd.mat', 'gm_dist') + + % Frequency Data + Dw.f = gm_dist.f; + Dw.psd_x = gm_dist.pxx_x; + Dw.psd_y = gm_dist.pxx_y; + Dw.psd_z = gm_dist.pxx_z; + + % Time data + Fs = 2*Dw.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] + N = 2*length(Dw.f); % Number of Samples match the one of the wanted PSD + T0 = N/Fs; % Signal Duration [s] + Dw.t = linspace(0, T0, N+1)'; % Time Vector [s] + + % ASD representation of the ground motion + C = zeros(N/2,1); + for i = 1:N/2 + C(i) = sqrt(Dw.psd_x(i)/T0); + end + + if args.Dw_x + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] + else + Dw.x = zeros(length(Dw.t), 1); + end + + if args.Dw_y + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] + else + Dw.y = zeros(length(Dw.t), 1); + end + + if args.Dw_y + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] + else + Dw.z = zeros(length(Dw.t), 1); + end + + else + Dw.t = [0,1]; % Time Vector [s] + Dw.x = [0,0]; % Ground Motion - X [m] + Dw.y = [0,0]; % Ground Motion - Y [m] + Dw.z = [0,0]; % Ground Motion - Z [m] + end + + %% Translation stage + if args.enable + % Load the PSD of disturbance + load('ustation_disturbance_psd.mat', 'dy_dist') + + % Frequency Data + Dy.f = dy_dist.f; + Dy.psd_x = dy_dist.pxx_fx; + Dy.psd_z = dy_dist.pxx_fz; + + % Time data + Fs = 2*Dy.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] + N = 2*length(Dy.f); % Number of Samples match the one of the wanted PSD + T0 = N/Fs; % Signal Duration [s] + Dy.t = linspace(0, T0, N+1)'; % Time Vector [s] + + % ASD representation of the disturbance voice + C = zeros(N/2,1); + for i = 1:N/2 + C(i) = sqrt(Dy.psd_x(i)/T0); + end + + if args.Fdy_x + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N] + else + Dy.x = zeros(length(Dy.t), 1); + end + + if args.Fdy_z + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N] + else + Dy.z = zeros(length(Dy.t), 1); + end + + else + Dy.t = [0,1]; % Time Vector [s] + Dy.x = [0,0]; % Translation Stage disturbances - X [N] + Dy.z = [0,0]; % Translation Stage disturbances - Z [N] + end + + %% Spindle + if args.enable + % Load the PSD of disturbance + load('ustation_disturbance_psd.mat', 'rz_dist') + + % Frequency Data + Rz.f = rz_dist.f; + Rz.psd_x = rz_dist.pxx_fx; + Rz.psd_y = rz_dist.pxx_fy; + Rz.psd_z = rz_dist.pxx_fz; + + % Time data + Fs = 2*Rz.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] + N = 2*length(Rz.f); % Number of Samples match the one of the wanted PSD + T0 = N/Fs; % Signal Duration [s] + Rz.t = linspace(0, T0, N+1)'; % Time Vector [s] + + % ASD representation of the disturbance voice + C = zeros(N/2,1); + for i = 1:N/2 + C(i) = sqrt(Rz.psd_x(i)/T0); + end + + if args.Frz_x + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N] + else + Rz.x = zeros(length(Rz.t), 1); + end + + if args.Frz_y + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N] + else + Rz.y = zeros(length(Rz.t), 1); + end + + if args.Frz_z + theta = 2*pi*rand(N/2,1); % Generate random phase [rad] + Cx = [0 ; C.*complex(cos(theta),sin(theta))]; + Cx = [Cx; flipud(conj(Cx(2:end)))];; + Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N] + else + Rz.z = zeros(length(Rz.t), 1); + end + + else + Rz.t = [0,1]; % Time Vector [s] + Rz.x = [0,0]; % Spindle disturbances - X [N] + Rz.y = [0,0]; % Spindle disturbances - X [N] + Rz.z = [0,0]; % Spindle disturbances - Z [N] + end + + u = zeros(100, 6); + Fd = u; + + Dw.x = Dw.x - Dw.x(1); + Dw.y = Dw.y - Dw.y(1); + Dw.z = Dw.z - Dw.z(1); + + Dy.x = Dy.x - Dy.x(1); + Dy.z = Dy.z - Dy.z(1); + + Rz.x = Rz.x - Rz.x(1); + Rz.y = Rz.y - Rz.y(1); + Rz.z = Rz.z - Rz.z(1); + + if exist('./mat', 'dir') + save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args'); + elseif exist('./matlab', 'dir') + save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args'); + end + +end +#+end_src + +*** =initializeController=: Initialize Controller +#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no +function [] = initializeController(args) + + arguments + args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop' + end + + controller = struct(); + + switch args.type + case 'open-loop' + controller.type = 1; + controller.name = 'Open-Loop'; + case 'dvf' + controller.type = 2; + controller.name = 'Decentralized Direct Velocity Feedback'; + case 'iff' + controller.type = 3; + controller.name = 'Decentralized Integral Force Feedback'; + case 'hac-dvf' + controller.type = 4; + controller.name = 'HAC-DVF'; + case 'ref-track-L' + controller.type = 5; + controller.name = 'Reference Tracking in the frame of the legs'; + case 'ref-track-iff-L' + controller.type = 6; + controller.name = 'Reference Tracking in the frame of the legs + IFF'; + case 'cascade-hac-lac' + controller.type = 7; + controller.name = 'Cascade Control + HAC-LAC'; + case 'hac-iff' + controller.type = 8; + controller.name = 'HAC-IFF'; + case 'stabilizing' + controller.type = 9; + controller.name = 'Stabilizing Controller'; + end + + if exist('./mat', 'dir') + save('mat/nass_model_controller.mat', 'controller'); + elseif exist('./matlab', 'dir') + save('matlab/mat/nass_model_controller.mat', 'controller'); + end + +end +#+end_src + +*** =computeReferencePose= +#+begin_src matlab :tangle matlab/src/computeReferencePose.m :comments none :mkdirp yes :eval no + function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn) + % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample + % + % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn) + % + % Inputs: + % - Dy - Reference of the Translation Stage [m] + % - Ry - Reference of the Tilt Stage [rad] + % - Rz - Reference of the Spindle [rad] + % - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad] + % - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad] + % + % Outputs: + % - WTr - + + %% Translation Stage + Rty = [1 0 0 0; + 0 1 0 Dy; + 0 0 1 0; + 0 0 0 1]; + + %% Tilt Stage - Pure rotating aligned with Ob + Rry = [ cos(Ry) 0 sin(Ry) 0; + 0 1 0 0; + -sin(Ry) 0 cos(Ry) 0; + 0 0 0 1]; + + %% Spindle - Rotation along the Z axis + Rrz = [cos(Rz) -sin(Rz) 0 0 ; + sin(Rz) cos(Rz) 0 0 ; + 0 0 1 0 ; + 0 0 0 1 ]; + + + %% Micro-Hexapod + Rhx = [1 0 0; + 0 cos(Dh(4)) -sin(Dh(4)); + 0 sin(Dh(4)) cos(Dh(4))]; + + Rhy = [ cos(Dh(5)) 0 sin(Dh(5)); + 0 1 0; + -sin(Dh(5)) 0 cos(Dh(5))]; + + Rhz = [cos(Dh(6)) -sin(Dh(6)) 0; + sin(Dh(6)) cos(Dh(6)) 0; + 0 0 1]; + + Rh = [1 0 0 Dh(1) ; + 0 1 0 Dh(2) ; + 0 0 1 Dh(3) ; + 0 0 0 1 ]; + + Rh(1:3, 1:3) = Rhz*Rhy*Rhx; + + %% Nano-Hexapod + Rnx = [1 0 0; + 0 cos(Dn(4)) -sin(Dn(4)); + 0 sin(Dn(4)) cos(Dn(4))]; + + Rny = [ cos(Dn(5)) 0 sin(Dn(5)); + 0 1 0; + -sin(Dn(5)) 0 cos(Dn(5))]; + + Rnz = [cos(Dn(6)) -sin(Dn(6)) 0; + sin(Dn(6)) cos(Dn(6)) 0; + 0 0 1]; + + Rn = [1 0 0 Dn(1) ; + 0 1 0 Dn(2) ; + 0 0 1 Dn(3) ; + 0 0 0 1 ]; + + Rn(1:3, 1:3) = Rnz*Rny*Rnx; + + %% Total Homogeneous transformation + WTr = Rty*Rry*Rrz*Rh*Rn; + end +#+end_src + +*** =extractNodes= +#+begin_src matlab :tangle matlab/src/extractNodes.m :comments none :mkdirp yes :eval no +function [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes(filename) +% extractNodes - +% +% Syntax: [n_xyz, nodes] = extractNodes(filename) +% +% Inputs: +% - filename - relative or absolute path of the file that contains the Matrix +% +% Outputs: +% - n_xyz - +% - nodes - table containing the node numbers and corresponding dof of the interfaced DoFs + +arguments + filename +end + +fid = fopen(filename,'rt'); + +if fid == -1 + error('Error opening the file'); +end + +n_xyz = []; % Contains nodes coordinates +n_i = []; % Contains nodes indices + +n_num = []; % Contains node numbers +n_dof = {}; % Contains node directions + +while 1 + % Read a line + nextline = fgetl(fid); + + % End of the file + if ~isstr(nextline), break, end + + % Line just before the list of nodes coordinates + if contains(nextline, 'NODE') && ... + contains(nextline, 'X') && ... + contains(nextline, 'Y') && ... + contains(nextline, 'Z') + + while 1 + nextline = fgetl(fid); + + if nextline < 0, break, end + + c = sscanf(nextline, ' %f'); + + if isempty(c), break, end + + n_xyz = [n_xyz; c(2:4)']; + n_i = [n_i; c(1)]; + end + end + + if nextline < 0, break, end + + % Line just before the list of node DOF + if contains(nextline, 'NODE LABEL') + + while 1 + nextline = fgetl(fid); + + if nextline < 0, break, end + + c = sscanf(nextline, ' %d %s'); + + if isempty(c), break, end + + n_num = [n_num; c(1)]; + + n_dof{length(n_dof)+1} = char(c(2:end)'); + end + + nodes = table(n_num, string(n_dof'), 'VariableNames', {'node_i', 'node_dof'}); + end + + if nextline < 0, break, end +end + +fclose(fid); + +int_i = unique(nodes.('node_i')); % indices of interface nodes + +% Extract XYZ coordinates of only the interface nodes +if length(n_xyz) > 0 && length(n_i) > 0 + int_xyz = n_xyz(logical(sum(n_i.*ones(1, length(int_i)) == int_i', 2)), :); +else + int_xyz = n_xyz; +end +#+end_src + +** Initialize Micro-Station Stages +*** =initializeGround=: Ground +#+begin_src matlab :tangle matlab/src/initializeGround.m :comments none :mkdirp yes :eval no +function [ground] = initializeGround(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid' + args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m] + end + + ground = struct(); + + switch args.type + case 'none' + ground.type = 0; + case 'rigid' + ground.type = 1; + end + + ground.shape = [2, 2, 0.5]; % [m] + ground.density = 2800; % [kg/m3] + + ground.rot_point = args.rot_point; + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'ground', '-append'); + else + save('mat/nass_model_stages.mat', 'ground'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'ground', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'ground'); + end + end +end +#+end_src + +*** =initializeGranite=: Granite +#+begin_src matlab :tangle matlab/src/initializeGranite.m :comments none :mkdirp yes :eval no +function [granite] = initializeGranite(args) + + arguments + args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible' + args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3] + args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m] + args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [N/(m/s)] + args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m] + args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m] + args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m] + args.sample_pos (1,1) double {mustBeNumeric} = 0.775 % Height of the measurment point [m] + end + + granite = struct(); + + switch args.type + case 'none' + granite.type = 0; + case 'rigid' + granite.type = 1; + case 'flexible' + granite.type = 2; + end + + granite.density = args.density; % [kg/m3] + granite.STEP = 'granite.STEP'; + + % Z-offset for the initial position of the sample with respect to the granite top surface. + granite.sample_pos = args.sample_pos; % [m] + + granite.K = args.K; % [N/m] + granite.C = args.C; % [N/(m/s)] + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'granite', '-append'); + else + save('mat/nass_model_stages.mat', 'granite'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'granite', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'granite'); + end + end + +end +#+end_src + +*** =initializeTy=: Translation Stage +#+begin_src matlab :tangle matlab/src/initializeTy.m :comments none :mkdirp yes :eval no +function [ty] = initializeTy(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' + end + + ty = struct(); + + switch args.type + case 'none' + ty.type = 0; + case 'rigid' + ty.type = 1; + case 'flexible' + ty.type = 2; + end + + % Ty Granite frame + ty.granite_frame.density = 7800; % [kg/m3] => 43kg + ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP'; + + % Guide Translation Ty + ty.guide.density = 7800; % [kg/m3] => 76kg + ty.guide.STEP = 'Ty_Guide.STEP'; + + % Ty - Guide_Translation12 + ty.guide12.density = 7800; % [kg/m3] + ty.guide12.STEP = 'Ty_Guide_12.STEP'; + + % Ty - Guide_Translation11 + ty.guide11.density = 7800; % [kg/m3] + ty.guide11.STEP = 'Ty_Guide_11.STEP'; + + % Ty - Guide_Translation22 + ty.guide22.density = 7800; % [kg/m3] + ty.guide22.STEP = 'Ty_Guide_22.STEP'; + + % Ty - Guide_Translation21 + ty.guide21.density = 7800; % [kg/m3] + ty.guide21.STEP = 'Ty_Guide_21.STEP'; + + % Ty - Plateau translation + ty.frame.density = 7800; % [kg/m3] + ty.frame.STEP = 'Ty_Stage.STEP'; + + % Ty Stator Part + ty.stator.density = 5400; % [kg/m3] + ty.stator.STEP = 'Ty_Motor_Stator.STEP'; + + % Ty Rotor Part + ty.rotor.density = 5400; % [kg/m3] + ty.rotor.STEP = 'Ty_Motor_Rotor.STEP'; + + ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad] + ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)] + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'ty', '-append'); + else + save('mat/nass_model_stages.mat', 'ty'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'ty', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'ty'); + end + end + +end +#+end_src + +*** =initializeRy=: Tilt Stage +#+begin_src matlab :tangle matlab/src/initializeRy.m :comments none :mkdirp yes :eval no +function [ry] = initializeRy(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' + args.Ry_init (1,1) double {mustBeNumeric} = 0 + end + + ry = struct(); + + switch args.type + case 'none' + ry.type = 0; + case 'rigid' + ry.type = 1; + case 'flexible' + ry.type = 2; + end + + % Ry - Guide for the tilt stage + ry.guide.density = 7800; % [kg/m3] + ry.guide.STEP = 'Tilt_Guide.STEP'; + + % Ry - Rotor of the motor + ry.rotor.density = 2400; % [kg/m3] + ry.rotor.STEP = 'Tilt_Motor_Axis.STEP'; + + % Ry - Motor + ry.motor.density = 3200; % [kg/m3] + ry.motor.STEP = 'Tilt_Motor.STEP'; + + % Ry - Plateau Tilt + ry.stage.density = 7800; % [kg/m3] + ry.stage.STEP = 'Tilt_Stage.STEP'; + + % Z-Offset so that the center of rotation matches the sample center; + ry.z_offset = 0.58178; % [m] + + ry.Ry_init = args.Ry_init; % [rad] + + ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8]; + ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4]; + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'ry', '-append'); + else + save('mat/nass_model_stages.mat', 'ry'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'ry', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'ry'); + end + end + +end +#+end_src + +*** =initializeRz=: Spindle +#+begin_src matlab :tangle matlab/src/initializeRz.m :comments none :mkdirp yes :eval no +function [rz] = initializeRz(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' + end + + rz = struct(); + + switch args.type + case 'none' + rz.type = 0; + case 'rigid' + rz.type = 1; + case 'flexible' + rz.type = 2; + end + + % Spindle - Slip Ring + rz.slipring.density = 7800; % [kg/m3] + rz.slipring.STEP = 'Spindle_Slip_Ring.STEP'; + + % Spindle - Rotor + rz.rotor.density = 7800; % [kg/m3] + rz.rotor.STEP = 'Spindle_Rotor.STEP'; + + % Spindle - Stator + rz.stator.density = 7800; % [kg/m3] + rz.stator.STEP = 'Spindle_Stator.STEP'; + + rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7]; + rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4]; + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'rz', '-append'); + else + save('mat/nass_model_stages.mat', 'rz'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'rz', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'rz'); + end + end + +end +#+end_src + +*** =initializeMicroHexapod=: Micro Hexapod + +#+begin_src matlab :tangle matlab/src/initializeMicroHexapod.m :comments none :mkdirp yes :eval no +function [micro_hexapod] = initializeMicroHexapod(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' + % initializeFramesPositions + args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 + args.MO_B (1,1) double {mustBeNumeric} = 270e-3 + % generateGeneralConfiguration + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3 + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180) + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3 + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180) + % initializeStrutDynamics + args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1) + args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1) + % initializeCylindricalPlatforms + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 + % initializeCylindricalStruts + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3 + % inverseKinematics + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + end + + stewart = initializeStewartPlatform(); + + stewart = initializeFramesPositions(stewart, ... + 'H', args.H, ... + 'MO_B', args.MO_B); + + stewart = generateGeneralConfiguration(stewart, ... + 'FH', args.FH, ... + 'FR', args.FR, ... + 'FTh', args.FTh, ... + 'MH', args.MH, ... + 'MR', args.MR, ... + 'MTh', args.MTh); + + stewart = computeJointsPose(stewart); + + stewart = initializeStrutDynamics(stewart, ... + 'K', args.Ki, ... + 'C', args.Ci); + + stewart = initializeJointDynamics(stewart, ... + 'type_F', 'universal_p', ... + 'type_M', 'spherical_p'); + + stewart = initializeCylindricalPlatforms(stewart, ... + 'Fpm', args.Fpm, ... + 'Fph', args.Fph, ... + 'Fpr', args.Fpr, ... + 'Mpm', args.Mpm, ... + 'Mph', args.Mph, ... + 'Mpr', args.Mpr); + + stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', args.Fsm, ... + 'Fsh', args.Fsh, ... + 'Fsr', args.Fsr, ... + 'Msm', args.Msm, ... + 'Msh', args.Msh, ... + 'Msr', args.Msr); + + stewart = computeJacobian(stewart); + + stewart = initializeStewartPose(stewart, ... + 'AP', args.AP, ... + 'ARB', args.ARB); + + stewart = initializeInertialSensor(stewart, 'type', 'none'); + + switch args.type + case 'none' + stewart.type = 0; + case 'rigid' + stewart.type = 1; + case 'flexible' + stewart.type = 2; + end + + micro_hexapod = stewart; + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'micro_hexapod', '-append'); + else + save('mat/nass_model_stages.mat', 'micro_hexapod'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'micro_hexapod'); + end + end +end +#+end_src + +*** =initializeNanoHexapod=: Nano-Hexapod +#+begin_src matlab :tangle matlab/src/initializeNanoHexapod.m :comments none :mkdirp yes :eval no +function [nano_hexapod] = initializeNanoHexapod(args) + + arguments + %% Bottom Flexible Joints + args.flex_bot_type char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof' + args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad] + args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad] + args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad] + args.flex_bot_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m] + args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)] + args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)] + args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)] + args.flex_bot_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)] + + %% Top Flexible Joints + args.flex_top_type char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof' + args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad] + args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad] + args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad] + args.flex_top_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m] + args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)] + args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)] + args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)] + args.flex_top_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)] + + %% Jacobian - Location of frame {A} and {B} + args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m] + + %% Relative Motion Sensor + args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts' + + %% Top Plate + args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid' + args.top_plate_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio + + %% Actuators + args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible' + args.actuator_Ga (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V] + args.actuator_Gs (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m] + % For 2DoF + args.actuator_k (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000 + args.actuator_ke (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605 + args.actuator_ka (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302 + args.actuator_c (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5 + args.actuator_ce (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100 + args.actuator_ca (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50 + args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m] + % For Flexible Frame + args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m] + args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m] + % Misalignment + args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m] + + args.actuator_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio + + %% Controller + args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none' + end + + nano_hexapod = struct(); + + nano_hexapod.flex_bot = struct(); + + switch args.flex_bot_type + case '2dof' + nano_hexapod.flex_bot.type = 1; + case '3dof' + nano_hexapod.flex_bot.type = 2; + case '4dof' + nano_hexapod.flex_bot.type = 3; + case 'flexible' + nano_hexapod.flex_bot.type = 4; + end + + nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad] + nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad] + nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad] + nano_hexapod.flex_bot.kz = args.flex_bot_kz; % Axial stiffness [N/m] + + nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)] + nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)] + nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)] + nano_hexapod.flex_bot.cz = args.flex_bot_cz; %[N/(m/s)] + + nano_hexapod.flex_top = struct(); + + switch args.flex_top_type + case '2dof' + nano_hexapod.flex_top.type = 1; + case '3dof' + nano_hexapod.flex_top.type = 2; + case '4dof' + nano_hexapod.flex_top.type = 3; + case 'flexible' + nano_hexapod.flex_top.type = 4; + end + + nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad] + nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad] + nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad] + nano_hexapod.flex_top.kz = args.flex_top_kz; % Axial stiffness [N/m] + + nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)] + nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)] + nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)] + nano_hexapod.flex_top.cz = args.flex_top_cz; %[N/(m/s)] + + nano_hexapod.motion_sensor = struct(); + + switch args.motion_sensor_type + case 'struts' + nano_hexapod.motion_sensor.type = 1; + case 'plates' + nano_hexapod.motion_sensor.type = 2; + end + + nano_hexapod.actuator = struct(); + + switch args.actuator_type + case '2dof' + nano_hexapod.actuator.type = 1; + case 'flexible frame' + nano_hexapod.actuator.type = 2; + case 'flexible' + nano_hexapod.actuator.type = 3; + end + + %% Actuator gain [N/V] + if all(args.actuator_Ga == 0) + switch args.actuator_type + case '2dof' + nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796); + case 'flexible frame' + nano_hexapod.actuator.Ga = ones(6,1); % TODO + case 'flexible' + nano_hexapod.actuator.Ga = ones(6,1)*23.2; + end + else + nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V] + end + + %% Sensor gain [V/m] + if all(args.actuator_Gs == 0) + switch args.actuator_type + case '2dof' + nano_hexapod.actuator.Gs = ones(6,1)*466664; + case 'flexible frame' + nano_hexapod.actuator.Gs = ones(6,1); % TODO + case 'flexible' + nano_hexapod.actuator.Gs = ones(6,1)*(-4898341); + end + else + nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m] + end + + switch args.actuator_type + case '2dof' + nano_hexapod.actuator.k = args.actuator_k; % [N/m] + nano_hexapod.actuator.ke = args.actuator_ke; % [N/m] + nano_hexapod.actuator.ka = args.actuator_ka; % [N/m] + + nano_hexapod.actuator.c = args.actuator_c; % [N/(m/s)] + nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)] + nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)] + + nano_hexapod.actuator.Leq = args.actuator_Leq; % [m] + + case 'flexible frame' + nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix + nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix + nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m] + + nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m] + nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m] + nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio + + case 'flexible' + nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix + nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix + nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m] + + nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment + nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio + + end + + nano_hexapod.geometry = struct(); + + Fa = [[-86.05, -74.78, 22.49], + [ 86.05, -74.78, 22.49], + [ 107.79, -37.13, 22.49], + [ 21.74, 111.91, 22.49], + [-21.74, 111.91, 22.49], + [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m] + + Mb = [[-28.47, -106.25, -22.50], + [ 28.47, -106.25, -22.50], + [ 106.25, 28.47, -22.50], + [ 77.78, 77.78, -22.50], + [-77.78, 77.78, -22.50], + [-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m] + + Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m] + + si = Fb - Fa; + si = si./vecnorm(si); % Normalize + + Fc = [[-29.362, -105.765, 52.605] + [ 29.362, -105.765, 52.605] + [ 106.276, 27.454, 52.605] + [ 76.914, 78.31, 52.605] + [-76.914, 78.31, 52.605] + [-106.276, 27.454, 52.605]]'*1e-3; % Meas pos w.r.t. {F} + Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M} + + nano_hexapod.geometry.Fa = Fa; + nano_hexapod.geometry.Fb = Fb; + nano_hexapod.geometry.Fc = Fc; + nano_hexapod.geometry.Mb = Mb; + nano_hexapod.geometry.Mc = Mc; + nano_hexapod.geometry.si = si; + nano_hexapod.geometry.MO_B = args.MO_B; + + Bb = Mb - [0; 0; args.MO_B]; + + nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)']; + + switch args.motion_sensor_type + case 'struts' + nano_hexapod.geometry.Js = nano_hexapod.geometry.J; + case 'plates' + Bc = Mc - [0; 0; args.MO_B]; + nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)']; + end + + nano_hexapod.top_plate = struct(); + + switch args.top_plate_type + case 'rigid' + nano_hexapod.top_plate.type = 1; + case 'flexible' + nano_hexapod.top_plate.type = 2; + + nano_hexapod.top_plate.R_flex = ... + {[ 0.53191886726305 0.4795690716524 0.69790817745892 + -0.29070157897799 0.8775041341865 -0.38141720787774 + -0.79533320729697 0 0.60617249143351 ], + [ 0.53191886726305 -0.4795690716524 -0.69790817745892 + 0.29070157897799 0.8775041341865 -0.38141720787774 + 0.79533320729697 0 0.60617249143351 ], + [-0.01420448131633 -0.9997254079576 -0.01863709726680 + 0.60600604129104 -0.0234330681729 0.79511481512719 + -0.79533320729697 0 0.60617249143351 ], + [-0.51771438594672 -0.5201563363051 0.67927108019212 + 0.31530446231304 -0.8540710660135 -0.41369760724945 + 0.79533320729697 0 0.60617249143351 ], + [-0.51771438594671 0.5201563363052 -0.67927108019211 + -0.31530446231304 -0.8540710660135 -0.41369760724945 + -0.79533320729697 0 0.60617249143351 ], + [-0.01420448131632 0.9997254079576 0.01863709726679 + -0.60600604129104 -0.0234330681729 0.79511481512719 + 0.79533320729697 0 0.60617249143351 ] }; + + + nano_hexapod.top_plate.R_enc = ... + { [-0.877504134186525 -0.479569071652412 0 + 0.479569071652412 -0.877504134186525 0 + 0 0 1 ], + [ 0.877504134186525 -0.479569071652413 0 + 0.479569071652413 0.877504134186525 0 + 0 0 1 ], + [ 0.023433068172945 0.999725407957606 0 + -0.999725407957606 0.023433068172945 0 + 0 0 1 ], + [-0.854071066013566 -0.520156336305202 0 + 0.520156336305202 -0.854071066013566 0 + 0 0 1 ], + [ 0.854071066013574 -0.520156336305191 0 + 0.520156336305191 0.854071066013574 0 + 0 0 1 ], + [-0.023433068172958 0.999725407957606 0 + -0.999725407957606 -0.023433068172958 0 + 0 0 1 ] }; + + nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix + nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix + nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m] + nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio + + end + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'nano_hexapod', '-append'); + else + save('mat/nass_model_stages.mat', 'nano_hexapod'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'nano_hexapod'); + end + end +end +#+end_src + +*** =initializeSample=: Sample + +#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no +function [sample] = initializeSample(args) + + arguments + args.type char {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0' + end + + sample = struct(); + + switch args.type + case '0' + sample.type = 0; + case '1' + sample.type = 1; + case '2' + sample.type = 2; + case '3' + sample.type = 3; + end + + if exist('./mat', 'dir') + if exist('./mat/nass_model_stages.mat', 'file') + save('mat/nass_model_stages.mat', 'sample', '-append'); + else + save('mat/nass_model_stages.mat', 'sample'); + end + elseif exist('./matlab', 'dir') + if exist('./matlab/mat/nass_model_stages.mat', 'file') + save('matlab/mat/nass_model_stages.mat', 'sample', '-append'); + else + save('matlab/mat/nass_model_stages.mat', 'sample'); + end + end + +end +#+end_src + +** Stewart platform +*** =initializeStewartPlatform=: Initialize the Stewart Platform structure +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeStewartPlatform.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Documentation + +#+name: fig:stewart-frames-position +#+caption: Definition of the position of the frames +[[file:figs/stewart-frames-position.png]] + +**** Function description +#+begin_src matlab + function [stewart] = initializeStewartPlatform() + % initializeStewartPlatform - Initialize the stewart structure + % + % Syntax: [stewart] = initializeStewartPlatform(args) + % + % Outputs: + % - stewart - A structure with the following sub-structures: + % - platform_F - + % - platform_M - + % - joints_F - + % - joints_M - + % - struts_F - + % - struts_M - + % - actuators - + % - geometry - + % - properties - +#+end_src + +**** Initialize the Stewart structure +#+begin_src matlab + stewart = struct(); + stewart.platform_F = struct(); + stewart.platform_M = struct(); + stewart.joints_F = struct(); + stewart.joints_M = struct(); + stewart.struts_F = struct(); + stewart.struts_M = struct(); + stewart.actuators = struct(); + stewart.sensors = struct(); + stewart.sensors.inertial = struct(); + stewart.sensors.force = struct(); + stewart.sensors.relative = struct(); + stewart.geometry = struct(); + stewart.kinematics = struct(); +#+end_src + +*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M} +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeFramesPositions.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Documentation + +#+name: fig:stewart-frames-position +#+caption: Definition of the position of the frames +[[file:figs/stewart-frames-position.png]] + +**** Function description +#+begin_src matlab + function [stewart] = initializeFramesPositions(stewart, args) + % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} + % + % Syntax: [stewart] = initializeFramesPositions(stewart, args) + % + % Inputs: + % - args - Can have the following fields: + % - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m] + % - MO_B [1x1] - Height of the frame {B} with respect to {M} [m] + % + % Outputs: + % - stewart - A structure with the following fields: + % - geometry.H [1x1] - Total Height of the Stewart Platform [m] + % - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m] + % - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m] + % - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m] +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 + args.MO_B (1,1) double {mustBeNumeric} = 50e-3 + end +#+end_src + +**** Compute the position of each frame +#+begin_src matlab + H = args.H; % Total Height of the Stewart Platform [m] + + FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m] + + MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] + + FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m] +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.geometry.H = H; + stewart.geometry.FO_M = FO_M; + stewart.platform_M.MO_B = MO_B; + stewart.platform_F.FO_A = FO_A; +#+end_src + +*** =generateGeneralConfiguration=: Generate a Very General Configuration +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/generateGeneralConfiguration.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Documentation + +#+begin_src latex :file stewart_bottom_plate.pdf :tangle no + \begin{tikzpicture} + % Internal and external limit + \draw[fill=white!80!black] (0, 0) circle [radius=3]; + % Circle where the joints are located + \draw[dashed] (0, 0) circle [radius=2.5]; + + % Bullets for the positions of the joints + \node[] (J1) at ( 80:2.5){$\bullet$}; + \node[] (J2) at (100:2.5){$\bullet$}; + \node[] (J3) at (200:2.5){$\bullet$}; + \node[] (J4) at (220:2.5){$\bullet$}; + \node[] (J5) at (320:2.5){$\bullet$}; + \node[] (J6) at (340:2.5){$\bullet$}; + + % Name of the points + \node[above right] at (J1) {$a_{1}$}; + \node[above left] at (J2) {$a_{2}$}; + \node[above left] at (J3) {$a_{3}$}; + \node[right ] at (J4) {$a_{4}$}; + \node[left ] at (J5) {$a_{5}$}; + \node[above right] at (J6) {$a_{6}$}; + + % First 2 angles + \draw[dashed, ->] (0:1) arc [start angle=0, end angle=80, radius=1] node[below right]{$\theta_{1}$}; + \draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left ]{$\theta_{2}$}; + + % Division of 360 degrees by 3 + \draw[dashed] (0, 0) -- ( 80:3.2); + \draw[dashed] (0, 0) -- (100:3.2); + \draw[dashed] (0, 0) -- (200:3.2); + \draw[dashed] (0, 0) -- (220:3.2); + \draw[dashed] (0, 0) -- (320:3.2); + \draw[dashed] (0, 0) -- (340:3.2); + + % Radius for the position of the joints + \draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5); + + \draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$}; + \draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$}; + \end{tikzpicture} +#+end_src + +**** Function description +#+begin_src matlab + function [stewart] = generateGeneralConfiguration(stewart, args) + % generateGeneralConfiguration - Generate a Very General Configuration + % + % Syntax: [stewart] = generateGeneralConfiguration(stewart, args) + % + % Inputs: + % - args - Can have the following fields: + % - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] + % - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] + % - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad] + % - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m] + % - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m] + % - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} + % - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3; + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); + end +#+end_src + +**** Compute the pose +#+begin_src matlab + Fa = zeros(3,6); + Mb = zeros(3,6); +#+end_src + +#+begin_src matlab + for i = 1:6 + Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; + Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; + end +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.platform_F.Fa = Fa; + stewart.platform_M.Mb = Mb; +#+end_src + +*** =computeJointsPose=: Compute the Pose of the Joints +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/computeJointsPose.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Documentation + +#+name: fig:stewart-struts +#+caption: Position and orientation of the struts +[[file:figs/stewart-struts.png]] + +**** Function description +#+begin_src matlab + function [stewart] = computeJointsPose(stewart) + % computeJointsPose - + % + % Syntax: [stewart] = computeJointsPose(stewart) + % + % Inputs: + % - stewart - A structure with the following fields + % - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} + % - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} + % - platform_F.FO_A [3x1] - Position of {A} with respect to {F} + % - platform_M.MO_B [3x1] - Position of {B} with respect to {M} + % - geometry.FO_M [3x1] - Position of {M} with respect to {F} + % + % Outputs: + % - stewart - A structure with the following added fields + % - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A} + % - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A} + % - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B} + % - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B} + % - geometry.l [6x1] - The i'th element is the initial length of strut i + % - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A} + % - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} + % - struts_F.l [6x1] - Length of the Fixed part of the i'th strut + % - struts_M.l [6x1] - Length of the Mobile part of the i'th strut + % - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} + % - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} +#+end_src + +**** Check the =stewart= structure elements +#+begin_src matlab + assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') + Fa = stewart.platform_F.Fa; + + assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb') + Mb = stewart.platform_M.Mb; + + assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A') + FO_A = stewart.platform_F.FO_A; + + assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B') + MO_B = stewart.platform_M.MO_B; + + assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M') + FO_M = stewart.geometry.FO_M; +#+end_src + +**** Compute the position of the Joints +#+begin_src matlab + Aa = Fa - repmat(FO_A, [1, 6]); + Bb = Mb - repmat(MO_B, [1, 6]); + + Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); + Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]); +#+end_src + +**** Compute the strut length and orientation +#+begin_src matlab + As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As + + l = vecnorm(Ab - Aa)'; +#+end_src + +#+begin_src matlab + Bs = (Bb - Ba)./vecnorm(Bb - Ba); +#+end_src + +**** Compute the orientation of the Joints +#+begin_src matlab + FRa = zeros(3,3,6); + MRb = zeros(3,3,6); + + for i = 1:6 + FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)]; + FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i)); + + MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)]; + MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); + end +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.geometry.Aa = Aa; + stewart.geometry.Ab = Ab; + stewart.geometry.Ba = Ba; + stewart.geometry.Bb = Bb; + stewart.geometry.As = As; + stewart.geometry.Bs = Bs; + stewart.geometry.l = l; + + stewart.struts_F.l = l/2; + stewart.struts_M.l = l/2; + + stewart.platform_F.FRa = FRa; + stewart.platform_M.MRb = MRb; +#+end_src + +*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeCylindricalPlatforms.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Function description +#+begin_src matlab + function [stewart] = initializeCylindricalPlatforms(stewart, args) + % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms + % + % Syntax: [stewart] = initializeCylindricalPlatforms(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Fpm [1x1] - Fixed Platform Mass [kg] + % - Fph [1x1] - Fixed Platform Height [m] + % - Fpr [1x1] - Fixed Platform Radius [m] + % - Mpm [1x1] - Mobile Platform Mass [kg] + % - Mph [1x1] - Mobile Platform Height [m] + % - Mpr [1x1] - Mobile Platform Radius [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - platform_F [struct] - structure with the following fields: + % - type = 1 + % - M [1x1] - Fixed Platform Mass [kg] + % - I [3x3] - Fixed Platform Inertia matrix [kg*m^2] + % - H [1x1] - Fixed Platform Height [m] + % - R [1x1] - Fixed Platform Radius [m] + % - platform_M [struct] - structure with the following fields: + % - M [1x1] - Mobile Platform Mass [kg] + % - I [3x3] - Mobile Platform Inertia matrix [kg*m^2] + % - H [1x1] - Mobile Platform Height [m] + % - R [1x1] - Mobile Platform Radius [m] +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + end +#+end_src + +**** Compute the Inertia matrices of platforms +#+begin_src matlab + I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/2 *args.Fpm * args.Fpr^2]); +#+end_src + +#+begin_src matlab + I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/2 *args.Mpm * args.Mpr^2]); +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.platform_F.type = 1; + + stewart.platform_F.I = I_F; + stewart.platform_F.M = args.Fpm; + stewart.platform_F.R = args.Fpr; + stewart.platform_F.H = args.Fph; +#+end_src + +#+begin_src matlab + stewart.platform_M.type = 1; + + stewart.platform_M.I = I_M; + stewart.platform_M.M = args.Mpm; + stewart.platform_M.R = args.Mpr; + stewart.platform_M.H = args.Mph; +#+end_src + +*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeCylindricalStruts.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Function description +#+begin_src matlab + function [stewart] = initializeCylindricalStruts(stewart, args) + % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts + % + % Syntax: [stewart] = initializeCylindricalStruts(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Fsm [1x1] - Mass of the Fixed part of the struts [kg] + % - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m] + % - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m] + % - Msm [1x1] - Mass of the Mobile part of the struts [kg] + % - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m] + % - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - struts_F [struct] - structure with the following fields: + % - M [6x1] - Mass of the Fixed part of the struts [kg] + % - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] + % - H [6x1] - Height of cylinder for the Fixed part of the struts [m] + % - R [6x1] - Radius of cylinder for the Fixed part of the struts [m] + % - struts_M [struct] - structure with the following fields: + % - M [6x1] - Mass of the Mobile part of the struts [kg] + % - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] + % - H [6x1] - Height of cylinder for the Mobile part of the struts [m] + % - R [6x1] - Radius of cylinder for the Mobile part of the struts [m] +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + end +#+end_src + +**** Compute the properties of the cylindrical struts + +#+begin_src matlab + Fsm = ones(6,1).*args.Fsm; + Fsh = ones(6,1).*args.Fsh; + Fsr = ones(6,1).*args.Fsr; + + Msm = ones(6,1).*args.Msm; + Msh = ones(6,1).*args.Msh; + Msr = ones(6,1).*args.Msr; +#+end_src + +#+begin_src matlab + I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut + I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut + + for i = 1:6 + I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ... + 1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ... + 1/2 * Fsm(i) * Fsr(i)^2]); + + I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ... + 1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ... + 1/2 * Msm(i) * Msr(i)^2]); + end +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.struts_M.type = 1; + + stewart.struts_M.I = I_M; + stewart.struts_M.M = Msm; + stewart.struts_M.R = Msr; + stewart.struts_M.H = Msh; +#+end_src + +#+begin_src matlab + stewart.struts_F.type = 1; + + stewart.struts_F.I = I_F; + stewart.struts_F.M = Fsm; + stewart.struts_F.R = Fsr; + stewart.struts_F.H = Fsh; +#+end_src + +*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeStrutDynamics.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Documentation + +#+name: fig:piezoelectric_stack +#+attr_html: :width 500px +#+caption: Example of a piezoelectric stach actuator (PI) +[[file:figs/piezoelectric_stack.jpg]] + +A simplistic model of such amplified actuator is shown in Figure ref:fig:actuator_model_simple where: +- $K$ represent the vertical stiffness of the actuator +- $C$ represent the vertical damping of the actuator +- $F$ represents the force applied by the actuator +- $F_{m}$ represents the total measured force +- $v_{m}$ represents the absolute velocity of the top part of the actuator +- $d_{m}$ represents the total relative displacement of the actuator + +#+begin_src latex :file actuator_model_simple.pdf :tangle no + \begin{tikzpicture} + \draw (-1, 0) -- (1, 0); + + % Spring, Damper, and Actuator + \draw[spring] (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$}; + \draw[damper] ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$}; + \draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$}; + + \node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){}; + + \node[left] at (fsens.west) {$F_{m}$}; + + \draw[dashed] (1, 0) -- ++(0.4, 0); + \draw[dashed] (1, 1.7) -- ++(0.4, 0); + + \draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$}; + + \draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$}; + \end{tikzpicture} +#+end_src + +#+name: fig:actuator_model_simple +#+caption: Simple model of an Actuator +#+RESULTS: +[[file:figs/actuator_model_simple.png]] + +**** Function description +#+begin_src matlab + function [stewart] = initializeStrutDynamics(stewart, args) + % initializeStrutDynamics - Add Stiffness and Damping properties of each strut + % + % Syntax: [stewart] = initializeStrutDynamics(args) + % + % Inputs: + % - args - Structure with the following fields: + % - K [6x1] - Stiffness of each strut [N/m] + % - C [6x1] - Damping of each strut [N/(m/s)] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - actuators.type = 1 + % - actuators.K [6x1] - Stiffness of each strut [N/m] + % - actuators.C [6x1] - Damping of each strut [N/(m/s)] +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical' + args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1) + args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1) + args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1) + args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1) + args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1) + args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1) + args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1) + args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1) + args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1) + end +#+end_src + +**** Add Stiffness and Damping properties of each strut +#+begin_src matlab + if strcmp(args.type, 'classical') + stewart.actuators.type = 1; + elseif strcmp(args.type, 'amplified') + stewart.actuators.type = 2; + end + + stewart.actuators.K = args.K; + stewart.actuators.C = args.C; + + stewart.actuators.k1 = args.k1; + stewart.actuators.c1 = args.c1; + + stewart.actuators.ka = args.ka; + stewart.actuators.ke = args.ke; + + stewart.actuators.F_gain = args.F_gain; + + stewart.actuators.ma = args.ma; + stewart.actuators.me = args.me; +#+end_src + +*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeJointDynamics.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Function description +#+begin_src matlab + function [stewart] = initializeJointDynamics(stewart, args) + % initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints + % + % Syntax: [stewart] = initializeJointDynamics(args) + % + % Inputs: + % - args - Structure with the following fields: + % - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p' + % - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p' + % - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad] + % - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad] + % - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)] + % - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)] + % - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad] + % - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad] + % - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)] + % - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - stewart.joints_F and stewart.joints_M: + % - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect) + % - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m] + % - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad] + % - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad] + % - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)] + % - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)] + % - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)] +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal' + args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical' + args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1) + args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1) + args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) + args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1) + args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1) + args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) + args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1) + args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1) + args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1) + args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1) + args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.K_M double {mustBeNumeric} = zeros(6,6) + args.M_M double {mustBeNumeric} = zeros(6,6) + args.n_xyz_M double {mustBeNumeric} = zeros(2,3) + args.xi_M double {mustBeNumeric} = 0.1 + args.step_file_M char {} = '' + args.K_F double {mustBeNumeric} = zeros(6,6) + args.M_F double {mustBeNumeric} = zeros(6,6) + args.n_xyz_F double {mustBeNumeric} = zeros(2,3) + args.xi_F double {mustBeNumeric} = 0.1 + args.step_file_F char {} = '' + end +#+end_src + +**** Add Actuator Type +#+begin_src matlab + switch args.type_F + case 'universal' + stewart.joints_F.type = 1; + case 'spherical' + stewart.joints_F.type = 2; + case 'universal_p' + stewart.joints_F.type = 3; + case 'spherical_p' + stewart.joints_F.type = 4; + case 'flexible' + stewart.joints_F.type = 5; + case 'universal_3dof' + stewart.joints_F.type = 6; + case 'spherical_3dof' + stewart.joints_F.type = 7; + end + + switch args.type_M + case 'universal' + stewart.joints_M.type = 1; + case 'spherical' + stewart.joints_M.type = 2; + case 'universal_p' + stewart.joints_M.type = 3; + case 'spherical_p' + stewart.joints_M.type = 4; + case 'flexible' + stewart.joints_M.type = 5; + case 'universal_3dof' + stewart.joints_M.type = 6; + case 'spherical_3dof' + stewart.joints_M.type = 7; + end +#+end_src + +**** Add Stiffness and Damping in Translation of each strut +Axial and Radial (shear) Stiffness +#+begin_src matlab + stewart.joints_M.Ka = args.Ka_M; + stewart.joints_M.Kr = args.Kr_M; + + stewart.joints_F.Ka = args.Ka_F; + stewart.joints_F.Kr = args.Kr_F; +#+end_src + +Translation Damping +#+begin_src matlab + stewart.joints_M.Ca = args.Ca_M; + stewart.joints_M.Cr = args.Cr_M; + + stewart.joints_F.Ca = args.Ca_F; + stewart.joints_F.Cr = args.Cr_F; +#+end_src + +**** Add Stiffness and Damping in Rotation of each strut +Rotational Stiffness +#+begin_src matlab + stewart.joints_M.Kf = args.Kf_M; + stewart.joints_M.Kt = args.Kt_M; + + stewart.joints_F.Kf = args.Kf_F; + stewart.joints_F.Kt = args.Kt_F; +#+end_src + +Rotational Damping +#+begin_src matlab + stewart.joints_M.Cf = args.Cf_M; + stewart.joints_M.Ct = args.Ct_M; + + stewart.joints_F.Cf = args.Cf_F; + stewart.joints_F.Ct = args.Ct_F; +#+end_src + +**** Stiffness and Mass matrices for flexible joint + +#+begin_src matlab + stewart.joints_F.M = args.M_F; + stewart.joints_F.K = args.K_F; + stewart.joints_F.n_xyz = args.n_xyz_F; + stewart.joints_F.xi = args.xi_F; + stewart.joints_F.xi = args.xi_F; + stewart.joints_F.step_file = args.step_file_F; + + stewart.joints_M.M = args.M_M; + stewart.joints_M.K = args.K_M; + stewart.joints_M.n_xyz = args.n_xyz_M; + stewart.joints_M.xi = args.xi_M; + stewart.joints_M.step_file = args.step_file_M; +#+end_src + +*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeStewartPose.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Function description +#+begin_src matlab + function [stewart] = initializeStewartPose(stewart, args) + % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose + % It uses the inverse kinematic + % + % Syntax: [stewart] = initializeStewartPose(stewart, args) + % + % Inputs: + % - stewart - A structure with the following fields + % - Aa [3x6] - The positions ai expressed in {A} + % - Bb [3x6] - The positions bi expressed in {B} + % - args - Can have the following fields: + % - AP [3x1] - The wanted position of {B} with respect to {A} + % - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A} +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + end +#+end_src + +**** Use the Inverse Kinematic function +#+begin_src matlab + [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.actuators.Leq = dLi; +#+end_src + +*** =computeJacobian=: Compute the Jacobian Matrix +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/computeJacobian.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Function description +#+begin_src matlab + function [stewart] = computeJacobian(stewart) + % computeJacobian - + % + % Syntax: [stewart] = computeJacobian(stewart) + % + % Inputs: + % - stewart - With at least the following fields: + % - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A} + % - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A} + % - actuators.K [6x1] - Total stiffness of the actuators + % + % Outputs: + % - stewart - With the 3 added field: + % - kinematics.J [6x6] - The Jacobian Matrix + % - kinematics.K [6x6] - The Stiffness Matrix + % - kinematics.C [6x6] - The Compliance Matrix +#+end_src + +**** Check the =stewart= structure elements +#+begin_src matlab + assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As') + As = stewart.geometry.As; + + assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab') + Ab = stewart.geometry.Ab; + + assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K') + Ki = stewart.actuators.K; +#+end_src + + +**** Compute Jacobian Matrix +#+begin_src matlab + J = [As' , cross(Ab, As)']; +#+end_src + +**** Compute Stiffness Matrix +#+begin_src matlab + K = J'*diag(Ki)*J; +#+end_src + +**** Compute Compliance Matrix +#+begin_src matlab + C = inv(K); +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.kinematics.J = J; + stewart.kinematics.K = K; + stewart.kinematics.C = C; +#+end_src + +*** =initializeInertialSensor=: Initialize the inertial sensor in each strut +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/initializeInertialSensor.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Geophone - Working Principle +From the schematic of the Z-axis geophone shown in Figure ref:fig:z_axis_geophone, we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$: +\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \] +with: +- $\omega_0 = \sqrt{\frac{k}{m}}$ +- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$ + +#+name: fig:z_axis_geophone +#+caption: Schematic of a Z-Axis geophone +[[file:figs/inertial_sensor.png]] + +We see that at frequencies above $\omega_0$: +\[ \frac{\dot{d}}{\dot{w}} \approx -1 \] + +And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support. + +We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass. + +**** Accelerometer - Working Principle +From the schematic of the Z-axis accelerometer shown in Figure ref:fig:z_axis_accelerometer, we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$: +\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \] +with: +- $\omega_0 = \sqrt{\frac{k}{m}}$ +- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$ + +#+name: fig:z_axis_accelerometer +#+caption: Schematic of a Z-Axis geophone +[[file:figs/inertial_sensor.png]] + +We see that at frequencies below $\omega_0$: +\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \] + +And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support. + +Note that there is trade-off between: +- the highest measurable acceleration $\omega_0$ +- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$ + +**** Function description +#+begin_src matlab + function [stewart] = initializeInertialSensor(stewart, args) + % initializeInertialSensor - Initialize the inertial sensor in each strut + % + % Syntax: [stewart] = initializeInertialSensor(args) + % + % Inputs: + % - args - Structure with the following fields: + % - type - 'geophone', 'accelerometer', 'none' + % - mass [1x1] - Weight of the inertial mass [kg] + % - freq [1x1] - Cutoff frequency [Hz] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - stewart.sensors.inertial + % - type - 1 (geophone), 2 (accelerometer), 3 (none) + % - K [1x1] - Stiffness [N/m] + % - C [1x1] - Damping [N/(m/s)] + % - M [1x1] - Inertial Mass [kg] + % - G [1x1] - Gain +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none' + args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2 + args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3 + end +#+end_src + +**** Compute the properties of the sensor +#+begin_src matlab + sensor = struct(); + + switch args.type + case 'geophone' + sensor.type = 1; + + sensor.M = args.mass; + sensor.K = sensor.M * (2*pi*args.freq)^2; + sensor.C = 2*sqrt(sensor.M * sensor.K); + case 'accelerometer' + sensor.type = 2; + + sensor.M = args.mass; + sensor.K = sensor.M * (2*pi*args.freq)^2; + sensor.C = 2*sqrt(sensor.M * sensor.K); + sensor.G = -sensor.K/sensor.M; + case 'none' + sensor.type = 3; + end +#+end_src + +**** Populate the =stewart= structure +#+begin_src matlab + stewart.sensors.inertial = sensor; +#+end_src + + + +*** =inverseKinematics=: Compute Inverse Kinematics +:PROPERTIES: +:header-args:matlab+: :tangle matlab/src/inverseKinematics.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +**** Theory +For inverse kinematic analysis, it is assumed that the position ${}^A\mathbf{P}$ and orientation of the moving platform ${}^A\mathbf{R}_B$ are given and the problem is to obtain the joint variables, namely, $\mathbf{L} = [l_1, l_2, \dots, l_6]^T$. + +From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as +\begin{align*} + l_i {}^A\hat{\mathbf{s}}_i &= {}^A\mathbf{A} + {}^A\mathbf{b}_i - {}^A\mathbf{a}_i \\ + &= {}^A\mathbf{A} + {}^A\mathbf{R}_b {}^B\mathbf{b}_i - {}^A\mathbf{a}_i +\end{align*} + +To obtain the length of each actuator and eliminate $\hat{\mathbf{s}}_i$, it is sufficient to dot multiply each side by itself: +\begin{equation} + l_i^2 \left[ {}^A\hat{\mathbf{s}}_i^T {}^A\hat{\mathbf{s}}_i \right] = \left[ {}^A\mathbf{P} + {}^A\mathbf{R}_B {}^B\mathbf{b}_i - {}^A\mathbf{a}_i \right]^T \left[ {}^A\mathbf{P} + {}^A\mathbf{R}_B {}^B\mathbf{b}_i - {}^A\mathbf{a}_i \right] +\end{equation} + +Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by: +\begin{equation} + l_i = \sqrt{{}^A\mathbf{P}^T {}^A\mathbf{P} + {}^B\mathbf{b}_i^T {}^B\mathbf{b}_i + {}^A\mathbf{a}_i^T {}^A\mathbf{a}_i - 2 {}^A\mathbf{P}^T {}^A\mathbf{a}_i + 2 {}^A\mathbf{P}^T \left[{}^A\mathbf{R}_B {}^B\mathbf{b}_i\right] - 2 \left[{}^A\mathbf{R}_B {}^B\mathbf{b}_i\right]^T {}^A\mathbf{a}_i} +\end{equation} + +If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation. +Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable. + +**** Function description +#+begin_src matlab + function [Li, dLi] = inverseKinematics(stewart, args) + % inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A} + % + % Syntax: [stewart] = inverseKinematics(stewart) + % + % Inputs: + % - stewart - A structure with the following fields + % - geometry.Aa [3x6] - The positions ai expressed in {A} + % - geometry.Bb [3x6] - The positions bi expressed in {B} + % - geometry.l [6x1] - Length of each strut + % - args - Can have the following fields: + % - AP [3x1] - The wanted position of {B} with respect to {A} + % - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} + % + % Outputs: + % - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A} + % - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A} +#+end_src + +**** Optional Parameters +#+begin_src matlab + arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + end +#+end_src + +**** Check the =stewart= structure elements +#+begin_src matlab + assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa') + Aa = stewart.geometry.Aa; + + assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb') + Bb = stewart.geometry.Bb; + + assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l') + l = stewart.geometry.l; +#+end_src + + +**** Compute +#+begin_src matlab + Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa)); +#+end_src + +#+begin_src matlab + dLi = Li-l; +#+end_src + * Footnotes [fn:2]M12/F40 model from Attocube [fn:1]Depending on the measuring range, gap can range from $\approx 1\,\mu m$ to $\approx 100\,\mu m$ diff --git a/test-bench-id31.pdf b/test-bench-id31.pdf index 8cd81ec..3bc8a2b 100644 Binary files a/test-bench-id31.pdf and b/test-bench-id31.pdf differ diff --git a/test-bench-id31.tex b/test-bench-id31.tex index 2ff06b6..69299b0 100644 --- a/test-bench-id31.tex +++ b/test-bench-id31.tex @@ -1,4 +1,4 @@ -% Created 2024-11-13 Wed 10:25 +% Created 2024-11-13 Wed 17:06 % Intended LaTeX compiler: pdflatex \documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt} @@ -54,6 +54,7 @@ High authority control is then applied \end{figure} \chapter{Short Stroke Metrology System} +\label{sec:org598c753} \label{sec:test_id31_metrology} The control of the nano-hexapod requires an external metrology system measuring the relative position of the nano-hexapod top platform with respect to the granite. As the long-stroke (\(\approx 1 \,cm^3\)) metrology system was not developed yet, a stroke stroke (\(> 100\,\mu m^3\)) was used instead to validate the nano-hexapod control. @@ -91,6 +92,7 @@ This way, the gap between the sensor and the reference sphere is much larger (he Nevertheless, the metrology system still has limited measurement range, as when the spheres are moving perpendicularly to the beam axis, the reflected light does not coincide with the incident light, and for some perpendicular displacement, the interference is too small to be detected. \section{Metrology Kinematics} +\label{sec:org1981747} \label{ssec:test_id31_metrology_kinematics} The developed short-stroke metrology system is schematically shown in Figure \ref{fig:test_id31_metrology_kinematics}. @@ -133,6 +135,7 @@ The five equations \eqref{eq:test_id31_metrology_kinematics} can be written in a \end{equation} \section{Rough alignment of the reference spheres} +\label{sec:orgd2560a6} \label{ssec:test_id31_metrology_sphere_rought_alignment} The two reference spheres are aligned with the rotation axis of the spindle. @@ -147,6 +150,7 @@ This is probably limited due to the poor coaxiality between the cylinders and th However, the alignment precision should be enough to stay in the acceptance of the interferometers. \section{Tip-Tilt adjustment of the interferometers} +\label{sec:orgb663f66} \label{ssec:test_id31_metrology_alignment} The short stroke metrology system is placed on top of the main granite using a gantry made of granite blocs to have good vibration and thermal stability (Figure \ref{fig:short_stroke_metrology_overview}). @@ -165,6 +169,7 @@ This is equivalent as to optimize the perpendicularity between the interferomete The lateral sensor heads (i.e. all except the top one), which are each fixed to a custom tip-tilt adjustment mechanism, are individually oriented such that the coupling efficient is maximized. \section{Fine Alignment of reference spheres using interferometers} +\label{sec:orgde2bc57} \label{ssec:test_id31_metrology_sphere_fine_alignment} Thanks to the good alignment of the two reference spheres with the spindle axis and to the fine adjustment of the interferometers orientations, the interferometer measurement is made possible during complete spindle rotation. @@ -196,6 +201,7 @@ Remaining error after alignment is in the order of \(\pm5\,\mu\text{rad}\) for a \section{Estimated measurement volume} +\label{sec:org88488f3} \label{ssec:test_id31_metrology_acceptance} Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited. @@ -222,6 +228,7 @@ The obtained lateral acceptance for pure displacements in any direction is estim \section{Estimated measurement errors} +\label{sec:org9791179} \label{ssec:test_id31_metrology_errors} When using the NASS, the accuracy of the sample's positioning is linked to the accuracy of the external metrology. @@ -236,6 +243,7 @@ As the interferometer is pointing to a sphere and not to a plane, lateral motion Then, the reference spheres have some deviations with respect to an ideal sphere. They are meant to be used with capacitive sensors which are integrating the shape errors over large surfaces. When using interferometers, the size of the ``light spot'' on the sphere surface is a circle with a diameter \(\approx 50\,\mu m\), therefore the system is more sensitive to shape errors with small features. + As the interferometer light is travelling in air, the measured distance is sensitive to any variation in the refractive index of the air. Therefore, any variation of air temperature, pressure or humidity will induce measurement errors. For a measurement length of \(40\,mm\), a temperature variation of \(0.1\,{}^oC\) induces an errors in the distance measurement of \(\approx 4\,nm\). @@ -259,5 +267,164 @@ The effect of the noise on the translation and rotation measurements is estimate \caption{\label{fig:test_id31_metrology_errors}Estimated measurement errors of the metrology. Cross-coupling between lateral motion and vertical measurement is shown in (\subref{fig:test_id31_xy_map_sphere}). Effect of interferometer noise on the measured translations and rotations is shown in (\subref{fig:test_id31_interf_noise}).} \end{figure} +\chapter{Identified Open Loop Plant} +\label{sec:org7b9ff96} +\label{sec:test_id31_open_loop_plant} +\section{First Open-Loop Plant Identification} +\label{sec:orgd1bd080} +\label{ssec:test_id31_open_loop_plant_first_id} + +The plant dynamics is first identified for a fixed spindle angle (at \(0\,\text{deg}\)) and without any payload. +The model dynamics is also identified in the same conditions. + +A first comparison between the model and the measured dynamics is done in Figure \ref{fig:test_id31_first_id}. +A good match can be observed for the diagonal dynamics (except the high frequency modes which are not modeled). +However, the coupling for the transfer function from command signals \(\mathbf{u}\) to estimated strut motion from the external metrology \(e\mathbf{\mathcal{L}}\) is larger than expected (Figure \ref{fig:test_id31_first_id_int}). + +\begin{figure}[htbp] +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_first_id_int.png} +\end{center} +\subcaption{\label{fig:test_id31_first_id_int}External Metrology} +\end{subfigure} +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_first_id_iff.png} +\end{center} +\subcaption{\label{fig:test_id31_first_id_iff}Force Sensors} +\end{subfigure} +\caption{\label{fig:test_id31_first_id}Comparison between the measured dynamics and the multi-body model dynamics. Both for the external metrology (\subref{fig:test_id31_first_id_int}) and force sensors (\subref{fig:test_id31_first_id_iff}).} +\end{figure} + +\section{Better Angular Alignment} +\label{sec:org205ba5e} +\label{ssec:test_id31_open_loop_plant_rz_alignment} + +One possible explanation of the increased coupling observed in Figure \ref{fig:test_id31_first_id_int} is the poor alignment between the external metrology axes (i.e. the interferometer supports) and the nano-hexapod axes. +To estimate this alignment, a decentralized low-bandwidth feedback controller based on the nano-hexapod encoders is implemented. +This allowed to perform two straight movements of the nano-hexapod along the \(x\) and \(y\) axes in the frame of the nano-hexapod. +During these two movements, the external metrology measurement is recorded and shown in Figure \ref{fig:test_id31_Rz_align_error}. +It was found that there is a misalignment of 2.7 degrees (rotation along the vertical axis) between the interferometer axes and nano-hexapod axes. +This was corrected by adding an offset to the spindle angle. +To check that the alignment has improved, the same movement was performed using the nano-hexapod while recording the signal of the external metrology. +Results shown in Figure \ref{fig:test_id31_Rz_align_correct} are indeed indicating much better alignment. + +\begin{figure}[htbp] +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/test_id31_Rz_align_error.png} +\end{center} +\subcaption{\label{fig:test_id31_Rz_align_error}Before alignment} +\end{subfigure} +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/test_id31_Rz_align_correct.png} +\end{center} +\subcaption{\label{fig:test_id31_Rz_align_correct}After alignment} +\end{subfigure} +\caption{\label{fig:test_id31_Rz_align_error}Measurement of the Nano-Hexapod axes in the frame of the external metrology. Before alignment (\subref{fig:test_id31_Rz_align_error}) and after alignment (\subref{fig:test_id31_Rz_align_correct}).} +\end{figure} + +\section{Open-Loop Identification after alignment} +\label{sec:orgcb4c885} +\label{ssec:test_id31_open_loop_plant_after_alignment} + +The plant dynamics is identified after the fine alignment and is compared with the model dynamics in Figure \ref{fig:test_id31_first_id_int_better_rz_align}. +Compared to the initial identification shown in Figure \ref{fig:test_id31_first_id_int}, the obtained coupling has decreased and is close to the coupling obtained with the multi-body model. + +\begin{figure}[htbp] +\centering +\includegraphics[scale=1]{figs/test_id31_first_id_int_better_rz_align.png} +\caption{\label{fig:test_id31_first_id_int_better_rz_align}Decrease of the coupling with better Rz alignment} +\end{figure} + +\section{Effect of Payload Mass} +\label{sec:org497746b} +\label{ssec:test_id31_open_loop_plant_mass} + +The system dynamics was identified with four payload conditions that are shown in Figure \ref{fig:test_id31_picture_masses}. +The obtained direct terms are compared with the model dynamics in Figure \ref{fig:test_nhexa_comp_simscape_diag_masses}. + +\begin{figure}[htbp] +\begin{subfigure}{0.24\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.99\linewidth]{figs/test_id31_picture_mass_m0.jpg} +\end{center} +\subcaption{\label{fig:test_id31_picture_mass_m0}$m=0\,\text{kg}$} +\end{subfigure} +\begin{subfigure}{0.24\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.99\linewidth]{figs/test_id31_picture_mass_m1.jpg} +\end{center} +\subcaption{\label{fig:test_id31_picture_mass_m1}$m=13\,\text{kg}$} +\end{subfigure} +\begin{subfigure}{0.24\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.99\linewidth]{figs/test_id31_picture_mass_m2.jpg} +\end{center} +\subcaption{\label{fig:test_id31_picture_mass_m2}$m=26\,\text{kg}$} +\end{subfigure} +\begin{subfigure}{0.24\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.99\linewidth]{figs/test_id31_picture_mass_m3.jpg} +\end{center} +\subcaption{\label{fig:test_id31_picture_mass_m3}$m=39\,\text{kg}$} +\end{subfigure} +\caption{\label{fig:test_id31_picture_masses}The four tested payload conditions. (\subref{fig:test_id31_picture_mass_m0}) without payload. (\subref{fig:test_id31_picture_mass_m1}) with \(13\,\text{kg}\) payload. (\subref{fig:test_id31_picture_mass_m2}) with \(26\,\text{kg}\) payload. (\subref{fig:test_id31_picture_mass_m3}) with \(39\,\text{kg}\) payload.} +\end{figure} + +\begin{figure}[htbp] +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_comp_simscape_int_diag_masses.png} +\end{center} +\subcaption{\label{fig:test_id31_comp_simscape_int_diag_masses}from $u$ to $d_e$} +\end{subfigure} +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_comp_simscape_iff_diag_masses.png} +\end{center} +\subcaption{\label{fig:test_id31_comp_simscape_iff_diag_masses}from $u$ to $V_s$} +\end{subfigure} +\caption{\label{fig:test_nhexa_comp_simscape_diag_masses}Comparison of the diagonal elements (i.e. ``direct'' terms) of the measured FRF matrix and the dynamics identified from the Simscape model. Both for the dynamics from \(u\) to \(e\mathcal{L}\) (\subref{fig:test_id31_comp_simscape_int_diag_masses}) and from \(u\) to \(V_s\) (\subref{fig:test_id31_comp_simscape_iff_diag_masses})} +\end{figure} + +\section{Effect of Rotation} +\label{sec:orgff15d08} +\label{ssec:test_id31_open_loop_plant_rotation} + +The dynamics was then identified while the Spindle was rotating at constant velocity. +Three identification experiments were performed: no spindle rotation, spindle rotation at \(36\,\text{deg}/s\) and at \(180\,\text{deg}/s\). + +The comparison of the obtained dynamics from command signal \(u\) to estimated strut error \(e\mathcal{L}\) is done in Figure \ref{fig:test_id31_effect_rotation}. +Both direct terms (Figure \ref{fig:test_id31_effect_rotation_direct}) and coupling terms (Figure \ref{fig:test_id31_effect_rotation_coupling}) are unaffected by the rotation. +The same can be observed for the dynamics from the command signal to the encoders and to the force sensors. +This confirms that the rotation has no significant effect on the plant dynamics. +This also indicates that the metrology kinematics is correct and is working in real time. + +\begin{figure}[htbp] +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_effect_rotation_direct.png} +\end{center} +\subcaption{\label{fig:test_id31_effect_rotation_direct}Direct terms} +\end{subfigure} +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/test_id31_effect_rotation_coupling.png} +\end{center} +\subcaption{\label{fig:test_id31_effect_rotation_coupling}Coupling terms} +\end{subfigure} +\caption{\label{fig:test_id31_effect_rotation}Effect of the spindle rotation on the plant dynamics from \(u\) to \(e\mathcal{L}\). Three rotational velocities are tested (\(0\,\text{deg}/s\), \(36\,\text{deg}/s\) and \(180\,\text{deg}/s\)). Both direct terms (\subref{fig:test_id31_effect_rotation_direct}) and coupling terms (\subref{fig:test_id31_effect_rotation_coupling}) are displayed.} +\end{figure} + +\section{Conclusion} +\label{sec:orgd703c88} + +\begin{itemize} +\item Good match between the model and experiment +\end{itemize} + \printbibliography[heading=bibintoc,title={Bibliography}] \end{document}