#+TITLE: Test Bench - Amplified Piezoelectric Actuator
:DRAWER:
#+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="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>

#+BIND: org-latex-image-default-option "scale=1"
#+BIND: org-latex-image-default-width ""

#+LaTeX_CLASS: scrreprt
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \bibliography{test-bench-apa.bib}

#+BIND: org-latex-bib-compiler "biber"

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

#+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 file raw replace
#+PROPERTY: header-args:latex+ :buffer no
#+PROPERTY: header-args:latex+ :tangle no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:

#+begin_export html
  <hr>
  <p>This report is also available as a <a href="./test-bench-apa.pdf">pdf</a>.</p>
  <hr>
#+end_export

#+latex: \clearpage

* Build                                                             :noexport:
#+NAME: startblock
#+BEGIN_SRC emacs-lisp :results none :tangle no
(add-to-list 'org-latex-classes
             '("scrreprt"
               "\\documentclass{scrreprt}"
               ("\\chapter{%s}" . "\\chapter*{%s}")
               ("\\section{%s}" . "\\section*{%s}")
               ("\\subsection{%s}" . "\\subsection*{%s}")
               ("\\paragraph{%s}" . "\\paragraph*{%s}")
               ))


;; Remove automatic org heading labels
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
  "Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
  (when (org-export-derived-backend-p backend 'latex)
    (replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
(add-to-list 'org-export-filter-headline-functions
             'my-latex-filter-removeOrgAutoLabels)

;; Remove all org comments in the output LaTeX file
(defun delete-org-comments (backend)
  (loop for comment in (reverse (org-element-map (org-element-parse-buffer)
                                    'comment 'identity))
        do
        (setf (buffer-substring (org-element-property :begin comment)
                                (org-element-property :end comment))
              "")))
(add-hook 'org-export-before-processing-hook 'delete-org-comments)

;; Use no package by default
(setq org-latex-packages-alist nil)
(setq org-latex-default-packages-alist nil)

;; Do not include the subtitle inside the title
(setq org-latex-subtitle-separate t)
(setq org-latex-subtitle-format "\\subtitle{%s}")

(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
                                       org-ref-acronyms-before-parsing))
#+END_SRC

* Notes                                                             :noexport:

Prefix for figures/section/tables =test_apa=

** Add the following reports
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/test-bench-apa95ml/test-bench-apa95ml.org][test-bench-apa95ml]]
  Maybe not useful
  - [X] See if the IFF root locus has been measured with the APA300ML
    *Yes*
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/test-bench-apa300ml/test-bench-apa300ml.org][test-bench-apa300ml]]
  - Model (Section 1)
  - Basic measurements (dimensions, electrical, stroke, etc...) (Section 2)
  - Dynamical measurements (Section 3)
  - Simscape Model (Section 4)

