From b75f5cbfbdc52f7f060bc3911524c53a862c5250 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Fri, 28 Mar 2025 16:23:36 +0100 Subject: [PATCH] Delete backup.org --- backup.org | 16300 ------------------------------------------ test-bench-id31.org | 1 + 2 files changed, 1 insertion(+), 16300 deletions(-) delete mode 100644 backup.org diff --git a/backup.org b/backup.org deleted file mode 100644 index 9124ad5..0000000 --- a/backup.org +++ /dev/null @@ -1,16300 +0,0 @@ -* DONE Identified Open Loop Plant -CLOSED: [2024-11-13 Wed 18:19] -<> -** Introduction :ignore: - -** Matlab Init :noexport:ignore: -#+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 - -** Simscape Model -<> -*** Introduction :ignore: - -*** Matlab Init :noexport:ignore: -#+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 - -*** Init model -#+begin_src matlab -%% Add directories to path for Simscape Model -addpath('mat') -addpath('matlab/subsystems') -addpath('STEPS/nano_hexapod') -addpath('STEPS/metrology') -addpath('STEPS/png') -#+end_src - -#+begin_src matlab -%% Options for Linearized -options = linearizeOptions; -options.SampleTime = 0; - -%% Name of the Simulink File -mdl = 'nass_model_id31'; -#+end_src - -#+begin_src matlab -open(mdl) -#+end_src - -#+begin_src matlab -load('mat/conf_simulink.mat'); - -%% Initialize each Simscape model elements -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); - -initializeSimscapeConfiguration(); -initializeDisturbances('enable', false); -initializeLoggingConfiguration('log', 'none'); - -initializeController('type', 'open-loop'); - -initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -initializeSample('type', '0'); - -initializePosError(); - -initializeReferences(); - -initializeRzEstimate('type', 'encoders'); - -initializeLion(); -#+end_src - -*** Identify Transfer functions -#+begin_src matlab -%% 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, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Encoders -io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors -io(io_i) = linio([mdl, '/Micro-Station/metrology_5dof/Lion_Metrology'], 1, 'output'); io_i = io_i + 1; % Interferometers -#+end_src - -#+begin_src matlab :exports none -%% Identify the dynamics for IFF plant -initializeSample('type', '0'); -Gm_m0 = linearize(mdl, io); -Gm_m0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -Gm_m0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... - 'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... - 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ... - 'd1', 'd2', 'd3', 'd4', 'd5'}; - -initializeSample('type', '1'); -Gm_m1 = linearize(mdl, io); -Gm_m1.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -Gm_m1.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... - 'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... - 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ... - 'd1', 'd2', 'd3', 'd4', 'd5'}; - -initializeSample('type', '2'); -Gm_m2 = linearize(mdl, io); -Gm_m2.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -Gm_m2.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... - 'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... - 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ... - 'd1', 'd2', 'd3', 'd4', 'd5'}; - -initializeSample('type', '3'); -Gm_m3 = linearize(mdl, io); -Gm_m3.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -Gm_m3.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ... - 'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... - 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ... - 'd1', 'd2', 'd3', 'd4', 'd5'}; -#+end_src - -#+begin_src matlab :exports none -%% Save Identified Plants -save('./matlab/mat/Gm.mat', 'Gm_m0', 'Gm_m1', 'Gm_m2', 'Gm_m3'); -#+end_src - -*** IFF Plant -#+begin_src matlab :exports none :results none -%% IFF transfer function - Simscape model -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%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_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%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_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%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_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%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_m3(sprintf('Fn%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 [V/V]'); set(gca, 'XTickLabel',[]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%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_m1(sprintf('Fn%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_m2(sprintf('Fn%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_m3(sprintf('Fn%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([-90, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_simscape_iff_ol_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_simscape_iff_ol_plant -#+caption: IFF transfer function - Simscape model -#+RESULTS: -[[file:figs/id31_simscape_iff_ol_plant.png]] - -*** Encoder plant -#+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(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%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_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%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_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%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_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%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_m3(sprintf('dL%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_m0(sprintf('dL%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_m1(sprintf('dL%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_m2(sprintf('dL%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_m3(sprintf('dL%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([-90, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_simscape_enc_ol_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_simscape_enc_ol_plant -#+caption: ENC transfer function - Simscape model -#+RESULTS: -[[file:figs/id31_simscape_enc_ol_plant.png]] - -*** HAC Undamped plant -#+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(freqs, abs(squeeze(freqresp(Gm_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_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_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_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_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_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_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_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_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_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_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_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([-90, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_simscape_int_ol_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_simscape_int_ol_plant -#+caption: INT transfer function - Simscape model -#+RESULTS: -[[file:figs/id31_simscape_int_ol_plant.png]] - -** Load Data :noexport: -#+begin_src matlab -%% Load identification data -data_m0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir)); -data_m1 = load(sprintf('%s/dynamics/2023-08-08_18-57_ol_plant_m1_Wz0.mat', mat_dir)); -data_m2 = load(sprintf('%s/dynamics/2023-08-08_17-12_ol_plant_m2_Wz0.mat', mat_dir)); -data_m3 = load(sprintf('%s/dynamics/2023-08-08_18-20_ol_plant_m3_Wz0.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none -% Sampling Time [s] -Ts = 1e-4; - -% Hannning Windows -Nfft = floor(2.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_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -** IFF Plant -#+begin_src matlab :exports none -%% IFF Plant (transfer function from u to taum) -G_iff_m0 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0.(sprintf("uL%i", i_strut)).Vs6]'; - - G_iff_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% IFF Plant (transfer function from u to taum) -G_iff_m1 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m1.(sprintf("uL%i", i_strut)).Vs1 ; data_m1.(sprintf("uL%i", i_strut)).Vs2 ; data_m1.(sprintf("uL%i", i_strut)).Vs3 ; data_m1.(sprintf("uL%i", i_strut)).Vs4 ; data_m1.(sprintf("uL%i", i_strut)).Vs5 ; data_m1.(sprintf("uL%i", i_strut)).Vs6]'; - - G_iff_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% IFF Plant (transfer function from u to taum) -G_iff_m2 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m2.(sprintf("uL%i", i_strut)).Vs1 ; data_m2.(sprintf("uL%i", i_strut)).Vs2 ; data_m2.(sprintf("uL%i", i_strut)).Vs3 ; data_m2.(sprintf("uL%i", i_strut)).Vs4 ; data_m2.(sprintf("uL%i", i_strut)).Vs5 ; data_m2.(sprintf("uL%i", i_strut)).Vs6]'; - - G_iff_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% IFF Plant (transfer function from u to taum) -G_iff_m3 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m3.(sprintf("uL%i", i_strut)).Vs1 ; data_m3.(sprintf("uL%i", i_strut)).Vs2 ; data_m3.(sprintf("uL%i", i_strut)).Vs3 ; data_m3.(sprintf("uL%i", i_strut)).Vs4 ; data_m3.(sprintf("uL%i", i_strut)).Vs5 ; data_m3.(sprintf("uL%i", i_strut)).Vs6]'; - - G_iff_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% Save Identified Plants -save('./matlab/mat/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 -%% 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_iff_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:6 - plot(f, abs(G_iff_m0(:,i, i)), 'color', colors(i,:), ... - 'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i)); -end -plot(f, abs(G_iff_m0(:, 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_m0(:,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_iff_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_iff_ol_plant_m0 -#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors -#+RESULTS: -[[file:figs/id31_iff_ol_plant_m0.png]] - -The measured frequency response functions from DAC voltages $u_i$ to measured voltages on the force sensors $\tau_{m,i}$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_iff. - -#+begin_src matlab :exports none :results none -%% Comparison of the Simscape model and identified IFF plant -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_m0(:, i, j)), 'color', [colors(1,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:)], ... - 'DisplayName', '$\tau_{m,i}/u_i$ - Identified'); -for i = 2:6 - plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:)], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ... - 'DisplayName', '$\tau_{m,i}/u_i$ - Simscape'); -for i = 2:6 - plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ... - 'DisplayName', '$\tau_{m,i}/u_j$ - Identified'); -plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... - 'DisplayName', '$\tau_{m,i}/u_j$ - Simscape'); -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_m0(:,i, i)), 'color', [colors(1,:)]); -end -for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(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 replace -exportFig('figs/id31_comp_simscape_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_comp_simscape_frf_ol_iff -#+caption: Comparison of the Simscape model and identified IFF plant -#+RESULTS: -[[file:figs/id31_comp_simscape_frf_ol_iff.png]] - -The effect of the payload mass on the diagonal elements are shown in Figure ref:fig:id31_effect_mass_frf_ol_iff. - -#+begin_src matlab :exports none :results none -%% Effect of the payload mass on the IFF plant -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', '$\tau_{m,i}/u_i$ - m0'); -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', '$\tau_{m,i}/u_i$ - m1'); -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', '$\tau_{m,i}/u_i$ - m2'); -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', '$\tau_{m,i}/u_i$ - m3'); -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', 2); - -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_effect_mass_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_effect_mass_frf_ol_iff -#+caption: Effect of the payload mass on the IFF plant -#+RESULTS: -[[file:figs/id31_effect_mass_frf_ol_iff.png]] - -** Encoder plant -#+begin_src matlab :exports none -%% ENC Plant (transfer function from u to taum) -G_enc_m0 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m0.(sprintf("uL%i", i_strut)).dL1 ; data_m0.(sprintf("uL%i", i_strut)).dL2 ; data_m0.(sprintf("uL%i", i_strut)).dL3 ; data_m0.(sprintf("uL%i", i_strut)).dL4 ; data_m0.(sprintf("uL%i", i_strut)).dL5 ; data_m0.(sprintf("uL%i", i_strut)).dL6]'; - - G_enc_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% ENC Plant (transfer function from u to taum) -G_enc_m1 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m1.(sprintf("uL%i", i_strut)).dL1 ; data_m1.(sprintf("uL%i", i_strut)).dL2 ; data_m1.(sprintf("uL%i", i_strut)).dL3 ; data_m1.(sprintf("uL%i", i_strut)).dL4 ; data_m1.(sprintf("uL%i", i_strut)).dL5 ; data_m1.(sprintf("uL%i", i_strut)).dL6]'; - - G_enc_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% ENC Plant (transfer function from u to taum) -G_enc_m2 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m2.(sprintf("uL%i", i_strut)).dL1 ; data_m2.(sprintf("uL%i", i_strut)).dL2 ; data_m2.(sprintf("uL%i", i_strut)).dL3 ; data_m2.(sprintf("uL%i", i_strut)).dL4 ; data_m2.(sprintf("uL%i", i_strut)).dL5 ; data_m2.(sprintf("uL%i", i_strut)).dL6]'; - - G_enc_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% ENC Plant (transfer function from u to taum) -G_enc_m3 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m3.(sprintf("uL%i", i_strut)).dL1 ; data_m3.(sprintf("uL%i", i_strut)).dL2 ; data_m3.(sprintf("uL%i", i_strut)).dL3 ; data_m3.(sprintf("uL%i", i_strut)).dL4 ; data_m3.(sprintf("uL%i", i_strut)).dL5 ; data_m3.(sprintf("uL%i", i_strut)).dL6]'; - - G_enc_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% Save Identified Plants -save('./matlab/mat/G_ol.mat', 'G_enc_m0', 'G_enc_m1', 'G_enc_m2', 'G_enc_m3', '-append'); -#+end_src - -The identified frequency response functions from general voltages $u_i$ to measured displacement of the struts by the encoders $d\mathcal{L}_i$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_enc. - -#+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_enc_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ... - 'DisplayName', '$d\mathcal{L}_i/u_j$ - Identified'); -for i = 1:5 - for j = i+1:6 - plot(f, abs(G_enc_m0(:, i, j)), 'color', [colors(1,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... - 'DisplayName', '$d\mathcal{L}_i/u_j$ - Simscape'); -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:)], ... - 'DisplayName', '$d\mathcal{L}_i/u_i$ - Identified'); -for i = 2:6 - plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:)], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ... - 'DisplayName', '$d\mathcal{L}_i/u_i$ - Simscape'); -for i = 2:6 - plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), '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_enc_m0(:,i, i)), 'color', [colors(1,:)]); -end -for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%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 replace -exportFig('figs/id31_comp_simscape_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_comp_simscape_frf_ol_enc -#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to encoders -#+RESULTS: -[[file:figs/id31_comp_simscape_frf_ol_enc.png]] - -#+begin_src matlab :exports none :results none -%% Effect of the payload mass on the transfer function from actuator voltage to encoder displacement -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ... - 'DisplayName', '$d\mathcal{L}_i/u_i$ - m0'); -for i = 2:6 - plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_enc_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ... - 'DisplayName', '$d\mathcal{L}_i/u_i$ - m1'); -for i = 2:6 - plot(f, abs(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_enc_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ... - 'DisplayName', '$d\mathcal{L}_i/u_i$ - m2'); -for i = 2:6 - plot(f, abs(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_enc_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ... - 'DisplayName', '$d\mathcal{L}_i/u_i$ - m3'); -for i = 2:6 - plot(f, abs(G_enc_m3(:,i, i)), '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, 2e-4]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5]); -end -for i = 1:6 - plot(f, 180/pi*angle(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5]); -end -for i = 1:6 - plot(f, 180/pi*angle(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5]); -end -for i = 1:6 - plot(f, 180/pi*angle(G_enc_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_effect_mass_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_effect_mass_frf_ol_enc -#+caption: Effect of the payload mass on the transfer function from actuator voltage to encoder displacement -#+RESULTS: -[[file:figs/id31_effect_mass_frf_ol_enc.png]] - -** HAC Undamped plant -#+begin_src matlab :exports none -%% INT Plant (transfer function from u to taum) -G_int_m0 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]'; - - G_int_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% INT Plant (transfer function from u to taum) -G_int_m1 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]'; - - G_int_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% INT Plant (transfer function from u to taum) -G_int_m2 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]'; - - G_int_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% INT Plant (transfer function from u to taum) -G_int_m3 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]'; - - G_int_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% Save Identified Plants -save('./matlab/mat/G_ol.mat', 'G_int_m0', 'G_int_m1', 'G_int_m2', 'G_int_m3', '-append'); -#+end_src - -The identified frequency response functions from actuator voltages $u_i$ to measured strut motion from the external metrology (i.e. the interferometers) are compare with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_int. - -#+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', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:6 - plot(f, abs(G_int_m0(:,i, i)), 'color', colors(i,:), ... - 'DisplayName', sprintf('$e\\mathcal{L}_%i/u_%i$', i, i)); -end -plot(f, abs(G_int_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... - 'DisplayName', '$-e\mathcal{L}_i/u_j$'); -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', 3); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(f, 180/pi*angle(G_int_m0(:,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_int_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_int_ol_plant_m0 -#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors -#+RESULTS: -[[file:figs/id31_int_ol_plant_m0.png]] - -#+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(:, 1, 2)), 'color', [colors(1,:), 0.2], ... - 'DisplayName', '$-e\mathcal{L}_i/u_j$ - Identified'); -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(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... - 'DisplayName', '$-e\mathcal{L}_i/u_j$ - Simscape'); -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:)], ... - 'DisplayName', '$-e\mathcal{L}_i/u_i$ - Identified'); -for i = 2:6 - plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:)], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ... - 'DisplayName', '$-e\mathcal{L}_i/u_i$ - Simscape'); -for i = 2:6 - plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), '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_m0(:,i, i)), 'color', [colors(1,:)]); -end -for i = 1:6 - plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(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 replace -exportFig('figs/id31_comp_simscape_frf_ol_int.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_comp_simscape_frf_ol_int -#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to estimated strut motion from interferometers -#+RESULTS: -[[file:figs/id31_comp_simscape_frf_ol_int.png]] - -#+begin_src matlab :exports none :results none -%% Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology -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', '$-e\mathcal{L}_i/u_i$ - m0'); -for i = 2:6 - plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_int_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ... - 'DisplayName', '$-e\mathcal{L}_i/u_i$ - m1'); -for i = 2:6 - plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_int_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ... - 'DisplayName', '$-e\mathcal{L}_i/u_i$ - m2'); -for i = 2:6 - plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(f, abs(G_int_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ... - 'DisplayName', '$-e\mathcal{L}_i/u_i$ - m3'); -for i = 2:6 - plot(f, abs(G_int_m3(:,i, i)), '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, 2e-4]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]); -end -for i = 1:6 - plot(f, 180/pi*angle(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5]); -end -for i = 1:6 - plot(f, 180/pi*angle(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5]); -end -for i = 1:6 - plot(f, 180/pi*angle(G_int_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_effect_mass_frf_ol_int.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_effect_mass_frf_ol_int -#+caption: Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology -#+RESULTS: -[[file:figs/id31_effect_mass_frf_ol_int.png]] - -** 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 -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]); -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_m1.pdf', 'width', 1000, 'height', 1000, 'transparent', false); -#+end_src - -#+RESULTS: -[[file:figs/sri2024_plant_m1.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 -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]); -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_m2.pdf', 'width', 1000, 'height', 1000, 'transparent', false); -#+end_src - -#+RESULTS: -[[file:figs/sri2024_plant_m2.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]] - -** Decoupling improvement thanks to better Rz alignment -*** Alignment procedure - -- Control based on encoders -- Slow moving in X and Y -- Compare with X and Y from interf - -#+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 -figure; -hold on; -plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Measurement') -plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off') -plot(1e6*[-10:10]*cos(2.7*pi/180), -1e6*[-10:10]*sin(2.7*pi/180), '--', 'color', colors(2,:), 'DisplayName', '$R_z = 2.7$ deg') -plot(1e6*[-10:10]*sin(2.7*pi/180), 1e6*[-10:10]*cos(2.7*pi/180), '--', 'color', colors(2,:), 'HandleVisibility', 'off') -hold off; -xlabel('Interf $D_x$ [$\mu$m]'); -ylabel('Interf $D_y$ [$\mu$m]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -axis equal -xlim([-10, 10]); ylim([-10, 10]); -xticks([-10:5:10]); yticks([-10:5:10]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_Rz_align_dx_dy_scans_before_calib.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_Rz_align_dx_dy_scans_before_calib -#+caption: description -#+RESULTS: -[[file:figs/id31_Rz_align_dx_dy_scans_before_calib.png]] - -#+begin_src matlab -figure; -hold on; -plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Initial') -plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off') -plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'After $R_z$ calib') -plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off') -hold off; -xlabel('Interf $D_x$ [$\mu$m]'); -ylabel('Interf $D_y$ [$\mu$m]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -axis equal -xlim([-10, 10]); ylim([-10, 10]); -xticks([-10:5:10]); yticks([-10:5:10]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_Rz_align_dx_dy_scans.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_Rz_align_dx_dy_scans -#+caption: description -#+RESULTS: -[[file:figs/id31_Rz_align_dx_dy_scans.png]] - - -*** m0 -#+begin_src matlab -data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir)); -data_align = load(sprintf('%s/dynamics/2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none -%% INT Plant (transfer function from u to taum) -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 -%% INT Plant (transfer function from u to taum) -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 :tangle no :exports results :results file replace -exportFig('figs/id31_coupling_decrease_rz_align.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_coupling_decrease_rz_align -#+caption: Decrease of the coupling with better Rz alignment -#+RESULTS: -[[file:figs/id31_coupling_decrease_rz_align.png]] - - -*** m3 -#+begin_src matlab -data = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir)); -data_align = load(sprintf('%s/dynamics/2023-08-21_15-52_damp_plant_m3_new_Rz.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none -%% INT Plant (transfer function from u to taum) -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 -%% INT Plant (transfer function from u to taum) -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 :tangle no :exports results :results file replace -exportFig('figs/id31_coupling_decrease_rz_align_m3.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_coupling_decrease_rz_align_m3 -#+caption: Decrease of the coupling with better Rz alignment -#+RESULTS: -[[file:figs/id31_coupling_decrease_rz_align_m3.png]] - -** Conclusion - -- Good match between the model and experiment - -* DONE Integral Force Feedback -<> -** Introduction :ignore: - -** Matlab Init :noexport:ignore: -#+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 - -** 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]] - - -* High Authority Control -<> -** Introduction :ignore: - -** Matlab Init :noexport:ignore: -#+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 - -** Identify Spurious modes -#+begin_src matlab -%% Load identification data -data = load(sprintf('%s/dynamics/2023-08-10_18-32_identify_spurious_modes.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 -[G1, f] = tfestimate(data.id_plant, data.d1, win, Noverlap, Nfft, 1/Ts); -[G2, ~] = tfestimate(data.id_plant, data.d2, win, Noverlap, Nfft, 1/Ts); -[G3, ~] = tfestimate(data.id_plant, data.d3, win, Noverlap, Nfft, 1/Ts); -[G4, ~] = tfestimate(data.id_plant, data.d4, win, Noverlap, Nfft, 1/Ts); -[G5, ~] = tfestimate(data.id_plant, data.d5, win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(G1), 'DisplayName', '1 - top'); -plot(f, abs(G2), 'DisplayName', '2 - bot'); -plot(f, abs(G3), 'DisplayName', '3 - top'); -plot(f, abs(G4), 'DisplayName', '4 - bot'); -plot(f, abs(G5), 'DisplayName', '5 - vertical'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); -xlim([500, 800]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -** HAC Plants -*** Introduction :ignore: - -*** 6x6 Plant -#+begin_src matlab -%% Load identification data -data = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_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 -%% HAC Plant -G_hac = 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_hac(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), 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_hac(:, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:6 - plot(f, abs(G_hac(:,i, i)), 'color', colors(i,:), ... - 'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i)); -end -plot(f, abs(G_hac(:, 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 [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(f, 180/pi*angle(G_hac(:,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([-180, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -Compare with Model: -#+begin_src matlab -load('Gm.mat'); -#+end_src - -#+begin_src matlab :exports none :results none -%% 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_hac(:,1,2)), 'color', [colors(3,:), 0.5], ... - 'DisplayName', 'Identified, Off-Diagonal'); -for i = 1:5 - for j = i+1:6 - plot(f, abs(G_hac(:, i, j)), 'color', [colors(3,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ... - 'DisplayName', 'Model, Off-Diagonal'); -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -plot(f, abs(G_hac(:,1,1)), 'color', [colors(1,:), 0.5], ... - 'DisplayName', 'Identified, Diagonal'); -for i = 2:6 - plot(f, abs(G_hac(:,i, i)), 'color', [colors(1,:), 0.5], ... - 'HandleVisibility', 'off'); -end -plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ... - 'DisplayName', 'Model, Diagonal'); -for i = 2:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), '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',[]); -ylim([1e-8, 1e-3]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', [colors(1,:), 0.5]) -end -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(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([-180, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_Ghac_6x6_plant_comp_model.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_Ghac_6x6_plant_comp_model -#+caption: 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology -#+RESULTS: -[[file:figs/id31_Ghac_6x6_plant_comp_model.png]] - - -*** Effect of Mass -#+begin_src matlab -%% Load identification data -data_m0 = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir)); -data_m1 = load(sprintf('%s/dynamics/2023-08-10_16-01_damp_plant_m1_Wz0.mat', mat_dir)); -data_m2 = load(sprintf('%s/dynamics/2023-08-10_17-28_damp_plant_m2_Wz0.mat', mat_dir)); -data_m3 = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none -% Sampling Time [s] -Ts = 1e-4; - -% Hannning Windows -Nfft = floor(2.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_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Plant (transfer function from u to taum) -G_hac_m0 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]'; - - G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% HAC Plant (transfer function from u to taum) -G_hac_m1 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]'; - - G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% HAC Plant (transfer function from u to taum) -G_hac_m2 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]'; - - G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% HAC Plant (transfer function from u to taum) -G_hac_m3 = zeros(length(f), 6, 6); - -for i_strut = 1:6 - eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]'; - - G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% Save Identified Plants -save('./matlab/mat/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 -%% Obtained transfer functions -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$'); -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 = 1$'); -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 = 2$'); -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 = 3$'); -for i = 2:6 - plot(f, abs(G_hac_m3(:,i, i)), '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', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]); -end -for i =1:6 - plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]); -end -for i =1:6 - plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]); -end -for i =1:6 - plot(f, 180/pi*angle(G_hac_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([-180, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_Ghac_effect_mass.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_Ghac_effect_mass -#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor -#+RESULTS: -[[file:figs/id31_Ghac_effect_mass.png]] - -*** Compare with the model -#+begin_src matlab -load('G_hac.mat') -load('Gm.mat') -#+end_src - -#+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$'); -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 = 1$'); -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 = 2$'); -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 = 3$'); -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$ - Model'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ... - 'DisplayName', '$m = 1$ - Model'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ... - 'DisplayName', '$m = 2$ - Model'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ... - 'DisplayName', '$m = 3$ - Model'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); -ylim([1e-8, 1e-3]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_hac_m0(:,1,1)), 'color', [colors(1,:), 0.5]); -for i = 2:6 - plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]); -end -plot(f, 180/pi*angle(G_hac_m1(:,1,1)), 'color', [colors(2,:), 0.5]); -for i = 2:6 - plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]); -end -plot(f, 180/pi*angle(G_hac_m2(:,1,1)), 'color', [colors(3,:), 0.5]); -for i = 2:6 - plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]); -end -plot(f, 180/pi*angle(G_hac_m3(:,1,1)), 'color', [colors(4,:), 0.5]); -for i = 2:6 - plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]); -end -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:)); -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:)); -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:)); -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3('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([-180, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_Ghac_plant_comp_model.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_Ghac_plant_comp_model -#+caption: Comparison of the identified HAC plant and the HAC plant extracted from the simscape model -#+RESULTS: -[[file:figs/id31_Ghac_plant_comp_model.png]] - - -*** Comparison with Undamped plant - -#+begin_src matlab -load('G_ol.mat'); -load('G_hac.mat'); -#+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,:), 0.5], ... - 'DisplayName', '$m = 0$ - OL'); -for i = 2:6 - plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ... - 'HandleVisibility', 'off') -end -plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(2,:), 0.5], ... - 'DisplayName', '$m = 0$ - Damped'); -for i = 2:6 - plot(f, abs(G_hac_m0(:,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',[]); -ylim([1e-8, 1e-3]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(f, 180/pi*angle(-G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]); -end -for i =1:6 - plot(f, 180/pi*angle(G_hac_m0(:,i, i)), '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([-180, 180]) - -linkaxes([ax1,ax2],'x'); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_comp_undamped_damped_plant_m0.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_comp_undamped_damped_plant_m0 -#+caption: description -#+RESULTS: -[[file:figs/id31_comp_undamped_damped_plant_m0.png]] - -** Robust HAC -#+begin_src matlab -load('G_hac.mat') -load('Gm.mat') -#+end_src - -*** Controller design -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*5; - -%% Double Integrator -% H_int = (wc^2)/(s + 1e-1*2*pi)^2; -H_int = wc/s; - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain - -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/30); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = 1./abs(G_hac_m0(i_f, 1, 1)); - -%% Decentralized HAC -Khac = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Integrator - eye(6); % 6x6 Diagonal -#+end_src - -Loop gain -#+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_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_0$'); -plot(f, abs(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_1$'); -plot(f, abs(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_2$'); -plot(f, abs(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:), 'DisplayName', '$m_3$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3); -ylim([1e-5, 1e2]); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:)); -plot(f, 180/pi*angle(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:)); -plot(f, 180/pi*angle(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:)); -plot(f, 180/pi*angle(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, '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([-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_robust_loop_gain.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_hac_robust_loop_gain -#+caption: description -#+RESULTS: -[[file:figs/id31_hac_robust_loop_gain.png]] - - -*** Verify Stability -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(4, 6, length(f)); - -% Loop gain -Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); -end - -Lmimo = pagemtimes(permute(G_hac_m1, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); -end - -Lmimo = pagemtimes(permute(G_hac_m2, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); -end - -Lmimo = pagemtimes(permute(G_hac_m3, [2,3,1]),squeeze(freqresp(Khac, 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('$m_%i$', 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'); -axis square -xlim([-1.5, 0.5]); ylim([-1, 1]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_hac_robust_nyquist.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_hac_robust_nyquist -#+caption: description -#+RESULTS: -[[file:figs/id31_hac_robust_nyquist.png]] - -*** Estimated performances - -*** Save Controller -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(1,1)); - -Kz = c2d(Khac(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_hac.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab -save('./matlab/mat/K_hac.mat', 'Khac') -#+end_src - - -** High Performance HAC -*** Introduction :ignore: -The goal is to make a controller specific for one mass in order to have high bandwidth. - -*** Mass 0 -**** Load Plant -#+begin_src matlab -load('G_hac.mat') -load('Gm.mat') -#+end_src - -**** Plant -#+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_hac_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:6 - plot(f, abs(G_hac_m0(:,i, i)), 'color', colors(i,:), ... - 'DisplayName', sprintf('$V_{s,%i}/u_%i$', i, i)); -end -plot(f, abs(G_hac_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... - 'DisplayName', '$V_{s,i}/u_j$'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--', ... - 'DisplayName', '$V_{s,i}/u_i$ - Model'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... - 'DisplayName', '$V_{s,i}/u_j$ - Model'); -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... - 'HandleVisibility', 'off'); - end -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(f, 180/pi*angle(G_hac_m0(:,i, i))); -end -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--'); -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 - -**** Controller design -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*50; - -%% Double Integrator -H_int = 1/(s + 0.1*2*pi) * ... - 1/(0.1*2*pi + s); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); -% H_int = wc/s; - -%% Lead to increase phase margin -a = 3; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); -a = 3; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead/sqrt(a)*(1 + s/(2.5*wc/sqrt(a)))/(1 + s/(2.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/500); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = 1./abs(G_hac_m0(i_f, 1, 1)); - -%% Decentralized HAC -Khac = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead * ... % Integrator - eye(6); % 6x6 Diagonal - -Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; -Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -#+end_src - -Loop gain -#+begin_src matlab :exports none :results none -%% Loop gain for the High Authority Control -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]); -end -for i = 1:5 - for j = i+1:6 - plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 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_high_perf_hac_m0_loop_gain.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_high_perf_hac_m0_loop_gain -#+caption: Loop gain for the High Authority Control -#+RESULTS: -[[file:figs/id31_high_perf_hac_m0_loop_gain.png]] - -**** Verify Stability -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(6, length(f)); - -% Loop gain -Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(:, 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; -plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.'); -plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.'); -for i = 1:6 - plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.'); - plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.'); -end -plot(-1, 0, 'kx', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_high_perf_hac_m0_nyquist.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_high_perf_hac_m0_nyquist -#+caption: Nyquist plot for the High Authority Control -#+RESULTS: -[[file:figs/id31_high_perf_hac_m0_nyquist.png]] - -**** Estimated performances -Loop gain with model -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]); -end -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -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))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 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 -Gm_cl_m0 = feedback(Gm_hac_m0, 0.8*Khac, 'name', -1); -#+end_src - -#+begin_src matlab -isstable(Gm_cl_m0) -#+end_src - -**** Save Controller -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(1,1)); - -Kz = c2d(Khac(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_hac_m0.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -**** Experimental Validation - -#+begin_src matlab -data_1rpm = load(sprintf('%s/scans/2023-08-18_10-43_m0_1rpm.mat', mat_dir)); -data_30rpm = load(sprintf('%s/scans/2023-08-18_10-45_m0_30rpm.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([rms(1e9*data_1rpm.Dy_int), rms(1e9*data_1rpm.Dz_int), rms(1e6*data_1rpm.Ry_int); rms(1e9*data_30rpm.Dy_int), rms(1e9*data_30rpm.Dz_int), rms(1e6*data_30rpm.Ry_int)], {'1rpm', '30rpm'}, {'Dy [nm]', 'Dz [nm]', 'Ry [urad]'}, ' %.1f '); -#+end_src - -#+RESULTS: -| | Dy [nm] | Dz [nm] | Ry [urad] | -|-------+---------+---------+-----------| -| 1rpm | 55.3 | 5.9 | 0.1 | -| 30rpm | 85.2 | 12.5 | 0.3 | - -**** Closed-Loop identification -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-18_11-03_m0_perf_hac.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 -[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts); -[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - - -*** Mass 1 -**** Load Plant -#+begin_src matlab -load('G_hac.mat') -load('Gm.mat') -#+end_src - -**** Plant -#+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_hac_m1(:, i, j)), 'color', [0, 0, 0, 0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:6 - plot(f, abs(G_hac_m1(:,i, i)), 'color', colors(i,:), ... - 'DisplayName', sprintf('$V_{s,%i}/u_%i$', i, i)); -end -plot(f, abs(G_hac_m1(:, 1, 2)), 'color', [0, 0, 0, 0.2], ... - 'DisplayName', '$V_{s,i}/u_j$'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--', ... - 'DisplayName', '$V_{s,i}/u_i$ - Model'); -plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... - 'DisplayName', '$V_{s,i}/u_j$ - Model'); -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ... - 'HandleVisibility', 'off'); - end -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(f, 180/pi*angle(G_hac_m1(:,i, i))); -end -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--'); -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 - -**** Plant Inverse -#+begin_src matlab -Gm_hac_red_m1 = flipRphZeros(-balred(Gm_hac_m1('eL1', 'u1'), 6, ... - balredOptions('StateProjection', 'MatchDC', ... - 'FreqIntervals', [0, 80]))); -#+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(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gm_hac_red_m1, freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); -ylim([1e-8, 1e-3]); - -ax2 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz')))); -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_red_m1, freqs, 'Hz')))); -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 -%% Plant Inverse -Gm_hac_inv_m1 = inv(Gm_hac_red_m1); -#+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(freqs, abs(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gm_hac_inv_m1, freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]); - -ax2 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz')))); -plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_inv_m1, freqs, 'Hz')))); -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 - -**** Controller design -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*30; - -%% Double Integrator -H_int = 1/(s + 0.1*2*pi) * ... - (50*2*pi + s)/(0.01*2*pi + s); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); -% H_int = wc/s; - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/200); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = 1./abs(G_hac_m0(i_f, 1, 1)); - -%% Decentralized HAC -Khac = 0.8*H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead * ... % Integrator - eye(6); % 6x6 Diagonal - -Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; -Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -#+end_src - -Loop gain -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]); -end -for i = 1:5 - for j = i+1:6 - plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 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 - -**** Verify Stability -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(6, length(f)); - -% Loop gain -Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(:, 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; -plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.'); -plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.'); -for i = 1:6 - plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.'); - plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.'); -end -plot(-1, 0, 'kx', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -**** Estimated performances -Loop gain with model -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]); -end -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -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))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 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 -Gm_cl_m0 = feedback(Gm_hac_m0, Khac, 'name', +1); -#+end_src - -#+begin_src matlab -isstable(Gm_hac_m0) -#+end_src - -**** Save Controller -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(1,1)); - -Kz = c2d(Khac(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_hac_m0.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -** DONE Tomography - Performances -*** First scan with closed-loop at middle - -#+begin_src matlab -data = load(sprintf('%s/scans/2023-08-17_15-22_tomography_30rpm_m0_robust.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none :results none -writerObj = VideoWriter('test2.avi'); %// initialize the VideoWriter object -open(writerObj) ; - -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -di = 500; -hold on; -for i = 1:floor(length(data.Dx_int)/di)-1 - if data.hac_status(di*(i+1)-1) == 0 - % Only open loop - plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:)) - elseif data.hac_status(di*i) == 1 - % Only closed loop - plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:)) - else - % Both open and closed loop - Dx_int = data.Dx_int(di*i:di*(i+1)-1); - Dy_int = data.Dy_int(di*i:di*(i+1)-1); - plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:)) - plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:)) - end - axis square - xlim([-3, 3]) - ylim([-3, 3]) - xlabel("X motion [$\mu m$]"); - ylabel("Y motion [$\mu m$]"); - F = getframe; %// Capture the frame - writeVideo(writerObj,F) %// add the frame to the movie -end -close(writerObj); -#+end_src - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ... - 'DisplayName', 'OL') -plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ... - 'DisplayName', 'CL') -% Get first time where closed-loop ON -[~, i] = find(data.hac_status == 1); -plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ... - 'DisplayName', 'CL, stabilized') -hold off; -axis equal -xlim([-3, 3]) -ylim([-3, 3]) -xticks([-3:1:3]) -yticks([-3:1:3]) -xlabel("X motion [$\mu m$]"); -ylabel("Y motion [$\mu m$]"); - -ax2 = nexttile; -hold on; -plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ... - 'DisplayName', 'OL') -plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ... - 'DisplayName', 'CL') -% Get first time where closed-loop ON -[~, i] = find(data.hac_status == 1); -plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ... - 'DisplayName', 'CL, stabilized') -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -hold off; -axis equal -xlabel("X motion [$\mu m$]"); -ylabel("Y motion [$\mu m$]"); -xlim([-0.3, 0.3]) -ylim([-0.3, 0.3]) -xticks([-0.3:0.1:0.3]) -yticks([-0.3:0.1:0.3]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_tomography_ol_cl_robust_hac_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_tomography_ol_cl_robust_hac_m0 -#+caption: description -#+RESULTS: -[[file:figs/id31_tomography_ol_cl_robust_hac_m0.png]] - - -*** Slow Rotation - 6RPM -#+begin_src matlab -%% Load measured noise -data_ol = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat', mat_dir)); -data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-16_m0_lac_on_1rpm.mat', mat_dir)); -data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-14_m0_hac_on_1rpm.mat', mat_dir)); -#+end_src - -#+begin_src matlab -%% Coordinate transform -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; - -a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5]; -data_ol.Dx_int = a(1,:); -data_ol.Dy_int = a(2,:); -data_ol.Dz_int = a(3,:); -data_ol.Rx_int = a(4,:); -data_ol.Ry_int = a(5,:); - -a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5]; -data_lac.Dx_int = a(1,:); -data_lac.Dy_int = a(2,:); -data_lac.Dz_int = a(3,:); -data_lac.Rx_int = a(4,:); -data_lac.Ry_int = a(5,:); - -a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5]; -data_hac.Dx_int = a(1,:); -data_hac.Dy_int = a(2,:); -data_hac.Dz_int = a(3,:); -data_hac.Rx_int = a(4,:); -data_hac.Ry_int = a(5,:); -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(30.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Dy, ~ ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Dz, ~ ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Rx, ~ ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Ry, ~ ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC'); -plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]'); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -xlim([1, 5e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion -figure; -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', '$D_y$ - OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', '$D_y$ - LAC'); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', '$D_y$ - HAC-LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('CAS [$m$]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -% xlim([0.1, 5e3]); ylim([1e-10, 1e-7]); -#+end_src - -*** Rapid Rotation - 30RPM -#+begin_src matlab -%% Load measured noise -data_ol = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir)); -data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-36_m0_lac_on_30rpm.mat', mat_dir)); -data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-34_m0_hac_on_30rpm.mat', mat_dir)); -#+end_src - -#+begin_src matlab -%% Coordinate transform -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; - -a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5]; -data_ol.Dx_int = a(1,:); -data_ol.Dy_int = a(2,:); -data_ol.Dz_int = a(3,:); -data_ol.Rx_int = a(4,:); -data_ol.Ry_int = a(5,:); - -a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5]; -data_lac.Dx_int = a(1,:); -data_lac.Dy_int = a(2,:); -data_lac.Dz_int = a(3,:); -data_lac.Rx_int = a(4,:); -data_lac.Ry_int = a(5,:); - -a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5]; -data_hac.Dx_int = a(1,:); -data_hac.Dy_int = a(2,:); -data_hac.Dz_int = a(3,:); -data_hac.Rx_int = a(4,:); -data_hac.Ry_int = a(5,:); -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(20.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Dy, ~ ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Dz, ~ ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Rx, ~ ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Ry, ~ ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC'); -plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]'); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -xlim([1, 5e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion -figure; -tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), ... - 'DisplayName', sprintf('OL: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_ol.Dy_int, 0)))); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), ... - 'DisplayName', sprintf('LAC: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_lac.Dy_int, 0)))); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), ... - 'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dy_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xticks([1e0, 1e1, 1e2]); -title('$D_y$') - -ax2 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), ... - 'DisplayName', sprintf('OL: $%.0f nm$ RMS', 1e9*rms(detrend(data_ol.Dz_int, 0)))); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), ... - 'DisplayName', sprintf('LAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_lac.Dz_int, 0)))); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), ... - 'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dz_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xticks([1e0, 1e1, 1e2]); -title('$D_z$') - -ax3 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), ... - 'DisplayName', sprintf('OL: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_ol.Ry_int, 0)))); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), ... - 'DisplayName', sprintf('LAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_lac.Ry_int, 0)))); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), ... - 'DisplayName', sprintf('HAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_hac.Ry_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xticks([1e0, 1e1, 1e2]); -title('$R_y$') - -linkaxes([ax1,ax2,ax3],'xy'); -xlim([0.1, 5e2]); ylim([1e-10, 3e-5]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_cas_m0_30rpm_ol_lac_hac.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_cas_m0_30rpm_ol_lac_hac -#+caption: Cumulative Amplitude Spectrum of the errors in $D_y$, $D_z$ and $R_y$ during a tomography scan at 30RPM. Three control configuration are compared: Open-Loop, Low Authority Control, and High Authority Control -#+RESULTS: -[[file:figs/id31_cas_m0_30rpm_ol_lac_hac.png]] - - -* DONE Dynamic Error Budgeting -<> -** Introduction :ignore: - -In this section, the noise budget is performed. -The vibrations of the sample is measured in different conditions using the external metrology. - - -** Matlab Init :noexport:ignore: -#+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 - -** Open-Loop Noise Budget -First, the noise is measured while no motion is performed. - -#+begin_src matlab -%% Load measured noise -data_m0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir)); -#+end_src - -Noise budget in the cartesian frame -#+begin_src matlab :exports none -%% Coordinate transform -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; - -a = J_int_to_X*[data_m0.d1; data_m0.d2; data_m0.d3; data_m0.d4; data_m0.d5]; -data_m0.t = Ts*[0:length(data_m0.d1)-1]; -data_m0.Dx_int = a(1,:); -data_m0.Dy_int = a(2,:); -data_m0.Dz_int = a(3,:); -data_m0.Rx_int = a(4,:); -data_m0.Ry_int = a(5,:); -#+end_src - -Data in the time domain -#+begin_src matlab :exports none :results none -%% Measured vibration with the interferometers -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -% plot(data_m0.t, 1e9*data_m0.Dx_int, '-', 'DisplayName', '$D_x$'); -plot(data_m0.t, 1e9*data_m0.Dy_int, '-', 'DisplayName', '$D_y$'); -plot(data_m0.t, 1e9*data_m0.Dz_int, '-', 'DisplayName', '$D_z$'); -% plot(data_m0.t, data_m0.Rx_int, 'DisplayName', '$R_x$'); -plot(data_m0.t, 1e9*data_m0.Ry_int, '-', 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Time [s]'); ylabel('Amplitude [nm]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -xlim([50, 54]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_noise_budget_interf_time_domain_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_noise_budget_interf_time_domain_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_noise_budget_interf_time_domain_m0.png]] - -In the frequency domain -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(2.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); -#+end_src - -#+begin_src matlab :exports none -% And we get the frequency vector -[pxx_Dx, f] = pwelch(detrend(data_m0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[pxx_Dy, ~] = pwelch(detrend(data_m0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[pxx_Dz, ~] = pwelch(detrend(data_m0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[pxx_Rx, ~] = pwelch(detrend(data_m0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[pxx_Ry, ~] = pwelch(detrend(data_m0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -% plot(f, sqrt(pxx_Dx), 'DisplayName', '$D_x$'); -plot(f, sqrt(pxx_Dy), 'DisplayName', '$D_y$'); -plot(f, sqrt(pxx_Dz), 'DisplayName', '$D_z$'); -% plot(f, sqrt(pxx_Rx), 'DisplayName', '$R_x$'); -plot(f, sqrt(pxx_Ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xlim([1, 5e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_noise_budget_interf_freq_domain_m0.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_noise_budget_interf_freq_domain_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_noise_budget_interf_freq_domain_m0.png]] - -#+begin_src matlab :exports none :results none -%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion -figure; -hold on; -% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dx)))), 'DisplayName', '$D_x$'); -plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dy)))), 'DisplayName', sprintf('$D_y$ - %.0f nm', rms(1e9*detrend(data_m0.Dy_int, 0)))); -plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dz)))), 'DisplayName', sprintf('$D_z$ - %.0f nm', rms(1e9*detrend(data_m0.Dz_int, 0)))); -% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Rx)))), 'DisplayName', '$R_x$'); -plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Ry)))), 'DisplayName', sprintf('$R_y$ - %.0f nrad', rms(1e9*detrend(data_m0.Ry_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('CAS [$m$]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -xlim([1, 5e3]); ylim([1e-10, 1e-7]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_noise_budget_open_loop_cas_m0.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_noise_budget_open_loop_cas_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_noise_budget_open_loop_cas_m0.png]] - -** Effect of rotation -#+begin_src matlab -data_Wz0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir)); -data_Wz1 = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat', mat_dir)); -data_Wz2 = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none -%% Coordinate transform -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; - -a = J_int_to_X*[data_Wz0.d1; data_Wz0.d2; data_Wz0.d3; data_Wz0.d4; data_Wz0.d5]; -data_Wz0.Dx_int = a(1,:); -data_Wz0.Dy_int = a(2,:); -data_Wz0.Dz_int = a(3,:); -data_Wz0.Rx_int = a(4,:); -data_Wz0.Ry_int = a(5,:); - -a = J_int_to_X*[data_Wz1.d1; data_Wz1.d2; data_Wz1.d3; data_Wz1.d4; data_Wz1.d5]; -data_Wz1.Dx_int = a(1,:); -data_Wz1.Dy_int = a(2,:); -data_Wz1.Dz_int = a(3,:); -data_Wz1.Rx_int = a(4,:); -data_Wz1.Ry_int = a(5,:); - -a = J_int_to_X*[data_Wz2.d1; data_Wz2.d2; data_Wz2.d3; data_Wz2.d4; data_Wz2.d5]; -data_Wz2.Dx_int = a(1,:); -data_Wz2.Dy_int = a(2,:); -data_Wz2.Dz_int = a(3,:); -data_Wz2.Rx_int = a(4,:); -data_Wz2.Ry_int = a(5,:); -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(20.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data_Wz0.pxx_Dx, data_Wz0.f] = pwelch(detrend(data_Wz0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz0.pxx_Dy, ~ ] = pwelch(detrend(data_Wz0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz0.pxx_Dz, ~ ] = pwelch(detrend(data_Wz0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz0.pxx_Rx, ~ ] = pwelch(detrend(data_Wz0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz0.pxx_Ry, ~ ] = pwelch(detrend(data_Wz0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_Wz1.pxx_Dx, data_Wz1.f] = pwelch(detrend(data_Wz1.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz1.pxx_Dy, ~ ] = pwelch(detrend(data_Wz1.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz1.pxx_Dz, ~ ] = pwelch(detrend(data_Wz1.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz1.pxx_Rx, ~ ] = pwelch(detrend(data_Wz1.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz1.pxx_Ry, ~ ] = pwelch(detrend(data_Wz1.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_Wz2.pxx_Dx, data_Wz2.f] = pwelch(detrend(data_Wz2.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz2.pxx_Dy, ~ ] = pwelch(detrend(data_Wz2.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz2.pxx_Dz, ~ ] = pwelch(detrend(data_Wz2.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz2.pxx_Rx, ~ ] = pwelch(detrend(data_Wz2.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_Wz2.pxx_Ry, ~ ] = pwelch(detrend(data_Wz2.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(data_Wz0.f, sqrt(data_Wz0.pxx_Dy), 'DisplayName', '$D_y$ - 0rpm'); -plot(data_Wz1.f, sqrt(data_Wz1.pxx_Dy), 'DisplayName', '$D_y$ - 1rpm'); -plot(data_Wz2.f, sqrt(data_Wz2.pxx_Dy), 'DisplayName', '$D_y$ - 30rpm'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]'); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -xlim([0.1, 5e3]); -#+end_src - -Rotation induces lots of vibrations, especially at high velocity. - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dy)))), ... - 'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dy_int, 0)))); -plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dy)))), ... - 'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dy_int, 0)))); -plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dy)))), ... - 'DisplayName', sprintf('30rpm: $%.1f \\mu m$', 1e6*rms(detrend(data_Wz2.Dy_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -xticks([1e0, 1e1, 1e2]); -title('$D_y$') - -ax2 = nexttile(); -hold on; -plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dz)))), ... - 'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dz_int, 0)))); -plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dz)))), ... - 'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dz_int, 0)))); -plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dz)))), ... - 'DisplayName', sprintf('30rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz2.Dz_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -xticks([1e0, 1e1, 1e2]); -title('$D_z$') - -ax3 = nexttile(); -hold on; -plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Ry)))), ... - 'DisplayName', sprintf('0rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz0.Ry_int, 0)))); -plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Ry)))), ... - 'DisplayName', sprintf('6rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz1.Ry_int, 0)))); -plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Ry)))), ... - 'DisplayName', sprintf('30rpm: $%.0f \\mu$rad', 1e6*rms(detrend(data_Wz2.Ry_int, 0)))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -xticks([1e0, 1e1, 1e2]); -title('$R_y$') - -linkaxes([ax1,ax2,ax3],'xy'); -xlim([0.1, 5e2]); ylim([1e-10, 3e-5]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_noise_budget_effect_rotation.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_noise_budget_effect_rotation -#+caption: Cumulative Amplitude Spectrum for the three important directions ($D_y$, $D_z$ and $R_y$). Three rotating velocities are shown. Integrated RMS values are shown in the legend. -#+RESULTS: -[[file:figs/id31_noise_budget_effect_rotation.png]] - -** Effect of LAC -#+begin_src matlab -%% Load measured noise -data_ol = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir)); -data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat', mat_dir)); -#+end_src - -#+begin_src matlab :exports none -%% Coordinate transform -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; - -a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5]; -data_ol.Dx_int = a(1,:); -data_ol.Dy_int = a(2,:); -data_ol.Dz_int = a(3,:); -data_ol.Rx_int = a(4,:); -data_ol.Ry_int = a(5,:); - -a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5]; -data_lac.Dx_int = a(1,:); -data_lac.Dy_int = a(2,:); -data_lac.Dz_int = a(3,:); -data_lac.Rx_int = a(4,:); -data_lac.Ry_int = a(5,:); -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(2.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); -#+end_src - -Effect of LAC (Figure ref:fig:id31_noise_budget_effect_lac_m0): -- reduce amplitude around 80Hz -- Inject some noise between 200 and 700Hz? - -#+begin_src matlab :exports none :results none -%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor -figure; -tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [$m/\sqrt{Hz}$]'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -title('$D_y$') - -ax2 = nexttile(); -hold on; -plot(data_ol.f , sqrt(data_ol.pxx_Dz ), 'DisplayName', '$D_z$ - OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', '$D_z$ - LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -title('$D_z$') - -ax3 = nexttile(); -hold on; -plot(data_ol.f , sqrt(data_ol.pxx_Ry ), 'DisplayName', '$R_y$ - OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', '$R_y$ - LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -title('$R_y$') - -linkaxes([ax1,ax2,ax3],'xy'); -xlim([1, 5e2]); ylim([1e-11, 2e-8]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_noise_budget_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_noise_budget_effect_lac_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_noise_budget_effect_lac_m0.png]] - -#+begin_src matlab :exports none :results none -%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion -figure; -tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('CAS [$m$]'); -title('$D_y$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title('$D_z$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -ax3 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title('$R_y$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -linkaxes([ax1,ax2,ax3],'xy'); -xlim([1, 1e3]); ylim([1e-10, 1e-7]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_cas_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_cas_effect_lac_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_cas_effect_lac_m0.png]] - -** Effect of HAC -#+begin_src matlab -%% Load measured noise -data_ol = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir)); -data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat' , mat_dir)); -data_hac = load(sprintf('%s/freq_analysis/2023-08-11_16-49_m0_hac_on.mat' , mat_dir)); -#+end_src - -#+begin_src matlab :exports none -%% Coordinate transform -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; - -a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5]; -data_ol.Dx_int = a(1,:); -data_ol.Dy_int = a(2,:); -data_ol.Dz_int = a(3,:); -data_ol.Rx_int = a(4,:); -data_ol.Ry_int = a(5,:); - -a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5]; -data_lac.Dx_int = a(1,:); -data_lac.Dy_int = a(2,:); -data_lac.Dz_int = a(3,:); -data_lac.Rx_int = a(4,:); -data_lac.Ry_int = a(5,:); - -a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5]; -data_hac.Dx_int = a(1,:); -data_hac.Dy_int = a(2,:); -data_hac.Dz_int = a(3,:); -data_hac.Rx_int = a(4,:); -data_hac.Ry_int = a(5,:); -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(2.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); - -[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Dy, ~ ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Dz, ~ ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Rx, ~ ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts); -[data_hac.pxx_Ry, ~ ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts); -#+end_src - -Bandwidth is approximately 10Hz. - -#+begin_src matlab :exports none :results none -%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion -figure; -tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f, sqrt(data_ol.pxx_Dy), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', 'LAC'); -plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', 'HAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]'); -title('$D_y$'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile(); -hold on; -plot(data_ol.f, sqrt(data_ol.pxx_Dz), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', 'LAC'); -plot(data_hac.f, sqrt(data_hac.pxx_Dz), 'DisplayName', 'HAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title('$D_z$'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax3 = nexttile(); -hold on; -plot(data_ol.f, sqrt(data_ol.pxx_Ry), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', 'LAC'); -plot(data_hac.f, sqrt(data_hac.pxx_Ry), 'DisplayName', 'HAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title('$R_y$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -linkaxes([ax1,ax2,ax3],'xy'); -xlim([1, 1e3]); ylim([1e-12, 1e-7]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_noise_budget_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_noise_budget_effect_hac_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_noise_budget_effect_hac_m0.png]] - - - -#+begin_src matlab :exports none :results none -%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion -figure; -tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC'); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', 'HAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('CAS [$m$]'); -title('$D_y$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC'); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), 'DisplayName', 'HAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title('$D_z$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -ax3 = nexttile(); -hold on; -plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL'); -plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC'); -plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), 'DisplayName', 'HAC'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title('$R_y$'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); - -linkaxes([ax1,ax2,ax3],'xy'); -xlim([1, 1e3]); ylim([1e-10, 1e-6]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_cas_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_cas_effect_hac_m0 -#+caption: Measured vibration with the interferometers -#+RESULTS: -[[file:figs/id31_cas_effect_hac_m0.png]] - -** TODO Noise coming from force sensor - -* 6DoF Control in Cartesian plane (rotating with the nano-hexapod) -<> -** Matlab Init :noexport:ignore: -#+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 - -** Introduction :ignore: - -** 5x5 plant in Cartesian plane -#+begin_src matlab :exports none -%% Load model -load('Gm.mat') -#+end_src - -#+begin_src matlab -%% Jacobian for 3x3 plant -n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -Jt_inv = pinv(n_hexapod.geometry.J'); -Jt_inv = Jt_inv(:,[1,2,3,4,5]); - -J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -Compute identified plant in the Cartesian plane: -#+begin_src matlab -%% Load Data -% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir)); -%% New data after calibrating the Rz-offset -data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.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_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Cartesian Plant -G_hac_m0 = zeros(length(f), 5, 6); - -for i_strut = 1:6 - Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]'; - - G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts); -end - -G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv)), [3,1,2]); -#+end_src - -Compute plant model in the Cartesian plane: -#+begin_src matlab -Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv; -Gm_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My'}; -Gm_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry'}; -#+end_src - -#+begin_src matlab :exports none :results none -%% 5x5 plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,1,1)), 'DisplayName', '$D_x$'); -plot(f, abs(G_cart(:,2,2)), 'DisplayName', '$D_y$'); -plot(f, abs(G_cart(:,3,3)), 'DisplayName', '$D_z$'); -plot(f, abs(G_cart(:,4,4)), 'DisplayName', '$R_x$'); -plot(f, abs(G_cart(:,5,5)), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); -% ylim([1e-4, 1e1]); - -ax2 = nexttile(); -hold on; -plot(f, 180/pi*angle(G_cart(:,1,1))); -plot(f, 180/pi*angle(G_cart(:,2,2))); -plot(f, 180/pi*angle(G_cart(:,3,3))); -plot(f, 180/pi*angle(G_cart(:,4,4))); -plot(f, 180/pi*angle(G_cart(:,5,5))); -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 - -** Controller Design -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*30; - -%% Double Integrator -H_int = 1/(s + 1*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,1,1)); - -%% Decentralized HAC -Khac_Dx = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*30; - -%% Double Integrator -H_int = 1/(s + 1*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,2,2)); - -%% Decentralized HAC -Khac_Dy = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*40; - -%% Double Integrator -H_int = 1/(s + 2*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,3,3)); - -%% Decentralized HAC -Khac_Dz = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*10; - -%% Double Integrator -H_int = 1/(s + 1.5*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,4,4)); - -%% Decentralized HAC -Khac_Rx = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*10; - -%% Double Integrator -H_int = 1/(s + 1.5*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,5,5)); - -%% Decentralized HAC -Khac_Ry = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab -Khac = blkdiag(Khac_Dx, Khac_Dy, Khac_Dz, Khac_Rx, Khac_Ry); -#+end_src - -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz')))); -plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz')))); -plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz')))); -plot(f, abs(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz')))); -plot(f, abs(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz')))); -plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz')))); -plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz')))); -plot(f, 180/pi*angle(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz')))); -plot(f, 180/pi*angle(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, 'Hz')))); -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 - -** Check Stability -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(5, length(f)); - -% Loop gain -Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(:, 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; -plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.') -plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.') -for i = 1:size(Ldet,1) - plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.') - plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.') -end -plot(-1, 0, 'kx'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -** Save controllers -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(1,1)); - -Kz = c2d(-Khac(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_hac_Dx.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(2,2)); - -Kz = c2d(-Khac(2,2)*(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_hac_Dy.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(3,3)); - -Kz = c2d(-Khac(3,3)*(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_hac_Dz.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(4,4)); - -Kz = c2d(-Khac(4,4)*(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_hac_Rx.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(5,5)); - -Kz = c2d(-Khac(5,5)*(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_hac_Ry.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -** Performances -2023-08-18_18-33_m0_1rpm_K_cart.mat - -* 3DoF Control in Cartesian plane (fixed) -<> -** Matlab Init :noexport:ignore: -#+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 - -** Introduction :ignore: -As only Dy, Dz and Ry directions are important, we could only control them. -This lead to a 3x3 plant that may be more decoupled than the 6x6 plant. - -** 3x3 plant in Cartesian plane -#+begin_src matlab :exports none -%% Load model -load('Gm.mat') -#+end_src - -#+begin_src matlab -%% Jacobian for 3x3 plant -n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -Jt_inv = pinv(n_hexapod.geometry.J'); - -J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -Compute identified plant in the Cartesian plane: -#+begin_src matlab -%% Load Data -% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir)); -%% New data after calibrating the Rz-offset -data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.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_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Cartesian Plant -G_hac_m0 = zeros(length(f), 5, 6); - -for i_strut = 1:6 - Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]'; - - G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts); -end - -G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]); -#+end_src - -Compute plant model in the Cartesian plane: -#+begin_src matlab -Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]); -Gm_cart.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'}; -#+end_src - -#+begin_important -Diagonal elements are matching quite well, but off-diagonal elements are very different. - -Why so much more coupling than from the model? -- Is it due to the metrology? The spheres could induce coupling as for instance X motion will also be seen as Z motion. - This is especially true if not well centered with the sphere (as seemed to be the case for the lateral interferometers). -#+end_important - -#+begin_src matlab :exports none :results none -%% 3x3 cartesian plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-8, 2e-4]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-8, 2e-4]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-9, 1e-2]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_cart_plant_3x3.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_cart_plant_3x3 -#+caption: 3x3 cartesian plant -#+RESULTS: -[[file:figs/id31_cart_plant_3x3.png]] - - -Normalization of outputs: -#+begin_src matlab -Gm_cart_normalized = -diag(1./diag(dcgain(Gm_cart)))*Gm_cart; -Gm_cart_normalized.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_normalized.OutputName = {'Dy', 'Dz', 'Ry'}; - -G_cart_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart))), permute(G_cart, [2,3,1])), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-4, 1e1]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-4, 1e1]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart_normalized(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-4, 1e1]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_normalized(:,1,1))); -plot(f, abs(G_cart_normalized(:,2,2))); -plot(f, abs(G_cart_normalized(:,3,3))); -plot(f, abs(G_cart_normalized(:,1,2)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_normalized(:,1,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_normalized(:,2,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_normalized(:,3,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_normalized(:,2,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_normalized(:,3,2)), 'color', [0,0,0,0.2]); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -ylim([1e-4, 1e1]); - -ax2 = nexttile(); -hold on; -plot(f, 180/pi*angle(G_cart_normalized(:,1,1))); -plot(f, 180/pi*angle(G_cart_normalized(:,2,2))); -plot(f, 180/pi*angle(G_cart_normalized(:,3,3))); -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 - -** Controller Design -*** Dy -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*30; - -%% Double Integrator -H_int = 1/(s + 0.1*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,1,1)); - -%% Decentralized HAC -Khac_Dy = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz')))); -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 - -*** Dz -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*50; - -%% Double Integrator -H_int = 1/(s + 0.1*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,2,2)); - -%% Decentralized HAC -Khac_Dz = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz')))); -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 - -*** Ry -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*30; - -%% Double Integrator -H_int = 1/(s + 0.1*2*pi) * ... - 1/(s + 0.01*2*pi); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/300); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = -1./abs(G_cart(i_f,3,3)); - -%% Decentralized HAC -Khac_Ry = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead; % Integrator -#+end_src - -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz')))); -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 - -*** 3x3 controller -#+begin_src matlab -Khac = blkdiag(Khac_Dy, Khac_Dz, Khac_Ry); -#+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_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))), 'DisplayName', '$D_y$'); -plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))), 'DisplayName', '$D_z$'); -plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz')))); -plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz')))); -plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz')))); -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/G_cart_loop_gain_diagonal_3dof.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:G_cart_loop_gain_diagonal_3dof -#+caption: description -#+RESULTS: -[[file:figs/G_cart_loop_gain_diagonal_3dof.png]] - - -** Check Stability -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(3, length(f)); - -% Loop gain -Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); -end -#+end_src - -#+begin_src matlab :exports none :results none -%% description -figure; -hold on; -plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.') -plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.') -for i = 1:3 - plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.') - plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.') -end -plot(-1, 0, 'kx'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/G_cart_nyquist_diagonal_3dof.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:G_cart_nyquist_diagonal_3dof -#+caption: description -#+RESULTS: -[[file:figs/G_cart_nyquist_diagonal_3dof.png]] - -** Save controllers -*** Save Controller -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(1,1)); - -Kz = c2d(-Khac(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_hac_Dy_3x3.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(2,2)); - -Kz = c2d(-Khac(2,2)*(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_hac_Dz_3x3.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K_order = order(Khac(3,3)); - -Kz = c2d(-Khac(3,3)*(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_hac_Ry_3x3.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -** Controller Design (normalized) -#+begin_src matlab -%% Wanted crossover -wc = 2*pi*30; - -%% Double Integrator -H_int = 1/(s + 0.1*2*pi) * ... - (50*2*pi + s)/(0.01*2*pi + s); -H_int = H_int/abs(evalfr(H_int, 1j*wc)); -% H_int = wc/s; - -%% Lead to increase phase margin -a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -% a = 2; % Amount of phase lead / width of the phase lead / high frequency gain -% H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); - -%% Low Pass filter to increase robustness -H_lpf = 1/(1 + s/2/pi/200); - -%% Notch at the top-plate resonance -% gm = 0.02; -% xi = 0.3; -% wn = 2*pi*665; - -% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -%% Gain to have unitary crossover at 30Hz -[~, i_f] = min(abs(f - wc/2/pi)); -H_gain = 1./abs(G_cart_normalized(i_f, 1, 1)); - -%% Decentralized HAC -Khac = H_gain * ... % Gain - H_int * ... % Integrator - H_lpf * ... % Low Pass Filter - H_lead * ... % Integrator - eye(3); % 6x6 Diagonal - -% Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; -% Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -#+end_src - -#+begin_src matlab :exports none -%% Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:3 - plot(f, abs(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e4]); - -ax2 = nexttile; -hold on; -for i = 1:3 - plot(f, 180/pi*angle(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 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 - -** Verify Stability -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(3, length(f)); - -% Loop gain -Lmimo = pagemtimes(permute(G_cart_normalized, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz'))); -for i_f = 2:length(f) - Ldet(:, 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; -plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.'); -plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.'); -for i = 1:3 - plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.'); - plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.'); -end -plot(-1, 0, 'kx', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - - -** Control Performances -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_10-36_m0_cart_fixed.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 -[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts); -[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts); -#+end_src - -- [ ] Compare with estimated performances - -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -* Complementary Filter Control -<> -** Matlab Init :noexport:ignore: -#+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 - -** Tomography -*** TODO Slow Tomography Scans Comparison of control performances :noexport: -- [ ] Comparison of different control strategies - *Maybe not so important here* - -#+begin_src matlab -% Decentralized in the frame of the struts -data = load("2023-08-18_10-43_m0_1rpm.mat"); -data.time = Ts*[0:length(data.Rz)-1]; - -% Rotating cartesian frame -data_cart = load("2023-08-18_18-33_m0_1rpm_K_cart.mat"); -data_cart.time = Ts*[0:length(data_cart.Rz)-1]; - -% Fixed cartesian frame -data_cart_fixed = load("2023-08-18_19-08_m0_1rpm_K_cart_fixed.mat"); -data_cart_fixed.time = Ts*[0:length(data_cart_fixed.Rz)-1]; - -% Fixed cartesian frame with Complementary Filters -data_cf = load("2023-08-21_14-28_m0_1rpm_K_cf.mat"); -data_cf.time = Ts*[0:length(data_cf.Rz)-1]; -#+end_src - -#+begin_src matlab -1e9*rms(data.Dx_int(data.time<45)) -1e9*rms(data_cart.Dx_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Dx_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Dx_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Dy_int(data.time<45)) -1e9*rms(data_cart.Dy_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Dy_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Dy_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Dz_int(data.time<45)) -1e9*rms(data_cart.Dz_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Dz_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Dz_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Rx_int(data.time<45)) -1e9*rms(data_cart.Rx_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Rx_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Rx_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Ry_int(data.time<45)) -1e9*rms(data_cart.Ry_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Ry_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Ry_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -figure; - -hold on; -plot(data.time, data.Dy_int) -plot(data_cart.time, data_cart.Dy_int) -plot(data_cart_fixed.time, data_cart_fixed.Dy_int) -plot(data_cf.time, data_cf.Dy_int) -hold off; -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(10.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data.pxx_Dx, data.f] = pwelch(detrend(data.Dx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Dy, ~ ] = pwelch(detrend(data.Dy_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Dz, ~ ] = pwelch(detrend(data.Dz_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Rx, ~ ] = pwelch(detrend(data.Rx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Ry, ~ ] = pwelch(detrend(data.Ry_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); - -[data_cart.pxx_Dx, data_cart.f] = pwelch(detrend(data_cart.Dx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Dy, ~ ] = pwelch(detrend(data_cart.Dy_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Dz, ~ ] = pwelch(detrend(data_cart.Dz_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Rx, ~ ] = pwelch(detrend(data_cart.Rx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Ry, ~ ] = pwelch(detrend(data_cart.Ry_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); - -[data_cart_fixed.pxx_Dx, data_cart_fixed.f] = pwelch(detrend(data_cart_fixed.Dx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Dy, ~ ] = pwelch(detrend(data_cart_fixed.Dy_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Dz, ~ ] = pwelch(detrend(data_cart_fixed.Dz_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Rx, ~ ] = pwelch(detrend(data_cart_fixed.Rx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Ry, ~ ] = pwelch(detrend(data_cart_fixed.Ry_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); - -[data_cf.pxx_Dx, data_cf.f] = pwelch(detrend(data_cf.Dx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Dy, ~ ] = pwelch(detrend(data_cf.Dy_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Dz, ~ ] = pwelch(detrend(data_cf.Dz_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Rx, ~ ] = pwelch(detrend(data_cf.Rx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Ry, ~ ] = pwelch(detrend(data_cf.Ry_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab -figure; - -hold on; -plot(data.f, data.pxx_Dy) -plot(data_cart.f, data_cart.pxx_Dy) -plot(data_cart_fixed.f, data_cart_fixed.pxx_Dy) -plot(data_cf.f, data_cf.pxx_Dy) -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xlim([0.1, 5e2]); -#+end_src - -#+begin_src matlab -figure; - -hold on; -plot(data.f, sqrt(flip(-cumtrapz(flip(data.f), flip(data.pxx_Dy))))) -plot(data_cart.f, sqrt(flip(-cumtrapz(flip(data_cart.f), flip(data_cart.pxx_Dy))))) -plot(data_cart_fixed.f, sqrt(flip(-cumtrapz(flip(data_cart_fixed.f), flip(data_cart_fixed.pxx_Dy))))) -plot(data_cf.f, sqrt(flip(-cumtrapz(flip(data_cf.f), flip(data_cf.pxx_Dy))))) -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xlim([0.1, 5e2]); -#+end_src - -*** TODO Medium velocity tomography scans with CF control :noexport: -#+begin_src matlab -data_m1_cf = load("/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/mat/nass/scans/2023-08-21_19-18_m1_6rpm_cf_control.mat"); -data_m1_cf.time = Ts*[0:length(data_m1_cf.Rz)-1]; -#+end_src - -#+begin_src matlab -data_m2_cf = load("/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/mat/nass/scans/2023-08-21_18-07_m2_6rpm_cf_control.mat"); -data_m2_cf.time = Ts*[0:length(data_m2_cf.Rz)-1]; -#+end_src - -And higher bandwidth: -#+begin_src matlab -data_m1_cf_high_fb = load("/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/mat/nass/scans/2023-08-21_19-24_m1_6rpm_cf_control_60Hz.mat"); -data_m1_cf_high_fb.time = Ts*[0:length(data_m1_cf_high_fb.Rz)-1]; -#+end_src - -#+begin_src matlab -figure; -hold on; -plot(data_m1_cf.Dy_int, detrend(data_m1_cf.Dz_int, 0), 'DisplayName', 'm1') -plot(data_m2_cf.Dy_int, detrend(data_m2_cf.Dz_int, 0), 'DisplayName', 'm2') -plot(data_m1_cf_high_fb.Dy_int, detrend(data_m1_cf_high_fb.Dz_int, 0), 'DisplayName', 'm1, high BW') -axis equal -legend -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m1.Dz_int(i_m1+50000:end), 0)) -1e9*rms(detrend(data_m1.Dy_int(i_m1+50000:end), 0)) -1e9*rms(detrend(data_m1.Ry_int(i_m1+50000:end), 0)) -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m1_cf.Dz_int, 0)) -1e9*rms(detrend(data_m1_cf.Dy_int, 0)) -1e9*rms(detrend(data_m1_cf.Ry_int, 0)) -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m2.Dz_int, 0)) -1e9*rms(detrend(data_m2.Dy_int, 0)) -1e9*rms(detrend(data_m2.Ry_int, 0)) -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m1_cf_high_fb.Dz_int, 0)) -1e9*rms(detrend(data_m1_cf_high_fb.Dy_int, 0)) -1e9*rms(detrend(data_m1_cf_high_fb.Ry_int, 0)) -#+end_src - -*** TODO Fast Tomography Scan with Complementary Filter Controller :noexport: -#+begin_src matlab -data_cf = load("/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/mat/nass/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat"); -data_cf.time = Ts*[0:length(data_cf.Rz)-1]; -#+end_src - -#+begin_src matlab -[~, i0] = find(data.hac_status == 1); -1e9*rms(data.Dy_int(i0(1)+5000:end)) -1e9*rms(data.Dz_int(i0(1)+5000:end)) - -1e9*rms(data_cf.Dy_int) -1e9*rms(data_cf.Dz_int) -#+end_src - -Same performance than the robust controller in terms of RMS residual motion. - -#+begin_src matlab -figure; plot3(data.Dx_int, data.Dy_int, data.Dz_int) -#+end_src - -*** Tomography - Effect of the scanning velocity :noexport: -- [ ] What are the controller used here? Why worst results than with the robust controller? - -#+begin_src matlab -data_1rpm = load("2023-08-18_10-43_m0_1rpm.mat"); -data_1rpm.time = Ts*[0:length(data_1rpm.Rz)-1]; -#+end_src - -#+begin_src matlab -data_30rpm = load("2023-08-18_10-45_m0_30rpm.mat"); -data_30rpm.time = Ts*[0:length(data_30rpm.Rz)-1]; -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*rms(detrend(data_1rpm.Dy_int, 0)), 1e9*rms(detrend(data_30rpm.Dy_int, 0)); 1e9*rms(detrend(data_1rpm.Dz_int, 0)), 1e9*rms(detrend(data_30rpm.Dz_int, 0)); 1e9*rms(detrend(data_1rpm.Ry_int, 0)), 1e9*rms(detrend(data_30rpm.Ry_int, 0))]', {'1RPM', '30RPM'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.1f '); -#+end_src - -#+name: tab:id31_tomography_effect_velocity_rms -#+caption: RMS values of the errors during tomography scan - Effect of $R_z$ velocity -#+attr_latex: :environment tabularx :width 0.5\linewidth :align lXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_y$ | $D_z$ | $R_y$ | -|-------+-------+-------+-------| -| 1RPM | 30.9 | 5.9 | 92.4 | -| 30RPM | 71.7 | 12.5 | 301.3 | - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ... - 1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ... - 1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ... - {'1rpm', '6rpm', '30rpm'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] | -|-------+-----------------+-----------------+--------------+-------------------------+-------------------------| -| 1rpm | 13 | 15 | 5 | 57 | 55 | -| 6rpm | 17 | 19 | 5 | 70 | 73 | -| 30rpm | 34 | 38 | 10 | 127 | 129 | - -** m0 -*** 3x3 plant in Cartesian plane -#+begin_src matlab :exports none -%% Load model -load('Gm.mat') -#+end_src - -#+begin_src matlab -%% Jacobian for 3x3 plant -n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -Jt_inv = pinv(n_hexapod.geometry.J'); - -J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -Compute identified plant in the Cartesian plane: -#+begin_src matlab -%% New data after calibrating the Rz-offset -data_m0 = load(sprintf('%s/dynamics/2023-08-21_13-32_damp_plant_m0.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_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Cartesian Plant -G_hac_m0 = zeros(length(f), 5, 6); - -for i_strut = 1:6 - Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]'; - - G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts); -end - -G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]); -#+end_src - -Compute plant model in the Cartesian plane: -#+begin_src matlab -Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]); -Gm_cart.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'}; -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-8, 2e-4]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-8, 2e-4]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-9, 1e-2]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)]); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)]); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)]); -plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--', 'color', [colors(3,:)]); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Magnitude'); - -ylim([1e-9, 1e-2]); -xlim([1, 5e2]); -#+end_src - -*** Plant Invert -Reduce model size -#+begin_src matlab -Gm_cart_dy = flipRphZeros(balred(-Gm_cart('Dy', 'Fy'), 10)); -Gm_cart_dz = flipRphZeros(balred(-Gm_cart('Dz', 'Fz'), 10)); -Gm_cart_ry = flipRphZeros(balred(-Gm_cart('Ry', 'My'), 10)); -#+end_src - -Add first resonance -#+begin_src matlab :eval no -% gm = 200; -% xi = 0.003; -% wn = 2*pi*670; - -% Gm_cart_dy = Gm_cart_dy*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -% gm = 200; -% xi = 0.003; -% wn = 2*pi*1086; - -% Gm_cart_dz = Gm_cart_dz*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); - -% gm = 200; -% xi = 0.003; -% wn = 2*pi*670; - -% Gm_cart_ry = Gm_cart_ry*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Gm_cart_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_comp_cart_plant_reduced_model.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:id31_comp_cart_plant_reduced_model -#+caption: Comparaison of the measured direct terms and the reduced order models -#+RESULTS: -[[file:figs/id31_comp_cart_plant_reduced_model.png]] - - -Invert and make realizable -#+begin_src matlab -Gm_cart_dy_inv = inv(Gm_cart_dy); -Gm_cart_dz_inv = inv(Gm_cart_dz); -Gm_cart_ry_inv = inv(Gm_cart_ry); -#+end_src - -#+begin_src matlab -isstable(Gm_cart_dy_inv) -isstable(Gm_cart_dz_inv) -isstable(Gm_cart_ry_inv) -#+end_src - -*** Save Plant Inverse -#+begin_src matlab :exports none :tangle no -K = -Gm_cart_dy_inv; -K_order = order(K); - -Kz = c2d(K*(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/G_dy_inv.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Gm_cart_dz_inv; -K_order = order(K); - -Kz = c2d(K*(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/G_dz_inv.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Gm_cart_ry_inv; -K_order = order(K); - -Kz = c2d(K*(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/G_ry_inv.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -*** Control Performances -**** 5Hz -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_10-59_m0_cf_5Hz.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 -[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts); -[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts); -#+end_src - -- [ ] Compare with estimated performances - -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -**** 20Hz -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_11-04_m0_cf_20Hz.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 -[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts); -[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts); -#+end_src - -- [ ] Compare with estimated performances - -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -**** Different bandwidth for different directions -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_11-16_m0_cf_different.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 -[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts); -[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts); -#+end_src - -- [ ] Compare with estimated performances - -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - - -**** Dz 25Hz -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/', 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 -[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts); -[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts); -#+end_src - -- [ ] Compare with estimated performances - -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - - -*** Better plant invert -**** Dy -#+begin_src matlab :exports none -opts = struct(); - -opts.stable = 1; % Enforce stable poles -opts.asymp = 1; % Force D matrix to be null -opts.relax = 1; % Use vector fitting with relaxed non-triviality constraint -opts.skip_pole = 0; % Do NOT skip pole identification -opts.skip_res = 0; % Do NOT skip identification of residues (C,D,E) -opts.cmplx_ss = 0; % Create real state space model with block diagonal A - -opts.spy1 = 0; % No plotting for first stage of vector fitting -opts.spy2 = 0; % Create magnitude plot for fitting of f(s) - -%% We define the number of iteration. -Niter = 100; -#+end_src - -#+begin_src matlab :exports none -N = 9; %Order of approximation - -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts); -end - -Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3); -isstable(Ginv_dy) -#+end_src - -**** Dz -#+begin_src matlab :exports none -N = 9; %Order of approximation - -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts); -end - -Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3); -isstable(Ginv_dz) -#+end_src - -**** Ry -#+begin_src matlab :exports none -N = 9; %Order of approximation - -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts); -end - -Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3); -isstable(Ginv_ry) -#+end_src - -**** Compare Invert plants - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,3)), '-'); -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, 2e3]); -#+end_src - -**** Save plant inverse -#+begin_src matlab :exports none :tangle no -K = -Ginv_dy; -K_order = order(K); - -Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dy, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_fit.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_dz; -K_order = order(K); - -Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dz, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_fit.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_ry; -K_order = order(K); - -Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_ry, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_fit.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -**** Compare Digital Invert plants - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,3)), '-'); -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, 2e3]); -#+end_src - -*** Control Performances -**** Better plant invert -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.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 -[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -*** Scans with good controller -**** 1rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 796 | 20 | 8 | 8209 | 73 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - - -**** 30rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 820 | 39 | 13 | 7790 | 156 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - -** m1 -*** 3x3 plant in Cartesian plane -#+begin_src matlab :exports none -%% Load model -load('Gm.mat') -#+end_src - -#+begin_src matlab -%% Jacobian for 3x3 plant -n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -Jt_inv = pinv(n_hexapod.geometry.J'); - -J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -Compute identified plant in the Cartesian plane: -#+begin_src matlab -%% New data after calibrating the Rz-offset -data_m1 = load(sprintf('%s/dynamics/2023-08-21_19-05_damp_plant_m1_new_Rz.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_m1.uL1.id_plant, data_m1.uL1.d1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Cartesian Plant -G_hac_m1 = zeros(length(f), 5, 6); - -for i_strut = 1:6 - Di = [data_m1.(sprintf("uL%i", i_strut)).d1 ; data_m1.(sprintf("uL%i", i_strut)).d2 ; data_m1.(sprintf("uL%i", i_strut)).d3 ; data_m1.(sprintf("uL%i", i_strut)).d4 ; data_m1.(sprintf("uL%i", i_strut)).d5]'; - - G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts); -end - -G_cart_m1 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m1, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]); -#+end_src - -Compute plant model in the Cartesian plane: -#+begin_src matlab -Gm_cart_m1 = J_int_to_X*Gm_hac_m1({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]); -Gm_cart_m1.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_m1.OutputName = {'Dy', 'Dz', 'Ry'}; -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-8, 2e-4]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-8, 2e-4]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart_m1(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-9, 1e-2]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -Normalization of outputs: -#+begin_src matlab -Gm_cart_m1_normalized = -diag(1./diag(dcgain(Gm_cart_m1)))*Gm_cart_m1; -Gm_cart_m1_normalized.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_m1_normalized.OutputName = {'Dy', 'Dz', 'Ry'}; - -G_cart_m1_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m1))), permute(G_cart_m1, [2,3,1])), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m1_normalized(:,1,1))); -plot(f, abs(G_cart_m1_normalized(:,2,2))); -plot(f, abs(G_cart_m1_normalized(:,3,3))); -plot(f, abs(G_cart_m1_normalized(:,1,2)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m1_normalized(:,1,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m1_normalized(:,2,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m1_normalized(:,3,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m1_normalized(:,2,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m1_normalized(:,3,2)), 'color', [0,0,0,0.2]); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -ylim([1e-4, 1e1]); - -ax2 = nexttile(); -hold on; -plot(f, 180/pi*angle(G_cart_m1_normalized(:,1,1))); -plot(f, 180/pi*angle(G_cart_m1_normalized(:,2,2))); -plot(f, 180/pi*angle(G_cart_m1_normalized(:,3,3))); -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 - -*** Better plant invert -#+begin_src matlab :exports none -opts = struct(); - -opts.stable = 1; % Enforce stable poles -opts.asymp = 1; % Force D matrix to be null -opts.relax = 1; % Use vector fitting with relaxed non-triviality constraint -opts.skip_pole = 0; % Do NOT skip pole identification -opts.skip_res = 0; % Do NOT skip identification of residues (C,D,E) -opts.cmplx_ss = 0; % Create real state space model with block diagonal A - -opts.spy1 = 0; % No plotting for first stage of vector fitting -opts.spy2 = 0; % Create magnitude plot for fitting of f(s) - -%% We define the number of iteration. -Niter = 100; -#+end_src - -#+begin_src matlab -N = 9; %Order of approximation -#+end_src - -**** Dy -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-2; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts); -end - -Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m1(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3); -isstable(Ginv_dy) -#+end_src - -**** Dz -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./sqrt(f(f>2&f<1500))', opts); -end - -Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m1(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3); -isstable(Ginv_dz) -#+end_src - -**** Ry -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts); -end - -Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m1(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3); -isstable(Ginv_ry) -#+end_src - -**** Compare Invert plants -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m1(:,1,1))), '-', 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m1(:,2,2))), '-', 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m1(:,3,3))), '-', 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m1(:,1,1)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m1(:,2,2)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m1(:,3,3)), '-'); -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, 2e3]); -#+end_src - -**** Save plant inverse -#+begin_src matlab :exports none :tangle no -K = -Ginv_dy; -K_order = order(K); - -Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dy, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m1.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_dz; -K_order = order(K); - -Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dz, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m1.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_ry; -K_order = order(K); - -Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_ry, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m1.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -**** Compare Digital Invert plants -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -*** TODO Control Performances -**** Better plant invert -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.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 -[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -*** TODO Scans with good controller -**** 1rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 796 | 20 | 8 | 8209 | 73 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - - -**** 30rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 820 | 39 | 13 | 7790 | 156 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - -** m2 -*** 3x3 plant in Cartesian plane -#+begin_src matlab :exports none -%% Load model -load('Gm.mat') -#+end_src - -#+begin_src matlab -%% Jacobian for 3x3 plant -n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -Jt_inv = pinv(n_hexapod.geometry.J'); - -J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -Compute identified plant in the Cartesian plane: -#+begin_src matlab -%% New data after calibrating the Rz-offset -data_m2 = load(sprintf('%s/dynamics/2023-08-21_17-32_damp_plant_m2_new_Rz.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_m2.uL1.id_plant, data_m2.uL1.d1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Cartesian Plant -G_hac_m2 = zeros(length(f), 5, 6); - -for i_strut = 1:6 - Di = [data_m2.(sprintf("uL%i", i_strut)).d1 ; data_m2.(sprintf("uL%i", i_strut)).d2 ; data_m2.(sprintf("uL%i", i_strut)).d3 ; data_m2.(sprintf("uL%i", i_strut)).d4 ; data_m2.(sprintf("uL%i", i_strut)).d5]'; - - G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts); -end - -G_cart_m2 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m2, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]); -#+end_src - -Compute plant model in the Cartesian plane: -#+begin_src matlab -Gm_cart_m2 = J_int_to_X*Gm_hac_m2({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]); -Gm_cart_m2.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_m2.OutputName = {'Dy', 'Dz', 'Ry'}; -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-8, 2e-4]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-8, 2e-4]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart_m2(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-9, 1e-2]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -Normalization of outputs: -#+begin_src matlab -Gm_cart_m2_normalized = -diag(1./diag(dcgain(Gm_cart_m2)))*Gm_cart_m2; -Gm_cart_m2_normalized.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_m2_normalized.OutputName = {'Dy', 'Dz', 'Ry'}; - -G_cart_m2_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m2))), permute(G_cart_m2, [2,3,1])), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m2_normalized(:,1,1))); -plot(f, abs(G_cart_m2_normalized(:,2,2))); -plot(f, abs(G_cart_m2_normalized(:,3,3))); -plot(f, abs(G_cart_m2_normalized(:,1,2)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m2_normalized(:,1,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m2_normalized(:,2,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m2_normalized(:,3,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m2_normalized(:,2,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m2_normalized(:,3,2)), 'color', [0,0,0,0.2]); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -ylim([1e-4, 1e1]); - -ax2 = nexttile(); -hold on; -plot(f, 180/pi*angle(G_cart_m2_normalized(:,1,1))); -plot(f, 180/pi*angle(G_cart_m2_normalized(:,2,2))); -plot(f, 180/pi*angle(G_cart_m2_normalized(:,3,3))); -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 - -*** Better plant invert -#+begin_src matlab :exports none -opts = struct(); - -opts.stable = 1; % Enforce stable poles -opts.asymp = 1; % Force D matrix to be null -opts.relax = 1; % Use vector fitting with relaxed non-triviality constraint -opts.skip_pole = 0; % Do NOT skip pole identification -opts.skip_res = 0; % Do NOT skip identification of residues (C,D,E) -opts.cmplx_ss = 0; % Create real state space model with block diagonal A - -opts.spy1 = 0; % No plotting for first stage of vector fitting -opts.spy2 = 0; % Create magnitude plot for fitting of f(s) - -%% We define the number of iteration. -Niter = 100; -#+end_src - -#+begin_src matlab -N = 9; %Order of approximation -#+end_src - -**** Dy -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-2; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts); -end - -Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m2(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3); -isstable(Ginv_dy) -#+end_src - -**** Dz -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts); -end - -Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m2(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3); -isstable(Ginv_dz) -#+end_src - -**** Ry -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts); -end - -Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m2(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3); -isstable(Ginv_ry) -#+end_src - -**** Compare Invert plants -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m2(:,1,1))), '-', 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m2(:,2,2))), '-', 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m2(:,3,3))), '-', 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m2(:,1,1)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m2(:,2,2)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m2(:,3,3)), '-'); -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, 2e3]); -#+end_src - -**** Save plant inverse -#+begin_src matlab :exports none :tangle no -K = -Ginv_dy; -K_order = order(K); - -Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dy, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m2.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_dz; -K_order = order(K); - -Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dz, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m2.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_ry; -K_order = order(K); - -Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_ry, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m2.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -**** Compare Digital Invert plants -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -*** TODO Control Performances -**** Better plant invert -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.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 -[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -*** TODO Scans with good controller -**** 1rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 796 | 20 | 8 | 8209 | 73 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - - -**** 30rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 820 | 39 | 13 | 7790 | 156 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - -** m3 -*** 3x3 plant in Cartesian plane -#+begin_src matlab :exports none -%% Load model -load('Gm.mat') -#+end_src - -#+begin_src matlab -%% Jacobian for 3x3 plant -n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -Jt_inv = pinv(n_hexapod.geometry.J'); - -J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -Compute identified plant in the Cartesian plane: -#+begin_src matlab -%% New data after calibrating the Rz-offset -data_m3 = load(sprintf('%s/dynamics/2023-08-21_16-33_damp_plant_m3_new_Rz_fast.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_m3.uL1.id_plant, data_m3.uL1.d1, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% HAC Cartesian Plant -G_hac_m3 = zeros(length(f), 5, 6); - -for i_strut = 1:6 - Di = [data_m3.(sprintf("uL%i", i_strut)).d1 ; data_m3.(sprintf("uL%i", i_strut)).d2 ; data_m3.(sprintf("uL%i", i_strut)).d3 ; data_m3.(sprintf("uL%i", i_strut)).d4 ; data_m3.(sprintf("uL%i", i_strut)).d5]'; - - G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts); -end - -G_cart_m3 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m3, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]); -#+end_src - -Compute plant model in the Cartesian plane: -#+begin_src matlab -Gm_cart_m3 = J_int_to_X*Gm_hac_m3({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]); -Gm_cart_m3.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_m3.OutputName = {'Dy', 'Dz', 'Ry'}; -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-8, 2e-4]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-8, 2e-4]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart_m3(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-9, 1e-2]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -Normalization of outputs: -#+begin_src matlab -Gm_cart_m3_normalized = -diag(1./diag(dcgain(Gm_cart_m3)))*Gm_cart_m3; -Gm_cart_m3_normalized.InputName = {'Fy', 'Fz', 'My'}; -Gm_cart_m3_normalized.OutputName = {'Dy', 'Dz', 'Ry'}; - -G_cart_m3_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m3))), permute(G_cart_m3, [2,3,1])), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax11 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,1,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$D_y$'); set(gca, 'XTickLabel',[]); -title('$F_y$') - -ax12 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,1,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$F_z$') - -ax13 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,1,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title('$M_y$') - -linkaxes([ax11,ax12,ax13],'y'); -ylim([1e-4, 1e1]); - -ax21 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,2,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('$F_z$'); set(gca, 'XTickLabel',[]); - -ax22 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,2,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -ax23 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,2,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); - -linkaxes([ax21,ax22,ax23],'y'); -ylim([1e-4, 1e1]); - -ax31 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,3,1))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('$M_y$'); - -ax32 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,3,2))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -ax33 = nexttile(); -hold on; -plot(f, abs(G_cart_m3_normalized(:,3,3))); -plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'My'), freqs, 'Hz'))), '--'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); - -linkaxes([ax31,ax32,ax33],'y'); -ylim([1e-4, 1e1]); - -linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x'); -xlim([1, 5e2]); -#+end_src - -#+begin_src matlab :exports none :results none -%% 3x3 plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m3_normalized(:,1,1))); -plot(f, abs(G_cart_m3_normalized(:,2,2))); -plot(f, abs(G_cart_m3_normalized(:,3,3))); -plot(f, abs(G_cart_m3_normalized(:,1,2)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m3_normalized(:,1,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m3_normalized(:,2,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m3_normalized(:,3,1)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m3_normalized(:,2,3)), 'color', [0,0,0,0.2]); -plot(f, abs(G_cart_m3_normalized(:,3,2)), 'color', [0,0,0,0.2]); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -ylim([1e-4, 1e1]); - -ax2 = nexttile(); -hold on; -plot(f, 180/pi*angle(G_cart_m3_normalized(:,1,1))); -plot(f, 180/pi*angle(G_cart_m3_normalized(:,2,2))); -plot(f, 180/pi*angle(G_cart_m3_normalized(:,3,3))); -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 - -*** Better plant invert -#+begin_src matlab :exports none -opts = struct(); - -opts.stable = 1; % Enforce stable poles -opts.asymp = 1; % Force D matrix to be null -opts.relax = 1; % Use vector fitting with relaxed non-triviality constraint -opts.skip_pole = 0; % Do NOT skip pole identification -opts.skip_res = 0; % Do NOT skip identification of residues (C,D,E) -opts.cmplx_ss = 0; % Create real state space model with block diagonal A - -opts.spy1 = 0; % No plotting for first stage of vector fitting -opts.spy2 = 0; % Create magnitude plot for fitting of f(s) - -%% We define the number of iteration. -Niter = 100; -#+end_src - -#+begin_src matlab -N = 9; %Order of approximation -#+end_src - -**** Dy -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts); -end - -Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m3(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3); -isstable(Ginv_dy) -#+end_src - -**** Dz -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts); -end - -Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m3(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3); -isstable(Ginv_dz) -#+end_src - -**** Ry -#+begin_src matlab :exports none -%Complex conjugate pairs, linearly spaced: -bet=linspace(f(1),f(end),N/2); -poles=[]; -for n=1:length(bet) - alf=-bet(n)*1e-1; - poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ]; -end -#+end_src - -#+begin_src matlab -%% Estimate resonance frequency and damping -for iter =1:Niter - [G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts); -end - -Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(G_cart_m3(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -Stable Inverse -#+begin_src matlab -Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3); -isstable(Ginv_ry) -#+end_src - -**** Compare Invert plants -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m3(:,1,1))), '-', 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m3(:,2,2))), '-', 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m3(:,3,3))), '-', 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m3(:,1,1)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m3(:,2,2)), '-'); -plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m3(:,3,3)), '-'); -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, 2e3]); -#+end_src - -**** Save plant inverse -#+begin_src matlab :exports none :tangle no -K = -Ginv_dy; -K_order = order(K); - -Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dy, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m3.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_dz; -K_order = order(K); - -Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_dz, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m3.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -#+begin_src matlab :exports none :tangle no -K = -Ginv_ry; -K_order = order(K); - -Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin'); -[num, den] = tfdata(Kz_ry, 'v'); - -formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n'; -fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m3.dat', 'w'); -fprintf(fileID, formatSpec, [num; den]'); -fclose(fileID); -#+end_src - -**** Compare Digital Invert plants -#+begin_src matlab :exports none :results none -%% Comparaison of the measured direct terms and the reduced order models -figure; -tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$'); -plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$'); -plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off'); -plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$'); -plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude'); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]); -plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]); -plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]); -plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]); -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, 2e3]); -#+end_src - -*** TODO Control Performances -**** Better plant invert -#+begin_src matlab -data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.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 -[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts); -[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts); -[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts); -#+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'); - -ax1 = nexttile(); -hold on; -plot(f, abs(S_dy), 'DisplayName', '$D_y$'); -plot(f, abs(S_dz), 'DisplayName', '$D_z$'); -plot(f, abs(S_ry), 'DisplayName', '$R_y$'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude'); -ylim([1e-3, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -*** TODO Scans with good controller -**** 1rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 796 | 20 | 8 | 8209 | 73 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - - -**** 30rpm -1RPM scans are performed for all the masses with the same controller. - -#+begin_src matlab -data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir)); -data_m0.time = Ts*[0:length(data_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0)); -data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0)); -data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0)); -data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0)); -data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ... - {'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] | -|----+---------+---------+---------+-----------+-----------| -| m0 | 820 | 39 | 13 | 7790 | 156 | - -#+begin_src matlab :exports none :results none -%% description -figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int); -hold off; -axis square -xlabel("Y motion [$nm$]"); -ylabel("Z motion [$nm$]"); -#+end_src - -* DONE Scans -<> -** Introduction :ignore: - -- Section ref:sec:id31_scans_tomography -- Section ref:sec:id31_scans_dz -- Section ref:sec:id31_scans_reflectivity -- Section ref:sec:id31_scans_dy -- Section ref:sec:id31_scans_diffraction_tomo - -** Matlab Init :noexport:ignore: -#+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 - -** $R_z$ scans: Tomography -<> -*** Introduction :ignore: - -m0: 30rpm, 6rpm, 1rpm -m1: 6rpm, 1rpm -m2: 6rpm, 1rpm -m3: 1rpm - -*** Robust Control - 1rpm -1RPM scans are performed for all the masses with the same robust controller. - -#+begin_src matlab -%% Load Tomography scans with robust controller -data_tomo_1rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-37_tomography_1rpm_m0.mat", mat_dir)); -data_tomo_1rpm_m0.time = Ts*[0:length(data_tomo_1rpm_m0.Rz)-1]; - -data_tomo_1rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-15_tomography_1rpm_m1.mat", mat_dir)); -data_tomo_1rpm_m1.time = Ts*[0:length(data_tomo_1rpm_m1.Rz)-1]; - -data_tomo_1rpm_m2 = load(sprintf("%s/scans/2023-08-11_10-59_tomography_1rpm_m2.mat", mat_dir)); -data_tomo_1rpm_m2.time = Ts*[0:length(data_tomo_1rpm_m2.Rz)-1]; - -data_tomo_1rpm_m3 = load(sprintf("%s/scans/2023-08-11_10-24_tomography_1rpm_m3.mat", mat_dir)); -data_tomo_1rpm_m3.time = Ts*[0:length(data_tomo_1rpm_m3.Rz)-1]; -#+end_src - -The problem for these scans is that the position initialization was not make properly, so the open-loop errors are quite large (see Figure ref:fig:id31_tomo_1rpm_robust_m0). - -#+begin_src matlab :exports none :results none -%% $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red -figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), ... - 'DisplayName', 'OL') -plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), ... - 'DisplayName', 'CL') -hold off; -axis equal -xlabel("X motion [$\mu m$]"); -ylabel("Y motion [$\mu m$]"); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); -% xlim([-3, 3]) -% ylim([-3, 3]) - -ax2 = nexttile; -hold on; -plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 0), ... - 'DisplayName', 'OL') -plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 1), ... - 'DisplayName', 'CL') -hold off; -% axis equal -% xlim([-3, 3]) -ylim([-0.2, 1.1]) -xlabel("Y motion [$\mu m$]"); -ylabel("Z motion [$\mu m$]"); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_tomo_1rpm_robust_m0.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_tomo_1rpm_robust_m0 -#+caption: $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red -#+RESULTS: -[[file:figs/id31_tomo_1rpm_robust_m0.png]] - -#+begin_src matlab -yztomographymovie('movies/tomography_1rpm_m0', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100]) -yztomographymovie('movies/tomography_1rpm_m0_di_5000', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100], 'di', 5000) -#+end_src - -The obtained open-loop and closed-loop errors are shown in tables ref:tab:id31_tomo_1rpm_robust_ol_errors and ref:tab:id31_tomo_1rpm_robust_cl_errors respectively. - -#+begin_src matlab -%% Compute RMS values while in closed-loop and open-loop -[~, i_m0] = find(data_tomo_1rpm_m0.hac_status == 1); -data_tomo_1rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dx_int(i_m0+50000:end), 0)); -data_tomo_1rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dy_int(i_m0+50000:end), 0)); -data_tomo_1rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dz_int(i_m0+50000:end), 0)); -data_tomo_1rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Rx_int(i_m0+50000:end), 0)); -data_tomo_1rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m0.Ry_int(i_m0+50000:end), 0)); - -data_tomo_1rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dx_int(1:i_m0), 0)); -data_tomo_1rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dy_int(1:i_m0), 0)); -data_tomo_1rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dz_int(1:i_m0), 0)); -data_tomo_1rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Rx_int(1:i_m0), 0)); -data_tomo_1rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m0.Ry_int(1:i_m0), 0)); - -%% Compute RMS values while in closed-loop and open-loop -[~, i_m1] = find(data_tomo_1rpm_m1.hac_status == 1); -data_tomo_1rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dx_int(i_m1+50000:end), 0)); -data_tomo_1rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dy_int(i_m1+50000:end), 0)); -data_tomo_1rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dz_int(i_m1+50000:end), 0)); -data_tomo_1rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Rx_int(i_m1+50000:end), 0)); -data_tomo_1rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m1.Ry_int(i_m1+50000:end), 0)); - -data_tomo_1rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dx_int(1:i_m1), 0)); -data_tomo_1rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dy_int(1:i_m1), 0)); -data_tomo_1rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dz_int(1:i_m1), 0)); -data_tomo_1rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Rx_int(1:i_m1), 0)); -data_tomo_1rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m1.Ry_int(1:i_m1), 0)); - -%% Compute RMS values while in closed-loop and open-loop -[~, i_m2] = find(data_tomo_1rpm_m2.hac_status == 1); -data_tomo_1rpm_m2.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dx_int(i_m2+50000:end), 0)); -data_tomo_1rpm_m2.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dy_int(i_m2+50000:end), 0)); -data_tomo_1rpm_m2.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dz_int(i_m2+50000:end), 0)); -data_tomo_1rpm_m2.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Rx_int(i_m2+50000:end), 0)); -data_tomo_1rpm_m2.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m2.Ry_int(i_m2+50000:end), 0)); - -data_tomo_1rpm_m2.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dx_int(1:i_m2), 0)); -data_tomo_1rpm_m2.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dy_int(1:i_m2), 0)); -data_tomo_1rpm_m2.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dz_int(1:i_m2), 0)); -data_tomo_1rpm_m2.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Rx_int(1:i_m2), 0)); -data_tomo_1rpm_m2.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m2.Ry_int(1:i_m2), 0)); - -%% Compute RMS values while in closed-loop and open-loop -[~, i_m3] = find(data_tomo_1rpm_m3.hac_status == 1); -data_tomo_1rpm_m3.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dx_int(i_m3+50000:end), 0)); -data_tomo_1rpm_m3.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dy_int(i_m3+50000:end), 0)); -data_tomo_1rpm_m3.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dz_int(i_m3+50000:end), 0)); -data_tomo_1rpm_m3.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Rx_int(i_m3+50000:end), 0)); -data_tomo_1rpm_m3.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m3.Ry_int(i_m3+50000:end), 0)); - -data_tomo_1rpm_m3.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dx_int(1:i_m3), 0)); -data_tomo_1rpm_m3.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dy_int(1:i_m3), 0)); -data_tomo_1rpm_m3.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dz_int(1:i_m3), 0)); -data_tomo_1rpm_m3.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Rx_int(1:i_m3), 0)); -data_tomo_1rpm_m3.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m3.Ry_int(1:i_m3), 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e6*data_tomo_1rpm_m0.Dx_rms_ol, 1e6*data_tomo_1rpm_m0.Dy_rms_ol, 1e9*data_tomo_1rpm_m0.Dz_rms_ol, 1e6*data_tomo_1rpm_m0.Rx_rms_ol, 1e6*data_tomo_1rpm_m0.Ry_rms_ol; ... - 1e6*data_tomo_1rpm_m1.Dx_rms_ol, 1e6*data_tomo_1rpm_m1.Dy_rms_ol, 1e9*data_tomo_1rpm_m1.Dz_rms_ol, 1e6*data_tomo_1rpm_m1.Rx_rms_ol, 1e6*data_tomo_1rpm_m1.Ry_rms_ol; ... - 1e6*data_tomo_1rpm_m2.Dx_rms_ol, 1e6*data_tomo_1rpm_m2.Dy_rms_ol, 1e9*data_tomo_1rpm_m2.Dz_rms_ol, 1e6*data_tomo_1rpm_m2.Rx_rms_ol, 1e6*data_tomo_1rpm_m2.Ry_rms_ol; ... - 1e6*data_tomo_1rpm_m3.Dx_rms_ol, 1e6*data_tomo_1rpm_m3.Dy_rms_ol, 1e9*data_tomo_1rpm_m3.Dz_rms_ol, 1e6*data_tomo_1rpm_m3.Rx_rms_ol, 1e6*data_tomo_1rpm_m3.Ry_rms_ol], ... - {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_tomo_1rpm_robust_ol_errors -#+caption: Measured error during open-loop tomography scans (1rpm) -#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] | -|-------+-----------------+-----------------+--------------+-------------------------+-------------------------| -| $m_0$ | 6 | 6 | 32 | 34 | 34 | -| $m_1$ | 6 | 7 | 26 | 51 | 55 | -| $m_2$ | 36 | 38 | 36 | 259 | 253 | -| $m_3$ | 31 | 33 | 38 | 214 | 203 | - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ... - 1e9*data_tomo_1rpm_m1.Dx_rms_cl, 1e9*data_tomo_1rpm_m1.Dy_rms_cl, 1e9*data_tomo_1rpm_m1.Dz_rms_cl, 1e9*data_tomo_1rpm_m1.Rx_rms_cl, 1e9*data_tomo_1rpm_m1.Ry_rms_cl; ... - 1e9*data_tomo_1rpm_m2.Dx_rms_cl, 1e9*data_tomo_1rpm_m2.Dy_rms_cl, 1e9*data_tomo_1rpm_m2.Dz_rms_cl, 1e9*data_tomo_1rpm_m2.Rx_rms_cl, 1e9*data_tomo_1rpm_m2.Ry_rms_cl; ... - 1e9*data_tomo_1rpm_m3.Dx_rms_cl, 1e9*data_tomo_1rpm_m3.Dy_rms_cl, 1e9*data_tomo_1rpm_m3.Dz_rms_cl, 1e9*data_tomo_1rpm_m3.Rx_rms_cl, 1e9*data_tomo_1rpm_m3.Ry_rms_cl], ... - {'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_tomo_1rpm_robust_cl_errors -#+caption: Measured error during closed-loop tomography scans (1rpm, robust controller) -#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] | -|-------+------------+------------+------------+--------------+--------------| -| $m_0$ | 13 | 15 | 5 | 57 | 55 | -| $m_1$ | 16 | 25 | 6 | 102 | 55 | -| $m_2$ | 25 | 25 | 7 | 120 | 103 | -| $m_3$ | 40 | 53 | 9 | 225 | 169 | - -*** TODO Slow Tomography Scans Comparison of control performances :noexport: -#+begin_src matlab -% Decentralized in the frame of the struts -data = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir)); -data.time = Ts*[0:length(data.Rz)-1]; - -% Rotating cartesian frame -data_cart = load(sprintf("%s/scans/2023-08-18_18-33_m0_1rpm_K_cart.mat", mat_dir)); -data_cart.time = Ts*[0:length(data_cart.Rz)-1]; - -% Fixed cartesian frame -data_cart_fixed = load(sprintf("%s/scans/2023-08-18_19-08_m0_1rpm_K_cart_fixed.mat", mat_dir)); -data_cart_fixed.time = Ts*[0:length(data_cart_fixed.Rz)-1]; - -% Fixed cartesian frame with Complementary Filters -data_cf = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir)); -data_cf.time = Ts*[0:length(data_cf.Rz)-1]; -#+end_src - -#+begin_src matlab -1e9*rms(data.Dx_int(data.time<45)) -1e9*rms(data_cart.Dx_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Dx_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Dx_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Dy_int(data.time<45)) -1e9*rms(data_cart.Dy_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Dy_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Dy_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Dz_int(data.time<45)) -1e9*rms(data_cart.Dz_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Dz_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Dz_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Rx_int(data.time<45)) -1e9*rms(data_cart.Rx_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Rx_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Rx_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -1e9*rms(data.Ry_int(data.time<45)) -1e9*rms(data_cart.Ry_int(data_cart.time<45)) -1e9*rms(data_cart_fixed.Ry_int(data_cart_fixed.time<45)) -1e9*rms(data_cf.Ry_int(data_cf.time<45)) -#+end_src - -#+begin_src matlab -figure; - -hold on; -plot(data.time, data.Dy_int) -plot(data_cart.time, data_cart.Dy_int) -plot(data_cart_fixed.time, data_cart_fixed.Dy_int) -plot(data_cf.time, data_cf.Dy_int) -hold off; -#+end_src - -#+begin_src matlab :exports none -% Hannning Windows -Nfft = floor(10.0/Ts); -win = hanning(Nfft); -Noverlap = floor(Nfft/2); - -[data.pxx_Dx, data.f] = pwelch(detrend(data.Dx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Dy, ~ ] = pwelch(detrend(data.Dy_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Dz, ~ ] = pwelch(detrend(data.Dz_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Rx, ~ ] = pwelch(detrend(data.Rx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data.pxx_Ry, ~ ] = pwelch(detrend(data.Ry_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts); - -[data_cart.pxx_Dx, data_cart.f] = pwelch(detrend(data_cart.Dx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Dy, ~ ] = pwelch(detrend(data_cart.Dy_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Dz, ~ ] = pwelch(detrend(data_cart.Dz_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Rx, ~ ] = pwelch(detrend(data_cart.Rx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart.pxx_Ry, ~ ] = pwelch(detrend(data_cart.Ry_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts); - -[data_cart_fixed.pxx_Dx, data_cart_fixed.f] = pwelch(detrend(data_cart_fixed.Dx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Dy, ~ ] = pwelch(detrend(data_cart_fixed.Dy_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Dz, ~ ] = pwelch(detrend(data_cart_fixed.Dz_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Rx, ~ ] = pwelch(detrend(data_cart_fixed.Rx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cart_fixed.pxx_Ry, ~ ] = pwelch(detrend(data_cart_fixed.Ry_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts); - -[data_cf.pxx_Dx, data_cf.f] = pwelch(detrend(data_cf.Dx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Dy, ~ ] = pwelch(detrend(data_cf.Dy_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Dz, ~ ] = pwelch(detrend(data_cf.Dz_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Rx, ~ ] = pwelch(detrend(data_cf.Rx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -[data_cf.pxx_Ry, ~ ] = pwelch(detrend(data_cf.Ry_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab -figure; - -hold on; -plot(data.f, data.pxx_Dy) -plot(data_cart.f, data_cart.pxx_Dy) -plot(data_cart_fixed.f, data_cart_fixed.pxx_Dy) -plot(data_cf.f, data_cf.pxx_Dy) -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xlim([0.1, 5e2]); -#+end_src - -#+begin_src matlab -figure; - -hold on; -plot(data.f, sqrt(flip(-cumtrapz(flip(data.f), flip(data.pxx_Dy))))) -plot(data_cart.f, sqrt(flip(-cumtrapz(flip(data_cart.f), flip(data_cart.pxx_Dy))))) -plot(data_cart_fixed.f, sqrt(flip(-cumtrapz(flip(data_cart_fixed.f), flip(data_cart_fixed.pxx_Dy))))) -plot(data_cf.f, sqrt(flip(-cumtrapz(flip(data_cf.f), flip(data_cf.pxx_Dy))))) -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1); -xlim([0.1, 5e2]); -#+end_src - -*** Robust Control - 6rpm -#+begin_src matlab -data_tomo_6rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-31_tomography_6rpm_m0.mat", mat_dir)); -data_tomo_6rpm_m0.time = Ts*[0:length(data_tomo_6rpm_m0.Rz)-1]; -#+end_src - -#+begin_src matlab -data_tomo_6rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-23_tomography_6rpm_m1.mat", mat_dir)); -data_tomo_6rpm_m1.time = Ts*[0:length(data_tomo_6rpm_m1.Rz)-1]; -#+end_src - -#+begin_src matlab -%% Compute RMS values while in closed-loop -[~, i_m0] = find(data_tomo_6rpm_m0.hac_status == 1); -data_tomo_6rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dx_int(i_m0+50000:end), 0)); -data_tomo_6rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dy_int(i_m0+50000:end), 0)); -data_tomo_6rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dz_int(i_m0+50000:end), 0)); -data_tomo_6rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Rx_int(i_m0+50000:end), 0)); -data_tomo_6rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m0.Ry_int(i_m0+50000:end), 0)); - -data_tomo_6rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dx_int(1:i_m0), 0)); -data_tomo_6rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dy_int(1:i_m0), 0)); -data_tomo_6rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dz_int(1:i_m0), 0)); -data_tomo_6rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Rx_int(1:i_m0), 0)); -data_tomo_6rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m0.Ry_int(1:i_m0), 0)); - -%% Compute RMS values while in closed-loop -[~, i_m1] = find(data_tomo_6rpm_m1.hac_status == 1); -data_tomo_6rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dx_int(i_m1+50000:end), 0)); -data_tomo_6rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dy_int(i_m1+50000:end), 0)); -data_tomo_6rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dz_int(i_m1+50000:end), 0)); -data_tomo_6rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Rx_int(i_m1+50000:end), 0)); -data_tomo_6rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m1.Ry_int(i_m1+50000:end), 0)); - -data_tomo_6rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dx_int(1:i_m1), 0)); -data_tomo_6rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dy_int(1:i_m1), 0)); -data_tomo_6rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dz_int(1:i_m1), 0)); -data_tomo_6rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Rx_int(1:i_m1), 0)); -data_tomo_6rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m1.Ry_int(1:i_m1), 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e6*data_tomo_6rpm_m0.Dx_rms_ol, 1e6*data_tomo_6rpm_m0.Dy_rms_ol, 1e9*data_tomo_6rpm_m0.Dz_rms_ol, 1e6*data_tomo_6rpm_m0.Rx_rms_ol, 1e6*data_tomo_6rpm_m0.Ry_rms_ol; ... - 1e6*data_tomo_6rpm_m1.Dx_rms_ol, 1e6*data_tomo_6rpm_m1.Dy_rms_ol, 1e9*data_tomo_6rpm_m1.Dz_rms_ol, 1e6*data_tomo_6rpm_m1.Rx_rms_ol, 1e6*data_tomo_6rpm_m1.Ry_rms_ol], ... - {'$m_0$', '$m_1$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_tomo_6rpm_robust_ol_errors -#+caption: Measured error during open-loop tomography scans (6rpm) -#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] | -|-------+-----------------+-----------------+--------------+-------------------------+-------------------------| -| $m_0$ | 8 | 7 | 20 | 41 | 41 | -| $m_1$ | 4 | 4 | 21 | 39 | 39 | - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ... - 1e9*data_tomo_6rpm_m1.Dx_rms_cl, 1e9*data_tomo_6rpm_m1.Dy_rms_cl, 1e9*data_tomo_6rpm_m1.Dz_rms_cl, 1e9*data_tomo_6rpm_m1.Rx_rms_cl, 1e9*data_tomo_6rpm_m1.Ry_rms_cl], ... - {'$m_0$', '$m_1$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_tomo_6rpm_robust_cl_errors -#+caption: Measured error during closed-loop tomography scans (6rpm, robust controller) -#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] | -|-------+------------+------------+------------+--------------+--------------| -| $m_0$ | 17 | 19 | 5 | 70 | 73 | -| $m_1$ | 20 | 26 | 7 | 110 | 77 | - -*** TODO Medium velocity tomography scans with CF control :noexport: -#+begin_src matlab -data_m1_cf = load(sprintf("%s/scans/2023-08-21_19-18_m1_6rpm_cf_control.mat", mat_dir)); -data_m1_cf.time = Ts*[0:length(data_m1_cf.Rz)-1]; -#+end_src - -#+begin_src matlab -data_m2_cf = load(sprintf("%s/scans/2023-08-21_18-07_m2_6rpm_cf_control.mat", mat_dir)); -data_m2_cf.time = Ts*[0:length(data_m2_cf.Rz)-1]; -#+end_src - -And higher bandwidth: -#+begin_src matlab -data_m1_cf_high_fb = load(sprintf("%s/scans/2023-08-21_19-24_m1_6rpm_cf_control_60Hz.mat", mat_dir)); -data_m1_cf_high_fb.time = Ts*[0:length(data_m1_cf_high_fb.Rz)-1]; -#+end_src - -#+begin_src matlab -figure; -hold on; -plot(data_m1_cf.Dy_int, detrend(data_m1_cf.Dz_int, 0)) -plot(data_m2_cf.Dy_int, detrend(data_m2_cf.Dz_int, 0)) -plot(data_m1_cf_high_fb.Dy_int, detrend(data_m1_cf_high_fb.Dz_int, 0)) -axis equal -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m1.Dz_int(i_m1+50000:end), 0)) -1e9*rms(detrend(data_m1.Dy_int(i_m1+50000:end), 0)) -1e9*rms(detrend(data_m1.Ry_int(i_m1+50000:end), 0)) -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m1_cf.Dz_int, 0)) -1e9*rms(detrend(data_m1_cf.Dy_int, 0)) -1e9*rms(detrend(data_m1_cf.Ry_int, 0)) -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m2.Dz_int, 0)) -1e9*rms(detrend(data_m2.Dy_int, 0)) -1e9*rms(detrend(data_m2.Ry_int, 0)) -#+end_src - -#+begin_src matlab -1e9*rms(detrend(data_m1_high_fb.Dz_int, 0)) -1e9*rms(detrend(data_m1_high_fb.Dy_int, 0)) -1e9*rms(detrend(data_m1_high_fb.Ry_int, 0)) -#+end_src - -*** Robust Control - 30rpm -#+begin_src matlab -%% Load Data -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]; -#+end_src - -#+begin_src matlab :exports none :results none -%% Measured motion during tomography scan at 30RPM with a robust controller -figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), ... - 'DisplayName', 'OL') -plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), ... - 'DisplayName', 'CL') -hold off; -axis square -xlabel("X motion [$\mu m$]"); -ylabel("Y motion [$\mu m$]"); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); -xlim([-3, 3]) -ylim([-3, 3]) - -ax2 = nexttile; -hold on; -plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 0), ... - 'DisplayName', 'OL') -plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 1), ... - 'DisplayName', 'CL') -hold off; -axis equal -xlim([-3, 3]) -ylim([-3, 3]) -xlabel("Y motion [$\mu m$]"); -ylabel("Z motion [$\mu m$]"); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_tomography_m0_30rpm_robust_xyz.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:id31_tomography_m0_30rpm_robust_xyz -#+caption: Measured motion during tomography scan at 30RPM with a robust controller -#+RESULTS: -[[file:figs/id31_tomography_m0_30rpm_robust_xyz.png]] - -#+begin_src matlab -%% Compute RMS values while in closed-loop -[~, i_m0] = find(data_tomo_30rpm_m0.hac_status == 1); -data_tomo_30rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dx_int(i_m0+50000:end), 0)); -data_tomo_30rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dy_int(i_m0+50000:end), 0)); -data_tomo_30rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dz_int(i_m0+50000:end), 0)); -data_tomo_30rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Rx_int(i_m0+50000:end), 0)); -data_tomo_30rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_30rpm_m0.Ry_int(i_m0+50000:end), 0)); - -data_tomo_30rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dx_int(1:i_m0), 0)); -data_tomo_30rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dy_int(1:i_m0), 0)); -data_tomo_30rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dz_int(1:i_m0), 0)); -data_tomo_30rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Rx_int(1:i_m0), 0)); -data_tomo_30rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_30rpm_m0.Ry_int(1:i_m0), 0)); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e6*data_tomo_30rpm_m0.Dx_rms_ol, 1e6*data_tomo_30rpm_m0.Dy_rms_ol, 1e9*data_tomo_30rpm_m0.Dz_rms_ol, 1e6*data_tomo_30rpm_m0.Rx_rms_ol, 1e6*data_tomo_30rpm_m0.Ry_rms_ol], ... - {'$m_0$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_tomo_30rpm_robust_ol_errors -#+caption: Measured error during open-loop tomography scans (30rpm) -#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] | -|-------+-----------------+-----------------+--------------+-------------------------+-------------------------| -| $m_0$ | 2 | 2 | 24 | 10 | 10 | - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ... - {'$m_0$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_tomo_30rpm_robust_cl_errors -#+caption: Measured error during closed-loop tomography scans (30rpm, robust controller) -#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] | -|-------+------------+------------+------------+--------------+--------------| -| $m_0$ | 34 | 38 | 10 | 127 | 129 | - -#+begin_src matlab :exports none -yztomography3dmovie('movies/id31_tomography_m0_30rpm_robust_xyz.avi', data_tomo_30rpm_m0, 'di', 300); -#+end_src - -#+begin_src matlab :exports none -yztomographymovie('movies/tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-3, 3], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-300, 300]) -#+end_src - -*** TODO Fast Tomography Scan with Complementary Filter Controller :noexport: -#+begin_src matlab -data_cf = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir)); -data_cf.time = Ts*[0:length(data_cf.Rz)-1]; -#+end_src - -#+begin_src matlab -[~, i0] = find(data.hac_status == 1); -1e9*rms(data.Dy_int(i0(1)+5000:end)) -1e9*rms(data.Dz_int(i0(1)+5000:end)) - -1e9*rms(data_cf.Dy_int) -1e9*rms(data_cf.Dz_int) -#+end_src - -Same performance than the robust controller in terms of RMS residual motion. - -#+begin_src matlab -figure; plot3(data.Dx_int, data.Dy_int, data.Dz_int) -#+end_src - -*** Tomography - Effect of the scanning velocity :noexport: -- [ ] What are the controller used here? Why worst results than with the robust controller? - -#+begin_src matlab -data_1rpm = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir)); -data_1rpm.time = Ts*[0:length(data_1rpm.Rz)-1]; -#+end_src - -#+begin_src matlab -data_30rpm = load(sprintf("%s/scans/2023-08-18_10-45_m0_30rpm.mat", mat_dir)); -data_30rpm.time = Ts*[0:length(data_30rpm.Rz)-1]; -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*rms(detrend(data_1rpm.Dy_int, 0)), 1e9*rms(detrend(data_30rpm.Dy_int, 0)); 1e9*rms(detrend(data_1rpm.Dz_int, 0)), 1e9*rms(detrend(data_30rpm.Dz_int, 0)); 1e9*rms(detrend(data_1rpm.Ry_int, 0)), 1e9*rms(detrend(data_30rpm.Ry_int, 0))]', {'1RPM', '30RPM'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.1f '); -#+end_src - -#+name: tab:id31_tomography_effect_velocity_rms -#+caption: RMS values of the errors during tomography scan - Effect of $R_z$ velocity -#+attr_latex: :environment tabularx :width 0.5\linewidth :align lXXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_y$ | $D_z$ | $R_y$ | -|-------+-------+-------+-------| -| 1RPM | 30.9 | 5.9 | 92.4 | -| 30RPM | 71.7 | 12.5 | 301.3 | - - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ... - 1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ... - 1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ... - {'1rpm', '6rpm', '30rpm'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f '); -#+end_src - -#+RESULTS: -| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] | -|-------+-----------------+-----------------+--------------+-------------------------+-------------------------| -| 1rpm | 13 | 15 | 5 | 57 | 55 | -| 6rpm | 17 | 19 | 5 | 70 | 73 | -| 30rpm | 34 | 38 | 10 | 127 | 129 | - -** $D_z$ scans: Dirty Layer Scans -<> -*** Step by Step $D_z$ motion -#+begin_src matlab -%% Load Dz MIM data -data_dz_steps_3nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_3_nm.mat", mat_dir)); -data_dz_steps_3nm.time = Ts*[0:length(data_dz_steps_3nm.Dz_int)-1]; - -data_dz_steps_10nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_10_nm.mat", mat_dir)); -data_dz_steps_10nm.time = Ts*[0:length(data_dz_steps_10nm.Dz_int)-1]; - -data_dz_steps_100nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_100_nm.mat", mat_dir)); -data_dz_steps_100nm.time = Ts*[0:length(data_dz_steps_100nm.Dz_int)-1]; - -data_dz_steps_1000nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_1000_nm.mat", mat_dir)); -data_dz_steps_1000nm.time = Ts*[0:length(data_dz_steps_1000nm.Dz_int)-1]; -#+end_src - -Three step sizes are tested: -- $10\,nm$ steps (Figure ref:fig:id31_dz_mim_10nm_steps) -- $100\,nm$ steps (Figure ref:fig:id31_dz_mim_100nm_steps) -- $1\,\mu m$ steps (Figure ref:fig:id31_dz_steps_response) - -#+begin_src matlab :exports none :results none -%% Dz MIM test with 10nm steps -figure; -hold on; -plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.Dz_int - mean(data_dz_steps_10nm.Dz_int(1:1000)))) -hold off; -xlabel('Time [s]'); -ylabel('$D_z$ Motion [nm]'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_dz_mim_10nm_steps.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_dz_mim_10nm_steps -#+caption: Dz MIM test with 10nm steps (low pass filter with cut-off frequency of 10Hz is applied) -#+RESULTS: -[[file:figs/id31_dz_mim_10nm_steps.png]] - -#+begin_src matlab :exports none :results none -%% Dz MIM test with 10nm steps -figure; -hold on; -plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.Dz_int - mean(data_dz_steps_100nm.Dz_int(1:1000)))) -hold off; -xlabel('Time [s]'); -ylabel('$D_z$ Motion [nm]'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_dz_mim_100nm_steps.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_dz_mim_100nm_steps -#+caption: Dz MIM test with 100nm steps -#+RESULTS: -[[file:figs/id31_dz_mim_100nm_steps.png]] - -#+begin_src matlab :exports none :results none -%% Dz step response - Stabilization time is around 70ms -figure; -[~, i] = find(data_dz_steps_1000nm.m_hexa_dz>data_dz_steps_1000nm.m_hexa_dz(1)); -i0 = i(1); - -figure; -hold on; -plot(data_dz_steps_1000nm.time-data_dz_steps_1000nm.time(i0), 1e9*(data_dz_steps_1000nm.Dz_int - mean(data_dz_steps_1000nm.Dz_int(1:1000)))) -plot([-1, 1], [1000-20, 1000-20], 'k--') -plot([-1, 1], [1000+20, 1000+20], 'k--') -xline(0, 'k--', 'LineWidth', 1.5) -xline(0.07, 'k--', 'LineWidth', 1.5) -hold off; -xlabel('Time [s]'); -ylabel('$D_z$ Motion [nm]'); -xlim([-0.1, 0.2]); -ylim([-100, 1600]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_dz_steps_response.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_dz_steps_response -#+caption: $D_z$ step response - Stabilization time is around 70ms -#+RESULTS: -[[file:figs/id31_dz_steps_response.png]] - - -*** Continuous $D_z$ motion: Dirty Layer 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 - -Two $D_z$ scans are performed: -- at $10\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0 -- at $100\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0_large - -#+begin_src matlab :exports none :results none -%% Dirty layer scan: Dz motion -figure; -hold on; -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.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.e_ry, ... - 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry))) -hold off; -xlabel('Time [s]'); -ylabel('Motion [$\mu$m]'); -xlim([0, 2.2]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_dirty_layer_scan_m0.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_dirty_layer_scan_m0 -#+caption: Dirty layer scan: $D_z$ motion at $10\,\mu m/s$ -#+RESULTS: -[[file:figs/id31_dirty_layer_scan_m0.png]] - -#+begin_src matlab :exports none :results none -%% Dirty layer scan: Dz motion -figure; -hold on; -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.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.e_ry, ... - 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry))) -hold off; -xlabel('Time [s]'); -ylabel('Motion [$\mu$m]'); -xlim([0, 2.2]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_dirty_layer_scan_m0_large.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_dirty_layer_scan_m0_large -#+caption: Dirty layer scan: $D_z$ motion at $100\,\mu m/s$ -#+RESULTS: -[[file:figs/id31_dirty_layer_scan_m0_large.png]] - -#+begin_src matlab -%% Not so good results with the CF controller -data_cf = load(sprintf("%s/scans/2023-08-21_19-20_dirty_layer_m1_cf.mat", mat_dir)); -data_cf.time = Ts*[0:length(data_cf.Dz_int)-1]; -#+end_src - -** $R_y$ scans: Reflectivity -<> -#+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 - -An $R_y$ scan is performed at $100\,\mu rad/s$ velocity (Figure ref:fig:id31_reflectivity_scan_Ry_100urads). -During the $R_y$ scan, the errors in $D_y$ are $D_z$ are kept small. - -#+begin_src matlab :exports none :results none -%% Ry reflectivity scan -figure; -hold on; -plot(data_ry.time, 1e6*data_ry.Ry_int, 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry))) -plot(data_ry.time, 1e6*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy))) -plot(data_ry.time, 1e6*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz))) -hold off; -xlabel('Time [s]'); -ylabel('$R_y$ motion [$\mu$rad]') -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -xlim([0, 6.2]); -ylim([-310, 310]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_reflectivity_scan_Ry_100urads.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_reflectivity_scan_Ry_100urads -#+caption: $R_y$ reflecitivity scan at $100\,\mu\text{rad}/s$ velocity -#+RESULTS: -[[file:figs/id31_reflectivity_scan_Ry_100urads.png]] - - -** $D_y$ Scans -<> -*** Introduction :ignore: -The steps generated by the IcePAP for the $T_y$ stage are send to the Speedgoat. -Then, we can know in real time what is the wanted position in $D_y$ during $T_y$ scans. - -*** Open Loop -#+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]; -#+end_src - -We can clearly see micro-stepping errors of the stepper motor used for the $T_y$ stage. -The errors have a period of $10\,\mu m$ with an amplitude of $\pm 100\,nm$. - -#+begin_src matlab :exports none :results none -%% Ty scan (at 10um/s) - Dy errors -figure; -plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0)) -xlabel('Ty position [$\mu$m]'); -ylabel('$D_y$ error [$\mu$m]'); -xlim([-100, 100]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_ty_scan_10ums_ol_dy_errors.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_ty_scan_10ums_ol_dy_errors -#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. The micro-stepping errors can clearly be seen with a period of $10\,\mu m$ and an amplitude of $\pm 100\,nm$ -#+RESULTS: -[[file:figs/id31_ty_scan_10ums_ol_dy_errors.png]] - -#+begin_src matlab :exports none :results none -%% Ty scan (at 10um/s) - Dz and Ry errors -figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0)) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$D_z$ error [$\mu$m]'); -xlim([-100, 100]) - -ax2 = nexttile; -hold on; -plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$R_y$ error [$\mu$rad]'); -xlim([-100, 100]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_ty_scan_10ums_ol_dz_ry_errors -#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. The $D_z$ error is most likely due to having the top interferometer pointing to a sphere. The large $R_y$ errors might also be due to the metrology system -#+RESULTS: -[[file:figs/id31_ty_scan_10ums_ol_dz_ry_errors.png]] - -*** Closed Loop -#+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 :exports none :results none -%% Ty scan (at 10um/s) - Dy errors -figure; -hold on; -plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0), ... - 'DisplayName', 'OL') -plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dy, 0), ... - 'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dy, 0)))) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$D_y$ error [$\mu$m]'); -xlim([-100, 100]) -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_ty_scan_10ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_ty_scan_10ums_cl_dy_errors -#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans -#+RESULTS: -[[file:figs/id31_ty_scan_10ums_cl_dy_errors.png]] - -#+begin_src matlab :exports none :results none -%% Ty scan (at 10um/s) - Dz and Ry errors -figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0), ... - 'DisplayName', 'OL') -plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dz, 0), ... - 'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0)))) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$D_z$ error [$\mu$m]'); -xlim([-100, 100]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry, ... - 'DisplayName', 'OL') -plot(1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_ry, ... - 'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_slow.e_ry, 0)))) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$R_y$ error [$\mu$rad]'); -xlim([-100, 100]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_ty_scan_10ums_cl_dz_ry_errors -#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans -#+RESULTS: -[[file:figs/id31_ty_scan_10ums_cl_dz_ry_errors.png]] - -*** Faster Scan -#+begin_src matlab -%% Fast Ty scan (100um/s) - OL -data_ty_ol_fast = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop.mat", mat_dir)); -data_ty_ol_fast.time = Ts*[0:length(data_ty_ol_fast.Dy_int)-1]; -#+end_src - -#+begin_src matlab -%% Fast Ty scan (10um/s) - CL -data_ty_cl_fast = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop.mat", mat_dir)); -data_ty_cl_fast.time = Ts*[0:length(data_ty_cl_fast.Dy_int)-1]; -#+end_src - -Because of micro-stepping errors of the Ty stepper motor, when scanning at high velocity this induce high frequency vibration that are outside the bandwidth of the feedback controller. - -At $100\,\mu m/s$, the micro-stepping errors with a period of $10\,\mu m$ (see Figure ref:fig:id31_ty_scan_10ums_ol_dy_errors) are at 10Hz. -These errors are them amplified by some resonances in the system. - -This could be easily solved by changing the stepper motor for a torque motor for instance. - -#+begin_src matlab :exports none :results none -%% Ty scan (at 100um/s) - Dy errors -figure; -hold on; -plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dy, 0), ... - 'DisplayName', 'OL') -plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dy, 0), ... - 'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dy, 0)))) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$D_y$ error [$\mu$m]'); -xlim([-100, 100]) -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_ty_scan_100ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_ty_scan_100ums_cl_dy_errors -#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans -#+RESULTS: -[[file:figs/id31_ty_scan_100ums_cl_dy_errors.png]] - -#+begin_src matlab :exports none :results none -%% Ty scan (at 100um/s) - Dz and Ry errors -figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dz, 0), ... - 'DisplayName', 'OL') -plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dz, 0), ... - 'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dz, 0)))) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$D_z$ error [$\mu$m]'); -xlim([-100, 100]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); - -ax2 = nexttile; -hold on; -plot(1e6*data_ty_ol_fast.Ty, 1e6*data_ty_ol_fast.e_ry, ... - 'DisplayName', 'OL') -plot(1e6*data_ty_cl_fast.Ty, 1e6*data_ty_cl_fast.e_ry, ... - 'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_fast.e_ry, 0)))) -hold off; -xlabel('Ty position [$\mu$m]'); -ylabel('$R_y$ error [$\mu$rad]'); -xlim([-100, 100]) -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_ty_scan_100ums_cl_dz_ry_errors -#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans -#+RESULTS: -[[file:figs/id31_ty_scan_100ums_cl_dz_ry_errors.png]] - -** Combined $R_z$ and $D_y$: Diffraction Tomography -<> -Instead of doing a fast $R_z$ motion a slow $D_y$, the idea is to perform slow $R_z$ (here 1rpm) and fast $D_y$ scans with the nano-hexapod. - -#+begin_src matlab -%% 100um/s - Robust controller -data_dt_100ums = load(sprintf("%s/scans/2023-08-18_17-12_diffraction_tomo_m0.mat", mat_dir)); -data_dt_100ums.time = Ts*[0:length(data_dt_100ums.Dy_int)-1]; - -%% 500um/s - Robust controller (Not used) -% data_dt_500ums = load(sprintf("%s/scans/2023-08-18_17-19_diffraction_tomo_m0_fast.mat", mat_dir)); -% data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1]; - -%% 500um/s - Complementary filters -data_dt_500ums = load(sprintf("%s/scans/2023-08-21_15-15_diffraction_tomo_m0_fast_cf.mat", mat_dir)); -data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1]; - -%% 1mm/s - Complementary filters -data_dt_1000ums = load(sprintf("%s/scans/2023-08-21_15-16_diffraction_tomo_m0_fast_cf.mat", mat_dir)); -data_dt_1000ums.time = Ts*[0:length(data_dt_1000ums.Dy_int)-1]; - -%% 5mm/s - Complementary filters -% data_dt_5000ums = load(sprintf("%s/scans/2023-08-21_18-03_diffraction_tomo_m2_fast_cf.mat", mat_dir)); -% data_dt_5000ums.time = Ts*[0:length(data_dt_5000ums.Dy_int)-1]; - -%% 10mm/s - Complementary filters -data_dt_10000ums = load(sprintf("%s/scans/2023-08-21_15-17_diffraction_tomo_m0_fast_cf.mat", mat_dir)); -data_dt_10000ums.time = Ts*[0:length(data_dt_10000ums.Dy_int)-1]; -#+end_src - -Here, the $D_y$ scans are performed only with the nano-hexapod (the Ty stage is not moving), so we are limited to $\pm 100\,\mu m$. - -Several $D_y$ velocities are tested: $0.1\,mm/s$, $0.5\,mm/s$, $1\,mm/s$ and $10\,mm/s$ (see Figure ref:fig:id31_diffraction_tomo_velocities). - -#+begin_src matlab :exports none :results none -%% Dy motion for several configured velocities -figure; -hold on; -plot(data_dt_10000ums.time, 1e6*data_dt_10000ums.Dy_int, ... - 'DisplayName', '$10 mm/s$') -plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ... - 'DisplayName', '$1 mm/s$') -plot(data_dt_500ums.time, 1e6*data_dt_500ums.Dy_int, ... - 'DisplayName', '$0.5 mm/s$') -plot(data_dt_100ums.time, 1e6*data_dt_100ums.Dy_int, ... - 'DisplayName', '$0.1 mm/s$') -hold off; -xlim([0, 4]); -xlabel('Time [s]'); -ylabel('$D_y$ position [$\mu$m]') -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_diffraction_tomo_velocities.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:id31_diffraction_tomo_velocities -#+caption: Dy motion for several configured velocities -#+RESULTS: -[[file:figs/id31_diffraction_tomo_velocities.png]] - -The corresponding "repetition rate" and $D_y$ scan per spindle turn are shown in Table ref:tab:diffraction_tomo_velocities. - -The main issue here is the "waiting" time between two scans that is in the order of 50ms. -By removing this waiting time (fairly easily), we can double the repetition rate at 10mm/s. - -#+name: tab:diffraction_tomo_velocities -#+caption: $D_y$ scaning repetition rate -#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXX -#+attr_latex: :center t :booktabs t -| $D_y$ Velocity | Repetition rate | Scans per turn (at 1RPM) | -|----------------+-----------------+--------------------------| -| 0.1 mm/s | 4 s | 15 | -| 0.5 mm/s | 0.9 s | 65 | -| 1 mm/s | 0.5 s | 120 | -| 10 mm/s | 0.18 s | 330 | - - -The scan results for a velocity of 1mm/s is shown in Figure ref:fig:id31_diffraction_tomo_1mms. -The $D_z$ and $R_y$ errors are quite small during the scan. - -The $D_y$ errors are quite large as the velocity is increased. -This type of scan can probably be massively improved by using feed-forward and optimizing the trajectory. -Also, if the detectors are triggered in position (the Speedgoat could generate an encoder signal for instance), we don't care about the $D_y$ errors. - -#+begin_src matlab :exports none :results none -%% Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM -figure; -hold on; -plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dz_int, ... - 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dz_int))) -plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Ry_int, ... - 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$radRMS', 1e6*rms(data_dt_1000ums.Ry_int))) -plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ... - 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy))) -plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.m_hexa_dy, 'k--', 'HandleVisibility', 'off') -hold off; -xlim([0, 1]) -xlabel('Time [s]'); -ylabel('Measurement [nm,nrad]') -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1); -ylim([-110, 110]) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/id31_diffraction_tomo_1mms.pdf', 'width', 'full', 'height', 'normal'); -#+end_src - -#+name: fig:id31_diffraction_tomo_1mms -#+caption: Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM -#+RESULTS: -[[file:figs/id31_diffraction_tomo_1mms.png]] - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable([1e9*rms(data_dt_100ums.Dy_int - data_dt_100ums.m_hexa_dy), 1e9*rms(data_dt_500ums.Dy_int - data_dt_500ums.m_hexa_dy), 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy), 1e9*rms(data_dt_10000ums.Dy_int - data_dt_10000ums.m_hexa_dy); -1e9*rms(data_dt_100ums.Dz_int), 1e9*rms(data_dt_500ums.Dz_int), 1e9*rms(data_dt_1000ums.Dz_int), 1e9*rms(data_dt_10000ums.Dz_int); -1e6*rms(data_dt_100ums.Ry_int), 1e6*rms(data_dt_500ums.Ry_int), 1e6*rms(data_dt_1000ums.Ry_int), 1e6*rms(data_dt_10000ums.Ry_int)]', {'0.1 mm/s' ,'0.5 mm/s', '1 mm/s', '10 mm/s'}, {'Velocity', '$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [$\mu\text{radRMS}$]'}, ' %.1f '); -#+end_src - -#+name: tab:id31_diffraction_tomo_results -#+caption: Obtained errors for several $D_y$ velocities -#+attr_latex: :environment tabularx :width \linewidth :align lXX -#+attr_latex: :center t :booktabs t -#+RESULTS: -| Velocity | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [$\mu\text{radRMS}$] | -|----------+---------------+---------------+----------------------------| -| 0.1 mm/s | 75.5 | 9.1 | 0.1 | -| 0.5 mm/s | 190.5 | 10.0 | 0.1 | -| 1 mm/s | 428.0 | 11.2 | 0.2 | -| 10 mm/s | 4639.9 | 55.9 | 1.4 | - -** Summary of experiments -For each conducted experiments, the $D_y$, $D_z$ and $R_y$ errors are computed and summarized in Table ref:tab:id31_experiments_results_summary. - -#+begin_src matlab -%% Summary of results -data_results = [... - 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl ; ... % Tomo 1rpm - 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl ; ... % Tomo 6rpm - 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl ; ... % Tomo 30rpm - 1e9*rms(detrend(data_dz_10ums.e_dy, 0)), 1e9*rms(detrend(data_dz_10ums.e_dz, 0)), 1e9*rms(detrend(data_dz_10ums.e_ry, 0)) ; ... % Dz 10um/s - 1e9*rms(detrend(data_dz_100ums.e_dy,0)), 1e9*rms(detrend(data_dz_100ums.e_dz,0)), 1e9*rms(detrend(data_dz_100ums.e_ry,0)) ; ... % Dz 100um/s - 1e9*rms(detrend(data_ry.e_dy,0)), 1e9*rms(detrend(data_ry.e_dz,0)), 1e9*rms(detrend(data_ry.e_ry,0)) ; ... % Ry 100urad/s - 1e9*rms(detrend(data_ty_cl_slow.e_dy, 0)), 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0)), 1e9*rms(detrend(data_ty_cl_slow.e_rz, 0)) ; ... % Dy 10 um/s - 1e9*rms(detrend(data_dt_100ums.Dy_int-data_dt_100ums.m_hexa_dy, 0)), 1e9*rms(detrend(data_dt_100ums.Dz_int, 0)), 1e9*rms(detrend(data_dt_100ums.Ry_int, 0)); ... % Diffraction tomo 0.1mm/s - 1e9*rms(detrend(data_dt_1000ums.Dy_int-data_dt_1000ums.m_hexa_dy,0)), 1e9*rms(detrend(data_dt_1000ums.Dz_int,0)), 1e9*rms(detrend(data_dt_1000ums.Ry_int,0)) ... % Diffraction tomo 1mm/s -]; -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm)', 'Tomography ($R_z$ 30rpm)', 'Dirty Layer ($D_z$ $10\,\mu m/s$)', 'Dirty Layer ($D_z$ $100\,\mu m/s$)', 'Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)', 'Lateral Scan ($D_y$ $10\,\mu m/s$)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)'}, {'$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [nradRMS]'}, ' %.0f '); -#+end_src - -#+name: tab:id31_experiments_results_summary -#+caption: Table caption -#+attr_latex: :environment tabularx :width \linewidth :align Xccc -#+attr_latex: :center t :booktabs t -#+RESULTS: -| | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [nradRMS] | -|----------------------------------------------------+---------------+---------------+-----------------| -| Tomography ($R_z$ 1rpm) | 15 | 5 | 55 | -| Tomography ($R_z$ 6rpm) | 19 | 5 | 73 | -| Tomography ($R_z$ 30rpm) | 38 | 10 | 129 | -| Dirty Layer ($D_z$ $10\,\mu m/s$) | 25 | 5 | 114 | -| Dirty Layer ($D_z$ $100\,\mu m/s$) | 34 | 15 | 130 | -| Reflectivity ($R_y$ $100\,\mu\text{rad}/s$) | 28 | 6 | 118 | -| Lateral Scan ($D_y$ $10\,\mu m/s$) | 21 | 10 | 37 | -| 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 | - -* 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: -:header-args:matlab+: :tangle matlab/src/unwrapphase.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: - -#+begin_src matlab -function [unwraped_phase] = unwrapphase(frf, f, args) -#+end_src - -#+begin_src matlab -arguments - frf - f - args.f0 (1,1) double {mustBeNumeric} = 1 -end -#+end_src - -#+begin_src matlab -unwraped_phase = unwrap(frf); -[~,i] = min(abs(f - args.f0)); -unwraped_phase = unwraped_phase - 2*pi*round(unwraped_phase(i)./(2*pi)); -#+end_src - -** =circlefit= -:PROPERTIES: -:header-args:matlab+: :tangle matlab/src/circlefit.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -#+begin_src matlab -function [xc,yc,R,a] = circlefit(x,y) -% -% [xc yx R] = circfit(x,y) -% -% fits a circle in x,y plane in a more accurate -% (less prone to ill condition ) -% procedure than circfit2 but using more memory -% x,y are column vector where (x(i),y(i)) is a measured point -% -% result is center point (yc,xc) and radius R -% an optional output is the vector of coeficient a -% describing the circle's equation -% -% x^2+y^2+a(1)*x+a(2)*y+a(3)=0 -% -% By: Izhak bucher 25/oct /1991, - x=x(:); y=y(:); - a=[x y ones(size(x))]\[-(x.^2+y.^2)]; - xc = -.5*a(1); - yc = -.5*a(2); - R = sqrt((a(1)^2+a(2)^2)/4-a(3)); -#+end_src - -** =yztomography3dmovie= -:PROPERTIES: -:header-args:matlab+: :tangle matlab/src/yztomography3dmovie.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -#+begin_src matlab -function [] = yztomography3dmovie(filename, data, args) -#+end_src - -#+begin_src matlab - arguments - filename - data - args.di (1,1) double {mustBeNumeric} = 500 - args.xlim (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim (2,1) double {mustBeNumeric} = [-3, 3] - args.zlim (2,1) double {mustBeNumeric} = [-0.4, 0.4] - args.view_az (1,1) double {mustBeNumeric} = -70 - args.view_el (1,1) double {mustBeNumeric} = 5 - end -#+end_src - -#+begin_src matlab -colors = colororder; -#+end_src - -#+begin_src matlab -writerObj = VideoWriter(filename); % initialize the VideoWriter object -open(writerObj); -#+end_src - -#+begin_src matlab -fig = figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -#+end_src - -#+begin_src matlab -di = args.di; -for i = 1:floor(length(data.Dx_int)/di)-1 - if data.hac_status(di*(i+1)-1) == 0 - % Only open loop - plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:)) - elseif data.hac_status(di*i) == 1 - % Only closed loop - plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:)) - else - % Both open and closed loop - Dx_int = data.Dx_int(di*i:di*(i+1)-1); - Dy_int = data.Dy_int(di*i:di*(i+1)-1); - Dz_int = data.Dz_int(di*i:di*(i+1)-1); - plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:)) - plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:)) - end - axis(ax1, 'equal') - xlim(ax1, args.xlim) - ylim(ax1, args.ylim) - zlim(ax1, args.zlim) - view(ax1, args.view_az, args.view_el) - xlabel(ax1, "X motion [$\mu$m]"); - ylabel(ax1, "Y motion [$\mu$m]"); - zlabel(ax1, "Z motion [$\mu$m]"); - drawnow() - writeVideo(writerObj,getframe(fig)) % add the frame to the movie -end -close(writerObj); -#+end_src - -** =yztomographymovie= -:PROPERTIES: -:header-args:matlab+: :tangle matlab/src/yztomographymovie.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -#+begin_src matlab -function [] = yztomographymovie(filename, data, args) -#+end_src - -#+begin_src matlab - arguments - filename - data - args.di (1,1) double {mustBeNumeric} = 500 - args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3] - end -#+end_src - -#+begin_src matlab -colors = colororder; -#+end_src - -#+begin_src matlab -writerObj = VideoWriter(filename); % initialize the VideoWriter object -open(writerObj); -#+end_src - -#+begin_src matlab -fig = figure; -tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -ax2 = nexttile; -#+end_src - -#+begin_src matlab -di = args.di; -for i = 1:floor(length(data.Dx_int)/di)-1 - if data.hac_status(di*(i+1)-1) == 0 - % Only open loop - plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:)) - plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:)) - elseif data.hac_status(di*i) == 1 - % Only closed loop - plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:)) - plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:)) - else - % Both open and closed loop - Dy_int = data.Dy_int(di*i:di*(i+1)-1); - Dz_int = data.Dz_int(di*i:di*(i+1)-1); - plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:)) - plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:)) - plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:)) - plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:)) - end - axis(ax1, 'square') - axis(ax2, 'square') - xlim(ax1, args.xlim_ax1) - ylim(ax1, args.ylim_ax1) - xlim(ax2, args.xlim_ax2) - ylim(ax2, args.ylim_ax2) - xlabel(ax1, "Y motion [$\mu m$]"); - ylabel(ax1, "Z motion [$\mu m$]"); - xlabel(ax2, "Y motion [$nm$]"); - ylabel(ax2, "Z motion [$nm$]"); - F = getframe(fig); %// Capture the frame - writeVideo(writerObj,F) %// add the frame to the movie -end -close(writerObj); -#+end_src - -** =yztomographymoviesri= -:PROPERTIES: -:header-args:matlab+: :tangle matlab/src/yztomographymoviesri.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -#+begin_src matlab -function [] = yztomographymoviesri(filename, data, args) -#+end_src - -#+begin_src matlab - arguments - filename - data - args.di (1,1) double {mustBeNumeric} = 500 - args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - end -#+end_src - -#+begin_src matlab -colors = colororder; -#+end_src - -#+begin_src matlab -writerObj = VideoWriter(filename); % initialize the VideoWriter object -open(writerObj); -#+end_src - -#+begin_src matlab -fig = figure; -tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -#+end_src - -#+begin_src matlab -di = args.di; -for i = 1:floor(length(data.Dx_int)/di)-1 - if data.hac_status(di*(i+1)-1) == 0 - % Only open loop - plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:)) - elseif data.hac_status(di*i) == 1 - % Only closed loop - plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:)) - else - % Both open and closed loop - Dy_int = data.Dy_int(di*i:di*(i+1)-1); - Dz_int = data.Dz_int(di*i:di*(i+1)-1); - plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:)) - plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:)) - end - axis(ax1, 'square') - xlim(ax1, args.xlim_ax1) - ylim(ax1, args.ylim_ax1) - xlabel(ax1, "Y motion [$\mu m$]"); - ylabel(ax1, "Z motion [$\mu m$]"); - F = getframe(fig); %// Capture the frame - writeVideo(writerObj,F) %// add the frame to the movie -end -close(writerObj); -#+end_src - -** =dyscanmoviesri= -:PROPERTIES: -:header-args:matlab+: :tangle matlab/src/dyscanmoviesri.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -#+begin_src matlab -function [] = dyscanmoviesri(filename, data, args) -#+end_src - -#+begin_src matlab - arguments - filename - data - args.di (1,1) double {mustBeNumeric} = 500 - args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3] - end -#+end_src - -#+begin_src matlab -colors = colororder; -#+end_src - -#+begin_src matlab -writerObj = VideoWriter(filename); % initialize the VideoWriter object -open(writerObj); -#+end_src - -#+begin_src matlab -fig = figure; -tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -ax2 = nexttile; -hold([ax1,ax2], 'on'); -plot(ax1, 1e6*data.Ty, 1e6*data.e_dy, '-', 'color', [colors(1,:), 0.2],'LineWidth',1); -plot(ax2, 1e6*data.Ty, 1e6*data.e_dz, '-', 'color', [colors(1,:), 0.2],'LineWidth',1); -#+end_src - -#+begin_src matlab -di = args.di; -for i = 1:floor(length(data.e_dy)/di)-1 - plt1 = plot(ax1, 1e6*data.Ty(di*i:di*(i+1)-1), 1e6*data.e_dy(di*i:di*(i+1)-1), '-', 'color', colors(1,:)); - plt2 = plot(ax2, 1e6*data.Ty(di*i:di*(i+1)-1), 1e6*data.e_dz(di*i:di*(i+1)-1), '-', 'color', colors(1,:)); - % axis(ax1, 'square') - xlim(ax1, args.xlim_ax1); - ylim(ax1, args.ylim_ax1); - xlim(ax2, args.xlim_ax2); - ylim(ax2, args.ylim_ax2); - set(ax1, 'XTickLabel',[]); - ylabel(ax1, "$D_y$ error [$\mu m$]"); - xlabel(ax2, "$D_y$ setpoint [$\mu m$]"); - ylabel(ax2, "Z motion [$\mu m$]"); - F = getframe(fig); %// Capture the frame - writeVideo(writerObj,F) %// add the frame to the movie - delete(plt1); - delete(plt2); -end -hold([ax1,ax2], 'off'); -close(writerObj); -#+end_src - -** =dyscanclmoviesri= -:PROPERTIES: -:header-args:matlab+: :tangle matlab/src/dyscanclmoviesri.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -#+begin_src matlab -function [] = dyscanclmoviesri(filename, data_ol, data_cl, args) -#+end_src - -#+begin_src matlab - arguments - filename - data_ol - data_cl - args.di (1,1) double {mustBeNumeric} = 500 - args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3] - args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3] - args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3] - end -#+end_src - -#+begin_src matlab -colors = colororder; -#+end_src - -#+begin_src matlab -writerObj = VideoWriter(filename); % initialize the VideoWriter object -open(writerObj); -#+end_src - -#+begin_src matlab -fig = figure; -tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None'); - -ax1 = nexttile; -ax2 = nexttile; -hold([ax1,ax2], 'on'); -plot(ax1, 1e6*data_ol.Ty, 1e6*data_ol.e_dy, '-', 'color', [colors(1,:), 0.2],'LineWidth',1); -plot(ax2, 1e6*data_ol.Ty, 1e6*data_ol.e_dz, '-', 'color', [colors(1,:), 0.2],'LineWidth',1); -plot(ax1, 1e6*data_cl.Ty, 1e6*data_cl.e_dy, '-', 'color', [colors(2,:), 0.2],'LineWidth',1); -plot(ax2, 1e6*data_cl.Ty, 1e6*data_cl.e_dz, '-', 'color', [colors(2,:), 0.2],'LineWidth',1); -#+end_src - -#+begin_src matlab -di = args.di; -for i = 1:floor(length(data_cl.e_dy)/di)-1 - plt1 = plot(ax1, 1e6*data_cl.Ty(di*i:di*(i+1)-1), 1e6*data_cl.e_dy(di*i:di*(i+1)-1), '-', 'color', colors(2,:)); - plt2 = plot(ax2, 1e6*data_cl.Ty(di*i:di*(i+1)-1), 1e6*data_cl.e_dz(di*i:di*(i+1)-1), '-', 'color', colors(2,:)); - % axis(ax1, 'square') - xlim(ax1, args.xlim_ax1); - ylim(ax1, args.ylim_ax1); - xlim(ax2, args.xlim_ax2); - ylim(ax2, args.ylim_ax2); - set(ax1, 'XTickLabel',[]); - ylabel(ax1, "$D_y$ error [$\mu m$]"); - xlabel(ax2, "$D_y$ setpoint [$\mu m$]"); - ylabel(ax2, "Z motion [$\mu m$]"); - F = getframe(fig); %// Capture the frame - writeVideo(writerObj,F) %// add the frame to the movie - delete(plt1); - delete(plt2); -end -hold([ax1,ax2], 'off'); -close(writerObj); -#+end_src - -** Simscape Configuration -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeSimscapeConfiguration.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = initializeSimscapeConfiguration(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.gravity logical {mustBeNumericOrLogical} = true - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - conf_simscape = struct(); -#+end_src - -*** Add Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - if args.gravity - conf_simscape.type = 1; - else - conf_simscape.type = 2; - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - save('./mat/conf_simscape.mat', 'conf_simscape'); -#+end_src - -** Logging Configuration -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeLoggingConfiguration.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = initializeLoggingConfiguration(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none' - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - conf_log = struct(); -#+end_src - -*** Add Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.log - case 'none' - conf_log.type = 0; - case 'all' - conf_log.type = 1; - case 'forces' - conf_log.type = 2; - end -#+end_src - -*** Sampling Time -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - conf_log.Ts = args.Ts; -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - save('./mat/conf_log.mat', 'conf_log'); -#+end_src - -** Ground -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeGround.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Simscape Model -:PROPERTIES: -:UNNUMBERED: t -:END: - -The model of the Ground is composed of: -- A *Cartesian* joint that is used to simulation the ground motion -- A solid that represents the ground on which the granite is located - -#+name: fig:simscape_model_ground -#+attr_org: :width 800 -#+caption: Simscape model for the Ground -[[file:figs/images/simscape_model_ground.png]] - -#+name: fig:simscape_picture_ground -#+attr_org: :width 800 -#+caption: Simscape picture for the Ground -[[file:figs/images/simscape_picture_ground.png]] - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [ground] = initializeGround(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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 -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =granite= structure. -#+begin_src matlab - ground = struct(); -#+end_src - -*** Add Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case 'none' - ground.type = 0; - case 'rigid' - ground.type = 1; - end -#+end_src - -*** Ground Solid properties -:PROPERTIES: -:UNNUMBERED: t -:END: -We set the shape and density of the ground solid element. -#+begin_src matlab - ground.shape = [2, 2, 0.5]; % [m] - ground.density = 2800; % [kg/m3] -#+end_src - -*** Rotation Point -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - ground.rot_point = args.rot_point; -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =ground= structure is saved. -#+begin_src matlab - save('./mat/stages.mat', 'ground', '-append'); -#+end_src - -** Granite -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeGranite.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Simscape Model -:PROPERTIES: -:UNNUMBERED: t -:END: - -The Simscape model of the granite is composed of: -- A cartesian joint such that the granite can vibrations along x, y and z axis -- A solid - -The output =sample_pos= corresponds to the impact point of the X-ray. - -#+name: fig:simscape_model_granite -#+attr_org: :width 800 -#+caption: Simscape model for the Granite -[[file:figs/images/simscape_model_granite.png]] - -#+name: fig:simscape_picture_granite -#+attr_org: :width 800 -#+caption: Simscape picture for the Granite -[[file:figs/images/simscape_picture_granite.png]] - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [granite] = initializeGranite(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false - args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3] - args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m] - args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [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.8-25e-3 % Height of the measurment point [m] - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =granite= structure. -#+begin_src matlab - granite = struct(); -#+end_src - -*** Add Granite Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case 'none' - granite.type = 0; - case 'rigid' - granite.type = 1; - case 'flexible' - granite.type = 2; - case 'modal-analysis' - granite.type = 3; - case 'init' - granite.type = 4; - end -#+end_src - -*** Material and Geometry -:PROPERTIES: -:UNNUMBERED: t -:END: - -Properties of the Material and link to the geometry of the granite. -#+begin_src matlab - granite.density = args.density; % [kg/m3] - granite.STEP = './STEPS/granite/granite.STEP'; -#+end_src - -Z-offset for the initial position of the sample with respect to the granite top surface. -#+begin_src matlab - granite.sample_pos = args.sample_pos; % [m] -#+end_src - -*** Stiffness and Damping properties -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - granite.K = args.K; % [N/m] - granite.C = args.C; % [N/(m/s)] -#+end_src - -*** Equilibrium position of the each joint. -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fgm'); - granite.Deq = -Fgm'./granite.K; - else - granite.Deq = zeros(6,1); - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =granite= structure is saved. -#+begin_src matlab - save('./mat/stages.mat', 'granite', '-append'); -#+end_src - -** Translation Stage -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeTy.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Simscape Model -:PROPERTIES: -:UNNUMBERED: t -:END: - -The Simscape model of the Translation stage consist of: -- One rigid body for the fixed part of the translation stage -- One rigid body for the moving part of the translation stage -- Four 6-DOF Joints that only have some rigidity in the X and Z directions. - The rigidity in rotation comes from the fact that we use multiple joints that are located at different points -- One 6-DOF joint that represent the Actuator. - It is used to impose the motion in the Y direction -- One 6-DOF joint to inject force disturbance in the X and Z directions - -#+name: fig:simscape_model_ty -#+ATTR_ORG: :width 800 -#+caption: Simscape model for the Translation Stage -[[file:figs/images/simscape_model_ty.png]] - -#+name: fig:simscape_picture_ty -#+attr_org: :width 800 -#+caption: Simscape picture for the Translation Stage -[[file:figs/images/simscape_picture_ty.png]] - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [ty] = initializeTy(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =ty= structure. -#+begin_src matlab - ty = struct(); -#+end_src - -*** Add Translation Stage Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case 'none' - ty.type = 0; - case 'rigid' - ty.type = 1; - case 'flexible' - ty.type = 2; - case 'modal-analysis' - ty.type = 3; - case 'init' - ty.type = 4; - end -#+end_src - -*** Material and Geometry -:PROPERTIES: -:UNNUMBERED: t -:END: -Define the density of the materials as well as the geometry (STEP files). -#+begin_src matlab - % Ty Granite frame - ty.granite_frame.density = 7800; % [kg/m3] => 43kg - ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP'; - - % Guide Translation Ty - ty.guide.density = 7800; % [kg/m3] => 76kg - ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP'; - - % Ty - Guide_Translation12 - ty.guide12.density = 7800; % [kg/m3] - ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP'; - - % Ty - Guide_Translation11 - ty.guide11.density = 7800; % [kg/m3] - ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP'; - - % Ty - Guide_Translation22 - ty.guide22.density = 7800; % [kg/m3] - ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP'; - - % Ty - Guide_Translation21 - ty.guide21.density = 7800; % [kg/m3] - ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP'; - - % Ty - Plateau translation - ty.frame.density = 7800; % [kg/m3] - ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP'; - - % Ty Stator Part - ty.stator.density = 5400; % [kg/m3] - ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP'; - - % Ty Rotor Part - ty.rotor.density = 5400; % [kg/m3] - ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP'; -#+end_src - -*** Stiffness and Damping properties -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad] - ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)] -#+end_src - -*** Equilibrium position of the each joint. -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Ftym'); - ty.Deq = -Ftym'./ty.K; - else - ty.Deq = zeros(6,1); - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =ty= structure is saved. -#+begin_src matlab - save('./mat/stages.mat', 'ty', '-append'); -#+end_src - -** Tilt Stage -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeRy.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Simscape Model -:PROPERTIES: -:UNNUMBERED: t -:END: - -The Simscape model of the Tilt stage is composed of: -- Two solid bodies for the two part of the stage -- *Four* 6-DOF joints to model the flexibility of the stage. - These joints are virtually located along the rotation axis and are connecting the two solid bodies. - These joints have some translation stiffness in the u-v-w directions aligned with the joint. - The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations -- A Bushing Joint used for the Actuator. - The Ry motion is imposed by the input. - -#+name: fig:simscape_model_ry -#+attr_org: :width 800 -#+caption: Simscape model for the Tilt Stage -[[file:figs/images/simscape_model_ry.png]] - -#+name: fig:simscape_picture_ry -#+attr_org: :width 800 -#+caption: Simscape picture for the Tilt Stage -[[file:figs/images/simscape_picture_ry.png]] - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [ry] = initializeRy(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false - args.Ry_init (1,1) double {mustBeNumeric} = 0 - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =ry= structure. -#+begin_src matlab - ry = struct(); -#+end_src - - -*** Add Tilt Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case 'none' - ry.type = 0; - case 'rigid' - ry.type = 1; - case 'flexible' - ry.type = 2; - case 'modal-analysis' - ry.type = 3; - case 'init' - ry.type = 4; - end -#+end_src - -*** Material and Geometry -:PROPERTIES: -:UNNUMBERED: t -:END: -Properties of the Material and link to the geometry of the Tilt stage. -#+begin_src matlab - % Ry - Guide for the tilt stage - ry.guide.density = 7800; % [kg/m3] - ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP'; - - % Ry - Rotor of the motor - ry.rotor.density = 2400; % [kg/m3] - ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP'; - - % Ry - Motor - ry.motor.density = 3200; % [kg/m3] - ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP'; - - % Ry - Plateau Tilt - ry.stage.density = 7800; % [kg/m3] - ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP'; -#+end_src - -Z-Offset so that the center of rotation matches the sample center; -#+begin_src matlab - ry.z_offset = 0.58178; % [m] -#+end_src - -#+begin_src matlab - ry.Ry_init = args.Ry_init; % [rad] -#+end_src - -*** Stiffness and Damping properties -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8]; - ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4]; -#+end_src - -*** Equilibrium position of the each joint. -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fym'); - ry.Deq = -Fym'./ry.K; - else - ry.Deq = zeros(6,1); - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =ry= structure is saved. -#+begin_src matlab - save('./mat/stages.mat', 'ry', '-append'); -#+end_src - -** Spindle -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeRz.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Simscape Model -:PROPERTIES: -:UNNUMBERED: t -:END: - -The Simscape model of the Spindle is composed of: -- Two rigid bodies: the stator and the rotor -- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction. -- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations - -#+name: fig:simscape_model_rz -#+attr_org: :width 800 -#+caption: Simscape model for the Spindle -[[file:figs/images/simscape_model_rz.png]] - -#+name: fig:simscape_picture_rz -#+attr_org: :width 800 -#+caption: Simscape picture for the Spindle -[[file:figs/images/simscape_picture_rz.png]] - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [rz] = initializeRz(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =rz= structure. -#+begin_src matlab - rz = struct(); -#+end_src - -*** Add Spindle Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case 'none' - rz.type = 0; - case 'rigid' - rz.type = 1; - case 'flexible' - rz.type = 2; - case 'modal-analysis' - rz.type = 3; - case 'init' - rz.type = 4; - end -#+end_src - -*** Material and Geometry -:PROPERTIES: -:UNNUMBERED: t -:END: - -Properties of the Material and link to the geometry of the spindle. -#+begin_src matlab - % Spindle - Slip Ring - rz.slipring.density = 7800; % [kg/m3] - rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP'; - - % Spindle - Rotor - rz.rotor.density = 7800; % [kg/m3] - rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP'; - - % Spindle - Stator - rz.stator.density = 7800; % [kg/m3] - rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP'; -#+end_src - -*** Stiffness and Damping properties -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7]; - rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4]; -#+end_src - -*** Equilibrium position of the each joint. -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab - if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fzm'); - rz.Deq = -Fzm'./rz.K; - else - rz.Deq = zeros(6,1); - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =rz= structure is saved. -#+begin_src matlab - save('./mat/stages.mat', 'rz', '-append'); -#+end_src - -** Micro Hexapod -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeMicroHexapod.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [micro_hexapod] = initializeMicroHexapod(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = '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) - % Force that stiffness of each joint should apply at t=0 - args.Foffset logical {mustBeNumericOrLogical} = false - end -#+end_src - -*** Function content -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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); -#+end_src - -#+begin_src matlab - stewart = initializeStrutDynamics(stewart, ... - 'K', args.Ki, ... - 'C', args.Ci); - - stewart = initializeJointDynamics(stewart, ... - 'type_F', 'universal_p', ... - 'type_M', 'spherical_p'); -#+end_src - -#+begin_src matlab - 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); -#+end_src - -#+begin_src matlab - stewart = initializeInertialSensor(stewart, 'type', 'none'); -#+end_src - -Equilibrium position of the each joint. -#+begin_src matlab - if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fhm'); - stewart.actuators.dLeq = -Fhm'./args.Ki; - else - stewart.actuators.dLeq = zeros(6,1); - end -#+end_src - -*** Add Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case 'none' - stewart.type = 0; - case 'rigid' - stewart.type = 1; - case 'flexible' - stewart.type = 2; - case 'modal-analysis' - stewart.type = 3; - case 'init' - stewart.type = 4; - case 'compliance' - stewart.type = 5; - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =micro_hexapod= structure is saved. -#+begin_src matlab - micro_hexapod = stewart; - save('./mat/stages.mat', 'micro_hexapod', '-append'); -#+end_src - -** Nano Hexapod -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeNanoHexapod.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -function [nano_hexapod] = initializeNanoHexapod(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -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} = ones(6,1)*0.38e6 % [N/m] - args.actuator_ke (6,1) double {mustBeNumeric} = ones(6,1)*1.75e6 % [N/m] - args.actuator_ka (6,1) double {mustBeNumeric} = ones(6,1)*3e7 % [N/m] - args.actuator_c (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)] - args.actuator_ce (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)] - args.actuator_ca (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)] - 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 -end -#+end_src - -*** Nano Hexapod Object -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -nano_hexapod = struct(); -#+end_src - -*** Flexible Joints - Bot -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -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)] -#+end_src - -*** Flexible Joints - Top -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -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)] -#+end_src - -*** Relative Motion Sensor -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -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 -#+end_src - -*** Amplified Piezoelectric Actuator -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -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 -#+end_src - -#+begin_src matlab -%% Actuator gain [N/V] -if all(args.actuator_Ga == 0) - switch args.actuator_type - case '2dof' - nano_hexapod.actuator.Ga = ones(6,1)*(-30.0); - case 'flexible frame' - nano_hexapod.actuator.Ga = ones(6,1); % TODO - case 'flexible' - nano_hexapod.actuator.Ga = ones(6,1)*23.4; - 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)*0.098; - case 'flexible frame' - nano_hexapod.actuator.Gs = ones(6,1); % TODO - case 'flexible' - nano_hexapod.actuator.Gs = ones(6,1)*(-4674824); - end -else - nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m] -end -#+end_src - -Mechanical characteristics: -#+begin_src matlab -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 - -#+end_src - -*** Geometry -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -nano_hexapod.geometry = struct(); -#+end_src - -Center of joints $a_i$ with respect to {F}: -#+begin_src matlab -Fa = [[-21.74, 111.91, 22.49], - [-107.79, -37.13, 22.49], - [-86.05, -74.78, 22.49], - [ 86.05, -74.78, 22.49], - [ 107.79, -37.13, 22.49], - [ 21.74, 111.91, 22.49]]'*1e-3; % Ai w.r.t. {F} [m] -#+end_src - -Center of joints $b_i$ with respect to {M}: -#+begin_src matlab -Mb = [[-77.78, 77.78, -22.50], - [-106.25, 28.47, -22.50], - [-28.47, -106.25, -22.50], - [ 28.47, -106.25, -22.50], - [ 106.25, 28.47, -22.50], - [ 77.78, 77.78, -22.50]]'*1e-3; % Bi w.r.t. {M} [m] -#+end_src - -Now compute the positions $b_i$ with respect to {F}: -#+begin_src matlab -Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m] -#+end_src - -The unit vector representing the orientation of the struts can then be computed: -#+begin_src matlab -si = Fb - Fa; -si = si./vecnorm(si); % Normalize -#+end_src - -Location of encoder measurement points when fixed on the plates: -#+begin_src matlab -Fc = [[-76.914, 78.31, 52.605] - [-106.276, 27.454, 52.605] - [-29.362, -105.765, 52.605] - [ 29.362, -105.765, 52.605] - [ 106.276, 27.454, 52.605] - [ 76.914, 78.31, 52.605]]'*1e-3; % Meas pos w.r.t. {F} -Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M} -#+end_src - -#+begin_src matlab -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; -#+end_src - -*** Jacobian for Actuators -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -Bb = Mb - [0; 0; args.MO_B]; - -nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)']; -#+end_src - -*** Jacobian for Sensors -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -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 -#+end_src - -*** Top Plate -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -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.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 ], - [ 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 ]}; - - - nano_hexapod.top_plate.R_enc = ... - { [ 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 ], - [-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 ]}; - - 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 -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+begin_src matlab -if nargout == 0 - save('./mat/stages.mat', 'nano_hexapod', '-append'); -end -#+end_src - -** Sample -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeSample.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [sample] = initializeSample(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0' - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =sample= structure. -#+begin_src matlab - sample = struct(); -#+end_src - -*** Add Sample Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - switch args.type - case '0' - sample.type = 0; - case '1' - sample.type = 1; - case '2' - sample.type = 2; - case '3' - sample.type = 3; - end -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =sample= structure is saved. -#+begin_src matlab - save('./mat/stages.mat', 'sample', '-append'); -#+end_src - -** Initialize Controller -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeController.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function Declaration and Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = initializeController(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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 -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =controller= structure. -#+begin_src matlab - controller = struct(); -#+end_src - -*** Controller Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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 -#+end_src - -*** Save the Structure -:PROPERTIES: -:UNNUMBERED: t -:END: -The =controller= structure is saved. -#+begin_src matlab - save('./mat/controller.mat', 'controller'); -#+end_src - -** Generate Reference Signals -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeReferences.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -*** Function Declaration and Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [ref] = initializeReferences(args) -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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 -#+end_src - - -*** Initialize Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - %% 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); -#+end_src - -*** Translation Stage -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - %% 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); -#+end_src - -*** Tilt Stage -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - %% 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); -#+end_src - -*** Spindle -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - %% 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); -#+end_src - -*** Micro Hexapod -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - %% 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('mat/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)); -#+end_src - -*** Save -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - %% Save - save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts'); - end -#+end_src - -** Initialize Position Errors -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializePosError.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -*** Function Declaration and Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = initializePosError(args) - % initializePosError - Initialize the position errors - % - % Syntax: [] = initializePosError(args) - % - % Inputs: - % - args - - -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.error logical {mustBeNumericOrLogical} = false - args.Dy (1,1) double {mustBeNumeric} = 0 % [m] - args.Ry (1,1) double {mustBeNumeric} = 0 % [m] - args.Rz (1,1) double {mustBeNumeric} = 0 % [m] - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =pos_error= structure. -#+begin_src matlab - pos_error = struct(); -#+end_src - -*** Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - if args.error - pos_error.type = 1; - else - pos_error.type = 0; - end -#+end_src - -*** Position Errors -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - pos_error.Dy = args.Dy; - pos_error.Ry = args.Ry; - pos_error.Rz = args.Rz; -#+end_src - -*** Save -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - save('./mat/pos_error.mat', 'pos_error'); -#+end_src -** Initialize Rz Estimate -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeRzEstimate.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -*** Function Declaration and Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -function [] = initializeRzEstimate(args) -% initializeRzEstimate - Initialize the position errors -% -% Syntax: [] = initializeRzEstimate(args) -% -% Inputs: -% - args - - -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -arguments - args.type char {mustBeMember(args.type,{'encoders', 'voltages'})} = 'encoders' -end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =rz_estimate= structure. -#+begin_src matlab -rz_estimate = struct(); -#+end_src - -*** Type -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -switch args.type - case 'encoders' - rz_estimate.type = 0; - case 'voltages' - rz_estimate.type = 1; -end -#+end_src - -*** Position Errors -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -load('mat/stages.mat', 'nano_hexapod'); -rz_estimate.J_L_to_X = pinv(nano_hexapod.geometry.J); -rz_estimate.pz_sensitivity = -240e-6/8.5; % [m/V] -#+end_src - -*** Save -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -save('./mat/stages.mat', 'rz_estimate', '-append'); -#+end_src - -** Initialize Lion Metrology -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeLion.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -*** Function Declaration and Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = initializeLion(args) - % initializeLion - Initialize the position errors - % - % Syntax: [] = initializeLion(args) - % - % Inputs: - % - args - - -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'rigid'})} = 'rigid' - end -#+end_src - -*** Structure initialization -:PROPERTIES: -:UNNUMBERED: t -:END: -First, we initialize the =rz_estimate= structure. -#+begin_src matlab -lion = struct(); -#+end_src - -*** Jacobian -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -lion.J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0; - 0.78740157480315 0.21259842519685 0 0 0; - 0 0 0 0 -1; - -13.1233595800525 13.1233595800525 0 0 0; - 0 0 -13.1233595800525 13.1233595800525 0]; -#+end_src - -*** Save -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab -save('./mat/stages.mat', 'lion', '-append'); -#+end_src - -** Initialize Disturbances -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/initializeDisturbances.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -*** Function Declaration and Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = initializeDisturbances(args) - % initializeDisturbances - Initialize the disturbances - % - % Syntax: [] = initializeDisturbances(args) - % - % Inputs: - % - args - - -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - % Global parameter to enable or disable the disturbances - args.enable logical {mustBeNumericOrLogical} = true - % Ground Motion - X direction - args.Dwx logical {mustBeNumericOrLogical} = true - % Ground Motion - Y direction - args.Dwy logical {mustBeNumericOrLogical} = true - % Ground Motion - Z direction - args.Dwz logical {mustBeNumericOrLogical} = true - % Translation Stage - X direction - args.Fty_x logical {mustBeNumericOrLogical} = true - % Translation Stage - Z direction - args.Fty_z logical {mustBeNumericOrLogical} = true - % Spindle - Z direction - args.Frz_z logical {mustBeNumericOrLogical} = true - end -#+end_src - - -*** Load Data -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - load('./mat/dist_psd.mat', 'dist_f'); -#+end_src - -We remove the first frequency point that usually is very large. -#+begin_src matlab :exports none - dist_f.f = dist_f.f(2:end); - dist_f.psd_gm = dist_f.psd_gm(2:end); - dist_f.psd_ty = dist_f.psd_ty(2:end); - dist_f.psd_rz = dist_f.psd_rz(2:end); -#+end_src - -*** Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -We define some parameters that will be used in the algorithm. -#+begin_src matlab - Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] - N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD - T0 = N/Fs; % Signal Duration [s] - df = 1/T0; % Frequency resolution of the DFT [Hz] - % Also equal to (dist_f.f(2)-dist_f.f(1)) - t = linspace(0, T0, N+1)'; % Time Vector [s] - Ts = 1/Fs; % Sampling Time [s] -#+end_src - -*** Ground Motion -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - phi = dist_f.psd_gm; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end -#+end_src - -#+begin_src matlab - if args.Dwx && args.enable - rng(111); - 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)))];; - Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] - else - Dwx = zeros(length(t), 1); - end -#+end_src - -#+begin_src matlab - if args.Dwy && args.enable - rng(112); - 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)))];; - Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] - else - Dwy = zeros(length(t), 1); - end -#+end_src - -#+begin_src matlab - if args.Dwy && args.enable - rng(113); - 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)))];; - Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] - else - Dwz = zeros(length(t), 1); - end -#+end_src - -*** Translation Stage - X direction -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - if args.Fty_x && args.enable - phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - rng(121); - 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)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N] - Fty_x = u; - else - Fty_x = zeros(length(t), 1); - end -#+end_src - -*** Translation Stage - Z direction -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - if args.Fty_z && args.enable - phi = dist_f.psd_ty; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - rng(122); - 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)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N] - Fty_z = u; - else - Fty_z = zeros(length(t), 1); - end -#+end_src - -*** Spindle - Z direction -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - if args.Frz_z && args.enable - phi = dist_f.psd_rz; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - rng(131); - 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)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N] - Frz_z = u; - else - Frz_z = zeros(length(t), 1); - end -#+end_src - -*** Direct Forces -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - u = zeros(length(t), 6); - Fd = u; -#+end_src - -*** Set initial value to zero -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - Dwx = Dwx - Dwx(1); - Dwy = Dwy - Dwy(1); - Dwz = Dwz - Dwz(1); - Fty_x = Fty_x - Fty_x(1); - Fty_z = Fty_z - Fty_z(1); - Frz_z = Frz_z - Frz_z(1); -#+end_src - -*** Save -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args'); -#+end_src - -** Initialize 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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeStewartPlatform.m][here]]. - -**** Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+name: fig:stewart-frames-position -#+caption: Definition of the position of the frames -[[file:figs/stewart-frames-position.png]] - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeFramesPositions.m][here]]. - -**** Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+name: fig:stewart-frames-position -#+caption: Definition of the position of the frames -[[file:figs/stewart-frames-position.png]] - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/generateGeneralConfiguration.m][here]]. - -**** Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}. -The radius of the circles can be chosen as well as the angles where the joints are located (see Figure ref:fig:joint_position_general). - -#+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 - -#+name: fig:joint_position_general -#+caption: Position of the joints -#+RESULTS: -[[file:figs/stewart_bottom_plate.png]] - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/computeJointsPose.m][here]]. - -**** Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+name: fig:stewart-struts -#+caption: Position and orientation of the struts -[[file:figs/stewart-struts.png]] - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 - -*** =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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeStewartPose.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); -#+end_src - -**** Populate the =stewart= structure -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - stewart.actuators.Leq = dLi; -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeCylindricalPlatforms.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeCylindricalStruts.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeStrutDynamics.m][here]]. - -**** Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeJointDynamics.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -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 -:PROPERTIES: -:UNNUMBERED: t -:END: -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 -:PROPERTIES: -:UNNUMBERED: t -:END: - -#+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 - -*** =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: -<> - -This Matlab function is accessible [[file:./matlab/src/initializeInertialSensor.m][here]]. - -**** Geophone - Working Principle -:PROPERTIES: -:UNNUMBERED: t -:END: -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 -:PROPERTIES: -:UNNUMBERED: t -:END: -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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - stewart.sensors.inertial = sensor; -#+end_src - -*** =displayArchitecture=: 3D plot of the Stewart platform architecture -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/displayArchitecture.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:./matlab/src/displayArchitecture.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = displayArchitecture(stewart, args) - % displayArchitecture - 3D plot of the Stewart platform architecture - % - % Syntax: [] = displayArchitecture(args) - % - % Inputs: - % - stewart - % - args - Structure with 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} - % - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} - % - F_color [color] - Color used for the Fixed elements - % - M_color [color] - Color used for the Mobile elements - % - L_color [color] - Color used for the Legs elements - % - frames [true/false] - Display the Frames - % - legs [true/false] - Display the Legs - % - joints [true/false] - Display the Joints - % - labels [true/false] - Display the Labels - % - platforms [true/false] - Display the Platforms - % - views ['all', 'xy', 'yz', 'xz', 'default'] - - % - % Outputs: -#+end_src - -**** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - stewart - args.AP (3,1) double {mustBeNumeric} = zeros(3,1) - args.ARB (3,3) double {mustBeNumeric} = eye(3) - args.F_color = [0 0.4470 0.7410] - args.M_color = [0.8500 0.3250 0.0980] - args.L_color = [0 0 0] - args.frames logical {mustBeNumericOrLogical} = true - args.legs logical {mustBeNumericOrLogical} = true - args.joints logical {mustBeNumericOrLogical} = true - args.labels logical {mustBeNumericOrLogical} = true - args.platforms logical {mustBeNumericOrLogical} = true - args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default' - end -#+end_src - -**** Check the =stewart= structure elements -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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, 'H'), 'stewart.geometry should have attribute H') - H = stewart.geometry.H; - - 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; -#+end_src - - -**** Figure Creation, Frames and Homogeneous transformations -:PROPERTIES: -:UNNUMBERED: t -:END: - -The reference frame of the 3d plot corresponds to the frame $\{F\}$. -#+begin_src matlab - if ~strcmp(args.views, 'all') - figure; - else - f = figure('visible', 'off'); - end - - hold on; -#+end_src - -We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$. -#+begin_src matlab - FTa = [eye(3), FO_A; ... - zeros(1,3), 1]; - ATb = [args.ARB, args.AP; ... - zeros(1,3), 1]; - BTm = [eye(3), -MO_B; ... - zeros(1,3), 1]; - - FTm = FTa*ATb*BTm; -#+end_src - -Let's define a parameter that define the length of the unit vectors used to display the frames. -#+begin_src matlab - d_unit_vector = H/4; -#+end_src - -Let's define a parameter used to position the labels with respect to the center of the element. -#+begin_src matlab - d_label = H/20; -#+end_src - -**** Fixed Base elements -:PROPERTIES: -:UNNUMBERED: t -:END: -Let's first plot the frame $\{F\}$. -#+begin_src matlab - Ff = [0, 0, 0]; - if args.frames - quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ... - [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color) - - if args.labels - text(Ff(1) + d_label, ... - Ff(2) + d_label, ... - Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color); - end - end -#+end_src - -Now plot the frame $\{A\}$ fixed to the Base. -#+begin_src matlab - if args.frames - quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ... - [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color) - - if args.labels - text(FO_A(1) + d_label, ... - FO_A(2) + d_label, ... - FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color); - end - end -#+end_src - -Let's then plot the circle corresponding to the shape of the Fixed base. -#+begin_src matlab - if args.platforms && stewart.platform_F.type == 1 - theta = [0:0.01:2*pi+0.01]; % Angles [rad] - v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal - center = [0; 0; 0]; % Center of the circle - radius = stewart.platform_F.R; % Radius of the circle [m] - - points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); - - plot3(points(1,:), ... - points(2,:), ... - points(3,:), '-', 'Color', args.F_color); - end -#+end_src - -Let's now plot the position and labels of the Fixed Joints -#+begin_src matlab - if args.joints - scatter3(Fa(1,:), ... - Fa(2,:), ... - Fa(3,:), 'MarkerEdgeColor', args.F_color); - if args.labels - for i = 1:size(Fa,2) - text(Fa(1,i) + d_label, ... - Fa(2,i), ... - Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color); - end - end - end -#+end_src - -**** Mobile Platform elements -:PROPERTIES: -:UNNUMBERED: t -:END: - -Plot the frame $\{M\}$. -#+begin_src matlab - Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F} - - if args.frames - FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors - quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ... - FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color) - - if args.labels - text(Fm(1) + d_label, ... - Fm(2) + d_label, ... - Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color); - end - end -#+end_src - -Plot the frame $\{B\}$. -#+begin_src matlab - FB = FO_A + args.AP; - - if args.frames - FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors - quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ... - FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color) - - if args.labels - text(FB(1) - d_label, ... - FB(2) + d_label, ... - FB(3) + d_label, '$\{B\}$', 'Color', args.M_color); - end - end -#+end_src - -Let's then plot the circle corresponding to the shape of the Mobile platform. -#+begin_src matlab - if args.platforms && stewart.platform_M.type == 1 - theta = [0:0.01:2*pi+0.01]; % Angles [rad] - v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal - center = Fm(1:3); % Center of the circle - radius = stewart.platform_M.R; % Radius of the circle [m] - - points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); - - plot3(points(1,:), ... - points(2,:), ... - points(3,:), '-', 'Color', args.M_color); - end -#+end_src - -Plot the position and labels of the rotation joints fixed to the mobile platform. -#+begin_src matlab - if args.joints - Fb = FTm*[Mb;ones(1,6)]; - - scatter3(Fb(1,:), ... - Fb(2,:), ... - Fb(3,:), 'MarkerEdgeColor', args.M_color); - - if args.labels - for i = 1:size(Fb,2) - text(Fb(1,i) + d_label, ... - Fb(2,i), ... - Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color); - end - end - end -#+end_src - -**** Legs -:PROPERTIES: -:UNNUMBERED: t -:END: -Plot the legs connecting the joints of the fixed base to the joints of the mobile platform. -#+begin_src matlab - if args.legs - for i = 1:6 - plot3([Fa(1,i), Fb(1,i)], ... - [Fa(2,i), Fb(2,i)], ... - [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color); - - if args.labels - text((Fa(1,i)+Fb(1,i))/2 + d_label, ... - (Fa(2,i)+Fb(2,i))/2, ... - (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color); - end - end - end -#+end_src - -**** Figure parameters -#+begin_src matlab - switch args.views - case 'default' - view([1 -0.6 0.4]); - case 'xy' - view([0 0 1]); - case 'xz' - view([0 -1 0]); - case 'yz' - view([1 0 0]); - end - axis equal; - axis off; -#+end_src - -**** Subplots -#+begin_src matlab - if strcmp(args.views, 'all') - hAx = findobj('type', 'axes'); - - figure; - s1 = subplot(2,2,1); - copyobj(get(hAx(1), 'Children'), s1); - view([0 0 1]); - axis equal; - axis off; - title('Top') - - s2 = subplot(2,2,2); - copyobj(get(hAx(1), 'Children'), s2); - view([1 -0.6 0.4]); - axis equal; - axis off; - - s3 = subplot(2,2,3); - copyobj(get(hAx(1), 'Children'), s3); - view([1 0 0]); - axis equal; - axis off; - title('Front') - - s4 = subplot(2,2,4); - copyobj(get(hAx(1), 'Children'), s4); - view([0 -1 0]); - axis equal; - axis off; - title('Side') - - close(f); - end -#+end_src - - -*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/describeStewartPlatform.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:./matlab/src/describeStewartPlatform.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [] = describeStewartPlatform(stewart) - % describeStewartPlatform - Display some text describing the current defined Stewart Platform - % - % Syntax: [] = describeStewartPlatform(args) - % - % Inputs: - % - stewart - % - % Outputs: -#+end_src - -**** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - stewart - end -#+end_src - -**** Geometry -#+begin_src matlab - fprintf('GEOMETRY:\n') - fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H) - - if stewart.platform_M.MO_B(3) > 0 - fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3)) - else - fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3)) - end - - fprintf('- The initial length of the struts are:\n') - fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l) - fprintf('\n') -#+end_src - -**** Actuators -#+begin_src matlab - fprintf('ACTUATORS:\n') - if stewart.actuators.type == 1 - fprintf('- The actuators are classical.\n') - fprintf('- The Stiffness and Damping of each actuators is:\n') - fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1)) - elseif stewart.actuators.type == 2 - fprintf('- The actuators are mechanicaly amplified.\n') - fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n') - fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1)) - fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n') - fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1)) - end - fprintf('\n') -#+end_src - -**** Joints -#+begin_src matlab - fprintf('JOINTS:\n') -#+end_src - -Type of the joints on the fixed base. -#+begin_src matlab - switch stewart.joints_F.type - case 1 - fprintf('- The joints on the fixed based are universal joints\n') - case 2 - fprintf('- The joints on the fixed based are spherical joints\n') - case 3 - fprintf('- The joints on the fixed based are perfect universal joints\n') - case 4 - fprintf('- The joints on the fixed based are perfect spherical joints\n') - end -#+end_src - -Type of the joints on the mobile platform. -#+begin_src matlab - switch stewart.joints_M.type - case 1 - fprintf('- The joints on the mobile based are universal joints\n') - case 2 - fprintf('- The joints on the mobile based are spherical joints\n') - case 3 - fprintf('- The joints on the mobile based are perfect universal joints\n') - case 4 - fprintf('- The joints on the mobile based are perfect spherical joints\n') - end -#+end_src - -Position of the fixed joints -#+begin_src matlab - fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n') - fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa) -#+end_src - -Position of the mobile joints -#+begin_src matlab - fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n') - fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb) - fprintf('\n') -#+end_src - -**** Kinematics -#+begin_src matlab - fprintf('KINEMATICS:\n') - - if isfield(stewart.kinematics, 'K') - fprintf('- The Stiffness matrix K is (in [N/m]):\n') - fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K) - end - - if isfield(stewart.kinematics, 'C') - fprintf('- The Damping matrix C is (in [m/N]):\n') - fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C) - end -#+end_src - -*** =generateCubicConfiguration=: Generate a Cubic Configuration -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/generateCubicConfiguration.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:./matlab/src/generateCubicConfiguration.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [stewart] = generateCubicConfiguration(stewart, args) - % generateCubicConfiguration - Generate a Cubic Configuration - % - % Syntax: [stewart] = generateCubicConfiguration(stewart, args) - % - % Inputs: - % - stewart - A structure with the following fields - % - geometry.H [1x1] - Total height of the platform [m] - % - args - Can have the following fields: - % - Hc [1x1] - Height of the "useful" part of the cube [m] - % - FOc [1x1] - Height of the center of the cube with respect to {F} [m] - % - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m] - % - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m] - % - % 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 - -**** Documentation -:PROPERTIES: -:UNNUMBERED: t -:END: -#+name: fig:cubic-configuration-definition -#+caption: Cubic Configuration -[[file:figs/cubic-configuration-definition.png]] - -**** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - stewart - args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 - args.FOc (1,1) double {mustBeNumeric} = 50e-3 - args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 - args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 - end -#+end_src - -**** Check the =stewart= structure elements -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') - H = stewart.geometry.H; -#+end_src - -**** Position of the Cube -:PROPERTIES: -:UNNUMBERED: t -:END: -We define the useful points of the cube with respect to the Cube's center. -${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}. - -#+begin_src matlab - sx = [ 2; -1; -1]; - sy = [ 0; 1; -1]; - sz = [ 1; 1; 1]; - - R = [sx, sy, sz]./vecnorm([sx, sy, sz]); - - L = args.Hc*sqrt(3); - - Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc]; - - CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg - CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg -#+end_src - -**** Compute the pose -:PROPERTIES: -:UNNUMBERED: t -:END: -We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). -#+begin_src matlab - CSi = (CCm - CCf)./vecnorm(CCm - CCf); -#+end_src - -We now which to compute the position of the joints $a_{i}$ and $b_{i}$. -#+begin_src matlab - Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; - Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; -#+end_src - -**** Populate the =stewart= structure -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - stewart.platform_F.Fa = Fa; - stewart.platform_M.Mb = Mb; -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/computeJacobian.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - J = [As' , cross(Ab, As)']; -#+end_src - -**** Compute Stiffness Matrix -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - K = J'*diag(Ki)*J; -#+end_src - -**** Compute Compliance Matrix -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - C = inv(K); -#+end_src - -**** Populate the =stewart= structure -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - stewart.kinematics.J = J; - stewart.kinematics.K = K; - stewart.kinematics.C = C; -#+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: -<> - -This Matlab function is accessible [[file:./matlab/src/inverseKinematics.m][here]]. - -**** Theory -:PROPERTIES: -:UNNUMBERED: t -:END: -For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{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{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\ - &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i -\end{align*} - -To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself: -\begin{equation} - l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{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\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 -:PROPERTIES: -:UNNUMBERED: t -:END: -#+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 - -*** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics -:PROPERTIES: -:header-args:matlab+: :tangle ./matlab/src/forwardKinematicsApprox.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:./matlab/src/forwardKinematicsApprox.m][here]]. - -**** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - function [P, R] = forwardKinematicsApprox(stewart, args) - % forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using - % the Jacobian Matrix - % - % Syntax: [P, R] = forwardKinematicsApprox(stewart, args) - % - % Inputs: - % - stewart - A structure with the following fields - % - kinematics.J [6x6] - The Jacobian Matrix - % - args - Can have the following fields: - % - dL [6x1] - Displacement of each strut [m] - % - % Outputs: - % - P [3x1] - The estimated position of {B} with respect to {A} - % - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A} -#+end_src - -**** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - stewart - args.dL (6,1) double {mustBeNumeric} = zeros(6,1) - end -#+end_src - -**** Check the =stewart= structure elements -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') - J = stewart.kinematics.J; -#+end_src - -**** Computation -:PROPERTIES: -:UNNUMBERED: t -:END: -From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the -position and orientation of {B} with respect to {A} using the following formula: -\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \] -#+begin_src matlab - X = J\args.dL; -#+end_src - -The position vector corresponds to the first 3 elements. -#+begin_src matlab - P = X(1:3); -#+end_src - -The next 3 elements are the orientation of {B} with respect to {A} expressed -using the screw axis. -#+begin_src matlab - theta = norm(X(4:6)); - s = X(4:6)/theta; -#+end_src - -We then compute the corresponding rotation matrix. -#+begin_src matlab - R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta); - s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta); - s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)]; -#+end_src diff --git a/test-bench-id31.org b/test-bench-id31.org index fadffc0..4221dad 100644 --- a/test-bench-id31.org +++ b/test-bench-id31.org @@ -3028,6 +3028,7 @@ plot(f, rga_m0, 'DisplayName', '$m = 0$ kg') plot(f, rga_m1, 'DisplayName', '$m = 13$ kg') plot(f, rga_m2, 'DisplayName', '$m = 26$ kg') plot(f, rga_m3, 'DisplayName', '$m = 39$ kg') +hold off; xlabel('Frequency [Hz]'); ylabel('RGA number'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); xlim([1, 1e2]); ylim([0, 10]);