#+TITLE: Stewart Platform - Tracking Control
: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="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="./js/readtheorg.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/thesis/latex/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:
Let's suppose the control objective is to position $\bm{\mathcal{X}}$ of the mobile platform of the Stewart platform such that it is following some reference position $\bm{r}_\mathcal{X}$.

Depending of the measured quantity, different control architectures can be used:
- If the struts length $\bm{\mathcal{L}}$ is measured, a decentralized control architecture can be used (Section [[sec:decentralized]])
- If the pose of the mobile platform $\bm{\mathcal{X}}$ is directly measured, a centralized control architecture can be used (Section [[sec:centralized]])
- If both $\bm{\mathcal{L}}$ and $\bm{\mathcal{X}}$ are measured, we can use an hybrid control architecture (Section [[sec:hybrid]])

* Decentralized Control Architecture using Strut Length
<<sec:decentralized>>
** 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

** Control Schematic
The control architecture is shown in Figure [[fig:control_measure_rotating_2dof]].

The required leg length $\bm{r}_\mathcal{L}$ is computed from the reference path $\bm{r}_\mathcal{X}$ using the inverse kinematics.

Then, a diagonal (decentralized) controller $\bm{K}_\mathcal{L}$ is used such that each leg lengths stays close to its required length.

#+begin_src latex :file control_measure_rotating_2dof.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block, align=center] (J) at (0, 0) {Inverse\\Kinematics};
    \node[addb={+}{}{}{}{-}, right=1 of J] (subr) {};
    \node[block, right=0.8 of subr] (K) {$\bm{K}_\mathcal{L}$};
    \node[block, right=1 of K] (G) {$\bm{G}_\mathcal{L}$};

    % Connections and labels
    \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
    \draw[->] (K.east) -- (G.west) node[above left]{$\bm{\tau}$};
    \draw[->] (G.east) node[above right]{$\bm{\mathcal{L}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
  \end{tikzpicture}
#+end_src

#+name: fig:control_measure_rotating_2dof
#+caption: Decentralized control for reference tracking
#+RESULTS:
[[file:figs/control_measure_rotating_2dof.png]]

** 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_p', 'type_M', 'spherical_p');
  stewart = initializeCylindricalPlatforms(stewart);
  stewart = initializeCylindricalStruts(stewart);
  stewart = computeJacobian(stewart);
  stewart = initializeStewartPose(stewart);
  stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
#+end_src

#+begin_src matlab
  ground = initializeGround('type', 'rigid');
  payload = initializePayload('type', 'none');
  controller = initializeController('type', 'open-loop');
#+end_src

#+begin_src matlab
  disturbances = initializeDisturbances();
  references = initializeReferences(stewart);
#+end_src

** Identification of the plant
Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{L}}$.
#+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, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
  io(io_i) = linio([mdl, '/Stewart Platform'],  1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]

  %% Run the linearization
  G = linearize(mdl, io);
  G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
  G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
#+end_src

** Plant Analysis
The diagonal terms of the plant is shown in Figure [[fig:plant_decentralized_diagonal]].

All the diagonal terms are equal.

#+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(G(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(G(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/plant_decentralized_diagonal.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:plant_decentralized_diagonal
#+caption: Diagonal Elements of the Plant ([[./figs/plant_decentralized_diagonal.png][png]], [[./figs/plant_decentralized_diagonal.pdf][pdf]])
[[file:figs/plant_decentralized_diagonal.png]]

The off-diagonal terms are shown in Figure [[fig:plant_decentralized_off_diagonal]].

We see that the plant is decoupled at low frequency which indicate that decentralized control may be a good idea.

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(G(1, 1), 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;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(G(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],'x');
#+end_src

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

#+name: fig:plant_decentralized_off_diagonal
#+caption: Diagonal Elements of the Plant ([[./figs/plant_decentralized_off_diagonal.png][png]], [[./figs/plant_decentralized_off_diagonal.pdf][pdf]])
[[file:figs/plant_decentralized_off_diagonal.png]]

** Controller Design
The controller consists of:
- A pure integrator
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin

The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:loop_gain_decentralized_L]].

#+begin_src matlab
  wc = 2*pi*30;
  Kl = diag(1./diag(abs(freqresp(G, wc)))) * wc/s * 1/(1 + s/3/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(Kl(i, i)*G(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(Kl(i, i)*G(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_decentralized_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:loop_gain_decentralized_L
#+caption: Loop Gain of the diagonal elements ([[./figs/loop_gain_decentralized_L.png][png]], [[./figs/loop_gain_decentralized_L.pdf][pdf]])
[[file:figs/loop_gain_decentralized_L.png]]

** Simulation
#+begin_src matlab
  t = linspace(0, 10, 1000);

  r = zeros(6, length(t));

  r(1, :) = 5e-3*sin(2*pi*t);

  references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src

#+begin_src matlab
  controller = initializeController('type', 'ref-track-L');
#+end_src

#+begin_src matlab
  sim('stewart_platform_model')
  simout_D = simout;
#+end_src

** Results
#+begin_src matlab :exports none
  figure;
  subplot(2, 3, 1);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(1, 1, :))-simout_D.x.Xr.Data(:, 1), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  subplot(2, 3, 2);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(2, 1, :))-simout_D.x.Xr.Data(:, 2), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  subplot(2, 3, 3);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(3, 1, :))-simout_D.x.Xr.Data(:, 3), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  subplot(2, 3, 4);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(4, 1, :))-simout_D.x.Xr.Data(:, 4), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  subplot(2, 3, 5);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(5, 1, :))-simout_D.x.Xr.Data(:, 5), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  subplot(2, 3, 6);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.r.Data(6, 1, :))-simout_D.x.Xr.Data(:, 6), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
