#+TITLE: Finite Element Model with Simscape
: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+ :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:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
#+PROPERTY: header-args:latex+ :results raw replace :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
:END:

* Amplified Piezoelectric Actuator - 3D elements
** Introduction                                                      :ignore:
The idea here is to:
- export a FEM of an amplified piezoelectric actuator from Ansys to Matlab
- import it into a Simscape model
- compare the obtained dynamics
- add 10kg mass on top of the actuator and identify the dynamics
- compare with results from Ansys where 10kg are directly added to the FEM

** Matlab Init                                             :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab
  addpath('./src/');
  addpath('./data/piezo_amplified_3d/');
#+end_src

#+begin_src matlab :exports none
  open('piezo_amplified_3d');
#+end_src

** Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
We first extract the stiffness and mass matrices.
#+begin_src matlab
  K = extractMatrix('piezo_amplified_3d_K.txt');
  M = extractMatrix('piezo_amplified_3d_M.txt');
#+end_src

Then, we extract the coordinates of the interface nodes.
#+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('piezo_amplified_3d.txt');
#+end_src

#+begin_src matlab
  save('./mat/piezo_amplified_3d.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
#+end_src

** Output parameters
#+begin_src matlab
  load('./mat/piezo_amplified_3d.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
#+end_src

#+RESULTS:
| Total number of Nodes     | 168959 |
| Number of interface Nodes |     13 |
| Number of Modes           |     30 |
| Size of M and K matrices  |    108 |

#+name: fig:amplified_piezo_interface_nodes
#+caption: Interface Nodes for the Amplified Piezo Actuator
[[file:figs/amplified_piezo_interface_nodes.png]]

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
#+end_src

#+caption: Coordinates of the interface nodes
#+RESULTS:
| Node i | Node Number |  x [m] | y [m] | z [m] |
|--------+-------------+--------+-------+-------|
|    1.0 |    168947.0 |    0.0 |  0.03 |   0.0 |
|    2.0 |    168949.0 |    0.0 | -0.03 |   0.0 |
|    3.0 |    168950.0 | -0.035 |   0.0 |   0.0 |
|    4.0 |    168951.0 | -0.028 |   0.0 |   0.0 |
|    5.0 |    168952.0 | -0.021 |   0.0 |   0.0 |
|    6.0 |    168953.0 | -0.014 |   0.0 |   0.0 |
|    7.0 |    168954.0 | -0.007 |   0.0 |   0.0 |
|    8.0 |    168955.0 |    0.0 |   0.0 |   0.0 |
|    9.0 |    168956.0 |  0.007 |   0.0 |   0.0 |
|   10.0 |    168957.0 |  0.014 |   0.0 |   0.0 |
|   11.0 |    168958.0 |  0.021 |   0.0 |   0.0 |
|   12.0 |    168959.0 |  0.035 |   0.0 |   0.0 |
|   13.0 |    168960.0 |  0.028 |   0.0 |   0.0 |

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(K(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Stiffness matrix
#+RESULTS:
| 300000000.0 |    -30000.0 |     8000.0 |    -200.0 |   -30.0 | -60000.0 |  20000000.0 |     -4000.0 |      500.0 |        8 |
|    -30000.0 | 100000000.0 |      400.0 |      30.0 |   200.0 |       -1 |      4000.0 |  -8000000.0 |      800.0 |        7 |
|      8000.0 |       400.0 | 50000000.0 | -800000.0 |  -300.0 |    -40.0 |       300.0 |       100.0 |  5000000.0 |  40000.0 |
|      -200.0 |        30.0 |  -800000.0 |   20000.0 |       5 |        1 |       -10.0 |          -2 |   -40000.0 |   -300.0 |
|       -30.0 |       200.0 |     -300.0 |         5 | 40000.0 |      0.3 |          -4 |       -10.0 |       40.0 |      0.4 |
|    -60000.0 |          -1 |      -40.0 |         1 |     0.3 |   3000.0 |      7000.0 |         0.8 |         -1 |   0.0003 |
|  20000000.0 |      4000.0 |      300.0 |     -10.0 |      -4 |   7000.0 | 300000000.0 |     20000.0 |     3000.0 |     80.0 |
|     -4000.0 |  -8000000.0 |      100.0 |        -2 |   -10.0 |      0.8 |     20000.0 | 100000000.0 |    -4000.0 |   -100.0 |
|       500.0 |       800.0 |  5000000.0 |  -40000.0 |    40.0 |       -1 |      3000.0 |     -4000.0 | 50000000.0 | 800000.0 |
|           8 |           7 |    40000.0 |    -300.0 |     0.4 |   0.0003 |        80.0 |      -100.0 |   800000.0 |  20000.0 |


#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(M(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Mass matrix
#+RESULTS:
|   0.03 |  2e-06 |  -2e-07 |  1e-08 |  2e-08 | 0.0002 | -0.001 |  2e-07 |  -8e-08 | -9e-10 |
|  2e-06 |   0.02 |  -5e-07 |  7e-09 |  3e-08 |  2e-08 | -3e-07 | 0.0003 |  -1e-08 |  1e-10 |
| -2e-07 | -5e-07 |    0.02 | -9e-05 |  4e-09 | -1e-08 |  2e-07 | -2e-08 | -0.0006 | -5e-06 |
|  1e-08 |  7e-09 |  -9e-05 |  1e-06 |  6e-11 |  4e-10 | -1e-09 |  3e-11 |   5e-06 |  3e-08 |
|  2e-08 |  3e-08 |   4e-09 |  6e-11 |  1e-06 |  2e-10 | -2e-09 |  2e-10 |  -7e-09 | -4e-11 |
| 0.0002 |  2e-08 |  -1e-08 |  4e-10 |  2e-10 |  2e-06 | -2e-06 | -1e-09 |  -7e-10 | -9e-12 |
| -0.001 | -3e-07 |   2e-07 | -1e-09 | -2e-09 | -2e-06 |   0.03 | -2e-06 |  -1e-07 | -5e-09 |
|  2e-07 | 0.0003 |  -2e-08 |  3e-11 |  2e-10 | -1e-09 | -2e-06 |   0.02 |  -8e-07 | -1e-08 |
| -8e-08 | -1e-08 | -0.0006 |  5e-06 | -7e-09 | -7e-10 | -1e-07 | -8e-07 |    0.02 |  9e-05 |
| -9e-10 |  1e-10 |  -5e-06 |  3e-08 | -4e-11 | -9e-12 | -5e-09 | -1e-08 |   9e-05 |  1e-06 |

Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.


** Piezoelectric parameters
Parameters for the APA95ML:

#+begin_src matlab
  d33 = 3e-10; % Strain constant [m/V]
  n = 80; % Number of layers per stack
  eT = 1.6e-7; % Permittivity under constant stress [F/m]
  sD = 2e-11; % Elastic compliance under constant electric displacement [m2/N]
  ka = 235e6; % Stack stiffness [N/m]
  C = 5e-6; % Stack capactiance [F]
#+end_src

#+begin_src matlab
  na = 2; % Number of stacks used as actuator
  ns = 1; % Number of stacks used as force sensor
#+end_src

The ratio of the developed force to applied voltage is $d_{33} n k_a$ in [N/V].
We denote this constant by $g_a$ and:
\[ F_a = g_a V_a, \quad g_a = d_{33} n k_a \]
#+begin_src matlab :results replace value
  d33*(na*n)*(ka/(na + ns)) % [N/V]
#+end_src

#+RESULTS:
: 3.76

From cite:fleming14_desig_model_contr_nanop_system (page 123), the relation between relative displacement and generated voltage is:
\[ V_s = \frac{d_{33}}{\epsilon^T s^D n} \Delta h \]
where:
- $V_s$: measured voltage [V]
- $d_{33}$: strain constant [m/V]
- $\epsilon^T$: permittivity under constant stress [F/m]
- $s^D$: elastic compliance under constant electric displacement [m^2/N]
- $n$: number of layers
- $\Delta h$: relative displacement [m]
#+begin_src matlab :results replace value
  1e-6*d33/(eT*sD*ns*n) % [V/um]
#+end_src

#+RESULTS:
: 1.1719

** Identification of the Dynamics
The flexible element is imported using the =Reduced Order Flexible Solid= simscape block.

To model the actuator, an =Internal Force= block is added between the nodes 3 and 12.
A =Relative Motion Sensor= block is added between the nodes 1 and 2 to measure the displacement and the amplified piezo.

One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.

We first set the mass to be zero.
#+begin_src matlab
  m = 0.01;
#+end_src

The dynamics is identified from the applied force to the measured relative displacement.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  Gh = -linearize(mdl, io);
#+end_src

Then, we add 10Kg of mass:
#+begin_src matlab
  m = 5;
#+end_src

And the dynamics is identified.

The two identified dynamics are compared in Figure [[fig:dynamics_act_disp_comp_mass]].
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  Ghm = -linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 4, 5000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gh, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/dynamics_act_disp_comp_mass.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:dynamics_act_disp_comp_mass
#+caption: Dynamics from $F$ to $d$ without a payload and with a 10kg payload
#+RESULTS:
[[file:figs/dynamics_act_disp_comp_mass.png]]

** Comparison with Ansys
Let's import the results from an Harmonic response analysis in Ansys.
#+begin_src matlab
  Gresp0 = readtable('FEA_HarmResponse_00kg.txt');
  Gresp10 = readtable('FEA_HarmResponse_10kg.txt');
#+end_src

The obtained dynamics from the Simscape model and from the Ansys analysis are compare in Figure [[fig:dynamics_force_disp_comp_anasys]].

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',1)
  plot(Gresp0{:, 2}, 1e-3*Gresp0{:, 3}, '--');

  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',2)
  plot(Gresp0{:, 2}, 1e-3*Gresp10{:, 3}, '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gh, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$, Simscape');
  set(gca,'ColorOrderIndex',1)
  plot(Gresp0{:, 2}, 180/pi*unwrap(pi/180*Gresp0{:, 4}), '--', ...
       'DisplayName', '$m = 0kg$, Ansys');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$, Simscape');
  set(gca,'ColorOrderIndex',2)
  plot(Gresp0{:, 2}, 180/pi*unwrap(pi/180*Gresp10{:, 4}), '--', ...
       'DisplayName', '$m = 10kg$, Ansys');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  legend('location', 'southwest');

  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/dynamics_force_disp_comp_anasys.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:dynamics_force_disp_comp_anasys
#+caption: Comparison of the obtained dynamics using Simscape with the harmonic response analysis using Ansys
#+RESULTS:
[[file:figs/dynamics_force_disp_comp_anasys.png]]

** Force Sensor
The dynamics is identified from internal forces applied between nodes 3 and 11 to the relative displacement of nodes 11 and 13.

The obtained dynamics is shown in Figure [[fig:dynamics_force_force_sensor_comp_mass]].

#+begin_src matlab
  m = 0;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1;

  Gf = linearize(mdl, io);
#+end_src

#+begin_src matlab
  m = 10;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fs'], 1, 'openoutput'); io_i = io_i + 1;

  Gfm = linearize(mdl, io);
#+end_src

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gf, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/dynamics_force_force_sensor_comp_mass.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:dynamics_force_force_sensor_comp_mass
#+caption: Dynamics from $F$ to $F_m$ for $m=0$ and $m = 10kg$
#+RESULTS:
[[file:figs/dynamics_force_force_sensor_comp_mass.png]]

** Distributed Actuator
#+begin_src matlab
  m = 0;
#+end_src

The dynamics is identified from the applied force to the measured relative displacement.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d_distri';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  Gd = linearize(mdl, io);
#+end_src

Then, we add 10Kg of mass:
#+begin_src matlab
  m = 10;
#+end_src

And the dynamics is identified.
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d_distri';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  Gdm = linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(1, 5, 5000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Gd, freqs, 'Hz'))), '--');
  plot(freqs, abs(squeeze(freqresp(Gdm, freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gh, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gd, freqs, 'Hz')))), '--', ...
       'DisplayName', '$m = 0kg$, distri');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdm, freqs, 'Hz')))), '--', ...
       'DisplayName', '$m = 10kg$, distri');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

