stewart-simscape/org/control-vibration-isolation.org

#+TITLE: Stewart Platform - Vibration Isolation
:DRAWER:
#+STARTUP: overview

#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas

#+HTML_LINK_HOME: ./index.html
#+HTML_LINK_UP: ./index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

* Introduction                                                        :ignore:

* HAC-LAC (Cascade) Control - Integral Control
** Introduction
In this section, we wish to study the use of the High Authority Control - Low Authority Control (HAC-LAC) architecture on the Stewart platform.

The control architectures are shown in Figures [[fig:control_arch_hac_iff]] and [[fig:control_arch_hac_dvf]].

First, the LAC loop is closed (the LAC control is described [[file:active-damping.org][here]]), and then the HAC controller is designed and the outer loop is closed.

#+begin_src latex :file control_arch_hac_iff.pdf
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{2.0cm}] (P) {};
  \node[above] at (P.north) {Plant};
  \node[block, below=0.7 of P] (Kiff) {$\bm{K}_\text{IFF}$};
  \node[block, below=0.7 of Kiff] (Khac) {$\bm{K}_\text{HAC}$};

  % Add
  \node[addb, left=1 of P] (add) {};

  \node[block, left=1 of add] (J) {$\bm{J}^{-T}$};

  % Input and outputs coordinates
  \coordinate[] (outputhac) at ($(P.south east)!0.75!(P.north east)$);
  \coordinate[] (outputiff) at ($(P.south east)!0.25!(P.north east)$);

  \draw[->] (outputiff) node[above right]{$\bm{\tau}_m$} -- ++(0.8, 0) |- (Kiff.east);
  \draw[->] (outputhac) node[above right]{$\bm{\mathcal{X}}$} -- ++(1.6, 0) |- (Khac.east);

  \draw[->] (Kiff.west) -| (add.south);

  \draw[->] (J.east) -- (add.west);
  \draw[<-] (J.west) node[above left]{$\bm{\mathcal{F}}$} -- ++(-0.8, 0) |- (Khac.west);
  \draw[->] (add.east) -- (P.west) node[above left]{$\bm{\tau}$};
\end{tikzpicture}
#+end_src

#+name: fig:control_arch_hac_iff
#+caption: HAC-LAC architecture with IFF
#+RESULTS:
[[file:figs/control_arch_hac_iff.png]]


#+begin_src latex :file control_arch_hac_dvf.pdf
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{2.0cm}] (P) {};
  \node[above] at (P.north) {Plant};
  \node[block, below=0.7 of P] (Kdvf) {$\bm{K}_\text{DVF}$};
  \node[block, below=0.7 of Kdvf] (Khac) {$\bm{K}_\text{HAC}$};

  % Add
  \node[addb, left=1 of P] (add) {};

  \node[block, left=1 of add] (J) {$\bm{J}^{-T}$};

  % Input and outputs coordinates
  \coordinate[] (outputhac) at ($(P.south east)!0.75!(P.north east)$);
  \coordinate[] (outputdvf) at ($(P.south east)!0.25!(P.north east)$);

  \draw[->] (outputdvf) node[above right]{$\delta \bm{\mathcal{L}}_m$} -- ++(0.8, 0) |- (Kdvf.east);
  \draw[->] (outputhac) node[above right]{$\bm{\mathcal{X}}$} -- ++(1.6, 0) |- (Khac.east);

  \draw[->] (Kdvf.west) -| (add.south);

  \draw[->] (J.east) -- (add.west);
  \draw[<-] (J.west) node[above left]{$\bm{\mathcal{F}}$} -- ++(-0.8, 0) |- (Khac.west);
  \draw[->] (add.east) -- (P.west) node[above left]{$\bm{\tau}$};
\end{tikzpicture}
#+end_src

#+name: fig:control_arch_hac_dvf
#+caption: HAC-LAC architecture with DVF
#+RESULTS:
[[file:figs/control_arch_hac_dvf.png]]

