tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add option to initialize spindle rotation speed

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-04-03 14:10:14 +02:00
parent 9c6d397fe4
commit d60bc3bfb0
12 changed files with 464 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mat/Foffset.mat
BIN
View File
Binary file not shown.
mat/conf_log.mat
BIN
View File
Binary file not shown.
mat/conf_simscape.mat
BIN
View File
Binary file not shown.
mat/controller.mat
BIN
View File
Binary file not shown.
mat/nass_disturbances.mat
BIN
View File
Binary file not shown.
mat/nass_references.mat
BIN
View File
Binary file not shown.
mat/optimal_stiffness_Gk_wz.mat
BIN
View File
Binary file not shown.
mat/stages.mat
BIN
View File
Binary file not shown.
matlab/nass_model.slx
BIN
View File
Binary file not shown.
org/optimal_stiffness.org
+414 -198
View File
@@ -100,6 +100,7 @@ We don't include gravity nor disturbances in this model as it adds complexity to
initializeSimscapeConfiguration('gravity', true);
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController();
#+end_src
#+begin_src matlab :exports none
@@ -114,26 +115,26 @@ We don't include gravity nor disturbances in this model as it adds complexity to
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1;
#+end_src
** Identification when not rotating
We set the range of stiffness that we want to use.
** Identification when rotating at maximum speed
#+begin_src matlab
Rz_rpm = linspace(0, 60, 6);
#+end_src
#+begin_src matlab
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 :exports none
Gk_iff = {zeros(length(Ks))};
Gk_dvf = {zeros(length(Ks))};
Gk_err = {zeros(length(Ks))};
Gk_wz_iff = {zeros(length(Ks), length(Rz_rpm))};
Gk_wz_dvf = {zeros(length(Ks), length(Rz_rpm))};
Gk_wz_err = {zeros(length(Ks), length(Rz_rpm))};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ks)
for j = 1:length(Rz_rpm)
initializeReferences('Rz_type', 'rotating-not-filtered', ...
'Rz_period', 60/Rz_rpm(j));
initializeNanoHexapod('k', Ks(i));
%% Run the linearization
@@ -143,113 +144,103 @@ We don't move any stage and no controller is used.
'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'}))};
Gk_wz_iff(i,j) = {minreal(G({'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))};
Gk_wz_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'};
Gk_err(i) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
Gk_wz_err(i,j) = {-minreal(G({'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'}, {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'}))*Jinvt};
end
#+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;
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]
#+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 :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 :exports none
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
#+end_src
#+begin_src matlab :exports none
save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', ...
'Gk_iff', 'Gk_dvf', 'Gk_err', ...
save('mat/optimal_stiffness_Gk_wz.mat', 'Ks', 'Rz_rpm', ...
'Gk_wz_iff', 'Gk_wz_dvf', 'Gk_wz_err');
#+end_src
** 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
** Change of dynamics
#+begin_src matlab :exports none
load('mat/optimal_stiffness_Gk_wz.mat');
#+end_src
Change of dynamics for decentralized IFF control.
Change of dynamics for IFF
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
freqs = logspace(-2, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gk_iff)
for i = 1:length(Ks)
for j = 1:length(Rz_rpm)
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'))), '--');
plot(freqs, abs(squeeze(freqresp(Gk_wz_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-');
end
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)
for i = 1:length(Ks)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '-', ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_iff{i,1}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
for j = 2:length(Rz_rpm)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--', ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
'HandleVisibility', 'off');
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]);
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
Change of dynamics for DVF
#+begin_src matlab :exports none
freqs = logspace(-2, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ks)
for j = 1:length(Rz_rpm)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gk_wz_dvf{i,j}('Dnlm1', '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)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_dvf{i,1}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
for j = 2:length(Rz_rpm)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_dvf{i,j}('Dnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
@@ -264,11 +255,11 @@ Change of dynamics from $F_x$ to $D_x$.
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Gk_err)
for i = 1:length(Ks)
for j = 1:length(Rz_rpm)
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'))), '--');
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
@@ -276,106 +267,43 @@ Change of dynamics from $F_x$ to $D_x$.
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Gk_err)
for i = 1:length(Ks)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
for j = 2:length(Rz_rpm)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gk_wz_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '--', ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), '-', ...
'HandleVisibility', 'off');
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]);
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
Change of dynamics from $F_z$ to $D_z$.
#+begin_src matlab :exports none
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)]);
#+end_src
** Change of coupling
#+begin_src matlab :exports none
load('mat/optimal_stiffness_Gk_wz.mat');
#+end_src
Change of coupling from $F_x$ to $D_y$ when not rotating and when rotating at 60rpm.
Change of coupling from $F_x$ to $D_y$ with the rotating speed.
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
hold on;
for i = 1:length(Gk_err)
for i = 1:length(Ks)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gk_err{i}( 'Ey', 'Fx'), freqs, 'Hz'))), '-', ...
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
for j = 1:length(Rz_rpm)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i}('Ey', 'Fx'), freqs, 'Hz'))), '--', ...
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('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');
#+end_src
Comparison of the coupling from $F_x$ to $D_y$ when rotating at 60rpm to the direct term $F_x$ to $D_x$.
#+begin_src matlab :exports none
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');
@@ -481,8 +409,16 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
legend('location', 'northwest');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/opt_stiff_micro_station_compliance.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:opt_stiff_micro_station_compliance
#+caption: Identified Compliance of the Micro-Station ([[./figs/opt_stiff_micro_station_compliance.png][png]], [[./figs/opt_stiff_micro_station_compliance.pdf][pdf]])
[[file:figs/opt_stiff_micro_station_compliance.png]]
** Identification of the dynamics with a rigid micro-station
*** Initialization
#+begin_src matlab :exports none
initializeReferences();
initializeDisturbances();
@@ -509,10 +445,9 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
#+end_src
#+begin_src matlab
initializeSample('type', 'rigid', 'mass', 20);
initializeSample('type', 'rigid', 'mass', 10);
#+end_src
*** Rigid micro-station
#+begin_src matlab
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
@@ -552,7 +487,6 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
#+end_src
** Identification of the dynamics with a flexible micro-station
*** Flexible micro-station
#+begin_src matlab
initializeGround();
initializeGranite();
@@ -602,7 +536,7 @@ And we identify the dynamics from forces/torques applied on the micro-hexapod to
load('mat/optimal_stiffness_micro_station_compliance.mat');
#+end_src
IFF plant
The IFF plant only changes when the stiffness is $10^7 [N/m]$ or higher.
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
@@ -624,15 +558,18 @@ IFF plant
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'))), '-');
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_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(Gmf_iff{i}('Fnlm1', 'Fnl1'), freqs, 'Hz'))), '--');
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_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]);
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northwest');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -660,15 +597,15 @@ DVF plant
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'))), '-');
plot(freqs, 180/pi*unwrap(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'))), '--');
plot(freqs, 180/pi*unwrap(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]);
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -696,15 +633,15 @@ X direction
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'))), '-');
plot(freqs, 180/pi*unwrap(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'))), '--');
plot(freqs, 180/pi*unwrap(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]);
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -732,15 +669,15 @@ Z direction
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'))), '-');
plot(freqs, 180/pi*unwrap(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'))), '--');
plot(freqs, 180/pi*unwrap(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]);
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -888,7 +825,7 @@ We then identify the dynamics for the following payload resonance frequencies =F
'Gf_iff', 'Gf_dvf', 'Gf_err');
#+end_src
** Change of optimal gain for decentralized control
** TODO 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.
@@ -920,10 +857,10 @@ Change of Mass
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'))), '-', ...
plot(freqs, 180/pi*unwrap(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'))), '-', ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
'HandleVisibility', 'off');
end
end
@@ -931,15 +868,118 @@ Change of Mass
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]);
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
Optimal gains:
#+begin_src matlab
opt_gains = [20 60 200 600 2000 6000 20000];
#+end_src
Change of poles with mass
#+begin_src matlab :exports none
i = 7;
figure;
hold on;
for j = 1:length(Ms)
set(gca,'ColorOrderIndex',j);
cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
#+end_src
#+begin_src matlab :exports none
i = 4;
gains = logspace(1, 3, 500);
figure;
hold on;
for j = 1:length(Ms)
for k = 1:length(gains)
set(gca,'ColorOrderIndex',j);
cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
end
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
#+begin_src matlab :exports none
i = 7;
gains = logspace(3, 5, 500);
figure;
hold on;
for j = 1:length(Ms)
for k = 1:length(gains)
set(gca,'ColorOrderIndex',j);
cl_poles = pole(feedback(Gm_iff{i,j}, (-gains(k)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
end
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
Change of payload resonance frequency
#+begin_src matlab :exports none
i = 1;
gains = logspace(0, 2, 100);
figure;
hold on;
for j = 1:length(Fs)
for k = 1:length(gains)
set(gca,'ColorOrderIndex',j);
cl_poles = pole(feedback(Gf_iff{i,j}, (-gains(k)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
end
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
#+begin_src matlab :exports none
i = 7;
gains = logspace(3, 5, 100);
figure;
hold on;
for j = 1:length(Fs)
for k = 1:length(gains)
set(gca,'ColorOrderIndex',j);
cl_poles = pole(feedback(Gf_iff{i,j}, (-gains(k)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(k)*ones(size(poles_damp)), poles_damp, '.');
end
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
@@ -963,10 +1003,10 @@ Change of payload resonance frequency
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'))), '-', ...
plot(freqs, 180/pi*unwrap(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'))), '-', ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_iff{i,j}('Fnlm1', 'Fnl1'), freqs, 'Hz')))), '-', ...
'HandleVisibility', 'off');
end
end
@@ -974,9 +1014,9 @@ Change of payload resonance frequency
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');
ylim([-270, 90]);
yticks([-360:90:360]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -1341,16 +1381,16 @@ For a soft nano-hexapod
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-');
plot(freqs, 180/pi*unwrap(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-');
plot(freqs, 180/pi*unwrap(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]);
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -1379,16 +1419,16 @@ For a stiff nano-hexapod
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-');
plot(freqs, 180/pi*unwrap(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-');
plot(freqs, 180/pi*unwrap(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]);
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
@@ -1396,3 +1436,179 @@ For a stiff nano-hexapod
** Conclusion :ignore:
* Total Change of dynamics
#+begin_src matlab :exports none
load('mat/optimal_stiffness_Gm_Gf.mat');
load('mat/optimal_stiffness_micro_station_compliance.mat');
load('mat/optimal_stiffness_Gk_wz.mat');
#+end_src
- =Gk_wz_err= - Change of spindle rotation speed
- =Gf_err= - Change of payload resonance
- =Gm_err= - Change of payload mass
- =Gmr_err= - Rigid Micro-Station
- =Gmf_err= - Flexible Micro-Station
Soft nano-hexapod
#+begin_src matlab :exports none
i = 1;
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
% =Gf_err= - Change of payload resonance
plot(freqs, abs(squeeze(freqresp(Gf_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Payload Freq');
for j = 2:length(Fs)
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'HandleVisibility', 'off');
end
% =Gm_err= - Change of payload mass
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Gm_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Payload Mass');
for j = 2:length(Ms)
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'HandleVisibility', 'off');
end
% =Gm_err= - Change of payload mass
set(gca,'ColorOrderIndex',3);
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,1}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Rotationg Speed');
for j = 2:length(Rz_rpm)
set(gca,'ColorOrderIndex',3);
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',4);
% =Gmr_err= - Rigid Micro-Station
plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Rigid $\mu$-station');
% =Gmf_err= - Flexible Micro-Station
plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), '-', ...
'DisplayName', 'Flexible $\mu$-station');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest');
ax2 = subplot(2, 1, 2);
hold on;
for j = 1:length(Rz_rpm)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
end
for j = 1:length(Fs)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
end
% =Gm_err= - Change of payload mass
for j = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
end
% =Gmr_err= - Rigid Micro-Station
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
% =Gmf_err= - Flexible Micro-Station
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
#+begin_src matlab :exports none
i = 1;
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
% =Gf_err= - Change of payload resonance
for j = 1:length(Fs)
plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
% =Gm_err= - Change of payload mass
for j = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
% Spindle Rotation Speed
for j = 1:length(Rz_rpm)
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
end
% =Gmr_err= - Rigid Micro-Station
plot(freqs, abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
% =Gmf_err= - Flexible Micro-Station
plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz'))), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
% =Gf_err= - Change of payload resonance
for j = 1:length(Fs)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
end
% =Gm_err= - Change of payload mass
for j = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
end
% Spindle Rotation Speed
for j = 1:length(Rz_rpm)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
end
% =Gmr_err= - Rigid Micro-Station
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
% =Gmf_err= - Flexible Micro-Station
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))), 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
#+end_src
Comparison with initial TF
#+begin_src matlab :exports none
i = 1;
G0 = abs(squeeze(freqresp(Gmr_err{i}('Ex', 'Fx'), freqs, 'Hz')));
freqs = logspace(-1, 3, 1000);
figure;
hold on;
% =Gf_err= - Change of payload resonance
for j = 1:length(Fs)
plot(freqs, abs(squeeze(freqresp(Gf_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
end
% =Gm_err= - Change of payload mass
for j = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
end
% Spindle Rotation Speed
for j = 1:length(Rz_rpm)
plot(freqs, abs(squeeze(freqresp(Gk_wz_err{i,j}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
end
% =Gmf_err= - Flexible Micro-Station
plot(freqs, abs(squeeze(freqresp(Gmf_err{i}('Ex', 'Fx'), freqs, 'Hz')))./G0, 'k-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
xlim([freqs(1), freqs(end)]);
ylim([1e-2, 1e2])
#+end_src
- [ ] Make a gif with the stiffness varying
org/simscape_subsystems.org
+12 -1
View File
@@ -1593,7 +1593,7 @@ The =controller= structure is saved.
% 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'})} = 'constant'
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]
@@ -1721,6 +1721,17 @@ The =controller= structure is saved.
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;
src/initializeReferences.m
+12 -1
View File
@@ -18,7 +18,7 @@ arguments
% 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'})} = 'constant'
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]
@@ -121,6 +121,17 @@ switch args.Rz_type
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;