#+end_src

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

#+name: fig:decentralized_control_Ex
#+caption: Position error $\bm{\epsilon}_\mathcal{X}$ ([[./figs/decentralized_control_Ex.png][png]], [[./figs/decentralized_control_Ex.pdf][pdf]])
[[file:figs/decentralized_control_Ex.png]]

#+begin_src matlab :exports none
  figure;
  subplot(2, 3, 1);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.rL.Data(1, 1, :))-simout_D.y.dLm.Data(:, 1), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('L1 [m]');

  subplot(2, 3, 2);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.rL.Data(2, 1, :))-simout_D.y.dLm.Data(:, 2), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('L1 [m]');

  subplot(2, 3, 3);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.rL.Data(3, 1, :))-simout_D.y.dLm.Data(:, 3), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('L1 [m]');

  subplot(2, 3, 4);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.rL.Data(4, 1, :))-simout_D.y.dLm.Data(:, 4), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('L4 [m]');

  subplot(2, 3, 5);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.rL.Data(5, 1, :))-simout_D.y.dLm.Data(:, 5), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('L5 [m]');

  subplot(2, 3, 6);
  hold on;
  plot(simout_D.r.r.Time, squeeze(simout_D.r.rL.Data(6, 1, :))-simout_D.y.dLm.Data(:, 6), 'k-')
  hold off;
  xlabel('Time [s]');
  ylabel('L6 [m]');
#+end_src

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

#+name: fig:decentralized_control_El
#+caption: Position error $\bm{\epsilon}_\mathcal{L}$ ([[./figs/decentralized_control_El.png][png]], [[./figs/decentralized_control_El.pdf][pdf]])
[[file:figs/decentralized_control_El.png]]

** Conclusion
Such control architecture is easy to implement and give good results.
The inverse kinematics is easy to compute.

However, as $\mathcal{X}$ is not directly measured, it is possible that important positioning errors are due to finite stiffness of the joints and other imperfections.

* Centralized Control Architecture using Pose Measurement
<<sec:centralized>>
** 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

** Control Schematic
The centralized controller takes the position error $\bm{\epsilon}_\mathcal{X}$ as an inputs and generate actuator forces $\bm{\tau}$ (see Figure [[fig:centralized_reference_tracking]]).
The signals are:
- reference path $\bm{r}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}$
- tracking error $\bm{\epsilon}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}$
- actuator forces $\bm{\tau} = \begin{bmatrix} \tau_1 & \tau_2 & \tau_3 & \tau_4 & \tau_5 & \tau_6 \end{bmatrix}$
- payload pose $\bm{\mathcal{X}} = \begin{bmatrix} x & y & z & R_x & R_y & R_z \end{bmatrix}$

#+begin_src latex :file centralized_reference_tracking.pdf
  \begin{tikzpicture}
    % Blocs
    \node[addb={+}{}{}{}{-}] (subr) at (0, 0) {};
    \node[block, right=0.8 of subr] (K) {$K$};
    \node[block, right=1 of K] (G) {$G$};

    % Connections and labels
    \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) -- (G.west) node[above left]{$\bm{\tau}$};
    \draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
  \end{tikzpicture}
#+end_src

#+name: fig:centralized_reference_tracking
#+caption: Centralized Controller
#+RESULTS:
[[file:figs/centralized_reference_tracking.png]]

Instead of designing a full MIMO controller $K$, we first try to make the plant more diagonal by pre- or post-multiplying some constant matrix, then we design a diagonal controller.

We can think of two ways to make the plant more diagonal that are described in sections [[sec:diagonal_control_L]] and [[sec:diagonal_control_X]].

#+begin_important
  Note here that the subtraction shown in Figure [[fig:centralized_reference_tracking]] is not a real subtraction.
  It is indeed a more complex computation explained in section [[sec:compute_pose_error]].
#+end_important

** 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_p', 'type_M', 'spherical_p');
  stewart = initializeCylindricalPlatforms(stewart);
  stewart = initializeCylindricalStruts(stewart);
  stewart = computeJacobian(stewart);
  stewart = initializeStewartPose(stewart);
  stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
#+end_src

#+begin_src matlab
  ground = initializeGround('type', 'rigid');
  payload = initializePayload('type', 'none');
  controller = initializeController('type', 'open-loop');
#+end_src

#+begin_src matlab
  disturbances = initializeDisturbances();
  references = initializeReferences(stewart);
#+end_src

** Identification of the plant
Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{X}}$.
#+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, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
  io(io_i) = linio([mdl, '/Relative Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [m]

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

** Diagonal Control - Leg's Frame
<<sec:diagonal_control_L>>
*** Control Architecture
The pose error $\bm{\epsilon}_\mathcal{X}$ is first converted in the frame of the leg by using the Jacobian matrix.
Then a diagonal controller $\bm{K}_\mathcal{L}$ is designed.
The final implemented controller is $\bm{K} = \bm{K}_\mathcal{L} \cdot \bm{J}$ as shown in Figure [[fig:centralized_reference_tracking_L]]

Note here that the transformation from the pose error $\bm{\epsilon}_\mathcal{X}$ to the leg's length errors by using the Jacobian matrix is only valid for small errors.

#+begin_src latex :file centralized_reference_tracking_L.pdf
  \begin{tikzpicture}
    % Blocs
    \node[addb={+}{}{}{}{-}] (subr) at (0, 0) {};
    \node[block, right= 1 of subr] (J) {$\bm{J}$};
    \node[block, right=0.8 of J] (K) {$\bm{K}_{\mathcal{L}}$};
    \node[block, right=1 of K] (G) {$\bm{G}$};

    % Connections and labels
    \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) -- (G.west) node[above left]{$\bm{\tau}$};
    \draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);

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

