nass-simscape/org/control_voice_coil.org

2301 lines
73 KiB
Org Mode

#+TITLE: Control of the NASS with Voice coil actuators
#+SETUPFILE: ./setup/org-setup-file.org
* Introduction :ignore:
* HAC-LAC + Cascade Control Topology
** Introduction :ignore:
#+name: fig:cascade_control_architecture
#+caption: Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs
#+RESULTS:
[[file:figs/cascade_control_architecture.png]]
** 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
open('nass_model.slx')
#+end_src
** Initialization
We initialize all the stages with the default parameters.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
#+end_src
The nano-hexapod is a voice coil based hexapod and the sample has a mass of 1kg.
#+begin_src matlab
initializeNanoHexapod('actuator', 'lorentz');
initializeSample('mass', 1);
#+end_src
We set the references that corresponds to a tomography experiment.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src
#+begin_src matlab
initializeDisturbances();
#+end_src
#+begin_src matlab
initializeController('type', 'cascade-hac-lac');
#+end_src
#+begin_src matlab
initializeSimscapeConfiguration('gravity', true);
#+end_src
We log the signals.
#+begin_src matlab
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
** Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
<<sec:lac_iff>>
*** Identification
Let's first identify the plant for the IFF controller.
#+begin_src matlab
%% 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; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
%% Run the linearization
G_iff = linearize(mdl, io, 0);
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
#+end_src
*** Plant
The obtained plant for IFF is shown in Figure [[fig:cascade_vc_iff_plant]].
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_iff(i, i), 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');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%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('location', 'northeast');
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/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(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_iff_plant
#+caption: IFF Plant ([[./figs/cascade_vc_iff_plant.png][png]], [[./figs/cascade_vc_iff_plant.pdf][pdf]])
[[file:figs/cascade_vc_iff_plant.png]]
*** Root Locus
As seen in the root locus (Figure [[fig:cascade_vc_iff_root_locus]], no damping can be added to modes corresponding to the resonance of the micro-station.
However, critical damping can be achieve for the resonances of the nano-hexapod as shown in the zoomed part of the root (Figure [[fig:cascade_vc_iff_root_locus]], left part).
The maximum damping is obtained for a control gain of $\approx 70$.
#+begin_src matlab :exports none
gains = logspace(0, 4, 500);
figure;
subplot(1, 2, 1);
hold on;
plot(real(pole(G_iff)), imag(pole(G_iff)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*500]);
xlim([-2*pi*500,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
subplot(1, 2, 2);
hold on;
plot(real(pole(G_iff)), imag(pole(G_iff)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*8]);
xlim([-2*pi*8,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_root_locus.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_iff_root_locus
#+caption: Root Locus for the IFF control ([[./figs/cascade_vc_iff_root_locus.png][png]], [[./figs/cascade_vc_iff_root_locus.pdf][pdf]])
[[file:figs/cascade_vc_iff_root_locus.png]]
*** Controller and Loop Gain
We create the $6 \times 6$ diagonal Integral Force Feedback controller.
The obtained loop gain is shown in Figure [[fig:cascade_vc_iff_loop_gain]].
#+begin_src matlab
Kiff = -70/s*eye(6);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), 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');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_iff_loop_gain
#+caption: Obtained Loop gain the IFF Control ([[./figs/cascade_vc_iff_loop_gain.png][png]], [[./figs/cascade_vc_iff_loop_gain.pdf][pdf]])
[[file:figs/cascade_vc_iff_loop_gain.png]]
** High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
<<sec:hac_joint_space>>
*** Identification of the damped plant
Let's identify the dynamics from $\bm{\tau}^\prime$ to $d\bm{\mathcal{L}}$ as shown in Figure [[fig:cascade_control_architecture]].
#+begin_src matlab
%% 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, '/Micro-Station'], 3, 'output', [], 'Dnlm'); io_i = io_i + 1; % Leg Displacement
%% Run the linearization
Gl = linearize(mdl, io, 0);
Gl.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
Gl.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src
There are some unstable poles in the Plant with very small imaginary parts.
These unstable poles are probably not physical, and they disappear when taking the minimum realization of the plant.
#+begin_src matlab
isstable(Gl)
Gl = minreal(Gl);
isstable(Gl)
#+end_src
*** Obtained Plant
The obtained dynamics is shown in Figure [[fig:cascade_vc_hac_joint_plant]].
Few things can be said on the dynamics:
- the dynamics of the diagonal elements are almost all the same
- the system is well decoupled below the resonances of the nano-hexapod (1Hz)
- the dynamics of the diagonal elements are almost equivalent to a critically damped mass-spring-system with some spurious resonances above 50Hz corresponding to the resonances of the micro-station
#+begin_src matlab :exports none
freqs = logspace(-1, 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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_hac_joint_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_hac_joint_plant
#+caption: Plant for the High Authority Control in the Joint Space ([[./figs/cascade_vc_hac_joint_plant.png][png]], [[./figs/cascade_vc_hac_joint_plant.pdf][pdf]])
[[file:figs/cascade_vc_hac_joint_plant.png]]
*** Controller Design and Loop Gain
As the plant is well decoupled, a diagonal plant is designed.
#+begin_src matlab
wc = 2*pi*10; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kl = (s + 2*pi*1)/s;
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), 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');
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gl*Kl, eye(6), -1))
#+end_src
** Primary Controller in the task space - $\bm{K}_\mathcal{X}$
<<sec:primary_controller>>
*** Identification of the linearized plant
We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\mathcal{X}$.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
Gp = linearize(mdl, io, 0);
Gp.InputName = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
Gp.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
A minus sign is added because the minus sign is already included in the plant identification.
#+begin_src matlab
isstable(Gp)
Gp = -minreal(Gp);
isstable(Gp)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Gpx = Gp*inv(nano_hexapod.kinematics.J');
Gpx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gpl = nano_hexapod.kinematics.J*Gp;
Gpl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
*** Obtained Plant
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx(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(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{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(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gpx(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(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:primary_plant_voice_coil_X
#+caption: Obtained Primary plant in the Task space ([[./figs/primary_plant_voice_coil_X.png][png]], [[./figs/primary_plant_voice_coil_X.pdf][pdf]])
[[file:figs/primary_plant_voice_coil_X.png]]
#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
labels = {'$\epsilon_{\mathcal{L}_1}/\tau_{1}$', '$\epsilon_{\mathcal{L}_2}/\tau_{2}$', '$\epsilon_{\mathcal{L}_3}/\tau_{3}$', '$\epsilon_{\mathcal{L}_4}/\tau_{4}$', '$\epsilon_{\mathcal{L}_5}/\tau_{5}$', '$\epsilon_{\mathcal{L}_6}/\tau_{6}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpl(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(Gpl(i, i), freqs, 'Hz'))), 'DisplayName', labels{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(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gpl(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(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:primary_plant_voice_coil_L
#+caption: Obtained Primary plant in the frame of the legs ([[./figs/primary_plant_voice_coil_L.png][png]], [[./figs/primary_plant_voice_coil_L.pdf][pdf]])
[[file:figs/primary_plant_voice_coil_L.png]]
*** Controller Design
#+begin_src matlab
wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
(1/h) * (1 + s/wc*h)/(1 + s/wc/h); % For Piezo
% Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (s + 2*pi*10)/s * (s + 2*pi*1)/s ; % For voice coil
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kp = Kp.*diag(1./diag(abs(freqresp(Gpx*Kp, wc))));
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp(i,i), 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');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/loop_gain_primary_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:loop_gain_primary_voice_coil_X
#+caption: Obtained Loop gain for the primary controller in the Task space ([[./figs/loop_gain_primary_voice_coil_X.png][png]], [[./figs/loop_gain_primary_voice_coil_X.pdf][pdf]])
[[file:figs/loop_gain_primary_voice_coil_X.png]]
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gpx*Kp, eye(6), -1))
#+end_src
And now we include the Jacobian inside the controller.
#+begin_src matlab
Kp = inv(nano_hexapod.kinematics.J')*Kp;
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(-Gp*Kp, eye(6), +1))
#+end_src
** Simulation
Let's first save the 3 controllers for further analysis:
#+begin_src matlab
save('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff', 'Kl', 'Kp')
#+end_src
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
And we simulate the system.
#+begin_src matlab
sim('nass_model');
#+end_src
#+begin_src matlab
cascade_hac_lac_lorentz = simout;
save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz', '-append');
#+end_src
** Results
*** Load the simulation results
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac', 'cascade_hac_lac_lorentz');
#+end_src
*** Control effort
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
F_pz = [nano_hexapod.kinematics.J'*cascade_hac_lac.u.Data']';
F_vc = [nano_hexapod.kinematics.J'*cascade_hac_lac_lorentz.u.Data']';
% F_pz = [-nano_hexapod.kinematics.J'*cascade_hac_lac.yn.Fnlm.Data']';
% F_vc = [-nano_hexapod.kinematics.J'*cascade_hac_lac_lorentz.yn.Fnlm.Data']';
#+end_src
#+begin_src matlab :exports none
labels = {'$\mathcal{F}_x$', '$\mathcal{F}_y$', '$\mathcal{F}_z$', '$\mathcal{M}_x$', '$\mathcal{M}_y$', '$\mathcal{M}_z$'};
figure;
ax1 = subplot(1, 2, 1);
hold on;
for i = 1:6
plot(cascade_hac_lac.u.Time, F_pz(:, i), 'DisplayName', labels{i})
end
hold off;
xlabel('Time [s]'); ylabel('Force/Torque Piezo [N, N$\cdot$m]');
legend('location', 'northeast');
ax2 = subplot(1, 2, 2);
hold on;
for i = 1:6
plot(cascade_hac_lac_lorentz.u.Time, F_vc(:, i), 'DisplayName', labels{i})
end
hold off;
xlabel('Time [s]'); ylabel('Force/Torque Lorentz [N, N$\cdot$m]');
legend('location', 'northeast');
#+end_src
#+begin_src matlab :exports none
labels = {'$\mathcal{F}_x$', '$\mathcal{F}_y$', '$\mathcal{F}_z$', '$\mathcal{M}_x$', '$\mathcal{M}_y$', '$\mathcal{M}_z$'};
figure;
hold on;
for i = 1:6
plot(cascade_hac_lac_lorentz.u.Time, F_vc(:, i), 'DisplayName', labels{i})
end
hold off;
xlabel('Time [s]'); ylabel('Force/Torque Lorentz [N, N$\cdot$m]');
xlim([0.5, inf]);
legend('location', 'northeast');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/actuator_force_torques_tomography_voice_coil.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:actuator_force_torques_tomography_voice_coil
#+caption: Actuator Action during a tomography experiment when using Voice Coil actuators ([[./figs/actuator_force_torques_tomography_voice_coil.png][png]], [[./figs/actuator_force_torques_tomography_voice_coil.pdf][pdf]])
[[file:figs/actuator_force_torques_tomography_voice_coil.png]]
*** Load the simulation results
#+begin_src matlab
n_av = 4;
han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));
#+end_src
#+begin_src matlab
t = cascade_hac_lac.Em.En.Time;
Ts = t(2)-t(1);
[pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
[pxx_ca, ~] = pwelch(cascade_hac_lac.Em.En.Data, han_win, [], [], 1/Ts);
[pxx_vc, ~] = pwelch(cascade_hac_lac_lorentz.Em.En.Data, han_win, [], [], 1/Ts);
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(pxx_ol(:, 1)))
plot(f, sqrt(pxx_vc(:, 1)))
hold off;
ylabel('$\Gamma_{D_x}$, $\Gamma_{D_y}$, $\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(pxx_ol(:, 2)))
plot(f, sqrt(pxx_vc(:, 2)))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(pxx_ol(:, 3)))
plot(f, sqrt(pxx_vc(:, 3)))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(pxx_ol(:, 4)))
plot(f, sqrt(pxx_vc(:, 4)))
hold off;
ylabel('$\Gamma_{R_x}$, $\Gamma_{R_y}$, $\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
xlabel('Frequency [Hz]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(pxx_ol(:, 5)))
plot(f, sqrt(pxx_vc(:, 5)))
hold off;
xlabel('Frequency [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')
plot(f, sqrt(pxx_vc(:, 6)), 'DisplayName', 'Voice-Coil')
hold off;
xlabel('Frequency [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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_psd_pos_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:exp_tomography_voice_coil_psd_pos_error
#+caption: Power Spectral Density of the position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_psd_pos_error.png][png]], [[./figs/exp_tomography_voice_coil_psd_pos_error.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_psd_pos_error.png]]
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 1))))))
hold off;
ylabel('CAS $D_x$, $D_y$, $D_z$ [$m$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 2))))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 3))))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 4))))))
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_x$, $R_y$, $R_z$ [$rad$]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 5))))))
hold off;
xlabel('Frequency [Hz]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 6))))), 'DisplayName', 'Voice Coil')
hold off;
xlabel('Frequency [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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_cap_pos_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:exp_tomography_voice_coil_cap_pos_error
#+caption: Cumulative Amplitude Spectrum of the position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_cap_pos_error.png][png]], [[./figs/exp_tomography_voice_coil_cap_pos_error.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_cap_pos_error.png]]
#+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))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 1))
hold off;
ylabel('$D_x$, $D_y$, $D_z$ [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 2))
hold off;
ax3 = subplot(2, 3, 3);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 3))
hold off;
ax4 = subplot(2, 3, 4);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 4))
hold off;
xlabel('Time [s]');
ylabel('$R_x$, $R_y$, $R_z$ [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 5))
hold off;
xlabel('Time [s]');
ax6 = subplot(2, 3, 6);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
hold off;
xlabel('Time [s]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_time_domain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:exp_tomography_voice_coil_time_domain
#+caption: Position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_time_domain.png][png]], [[./figs/exp_tomography_voice_coil_time_domain.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_time_domain.png]]
** Compliance of the nano-hexapod
*** Identification
Let's identify the Compliance of the NASS:
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances/Fd'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the sample
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
First in open-loop:
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_ol = linearize(mdl, io, 0);
Gc_ol.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_ol.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
Then with the IFF control.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_iff = linearize(mdl, io, 0);
Gc_iff.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_iff.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
With the HAC control added
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_hac = linearize(mdl, io, 0);
Gc_hac.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_hac.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
Finally with the primary controller
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_pri = linearize(mdl, io, 0);
Gc_pri.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_pri.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
*** Obtained Compliance
#+begin_src matlab :exports none
labels = {'$\epsilon_x/F_{xd}$', '$\epsilon_y/F_{yd}$', '$\epsilon_z/F_{zd}$', '$\epsilon_{R_x}/M_{xd}$', '$\epsilon_{R_y}/M_{yd}$', '$\epsilon_{R_z}/M_{zd}$'};
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_ol( 1, 1), freqs, 'Hz'))), 'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(Gc_iff(1, 1), freqs, 'Hz'))), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(Gc_hac(1, 1), freqs, 'Hz'))), 'DisplayName', 'HAC');
plot(freqs, abs(squeeze(freqresp(Gc_pri(1, 1), freqs, 'Hz'))), 'DisplayName', 'PRI');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [m/N]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
ax2 = subplot(1, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_ol( 4, 4), freqs, 'Hz'))), 'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(Gc_iff(4, 4), freqs, 'Hz'))), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(Gc_hac(4, 4), freqs, 'Hz'))), 'DisplayName', 'HAC');
plot(freqs, abs(squeeze(freqresp(Gc_pri(4, 4), freqs, 'Hz'))), 'DisplayName', 'PRI');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [$\frac{rad}{Nm}$]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/compliance_evolution_vc_cascade_control.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:compliance_evolution_vc_cascade_control
#+caption: Evolution of the NASS compliance with each control loop added ([[./figs/compliance_evolution_vc_cascade_control.png][png]], [[./figs/compliance_evolution_vc_cascade_control.pdf][pdf]])
[[file:figs/compliance_evolution_vc_cascade_control.png]]
*** Comparison with Piezo
Let's initialize a nano-hexapod with piezoelectric actuators.
#+begin_src matlab
initializeNanoHexapod('actuator', 'piezo');
#+end_src
We don't use any controller.
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_pz = linearize(mdl, io, 0);
Gc_pz.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_pz.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_pri(1, 1), freqs, 'Hz'))), 'DisplayName', 'PRI');
plot(freqs, abs(squeeze(freqresp(Gc_pz( 1, 1), freqs, 'Hz'))), 'DisplayName', 'PZ - OL');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [m/N]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
ax2 = subplot(1, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_pri(4, 4), freqs, 'Hz'))), 'DisplayName', 'PRI');
plot(freqs, abs(squeeze(freqresp(Gc_pz( 4, 4), freqs, 'Hz'))), 'DisplayName', 'PZ - OL');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [$\frac{rad}{Nm}$]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/compliance_comp_pz_vc_cascade.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:compliance_comp_pz_vc_cascade
#+caption: Comparison of the compliance using the open-loop piezo-nano hexapod and the voice coil nano-hexapod with the cascade control topology ([[./figs/compliance_comp_pz_vc_cascade.png][png]], [[./figs/compliance_comp_pz_vc_cascade.pdf][pdf]])
[[file:figs/compliance_comp_pz_vc_cascade.png]]
** Robustness to Payload Variability
*** Initialization
Let's change the payload mass, and see if the controller design for a payload mass of 1 still gives good performance.
#+begin_src matlab
initializeSample('mass', 50);
#+end_src
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
*** Low Authority Control
Let's first identify the transfer function for the Low Authority control.
#+begin_src matlab
%% 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; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
%% Run the linearization
G_iff_m = linearize(mdl, io, 0);
G_iff_m.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff_m.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
#+end_src
The obtained dynamics is compared when using a payload of 1Kg in Figure [[fig:voice_coil_variability_mass_iff]].
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_iff_m(i, i), 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');
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%i}/\\tau_%i$', i, i));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m(i, i), 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');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_iff
#+caption: Dynamics of the LAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff.png][png]], [[./figs/voice_coil_variability_mass_iff.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_iff.png]]
A gain of 50 will here suffice to obtain critical damping of the nano-hexapod modes.
Let's load the IFF controller designed when the payload has a mass of 1Kg.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), 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');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_iff_loop_gain
#+caption: Loop gain for the IFF Control when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff_loop_gain.png][png]], [[./figs/voice_coil_variability_mass_iff_loop_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_iff_loop_gain.png]]
*** High Authority Control
We use the Integral Force Feedback developed with a mass of 1Kg and we identify the dynamics for the High Authority Controller in the case of the 50Kg payload
#+begin_src matlab
%% 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, '/Micro-Station'], 3, 'output', [], 'Dnlm'); io_i = io_i + 1; % Leg Displacement
%% Run the linearization
Gl_m = linearize(mdl, io, 0);
Gl_m.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
Gl_m.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
isstable(Gl_m)
Gl_m = minreal(Gl_m);
isstable(Gl_m)
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl_m(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, 1, 2);
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
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i), 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();
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_hac_plant
#+caption: Dynamics of the HAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_plant.png][png]], [[./figs/voice_coil_variability_mass_hac_plant.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_hac_plant.png]]
We load the HAC controller design when the payload has a mass of 1Kg.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), 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');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_hac_lool_gain
#+caption: Loop Gain of the HAC when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_hac_lool_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_hac_lool_gain.png]]
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gl_m*Kl, eye(6), -1))
#+end_src
*** Primary Plant
We use the Low Authority Controller developed with a mass of 1Kg and we identify the dynamics for the Primary controller in the case of the 50Kg payload.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
Gp_m = linearize(mdl, io, 0);
Gp_m.InputName = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
Gp_m.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
A minus sign is added to cancel the minus sign already included in the identified plant.
#+begin_src matlab
isstable(Gp_m)
Gp_m = -minreal(Gp_m);
isstable(Gp_m)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Gpx_m = Gp_m*inv(nano_hexapod.kinematics.J');
Gpx_m.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gpl_m = nano_hexapod.kinematics.J*Gp_m;
Gpl_m.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
#+begin_important
There are two zeros with positive real part for the plant in the y direction at about 100Hz.
This is problematic as it limits the bandwidth to be less than $\approx 50\ \text{Hz}$.
It is important here to physically understand why such "positive" zero appears.
If we make a "rigid" 50kg paylaod, the positive zero disappears.
#+end_important
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx_m(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(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i), 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();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gpx_m(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gpx_m(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(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_primary_plant
#+caption: Dynamics of the Primary plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_plant.png][png]], [[./figs/voice_coil_variability_mass_primary_plant.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_primary_plant.png]]
We load the primary controller that was design when the payload has a mass of 1Kg.
We load the HAC controller design when the payload has a mass of 1Kg.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
Kp_x = nano_hexapod.kinematics.J'*Kp;
#+end_src
#+begin_src matlab
wc = 2*pi*50; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
(1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
(s + 2*pi*1)/s * ...
1/(1+s/2/wc); % For Piezo
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kp = Kp.*diag(1./diag(abs(freqresp(Gpx_m*Kp, wc))));
#+end_src
#+begin_src matlab :exports none
labels = {'$L_{x}$', '$L_{y}$', '$L_{z}$', '$L_{R_x}$', '$L_{R_y}$', '$L_{R_z}$'};
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), 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', 'northwest')
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_primary_lool_gain
#+caption: Loop Gain of the Primary loop when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_primary_lool_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_primary_lool_gain.png]]
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gpx_m*Kp_x, eye(6), -1))
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gp_m*Kp, eye(6), -1))
#+end_src
*** Simulation
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
And we simulate the system.
#+begin_src matlab
sim('nass_model');
#+end_src
#+begin_src matlab
cascade_hac_lac_lorentz_high_mass = simout;
save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz_high_mass', '-append');
#+end_src
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
#+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))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 1))
hold off;
ylabel('$D_x$, $D_y$, $D_z$ [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 2))
hold off;
ax3 = subplot(2, 3, 3);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 3))
hold off;
ax4 = subplot(2, 3, 4);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 4))
hold off;
xlabel('Time [s]');
ylabel('$R_x$, $R_y$, $R_z$ [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 5))
hold off;
xlabel('Time [s]');
ax6 = subplot(2, 3, 6);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
hold off;
xlabel('Time [s]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
* Other analysis
** Robustness to Payload Variability
- [ ] For 3/masses (1kg, 10kg, 50kg), plot each of the 3 plants
** Direct HAC control in the task space - $\bm{K}_\mathcal{X}$
*** Introduction :ignore:
#+name: fig:control_architecture_hac_iff_pos_X
#+caption: Control Architecture containing an IFF controller and a Controller in the task space
[[file:figs/control_architecture_hac_iff_pos_X.png]]
*** Identification
#+begin_src matlab
initializeController('type', 'hac-iff');
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/HAC-IFF/Kx'], 1, 'input'); io_i = io_i + 1; % Control input
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
isstable(G)
G = -minreal(G);
isstable(G)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Gx = G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gl = nano_hexapod.kinematics.J*G;
Gl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
*** Obtained Plant in the Task Space
#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
labels = {'$\epsilon_x/F_{x}$', '$\epsilon_y/F_{y}$', '$\epsilon_z/F_{z}$', '$\epsilon_{R_x}/M_{x}$', '$\epsilon_{R_y}/M_{y}$', '$\epsilon_{R_z}/M_{z}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(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(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{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(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');
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
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]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Obtained Plant in the Joint Space
#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
labels = {'$\epsilon_{\mathcal{L}_1}/\tau_{1}$', '$\epsilon_{\mathcal{L}_2}/\tau_{2}$', '$\epsilon_{\mathcal{L}_3}/\tau_{3}$', '$\epsilon_{\mathcal{L}_4}/\tau_{4}$', '$\epsilon_{\mathcal{L}_5}/\tau_{5}$', '$\epsilon_{\mathcal{L}_6}/\tau_{6}$'};
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', labels{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
*** Controller Design in the Joint Space
#+begin_src matlab
wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(s + 2*pi*10)/s * ... % Pseudo Integrator
1/(1+s/2/pi/500); % Low pass Filter
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc))));
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kx(i,i), 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');
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gx*Kx, eye(6), -1))
#+end_src
#+begin_src matlab
wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(s + 2*pi*1)/s * ... % Pseudo Integrator
(s + 2*pi*10)/s * ... % Pseudo Integrator
1/(1+s/2/pi/500); % Low pass Filter
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), 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:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), 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');
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gl*Kl, eye(6), -1))
#+end_src
** On the usefulness of the High Authority Control loop / Linearization loop
*** Introduction :ignore:
Let's see what happens is we omit the HAC loop and we directly try to control the damped plant using the measurement of the sample with respect to the granite $\bm{\mathcal{X}}$.
We can do that in two different ways:
- in the task space as shown in Figure [[fig:control_architecture_iff_X]]
- in the space of the legs as shown in Figure [[fig:control_architecture_iff_L]]
#+begin_src latex :file control_architecture_iff_X.pdf
\begin{tikzpicture}
% Blocs
\node[block={3.0cm}{3.0cm}] (P) {Plant};
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
\node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
\node[block, left= of addF] (J) {$\bm{J}^{-T}$};
\node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
\node[addb={+}{}{}{}{-}, left= of K] (subr) {};
% Connections and labels
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
\draw[->] (Kiff.west) -| (addF.north);
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
\draw[->] (outputX) -- ++(1.6, 0);
\draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -2) -| (subr.south);
\draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}}$};
\draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
\draw[->] (J.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
\end{tikzpicture}
#+end_src
#+name: fig:control_architecture_iff_X
#+caption: IFF control + primary controller in the task space
#+RESULTS:
[[file:figs/control_architecture_iff_X.png]]
#+begin_src latex :file control_architecture_iff_L.pdf
\begin{tikzpicture}
% Blocs
\node[block={3.0cm}{3.0cm}] (P) {Plant};
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
\node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
\node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
\node[block, left= of K] (J) {$\bm{J}$};
\node[addb={+}{}{}{}{-}, left= of J] (subr) {};
% Connections and labels
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
\draw[->] (Kiff.west) -| (addF.north);
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
\draw[->] (outputX) -- ++(1.6, 0);
\draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -2) -| (subr.south);
\draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\draw[->] (subr.east) -- (J.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}}$};
\draw[->] (J.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{L}}$};
\draw[->] (K.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
\end{tikzpicture}
#+end_src
#+name: fig:control_architecture_iff_L
#+caption: HAC-LAC control architecture in the frame of the legs
#+RESULTS:
[[file:figs/control_architecture_iff_L.png]]
*** Identification
#+begin_src matlab
initializeController('type', 'hac-iff');
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/HAC-IFF/Kx'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
isstable(G)
G = -minreal(G);
isstable(G)
#+end_src
*** Plant in the Task space
The obtained plant is shown in Figure
#+begin_src matlab
Gx = G*inv(nano_hexapod.kinematics.J');
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 4, 1000);
labels = {'$\epsilon_x/\mathcal{F}_{x}$', '$\epsilon_y/\mathcal{F}_{y}$', '$\epsilon_z/\mathcal{F}_{z}$', '$\epsilon_{R_x}/\mathcal{M}_{x}$', '$\epsilon_{R_y}/\mathcal{M}_{y}$', '$\epsilon_{R_z}/\mathcal{M}_{z}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(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(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{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(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');
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
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]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Plant in the Leg's space
#+begin_src matlab
Gl = nano_hexapod.kinematics.J*G;
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 4, 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
** DVF instead of IFF?
*** Initialization and Identification
#+begin_src matlab
initializeController('type', 'hac-dvf');
Kdvf = tf(zeros(6));
#+end_src
#+begin_src matlab
%% 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; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Displacement Sensors
%% Run the linearization
G_dvf = linearize(mdl, io, 0);
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_dvf.OutputName = {'Dlm1', 'Dlm2', 'Dlm3', 'Dlm4', 'Dlm5', 'Dlm6'};
#+end_src
*** Obtained Plant
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_dvf(i, i), 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');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(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('location', 'northeast');
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/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(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(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
#+begin_src matlab :exports none
gains = logspace(0, 4, 500);
figure;
subplot(1, 2, 1);
hold on;
plot(real(pole(G_dvf)), imag(pole(G_dvf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_dvf)), imag(tzero(G_dvf)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_dvf, (s*gains(i))*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*500]);
xlim([-2*pi*500,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
subplot(1, 2, 2);
hold on;
plot(real(pole(G_dvf)), imag(pole(G_dvf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_dvf)), imag(tzero(G_dvf)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_dvf, (s*gains(i))*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*8]);
xlim([-2*pi*8,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
*** Controller
#+begin_src matlab
Kdvf = -850*s/(1+s/2/pi/1000)*eye(6);
#+end_src
*** HAC Identification
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/HAC-DVF/Kx'], 1, 'input'); io_i = io_i + 1; % Control input
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
isstable(G)
G = -minreal(G);
isstable(G)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Gx = G*inv(nano_hexapod.kinematics.J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gl = nano_hexapod.kinematics.J*G;
Gl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
*** Conclusion
#+begin_important
DVF can be used instead of IFF.
#+end_important