** TODO [#B] Add sitffness of APA shell from FEM

** TODO [#B] Check things about resistor in parallel with the force sensor

Verify that everything interesting to say about that is either done before in the thesis or in this report.

* Introduction                                                        :ignore:


#+name: fig:test_apa_received
#+attr_latex: :width 0.7\linewidth
#+caption: Picture of 5 out of the 7 received APA300ML
[[file:figs/test_apa_received.jpg]]

The first goal is to characterize the APA300ML in terms of:
- The, geometric features, electrical capacitance, stroke, hysteresis, spurious resonances.
  This is performed in Section ref:sec:test_apa_basic_meas.
- The dynamics from the generated DAC voltage (going to the voltage amplifiers and then applied on the actuator stacks) to the induced displacement, and to the measured voltage by the force sensor stack.
  Also the "actuator constant" and "sensor constant" are identified.
  This is done in Section ref:sec:test_apa_dynamics.
- Compare the measurements with the Simscape models (2DoF, Super-Element) in order to tuned/validate the models.
  This is explained in Section ref:sec:test_apa_simscape.

#+name: tab:test_apa_section_matlab_code
#+caption: Report sections and corresponding Matlab files
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
#+attr_latex: :center t :booktabs t
| *Sections*                              | *Matlab File*                 |
|-----------------------------------------+-------------------------------|
| Section ref:sec:test_apa_basic_meas     | =test_apa_1_basic_meas.m=     |
| Section ref:sec:test_apa_dynamics       | =test_apa_2_dynamics.m=       |
| Section ref:sec:test_apa_model_2dof     | =test_apa_3_model_2dof.m=     |
| Section ref:sec:test_apa_model_flexible | =test_apa_4_model_flexible.m= |

* First Basic Measurements
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_1_basic_meas.m
:END:
<<sec:test_apa_basic_meas>>

** Introduction                                                      :ignore:

Before using the measurement bench to characterize the APA300ML, first simple measurements are performed:
- Section ref:sec:test_apa_geometrical_measurements: the geometric tolerances of the interface planes are checked
- Section ref:sec:test_apa_electrical_measurements: the capacitance of the piezoelectric stacks is measured
- Section ref:sec:test_apa_stroke_measurements: the stroke of each APA is measured
- Section ref:sec:test_apa_spurious_resonances: the "spurious" resonances of the APA are investigated

** 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 :tangle no :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Geometrical Measurements
<<sec:test_apa_geometrical_measurements>>

To measure the flatness of the two mechanical interfaces of the APA300ML, a small measurement bench is installed on top of a metrology granite with very good flatness.

As shown in Figure ref:fig:test_apa_flatness_setup, the APA is fixed to a clamp while a measuring probe[fn:3] is used to measure the height of 4 points on each of the APA300ML interfaces.

From the X-Y-Z coordinates of the measured 8 points, the flatness is estimated by best fitting[fn:4] a plane through all the points.

#+name: fig:test_apa_flatness_setup
#+attr_latex: :width 0.4\linewidth
#+caption: Measurement setup for flatness estimation of the two mechanical interfaces
[[file:figs/test_apa_flatness_setup.png]]

#+begin_src matlab
%% Measured height for all the APA at the 8 locations
apa1 = 1e-6*[0, -0.5 , 3.5  , 3.5  , 42   , 45.5, 52.5 , 46];
apa2 = 1e-6*[0, -2.5 , -3   , 0    , -1.5 , 1   , -2   , -4];
apa3 = 1e-6*[0, -1.5 , 15   , 17.5 , 6.5  , 6.5 , 21   , 23];
apa4 = 1e-6*[0, 6.5  , 14.5 , 9    , 16   , 22  , 29.5 , 21];
apa5 = 1e-6*[0, -12.5, 16.5 , 28.5 , -43  , -52 , -22.5, -13.5];
apa6 = 1e-6*[0, -8   , -2   , 5    , -57.5, -62 , -55.5, -52.5];
apa7 = 1e-6*[0, 9    , -18.5, -30  , 31   , 46.5, 16.5 , 7.5];

apa = {apa1, apa2, apa3, apa4, apa5, apa6, apa7};

%% X-Y positions of the measurements points
W = 20e-3;   % Width [m]
L = 61e-3;   % Length [m]
d = 1e-3;    % Distance from border [m]
l = 15.5e-3; % [m]

pos = [[-L/2 + d,      W/2 - d];
       [-L/2 + l - d,  W/2 - d];
       [-L/2 + l - d, -W/2 + d];
       [-L/2 + d,     -W/2 + d];
       [L/2 - l + d,   W/2 - d];
       [L/2 - d,       W/2 - d];
       [L/2 - d,      -W/2 + d];
       [L/2 - l + d,  -W/2 + d]]';

%% Using fminsearch to find the best fitting plane
apa_d = zeros(1, 7); % Measured flatness of the APA
for i = 1:7
    fun = @(x)max(abs(([pos; apa{i}]-[0;0;x(1)])'*([x(2:3);1]/norm([x(2:3);1]))));
    x0 = [0;0;0];
    [x, min_d] = fminsearch(fun,x0);
    apa_d(i) = min_d;
end
#+end_src

The measured flatness, summarized in Table ref:tab:test_apa_flatness_meas, are within the specifications.

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(1e6*apa_d', {'APA 1', 'APA 2', 'APA 3', 'APA 4', 'APA 5', 'APA 6', 'APA 7'}, {'*Flatness* $[\mu m]$'}, ' %.1f ');
#+end_src

#+name: tab:test_apa_flatness_meas
#+caption: Estimated flatness of the APA300ML interfaces
#+attr_latex: :environment tabularx :width 0.3\linewidth :align Xc
#+attr_latex: :center t :booktabs t
#+RESULTS:
|       | *Flatness* $[\mu m]$ |
|-------+----------------------|
| APA 1 |                  8.9 |
| APA 2 |                  3.1 |
| APA 3 |                  9.1 |
| APA 4 |                  3.0 |
| APA 5 |                  1.9 |
| APA 6 |                  7.1 |
| APA 7 |                 18.7 |

** Electrical Measurements
<<sec:test_apa_electrical_measurements>>

From the documentation of the APA300ML, the total capacitance of the three stacks should be between $18\,\mu F$ and $26\,\mu F$ with a nominal capacitance of $20\,\mu F$.

The capacitance of the piezoelectric stacks found in the APA300ML have been measured with the LCR meter[fn:1] shown in Figure ref:fig:test_apa_lcr_meter.
The two stacks used as an actuator and the stack used as a force sensor are measured separately.

#+name: fig:test_apa_lcr_meter
#+caption: LCR Meter used for the measurements
#+attr_latex: :width 0.6\linewidth
[[file:figs/test_apa_lcr_meter.jpg]]

The measured capacitance are summarized in Table ref:tab:test_apa_capacitance and the average capacitance of one stack is $\approx 5 \mu F$.
However, the measured capacitance of the stacks of "APA 3" is only half of the expected capacitance.
This may indicate a manufacturing defect.

The measured capacitance is found to be lower than the specified one.
This may be due to the fact that the manufacturer measures the capacitance with large signals ($-20\,V$ to $150\,V$) while it was here measured with small signals.

#+name: tab:test_apa_capacitance
#+caption: Capacitance measured with the LCR meter. The excitation signal is a sinus at 1kHz
#+attr_latex: :environment tabularx :width 0.5\linewidth :align lcc
#+attr_latex: :center t :booktabs t
|       | *Sensor Stack* | *Actuator Stacks* |
|-------+----------------+-------------------|
| APA 1 |           5.10 |             10.03 |
| APA 2 |           4.99 |              9.85 |
| APA 3 |           1.72 |              5.18 |
| APA 4 |           4.94 |              9.82 |
| APA 5 |           4.90 |              9.66 |
| APA 6 |           4.99 |              9.91 |
| APA 7 |           4.85 |              9.85 |

** Stroke and Hysteresis Measurement
<<sec:test_apa_stroke_measurements>>

The goal is here to verify that the stroke of the APA300ML is as specified in the datasheet.
To do so, one side of the APA is fixed to the granite, and a displacement probe[fn:2] is located on the other side as shown in Figure ref:fig:test_apa_stroke_bench.

Then, the voltage across the two actuator stacks is varied from $-20\,V$ to $150\,V$ using a DAC and a voltage amplifier.
Note that the voltage is here slowly varied as the displacement probe has a very low measurement bandwidth (see Figure ref:fig:test_apa_stroke_bench, left).

#+name: fig:test_apa_stroke_bench
#+caption: Bench to measured the APA stroke
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_apa_stroke_bench.jpg]]

The measured APA displacement is shown as a function of the applied voltage in Figure ref:fig:test_apa_stroke_result, right.

Typical hysteresis curves for piezoelectric stack actuators can be observed.
The measured stroke is approximately $250\,\mu m$ when using only two of the three stacks, which is enough for the current application.
This is even above what is specified as the nominal stroke in the data-sheet ($304\,\mu m$, therefore $\approx 200\,\mu m$ if only two stacks are used).

It is clear from Figure ref:fig:test_apa_stroke_result that "APA 3" has an issue compared to the other units.
This confirms the abnormal electrical measurements made in Section ref:sec:test_apa_electrical_measurements.
This unit was send sent back to Cedrat and a new one was shipped back.
From now on, only the six APA that behave as expected will be used.

#+begin_src matlab
%% Load the measured strokes
load('meas_apa_stroke.mat', 'apa300ml_2s')
#+end_src

#+begin_src matlab :exports none :results none
%% Results of the measured APA stroke
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

% Generated voltage across the two piezoelectric stack actuators to estimate the stroke of the APA300ML
ax1 = nexttile();
plot(apa300ml_2s{1}.t - apa300ml_2s{1}.t(1), 20*apa300ml_2s{1}.V, 'k-')
xlabel('Time [s]'); ylabel('Voltage [V]')
ylim([-20, 160])

% Measured displacement as a function of the applied voltage
ax2 = nexttile();
hold on;
for i = 1:7
    plot(20*apa300ml_2s{i}.V, 1e6*apa300ml_2s{i}.d, 'DisplayName', sprintf('APA %i', i))
end
hold off;
xlabel('Voltage [V]'); ylabel('Displacement [$\mu m$]')
legend('location', 'southwest', 'FontSize', 8)
xlim([-20, 150]); ylim([-250, 0]);
#+end_src

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

#+name: fig:test_apa_stroke_result
#+caption: Generated voltage across the two piezoelectric stack actuators to estimate the stroke of the APA300ML (left). Measured displacement as a function of the applied voltage (right)
#+RESULTS:
[[file:figs/test_apa_stroke_result.png]]

** Flexible Mode Measurement
SCHEDULED: <2024-03-27 Wed>
<<sec:test_apa_spurious_resonances>>

In this section, the flexible modes of the APA300ML are investigated both experimentally and using a Finite Element Model.

To experimentally estimate these modes, the APA is fixed on one end (see Figure ref:fig:test_apa_meas_setup_torsion).
A Laser Doppler Vibrometer[fn:6] is used to measure the difference of motion between two "red" points (i.e. the torsion of the APA along the vertical direction) and an instrumented hammer[fn:7] is used to excite the flexible modes.
Using this setup, the transfer function from the injected force to the measured rotation can be computed in different conditions and the frequency and mode shapes of the flexible modes can be estimated.

The flexible modes for the same condition (i.e. one mechanical interface of the APA300ML fixed) are estimated using a finite element software and the results are shown in Figure ref:fig:test_apa_mode_shapes.

#+name: fig:test_apa_mode_shapes
#+caption: Spurious resonances - Change this with the updated FEM analysis of the APA300ML
#+attr_latex: :width 0.9\linewidth
[[file:figs/test_apa_mode_shapes.png]]

#+name: fig:test_apa_meas_setup_torsion
#+caption: Measurement setup with a Laser Doppler Vibrometer and one instrumental hammer. Here the $Z$ torsion is measured.
#+attr_latex: :width 0.6\linewidth
[[file:figs/test_apa_meas_setup_torsion.jpg]]

Two other similar measurements are performed to measured the bending of the APA around the $X$ direction and around the $Y$ direction (see Figure ref:fig:test_apa_meas_setup_modes).

#+name: fig:test_apa_meas_setup_modes
#+caption: Experimental setup to measured flexible modes of the APA300ML. For the bending in the $X$ direction, the impact point is located at the back of the top measurement point. For the bending in the $Y$ direction, the impact point is located on the back surface of the top interface (on the back of the 2 measurements points).
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_apa_meas_setup_X_bending}$X$ bending}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_apa_meas_setup_X_bending.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_apa_meas_setup_Y_bending}$Y$ Bending}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
[[file:figs/test_apa_meas_setup_Y_bending.jpg]]
#+end_subfigure
#+end_figure

#+begin_src matlab
%% X-Bending Identification
% Load Data
bending_X = load('apa300ml_bending_X_top.mat');

% Spectral Analysis setup
Ts = bending_X.Track1_X_Resolution; % Sampling Time [s]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

% Compute the transfer function from applied force to measured rotation
[G_bending_X, f]  = tfestimate(bending_X.Track1, bending_X.Track2, win, Noverlap, Nfft, 1/Ts);

%% Y-Bending identification
% Load Data
bending_Y = load('apa300ml_bending_Y_top.mat');

% Compute the transfer function
[G_bending_Y, ~]  = tfestimate(bending_Y.Track1, bending_Y.Track2, win, Noverlap, Nfft, 1/Ts);

%% Z-Torsion identification
% Load data
torsion = load('apa300ml_torsion_top.mat');

% Compute transfer function
[G_torsion_top, ~]  = tfestimate(torsion.Track1, torsion.Track2, win, Noverlap, Nfft, 1/Ts);

% Load Data
torsion = load('apa300ml_torsion_left.mat');

% Compute transfer function
[G_torsion, ~]  = tfestimate(torsion.Track1, torsion.Track2, win, Noverlap, Nfft, 1/Ts);
#+end_src

The three measured frequency response functions are shown in Figure ref:fig:test_apa_meas_freq_compare.
- a clear $x$ bending mode at $280\,\text{Hz}$
- a clear $y$ bending mode at $412\,\text{Hz}$
- for the $z$ torsion test, the $y$ bending mode is also excited and observed, and we may see a mode at $800\,\text{Hz}$