#+name: fig:centralized_reference_tracking_L
#+caption: Controller in the frame of the legs
#+RESULTS:
[[file:figs/centralized_reference_tracking_L.png]]

*** Plant Analysis
We now multiply the plant by the Jacobian matrix as shown in Figure [[fig:centralized_reference_tracking_L]] to obtain a more diagonal plant.

#+begin_src matlab
  Gl = stewart.kinematics.J*G;
  Gl.OutputName  = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
#+end_src

#+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(Gl(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(Gl(i, i), freqs, 'Hz'))), 'DisplayName', ['$d\mathcal{L}_' num2str(i) '/\tau_' num2str(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();

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

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

#+name: fig:plant_centralized_diagonal_L
#+caption: Diagonal Elements of the plant $\bm{J} \bm{G}$ ([[./figs/plant_centralized_diagonal_L.png][png]], [[./figs/plant_centralized_diagonal_L.pdf][pdf]])
[[file:figs/plant_centralized_diagonal_L.png]]

All the diagonal elements are identical.
This will simplify the design of the controller as all the elements of the diagonal controller can be made identical.

The off-diagonal terms of the controller are shown in Figure [[fig:plant_centralized_off_diagonal_L]].

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

  figure;

  ax1 = subplot(2, 1, 1);
  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',[]);

  ax2 = subplot(2, 1, 2);
  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],'x');
#+end_src

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

#+name: fig:plant_centralized_off_diagonal_L
#+caption: Off Diagonal Elements of the plant $\bm{J} \bm{G}$ ([[./figs/plant_centralized_off_diagonal_L.png][png]], [[./figs/plant_centralized_off_diagonal_L.pdf][pdf]])
[[file:figs/plant_centralized_off_diagonal_L.png]]

We can see that this *totally decouples the system at low frequency*.

This was expected since:
\[ \bm{G}(\omega = 0) = \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \]

Thus $J \cdot G(\omega = 0) = J \cdot \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)$ is a diagonal matrix containing the inverse of the joint's stiffness.

*** Controller Design
The controller consists of:
- A pure integrator
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin

The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:loop_gain_centralized_L]].