** Matlab Init                                                     :noexport:
#+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
simulinkproject('../');
#+end_src

#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src

** Initialization
We first initialize the Stewart platform.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src

The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src

** Identification
*** Introduction                                                    :ignore:
We identify the transfer function from the actuator forces $\bm{\tau}$ to the absolute displacement of the mobile platform $\bm{\mathcal{X}}$ in three different cases:
- Open Loop plant
- Already damped plant using Integral Force Feedback
- Already damped plant using Direct velocity feedback

*** HAC - Without LAC
#+begin_src matlab
controller = initializeController('type', 'open-loop');
#+end_src

#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_ol = linearize(mdl, io);
G_ol.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_ol.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

*** HAC - IFF
#+begin_src matlab
controller = initializeController('type', 'iff');
K_iff = -(1e4/s)*eye(6);
#+end_src

#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

*** HAC - DVF
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
#+end_src

#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

** Control Architecture
We use the Jacobian to express the actuator forces in the cartesian frame, and thus we obtain the transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$.

#+begin_src matlab
Gc_ol = minreal(G_ol)/stewart.kinematics.J';
Gc_ol.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

Gc_iff = minreal(G_iff)/stewart.kinematics.J';
Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

We then design a controller based on the transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$, finally, we will pre-multiply the controller by $\bm{J}^{-T}$.

** 6x6 Plant Comparison
#+begin_src matlab :exports none
p_handle = zeros(6*6,1);

fig = figure;
for ix = 1:6
    for iy = 1:6
        p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
        hold on;
        set(gca,'ColorOrderIndex',1);
        plot(freqs, abs(squeeze(freqresp(Gc_ol(ix, iy), freqs, 'Hz'))));
        set(gca,'ColorOrderIndex',2);
        plot(freqs, abs(squeeze(freqresp(Gc_iff(ix, iy), freqs, 'Hz'))));
        set(gca,'ColorOrderIndex',3);
        plot(freqs, abs(squeeze(freqresp(Gc_dvf(ix, iy), freqs, 'Hz'))));
        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
        if ix < 6
            xticklabels({});
        end
        if iy > 1
            yticklabels({});
        end
    end
end

linkaxes(p_handle, 'xy')
xlim([freqs(1), freqs(end)]);
ylim([1e-9 1e-3]);

han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Plant');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_coupling_jacobian.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_coupling_jacobian
#+caption: Norm of the transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ ([[./figs/hac_lac_coupling_jacobian.png][png]], [[./figs/hac_lac_coupling_jacobian.pdf][pdf]])
[[file:figs/hac_lac_coupling_jacobian.png]]

** HAC - DVF
*** Plant
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_plant_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_plant_dvf
#+caption: Diagonal elements of the plant for HAC control when DVF is previously applied ([[./figs/hac_lac_plant_dvf.png][png]], [[./figs/hac_lac_plant_dvf.pdf][pdf]])
[[file:figs/hac_lac_plant_dvf.png]]

*** Controller Design
We design a diagonal controller with equal bandwidth for the 6 terms.
The controller is a pure integrator with a small lead near the crossover.

#+begin_src matlab
wc = 2*pi*300; % Wanted Bandwidth [rad/s]

h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));

Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s;
#+end_src

#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Kd_dvf(1,1)*Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(2,2)*Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(3,3)*Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(4,4)*Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(5,5)*Gc_dvf('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(6,6)*Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(1,1)*Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(2,2)*Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(3,3)*Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(4,4)*Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(5,5)*Gc_dvf('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(6,6)*Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_loop_gain_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_loop_gain_dvf
#+caption: Diagonal elements of the Loop Gain for the HAC control ([[./figs/hac_lac_loop_gain_dvf.png][png]], [[./figs/hac_lac_loop_gain_dvf.pdf][pdf]])
[[file:figs/hac_lac_loop_gain_dvf.png]]


