Work on HAC-DVF control architecture

This commit is contained in:
Thomas Dehaeze 2020-04-14 17:22:53 +02:00
parent 94eb5e16a7
commit 163b80d8a2
13 changed files with 645 additions and 254 deletions

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 96 KiB

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

BIN
mat/tomo_exp_hac_dvf.mat Normal file

Binary file not shown.

Binary file not shown.

View File

@ -81,18 +81,12 @@ The nano-hexapod is considered to be a rigid body.
initializeSample('mass', 1);
#+end_src
We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src
No controller is used (Open Loop).
#+begin_src matlab
initializeController('type', 'open-loop');
#+end_src
And we put some gravity.
We don't gravity.
#+begin_src matlab
initializeSimscapeConfiguration('gravity', false);
#+end_src
@ -121,6 +115,12 @@ And we initialize the disturbances to be equal to zero.
);
#+end_src
We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src
We simulate the model.
#+begin_src matlab
sim('nass_model');
@ -202,19 +202,25 @@ And we save the obtained data.
In this section, we also perform a tomography experiment with the sample's center of mass aligned with the rotation axis.
However this time, we include perturbations such as ground motion and stage vibrations.
** Simulation Setup
** TODO Simulation Setup
We now activate the disturbances.
#+begin_src matlab
initializeDisturbances(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
#+end_src
We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src
We simulate the model.
#+begin_src matlab
sim('nass_model');
@ -292,11 +298,106 @@ And we save the obtained data.
[[file:figs/exp_tomo_dist.png]]
** Conclusion
#+begin_important
Error motion is what expected from the disturbance measurements.
#+end_important
* Tomography Experiment with Ty raster scans
<<sec:tomo_dist_ty_scans>>
** Introduction :ignore:
In this section, we also perform a tomography experiment with scans of the Translation stage.
All the perturbations are included.
** Simulation Setup
We now activate the disturbances.
#+begin_src matlab
initializeDisturbances(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
#+end_src
We initialize the reference path for all the stages.
The Spindle which is rotating at 60rpm and the translation stage is following a triangular path.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1, ...
'Dy_type', 'triangular', 'Dy_amplitude', 5e-3, 'Dy_period', 10);
#+end_src
We simulate the model.
#+begin_src matlab
sim('nass_model');
#+end_src
And we save the obtained data.
#+begin_src matlab
scans_rz_align_dist = simout;
save('./mat/experiment_tomography.mat', 'scans_rz_align_dist', '-append');
#+end_src
** Analysis
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'scans_rz_align_dist');
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 1))
hold off;
ylabel('Displacement $\epsilon_x$ [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 2))
hold off;
ylabel('Displacement $\epsilon_y$ [m]');
ax3 = subplot(2, 3, 3);
hold on;
plot(scans_rz_align_dist.Em.Eg.Time, scans_rz_align_dist.Em.Eg.Data(:, 3))
hold off;
ylabel('Displacement $\epsilon_z$ [m]');
ax4 = subplot(2, 3, 4);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 4))
hold off;
ylabel('Rotation $\epsilon_{R_x}$ [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 5))
hold off;
xlabel('Time [s]');
ylabel('Rotation $\epsilon_{R_y}$ [rad]');
ax6 = subplot(2, 3, 6);
hold on;
plot(scans_rz_align_dist.Em.En.Time, scans_rz_align_dist.Em.En.Data(:, 6))
hold off;
ylabel('Rotation $\epsilon_{R_z}$ [rad]');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([0.5, inf]);
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_scans_rz_dist.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:exp_scans_rz_dist
#+CAPTION: X-Y-Z translations and rotations of the sample w.r.t. the granite when performing tomography experiment and scans with the translation stage at the same time ([[./figs/exp_scans_rz_dist.png][png]], [[./figs/exp_scans_rz_dist.pdf][pdf]])
[[file:figs/exp_scans_rz_dist.png]]
** Conclusion
* Tomography when the micro-hexapod is not centered
<<sec:tomo_hexa_trans>>
** Introduction :ignore:

View File

@ -28,7 +28,7 @@
* Introduction :ignore:
* Low Authority Control - Decentralized Integral Force Feedback
* Low Authority Control - Decentralized Direct Velocity Feedback
** Introduction :ignore:
** Matlab Init :noexport:ignore:
@ -62,30 +62,37 @@ We initialize all the stages with the default parameters.
initializeMirror();
#+end_src
We set the references that corresponds to a tomography experiment.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 1);
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
#+end_src
#+begin_src matlab
initializeController('type', 'hac-iff');
initializeController('type', 'hac-dvf');
#+end_src
We set the stiffness of the payload fixation:
#+begin_src matlab
Kp = 1e8; % [N/m]
#+end_src
** Identification
#+begin_src matlab
Kx = tf(zeros(6));
Kiff = tf(zeros(6));
K = tf(zeros(6));
Kdvf = tf(zeros(6));
#+end_src
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
Gm_iff = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
Gm_dvf = {zeros(length(Ms), 1)};
#+end_src
The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
initializeNanoHexapod('k', 1e5, 'c', 2e2);
#+end_src
@ -97,25 +104,22 @@ We set the references that corresponds to a tomography experiment.
%% 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; % Force Sensors
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', 200*ones(6,1));
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
Gm_iff(i) = {G_iff};
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
Gm_dvf(i) = {G_dvf};
end
#+end_src
#+begin_src matlab :exports none
save('./mat/optimal_stiffness_control.mat', 'Gm_iff');
#+end_src
** Controller Design
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
@ -125,17 +129,16 @@ We set the references that corresponds to a tomography experiment.
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_iff{i}(1, 1), freqs, 'Hz')))), ...
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_dvf{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
end
hold off;
@ -152,19 +155,19 @@ Root Locus
#+begin_src matlab :exports none :post
figure;
gains = logspace(0, 3, 300);
gains = logspace(1, 4, 300);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(real(pole(Gm_iff{i})), imag(pole(Gm_iff{i})), 'x', ...
plot(real(pole(Gm_dvf{i})), imag(pole(Gm_dvf{i})), 'x', ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(real(tzero(Gm_iff{i})), imag(tzero(Gm_iff{i})), 'o', ...
plot(real(tzero(Gm_dvf{i})), imag(tzero(Gm_dvf{i})), 'o', ...
'HandleVisibility', 'off');
for k = 1:length(gains)
set(gca,'ColorOrderIndex',i);
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
@ -178,13 +181,13 @@ Root Locus
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
exportFig('figs/opt_stdvf_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:opt_stiff_iff_root_locus
#+name: fig:opt_stdvf_dvf_root_locus
#+caption: Root Locus for the
#+RESULTS:
[[file:figs/opt_stiff_iff_root_locus.png]]
[[file:figs/opt_stdvf_dvf_root_locus.png]]
Damping as function of the gain
#+begin_src matlab :exports none
@ -199,48 +202,38 @@ Damping as function of the gain
figure;
gains = logspace(0, 3, 100);
gains = logspace(1, 4, 100);
hold on;
for i = 1:length(Ms)
for k = 1:length(gains)
cl_poles = pole(feedback(Gm_iff{i}, -(gains(k)/s)*eye(6)));
cl_poles = pole(feedback(Gm_dvf{i}, (gains(k)*s)*eye(6)));
set(gca,'ColorOrderIndex',i);
plot(gains(k), sin(-pi/2 + angle(cl_poles)), '.', 'color', colors(i, :));
end
end
hold off;
xlabel('IFF Gain'); ylabel('Modal Damping');
xlabel('DVF Gain'); ylabel('Modal Damping');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylim([0, 1]);
#+end_src
#+begin_src matlab
Kiff = -200/s*eye(6);
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
#+end_src
* Primary Control
* Identification of the dynamics for the Primary controller
** Introduction :ignore:
Let's identify the dynamics from actuator forces $\bm{\tau}$ to displacement as measured by the metrology $\bm{\mathcal{X}}$:
\[ \bm{G}(s) = \frac{\bm{\mathcal{X}}}{\bm{\tau}} \]
Then, we compute both the transfer function from forces applied by the actuators $\bm{\mathcal{F}}$ to the measured position error in the frame of the nano-hexapod $\bm{\epsilon}_{\mathcal{X}_n}$:
\[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
and from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
\[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
** Identification
#+begin_src matlab
Gm_x = {zeros(length(Ms), 1)};
Gm_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
#+end_src
We identify these dynamics with and without using the DVF controller.
#+begin_src matlab :exports none
%% Name of the Simulink File
@ -252,9 +245,55 @@ Damping as function of the gain
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
#+end_src
** Identification of the undamped plant :ignore:
#+begin_src matlab :exports none
Kdvf_backup = Kdvf;
Kdvf = tf(zeros(6));
#+end_src
#+begin_src matlab :exports none
G_x = {zeros(length(Ms), 1)};
G_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', 200*ones(6,1));
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Gx = -G*inv(nano_hexapod.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_x(i) = {Gx};
Gl = -nano_hexapod.J*G;
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
G_l(i) = {Gl};
end
#+end_src
#+begin_src matlab :exports none
Kdvf = Kdvf_backup;
#+end_src
** Identification of the damped plant :ignore:
#+begin_src matlab :exports none
Gm_x = {zeros(length(Ms), 1)};
Gm_l = {zeros(length(Ms), 1)};
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
@ -271,71 +310,107 @@ Damping as function of the gain
end
#+end_src
** Controller in the task space
** Obtained dynamics for the Undamped plant :ignore:
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
ax2 = subplot(2, 2, 3);
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_z/\mathcal{F}_z$')
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), 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]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ylim([-270, 90]);
yticks([-360:90:360]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
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', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Translation
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
freqs = logspace(0, 3, 5000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), 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(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
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', 'southwest');
linkaxes([ax1,ax2],'x');
#+end_src
* Primary Control in the task space
** Introduction :ignore:
** Plant in the task space
Let's look $\bm{G}_\mathcal{X}(s)$.
#+begin_src matlab :exports none
freqs = logspace(0, 3, 5000);
figure;
@ -394,85 +469,6 @@ Damping as function of the gain
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab
Kx = tf(zeros(6));
h = 1.5;
Kx(1,1) = 2e6 * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
(s/2/pi/10 + 1)/(s/2/pi/10);
Kx(2,2) = Kx(1,1);
h = 1.5;
Kx(3,3) = 1e7 * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
(s/2/pi/10 + 1)/(s/2/pi/10);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('$\mathcal{X}_z/\mathcal{F}_z$')
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
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', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Rotations
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
@ -533,20 +529,95 @@ Damping as function of the gain
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
** Control in the task space
#+begin_src matlab
Kx = tf(zeros(6));
h = 2.5;
Kx(1,1) = 3e7 * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1);
Kx(2,2) = Kx(1,1);
h = 2.5;
Kx(3,3) = 3e7 * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1);
#+end_src
#+begin_src matlab
h = 1.5;
Kx(4,4) = 1e5 * ...
Kx(4,4) = 5e5 * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
(s/2/pi/10 + 1)/(s/2/pi/10);
(s/2/pi/1 + 1)/(s/2/pi/1);
Kx(5,5) = Kx(4,4);
h = 1.5;
Kx(6,6) = 2e5 * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
(s/2/pi/10 + 1)/(s/2/pi/10);
Kx(6,6) = 5e4 * ...
1/h*(s/(2*pi*30/h) + 1)/(s/(2*pi*30*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
title('Loop Gain $x$ and $y$')
ax2 = subplot(2, 2, 2);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
title('Loop Gain $z$')
ax3 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(1, 1)*Kx(1,1), freqs, 'Hz')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(2, 2)*Kx(2,2), freqs, 'Hz')))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-270, 90]);
yticks([-360:90:360]);
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_x{i}(3, 3)*Kx(3,3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
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', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :exports none
@ -618,67 +689,8 @@ Damping as function of the gain
** Simulation
** Control in the leg space
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), ...
'DisplayName', sprintf('$d\\mathcal{L}_%i / \\tau_%i$', i, i));
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();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
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,ax3,ax4],'x');
#+end_src
* Primary Control in the leg space
** Plant in the task space
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
@ -702,24 +714,26 @@ Damping as function of the gain
for i = 1:length(Ms)
for j = 1:6
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz'))));
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm_l{i}(j, j), freqs, 'Hz')))));
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');
#+end_src
** Control in the leg space
#+begin_src matlab
h = 1.5;
Kl = 5e6 * eye(6) * ...
Kl = 2e7 * eye(6) * ...
1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
(s/2/pi/1 + 1)/(s/2/pi/1) * ...
(s/2/pi/10 + 1)/(s/2/pi/10);
(s/2/pi/10 + 1)/(s/2/pi/10) * ...
1/(1 + s/2/pi/500);
#+end_src
#+begin_src matlab
@ -762,5 +776,281 @@ Damping as function of the gain
linkaxes([ax1,ax2],'x');
#+end_src
** Simulations
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
K = Kl*nano_hexapod.J;
#+end_src
* Simulations
#+begin_src matlab
initializeDisturbances('Fty_x', false, 'Fty_z', false);
initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
#+begin_src matlab
hac_dvf_L = {zeros(length(Ms)), 1};
for i = 1:length(Ms)
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
initializeReferences('Rz_type', 'rotating', 'Rz_period', Ms(i));
sim('nass_model');
hac_dvf_L(i) = {simout};
end
#+end_src
#+begin_src matlab
save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
#+end_src
** Results
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
#+end_src
#+begin_src matlab
n_av = 4;
han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
#+end_src
#+begin_src matlab
t = simout.Em.En.Time;
Ts = t(2)-t(1);
[pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
pxx_dvf_L = zeros(length(f), 6, length(Ms));
for i = 1:length(Ms)
[pxx, ~] = pwelch(hac_dvf_L{i}.Em.En.Data(ceil(0.2/Ts):end,:), han_win, [], [], 1/Ts);
pxx_dvf_L(:, :, i) = pxx;
end
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(pxx_ol(:, 1)))
for i = 1:length(Ms)
plot(f, sqrt(pxx_dvf_L(:, 1, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_x}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(pxx_ol(:, 2)))
for i = 1:length(Ms)
plot(f, sqrt(pxx_dvf_L(:, 2, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(pxx_ol(:, 3)))
for i = 1:length(Ms)
plot(f, sqrt(pxx_dvf_L(:, 3, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(pxx_ol(:, 4)))
for i = 1:length(Ms)
plot(f, sqrt(pxx_dvf_L(:, 4, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(pxx_ol(:, 5)))
for i = 1:length(Ms)
plot(f, sqrt(pxx_dvf_L(:, 5, i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
end
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
for i = 1:length(Ms)
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_x$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
for i = 1:length(Ms)
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_y$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
for i = 1:length(Ms)
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_z$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
for i = 1:length(Ms)
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_x$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
for i = 1:length(Ms)
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_y$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
end
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_z$ [$rad$]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylim([1e-11, 1e-5]);
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
for i = 1:length(Ms)
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
end
hold off;
xlabel('Time [s]');
ylabel('Dx [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
for i = 1:length(Ms)
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
end
hold off;
xlabel('Time [s]');
ylabel('Dy [m]');
ax3 = subplot(2, 3, 3);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
for i = 1:length(Ms)
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
end
hold off;
xlabel('Time [s]');
ylabel('Dz [m]');
ax4 = subplot(2, 3, 4);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
for i = 1:length(Ms)
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
end
hold off;
xlabel('Time [s]');
ylabel('Rx [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
for i = 1:length(Ms)
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
end
hold off;
xlabel('Time [s]');
ylabel('Ry [rad]');
ax6 = subplot(2, 3, 6);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), ...
'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
end
hold off;
xlabel('Time [s]');
ylabel('Rz [rad]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src