#+begin_src matlab
  wc = 2*pi*30;
  Kl = diag(1./diag(abs(freqresp(Gl, wc)))) * wc/s * 1/(1 + s/3/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(Kl(i, i)*Gl(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(Kl(i, i)*Gl(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_centralized_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:loop_gain_centralized_L
#+caption: Loop Gain of the diagonal elements ([[./figs/loop_gain_centralized_L.png][png]], [[./figs/loop_gain_centralized_L.pdf][pdf]])
[[file:figs/loop_gain_centralized_L.png]]

The controller $\bm{K} = \bm{K}_\mathcal{L} \bm{J}$ is computed.
#+begin_src matlab
  K = Kl*stewart.kinematics.J;
#+end_src

*** Simulation
We specify the reference path to follow.
#+begin_src matlab
  t = linspace(0, 10, 1000);

  r = zeros(6, length(t));

  r(1, :) = 5e-3*sin(2*pi*t);

  references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src

#+begin_src matlab
  controller = initializeController('type', 'ref-track-X');
#+end_src

We run the simulation and we save the results.
#+begin_src matlab
  sim('stewart_platform_model')
  simout_L = simout;
#+end_src

** Diagonal Control - Cartesian Frame
<<sec:diagonal_control_X>>
*** Control Architecture
A diagonal controller $\bm{K}_\mathcal{X}$ take the pose error $\bm{\epsilon}_\mathcal{X}$ and generate cartesian forces $\bm{\mathcal{F}}$ that are then converted to actuators forces using the Jacobian as shown in Figure e [[fig:centralized_reference_tracking_X]].

The final implemented controller is $\bm{K} = \bm{J}^{-T} \cdot \bm{K}_\mathcal{X}$.

#+begin_src latex :file centralized_reference_tracking_X.pdf
  \begin{tikzpicture}
    % Blocs
    \node[addb={+}{}{}{}{-}] (subr) at (0, 0) {};
    \node[block, right= of subr] (K) {$\bm{K}_{\mathcal{X}}$};
    \node[block, right= of K] (J) {$\bm{J}^{-T}$};
    \node[block, right= of J] (G) {$\bm{G}$};

    % Connections and labels
    \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) -- (G.west) node[above left]{$\bm{\tau}$};
    \draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);

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

#+name: fig:centralized_reference_tracking_X
#+caption: Controller in the cartesian frame
#+RESULTS:
[[file:figs/centralized_reference_tracking_X.png]]

*** Plant Analysis
We now multiply the plant by the Jacobian matrix as shown in Figure [[fig:centralized_reference_tracking_X]] to obtain a more diagonal plant.

#+begin_src matlab
  Gx = G*inv(stewart.kinematics.J');
  Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

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

  labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};

  figure;

  ax1 = subplot(2, 1, 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',[]);

  ax2 = subplot(2, 1, 2);
  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();

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

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

#+name: fig:plant_centralized_diagonal_X
#+caption: Diagonal Elements of the plant $\bm{G} \bm{J}^{-T}$ ([[./figs/plant_centralized_diagonal_X.png][png]], [[./figs/plant_centralized_diagonal_X.pdf][pdf]])
[[file:figs/plant_centralized_diagonal_X.png]]

The diagonal terms are not the same.
The resonances of the system are "decoupled".
For instance, the vertical resonance of the system is only present on the diagonal term corresponding to $D_z/\mathcal{F}_z$.

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

  figure;

  ax1 = subplot(2, 1, 1);
  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',[]);

  ax2 = subplot(2, 1, 2);
  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],'x');
#+end_src

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

#+name: fig:plant_centralized_off_diagonal_X
#+caption: Off Diagonal Elements of the plant $\bm{G} \bm{J}^{-T}$ ([[./figs/plant_centralized_off_diagonal_X.png][png]], [[./figs/plant_centralized_off_diagonal_X.pdf][pdf]])
[[file:figs/plant_centralized_off_diagonal_X.png]]

Here the system is almost decoupled at all frequencies except for the transfer functions $\frac{R_y}{\mathcal{F}_x}$ and  $\frac{R_x}{\mathcal{F}_y}$.

This is due to the fact that the compliance matrix of the Stewart platform is not diagonal.
#+begin_src matlab :exports results :results value table replace :tangle no
  inv(stewart.kinematics.K)
#+end_src

#+RESULTS:
|    4.75e-08 | -1.9751e-24 | 7.3536e-25 |   5.915e-23 |  3.2093e-07 |  5.8696e-24 |
| -7.1302e-25 |    4.75e-08 | 2.8866e-25 | -3.2093e-07 |   -5.38e-24 | -3.2725e-23 |
|  7.9012e-26 | -6.3991e-25 |  2.099e-08 |  1.9073e-23 |  5.3384e-25 | -6.4867e-40 |
|  1.3724e-23 | -3.2093e-07 | 1.2799e-23 |  5.1863e-06 |  4.9412e-22 | -3.8269e-24 |
|  3.2093e-07 |  7.6013e-24 | 1.2239e-23 |  6.8886e-22 |  5.1863e-06 | -2.6025e-22 |
|   7.337e-24 | -3.2395e-23 | -1.571e-39 |   9.927e-23 | -3.2531e-22 |  1.7073e-06 |

One way to have this compliance matrix diagonal (and thus having a decoupled plant at DC) is to use a *cubic architecture* with the center of the cube's coincident with frame $\{A\}$.

This control architecture can also give a dynamically decoupled plant if the Center of mass of the payload is also coincident with frame $\{A\}$.

*** Controller Design
The controller consists of:
- A pure integrator
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin

The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:loop_gain_centralized_X]].

