#+TITLE: Control of the Nano-Active-Stabilization-System
: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: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

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

#+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:
The system consist of the following inputs and outputs (Figure [[fig:control_architecture_plant]]):
- $\bm{\tau}$: Forces applied in each leg
- $\bm{\tau}_m$: Force sensor located in each leg
- $\bm{\mathcal{X}}$: Measurement of the payload position with respect to the granite
- $d\bm{\mathcal{L}}$: Measurement of the (small) relative motion of each leg

#+begin_src latex :file control_architecture_plant.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

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

    \draw[<-] (inputF) -- ++(-1, 0) node[above right]{$\bm{\tau}$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_plant
#+caption: Block diagram with the inputs and outputs of the system
#+RESULTS:
[[file:figs/control_architecture_plant.png]]

In order to position the Sample with respect to the granite, we must use the measurement $\bm{\mathcal{X}}$ in the control loop.
The wanted position of the sample with respect to the granite is represented by $\bm{r}_\mathcal{X}$.
From $\bm{r}_\mathcal{X}$ and $\bm{\mathcal{X}}$, we can compute the required small change of pose of the nano-hexapod's top platform expressed in the frame of the nano-hexapod's base as shown in Figure [[fig:control_architecture_pos_error]].

This can we considered as:
- the position error $\bm{\epsilon}_{\mathcal{X}_n}$ expressed in a frame attach to the base of the nano-hexapod
- the wanted (small) pose displacement $\bm{r}_{d\mathcal{X}_n}$ of the nano-hexapod mobile platform with respect to its base

#+begin_src latex :file control_architecture_pos_error.pdf
  \begin{tikzpicture}
    \node[block, align=center] (Ex) {Compute\\Pos. Error};
    \draw[<-] (Ex.west)  -- ++(-1, 0)node[above right]{$\bm{r}_{\mathcal{X}}$};
    \draw[<-] (Ex.south) -- ++(0, -1)node[above right]{$\bm{\mathcal{X}}$};
    \draw[->] (Ex.east)  -- ++(1,  0)node[above left ]{$\bm{r}_{\mathcal{X}_n}$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_pos_error
#+caption: Block diagram corresponding to the computation of the position error in the frame of the nano-hexapod
#+RESULTS:
[[file:figs/control_architecture_pos_error.png]]

In this document, we see how the different outputs of the system can be used to control of position $\bm{\mathcal{X}}$.

* Control Configuration - Introduction
In this section, we discuss the control configuration for the NASS.

From cite:skogestad07_multiv_feedb_contr:
#+begin_quote
We define the *control configuration* to be the restrictions imposed on the overall controller $K$ by decomposing it into a set of *local controllers* with predetermined links and with a possibly predetermined design sequence where subcontrollers are designed locally.

Some elements used to build up a specific control configuration are:
- *Cascade controllers*. The output from one controller is the input to another
- *Decentralized controllers*. The control system consists of independent feedback controllers which interconnect a subset of the output measurements with a subset of the manipulated inputs.
  These subsets should not be used by any other controller
- *Feedforward elements*. Link measured disturbances and manipulated inputs
- *Decoupling elements*. Link one set of manipulated inputs with another set of manipulated inputs.
  They are used to improve the performance of decentralized control systems.
#+end_quote

Decoupling elements will be used to convert quantities from the joint space to the task space and vice-versa.

Decentralized controllers will be largely used both for Tracking control (Section [[sec:tracking_control]]) and for Active Damping techniques (Section [[sec:active_damping]])

Combining both can be done in an HAC-LAC topology presented in Section [[sec:hac_lac]].

The use of decentralized controllers is proposed in Section [[sec:cascade_control]].

* Tracking Control in the Frame of the Nano-Hexapod - Basic Architectures
<<sec:tracking_control>>
** Introduction                                                      :ignore:
In this section, we suppose that we want to track some reference position $\bm{r}_{\mathcal{X}_n}$ corresponding to the pose of the nano-hexapod's mobile platform with respect to its fixed base.

To do so, we have to the use the leg's length measurement $d\bm{\mathcal{L}}$.

However, thanks to the forward and inverse kinematics, the controller can either be designed in the task space or in the joint space.

These to configuration are described in the next two sections.

** Control in the frame of the Legs
<<sec:tracking_control_leg_frame>>

From the wanted small change in pose of the nano-hexapod's mobile platform $\bm{r}_{d\mathcal{X}_n}$, we can use the Inverse Kinematics of the nano-hexapod to compute the corresponding small change of the leg length of the nano-hexapod $\bm{r}_{d\mathcal{L}}$.
Then, this is subtracted by the measurement of the leg relative displacement $d\bm{\mathcal{L}}$ to obtain to displacement error of each leg $\bm{\epsilon}_{d\mathcal{L}}$.
Finally, a diagonal (Decentralized) controller $\bm{K}_\mathcal{L}$ can be used.

#+begin_src latex :file control_architecture_leg_frame.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, left= of inputF] (K) {$\bm{K}_\mathcal{L}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
    \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputX) -- ++(1.0, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};
    \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
  \end{tikzpicture}
#+end_src

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

** Control in the Cartesian frame
<<sec:tracking_control_task_frame>>

From the relative displacement of each leg $d\bm{\mathcal{L}}$, the pose of the nano-hexapod's mobile platform $\bm{\mathcal{X}_n}$ is estimated.
It is then subtracted from reference pose of the nano-hexapod $\bm{r}_{\mathcal{X}_n}$ to obtain the pose error $\bm{\epsilon}_{\mathcal{X}_n}$.
A diagonal controller $\bm{K}_\mathcal{X}$ is used to generate forces and torques applied on the payload in a frame attached to the nano-hexapod's base.
These forces are then converted to forces applied in each of the nano-hexapod's actuators by the use of the Jacobian $\bm{J}^{-T}$.

#+begin_src latex :file control_architecture_cartesian_frame.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, align=center, below=0.4 of P] (Forkin) {Forward\\Kinematics};

    \node[block, left= of inputF] (J) {$\bm{J}^{-T}$};
    \node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} |- (Forkin.east);
    \draw[->] (Forkin.west) -| (subr.south) node[below right]{$\bm{\mathcal{X}_n}$};

    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}_n}$};
    \draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (J.east) -- (inputF.west) node[above left]{$\bm{\tau}$};
    \draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_cartesian_frame
