14747 lines
493 KiB
Org Mode
14747 lines
493 KiB
Org Mode
|
#+TITLE: Nano-Hexapod on the micro-station
|
||
|
|
: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-id31.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-id31.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:
|
||
|
|
|
||
|
|
* Introduction :ignore:
|
||
|
|
|
||
|
|
* Short Stroke Metrology System
|
||
|
|
<<sec:id31_short_stroke_metrology>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
The control of the nano-hexapod requires an external metrology system measuring the relative position of the nano-hexapod top platform with respect to the granite.
|
||
|
|
As the long-stroke ($\approx 1\,cm^3$) metrology system is not developed yet, a stroke stroke ($> 100\,\mu m^3$) can be used instead to validate the nano-hexapod control.
|
||
|
|
|
||
|
|
This short stroke metrology system consists of 5 interferometers pointing at 2 spheres fixed on top of the nano-hexapod (Figure ref:fig:LION_picture_close).
|
||
|
|
|
||
|
|
#+name: fig:LION_picture_close
|
||
|
|
#+caption: Metrology system with LION spheres (1 inch diameter) and 5 interferometers fixed to their individual tip-tilts
|
||
|
|
#+attr_latex: :width 0.5\linewidth
|
||
|
|
[[file:figs/LION_picture_close.jpg]]
|
||
|
|
|
||
|
|
This short stroke metrology system is fixed to the main granite using a gantry made of granite blocs to have good vibration and thermal stability (see Figure ref:fig:short_stroke_metrology_overview).
|
||
|
|
|
||
|
|
#+name: fig:short_stroke_metrology_overview
|
||
|
|
#+caption: Granite gantry used to fix the short-stroke metrology system
|
||
|
|
#+attr_latex: :width \linewidth
|
||
|
|
[[file:figs/short_stroke_metrology_overview.jpg]]
|
||
|
|
|
||
|
|
As the metrology system as limited stroke (estimated to be in the order of hundreds of micro-meters in x-y-z), it has to be well aligned in the rest position.
|
||
|
|
|
||
|
|
The alignment procedure is as follows:
|
||
|
|
1. The granite is aligned to be perpendicular to gravity (using inclinometer and adjusting airlocks)
|
||
|
|
2. The height of micro-hexapod is tuned to be able to position the short stroke metrology without additional shim
|
||
|
|
3. It is verified that the spindle axis is well perpendicular to the granite using the laser tracker
|
||
|
|
4. The micro hexapod is then used to align the two spheres with the spindle axis.
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Kinematics
|
||
|
|
|
||
|
|
#+name: fig:LION_metrology_interferometers
|
||
|
|
#+caption: Schematic of the measurement system
|
||
|
|
#+attr_latex: :width 0.5\linewidth
|
||
|
|
[[file:figs/LION_metrology_interferometers.png]]
|
||
|
|
|
||
|
|
We have the following set of equations:
|
||
|
|
\begin{align}
|
||
|
|
d_1 &= +D_y - l_2 R_x \\
|
||
|
|
d_2 &= +D_y + l_1 R_x \\
|
||
|
|
d_3 &= -D_x - l_2 R_y \\
|
||
|
|
d_4 &= -D_x + l_1 R_y \\
|
||
|
|
d_5 &= -D_z
|
||
|
|
\end{align}
|
||
|
|
|
||
|
|
That can be written as a linear transformation:
|
||
|
|
\begin{equation}
|
||
|
|
\begin{bmatrix}
|
||
|
|
d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
|
||
|
|
\end{bmatrix} = \begin{bmatrix}
|
||
|
|
0 & 1 & 0 & -l_2 & 0 \\
|
||
|
|
0 & 1 & 0 & l_1 & 0 \\
|
||
|
|
-1 & 0 & 0 & 0 & -l_2 \\
|
||
|
|
-1 & 0 & 0 & 0 & l_1 \\
|
||
|
|
0 & 0 & -1 & 0 & 0
|
||
|
|
\end{bmatrix} \cdot \begin{bmatrix}
|
||
|
|
D_x \\ D_y \\ D_z \\ R_x \\ R_y
|
||
|
|
\end{bmatrix}
|
||
|
|
\end{equation}
|
||
|
|
|
||
|
|
By inverting the matrix, we obtain the Jacobian relation:
|
||
|
|
\begin{equation}
|
||
|
|
\begin{bmatrix}
|
||
|
|
D_x \\ D_y \\ D_z \\ R_x \\ R_y
|
||
|
|
\end{bmatrix} = \begin{bmatrix}
|
||
|
|
0 & 1 & 0 & -l_2 & 0 \\
|
||
|
|
0 & 1 & 0 & l_1 & 0 \\
|
||
|
|
-1 & 0 & 0 & 0 & -l_2 \\
|
||
|
|
-1 & 0 & 0 & 0 & l_1 \\
|
||
|
|
0 & 0 & -1 & 0 & 0
|
||
|
|
\end{bmatrix}^{-1} \cdot \begin{bmatrix}
|
||
|
|
d_1 \\ d_2 \\ d_3 \\ d_4 \\ d_5
|
||
|
|
\end{bmatrix}
|
||
|
|
\end{equation}
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Parameters
|
||
|
|
H = 150e-3;
|
||
|
|
l1 = (150-48-42)*1e-3;
|
||
|
|
l2 = (76.2+48+42-150)*1e-3;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Transformation matrix
|
||
|
|
Hm = [ 0 1 0 -l2 0;
|
||
|
|
0 1 0 l1 0;
|
||
|
|
-1 0 0 0 -l2;
|
||
|
|
-1 0 0 0 l1;
|
||
|
|
0 0 -1 0 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable(pinv(Hm), {'*Dx*', '*Dy*', '*Dz*', '*Rx*', '*Ry*'}, {'*d1*', '*d2*', '*d3*', '*d4*', '*d5*'}, ' %.2f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:jacobian_metrology
|
||
|
|
#+caption: Jacobian matrix for the metrology system
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | *d1* | *d2* | *d3* | *d4* | *d5* |
|
||
|
|
|------+--------+-------+--------+-------+------|
|
||
|
|
| *Dx* | 0.0 | 0.0 | -0.79 | -0.21 | 0.0 |
|
||
|
|
| *Dy* | 0.79 | 0.21 | 0.0 | 0.0 | 0.0 |
|
||
|
|
| *Dz* | 0.0 | 0.0 | 0.0 | 0.0 | -1.0 |
|
||
|
|
| *Rx* | -13.12 | 13.12 | -0.0 | 0.0 | 0.0 |
|
||
|
|
| *Ry* | -0.0 | -0.0 | -13.12 | 13.12 | 0.0 |
|
||
|
|
|
||
|
|
** Rough alignment of spheres using comparators
|
||
|
|
Bottom Sphere, then top sphere.
|
||
|
|
|
||
|
|
Alignment better than 10um.
|
||
|
|
But the coaxiality between the cylinder and the sphere might not be good.
|
||
|
|
|
||
|
|
#+name: fig:align_top_sphere_comparators
|
||
|
|
#+attr_latex: :width \linewidth
|
||
|
|
#+caption: Two mechanical comparators used to align the top sphere with the rotation axis of the spindle
|
||
|
|
[[file:figs/align_top_sphere_comparators.jpg]]
|
||
|
|
|
||
|
|
** Alignment of spheres using interferometers
|
||
|
|
*** Angular alignment
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry', 1);
|
||
|
|
data_it1 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 3);
|
||
|
|
data_it2 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Offset wrong points
|
||
|
|
i_it0 = find(abs(data_it0.Rx_int_filtered(2:end)-data_it0.Rx_int_filtered(1:end-1))>1e-5);
|
||
|
|
data_it0.Rx_int_filtered(i_it0+1:end) = data_it0.Rx_int_filtered(i_it0+1:end) + data_it0.Rx_int_filtered(i_it0) - data_it0.Rx_int_filtered(i_it0+1);
|
||
|
|
i_it1 = find(abs(data_it1.Rx_int_filtered(2:end)-data_it1.Rx_int_filtered(1:end-1))>1e-5);
|
||
|
|
data_it1.Rx_int_filtered(i_it1+1:end) = data_it1.Rx_int_filtered(i_it1+1:end) + data_it1.Rx_int_filtered(i_it1) - data_it1.Rx_int_filtered(i_it1+1);
|
||
|
|
i_it2 = find(abs(data_it2.Rx_int_filtered(2:end)-data_it2.Rx_int_filtered(1:end-1))>1e-5);
|
||
|
|
data_it2.Rx_int_filtered(i_it2+1:end) = data_it2.Rx_int_filtered(i_it2+1:end) + data_it2.Rx_int_filtered(i_it2) - data_it2.Rx_int_filtered(i_it2+1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute circle fit and get radius
|
||
|
|
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered);
|
||
|
|
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered);
|
||
|
|
[~, ~, R_it2, ~] = circlefit(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Rx/Ry alignment of the spheres using the micro-station
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_it0.Rx_int_filtered, 1e6*data_it0.Ry_int_filtered, '-', ...
|
||
|
|
'DisplayName', sprintf('It. 0: $R = %.0f \\mu$rad', R_it0))
|
||
|
|
plot(1e6*data_it1.Rx_int_filtered, 1e6*data_it1.Ry_int_filtered, '-', ...
|
||
|
|
'DisplayName', sprintf('It. 1: $R = %.0f \\mu$rad', R_it1))
|
||
|
|
plot(1e6*data_it2.Rx_int_filtered, 1e6*data_it2.Ry_int_filtered, '-', ...
|
||
|
|
'DisplayName', sprintf('It. 2: $R = %.0f \\mu$rad', R_it2))
|
||
|
|
hold off;
|
||
|
|
xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]');
|
||
|
|
axis equal
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_metrology_align_rx_ry.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_metrology_align_rx_ry
|
||
|
|
#+caption: Rx/Ry alignment of the spheres using the micro-station
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_metrology_align_rx_ry.png]]
|
||
|
|
|
||
|
|
*** Eccentricity alignment
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
data_it0 = h5scan(data_dir, 'alignment', 'h1rx_h1ry_0002', 5);
|
||
|
|
data_it1 = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Offset wrong points
|
||
|
|
i_it0 = find(abs(data_it0.Dy_int_filtered(2:end)-data_it0.Dy_int_filtered(1:end-1))>1e-5);
|
||
|
|
data_it0.Dy_int_filtered(i_it0+1:end) = data_it0.Dy_int_filtered(i_it0+1:end) + data_it0.Dy_int_filtered(i_it0) - data_it0.Dy_int_filtered(i_it0+1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute circle fit and get radius
|
||
|
|
[~, ~, R_it0, ~] = circlefit(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered);
|
||
|
|
[~, ~, R_it1, ~] = circlefit(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dx/Dy alignment of the spheres using the micro-station
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_it0.Dx_int_filtered, 1e6*data_it0.Dy_int_filtered, '-', ...
|
||
|
|
'DisplayName', sprintf('It. 0: $R = %.0f \\mu$m', R_it0))
|
||
|
|
plot(1e6*data_it1.Dx_int_filtered, 1e6*data_it1.Dy_int_filtered, '-', ...
|
||
|
|
'DisplayName', sprintf('It. 1: $R = %.0f \\mu$m', R_it1))
|
||
|
|
hold off;
|
||
|
|
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
|
||
|
|
axis equal
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_metrology_align_dx_dy.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_metrology_align_dx_dy
|
||
|
|
#+caption: Dx/Dy alignment of the spheres using the micro-station
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_metrology_align_dx_dy.png]]
|
||
|
|
|
||
|
|
** Residual error after alignment
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
data = h5scan(data_dir, 'alignment', 'h1dx_h1dy', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
- Dx and Dy are less than 1um.
|
||
|
|
- Dz less than 0.1um.
|
||
|
|
- Rx and Ry less than 4urad.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Remaining errors after aligning the metrology using the interferometers
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data.nth, 1e6*data.Dx_int_filtered, 'DisplayName', '$D_x$')
|
||
|
|
plot(data.nth, 1e6*data.Dy_int_filtered, 'DisplayName', '$D_y$')
|
||
|
|
plot(data.nth, 1e6*data.Dz_int_filtered, 'DisplayName', '$D_z$')
|
||
|
|
plot(data.nth, 1e6*data.Rx_int_filtered, 'DisplayName', '$R_x$')
|
||
|
|
plot(data.nth, 1e6*data.Ry_int_filtered, 'DisplayName', '$R_y$')
|
||
|
|
hold off;
|
||
|
|
xlabel('$R_z$ [deg]'); ylabel('Remaining Errors [$\mu$m, $\mu$rad]')
|
||
|
|
xlim([0, 360]); xticks(0:90:360);
|
||
|
|
ylim([-5, 5]); yticks(-5:1:5);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_align_metorlogy_errors_after_align.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_align_metorlogy_errors_after_align
|
||
|
|
#+caption: Remaining errors after aligning the metrology using the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_align_metorlogy_errors_after_align.png]]
|
||
|
|
|
||
|
|
|
||
|
|
** Metrology acceptance
|
||
|
|
Because the interferometers are pointing to spheres and not flat surfaces, the lateral acceptance is limited.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data = h5scan(data_dir, 'metrology_acceptance', 'after_int_align_meshXY', 1);
|
||
|
|
|
||
|
|
x = 1e3*detrend(data.h1tx, 0); % [um]
|
||
|
|
y = 1e3*detrend(data.h1ty, 0); % [um]
|
||
|
|
z = 1e6*data.Dz_int_filtered - max(data.Dz_int_filtered); % [um]
|
||
|
|
|
||
|
|
mdl = scatteredInterpolant(x, y, z);
|
||
|
|
[xg, yg] = meshgrid(unique(x), unique(y));
|
||
|
|
zg = mdl(xg, yg);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% XY mapping of the Z measurement by the interferometer
|
||
|
|
figure;
|
||
|
|
[~,c] = contour3(xg,yg,zg,30);
|
||
|
|
c.LineWidth = 3;
|
||
|
|
xlabel('$\mu$-hexapod $D_x$ [$\mu$m]');
|
||
|
|
ylabel('$\mu$-hexapod $D_y$ [$\mu$m]');
|
||
|
|
zlabel('Interf $D_z$ [$\mu$m]');
|
||
|
|
zlim([-1, 0]);
|
||
|
|
xticks(-100:50:100);
|
||
|
|
yticks(-100:50:100);
|
||
|
|
zticks(-1:0.2:0);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/metrology_alignment_xy_map.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:metrology_alignment_xy_map
|
||
|
|
#+caption: XY mapping of the Z measurement by the interferometer
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/metrology_alignment_xy_map.png]]
|
||
|
|
|
||
|
|
|
||
|
|
* Simscape Model
|
||
|
|
<<sec:id31_simscape_model>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Init model
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Add directories to path for Simscape Model
|
||
|
|
addpath('mat')
|
||
|
|
addpath('matlab/subsystems')
|
||
|
|
addpath('STEPS/nano_hexapod')
|
||
|
|
addpath('STEPS/metrology')
|
||
|
|
addpath('STEPS/png')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Options for Linearized
|
||
|
|
options = linearizeOptions;
|
||
|
|
options.SampleTime = 0;
|
||
|
|
|
||
|
|
%% Name of the Simulink File
|
||
|
|
mdl = 'nass_model_id31';
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
open(mdl)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
load('mat/conf_simulink.mat');
|
||
|
|
|
||
|
|
%% Initialize each Simscape model elements
|
||
|
|
initializeGround();
|
||
|
|
initializeGranite();
|
||
|
|
initializeTy();
|
||
|
|
initializeRy();
|
||
|
|
initializeRz();
|
||
|
|
initializeMicroHexapod();
|
||
|
|
|
||
|
|
initializeSimscapeConfiguration();
|
||
|
|
initializeDisturbances('enable', false);
|
||
|
|
initializeLoggingConfiguration('log', 'none');
|
||
|
|
|
||
|
|
initializeController('type', 'open-loop');
|
||
|
|
|
||
|
|
initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
initializeSample('type', '0');
|
||
|
|
|
||
|
|
initializePosError();
|
||
|
|
|
||
|
|
initializeReferences();
|
||
|
|
|
||
|
|
initializeRzEstimate('type', 'encoders');
|
||
|
|
|
||
|
|
initializeLion();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Identify Transfer functions
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
|
||
|
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Encoders
|
||
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors
|
||
|
|
io(io_i) = linio([mdl, '/Micro-Station/metrology_5dof/Lion_Metrology'], 1, 'output'); io_i = io_i + 1; % Interferometers
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Identify the dynamics for IFF plant
|
||
|
|
initializeSample('type', '0');
|
||
|
|
Gm_m0 = linearize(mdl, io);
|
||
|
|
Gm_m0.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
Gm_m0.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
|
||
|
|
'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
||
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
|
||
|
|
'd1', 'd2', 'd3', 'd4', 'd5'};
|
||
|
|
|
||
|
|
initializeSample('type', '1');
|
||
|
|
Gm_m1 = linearize(mdl, io);
|
||
|
|
Gm_m1.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
Gm_m1.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
|
||
|
|
'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
||
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
|
||
|
|
'd1', 'd2', 'd3', 'd4', 'd5'};
|
||
|
|
|
||
|
|
initializeSample('type', '2');
|
||
|
|
Gm_m2 = linearize(mdl, io);
|
||
|
|
Gm_m2.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
Gm_m2.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
|
||
|
|
'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
||
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
|
||
|
|
'd1', 'd2', 'd3', 'd4', 'd5'};
|
||
|
|
|
||
|
|
initializeSample('type', '3');
|
||
|
|
Gm_m3 = linearize(mdl, io);
|
||
|
|
Gm_m3.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
Gm_m3.OutputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6', ...
|
||
|
|
'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
||
|
|
'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6', ...
|
||
|
|
'd1', 'd2', 'd3', 'd4', 'd5'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Save Identified Plants
|
||
|
|
save('./matlab/mat/Gm.mat', 'Gm_m0', 'Gm_m1', 'Gm_m2', 'Gm_m3');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** IFF Plant
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% IFF transfer function - Simscape model
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_simscape_iff_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_simscape_iff_ol_plant
|
||
|
|
#+caption: IFF transfer function - Simscape model
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_simscape_iff_ol_plant.png]]
|
||
|
|
|
||
|
|
** Encoder plant
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_simscape_enc_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_simscape_enc_ol_plant
|
||
|
|
#+caption: ENC transfer function - Simscape model
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_simscape_enc_ol_plant.png]]
|
||
|
|
|
||
|
|
** HAC Undamped plant
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_simscape_int_ol_plant.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_simscape_int_ol_plant
|
||
|
|
#+caption: INT transfer function - Simscape model
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_simscape_int_ol_plant.png]]
|
||
|
|
|
||
|
|
* Identified Open Loop Plant
|
||
|
|
<<sec:id31_open_loop_plant>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Simscape Model
|
||
|
|
load('Gm.mat', 'Gm_m0', 'Gm_m1', 'Gm_m2', 'Gm_m3');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Load Data :noexport:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load identification data
|
||
|
|
data_m0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
|
||
|
|
data_m1 = load(sprintf('%s/dynamics/2023-08-08_18-57_ol_plant_m1_Wz0.mat', mat_dir));
|
||
|
|
data_m2 = load(sprintf('%s/dynamics/2023-08-08_17-12_ol_plant_m2_Wz0.mat', mat_dir));
|
||
|
|
data_m3 = load(sprintf('%s/dynamics/2023-08-08_18-20_ol_plant_m3_Wz0.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(2.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** IFF Plant
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_m0 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m0.(sprintf("uL%i", i_strut)).Vs1 ; data_m0.(sprintf("uL%i", i_strut)).Vs2 ; data_m0.(sprintf("uL%i", i_strut)).Vs3 ; data_m0.(sprintf("uL%i", i_strut)).Vs4 ; data_m0.(sprintf("uL%i", i_strut)).Vs5 ; data_m0.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_m1 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m1.(sprintf("uL%i", i_strut)).Vs1 ; data_m1.(sprintf("uL%i", i_strut)).Vs2 ; data_m1.(sprintf("uL%i", i_strut)).Vs3 ; data_m1.(sprintf("uL%i", i_strut)).Vs4 ; data_m1.(sprintf("uL%i", i_strut)).Vs5 ; data_m1.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_m2 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m2.(sprintf("uL%i", i_strut)).Vs1 ; data_m2.(sprintf("uL%i", i_strut)).Vs2 ; data_m2.(sprintf("uL%i", i_strut)).Vs3 ; data_m2.(sprintf("uL%i", i_strut)).Vs4 ; data_m2.(sprintf("uL%i", i_strut)).Vs5 ; data_m2.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_m3 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m3.(sprintf("uL%i", i_strut)).Vs1 ; data_m3.(sprintf("uL%i", i_strut)).Vs2 ; data_m3.(sprintf("uL%i", i_strut)).Vs3 ; data_m3.(sprintf("uL%i", i_strut)).Vs4 ; data_m3.(sprintf("uL%i", i_strut)).Vs5 ; data_m3.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Save Identified Plants
|
||
|
|
save('./matlab/mat/G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Measured transfer function from generated voltages to measured voltage on the force sensors
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_iff_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_iff_m0(:,i, i)), 'color', colors(i,:), ...
|
||
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m0(:,i, 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([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_iff_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_iff_ol_plant_m0
|
||
|
|
#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_iff_ol_plant_m0.png]]
|
||
|
|
|
||
|
|
The measured frequency response functions from DAC voltages $u_i$ to measured voltages on the force sensors $\tau_{m,i}$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_iff.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparison of the Simscape model and identified IFF plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_iff_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:)], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Identified');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Simscape');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Identified');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('Fn%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Simscape');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:)]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('Fn%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_comp_simscape_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_comp_simscape_frf_ol_iff
|
||
|
|
#+caption: Comparison of the Simscape model and identified IFF plant
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_comp_simscape_frf_ol_iff.png]]
|
||
|
|
|
||
|
|
The effect of the payload mass on the diagonal elements are shown in Figure ref:fig:id31_effect_mass_frf_ol_iff.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Effect of the payload mass on the IFF plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_iff_m0(:,1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - m0');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m1(:,1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - m1');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m2(:,1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - m2');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m3(:,1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - m3');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_effect_mass_frf_ol_iff.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_effect_mass_frf_ol_iff
|
||
|
|
#+caption: Effect of the payload mass on the IFF plant
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_effect_mass_frf_ol_iff.png]]
|
||
|
|
|
||
|
|
** Encoder plant
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% ENC Plant (transfer function from u to taum)
|
||
|
|
G_enc_m0 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m0.(sprintf("uL%i", i_strut)).dL1 ; data_m0.(sprintf("uL%i", i_strut)).dL2 ; data_m0.(sprintf("uL%i", i_strut)).dL3 ; data_m0.(sprintf("uL%i", i_strut)).dL4 ; data_m0.(sprintf("uL%i", i_strut)).dL5 ; data_m0.(sprintf("uL%i", i_strut)).dL6]';
|
||
|
|
|
||
|
|
G_enc_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% ENC Plant (transfer function from u to taum)
|
||
|
|
G_enc_m1 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m1.(sprintf("uL%i", i_strut)).dL1 ; data_m1.(sprintf("uL%i", i_strut)).dL2 ; data_m1.(sprintf("uL%i", i_strut)).dL3 ; data_m1.(sprintf("uL%i", i_strut)).dL4 ; data_m1.(sprintf("uL%i", i_strut)).dL5 ; data_m1.(sprintf("uL%i", i_strut)).dL6]';
|
||
|
|
|
||
|
|
G_enc_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% ENC Plant (transfer function from u to taum)
|
||
|
|
G_enc_m2 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m2.(sprintf("uL%i", i_strut)).dL1 ; data_m2.(sprintf("uL%i", i_strut)).dL2 ; data_m2.(sprintf("uL%i", i_strut)).dL3 ; data_m2.(sprintf("uL%i", i_strut)).dL4 ; data_m2.(sprintf("uL%i", i_strut)).dL5 ; data_m2.(sprintf("uL%i", i_strut)).dL6]';
|
||
|
|
|
||
|
|
G_enc_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% ENC Plant (transfer function from u to taum)
|
||
|
|
G_enc_m3 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m3.(sprintf("uL%i", i_strut)).dL1 ; data_m3.(sprintf("uL%i", i_strut)).dL2 ; data_m3.(sprintf("uL%i", i_strut)).dL3 ; data_m3.(sprintf("uL%i", i_strut)).dL4 ; data_m3.(sprintf("uL%i", i_strut)).dL5 ; data_m3.(sprintf("uL%i", i_strut)).dL6]';
|
||
|
|
|
||
|
|
G_enc_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Save Identified Plants
|
||
|
|
save('./matlab/mat/G_ol.mat', 'G_enc_m0', 'G_enc_m1', 'G_enc_m2', 'G_enc_m3', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The identified frequency response functions from general voltages $u_i$ to measured displacement of the struts by the encoders $d\mathcal{L}_i$ are compared with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_enc.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_enc_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_j$ - Identified');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_enc_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_j$ - Simscape');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:)], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_i$ - Identified');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_i$ - Simscape');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:)]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('dL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_comp_simscape_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_comp_simscape_frf_ol_enc
|
||
|
|
#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to encoders
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_comp_simscape_frf_ol_enc.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Effect of the payload mass on the transfer function from actuator voltage to encoder displacement
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_enc_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_i$ - m0');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_enc_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_i$ - m1');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_enc_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_i$ - m2');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_enc_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$d\mathcal{L}_i/u_i$ - m3');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_enc_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_enc_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_enc_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_enc_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_enc_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_effect_mass_frf_ol_enc.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_effect_mass_frf_ol_enc
|
||
|
|
#+caption: Effect of the payload mass on the transfer function from actuator voltage to encoder displacement
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_effect_mass_frf_ol_enc.png]]
|
||
|
|
|
||
|
|
** HAC Undamped plant
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int_m0 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int_m1 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int_m2 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int_m3 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Save Identified Plants
|
||
|
|
save('./matlab/mat/G_ol.mat', 'G_int_m0', 'G_int_m1', 'G_int_m2', 'G_int_m3', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The identified frequency response functions from actuator voltages $u_i$ to measured strut motion from the external metrology (i.e. the interferometers) are compare with the simscape model in Figure ref:fig:id31_comp_simscape_frf_ol_int.
|
||
|
|
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Measured transfer function from generated voltages to measured voltage on the force sensors
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_int_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_int_m0(:,i, i)), 'color', colors(i,:), ...
|
||
|
|
'DisplayName', sprintf('$e\\mathcal{L}_%i/u_%i$', i, i));
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_m0(:,i, 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([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_int_ol_plant_m0.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_int_ol_plant_m0
|
||
|
|
#+caption: Measured transfer function from generated voltages to measured voltage on the force sensors
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_int_ol_plant_m0.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_int_m0(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$ - Identified');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_int_m0(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$ - Simscape');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:)], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - Identified');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - Simscape');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:)]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:)]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_comp_simscape_frf_ol_int.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_comp_simscape_frf_ol_int
|
||
|
|
#+caption: Comparison of the Simscape model and identified plant - Transfer functions from voltages to estimated strut motion from interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_comp_simscape_frf_ol_int.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - m0');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - m1');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - m2');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - m3');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_effect_mass_frf_ol_int.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_effect_mass_frf_ol_int
|
||
|
|
#+caption: Effect of the payload mass on the transfer functions from actuator voltage to measured strut motion by the external metrology
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_effect_mass_frf_ol_int.png]]
|
||
|
|
|
||
|
|
** Decoupling improvement thanks to better Rz alignment
|
||
|
|
*** Alignment procedure
|
||
|
|
|
||
|
|
- Control based on encoders
|
||
|
|
- Slow moving in X and Y
|
||
|
|
- Compare with X and Y from interf
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
data_1_dx = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 2);
|
||
|
|
data_1_dy = h5scan(data_dir, 'align_int_enc_Rz', 'tx_first_scan', 3);
|
||
|
|
data_2_dx = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 1);
|
||
|
|
data_2_dy = h5scan(data_dir, 'align_int_enc_Rz', 'verif-after-correct-offset', 2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Measurement')
|
||
|
|
plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off')
|
||
|
|
plot(1e6*[-10:10]*cos(2.7*pi/180), -1e6*[-10:10]*sin(2.7*pi/180), '--', 'color', colors(2,:), 'DisplayName', '$R_z = 2.7$ deg')
|
||
|
|
plot(1e6*[-10:10]*sin(2.7*pi/180), 1e6*[-10:10]*cos(2.7*pi/180), '--', 'color', colors(2,:), 'HandleVisibility', 'off')
|
||
|
|
hold off;
|
||
|
|
xlabel('Interf $D_x$ [$\mu$m]');
|
||
|
|
ylabel('Interf $D_y$ [$\mu$m]');
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
axis equal
|
||
|
|
xlim([-10, 10]); ylim([-10, 10]);
|
||
|
|
xticks([-10:5:10]); yticks([-10:5:10]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Rz_align_dx_dy_scans_before_calib.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Rz_align_dx_dy_scans_before_calib
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Rz_align_dx_dy_scans_before_calib.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_1_dx.Dx_int_filtered, 1e6*data_1_dx.Dy_int_filtered, 'color', colors(1,:), 'DisplayName', 'Initial')
|
||
|
|
plot(1e6*data_1_dy.Dx_int_filtered, 1e6*data_1_dy.Dy_int_filtered, 'color', colors(1,:), 'HandleVisibility', 'off')
|
||
|
|
plot(1e6*data_2_dx.Dx_int_filtered, 1e6*data_2_dx.Dy_int_filtered, 'color', colors(2,:), 'DisplayName', 'After $R_z$ calib')
|
||
|
|
plot(1e6*data_2_dy.Dx_int_filtered, 1e6*data_2_dy.Dy_int_filtered, 'color', colors(2,:), 'HandleVisibility', 'off')
|
||
|
|
hold off;
|
||
|
|
xlabel('Interf $D_x$ [$\mu$m]');
|
||
|
|
ylabel('Interf $D_y$ [$\mu$m]');
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
axis equal
|
||
|
|
xlim([-10, 10]); ylim([-10, 10]);
|
||
|
|
xticks([-10:5:10]); yticks([-10:5:10]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Rz_align_dx_dy_scans.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Rz_align_dx_dy_scans
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Rz_align_dx_dy_scans.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** m0
|
||
|
|
#+begin_src matlab
|
||
|
|
data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
|
||
|
|
data_align = load(sprintf('%s/dynamics/2023-08-17_17-37_ol_plant_m0_Wz0_new_Rz_align.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int_align = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Decrease of the coupling with better Rz alignment
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$ - $R_z$ align');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - $R_z$ align');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_coupling_decrease_rz_align.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_coupling_decrease_rz_align
|
||
|
|
#+caption: Decrease of the coupling with better Rz alignment
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_coupling_decrease_rz_align.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** m3
|
||
|
|
#+begin_src matlab
|
||
|
|
data = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir));
|
||
|
|
data_align = load(sprintf('%s/dynamics/2023-08-21_15-52_damp_plant_m3_new_Rz.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% INT Plant (transfer function from u to taum)
|
||
|
|
G_int_align = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_align.(sprintf("uL%i", i_strut)).e_L1 ; data_align.(sprintf("uL%i", i_strut)).e_L2 ; data_align.(sprintf("uL%i", i_strut)).e_L3 ; data_align.(sprintf("uL%i", i_strut)).e_L4 ; data_align.(sprintf("uL%i", i_strut)).e_L5 ; data_align.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_int_align(:,:,i_strut) = tfestimate(data_align.(sprintf("uL%i", i_strut)).id_plant, eL, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Decrease of the coupling with better Rz alignment
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_int(:, i, j)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_j$ - $R_z$ align');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_int_align(:, i, j)), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(2,:)], ...
|
||
|
|
'DisplayName', '$-e\mathcal{L}_i/u_i$ - $R_z$ align');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_align(:,i, i)), 'color', [colors(2,:)], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int(:,i, i)), 'color', [colors(1,:)]);
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_int_align(:,i, i)), 'color', [colors(2,:)]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_coupling_decrease_rz_align_m3.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_coupling_decrease_rz_align_m3
|
||
|
|
#+caption: Decrease of the coupling with better Rz alignment
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_coupling_decrease_rz_align_m3.png]]
|
||
|
|
|
||
|
|
** Conclusion
|
||
|
|
|
||
|
|
- Good match between the model and experiment
|
||
|
|
|
||
|
|
* TODO Noise Budget
|
||
|
|
<<sec:id31_noise_budget>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
In this section, the noise budget is performed.
|
||
|
|
The vibrations of the sample is measured in different conditions using the external metrology.
|
||
|
|
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Open-Loop Noise Budget
|
||
|
|
First, the noise is measured while no motion is performed.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load measured noise
|
||
|
|
data_m0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Noise budget in the cartesian frame
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Coordinate transform
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_m0.d1; data_m0.d2; data_m0.d3; data_m0.d4; data_m0.d5];
|
||
|
|
data_m0.t = Ts*[0:length(data_m0.d1)-1];
|
||
|
|
data_m0.Dx_int = a(1,:);
|
||
|
|
data_m0.Dy_int = a(2,:);
|
||
|
|
data_m0.Dz_int = a(3,:);
|
||
|
|
data_m0.Rx_int = a(4,:);
|
||
|
|
data_m0.Ry_int = a(5,:);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Data in the time domain
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Measured vibration with the interferometers
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
% plot(data_m0.t, 1e9*data_m0.Dx_int, '-', 'DisplayName', '$D_x$');
|
||
|
|
plot(data_m0.t, 1e9*data_m0.Dy_int, '-', 'DisplayName', '$D_y$');
|
||
|
|
plot(data_m0.t, 1e9*data_m0.Dz_int, '-', 'DisplayName', '$D_z$');
|
||
|
|
% plot(data_m0.t, data_m0.Rx_int, 'DisplayName', '$R_x$');
|
||
|
|
plot(data_m0.t, 1e9*data_m0.Ry_int, '-', 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Time [s]'); ylabel('Amplitude [nm]');
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([50, 54])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_noise_budget_interf_time_domain_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_noise_budget_interf_time_domain_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_noise_budget_interf_time_domain_m0.png]]
|
||
|
|
|
||
|
|
In the frequency domain
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(2.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[pxx_Dx, f] = pwelch(detrend(data_m0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[pxx_Dy, ~] = pwelch(detrend(data_m0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[pxx_Dz, ~] = pwelch(detrend(data_m0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[pxx_Rx, ~] = pwelch(detrend(data_m0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[pxx_Ry, ~] = pwelch(detrend(data_m0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
% plot(f, sqrt(pxx_Dx), 'DisplayName', '$D_x$');
|
||
|
|
plot(f, sqrt(pxx_Dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, sqrt(pxx_Dz), 'DisplayName', '$D_z$');
|
||
|
|
% plot(f, sqrt(pxx_Rx), 'DisplayName', '$R_x$');
|
||
|
|
plot(f, sqrt(pxx_Ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([1, 5e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_noise_budget_interf_freq_domain_m0.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_noise_budget_interf_freq_domain_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_noise_budget_interf_freq_domain_m0.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dx)))), 'DisplayName', '$D_x$');
|
||
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dy)))), 'DisplayName', sprintf('$D_y$ - %.0f nm', rms(1e9*detrend(data_m0.Dy_int, 0))));
|
||
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Dz)))), 'DisplayName', sprintf('$D_z$ - %.0f nm', rms(1e9*detrend(data_m0.Dz_int, 0))));
|
||
|
|
% plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Rx)))), 'DisplayName', '$R_x$');
|
||
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_Ry)))), 'DisplayName', sprintf('$R_y$ - %.0f nrad', rms(1e9*detrend(data_m0.Ry_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([1, 5e3]); ylim([1e-10, 1e-7]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_noise_budget_open_loop_cas_m0.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_noise_budget_open_loop_cas_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_noise_budget_open_loop_cas_m0.png]]
|
||
|
|
|
||
|
|
** Effect of LAC
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load measured noise
|
||
|
|
data_ol = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
|
||
|
|
data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Coordinate transform
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
|
||
|
|
data_ol.Dx_int = a(1,:);
|
||
|
|
data_ol.Dy_int = a(2,:);
|
||
|
|
data_ol.Dz_int = a(3,:);
|
||
|
|
data_ol.Rx_int = a(4,:);
|
||
|
|
data_ol.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
|
||
|
|
data_lac.Dx_int = a(1,:);
|
||
|
|
data_lac.Dy_int = a(2,:);
|
||
|
|
data_lac.Dz_int = a(3,:);
|
||
|
|
data_lac.Rx_int = a(4,:);
|
||
|
|
data_lac.Ry_int = a(5,:);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(2.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
|
||
|
|
[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Effect of LAC (Figure ref:fig:id31_noise_budget_effect_lac_m0):
|
||
|
|
- reduce amplitude around 80Hz
|
||
|
|
- Inject some noise between 200 and 700Hz?
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [$m/\sqrt{Hz}$]');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
title('$D_y$')
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f , sqrt(data_ol.pxx_Dz ), 'DisplayName', '$D_z$ - OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', '$D_z$ - LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
title('$D_z$')
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f , sqrt(data_ol.pxx_Ry ), 'DisplayName', '$R_y$ - OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', '$R_y$ - LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
title('$R_y$')
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3],'xy');
|
||
|
|
xlim([1, 5e2]); ylim([1e-11, 2e-8]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_noise_budget_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_noise_budget_effect_lac_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_noise_budget_effect_lac_m0.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
|
||
|
|
title('$D_y$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$D_z$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$R_y$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3],'xy');
|
||
|
|
xlim([1, 1e3]); ylim([1e-10, 1e-7]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_cas_effect_lac_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_cas_effect_lac_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_cas_effect_lac_m0.png]]
|
||
|
|
|
||
|
|
** Effect of rotation
|
||
|
|
#+begin_src matlab
|
||
|
|
data_Wz0 = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
|
||
|
|
data_Wz1 = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat', mat_dir));
|
||
|
|
data_Wz2 = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Coordinate transform
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_Wz0.d1; data_Wz0.d2; data_Wz0.d3; data_Wz0.d4; data_Wz0.d5];
|
||
|
|
data_Wz0.Dx_int = a(1,:);
|
||
|
|
data_Wz0.Dy_int = a(2,:);
|
||
|
|
data_Wz0.Dz_int = a(3,:);
|
||
|
|
data_Wz0.Rx_int = a(4,:);
|
||
|
|
data_Wz0.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_Wz1.d1; data_Wz1.d2; data_Wz1.d3; data_Wz1.d4; data_Wz1.d5];
|
||
|
|
data_Wz1.Dx_int = a(1,:);
|
||
|
|
data_Wz1.Dy_int = a(2,:);
|
||
|
|
data_Wz1.Dz_int = a(3,:);
|
||
|
|
data_Wz1.Rx_int = a(4,:);
|
||
|
|
data_Wz1.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_Wz2.d1; data_Wz2.d2; data_Wz2.d3; data_Wz2.d4; data_Wz2.d5];
|
||
|
|
data_Wz2.Dx_int = a(1,:);
|
||
|
|
data_Wz2.Dy_int = a(2,:);
|
||
|
|
data_Wz2.Dz_int = a(3,:);
|
||
|
|
data_Wz2.Rx_int = a(4,:);
|
||
|
|
data_Wz2.Ry_int = a(5,:);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(20.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
|
||
|
|
[data_Wz0.pxx_Dx, data_Wz0.f] = pwelch(detrend(data_Wz0.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz0.pxx_Dy, ~ ] = pwelch(detrend(data_Wz0.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz0.pxx_Dz, ~ ] = pwelch(detrend(data_Wz0.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz0.pxx_Rx, ~ ] = pwelch(detrend(data_Wz0.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz0.pxx_Ry, ~ ] = pwelch(detrend(data_Wz0.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_Wz1.pxx_Dx, data_Wz1.f] = pwelch(detrend(data_Wz1.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz1.pxx_Dy, ~ ] = pwelch(detrend(data_Wz1.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz1.pxx_Dz, ~ ] = pwelch(detrend(data_Wz1.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz1.pxx_Rx, ~ ] = pwelch(detrend(data_Wz1.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz1.pxx_Ry, ~ ] = pwelch(detrend(data_Wz1.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_Wz2.pxx_Dx, data_Wz2.f] = pwelch(detrend(data_Wz2.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz2.pxx_Dy, ~ ] = pwelch(detrend(data_Wz2.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz2.pxx_Dz, ~ ] = pwelch(detrend(data_Wz2.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz2.pxx_Rx, ~ ] = pwelch(detrend(data_Wz2.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_Wz2.pxx_Ry, ~ ] = pwelch(detrend(data_Wz2.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_Wz0.f, sqrt(data_Wz0.pxx_Dy), 'DisplayName', '$D_y$ - 0rpm');
|
||
|
|
plot(data_Wz1.f, sqrt(data_Wz1.pxx_Dy), 'DisplayName', '$D_y$ - 1rpm');
|
||
|
|
plot(data_Wz2.f, sqrt(data_Wz2.pxx_Dy), 'DisplayName', '$D_y$ - 30rpm');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([0.1, 5e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Rotation induces lots of vibrations, especially at high velocity.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dy)))), ...
|
||
|
|
'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dy_int, 0))));
|
||
|
|
plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dy)))), ...
|
||
|
|
'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dy_int, 0))));
|
||
|
|
plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dy)))), ...
|
||
|
|
'DisplayName', sprintf('30rpm: $%.1f \\mu m$', 1e6*rms(detrend(data_Wz2.Dy_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xticks([1e0, 1e1, 1e2]);
|
||
|
|
title('$D_y$')
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Dz)))), ...
|
||
|
|
'DisplayName', sprintf('0rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz0.Dz_int, 0))));
|
||
|
|
plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Dz)))), ...
|
||
|
|
'DisplayName', sprintf('6rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz1.Dz_int, 0))));
|
||
|
|
plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Dz)))), ...
|
||
|
|
'DisplayName', sprintf('30rpm: $%.0f nm$', 1e9*rms(detrend(data_Wz2.Dz_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xticks([1e0, 1e1, 1e2]);
|
||
|
|
title('$D_z$')
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_Wz0.f, sqrt(flip(-cumtrapz(flip(data_Wz0.f), flip(data_Wz0.pxx_Ry)))), ...
|
||
|
|
'DisplayName', sprintf('0rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz0.Ry_int, 0))));
|
||
|
|
plot(data_Wz1.f, sqrt(flip(-cumtrapz(flip(data_Wz1.f), flip(data_Wz1.pxx_Ry)))), ...
|
||
|
|
'DisplayName', sprintf('6rpm: $%.1f \\mu$rad', 1e6*rms(detrend(data_Wz1.Ry_int, 0))));
|
||
|
|
plot(data_Wz2.f, sqrt(flip(-cumtrapz(flip(data_Wz2.f), flip(data_Wz2.pxx_Ry)))), ...
|
||
|
|
'DisplayName', sprintf('30rpm: $%.0f \\mu$rad', 1e6*rms(detrend(data_Wz2.Ry_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xticks([1e0, 1e1, 1e2]);
|
||
|
|
title('$R_y$')
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3],'xy');
|
||
|
|
xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_noise_budget_effect_rotation.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_noise_budget_effect_rotation
|
||
|
|
#+caption: Cumulative Amplitude Spectrum for the three important directions ($D_y$, $D_z$ and $R_y$). Three rotating velocities are shown. Integrated RMS values are shown in the legend.
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_noise_budget_effect_rotation.png]]
|
||
|
|
|
||
|
|
** Effect of HAC
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load measured noise
|
||
|
|
data_ol = load(sprintf('%s/freq_analysis/2023-08-11_16-51_m0_lac_off.mat', mat_dir));
|
||
|
|
data_lac = load(sprintf('%s/freq_analysis/2023-08-11_16-46_m0_lac_on.mat' , mat_dir));
|
||
|
|
data_hac = load(sprintf('%s/freq_analysis/2023-08-11_16-49_m0_hac_on.mat' , mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Coordinate transform
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
|
||
|
|
data_ol.Dx_int = a(1,:);
|
||
|
|
data_ol.Dy_int = a(2,:);
|
||
|
|
data_ol.Dz_int = a(3,:);
|
||
|
|
data_ol.Rx_int = a(4,:);
|
||
|
|
data_ol.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
|
||
|
|
data_lac.Dx_int = a(1,:);
|
||
|
|
data_lac.Dy_int = a(2,:);
|
||
|
|
data_lac.Dz_int = a(3,:);
|
||
|
|
data_lac.Rx_int = a(4,:);
|
||
|
|
data_lac.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
|
||
|
|
data_hac.Dx_int = a(1,:);
|
||
|
|
data_hac.Dy_int = a(2,:);
|
||
|
|
data_hac.Dz_int = a(3,:);
|
||
|
|
data_hac.Rx_int = a(4,:);
|
||
|
|
data_hac.Ry_int = a(5,:);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(2.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
|
||
|
|
[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Dy, ~ ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Dz, ~ ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Rx, ~ ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Ry, ~ ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Bandwidth is approximately 10Hz.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(data_ol.pxx_Dy), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', 'LAC');
|
||
|
|
plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', 'HAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]');
|
||
|
|
title('$D_y$');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(data_ol.pxx_Dz), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Dz), 'DisplayName', 'LAC');
|
||
|
|
plot(data_hac.f, sqrt(data_hac.pxx_Dz), 'DisplayName', 'HAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$D_z$');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(data_ol.pxx_Ry), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Ry), 'DisplayName', 'LAC');
|
||
|
|
plot(data_hac.f, sqrt(data_hac.pxx_Ry), 'DisplayName', 'HAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$R_y$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3],'xy');
|
||
|
|
xlim([1, 1e3]); ylim([1e-12, 1e-7]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_noise_budget_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_noise_budget_effect_hac_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_noise_budget_effect_hac_m0.png]]
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', 'LAC');
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', 'HAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
|
||
|
|
title('$D_y$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), 'DisplayName', 'LAC');
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), 'DisplayName', 'HAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$D_z$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), 'DisplayName', 'OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), 'DisplayName', 'LAC');
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), 'DisplayName', 'HAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$R_y$');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3],'xy');
|
||
|
|
xlim([1, 1e3]); ylim([1e-10, 1e-6]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_cas_effect_hac_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_cas_effect_hac_m0
|
||
|
|
#+caption: Measured vibration with the interferometers
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_cas_effect_hac_m0.png]]
|
||
|
|
|
||
|
|
** TODO Noise coming from force sensor
|
||
|
|
|
||
|
|
* Integral Force Feedback
|
||
|
|
<<sec:id31_iff>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** IFF Plants
|
||
|
|
*** Introduction :ignore:
|
||
|
|
|
||
|
|
*** 6x6 Plant
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load identification data
|
||
|
|
data = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data.(sprintf("uL%i", i_strut)).Vs1 ; data.(sprintf("uL%i", i_strut)).Vs2 ; data.(sprintf("uL%i", i_strut)).Vs3 ; data.(sprintf("uL%i", i_strut)).Vs4 ; data.(sprintf("uL%i", i_strut)).Vs5 ; data.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_iff(:,i, i)), 'color', colors(i,:), ...
|
||
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff(:,i, 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([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Giff_plant.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Giff_plant
|
||
|
|
#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Giff_plant.png]]
|
||
|
|
|
||
|
|
Compare with Model:
|
||
|
|
#+begin_src matlab
|
||
|
|
load('Gm_iff.mat');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_iff(:, i, j)), 'color', [colors(3,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, j), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_iff_m0(i, i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
% plot(f, abs(G_iff(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
% 'DisplayName', '$\tau_{m,i}/u_j$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
% ax2 = nexttile;
|
||
|
|
% hold on;
|
||
|
|
% for i =1:6
|
||
|
|
% plot(f, 180/pi*angle(G_iff(:,i, 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([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Effect of Rotation
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load identification data
|
||
|
|
data_Wz0 = load(sprintf('%s/dynamics/2023-08-08_16-17_ol_plant_m0_Wz0.mat', mat_dir));
|
||
|
|
data_Wz1 = load(sprintf('%s/dynamics/2023-08-08_16-28_ol_plant_m0_Wz36.mat', mat_dir));
|
||
|
|
data_Wz2 = load(sprintf('%s/dynamics/2023-08-08_16-45_ol_plant_m0_Wz180.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_Wz0 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_Wz0.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz0.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_Wz0(:,:,i_strut) = tfestimate(data_Wz0.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_Wz1 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_Wz1.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz1.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_Wz1(:,:,i_strut) = tfestimate(data_Wz1.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% IFF Plant (transfer function from u to taum)
|
||
|
|
G_iff_Wz2 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_Wz2.(sprintf("uL%i", i_strut)).Vs1 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs2 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs3 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs4 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs5 ; data_Wz2.(sprintf("uL%i", i_strut)).Vs6]';
|
||
|
|
|
||
|
|
G_iff_Wz2(:,:,i_strut) = tfestimate(data_Wz2.(sprintf("uL%i", i_strut)).id_plant, eL, win, [], [], 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Save Identified Plants
|
||
|
|
save('./mat/G_iff.mat', 'G_iff_Wz0', 'G_iff_Wz1', 'G_iff_Wz2', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_iff_Wz0(:, 1, 1)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$\Omega_z = 0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_Wz1(:, 1, 1)), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'DisplayName', '$\Omega_z = 36$ deg/s');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_Wz2(:, 1, 1)), 'color', [colors(3,:), 0.2], ...
|
||
|
|
'DisplayName', '$\Omega_z = 180$ deg/s');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_Wz0(:,i, i)), 'color', [colors(1,:), 0.2]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_Wz1(:,i, i)), 'color', [colors(2,:), 0.2]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_Wz2(:,i, i)), 'color', [colors(3,:), 0.2]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Giff_effect_rotation.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Giff_effect_rotation
|
||
|
|
#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Giff_effect_rotation.png]]
|
||
|
|
|
||
|
|
*** Effect of Mass
|
||
|
|
#+begin_src matlab
|
||
|
|
load('G_ol.mat', 'G_iff_m0', 'G_iff_m1', 'G_iff_m2', 'G_iff_m3', 'f');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Giff_effect_mass.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Giff_effect_mass
|
||
|
|
#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Giff_effect_mass.png]]
|
||
|
|
|
||
|
|
*** Compare with the model
|
||
|
|
#+begin_src matlab
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparison of the identified IFF plant and the IFF plant extracted from the simscape model
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_iff_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_iff_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', '$m = 0$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', '$m = 1$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', '$m = 2$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
|
||
|
|
'DisplayName', '$m = 3$ - Model');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_iff_m0(:,1,1)), 'color', [colors(1,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(f, 180/pi*angle(G_iff_m1(:,1,1)), 'color', [colors(2,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(f, 180/pi*angle(G_iff_m2(:,1,1)), 'color', [colors(3,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(f, 180/pi*angle(G_iff_m3(:,1,1)), 'color', [colors(4,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_iff_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m0('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m1('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m2('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_m3('Fn1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360);
|
||
|
|
ylim([-90, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Giff_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Giff_plant_comp_model
|
||
|
|
#+caption: Comparison of the identified IFF plant and the IFF plant extracted from the simscape model
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Giff_plant_comp_model.png]]
|
||
|
|
|
||
|
|
|
||
|
|
** IFF Controller
|
||
|
|
*** Controller Design
|
||
|
|
Test second order high pass filter:
|
||
|
|
#+begin_src matlab
|
||
|
|
wz = 2*pi*10;
|
||
|
|
xiz = 0.7;
|
||
|
|
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1)
|
||
|
|
% s/(2*pi*1)/(1 + s/(2*pi*1)) * ... % HPF: reduce gain at low frequency
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
We want integral action between 20Hz and 200Hz.
|
||
|
|
#+begin_src matlab
|
||
|
|
%% IFF Controller
|
||
|
|
Kiff = -1e2 * ... % Gain
|
||
|
|
1/(0.01*2*pi + s) * ... % LPF: provides integral action
|
||
|
|
Ghpf * ...
|
||
|
|
eye(6); % Diagonal 6x6 controller
|
||
|
|
Kiff.InputName = {'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'};
|
||
|
|
Kiff.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Loop Gain:
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% IFF Loop gain of the diagonal terms
|
||
|
|
Kiff_frf = squeeze(freqresp(Kiff(1,1), f, 'Hz'));
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_iff_m0(:, 1, 1).*Kiff_frf), 'color', colors(1,:), ...
|
||
|
|
'DisplayName', '$m = 0$');
|
||
|
|
plot(f, abs(G_iff_m1(:, 1, 1).*Kiff_frf), 'color', colors(2,:), ...
|
||
|
|
'DisplayName', '$m = 1$');
|
||
|
|
plot(f, abs(G_iff_m2(:, 1, 1).*Kiff_frf), 'color', colors(3,:), ...
|
||
|
|
'DisplayName', '$m = 2$');
|
||
|
|
plot(f, abs(G_iff_m3(:, 1, 1).*Kiff_frf), 'color', colors(4,:), ...
|
||
|
|
'DisplayName', '$m = 3$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-2, 1e1]);
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(-G_iff_m0(:,1,1).*Kiff_frf), 'color', colors(1,:));
|
||
|
|
plot(f, 180/pi*angle(-G_iff_m1(:,1,1).*Kiff_frf), 'color', colors(2,:));
|
||
|
|
plot(f, 180/pi*angle(-G_iff_m2(:,1,1).*Kiff_frf), 'color', colors(3,:));
|
||
|
|
plot(f, 180/pi*angle(-G_iff_m3(:,1,1).*Kiff_frf), 'color', colors(4,:));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_iff_loop_gain_diagonal_terms.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_iff_loop_gain_diagonal_terms
|
||
|
|
#+caption: IFF Loop gain of the diagonal terms
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_iff_loop_gain_diagonal_terms.png]]
|
||
|
|
|
||
|
|
Root Locus to obtain optimal gain.
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Root Locus for IFF
|
||
|
|
gains = logspace(-2, 2, 100);
|
||
|
|
Gm_iff_m0 = Gm_m0({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
|
||
|
|
Gm_iff_m1 = Gm_m1({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
|
||
|
|
Gm_iff_m2 = Gm_m2({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
|
||
|
|
Gm_iff_m3 = Gm_m3({'Fn1', 'Fn2', 'Fn3', 'Fn4', 'Fn5', 'Fn6'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'});
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 4, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(real(pole(Gm_iff_m0)), imag(pole(Gm_iff_m0)), 'x', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', '$g = 0$');
|
||
|
|
plot(real(tzero(Gm_iff_m0)), imag(tzero(Gm_iff_m0)), 'o', 'color', colors(1,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
|
||
|
|
for g = gains
|
||
|
|
clpoles = pole(feedback(Gm_iff_m0, g*Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
|
||
|
|
% Optimal gain
|
||
|
|
clpoles = pole(feedback(Gm_iff_m0, Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
|
||
|
|
'DisplayName', '$g_{opt}$');
|
||
|
|
hold off;
|
||
|
|
axis equal;
|
||
|
|
xlim([-640, 0]); ylim([0, 1600]);
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$m_0$');
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(real(pole(Gm_iff_m1)), imag(pole(Gm_iff_m1)), 'x', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', '$g = 0$');
|
||
|
|
plot(real(tzero(Gm_iff_m1)), imag(tzero(Gm_iff_m1)), 'o', 'color', colors(2,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
|
||
|
|
for g = gains
|
||
|
|
clpoles = pole(feedback(Gm_iff_m1, g*Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
|
||
|
|
% Optimal gain
|
||
|
|
clpoles = pole(feedback(Gm_iff_m1, Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
|
||
|
|
'DisplayName', '$g_{opt}$');
|
||
|
|
hold off;
|
||
|
|
axis equal;
|
||
|
|
xlim([-320, 0]); ylim([0, 800]);
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$m_1$');
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(real(pole(Gm_iff_m2)), imag(pole(Gm_iff_m2)), 'x', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', '$g = 0$');
|
||
|
|
plot(real(tzero(Gm_iff_m2)), imag(tzero(Gm_iff_m2)), 'o', 'color', colors(3,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
|
||
|
|
for g = gains
|
||
|
|
clpoles = pole(feedback(Gm_iff_m2, g*Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
|
||
|
|
% Optimal gain
|
||
|
|
clpoles = pole(feedback(Gm_iff_m2, Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
|
||
|
|
'DisplayName', '$g_{opt}$');
|
||
|
|
hold off;
|
||
|
|
axis equal;
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
xlim([-240, 0]); ylim([0, 600]);
|
||
|
|
title('$m_2$');
|
||
|
|
|
||
|
|
ax4 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(real(pole(Gm_iff_m3)), imag(pole(Gm_iff_m3)), 'x', 'color', colors(4,:), ...
|
||
|
|
'DisplayName', '$g = 0$');
|
||
|
|
plot(real(tzero(Gm_iff_m3)), imag(tzero(Gm_iff_m3)), 'o', 'color', colors(4,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
|
||
|
|
for g = gains
|
||
|
|
clpoles = pole(feedback(Gm_iff_m3, g*Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(4,:), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
|
||
|
|
% Optimal gain
|
||
|
|
clpoles = pole(feedback(Gm_iff_m3, Kiff, +1));
|
||
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(5,:), ...
|
||
|
|
'DisplayName', '$g_{opt}$');
|
||
|
|
hold off;
|
||
|
|
axis equal;
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
xlim([-160, 0]); ylim([0, 400]);
|
||
|
|
title('$m_3$');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_iff_root_locus.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_iff_root_locus
|
||
|
|
#+caption: Root Locus for IFF. Green crosses are closed-loop poles for the same choosen IFF gain.
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_iff_root_locus.png]]
|
||
|
|
|
||
|
|
*** TODO Verify Stability
|
||
|
|
Verify Stability with Nyquist plot:
|
||
|
|
|
||
|
|
- Why bad stability margins?
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(4, 6, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_iff_m0, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
|
||
|
|
Lmimo = pagemtimes(permute(G_iff_m1, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
|
||
|
|
Lmimo = pagemtimes(permute(G_iff_m2, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
|
||
|
|
Lmimo = pagemtimes(permute(G_iff_m3, [2,3,1]),squeeze(freqresp(Kiff, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Plot of the eigenvalues of L in the complex plane
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
for i_mass = 1:4
|
||
|
|
plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'DisplayName', sprintf('%i masses', i_mass));
|
||
|
|
plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
for i = 1:6
|
||
|
|
plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
legend('location', 'southeast');
|
||
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save Controller
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Kiff(1,1));
|
||
|
|
|
||
|
|
Kz = c2d(Kiff(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_iff.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./matlab/mat/K_iff.mat', 'Kiff')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Estimated Damped Plant
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Damped plant from Simscape model
|
||
|
|
Gm_hac_m0 = -feedback(Gm_m0, Kiff, 'name', +1);
|
||
|
|
Gm_hac_m1 = -feedback(Gm_m1, Kiff, 'name', +1);
|
||
|
|
Gm_hac_m2 = -feedback(Gm_m2, Kiff, 'name', +1);
|
||
|
|
Gm_hac_m3 = -feedback(Gm_m3, Kiff, 'name', +1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Verify Stability
|
||
|
|
isstable(Gm_hac_m0)
|
||
|
|
isstable(Gm_hac_m1)
|
||
|
|
isstable(Gm_hac_m2)
|
||
|
|
isstable(Gm_hac_m3)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Save Damped Plants
|
||
|
|
save('./matlab/mat/Gm.mat', 'Gm_hac_m0', 'Gm_hac_m1', 'Gm_hac_m2', 'Gm_hac_m3', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Estimated damped plant from the Simscape model
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - $m_3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_hac_damped_plant_estimated_simscape.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_hac_damped_plant_estimated_simscape
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_hac_damped_plant_estimated_simscape.png]]
|
||
|
|
|
||
|
|
|
||
|
|
* High Authority Control
|
||
|
|
<<sec:id31_iff_hac>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Identify Spurious modes
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load identification data
|
||
|
|
data = load(sprintf('%s/dynamics/2023-08-10_18-32_identify_spurious_modes.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[G1, f] = tfestimate(data.id_plant, data.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[G2, ~] = tfestimate(data.id_plant, data.d2, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[G3, ~] = tfestimate(data.id_plant, data.d3, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[G4, ~] = tfestimate(data.id_plant, data.d4, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[G5, ~] = tfestimate(data.id_plant, data.d5, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G1), 'DisplayName', '1 - top');
|
||
|
|
plot(f, abs(G2), 'DisplayName', '2 - bot');
|
||
|
|
plot(f, abs(G3), 'DisplayName', '3 - top');
|
||
|
|
plot(f, abs(G4), 'DisplayName', '4 - bot');
|
||
|
|
plot(f, abs(G5), 'DisplayName', '5 - vertical');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
||
|
|
xlim([500, 800])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** HAC Plants
|
||
|
|
*** Introduction :ignore:
|
||
|
|
|
||
|
|
*** 6x6 Plant
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load identification data
|
||
|
|
data = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data.uL1.id_plant, data.uL1.e_L1, win, [], [], 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Plant
|
||
|
|
G_hac = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data.(sprintf("uL%i", i_strut)).e_L1 ; data.(sprintf("uL%i", i_strut)).e_L2 ; data.(sprintf("uL%i", i_strut)).e_L3 ; data.(sprintf("uL%i", i_strut)).e_L4 ; data.(sprintf("uL%i", i_strut)).e_L5 ; data.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_hac(:,:,i_strut) = tfestimate(data.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, [], [], 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_hac(:,i, i)), 'color', colors(i,:), ...
|
||
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac(:,i, 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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compare with Model:
|
||
|
|
#+begin_src matlab
|
||
|
|
load('Gm.mat');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_hac(:,1,2)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', 'Identified, Off-Diagonal');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_hac(:, i, j)), 'color', [colors(3,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', 'Model, Off-Diagonal');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(4,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac(:,1,1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', 'Identified, Diagonal');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', 'Model, Diagonal');
|
||
|
|
for i = 2:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac(:,i, i)), 'color', [colors(1,:), 0.5])
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i)), freqs, 'Hz'))), 'color', [colors(2,:), 0.5])
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Ghac_6x6_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Ghac_6x6_plant_comp_model
|
||
|
|
#+caption: 6x6 plant from generated voltages to displacement of the struts as measured by the external metrology
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Ghac_6x6_plant_comp_model.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** Effect of Mass
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load identification data
|
||
|
|
data_m0 = load(sprintf('%s/dynamics/2023-08-17_17-53_damp_plant_m0_Wz0.mat', mat_dir));
|
||
|
|
data_m1 = load(sprintf('%s/dynamics/2023-08-10_16-01_damp_plant_m1_Wz0.mat', mat_dir));
|
||
|
|
data_m2 = load(sprintf('%s/dynamics/2023-08-10_17-28_damp_plant_m2_Wz0.mat', mat_dir));
|
||
|
|
data_m3 = load(sprintf('%s/dynamics/2023-08-10_18-16_damp_plant_m3_Wz0.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(2.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.e_L1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Plant (transfer function from u to taum)
|
||
|
|
G_hac_m0 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m0.(sprintf("uL%i", i_strut)).e_L1 ; data_m0.(sprintf("uL%i", i_strut)).e_L2 ; data_m0.(sprintf("uL%i", i_strut)).e_L3 ; data_m0.(sprintf("uL%i", i_strut)).e_L4 ; data_m0.(sprintf("uL%i", i_strut)).e_L5 ; data_m0.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Plant (transfer function from u to taum)
|
||
|
|
G_hac_m1 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m1.(sprintf("uL%i", i_strut)).e_L1 ; data_m1.(sprintf("uL%i", i_strut)).e_L2 ; data_m1.(sprintf("uL%i", i_strut)).e_L3 ; data_m1.(sprintf("uL%i", i_strut)).e_L4 ; data_m1.(sprintf("uL%i", i_strut)).e_L5 ; data_m1.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Plant (transfer function from u to taum)
|
||
|
|
G_hac_m2 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m2.(sprintf("uL%i", i_strut)).e_L1 ; data_m2.(sprintf("uL%i", i_strut)).e_L2 ; data_m2.(sprintf("uL%i", i_strut)).e_L3 ; data_m2.(sprintf("uL%i", i_strut)).e_L4 ; data_m2.(sprintf("uL%i", i_strut)).e_L5 ; data_m2.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Plant (transfer function from u to taum)
|
||
|
|
G_hac_m3 = zeros(length(f), 6, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
eL = [data_m3.(sprintf("uL%i", i_strut)).e_L1 ; data_m3.(sprintf("uL%i", i_strut)).e_L2 ; data_m3.(sprintf("uL%i", i_strut)).e_L3 ; data_m3.(sprintf("uL%i", i_strut)).e_L4 ; data_m3.(sprintf("uL%i", i_strut)).e_L5 ; data_m3.(sprintf("uL%i", i_strut)).e_L6]';
|
||
|
|
|
||
|
|
G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, -detrend(eL, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Save Identified Plants
|
||
|
|
save('./matlab/mat/G_hac.mat', 'G_hac_m0', 'G_hac_m1', 'G_hac_m2', 'G_hac_m3', 'f');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer functions
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Ghac_effect_mass.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Ghac_effect_mass
|
||
|
|
#+caption: Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Ghac_effect_mass.png]]
|
||
|
|
|
||
|
|
*** Compare with the model
|
||
|
|
#+begin_src matlab
|
||
|
|
load('G_hac.mat')
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 0$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m1(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 1$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m2(:, 1, 1)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 2$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m3(:, 1, 1)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 3$');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:), ...
|
||
|
|
'DisplayName', '$m = 0$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:), ...
|
||
|
|
'DisplayName', '$m = 1$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:), ...
|
||
|
|
'DisplayName', '$m = 2$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:), ...
|
||
|
|
'DisplayName', '$m = 3$ - Model');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,1,1)), 'color', [colors(1,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(f, 180/pi*angle(G_hac_m1(:,1,1)), 'color', [colors(2,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m1(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(f, 180/pi*angle(G_hac_m2(:,1,1)), 'color', [colors(3,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m2(:,i, i)), 'color', [colors(3,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(f, 180/pi*angle(G_hac_m3(:,1,1)), 'color', [colors(4,:), 0.5]);
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m3(:,i, i)), 'color', [colors(4,:), 0.5]);
|
||
|
|
end
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(1,:));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(2,:));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m2('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(3,:));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m3('eL1', 'u1'), freqs, 'Hz'))), '--', 'color', colors(4,:));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_Ghac_plant_comp_model.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_Ghac_plant_comp_model
|
||
|
|
#+caption: Comparison of the identified HAC plant and the HAC plant extracted from the simscape model
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_Ghac_plant_comp_model.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** Comparison with Undamped plant
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
load('G_ol.mat');
|
||
|
|
load('G_hac.mat');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_int_m0(:, 1, 1)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 0$ - OL');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m0(:, 1, 1)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'DisplayName', '$m = 0$ - Damped');
|
||
|
|
for i = 2:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5], ...
|
||
|
|
'HandleVisibility', 'off')
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(-G_int_m0(:,i, i)), 'color', [colors(1,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,i, i)), 'color', [colors(2,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_comp_undamped_damped_plant_m0.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_comp_undamped_damped_plant_m0
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_comp_undamped_damped_plant_m0.png]]
|
||
|
|
|
||
|
|
** Robust HAC
|
||
|
|
#+begin_src matlab
|
||
|
|
load('G_hac.mat')
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Controller design
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*5;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
% H_int = (wc^2)/(s + 1e-1*2*pi)^2;
|
||
|
|
H_int = wc/s;
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/30);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Integrator
|
||
|
|
eye(6); % 6x6 Diagonal
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Loop gain
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$m_0$');
|
||
|
|
plot(f, abs(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$m_1$');
|
||
|
|
plot(f, abs(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$m_2$');
|
||
|
|
plot(f, abs(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:), 'DisplayName', '$m_3$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
ylim([1e-5, 1e2]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(1,:));
|
||
|
|
plot(f, 180/pi*angle(G_hac_m1(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(2,:));
|
||
|
|
plot(f, 180/pi*angle(G_hac_m2(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(3,:));
|
||
|
|
plot(f, 180/pi*angle(G_hac_m3(:,1, 1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))), 'color', colors(4,:));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_hac_robust_loop_gain.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_hac_robust_loop_gain
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_hac_robust_loop_gain.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** Verify Stability
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(4, 6, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(1,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
|
||
|
|
Lmimo = pagemtimes(permute(G_hac_m1, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(2,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
|
||
|
|
Lmimo = pagemtimes(permute(G_hac_m2, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(3,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
|
||
|
|
Lmimo = pagemtimes(permute(G_hac_m3, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(4,:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Plot of the eigenvalues of L in the complex plane
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
for i_mass = 1:4
|
||
|
|
plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'DisplayName', sprintf('$m_%i$', i_mass));
|
||
|
|
plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
for i = 1:6
|
||
|
|
plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ...
|
||
|
|
'.', 'color', colors(i_mass, :), ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
legend('location', 'southeast');
|
||
|
|
axis square
|
||
|
|
xlim([-1.5, 0.5]); ylim([-1, 1]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_hac_robust_nyquist.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_hac_robust_nyquist
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_hac_robust_nyquist.png]]
|
||
|
|
|
||
|
|
*** Estimated performances
|
||
|
|
|
||
|
|
*** Save Controller
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(1,1));
|
||
|
|
|
||
|
|
Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./matlab/mat/K_hac.mat', 'Khac')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
** High Performance HAC
|
||
|
|
*** Introduction :ignore:
|
||
|
|
The goal is to make a controller specific for one mass in order to have high bandwidth.
|
||
|
|
|
||
|
|
*** Mass 0
|
||
|
|
**** Load Plant
|
||
|
|
#+begin_src matlab
|
||
|
|
load('G_hac.mat')
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Plant
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_hac_m0(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i, i)), 'color', colors(i,:), ...
|
||
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m0(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--', ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Model');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,i, i)));
|
||
|
|
end
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0('eL1', 'u1'), freqs, 'Hz'))), 'k--');
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Controller design
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*50;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 0.1*2*pi) * ...
|
||
|
|
1/(0.1*2*pi + s);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
% H_int = wc/s;
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 3; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
a = 3; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead/sqrt(a)*(1 + s/(2.5*wc/sqrt(a)))/(1 + s/(2.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/500);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead * ... % Integrator
|
||
|
|
eye(6); % 6x6 Diagonal
|
||
|
|
|
||
|
|
Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
||
|
|
Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Loop gain
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Loop gain for the High Authority Control
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_high_perf_hac_m0_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_high_perf_hac_m0_loop_gain
|
||
|
|
#+caption: Loop gain for the High Authority Control
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_high_perf_hac_m0_loop_gain.png]]
|
||
|
|
|
||
|
|
**** Verify Stability
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(6, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Plot of the eigenvalues of L in the complex plane
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.');
|
||
|
|
plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
|
||
|
|
for i = 1:6
|
||
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
|
||
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_high_perf_hac_m0_nyquist.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_high_perf_hac_m0_nyquist
|
||
|
|
#+caption: Nyquist plot for the High Authority Control
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_high_perf_hac_m0_nyquist.png]]
|
||
|
|
|
||
|
|
**** Estimated performances
|
||
|
|
Loop gain with model
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cl_m0 = feedback(Gm_hac_m0, 0.8*Khac, 'name', -1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
isstable(Gm_cl_m0)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Save Controller
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(1,1));
|
||
|
|
|
||
|
|
Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_m0.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Experimental Validation
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_1rpm = load(sprintf('%s/scans/2023-08-18_10-43_m0_1rpm.mat', mat_dir));
|
||
|
|
data_30rpm = load(sprintf('%s/scans/2023-08-18_10-45_m0_30rpm.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([rms(1e9*data_1rpm.Dy_int), rms(1e9*data_1rpm.Dz_int), rms(1e6*data_1rpm.Ry_int); rms(1e9*data_30rpm.Dy_int), rms(1e9*data_30rpm.Dz_int), rms(1e6*data_30rpm.Ry_int)], {'1rpm', '30rpm'}, {'Dy [nm]', 'Dz [nm]', 'Ry [urad]'}, ' %.1f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dy [nm] | Dz [nm] | Ry [urad] |
|
||
|
|
|-------+---------+---------+-----------|
|
||
|
|
| 1rpm | 55.3 | 5.9 | 0.1 |
|
||
|
|
| 30rpm | 85.2 | 12.5 | 0.3 |
|
||
|
|
|
||
|
|
**** Closed-Loop identification
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-18_11-03_m0_perf_hac.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** Mass 1
|
||
|
|
**** Load Plant
|
||
|
|
#+begin_src matlab
|
||
|
|
load('G_hac.mat')
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Plant
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_hac_m1(:, i, j)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_hac_m1(:,i, i)), 'color', colors(i,:), ...
|
||
|
|
'DisplayName', sprintf('$\\tau_{m,%i}/u_%i$', i, i));
|
||
|
|
end
|
||
|
|
plot(f, abs(G_hac_m1(:, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--', ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL2', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'DisplayName', '$\tau_{m,i}/u_j$ - Model');
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1(sprintf('eL%i', i), sprintf('u%i', j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i =1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m1(:,i, i)));
|
||
|
|
end
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))), 'k--');
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Plant Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_hac_red_m1 = flipRphZeros(-balred(Gm_hac_m1('eL1', 'u1'), 6, ...
|
||
|
|
balredOptions('StateProjection', 'MatchDC', ...
|
||
|
|
'FreqIntervals', [0, 80])));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_red_m1, freqs, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m1('eL1', 'u1'), freqs, 'Hz'))));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_red_m1, freqs, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Plant Inverse
|
||
|
|
Gm_hac_inv_m1 = inv(Gm_hac_red_m1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz'))));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_inv_m1, freqs, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(inv(Gm_hac_m1('eL1', 'u1')), freqs, 'Hz'))));
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_inv_m1, freqs, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Controller design
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*30;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 0.1*2*pi) * ...
|
||
|
|
(50*2*pi + s)/(0.01*2*pi + s);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
% H_int = wc/s;
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/200);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = 1./abs(G_hac_m0(i_f, 1, 1));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac = 0.8*H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead * ... % Integrator
|
||
|
|
eye(6); % 6x6 Diagonal
|
||
|
|
|
||
|
|
Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
||
|
|
Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Loop gain
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(f, abs(G_hac_m0(:,i,j).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(f, 180/pi*angle(G_hac_m0(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Verify Stability
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(6, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_hac_m0, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Plot of the eigenvalues of L in the complex plane
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.');
|
||
|
|
plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
|
||
|
|
for i = 1:6
|
||
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
|
||
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Estimated performances
|
||
|
|
Loop gain with model
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
end
|
||
|
|
for i = 1:5
|
||
|
|
for j = i+1:6
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', j))*Khac(i,i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:6
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm_hac_m0(sprintf('eL%i', i), sprintf('u%i', i))*Khac(i,i), freqs, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cl_m0 = feedback(Gm_hac_m0, Khac, 'name', +1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
isstable(Gm_hac_m0)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Save Controller
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(1,1));
|
||
|
|
|
||
|
|
Kz = c2d(Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_m0.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Tomography - Performances
|
||
|
|
*** First scan with closed-loop at middle
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data = load(sprintf('%s/scans/2023-08-17_15-22_tomography_30rpm_m0_robust.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
writerObj = VideoWriter('test2.avi'); %// initialize the VideoWriter object
|
||
|
|
open(writerObj) ;
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
di = 500;
|
||
|
|
hold on;
|
||
|
|
for i = 1:floor(length(data.Dx_int)/di)-1
|
||
|
|
if data.hac_status(di*(i+1)-1) == 0
|
||
|
|
% Only open loop
|
||
|
|
plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
|
||
|
|
elseif data.hac_status(di*i) == 1
|
||
|
|
% Only closed loop
|
||
|
|
plot(1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
|
||
|
|
else
|
||
|
|
% Both open and closed loop
|
||
|
|
Dx_int = data.Dx_int(di*i:di*(i+1)-1);
|
||
|
|
Dy_int = data.Dy_int(di*i:di*(i+1)-1);
|
||
|
|
plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
|
||
|
|
plot(1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
|
||
|
|
end
|
||
|
|
axis square
|
||
|
|
xlim([-3, 3])
|
||
|
|
ylim([-3, 3])
|
||
|
|
xlabel("X motion [$\mu m$]");
|
||
|
|
ylabel("Y motion [$\mu m$]");
|
||
|
|
F = getframe; %// Capture the frame
|
||
|
|
writeVideo(writerObj,F) %// add the frame to the movie
|
||
|
|
end
|
||
|
|
close(writerObj);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ...
|
||
|
|
'DisplayName', 'CL')
|
||
|
|
% Get first time where closed-loop ON
|
||
|
|
[~, i] = find(data.hac_status == 1);
|
||
|
|
plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ...
|
||
|
|
'DisplayName', 'CL, stabilized')
|
||
|
|
hold off;
|
||
|
|
axis equal
|
||
|
|
xlim([-3, 3])
|
||
|
|
ylim([-3, 3])
|
||
|
|
xticks([-3:1:3])
|
||
|
|
yticks([-3:1:3])
|
||
|
|
xlabel("X motion [$\mu m$]");
|
||
|
|
ylabel("Y motion [$\mu m$]");
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data.Dx_int(data.hac_status == 0), 1e6*data.Dy_int(data.hac_status == 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data.Dx_int(data.hac_status == 1), 1e6*data.Dy_int(data.hac_status == 1), ...
|
||
|
|
'DisplayName', 'CL')
|
||
|
|
% Get first time where closed-loop ON
|
||
|
|
[~, i] = find(data.hac_status == 1);
|
||
|
|
plot(1e6*data.Dx_int(i+50000:end), 1e6*data.Dy_int(i+50000:end), ...
|
||
|
|
'DisplayName', 'CL, stabilized')
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
hold off;
|
||
|
|
axis equal
|
||
|
|
xlabel("X motion [$\mu m$]");
|
||
|
|
ylabel("Y motion [$\mu m$]");
|
||
|
|
xlim([-0.3, 0.3])
|
||
|
|
ylim([-0.3, 0.3])
|
||
|
|
xticks([-0.3:0.1:0.3])
|
||
|
|
yticks([-0.3:0.1:0.3])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_tomography_ol_cl_robust_hac_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_tomography_ol_cl_robust_hac_m0
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_tomography_ol_cl_robust_hac_m0.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** Slow Rotation - 6RPM
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load measured noise
|
||
|
|
data_ol = load(sprintf('%s/freq_analysis/2023-08-11_17-18_m0_lac_off_1rpm.mat', mat_dir));
|
||
|
|
data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-16_m0_lac_on_1rpm.mat', mat_dir));
|
||
|
|
data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-14_m0_hac_on_1rpm.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Coordinate transform
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
|
||
|
|
data_ol.Dx_int = a(1,:);
|
||
|
|
data_ol.Dy_int = a(2,:);
|
||
|
|
data_ol.Dz_int = a(3,:);
|
||
|
|
data_ol.Rx_int = a(4,:);
|
||
|
|
data_ol.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
|
||
|
|
data_lac.Dx_int = a(1,:);
|
||
|
|
data_lac.Dy_int = a(2,:);
|
||
|
|
data_lac.Dz_int = a(3,:);
|
||
|
|
data_lac.Rx_int = a(4,:);
|
||
|
|
data_lac.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
|
||
|
|
data_hac.Dx_int = a(1,:);
|
||
|
|
data_hac.Dy_int = a(2,:);
|
||
|
|
data_hac.Dz_int = a(3,:);
|
||
|
|
data_hac.Rx_int = a(4,:);
|
||
|
|
data_hac.Ry_int = a(5,:);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(30.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
|
||
|
|
[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Dy, ~ ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Dz, ~ ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Rx, ~ ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Ry, ~ ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
|
||
|
|
plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([1, 5e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), 'DisplayName', '$D_y$ - OL');
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), 'DisplayName', '$D_y$ - LAC');
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), 'DisplayName', '$D_y$ - HAC-LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [$m$]');
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
% xlim([0.1, 5e3]); ylim([1e-10, 1e-7]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Rapid Rotation - 30RPM
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load measured noise
|
||
|
|
data_ol = load(sprintf('%s/freq_analysis/2023-08-11_17-39_m0_lac_off_30rpm.mat', mat_dir));
|
||
|
|
data_lac = load(sprintf('%s/freq_analysis/2023-08-11_17-36_m0_lac_on_30rpm.mat', mat_dir));
|
||
|
|
data_hac = load(sprintf('%s/freq_analysis/2023-08-11_17-34_m0_hac_on_30rpm.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Coordinate transform
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_ol.d1; data_ol.d2; data_ol.d3; data_ol.d4; data_ol.d5];
|
||
|
|
data_ol.Dx_int = a(1,:);
|
||
|
|
data_ol.Dy_int = a(2,:);
|
||
|
|
data_ol.Dz_int = a(3,:);
|
||
|
|
data_ol.Rx_int = a(4,:);
|
||
|
|
data_ol.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_lac.d1; data_lac.d2; data_lac.d3; data_lac.d4; data_lac.d5];
|
||
|
|
data_lac.Dx_int = a(1,:);
|
||
|
|
data_lac.Dy_int = a(2,:);
|
||
|
|
data_lac.Dz_int = a(3,:);
|
||
|
|
data_lac.Rx_int = a(4,:);
|
||
|
|
data_lac.Ry_int = a(5,:);
|
||
|
|
|
||
|
|
a = J_int_to_X*[data_hac.d1; data_hac.d2; data_hac.d3; data_hac.d4; data_hac.d5];
|
||
|
|
data_hac.Dx_int = a(1,:);
|
||
|
|
data_hac.Dy_int = a(2,:);
|
||
|
|
data_hac.Dz_int = a(3,:);
|
||
|
|
data_hac.Rx_int = a(4,:);
|
||
|
|
data_hac.Ry_int = a(5,:);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(20.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
|
||
|
|
[data_ol.pxx_Dx, data_ol.f] = pwelch(detrend(data_ol.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dy, ~ ] = pwelch(detrend(data_ol.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Dz, ~ ] = pwelch(detrend(data_ol.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Rx, ~ ] = pwelch(detrend(data_ol.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_ol.pxx_Ry, ~ ] = pwelch(detrend(data_ol.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_lac.pxx_Dx, data_lac.f] = pwelch(detrend(data_lac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dy, ~ ] = pwelch(detrend(data_lac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Dz, ~ ] = pwelch(detrend(data_lac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Rx, ~ ] = pwelch(detrend(data_lac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_lac.pxx_Ry, ~ ] = pwelch(detrend(data_lac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_hac.pxx_Dx, data_hac.f] = pwelch(detrend(data_hac.Dx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Dy, ~ ] = pwelch(detrend(data_hac.Dy_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Dz, ~ ] = pwelch(detrend(data_hac.Dz_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Rx, ~ ] = pwelch(detrend(data_hac.Rx_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_hac.pxx_Ry, ~ ] = pwelch(detrend(data_hac.Ry_int, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f , sqrt(data_ol.pxx_Dy ), 'DisplayName', '$D_y$ - OL');
|
||
|
|
plot(data_lac.f, sqrt(data_lac.pxx_Dy), 'DisplayName', '$D_y$ - LAC');
|
||
|
|
plot(data_hac.f, sqrt(data_hac.pxx_Dy), 'DisplayName', '$D_y$ - HAC-LAC');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [$V/\sqrt{Hz}$]');
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([1, 5e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Cumulative Amplitude Spectrum of the measured Dx and Dy motion
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dy)))), ...
|
||
|
|
'DisplayName', sprintf('OL: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_ol.Dy_int, 0))));
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dy)))), ...
|
||
|
|
'DisplayName', sprintf('LAC: $%.1f \\mu m$ RMS', 1e6*rms(detrend(data_lac.Dy_int, 0))));
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dy)))), ...
|
||
|
|
'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dy_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('CAS [m rms, rad RMS]');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xticks([1e0, 1e1, 1e2]);
|
||
|
|
title('$D_y$')
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Dz)))), ...
|
||
|
|
'DisplayName', sprintf('OL: $%.0f nm$ RMS', 1e9*rms(detrend(data_ol.Dz_int, 0))));
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Dz)))), ...
|
||
|
|
'DisplayName', sprintf('LAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_lac.Dz_int, 0))));
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Dz)))), ...
|
||
|
|
'DisplayName', sprintf('HAC: $%.0f nm$ RMS', 1e9*rms(detrend(data_hac.Dz_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xticks([1e0, 1e1, 1e2]);
|
||
|
|
title('$D_z$')
|
||
|
|
|
||
|
|
ax3 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(data_ol.f, sqrt(flip(-cumtrapz(flip(data_ol.f), flip(data_ol.pxx_Ry)))), ...
|
||
|
|
'DisplayName', sprintf('OL: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_ol.Ry_int, 0))));
|
||
|
|
plot(data_lac.f, sqrt(flip(-cumtrapz(flip(data_lac.f), flip(data_lac.pxx_Ry)))), ...
|
||
|
|
'DisplayName', sprintf('LAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_lac.Ry_int, 0))));
|
||
|
|
plot(data_hac.f, sqrt(flip(-cumtrapz(flip(data_hac.f), flip(data_hac.pxx_Ry)))), ...
|
||
|
|
'DisplayName', sprintf('HAC: $%.1f \\mu$radRMS', 1e6*rms(detrend(data_hac.Ry_int, 0))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xticks([1e0, 1e1, 1e2]);
|
||
|
|
title('$R_y$')
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3],'xy');
|
||
|
|
xlim([0.1, 5e2]); ylim([1e-10, 3e-5]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_cas_m0_30rpm_ol_lac_hac.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_cas_m0_30rpm_ol_lac_hac
|
||
|
|
#+caption: Cumulative Amplitude Spectrum of the errors in $D_y$, $D_z$ and $R_y$ during a tomography scan at 30RPM. Three control configuration are compared: Open-Loop, Low Authority Control, and High Authority Control
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_cas_m0_30rpm_ol_lac_hac.png]]
|
||
|
|
|
||
|
|
|
||
|
|
* 6DoF Control in Cartesian plane (rotating with the nano-hexapod)
|
||
|
|
<<sec:id31_cart_hac_rot>>
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Introduction :ignore:
|
||
|
|
As only Dy, Dz and Ry directions are important, we could only control them.
|
||
|
|
This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
|
||
|
|
|
||
|
|
** 5x5 plant in Cartesian plane
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Load model
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Jacobian for 3x3 plant
|
||
|
|
n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
Jt_inv = pinv(n_hexapod.geometry.J');
|
||
|
|
Jt_inv = Jt_inv(:,[1,2,3,4,5]);
|
||
|
|
|
||
|
|
J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute identified plant in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir));
|
||
|
|
%% New data after calibrating the Rz-offset
|
||
|
|
data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Cartesian Plant
|
||
|
|
G_hac_m0 = zeros(length(f), 5, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
|
||
|
|
|
||
|
|
G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
|
||
|
|
G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv)), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute plant model in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv;
|
||
|
|
Gm_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My'};
|
||
|
|
Gm_cart.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 5x5 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1)), 'DisplayName', '$D_x$');
|
||
|
|
plot(f, abs(G_cart(:,2,2)), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(G_cart(:,3,3)), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(G_cart(:,4,4)), 'DisplayName', '$R_x$');
|
||
|
|
plot(f, abs(G_cart(:,5,5)), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
% ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,1,1)));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,2,2)));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,3,3)));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,4,4)));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,5,5)));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Controller Design
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*30;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 1*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,1,1));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Dx = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*30;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 1*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,2,2));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Dy = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*40;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 2*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,3,3));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Dz = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*10;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 1.5*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,4,4));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Rx = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*10;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 1.5*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,5,5));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Ry = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
Khac = blkdiag(Khac_Dx, Khac_Dy, Khac_Dz, Khac_Rx, Khac_Ry);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))));
|
||
|
|
plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz'))));
|
||
|
|
plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz'))));
|
||
|
|
plot(f, abs(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz'))));
|
||
|
|
plot(f, abs(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac(1,1), f, 'Hz'))));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac(2,2), f, 'Hz'))));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac(3,3), f, 'Hz'))));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,4,4).*squeeze(freqresp(Khac(4,4), f, 'Hz'))));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,5,5).*squeeze(freqresp(Khac(5,5), f, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Check Stability
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(5, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Plot of the eigenvalues of L in the complex plane
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.')
|
||
|
|
plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.')
|
||
|
|
for i = 1:size(Ldet,1)
|
||
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.')
|
||
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.')
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Save controllers
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(1,1));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dx.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(2,2));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(2,2)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dy.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(3,3));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(3,3)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dz.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(4,4));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(4,4)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Rx.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(5,5));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(5,5)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Ry.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Performances
|
||
|
|
2023-08-18_18-33_m0_1rpm_K_cart.mat
|
||
|
|
|
||
|
|
* 3DoF Control in Cartesian plane (fixed)
|
||
|
|
<<sec:id31_cart_hac_fix>>
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** Introduction :ignore:
|
||
|
|
As only Dy, Dz and Ry directions are important, we could only control them.
|
||
|
|
This lead to a 3x3 plant that may be more decoupled than the 6x6 plant.
|
||
|
|
|
||
|
|
** 3x3 plant in Cartesian plane
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Load model
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Jacobian for 3x3 plant
|
||
|
|
n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
Jt_inv = pinv(n_hexapod.geometry.J');
|
||
|
|
|
||
|
|
J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute identified plant in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
% data_m0 = load(sprintf('%s/dynamics/2023-08-17_10-44_lac_plant_m0_Wz0_interf.mat', mat_dir));
|
||
|
|
%% New data after calibrating the Rz-offset
|
||
|
|
data_m0 = load(sprintf('%s/dynamics/2023-08-17_16-23_lac_plant_m0_Wz0_interf_Rz_align.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Cartesian Plant
|
||
|
|
G_hac_m0 = zeros(length(f), 5, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
|
||
|
|
|
||
|
|
G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
|
||
|
|
G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute plant model in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
|
||
|
|
Gm_cart.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_important
|
||
|
|
Diagonal elements are matching quite well, but off-diagonal elements are very different.
|
||
|
|
|
||
|
|
Why so much more coupling than from the model?
|
||
|
|
- Is it due to the metrology? The spheres could induce coupling as for instance X motion will also be seen as Z motion.
|
||
|
|
This is especially true if not well centered with the sphere (as seemed to be the case for the lateral interferometers).
|
||
|
|
#+end_important
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 cartesian plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-9, 1e-2]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_cart_plant_3x3.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_cart_plant_3x3
|
||
|
|
#+caption: 3x3 cartesian plant
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_cart_plant_3x3.png]]
|
||
|
|
|
||
|
|
|
||
|
|
Normalization of outputs:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_normalized = -diag(1./diag(dcgain(Gm_cart)))*Gm_cart;
|
||
|
|
Gm_cart_normalized.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
|
||
|
|
G_cart_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart))), permute(G_cart, [2,3,1])), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_normalized('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_normalized(:,1,1)));
|
||
|
|
plot(f, abs(G_cart_normalized(:,2,2)));
|
||
|
|
plot(f, abs(G_cart_normalized(:,3,3)));
|
||
|
|
plot(f, abs(G_cart_normalized(:,1,2)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_normalized(:,1,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_normalized(:,2,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_normalized(:,3,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_normalized(:,2,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_normalized(:,3,2)), 'color', [0,0,0,0.2]);
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_normalized(:,1,1)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_normalized(:,2,2)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_normalized(:,3,3)));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Controller Design
|
||
|
|
*** Dy
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*30;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 0.1*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,1,1));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Dy = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Dz
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*50;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 0.1*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,2,2));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Dz = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Ry
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*30;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 0.1*2*pi) * ...
|
||
|
|
1/(s + 0.01*2*pi);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = H_lead*1/sqrt(a)*(1 + s/(1.5*wc/sqrt(a)))/(1 + s/(1.5*wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/300);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = -1./abs(G_cart(i_f,3,3));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac_Ry = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead; % Integrator
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** 3x3 controller
|
||
|
|
#+begin_src matlab
|
||
|
|
Khac = blkdiag(Khac_Dy, Khac_Dz, Khac_Ry);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,1,1).*squeeze(freqresp(Khac_Dy, f, 'Hz'))));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,2,2).*squeeze(freqresp(Khac_Dz, f, 'Hz'))));
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,3,3).*squeeze(freqresp(Khac_Ry, f, 'Hz'))));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/G_cart_loop_gain_diagonal_3dof.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:G_cart_loop_gain_diagonal_3dof
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/G_cart_loop_gain_diagonal_3dof.png]]
|
||
|
|
|
||
|
|
|
||
|
|
** Check Stability
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(3, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_cart, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.')
|
||
|
|
plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.')
|
||
|
|
for i = 1:3
|
||
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.')
|
||
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.')
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/G_cart_nyquist_diagonal_3dof.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:G_cart_nyquist_diagonal_3dof
|
||
|
|
#+caption: description
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/G_cart_nyquist_diagonal_3dof.png]]
|
||
|
|
|
||
|
|
** Save controllers
|
||
|
|
*** Save Controller
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(1,1));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(1,1)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dy_3x3.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(2,2));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(2,2)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Dz_3x3.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K_order = order(Khac(3,3));
|
||
|
|
|
||
|
|
Kz = c2d(-Khac(3,3)*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/K_hac_Ry_3x3.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Controller Design (normalized)
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Wanted crossover
|
||
|
|
wc = 2*pi*30;
|
||
|
|
|
||
|
|
%% Double Integrator
|
||
|
|
H_int = 1/(s + 0.1*2*pi) * ...
|
||
|
|
(50*2*pi + s)/(0.01*2*pi + s);
|
||
|
|
H_int = H_int/abs(evalfr(H_int, 1j*wc));
|
||
|
|
% H_int = wc/s;
|
||
|
|
|
||
|
|
%% Lead to increase phase margin
|
||
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
% a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
||
|
|
% H_lead = H_lead*1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
||
|
|
|
||
|
|
%% Low Pass filter to increase robustness
|
||
|
|
H_lpf = 1/(1 + s/2/pi/200);
|
||
|
|
|
||
|
|
%% Notch at the top-plate resonance
|
||
|
|
% gm = 0.02;
|
||
|
|
% xi = 0.3;
|
||
|
|
% wn = 2*pi*665;
|
||
|
|
|
||
|
|
% H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
%% Gain to have unitary crossover at 30Hz
|
||
|
|
[~, i_f] = min(abs(f - wc/2/pi));
|
||
|
|
H_gain = 1./abs(G_cart_normalized(i_f, 1, 1));
|
||
|
|
|
||
|
|
%% Decentralized HAC
|
||
|
|
Khac = H_gain * ... % Gain
|
||
|
|
H_int * ... % Integrator
|
||
|
|
H_lpf * ... % Low Pass Filter
|
||
|
|
H_lead * ... % Integrator
|
||
|
|
eye(3); % 6x6 Diagonal
|
||
|
|
|
||
|
|
% Khac.InputName = {'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'};
|
||
|
|
% Khac.OutputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Loop Gain
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:3
|
||
|
|
plot(f, abs(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||
|
|
ylim([1e-4, 1e4]);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:3
|
||
|
|
plot(f, 180/pi*angle(G_cart_normalized(:,i,i).*squeeze(freqresp(Khac(i,i), f, 'Hz'))), 'color', [colors(i,:), 0.5]);
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Verify Stability
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Compute the Eigenvalues of the loop gain
|
||
|
|
Ldet = zeros(3, length(f));
|
||
|
|
|
||
|
|
% Loop gain
|
||
|
|
Lmimo = pagemtimes(permute(G_cart_normalized, [2,3,1]),squeeze(freqresp(Khac, f, 'Hz')));
|
||
|
|
for i_f = 2:length(f)
|
||
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Plot of the eigenvalues of L in the complex plane
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(real(squeeze(Ldet(1,:))), imag(squeeze(Ldet(1,:))), 'k.');
|
||
|
|
plot(real(squeeze(Ldet(1,:))), -imag(squeeze(Ldet(1,:))), 'k.');
|
||
|
|
for i = 1:3
|
||
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.');
|
||
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.');
|
||
|
|
end
|
||
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Real'); ylabel('Imag');
|
||
|
|
xlim([-3, 1]); ylim([-2, 2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
** Control Performances
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_10-36_m0_cart_fixed.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
- [ ] Compare with estimated performances
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
* Complementary Filter Control
|
||
|
|
<<sec:id31_cart_hac_complementary_filter>>
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** m0
|
||
|
|
*** 3x3 plant in Cartesian plane
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Load model
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Jacobian for 3x3 plant
|
||
|
|
n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
Jt_inv = pinv(n_hexapod.geometry.J');
|
||
|
|
|
||
|
|
J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute identified plant in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% New data after calibrating the Rz-offset
|
||
|
|
data_m0 = load(sprintf('%s/dynamics/2023-08-21_13-32_damp_plant_m0.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m0.uL1.id_plant, data_m0.uL1.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Cartesian Plant
|
||
|
|
G_hac_m0 = zeros(length(f), 5, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
Di = [data_m0.(sprintf("uL%i", i_strut)).d1 ; data_m0.(sprintf("uL%i", i_strut)).d2 ; data_m0.(sprintf("uL%i", i_strut)).d3 ; data_m0.(sprintf("uL%i", i_strut)).d4 ; data_m0.(sprintf("uL%i", i_strut)).d5]';
|
||
|
|
|
||
|
|
G_hac_m0(:,:,i_strut) = tfestimate(data_m0.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
|
||
|
|
G_cart = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m0, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute plant model in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart = J_int_to_X*Gm_hac_m0({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
|
||
|
|
Gm_cart.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-9, 1e-2]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dy', 'Fy'), freqs, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Dz', 'Fz'), freqs, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart('Ry', 'My'), freqs, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||
|
|
|
||
|
|
ylim([1e-9, 1e-2]);
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Plant Invert
|
||
|
|
Reduce model size
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_dy = flipRphZeros(balred(-Gm_cart('Dy', 'Fy'), 10));
|
||
|
|
Gm_cart_dz = flipRphZeros(balred(-Gm_cart('Dz', 'Fz'), 10));
|
||
|
|
Gm_cart_ry = flipRphZeros(balred(-Gm_cart('Ry', 'My'), 10));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Add first resonance
|
||
|
|
#+begin_src matlab :eval no
|
||
|
|
% gm = 200;
|
||
|
|
% xi = 0.003;
|
||
|
|
% wn = 2*pi*670;
|
||
|
|
|
||
|
|
% Gm_cart_dy = Gm_cart_dy*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
% gm = 200;
|
||
|
|
% xi = 0.003;
|
||
|
|
% wn = 2*pi*1086;
|
||
|
|
|
||
|
|
% Gm_cart_dz = Gm_cart_dz*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
|
||
|
|
% gm = 200;
|
||
|
|
% xi = 0.003;
|
||
|
|
% wn = 2*pi*670;
|
||
|
|
|
||
|
|
% Gm_cart_ry = Gm_cart_ry*(s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gm_cart_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gm_cart_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_comp_cart_plant_reduced_model.pdf', 'width', 'wide', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_comp_cart_plant_reduced_model
|
||
|
|
#+caption: Comparaison of the measured direct terms and the reduced order models
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_comp_cart_plant_reduced_model.png]]
|
||
|
|
|
||
|
|
|
||
|
|
Invert and make realizable
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_dy_inv = inv(Gm_cart_dy);
|
||
|
|
Gm_cart_dz_inv = inv(Gm_cart_dz);
|
||
|
|
Gm_cart_ry_inv = inv(Gm_cart_ry);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
isstable(Gm_cart_dy_inv)
|
||
|
|
isstable(Gm_cart_dz_inv)
|
||
|
|
isstable(Gm_cart_ry_inv)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save Plant Inverse
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Gm_cart_dy_inv;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Gm_cart_dz_inv;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Gm_cart_ry_inv;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4);
|
||
|
|
[num, den] = tfdata(Kz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Control Performances
|
||
|
|
**** 5Hz
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_10-59_m0_cf_5Hz.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
- [ ] Compare with estimated performances
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** 20Hz
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_11-04_m0_cf_20Hz.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
- [ ] Compare with estimated performances
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Different bandwidth for different directions
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_11-16_m0_cf_different.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
- [ ] Compare with estimated performances
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** Dz 25Hz
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dx, f] = tfestimate(data_cl.Dx.id_cl, data_cl.Dx.e_dx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dy, ~] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rx, ~] = tfestimate(data_cl.Rx.id_cl, data_cl.Rx.e_rx, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_rz, ~] = tfestimate(data_cl.Rz.id_cl, data_cl.Rz.e_rz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
- [ ] Compare with estimated performances
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** Better plant invert
|
||
|
|
**** Dy
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
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)
|
||
|
|
|
||
|
|
%% We define the number of iteration.
|
||
|
|
Niter = 100;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
N = 9; %Order of approximation
|
||
|
|
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dy)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dz
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
N = 9; %Order of approximation
|
||
|
|
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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],'x');
|
||
|
|
xlim([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dz)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Ry
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
N = 9; %Order of approximation
|
||
|
|
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_ry)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Invert plants
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,3)), '-');
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Save plant inverse
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dy;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dy, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_fit.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dz;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_fit.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_ry;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_ry, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_fit.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Digital Invert plants
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart(:,1,1))), '-', 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart(:,2,2))), '-', 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart(:,3,3))), '-', 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart(:,1,1)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart(:,2,2)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart(:,3,3)), '-');
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Control Performances
|
||
|
|
**** Better plant invert
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Scans with good controller
|
||
|
|
**** 1rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 796 | 20 | 8 | 8209 | 73 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** 30rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 820 | 39 | 13 | 7790 | 156 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** m1
|
||
|
|
*** 3x3 plant in Cartesian plane
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Load model
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Jacobian for 3x3 plant
|
||
|
|
n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
Jt_inv = pinv(n_hexapod.geometry.J');
|
||
|
|
|
||
|
|
J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute identified plant in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% New data after calibrating the Rz-offset
|
||
|
|
data_m1 = load(sprintf('%s/dynamics/2023-08-21_19-05_damp_plant_m1_new_Rz.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m1.uL1.id_plant, data_m1.uL1.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Cartesian Plant
|
||
|
|
G_hac_m1 = zeros(length(f), 5, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
Di = [data_m1.(sprintf("uL%i", i_strut)).d1 ; data_m1.(sprintf("uL%i", i_strut)).d2 ; data_m1.(sprintf("uL%i", i_strut)).d3 ; data_m1.(sprintf("uL%i", i_strut)).d4 ; data_m1.(sprintf("uL%i", i_strut)).d5]';
|
||
|
|
|
||
|
|
G_hac_m1(:,:,i_strut) = tfestimate(data_m1.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
|
||
|
|
G_cart_m1 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m1, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute plant model in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_m1 = J_int_to_X*Gm_hac_m1({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
|
||
|
|
Gm_cart_m1.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_m1.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m1('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-9, 1e-2]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Normalization of outputs:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_m1_normalized = -diag(1./diag(dcgain(Gm_cart_m1)))*Gm_cart_m1;
|
||
|
|
Gm_cart_m1_normalized.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_m1_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
|
||
|
|
G_cart_m1_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m1))), permute(G_cart_m1, [2,3,1])), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,1,1)));
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,2,2)));
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,3,3)));
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,1,2)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,1,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,2,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,3,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,2,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m1_normalized(:,3,2)), 'color', [0,0,0,0.2]);
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m1_normalized(:,1,1)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_m1_normalized(:,2,2)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_m1_normalized(:,3,3)));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Better plant invert
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
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)
|
||
|
|
|
||
|
|
%% We define the number of iteration.
|
||
|
|
Niter = 100;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
N = 9; %Order of approximation
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dy
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-2;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dy)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dz
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./sqrt(f(f>2&f<1500))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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],'x');
|
||
|
|
xlim([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dz)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Ry
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m1(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_ry = inv(flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_ry)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Invert plants
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m1(:,1,1))), '-', 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m1(:,2,2))), '-', 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m1(:,3,3))), '-', 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m1(:,1,1)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m1(:,2,2)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m1(:,3,3)), '-');
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Save plant inverse
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dy;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dy, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m1.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dz;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m1.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_ry;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_ry, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m1.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Digital Invert plants
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart_m1(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m1(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m1(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m1(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m1(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m1(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Control Performances
|
||
|
|
**** Better plant invert
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Scans with good controller
|
||
|
|
**** 1rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 796 | 20 | 8 | 8209 | 73 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** 30rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 820 | 39 | 13 | 7790 | 156 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** m2
|
||
|
|
*** 3x3 plant in Cartesian plane
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Load model
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Jacobian for 3x3 plant
|
||
|
|
n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
Jt_inv = pinv(n_hexapod.geometry.J');
|
||
|
|
|
||
|
|
J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute identified plant in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% New data after calibrating the Rz-offset
|
||
|
|
data_m2 = load(sprintf('%s/dynamics/2023-08-21_17-32_damp_plant_m2_new_Rz.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m2.uL1.id_plant, data_m2.uL1.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Cartesian Plant
|
||
|
|
G_hac_m2 = zeros(length(f), 5, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
Di = [data_m2.(sprintf("uL%i", i_strut)).d1 ; data_m2.(sprintf("uL%i", i_strut)).d2 ; data_m2.(sprintf("uL%i", i_strut)).d3 ; data_m2.(sprintf("uL%i", i_strut)).d4 ; data_m2.(sprintf("uL%i", i_strut)).d5]';
|
||
|
|
|
||
|
|
G_hac_m2(:,:,i_strut) = tfestimate(data_m2.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
|
||
|
|
G_cart_m2 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m2, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute plant model in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_m2 = J_int_to_X*Gm_hac_m2({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
|
||
|
|
Gm_cart_m2.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_m2.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m2('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-9, 1e-2]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Normalization of outputs:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_m2_normalized = -diag(1./diag(dcgain(Gm_cart_m2)))*Gm_cart_m2;
|
||
|
|
Gm_cart_m2_normalized.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_m2_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
|
||
|
|
G_cart_m2_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m2))), permute(G_cart_m2, [2,3,1])), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,1,1)));
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,2,2)));
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,3,3)));
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,1,2)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,1,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,2,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,3,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,2,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m2_normalized(:,3,2)), 'color', [0,0,0,0.2]);
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m2_normalized(:,1,1)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_m2_normalized(:,2,2)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_m2_normalized(:,3,3)));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Better plant invert
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
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)
|
||
|
|
|
||
|
|
%% We define the number of iteration.
|
||
|
|
Niter = 100;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
N = 9; %Order of approximation
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dy
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-2;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>1&f<1500,1,1).', 1i*2*pi*f(f>1&f<1500)', poles, 1./sqrt(f(f>1&f<1500))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dy = inv(-flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dy)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dz
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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],'x');
|
||
|
|
xlim([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dz)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Ry
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m2(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_ry)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Invert plants
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m2(:,1,1))), '-', 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m2(:,2,2))), '-', 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m2(:,3,3))), '-', 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m2(:,1,1)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m2(:,2,2)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m2(:,3,3)), '-');
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Save plant inverse
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dy;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dy, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m2.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dz;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m2.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_ry;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_ry, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m2.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Digital Invert plants
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart_m2(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m2(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m2(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m2(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m2(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m2(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Control Performances
|
||
|
|
**** Better plant invert
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Scans with good controller
|
||
|
|
**** 1rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 796 | 20 | 8 | 8209 | 73 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** 30rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 820 | 39 | 13 | 7790 | 156 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** m3
|
||
|
|
*** 3x3 plant in Cartesian plane
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% Load model
|
||
|
|
load('Gm.mat')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Jacobian for 3x3 plant
|
||
|
|
n_hexapod = initializeNanoHexapod('flex_bot_type', '2dof', ...
|
||
|
|
'flex_top_type', '3dof', ...
|
||
|
|
'motion_sensor_type', 'plates', ...
|
||
|
|
'actuator_type', '2dof');
|
||
|
|
|
||
|
|
Jt_inv = pinv(n_hexapod.geometry.J');
|
||
|
|
|
||
|
|
J_int_to_X = [0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute identified plant in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% New data after calibrating the Rz-offset
|
||
|
|
data_m3 = load(sprintf('%s/dynamics/2023-08-21_16-33_damp_plant_m3_new_Rz_fast.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[~, f] = tfestimate(data_m3.uL1.id_plant, data_m3.uL1.d1, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%% HAC Cartesian Plant
|
||
|
|
G_hac_m3 = zeros(length(f), 5, 6);
|
||
|
|
|
||
|
|
for i_strut = 1:6
|
||
|
|
Di = [data_m3.(sprintf("uL%i", i_strut)).d1 ; data_m3.(sprintf("uL%i", i_strut)).d2 ; data_m3.(sprintf("uL%i", i_strut)).d3 ; data_m3.(sprintf("uL%i", i_strut)).d4 ; data_m3.(sprintf("uL%i", i_strut)).d5]';
|
||
|
|
|
||
|
|
G_hac_m3(:,:,i_strut) = tfestimate(data_m3.(sprintf("uL%i", i_strut)).id_plant, detrend(Di, 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
end
|
||
|
|
|
||
|
|
G_cart_m3 = permute(pagemtimes(J_int_to_X, pagemtimes(permute(G_hac_m3, [2,3,1]), Jt_inv(:,[2,3,5]))), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Compute plant model in the Cartesian plane:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_m3 = J_int_to_X*Gm_hac_m3({'d1', 'd2', 'd3', 'd4', 'd5'}, {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'})*Jt_inv(:,[2,3,5]);
|
||
|
|
Gm_cart_m3.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_m3.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-8, 2e-4]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-9, 1e-2]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Normalization of outputs:
|
||
|
|
#+begin_src matlab
|
||
|
|
Gm_cart_m3_normalized = -diag(1./diag(dcgain(Gm_cart_m3)))*Gm_cart_m3;
|
||
|
|
Gm_cart_m3_normalized.InputName = {'Fy', 'Fz', 'My'};
|
||
|
|
Gm_cart_m3_normalized.OutputName = {'Dy', 'Dz', 'Ry'};
|
||
|
|
|
||
|
|
G_cart_m3_normalized = permute(pagemtimes(-diag(1./diag(dcgain(Gm_cart_m3))), permute(G_cart_m3, [2,3,1])), [3,1,2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 3, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax11 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,1,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$D_y$'); set(gca, 'XTickLabel',[]);
|
||
|
|
title('$F_y$')
|
||
|
|
|
||
|
|
ax12 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,1,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$F_z$')
|
||
|
|
|
||
|
|
ax13 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,1,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dy', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
title('$M_y$')
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13],'y');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax21 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,2,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('$F_z$'); set(gca, 'XTickLabel',[]);
|
||
|
|
|
||
|
|
ax22 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,2,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax23 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,2,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Dz', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax21,ax22,ax23],'y');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax31 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,3,1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fy'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('$M_y$');
|
||
|
|
|
||
|
|
ax32 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,3,2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'Fz'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax33 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,3,3)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gm_cart_m3_normalized('Ry', 'My'), freqs, 'Hz'))), '--');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax31,ax32,ax33],'y');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
linkaxes([ax11,ax12,ax13,ax21,ax22,ax23,ax31,ax32,ax33],'x');
|
||
|
|
xlim([1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% 3x3 plant
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,1,1)));
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,2,2)));
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,3,3)));
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,1,2)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,1,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,2,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,3,1)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,2,3)), 'color', [0,0,0,0.2]);
|
||
|
|
plot(f, abs(G_cart_m3_normalized(:,3,2)), 'color', [0,0,0,0.2]);
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
ylim([1e-4, 1e1]);
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m3_normalized(:,1,1)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_m3_normalized(:,2,2)));
|
||
|
|
plot(f, 180/pi*angle(G_cart_m3_normalized(:,3,3)));
|
||
|
|
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([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Better plant invert
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
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)
|
||
|
|
|
||
|
|
%% We define the number of iteration.
|
||
|
|
Niter = 100;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
N = 9; %Order of approximation
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dy
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,1,1).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dy = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dy = inv(flipRphZeros(Gfit_dy))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dy)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Dz
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<1500,2,2).', 1i*2*pi*f(f>2&f<1500)', poles, 1./(f(f>2&f<1500))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_dz = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_dz, f, '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],'x');
|
||
|
|
xlim([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_dz = inv(-flipRphZeros(Gfit_dz))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_dz)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Ry
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
%Complex conjugate pairs, linearly spaced:
|
||
|
|
bet=linspace(f(1),f(end),N/2);
|
||
|
|
poles=[];
|
||
|
|
for n=1:length(bet)
|
||
|
|
alf=-bet(n)*1e-1;
|
||
|
|
poles=[poles (alf-i*bet(n)) (alf+i*bet(n)) ];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Estimate resonance frequency and damping
|
||
|
|
for iter =1:Niter
|
||
|
|
[G_est, poles, ~, frf_est] = vectfit3(G_cart_m3(f>2&f<800,3,3).', 1i*2*pi*f(f>2&f<800)', poles, 1./(f(f>2&f<800))', opts);
|
||
|
|
end
|
||
|
|
|
||
|
|
Gfit_ry = ss(G_est.A, G_est.B, G_est.C, G_est.D);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Gfit_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Stable Inverse
|
||
|
|
#+begin_src matlab
|
||
|
|
Ginv_ry = inv(-flipRphZeros(Gfit_ry))/(1 + s/2/pi/1e3);
|
||
|
|
isstable(Ginv_ry)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Invert plants
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dy, f, 'Hz')).*abs(G_cart_m3(:,1,1))), '-', 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_dz, f, 'Hz')).*abs(G_cart_m3(:,2,2))), '-', 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Ginv_ry, f, 'Hz')).*abs(G_cart_m3(:,3,3))), '-', 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dy, f, 'Hz')).*G_cart_m3(:,1,1)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_dz, f, 'Hz')).*G_cart_m3(:,2,2)), '-');
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Ginv_ry, f, 'Hz')).*G_cart_m3(:,3,3)), '-');
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Save plant inverse
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dy;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dy = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dy, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dy_inv_m3.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_dz;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_dz = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_dz, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_dz_inv_m3.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :tangle no
|
||
|
|
K = -Ginv_ry;
|
||
|
|
K_order = order(K);
|
||
|
|
|
||
|
|
Kz_ry = c2d(K*(1 + s/2/pi/2e3)^(9-K_order)/(1 + s/2/pi/2e3)^(9-K_order), 1e-4, 'tustin');
|
||
|
|
[num, den] = tfdata(Kz_ry, 'v');
|
||
|
|
|
||
|
|
formatSpec = '%.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n';
|
||
|
|
fileID = fopen('/home/thomas/mnt/data_id31/nass/controllers/G_ry_inv_m3.dat', 'w');
|
||
|
|
fprintf(fileID, formatSpec, [num; den]');
|
||
|
|
fclose(fileID);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compare Digital Invert plants
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Comparaison of the measured direct terms and the reduced order models
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, 1./abs(G_cart_m3(:,1,1)), 'color', [colors(1,:)], 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m3(:,2,2)), 'color', [colors(2,:)], 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)], 'HandleVisibility', 'off');
|
||
|
|
plot(f, 1./abs(G_cart_m3(:,3,3)), 'color', [colors(3,:)], 'DisplayName', '$R_y$');
|
||
|
|
plot(f, abs(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)], 'HandleVisibility', 'off');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Magnitude');
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m3(:,1,1)), 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dy, f, 'Hz'))), '--', 'color', [colors(1,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m3(:,2,2)), 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_dz, f, 'Hz'))), '--', 'color', [colors(2,:)]);
|
||
|
|
plot(f, 180/pi*angle(1./G_cart_m3(:,3,3)), 'color', [colors(3,:)]);
|
||
|
|
plot(f, 180/pi*angle(squeeze(freqresp(Kz_ry, f, 'Hz'))), '--', 'color', [colors(3,:)]);
|
||
|
|
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([1, 2e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Control Performances
|
||
|
|
**** Better plant invert
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cl = load(sprintf('%s/dynamics/2023-08-21_13-36_m0_cf_inv_fit.mat', mat_dir));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Sampling Time [s]
|
||
|
|
Ts = 1e-4;
|
||
|
|
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(1.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% And we get the frequency vector
|
||
|
|
[S_dy, f] = tfestimate(data_cl.Dy.id_cl, data_cl.Dy.e_dy, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_dz, ~] = tfestimate(data_cl.Dz.id_cl, data_cl.Dz.e_dz, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[S_ry, ~] = tfestimate(data_cl.Ry.id_cl, data_cl.Ry.e_ry, win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Obtained transfer function from generated voltages to measured voltages on the piezoelectric force sensor
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(S_dy), 'DisplayName', '$D_y$');
|
||
|
|
plot(f, abs(S_dz), 'DisplayName', '$D_z$');
|
||
|
|
plot(f, abs(S_ry), 'DisplayName', '$R_y$');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||
|
|
ylim([1e-3, 1e1]);
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
||
|
|
xlim([1, 1e3]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Scans with good controller
|
||
|
|
**** 1rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 796 | 20 | 8 | 8209 | 73 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** 30rpm
|
||
|
|
1RPM scans are performed for all the masses with the same controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m0 = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
|
||
|
|
data_m0.time = Ts*[0:length(data_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
data_m0.Dx_rms_cl = rms(detrend(data_m0.Dx_int, 0));
|
||
|
|
data_m0.Dy_rms_cl = rms(detrend(data_m0.Dy_int, 0));
|
||
|
|
data_m0.Dz_rms_cl = rms(detrend(data_m0.Dz_int, 0));
|
||
|
|
data_m0.Rx_rms_cl = rms(detrend(data_m0.Rx_int, 0));
|
||
|
|
data_m0.Ry_rms_cl = rms(detrend(data_m0.Ry_int, 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_m0.Dx_rms_cl, 1e9*data_m0.Dy_rms_cl, 1e9*data_m0.Dz_rms_cl, 1e9*data_m0.Rx_rms_cl, 1e9*data_m0.Ry_rms_cl], ...
|
||
|
|
{'m0'}, {'Dx [nm]', 'Dy [nm]', 'Dz [nm]', 'Rx [nrad]', 'Ry [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | Dx [nm] | Dy [nm] | Dz [nm] | Rx [nrad] | Ry [nrad] |
|
||
|
|
|----+---------+---------+---------+-----------+-----------|
|
||
|
|
| m0 | 820 | 39 | 13 | 7790 | 156 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% description
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e9*data_m0.Dy_int, 1e9*data_m0.Dz_int);
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("Y motion [$nm$]");
|
||
|
|
ylabel("Z motion [$nm$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
* Scans
|
||
|
|
<<sec:id31_scans>>
|
||
|
|
** Introduction :ignore:
|
||
|
|
|
||
|
|
- Section ref:sec:id31_scans_tomography
|
||
|
|
- Section ref:sec:id31_scans_dz
|
||
|
|
- Section ref:sec:id31_scans_reflectivity
|
||
|
|
- Section ref:sec:id31_scans_dy
|
||
|
|
- Section ref:sec:id31_scans_diffraction_tomo
|
||
|
|
|
||
|
|
** 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
|
||
|
|
|
||
|
|
** $R_z$ scans: Tomography
|
||
|
|
<<sec:id31_scans_tomography>>
|
||
|
|
*** Introduction :ignore:
|
||
|
|
|
||
|
|
m0: 30rpm, 6rpm, 1rpm
|
||
|
|
m1: 6rpm, 1rpm
|
||
|
|
m2: 6rpm, 1rpm
|
||
|
|
m3: 1rpm
|
||
|
|
|
||
|
|
*** Robust Control - 1rpm
|
||
|
|
1RPM scans are performed for all the masses with the same robust controller.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Tomography scans with robust controller
|
||
|
|
data_tomo_1rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-37_tomography_1rpm_m0.mat", mat_dir));
|
||
|
|
data_tomo_1rpm_m0.time = Ts*[0:length(data_tomo_1rpm_m0.Rz)-1];
|
||
|
|
|
||
|
|
data_tomo_1rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-15_tomography_1rpm_m1.mat", mat_dir));
|
||
|
|
data_tomo_1rpm_m1.time = Ts*[0:length(data_tomo_1rpm_m1.Rz)-1];
|
||
|
|
|
||
|
|
data_tomo_1rpm_m2 = load(sprintf("%s/scans/2023-08-11_10-59_tomography_1rpm_m2.mat", mat_dir));
|
||
|
|
data_tomo_1rpm_m2.time = Ts*[0:length(data_tomo_1rpm_m2.Rz)-1];
|
||
|
|
|
||
|
|
data_tomo_1rpm_m3 = load(sprintf("%s/scans/2023-08-11_10-24_tomography_1rpm_m3.mat", mat_dir));
|
||
|
|
data_tomo_1rpm_m3.time = Ts*[0:length(data_tomo_1rpm_m3.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The problem for these scans is that the position initialization was not make properly, so the open-loop errors are quite large (see Figure ref:fig:id31_tomo_1rpm_robust_m0).
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_tomo_1rpm_m0.Dx_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), ...
|
||
|
|
'DisplayName', 'CL')
|
||
|
|
hold off;
|
||
|
|
axis equal
|
||
|
|
xlabel("X motion [$\mu m$]");
|
||
|
|
ylabel("Y motion [$\mu m$]");
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
% xlim([-3, 3])
|
||
|
|
% ylim([-3, 3])
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 0), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_tomo_1rpm_m0.Dy_int(data_tomo_1rpm_m0.hac_status == 1), 1e6*data_tomo_1rpm_m0.Dz_int(data_tomo_1rpm_m0.hac_status == 1), ...
|
||
|
|
'DisplayName', 'CL')
|
||
|
|
hold off;
|
||
|
|
% axis equal
|
||
|
|
% xlim([-3, 3])
|
||
|
|
ylim([-0.2, 1.1])
|
||
|
|
xlabel("Y motion [$\mu m$]");
|
||
|
|
ylabel("Z motion [$\mu m$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_tomo_1rpm_robust_m0.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_tomo_1rpm_robust_m0
|
||
|
|
#+caption: $D_x$, $D_y$ and $D_z$ motion during a slow (1RPM) tomography experiment. Open Loop data is shown in blue and closed-loop data in red
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_tomo_1rpm_robust_m0.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
yztomographymovie('movies/tomography_1rpm_m0', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100])
|
||
|
|
yztomographymovie('movies/tomography_1rpm_m0_di_5000', data_tomo_1rpm_m0, 'xlim_ax1', [-10, 15], 'ylim_ax1', [-0.2, 1.2], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-100, 100], 'di', 5000)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The obtained open-loop and closed-loop errors are shown in tables ref:tab:id31_tomo_1rpm_robust_ol_errors and ref:tab:id31_tomo_1rpm_robust_cl_errors respectively.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop and open-loop
|
||
|
|
[~, i_m0] = find(data_tomo_1rpm_m0.hac_status == 1);
|
||
|
|
data_tomo_1rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dx_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dy_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m0.Dz_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m0.Rx_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m0.Ry_int(i_m0+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_1rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dx_int(1:i_m0), 0));
|
||
|
|
data_tomo_1rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dy_int(1:i_m0), 0));
|
||
|
|
data_tomo_1rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m0.Dz_int(1:i_m0), 0));
|
||
|
|
data_tomo_1rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m0.Rx_int(1:i_m0), 0));
|
||
|
|
data_tomo_1rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m0.Ry_int(1:i_m0), 0));
|
||
|
|
|
||
|
|
%% Compute RMS values while in closed-loop and open-loop
|
||
|
|
[~, i_m1] = find(data_tomo_1rpm_m1.hac_status == 1);
|
||
|
|
data_tomo_1rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dx_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dy_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m1.Dz_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m1.Rx_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m1.Ry_int(i_m1+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_1rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dx_int(1:i_m1), 0));
|
||
|
|
data_tomo_1rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dy_int(1:i_m1), 0));
|
||
|
|
data_tomo_1rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m1.Dz_int(1:i_m1), 0));
|
||
|
|
data_tomo_1rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m1.Rx_int(1:i_m1), 0));
|
||
|
|
data_tomo_1rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m1.Ry_int(1:i_m1), 0));
|
||
|
|
|
||
|
|
%% Compute RMS values while in closed-loop and open-loop
|
||
|
|
[~, i_m2] = find(data_tomo_1rpm_m2.hac_status == 1);
|
||
|
|
data_tomo_1rpm_m2.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dx_int(i_m2+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m2.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dy_int(i_m2+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m2.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m2.Dz_int(i_m2+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m2.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m2.Rx_int(i_m2+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m2.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m2.Ry_int(i_m2+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_1rpm_m2.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dx_int(1:i_m2), 0));
|
||
|
|
data_tomo_1rpm_m2.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dy_int(1:i_m2), 0));
|
||
|
|
data_tomo_1rpm_m2.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m2.Dz_int(1:i_m2), 0));
|
||
|
|
data_tomo_1rpm_m2.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m2.Rx_int(1:i_m2), 0));
|
||
|
|
data_tomo_1rpm_m2.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m2.Ry_int(1:i_m2), 0));
|
||
|
|
|
||
|
|
%% Compute RMS values while in closed-loop and open-loop
|
||
|
|
[~, i_m3] = find(data_tomo_1rpm_m3.hac_status == 1);
|
||
|
|
data_tomo_1rpm_m3.Dx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dx_int(i_m3+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m3.Dy_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dy_int(i_m3+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m3.Dz_rms_cl = rms(detrend(data_tomo_1rpm_m3.Dz_int(i_m3+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m3.Rx_rms_cl = rms(detrend(data_tomo_1rpm_m3.Rx_int(i_m3+50000:end), 0));
|
||
|
|
data_tomo_1rpm_m3.Ry_rms_cl = rms(detrend(data_tomo_1rpm_m3.Ry_int(i_m3+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_1rpm_m3.Dx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dx_int(1:i_m3), 0));
|
||
|
|
data_tomo_1rpm_m3.Dy_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dy_int(1:i_m3), 0));
|
||
|
|
data_tomo_1rpm_m3.Dz_rms_ol = rms(detrend(data_tomo_1rpm_m3.Dz_int(1:i_m3), 0));
|
||
|
|
data_tomo_1rpm_m3.Rx_rms_ol = rms(detrend(data_tomo_1rpm_m3.Rx_int(1:i_m3), 0));
|
||
|
|
data_tomo_1rpm_m3.Ry_rms_ol = rms(detrend(data_tomo_1rpm_m3.Ry_int(1:i_m3), 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e6*data_tomo_1rpm_m0.Dx_rms_ol, 1e6*data_tomo_1rpm_m0.Dy_rms_ol, 1e9*data_tomo_1rpm_m0.Dz_rms_ol, 1e6*data_tomo_1rpm_m0.Rx_rms_ol, 1e6*data_tomo_1rpm_m0.Ry_rms_ol; ...
|
||
|
|
1e6*data_tomo_1rpm_m1.Dx_rms_ol, 1e6*data_tomo_1rpm_m1.Dy_rms_ol, 1e9*data_tomo_1rpm_m1.Dz_rms_ol, 1e6*data_tomo_1rpm_m1.Rx_rms_ol, 1e6*data_tomo_1rpm_m1.Ry_rms_ol; ...
|
||
|
|
1e6*data_tomo_1rpm_m2.Dx_rms_ol, 1e6*data_tomo_1rpm_m2.Dy_rms_ol, 1e9*data_tomo_1rpm_m2.Dz_rms_ol, 1e6*data_tomo_1rpm_m2.Rx_rms_ol, 1e6*data_tomo_1rpm_m2.Ry_rms_ol; ...
|
||
|
|
1e6*data_tomo_1rpm_m3.Dx_rms_ol, 1e6*data_tomo_1rpm_m3.Dy_rms_ol, 1e9*data_tomo_1rpm_m3.Dz_rms_ol, 1e6*data_tomo_1rpm_m3.Rx_rms_ol, 1e6*data_tomo_1rpm_m3.Ry_rms_ol], ...
|
||
|
|
{'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomo_1rpm_robust_ol_errors
|
||
|
|
#+caption: Measured error during open-loop tomography scans (1rpm)
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|
||
|
|
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
|
||
|
|
| $m_0$ | 6 | 6 | 32 | 34 | 34 |
|
||
|
|
| $m_1$ | 6 | 7 | 26 | 51 | 55 |
|
||
|
|
| $m_2$ | 36 | 38 | 36 | 259 | 253 |
|
||
|
|
| $m_3$ | 31 | 33 | 38 | 214 | 203 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
|
||
|
|
1e9*data_tomo_1rpm_m1.Dx_rms_cl, 1e9*data_tomo_1rpm_m1.Dy_rms_cl, 1e9*data_tomo_1rpm_m1.Dz_rms_cl, 1e9*data_tomo_1rpm_m1.Rx_rms_cl, 1e9*data_tomo_1rpm_m1.Ry_rms_cl; ...
|
||
|
|
1e9*data_tomo_1rpm_m2.Dx_rms_cl, 1e9*data_tomo_1rpm_m2.Dy_rms_cl, 1e9*data_tomo_1rpm_m2.Dz_rms_cl, 1e9*data_tomo_1rpm_m2.Rx_rms_cl, 1e9*data_tomo_1rpm_m2.Ry_rms_cl; ...
|
||
|
|
1e9*data_tomo_1rpm_m3.Dx_rms_cl, 1e9*data_tomo_1rpm_m3.Dy_rms_cl, 1e9*data_tomo_1rpm_m3.Dz_rms_cl, 1e9*data_tomo_1rpm_m3.Rx_rms_cl, 1e9*data_tomo_1rpm_m3.Ry_rms_cl], ...
|
||
|
|
{'$m_0$', '$m_1$', '$m_2$', '$m_3$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomo_1rpm_robust_cl_errors
|
||
|
|
#+caption: Measured error during closed-loop tomography scans (1rpm, robust controller)
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
|
||
|
|
|-------+------------+------------+------------+--------------+--------------|
|
||
|
|
| $m_0$ | 13 | 15 | 5 | 57 | 55 |
|
||
|
|
| $m_1$ | 16 | 25 | 6 | 102 | 55 |
|
||
|
|
| $m_2$ | 25 | 25 | 7 | 120 | 103 |
|
||
|
|
| $m_3$ | 40 | 53 | 9 | 225 | 169 |
|
||
|
|
|
||
|
|
*** TODO Slow Tomography Scans Comparison of control performances :noexport:
|
||
|
|
#+begin_src matlab
|
||
|
|
% Decentralized in the frame of the struts
|
||
|
|
data = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir));
|
||
|
|
data.time = Ts*[0:length(data.Rz)-1];
|
||
|
|
|
||
|
|
% Rotating cartesian frame
|
||
|
|
data_cart = load(sprintf("%s/scans/2023-08-18_18-33_m0_1rpm_K_cart.mat", mat_dir));
|
||
|
|
data_cart.time = Ts*[0:length(data_cart.Rz)-1];
|
||
|
|
|
||
|
|
% Fixed cartesian frame
|
||
|
|
data_cart_fixed = load(sprintf("%s/scans/2023-08-18_19-08_m0_1rpm_K_cart_fixed.mat", mat_dir));
|
||
|
|
data_cart_fixed.time = Ts*[0:length(data_cart_fixed.Rz)-1];
|
||
|
|
|
||
|
|
% Fixed cartesian frame with Complementary Filters
|
||
|
|
data_cf = load(sprintf("%s/scans/2023-08-21_14-28_m0_1rpm_K_cf.mat", mat_dir));
|
||
|
|
data_cf.time = Ts*[0:length(data_cf.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(data.Dx_int(data.time<45))
|
||
|
|
1e9*rms(data_cart.Dx_int(data_cart.time<45))
|
||
|
|
1e9*rms(data_cart_fixed.Dx_int(data_cart_fixed.time<45))
|
||
|
|
1e9*rms(data_cf.Dx_int(data_cf.time<45))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(data.Dy_int(data.time<45))
|
||
|
|
1e9*rms(data_cart.Dy_int(data_cart.time<45))
|
||
|
|
1e9*rms(data_cart_fixed.Dy_int(data_cart_fixed.time<45))
|
||
|
|
1e9*rms(data_cf.Dy_int(data_cf.time<45))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(data.Dz_int(data.time<45))
|
||
|
|
1e9*rms(data_cart.Dz_int(data_cart.time<45))
|
||
|
|
1e9*rms(data_cart_fixed.Dz_int(data_cart_fixed.time<45))
|
||
|
|
1e9*rms(data_cf.Dz_int(data_cf.time<45))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(data.Rx_int(data.time<45))
|
||
|
|
1e9*rms(data_cart.Rx_int(data_cart.time<45))
|
||
|
|
1e9*rms(data_cart_fixed.Rx_int(data_cart_fixed.time<45))
|
||
|
|
1e9*rms(data_cf.Rx_int(data_cf.time<45))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(data.Ry_int(data.time<45))
|
||
|
|
1e9*rms(data_cart.Ry_int(data_cart.time<45))
|
||
|
|
1e9*rms(data_cart_fixed.Ry_int(data_cart_fixed.time<45))
|
||
|
|
1e9*rms(data_cf.Ry_int(data_cf.time<45))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
plot(data.time, data.Dy_int)
|
||
|
|
plot(data_cart.time, data_cart.Dy_int)
|
||
|
|
plot(data_cart_fixed.time, data_cart_fixed.Dy_int)
|
||
|
|
plot(data_cf.time, data_cf.Dy_int)
|
||
|
|
hold off;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
% Hannning Windows
|
||
|
|
Nfft = floor(10.0/Ts);
|
||
|
|
win = hanning(Nfft);
|
||
|
|
Noverlap = floor(Nfft/2);
|
||
|
|
|
||
|
|
[data.pxx_Dx, data.f] = pwelch(detrend(data.Dx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data.pxx_Dy, ~ ] = pwelch(detrend(data.Dy_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data.pxx_Dz, ~ ] = pwelch(detrend(data.Dz_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data.pxx_Rx, ~ ] = pwelch(detrend(data.Rx_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data.pxx_Ry, ~ ] = pwelch(detrend(data.Ry_int(data.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_cart.pxx_Dx, data_cart.f] = pwelch(detrend(data_cart.Dx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart.pxx_Dy, ~ ] = pwelch(detrend(data_cart.Dy_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart.pxx_Dz, ~ ] = pwelch(detrend(data_cart.Dz_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart.pxx_Rx, ~ ] = pwelch(detrend(data_cart.Rx_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart.pxx_Ry, ~ ] = pwelch(detrend(data_cart.Ry_int(data_cart.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_cart_fixed.pxx_Dx, data_cart_fixed.f] = pwelch(detrend(data_cart_fixed.Dx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart_fixed.pxx_Dy, ~ ] = pwelch(detrend(data_cart_fixed.Dy_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart_fixed.pxx_Dz, ~ ] = pwelch(detrend(data_cart_fixed.Dz_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart_fixed.pxx_Rx, ~ ] = pwelch(detrend(data_cart_fixed.Rx_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cart_fixed.pxx_Ry, ~ ] = pwelch(detrend(data_cart_fixed.Ry_int(data_cart_fixed.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
|
||
|
|
[data_cf.pxx_Dx, data_cf.f] = pwelch(detrend(data_cf.Dx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cf.pxx_Dy, ~ ] = pwelch(detrend(data_cf.Dy_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cf.pxx_Dz, ~ ] = pwelch(detrend(data_cf.Dz_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cf.pxx_Rx, ~ ] = pwelch(detrend(data_cf.Rx_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
[data_cf.pxx_Ry, ~ ] = pwelch(detrend(data_cf.Ry_int(data_cf.time<45), 0), win, Noverlap, Nfft, 1/Ts);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
plot(data.f, data.pxx_Dy)
|
||
|
|
plot(data_cart.f, data_cart.pxx_Dy)
|
||
|
|
plot(data_cart_fixed.f, data_cart_fixed.pxx_Dy)
|
||
|
|
plot(data_cf.f, data_cf.pxx_Dy)
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([0.1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
plot(data.f, sqrt(flip(-cumtrapz(flip(data.f), flip(data.pxx_Dy)))))
|
||
|
|
plot(data_cart.f, sqrt(flip(-cumtrapz(flip(data_cart.f), flip(data_cart.pxx_Dy)))))
|
||
|
|
plot(data_cart_fixed.f, sqrt(flip(-cumtrapz(flip(data_cart_fixed.f), flip(data_cart_fixed.pxx_Dy)))))
|
||
|
|
plot(data_cf.f, sqrt(flip(-cumtrapz(flip(data_cf.f), flip(data_cf.pxx_Dy)))))
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
% legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([0.1, 5e2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Robust Control - 6rpm
|
||
|
|
#+begin_src matlab
|
||
|
|
data_tomo_6rpm_m0 = load(sprintf("%s/scans/2023-08-11_11-31_tomography_6rpm_m0.mat", mat_dir));
|
||
|
|
data_tomo_6rpm_m0.time = Ts*[0:length(data_tomo_6rpm_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_tomo_6rpm_m1 = load(sprintf("%s/scans/2023-08-11_11-23_tomography_6rpm_m1.mat", mat_dir));
|
||
|
|
data_tomo_6rpm_m1.time = Ts*[0:length(data_tomo_6rpm_m1.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
[~, i_m0] = find(data_tomo_6rpm_m0.hac_status == 1);
|
||
|
|
data_tomo_6rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dx_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dy_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m0.Dz_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m0.Rx_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m0.Ry_int(i_m0+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_6rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dx_int(1:i_m0), 0));
|
||
|
|
data_tomo_6rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dy_int(1:i_m0), 0));
|
||
|
|
data_tomo_6rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m0.Dz_int(1:i_m0), 0));
|
||
|
|
data_tomo_6rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m0.Rx_int(1:i_m0), 0));
|
||
|
|
data_tomo_6rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m0.Ry_int(1:i_m0), 0));
|
||
|
|
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
[~, i_m1] = find(data_tomo_6rpm_m1.hac_status == 1);
|
||
|
|
data_tomo_6rpm_m1.Dx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dx_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m1.Dy_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dy_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m1.Dz_rms_cl = rms(detrend(data_tomo_6rpm_m1.Dz_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m1.Rx_rms_cl = rms(detrend(data_tomo_6rpm_m1.Rx_int(i_m1+50000:end), 0));
|
||
|
|
data_tomo_6rpm_m1.Ry_rms_cl = rms(detrend(data_tomo_6rpm_m1.Ry_int(i_m1+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_6rpm_m1.Dx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dx_int(1:i_m1), 0));
|
||
|
|
data_tomo_6rpm_m1.Dy_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dy_int(1:i_m1), 0));
|
||
|
|
data_tomo_6rpm_m1.Dz_rms_ol = rms(detrend(data_tomo_6rpm_m1.Dz_int(1:i_m1), 0));
|
||
|
|
data_tomo_6rpm_m1.Rx_rms_ol = rms(detrend(data_tomo_6rpm_m1.Rx_int(1:i_m1), 0));
|
||
|
|
data_tomo_6rpm_m1.Ry_rms_ol = rms(detrend(data_tomo_6rpm_m1.Ry_int(1:i_m1), 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e6*data_tomo_6rpm_m0.Dx_rms_ol, 1e6*data_tomo_6rpm_m0.Dy_rms_ol, 1e9*data_tomo_6rpm_m0.Dz_rms_ol, 1e6*data_tomo_6rpm_m0.Rx_rms_ol, 1e6*data_tomo_6rpm_m0.Ry_rms_ol; ...
|
||
|
|
1e6*data_tomo_6rpm_m1.Dx_rms_ol, 1e6*data_tomo_6rpm_m1.Dy_rms_ol, 1e9*data_tomo_6rpm_m1.Dz_rms_ol, 1e6*data_tomo_6rpm_m1.Rx_rms_ol, 1e6*data_tomo_6rpm_m1.Ry_rms_ol], ...
|
||
|
|
{'$m_0$', '$m_1$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomo_6rpm_robust_ol_errors
|
||
|
|
#+caption: Measured error during open-loop tomography scans (6rpm)
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|
||
|
|
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
|
||
|
|
| $m_0$ | 8 | 7 | 20 | 41 | 41 |
|
||
|
|
| $m_1$ | 4 | 4 | 21 | 39 | 39 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
|
||
|
|
1e9*data_tomo_6rpm_m1.Dx_rms_cl, 1e9*data_tomo_6rpm_m1.Dy_rms_cl, 1e9*data_tomo_6rpm_m1.Dz_rms_cl, 1e9*data_tomo_6rpm_m1.Rx_rms_cl, 1e9*data_tomo_6rpm_m1.Ry_rms_cl], ...
|
||
|
|
{'$m_0$', '$m_1$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomo_6rpm_robust_cl_errors
|
||
|
|
#+caption: Measured error during closed-loop tomography scans (6rpm, robust controller)
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
|
||
|
|
|-------+------------+------------+------------+--------------+--------------|
|
||
|
|
| $m_0$ | 17 | 19 | 5 | 70 | 73 |
|
||
|
|
| $m_1$ | 20 | 26 | 7 | 110 | 77 |
|
||
|
|
|
||
|
|
*** TODO Medium velocity tomography scans with CF control :noexport:
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m1_cf = load(sprintf("%s/scans/2023-08-21_19-18_m1_6rpm_cf_control.mat", mat_dir));
|
||
|
|
data_m1_cf.time = Ts*[0:length(data_m1_cf.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m2_cf = load(sprintf("%s/scans/2023-08-21_18-07_m2_6rpm_cf_control.mat", mat_dir));
|
||
|
|
data_m2_cf.time = Ts*[0:length(data_m2_cf.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
And higher bandwidth:
|
||
|
|
#+begin_src matlab
|
||
|
|
data_m1_cf_high_fb = load(sprintf("%s/scans/2023-08-21_19-24_m1_6rpm_cf_control_60Hz.mat", mat_dir));
|
||
|
|
data_m1_cf_high_fb.time = Ts*[0:length(data_m1_cf_high_fb.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_m1_cf.Dy_int, detrend(data_m1_cf.Dz_int, 0))
|
||
|
|
plot(data_m2_cf.Dy_int, detrend(data_m2_cf.Dz_int, 0))
|
||
|
|
plot(data_m1_cf_high_fb.Dy_int, detrend(data_m1_cf_high_fb.Dz_int, 0))
|
||
|
|
axis equal
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(detrend(data_m1.Dz_int(i_m1+50000:end), 0))
|
||
|
|
1e9*rms(detrend(data_m1.Dy_int(i_m1+50000:end), 0))
|
||
|
|
1e9*rms(detrend(data_m1.Ry_int(i_m1+50000:end), 0))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(detrend(data_m1_cf.Dz_int, 0))
|
||
|
|
1e9*rms(detrend(data_m1_cf.Dy_int, 0))
|
||
|
|
1e9*rms(detrend(data_m1_cf.Ry_int, 0))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(detrend(data_m2.Dz_int, 0))
|
||
|
|
1e9*rms(detrend(data_m2.Dy_int, 0))
|
||
|
|
1e9*rms(detrend(data_m2.Ry_int, 0))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
1e9*rms(detrend(data_m1_high_fb.Dz_int, 0))
|
||
|
|
1e9*rms(detrend(data_m1_high_fb.Dy_int, 0))
|
||
|
|
1e9*rms(detrend(data_m1_high_fb.Ry_int, 0))
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Robust Control - 30rpm
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Data
|
||
|
|
data_tomo_30rpm_m0 = load(sprintf("%s/scans/2023-08-17_15-26_tomography_30rpm_m0_robust.mat", mat_dir));
|
||
|
|
data_tomo_30rpm_m0.time = Ts*[0:length(data_tomo_30rpm_m0.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Measured motion during tomography scan at 30RPM with a robust controller
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_tomo_30rpm_m0.Dx_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), ...
|
||
|
|
'DisplayName', 'CL')
|
||
|
|
hold off;
|
||
|
|
axis square
|
||
|
|
xlabel("X motion [$\mu m$]");
|
||
|
|
ylabel("Y motion [$\mu m$]");
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([-3, 3])
|
||
|
|
ylim([-3, 3])
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 0), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_tomo_30rpm_m0.Dy_int(data_tomo_30rpm_m0.hac_status == 1), 1e6*data_tomo_30rpm_m0.Dz_int(data_tomo_30rpm_m0.hac_status == 1), ...
|
||
|
|
'DisplayName', 'CL')
|
||
|
|
hold off;
|
||
|
|
axis equal
|
||
|
|
xlim([-3, 3])
|
||
|
|
ylim([-3, 3])
|
||
|
|
xlabel("Y motion [$\mu m$]");
|
||
|
|
ylabel("Z motion [$\mu m$]");
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_tomography_m0_30rpm_robust_xyz.pdf', 'width', 'full', 'height', 'tall');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_tomography_m0_30rpm_robust_xyz
|
||
|
|
#+caption: Measured motion during tomography scan at 30RPM with a robust controller
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_tomography_m0_30rpm_robust_xyz.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Compute RMS values while in closed-loop
|
||
|
|
[~, i_m0] = find(data_tomo_30rpm_m0.hac_status == 1);
|
||
|
|
data_tomo_30rpm_m0.Dx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dx_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_30rpm_m0.Dy_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dy_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_30rpm_m0.Dz_rms_cl = rms(detrend(data_tomo_30rpm_m0.Dz_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_30rpm_m0.Rx_rms_cl = rms(detrend(data_tomo_30rpm_m0.Rx_int(i_m0+50000:end), 0));
|
||
|
|
data_tomo_30rpm_m0.Ry_rms_cl = rms(detrend(data_tomo_30rpm_m0.Ry_int(i_m0+50000:end), 0));
|
||
|
|
|
||
|
|
data_tomo_30rpm_m0.Dx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dx_int(1:i_m0), 0));
|
||
|
|
data_tomo_30rpm_m0.Dy_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dy_int(1:i_m0), 0));
|
||
|
|
data_tomo_30rpm_m0.Dz_rms_ol = rms(detrend(data_tomo_30rpm_m0.Dz_int(1:i_m0), 0));
|
||
|
|
data_tomo_30rpm_m0.Rx_rms_ol = rms(detrend(data_tomo_30rpm_m0.Rx_int(1:i_m0), 0));
|
||
|
|
data_tomo_30rpm_m0.Ry_rms_ol = rms(detrend(data_tomo_30rpm_m0.Ry_int(1:i_m0), 0));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e6*data_tomo_30rpm_m0.Dx_rms_ol, 1e6*data_tomo_30rpm_m0.Dy_rms_ol, 1e9*data_tomo_30rpm_m0.Dz_rms_ol, 1e6*data_tomo_30rpm_m0.Rx_rms_ol, 1e6*data_tomo_30rpm_m0.Ry_rms_ol], ...
|
||
|
|
{'$m_0$'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomo_30rpm_robust_ol_errors
|
||
|
|
#+caption: Measured error during open-loop tomography scans (30rpm)
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|
||
|
|
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
|
||
|
|
| $m_0$ | 2 | 2 | 24 | 10 | 10 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
|
||
|
|
{'$m_0$'}, {'$D_x$ [nm]', '$D_y$ [nm]', '$D_z$ [nm]', '$R_x$ [nrad]', '$R_y$ [nrad]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomo_30rpm_robust_cl_errors
|
||
|
|
#+caption: Measured error during closed-loop tomography scans (30rpm, robust controller)
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align cXXXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [nm] | $D_y$ [nm] | $D_z$ [nm] | $R_x$ [nrad] | $R_y$ [nrad] |
|
||
|
|
|-------+------------+------------+------------+--------------+--------------|
|
||
|
|
| $m_0$ | 34 | 38 | 10 | 127 | 129 |
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
yztomography3dmovie('movies/id31_tomography_m0_30rpm_robust_xyz.avi', data_tomo_30rpm_m0, 'di', 300);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
yztomographymovie('movies/tomography_30rpm_m0_robust', data_tomo_30rpm_m0, 'xlim_ax1', [-3, 3], 'ylim_ax1', [-3, 3], 'xlim_ax2', [-300, 300], 'ylim_ax2', [-300, 300])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** TODO Fast Tomography Scan with Complementary Filter Controller :noexport:
|
||
|
|
#+begin_src matlab
|
||
|
|
data_cf = load(sprintf("%s/scans/2023-08-21_14-33_m0_30rpm_cf_control.mat", mat_dir));
|
||
|
|
data_cf.time = Ts*[0:length(data_cf.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
[~, i0] = find(data.hac_status == 1);
|
||
|
|
1e9*rms(data.Dy_int(i0(1)+5000:end))
|
||
|
|
1e9*rms(data.Dz_int(i0(1)+5000:end))
|
||
|
|
|
||
|
|
1e9*rms(data_cf.Dy_int)
|
||
|
|
1e9*rms(data_cf.Dz_int)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Same performance than the robust controller in terms of RMS residual motion.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
figure; plot3(data.Dx_int, data.Dy_int, data.Dz_int)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Tomography - Effect of the scanning velocity :noexport:
|
||
|
|
- [ ] What are the controller used here? Why worst results than with the robust controller?
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_1rpm = load(sprintf("%s/scans/2023-08-18_10-43_m0_1rpm.mat", mat_dir));
|
||
|
|
data_1rpm.time = Ts*[0:length(data_1rpm.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_30rpm = load(sprintf("%s/scans/2023-08-18_10-45_m0_30rpm.mat", mat_dir));
|
||
|
|
data_30rpm.time = Ts*[0:length(data_30rpm.Rz)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*rms(detrend(data_1rpm.Dy_int, 0)), 1e9*rms(detrend(data_30rpm.Dy_int, 0)); 1e9*rms(detrend(data_1rpm.Dz_int, 0)), 1e9*rms(detrend(data_30rpm.Dz_int, 0)); 1e9*rms(detrend(data_1rpm.Ry_int, 0)), 1e9*rms(detrend(data_30rpm.Ry_int, 0))]', {'1RPM', '30RPM'}, {'$D_y$', '$D_z$', '$R_y$'}, ' %.1f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_tomography_effect_velocity_rms
|
||
|
|
#+caption: RMS values of the errors during tomography scan - Effect of $R_z$ velocity
|
||
|
|
#+attr_latex: :environment tabularx :width 0.5\linewidth :align lXXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_y$ | $D_z$ | $R_y$ |
|
||
|
|
|-------+-------+-------+-------|
|
||
|
|
| 1RPM | 30.9 | 5.9 | 92.4 |
|
||
|
|
| 30RPM | 71.7 | 12.5 | 301.3 |
|
||
|
|
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*data_tomo_1rpm_m0.Dx_rms_cl, 1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Rx_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl; ...
|
||
|
|
1e9*data_tomo_6rpm_m0.Dx_rms_cl, 1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Rx_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl; ...
|
||
|
|
1e9*data_tomo_30rpm_m0.Dx_rms_cl, 1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Rx_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl], ...
|
||
|
|
{'1rpm', '6rpm', '30rpm'}, {'$D_x$ [$\mu m$]', '$D_y$ [$\mu m$]', '$D_z$ [$nm$]', '$R_x$ [$\mu\text{rad}$]', '$R_y$ [$\mu\text{rad}$]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_x$ [$\mu m$] | $D_y$ [$\mu m$] | $D_z$ [$nm$] | $R_x$ [$\mu\text{rad}$] | $R_y$ [$\mu\text{rad}$] |
|
||
|
|
|-------+-----------------+-----------------+--------------+-------------------------+-------------------------|
|
||
|
|
| 1rpm | 13 | 15 | 5 | 57 | 55 |
|
||
|
|
| 6rpm | 17 | 19 | 5 | 70 | 73 |
|
||
|
|
| 30rpm | 34 | 38 | 10 | 127 | 129 |
|
||
|
|
|
||
|
|
** $D_z$ scans: Dirty Layer Scans
|
||
|
|
<<sec:id31_scans_dz>>
|
||
|
|
*** Step by Step $D_z$ motion
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load Dz MIM data
|
||
|
|
data_dz_steps_3nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_3_nm.mat", mat_dir));
|
||
|
|
data_dz_steps_3nm.time = Ts*[0:length(data_dz_steps_3nm.Dz_int)-1];
|
||
|
|
|
||
|
|
data_dz_steps_10nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_10_nm.mat", mat_dir));
|
||
|
|
data_dz_steps_10nm.time = Ts*[0:length(data_dz_steps_10nm.Dz_int)-1];
|
||
|
|
|
||
|
|
data_dz_steps_100nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_100_nm.mat", mat_dir));
|
||
|
|
data_dz_steps_100nm.time = Ts*[0:length(data_dz_steps_100nm.Dz_int)-1];
|
||
|
|
|
||
|
|
data_dz_steps_1000nm = load(sprintf("%s/scans/2023-08-18_14-57_dz_mim_1000_nm.mat", mat_dir));
|
||
|
|
data_dz_steps_1000nm.time = Ts*[0:length(data_dz_steps_1000nm.Dz_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Three step sizes are tested:
|
||
|
|
- $10\,nm$ steps (Figure ref:fig:id31_dz_mim_10nm_steps)
|
||
|
|
- $100\,nm$ steps (Figure ref:fig:id31_dz_mim_100nm_steps)
|
||
|
|
- $1\,\mu m$ steps (Figure ref:fig:id31_dz_steps_response)
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dz MIM test with 10nm steps
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dz_steps_10nm.time, 1e9*(data_dz_steps_10nm.Dz_int_filtered - mean(data_dz_steps_10nm.Dz_int_filtered(1:1000))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('$D_z$ Motion [nm]');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_dz_mim_10nm_steps.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_dz_mim_10nm_steps
|
||
|
|
#+caption: Dz MIM test with 10nm steps (low pass filter with cut-off frequency of 10Hz is applied)
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_dz_mim_10nm_steps.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dz MIM test with 10nm steps
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dz_steps_100nm.time, 1e9*(data_dz_steps_100nm.Dz_int - mean(data_dz_steps_100nm.Dz_int(1:1000))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('$D_z$ Motion [nm]');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_dz_mim_100nm_steps.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_dz_mim_100nm_steps
|
||
|
|
#+caption: Dz MIM test with 100nm steps
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_dz_mim_100nm_steps.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dz step response - Stabilization time is around 70ms
|
||
|
|
figure;
|
||
|
|
[~, i] = find(data_dz_steps_1000nm.m_hexa_dz>data_dz_steps_1000nm.m_hexa_dz(1));
|
||
|
|
i0 = i(1);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dz_steps_1000nm.time-data_dz_steps_1000nm.time(i0), 1e9*(data_dz_steps_1000nm.Dz_int - mean(data_dz_steps_1000nm.Dz_int(1:1000))))
|
||
|
|
plot([-1, 1], [1000-20, 1000-20], 'k--')
|
||
|
|
plot([-1, 1], [1000+20, 1000+20], 'k--')
|
||
|
|
xline(0, 'k--', 'LineWidth', 1.5)
|
||
|
|
xline(0.07, 'k--', 'LineWidth', 1.5)
|
||
|
|
hold off;
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('$D_z$ Motion [nm]');
|
||
|
|
xlim([-0.1, 0.2]);
|
||
|
|
ylim([-100, 1600])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_dz_steps_response.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_dz_steps_response
|
||
|
|
#+caption: $D_z$ step response - Stabilization time is around 70ms
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_dz_steps_response.png]]
|
||
|
|
|
||
|
|
|
||
|
|
*** Continuous $D_z$ motion: Dirty Layer Scans
|
||
|
|
#+begin_src matlab
|
||
|
|
data_dz_10ums = load(sprintf("%s/scans/2023-08-18_15-33_dirty_layer_m0_small.mat", mat_dir));
|
||
|
|
data_dz_10ums.time = Ts*[0:length(data_dz_10ums.Dz_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
data_dz_100ums = load(sprintf("%s/scans/2023-08-18_15-32_dirty_layer_m0.mat", mat_dir));
|
||
|
|
data_dz_100ums.time = Ts*[0:length(data_dz_100ums.Dz_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Two $D_z$ scans are performed:
|
||
|
|
- at $10\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0
|
||
|
|
- at $100\,\mu m/s$ in Figure ref:fig:id31_dirty_layer_scan_m0_large
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dirty layer scan: Dz motion
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dz_10ums.time, 1e6*data_dz_10ums.Dz_int, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dz)))
|
||
|
|
plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_dy, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_10ums.e_dy)))
|
||
|
|
plot(data_dz_10ums.time, 1e6*data_dz_10ums.e_ry, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_10ums.e_ry)))
|
||
|
|
hold off;
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('Motion [$\mu$m]');
|
||
|
|
xlim([0, 2.2])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_dirty_layer_scan_m0.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_dirty_layer_scan_m0
|
||
|
|
#+caption: Dirty layer scan: $D_z$ motion at $10\,\mu m/s$
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_dirty_layer_scan_m0.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dirty layer scan: Dz motion
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dz_100ums.time, 1e6*data_dz_100ums.Dz_int, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dz)))
|
||
|
|
plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_dy, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_dz_100ums.e_dy)))
|
||
|
|
plot(data_dz_100ums.time, 1e6*data_dz_100ums.e_ry, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_dz_100ums.e_ry)))
|
||
|
|
hold off;
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('Motion [$\mu$m]');
|
||
|
|
xlim([0, 2.2])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_dirty_layer_scan_m0_large.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_dirty_layer_scan_m0_large
|
||
|
|
#+caption: Dirty layer scan: $D_z$ motion at $100\,\mu m/s$
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_dirty_layer_scan_m0_large.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Not so good results with the CF controller
|
||
|
|
data_cf = load(sprintf("%s/scans/2023-08-21_19-20_dirty_layer_m1_cf.mat", mat_dir));
|
||
|
|
data_cf.time = Ts*[0:length(data_cf.Dz_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** $R_y$ scans: Reflectivity
|
||
|
|
<<sec:id31_scans_reflectivity>>
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Load data for the reflectivity scan
|
||
|
|
data_ry = load(sprintf("%s/scans/2023-08-18_15-24_first_reflectivity_m0.mat", mat_dir));
|
||
|
|
data_ry.time = Ts*[0:length(data_ry.Ry_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
An $R_y$ scan is performed at $100\,\mu rad/s$ velocity (Figure ref:fig:id31_reflectivity_scan_Ry_100urads).
|
||
|
|
During the $R_y$ scan, the errors in $D_y$ are $D_z$ are kept small.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ry reflectivity scan
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_ry.time, 1e6*data_ry.Ry_int, 'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$rad RMS', 1e6*rms(data_ry.e_ry)))
|
||
|
|
plot(data_ry.time, 1e6*data_ry.e_dy, 'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nm RMS', 1e9*rms(data_ry.e_dy)))
|
||
|
|
plot(data_ry.time, 1e6*data_ry.e_dz, 'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nm RMS', 1e9*rms(data_ry.e_dz)))
|
||
|
|
hold off;
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('$R_y$ motion [$\mu$rad]')
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
xlim([0, 6.2]);
|
||
|
|
ylim([-310, 310])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_reflectivity_scan_Ry_100urads.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_reflectivity_scan_Ry_100urads
|
||
|
|
#+caption: $R_y$ reflecitivity scan at $100\,\mu\text{rad}/s$ velocity
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_reflectivity_scan_Ry_100urads.png]]
|
||
|
|
|
||
|
|
|
||
|
|
** $D_y$ Scans
|
||
|
|
<<sec:id31_scans_dy>>
|
||
|
|
*** Introduction :ignore:
|
||
|
|
The steps generated by the IcePAP for the $T_y$ stage are send to the Speedgoat.
|
||
|
|
Then, we can know in real time what is the wanted position in $D_y$ during $T_y$ scans.
|
||
|
|
|
||
|
|
*** Open Loop
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Slow Ty scan (10um/s)
|
||
|
|
data_ty_ol_slow = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop_slow.mat", mat_dir));
|
||
|
|
data_ty_ol_slow.time = Ts*[0:length(data_ty_ol_slow.Dy_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
We can clearly see micro-stepping errors of the stepper motor used for the $T_y$ stage.
|
||
|
|
The errors have a period of $10\,\mu m$ with an amplitude of $\pm 100\,nm$.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ty scan (at 10um/s) - Dy errors
|
||
|
|
figure;
|
||
|
|
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0))
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$D_y$ error [$\mu$m]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_ty_scan_10ums_ol_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_ty_scan_10ums_ol_dy_errors
|
||
|
|
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. The micro-stepping errors can clearly be seen with a period of $10\,\mu m$ and an amplitude of $\pm 100\,nm$
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_ty_scan_10ums_ol_dy_errors.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ty scan (at 10um/s) - Dz and Ry errors
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$D_z$ error [$\mu$m]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry)
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$R_y$ error [$\mu$rad]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_ty_scan_10ums_ol_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_ty_scan_10ums_ol_dz_ry_errors
|
||
|
|
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. The $D_z$ error is most likely due to having the top interferometer pointing to a sphere. The large $R_y$ errors might also be due to the metrology system
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_ty_scan_10ums_ol_dz_ry_errors.png]]
|
||
|
|
|
||
|
|
*** Closed Loop
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Slow Ty scan (10um/s) - CL
|
||
|
|
data_ty_cl_slow = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop_slow.mat", mat_dir));
|
||
|
|
data_ty_cl_slow.time = Ts*[0:length(data_ty_cl_slow.Dy_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ty scan (at 10um/s) - Dy errors
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dy, 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dy, 0), ...
|
||
|
|
'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dy, 0))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$D_y$ error [$\mu$m]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_ty_scan_10ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_ty_scan_10ums_cl_dy_errors
|
||
|
|
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_ty_scan_10ums_cl_dy_errors.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ty scan (at 10um/s) - Dz and Ry errors
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_slow.Ty, 1e6*detrend(data_ty_ol_slow.e_dz, 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_ty_cl_slow.Ty, 1e6*detrend(data_ty_cl_slow.e_dz, 0), ...
|
||
|
|
'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$D_z$ error [$\mu$m]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_slow.Ty, 1e6*data_ty_ol_slow.e_ry, ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_ty_cl_slow.Ty, 1e6*data_ty_cl_slow.e_ry, ...
|
||
|
|
'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_slow.e_ry, 0))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$R_y$ error [$\mu$rad]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_ty_scan_10ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_ty_scan_10ums_cl_dz_ry_errors
|
||
|
|
#+caption: $T_y$ scan (at $10\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_ty_scan_10ums_cl_dz_ry_errors.png]]
|
||
|
|
|
||
|
|
*** Faster Scan
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Fast Ty scan (100um/s) - OL
|
||
|
|
data_ty_ol_fast = load(sprintf("%s/scans/2023-08-21_20-05_ty_scan_m1_open_loop.mat", mat_dir));
|
||
|
|
data_ty_ol_fast.time = Ts*[0:length(data_ty_ol_fast.Dy_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Fast Ty scan (10um/s) - CL
|
||
|
|
data_ty_cl_fast = load(sprintf("%s/scans/2023-08-21_20-07_ty_scan_m1_cf_closed_loop.mat", mat_dir));
|
||
|
|
data_ty_cl_fast.time = Ts*[0:length(data_ty_cl_fast.Dy_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Because of micro-stepping errors of the Ty stepper motor, when scanning at high velocity this induce high frequency vibration that are outside the bandwidth of the feedback controller.
|
||
|
|
|
||
|
|
At $100\,\mu m/s$, the micro-stepping errors with a period of $10\,\mu m$ (see Figure ref:fig:id31_ty_scan_10ums_ol_dy_errors) are at 10Hz.
|
||
|
|
These errors are them amplified by some resonances in the system.
|
||
|
|
|
||
|
|
This could be easily solved by changing the stepper motor for a torque motor for instance.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ty scan (at 100um/s) - Dy errors
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dy, 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dy, 0), ...
|
||
|
|
'DisplayName', sprintf('CL - $\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dy, 0))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$D_y$ error [$\mu$m]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_ty_scan_100ums_cl_dy_errors.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_ty_scan_100ums_cl_dy_errors
|
||
|
|
#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_y$ errors. Open-loop and Closed-loop scans
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_ty_scan_100ums_cl_dy_errors.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Ty scan (at 100um/s) - Dz and Ry errors
|
||
|
|
figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_fast.Ty, 1e6*detrend(data_ty_ol_fast.e_dz, 0), ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_ty_cl_fast.Ty, 1e6*detrend(data_ty_cl_fast.e_dz, 0), ...
|
||
|
|
'DisplayName', sprintf('Cl - $\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(detrend(data_ty_cl_fast.e_dz, 0))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$D_z$ error [$\mu$m]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(1e6*data_ty_ol_fast.Ty, 1e6*data_ty_ol_fast.e_ry, ...
|
||
|
|
'DisplayName', 'OL')
|
||
|
|
plot(1e6*data_ty_cl_fast.Ty, 1e6*data_ty_cl_fast.e_ry, ...
|
||
|
|
'DisplayName', sprintf('Cl - $\\epsilon R_y = %.2f$ uradRMS', 1e6*rms(detrend(data_ty_cl_fast.e_ry, 0))))
|
||
|
|
hold off;
|
||
|
|
xlabel('Ty position [$\mu$m]');
|
||
|
|
ylabel('$R_y$ error [$\mu$rad]');
|
||
|
|
xlim([-100, 100])
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_ty_scan_100ums_cl_dz_ry_errors.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_ty_scan_100ums_cl_dz_ry_errors
|
||
|
|
#+caption: $T_y$ scan (at $100\,\mu m/s$) - $D_z$ and $R_y$ errors. Open-loop and Closed-loop scans
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_ty_scan_100ums_cl_dz_ry_errors.png]]
|
||
|
|
|
||
|
|
** Combined $R_z$ and $D_y$: Diffraction Tomography
|
||
|
|
<<sec:id31_scans_diffraction_tomo>>
|
||
|
|
Instead of doing a fast $R_z$ motion a slow $D_y$, the idea is to perform slow $R_z$ (here 1rpm) and fast $D_y$ scans with the nano-hexapod.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% 100um/s - Robust controller
|
||
|
|
data_dt_100ums = load(sprintf("%s/scans/2023-08-18_17-12_diffraction_tomo_m0.mat", mat_dir));
|
||
|
|
data_dt_100ums.time = Ts*[0:length(data_dt_100ums.Dy_int)-1];
|
||
|
|
|
||
|
|
%% 500um/s - Robust controller (Not used)
|
||
|
|
% data_dt_500ums = load(sprintf("%s/scans/2023-08-18_17-19_diffraction_tomo_m0_fast.mat", mat_dir));
|
||
|
|
% data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];
|
||
|
|
|
||
|
|
%% 500um/s - Complementary filters
|
||
|
|
data_dt_500ums = load(sprintf("%s/scans/2023-08-21_15-15_diffraction_tomo_m0_fast_cf.mat", mat_dir));
|
||
|
|
data_dt_500ums.time = Ts*[0:length(data_dt_500ums.Dy_int)-1];
|
||
|
|
|
||
|
|
%% 1mm/s - Complementary filters
|
||
|
|
data_dt_1000ums = load(sprintf("%s/scans/2023-08-21_15-16_diffraction_tomo_m0_fast_cf.mat", mat_dir));
|
||
|
|
data_dt_1000ums.time = Ts*[0:length(data_dt_1000ums.Dy_int)-1];
|
||
|
|
|
||
|
|
%% 5mm/s - Complementary filters
|
||
|
|
% data_dt_5000ums = load(sprintf("%s/scans/2023-08-21_18-03_diffraction_tomo_m2_fast_cf.mat", mat_dir));
|
||
|
|
% data_dt_5000ums.time = Ts*[0:length(data_dt_5000ums.Dy_int)-1];
|
||
|
|
|
||
|
|
%% 10mm/s - Complementary filters
|
||
|
|
data_dt_10000ums = load(sprintf("%s/scans/2023-08-21_15-17_diffraction_tomo_m0_fast_cf.mat", mat_dir));
|
||
|
|
data_dt_10000ums.time = Ts*[0:length(data_dt_10000ums.Dy_int)-1];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Here, the $D_y$ scans are performed only with the nano-hexapod (the Ty stage is not moving), so we are limited to $\pm 100\,\mu m$.
|
||
|
|
|
||
|
|
Several $D_y$ velocities are tested: $0.1\,mm/s$, $0.5\,mm/s$, $1\,mm/s$ and $10\,mm/s$ (see Figure ref:fig:id31_diffraction_tomo_velocities).
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Dy motion for several configured velocities
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dt_10000ums.time, 1e6*data_dt_10000ums.Dy_int, ...
|
||
|
|
'DisplayName', '$10 mm/s$')
|
||
|
|
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
|
||
|
|
'DisplayName', '$1 mm/s$')
|
||
|
|
plot(data_dt_500ums.time, 1e6*data_dt_500ums.Dy_int, ...
|
||
|
|
'DisplayName', '$0.5 mm/s$')
|
||
|
|
plot(data_dt_100ums.time, 1e6*data_dt_100ums.Dy_int, ...
|
||
|
|
'DisplayName', '$0.1 mm/s$')
|
||
|
|
hold off;
|
||
|
|
xlim([0, 4]);
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('$D_y$ position [$\mu$m]')
|
||
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_diffraction_tomo_velocities.pdf', 'width', 'wide', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_diffraction_tomo_velocities
|
||
|
|
#+caption: Dy motion for several configured velocities
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_diffraction_tomo_velocities.png]]
|
||
|
|
|
||
|
|
The corresponding "repetition rate" and $D_y$ scan per spindle turn are shown in Table ref:tab:diffraction_tomo_velocities.
|
||
|
|
|
||
|
|
The main issue here is the "waiting" time between two scans that is in the order of 50ms.
|
||
|
|
By removing this waiting time (fairly easily), we can double the repetition rate at 10mm/s.
|
||
|
|
|
||
|
|
#+name: tab:diffraction_tomo_velocities
|
||
|
|
#+caption: $D_y$ scaning repetition rate
|
||
|
|
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
| $D_y$ Velocity | Repetition rate | Scans per turn (at 1RPM) |
|
||
|
|
|----------------+-----------------+--------------------------|
|
||
|
|
| 0.1 mm/s | 4 s | 15 |
|
||
|
|
| 0.5 mm/s | 0.9 s | 65 |
|
||
|
|
| 1 mm/s | 0.5 s | 120 |
|
||
|
|
| 10 mm/s | 0.18 s | 330 |
|
||
|
|
|
||
|
|
|
||
|
|
The scan results for a velocity of 1mm/s is shown in Figure ref:fig:id31_diffraction_tomo_1mms.
|
||
|
|
The $D_z$ and $R_y$ errors are quite small during the scan.
|
||
|
|
|
||
|
|
The $D_y$ errors are quite large as the velocity is increased.
|
||
|
|
This type of scan can probably be massively improved by using feed-forward and optimizing the trajectory.
|
||
|
|
Also, if the detectors are triggered in position (the Speedgoat could generate an encoder signal for instance), we don't care about the $D_y$ errors.
|
||
|
|
|
||
|
|
#+begin_src matlab :exports none :results none
|
||
|
|
%% Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
|
||
|
|
figure;
|
||
|
|
hold on;
|
||
|
|
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dz_int, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon D_z = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dz_int)))
|
||
|
|
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Ry_int, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon R_y = %.2f$ $\\mu$radRMS', 1e6*rms(data_dt_1000ums.Ry_int)))
|
||
|
|
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.Dy_int, ...
|
||
|
|
'DisplayName', sprintf('$\\epsilon D_y = %.0f$ nmRMS', 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy)))
|
||
|
|
plot(data_dt_1000ums.time, 1e6*data_dt_1000ums.m_hexa_dy, 'k--', 'HandleVisibility', 'off')
|
||
|
|
hold off;
|
||
|
|
xlim([0, 1])
|
||
|
|
xlabel('Time [s]');
|
||
|
|
ylabel('Measurement [nm,nrad]')
|
||
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||
|
|
ylim([-110, 110])
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||
|
|
exportFig('figs/id31_diffraction_tomo_1mms.pdf', 'width', 'full', 'height', 'normal');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:id31_diffraction_tomo_1mms
|
||
|
|
#+caption: Diffraction tomography with Dy velocity of 1mm/s and Rz velocity of 1RPM
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/id31_diffraction_tomo_1mms.png]]
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable([1e9*rms(data_dt_100ums.Dy_int - data_dt_100ums.m_hexa_dy), 1e9*rms(data_dt_500ums.Dy_int - data_dt_500ums.m_hexa_dy), 1e9*rms(data_dt_1000ums.Dy_int - data_dt_1000ums.m_hexa_dy), 1e9*rms(data_dt_10000ums.Dy_int - data_dt_10000ums.m_hexa_dy);
|
||
|
|
1e9*rms(data_dt_100ums.Dz_int), 1e9*rms(data_dt_500ums.Dz_int), 1e9*rms(data_dt_1000ums.Dz_int), 1e9*rms(data_dt_10000ums.Dz_int);
|
||
|
|
1e6*rms(data_dt_100ums.Ry_int), 1e6*rms(data_dt_500ums.Ry_int), 1e6*rms(data_dt_1000ums.Ry_int), 1e6*rms(data_dt_10000ums.Ry_int)]', {'0.1 mm/s' ,'0.5 mm/s', '1 mm/s', '10 mm/s'}, {'Velocity', '$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [$\mu\text{radRMS}$]'}, ' %.1f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_diffraction_tomo_results
|
||
|
|
#+caption: Obtained errors for several $D_y$ velocities
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align lXX
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| Velocity | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [$\mu\text{radRMS}$] |
|
||
|
|
|----------+---------------+---------------+----------------------------|
|
||
|
|
| 0.1 mm/s | 75.5 | 9.1 | 0.1 |
|
||
|
|
| 0.5 mm/s | 190.5 | 10.0 | 0.1 |
|
||
|
|
| 1 mm/s | 428.0 | 11.2 | 0.2 |
|
||
|
|
| 10 mm/s | 4639.9 | 55.9 | 1.4 |
|
||
|
|
|
||
|
|
** Summary of experiments
|
||
|
|
For each conducted experiments, the $D_y$, $D_z$ and $R_y$ errors are computed and summarized in Table ref:tab:id31_experiments_results_summary.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Summary of results
|
||
|
|
data_results = [...
|
||
|
|
1e9*data_tomo_1rpm_m0.Dy_rms_cl, 1e9*data_tomo_1rpm_m0.Dz_rms_cl, 1e9*data_tomo_1rpm_m0.Ry_rms_cl ; ... % Tomo 1rpm
|
||
|
|
1e9*data_tomo_6rpm_m0.Dy_rms_cl, 1e9*data_tomo_6rpm_m0.Dz_rms_cl, 1e9*data_tomo_6rpm_m0.Ry_rms_cl ; ... % Tomo 6rpm
|
||
|
|
1e9*data_tomo_30rpm_m0.Dy_rms_cl, 1e9*data_tomo_30rpm_m0.Dz_rms_cl, 1e9*data_tomo_30rpm_m0.Ry_rms_cl ; ... % Tomo 30rpm
|
||
|
|
1e9*rms(detrend(data_dz_10ums.e_dy, 0)), 1e9*rms(detrend(data_dz_10ums.e_dz, 0)), 1e9*rms(detrend(data_dz_10ums.e_ry, 0)) ; ... % Dz 10um/s
|
||
|
|
1e9*rms(detrend(data_dz_100ums.e_dy,0)), 1e9*rms(detrend(data_dz_100ums.e_dz,0)), 1e9*rms(detrend(data_dz_100ums.e_ry,0)) ; ... % Dz 100um/s
|
||
|
|
1e9*rms(detrend(data_ry.e_dy,0)), 1e9*rms(detrend(data_ry.e_dz,0)), 1e9*rms(detrend(data_ry.e_ry,0)) ; ... % Ry 100urad/s
|
||
|
|
1e9*rms(detrend(data_ty_cl_slow.e_dy, 0)), 1e9*rms(detrend(data_ty_cl_slow.e_dz, 0)), 1e9*rms(detrend(data_ty_cl_slow.e_rz, 0)) ; ... % Dy 10 um/s
|
||
|
|
1e9*rms(detrend(data_dt_100ums.Dy_int-data_dt_100ums.m_hexa_dy, 0)), 1e9*rms(detrend(data_dt_100ums.Dz_int, 0)), 1e9*rms(detrend(data_dt_100ums.Ry_int, 0)); ... % Diffraction tomo 0.1mm/s
|
||
|
|
1e9*rms(detrend(data_dt_1000ums.Dy_int-data_dt_1000ums.m_hexa_dy,0)), 1e9*rms(detrend(data_dt_1000ums.Dz_int,0)), 1e9*rms(detrend(data_dt_1000ums.Ry_int,0)) ... % Diffraction tomo 1mm/s
|
||
|
|
];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||
|
|
data2orgtable(data_results, {'Tomography ($R_z$ 1rpm)', 'Tomography ($R_z$ 6rpm)', 'Tomography ($R_z$ 30rpm)', 'Dirty Layer ($D_z$ $10\,\mu m/s$)', 'Dirty Layer ($D_z$ $100\,\mu m/s$)', 'Reflectivity ($R_y$ $100\,\mu\text{rad}/s$)', 'Lateral Scan ($D_y$ $10\,\mu m/s$)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s)', 'Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s)'}, {'$D_y$ [nmRMS]', '$D_z$ [nmRMS]', '$R_y$ [nradRMS]'}, ' %.0f ');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: tab:id31_experiments_results_summary
|
||
|
|
#+caption: Table caption
|
||
|
|
#+attr_latex: :environment tabularx :width \linewidth :align Xccc
|
||
|
|
#+attr_latex: :center t :booktabs t
|
||
|
|
#+RESULTS:
|
||
|
|
| | $D_y$ [nmRMS] | $D_z$ [nmRMS] | $R_y$ [nradRMS] |
|
||
|
|
|----------------------------------------------------+---------------+---------------+-----------------|
|
||
|
|
| Tomography ($R_z$ 1rpm) | 15 | 5 | 55 |
|
||
|
|
| Tomography ($R_z$ 6rpm) | 19 | 5 | 73 |
|
||
|
|
| Tomography ($R_z$ 30rpm) | 38 | 10 | 129 |
|
||
|
|
| Dirty Layer ($D_z$ $10\,\mu m/s$) | 25 | 5 | 114 |
|
||
|
|
| Dirty Layer ($D_z$ $100\,\mu m/s$) | 34 | 15 | 130 |
|
||
|
|
| Reflectivity ($R_y$ $100\,\mu\text{rad}/s$) | 28 | 6 | 118 |
|
||
|
|
| Lateral Scan ($D_y$ $10\,\mu m/s$) | 21 | 10 | 37 |
|
||
|
|
| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 0.1mm/s) | 75 | 9 | 118 |
|
||
|
|
| Diffraction Tomography ($R_z$ 1rpm, $D_y$ 1mm/s) | 428 | 11 | 169 |
|
||
|
|
|
||
|
|
* 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/STEPS/'); % Path for STEPS files
|
||
|
|
addpath('./matlab/subsystems/'); % Path for Simulink subsystems
|
||
|
|
addpath('./matlab/mat/'); % Path for data
|
||
|
|
addpath('./matlab/src/'); % Path for functions
|
||
|
|
addpath('./matlab/'); % Path for scripts
|
||
|
|
#+END_SRC
|
||
|
|
|
||
|
|
#+NAME: m-init-path-tangle
|
||
|
|
#+BEGIN_SRC matlab
|
||
|
|
%% Path for functions, data and scripts
|
||
|
|
addpath('./STEPS/'); % Path for STEPS files
|
||
|
|
addpath('./subsystems/'); % Path for Simulink subsystems
|
||
|
|
addpath('./mat/'); % Path for data
|
||
|
|
addpath('./src/'); % Path for functions
|
||
|
|
#+END_SRC
|
||
|
|
|
||
|
|
** Initialize other elements
|
||
|
|
#+NAME: m-init-other
|
||
|
|
#+BEGIN_SRC matlab
|
||
|
|
%% Colors for the figures
|
||
|
|
colors = colororder;
|
||
|
|
freqs = logspace(0, 3, 1000);
|
||
|
|
Ts = 1e-4;
|
||
|
|
data_dir = "/home/thomas/mnt/data_id31/id31nass/id31/20230801/RAW_DATA"
|
||
|
|
mat_dir = "/home/thomas/mnt/data_id31/nass"
|
||
|
|
#+END_SRC
|
||
|
|
|
||
|
|
** =h5scan= - Easily load h5 files
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/src/h5scan.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:h5scan>>
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
|
||
|
|
|
||
|
|
i = cellfun(@(x) isa(x,'detector'),varargin);
|
||
|
|
if any(i), det = varargin{i}; varargin = varargin(~i); else, det = []; end;
|
||
|
|
if ~isstr(ds), ds = sprintf('%.4d',ds); end;
|
||
|
|
|
||
|
|
f = sprintf('%s/%s/%s_%s/%s_%s.h5',pth,smp,smp,ds,smp,ds);
|
||
|
|
h = h5info(f,sprintf('/%d.1/measurement',sn));
|
||
|
|
fid = H5F.open(f);
|
||
|
|
for i = 1:length(h.Links),
|
||
|
|
nm = h.Links(i).Name;
|
||
|
|
try,
|
||
|
|
id = H5D.open(fid,h.Links(i).Value{1});
|
||
|
|
cntrs.(nm) = H5D.read(id);
|
||
|
|
H5D.close(id);
|
||
|
|
if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
|
||
|
|
catch,
|
||
|
|
warning('solving problem with %s\n',nm);
|
||
|
|
cntrs.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
|
||
|
|
end;
|
||
|
|
[~,tp.(nm)] = fileparts(h.Links(i).Value{1});
|
||
|
|
end;
|
||
|
|
try,
|
||
|
|
h = h5info(f,sprintf('/%d.2/measurement',sn));
|
||
|
|
catch,
|
||
|
|
h = [];
|
||
|
|
end;
|
||
|
|
if ~isempty(h),
|
||
|
|
for i = 1:length(h.Links),
|
||
|
|
nm = h.Links(i).Name;
|
||
|
|
try,
|
||
|
|
id = H5D.open(fid,h.Links(i).Value{1});
|
||
|
|
cntrs.part2.(nm) = H5D.read(id);
|
||
|
|
H5D.close(id);
|
||
|
|
catch,
|
||
|
|
warning('solving problem with %s\n',nm);
|
||
|
|
cntrs.part2.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
|
||
|
|
end;
|
||
|
|
[~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
|
||
|
|
end;
|
||
|
|
end;
|
||
|
|
if length(varargin),
|
||
|
|
fn = sprintf('/%d.1/instrument/positioners/',sn);
|
||
|
|
h = h5info(f,fn);
|
||
|
|
[~,k,m] = intersect({h.Datasets.Name},varargin,'stable');
|
||
|
|
h.Datasets = h.Datasets(k);
|
||
|
|
for i = 1:length(h.Datasets),
|
||
|
|
id = H5D.open(fid,[fn h.Datasets(i).Name]);
|
||
|
|
cntrs.(h.Datasets(i).Name) = H5D.read(id);
|
||
|
|
H5D.close(id);
|
||
|
|
end;
|
||
|
|
end;
|
||
|
|
|
||
|
|
H5F.close(fid);
|
||
|
|
|
||
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
|
|
||
|
|
function A = vrtlds(f,nm,det)
|
||
|
|
%try,
|
||
|
|
n = 0; A = [];
|
||
|
|
fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
|
||
|
|
while exist(fn) == 2,
|
||
|
|
fid = H5F.open(fn); n = n+1;
|
||
|
|
id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
|
||
|
|
if 2 < nargin & strcmp(nm,'p3') & ~isempty(det),
|
||
|
|
fprintf('integrating %s\n',fn);
|
||
|
|
if isempty(A),
|
||
|
|
A = integrate(det,double(H5D.read(id)),1);
|
||
|
|
else,
|
||
|
|
tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
|
||
|
|
end;
|
||
|
|
else,
|
||
|
|
fprintf('loading %s\n',fn);
|
||
|
|
A = cat(3,A,H5D.read(id));
|
||
|
|
end;
|
||
|
|
H5D.close(id); H5F.close(fid);
|
||
|
|
fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
|
||
|
|
end;
|
||
|
|
%catch,
|
||
|
|
% A = [];
|
||
|
|
%end;
|
||
|
|
|
||
|
|
% fid = H5F.open...
|
||
|
|
% id = H5D.open...
|
||
|
|
% sid = H5D.get_space(id);
|
||
|
|
% [ndims,h5_dims]=H5S.get_simple_extent_dims(sid)
|
||
|
|
|
||
|
|
% Read a 2x3 hyperslab of data from a dataset, starting in the 4th row and 5th column of the example dataset.
|
||
|
|
% Create a property list identifier, then open the HDF5 file and the dataset /g1/g1.1/dset1.1.1.
|
||
|
|
|
||
|
|
% fid = H5F.open('example.h5');
|
||
|
|
% id = H5D.open(fid,'/g1/g1.1/dset1.1.1');
|
||
|
|
|
||
|
|
% dims = ([500 1679 1475];
|
||
|
|
% msid = H5S.create_simple(3,dims,[]);
|
||
|
|
% sid = H5D.get_space(id);
|
||
|
|
% offset = [n*500 0 0];
|
||
|
|
% block = dims; % d1: 500 or min(d1tot-n*500,500)
|
||
|
|
% H5S.select_hyperslab(sid,'H5S_SELECT_SET',offset,[],[],block);
|
||
|
|
% data = H5D.read(id,'H5ML_DEFAULT',msid,sid,'H5P_DEFAULT');
|
||
|
|
% H5D.close(id);
|
||
|
|
% H5F.close(fid);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** =circlefit=
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/src/circlefit.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:h5scan>>
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
function [xc,yc,R,a] = circlefit(x,y)
|
||
|
|
%
|
||
|
|
% [xc yx R] = circfit(x,y)
|
||
|
|
%
|
||
|
|
% fits a circle in x,y plane in a more accurate
|
||
|
|
% (less prone to ill condition )
|
||
|
|
% procedure than circfit2 but using more memory
|
||
|
|
% x,y are column vector where (x(i),y(i)) is a measured point
|
||
|
|
%
|
||
|
|
% result is center point (yc,xc) and radius R
|
||
|
|
% an optional output is the vector of coeficient a
|
||
|
|
% describing the circle's equation
|
||
|
|
%
|
||
|
|
% x^2+y^2+a(1)*x+a(2)*y+a(3)=0
|
||
|
|
%
|
||
|
|
% By: Izhak bucher 25/oct /1991,
|
||
|
|
x=x(:); y=y(:);
|
||
|
|
a=[x y ones(size(x))]\[-(x.^2+y.^2)];
|
||
|
|
xc = -.5*a(1);
|
||
|
|
yc = -.5*a(2);
|
||
|
|
R = sqrt((a(1)^2+a(2)^2)/4-a(3));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** =yztomography3dmovie=
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/src/yztomography3dmovie.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:yztomography3dmovie>>
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = yztomography3dmovie(filename, data, args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
filename
|
||
|
|
data
|
||
|
|
args.di (1,1) double {mustBeNumeric} = 500
|
||
|
|
args.xlim (2,1) double {mustBeNumeric} = [-3, 3]
|
||
|
|
args.ylim (2,1) double {mustBeNumeric} = [-3, 3]
|
||
|
|
args.zlim (2,1) double {mustBeNumeric} = [-0.4, 0.4]
|
||
|
|
args.view_az (1,1) double {mustBeNumeric} = -70
|
||
|
|
args.view_el (1,1) double {mustBeNumeric} = 5
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
colors = colororder;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
writerObj = VideoWriter(filename); % initialize the VideoWriter object
|
||
|
|
open(writerObj);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
fig = figure;
|
||
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
di = args.di;
|
||
|
|
for i = 1:floor(length(data.Dx_int)/di)-1
|
||
|
|
if data.hac_status(di*(i+1)-1) == 0
|
||
|
|
% Only open loop
|
||
|
|
plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
|
||
|
|
elseif data.hac_status(di*i) == 1
|
||
|
|
% Only closed loop
|
||
|
|
plot3(ax1, 1e6*data.Dx_int(di*i:di*(i+1)-1), 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
|
||
|
|
else
|
||
|
|
% Both open and closed loop
|
||
|
|
Dx_int = data.Dx_int(di*i:di*(i+1)-1);
|
||
|
|
Dy_int = data.Dy_int(di*i:di*(i+1)-1);
|
||
|
|
Dz_int = data.Dz_int(di*i:di*(i+1)-1);
|
||
|
|
plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
|
||
|
|
plot3(ax1, 1e6*Dx_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
|
||
|
|
end
|
||
|
|
axis(ax1, 'equal')
|
||
|
|
xlim(ax1, args.xlim)
|
||
|
|
ylim(ax1, args.ylim)
|
||
|
|
zlim(ax1, args.zlim)
|
||
|
|
view(ax1, args.view_az, args.view_el)
|
||
|
|
xlabel(ax1, "X motion [$\mu$m]");
|
||
|
|
ylabel(ax1, "Y motion [$\mu$m]");
|
||
|
|
zlabel(ax1, "Z motion [$\mu$m]");
|
||
|
|
drawnow()
|
||
|
|
writeVideo(writerObj,getframe(fig)) % add the frame to the movie
|
||
|
|
end
|
||
|
|
close(writerObj);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** =yztomographymovie=
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle matlab/src/yztomographymovie.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:h5scan>>
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = yztomographymovie(filename, data, args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
filename
|
||
|
|
data
|
||
|
|
args.di (1,1) double {mustBeNumeric} = 500
|
||
|
|
args.xlim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
|
||
|
|
args.ylim_ax1 (2,1) double {mustBeNumeric} = [-3, 3]
|
||
|
|
args.xlim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
|
||
|
|
args.ylim_ax2 (2,1) double {mustBeNumeric} = [-3, 3]
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
colors = colororder;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
writerObj = VideoWriter(filename); % initialize the VideoWriter object
|
||
|
|
open(writerObj);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
fig = figure;
|
||
|
|
tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile;
|
||
|
|
ax2 = nexttile;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
di = args.di;
|
||
|
|
for i = 1:floor(length(data.Dx_int)/di)-1
|
||
|
|
if data.hac_status(di*(i+1)-1) == 0
|
||
|
|
% Only open loop
|
||
|
|
plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
|
||
|
|
plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(1,:))
|
||
|
|
elseif data.hac_status(di*i) == 1
|
||
|
|
% Only closed loop
|
||
|
|
plot(ax1, 1e6*data.Dy_int(di*i:di*(i+1)-1), 1e6*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
|
||
|
|
plot(ax2, 1e9*data.Dy_int(di*i:di*(i+1)-1), 1e9*data.Dz_int(di*i:di*(i+1)-1), '-', 'color', colors(2,:))
|
||
|
|
else
|
||
|
|
% Both open and closed loop
|
||
|
|
Dy_int = data.Dy_int(di*i:di*(i+1)-1);
|
||
|
|
Dz_int = data.Dz_int(di*i:di*(i+1)-1);
|
||
|
|
plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
|
||
|
|
plot(ax1, 1e6*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e6*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
|
||
|
|
plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 0), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 0), '-', 'color', colors(1,:))
|
||
|
|
plot(ax2, 1e9*Dy_int(data.hac_status(di*i:di*(i+1)-1) == 1), 1e9*Dz_int(data.hac_status(di*i:di*(i+1)-1) == 1), '-', 'color', colors(2,:))
|
||
|
|
end
|
||
|
|
axis(ax1, 'square')
|
||
|
|
axis(ax2, 'square')
|
||
|
|
xlim(ax1, args.xlim_ax1)
|
||
|
|
ylim(ax1, args.ylim_ax1)
|
||
|
|
xlim(ax2, args.xlim_ax2)
|
||
|
|
ylim(ax2, args.ylim_ax2)
|
||
|
|
xlabel(ax1, "Y motion [$\mu m$]");
|
||
|
|
ylabel(ax1, "Z motion [$\mu m$]");
|
||
|
|
xlabel(ax2, "Y motion [$nm$]");
|
||
|
|
ylabel(ax2, "Z motion [$nm$]");
|
||
|
|
F = getframe(fig); %// Capture the frame
|
||
|
|
writeVideo(writerObj,F) %// add the frame to the movie
|
||
|
|
end
|
||
|
|
close(writerObj);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Simscape Configuration
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeSimscapeConfiguration.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeSimscapeConfiguration>>
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializeSimscapeConfiguration(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.gravity logical {mustBeNumericOrLogical} = true
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
conf_simscape = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.gravity
|
||
|
|
conf_simscape.type = 1;
|
||
|
|
else
|
||
|
|
conf_simscape.type = 2;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/conf_simscape.mat', 'conf_simscape');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Logging Configuration
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeLoggingConfiguration.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeLoggingConfiguration>>
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializeLoggingConfiguration(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
|
||
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
conf_log = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.log
|
||
|
|
case 'none'
|
||
|
|
conf_log.type = 0;
|
||
|
|
case 'all'
|
||
|
|
conf_log.type = 1;
|
||
|
|
case 'forces'
|
||
|
|
conf_log.type = 2;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Sampling Time
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
conf_log.Ts = args.Ts;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/conf_log.mat', 'conf_log');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Ground
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeGround.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeGround>>
|
||
|
|
|
||
|
|
*** Simscape Model
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
The model of the Ground is composed of:
|
||
|
|
- A *Cartesian* joint that is used to simulation the ground motion
|
||
|
|
- A solid that represents the ground on which the granite is located
|
||
|
|
|
||
|
|
#+name: fig:simscape_model_ground
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape model for the Ground
|
||
|
|
[[file:figs/images/simscape_model_ground.png]]
|
||
|
|
|
||
|
|
#+name: fig:simscape_picture_ground
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape picture for the Ground
|
||
|
|
[[file:figs/images/simscape_picture_ground.png]]
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [ground] = initializeGround(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
|
||
|
|
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =granite= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
ground = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'none'
|
||
|
|
ground.type = 0;
|
||
|
|
case 'rigid'
|
||
|
|
ground.type = 1;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Ground Solid properties
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
We set the shape and density of the ground solid element.
|
||
|
|
#+begin_src matlab
|
||
|
|
ground.shape = [2, 2, 0.5]; % [m]
|
||
|
|
ground.density = 2800; % [kg/m3]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Rotation Point
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
ground.rot_point = args.rot_point;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =ground= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'ground', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Granite
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeGranite.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeGranite>>
|
||
|
|
|
||
|
|
*** Simscape Model
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
The Simscape model of the granite is composed of:
|
||
|
|
- A cartesian joint such that the granite can vibrations along x, y and z axis
|
||
|
|
- A solid
|
||
|
|
|
||
|
|
The output =sample_pos= corresponds to the impact point of the X-ray.
|
||
|
|
|
||
|
|
#+name: fig:simscape_model_granite
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape model for the Granite
|
||
|
|
[[file:figs/images/simscape_model_granite.png]]
|
||
|
|
|
||
|
|
#+name: fig:simscape_picture_granite
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape picture for the Granite
|
||
|
|
[[file:figs/images/simscape_picture_granite.png]]
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [granite] = initializeGranite(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
|
||
|
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||
|
|
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
|
||
|
|
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
|
||
|
|
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
|
||
|
|
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
|
||
|
|
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
|
||
|
|
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
|
||
|
|
args.sample_pos (1,1) double {mustBeNumeric} = 0.8-25e-3 % Height of the measurment point [m]
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =granite= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
granite = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Granite Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'none'
|
||
|
|
granite.type = 0;
|
||
|
|
case 'rigid'
|
||
|
|
granite.type = 1;
|
||
|
|
case 'flexible'
|
||
|
|
granite.type = 2;
|
||
|
|
case 'modal-analysis'
|
||
|
|
granite.type = 3;
|
||
|
|
case 'init'
|
||
|
|
granite.type = 4;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Material and Geometry
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
Properties of the Material and link to the geometry of the granite.
|
||
|
|
#+begin_src matlab
|
||
|
|
granite.density = args.density; % [kg/m3]
|
||
|
|
granite.STEP = './STEPS/granite/granite.STEP';
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Z-offset for the initial position of the sample with respect to the granite top surface.
|
||
|
|
#+begin_src matlab
|
||
|
|
granite.sample_pos = args.sample_pos; % [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Stiffness and Damping properties
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
granite.K = args.K; % [N/m]
|
||
|
|
granite.C = args.C; % [N/(m/s)]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Equilibrium position of the each joint.
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||
|
|
load('mat/Foffset.mat', 'Fgm');
|
||
|
|
granite.Deq = -Fgm'./granite.K;
|
||
|
|
else
|
||
|
|
granite.Deq = zeros(6,1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =granite= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'granite', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Translation Stage
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeTy.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeTy>>
|
||
|
|
|
||
|
|
*** Simscape Model
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
The Simscape model of the Translation stage consist of:
|
||
|
|
- One rigid body for the fixed part of the translation stage
|
||
|
|
- One rigid body for the moving part of the translation stage
|
||
|
|
- Four 6-DOF Joints that only have some rigidity in the X and Z directions.
|
||
|
|
The rigidity in rotation comes from the fact that we use multiple joints that are located at different points
|
||
|
|
- One 6-DOF joint that represent the Actuator.
|
||
|
|
It is used to impose the motion in the Y direction
|
||
|
|
- One 6-DOF joint to inject force disturbance in the X and Z directions
|
||
|
|
|
||
|
|
#+name: fig:simscape_model_ty
|
||
|
|
#+ATTR_ORG: :width 800
|
||
|
|
#+caption: Simscape model for the Translation Stage
|
||
|
|
[[file:figs/images/simscape_model_ty.png]]
|
||
|
|
|
||
|
|
#+name: fig:simscape_picture_ty
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape picture for the Translation Stage
|
||
|
|
[[file:figs/images/simscape_picture_ty.png]]
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [ty] = initializeTy(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
|
||
|
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =ty= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
ty = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Translation Stage Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'none'
|
||
|
|
ty.type = 0;
|
||
|
|
case 'rigid'
|
||
|
|
ty.type = 1;
|
||
|
|
case 'flexible'
|
||
|
|
ty.type = 2;
|
||
|
|
case 'modal-analysis'
|
||
|
|
ty.type = 3;
|
||
|
|
case 'init'
|
||
|
|
ty.type = 4;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Material and Geometry
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Define the density of the materials as well as the geometry (STEP files).
|
||
|
|
#+begin_src matlab
|
||
|
|
% Ty Granite frame
|
||
|
|
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
|
||
|
|
ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP';
|
||
|
|
|
||
|
|
% Guide Translation Ty
|
||
|
|
ty.guide.density = 7800; % [kg/m3] => 76kg
|
||
|
|
ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP';
|
||
|
|
|
||
|
|
% Ty - Guide_Translation12
|
||
|
|
ty.guide12.density = 7800; % [kg/m3]
|
||
|
|
ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP';
|
||
|
|
|
||
|
|
% Ty - Guide_Translation11
|
||
|
|
ty.guide11.density = 7800; % [kg/m3]
|
||
|
|
ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP';
|
||
|
|
|
||
|
|
% Ty - Guide_Translation22
|
||
|
|
ty.guide22.density = 7800; % [kg/m3]
|
||
|
|
ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP';
|
||
|
|
|
||
|
|
% Ty - Guide_Translation21
|
||
|
|
ty.guide21.density = 7800; % [kg/m3]
|
||
|
|
ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP';
|
||
|
|
|
||
|
|
% Ty - Plateau translation
|
||
|
|
ty.frame.density = 7800; % [kg/m3]
|
||
|
|
ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP';
|
||
|
|
|
||
|
|
% Ty Stator Part
|
||
|
|
ty.stator.density = 5400; % [kg/m3]
|
||
|
|
ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
|
||
|
|
|
||
|
|
% Ty Rotor Part
|
||
|
|
ty.rotor.density = 5400; % [kg/m3]
|
||
|
|
ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Stiffness and Damping properties
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
|
||
|
|
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Equilibrium position of the each joint.
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||
|
|
load('mat/Foffset.mat', 'Ftym');
|
||
|
|
ty.Deq = -Ftym'./ty.K;
|
||
|
|
else
|
||
|
|
ty.Deq = zeros(6,1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =ty= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'ty', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Tilt Stage
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeRy.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeRy>>
|
||
|
|
|
||
|
|
*** Simscape Model
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
The Simscape model of the Tilt stage is composed of:
|
||
|
|
- Two solid bodies for the two part of the stage
|
||
|
|
- *Four* 6-DOF joints to model the flexibility of the stage.
|
||
|
|
These joints are virtually located along the rotation axis and are connecting the two solid bodies.
|
||
|
|
These joints have some translation stiffness in the u-v-w directions aligned with the joint.
|
||
|
|
The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations
|
||
|
|
- A Bushing Joint used for the Actuator.
|
||
|
|
The Ry motion is imposed by the input.
|
||
|
|
|
||
|
|
#+name: fig:simscape_model_ry
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape model for the Tilt Stage
|
||
|
|
[[file:figs/images/simscape_model_ry.png]]
|
||
|
|
|
||
|
|
#+name: fig:simscape_picture_ry
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape picture for the Tilt Stage
|
||
|
|
[[file:figs/images/simscape_picture_ry.png]]
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [ry] = initializeRy(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
|
||
|
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||
|
|
args.Ry_init (1,1) double {mustBeNumeric} = 0
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =ry= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
ry = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** Add Tilt Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'none'
|
||
|
|
ry.type = 0;
|
||
|
|
case 'rigid'
|
||
|
|
ry.type = 1;
|
||
|
|
case 'flexible'
|
||
|
|
ry.type = 2;
|
||
|
|
case 'modal-analysis'
|
||
|
|
ry.type = 3;
|
||
|
|
case 'init'
|
||
|
|
ry.type = 4;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Material and Geometry
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Properties of the Material and link to the geometry of the Tilt stage.
|
||
|
|
#+begin_src matlab
|
||
|
|
% Ry - Guide for the tilt stage
|
||
|
|
ry.guide.density = 7800; % [kg/m3]
|
||
|
|
ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP';
|
||
|
|
|
||
|
|
% Ry - Rotor of the motor
|
||
|
|
ry.rotor.density = 2400; % [kg/m3]
|
||
|
|
ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP';
|
||
|
|
|
||
|
|
% Ry - Motor
|
||
|
|
ry.motor.density = 3200; % [kg/m3]
|
||
|
|
ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
|
||
|
|
|
||
|
|
% Ry - Plateau Tilt
|
||
|
|
ry.stage.density = 7800; % [kg/m3]
|
||
|
|
ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Z-Offset so that the center of rotation matches the sample center;
|
||
|
|
#+begin_src matlab
|
||
|
|
ry.z_offset = 0.58178; % [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
ry.Ry_init = args.Ry_init; % [rad]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Stiffness and Damping properties
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
|
||
|
|
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Equilibrium position of the each joint.
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||
|
|
load('mat/Foffset.mat', 'Fym');
|
||
|
|
ry.Deq = -Fym'./ry.K;
|
||
|
|
else
|
||
|
|
ry.Deq = zeros(6,1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =ry= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'ry', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Spindle
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeRz.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeRz>>
|
||
|
|
|
||
|
|
*** Simscape Model
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
The Simscape model of the Spindle is composed of:
|
||
|
|
- Two rigid bodies: the stator and the rotor
|
||
|
|
- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.
|
||
|
|
- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations
|
||
|
|
|
||
|
|
#+name: fig:simscape_model_rz
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape model for the Spindle
|
||
|
|
[[file:figs/images/simscape_model_rz.png]]
|
||
|
|
|
||
|
|
#+name: fig:simscape_picture_rz
|
||
|
|
#+attr_org: :width 800
|
||
|
|
#+caption: Simscape picture for the Spindle
|
||
|
|
[[file:figs/images/simscape_picture_rz.png]]
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [rz] = initializeRz(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
|
||
|
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =rz= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
rz = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Spindle Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'none'
|
||
|
|
rz.type = 0;
|
||
|
|
case 'rigid'
|
||
|
|
rz.type = 1;
|
||
|
|
case 'flexible'
|
||
|
|
rz.type = 2;
|
||
|
|
case 'modal-analysis'
|
||
|
|
rz.type = 3;
|
||
|
|
case 'init'
|
||
|
|
rz.type = 4;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Material and Geometry
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
Properties of the Material and link to the geometry of the spindle.
|
||
|
|
#+begin_src matlab
|
||
|
|
% Spindle - Slip Ring
|
||
|
|
rz.slipring.density = 7800; % [kg/m3]
|
||
|
|
rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP';
|
||
|
|
|
||
|
|
% Spindle - Rotor
|
||
|
|
rz.rotor.density = 7800; % [kg/m3]
|
||
|
|
rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
|
||
|
|
|
||
|
|
% Spindle - Stator
|
||
|
|
rz.stator.density = 7800; % [kg/m3]
|
||
|
|
rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Stiffness and Damping properties
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
|
||
|
|
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Equilibrium position of the each joint.
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||
|
|
load('mat/Foffset.mat', 'Fzm');
|
||
|
|
rz.Deq = -Fzm'./rz.K;
|
||
|
|
else
|
||
|
|
rz.Deq = zeros(6,1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =rz= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'rz', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Micro Hexapod
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeMicroHexapod.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeMicroHexapod>>
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [micro_hexapod] = initializeMicroHexapod(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
|
||
|
|
% initializeFramesPositions
|
||
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
|
||
|
|
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
|
||
|
|
% generateGeneralConfiguration
|
||
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
|
||
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
|
||
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
|
||
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
|
||
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
|
||
|
|
% initializeStrutDynamics
|
||
|
|
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
|
||
|
|
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
|
||
|
|
% initializeCylindricalPlatforms
|
||
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
||
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
||
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
|
||
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
||
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
||
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
||
|
|
% initializeCylindricalStruts
|
||
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
||
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
||
|
|
% inverseKinematics
|
||
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||
|
|
% Force that stiffness of each joint should apply at t=0
|
||
|
|
args.Foffset logical {mustBeNumericOrLogical} = false
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Function content
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart = initializeStewartPlatform();
|
||
|
|
|
||
|
|
stewart = initializeFramesPositions(stewart, ...
|
||
|
|
'H', args.H, ...
|
||
|
|
'MO_B', args.MO_B);
|
||
|
|
|
||
|
|
stewart = generateGeneralConfiguration(stewart, ...
|
||
|
|
'FH', args.FH, ...
|
||
|
|
'FR', args.FR, ...
|
||
|
|
'FTh', args.FTh, ...
|
||
|
|
'MH', args.MH, ...
|
||
|
|
'MR', args.MR, ...
|
||
|
|
'MTh', args.MTh);
|
||
|
|
|
||
|
|
stewart = computeJointsPose(stewart);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart = initializeStrutDynamics(stewart, ...
|
||
|
|
'K', args.Ki, ...
|
||
|
|
'C', args.Ci);
|
||
|
|
|
||
|
|
stewart = initializeJointDynamics(stewart, ...
|
||
|
|
'type_F', 'universal_p', ...
|
||
|
|
'type_M', 'spherical_p');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
||
|
|
'Fpm', args.Fpm, ...
|
||
|
|
'Fph', args.Fph, ...
|
||
|
|
'Fpr', args.Fpr, ...
|
||
|
|
'Mpm', args.Mpm, ...
|
||
|
|
'Mph', args.Mph, ...
|
||
|
|
'Mpr', args.Mpr);
|
||
|
|
|
||
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
||
|
|
'Fsm', args.Fsm, ...
|
||
|
|
'Fsh', args.Fsh, ...
|
||
|
|
'Fsr', args.Fsr, ...
|
||
|
|
'Msm', args.Msm, ...
|
||
|
|
'Msh', args.Msh, ...
|
||
|
|
'Msr', args.Msr);
|
||
|
|
|
||
|
|
stewart = computeJacobian(stewart);
|
||
|
|
|
||
|
|
stewart = initializeStewartPose(stewart, ...
|
||
|
|
'AP', args.AP, ...
|
||
|
|
'ARB', args.ARB);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Equilibrium position of the each joint.
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
|
||
|
|
load('mat/Foffset.mat', 'Fhm');
|
||
|
|
stewart.actuators.dLeq = -Fhm'./args.Ki;
|
||
|
|
else
|
||
|
|
stewart.actuators.dLeq = zeros(6,1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'none'
|
||
|
|
stewart.type = 0;
|
||
|
|
case 'rigid'
|
||
|
|
stewart.type = 1;
|
||
|
|
case 'flexible'
|
||
|
|
stewart.type = 2;
|
||
|
|
case 'modal-analysis'
|
||
|
|
stewart.type = 3;
|
||
|
|
case 'init'
|
||
|
|
stewart.type = 4;
|
||
|
|
case 'compliance'
|
||
|
|
stewart.type = 5;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =micro_hexapod= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
micro_hexapod = stewart;
|
||
|
|
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Nano Hexapod
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeNanoHexapod.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeNanoHexapod>>
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [nano_hexapod] = initializeNanoHexapod(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
%% Bottom Flexible Joints
|
||
|
|
args.flex_bot_type char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
|
||
|
|
args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
|
||
|
|
args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
|
||
|
|
args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
|
||
|
|
args.flex_bot_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
|
||
|
|
args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
|
||
|
|
args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
|
||
|
|
args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
|
||
|
|
args.flex_bot_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
|
||
|
|
|
||
|
|
%% Top Flexible Joints
|
||
|
|
args.flex_top_type char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
|
||
|
|
args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
|
||
|
|
args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
|
||
|
|
args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
|
||
|
|
args.flex_top_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
|
||
|
|
args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
|
||
|
|
args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
|
||
|
|
args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
|
||
|
|
args.flex_top_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
|
||
|
|
|
||
|
|
%% Jacobian - Location of frame {A} and {B}
|
||
|
|
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
||
|
|
|
||
|
|
%% Relative Motion Sensor
|
||
|
|
args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
|
||
|
|
|
||
|
|
%% Top Plate
|
||
|
|
args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
|
||
|
|
args.top_plate_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
|
||
|
|
|
||
|
|
%% Actuators
|
||
|
|
args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
|
||
|
|
args.actuator_Ga (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
|
||
|
|
args.actuator_Gs (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
|
||
|
|
% For 2DoF
|
||
|
|
args.actuator_k (6,1) double {mustBeNumeric} = ones(6,1)*0.38e6 % [N/m]
|
||
|
|
args.actuator_ke (6,1) double {mustBeNumeric} = ones(6,1)*1.75e6 % [N/m]
|
||
|
|
args.actuator_ka (6,1) double {mustBeNumeric} = ones(6,1)*3e7 % [N/m]
|
||
|
|
args.actuator_c (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)]
|
||
|
|
args.actuator_ce (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
|
||
|
|
args.actuator_ca (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
|
||
|
|
args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
|
||
|
|
% For Flexible Frame
|
||
|
|
args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
|
||
|
|
args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
|
||
|
|
% Misalignment
|
||
|
|
args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
|
||
|
|
|
||
|
|
args.actuator_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Nano Hexapod Object
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Flexible Joints - Bot
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.flex_bot = struct();
|
||
|
|
|
||
|
|
switch args.flex_bot_type
|
||
|
|
case '2dof'
|
||
|
|
nano_hexapod.flex_bot.type = 1;
|
||
|
|
case '3dof'
|
||
|
|
nano_hexapod.flex_bot.type = 2;
|
||
|
|
case '4dof'
|
||
|
|
nano_hexapod.flex_bot.type = 3;
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.flex_bot.type = 4;
|
||
|
|
end
|
||
|
|
|
||
|
|
nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
|
||
|
|
nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
|
||
|
|
nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
|
||
|
|
nano_hexapod.flex_bot.kz = args.flex_bot_kz; % Axial stiffness [N/m]
|
||
|
|
|
||
|
|
nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
|
||
|
|
nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
|
||
|
|
nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
|
||
|
|
nano_hexapod.flex_bot.cz = args.flex_bot_cz; %[N/(m/s)]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Flexible Joints - Top
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.flex_top = struct();
|
||
|
|
|
||
|
|
switch args.flex_top_type
|
||
|
|
case '2dof'
|
||
|
|
nano_hexapod.flex_top.type = 1;
|
||
|
|
case '3dof'
|
||
|
|
nano_hexapod.flex_top.type = 2;
|
||
|
|
case '4dof'
|
||
|
|
nano_hexapod.flex_top.type = 3;
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.flex_top.type = 4;
|
||
|
|
end
|
||
|
|
|
||
|
|
nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
|
||
|
|
nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
|
||
|
|
nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
|
||
|
|
nano_hexapod.flex_top.kz = args.flex_top_kz; % Axial stiffness [N/m]
|
||
|
|
|
||
|
|
nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
|
||
|
|
nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
|
||
|
|
nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
|
||
|
|
nano_hexapod.flex_top.cz = args.flex_top_cz; %[N/(m/s)]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Relative Motion Sensor
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.motion_sensor = struct();
|
||
|
|
|
||
|
|
switch args.motion_sensor_type
|
||
|
|
case 'struts'
|
||
|
|
nano_hexapod.motion_sensor.type = 1;
|
||
|
|
case 'plates'
|
||
|
|
nano_hexapod.motion_sensor.type = 2;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Amplified Piezoelectric Actuator
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.actuator = struct();
|
||
|
|
|
||
|
|
switch args.actuator_type
|
||
|
|
case '2dof'
|
||
|
|
nano_hexapod.actuator.type = 1;
|
||
|
|
case 'flexible frame'
|
||
|
|
nano_hexapod.actuator.type = 2;
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.actuator.type = 3;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Actuator gain [N/V]
|
||
|
|
if all(args.actuator_Ga == 0)
|
||
|
|
switch args.actuator_type
|
||
|
|
case '2dof'
|
||
|
|
nano_hexapod.actuator.Ga = ones(6,1)*(-30.0);
|
||
|
|
case 'flexible frame'
|
||
|
|
nano_hexapod.actuator.Ga = ones(6,1); % TODO
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.actuator.Ga = ones(6,1)*23.4;
|
||
|
|
end
|
||
|
|
else
|
||
|
|
nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Sensor gain [V/m]
|
||
|
|
if all(args.actuator_Gs == 0)
|
||
|
|
switch args.actuator_type
|
||
|
|
case '2dof'
|
||
|
|
nano_hexapod.actuator.Gs = ones(6,1)*0.098;
|
||
|
|
case 'flexible frame'
|
||
|
|
nano_hexapod.actuator.Gs = ones(6,1); % TODO
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.actuator.Gs = ones(6,1)*(-4674824);
|
||
|
|
end
|
||
|
|
else
|
||
|
|
nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Mechanical characteristics:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.actuator_type
|
||
|
|
case '2dof'
|
||
|
|
nano_hexapod.actuator.k = args.actuator_k; % [N/m]
|
||
|
|
nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
|
||
|
|
nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
|
||
|
|
|
||
|
|
nano_hexapod.actuator.c = args.actuator_c; % [N/(m/s)]
|
||
|
|
nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
|
||
|
|
nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
|
||
|
|
|
||
|
|
nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
|
||
|
|
|
||
|
|
case 'flexible frame'
|
||
|
|
nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
|
||
|
|
nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
|
||
|
|
nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
|
||
|
|
|
||
|
|
nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
|
||
|
|
nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
|
||
|
|
nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
|
||
|
|
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
|
||
|
|
nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
|
||
|
|
nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
|
||
|
|
|
||
|
|
nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
|
||
|
|
nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
|
||
|
|
|
||
|
|
end
|
||
|
|
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Geometry
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.geometry = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Center of joints $a_i$ with respect to {F}:
|
||
|
|
#+begin_src matlab
|
||
|
|
Fa = [[-21.74, 111.91, 22.49],
|
||
|
|
[-107.79, -37.13, 22.49],
|
||
|
|
[-86.05, -74.78, 22.49],
|
||
|
|
[ 86.05, -74.78, 22.49],
|
||
|
|
[ 107.79, -37.13, 22.49],
|
||
|
|
[ 21.74, 111.91, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Center of joints $b_i$ with respect to {M}:
|
||
|
|
#+begin_src matlab
|
||
|
|
Mb = [[-77.78, 77.78, -22.50],
|
||
|
|
[-106.25, 28.47, -22.50],
|
||
|
|
[-28.47, -106.25, -22.50],
|
||
|
|
[ 28.47, -106.25, -22.50],
|
||
|
|
[ 106.25, 28.47, -22.50],
|
||
|
|
[ 77.78, 77.78, -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Now compute the positions $b_i$ with respect to {F}:
|
||
|
|
#+begin_src matlab
|
||
|
|
Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The unit vector representing the orientation of the struts can then be computed:
|
||
|
|
#+begin_src matlab
|
||
|
|
si = Fb - Fa;
|
||
|
|
si = si./vecnorm(si); % Normalize
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Location of encoder measurement points when fixed on the plates:
|
||
|
|
#+begin_src matlab
|
||
|
|
Fc = [[-76.914, 78.31, 52.605]
|
||
|
|
[-106.276, 27.454, 52.605]
|
||
|
|
[-29.362, -105.765, 52.605]
|
||
|
|
[ 29.362, -105.765, 52.605]
|
||
|
|
[ 106.276, 27.454, 52.605]
|
||
|
|
[ 76.914, 78.31, 52.605]]'*1e-3; % Meas pos w.r.t. {F}
|
||
|
|
Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.geometry.Fa = Fa;
|
||
|
|
nano_hexapod.geometry.Fb = Fb;
|
||
|
|
nano_hexapod.geometry.Fc = Fc;
|
||
|
|
nano_hexapod.geometry.Mb = Mb;
|
||
|
|
nano_hexapod.geometry.Mc = Mc;
|
||
|
|
nano_hexapod.geometry.si = si;
|
||
|
|
nano_hexapod.geometry.MO_B = args.MO_B;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Jacobian for Actuators
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
Bb = Mb - [0; 0; args.MO_B];
|
||
|
|
|
||
|
|
nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Jacobian for Sensors
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.motion_sensor_type
|
||
|
|
case 'struts'
|
||
|
|
nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
|
||
|
|
case 'plates'
|
||
|
|
Bc = Mc - [0; 0; args.MO_B];
|
||
|
|
nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Top Plate
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
nano_hexapod.top_plate = struct();
|
||
|
|
|
||
|
|
switch args.top_plate_type
|
||
|
|
case 'rigid'
|
||
|
|
nano_hexapod.top_plate.type = 1;
|
||
|
|
case 'flexible'
|
||
|
|
nano_hexapod.top_plate.type = 2;
|
||
|
|
|
||
|
|
nano_hexapod.top_plate.R_flex = ...
|
||
|
|
{[-0.51771438594671 0.5201563363052 -0.67927108019211
|
||
|
|
-0.31530446231304 -0.8540710660135 -0.41369760724945
|
||
|
|
-0.79533320729697 0 0.60617249143351 ],
|
||
|
|
[-0.01420448131632 0.9997254079576 0.01863709726679
|
||
|
|
-0.60600604129104 -0.0234330681729 0.79511481512719
|
||
|
|
0.79533320729697 0 0.60617249143351 ],
|
||
|
|
[ 0.53191886726305 0.4795690716524 0.69790817745892
|
||
|
|
-0.29070157897799 0.8775041341865 -0.38141720787774
|
||
|
|
-0.79533320729697 0 0.60617249143351 ],
|
||
|
|
[ 0.53191886726305 -0.4795690716524 -0.69790817745892
|
||
|
|
0.29070157897799 0.8775041341865 -0.38141720787774
|
||
|
|
0.79533320729697 0 0.60617249143351 ],
|
||
|
|
[-0.01420448131633 -0.9997254079576 -0.01863709726680
|
||
|
|
0.60600604129104 -0.0234330681729 0.79511481512719
|
||
|
|
-0.79533320729697 0 0.60617249143351 ],
|
||
|
|
[-0.51771438594672 -0.5201563363051 0.67927108019212
|
||
|
|
0.31530446231304 -0.8540710660135 -0.41369760724945
|
||
|
|
0.79533320729697 0 0.60617249143351 ]};
|
||
|
|
|
||
|
|
|
||
|
|
nano_hexapod.top_plate.R_enc = ...
|
||
|
|
{ [ 0.854071066013574 -0.520156336305191 0
|
||
|
|
0.520156336305191 0.854071066013574 0
|
||
|
|
0 0 1 ],
|
||
|
|
[-0.023433068172958 0.999725407957606 0
|
||
|
|
-0.999725407957606 -0.023433068172958 0
|
||
|
|
0 0 1 ],
|
||
|
|
[-0.877504134186525 -0.479569071652412 0
|
||
|
|
0.479569071652412 -0.877504134186525 0
|
||
|
|
0 0 1 ],
|
||
|
|
[ 0.877504134186525 -0.479569071652413 0
|
||
|
|
0.479569071652413 0.877504134186525 0
|
||
|
|
0 0 1 ],
|
||
|
|
[ 0.023433068172945 0.999725407957606 0
|
||
|
|
-0.999725407957606 0.023433068172945 0
|
||
|
|
0 0 1 ],
|
||
|
|
[-0.854071066013566 -0.520156336305202 0
|
||
|
|
0.520156336305202 -0.854071066013566 0
|
||
|
|
0 0 1 ]};
|
||
|
|
|
||
|
|
nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
|
||
|
|
nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
|
||
|
|
nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
|
||
|
|
nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
|
||
|
|
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if nargout == 0
|
||
|
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Sample
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeSample.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeSample>>
|
||
|
|
|
||
|
|
*** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [sample] = initializeSample(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0'
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =sample= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
sample = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Add Sample Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case '0'
|
||
|
|
sample.type = 0;
|
||
|
|
case '1'
|
||
|
|
sample.type = 1;
|
||
|
|
case '2'
|
||
|
|
sample.type = 2;
|
||
|
|
case '3'
|
||
|
|
sample.type = 3;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =sample= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'sample', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Initialize Controller
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeController.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeController>>
|
||
|
|
|
||
|
|
*** Function Declaration and Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializeController(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =controller= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
controller = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Controller Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'open-loop'
|
||
|
|
controller.type = 1;
|
||
|
|
controller.name = 'Open-Loop';
|
||
|
|
case 'dvf'
|
||
|
|
controller.type = 2;
|
||
|
|
controller.name = 'Decentralized Direct Velocity Feedback';
|
||
|
|
case 'iff'
|
||
|
|
controller.type = 3;
|
||
|
|
controller.name = 'Decentralized Integral Force Feedback';
|
||
|
|
case 'hac-dvf'
|
||
|
|
controller.type = 4;
|
||
|
|
controller.name = 'HAC-DVF';
|
||
|
|
case 'ref-track-L'
|
||
|
|
controller.type = 5;
|
||
|
|
controller.name = 'Reference Tracking in the frame of the legs';
|
||
|
|
case 'ref-track-iff-L'
|
||
|
|
controller.type = 6;
|
||
|
|
controller.name = 'Reference Tracking in the frame of the legs + IFF';
|
||
|
|
case 'cascade-hac-lac'
|
||
|
|
controller.type = 7;
|
||
|
|
controller.name = 'Cascade Control + HAC-LAC';
|
||
|
|
case 'hac-iff'
|
||
|
|
controller.type = 8;
|
||
|
|
controller.name = 'HAC-IFF';
|
||
|
|
case 'stabilizing'
|
||
|
|
controller.type = 9;
|
||
|
|
controller.name = 'Stabilizing Controller';
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save the Structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
The =controller= structure is saved.
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/controller.mat', 'controller');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Generate Reference Signals
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeReferences.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeReferences>>
|
||
|
|
|
||
|
|
*** Function Declaration and Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [ref] = initializeReferences(args)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
% Sampling Frequency [s]
|
||
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||
|
|
% Maximum simulation time [s]
|
||
|
|
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
||
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
||
|
|
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
||
|
|
% Amplitude of the displacement [m]
|
||
|
|
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
||
|
|
% Period of the displacement [s]
|
||
|
|
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
||
|
|
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
||
|
|
% Amplitude [rad]
|
||
|
|
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
||
|
|
% Period of the displacement [s]
|
||
|
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
% Either "constant" / "rotating"
|
||
|
|
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
|
||
|
|
% Initial angle [rad]
|
||
|
|
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
||
|
|
% Period of the rotating [s]
|
||
|
|
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
% For now, only constant is implemented
|
||
|
|
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
|
||
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
||
|
|
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
||
|
|
% For now, only constant is implemented
|
||
|
|
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
|
||
|
|
% Initial position of the two masses
|
||
|
|
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
|
||
|
|
% For now, only constant is implemented
|
||
|
|
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
|
||
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform
|
||
|
|
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** Initialize Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Set Sampling Time
|
||
|
|
Ts = args.Ts;
|
||
|
|
Tmax = args.Tmax;
|
||
|
|
|
||
|
|
%% Low Pass Filter to filter out the references
|
||
|
|
s = zpk('s');
|
||
|
|
w0 = 2*pi*10;
|
||
|
|
xi = 1;
|
||
|
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Translation Stage
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Translation stage - Dy
|
||
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
||
|
|
Dy = zeros(length(t), 1);
|
||
|
|
Dyd = zeros(length(t), 1);
|
||
|
|
Dydd = zeros(length(t), 1);
|
||
|
|
switch args.Dy_type
|
||
|
|
case 'constant'
|
||
|
|
Dy(:) = args.Dy_amplitude;
|
||
|
|
Dyd(:) = 0;
|
||
|
|
Dydd(:) = 0;
|
||
|
|
case 'triangular'
|
||
|
|
% This is done to unsure that we start with no displacement
|
||
|
|
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
|
||
|
|
i0 = find(t>=args.Dy_period/4,1);
|
||
|
|
Dy(1:end-i0+1) = Dy_raw(i0:end);
|
||
|
|
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
|
||
|
|
|
||
|
|
% The signal is filtered out
|
||
|
|
Dy = lsim(H_lpf, Dy, t);
|
||
|
|
Dyd = lsim(H_lpf*s, Dy, t);
|
||
|
|
Dydd = lsim(H_lpf*s^2, Dy, t);
|
||
|
|
case 'sinusoidal'
|
||
|
|
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
|
||
|
|
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
|
||
|
|
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
|
||
|
|
otherwise
|
||
|
|
warning('Dy_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Tilt Stage
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Tilt Stage - Ry
|
||
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
||
|
|
Ry = zeros(length(t), 1);
|
||
|
|
Ryd = zeros(length(t), 1);
|
||
|
|
Rydd = zeros(length(t), 1);
|
||
|
|
|
||
|
|
switch args.Ry_type
|
||
|
|
case 'constant'
|
||
|
|
Ry(:) = args.Ry_amplitude;
|
||
|
|
Ryd(:) = 0;
|
||
|
|
Rydd(:) = 0;
|
||
|
|
case 'triangular'
|
||
|
|
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
|
||
|
|
i0 = find(t>=args.Ry_period/4,1);
|
||
|
|
Ry(1:end-i0+1) = Ry_raw(i0:end);
|
||
|
|
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
|
||
|
|
|
||
|
|
% The signal is filtered out
|
||
|
|
Ry = lsim(H_lpf, Ry, t);
|
||
|
|
Ryd = lsim(H_lpf*s, Ry, t);
|
||
|
|
Rydd = lsim(H_lpf*s^2, Ry, t);
|
||
|
|
case 'sinusoidal'
|
||
|
|
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
|
||
|
|
|
||
|
|
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
|
||
|
|
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
|
||
|
|
otherwise
|
||
|
|
warning('Ry_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Spindle
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Spindle - Rz
|
||
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
||
|
|
Rz = zeros(length(t), 1);
|
||
|
|
Rzd = zeros(length(t), 1);
|
||
|
|
Rzdd = zeros(length(t), 1);
|
||
|
|
|
||
|
|
switch args.Rz_type
|
||
|
|
case 'constant'
|
||
|
|
Rz(:) = args.Rz_amplitude;
|
||
|
|
Rzd(:) = 0;
|
||
|
|
Rzdd(:) = 0;
|
||
|
|
case 'rotating-not-filtered'
|
||
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
||
|
|
|
||
|
|
% The signal is filtered out
|
||
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
||
|
|
Rzd(:) = 2*pi/args.Rz_period;
|
||
|
|
Rzdd(:) = 0;
|
||
|
|
|
||
|
|
% We add the angle offset
|
||
|
|
Rz = Rz + args.Rz_amplitude;
|
||
|
|
|
||
|
|
case 'rotating'
|
||
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
||
|
|
|
||
|
|
% The signal is filtered out
|
||
|
|
Rz = lsim(H_lpf, Rz, t);
|
||
|
|
Rzd = lsim(H_lpf*s, Rz, t);
|
||
|
|
Rzdd = lsim(H_lpf*s^2, Rz, t);
|
||
|
|
|
||
|
|
% We add the angle offset
|
||
|
|
Rz = Rz + args.Rz_amplitude;
|
||
|
|
otherwise
|
||
|
|
warning('Rz_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Micro Hexapod
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Micro-Hexapod
|
||
|
|
t = [0, Ts];
|
||
|
|
Dh = zeros(length(t), 6);
|
||
|
|
Dhl = zeros(length(t), 6);
|
||
|
|
|
||
|
|
switch args.Dh_type
|
||
|
|
case 'constant'
|
||
|
|
Dh = [args.Dh_pos, args.Dh_pos];
|
||
|
|
|
||
|
|
load('mat/stages.mat', 'micro_hexapod');
|
||
|
|
|
||
|
|
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
|
||
|
|
|
||
|
|
tx = args.Dh_pos(4);
|
||
|
|
ty = args.Dh_pos(5);
|
||
|
|
tz = args.Dh_pos(6);
|
||
|
|
|
||
|
|
ARB = [cos(tz) -sin(tz) 0;
|
||
|
|
sin(tz) cos(tz) 0;
|
||
|
|
0 0 1]*...
|
||
|
|
[ cos(ty) 0 sin(ty);
|
||
|
|
0 1 0;
|
||
|
|
-sin(ty) 0 cos(ty)]*...
|
||
|
|
[1 0 0;
|
||
|
|
0 cos(tx) -sin(tx);
|
||
|
|
0 sin(tx) cos(tx)];
|
||
|
|
|
||
|
|
[~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
|
||
|
|
Dhl = [Dhl, Dhl];
|
||
|
|
otherwise
|
||
|
|
warning('Dh_type is not set correctly');
|
||
|
|
end
|
||
|
|
|
||
|
|
Dh = struct('time', t, 'signals', struct('values', Dh));
|
||
|
|
Dhl = struct('time', t, 'signals', struct('values', Dhl));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
%% Save
|
||
|
|
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Initialize Position Errors
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializePosError.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
||
|
|
:header-args:matlab+: :eval no :results none
|
||
|
|
:END:
|
||
|
|
<<sec:initializePosError>>
|
||
|
|
|
||
|
|
*** Function Declaration and Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializePosError(args)
|
||
|
|
% initializePosError - Initialize the position errors
|
||
|
|
%
|
||
|
|
% Syntax: [] = initializePosError(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args -
|
||
|
|
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.error logical {mustBeNumericOrLogical} = false
|
||
|
|
args.Dy (1,1) double {mustBeNumeric} = 0 % [m]
|
||
|
|
args.Ry (1,1) double {mustBeNumeric} = 0 % [m]
|
||
|
|
args.Rz (1,1) double {mustBeNumeric} = 0 % [m]
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =pos_error= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
pos_error = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.error
|
||
|
|
pos_error.type = 1;
|
||
|
|
else
|
||
|
|
pos_error.type = 0;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Position Errors
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
pos_error.Dy = args.Dy;
|
||
|
|
pos_error.Ry = args.Ry;
|
||
|
|
pos_error.Rz = args.Rz;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/pos_error.mat', 'pos_error');
|
||
|
|
#+end_src
|
||
|
|
** Initialize Rz Estimate
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeRzEstimate.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
||
|
|
:header-args:matlab+: :eval no :results none
|
||
|
|
:END:
|
||
|
|
<<sec:initializeRzEstimate>>
|
||
|
|
|
||
|
|
*** Function Declaration and Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializeRzEstimate(args)
|
||
|
|
% initializeRzEstimate - Initialize the position errors
|
||
|
|
%
|
||
|
|
% Syntax: [] = initializeRzEstimate(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args -
|
||
|
|
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'encoders', 'voltages'})} = 'encoders'
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =rz_estimate= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
rz_estimate = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type
|
||
|
|
case 'encoders'
|
||
|
|
rz_estimate.type = 0;
|
||
|
|
case 'voltages'
|
||
|
|
rz_estimate.type = 1;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Position Errors
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
load('mat/stages.mat', 'nano_hexapod');
|
||
|
|
rz_estimate.J_L_to_X = pinv(nano_hexapod.geometry.J);
|
||
|
|
rz_estimate.pz_sensitivity = -240e-6/8.5; % [m/V]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'rz_estimate', '-append');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Initialize Lion Metrology
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeLion.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
||
|
|
:header-args:matlab+: :eval no :results none
|
||
|
|
:END:
|
||
|
|
<<sec:initializeLion>>
|
||
|
|
|
||
|
|
*** Function Declaration and Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializeLion(args)
|
||
|
|
% initializeLion - Initialize the position errors
|
||
|
|
%
|
||
|
|
% Syntax: [] = initializeLion(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args -
|
||
|
|
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
args.type char {mustBeMember(args.type,{'rigid'})} = 'rigid'
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Structure initialization
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
First, we initialize the =rz_estimate= structure.
|
||
|
|
#+begin_src matlab
|
||
|
|
lion = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Jacobian
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
lion.J_int_to_X = [ 0 0 -0.787401574803149 -0.212598425196851 0;
|
||
|
|
0.78740157480315 0.21259842519685 0 0 0;
|
||
|
|
0 0 0 0 -1;
|
||
|
|
-13.1233595800525 13.1233595800525 0 0 0;
|
||
|
|
0 0 -13.1233595800525 13.1233595800525 0];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/stages.mat', 'lion', '-append');
|
||
|
|
#+end_src
|
||
|
|
** Initialize Disturbances
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeDisturbances.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
||
|
|
:header-args:matlab+: :eval no :results none
|
||
|
|
:END:
|
||
|
|
<<sec:initializeDisturbances>>
|
||
|
|
|
||
|
|
*** Function Declaration and Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = initializeDisturbances(args)
|
||
|
|
% initializeDisturbances - Initialize the disturbances
|
||
|
|
%
|
||
|
|
% Syntax: [] = initializeDisturbances(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args -
|
||
|
|
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
% Global parameter to enable or disable the disturbances
|
||
|
|
args.enable logical {mustBeNumericOrLogical} = true
|
||
|
|
% Ground Motion - X direction
|
||
|
|
args.Dwx logical {mustBeNumericOrLogical} = true
|
||
|
|
% Ground Motion - Y direction
|
||
|
|
args.Dwy logical {mustBeNumericOrLogical} = true
|
||
|
|
% Ground Motion - Z direction
|
||
|
|
args.Dwz logical {mustBeNumericOrLogical} = true
|
||
|
|
% Translation Stage - X direction
|
||
|
|
args.Fty_x logical {mustBeNumericOrLogical} = true
|
||
|
|
% Translation Stage - Z direction
|
||
|
|
args.Fty_z logical {mustBeNumericOrLogical} = true
|
||
|
|
% Spindle - Z direction
|
||
|
|
args.Frz_z logical {mustBeNumericOrLogical} = true
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** Load Data
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
load('./mat/dist_psd.mat', 'dist_f');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
We remove the first frequency point that usually is very large.
|
||
|
|
#+begin_src matlab :exports none
|
||
|
|
dist_f.f = dist_f.f(2:end);
|
||
|
|
dist_f.psd_gm = dist_f.psd_gm(2:end);
|
||
|
|
dist_f.psd_ty = dist_f.psd_ty(2:end);
|
||
|
|
dist_f.psd_rz = dist_f.psd_rz(2:end);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
We define some parameters that will be used in the algorithm.
|
||
|
|
#+begin_src matlab
|
||
|
|
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||
|
|
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
|
||
|
|
T0 = N/Fs; % Signal Duration [s]
|
||
|
|
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
||
|
|
% Also equal to (dist_f.f(2)-dist_f.f(1))
|
||
|
|
t = linspace(0, T0, N+1)'; % Time Vector [s]
|
||
|
|
Ts = 1/Fs; % Sampling Time [s]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Ground Motion
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
phi = dist_f.psd_gm;
|
||
|
|
C = zeros(N/2,1);
|
||
|
|
for i = 1:N/2
|
||
|
|
C(i) = sqrt(phi(i)*df);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Dwx && args.enable
|
||
|
|
rng(111);
|
||
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||
|
|
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
||
|
|
else
|
||
|
|
Dwx = zeros(length(t), 1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Dwy && args.enable
|
||
|
|
rng(112);
|
||
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||
|
|
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
||
|
|
else
|
||
|
|
Dwy = zeros(length(t), 1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Dwy && args.enable
|
||
|
|
rng(113);
|
||
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||
|
|
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
||
|
|
else
|
||
|
|
Dwz = zeros(length(t), 1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Translation Stage - X direction
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Fty_x && args.enable
|
||
|
|
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
|
||
|
|
C = zeros(N/2,1);
|
||
|
|
for i = 1:N/2
|
||
|
|
C(i) = sqrt(phi(i)*df);
|
||
|
|
end
|
||
|
|
rng(121);
|
||
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
||
|
|
Fty_x = u;
|
||
|
|
else
|
||
|
|
Fty_x = zeros(length(t), 1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Translation Stage - Z direction
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Fty_z && args.enable
|
||
|
|
phi = dist_f.psd_ty;
|
||
|
|
C = zeros(N/2,1);
|
||
|
|
for i = 1:N/2
|
||
|
|
C(i) = sqrt(phi(i)*df);
|
||
|
|
end
|
||
|
|
rng(122);
|
||
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
||
|
|
Fty_z = u;
|
||
|
|
else
|
||
|
|
Fty_z = zeros(length(t), 1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Spindle - Z direction
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.Frz_z && args.enable
|
||
|
|
phi = dist_f.psd_rz;
|
||
|
|
C = zeros(N/2,1);
|
||
|
|
for i = 1:N/2
|
||
|
|
C(i) = sqrt(phi(i)*df);
|
||
|
|
end
|
||
|
|
rng(131);
|
||
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
||
|
|
Frz_z = u;
|
||
|
|
else
|
||
|
|
Frz_z = zeros(length(t), 1);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Direct Forces
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
u = zeros(length(t), 6);
|
||
|
|
Fd = u;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Set initial value to zero
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
Dwx = Dwx - Dwx(1);
|
||
|
|
Dwy = Dwy - Dwy(1);
|
||
|
|
Dwz = Dwz - Dwz(1);
|
||
|
|
Fty_x = Fty_x - Fty_x(1);
|
||
|
|
Fty_z = Fty_z - Fty_z(1);
|
||
|
|
Frz_z = Frz_z - Frz_z(1);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** Save
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
** Initialize Stewart Platform
|
||
|
|
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeStewartPlatform.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeStewartPlatform>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeStewartPlatform.m][here]].
|
||
|
|
|
||
|
|
**** Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+name: fig:stewart-frames-position
|
||
|
|
#+caption: Definition of the position of the frames
|
||
|
|
[[file:figs/stewart-frames-position.png]]
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeStewartPlatform()
|
||
|
|
% initializeStewartPlatform - Initialize the stewart structure
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeStewartPlatform(args)
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - A structure with the following sub-structures:
|
||
|
|
% - platform_F -
|
||
|
|
% - platform_M -
|
||
|
|
% - joints_F -
|
||
|
|
% - joints_M -
|
||
|
|
% - struts_F -
|
||
|
|
% - struts_M -
|
||
|
|
% - actuators -
|
||
|
|
% - geometry -
|
||
|
|
% - properties -
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Initialize the Stewart structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart = struct();
|
||
|
|
stewart.platform_F = struct();
|
||
|
|
stewart.platform_M = struct();
|
||
|
|
stewart.joints_F = struct();
|
||
|
|
stewart.joints_M = struct();
|
||
|
|
stewart.struts_F = struct();
|
||
|
|
stewart.struts_M = struct();
|
||
|
|
stewart.actuators = struct();
|
||
|
|
stewart.sensors = struct();
|
||
|
|
stewart.sensors.inertial = struct();
|
||
|
|
stewart.sensors.force = struct();
|
||
|
|
stewart.sensors.relative = struct();
|
||
|
|
stewart.geometry = struct();
|
||
|
|
stewart.kinematics = struct();
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeFramesPositions.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeFramesPositions>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeFramesPositions.m][here]].
|
||
|
|
|
||
|
|
**** Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+name: fig:stewart-frames-position
|
||
|
|
#+caption: Definition of the position of the frames
|
||
|
|
[[file:figs/stewart-frames-position.png]]
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeFramesPositions(stewart, args)
|
||
|
|
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Can have the following fields:
|
||
|
|
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||
|
|
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - A structure with the following fields:
|
||
|
|
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
||
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the position of each frame
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
H = args.H; % Total Height of the Stewart Platform [m]
|
||
|
|
|
||
|
|
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
||
|
|
|
||
|
|
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||
|
|
|
||
|
|
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.geometry.H = H;
|
||
|
|
stewart.geometry.FO_M = FO_M;
|
||
|
|
stewart.platform_M.MO_B = MO_B;
|
||
|
|
stewart.platform_F.FO_A = FO_A;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =generateGeneralConfiguration=: Generate a Very General Configuration
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/generateGeneralConfiguration.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:generateGeneralConfiguration>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/generateGeneralConfiguration.m][here]].
|
||
|
|
|
||
|
|
**** Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
|
||
|
|
The radius of the circles can be chosen as well as the angles where the joints are located (see Figure [[fig:joint_position_general]]).
|
||
|
|
|
||
|
|
#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
|
||
|
|
\begin{tikzpicture}
|
||
|
|
% Internal and external limit
|
||
|
|
\draw[fill=white!80!black] (0, 0) circle [radius=3];
|
||
|
|
% Circle where the joints are located
|
||
|
|
\draw[dashed] (0, 0) circle [radius=2.5];
|
||
|
|
|
||
|
|
% Bullets for the positions of the joints
|
||
|
|
\node[] (J1) at ( 80:2.5){$\bullet$};
|
||
|
|
\node[] (J2) at (100:2.5){$\bullet$};
|
||
|
|
\node[] (J3) at (200:2.5){$\bullet$};
|
||
|
|
\node[] (J4) at (220:2.5){$\bullet$};
|
||
|
|
\node[] (J5) at (320:2.5){$\bullet$};
|
||
|
|
\node[] (J6) at (340:2.5){$\bullet$};
|
||
|
|
|
||
|
|
% Name of the points
|
||
|
|
\node[above right] at (J1) {$a_{1}$};
|
||
|
|
\node[above left] at (J2) {$a_{2}$};
|
||
|
|
\node[above left] at (J3) {$a_{3}$};
|
||
|
|
\node[right ] at (J4) {$a_{4}$};
|
||
|
|
\node[left ] at (J5) {$a_{5}$};
|
||
|
|
\node[above right] at (J6) {$a_{6}$};
|
||
|
|
|
||
|
|
% First 2 angles
|
||
|
|
\draw[dashed, ->] (0:1) arc [start angle=0, end angle=80, radius=1] node[below right]{$\theta_{1}$};
|
||
|
|
\draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left ]{$\theta_{2}$};
|
||
|
|
|
||
|
|
% Division of 360 degrees by 3
|
||
|
|
\draw[dashed] (0, 0) -- ( 80:3.2);
|
||
|
|
\draw[dashed] (0, 0) -- (100:3.2);
|
||
|
|
\draw[dashed] (0, 0) -- (200:3.2);
|
||
|
|
\draw[dashed] (0, 0) -- (220:3.2);
|
||
|
|
\draw[dashed] (0, 0) -- (320:3.2);
|
||
|
|
\draw[dashed] (0, 0) -- (340:3.2);
|
||
|
|
|
||
|
|
% Radius for the position of the joints
|
||
|
|
\draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);
|
||
|
|
|
||
|
|
\draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
|
||
|
|
\draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
|
||
|
|
\end{tikzpicture}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:joint_position_general
|
||
|
|
#+caption: Position of the joints
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/stewart_bottom_plate.png]]
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||
|
|
% generateGeneralConfiguration - Generate a Very General Configuration
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Can have the following fields:
|
||
|
|
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||
|
|
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||
|
|
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||
|
|
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||
|
|
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||
|
|
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
||
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the pose
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
Fa = zeros(3,6);
|
||
|
|
Mb = zeros(3,6);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
for i = 1:6
|
||
|
|
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||
|
|
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.platform_F.Fa = Fa;
|
||
|
|
stewart.platform_M.Mb = Mb;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =computeJointsPose=: Compute the Pose of the Joints
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/computeJointsPose.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:computeJointsPose>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/computeJointsPose.m][here]].
|
||
|
|
|
||
|
|
**** Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+name: fig:stewart-struts
|
||
|
|
#+caption: Position and orientation of the struts
|
||
|
|
[[file:figs/stewart-struts.png]]
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = computeJointsPose(stewart)
|
||
|
|
% computeJointsPose -
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = computeJointsPose(stewart)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart - A structure with the following fields
|
||
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
||
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
||
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - A structure with the following added fields
|
||
|
|
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||
|
|
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||
|
|
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||
|
|
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||
|
|
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
||
|
|
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||
|
|
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||
|
|
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
||
|
|
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
||
|
|
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||
|
|
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Check the =stewart= structure elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||
|
|
Fa = stewart.platform_F.Fa;
|
||
|
|
|
||
|
|
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||
|
|
Mb = stewart.platform_M.Mb;
|
||
|
|
|
||
|
|
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||
|
|
FO_A = stewart.platform_F.FO_A;
|
||
|
|
|
||
|
|
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||
|
|
MO_B = stewart.platform_M.MO_B;
|
||
|
|
|
||
|
|
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
||
|
|
FO_M = stewart.geometry.FO_M;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the position of the Joints
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
Aa = Fa - repmat(FO_A, [1, 6]);
|
||
|
|
Bb = Mb - repmat(MO_B, [1, 6]);
|
||
|
|
|
||
|
|
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
||
|
|
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the strut length and orientation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
||
|
|
|
||
|
|
l = vecnorm(Ab - Aa)';
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the orientation of the Joints
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
FRa = zeros(3,3,6);
|
||
|
|
MRb = zeros(3,3,6);
|
||
|
|
|
||
|
|
for i = 1:6
|
||
|
|
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
||
|
|
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
||
|
|
|
||
|
|
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
||
|
|
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.geometry.Aa = Aa;
|
||
|
|
stewart.geometry.Ab = Ab;
|
||
|
|
stewart.geometry.Ba = Ba;
|
||
|
|
stewart.geometry.Bb = Bb;
|
||
|
|
stewart.geometry.As = As;
|
||
|
|
stewart.geometry.Bs = Bs;
|
||
|
|
stewart.geometry.l = l;
|
||
|
|
|
||
|
|
stewart.struts_F.l = l/2;
|
||
|
|
stewart.struts_M.l = l/2;
|
||
|
|
|
||
|
|
stewart.platform_F.FRa = FRa;
|
||
|
|
stewart.platform_M.MRb = MRb;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeStewartPose.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeStewartPose>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeStewartPose.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeStewartPose(stewart, args)
|
||
|
|
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
|
||
|
|
% It uses the inverse kinematic
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeStewartPose(stewart, args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart - A structure with the following fields
|
||
|
|
% - Aa [3x6] - The positions ai expressed in {A}
|
||
|
|
% - Bb [3x6] - The positions bi expressed in {B}
|
||
|
|
% - args - Can have the following fields:
|
||
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Use the Inverse Kinematic function
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.actuators.Leq = dLi;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeCylindricalPlatforms.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeCylindricalPlatforms>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeCylindricalPlatforms.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||
|
|
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Structure with the following fields:
|
||
|
|
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||
|
|
% - Fph [1x1] - Fixed Platform Height [m]
|
||
|
|
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||
|
|
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||
|
|
% - Mph [1x1] - Mobile Platform Height [m]
|
||
|
|
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - platform_F [struct] - structure with the following fields:
|
||
|
|
% - type = 1
|
||
|
|
% - M [1x1] - Fixed Platform Mass [kg]
|
||
|
|
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||
|
|
% - H [1x1] - Fixed Platform Height [m]
|
||
|
|
% - R [1x1] - Fixed Platform Radius [m]
|
||
|
|
% - platform_M [struct] - structure with the following fields:
|
||
|
|
% - M [1x1] - Mobile Platform Mass [kg]
|
||
|
|
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||
|
|
% - H [1x1] - Mobile Platform Height [m]
|
||
|
|
% - R [1x1] - Mobile Platform Radius [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
||
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the Inertia matrices of platforms
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||
|
|
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||
|
|
1/2 *args.Fpm * args.Fpr^2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||
|
|
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||
|
|
1/2 *args.Mpm * args.Mpr^2]);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.platform_F.type = 1;
|
||
|
|
|
||
|
|
stewart.platform_F.I = I_F;
|
||
|
|
stewart.platform_F.M = args.Fpm;
|
||
|
|
stewart.platform_F.R = args.Fpr;
|
||
|
|
stewart.platform_F.H = args.Fph;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.platform_M.type = 1;
|
||
|
|
|
||
|
|
stewart.platform_M.I = I_M;
|
||
|
|
stewart.platform_M.M = args.Mpm;
|
||
|
|
stewart.platform_M.R = args.Mpr;
|
||
|
|
stewart.platform_M.H = args.Mph;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeCylindricalStruts.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeCylindricalStruts>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeCylindricalStruts.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||
|
|
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Structure with the following fields:
|
||
|
|
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||
|
|
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||
|
|
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||
|
|
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||
|
|
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||
|
|
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - struts_F [struct] - structure with the following fields:
|
||
|
|
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
||
|
|
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||
|
|
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||
|
|
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||
|
|
% - struts_M [struct] - structure with the following fields:
|
||
|
|
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
||
|
|
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||
|
|
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||
|
|
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the properties of the cylindrical struts
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
Fsm = ones(6,1).*args.Fsm;
|
||
|
|
Fsh = ones(6,1).*args.Fsh;
|
||
|
|
Fsr = ones(6,1).*args.Fsr;
|
||
|
|
|
||
|
|
Msm = ones(6,1).*args.Msm;
|
||
|
|
Msh = ones(6,1).*args.Msh;
|
||
|
|
Msr = ones(6,1).*args.Msr;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
||
|
|
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
||
|
|
|
||
|
|
for i = 1:6
|
||
|
|
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||
|
|
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||
|
|
1/2 * Fsm(i) * Fsr(i)^2]);
|
||
|
|
|
||
|
|
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||
|
|
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||
|
|
1/2 * Msm(i) * Msr(i)^2]);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.struts_M.type = 1;
|
||
|
|
|
||
|
|
stewart.struts_M.I = I_M;
|
||
|
|
stewart.struts_M.M = Msm;
|
||
|
|
stewart.struts_M.R = Msr;
|
||
|
|
stewart.struts_M.H = Msh;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.struts_F.type = 1;
|
||
|
|
|
||
|
|
stewart.struts_F.I = I_F;
|
||
|
|
stewart.struts_F.M = Fsm;
|
||
|
|
stewart.struts_F.R = Fsr;
|
||
|
|
stewart.struts_F.H = Fsh;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeStrutDynamics.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeStrutDynamics>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeStrutDynamics.m][here]].
|
||
|
|
|
||
|
|
**** Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+name: fig:piezoelectric_stack
|
||
|
|
#+attr_html: :width 500px
|
||
|
|
#+caption: Example of a piezoelectric stach actuator (PI)
|
||
|
|
[[file:figs/piezoelectric_stack.jpg]]
|
||
|
|
|
||
|
|
A simplistic model of such amplified actuator is shown in Figure [[fig:actuator_model_simple]] where:
|
||
|
|
- $K$ represent the vertical stiffness of the actuator
|
||
|
|
- $C$ represent the vertical damping of the actuator
|
||
|
|
- $F$ represents the force applied by the actuator
|
||
|
|
- $F_{m}$ represents the total measured force
|
||
|
|
- $v_{m}$ represents the absolute velocity of the top part of the actuator
|
||
|
|
- $d_{m}$ represents the total relative displacement of the actuator
|
||
|
|
|
||
|
|
#+begin_src latex :file actuator_model_simple.pdf :tangle no
|
||
|
|
\begin{tikzpicture}
|
||
|
|
\draw (-1, 0) -- (1, 0);
|
||
|
|
|
||
|
|
% Spring, Damper, and Actuator
|
||
|
|
\draw[spring] (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
|
||
|
|
\draw[damper] ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
|
||
|
|
\draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};
|
||
|
|
|
||
|
|
\node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};
|
||
|
|
|
||
|
|
\node[left] at (fsens.west) {$F_{m}$};
|
||
|
|
|
||
|
|
\draw[dashed] (1, 0) -- ++(0.4, 0);
|
||
|
|
\draw[dashed] (1, 1.7) -- ++(0.4, 0);
|
||
|
|
|
||
|
|
\draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};
|
||
|
|
|
||
|
|
\draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
|
||
|
|
\end{tikzpicture}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+name: fig:actuator_model_simple
|
||
|
|
#+caption: Simple model of an Actuator
|
||
|
|
#+RESULTS:
|
||
|
|
[[file:figs/actuator_model_simple.png]]
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeStrutDynamics(stewart, args)
|
||
|
|
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Structure with the following fields:
|
||
|
|
% - K [6x1] - Stiffness of each strut [N/m]
|
||
|
|
% - C [6x1] - Damping of each strut [N/(m/s)]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - actuators.type = 1
|
||
|
|
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
||
|
|
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
|
||
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
||
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
||
|
|
args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
|
||
|
|
args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
|
||
|
|
args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
|
||
|
|
args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
|
||
|
|
args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
|
||
|
|
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
||
|
|
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Add Stiffness and Damping properties of each strut
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
if strcmp(args.type, 'classical')
|
||
|
|
stewart.actuators.type = 1;
|
||
|
|
elseif strcmp(args.type, 'amplified')
|
||
|
|
stewart.actuators.type = 2;
|
||
|
|
end
|
||
|
|
|
||
|
|
stewart.actuators.K = args.K;
|
||
|
|
stewart.actuators.C = args.C;
|
||
|
|
|
||
|
|
stewart.actuators.k1 = args.k1;
|
||
|
|
stewart.actuators.c1 = args.c1;
|
||
|
|
|
||
|
|
stewart.actuators.ka = args.ka;
|
||
|
|
stewart.actuators.ke = args.ke;
|
||
|
|
|
||
|
|
stewart.actuators.F_gain = args.F_gain;
|
||
|
|
|
||
|
|
stewart.actuators.ma = args.ma;
|
||
|
|
stewart.actuators.me = args.me;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeJointDynamics.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeJointDynamics>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeJointDynamics.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeJointDynamics(stewart, args)
|
||
|
|
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeJointDynamics(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Structure with the following fields:
|
||
|
|
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
||
|
|
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
||
|
|
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
|
||
|
|
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
|
||
|
|
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
|
||
|
|
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
|
||
|
|
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
|
||
|
|
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
|
||
|
|
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
|
||
|
|
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - stewart.joints_F and stewart.joints_M:
|
||
|
|
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
|
||
|
|
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
|
||
|
|
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
|
||
|
|
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
|
||
|
|
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
|
||
|
|
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
|
||
|
|
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
|
||
|
|
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
|
||
|
|
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
||
|
|
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||
|
|
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
||
|
|
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||
|
|
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
||
|
|
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||
|
|
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
||
|
|
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||
|
|
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
||
|
|
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||
|
|
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
||
|
|
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||
|
|
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
||
|
|
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||
|
|
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
||
|
|
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||
|
|
args.K_M double {mustBeNumeric} = zeros(6,6)
|
||
|
|
args.M_M double {mustBeNumeric} = zeros(6,6)
|
||
|
|
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
||
|
|
args.xi_M double {mustBeNumeric} = 0.1
|
||
|
|
args.step_file_M char {} = ''
|
||
|
|
args.K_F double {mustBeNumeric} = zeros(6,6)
|
||
|
|
args.M_F double {mustBeNumeric} = zeros(6,6)
|
||
|
|
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
||
|
|
args.xi_F double {mustBeNumeric} = 0.1
|
||
|
|
args.step_file_F char {} = ''
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Add Actuator Type
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.type_F
|
||
|
|
case 'universal'
|
||
|
|
stewart.joints_F.type = 1;
|
||
|
|
case 'spherical'
|
||
|
|
stewart.joints_F.type = 2;
|
||
|
|
case 'universal_p'
|
||
|
|
stewart.joints_F.type = 3;
|
||
|
|
case 'spherical_p'
|
||
|
|
stewart.joints_F.type = 4;
|
||
|
|
case 'flexible'
|
||
|
|
stewart.joints_F.type = 5;
|
||
|
|
case 'universal_3dof'
|
||
|
|
stewart.joints_F.type = 6;
|
||
|
|
case 'spherical_3dof'
|
||
|
|
stewart.joints_F.type = 7;
|
||
|
|
end
|
||
|
|
|
||
|
|
switch args.type_M
|
||
|
|
case 'universal'
|
||
|
|
stewart.joints_M.type = 1;
|
||
|
|
case 'spherical'
|
||
|
|
stewart.joints_M.type = 2;
|
||
|
|
case 'universal_p'
|
||
|
|
stewart.joints_M.type = 3;
|
||
|
|
case 'spherical_p'
|
||
|
|
stewart.joints_M.type = 4;
|
||
|
|
case 'flexible'
|
||
|
|
stewart.joints_M.type = 5;
|
||
|
|
case 'universal_3dof'
|
||
|
|
stewart.joints_M.type = 6;
|
||
|
|
case 'spherical_3dof'
|
||
|
|
stewart.joints_M.type = 7;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Add Stiffness and Damping in Translation of each strut
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Axial and Radial (shear) Stiffness
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.joints_M.Ka = args.Ka_M;
|
||
|
|
stewart.joints_M.Kr = args.Kr_M;
|
||
|
|
|
||
|
|
stewart.joints_F.Ka = args.Ka_F;
|
||
|
|
stewart.joints_F.Kr = args.Kr_F;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Translation Damping
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.joints_M.Ca = args.Ca_M;
|
||
|
|
stewart.joints_M.Cr = args.Cr_M;
|
||
|
|
|
||
|
|
stewart.joints_F.Ca = args.Ca_F;
|
||
|
|
stewart.joints_F.Cr = args.Cr_F;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Add Stiffness and Damping in Rotation of each strut
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Rotational Stiffness
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.joints_M.Kf = args.Kf_M;
|
||
|
|
stewart.joints_M.Kt = args.Kt_M;
|
||
|
|
|
||
|
|
stewart.joints_F.Kf = args.Kf_F;
|
||
|
|
stewart.joints_F.Kt = args.Kt_F;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Rotational Damping
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.joints_M.Cf = args.Cf_M;
|
||
|
|
stewart.joints_M.Ct = args.Ct_M;
|
||
|
|
|
||
|
|
stewart.joints_F.Cf = args.Cf_F;
|
||
|
|
stewart.joints_F.Ct = args.Ct_F;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Stiffness and Mass matrices for flexible joint
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.joints_F.M = args.M_F;
|
||
|
|
stewart.joints_F.K = args.K_F;
|
||
|
|
stewart.joints_F.n_xyz = args.n_xyz_F;
|
||
|
|
stewart.joints_F.xi = args.xi_F;
|
||
|
|
stewart.joints_F.xi = args.xi_F;
|
||
|
|
stewart.joints_F.step_file = args.step_file_F;
|
||
|
|
|
||
|
|
stewart.joints_M.M = args.M_M;
|
||
|
|
stewart.joints_M.K = args.K_M;
|
||
|
|
stewart.joints_M.n_xyz = args.n_xyz_M;
|
||
|
|
stewart.joints_M.xi = args.xi_M;
|
||
|
|
stewart.joints_M.step_file = args.step_file_M;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =initializeInertialSensor=: Initialize the inertial sensor in each strut
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/initializeInertialSensor.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:initializeInertialSensor>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/initializeInertialSensor.m][here]].
|
||
|
|
|
||
|
|
**** Geophone - Working Principle
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
From the schematic of the Z-axis geophone shown in Figure [[fig:z_axis_geophone]], we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
|
||
|
|
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
||
|
|
with:
|
||
|
|
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
||
|
|
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
||
|
|
|
||
|
|
#+name: fig:z_axis_geophone
|
||
|
|
#+caption: Schematic of a Z-Axis geophone
|
||
|
|
[[file:figs/inertial_sensor.png]]
|
||
|
|
|
||
|
|
We see that at frequencies above $\omega_0$:
|
||
|
|
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
|
||
|
|
|
||
|
|
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
|
||
|
|
|
||
|
|
We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
|
||
|
|
|
||
|
|
**** Accelerometer - Working Principle
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
From the schematic of the Z-axis accelerometer shown in Figure [[fig:z_axis_accelerometer]], we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
|
||
|
|
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
||
|
|
with:
|
||
|
|
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
||
|
|
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
||
|
|
|
||
|
|
#+name: fig:z_axis_accelerometer
|
||
|
|
#+caption: Schematic of a Z-Axis geophone
|
||
|
|
[[file:figs/inertial_sensor.png]]
|
||
|
|
|
||
|
|
We see that at frequencies below $\omega_0$:
|
||
|
|
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
|
||
|
|
|
||
|
|
And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
|
||
|
|
|
||
|
|
Note that there is trade-off between:
|
||
|
|
- the highest measurable acceleration $\omega_0$
|
||
|
|
- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = initializeInertialSensor(stewart, args)
|
||
|
|
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = initializeInertialSensor(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - args - Structure with the following fields:
|
||
|
|
% - type - 'geophone', 'accelerometer', 'none'
|
||
|
|
% - mass [1x1] - Weight of the inertial mass [kg]
|
||
|
|
% - freq [1x1] - Cutoff frequency [Hz]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - stewart.sensors.inertial
|
||
|
|
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
||
|
|
% - K [1x1] - Stiffness [N/m]
|
||
|
|
% - C [1x1] - Damping [N/(m/s)]
|
||
|
|
% - M [1x1] - Inertial Mass [kg]
|
||
|
|
% - G [1x1] - Gain
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
||
|
|
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
||
|
|
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the properties of the sensor
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
sensor = struct();
|
||
|
|
|
||
|
|
switch args.type
|
||
|
|
case 'geophone'
|
||
|
|
sensor.type = 1;
|
||
|
|
|
||
|
|
sensor.M = args.mass;
|
||
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||
|
|
case 'accelerometer'
|
||
|
|
sensor.type = 2;
|
||
|
|
|
||
|
|
sensor.M = args.mass;
|
||
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||
|
|
sensor.G = -sensor.K/sensor.M;
|
||
|
|
case 'none'
|
||
|
|
sensor.type = 3;
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.sensors.inertial = sensor;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =displayArchitecture=: 3D plot of the Stewart platform architecture
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/displayArchitecture.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:displayArchitecture>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/displayArchitecture.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = displayArchitecture(stewart, args)
|
||
|
|
% displayArchitecture - 3D plot of the Stewart platform architecture
|
||
|
|
%
|
||
|
|
% Syntax: [] = displayArchitecture(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart
|
||
|
|
% - args - Structure with the following fields:
|
||
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||
|
|
% - F_color [color] - Color used for the Fixed elements
|
||
|
|
% - M_color [color] - Color used for the Mobile elements
|
||
|
|
% - L_color [color] - Color used for the Legs elements
|
||
|
|
% - frames [true/false] - Display the Frames
|
||
|
|
% - legs [true/false] - Display the Legs
|
||
|
|
% - joints [true/false] - Display the Joints
|
||
|
|
% - labels [true/false] - Display the Labels
|
||
|
|
% - platforms [true/false] - Display the Platforms
|
||
|
|
% - views ['all', 'xy', 'yz', 'xz', 'default'] -
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||
|
|
args.F_color = [0 0.4470 0.7410]
|
||
|
|
args.M_color = [0.8500 0.3250 0.0980]
|
||
|
|
args.L_color = [0 0 0]
|
||
|
|
args.frames logical {mustBeNumericOrLogical} = true
|
||
|
|
args.legs logical {mustBeNumericOrLogical} = true
|
||
|
|
args.joints logical {mustBeNumericOrLogical} = true
|
||
|
|
args.labels logical {mustBeNumericOrLogical} = true
|
||
|
|
args.platforms logical {mustBeNumericOrLogical} = true
|
||
|
|
args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Check the =stewart= structure elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||
|
|
FO_A = stewart.platform_F.FO_A;
|
||
|
|
|
||
|
|
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||
|
|
MO_B = stewart.platform_M.MO_B;
|
||
|
|
|
||
|
|
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
||
|
|
H = stewart.geometry.H;
|
||
|
|
|
||
|
|
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||
|
|
Fa = stewart.platform_F.Fa;
|
||
|
|
|
||
|
|
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||
|
|
Mb = stewart.platform_M.Mb;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** Figure Creation, Frames and Homogeneous transformations
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
The reference frame of the 3d plot corresponds to the frame $\{F\}$.
|
||
|
|
#+begin_src matlab
|
||
|
|
if ~strcmp(args.views, 'all')
|
||
|
|
figure;
|
||
|
|
else
|
||
|
|
f = figure('visible', 'off');
|
||
|
|
end
|
||
|
|
|
||
|
|
hold on;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
|
||
|
|
#+begin_src matlab
|
||
|
|
FTa = [eye(3), FO_A; ...
|
||
|
|
zeros(1,3), 1];
|
||
|
|
ATb = [args.ARB, args.AP; ...
|
||
|
|
zeros(1,3), 1];
|
||
|
|
BTm = [eye(3), -MO_B; ...
|
||
|
|
zeros(1,3), 1];
|
||
|
|
|
||
|
|
FTm = FTa*ATb*BTm;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Let's define a parameter that define the length of the unit vectors used to display the frames.
|
||
|
|
#+begin_src matlab
|
||
|
|
d_unit_vector = H/4;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Let's define a parameter used to position the labels with respect to the center of the element.
|
||
|
|
#+begin_src matlab
|
||
|
|
d_label = H/20;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Fixed Base elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Let's first plot the frame $\{F\}$.
|
||
|
|
#+begin_src matlab
|
||
|
|
Ff = [0, 0, 0];
|
||
|
|
if args.frames
|
||
|
|
quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
|
||
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
||
|
|
|
||
|
|
if args.labels
|
||
|
|
text(Ff(1) + d_label, ...
|
||
|
|
Ff(2) + d_label, ...
|
||
|
|
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Now plot the frame $\{A\}$ fixed to the Base.
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.frames
|
||
|
|
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
|
||
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
||
|
|
|
||
|
|
if args.labels
|
||
|
|
text(FO_A(1) + d_label, ...
|
||
|
|
FO_A(2) + d_label, ...
|
||
|
|
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Let's then plot the circle corresponding to the shape of the Fixed base.
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.platforms && stewart.platform_F.type == 1
|
||
|
|
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
||
|
|
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
|
||
|
|
center = [0; 0; 0]; % Center of the circle
|
||
|
|
radius = stewart.platform_F.R; % Radius of the circle [m]
|
||
|
|
|
||
|
|
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||
|
|
|
||
|
|
plot3(points(1,:), ...
|
||
|
|
points(2,:), ...
|
||
|
|
points(3,:), '-', 'Color', args.F_color);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Let's now plot the position and labels of the Fixed Joints
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.joints
|
||
|
|
scatter3(Fa(1,:), ...
|
||
|
|
Fa(2,:), ...
|
||
|
|
Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
||
|
|
if args.labels
|
||
|
|
for i = 1:size(Fa,2)
|
||
|
|
text(Fa(1,i) + d_label, ...
|
||
|
|
Fa(2,i), ...
|
||
|
|
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Mobile Platform elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
|
||
|
|
Plot the frame $\{M\}$.
|
||
|
|
#+begin_src matlab
|
||
|
|
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
|
||
|
|
|
||
|
|
if args.frames
|
||
|
|
FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||
|
|
quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
|
||
|
|
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
|
||
|
|
|
||
|
|
if args.labels
|
||
|
|
text(Fm(1) + d_label, ...
|
||
|
|
Fm(2) + d_label, ...
|
||
|
|
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Plot the frame $\{B\}$.
|
||
|
|
#+begin_src matlab
|
||
|
|
FB = FO_A + args.AP;
|
||
|
|
|
||
|
|
if args.frames
|
||
|
|
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||
|
|
quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
|
||
|
|
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
|
||
|
|
|
||
|
|
if args.labels
|
||
|
|
text(FB(1) - d_label, ...
|
||
|
|
FB(2) + d_label, ...
|
||
|
|
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Let's then plot the circle corresponding to the shape of the Mobile platform.
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.platforms && stewart.platform_M.type == 1
|
||
|
|
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
||
|
|
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
|
||
|
|
center = Fm(1:3); % Center of the circle
|
||
|
|
radius = stewart.platform_M.R; % Radius of the circle [m]
|
||
|
|
|
||
|
|
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||
|
|
|
||
|
|
plot3(points(1,:), ...
|
||
|
|
points(2,:), ...
|
||
|
|
points(3,:), '-', 'Color', args.M_color);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Plot the position and labels of the rotation joints fixed to the mobile platform.
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.joints
|
||
|
|
Fb = FTm*[Mb;ones(1,6)];
|
||
|
|
|
||
|
|
scatter3(Fb(1,:), ...
|
||
|
|
Fb(2,:), ...
|
||
|
|
Fb(3,:), 'MarkerEdgeColor', args.M_color);
|
||
|
|
|
||
|
|
if args.labels
|
||
|
|
for i = 1:size(Fb,2)
|
||
|
|
text(Fb(1,i) + d_label, ...
|
||
|
|
Fb(2,i), ...
|
||
|
|
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Legs
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
|
||
|
|
#+begin_src matlab
|
||
|
|
if args.legs
|
||
|
|
for i = 1:6
|
||
|
|
plot3([Fa(1,i), Fb(1,i)], ...
|
||
|
|
[Fa(2,i), Fb(2,i)], ...
|
||
|
|
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
|
||
|
|
|
||
|
|
if args.labels
|
||
|
|
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
|
||
|
|
(Fa(2,i)+Fb(2,i))/2, ...
|
||
|
|
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Figure parameters
|
||
|
|
#+begin_src matlab
|
||
|
|
switch args.views
|
||
|
|
case 'default'
|
||
|
|
view([1 -0.6 0.4]);
|
||
|
|
case 'xy'
|
||
|
|
view([0 0 1]);
|
||
|
|
case 'xz'
|
||
|
|
view([0 -1 0]);
|
||
|
|
case 'yz'
|
||
|
|
view([1 0 0]);
|
||
|
|
end
|
||
|
|
axis equal;
|
||
|
|
axis off;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Subplots
|
||
|
|
#+begin_src matlab
|
||
|
|
if strcmp(args.views, 'all')
|
||
|
|
hAx = findobj('type', 'axes');
|
||
|
|
|
||
|
|
figure;
|
||
|
|
s1 = subplot(2,2,1);
|
||
|
|
copyobj(get(hAx(1), 'Children'), s1);
|
||
|
|
view([0 0 1]);
|
||
|
|
axis equal;
|
||
|
|
axis off;
|
||
|
|
title('Top')
|
||
|
|
|
||
|
|
s2 = subplot(2,2,2);
|
||
|
|
copyobj(get(hAx(1), 'Children'), s2);
|
||
|
|
view([1 -0.6 0.4]);
|
||
|
|
axis equal;
|
||
|
|
axis off;
|
||
|
|
|
||
|
|
s3 = subplot(2,2,3);
|
||
|
|
copyobj(get(hAx(1), 'Children'), s3);
|
||
|
|
view([1 0 0]);
|
||
|
|
axis equal;
|
||
|
|
axis off;
|
||
|
|
title('Front')
|
||
|
|
|
||
|
|
s4 = subplot(2,2,4);
|
||
|
|
copyobj(get(hAx(1), 'Children'), s4);
|
||
|
|
view([0 -1 0]);
|
||
|
|
axis equal;
|
||
|
|
axis off;
|
||
|
|
title('Side')
|
||
|
|
|
||
|
|
close(f);
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/describeStewartPlatform.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:describeStewartPlatform>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/describeStewartPlatform.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [] = describeStewartPlatform(stewart)
|
||
|
|
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
|
||
|
|
%
|
||
|
|
% Syntax: [] = describeStewartPlatform(args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Geometry
|
||
|
|
#+begin_src matlab
|
||
|
|
fprintf('GEOMETRY:\n')
|
||
|
|
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
|
||
|
|
|
||
|
|
if stewart.platform_M.MO_B(3) > 0
|
||
|
|
fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3))
|
||
|
|
else
|
||
|
|
fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
|
||
|
|
end
|
||
|
|
|
||
|
|
fprintf('- The initial length of the struts are:\n')
|
||
|
|
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
|
||
|
|
fprintf('\n')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Actuators
|
||
|
|
#+begin_src matlab
|
||
|
|
fprintf('ACTUATORS:\n')
|
||
|
|
if stewart.actuators.type == 1
|
||
|
|
fprintf('- The actuators are classical.\n')
|
||
|
|
fprintf('- The Stiffness and Damping of each actuators is:\n')
|
||
|
|
fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
|
||
|
|
elseif stewart.actuators.type == 2
|
||
|
|
fprintf('- The actuators are mechanicaly amplified.\n')
|
||
|
|
fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
|
||
|
|
fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
|
||
|
|
fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
|
||
|
|
fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
|
||
|
|
end
|
||
|
|
fprintf('\n')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Joints
|
||
|
|
#+begin_src matlab
|
||
|
|
fprintf('JOINTS:\n')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Type of the joints on the fixed base.
|
||
|
|
#+begin_src matlab
|
||
|
|
switch stewart.joints_F.type
|
||
|
|
case 1
|
||
|
|
fprintf('- The joints on the fixed based are universal joints\n')
|
||
|
|
case 2
|
||
|
|
fprintf('- The joints on the fixed based are spherical joints\n')
|
||
|
|
case 3
|
||
|
|
fprintf('- The joints on the fixed based are perfect universal joints\n')
|
||
|
|
case 4
|
||
|
|
fprintf('- The joints on the fixed based are perfect spherical joints\n')
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Type of the joints on the mobile platform.
|
||
|
|
#+begin_src matlab
|
||
|
|
switch stewart.joints_M.type
|
||
|
|
case 1
|
||
|
|
fprintf('- The joints on the mobile based are universal joints\n')
|
||
|
|
case 2
|
||
|
|
fprintf('- The joints on the mobile based are spherical joints\n')
|
||
|
|
case 3
|
||
|
|
fprintf('- The joints on the mobile based are perfect universal joints\n')
|
||
|
|
case 4
|
||
|
|
fprintf('- The joints on the mobile based are perfect spherical joints\n')
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Position of the fixed joints
|
||
|
|
#+begin_src matlab
|
||
|
|
fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
|
||
|
|
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
Position of the mobile joints
|
||
|
|
#+begin_src matlab
|
||
|
|
fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
|
||
|
|
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
|
||
|
|
fprintf('\n')
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Kinematics
|
||
|
|
#+begin_src matlab
|
||
|
|
fprintf('KINEMATICS:\n')
|
||
|
|
|
||
|
|
if isfield(stewart.kinematics, 'K')
|
||
|
|
fprintf('- The Stiffness matrix K is (in [N/m]):\n')
|
||
|
|
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
|
||
|
|
end
|
||
|
|
|
||
|
|
if isfield(stewart.kinematics, 'C')
|
||
|
|
fprintf('- The Damping matrix C is (in [m/N]):\n')
|
||
|
|
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =generateCubicConfiguration=: Generate a Cubic Configuration
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/generateCubicConfiguration.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:generateCubicConfiguration>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/generateCubicConfiguration.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = generateCubicConfiguration(stewart, args)
|
||
|
|
% generateCubicConfiguration - Generate a Cubic Configuration
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart - A structure with the following fields
|
||
|
|
% - geometry.H [1x1] - Total height of the platform [m]
|
||
|
|
% - args - Can have the following fields:
|
||
|
|
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||
|
|
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||
|
|
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
||
|
|
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - updated Stewart structure with the added fields:
|
||
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Documentation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+name: fig:cubic-configuration-definition
|
||
|
|
#+caption: Cubic Configuration
|
||
|
|
[[file:figs/cubic-configuration-definition.png]]
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||
|
|
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||
|
|
args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||
|
|
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Check the =stewart= structure elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
||
|
|
H = stewart.geometry.H;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Position of the Cube
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
We define the useful points of the cube with respect to the Cube's center.
|
||
|
|
${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}.
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
sx = [ 2; -1; -1];
|
||
|
|
sy = [ 0; 1; -1];
|
||
|
|
sz = [ 1; 1; 1];
|
||
|
|
|
||
|
|
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
||
|
|
|
||
|
|
L = args.Hc*sqrt(3);
|
||
|
|
|
||
|
|
Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
|
||
|
|
|
||
|
|
CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
|
||
|
|
CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute the pose
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).
|
||
|
|
#+begin_src matlab
|
||
|
|
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
|
||
|
|
#+begin_src matlab
|
||
|
|
Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||
|
|
Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.platform_F.Fa = Fa;
|
||
|
|
stewart.platform_M.Mb = Mb;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =computeJacobian=: Compute the Jacobian Matrix
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/computeJacobian.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:computeJacobian>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/computeJacobian.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [stewart] = computeJacobian(stewart)
|
||
|
|
% computeJacobian -
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = computeJacobian(stewart)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart - With at least the following fields:
|
||
|
|
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||
|
|
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||
|
|
% - actuators.K [6x1] - Total stiffness of the actuators
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - stewart - With the 3 added field:
|
||
|
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
||
|
|
% - kinematics.K [6x6] - The Stiffness Matrix
|
||
|
|
% - kinematics.C [6x6] - The Compliance Matrix
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Check the =stewart= structure elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||
|
|
As = stewart.geometry.As;
|
||
|
|
|
||
|
|
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||
|
|
Ab = stewart.geometry.Ab;
|
||
|
|
|
||
|
|
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||
|
|
Ki = stewart.actuators.K;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** Compute Jacobian Matrix
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
J = [As' , cross(Ab, As)'];
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute Stiffness Matrix
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
K = J'*diag(Ki)*J;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Compute Compliance Matrix
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
C = inv(K);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Populate the =stewart= structure
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
stewart.kinematics.J = J;
|
||
|
|
stewart.kinematics.K = K;
|
||
|
|
stewart.kinematics.C = C;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
*** =inverseKinematics=: Compute Inverse Kinematics
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/inverseKinematics.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:inverseKinematics>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/inverseKinematics.m][here]].
|
||
|
|
|
||
|
|
**** Theory
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
||
|
|
|
||
|
|
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
||
|
|
\begin{align*}
|
||
|
|
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
||
|
|
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
||
|
|
\end{align*}
|
||
|
|
|
||
|
|
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
||
|
|
\begin{equation}
|
||
|
|
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
||
|
|
\end{equation}
|
||
|
|
|
||
|
|
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
||
|
|
\begin{equation}
|
||
|
|
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
||
|
|
\end{equation}
|
||
|
|
|
||
|
|
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
||
|
|
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [Li, dLi] = inverseKinematics(stewart, args)
|
||
|
|
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||
|
|
%
|
||
|
|
% Syntax: [stewart] = inverseKinematics(stewart)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart - A structure with the following fields
|
||
|
|
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||
|
|
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||
|
|
% - geometry.l [6x1] - Length of each strut
|
||
|
|
% - args - Can have the following fields:
|
||
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||
|
|
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Check the =stewart= structure elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||
|
|
Aa = stewart.geometry.Aa;
|
||
|
|
|
||
|
|
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||
|
|
Bb = stewart.geometry.Bb;
|
||
|
|
|
||
|
|
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||
|
|
l = stewart.geometry.l;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
|
||
|
|
**** Compute
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
dLi = Li-l;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
*** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ./matlab/src/forwardKinematicsApprox.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:forwardKinematicsApprox>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:./matlab/src/forwardKinematicsApprox.m][here]].
|
||
|
|
|
||
|
|
**** Function description
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||
|
|
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||
|
|
% the Jacobian Matrix
|
||
|
|
%
|
||
|
|
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - stewart - A structure with the following fields
|
||
|
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
||
|
|
% - args - Can have the following fields:
|
||
|
|
% - dL [6x1] - Displacement of each strut [m]
|
||
|
|
%
|
||
|
|
% Outputs:
|
||
|
|
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||
|
|
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Optional Parameters
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
arguments
|
||
|
|
stewart
|
||
|
|
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||
|
|
end
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Check the =stewart= structure elements
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
#+begin_src matlab
|
||
|
|
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
||
|
|
J = stewart.kinematics.J;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
**** Computation
|
||
|
|
:PROPERTIES:
|
||
|
|
:UNNUMBERED: t
|
||
|
|
:END:
|
||
|
|
From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
|
||
|
|
position and orientation of {B} with respect to {A} using the following formula:
|
||
|
|
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||
|
|
#+begin_src matlab
|
||
|
|
X = J\args.dL;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The position vector corresponds to the first 3 elements.
|
||
|
|
#+begin_src matlab
|
||
|
|
P = X(1:3);
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
The next 3 elements are the orientation of {B} with respect to {A} expressed
|
||
|
|
using the screw axis.
|
||
|
|
#+begin_src matlab
|
||
|
|
theta = norm(X(4:6));
|
||
|
|
s = X(4:6)/theta;
|
||
|
|
#+end_src
|
||
|
|
|
||
|
|
We then compute the corresponding rotation matrix.
|
||
|
|
#+begin_src matlab
|
||
|
|
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||
|
|
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||
|
|
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||
|
|
#+end_src
|