Finally, we pre-multiply the diagonal controller by $\bm{J}^{-T}$ prior implementation.
#+begin_src matlab
K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf;
#+end_src

*** Obtained Performance
We identify the transmissibility and compliance of the system.

#+begin_src matlab
controller = initializeController('type', 'open-loop');
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
[C_ol, C_norm_ol, ~] = computeCompliance();
#+end_src

#+begin_src matlab
controller = initializeController('type', 'dvf');
[T_dvf, T_norm_dvf, ~] = computeTransmissibility();
[C_dvf, C_norm_dvf, ~] = computeCompliance();
#+end_src

#+begin_src matlab
controller = initializeController('type', 'hac-dvf');
[T_hac_dvf, T_norm_hac_dvf, ~] = computeTransmissibility();
[C_hac_dvf, C_norm_hac_dvf, ~] = computeCompliance();
#+end_src

#+begin_src matlab :exports none
figure;

subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_dvf)
plot(freqs, T_norm_hac_dvf)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');

subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_dvf, 'DisplayName', 'DVF')
plot(freqs, C_norm_hac_dvf, 'DisplayName', 'HAC-DVF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_T_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_C_T_dvf
#+caption: Obtained Compliance and Transmissibility ([[./figs/hac_lac_C_T_dvf.png][png]], [[./figs/hac_lac_C_T_dvf.pdf][pdf]])
[[file:figs/hac_lac_C_T_dvf.png]]

** HAC - IFF
*** Plant
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_iff('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Rz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_plant_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_plant_iff
#+caption: Diagonal elements of the plant for HAC control when IFF is previously applied ([[./figs/hac_lac_plant_iff.png][png]], [[./figs/hac_lac_plant_iff.pdf][pdf]])
[[file:figs/hac_lac_plant_iff.png]]

*** Controller Design
We design a diagonal controller with equal bandwidth for the 6 terms.
The controller is a pure integrator with a small lead near the crossover.

#+begin_src matlab
wc = 2*pi*300; % Wanted Bandwidth [rad/s]

h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));

Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s;
#+end_src