#+caption: Control in the cartesian Frame (rotating frame attached to the nano-hexapod's base)
#+RESULTS:
[[file:figs/control_architecture_cartesian_frame.png]]

* Active Damping Architecture - Collocated Control ([[file:control_active_damping.org][link]])
<<sec:active_damping>>
** Introduction                                                      :ignore:
From cite:preumont18_vibrat_contr_activ_struc_fourt_edition:
#+begin_quote
Active damping is very effective in reducing the settling time of transient disturbances and the effect of steady state disturbances near the resonance frequencies of the system; however, away from the resonances, the active damping is completely ineffective and leaves the closed-loop response essentially unchanged.
Such low-gain controllers are often called Low Authority Controllers (LAC), because they modify the poles of the system only slightly.
#+end_quote

Two very well known active damping techniques are *Integral Force Feedback* and *Direct Velocity Feedback*.

These two active damping techniques are collocated control techniques.

The active damping techniques are studied in [[file:control_active_damping.org][this]] document.

** Integral Force Feedback
<<sec:active_damping_iff>>

In this active damping technique, the force sensors in each leg is used.

The controller $\bm{K}_\text{IFF}$ is a diagonal matrix, each of its diagonal element consists of:
- an pure integrator
- a gain $g$ that can be tuned to achieve a maximum damping

\begin{equation}
  \bm{K}_\text{IFF}(s) = \frac{g}{s} \bm{I}_{6}
\end{equation}

A lead-lag can also be used instead of a pure integrator.

#+begin_src latex :file control_architecture_iff.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
    \draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_iff
#+caption: Integral Force Feedback
#+RESULTS:
[[file:figs/control_architecture_iff.png]]

** Direct Relative Velocity Feedback
<<sec:active_damping_dvf>>

The controller $\bm{K}_\text{DVF}$ is a diagonal matrix.
Each diagonal element consists of:
- a derivative action up to some frequency $\omega_0$
- a gain $g$ that can be tuned to achieve a maximum damping

\begin{equation}
  \bm{K}_\text{DVF}(s) = \frac{g s}{\omega_0 + s} \bm{I}_{6}
