nass-simscape/org/control_force.org

#+TITLE: Force Control applied on the Simscape Model
#+SETUPFILE: ./setup/org-setup-file.org

Cascade Control where the inner loop goal is to control the total wrench (forces and torques) applied to the upper part of the nano-hexapod.

The force will induce an acceleration of the upper part, thus we can consider the new input $\bm{r}_\mathcal{F}$ as an acceleration input.

Ideas:
- This can be used as *feedforward*: we now the wanted 6dof reference path for the sample, thus we can compute the required acceleration and thus the required force. To do so, we need information on the inertia of the sample.

#+begin_src latex :file control_cascade_force_F.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, left= of subF] (Kx) {$\bm{K}_\mathcal{X}$};
    \node[block, align=center, left= of Kx] (Ex) {Compute\\Pos. Error};

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

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

    \draw[->] (outputF) -- ++(1, 0);
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} node[below]{$\bm{\tau}_m$} |- (J.east);
    \draw[->] (J.west) -| (subF.north)node[above left]{$\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[->] (outputX) -- ++(1.6, 0) node[above left]{$\bm{\mathcal{X}}$};
    \draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} -- ++(0, -2) -| (Ex.south);

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

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

#+begin_src latex :file control_cascade_force_tau.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}_\tau$};
    \node[addb={+}{}{-}{}{}, left= of K] (subF) {};
    \node[block, left= of subF] (J) {$\bm{J}^{-T}$};
    \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[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};

    \draw[->] (outputF) -- ++(1, 0);
    \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} node[below]{$\bm{\tau}_m$} -- ++(0, 1) -| (subF.north);
    \draw[->] (subF.east) -- (K.west) node[above left]{$\bm{\epsilon}_\tau$};
    \draw[->] (K.east) -- (inputF) node[above left]{$\bm{\tau}$};

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

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

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