#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Kd_iff(1,1)*Gc_iff('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(2,2)*Gc_iff('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(3,3)*Gc_iff('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(4,4)*Gc_iff('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(5,5)*Gc_iff('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(6,6)*Gc_iff('Rz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);

ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(1,1)*Gc_iff('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(2,2)*Gc_iff('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(3,3)*Gc_iff('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(4,4)*Gc_iff('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(5,5)*Gc_iff('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(6,6)*Gc_iff('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_loop_gain_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_loop_gain_iff
#+caption: Diagonal elements of the Loop Gain for the HAC control ([[./figs/hac_lac_loop_gain_iff.png][png]], [[./figs/hac_lac_loop_gain_iff.pdf][pdf]])
[[file:figs/hac_lac_loop_gain_iff.png]]


Finally, we pre-multiply the diagonal controller by $\bm{J}^{-T}$ prior implementation.
#+begin_src matlab
K_hac_iff = inv(stewart.kinematics.J')*Kd_iff;
#+end_src

*** Obtained Performance
We identify the transmissibility and compliance of the system.

#+begin_src matlab
controller = initializeController('type', 'open-loop');
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
[C_ol, C_norm_ol, ~] = computeCompliance();
#+end_src

#+begin_src matlab
controller = initializeController('type', 'iff');
[T_iff, T_norm_iff, ~] = computeTransmissibility();
[C_iff, C_norm_iff, ~] = computeCompliance();
#+end_src

#+begin_src matlab
controller = initializeController('type', 'hac-iff');
[T_hac_iff, T_norm_hac_iff, ~] = computeTransmissibility();
[C_hac_iff, C_norm_hac_iff, ~] = computeCompliance();
#+end_src

#+begin_src matlab :exports none
figure;

subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_iff)
plot(freqs, T_norm_hac_iff)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');

subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_iff, 'DisplayName', 'IFF')
plot(freqs, C_norm_hac_iff, 'DisplayName', 'HAC-IFF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_T_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_C_T_iff
#+caption: Obtained Compliance and Transmissibility ([[./figs/hac_lac_C_T_iff.png][png]], [[./figs/hac_lac_C_T_iff.pdf][pdf]])
[[file:figs/hac_lac_C_T_iff.png]]

** Comparison
#+begin_src matlab :exports none
p_handle = zeros(6*6,1);

fig = figure;
for ix = 1:6
    for iy = 1:6
        p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
        hold on;
        set(gca,'ColorOrderIndex',1);
        plot(freqs, abs(squeeze(freqresp(C_ol(ix, iy), freqs, 'Hz'))));
        set(gca,'ColorOrderIndex',2);
        plot(freqs, abs(squeeze(freqresp(C_hac_dvf(ix, iy), freqs, 'Hz'))));
        set(gca,'ColorOrderIndex',3);
        plot(freqs, abs(squeeze(freqresp(C_hac_iff(ix, iy), freqs, 'Hz'))));
        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
        if ix < 6
            xticklabels({});
        end
        if iy > 1
            yticklabels({});
        end
    end
end

linkaxes(p_handle, 'xy')
ylim([1e-10, 1e-3]);
xlim([freqs(1), freqs(end)]);

han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Compliance');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_full_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_C_full_comparison
#+caption: Comparison of the norm of the Compliance matrices for the HAC-LAC architecture ([[./figs/hac_lac_C_full_comparison.png][png]], [[./figs/hac_lac_C_full_comparison.pdf][pdf]])
[[file:figs/hac_lac_C_full_comparison.png]]

#+begin_src matlab :exports none
p_handle = zeros(6*6,1);

fig = figure;
for ix = 1:6
    for iy = 1:6
        p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
        hold on;
        set(gca,'ColorOrderIndex',1);
        plot(freqs, abs(squeeze(freqresp(T_ol(ix, iy), freqs, 'Hz'))));
        set(gca,'ColorOrderIndex',2);
        plot(freqs, abs(squeeze(freqresp(T_hac_dvf(ix, iy), freqs, 'Hz'))));
        set(gca,'ColorOrderIndex',3);
        plot(freqs, abs(squeeze(freqresp(T_hac_iff(ix, iy), freqs, 'Hz'))));
        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
        if ix < 6
            xticklabels({});
        end
        if iy > 1
            yticklabels({});
        end
    end
end

linkaxes(p_handle, 'xy')
ylim([1e-5, 10]);
xlim([freqs(1), freqs(end)]);

han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Transmissibility');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_T_full_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_T_full_comparison
#+caption: Comparison of the norm of the Transmissibility matrices for the HAC-LAC architecture ([[./figs/hac_lac_T_full_comparison.png][png]], [[./figs/hac_lac_T_full_comparison.pdf][pdf]])
[[file:figs/hac_lac_T_full_comparison.png]]

#+begin_src matlab :exports none
figure;

subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_hac_dvf)
plot(freqs, T_norm_hac_iff)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');

subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_hac_dvf, 'DisplayName', 'HAC-DVF')
plot(freqs, C_norm_hac_iff, 'DisplayName', 'HAC-IFF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_T_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hac_lac_C_T_comparison
#+caption: Comparison of the Frobenius norm of the Compliance and Transmissibility for the HAC-LAC architecture with both IFF and DVF ([[./figs/hac_lac_C_T_comparison.png][png]], [[./figs/hac_lac_C_T_comparison.pdf][pdf]])
[[file:figs/hac_lac_C_T_comparison.png]]

* MIMO Analysis
** Introduction                                                      :ignore:
Let's define the system as shown in figure [[fig:general_control_names]].

#+begin_src latex :file general_control_names.pdf
\begin{tikzpicture}

  % Blocs
  \node[block={2.0cm}{2.0cm}] (P) {$P$};
  \node[block={1.5cm}{1.5cm}, below=0.7 of P] (K) {$K$};

  % Input and outputs coordinates
  \coordinate[] (inputw)  at ($(P.south west)!0.75!(P.north west)$);
  \coordinate[] (inputu)  at ($(P.south west)!0.25!(P.north west)$);
  \coordinate[] (outputz) at ($(P.south east)!0.75!(P.north east)$);
  \coordinate[] (outputv) at ($(P.south east)!0.25!(P.north east)$);

  % Connections and labels
  \draw[<-] (inputw) node[above left, align=right]{(weighted)\\exogenous inputs\\$w$} -- ++(-1.5, 0);
  \draw[<-] (inputu) -- ++(-0.8, 0) |- node[left, near start, align=right]{control signals\\$u$} (K.west);

  \draw[->] (outputz) node[above right, align=left]{(weighted)\\exogenous outputs\\$z$} -- ++(1.5, 0);
  \draw[->] (outputv) -- ++(0.8, 0) |- node[right, near start, align=left]{sensed output\\$v$} (K.east);
\end{tikzpicture}
#+end_src

#+name: fig:general_control_names
#+caption: General Control Architecture
#+RESULTS:
[[file:figs/general_control_names.png]]

#+name: tab:general_plant_signals
#+caption: Signals definition for the generalized plant
|                     | *Symbol*                    | *Meaning*                              |
|---------------------+-----------------------------+----------------------------------------|
| *Exogenous Inputs*  | $\bm{\mathcal{X}}_w$        | Ground motion                          |
|                     | $\bm{\mathcal{F}}_d$        | External Forces applied to the Payload |
|                     | $\bm{r}$                    | Reference signal for tracking          |
|---------------------+-----------------------------+----------------------------------------|
| *Exogenous Outputs* | $\bm{\mathcal{X}}$          | Absolute Motion of the Payload         |
|                     | $\bm{\tau}$                 | Actuator Rate                          |
|---------------------+-----------------------------+----------------------------------------|
| *Sensed Outputs*    | $\bm{\tau}_m$               | Force Sensors in each leg              |
|                     | $\delta \bm{\mathcal{L}}_m$ | Measured displacement of each leg      |
|                     | $\bm{\mathcal{X}}$          | Absolute Motion of the Payload         |
|---------------------+-----------------------------+----------------------------------------|
| *Control Signals*   | $\bm{\tau}$                 | Actuator Inputs                        |

** Matlab Init                                                     :noexport:
#+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
simulinkproject('../');
#+end_src

#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src

** Initialization
We first initialize the Stewart platform.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src

The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src

** Identification
*** HAC - Without LAC
#+begin_src matlab
controller = initializeController('type', 'open-loop');
#+end_src

#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_ol = linearize(mdl, io);
G_ol.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_ol.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

*** HAC - DVF
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
#+end_src

#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

*** Cartesian Frame
#+begin_src matlab
Gc_ol = minreal(G_ol)/stewart.kinematics.J';
Gc_ol.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};

Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

** Singular Value Decomposition
#+begin_src matlab
freqs = logspace(1, 4, 1000);

U_ol = zeros(6,6,length(freqs));
S_ol = zeros(6,length(freqs));
V_ol = zeros(6,6,length(freqs));

U_dvf = zeros(6,6,length(freqs));
S_dvf = zeros(6,length(freqs));
V_dvf = zeros(6,6,length(freqs));

for i = 1:length(freqs)
    [U,S,V] = svd(freqresp(Gc_ol, freqs(i), 'Hz'));
    U_ol(:,:,i) = U;
    S_ol(:,i) = diag(S);
    V_ol(:,:,i) = V;

    [U,S,V] = svd(freqresp(Gc_dvf, freqs(i), 'Hz'));
    U_dvf(:,:,i) = U;
    S_dvf(:,i) = diag(S);
    V_dvf(:,:,i) = V;
end
#+end_src

#+begin_src matlab :exports none
figure;

ax1 = subplot(1,2,1);
hold on;
plot(freqs, S_ol(1,:), '-');
plot(freqs, S_ol(2,:), '--');
plot(freqs, S_ol(3,:), '-.');
plot(freqs, S_ol(4,:), '--');
plot(freqs, S_ol(5,:), '-');
plot(freqs, S_ol(6,:), '-.');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
title('Undamped Plant');

ax2 = subplot(1,2,2);
hold on;
plot(freqs, S_dvf(1,:), '-' , 'DisplayName', '$\sigma_1$');
plot(freqs, S_dvf(2,:), '--', 'DisplayName', '$\sigma_2$');
plot(freqs, S_dvf(3,:), '-.', 'DisplayName', '$\sigma_3$');
plot(freqs, S_dvf(4,:), '-' , 'DisplayName', '$\sigma_4$');
plot(freqs, S_dvf(5,:), '--', 'DisplayName', '$\sigma_5$');
plot(freqs, S_dvf(6,:), '-.', 'DisplayName', '$\sigma_6$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
title('Damped Plant - DVF');

linkaxes([ax1, ax2], 'xy');
legend();
#+end_src

#+begin_src matlab :exports none
figure;

ax1 = subplot(1,2,1);
hold on;
for i = 1:6
    plot(freqs, abs(squeeze(V_ol(i,1,:))), '-' , 'DisplayName', Gc_ol.InputName{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
legend();

ax2 = subplot(1,2,2);
hold on;
for i = 1:6
    plot(freqs, abs(squeeze(U_ol(i,1,:))), '-' , 'DisplayName', Gc_ol.OutputName{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
legend();

linkaxes([ax1,ax2], 'x');
#+end_src

* Diagonal Control based on the damped plant
** Introduction                                                      :ignore:
From cite:skogestad07_multiv_feedb_contr, a simple approach to multivariable control is the following two-step procedure:
1. *Design a pre-compensator* $W_1$, which counteracts the interactions in the plant and results in a new *shaped plant* $G_S(s) = G(s) W_1(s)$ which is *more diagonal and easier to control* than the original plant $G(s)$.
2. *Design a diagonal controller* $K_S(s)$ for the shaped plant using methods similar to those for SISO systems.

The overall controller is then:
\[ K(s) = W_1(s)K_s(s) \]

There are mainly three different cases:
1. *Dynamic decoupling*: $G_S(s)$ is diagonal at all frequencies. For that we can choose $W_1(s) = G^{-1}(s)$ and this is an inverse-based controller.
2. *Steady-state decoupling*: $G_S(0)$ is diagonal. This can be obtained by selecting $W_1(s) = G^{-1}(0)$.
3. *Approximate decoupling at frequency $\w_0$*: $G_S(j\w_0)$ is as diagonal as possible. Decoupling the system at $\w_0$ is a good choice because the effect on performance of reducing interaction is normally greatest at this frequency.

** Initialization
We first initialize the Stewart platform.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src

The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src

** Identification
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
#+end_src

#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src

** Steady State Decoupling
*** Pre-Compensator Design
We choose $W_1 = G^{-1}(0)$.
#+begin_src matlab
W1 = inv(freqresp(G_dvf, 0));
#+end_src

The (static) decoupled plant is $G_s(s) = G(s) W_1$.
#+begin_src matlab
Gs = G_dvf*W1;
#+end_src

In the case of the Stewart platform, the pre-compensator for static decoupling is equal to $\mathcal{K} \bm{J}$:
\begin{align*}
  W_1 &= \left( \frac{\bm{\mathcal{X}}}{\bm{\tau}}(s=0) \right)^{-1}\\
      &= \left( \frac{\bm{\mathcal{X}}}{\bm{\tau}}(s=0) \bm{J}^T \right)^{-1}\\
      &= \left( \bm{C} \bm{J}^T \right)^{-1}\\
      &= \left( \bm{J}^{-1} \mathcal{K}^{-1} \right)^{-1}\\
      &= \mathcal{K} \bm{J}
\end{align*}

The static decoupled plant is schematic shown in Figure [[fig:control_arch_static_decoupling]] and the bode plots of its diagonal elements are shown in Figure [[fig:static_decoupling_diagonal_plant]].

#+begin_src latex :file control_arch_static_decoupling.pdf
\begin{tikzpicture}
  % Blocs
  \node[block] (G) {$G(s)$};
  \node[block, left=1 of G] (J) {$\mathcal{K}\bm{J}$};
  \node[block, left=1 of J] (Ks) {$\bm{K}_s(s)$};

  \draw[->] (Ks.east) -- (J.west);
  \draw[->] (J.east) -- (G.west) node[above left]{$\bm{\tau}$};
  \draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ++(0.8, -0.8) -| ($(Ks.west) + (-0.8, 0)$) -- (Ks.west);

  \begin{scope}[on background layer]
    \node[fit={(J.north west) (G.south east)}, inner sep=4pt, draw, dashed, fill=black!20!white, label={$G_s(s)$}] {};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:control_arch_static_decoupling
#+caption: Static Decoupling of the Stewart platform
#+RESULTS:
[[file:figs/control_arch_static_decoupling.png]]

#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);

figure;

ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
    plot(freqs, abs(squeeze(freqresp(Gs(i, i), 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:6
    plot(freqs, 180/pi*angle(squeeze(freqresp(Gs(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/static_decoupling_diagonal_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:static_decoupling_diagonal_plant
#+caption: Bode plot of the diagonal elements of $G_s(s)$ ([[./figs/static_decoupling_diagonal_plant.png][png]], [[./figs/static_decoupling_diagonal_plant.pdf][pdf]])
[[file:figs/static_decoupling_diagonal_plant.png]]

*** Diagonal Control Design
We design a diagonal controller $K_s(s)$ that consist of a pure integrator and a lead around the crossover.

#+begin_src matlab
wc = 2*pi*300; % Wanted Bandwidth [rad/s]

h = 1.5;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));

Ks_dvf = diag(1./abs(diag(freqresp(1/s*Gs, wc)))) .* H_lead .* 1/s;
#+end_src

The overall controller is then $K(s) = W_1 K_s(s)$ as shown in Figure [[fig:control_arch_static_decoupling_K]].

#+begin_src matlab
K_hac_dvf = W1 * Ks_dvf;
#+end_src

#+begin_src latex :file control_arch_static_decoupling_K.pdf
\begin{tikzpicture}
  % Blocs
  \node[block] (G) {$G(s)$};
  \node[block, left=1 of G] (J) {$\mathcal{K}\bm{J}$};
  \node[block, left=1 of J] (Ks) {$\bm{K}_s(s)$};

  \draw[->] (Ks.east) -- (J.west);
  \draw[->] (J.east) -- (G.west) node[above left]{$\bm{\tau}$};
  \draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ++(0.8, -0.8) -| ($(Ks.west) + (-0.8, 0)$) -- (Ks.west);

  \begin{scope}[on background layer]
    \node[fit={(Ks.north west) (J.south east)}, inner sep=4pt, draw, dashed, fill=black!20!white, label={$K(s)$}] {};
  \end{scope}
\end{tikzpicture}
#+end_src

#+name: fig:control_arch_static_decoupling_K
#+caption: Controller including the static decoupling matrix
#+RESULTS:
[[file:figs/control_arch_static_decoupling_K.png]]

*** Results
We identify the transmissibility and compliance of the Stewart platform under open-loop and closed-loop control.

#+begin_src matlab
controller = initializeController('type', 'open-loop');
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
[C_ol, C_norm_ol, ~] = computeCompliance();
#+end_src

#+begin_src matlab
controller = initializeController('type', 'hac-dvf');
[T_hac_dvf, T_norm_hac_dvf, ~] = computeTransmissibility();
[C_hac_dvf, C_norm_hac_dvf, ~] = computeCompliance();
#+end_src

The results are shown in figure

#+begin_src matlab :exports none
figure;

subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_hac_dvf)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');

subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_hac_dvf, 'DisplayName', 'HAC-DVF - Static decoupl.')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/static_decoupling_C_T_frobenius_norm.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:static_decoupling_C_T_frobenius_norm
#+caption: Frobenius norm of the Compliance and transmissibility matrices ([[./figs/static_decoupling_C_T_frobenius_norm.png][png]], [[./figs/static_decoupling_C_T_frobenius_norm.pdf][pdf]])
[[file:figs/static_decoupling_C_T_frobenius_norm.png]]

** TODO Decoupling at Crossover
- [ ] Find a method for real approximation of a complex matrix
 
* Time Domain Simulation
** Matlab Init                                                     :noexport:
#+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
simulinkproject('../');
#+end_src

#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src

** Initialization
We first initialize the Stewart platform.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src

The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src

#+begin_src matlab
load('./mat/motion_error_ol.mat', 'Eg')
#+end_src

** HAC IFF
#+begin_src matlab
controller = initializeController('type', 'iff');
K_iff = -(1e4/s)*eye(6);

%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

Gc_iff = minreal(G_iff)/stewart.kinematics.J';
Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

#+begin_src matlab
wc = 2*pi*100; % Wanted Bandwidth [rad/s]

h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));

Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s;
K_hac_iff = inv(stewart.kinematics.J')*Kd_iff;
#+end_src

#+begin_src matlab
controller = initializeController('type', 'hac-iff');
#+end_src

** HAC-DVF
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);

%% Name of the Simulink File
mdl = 'stewart_platform_model';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'],              1, 'input');      io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]

%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

#+begin_src matlab
wc = 2*pi*100; % Wanted Bandwidth [rad/s]

h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));

Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s;

K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf;
#+end_src

#+begin_src matlab
controller = initializeController('type', 'hac-dvf');
#+end_src

** Results

#+begin_src matlab
figure;
subplot(1, 2, 1);
hold on;
plot(Eg.Time, Eg.Data(:, 1), 'DisplayName', 'X');
plot(Eg.Time, Eg.Data(:, 2), 'DisplayName', 'Y');
plot(Eg.Time, Eg.Data(:, 3), 'DisplayName', 'Z');
hold off;
xlabel('Time [s]');
ylabel('Position error [m]');
legend();

subplot(1, 2, 2);
hold on;
plot(simout.Xa.Time, simout.Xa.Data(:, 1));
plot(simout.Xa.Time, simout.Xa.Data(:, 2));
plot(simout.Xa.Time, simout.Xa.Data(:, 3));
hold off;
xlabel('Time [s]');
ylabel('Orientation error [rad]');
#+end_src

* Functions
** =initializeController=: Initialize the Controller
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeController.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeController>>

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [controller] = initializeController(args)
% initializeController - Initialize the Controller
%
% Syntax: [] = initializeController(args)
%
% Inputs:
%    - args - Can have the following fields:
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
    args.type   char   {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop'
end
#+end_src

*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
controller = struct();
#+end_src

*** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
  case 'open-loop'
    controller.type = 0;
  case 'iff'
    controller.type = 1;
  case 'dvf'
    controller.type = 2;
  case 'hac-iff'
    controller.type = 3;
  case 'hac-dvf'
    controller.type = 4;
  case 'ref-track-L'
    controller.type = 5;
  case 'ref-track-X'
    controller.type = 6;
  case 'ref-track-hac-dvf'
    controller.type = 7;
end
#+end_src