#+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 :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.


** 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;
#+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 = 10;
#+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(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'))), '-');
  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

* Integral Force Feedback with Amplified Piezo
** 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]]