#+begin_src matlab
  wc = 2*pi*30;
  Kx = diag(1./diag(abs(freqresp(Gx, wc)))) * wc/s * 1/(1 + s/3/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(Kx(i,i)*Gx(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(Kx(i,i)*Gx(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_centralized_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:loop_gain_centralized_X
#+caption: Loop Gain of the diagonal elements ([[./figs/loop_gain_centralized_X.png][png]], [[./figs/loop_gain_centralized_X.pdf][pdf]])
[[file:figs/loop_gain_centralized_X.png]]

The controller $\bm{K} = \bm{J}^{-T} \bm{K}_\mathcal{X}$ is computed.
#+begin_src matlab
  K = inv(stewart.kinematics.J')*Kx;
#+end_src

*** Simulation
We specify the reference path to follow.
#+begin_src matlab
  t = linspace(0, 10, 1000);

  r = zeros(6, length(t));

  r(1, :) = 5e-3*sin(2*pi*t);

  references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src

#+begin_src matlab
  controller = initializeController('type', 'ref-track-X');
#+end_src

We run the simulation and we save the results.
#+begin_src matlab
  sim('stewart_platform_model')
  simout_X = simout;
#+end_src

** Diagonal Control - Steady State Decoupling
<<sec:diagonal_control_SD>>
*** Control Architecture
The plant $\bm{G}$ is pre-multiply by $\bm{G}^{-1}(\omega = 0)$ such that the "shaped plant" $\bm{G}_0 = \bm{G} \bm{G}^{-1}(\omega = 0)$ is diagonal at low frequency.

Then a diagonal controller $\bm{K}_0$ is designed.

The control architecture is shown in Figure [[fig:centralized_reference_tracking_S]].

#+begin_src latex :file centralized_reference_tracking_S.pdf
  \begin{tikzpicture}
    % Blocs
    \node[addb={+}{}{}{}{-}] (subr) at (0, 0) {};
    \node[block, right=0.8 of subr] (K) {$\bm{K}_{0}$};
    \node[block, right= 1 of K] (J) {$\bm{G}^{-1}(\omega=0)$};
    \node[block, right=1 of J] (G) {$\bm{G}$};

    % Connections and labels
    \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);
    \draw[->] (J.east) -- (G.west) node[above left]{$\bm{\tau}$};
    \draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ($(G.east)+(1, -1)$) -| (subr.south);

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

#+name: fig:centralized_reference_tracking_S
#+caption: Static Decoupling of the Plant
#+RESULTS:
[[file:figs/centralized_reference_tracking_S.png]]

*** Plant Analysis
The plant is pre-multiplied by $\bm{G}^{-1}(\omega = 0)$.
The diagonal elements of the shaped plant are shown in Figure [[fig:plant_centralized_diagonal_SD]].

#+begin_src matlab
  G0 = G*inv(freqresp(G, 0));
#+end_src

#+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(G0(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(G0(i, i), freqs, 'Hz'))), 'DisplayName', ['$G_0(' num2str(i) ',' num2str(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();

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

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

#+name: fig:plant_centralized_diagonal_SD
#+caption: Diagonal Elements of the plant $\bm{G} \bm{G}^{-1}(\omega = 0)$ ([[./figs/plant_centralized_diagonal_SD.png][png]], [[./figs/plant_centralized_diagonal_SD.pdf][pdf]])
[[file:figs/plant_centralized_diagonal_SD.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  hold on;
  for i = 1:5
    for j = i+1:6
      plot(freqs, abs(squeeze(freqresp(G0(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, abs(squeeze(freqresp(G0(1, 1), 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;
  for i = 1:5
    for j = i+1:6
      plot(freqs, 180/pi*angle(squeeze(freqresp(G0(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
    end
  end
  set(gca,'ColorOrderIndex',1);
  plot(freqs, 180/pi*angle(squeeze(freqresp(G0(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],'x');
#+end_src

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

#+name: fig:plant_centralized_off_diagonal_SD
#+caption: Off Diagonal Elements of the plant $\bm{G} \bm{J}^{-T}$ ([[./figs/plant_centralized_off_diagonal_SD.png][png]], [[./figs/plant_centralized_off_diagonal_SD.pdf][pdf]])
[[file:figs/plant_centralized_off_diagonal_SD.png]]

*** Controller Design
We have that:
\[ \bm{G}^{-1}(\omega = 0) = \left(\frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\right)^{-1} = \left( \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) \right)^{-1} = \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \bm{J} \]

And because:
- all the leg stiffness are equal
- the controller equal to a $\bm{K}_0(s) = k(s) \bm{I}_6$
We have that $\bm{K}_0(s)$ commutes with $\bm{G}^{-1}(\omega = 0)$ and thus the overall controller $\bm{K}$ is the same as the one obtain in section [[sec:diagonal_control_L]].

** Comparison
*** Obtained MIMO Controllers
#+begin_src matlab :exports none
  figure;
  bode(inv(stewart.kinematics.J')*Kx, Kl*stewart.kinematics.J)
#+end_src

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

#+name: fig:centralized_control_comp_K
#+caption: Comparison of the MIMO controller $\bm{K}$ for both centralized architectures ([[./figs/centralized_control_comp_K.png][png]], [[./figs/centralized_control_comp_K.pdf][pdf]])
[[file:figs/centralized_control_comp_K.png]]

*** Simulation Results
The position error $\bm{\epsilon}_\mathcal{X}$ for both centralized architecture are shown in Figure [[fig:centralized_control_comp_Ex]].
The corresponding leg's length errors $\bm{\epsilon}_\mathcal{L}$ are shown in Figure [[fig:centralized_control_comp_El]].

Based on Figure [[fig:centralized_control_comp_Ex]], we can see that:
- There is some tracking error $\epsilon_x$
- The errors $\epsilon_y$, $\epsilon_{R_x}$ and $\epsilon_{R_z}$ are quite negligible
- There is some error in the vertical position $\epsilon_z$.
  The frequency of the error $\epsilon_z$ is twice the frequency of the reference path $r_x$.
- There is some error $\epsilon_{R_y}$.
  This error is much lower when using the diagonal control in the frame of the leg instead of the cartesian frame.

#+begin_src matlab :exports none
  figure;
  subplot(2, 3, 1);
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(1, 1, :))-simout_L.x.Xr.Data(:, 1))
  plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(1, 1, :))-simout_X.x.Xr.Data(:, 1))
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  subplot(2, 3, 2);
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(2, 1, :))-simout_L.x.Xr.Data(:, 2))
  plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(2, 1, :))-simout_X.x.Xr.Data(:, 2))
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  subplot(2, 3, 3);
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(3, 1, :))-simout_L.x.Xr.Data(:, 3))
  plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(3, 1, :))-simout_X.x.Xr.Data(:, 3))
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  subplot(2, 3, 4);
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(4, 1, :))-simout_L.x.Xr.Data(:, 4))
  plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(4, 1, :))-simout_X.x.Xr.Data(:, 4))
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  subplot(2, 3, 5);
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(5, 1, :))-simout_L.x.Xr.Data(:, 5))
  plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(5, 1, :))-simout_X.x.Xr.Data(:, 5))
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  subplot(2, 3, 6);
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.r.Data(6, 1, :))-simout_L.x.Xr.Data(:, 6), 'DisplayName', '$K_\mathcal{L}$')
  plot(simout_X.r.r.Time, squeeze(simout_X.r.r.Data(6, 1, :))-simout_X.x.Xr.Data(:, 6), 'DisplayName', '$K_\mathcal{X}$')
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
  legend();
#+end_src

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

#+name: fig:centralized_control_comp_Ex
#+caption: Comparison of the position error $\bm{\epsilon}_\mathcal{X}$ for both centralized controllers ([[./figs/centralized_control_comp_Ex.png][png]], [[./figs/centralized_control_comp_Ex.pdf][pdf]])
[[file:figs/centralized_control_comp_Ex.png]]

#+begin_src matlab :exports none
  figure;
  hold on;
  plot(simout_L.r.r.Time, squeeze(simout_L.r.rL.Data(6, 1, :))-simout_L.y.dLm.Data(:, 6), 'DisplayName', '$K_\mathcal{L}$')
  plot(simout_X.r.r.Time, squeeze(simout_X.r.rL.Data(6, 1, :))-simout_X.y.dLm.Data(:, 6), 'DisplayName', '$K_\mathcal{X}$')
  for i = 2:6
    set(gca,'ColorOrderIndex',1);
    plot(simout_L.r.r.Time, squeeze(simout_L.r.rL.Data(i, 1, :))-simout_L.y.dLm.Data(:, i), 'HandleVisibility', 'off')
    plot(simout_X.r.r.Time, squeeze(simout_X.r.rL.Data(i, 1, :))-simout_X.y.dLm.Data(:, i), 'HandleVisibility', 'off')
  end
  hold off;
  xlabel('Time [s]');
  ylabel('$\epsilon_\mathcal{L}$ [m]');
  legend();
#+end_src

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

#+name: fig:centralized_control_comp_El
#+caption: Comparison of the leg's length error $\bm{\epsilon}_\mathcal{L}$ for both centralized controllers ([[./figs/centralized_control_comp_El.png][png]], [[./figs/centralized_control_comp_El.pdf][pdf]])
[[file:figs/centralized_control_comp_El.png]]

** Conclusion
Both control architecture gives similar results even tough the control in the Leg's frame gives slightly better results.

The main differences between the control architectures used in sections [[sec:diagonal_control_L]] and [[sec:diagonal_control_X]] are summarized in Table [[tab:centralized_control_comp]].

#+name: tab:centralized_control_comp
#+caption: Comparison of the two centralized control architectures
|                             | *Leg's Frame*                     | *Cartesian Frame*                     | *Static Decoupling*            |
|-----------------------------+-----------------------------------+---------------------------------------+--------------------------------|
| *Plant Meaning*             | $\delta\mathcal{L}_i/\tau_i$      | $\delta\mathcal{X}_i/\mathcal{F}_i$   | No physical meaning            |
| *Obtained Decoupling*       | Decoupled at DC                   | Dynamical decoupling except few terms | Decoupled at DC                |
| *Diagonal Elements*         | Identical with all the Resonances | Different, resonances are cancel out  | No Alternating poles and zeros |
| *Mechanical Architecture*   | Architecture Independent          | Better with Cubic Architecture        |                                |
| *Robustness to Uncertainty* | Good (only depends on $J$)        | Good (only depends on $J$)            | Bad (depends on the mass)      |

These decoupling methods only uses the Jacobian matrix which only depends on the Stewart platform geometry.
Thus, this method should be quite robust against parameter variation (e.g. the payload mass).

* Hybrid Control Architecture - HAC-LAC with relative DVF
<<sec:hybrid>>
** Introduction                                                      :ignore:
** 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

** Control Schematic
Let's consider the control schematic shown in Figure [[fig:hybrid_reference_tracking]].

The first loop containing $\bm{K}_\mathcal{L}$ is a Decentralized Direct (Relative) Velocity Feedback.

A reference $\bm{r}_\mathcal{L}$ is computed using the inverse kinematics and corresponds to the wanted motion of each leg.
The actual length of each leg $\bm{\mathcal{L}}$ is subtracted and then passed trough the controller $\bm{K}_\mathcal{L}$.

The controller is a diagonal controller with pure derivative action on the diagonal.

The effect of this loop is:
- it adds damping to the system (the force applied in each actuator is proportional to the relative velocity of the strut)
- it however does not go "against" the reference path $\bm{r}_\mathcal{X}$ thanks to the use of the inverse kinematics

Then, the second loop containing $\bm{K}_\mathcal{X}$ is designed based on the already damped plant (represented by the gray area).
This second loop is responsible for the reference tracking.

#+begin_src latex :file hybrid_reference_tracking.pdf
  \begin{tikzpicture}
    \node[block={3.0cm}{3.0cm}] (G) {$G$};

    % Input and outputs coordinates
    \coordinate[] (outputX) at ($(G.south east)!0.25!(G.north east)$);
    \coordinate[] (outputL) at ($(G.south east)!0.75!(G.north east)$);

    \draw[->] (outputX) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] (outputL) -- ++(1.8, 0) node[above left]{$\bm{\mathcal{L}}$};

    % Blocs
    \node[addb, left= of G] (addF) {};
    \node[block, left=1.2 of addF] (Kx) {$\bm{K}_\mathcal{X}$};
    \node[addb={+}{}{}{}{-}, left= of Kx] (subx) {};

    \node[block, above= of addF] (Kl) {$\bm{K}_\mathcal{L}$};
    \node[addb={+}{}{}{-}{}, above= of Kl] (subl) {};

    \node[block, align=center, left= of subl] (invK) {Inverse\\Kinematics};

    % Connections and labels
    \draw[<-] (subx.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1.2, 0);
    \draw[->] ($(subx.west) + (-0.8, 0)$)node[branch]{} |- (invK.west);
    \draw[->] (invK.east) -- (subl.west) node[above left]{$\bm{r}_\mathcal{L}$};
    \draw[->] (subl.south) -- (Kl.north) node[above right]{$\bm{\epsilon}_\mathcal{L}$};
    \draw[->] (Kl.south) -- (addF.north);

    \draw[->] (subx.east) -- (Kx.west) node[above left]{$\bm{\epsilon}_\mathcal{X}$};
    \draw[->] (Kx.east) node[above right]{$\bm{\tau}_\mathcal{X}$} -- (addF.west);
    \draw[->] (addF.east) -- (G.west) node[above left]{$\bm{\tau}$};

    \draw[->] ($(outputL.east) + (0.4, 0)$)node[branch](L){} |- (subl.east);
    \draw[->] ($(outputX.east) + (1.2, 0)$)node[branch]{} -- ++(0, -1.6) -| (subx.south);

    \begin{scope}[on background layer]
      \node[fit={(G.south-|Kl.west) (L|-subl.north)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Ktot) {};
    \end{scope}
  \end{tikzpicture}
#+end_src

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

** 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_p', 'type_M', 'spherical_p');
  stewart = initializeCylindricalPlatforms(stewart);
  stewart = initializeCylindricalStruts(stewart);
  stewart = computeJacobian(stewart);
  stewart = initializeStewartPose(stewart);
  stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
#+end_src

#+begin_src matlab
  ground = initializeGround('type', 'rigid');
  payload = initializePayload('type', 'none');
  controller = initializeController('type', 'open-loop');
#+end_src

#+begin_src matlab
  disturbances = initializeDisturbances();
  references = initializeReferences(stewart);
#+end_src

** First Control Loop - $\bm{K}_\mathcal{L}$
*** Identification
Let's identify the transfer function from $\bm{\tau}$ to $\bm{L}$.
#+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, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
  io(io_i) = linio([mdl, '/Stewart Platform'],  1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]

  %% Run the linearization
  Gl = linearize(mdl, io);
  Gl.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
  Gl.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
#+end_src

*** Obtained Plant
The diagonal elements of the plant are shown in Figure [[fig:hybrid_control_Kl_plant_diagonal]] while the off diagonal terms are shown in Figure [[fig:hybrid_control_Kl_plant_off_diagonal]].

#+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(Gl(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(Gl(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/hybrid_control_Kl_plant_diagonal.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:hybrid_control_Kl_plant_diagonal
#+caption: Diagonal elements of the plant for the design of $\bm{K}_\mathcal{L}$ ([[./figs/hybrid_control_Kl_plant_diagonal.png][png]], [[./figs/hybrid_control_Kl_plant_diagonal.pdf][pdf]])
[[file:figs/hybrid_control_Kl_plant_diagonal.png]]

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

  figure;

  ax1 = subplot(2, 1, 1);
  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',[]);

  ax2 = subplot(2, 1, 2);
  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],'x');