** Distributed Actuator and Force Sensor
#+begin_src matlab
  m = 0;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1;

  Gfd = linearize(mdl, io);
#+end_src

#+begin_src matlab
  m = 10;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1;

  Gfdm = linearize(mdl, io);
#+end_src

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gf, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))), '-');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Gfd, freqs, 'Hz'))), '--');
  plot(freqs, abs(squeeze(freqresp(Gfdm, freqs, 'Hz'))), '--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfd, freqs, 'Hz')))), '--', ...
       'DisplayName', '$m = 0kg$, distri');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfdm, freqs, 'Hz')))), '--', ...
       'DisplayName', '$m = 10kg$, distri');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

** Dynamics from input voltage to displacement
#+begin_src matlab
  m = 5;
#+end_src

And the dynamics is identified.

The two identified dynamics are compared in Figure [[fig:dynamics_act_disp_comp_mass]].
#+begin_src matlab
  %% Name of the Simulink File
  mdl = 'piezo_amplified_3d';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/V'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  G = -linearize(mdl, io);
#+end_src

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  % xlim([10, 5e3]);
#+end_src

#+begin_src matlab
  save('../test-bench-apa/mat/fem_model_5kg.mat', 'G')
#+end_src

** Dynamics from input voltage to output voltage
#+begin_src matlab
  m = 5;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;

  G = -linearize(mdl, io);