#+begin_src matlab :exports none
figure;
hold on;
plot(f, abs(G_bending_X), 'DisplayName', '$X$ bending');
plot(f, abs(G_bending_Y), 'DisplayName', '$Y$ bending');
plot(f, abs(G_torsion),   'DisplayName', '$Z$ torsion');
text(280, 5.5e-2,{'280Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(412, 1.5e-2,{'412Hz'},'VerticalAlignment','bottom','HorizontalAlignment','center')
text(800, 6e-4,{'800Hz'}, 'VerticalAlignment', 'bottom','HorizontalAlignment','center')
hold off;
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude');
xlim([50, 2e3]); ylim([5e-5, 2e-1]);
legend('location', 'northeast', 'FontSize', 8)
#+end_src

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

#+name: fig:test_apa_meas_freq_compare
#+caption: Obtained frequency response functions for the 3 tests with the instrumented hammer
#+RESULTS:
[[file:figs/test_apa_meas_freq_compare.png]]

#+name: tab:test_apa_measured_modes_freq
#+caption: Measured frequency of the modes
#+attr_latex: :environment tabularx :width 0.5\linewidth :align Xcc
#+attr_latex: :center t :booktabs t
| *Mode*      | *FEM* | *Measured Frequency* |
|-------------+-------+----------------------|
| $X$ bending |       | 280Hz                |
| $Y$ bending |       | 410Hz                |
| $Z$ torsion |       | 800Hz                |

** Conclusion                                                        :ignore:

* Dynamical measurements
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_2_dynamics.m
:END:
<<sec:test_apa_dynamics>>

** Introduction                                                      :ignore:
After the basic measurements on the APA were performed in Section ref:sec:test_apa_basic_meas, a new test bench is used to better characterize the APA.

This test bench is shown in Figure ref:fig:test_bench_apa and consists of the APA300ML fixed on one end to the fixed granite, and on the other end to the 5kg granite vertically guided with an air bearing.
An encoder is used to measure the relative motion between the two granites (i.e. the displacement of the APA).

#+name: fig:test_bench_apa
#+caption: Test bench used to characterize the APA300ML
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_apa_bench_picture}Picture of the test bench}
#+attr_latex: :options {0.3\textwidth}
#+begin_subfigure
#+attr_latex: :height 8cm
[[file:figs/test_apa_bench_picture.jpg]]
#+end_subfigure
#+attr_latex: :caption \subcaption{\label{fig:test_apa_bench_picture_encoder}Zoom on the APA with the encoder}
#+attr_latex: :options {0.69\textwidth}
#+begin_subfigure
#+attr_latex: :height 8cm
[[file:figs/test_apa_bench_picture_encoder.jpg]]
#+end_subfigure
#+end_figure

The bench is schematically shown in Figure ref:fig:test_apa_schematic and the signal used are summarized in Table ref:tab:test_apa_variables.

#+name: fig:test_apa_schematic
#+caption: Schematic of the Test Bench
#+attr_latex: :scale 1
[[file:figs/test_apa_schematic.png]]

#+name: tab:test_apa_variables
#+caption: Variables used during the measurements
#+attr_latex: :environment tabularx :width 0.6\linewidth :align cXc
#+attr_latex: :center t :booktabs t
| *Variable* | *Description*                | *Unit* |
|------------+------------------------------+--------|
| $u$        | Output DAC Voltage           | $V$    |
| $V_a$      | Output Amplifier Voltage     | $V$    |
| $V_s$      | Measured Stack Voltage (ADC) | $V$    |
| $d_e$      | Encoder Measurement          | $m$    |

This bench will be used to:
- measure the dynamics of the APA (from $V_a$ to $d_e$ and $d_a$ in Section ref:ssec:test_apa_meas_frf_disp, and from $V_a$ to $V_s$ in section ref:ssec:test_apa_meas_frf_force)
- estimate the added damping using Integral Force Feedback (Section ref:ssec:test_apa_iff_locus)

These measurements will also be used to tune the model of the APA in Section ref:sec:test_apa_simscape.

** 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 :tangle no :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

** Hysteresis
<<ssec:test_apa_hysteresis>>

As the payload is vertically guided without friction, the hysteresis of the APA can be estimated from the motion of the payload.

A quasi static sinusoidal excitation $V_a$ with an offset of $65\,V$ (halfway between $-20\,V$ and $150\,V$), and an amplitude varying from $4\,V$ up to $80\,V$.

For each excitation amplitude, the vertical displacement $d_e$ of the mass is measured and displayed as a function of the applied voltage..

#+begin_src matlab
%% Load measured data - hysteresis
apa_hyst = load('frf_data_1_hysteresis.mat', 't', 'u', 'de');

% Initial time set to zero
apa_hyst.t = apa_hyst.t - apa_hyst.t(1);

ampls = [0.1, 0.2, 0.4, 1, 2, 4]; % Excitation voltage amplitudes
#+end_src

The measured displacements as a function of the output voltages are shown in Figure ref:fig:test_apa_meas_hysteresis.
It is interesting to see that the hysteresis is increasing with the excitation amplitude.

#+begin_src matlab :exports none
%% Measured displacement as a function of the output voltage
figure;

hold on;
for i = [6,5,4,2]
    i_lim = apa_hyst.t > i*5-1 & apa_hyst.t < i*5;
    plot(20*apa_hyst.u(i_lim), 1e6*detrend(apa_hyst.de(i_lim), 0), ...
         'DisplayName', sprintf('$V_a = 65 + %.0f \\sin (\\omega t) \\ [V]$', 20*ampls(i)))
end
hold off;
xlabel('Stack Voltage $V_a$ [V]'); ylabel('Displacement $d_e$ [$\mu$m]');
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
xlim([-20, 150]);
ylim([-120, 120]);
#+end_src

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

#+name: fig:test_apa_meas_hysteresis
#+caption: Obtained hysteresis curves (displacement as a function of applied voltage) for multiple excitation amplitudes
#+RESULTS:
[[file:figs/test_apa_meas_hysteresis.png]]

** Axial stiffness
<<ssec:test_apa_stiffness>>

In order to estimate the stiffness of the APA, a weight with known mass $m_a = 6.4\,\text{kg}$ is added on top of the suspended granite and the deflection $d_e$ is measured using the encoder.

The APA stiffness can then be estimated from equation eqref:eq:test_apa_stiffness.

\begin{equation} \label{eq:test_apa_stiffness}
  k_{\text{apa}} = \frac{m_a g}{\Delta d_e}
\end{equation}

#+begin_src matlab
%% Load data for stiffness measurement
apa_nums = [1 2 4 5 6 8];
apa_mass = {};
for i = 1:length(apa_nums)
    apa_mass(i) = {load(sprintf('frf_data_%i_add_mass_closed_circuit.mat', apa_nums(i)), 't', 'de')};
    % The initial displacement is set to zero
    apa_mass{i}.de = apa_mass{i}.de - mean(apa_mass{i}.de(apa_mass{i}.t<11));
end

added_mass = 6.4; % Added mass [kg]
#+end_src

The measured displacement $d_e$ as a function of time is shown in Figure ref:fig:test_apa_meas_stiffness_time.
It can be seen that there are some drifts in the measured displacement (probably due to piezoelectric creep) and the that displacement does not come back to the initial position after the mass is removed (probably due to piezoelectric hysteresis).
These two effects induce some uncertainties in the measured stiffness.

#+begin_src matlab :exports none
%% Plot the deflection at a function of time
figure;
hold on;
plot(apa_mass{2}.t(1:100:end)-apa_mass{2}.t(1), 1e6*apa_mass{2}.de(1:100:end), 'k-');
plot([0,20], [-0.4, -0.4], 'k--', 'LineWidth', 0.5)
plot([0,20], [-4.5, -4.5], 'k--', 'LineWidth', 0.5)
plot([0,20], [-37.4, -37.4], 'k--', 'LineWidth', 0.5)
% first stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [2, -0.4, 0, -37];
text(2.5, -20, sprintf('$d_1$'), 'horizontalalignment', 'left');

% second stroke for stiffness measurements
anArrow = annotation('doublearrow', 'LineWidth', 0.5);
anArrow.Parent = gca;
anArrow.Position = [18, -37.4, 0, 32.9];
text(18.5, -20, sprintf('$d_2$'), 'horizontalalignment', 'left');

% annotation('textarrow',[],y,'String',' Growth ','FontSize',13,'Linewidth',2)
hold off;
xlabel('Time [s]'); ylabel('Displacement $d_e$ [$\mu$m]');
#+end_src

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

#+name: fig:test_apa_meas_stiffness_time
#+caption: Measured displacement when adding the mass (at $t \approx 3\,s$) and removing the mass(at $t \approx 13\,s$)
#+RESULTS:
[[file:figs/test_apa_meas_stiffness_time.png]]

The stiffnesses are computed for all the APA from the two displacements $d_1$ and $d_2$ (see Figure ref:fig:test_apa_meas_stiffness_time) leading to two stiffness estimations $k_1$ and $k_2$.
These estimated stiffnesses are summarized in Table ref:tab:test_apa_measured_stiffnesses and are found to be close to the nominal stiffness $k = 1.8\,N/\mu m$ found in the APA300ML manual.

#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable(1e-6*apa_k, cellstr(num2str(apa_nums')), {'APA', '$k_1$', '$k_2$'}, ' %.2f ');
#+end_src

#+name: tab:test_apa_measured_stiffnesses
#+caption: Measured stiffnesses (in $N/\mu m$)
#+attr_latex: :environment tabularx :width 0.2\linewidth :align ccc
#+attr_latex: :center t :booktabs t :float t
#+RESULTS:
| APA | $k_1$ | $k_2$ |
|-----+-------+-------|
|   1 |  1.68 |   1.9 |
|   2 |  1.69 |   1.9 |
|   4 |   1.7 |  1.91 |
|   5 |   1.7 |  1.93 |
|   6 |   1.7 |  1.92 |
|   8 |  1.73 |  1.98 |