#+end_src

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

#+name: fig:hybrid_control_Kl_plant_off_diagonal
#+caption: Off-diagonal elements of the plant for the design of $\bm{K}_\mathcal{L}$ ([[./figs/hybrid_control_Kl_plant_off_diagonal.png][png]], [[./figs/hybrid_control_Kl_plant_off_diagonal.pdf][pdf]])
[[file:figs/hybrid_control_Kl_plant_off_diagonal.png]]

*** Controller Design
We apply a decentralized (diagonal) direct velocity feedback.
Thus, we apply a pure derivative action.
In order to make the controller realizable, we add a low pass filter at high frequency.
The gain of the controller is chosen such that the resonances are critically damped.

The obtain loop gain is shown in Figure [[fig:hybrid_control_Kl_loop_gain]].

#+begin_src matlab
  Kl = 1e4 * s / (1 + s/2/pi/1e4) * eye(6);
#+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(Gl(1, 1)*Kl(1,1), 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(Gl(1, 1)*Kl(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],'x');
#+end_src

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

#+name: fig:hybrid_control_Kl_loop_gain
#+caption: Obtain Loop Gain for the DVF control loop ([[./figs/hybrid_control_Kl_loop_gain.png][png]], [[./figs/hybrid_control_Kl_loop_gain.pdf][pdf]])
[[file:figs/hybrid_control_Kl_loop_gain.png]]