#+end_src

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))));
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([10, 5e3]);
#+end_src

* APA300ML
** Introduction                                                      :ignore:

#+name: fig:apa300ml_ansys
#+caption: Ansys FEM of the APA300ML
[[file:figs/apa300ml_ansys.jpg]]

** Matlab Init                                             :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab
  addpath('./src/');
  addpath('./data/APA300ML/');
#+end_src

#+begin_src matlab :exports none
  open('APA300ML_test_bench');
#+end_src

** Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
We first extract the stiffness and mass matrices.
#+begin_src matlab
  K = extractMatrix('mat_K-48modes-7MDoF.matrix');
  M = extractMatrix('mat_M-48modes-7MDoF.matrix');
#+end_src

#+begin_src matlab
  K = extractMatrix('mat_K-80modes-7MDoF.matrix');
  M = extractMatrix('mat_M-80modes-7MDoF.matrix');
#+end_src

Then, we extract the coordinates of the interface nodes.
#+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('Nodes_MDoF_NLIST_MLIST.txt');
#+end_src

#+begin_src matlab
  save('./mat/APA300ML.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
#+end_src

** Output parameters
#+begin_src matlab
  load('./mat/APA300ML.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
#+end_src

#+RESULTS:
| Total number of Nodes     |  7 |
| Number of interface Nodes |  7 |
| Number of Modes           |  6 |
| Size of M and K matrices  | 48 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
#+end_src

#+caption: Coordinates of the interface nodes
#+RESULTS:
| Node i | Node Number |   x [m] |  y [m] | z [m] |
|--------+-------------+---------+--------+-------|
|    1.0 |     53917.0 |     0.0 | -0.015 |   0.0 |
|    2.0 |     53918.0 |     0.0 |  0.015 |   0.0 |
|    3.0 |     53919.0 | -0.0325 |    0.0 |   0.0 |
|    4.0 |     53920.0 | -0.0125 |    0.0 |   0.0 |
|    5.0 |     53921.0 | -0.0075 |    0.0 |   0.0 |
|    6.0 |     53922.0 |  0.0125 |    0.0 |   0.0 |
|    7.0 |     53923.0 |  0.0325 |    0.0 |   0.0 |

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(K(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Stiffness matrix
#+RESULTS:
| 200000000.0 |   30000.0 |    50000.0 |    200.0 |  -100.0 | -300000.0 |  10000000.0 |   -6000.0 |    20000.0 |     -60.0 |
|     30000.0 | 7000000.0 |    10000.0 |     30.0 |   -30.0 |     -70.0 |      7000.0 | -500000.0 |     3000.0 |     -10.0 |
|     50000.0 |   10000.0 | 30000000.0 | 200000.0 |  -200.0 |    -100.0 |     20000.0 |   -2000.0 |  2000000.0 |   -9000.0 |
|       200.0 |      30.0 |   200000.0 |   1000.0 |    -0.8 |      -0.4 |        50.0 |        -6 |     9000.0 |     -30.0 |
|      -100.0 |     -30.0 |     -200.0 |     -0.8 | 10000.0 |       0.2 |       -40.0 |      10.0 |       20.0 |     -0.05 |
|   -300000.0 |     -70.0 |     -100.0 |     -0.4 |     0.2 |     900.0 |    -30000.0 |      10.0 |      -40.0 |       0.1 |
|  10000000.0 |    7000.0 |    20000.0 |     50.0 |   -40.0 |  -30000.0 | 200000000.0 |  -50000.0 |    30000.0 |     -50.0 |
|     -6000.0 | -500000.0 |    -2000.0 |       -6 |    10.0 |      10.0 |    -50000.0 | 7000000.0 |    -4000.0 |         8 |
|     20000.0 |    3000.0 |  2000000.0 |   9000.0 |    20.0 |     -40.0 |     30000.0 |   -4000.0 | 30000000.0 | -200000.0 |
|       -60.0 |     -10.0 |    -9000.0 |    -30.0 |   -0.05 |       0.1 |       -50.0 |         8 |  -200000.0 |    1000.0 |


#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(M(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Mass matrix
#+RESULTS:
|    0.01 |  7e-06 |  -5e-06 | -6e-08 |  3e-09 | -5e-05 | -0.0005 | -2e-07 |  -3e-06 |  1e-08 |
|   7e-06 |  0.009 |   4e-07 |  6e-09 | -4e-09 | -3e-08 |  -2e-07 |  6e-05 |   5e-07 | -1e-09 |
|  -5e-06 |  4e-07 |    0.01 |  2e-05 |  2e-08 |  3e-08 |  -2e-06 | -1e-07 | -0.0002 |  9e-07 |
|  -6e-08 |  6e-09 |   2e-05 |  3e-07 |  1e-10 |  3e-10 |  -7e-09 |  2e-10 |  -9e-07 |  3e-09 |
|   3e-09 | -4e-09 |   2e-08 |  1e-10 |  1e-07 | -3e-12 |   6e-09 | -2e-10 |  -3e-09 |  9e-12 |
|  -5e-05 | -3e-08 |   3e-08 |  3e-10 | -3e-12 |  6e-07 |   1e-06 | -3e-09 |   2e-08 | -7e-11 |
| -0.0005 | -2e-07 |  -2e-06 | -7e-09 |  6e-09 |  1e-06 |    0.01 | -8e-06 |  -2e-06 |  9e-09 |
|  -2e-07 |  6e-05 |  -1e-07 |  2e-10 | -2e-10 | -3e-09 |  -8e-06 |  0.009 |   1e-07 |  2e-09 |
|  -3e-06 |  5e-07 | -0.0002 | -9e-07 | -3e-09 |  2e-08 |  -2e-06 |  1e-07 |    0.01 | -2e-05 |
|   1e-08 | -1e-09 |   9e-07 |  3e-09 |  9e-12 | -7e-11 |   9e-09 |  2e-09 |  -2e-05 |  3e-07 |

Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.

** Piezoelectric parameters
Parameters for the APA300ML:

#+begin_src matlab
  d33 = 3e-10; % Strain constant [m/V]
  n = 80; % Number of layers per stack
  eT = 1.6e-8; % Permittivity under constant stress [F/m]
  sD = 2e-11; % Elastic compliance under constant electric displacement [m2/N]
  ka = 235e6; % Stack stiffness [N/m]
  C = 5e-6; % Stack capactiance [F]
#+end_src

#+begin_src matlab
  na = 2; % Number of stacks used as actuator
  ns = 1; % Number of stacks used as force sensor
#+end_src

The ratio of the developed force to applied voltage is $d_{33} n k_a$ in [N/V].
We denote this constant by $g_a$ and:
\[ F_a = g_a V_a, \quad g_a = d_{33} n k_a \]
#+begin_src matlab :results replace value
  d33*(na*n)*(ka/(na + ns)) % [N/V]
#+end_src

#+RESULTS:
: 0.42941

From cite:fleming14_desig_model_contr_nanop_system (page 123), the relation between relative displacement and generated voltage is:
\[ V_s = \frac{d_{33}}{\epsilon^T s^D n} \Delta h \]
where:
- $V_s$: measured voltage [V]
- $d_{33}$: strain constant [m/V]
- $\epsilon^T$: permittivity under constant stress [F/m]
- $s^D$: elastic compliance under constant electric displacement [m^2/N]
- $n$: number of layers
- $\Delta h$: relative displacement [m]
#+begin_src matlab :results replace value
  1e-6*d33/(eT*sD*ns*n) % [V/um]
#+end_src

#+RESULTS:
: 5.8594

** Identification of the APA Characteristics
*** Stiffness
#+begin_src matlab :exports none
  m = 0.001;
#+end_src

The transfer function from vertical external force to the relative vertical displacement is identified.

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
#+end_src

The inverse of its DC gain is the axial stiffness of the APA:
#+begin_src matlab :results replace value
  1e-6/dcgain(G) % [N/um]
#+end_src

#+RESULTS:
: 1.8634

The specified stiffness in the datasheet is $k = 1.8\, [N/\mu m]$.

*** Resonance Frequency
The resonance frequency is specified to be between 650Hz and 840Hz.
This is also the case for the FEM model (Figure [[fig:apa300ml_resonance]]).

#+begin_src matlab :exports none
  freqs = logspace(2, 4, 5000);

  figure;
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude');
  hold off;
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_resonance.pdf', 'width', 'wide', 'height', 'normal');
#+end_src

#+name: fig:apa300ml_resonance
#+caption: First resonance is around 800Hz
#+RESULTS:
[[file:figs/apa300ml_resonance.png]]

*** Amplification factor
The amplification factor is the ratio of the axial displacement to the stack displacement.

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/d'], 1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
#+end_src

The ratio of the two displacement is computed from the FEM model.
#+begin_src matlab :results replace value
  -dcgain(G(1,1))./dcgain(G(2,1))
#+end_src

#+RESULTS:
: 4.936

If we take the ratio of the piezo height and length (approximation of the amplification factor):
#+begin_src matlab :results replace value
  75/15
#+end_src

#+RESULTS:
: 5

*** Stroke

Estimation of the actuator stroke:
\[ \Delta H = A n \Delta L \]
with:
- $\Delta H$ Axial Stroke of the APA
- $A$ Amplification factor (5 for the APA300ML)
- $n$ Number of stack used
- $\Delta L$ Stroke of the stack (0.1% of its length)

#+begin_src matlab :results replace value
  1e6 * 5 * 3 * 20e-3 * 0.1e-2
#+end_src

#+RESULTS:
: 300

This is exactly the specified stroke in the data-sheet.

** Identification of the Dynamics
The flexible element is imported using the =Reduced Order Flexible Solid= simscape block.

To model the actuator, an =Internal Force= block is added between the nodes 3 and 12.
A =Relative Motion Sensor= block is added between the nodes 1 and 2 to measure the displacement and the amplified piezo.

One mass is fixed at one end of the piezo-electric stack actuator, the other end is fixed to the world frame.

We first set the mass to be zero.
#+begin_src matlab :exports none
  m = 0.01;
#+end_src

The dynamics is identified from the applied force to the measured relative displacement.
#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  Gh = -linearize(mdl, io);
#+end_src

The same dynamics is identified for a payload mass of 10Kg.
#+begin_src matlab
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  Ghm = -linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 4, 5000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gh, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Ghm, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gh, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Ghm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_plant_dynamics.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:apa300ml_plant_dynamics
#+caption: Transfer function from forces applied by the stack to the axial displacement of the APA
#+RESULTS:
[[file:figs/apa300ml_plant_dynamics.png]]

** IFF
Let's use 2 stacks as actuators and 1 stack as force sensor.

The transfer function from actuator to sensors is identified and shown in Figure [[fig:apa300ml_iff_plant]].
#+begin_src matlab :exports none
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;

  Giff = -linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 4, 5000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Giff, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Giff, freqs, 'Hz')))), '-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_iff_plant.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:apa300ml_iff_plant