The stiffness can also be computed using equation eqref:eq:test_apa_res_freq by knowing the main vertical resonance frequency $\omega_z \approx 95\,\text{Hz}$ (estimated by the dynamical measurements shown in section ref:ssec:test_apa_meas_dynamics) and the suspended mass $m_{\text{sus}} = 5.7\,\text{kg}$.

\begin{equation} \label{eq:test_apa_res_freq}
\omega_z = \sqrt{\frac{k}{m_{\text{sus}}}}
\end{equation}

The obtain stiffness is $k \approx 2\,N/\mu m$ which is close to the values found in the documentation and by the "static deflection" method.


However, changes in the electrical impedance connected to the piezoelectric stacks impacts the mechanical compliance (or stiffness) of the piezoelectric stack [[cite:&reza06_piezoel_trans_vibrat_contr_dampin chap. 2]].

To estimate this effect, the stiffness of the APA if measured using the "static deflection" method in two cases:
- $k_{\text{os}}$: piezoelectric stacks left unconnected (or connect to the high impedance ADC)
- $k_{\text{sc}}$: piezoelectric stacks short circuited (or connected to the voltage amplifier with small output impedance)

The open-circuit stiffness is estimated at $k_{\text{oc}} \approx 2.3\,N/\mu m$ and the closed-circuit stiffness $k_{\text{sc}} \approx 1.7\,N/\mu m$.

#+begin_src matlab
%% Load Data
add_mass_oc = load('frf_data_1_add_mass_open_circuit.mat',   't', 'de');
add_mass_cc = load('frf_data_1_add_mass_closed_circuit.mat', 't', 'de');

%% Zero displacement at initial time
add_mass_oc.de = add_mass_oc.de - mean(add_mass_oc.de(add_mass_oc.t<11));
add_mass_cc.de = add_mass_cc.de - mean(add_mass_cc.de(add_mass_cc.t<11));

%% Estimation of the stiffness in Open Circuit and Closed-Circuit
apa_k_oc = 9.8 * added_mass / (mean(add_mass_oc.de(add_mass_oc.t > 12 & add_mass_oc.t < 12.5)) - mean(add_mass_oc.de(add_mass_oc.t > 20 & add_mass_oc.t < 20.5)));
apa_k_sc = 9.8 * added_mass / (mean(add_mass_cc.de(add_mass_cc.t > 12 & add_mass_cc.t < 12.5)) - mean(add_mass_cc.de(add_mass_cc.t > 20 & add_mass_cc.t < 20.5)));

%% Estimated coupling factor
sqrt(1 - apa_k_sc/apa_k_oc)
#+end_src

** Dynamics
<<ssec:test_apa_meas_dynamics>>

In this section, the dynamics of the system from the excitation voltage $u$ to encoder measured displacement $d_e$  and to the force sensor voltage $V_s$ is identified.

#+begin_src matlab
%% Identification using sweep sine (low frequency)
load('frf_data_sweep.mat');
load('frf_data_noise_hf.mat');

%% Sampling Frequency
Ts = 1e-4; % Sampling Time [s]
Fs = 1/Ts; % Sampling Frequency [Hz]

%% "Hanning" windows used for the spectral analysis:
Nfft = floor(2/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Separation of frequencies: low freqs using sweep sine, and high freq using noise
% Only used to have the frequency vector "f"
[~, f] = tfestimate(apa_sweep{1}.u, apa_sweep{1}.de, win, Noverlap, Nfft, 1/Ts);
i_lf = f <= 350;
i_hf = f >  350;

%% FRF estimation of the transfer function from u to de
enc_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.u,    apa_sweep{i}.de,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.de, win, Noverlap, Nfft, 1/Ts);
    enc_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end

%% FRF estimation of the transfer function from u to Vs
iff_frf = zeros(length(f), length(apa_nums));
for i = 1:length(apa_nums)
    [frf_lf, ~] = tfestimate(apa_sweep{i}.u,    apa_sweep{i}.Vs,    win, Noverlap, Nfft, 1/Ts);
    [frf_hf, ~] = tfestimate(apa_noise_hf{i}.u, apa_noise_hf{i}.Vs, win, Noverlap, Nfft, 1/Ts);
    iff_frf(:, i) = [frf_lf(i_lf); frf_hf(i_hf)];
end
#+end_src

