diff --git a/mat/conf_log.mat b/mat/conf_log.mat index 4d59937..45da412 100644 Binary files a/mat/conf_log.mat and b/mat/conf_log.mat differ diff --git a/mat/conf_simscape.mat b/mat/conf_simscape.mat index 043ff32..e3d105d 100644 Binary files a/mat/conf_simscape.mat and b/mat/conf_simscape.mat differ diff --git a/mat/controller.mat b/mat/controller.mat index ecf89e0..dab818b 100644 Binary files a/mat/controller.mat and b/mat/controller.mat differ diff --git a/mat/nass_disturbances.mat b/mat/nass_disturbances.mat index ff930ed..9864ab6 100644 Binary files a/mat/nass_disturbances.mat and b/mat/nass_disturbances.mat differ diff --git a/mat/nass_references.mat b/mat/nass_references.mat index e66f3e9..f063dee 100644 Binary files a/mat/nass_references.mat and b/mat/nass_references.mat differ diff --git a/mat/optimal_stiffness_Gk_wz.mat b/mat/optimal_stiffness_Gk_wz.mat index 9519100..3d10cf0 100644 Binary files a/mat/optimal_stiffness_Gk_wz.mat and b/mat/optimal_stiffness_Gk_wz.mat differ diff --git a/mat/optimal_stiffness_Gm_Gf.mat b/mat/optimal_stiffness_Gm_Gf.mat index fba2cd9..3a9f39e 100644 Binary files a/mat/optimal_stiffness_Gm_Gf.mat and b/mat/optimal_stiffness_Gm_Gf.mat differ diff --git a/mat/optimal_stiffness_micro_station_compliance.mat b/mat/optimal_stiffness_micro_station_compliance.mat new file mode 100644 index 0000000..53ed532 Binary files /dev/null and b/mat/optimal_stiffness_micro_station_compliance.mat differ diff --git a/mat/stages.mat b/mat/stages.mat index 005cac0..01bb001 100644 Binary files a/mat/stages.mat and b/mat/stages.mat differ diff --git a/matlab/nass_model.slx b/matlab/nass_model.slx index 1282eb7..903f37f 100644 Binary files a/matlab/nass_model.slx and b/matlab/nass_model.slx differ diff --git a/matlab/optimal_stiffness.m b/matlab/optimal_stiffness.m new file mode 100644 index 0000000..422a2b4 --- /dev/null +++ b/matlab/optimal_stiffness.m @@ -0,0 +1,1242 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +load('mat/conf_simulink.mat'); +open('nass_model.slx') + +% Initialization +% We initialize all the stages with the default parameters. + +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeAxisc(); +initializeMirror(); + + + +% The worst case scenario is a rotation speed of 60rpm with a payload mass of 10Kg. + +initializeSample('mass', 10); + + + +% We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics. + +initializeSimscapeConfiguration('gravity', true); +initializeDisturbances('enable', false); +initializeLoggingConfiguration('log', 'none'); + +%% Name of the Simulink File +mdl = 'nass_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; + +% Identification when not rotating +% We set the range of stiffness that we want to use. + +Ks = logspace(3,9,7); % [N/m] + + + +% We don't move any stage and no controller is used. + +initializeReferences(); +initializeController(); + +Gk_iff = {zeros(length(Ks))}; +Gk_dvf = {zeros(length(Ks))}; +Gk_err = {zeros(length(Ks))}; + +for i = 1:length(Ks) + initializeNanoHexapod('k', Ks(i)); + + %% Run the linearization + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gk_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gk_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gk_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; +end + +% Identification when rotating at maximum speed +% We now set the reference path such that the Spindle is rotating at 60rpm and such that it is at the zero position at the time of the identification. + +Rz_rpm = 60; + +initializeReferences('Rz_type', 'rotating', ... + 'Rz_period', 60/Rz_rpm, ... % Rotation period [s] + 'Rz_amplitude', -0.2*(2*pi*Rz_rpm/60)); % Angle offset [rad] + +load('mat/nass_references.mat', 'Rz'); % We load the reference for the Spindle +[~, i_end] = min(abs(Rz.signals.values)); % Obtain the indice where the spindle angle is zero +t_sim = Rz.time(i_end); % Simulation time before identification [s] + + + +% We here use a decentralized controller that is used to stabilize the nano-hexapod until the identification is made. +% This controller virtually adds stiffness in each of the nano-hexapod leg. + +k_sta = -1e8; +initializeController('type', 'stabilizing'); + +Gk_wz_iff = {zeros(length(Ks))}; +Gk_wz_dvf = {zeros(length(Ks))}; +Gk_wz_err = {zeros(length(Ks))}; + +for i = 1:length(Ks) + initializeNanoHexapod('k', Ks(i)); + + %% Run the linearization + G = linearize(mdl, io, t_sim); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gk_wz_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gk_wz_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gk_wz_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; +end + +save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ... + 'Gk_iff', 'Gk_dvf', 'Gk_err', ... + 'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err'); + +% Change of dynamics + +load('mat/optimal_stiffness_Gk_wz.mat'); + + + +% Change of dynamics for decentralized IFF control. + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Gk_iff) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_iff{i}( 'Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +title('Soft Nano-Hexapod'); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Gk_iff) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i))); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% Change of dynamics from $F_x$ to $D_x$. + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Gk_err) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_err{i}( 'Ex', 'Fx'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Gk_err) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i))); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% Change of dynamics from $F_z$ to $D_z$. + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Gk_err) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_err{i}( 'Ez', 'Fz'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title('Soft Nano-Hexapod'); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Gk_err) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i))); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Change of coupling + +load('mat/optimal_stiffness_Gk_wz.mat'); + + + +% Change of coupling from $F_x$ to $D_y$ when not rotating and when rotating at 60rpm. + +freqs = logspace(-1, 3, 1000); + +figure; + +hold on; +for i = 1:length(Gk_err) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_err{i}( 'Ey', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i))); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]'); +xlim([freqs(1), freqs(end)]); +legend('location', 'northeast'); + + + +% Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the direct term $F_x$ to $D_x$. + +freqs = logspace(-1, 3, 1000); + +figure; + +hold on; +for i = 1:length(Gk_err) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i))); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ... + 'HandleVisibility', 'off'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]'); +xlim([freqs(1), freqs(end)]); +legend('location', 'northeast'); + +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +load('mat/conf_simulink.mat'); +open('nass_model.slx') + +% Identification of the micro-station compliance +% We initialize all the stages with the default parameters. + +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod('type', 'compliance'); + + + +% We put nothing on top of the micro-hexapod. + +initializeAxisc('type', 'none'); +initializeMirror('type', 'none'); +initializeNanoHexapod('type', 'none'); +initializeSample('type', 'none'); + +initializeReferences(); +initializeDisturbances(); +initializeController(); +initializeSimscapeConfiguration(); +initializeLoggingConfiguration(); + + + +% And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point. + +%% Name of the Simulink File +mdl = 'nass_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform +io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform + +%% Run the linearization +Gm = linearize(mdl, io, 0); +Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'}; +Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'}; + +labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'}; + +freqs = logspace(1, 3, 1000); + +figure; + +hold on; +plot(freqs, abs(squeeze(freqresp(Gm(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', labels{1}); +for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm(1, i), freqs, 'Hz'))), 'DisplayName', labels{i}); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); +ylabel('Compliance'); +legend('location', 'northwest'); + +% Initialization + +initializeReferences(); +initializeDisturbances(); +initializeController(); +initializeSimscapeConfiguration(); +initializeLoggingConfiguration(); +initializeSimscapeConfiguration('gravity', false); + +%% Name of the Simulink File +mdl = 'nass_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; + +Ks = logspace(3,9,7); % [N/m] + +initializeSample('type', 'rigid', 'mass', 20); + +% Rigid micro-station + +initializeGround('type', 'rigid'); +initializeGranite('type', 'rigid'); +initializeTy('type', 'rigid'); +initializeRy('type', 'rigid'); +initializeRz('type', 'rigid'); +initializeMicroHexapod('type', 'rigid'); + +initializeAxisc('type', 'rigid'); +initializeMirror('type', 'rigid'); + +Gmr_iff = {zeros(length(Ks))}; +Gmr_dvf = {zeros(length(Ks))}; +Gmr_err = {zeros(length(Ks))}; + +for i = 1:length(Ks) + initializeNanoHexapod('k', Ks(i)); + + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gmr_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gmr_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gmr_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; +end + +% Flexible micro-station + +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); + +initializeAxisc(); +initializeMirror(); + +Gmf_iff = {zeros(length(Ks))}; +Gmf_dvf = {zeros(length(Ks))}; +Gmf_err = {zeros(length(Ks))}; + +for i = 1:length(Ks) + initializeNanoHexapod('k', Ks(i)); + + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gmf_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gmf_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gmf_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; +end + +save('mat/optimal_stiffness_micro_station_compliance.mat', 'Ks', ... + 'Gmr_iff', 'Gmr_dvf', 'Gmr_err', ... + 'Gmf_iff', 'Gmf_dvf', 'Gmf_err'); + +% Obtained Dynamics + +load('mat/optimal_stiffness_micro_station_compliance.mat'); + + + +% IFF plant + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% DVF plant + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% X direction + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% Z direction + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +load('mat/conf_simulink.mat'); +open('nass_model.slx') + +% Initialization +% We initialize all the stages with the default parameters. + +initializeGround(); +initializeGranite(); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod(); +initializeAxisc(); +initializeMirror(); + + + +% We don't include disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics. + +initializeSimscapeConfiguration('gravity', true); +initializeDisturbances('enable', false); + + + +% We set the controller type to Open-Loop, and we do not need to log any signal. + +initializeController(); +initializeLoggingConfiguration('log', 'none'); +initializeReferences(); + +%% Name of the Simulink File +mdl = 'nass_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; + +% Identification of the dynamics while change the payload dynamics +% - Change of mass: from 1kg to 50kg +% - Change of resonance frequency: from 50Hz to 500Hz +% - The damping ratio of the payload is fixed to $\xi = 0.2$ + +% We identify the dynamics for the following payload masses =Ms= and nano-hexapod leg's stiffnesses =Ks=: + +Ms = [1, 20, 50]; % [Kg] +Ks = logspace(3,9,7); % [N/m] + +Gm_iff = {zeros(length(Ks), length(Ms))}; +Gm_dvf = {zeros(length(Ks), length(Ms))}; +Gm_err = {zeros(length(Ks), length(Ms))}; + +for i = 1:length(Ks) + for j = 1:length(Ms) + initializeNanoHexapod('k', Ks(i)); + initializeSample('mass', Ms(j), 'freq', 100*ones(6,1)); + + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gm_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gm_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gm_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; + end +end + + + +% We then identify the dynamics for the following payload resonance frequencies =Fs=: + +Fs = [50, 200, 500]; % [Hz] + +Gf_iff = {zeros(length(Ks), length(Fs))}; +Gf_dvf = {zeros(length(Ks), length(Fs))}; +Gf_err = {zeros(length(Ks), length(Fs))}; + +for i = 1:length(Ks) + for j = 1:length(Fs) + initializeNanoHexapod('k', Ks(i)); + initializeSample('mass', 20, 'freq', Fs(j)*ones(6,1)); + + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gf_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gf_dvf(i,j) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gf_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; + end +end + +save('mat/optimal_stiffness_Gm_Gf.mat', 'Ks', 'Ms', 'Fs', ... + 'Gm_iff', 'Gm_dvf', 'Gm_err', ... + 'Gf_iff', 'Gf_dvf', 'Gf_err'); + +% Change of optimal gain for decentralized control +% For each payload, compute the optimal gain for the IFF control. +% The optimal value corresponds to critical damping to *all* the 6 modes of the nano-hexapod. + + +load('mat/optimal_stiffness_Gm_Gf.mat'); + + + +% Change of Mass + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + for j = 1:length(Ms) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + for j = 1:length(Ms) + set(gca,'ColorOrderIndex',i); + if j == 1 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i))); + else + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); + end + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% Change of payload resonance frequency + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + for j = 1:length(Fs) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + for j = 1:length(Fs) + set(gca,'ColorOrderIndex',i); + if j == 1 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i))); + else + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); + end + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Change of dynamics for the primary controller +% For each stiffness, plot the total spread of dynamics. + + +load('mat/optimal_stiffness_Gm_Gf.mat'); + +% Frequency variation +% Same payload mass, but different stiffness resulting in different resonance frequency. + +% All curves + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + for j = 1:length(Fs) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + for j = 1:length(Fs) + set(gca,'ColorOrderIndex',i); + if j == 1 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i))); + else + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); + end + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% X direction + +i = 1; + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 2, 1); +hold on; +for j = 1:length(Fs) + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title(sprintf('$k = %.0e$ [N/m]', Ks(i))) + +ax2 = subplot(2, 2, 3); +hold on; +for j = 1:length(Fs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +i = 7; + +ax1 = subplot(2, 2, 2); +hold on; +for j = 1:length(Fs) + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title(sprintf('$k = %.0e$ [N/m]', Ks(i))) + +ax2 = subplot(2, 2, 4); +hold on; +for j = 1:length(Fs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% Z direction: +% We can see two mass lines for the soft nano-hexapod: +% - The first mass line corresponds to $\frac{1}{(m_n + m_p)s^2}$ where $m_p = 20\ [kg]$ is the mass of the payload and $m_n = 15\ [Kg]$ is the mass of the nano-hexapod top platform and attached mirror +% - The second mass line corresponds to $\frac{1}{m_n s^2}$ + + +i = 1; + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 2, 1); +hold on; +for j = 1:length(Fs) + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title(sprintf('$k = %.0e$ [N/m]', Ks(i))) + +ax2 = subplot(2, 2, 3); +hold on; +for j = 1:length(Fs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +i = 7; + +ax1 = subplot(2, 2, 2); +hold on; +for j = 1:length(Fs) + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title(sprintf('$k = %.0e$ [N/m]', Ks(i))) + +ax2 = subplot(2, 2, 4); +hold on; +for j = 1:length(Fs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$\\omega_0 = %.0f$ [Hz]', Fs(j))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Mass variation +% All mixed, X direction + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + for j = 1:length(Ms) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + for j = 1:length(Ms) + set(gca,'ColorOrderIndex',i); + if j == 1 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i))); + else + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); + end + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% All mixed, Z direction + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for i = 1:length(Ks) + for j = 1:length(Ms) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for i = 1:length(Ks) + for j = 1:length(Ms) + set(gca,'ColorOrderIndex',i); + if j == 1 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$K = %.0e$ [N/m]', Ks(i))); + else + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ... + 'HandleVisibility', 'off'); + end + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% Z direction + +freqs = logspace(-1, 3, 1000); + +figure; + +i = 1; + +ax1 = subplot(2, 2, 1); +hold on; +for j = 1:length(Ms) + plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title(sprintf('$k = %.0e$ [N/m]', Ks(i))) + +ax2 = subplot(2, 2, 3); +hold on; +for j = 1:length(Ms) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +i = 7; + +ax1 = subplot(2, 2, 2); +hold on; +for j = 1:length(Ms) + plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +title(sprintf('$k = %.0e$ [N/m]', Ks(i))) + +ax2 = subplot(2, 2, 4); +hold on; +for j = 1:length(Ms) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ez', 'Fz'), freqs, 'Hz'))), '-', ... + 'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(j))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Total variation +% Total change of dynamics due to change of the payload: +% - mass from 1kg to 50kg +% - main resonance from 50Hz to 500Hz + +% For a soft nano-hexapod + +i = 1; + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for j = 1:length(Fs) + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +end +for j = 1:length(Ms) + plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for j = 1:length(Fs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +for j = 1:length(Ms) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% For a stiff nano-hexapod + +i = 7; + +freqs = logspace(-1, 3, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +for j = 1:length(Fs) + plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +end +for j = 1:length(Ms) + plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +for j = 1:length(Fs) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +for j = 1:length(Ms) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-'); +end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-180, -90, 0, 90, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); diff --git a/org/optimal_stiffness.org b/org/optimal_stiffness.org index 37c1f10..274f296 100644 --- a/org/optimal_stiffness.org +++ b/org/optimal_stiffness.org @@ -22,7 +22,7 @@ #+PROPERTY: header-args:matlab+ :exports both #+PROPERTY: header-args:matlab+ :eval no-export #+PROPERTY: header-args:matlab+ :output-dir figs -#+PROPERTY: header-args:matlab+ :tangle no +#+PROPERTY: header-args:matlab+ :tangle ../matlab/optimal_stiffness.m #+PROPERTY: header-args:matlab+ :mkdirp yes #+PROPERTY: header-args:shell :eval no-export @@ -73,6 +73,7 @@ The overall goal is to design a nano-hexapod that will allow the highest possibl #+end_src #+begin_src matlab + load('mat/conf_simulink.mat'); open('nass_model.slx') #+end_src @@ -89,20 +90,15 @@ We initialize all the stages with the default parameters. initializeMirror(); #+end_src -The worst case scenario is a rotation speed of 60rpm with a payload mass of 1Kg. +The worst case scenario is a rotation speed of 60rpm with a payload mass of 10Kg. #+begin_src matlab initializeSample('mass', 10); #+end_src We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics. #+begin_src matlab - initializeSimscapeConfiguration('gravity', false); + initializeSimscapeConfiguration('gravity', true); initializeDisturbances('enable', false); -#+end_src - -We set the controller type to Open-Loop, and we do not need to log any signal. -#+begin_src matlab - initializeController('type', 'stabilizing'); initializeLoggingConfiguration('log', 'none'); #+end_src @@ -121,20 +117,22 @@ We set the controller type to Open-Loop, and we do not need to log any signal. ** Identification when not rotating We set the range of stiffness that we want to use. #+begin_src matlab - Ks = logspace(3,9,7) + Ks = logspace(3,9,7); % [N/m] #+end_src +We don't move any stage and no controller is used. #+begin_src matlab initializeReferences(); + initializeController(); #+end_src -#+begin_src matlab +#+begin_src matlab :exports none Gk_iff = {zeros(length(Ks))}; Gk_dvf = {zeros(length(Ks))}; Gk_err = {zeros(length(Ks))}; #+end_src -#+begin_src matlab +#+begin_src matlab :exports none for i = 1:length(Ks) initializeNanoHexapod('k', Ks(i)); @@ -156,6 +154,7 @@ We set the range of stiffness that we want to use. #+end_src ** Identification when rotating at maximum speed +We now set the reference path such that the Spindle is rotating at 60rpm and such that it is at the zero position at the time of the identification. #+begin_src matlab Rz_rpm = 60; @@ -168,17 +167,20 @@ We set the range of stiffness that we want to use. t_sim = Rz.time(i_end); % Simulation time before identification [s] #+end_src +We here use a decentralized controller that is used to stabilize the nano-hexapod until the identification is made. +This controller virtually adds stiffness in each of the nano-hexapod leg. #+begin_src matlab k_sta = -1e8; + initializeController('type', 'stabilizing'); #+end_src -#+begin_src matlab +#+begin_src matlab :exports none Gk_wz_iff = {zeros(length(Ks))}; Gk_wz_dvf = {zeros(length(Ks))}; Gk_wz_err = {zeros(length(Ks))}; #+end_src -#+begin_src matlab +#+begin_src matlab :exports none for i = 1:length(Ks) initializeNanoHexapod('k', Ks(i)); @@ -199,13 +201,17 @@ We set the range of stiffness that we want to use. end #+end_src -#+begin_src matlab +#+begin_src matlab :exports none save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ... 'Gk_iff', 'Gk_dvf', 'Gk_err', ... 'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err'); #+end_src -** Change of dynamics +** TODO Change of dynamics +- [ ] problem of dynamics at low frequency + Check if gravity is a problem + Think of a before way to identify the dynamics + #+begin_src matlab :exports none load('mat/optimal_stiffness_Gk_wz.mat'); #+end_src @@ -397,11 +403,11 @@ Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the dir ** 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 @@ -409,9 +415,337 @@ Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the dir #+end_src #+begin_src matlab + load('mat/conf_simulink.mat'); open('nass_model.slx') #+end_src +** Identification of the micro-station compliance +We initialize all the stages with the default parameters. +#+begin_src matlab + initializeGround(); + initializeGranite(); + initializeTy(); + initializeRy(); + initializeRz(); + initializeMicroHexapod('type', 'compliance'); +#+end_src + +We put nothing on top of the micro-hexapod. +#+begin_src matlab + initializeAxisc('type', 'none'); + initializeMirror('type', 'none'); + initializeNanoHexapod('type', 'none'); + initializeSample('type', 'none'); +#+end_src + +#+begin_src matlab :exports none + initializeReferences(); + initializeDisturbances(); + initializeController(); + initializeSimscapeConfiguration(); + initializeLoggingConfiguration(); +#+end_src + +And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point. +#+begin_src matlab :exports noone + %% Name of the Simulink File + mdl = 'nass_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform + io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform + + %% Run the linearization + Gm = linearize(mdl, io, 0); + Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'}; + Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'}; +#+end_src + +#+begin_src matlab :exports none + labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'}; + + freqs = logspace(1, 3, 1000); + + figure; + + hold on; + plot(freqs, abs(squeeze(freqresp(Gm(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', labels{1}); + for i = 2:6 + plot(freqs, abs(squeeze(freqresp(Gm(1, i), freqs, 'Hz'))), 'DisplayName', labels{i}); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); + ylabel('Compliance'); + legend('location', 'northwest'); +#+end_src + +** Identification of the dynamics with a rigid micro-station +*** Initialization +#+begin_src matlab :exports none + initializeReferences(); + initializeDisturbances(); + initializeController(); + initializeSimscapeConfiguration(); + initializeLoggingConfiguration(); + initializeSimscapeConfiguration('gravity', false); +#+end_src + +#+begin_src matlab :exports none + %% Name of the Simulink File + mdl = 'nass_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; +#+end_src + +#+begin_src matlab + Ks = logspace(3,9,7); % [N/m] +#+end_src + +#+begin_src matlab + initializeSample('type', 'rigid', 'mass', 20); +#+end_src + +*** Rigid micro-station +#+begin_src matlab + initializeGround('type', 'rigid'); + initializeGranite('type', 'rigid'); + initializeTy('type', 'rigid'); + initializeRy('type', 'rigid'); + initializeRz('type', 'rigid'); + initializeMicroHexapod('type', 'rigid'); + + initializeAxisc('type', 'rigid'); + initializeMirror('type', 'rigid'); +#+end_src + +#+begin_src matlab :exports none + Gmr_iff = {zeros(length(Ks))}; + Gmr_dvf = {zeros(length(Ks))}; + Gmr_err = {zeros(length(Ks))}; +#+end_src + +#+begin_src matlab :exports none + for i = 1:length(Ks) + initializeNanoHexapod('k', Ks(i)); + + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gmr_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gmr_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gmr_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; + end +#+end_src + +** Identification of the dynamics with a flexible micro-station +*** Flexible micro-station +#+begin_src matlab + initializeGround(); + initializeGranite(); + initializeTy(); + initializeRy(); + initializeRz(); + initializeMicroHexapod(); + + initializeAxisc(); + initializeMirror(); +#+end_src + +#+begin_src matlab :exports none + Gmf_iff = {zeros(length(Ks))}; + Gmf_dvf = {zeros(length(Ks))}; + Gmf_err = {zeros(length(Ks))}; +#+end_src + +#+begin_src matlab :exports none + for i = 1:length(Ks) + initializeNanoHexapod('k', Ks(i)); + + G = linearize(mdl, io); + G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6', ... + 'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6', ... + 'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}; + + Gmf_iff(i) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + Gmf_dvf(i) = {minreal(G({'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))}; + + Jinvt = tf(inv(nano_hexapod.J)'); + Jinvt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + Jinvt.OutputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}; + Gmf_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt}; + end +#+end_src + +#+begin_src matlab :exports none + save('mat/optimal_stiffness_micro_station_compliance.mat', 'Ks', ... + 'Gmr_iff', 'Gmr_dvf', 'Gmr_err', ... + 'Gmf_iff', 'Gmf_dvf', 'Gmf_err'); +#+end_src + +** Obtained Dynamics +#+begin_src matlab :exports none + load('mat/optimal_stiffness_micro_station_compliance.mat'); +#+end_src + +IFF plant +#+begin_src matlab :exports none + freqs = logspace(-1, 3, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); +#+end_src + +DVF plant +#+begin_src matlab :exports none + freqs = logspace(-1, 3, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_dvf{i}('Dnlm1', 'Fnl1'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); +#+end_src + +X direction +#+begin_src matlab :exports none + freqs = logspace(-1, 3, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); +#+end_src + +Z direction +#+begin_src matlab :exports none + freqs = logspace(-1, 3, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:length(Ks) + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmr_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '-'); + set(gca,'ColorOrderIndex',i); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gmf_err{i}('Ez', 'Fz'), freqs, 'Hz'))), '--'); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); +#+end_src + ** Conclusion :ignore: * Payload "Impedance" Effect <> @@ -432,6 +766,7 @@ Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the dir #+end_src #+begin_src matlab + load('mat/conf_simulink.mat'); open('nass_model.slx') #+end_src @@ -448,16 +783,17 @@ We initialize all the stages with the default parameters. initializeMirror(); #+end_src -We don't include gravity nor disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics. +We don't include disturbances in this model as it adds complexity to the simulations and does not alter the obtained dynamics. #+begin_src matlab - initializeSimscapeConfiguration('gravity', false); + initializeSimscapeConfiguration('gravity', true); initializeDisturbances('enable', false); #+end_src We set the controller type to Open-Loop, and we do not need to log any signal. #+begin_src matlab - initializeController('type', 'stabilizing'); + initializeController(); initializeLoggingConfiguration('log', 'none'); + initializeReferences(); #+end_src #+begin_src matlab :exports none @@ -472,27 +808,24 @@ We set the controller type to Open-Loop, and we do not need to log any signal. io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; #+end_src -** Change of payload dynamics +** Identification of the dynamics while change the payload dynamics - Change of mass: from 1kg to 50kg - Change of resonance frequency: from 50Hz to 500Hz - The damping ratio of the payload is fixed to $\xi = 0.2$ -#+begin_src matlab - initializeReferences(); - Ks = logspace(3,9,7) % [N/m] -#+end_src - +We identify the dynamics for the following payload masses =Ms= and nano-hexapod leg's stiffnesses =Ks=: #+begin_src matlab Ms = [1, 20, 50]; % [Kg] + Ks = logspace(3,9,7); % [N/m] #+end_src -#+begin_src matlab +#+begin_src matlab :exports none Gm_iff = {zeros(length(Ks), length(Ms))}; Gm_dvf = {zeros(length(Ks), length(Ms))}; Gm_err = {zeros(length(Ks), length(Ms))}; #+end_src -#+begin_src matlab +#+begin_src matlab :exports none for i = 1:length(Ks) for j = 1:length(Ms) initializeNanoHexapod('k', Ks(i)); @@ -515,17 +848,18 @@ We set the controller type to Open-Loop, and we do not need to log any signal. end #+end_src +We then identify the dynamics for the following payload resonance frequencies =Fs=: #+begin_src matlab Fs = [50, 200, 500]; % [Hz] #+end_src -#+begin_src matlab +#+begin_src matlab :exports none Gf_iff = {zeros(length(Ks), length(Fs))}; Gf_dvf = {zeros(length(Ks), length(Fs))}; Gf_err = {zeros(length(Ks), length(Fs))}; #+end_src -#+begin_src matlab +#+begin_src matlab :exports none for i = 1:length(Ks) for j = 1:length(Fs) initializeNanoHexapod('k', Ks(i)); @@ -548,13 +882,12 @@ We set the controller type to Open-Loop, and we do not need to log any signal. end #+end_src -#+begin_src matlab +#+begin_src matlab :exports none save('mat/optimal_stiffness_Gm_Gf.mat', 'Ks', 'Ms', 'Fs', ... 'Gm_iff', 'Gm_dvf', 'Gm_err', ... 'Gf_iff', 'Gf_dvf', 'Gf_err'); #+end_src -** Plots ** Change of optimal gain for decentralized control For each payload, compute the optimal gain for the IFF control. The optimal value corresponds to critical damping to *all* the 6 modes of the nano-hexapod. @@ -1061,6 +1394,5 @@ For a stiff nano-hexapod xlim([freqs(1), freqs(end)]); #+end_src - ** Conclusion :ignore: - +* Total Change of dynamics