#+caption: Transfer function from actuator to force sensor
#+RESULTS:
[[file:figs/apa300ml_iff_plant.png]]

For root locus corresponding to IFF is shown in Figure [[fig:apa300ml_iff_root_locus]].

#+begin_src matlab :exports none
  figure;

  gains = logspace(0, 5, 500);

  hold on;
  plot(real(pole(Giff)),  imag(pole(Giff)), 'kx');
  plot(real(tzero(Giff)),  imag(tzero(Giff)), 'ko');
  for k = 1:length(gains)
      cl_poles = pole(feedback(Giff, gains(k)/s));
      plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  hold off;
  axis square;
  xlim([-500, 10]); ylim([0, 510]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:apa300ml_iff_root_locus
#+caption: Root Locus for IFF
#+RESULTS:
[[file:figs/apa300ml_iff_root_locus.png]]

** DVF
Now the dynamics from the stack actuator to the relative motion sensor is identified and shown in Figure [[fig:apa300ml_dvf_plant]].
#+begin_src matlab :exports none
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'], 1, 'openoutput'); io_i = io_i + 1;

  G = -linearize(mdl, io);
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 4, 5000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(G, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G, freqs, 'Hz')))), '-');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-360 0]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_dvf_plant.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:apa300ml_dvf_plant
#+caption: Transfer function from stack actuator to relative motion sensor
#+RESULTS:
[[file:figs/apa300ml_dvf_plant.png]]

The root locus for DVF is shown in Figure [[fig:apa300ml_dvf_root_locus]].

#+begin_src matlab :exports none
  figure;

  gains = logspace(0, 5, 500);

  hold on;
  plot(real(pole(G)),  imag(pole(G)), 'kx');
  plot(real(tzero(G)),  imag(tzero(G)), 'ko');
  for k = 1:length(gains)
      cl_poles = pole(feedback(G, gains(k)*s));
      plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  hold off;
  axis square;
  xlim([-500, 10]); ylim([0, 510]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall');
#+end_src

#+name: fig:apa300ml_dvf_root_locus
#+caption: Root Locus for Direct Velocity Feedback
#+RESULTS:
[[file:figs/apa300ml_dvf_root_locus.png]]

** TODO Sensor Fusion                                              :noexport:
- [ ] What is the goal of that? Special control properties, lower the sensor noise?

Use the relative motion sensor at low frequency and the force sensor at high frequency.
#+begin_src matlab
  m = 10;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Va'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
#+end_src

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

  Gresp = squeeze(freqresp(G, freqs, 'Hz'));

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(Gresp(1, :)), '-');
  plot(freqs, abs(Gresp(2, :)), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(Gresp(1, :))), '-', ...
       'DisplayName', 'Force');
  plot(freqs, 180/pi*unwrap(angle(Gresp(2, :))), '-', ...
       'DisplayName', 'Displacement');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