#+begin_src matlab :tangle no :exports none
%% Save the identified dynamics for further analysis
save('matlab/mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

#+begin_src matlab :eval no
%% Save the identified dynamics for further analysis
save('mat/meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');
#+end_src

The obtained transfer functions for the 6 APA between the excitation voltage $u$ and the encoder displacement $d_e$ are shown in Figure ref:fig:test_apa_frf_encoder.
The obtained transfer functions are close to a mass-spring-damper system.
The following can be observed:
- A "stiffness line" indicating a static gain equal to $\approx -17\,\mu m/V$.
  The minus sign comes from the fact that an increase in voltage stretches the piezoelectric stack that then reduces the height of the APA
- A lightly damped resonance at $95\,\text{Hz}$
- A "mass line" up to $\approx 800\,\text{Hz}$, above which some resonances appear. These additional resonances might be coming from the limited stiffness of the encoder support or from the limited compliance of the APA support.

#+begin_src matlab :exports none
%% Plot the FRF from u to de
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
ylim([1e-8, 1e-3]);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_apa_frf_encoder
#+caption: Estimated Frequency Response Function from generated voltage $u$ to the encoder displacement $d_e$ for the 6 APA300ML
#+RESULTS:
[[file:figs/test_apa_frf_encoder.png]]

The dynamics from $u$ to the measured voltage across the sensor stack $V_s$ is also identified and shown in Figure ref:fig:test_apa_frf_force.

A lightly damped resonance is observed at $95\,\text{Hz}$ and a lightly damped anti-resonance at $41\,\text{Hz}$.
No additional resonances is present up to at least $2\,\text{kHz}$ indicating at Integral Force Feedback can be applied without stability issues from high frequency flexible modes.

As illustrated by the Root Locus, the poles of the closed-loop system converges to the zeros of the open-loop plant.
Suppose that a controller with a very high gain is implemented such that the voltage $V_s$ across the sensor stack is zero.
In that case, because of the very high controller gain, no stress and strain is present on the sensor stack (and on the actuator stacks are well, as they are both in series).
Such closed-loop system would therefore virtually corresponds to a system for which the piezoelectric stacks have been removed and just the mechanical shell is kept.
From this analysis, the axial stiffness of the shell can be estimated to be $k_{\text{shell}} = 5.7 \cdot (2\pi \cdot 41)^2 = 0.38\,N/\mu m$.
# TODO - Compare with FEM result

Such reasoning can lead to very interesting insight into the system just from an open-loop identification.

#+begin_src matlab :exports none
%% Plot the FRF from u to Vs
figure;
tiledlayout(2, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), ...
         'DisplayName', sprintf('APA %i', apa_nums(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

linkaxes([ax1,ax2],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_apa_frf_force
#+caption: Estimated Frequency Response Function from generated voltage $u$ to the sensor stack voltage $V_s$ for the 6 APA300ML
#+RESULTS:
[[file:figs/test_apa_frf_force.png]]

All the identified dynamics of the six APA300ML (both when looking at the encoder in Figure ref:fig:test_apa_frf_encoder and at the force sensor in Figure ref:fig:test_apa_frf_force) are almost identical, indicating good manufacturing repeatability for the piezoelectric stacks and the mechanical lever.

** Effect of the resistor on the IFF Plant
<<ssec:test_apa_resistance_sensor_stack>>

A resistor $R \approx 80.6\,k\Omega$ is added in parallel with the sensor stack which has the effect to form a high pass filter with the capacitance of the stack.

As explain before, this is done for two reasons:
1. Limit the voltage offset due to the input bias current of the ADC
2. Limit the low frequency gain

The (low frequency) transfer function from $u$ to $V_s$ with and without this resistor have been measured and are compared in Figure ref:fig:test_apa_effect_resistance.
It is confirmed that the added resistor as the effect of adding an high pass filter with a cut-off frequency of $\approx 0.35\,\text{Hz}$.

#+begin_src matlab
%% Load the data
wi_k = load('frf_data_1_sweep_lf_with_R.mat', 't', 'Vs', 'u'); % With the resistor
wo_k = load('frf_data_1_sweep_lf.mat',        't', 'Vs', 'u'); % Without the resistor

%% Large Hanning window for good low frequency estimate
Nfft = floor(50/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer functions from u to Vs
[frf_wo_k, f] = tfestimate(wo_k.u, wo_k.Vs, win, Noverlap, Nfft, 1/Ts);
[frf_wi_k, ~] = tfestimate(wi_k.u, wi_k.Vs, win, Noverlap, Nfft, 1/Ts);

%% Model for the high pass filter
C = 5.1e-6; % Sensor Stack capacitance [F]
R = 80.6e3; % Parallel Resistor [Ohm]

f0 = 1/(2*pi*R*C); % Crossover frequency of RC HPF [Hz]

G_hpf = 0.6*(s/2*pi*f0)/(1 + s/2*pi*f0);
#+end_src

#+begin_src matlab :exports none
%% Compare the HPF model and the measured FRF
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(frf_wo_k), 'DisplayName', 'Without $R$');
plot(f, abs(frf_wi_k), 'DisplayName', 'With $R$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-1, 1e0]);
legend('location', 'southeast')

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(frf_wo_k));
plot(f, 180/pi*angle(frf_wi_k));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360); ylim([-45, 90]);

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

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

#+name: fig:test_apa_effect_resistance
#+caption: Transfer function from u to $V_s$ with and without the resistor $R$ in parallel with the piezoelectric stack used as the force sensor
#+RESULTS:
[[file:figs/test_apa_effect_resistance.png]]

** Integral Force Feedback
<<ssec:test_apa_iff_locus>>

This test bench can also be used to estimate the damping added by the implementation of an Integral Force Feedback strategy.

#+begin_src matlab
%% Load identification Data
data = load("2023-03-17_11-28_iff_plant.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(5/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Compute the transfer function from applied force to measured rotation
[G_iff, f]  = tfestimate(data.id_plant, data.Vs, win, Noverlap, Nfft, 1/Ts);
#+end_src

First, the transfer function eqref:eq:test_apa_iff_manual_fit is manually tuned to match the identified dynamics from generated voltage $u$ to the measured sensor stack voltage $V_s$ in Section ref:ssec:test_apa_meas_dynamics.

The obtained parameter values are $\omega_{\textsc{hpf}} = 0.4\, \text{Hz}$, $\omega_{z} = 42.7\, \text{Hz}$, $\xi_{z} = 0.4\,\%$, $\omega_{p} = 95.2\, \text{Hz}$, $\xi_{p} = 2\,\%$ and $g_0 = 0.64$.

\begin{equation} \label{eq:test_apa_iff_manual_fit}
G_{\textsc{iff},m}(s) = g_0 \cdot \frac{1 + 2 \xi_z \frac{s}{\omega_z} + \frac{s^2}{\omega_z^2}}{1 + 2 \xi_p \frac{s}{\omega_p} + \frac{s^2}{\omega_p^2}} \cdot \frac{s}{\omega_{\textsc{hpf}} + s}
\end{equation}

The comparison between the identified plant and the manually tuned transfer function is done in Figure ref:fig:test_apa_iff_plant_comp_manual_fit.

#+begin_src matlab
%% Basic manually tuned model
w0z = 2*pi*42.7; % Zero frequency
xiz = 0.004; % Zero damping

w0p = 2*pi*95.2; % Pole frequency
xip = 0.02; % Pole damping

G_iff_model = exp(-2*s*Ts)*0.64*(1 + 2*xiz/w0z*s + s^2/w0z^2)/(1 + 2*xip/w0p*s + s^2/w0p^2)*(s/(s+2*pi*0.4));
#+end_src

#+begin_src matlab :exports none :results none
%% Identified IFF plant and manually tuned model of the plant
figure;
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(G_iff), 'color', colors(2,:), 'DisplayName', 'Identified plant')
plot(f, abs(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--', 'DisplayName', 'Manual fit')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
plot(f, 180/pi*angle(G_iff), 'color', colors(2,:));
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_model, f, 'Hz'))), 'k--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:45:360);
ylim([-90, 180])

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

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

#+name: fig:test_apa_iff_plant_comp_manual_fit
#+caption: Identified IFF plant and manually tuned model of the plant (a time delay of $200\,\mu s$ is added to the model of the plant to better match the identified phase)
#+RESULTS:
[[file:figs/test_apa_iff_plant_comp_manual_fit.png]]

The implemented Integral Force Feedback Controller transfer function is shown in equation eqref:eq:test_apa_Kiff_formula.
It contains an high pass filter (cut-off frequency of $2\,\text{Hz}$) to limit the low frequency gain, a low pass filter to add integral action above $20\,\text{Hz}$, a second low pass filter to add robustness to high frequency resonances and a tunable gain $g$.

\begin{equation} \label{eq:test_apa_Kiff_formula}
K_{\textsc{iff}}(s) = -10 \cdot g \cdot \frac{s}{s + 2\pi \cdot 2} \cdot \frac{1}{1 + 2\pi \cdot 20} \cdot \frac{1}{s + 2\pi\cdot 2000}
\end{equation}

#+begin_src matlab
%% Integral Force Feedback Controller
K_iff = -10*(1/(s + 2*pi*20)) * ... % LPF: provides integral action above 20Hz
            (s/(s + 2*pi*2))  * ... % HPF: limit low frequency gain
            (1/(1 + s/2/pi/2e3));   % LPF: more robust to high frequency resonances
#+end_src

To estimate how the dynamics of the APA changes when the Integral Force Feedback controller is implemented, the test bench shown in Figure ref:fig:test_apa_iff_schematic is used.
The transfer function from the "damped" plant input $u\prime$ to the encoder displacement $d_e$ is identified for several IFF controller gains $g$.

#+name: fig:test_apa_iff_schematic
#+caption: Figure caption
[[file:figs/test_apa_iff_schematic.png]]

#+begin_src matlab
%% Load Data
data = load("2023-03-17_14-10_damped_plants_new.mat");

%% Spectral Analysis setup
Ts = 1e-4; % Sampling Time [s]
Nfft = floor(1/Ts);
win = hanning(Nfft);
Noverlap = floor(Nfft/2);

%% Get the frequency vector
[~, f]  = tfestimate(data.data(1).id_plant(1:end), data.data(1).dL(1:end), win, Noverlap, Nfft, 1/Ts);

%% Gains used for analysis are between 1 and 50
i_kept = [5:10]

%% Identify the damped plant from u' to de for different IFF gains
G_dL_frf = {zeros(1,length(i_kept))};

for i = 1:length(i_kept)
    [G_dL, ~]  = tfestimate(data.data(i_kept(i)).id_plant(1:end), data.data(i_kept(i)).dL(1:end), win, Noverlap, Nfft, 1/Ts);
    G_dL_frf(i) = {G_dL};
end
#+end_src

The identified dynamics are then fitted by second order transfer functions.
The comparison between the identified damped dynamics and the fitted second order transfer functions is done in Figure ref:fig:test_apa_identified_damped_plants for different gains $g$.
It is clear that large amount of damping is added when the gain is increased and that the frequency of the pole is shifted to lower frequencies.

#+begin_src matlab
%% Fit the data with 2nd order transfer function using vectfit3
opts = struct();

opts.stable = 1;    % Enforce stable poles
opts.asymp = 1;     % Force D matrix to be null
opts.relax = 1;     % Use vector fitting with relaxed non-triviality constraint
opts.skip_pole = 0; % Do NOT skip pole identification
opts.skip_res = 0;  % Do NOT skip identification of residues (C,D,E)
opts.cmplx_ss = 0;  % Create real state space model with block diagonal A

opts.spy1 = 0;      % No plotting for first stage of vector fitting
opts.spy2 = 0;      % Create magnitude plot for fitting of f(s)


Niter = 100; % Number of iteration.
N = 2; % Order of approximation
poles = [-25 - 1i*60, -25 + 1i*60]; % First get for the pole location

G_dL_id  = {zeros(1,length(i_kept))};

% Identification just between two frequencies
f_keep = (f>20 & f<200);

for i = 1:length(i_kept)
    %% Estimate resonance frequency and damping
    for iter = 1:Niter
        [G_est, poles, ~, frf_est] = vectfit3(G_dL_frf{i}(f_keep).', 1i*2*pi*f(f_keep)', poles, ones(size(f(f_keep)))', opts);
    end
    G_dL_id(i) = {ss(G_est.A, G_est.B, G_est.C, G_est.D)};
end
#+end_src

#+begin_src matlab :exports none :results none
%% Identified dynamics from u' to de for different IFF gains
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile();
hold on;
for i = 1:length(i_kept)
    plot(f, abs(G_dL_frf{i}), 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %.0f', data.gains(i_kept(i))))
    plot(f, abs(squeeze(freqresp(G_dL_id{i}, f, 'Hz'))), '--', 'color', [colors(i,:), 1], 'HandleVisibility', 'off')
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude $d_L/V_a$ [m/V]');
xlim([10, 1e3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
#+end_src

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

#+name: fig:test_apa_identified_damped_plants
#+caption: Identified dynamics (solid lines) and fitted transfer functions (dashed lines) from $u\prime$ to $d_e$ for different IFF gains
#+RESULTS:
[[file:figs/test_apa_identified_damped_plants.png]]

The evolution of the pole in the complex plane as a function of the controller gain $g$ (i.e. the "root locus") is computed:
- using the IFF plant model eqref:eq:test_apa_iff_manual_fit and the implemented controller eqref:eq:test_apa_Kiff_formula
- from the fitted transfer functions of the damped plants experimentally identified for several controller gains

The two obtained root loci are compared in Figure ref:fig:test_apa_iff_root_locus and are in good agreement considering that the damped plants were only fitted using a second order transfer function.

#+begin_src matlab :exports none :results none
%% Root Locus of the APA300ML with Integral Force Feedback
% Comparison between the computed root locus from the plant model and the root locus estimated from the damped plant pole identification
figure;
gains = logspace(-1, 3, 1000);

figure;
hold on;
G_iff_poles = pole(G_iff_model);
i = imag(G_iff_poles) > 100; % Only keep relevant poles
plot(real(G_iff_poles(i)),  imag(G_iff_poles(i)),  'kx', ...
    'DisplayName', '$g = 0$');
G_iff_zeros = tzero(G_iff_model);
i = imag(G_iff_zeros) > 100; % Only keep relevant zeros
plot(real(G_iff_zeros(i)), imag(G_iff_zeros(i)), 'ko', ...
    'HandleVisibility', 'off');

for g = gains
    clpoles = pole(feedback(G_iff_model, g*K_iff, 1));
    i = imag(clpoles) > 100; % Only keep relevant poles
    plot(real(clpoles(i)), imag(clpoles(i)), 'k.', ...
        'HandleVisibility', 'off');
end

for i = 1:length(i_kept)
    plot(real(pole(G_dL_id{i})), imag(pole(G_dL_id{i})), 'x', 'color', [colors(i,:), 1], 'DisplayName', sprintf('g = %1.f', data.gains(i_kept(i))));
end
ylim([0, 700]);
xlim([-600,100]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
legend('location', 'northwest');
#+end_src

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

#+name: fig:test_apa_iff_root_locus
#+caption: Root Locus of the APA300ML with Integral Force Feedback - Comparison between the computed root locus from the plant model (black line) and the root locus estimated from the damped plant pole identification (colorful crosses)
#+RESULTS:
[[file:figs/test_apa_iff_root_locus.png]]

** Conclusion                                                        :ignore:

#+begin_important
So far, all the measured FRF are showing the dynamical behavior that was expected.
#+end_important

* 2 Degrees of Freedom Model
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_3_model_2dof.m
:END:
<<sec:test_apa_model_2dof>>

** Introduction                                                      :ignore:
In this section, a simscape model (Figure ref:fig:test_apa_bench_model) of the measurement bench is used to compare the model of the APA with the measured frequency response functions.

After the transfer functions are extracted from the model (Section ref:sec:simscape_bench_apa_first_id), the comparison of the obtained dynamics with the measured FRF will permit to:
1. Estimate the "actuator constant" and "sensor constant" (Section ref:sec:simscape_bench_apa_id_constants)
- "Actuator constant": Gain from the applied voltage $V_a$ to the generated Force $F_a$
- "Sensor constant": Gain from the sensor stack strain $\delta L$ to the generated voltage $V_s$
2. Tune the model of the APA to match the measured dynamics (Section ref:sec:simscape_bench_apa_tune_2dof_model)

#+name: fig:test_apa_bench_model
#+caption: Screenshot of the Simscape model
#+attr_latex: :width 0.8\linewidth
[[file:figs/test_apa_bench_model.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 :tangle no :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :tangle no :noweb yes
<<m-init-path-simscape>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-simscape-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

#+begin_src matlab :exports none
%% Input/Output definition of the Model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % DAC Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
#+end_src

** Two Degrees of Freedom APA Model
<<ssec:test_apa_2dof_model>>

The APA model shown in Figure ref:fig:test_apa_2dof_model is adapted from cite:souleille18_concep_activ_mount_space_applic.

It can be decomposed into three components:
- the shell whose axial properties are represented by $k_1$ and $c_1$
- the actuator stacks whose contribution in the axial stiffness is represented by $k_a$ and $c_a$.
  A force source $\tau$ represents the axial force induced by the force sensor stacks.
  The gain $g_a$ (in $N/m$) is used to convert the applied voltage $V_a$ to the axial force $\tau$
- the actuator stacks whose contribution in the axial stiffness is represented by $k_e$ and $c_e$.
  A "strain sensor" $d_L$, and a gain $g_s$ (in $V/m$) that converts this strain into a generated voltage

Such simple model has some limitations:
- it only represents the axial characteristics of the APA (infinitely rigid in other directions)
- some physical insights are lost such as the amplification factor, the real stress and strain on the piezoelectric stacks
- it is fully linear and therefore the creep and hysteresis of the piezoelectric stacks are not modelled

#+name: fig:test_apa_2dof_model
#+caption: Schematic of the two degrees of freedom model of the APA300ML
[[file:figs/test_apa_2dof_model.png]]

** Tuning of the APA model
<<ssec:test_apa_2dof_model_tuning>>

9 parameters ($m$, $k_1$, $c_1$, $k_e$, $c_e$, $k_a$, $c_a$, $g_s$ and $g_a$) have to be tuned such that the dynamics of the model (Figure ref:fig:test_apa_2dof_model_simscape) well represents the identified dynamics in Section ref:sec:test_apa_dynamics.

#+name: fig:test_apa_2dof_model_simscape
#+caption: Schematic of the two degrees of freedom model of the APA300ML with input $V_a$ and outputs $d_e$ and $V_s$
[[file:figs/test_apa_2dof_model_simscape.png]]

#+begin_src matlab
%% Stiffness values for the 2DoF APA model
k1 = 0.38e6; % Estimated Shell Stiffness [N/m]

w0 = 2*pi*95; % Resonance frequency [rad/s]
m = 5.7; % Suspended mass [kg]
ktot = m*(w0)^2; % Total Axial Stiffness to have to wanted resonance frequency [N/m]

ka = 1.5*(ktot-k1); % Stiffness of the (two) actuator stacks [N/m]
ke = 2*ka; % Stiffness of the Sensor stack [N/m]

%% Damping values for the 2DoF APA model
c1 = 20;   % Damping for the Shell [N/(m/s)]
ca = 100;  % Damping of the actuators stacks [N/(m/s)]
ce = 2*ca; % Damping of the sensor stack [N/(m/s)]
#+end_src

#+begin_src matlab
%% Estimation ot the sensor and actuator gains
% Initialize the structure with unitary sensor and actuator "gains"
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'k',  k1,  ...
    'ka', ka,  ...
    'ke', ke,  ...
    'c',  c1, ...
    'ca', ca, ...
    'ce', ce, ...
    'Ga', 1,   ... % Actuator constant [N/V]
    'Gs', 1    ... % Sensor constant [V/m]
);

c_granite = 0; % Do not take into account damping added by the air bearing

% Run the linearization
G_norm = linearize(mdl, io, 0.0, opts);
G_norm.InputName  = {'u'};
G_norm.OutputName = {'Vs', 'de'};

% Load Identification Data to estimate the two gains
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');

% Estimation ot the Actuator Gain
fa = 10; % Frequency where the two FRF should match [Hz]
[~, i_f] = min(abs(f - fa));
ga = -abs(enc_frf(i_f,1))./abs(evalfr(G_norm('de', 'u'), 1i*2*pi*fa));

% Estimation ot the Sensor Gain
fs = 600; % Frequency where the two FRF should match [Hz]
[~, i_f] = min(abs(f - fs))
gs = -abs(iff_frf(i_f,1))./abs(evalfr(G_norm('Vs', 'u'), 1i*2*pi*fs))/ga;
#+end_src

First, the mass supported by the APA300ML can simply be estimated from the geometry and density of the different parts or by directly measuring it using a precise weighing scale.
Both methods leads to an estimated mass of $5.7\,\text{kg}$.

Then, the axial stiffness of the shell was estimated at $k_1 = 0.38\,N/\mu m$ in Section ref:ssec:test_apa_meas_dynamics from the frequency of the anti-resonance seen on Figure ref:fig:test_apa_frf_force.
Similarly, $c_1$ can be estimated from the damping ratio of the same anti-resonance and is found to be close to $20\,Ns/m$.

Then, it is reasonable to make the assumption that the sensor stacks and the two actuator stacks have identical mechanical characteristics[fn:5].
Therefore, we have $k_e = 2 k_a$ and $c_e = 2 c_a$ as the actuator stack is composed of two stacks in series.
In that case, the total stiffness of the APA model is described by eqref:eq:test_apa_2dof_stiffness.

\begin{equation}\label{eq:test_apa_2dof_stiffness}
k_{\text{tot}} = k_1 + \frac{k_e k_a}{k_e + k_a} = k_1 + \frac{2}{3} k_a
\end{equation}

Knowing from eqref:eq:test_apa_tot_stiffness that the total stiffness is $k_{\text{tot}} = 2\,N/\mu m$, we get from eqref:eq:test_apa_2dof_stiffness that $k_a = 2.5\,N/\mu m$ and $k_e = 5\,N/\mu m$.

\begin{equation}\label{eq:test_apa_tot_stiffness}
\omega_0 = \frac{k_{\text{tot}}}{m} \Longrightarrow k_{\text{tot}} = m \omega_0^2 = 2\,N/\mu m \quad \text{with}\ m = 5.7\,\text{kg}\ \text{and}\ \omega_0 = 2\pi \cdot 95\, \text{rad}/s
\end{equation}

Then, $c_a$ (and therefore $c_e = 2 c_a$) can be tuned to match the damping ratio of the identified resonance.
$c_a = 100\,Ns/m$ and $c_e = 200\,Ns/m$ are obtained.

Finally, the two gains $g_s$ and $g_a$ can be used to match the gain of the identified transfer functions.

The obtained parameters of the model shown in Figure ref:fig:test_apa_2dof_model_simscape are summarized in Table ref:tab:test_apa_2dof_parameters.

#+name: tab:test_apa_2dof_parameters
#+caption: Summary of the obtained parameters for the 2 DoF APA300ML model
#+attr_latex: :environment tabularx :width 0.3\linewidth :align cc
#+attr_latex: :center t :booktabs t
| *Parameter* | *Value*          |
|-------------+------------------|
| $m$         | $5.7\,\text{kg}$ |
| $k_1$       | $0.38\,N/\mu m$  |
| $k_e$       | $5.0\, N/\mu m$  |
| $k_a$       | $2.5\,N/\mu m$   |
| $c_1$       | $20\,Ns/m$       |
| $c_e$       | $200\,Ns/m$      |
| $c_a$       | $100\,Ns/m$      |
| $g_a$       | $-2.58\,N/V$     |
| $g_s$       | $4.6\,V/\mu m$   |

** Obtained Dynamics
<<ssec:test_apa_2dof_model_result>>

The dynamics of the 2DoF APA300ML model is now extracted using optimized parameters (listed in Table ref:tab:test_apa_2dof_parameters) from the Simscape model.
It is compared with the experimental data in Figure ref:fig:test_apa_2dof_comp_frf.

A good match can be observed between the model and the experimental data, both for the encoder and for the force sensor.
This indicates that this model represents well the axial dynamics of the APA300ML.

#+begin_src matlab
%% 2DoF APA300ML with optimized parameters
n_hexapod = struct();
n_hexapod.actuator = initializeAPA(...
    'type', '2dof', ...
    'k',  k1,  ...
    'ka', ka,  ...
    'ke', ke,  ...
    'c',  c1, ...
    'ca', ca, ...
    'ce', ce, ...
    'Ga', ga, ...
    'Gs', gs  ...
);

%% Identification of the APA300ML with optimized parameters
G_2dof = exp(-s*Ts)*linearize(mdl, io, 0.0, opts);
G_2dof.InputName  = {'u'};
G_2dof.OutputName = {'Vs', 'de'};
#+end_src

#+begin_src matlab :exports none
%% Comparison of the measured FRF and the optimized 2DoF model of the APA300ML
freqs = 5*logspace(0, 3, 1000);
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_2dof('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax1b = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
for i = 2:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_2dof('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '2DoF Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_2dof('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

ax2b = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_2dof('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

linkaxes([ax1,ax2,ax1b,ax2b],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_apa_2dof_comp_frf
#+caption: Comparison of the measured FRF and the optimized 2DoF model of the APA300ML
#+RESULTS:
[[file:figs/test_apa_2dof_comp_frf.png]]

** Conclusion                                                        :ignore:

* APA300ML - Super Element
:PROPERTIES:
:header-args:matlab+: :tangle matlab/test_apa_4_model_flexible.m
:END:
<<sec:test_apa_model_flexible>>

** Introduction                                                      :ignore:
In this section, a /super element/ of the Amplified Piezoelectric Actuator "APA300ML" is extracted using a Finite Element Software.
It is then imported in Simscape (using the stiffness and mass matrices) and it is included in the same model that was used in ref:sec:test_apa_model_2dof.

This procedure is illustrated in Figure ref:fig:test_apa_super_element_simscape.

#+name: fig:test_apa_super_element_simscape
#+caption: Finite Element Model of the APA300ML with "remotes points" on the left. Simscape model with included "Reduced Order Flexible Solid" on the right.
#+attr_latex: :width 1.0\linewidth
[[file:figs/test_apa_super_element_simscape.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 :tangle no :noweb yes
<<m-init-path>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle>>
#+end_src

#+begin_src matlab :tangle no :noweb yes
<<m-init-path-simscape>>
#+end_src

#+begin_src matlab :eval no :noweb yes
<<m-init-path-simscape-tangle>>
#+end_src

#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src

#+begin_src matlab :exports none
%% Input/Output definition of the Model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/u'],  1, 'openinput');  io_i = io_i + 1; % DAC Voltage
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
#+end_src

** Extraction of the super-element

- Explain how the "remote points" are chosen
- Show some parts of the mass and stiffness matrices?
- Say which materials were used?
- Maybe this was already explain earlier in the manuscript

** Identification of the Actuator and Sensor constants
<<ssec:test_apa_flexible_ga_gs>>

Once the APA300ML /super element/ is included in the Simscape model, the transfer function from $F_a$ to $d_L$ and $d_e$ can be identified.
The gains $g_a$ and $g_s$ can then be tuned such that the gain of the transfer functions are matching the identified ones.

By doing so, $g_s = 4.9\,V/\mu m$ and $g_a = 23.2\,N/V$ are obtained.

#+begin_src matlab
%% Identification of the actuator and sensor "constants"
% Initialize the APA as a flexible body with unity "constants"
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga', 1, ...
    'gs', 1);

c_granite = 100; % Rought estimation of the damping added by the air bearing

% Identify the dynamics
G_norm = linearize(mdl, io, 0.0, opts);
G_norm.InputName  = {'u'};
G_norm.OutputName = {'Vs', 'de'};

% Load Identification Data to estimate the two gains
load('meas_apa_frf.mat', 'f', 'Ts', 'enc_frf', 'iff_frf', 'apa_nums');

% Actuator Constant in [N/V]
ga = -mean(abs(enc_frf(f>10 & f<20)))./dcgain(G_norm('de', 'u'));

% Sensor Constant in [V/m]
gs = -mean(abs(iff_frf(f>400 & f<500)))./(ga*abs(squeeze(freqresp(G_norm('Vs', 'u'), 1e3, 'Hz'))));
#+end_src


To make sure these "gains" are physically valid, it is possible to estimate them from physical properties of the piezoelectric stack material.

From [[cite:&fleming14_desig_model_contr_nanop_system p. 123]], the relation between relative displacement $d_L$ of the sensor stack and generated voltage $V_s$ is given by eqref:eq:test_apa_piezo_strain_to_voltage and from [[cite:&fleming10_integ_strain_force_feedb_high]] the relation between the force $F_a$ and the applied voltage $V_a$ is given by eqref:eq:test_apa_piezo_voltage_to_force.

\begin{subequations}
\begin{align}
  V_s &= \underbrace{\frac{d_{33}}{\epsilon^T s^D n}}_{g_s} d_L \label{eq:test_apa_piezo_strain_to_voltage} \\
  F_a &= \underbrace{d_{33} n k_a}_{g_a} \cdot V_a, \quad k_a = \frac{c^{E} A}{L} \label{eq:test_apa_piezo_voltage_to_force}
\end{align}
\end{subequations}

Parameters used in equations eqref:eq:test_apa_piezo_strain_to_voltage and eqref:eq:test_apa_piezo_voltage_to_force are described in Table ref:tab:test_apa_piezo_properties.

Unfortunately, the manufacturer of the stack was not willing to share the piezoelectric material properties of the stack used in the APA300ML.
However, based on available properties of the APA300ML stacks in the data-sheet, the soft Lead Zirconate Titanate "THP5H" from Thorlabs seemed to match quite well the observed properties.
The properties of this "THP5H" material used to compute $g_a$ and $g_s$ are listed in Table ref:tab:test_apa_piezo_properties.

From these parameters, $g_s = 5.1\,V/\mu m$ and $g_a = 26\,N/V$ were obtained which are very close to the identified constants using the experimentally identified transfer functions.

#+name: tab:test_apa_piezo_properties
#+caption: Piezoelectric properties used for the estimation of the sensor and actuators "gains"
#+attr_latex: :environment tabularx :width 1\linewidth :align ccX
#+attr_latex: :center t :booktabs t
| *Parameter*    | *Value*                    | *Description*                                                |
|----------------+----------------------------+--------------------------------------------------------------|
| $d_{33}$       | $680 \cdot 10^{-12}\,m/V$  | Piezoelectric constant                                       |
| $\epsilon^{T}$ | $4.0 \cdot 10^{-8}\,F/m$   | Permittivity under constant stress                           |
| $s^{D}$        | $21 \cdot 10^{-12}\,m^2/N$ | Elastic compliance understand constant electric displacement |
| $c^{E}$        | $48 \cdot 10^{9}\,N/m^2$   | Young's modulus of elasticity                                |
| $L$            | $20\,mm$ per stack         | Length of the stack                                          |
| $A$            | $10^{-4}\,m^2$             | Area of the piezoelectric stack                              |
| $n$            | $160$ per stack            | Number of layers in the piezoelectric stack                  |

#+begin_src matlab
%% Estimate "Sensor Constant" - (THP5H)
d33 = 680e-12;        % Strain constant [m/V]
n   = 160;            % Number of layers per stack
eT  = 4500*8.854e-12; % Permittivity under constant stress [F/m]
sD  = 21e-12;         % Compliance under constant electric displacement [m2/N]

gs = d33/(eT*sD*n);   % Sensor Constant [V/m]

%% Estimate "Actuator Constant" - (THP5H)
d33 = 680e-12;   % Strain constant [m/V]
n   = 320;       % Number of layers

cE  = 1/sD;      % Youngs modulus [N/m^2]
A   = (10e-3)^2; % Area of the stacks [m^2]
L   = 40e-3;     % Length of the two stacks [m]
ka  = cE*A/L;    % Stiffness of the two stacks [N/m]

ga = d33*n*ka;   % Actuator Constant [N/V]
#+end_src

** Comparison of the obtained dynamics

The obtained dynamics using the /super element/ with the tuned "sensor gain" and "actuator gain" are compared with the experimentally identified frequency response functions in Figure ref:fig:test_apa_super_element_comp_frf.

A good match between the model and the experimental results is observed.
- the /super element/

This model represents fairly

The flexible model is a bit "soft" as compared with the experimental results.

This method can be used to model piezoelectric stack actuators as well as amplified piezoelectric stack actuators.

#+begin_src matlab
%% Idenfify the dynamics of the Simscape model with correct actuator and sensor "constants"
% Initialize the APA
n_hexapod.actuator = initializeAPA(...
    'type', 'flexible', ...
    'ga',   23.2, ... % Actuator gain [N/V]
    'gs',   -4.9e6);    % Sensor gain [V/m]

% Identify with updated constants
G_flex = exp(-Ts*s)*linearize(mdl, io, 0.0, opts);
G_flex.InputName  = {'u'};
G_flex.OutputName = {'Vs', 'de'};
#+end_src

#+begin_src matlab :exports none
%% Comparison of the measured FRF and the "Flexible" model of the APA300ML
freqs = 5*logspace(0, 3, 1000);
figure;
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');

ax1 = nexttile([2,1]);
hold on;
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
for i = 1:length(apa_nums)
    plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_flex('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '"Flexible" Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $d_e/u$ [m/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-8, 1e-3]);
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);

ax1b = nexttile([2,1]);
hold on;
plot(f, abs(iff_frf(:, 1)), 'color', [0,0,0,0.2], 'DisplayName', 'Identified');
for i = 2:length(apa_nums)
    plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], 'HandleVisibility', 'off');
end
plot(freqs, abs(squeeze(freqresp(G_flex('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '"Flexible" Model')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude $V_s/u$ [V/V]'); set(gca, 'XTickLabel',[]);
hold off;
ylim([1e-2, 1e2]);
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);

ax2 = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex('de', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

ax2b = nexttile;
hold on;
for i = 1:length(apa_nums)
    plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
end
plot(freqs, 180/pi*angle(squeeze(freqresp(G_flex('Vs', 'u'), freqs, 'Hz'))), '--', 'color', colors(2,:))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
hold off;
yticks(-360:90:360); ylim([-180, 180]);

linkaxes([ax1,ax2,ax1b,ax2b],'x');
xlim([10, 2e3]);
#+end_src

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

#+name: fig:test_apa_super_element_comp_frf
#+caption: Comparison of the measured FRF and the "Flexible" model of the APA300ML
#+RESULTS:
[[file:figs/test_apa_super_element_comp_frf.png]]

** Conclusion                                                        :ignore:

* Conclusion
<<sec:test_apa_conclusion>>

- Compare 2DoF and FEM models (usefulness of the two)
- Good match between all the APA: will simplify the modeling and control of the nano-hexapod
- No advantage of the FEM model here (as only uniaxial behavior is checked), but may be useful later

* Bibliography                                                        :ignore:
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]

* Helping Functions                                                 :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./matlab/src/'); % Path for scripts
addpath('./matlab/mat/'); % Path for data
addpath('./matlab/');
#+END_SRC

#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./src/'); % Path for scripts
addpath('./mat/'); % Path for data
#+END_SRC

** Initialize Simscape
#+NAME: m-init-path-simscape
#+BEGIN_SRC matlab
addpath('./matlab/STEPS/'); % Path for Simscape Model

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

%% Open Simscape Model
mdl = 'test_apa_simscape'; % Name of the Simulink File
open(mdl); % Open Simscape Model
#+END_SRC

#+NAME: m-init-path-simscape-tangle
#+BEGIN_SRC matlab
addpath('./STEPS/'); % Path for Simscape Model

%% Linearization options
opts = linearizeOptions;
opts.SampleTime = 0;

%% Open Simscape Model
mdl = 'test_apa_simscape'; % Name of the Simulink File
open(mdl); % Open Simscape Model
#+END_SRC

** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
#+END_SRC

** =initializeAPA= - Initialize APA
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeAPA.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeAPA>>

*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
function [actuator] = initializeAPA(args)
% initializeAPA -
%
% Syntax: [actuator] = initializeAPA(args)
%
% Inputs:
%    - args -
%
% Outputs:
%    - actuator -
#+end_src

*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
arguments
    args.type      char   {mustBeMember(args.type,{'2dof', 'flexible frame', 'flexible'})} = '2dof'

    % Actuator and Sensor constants
    args.Ga  (1,1) double {mustBeNumeric} = 0
    args.Gs  (1,1) double {mustBeNumeric} = 0

    % For 2DoF
    args.k   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*0.38e6
    args.ke  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*1.75e6
    args.ka  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e7

    args.c   (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*3e1
    args.ce  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2e1
    args.ca  (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2e1

    args.Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056

    % Force Flexible APA
    args.xi (1,1) double {mustBeNumeric, mustBePositive} = 0.01
    args.d_align (3,1) double {mustBeNumeric} = zeros(3,1) % [m]
    args.d_align_bot (3,1) double {mustBeNumeric} = zeros(3,1) % [m]
    args.d_align_top (3,1) double {mustBeNumeric} = zeros(3,1) % [m]

    % For Flexible Frame
    args.ks (1,1) double {mustBeNumeric, mustBePositive} = 235e6
    args.cs (1,1) double {mustBeNumeric, mustBePositive} = 1e1
end
#+end_src

*** Initialize Structure
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
actuator = struct();
#+end_src

*** Type
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
switch args.type
  case '2dof'
    actuator.type = 1;
  case 'flexible frame'
    actuator.type = 2;
  case 'flexible'
    actuator.type = 3;
end
#+end_src

*** Actuator/Sensor Constants
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
if args.Ga == 0
    switch args.type
      case '2dof'
        actuator.Ga = -30.0;
      case 'flexible frame'
        actuator.Ga = 1; % TODO
      case 'flexible'
        actuator.Ga = 23.4;
    end
else
    actuator.Ga = args.Ga; % Actuator gain [N/V]
end
#+end_src

#+begin_src matlab
if args.Gs == 0
    switch args.type
      case '2dof'
        actuator.Gs = 0.098;
      case 'flexible frame'
        actuator.Gs = 1; % TODO
      case 'flexible'
        actuator.Gs = -4674824;
    end
else
    actuator.Gs = args.Gs; % Sensor gain [V/m]
end
#+end_src

*** 2DoF parameters
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
actuator.k  = args.k;  % [N/m]
actuator.ke = args.ke; % [N/m]
actuator.ka = args.ka; % [N/m]

actuator.c  = args.c;  % [N/(m/s)]
actuator.ce = args.ce; % [N/(m/s)]
actuator.ca = args.ca; % [N/(m/s)]

actuator.Leq = args.Leq; % [m]
#+end_src

*** Flexible frame and fully flexible
:PROPERTIES:
:UNNUMBERED: t
:END:

#+begin_src matlab
switch args.type
  case 'flexible frame'
    actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
    actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
    actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
  case 'flexible'
    actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
    actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
    actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
    actuator.d_align = args.d_align;
    actuator.d_align_bot = args.d_align_bot;
    actuator.d_align_top = args.d_align_top;
end

actuator.xi = args.xi; % Damping ratio

actuator.ks = args.ks; % Stiffness of one stack [N/m]
actuator.cs = args.cs; % Damping of one stack [N/m]
#+end_src

* Footnotes

[fn:7]Kistler 9722A
[fn:6]Polytec controller 3001 with sensor heads OFV512
[fn:5]Note that this is not fully correct as it was shown in Section ref:ssec:test_apa_stiffness that the electrical boundaries of the piezoelectric stack impacts its stiffness and that the sensor stack is almost open-circuited while the actuator stacks are almost short-circuited.
[fn:4]The Matlab =fminsearch= command is used to fit the plane
[fn:3]Heidenhain MT25, specified accuracy of $0.5\,\mu m$
[fn:2]Millimar 1318 probe, specified linearity better than $1\,\mu m$
[fn:1]LCR-819 from Gwinstek, specified accuracy of $0.05\%$, measured frequency is set at $1\,\text{kHz}$