\end{equation}

#+begin_src latex :file control_architecture_dvf.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, below=0.4 of P] (Kdvf) {$\bm{K}_\text{DVF}$};
    \node[addb={+}{}{}{}{-}, left= of inputF] (addF) {};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[above left]{$\bm{\tau}_m$};
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} |- (Kdvf.east);
    \draw[->] (Kdvf.west) -| (addF.south);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
    \draw[<-] (addF.west) -- ++(-1, 0) node[above right]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_dvf
#+caption: Direct Velocity Feedback
#+RESULTS:
[[file:figs/control_architecture_dvf.png]]

* HAC-LAC Architectures ([[file:control_hac_lac.org][link]])
<<sec:hac_lac>>
** Introduction                                                      :ignore:
Here we can combine Active Damping Techniques (Low authority control) with a tracking controller (high authority control).
Usually, the low authority controller is designed first, and the high authority controller is designed based on the damped plant.

From cite:preumont18_vibrat_contr_activ_struc_fourt_edition:
#+begin_quote
The HAC/LAC approach consist of combining the two approached in a dual-loop control as shown in Figure [[fig:control_architecture_hac_lac]].
The inner loop uses a set of collocated actuator/sensor pairs for decentralized active damping with guaranteed stability ; the outer loop consists of a non-collocated HAC based on a model of the actively damped structure.
This approach has the following advantages:
- The active damping extends outside the bandwidth of the HAC and reduces the settling time of the modes which are outsite the bandwidth
- The active damping makes it easier to gain-stabilize the modes outside the bandwidth of the output loop (improved gain margin)
- The larger damping of the modes within the controller bandwidth makes them more robust to the parmetric uncertainty (improved phase margin)
#+end_quote

#+begin_src latex :file control_architecture_hac_lac.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputH) at ($(P.south west)!0.3!(P.north west)$);
    \coordinate[] (inputL) at ($(P.south west)!0.7!(P.north west)$);
    \coordinate[] (outputH) at ($(P.south east)!0.3!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.7!(P.north east)$);

    \node[block, above=0.6 of P] (Klac) {$\bm{K}_\text{LAC}$};
    \node[block, left= of inputH] (Khac) {$\bm{K}_\text{HAC}$};
    \node[addb={+}{}{}{}{-}, left= of Khac] (subr) {};

    % Connections and labels
    \draw[->] (outputL) -- ++(1, 0);
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} node[below]{$x^\prime$} |- (Klac.east);
    \draw[->] (Klac.west) -| ($(inputL) + (-0.6, 0)$) -- (inputL) node[above left]{$u^\prime$};

    \draw[<-] (subr.west)node[above left]{$r$} -- ++(-1, 0);
    \draw[->] (outputH) -- ++(1, 0);
    \draw[->] ($(outputH) + (0.6, 0)$)node[branch]{} node[above]{$x$} -- ++(0, -1.5) -| (subr.south);
    \draw[->] (subr.east) -- (Khac.west) node[above left]{$\epsilon$};
    \draw[->] (Khac.east) -- (inputH) node[above left]{$u$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_hac_lac
#+caption: HAC-LAC Control Architecture
#+RESULTS:
[[file:figs/control_architecture_hac_lac.png]]

If there is only one input to the system, the HAC-LAC topology can be represented as depicted in Figure [[fig:control_architecture_hac_lac_one_input]].
Usually, the Low Authority Controller is first design, and then the High Authority Controller is designed based on the damped plant.

#+begin_src latex :file control_architecture_hac_lac_one_input.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (input)   at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputH) at ($(P.south east)!0.2!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);

    \node[block, above=0.6 of P] (Klac) {$\bm{K}_\text{LAC}$};
    \node[addb, left= of input] (addF) {};
    \node[block, left= of addF] (Khac) {$\bm{K}_\text{HAC}$};
    \node[addb={+}{}{}{}{-}, left= of Khac] (subr) {};

    % Connections and labels
    \draw[->] (outputL) -- ++(0.6, 0) coordinate(eastlac) |- (Klac.east);
    \node[above right] at (outputL){$x^\prime$};
    \draw[->] (Klac.west) -| (addF.north);
    \draw[->] (addF.east) -- (input) node[above left]{$u^\prime$};

    \draw[<-] (subr.west)node[above left]{$r$} -- ++(-1, 0);
    \draw[->] (outputH) -- ++(1.8, 0);
    \draw[->] ($(outputH) + (1.2, 0)$)node[branch]{} node[above]{$x$} -- ++(0, -1.5) -| (subr.south);
    \draw[->] (subr.east) -- (Khac.west) node[above left]{$\epsilon$};
    \draw[->] (Khac.east) -- (addF.west) node[above left=0 and 0.2]{$u$};

    \begin{scope}[on background layer]
      \node[fit={(Klac.north-|eastlac) (addF.west|-P.south)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Pi) {};
      \node[anchor={north west}] at (Pi.north west){$\text{Damped Plant}$};
    \end{scope}
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_hac_lac_one_input
#+caption: HAC-LAC Architecture with a system having only one input
#+RESULTS:
[[file:figs/control_architecture_hac_lac_one_input.png]]

** HAC-LAC using IFF and Tracking control in the frame of the Legs
#+begin_src latex :file control_architecture_hac_iff_L.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
    \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};

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

    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};
    \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_hac_iff_L