** Second Control Loop - $\bm{K}_\mathcal{X}$
*** Identification
#+begin_src matlab
  Kx = tf(zeros(6));

  controller = initializeController('type', 'ref-track-hac-dvf');
#+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, '/Relative Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [m]

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

*** Obtained Plant
We use the Jacobian matrix to apply forces in the cartesian frame.
#+begin_src matlab
  Gx = G*inv(stewart.kinematics.J');
  Gx.InputName  = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src

The obtained plant is shown in Figure [[fig:hybrid_control_Kx_plant]].
#+begin_src matlab :exports none
  freqs = logspace(1, 4, 1000);

  labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};

  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

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

#+name: fig:hybrid_control_Kx_plant
#+caption: Diagonal and Off-diagonal elements of the plant for the design of $\bm{K}_\mathcal{L}$ ([[./figs/hybrid_control_Kx_plant.png][png]], [[./figs/hybrid_control_Kx_plant.pdf][pdf]])
[[file:figs/hybrid_control_Kx_plant.png]]

*** Controller Design
The controller consists of:
- A pure integrator
- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin

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

  h = 3; % Lead parameter

  Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc/2 + 1)/(s/wc/2));

  % 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(1, 3, 1000);

  figure;

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

#+name: fig:hybrid_control_Kx_loop_gain
#+caption: Obtained Loop Gain for the controller $\bm{K}_\mathcal{X}$ ([[./figs/hybrid_control_Kx_loop_gain.png][png]], [[./figs/hybrid_control_Kx_loop_gain.pdf][pdf]])
[[file:figs/hybrid_control_Kx_loop_gain.png]]