Merge around 1kHz
#+begin_src matlab :exports none
  freqs = logspace(1, 5, 1000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(Gresp(1, :).*squeeze(freqresp(0.0065/s^2, freqs, 'Hz'))'), '-');
  plot(freqs, abs(Gresp(2, :)), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(Gresp(1, :).*squeeze(freqresp(1/s^2, freqs, 'Hz'))')), '-', ...
       'DisplayName', 'Force');
  plot(freqs, 180/pi*unwrap(angle(Gresp(2, :))), '-', ...
       'DisplayName', 'Displacement');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

LPF and HPF
#+begin_src matlab
  n = 3; w0 = 2*pi*1e3; G0 = 1/10; G1 = 1e5; Gc = 1/2;
  W1 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;

  n = 3; w0 = 2*pi*1e3; G0 = 1e5; G1 = 0.1; Gc = 1/2;
  W2 = (((1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (G0/Gc)^(1/n))/((1/G1)^(1/n)*(1/w0)*sqrt((1-(G0/Gc)^(2/n))/(1-(Gc/G1)^(2/n)))*s + (1/Gc)^(1/n)))^n;
#+end_src

#+begin_src matlab
  P = [W1 -W1;
       0   W2;
       1   0];
      
  [H2, ~, gamma, ~] = hinfsyn(P, 1, 1,'TOLGAM', 0.001, 'METHOD', 'ric', 'DISPLAY', 'on');
  H1 = 1 - H2;
#+end_src

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

  Gresp = squeeze(freqresp(G, freqs, 'Hz'));

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(Gresp(1, :).*squeeze(freqresp(0.0065/s^2*H2, freqs, 'Hz'))'), '-');
  plot(freqs, abs(Gresp(2, :).*squeeze(freqresp(H1, freqs, 'Hz'))'), '-');
  plot(freqs, abs(Gresp(2, :).*squeeze(freqresp(H1, freqs, 'Hz'))' + Gresp(1, :).*squeeze(freqresp(0.0065/s^2*H2, freqs, 'Hz'))'), 'k--');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(Gresp(1, :).*squeeze(freqresp(0.0065/s^2*H2, freqs, 'Hz'))')), '-', ...
       'DisplayName', 'Force');
  plot(freqs, 180/pi*unwrap(angle(Gresp(2, :).*squeeze(freqresp(H1, freqs, 'Hz'))')), '-', ...
       'DisplayName', 'Displacement');
  plot(freqs, 180/pi*unwrap(angle(Gresp(2, :).*squeeze(freqresp(H1, freqs, 'Hz'))' + Gresp(1, :).*squeeze(freqresp(0.0065/s^2*H2, freqs, 'Hz'))')), 'k--', ...
       'DisplayName', 'Super Sensor');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

#+begin_src matlab :exports none
  Gss = -zpk(G(1)*0.0065/s^2*H2 + G(2)*H1);

  freqs = logspace(1, 5, 1000);

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gss, freqs, 'Hz'))'), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*(angle(squeeze(freqresp(Gss, freqs, 'Hz'))')), '-', ...
       'DisplayName', 'Force');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

Root locus
#+begin_src matlab
  figure;

  gains = logspace(4, 7, 100);

  hold on;
  plot(real(pole(Gss)),  imag(pole(Gss)), 'kx');
  plot(real(tzero(Gss)),  imag(tzero(Gss)), 'ko');
  for k = 1:length(gains)
      cl_poles = pole(feedback(Gss, gains(k)*s));
      plot(real(cl_poles), imag(cl_poles), 'k.');
  end
  hold off;
  axis square;
  xlim([-500, 10]); ylim([0, 510]);

  xlabel('Real Part'); ylabel('Imaginary Part');
#+end_src

** Identification for a simpler model
The goal in this section is to identify the parameters of a simple APA model from the FEM.
This can be useful is a lower order model is to be used for simulations.

The presented model is based on cite:souleille18_concep_activ_mount_space_applic.

The model represents the Amplified Piezo Actuator (APA) from Cedrat-Technologies (Figure [[fig:souleille18_model_piezo]]).
The parameters are shown in the table below.

#+name: fig:souleille18_model_piezo
#+caption: Picture of an APA100M from Cedrat Technologies. Simplified model of a one DoF payload mounted on such isolator
[[file:./figs/souleille18_model_piezo.png]]

#+caption: Parameters used for the model of the APA 100M
|       | Meaning                                                        |
|-------+----------------------------------------------------------------|
| $k_e$ | Stiffness used to adjust the pole of the isolator              |
| $k_1$ | Stiffness of the metallic suspension when the stack is removed |
| $k_a$ | Stiffness of the actuator                                      |
| $c_1$ | Added viscous damping                                          |

The goal is to determine $k_e$, $k_a$ and $k_1$ so that the simplified model fits the FEM model.

\[ \alpha = \frac{x_1}{f}(\omega=0) = \frac{\frac{k_e}{k_e + k_a}}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]
\[ \beta = \frac{x_1}{F}(\omega=0) = \frac{1}{k_1 + \frac{k_e k_a}{k_e + k_a}} \]

If we can fix $k_a$, we can determine $k_e$ and $k_1$ with:
\[ k_e = \frac{k_a}{\frac{\beta}{\alpha} - 1} \]
\[ k_1 = \frac{1}{\beta} - \frac{k_e k_a}{k_e + k_a} \]

#+begin_src matlab :exports none
  m = 10;
#+end_src

#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'APA300ML_test_bench';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1; % External Vertical Force [N]
  io(io_i) = linio([mdl, '/w'],  1, 'openinput');  io_i = io_i + 1; % Base Motion [m]
  io(io_i) = linio([mdl, '/Fa'], 1, 'openinput');  io_i = io_i + 1; % Actuator Force [N]
  io(io_i) = linio([mdl, '/y'],  1, 'openoutput'); io_i = io_i + 1; % Vertical Displacement [m]
  io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor [V]
  io(io_i) = linio([mdl, '/d'],  1, 'openoutput'); io_i = io_i + 1; % Stack Displacement [m]

  G = linearize(mdl, io);

  G.InputName = {'Fd', 'w', 'Fa'};
  G.OutputName = {'y', 'Fs', 'd'};
#+end_src

From the identified dynamics, compute $\alpha$ and $\beta$
#+begin_src matlab
  alpha = abs(dcgain(G('y', 'Fa')));
  beta  = abs(dcgain(G('y', 'Fd')));
#+end_src

$k_a$ is estimated using the following formula:
#+begin_src matlab
  ka = 0.8/abs(dcgain(G('y', 'Fa')));
#+end_src
The factor can be adjusted to better match the curves.

Then $k_e$ and $k_1$ are computed.
#+begin_src matlab
  ke = ka/(beta/alpha - 1);
  k1 = 1/beta - ke*ka/(ke + ka);
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable(1e-6*[ka; ke; k1], {'ka', 'ke', 'k1'}, {'Value [N/um]'}, ' %.1f ');
#+end_src

#+RESULTS:
|    | Value [N/um] |
|----+--------------|
| ka |         42.9 |
| ke |          1.5 |
| k1 |          0.4 |

The damping in the system is adjusted to match the FEM model if necessary.
#+begin_src matlab
  c1 = 1e2;
#+end_src

Analytical model of the simpler system:
#+begin_src matlab
  Ga = 1/(m*s^2 + k1 + c1*s + ke*ka/(ke + ka)) * ...
       [ 1 ,              k1 + c1*s + ke*ka/(ke + ka)  , ke/(ke + ka) ;
        -ke*ka/(ke + ka), ke*ka/(ke + ka)*m*s^2 ,       -ke/(ke + ka)*(m*s^2 + c1*s + k1)];

  Ga.InputName = {'Fd', 'w', 'Fa'};
  Ga.OutputName = {'y', 'Fs'};
#+end_src

Adjust the DC gain for the force sensor:
#+begin_src matlab
  lambda = dcgain(Ga('Fs', 'Fd'))/dcgain(G('Fs', 'Fd'));
#+end_src

#+begin_src matlab :exports none
  freqs = logspace(0, 5, 1000);

  figure;

  ay = subplot(2, 3, 1);
  title('$\displaystyle \frac{x_1}{w}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('y', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax2 = subplot(2, 3, 2);
  title('$\displaystyle \frac{x_1}{f}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fa'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('y', 'Fa'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax3 = subplot(2, 3, 3);
  title('$\displaystyle \frac{x_1}{F}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G( 'y', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('y', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax4 = subplot(2, 3, 4);
  title('$\displaystyle \frac{F_s}{w}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(lambda*G( 'Fs', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax5 = subplot(2, 3, 5);
  title('$\displaystyle \frac{F_s}{f}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(lambda*G( 'Fs', 'Fa'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'Fa'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax6 = subplot(2, 3, 6);
  title('$\displaystyle \frac{F_s}{F}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(lambda*G( 'Fs', 'Fd'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Ga('Fs', 'Fd'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/apa300ml_comp_simpler_model.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:apa300ml_comp_simpler_model
#+caption: Comparison of the Dynamics between the FEM model and the simplified one
#+RESULTS:
[[file:figs/apa300ml_comp_simpler_model.png]]

* Flexible Joint
** Introduction                                                      :ignore:

The flexor in Figure [[fig:flexor_id16_screenshot]] is studied with a FEM.

#+name: fig:flexor_id16_screenshot
#+caption: Flexor studied
[[file:figs/flexor_id16_screenshot.png]]

** Matlab Init                                             :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab
  addpath('./src/');
  addpath('./data/flexor_ID16/');
#+end_src

#+begin_src matlab :exports none
  open('flexor_ID16.slx');
#+end_src

** Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
We first extract the stiffness and mass matrices.
#+begin_src matlab
  K = extractMatrix('mat_K_6modes_2MDoF.matrix');
  M = extractMatrix('mat_M_6modes_2MDoF.matrix');
#+end_src

Then, we extract the coordinates of the interface nodes.
#+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('out_nodes_3D.txt');
#+end_src

#+begin_src matlab
  save('./mat/flexor_ID16.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
#+end_src

** Output parameters
#+begin_src matlab
  load('./mat/flexor_ID16.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
#+end_src

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable([length(n_i); length(int_i); size(M,1) - 6*length(int_i); size(M,1)], {'Total number of Nodes', 'Number of interface Nodes', 'Number of Modes', 'Size of M and K matrices'}, {}, ' %.0f ');
#+end_src

#+RESULTS:
| Total number of Nodes     |  2 |
| Number of interface Nodes |  2 |
| Number of Modes           |  6 |
| Size of M and K matrices  | 18 |

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([[1:length(int_i)]', int_i, int_xyz], {}, {'Node i', 'Node Number', 'x [m]', 'y [m]', 'z [m]'}, ' %f ');
#+end_src

#+caption: Coordinates of the interface nodes
#+RESULTS:
| Node i | Node Number | x [m] | y [m] | z [m] |
|--------+-------------+-------+-------+-------|
|    1.0 |    181278.0 |   0.0 |   0.0 |   0.0 |
|    2.0 |    181279.0 |   0.0 |   0.0 |  -0.0 |

#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(K(1:10, 1:10), {}, {}, ' %.2e ');
#+end_src

#+caption: First 10x10 elements of the Stiffness matrix
#+RESULTS:
|  11200000.0 |       195.0 |       2220.0 |    -0.719 |   -265.0 |      1.59 | -11200000.0 |      -213.0 |      -2220.0 |    0.147 |
|       195.0 |  11400000.0 |       1290.0 |    -148.0 |   -0.188 |      2.41 |      -212.0 | -11400000.0 |      -1290.0 |    148.0 |
|      2220.0 |      1290.0 |  119000000.0 |      1.31 |     1.49 |      1.79 |     -2220.0 |     -1290.0 | -119000000.0 |    -1.31 |
|      -0.719 |      -148.0 |         1.31 |      33.0 | 0.000488 | -0.000977 |       0.141 |       148.0 |        -1.31 |    -33.0 |
|      -265.0 |      -0.188 |         1.49 |  0.000488 |     33.0 |   0.00293 |       266.0 |       0.154 |        -1.49 |  0.00026 |
|        1.59 |        2.41 |         1.79 | -0.000977 |  0.00293 |     236.0 |       -1.32 |       -2.55 |        -1.79 | 0.000379 |
| -11200000.0 |      -212.0 |      -2220.0 |     0.141 |    266.0 |     -1.32 |  11400000.0 |     24600.0 |       1640.0 |    120.0 |
|      -213.0 | -11400000.0 |      -1290.0 |     148.0 |    0.154 |     -2.55 |     24600.0 |  11400000.0 |       1290.0 |    -72.0 |
|     -2220.0 |     -1290.0 | -119000000.0 |     -1.31 |    -1.49 |     -1.79 |      1640.0 |      1290.0 |  119000000.0 |     1.32 |
|       0.147 |       148.0 |        -1.31 |     -33.0 |  0.00026 |  0.000379 |       120.0 |       -72.0 |         1.32 |     34.7 |


#+begin_src matlab :exports results :results value table replace :tangle no
  data2orgtable(M(1:10, 1:10), {}, {}, ' %.1g ');
#+end_src

#+caption: First 10x10 elements of the Mass matrix
#+RESULTS:
|   0.02 |   1e-09 | -4e-08 |  -1e-10 | 0.0002 | -3e-11 |  0.004 |  5e-08 |  7e-08 |  1e-10 |
|  1e-09 |    0.02 | -3e-07 | -0.0002 | -1e-10 | -2e-09 |  2e-08 |  0.004 |  3e-07 |  1e-05 |
| -4e-08 |  -3e-07 |   0.02 |   7e-10 | -2e-09 |  1e-09 |  3e-07 |  7e-08 |  0.003 |  1e-09 |
| -1e-10 | -0.0002 |  7e-10 |   4e-06 | -1e-12 | -6e-13 |  2e-10 | -7e-06 | -8e-10 | -1e-09 |
| 0.0002 |  -1e-10 | -2e-09 |  -1e-12 |  3e-06 |  2e-13 |  9e-06 |  4e-11 |  2e-09 | -3e-13 |
| -3e-11 |  -2e-09 |  1e-09 |  -6e-13 |  2e-13 |  4e-07 |  8e-11 |  9e-10 | -1e-09 |  2e-12 |
|  0.004 |   2e-08 |  3e-07 |   2e-10 |  9e-06 |  8e-11 |   0.02 | -7e-08 | -3e-07 | -2e-10 |
|  5e-08 |   0.004 |  7e-08 |  -7e-06 |  4e-11 |  9e-10 | -7e-08 |   0.01 | -4e-08 | 0.0002 |
|  7e-08 |   3e-07 |  0.003 |  -8e-10 |  2e-09 | -1e-09 | -3e-07 | -4e-08 |   0.02 | -1e-09 |
|  1e-10 |   1e-05 |  1e-09 |  -1e-09 | -3e-13 |  2e-12 | -2e-10 | 0.0002 | -1e-09 |  2e-06 |

Using =K=, =M= and =int_xyz=, we can use the =Reduced Order Flexible Solid= simscape block.

** Flexible Joint Characteristics
The most important parameters of the flexible joint can be directly estimated from the stiffness matrix.

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([1e-6*K(3,3), K(4,4), K(5,5), K(6,6); 60, 15, 15, 20]', {'Axial Stiffness [N/um]', 'Bending Stiffness [Nm/rad]', 'Bending Stiffness [Nm/rad]', 'Torsion Stiffness [Nm/rad]'}, {'*Caracteristic*', '*Value*', '*Estimation by Francois*'}, ' %0.f ');
#+end_src

#+RESULTS:
| *Caracteristic*            | *Value* | *Estimation by Francois* |
|----------------------------+---------+--------------------------|
| Axial Stiffness [N/um]     |     119 |                       60 |
| Bending Stiffness [Nm/rad] |      33 |                       15 |
| Bending Stiffness [Nm/rad] |      33 |                       15 |
| Torsion Stiffness [Nm/rad] |     236 |                       20 |

** Identification of the parameters using Simscape
The flexor is now imported into Simscape and its parameters are estimated using an identification.

#+begin_src matlab :exports none
  m = 1;
#+end_src

The dynamics is identified from the applied force/torque to the measured displacement/rotation of the flexor.
#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'flexor_ID16';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/T'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1;

  G = linearize(mdl, io);
#+end_src

And we find the same parameters as the one estimated from the Stiffness matrix.

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([1e-6*K(3,3), K(4,4), K(5,5), K(6,6) ; 1e-6./dcgain(G(3,3)), 1./dcgain(G(4,4)), 1./dcgain(G(5,5)), 1./dcgain(G(6,6))]', {'Axial Stiffness Dz [N/um]', 'Bending Stiffness Rx [Nm/rad]', 'Bending Stiffness Ry [Nm/rad]', 'Torsion Stiffness Rz [Nm/rad]'}, {'*Caracteristic*', '*Value*', '*Identification*'}, ' %0.f ');
#+end_src

#+RESULTS:
| *Caracteristic*               | *Value* | *Identification* |
|-------------------------------+---------+------------------|
| Axial Stiffness Dz [N/um]     |     119 |              119 |
| Bending Stiffness Rx [Nm/rad] |      33 |               33 |
| Bending Stiffness Ry [Nm/rad] |      33 |               33 |
| Torsion Stiffness Rz [Nm/rad] |     236 |              236 |

** Simpler Model

Let's now model the flexible joint with a "perfect" Bushing joint as shown in Figure [[fig:flexible_joint_simscape]].

#+name: fig:flexible_joint_simscape
#+caption: Bushing Joint used to model the flexible joint
[[file:figs/flexible_joint_simscape.png]]

The parameters of the Bushing joint (stiffnesses) are estimated from the Stiffness matrix that was computed from the FEM.
#+begin_src matlab
  Kx = K(1,1); % [N/m]
  Ky = K(2,2); % [N/m]
  Kz = K(3,3); % [N/m]
  Krx = K(4,4); % [Nm/rad]
  Kry = K(5,5); % [Nm/rad]
  Krz =  K(6,6); % [Nm/rad]
#+end_src

The dynamics from the applied force/torque to the measured displacement/rotation of the flexor is identified again for this simpler model.
#+begin_src matlab :exports none
  %% Name of the Simulink File
  mdl = 'flexor_ID16_simplified';

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/T'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1;

  Gs = linearize(mdl, io);
#+end_src

The two obtained dynamics are compared in Figure

#+begin_src matlab :exports none
  freqs = logspace(0, 5, 1000);

  figure;

  ax1 = subplot(1,2,1);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), '-', 'DisplayName', '$D_x/F_x$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Gs(1,1), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(G(2,2), freqs, 'Hz'))), '-', 'DisplayName', '$D_y/F_y$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Gs(2,2), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',3)
  plot(freqs, abs(squeeze(freqresp(G(3,3), freqs, 'Hz'))), '-', 'DisplayName', '$D_z/F_z$');
  set(gca,'ColorOrderIndex',3)
  plot(freqs, abs(squeeze(freqresp(Gs(3,3), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
  hold off;
  legend('location', 'northeast');

  ax2 = subplot(1,2,2);
  hold on;
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(G(4,4), freqs, 'Hz'))), '-', 'DisplayName', '$R_x/M_x$');
  set(gca,'ColorOrderIndex',1)
  plot(freqs, abs(squeeze(freqresp(Gs(4,4), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(G(5,5), freqs, 'Hz'))), '-', 'DisplayName', '$R_y/M_y$');
  set(gca,'ColorOrderIndex',2)
  plot(freqs, abs(squeeze(freqresp(Gs(5,5), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
  set(gca,'ColorOrderIndex',3)
  plot(freqs, abs(squeeze(freqresp(G(6,6), freqs, 'Hz'))), '-', 'DisplayName', '$R_z/M_z$');
  set(gca,'ColorOrderIndex',3)
  plot(freqs, abs(squeeze(freqresp(Gs(6,6), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
  hold off;
  legend('location', 'northeast');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/flexor_ID16_compare_bushing_joint.pdf', 'width', 'full', 'height', 'tall');
#+end_src

#+name: fig:flexor_ID16_compare_bushing_joint
#+caption: Comparison of the Joint compliance between the FEM model and the simpler model
#+RESULTS:
[[file:figs/flexor_ID16_compare_bushing_joint.png]]

* Integral Force Feedback with Amplified Piezo
** Introduction                                                      :ignore:
In this section, we try to replicate the results obtained in cite:souleille18_concep_activ_mount_space_applic.

** Matlab Init                                             :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab
  addpath('./src/');
  addpath('./data/piezo_amplified_IFF/');
#+end_src

#+begin_src matlab :exports none
  open('piezo_amplified_IFF');
#+end_src

** Import Mass Matrix, Stiffness Matrix, and Interface Nodes Coordinates
We first extract the stiffness and mass matrices.
#+begin_src matlab
  K = extractMatrix('piezo_amplified_IFF_K.txt');
  M = extractMatrix('piezo_amplified_IFF_M.txt');
#+end_src

Then, we extract the coordinates of the interface nodes.
#+begin_src matlab
  [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('piezo_amplified_IFF.txt');
#+end_src

** IFF Plant
The transfer function from the force actuator to the force sensor is identified and shown in Figure [[fig:piezo_amplified_iff_plant]].

#+begin_src matlab
  Kiff = tf(0);
#+end_src

#+begin_src matlab
  m = 0;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Kiff'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/G'],    1, 'openoutput'); io_i = io_i + 1;

  Gf = linearize(mdl, io);
#+end_src

#+begin_src matlab
  m = 10;
#+end_src

#+begin_src matlab
  Gfm = linearize(mdl, io);
#+end_src

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gf, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Gfm, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-390 30]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/piezo_amplified_iff_plant.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:piezo_amplified_iff_plant
#+caption: IFF Plant
#+RESULTS:
[[file:figs/piezo_amplified_iff_plant.png]]

** IFF controller
The controller is defined and the loop gain is shown in Figure [[fig:piezo_amplified_iff_loop_gain]].
#+begin_src matlab
  Kiff = -1e12/s;
#+end_src

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

  figure;

  ax1 = subplot(2,1,1);
  hold on;
  plot(freqs, abs(squeeze(freqresp(Gf*Kiff, freqs, 'Hz'))), '-');
  plot(freqs, abs(squeeze(freqresp(Gfm*Kiff, freqs, 'Hz'))), '-');
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
  hold off;

  ax2 = subplot(2,1,2);
  hold on;
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gf*Kiff, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 0kg$');
  plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gfm*Kiff, freqs, 'Hz')))), '-', ...
       'DisplayName', '$m = 10kg$');
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
  yticks(-360:90:360);
  ylim([-180 180]);
  xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
  hold off;
  linkaxes([ax1,ax2],'x');
  xlim([freqs(1), freqs(end)]);
  legend('location', 'southwest');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/piezo_amplified_iff_loop_gain.pdf', 'width', 'full', 'height', 'full');
#+end_src

#+name: fig:piezo_amplified_iff_loop_gain
#+caption: IFF Loop Gain
#+RESULTS:
[[file:figs/piezo_amplified_iff_loop_gain.png]]

** Closed Loop System
#+begin_src matlab
  m = 10;
#+end_src

#+begin_src matlab
  Kiff = -1e12/s;
#+end_src

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

  %% Input/Output definition
  clear io; io_i = 1;
  io(io_i) = linio([mdl, '/Dw'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/Fd'], 1, 'openinput');  io_i = io_i + 1;
  io(io_i) = linio([mdl, '/d'],  1, 'openoutput'); io_i = io_i + 1;
  io(io_i) = linio([mdl, '/G'],  1, 'output'); io_i = io_i + 1;

  Giff = linearize(mdl, io);
  Giff.InputName  = {'w', 'f', 'F'};
  Giff.OutputName  = {'x1', 'Fs'};
#+end_src

#+begin_src matlab
  Kiff = tf(0);
#+end_src

#+begin_src matlab
  G = linearize(mdl, io);
  G.InputName  = {'w', 'f', 'F'};
  G.OutputName  = {'x1', 'Fs'};
#+end_src

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

  figure;

  ax1 = subplot(2, 3, 1);
  title('$\displaystyle \frac{x_1}{w}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('x1', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax2 = subplot(2, 3, 2);
  title('$\displaystyle \frac{x_1}{f}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('x1', 'f'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'f'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax3 = subplot(2, 3, 3);
  title('$\displaystyle \frac{x_1}{F}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('x1', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('x1', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax4 = subplot(2, 3, 4);
  title('$\displaystyle \frac{F_s}{w}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('Fs', 'w'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'w'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/m]');xlabel('Frequency [Hz]');

  ax5 = subplot(2, 3, 5);
  title('$\displaystyle \frac{F_s}{f}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('Fs', 'f'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'f'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]');xlabel('Frequency [Hz]');

  ax6 = subplot(2, 3, 6);
  title('$\displaystyle \frac{F_s}{F}$')
  hold on;
  plot(freqs, abs(squeeze(freqresp(G('Fs', 'F'), freqs, 'Hz'))));
  plot(freqs, abs(squeeze(freqresp(Giff('Fs', 'F'), freqs, 'Hz'))));
  hold off;
  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
  ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
#+end_src

#+begin_src matlab :tangle no :exports results :results file replace
  exportFig('figs/piezo_amplified_iff_comp.pdf', 'width', 1500, 'height', 'full');
#+end_src

#+name: fig:piezo_amplified_iff_comp
#+caption: OL and CL transfer functions
#+RESULTS:
[[file:figs/piezo_amplified_iff_comp.png]]


#+name: fig:souleille18_results
#+caption: Results obtained in cite:souleille18_concep_activ_mount_space_applic
[[file:figs/souleille18_results.png]]

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