#+caption: IFF + Control in the frame of the legs
#+RESULTS:
[[file:figs/control_architecture_hac_iff_L.png]]

** HAC-LAC using IFF and Tracking control in the Cartesian frame
#+begin_src latex :file control_architecture_hac_iff_X.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (J) {$\bm{J}^{-T}$};
    \node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};

    \node[block, align=center, below=0.4 of P] (Forkin) {Forward\\Kinematics};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} |- (Forkin.east);
    \draw[->] (Forkin.west) -| (subr.south) node[below right]{$\bm{\mathcal{X}_n}$};

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

    \draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}_n}$};
    \draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (J.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_hac_iff_X
#+caption: IFF + Control in the cartesian frame
#+RESULTS:
[[file:figs/control_architecture_hac_iff_X.png]]

** HAC-LAC using IFF - the HAC controller is positioning the sample w.r.t. the granite in the task space
#+begin_src latex :file control_architecture_hac_iff_pos_X.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (J) {$\bm{J}^{-T}$};
    \node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
    \node[block, align=center, left= of K] (Ex) {Compute\\Pos. Error};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

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

    \draw[->] (outputX) -- ++(1.6, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} -- ++(0, -2) -| (Ex.south);

    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (Ex.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}_n}$};
    \draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (J.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+RESULTS:
[[file:figs/control_architecture_hac_iff_pos_X.png]]

** HAC-LAC using IFF - the HAC controller is positioning the sample w.r.t. the granite in the space of the legs
#+begin_src latex :file control_architecture_hac_iff_pos_L.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
    \node[block, left= of K] (J) {$\bm{J}$};
    \node[block, align=center, left= of J] (Ex) {Compute\\Pos. Error};

    % Connections and labels
    \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

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

    \draw[->] (outputX) -- ++(1.6, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} -- ++(0, -2) -| (Ex.south);

    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}_n}$};
    \draw[->] (J.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
  \end{tikzpicture}
#+end_src

#+RESULTS:
[[file:figs/control_architecture_hac_iff_pos_L.png]]

* Cascade Architectures ([[file:control_cascade.org][link]])
<<sec:cascade_control>>
** Introduction                                                      :ignore:
The principle of Cascade control is shown in Figure [[fig:control_architecture_cascade_control]] and explained as follow:
#+begin_quote
To follow *two objectives* with different properties in one control system, usually a *hierarchy* of two feedback loops is used in practice.
This kind of control topology is called *cascade control*, which is used when there are *several measurements and one prime control variable*.
Cascade control is implemented by *nesting* the control loops, as shown in Figure [[fig:control_architecture_cascade_control]].
The output control loop is called the *primary loop*, while the inner loop is called the secondary loop and is used to fulfill a secondary objective in the closed-loop system. -- cite:taghirad13_paral
#+end_quote

#+begin_src latex :file control_architecture_cascade_control.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (input)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputi) at ($(P.south east)!0.3!(P.north east)$);
    \coordinate[] (outputo) at ($(P.south east)!0.7!(P.north east)$);

    \node[block, left= of input] (Ki) {$K_s$};
    \node[addb={+}{}{}{}{-}, left= of Ki] (subi) {};
    \node[block, left= of subi] (Ko) {$K_p$};
    \node[addb={+}{}{}{}{-}, left= of Ko] (subo) {};

    \node[above=0 of Ko, align=center] {Primary\\Controller};
    \node[above=0 of Ki, align=center] {Secondary\\Controller};

    % Connections and labels
    \draw[->] (outputi) -- ++(1.0, 0);
    \draw[->] ($(outputi) + (0.6, 0)$)node[branch](easti){} node[above]{$y_s$} -- ++(0, -1.3)coordinate(southi) -| (subi.south);

    \draw[->] (outputo) -- ++(1.8, 0);
    \draw[->] ($(outputo) + (1.4, 0)$)node[branch](easto){} node[above]{$y_p$} -- ++(0, -3.1)coordinate(southo) -| (subo.south);

    \draw[<-] (subo.west) node[above left=0 and 0.2]{$r_p$} -- ++(-1, 0);
    \draw[->] (subo.east) -- (Ko.west) node[above left]{$\epsilon_p$};
    \draw[->] (Ko.east) -- (subi.west) node[above left=0 and 0.2]{$r_s$};
    \draw[->] (subi.east) -- (Ki.west) node[above left]{$\epsilon_s$};
    \draw[->] (Ki.east) -- (input)     node[above left]{$u$};

    \begin{scope}[on background layer]
      \node[fit={(P.north-|easti) (subi.west|-southi)}, inner sep=8pt, opacity=0] (Plin) {};
      \node[fit={(Plin.north-|easto) (subo.west|-southo)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Po) {};
      \node[fit={(P.north-|easti) (subi.west|-southi)}, fill=black!40!white, draw, dashed, inner sep=8pt] (Pi) {};
      \node[anchor={north west}] at (Po.north west){$\text{Outer Loop}$};
      \node[anchor={north west}] at (Pi.north west){$\text{Inner Loop}$};
    \end{scope}
  \end{tikzpicture}
#+end_src

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

This control topology seems adapted for the NASS, as indeed we have more inputs than outputs

In the NASS's case:
- The primary objective is to position the sample with respect to the granite, thus the outer loop (and primary controller) should corresponds to a motion control loop

The inner loop can be composed of the system controlled with the HAC-LAC topology.

** Cascade Control with HAC-LAC Inner Loop and Primary Controller in the task space
#+begin_src latex :file control_architecture_cascade_L.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (K) {$\bm{K}_{\mathcal{L}}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
    \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
    \node[block, left= of J] (Kx) {$\bm{K}_\mathcal{X}$};

    \node[block, align=center, left= of Kx] (Ex) {Compute\\Pos. Error};

    % Connections and labels
    \draw[->] (outputF) -- ++(1.0, 0);
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch](taum){} node[below]{$\bm{\tau}_m$} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1.8, 0);
    \draw[->] ($(outputL) + (1.4, 0)$)node[branch]{} node[above]{$d\bm{\mathcal{L}}$} -- ++(0, -1.2) node(Plinse){} -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left=0 and 8pt]{$\bm{\tau}^\prime$};

    \draw[->] (outputX) -- ++(2.6, 0);
    \draw[->] ($(outputX) + (2.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -3.0) -| (Ex.south);

    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (Ex.east) -- (Kx.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
    \draw[->] (Kx.east) -- (J.west) node[above left=0 and 6pt]{$\bm{r}_{\mathcal{X}_n}$};
    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};

    \begin{scope}[on background layer]
      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, opacity=0, inner sep=10pt] (Pdamped) {};

      \node[fit={(Pdamped.north-|J.west) (Plinse)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Plin) {};
      \node[anchor={north west}] at (Plin.north west){$P_\text{lin}$};

      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, fill=black!40!white, draw, dashed, inner sep=10pt] (Pdamped) {};
      \node[anchor={north west}] at (Pdamped.north west){$P_\text{damped}$};
    \end{scope}

  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_cascade_L
