tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
master
nass-simscape/org/control_virtual_mass.org

712 lines
22 KiB
Org Mode
Raw Permalink Normal View History

Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
#+TITLE: Decentralize control to add virtual mass
#+SETUPFILE: ./setup/org-setup-file.org
* 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
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
#+begin_src matlab
load('mat/conf_simulink.mat');
open('nass_model.slx')
#+end_src
* Initialization
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeSimscapeConfiguration();
initializeDisturbances('enable', false);
initializeLoggingConfiguration('log', 'none');
initializeController('type', 'hac-dvf');
#+end_src
Correct stupid error
2020-04-17 14:32:08 +02:00
The nano-hexapod has the following leg's stiffness and damping.
#+begin_src matlab
initializeNanoHexapod('k', 1e5, 'c', 2e2);
#+end_src
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
We set the stiffness of the payload fixation:
#+begin_src matlab
Kp = 1e8; % [N/m]
#+end_src
Correct stupid error
2020-04-17 14:32:08 +02:00
#+begin_src matlab :exports none
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
K = tf(zeros(6));
Kdvf = tf(zeros(6));
#+end_src
Correct stupid error
2020-04-17 14:32:08 +02:00
* Identification
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
We identify the system for the following payload masses:
#+begin_src matlab
Ms = [1, 10, 50];
#+end_src
Correct stupid error
2020-04-17 14:32:08 +02:00
Identification of the transfer function from $\tau$ to $d\mathcal{L}$.
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
#+begin_src matlab :exports none
Gm = {zeros(length(Ms), 1)};
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
Correct stupid error
2020-04-17 14:32:08 +02:00
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;
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
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
Correct stupid error
2020-04-17 14:32:08 +02:00
G = linearize(mdl, io);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
Gm(i) = {G};
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
end
#+end_src
Correct stupid error
2020-04-17 14:32:08 +02:00
Identification of the Primary plant without virtual add of mass
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
#+begin_src matlab :exports none
G_x = {zeros(length(Ms), 1)};
G_l = {zeros(length(Ms), 1)};
Correct stupid error
2020-04-17 14:32:08 +02:00
load('mat/stages.mat', 'nano_hexapod');
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
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);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gx = -G*inv(nano_hexapod.kinematics.J');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_x(i) = {Gx};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gl = -nano_hexapod.kinematics.J*G;
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
G_l(i) = {Gl};
end
#+end_src
* Adding Virtual Mass in the Leg's Space
** Plant
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm{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)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm{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', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_plant_L.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_plant_L
#+caption: Transfer function from $\tau_i$ to $d\mathcal{L}_i$ for three payload masses
#+RESULTS:
[[file:figs/virtual_mass_plant_L.png]]
** Controller Design
#+begin_src matlab
Kdvf = 10*s^2/(1+s/2/pi/500)^2*eye(6);
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(Gm{i}*Kdvf, eye(6), -1))
end
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 4, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:length(Ms)
plot(freqs, abs(squeeze(freqresp(Gm{i}(1, 1)*Kdvf(1,1), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:length(Ms)
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm{i}(1, 1)*Kdvf(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([-180, 180]);
yticks([-360:90:360]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_loop_gain_L.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_loop_gain_L
#+caption: Loop Gain for the addition of virtual mass in the leg's space
#+RESULTS:
[[file:figs/virtual_mass_loop_gain_L.png]]
** Identification of the Primary Plant
#+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, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
load('mat/stages.mat', 'nano_hexapod');
GmL_x = {zeros(length(Ms), 1)};
GmL_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-not-filtered', 'Rz_period', Ms(i));
%% Run the linearization
G = linearize(mdl, io);
Correct stupid error
2020-04-17 14:32:08 +02:00
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gx = -G*inv(nano_hexapod.kinematics.J');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
GmL_x(i) = {Gx};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gl = -nano_hexapod.kinematics.J*G;
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
GmL_l(i) = {Gl};
end
#+end_src
#+begin_src matlab :exports none
Correct stupid error
2020-04-17 14:32:08 +02:00
freqs = logspace(0, 3, 1000);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
figure;
ax1 = subplot(2, 2, 1);
hold on;
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'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmL_x{i}(1, 1), freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmL_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('$\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(G_x{i}(3, 3), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmL_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')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_x{i}(1, 1), freqs, 'Hz')))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_x{i}(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(G_x{i}(3, 3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_x{i}(3, 3), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
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 :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_L_primary_plant_X.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_L_primary_plant_X
#+caption: Comparison of the transfer function from $\mathcal{F}_{x,y,z}$ to $\mathcal{X}_{x,y,z}$ with and without the virtual addition of mass in the leg's space
#+RESULTS:
[[file:figs/virtual_mass_L_primary_plant_X.png]]
#+begin_src matlab :exports none
Correct stupid error
2020-04-17 14:32:08 +02:00
freqs = logspace(0, 3, 1000);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
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'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmL_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)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_l{i}(1, 1), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
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
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_L_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_L_primary_plant_L
#+caption: Comparison of the transfer function from $\tau_i$ to $\mathcal{L}_{i}$ with and without the virtual addition of mass in the leg's space
#+RESULTS:
[[file:figs/virtual_mass_L_primary_plant_L.png]]
* Adding Virtual Mass in the Task Space
** Plant
Let's look at the transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}}$:
\[ \frac{d\bm{\mathcal{L}}}{\bm{\mathcal{F}}} = \bm{J}^{-1} \frac{d\bm{\mathcal{L}}}{\bm{\tau}} \bm{J}^{-T} \]
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
GmX = {zeros(length(Ms), 1)};
for i = 1:length(Ms)
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
GmX(i) = {inv(nano_hexapod.kinematics.J) * Gm{i} * inv(nano_hexapod.kinematics.J')};
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
end
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX{i}(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX{i}(2, 2), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX{i}(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX{i}(2, 2), freqs, 'Hz')))), ...
'HandleVisibility', 'off');
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');
Correct stupid error
2020-04-17 14:32:08 +02:00
ax3 = subplot(2, 2, 2);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX{i}(3, 3), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
Correct stupid error
2020-04-17 14:32:08 +02:00
ax4 = subplot(2, 2, 4);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX{i}(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', 'northeast');
Correct stupid error
2020-04-17 14:32:08 +02:00
linkaxes([ax1,ax2,ax3,ax4],'x');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_plant_X.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_plant_X
#+caption: Dynamics from $\mathcal{F}_{x,y,z}$ to $\mathcal{X}_{x,y,z}$ used for virtual mass addition in the task space
#+RESULTS:
[[file:figs/virtual_mass_plant_X.png]]
** Controller Design
#+begin_src matlab
KmX = (s^2*1/(1+s/2/pi/500)^2*diag([1 1 50 0 0 0]));
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(GmX{i}*KmX, eye(6), -1))
end
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(LmX(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(LmX(2, 2), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Correct stupid error
2020-04-17 14:32:08 +02:00
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Correct stupid error
2020-04-17 14:32:08 +02:00
ax3 = subplot(2, 2, 3);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(LmX(1, 1), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(LmX(2, 2), freqs, 'Hz')))), ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
Correct stupid error
2020-04-17 14:32:08 +02:00
ylim([-180, 180]);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
yticks([-360:90:360]);
Correct stupid error
2020-04-17 14:32:08 +02:00
legend('location', 'southwest');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Correct stupid error
2020-04-17 14:32:08 +02:00
ax2 = subplot(2, 2, 2);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(LmX(3, 3), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Correct stupid error
2020-04-17 14:32:08 +02:00
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Correct stupid error
2020-04-17 14:32:08 +02:00
ax4 = subplot(2, 2, 4);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
hold on;
for i = 1:length(Ms)
LmX = GmX{i}*KmX;
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(LmX(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]');
Correct stupid error
2020-04-17 14:32:08 +02:00
ylim([-180, 180]);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
yticks([-360:90:360]);
Correct stupid error
2020-04-17 14:32:08 +02:00
legend('location', 'southwest');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Correct stupid error
2020-04-17 14:32:08 +02:00
linkaxes([ax1,ax2,ax3,ax4],'x');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_loop_gain_X.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_loop_gain_X
#+caption: Loop gain for virtual mass addition in the task space
#+RESULTS:
[[file:figs/virtual_mass_loop_gain_X.png]]
#+begin_src matlab
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Kdvf = inv(nano_hexapod.kinematics.J')*KmX*inv(nano_hexapod.kinematics.J);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(Gm{i}*Kdvf, eye(6), -1))
end
#+end_src
** Identification of the Primary Plant
#+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, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
load('mat/stages.mat', 'nano_hexapod');
GmX_x = {zeros(length(Ms), 1)};
GmX_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-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'};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gx = -G*inv(nano_hexapod.kinematics.J');
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
GmX_x(i) = {Gx};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gl = -nano_hexapod.kinematics.J*G;
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
GmX_l(i) = {Gl};
end
#+end_src
#+begin_src matlab :exports none
Correct stupid error
2020-04-17 14:32:08 +02:00
freqs = logspace(0, 3, 1000);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
figure;
ax1 = subplot(2, 2, 1);
hold on;
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'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX_x{i}(1, 1), freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX_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('$\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(G_x{i}(3, 3), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX_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')))));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_x{i}(1, 1), freqs, 'Hz')))), '--');
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_x{i}(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(G_x{i}(3, 3), freqs, 'Hz')))), ...
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_x{i}(3, 3), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
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 :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_X_primary_plant_X.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_X_primary_plant_X
#+caption: Comparison of the transfer function from $\mathcal{F}_{x,y,z}$ to $\mathcal{X}_{x,y,z}$ with and without the virtual addition of mass in the task space
#+RESULTS:
[[file:figs/virtual_mass_X_primary_plant_X.png]]
#+begin_src matlab :exports none
Correct stupid error
2020-04-17 14:32:08 +02:00
freqs = logspace(0, 3, 1000);
Add analysis about virtual mass addition
2020-04-17 14:11:34 +02:00
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'))));
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(GmX_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)));
set(gca,'ColorOrderIndex',i);
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_l{i}(1, 1), freqs, 'Hz')))), '--', ...
'HandleVisibility', 'off');
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
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/virtual_mass_X_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:virtual_mass_X_primary_plant_L
#+caption: Comparison of the transfer function from $\tau_i$ to $\mathcal{L}_{i}$ with and without the virtual addition of mass in the task space
#+RESULTS:
[[file:figs/virtual_mass_X_primary_plant_L.png]]
Reference in New Issue Copy Permalink