Then we include the Jacobian in the controller matrix.
#+begin_src matlab
  Kx = inv(stewart.kinematics.J')*Kx;
#+end_src

** Simulations
We specify the reference path to follow.
#+begin_src matlab
  t = linspace(0, 10, 10000);

  r = zeros(6, length(t));

  r(1, :) = 5e-3*sin(2*pi*t);

  references = initializeReferences(stewart, 't', t, 'r', r);
#+end_src

We run the simulation and we save the results.
#+begin_src matlab
  sim('stewart_platform_model')
  simout_H = simout;
#+end_src

The obtained position error is shown in Figure [[fig:hybrid_control_Ex]].

#+begin_src matlab :exports none
  figure;
  subplot(2, 3, 1);
  hold on;
  plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(1, 1, :))-simout_H.x.Xr.Data(:, 1))
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  subplot(2, 3, 2);
  hold on;
  plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(2, 1, :))-simout_H.x.Xr.Data(:, 2))
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  subplot(2, 3, 3);
  hold on;
  plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(3, 1, :))-simout_H.x.Xr.Data(:, 3))
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  subplot(2, 3, 4);
  hold on;
  plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(4, 1, :))-simout_H.x.Xr.Data(:, 4))
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  subplot(2, 3, 5);
  hold on;
  plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(5, 1, :))-simout_H.x.Xr.Data(:, 5))
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  subplot(2, 3, 6);
  hold on;
  plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(6, 1, :))-simout_H.x.Xr.Data(:, 6))
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
#+end_src

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

#+name: fig:hybrid_control_Ex
#+caption: Obtained position error $\bm{\epsilon}_\mathcal{X}$ ([[./figs/hybrid_control_Ex.png][png]], [[./figs/hybrid_control_Ex.pdf][pdf]])
[[file:figs/hybrid_control_Ex.png]]

** Conclusion

* Position Error computation
<<sec:compute_pose_error>>

Let's note:
- $\{W\}$ the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed).
  The center of the frame if $O_W$
- $\{M\}$ is the frame fixed to the measured elements.
  $O_M$ is the point where the pose of the element is measured
- $\{R\}$ is a virtual frame corresponding to the wanted pose of the element.
  $O_R$ is the origin of this frame where the we want to position the point $O_M$ of the element.
- $\{V\}$ is a frame which its axes are aligned with $\{W\}$ and its origin $O_V$ is coincident with the $O_M$

Reference Position with respect to fixed frame {W}: ${}^WT_R$
#+begin_src matlab
  Dxr = 0;
  Dyr = 0;
  Dzr = 0.1;
  Rxr = pi;
  Ryr = 0;
  Rzr = 0;
#+end_src

Measured Position with respect to fixed frame {W}: ${}^WT_M$
#+begin_src matlab
  Dxm = 0;
  Dym = 0;
  Dzm = 0;
  Rxm = pi;
  Rym = 0;
  Rzm = 0;
#+end_src

We measure the position and orientation (pose) of the element represented by the frame $\{M\}$ with respect to frame $\{W\}$.
Thus we can compute the Homogeneous transformation matrix ${}^WT_M$.
#+begin_src matlab
    %% Measured Pose
    WTm = zeros(4,4);

    WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0;
         sin(Rzm)  cos(Rzm) 0;
         0        0         1] * ...
        [cos(Rym)  0        sin(Rym);
         0        1        0;
         -sin(Rym)  0        cos(Rym)] * ...
        [1        0        0;
         0        cos(Rxm) -sin(Rxm);
         0        sin(Rxm)  cos(Rxm)];
    WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1];
#+end_src

We can also compute the Homogeneous transformation matrix ${}^WT_R$ corresponding to the transformation required to go from fixed frame $\{W\}$ to the wanted frame $\{R\}$.
#+begin_src matlab
    %% Reference Pose
    WTr = zeros(4,4);

    WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0;
         sin(Rzr)  cos(Rzr) 0;
         0        0         1] * ...
        [cos(Ryr)  0        sin(Ryr);
         0        1        0;
         -sin(Ryr)  0        cos(Ryr)] * ...
        [1        0        0;
         0        cos(Rxr) -sin(Rxr);
         0        sin(Rxr)  cos(Rxr)];
    WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1];
#+end_src

We can also compute ${}^WT_V$.
#+begin_src matlab
  WTv = eye(4);
  WTv(1:3, 4) = WTm(1:3, 4);
#+end_src

Now we want to express ${}^MT_R$ which corresponds to the transformation required to go to wanted position expressed in the frame of the measured element.
This homogeneous transformation can be computed from the previously computed matrices:
\[ {}^MT_R = ({{}^WT_M}^{-1}) {}^WT_R \]

#+begin_src matlab
    %% Wanted pose expressed in a frame corresponding to the actual measured pose
    MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
#+end_src

Now we want to express ${}^VT_R$:
\[ {}^VT_R = ({{}^WT_V}^{-1}) {}^WT_R \]
#+begin_src matlab
    %% Wanted pose expressed in a frame coincident with the actual position but with no rotation
    VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr;
#+end_src

#+begin_src matlab
    %% Extract Translations and Rotations from the Homogeneous Matrix
    T = MTr;
    Edx = T(1, 4);
    Edy = T(2, 4);
    Edz = T(3, 4);

    % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
    Ery = atan2( T(1, 3),          sqrt(T(1, 1)^2 + T(1, 2)^2));
    Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
    Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
#+end_src