#+caption: Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs
#+RESULTS:
[[file:figs/control_architecture_cascade_L.png]]

** Cascade Control with HAC-LAC Inner Loop and Primary Controller in the joint space
#+begin_src latex :file control_architecture_cascade_X.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF)  at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
    \node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
    \node[block, left= of addF] (K) {$\bm{K}_{\mathcal{L}}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subr) {};
    \node[block, left= of subr] (Kl) {$\bm{K}_\mathcal{L}$};
    \node[block, align=center, left= of Kl] (J) {Inverse\\Kinematics};

    \node[block, align=center, left= of J] (Ex) {Compute\\Pos. Error};

    % Connections and labels
    \draw[->] (outputF) -- ++(1.0, 0);
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch](taum){} node[below]{$\bm{\tau}_m$} |- (Kiff.east);
    \draw[->] (Kiff.west) -| (addF.north);
    \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};

    \draw[->] (outputL) -- ++(1.8, 0);
    \draw[->] ($(outputL) + (1.4, 0)$)node[branch]{} node[above]{$d\bm{\mathcal{L}}$} -- ++(0, -1.2) node(Plinse){} -| (subr.south);
    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
    \draw[->] (K.east) -- (addF.west) node[above left=0 and 8pt]{$\bm{\tau}^\prime$};

    \draw[->] (outputX) -- ++(2.6, 0);
    \draw[->] ($(outputX) + (2.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -3.0) -| (Ex.south);

    \draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
    \draw[->] (Ex.east) -- (J.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
    \draw[->] (J.east) -- (Kl.west) node[above left]{$\bm{r}_{\mathcal{L}}$};
    \draw[->] (Kl.east) -- (subr.west) node[above left=0 and 6pt]{$\bm{r}_{d\mathcal{L}}$};

    \begin{scope}[on background layer]
      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, opacity=0, inner sep=10pt] (Pdamped) {};

      \node[fit={(Pdamped.north-|subr.west) (Plinse)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Plin) {};
      \node[anchor={north west}] at (Plin.north west){$P_\text{lin}$};

      \node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, fill=black!40!white, draw, dashed, inner sep=10pt] (Pdamped) {};
      \node[anchor={north west}] at (Pdamped.north west){$P_\text{damped}$};
    \end{scope}

  \end{tikzpicture}
#+end_src

#+name: fig:control_architecture_cascade_X
#+caption: Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the Cartesian Frame
#+RESULTS:
[[file:figs/control_architecture_cascade_X.png]]

* Sensor Fusion Architectures                                       :noexport:
<<sec:sensor_fusion>>

* $\mathcal{H}_\infty$ Architectures                                :noexport:
<<sec:h_infinity>>

* Force Control ([[file:control_force.org][link]])
Signals:
- $\bm{r}_\mathcal{F}$ is the wanted total force/torque to be applied to the payload
- $\bm{\epsilon}_\mathcal{F}$ is the force/torque errors that should be applied to the payload
- $\bm{\tau}$ is the force applied in each actuator

#+begin_src latex :file control_architecture_force.pdf
  \begin{tikzpicture}
    % Blocs
    \node[block={3.0cm}{3.0cm}] (P) {Plant};
    \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
    \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
    \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
    \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);

    \node[block, left= of inputF] (Jt) {$\bm{J}^{-T}$};
    \node[block, left= of Jt] (K) {$\bm{K}_\mathcal{F}$};
    \node[addb={+}{}{}{}{-}, left= of K] (subF) {};

    \node[block, below=0.4 of P] (J) {$\bm{J}^{T}$};

    % Connections and labels
    \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
    \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};

    \draw[->] (outputF) -- ++(1.8, 0);
    \draw[->] ($(outputF) + (1.4, 0)$)node[branch]{} node[above]{$\bm{\tau}_m$} |- (J.east);
    \draw[->] (J.west) -| (subF.south)node[below right]{$\bm{\mathcal{F}}_m$};
    \draw[->] (subF.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{F}}$};
    \draw[->] (K.east) -- (Jt.west) node[above left]{$\bm{\mathcal{F}}$};
    \draw[->] (Jt.east) -- (inputF) node[above left]{$\bm{\tau}$};
    \draw[<-] (subF.west) -- ++(-1, 0) node[above right]{$\bm{r}_{\mathcal{F}}$};
  \end{tikzpicture}
#+end_src

#+RESULTS:
[[file:figs/control_architecture_force.png]]

* Bibliography                                                        :ignore:
bibliographystyle:unsrt
bibliography:ref.bib