1161 lines
34 KiB
Org Mode
1161 lines
34 KiB
Org Mode
#+TITLE: DCM - Dynamical Multi-Body Model
|
|
: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]
|
|
#+LaTeX_HEADER_EXTRA: \input{preamble.tex}
|
|
#+LATEX_HEADER_EXTRA: \bibliography{ref}
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+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: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="./dcm-simscape.pdf">pdf</a>.</p>
|
|
<hr>
|
|
#+end_export
|
|
|
|
\clearpage
|
|
|
|
* System Kinematics
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_kinematics.m
|
|
:END:
|
|
** Introduction :ignore:
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_kinematics.m
|
|
% Computation of the DCM kinematics
|
|
#+end_src
|
|
|
|
#+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
|
|
|
|
** Bragg Angle
|
|
#+begin_src matlab
|
|
%% Tested bragg angles
|
|
bragg = linspace(5, 80, 1000); % Bragg angle [deg]
|
|
d_off = 10.5e-3; % Wanted offset between x-rays [m]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Vertical Jack motion as a function of Bragg angle
|
|
dz = d_off./(2*cos(bragg*pi/180));
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Jack motion as a function of Bragg angle
|
|
figure;
|
|
plot(bragg, 1e3*dz)
|
|
xlabel('Bragg angle [deg]'); ylabel('Jack Motion [mm]');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/jack_motion_bragg_angle.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:jack_motion_bragg_angle
|
|
#+caption: Jack motion as a function of Bragg angle
|
|
#+RESULTS:
|
|
[[file:figs/jack_motion_bragg_angle.png]]
|
|
|
|
#+begin_src matlab :results value replace :exports both
|
|
%% Required Jack stroke
|
|
ans = 1e3*(dz(end) - dz(1))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 24.963
|
|
|
|
** Kinematics (111 Crystal)
|
|
*** Introduction :ignore:
|
|
The reference frame is taken at the center of the 111 second crystal.
|
|
|
|
*** Interferometers - 111 Crystal
|
|
|
|
Three interferometers are pointed to the bottom surface of the 111 crystal.
|
|
|
|
The position of the measurement points are shown in Figure [[fig:sensor_111_crystal_points]] as well as the origin where the motion of the crystal is computed.
|
|
|
|
#+begin_src latex :file sensor_111_crystal_points.pdf
|
|
\begin{tikzpicture}
|
|
% Crystal
|
|
\draw (-15/2, -3.5/2) rectangle (15/2, 3.5/2);
|
|
|
|
% Measurement Points
|
|
\node[branch] (a1) at (-7, 1.5){};
|
|
\node[branch] (a2) at ( 0, -1.5){};
|
|
\node[branch] (a3) at ( 7, 1.5){};
|
|
|
|
% Labels
|
|
\node[right] at (a1) {$\mathcal{O}_1 = (-0.07, -0.015)$};
|
|
\node[right] at (a2) {$\mathcal{O}_2 = (0, 0.015)$};
|
|
\node[left] at (a3) {$\mathcal{O}_3 = ( 0.07, -0.015)$};
|
|
|
|
% Origin
|
|
\draw[->] (0, 0) node[] -- ++(1, 0) node[right]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, -1) node[below]{$y$};
|
|
\draw[fill, color=black] (0, 0) circle (0.05);
|
|
|
|
\node[left] at (0,0) {$\mathcal{O}_{111}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:sensor_111_crystal_points
|
|
#+caption: Bottom view of the second crystal 111. Position of the measurement points.
|
|
#+RESULTS:
|
|
[[file:figs/sensor_111_crystal_points.png]]
|
|
|
|
The inverse kinematics consisting of deriving the interferometer measurements from the motion of the crystal (see Figure [[fig:schematic_sensor_jacobian_inverse_kinematics]]):
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
x_1 \\ x_2 \\ x_3
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{s,111}
|
|
\begin{bmatrix}
|
|
d_z \\ r_y \\ r_x
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_inverse_kinematics.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js) {$\bm{J}_{s,111}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} -- (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics
|
|
#+caption: Inverse Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_inverse_kinematics.png]]
|
|
|
|
|
|
From the Figure [[fig:sensor_111_crystal_points]], the inverse kinematics can be solved as follow (for small motion):
|
|
\begin{equation}
|
|
\bm{J}_{s,111}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.07 & -0.015 \\
|
|
1 & 0 & 0.015 \\
|
|
1 & -0.07 & -0.015
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab
|
|
%% Sensor Jacobian matrix for 111 crystal
|
|
J_s_111 = [1, 0.07, -0.015
|
|
1, 0, 0.015
|
|
1, -0.07, -0.015];
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(J_s_111, {}, {}, ' %.3f ');
|
|
#+end_src
|
|
|
|
#+name: tab:jacobian_sensor_111
|
|
#+caption: Sensor Jacobian $\bm{J}_{s,111}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 1.0 | 0.07 | -0.015 |
|
|
| 1.0 | 0.0 | 0.015 |
|
|
| 1.0 | -0.07 | -0.015 |
|
|
|
|
The forward kinematics is solved by inverting the Jacobian matrix (see Figure [[fig:schematic_sensor_jacobian_forward_kinematics]]).
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
d_z \\ r_y \\ r_x
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{s,111}^{-1}
|
|
\begin{bmatrix}
|
|
x_1 \\ x_2 \\ x_3
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_sensor_jacobian_forward_kinematics.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Js_inv) {$\bm{J}_{s,111}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Js_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} -- (Js_inv.west);
|
|
\draw[->] (Js_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_forward_kinematics
|
|
#+caption: Forward Kinematics - Interferometers
|
|
#+RESULTS:
|
|
[[file:figs/schematic_sensor_jacobian_forward_kinematics.png]]
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_s_111), {}, {}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_sensor_111
|
|
#+caption: Inverse of the sensor Jacobian $\bm{J}_{s,111}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.25 | 0.5 | 0.25 |
|
|
| 7.14 | 0.0 | -7.14 |
|
|
| -16.67 | 33.33 | -16.67 |
|
|
|
|
*** Piezo - 111 Crystal
|
|
|
|
The location of the actuators with respect with the center of the 111 second crystal are shown in Figure [[fig:actuator_jacobian_111_points]].
|
|
|
|
#+name: fig:actuator_jacobian_111_points
|
|
#+caption: Location of actuators with respect to the center of the 111 second crystal (bottom view)
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/actuator_jacobian_111_points.png]]
|
|
|
|
Inverse Kinematics consist of deriving the axial (z) motion of the 3 actuators from the motion of the crystal's center.
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
d_{u_r} \\ d_{u_h} \\ d_{d}
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{a,111}
|
|
\begin{bmatrix}
|
|
d_z \\ r_y \\ r_x
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_actuator_jacobian_inverse_kinematics.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Ja) {$\bm{J}_{a,111}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} -- (Ja.west);
|
|
\draw[->] (Ja.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_d \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_sensor_jacobian_inverse_kinematics
|
|
#+caption: Inverse Kinematics - Actuators
|
|
#+RESULTS:
|
|
[[file:figs/schematic_actuator_jacobian_inverse_kinematics.png]]
|
|
|
|
Based on the geometry in Figure [[fig:actuator_jacobian_111_points]], we obtain:
|
|
\begin{equation}
|
|
\bm{J}_{a,111}
|
|
=
|
|
\begin{bmatrix}
|
|
1 & 0.14 & -0.1525 \\
|
|
1 & 0.14 & 0.0675 \\
|
|
1 & -0.14 & -0.0425
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab
|
|
%% Actuator Jacobian - 111 crystal
|
|
J_a_111 = [1, 0.14, -0.1525
|
|
1, 0.14, 0.0675
|
|
1, -0.14, -0.0425];
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(J_a_111, {}, {}, ' %.4f ');
|
|
#+end_src
|
|
|
|
#+name: tab:jacobian_actuator_111
|
|
#+caption: Actuator Jacobian $\bm{J}_{a,111}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 1.0 | 0.14 | -0.1525 |
|
|
| 1.0 | 0.14 | 0.0675 |
|
|
| 1.0 | -0.14 | -0.0425 |
|
|
|
|
The forward Kinematics is solved by inverting the Jacobian matrix:
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
d_z \\ r_y \\ r_x
|
|
\end{bmatrix}
|
|
=
|
|
\bm{J}_{a,111}^{-1}
|
|
\begin{bmatrix}
|
|
d_{u_r} \\ d_{u_h} \\ d_{d}
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
#+begin_src latex :file schematic_actuator_jacobian_forward_kinematics.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (Ja_inv) {$\bm{J}_{a,111}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja_inv.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_d \end{bmatrix}$} -- (Ja_inv.west);
|
|
\draw[->] (Ja_inv.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_actuator_jacobian_forward_kinematics
|
|
#+caption: Forward Kinematics - Actuators for 111 crystal
|
|
#+RESULTS:
|
|
[[file:figs/schematic_actuator_jacobian_forward_kinematics.png]]
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(inv(J_a_111), {}, {}, ' %.4f ');
|
|
#+end_src
|
|
|
|
#+name: tab:inverse_jacobian_actuator_111
|
|
#+caption: Inverse of the actuator Jacobian $\bm{J}_{a,111}^{-1}$
|
|
#+attr_latex: :environment tabularx :width 0.3\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 0.0568 | 0.4432 | 0.5 |
|
|
| 1.7857 | 1.7857 | -3.5714 |
|
|
| -4.5455 | 4.5455 | 0.0 |
|
|
|
|
** Save Kinematics
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/mat/dcm_kinematics.mat', 'J_a_111', 'J_s_111')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
save('mat/dcm_kinematics.mat', 'J_a_111', 'J_s_111')
|
|
#+end_src
|
|
|
|
* System Identification
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_identification.m
|
|
:END:
|
|
** Introduction :ignore:
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_identification.m
|
|
% Extraction of system dynamics using Simscape model
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Identification
|
|
Let's considered the system $\bm{G}(s)$ with:
|
|
- 3 inputs: force applied to the 3 fast jacks
|
|
- 3 outputs: measured displacement by the 3 interferometers pointing at the 111 second crystal
|
|
|
|
It is schematically shown in Figure [[fig:schematic_system_inputs_outputs]].
|
|
|
|
#+begin_src latex :file schematic_system_inputs_outputs.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$\bm{G}(s)$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(G.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} -- (G.west);
|
|
\draw[->] (G.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_system_inputs_outputs
|
|
#+caption: Dynamical system with inputs and outputs
|
|
#+RESULTS:
|
|
[[file:figs/schematic_system_inputs_outputs.png]]
|
|
|
|
The system is identified from the Simscape model.
|
|
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Interferometers {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Extraction of the dynamics
|
|
G = linearize(mdl, io);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Input and Output names
|
|
G.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G.OutputName = {'int_111_1', 'int_111_2', 'int_111_3'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results output replace :exports both :tangle no
|
|
size(G)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: size(G)
|
|
: State-space model with 3 outputs, 3 inputs, and 24 states.
|
|
|
|
** Plant in the frame of the fastjacks
|
|
#+begin_src matlab
|
|
load('mat/dcm_kinematics.mat');
|
|
#+end_src
|
|
|
|
Using the forward and inverse kinematics, we can computed the dynamics from piezo forces to axial motion of the 3 fastjacks (see Figure [[fig:schematic_jacobian_frame_fastjack]]).
|
|
|
|
#+begin_src latex :file schematic_jacobian_frame_fastjack.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$\bm{G}(s)$};
|
|
\node[block, right=1.5 of G] (Js) {$\bm{J}_{s}^{-1}$};
|
|
\node[block, right=1.5 of Js] (Ja) {$\bm{J}_{a}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(G.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} -- (G.west);
|
|
\draw[->] (G.east) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- node[midway, above]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$} (Ja.west);
|
|
\draw[->] (Ja.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_{u_r} \\ d_{u_h} \\ d_{d} \end{bmatrix}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(G.south west) ($(Ja.east)+(0, 1.4)$)}, fill=black!20!white, draw, inner sep=6pt] (system) {};
|
|
\node[above] at (system.north) {$\bm{G}_{\text{fj}}(s)$};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_frame_fastjack
|
|
#+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_fastjack.png]]
|
|
|
|
|
|
#+begin_src matlab
|
|
%% Compute the system in the frame of the fastjacks
|
|
G_pz = J_a_111*inv(J_s_111)*G;
|
|
#+end_src
|
|
|
|
The DC gain of the new system shows that the system is well decoupled at low frequency.
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_pz)
|
|
#+end_src
|
|
|
|
#+name: tab:dc_gain_plan_fj
|
|
#+caption: DC gain of the plant in the frame of the fast jacks $\bm{G}_{\text{fj}}$
|
|
#+attr_latex: :environment tabularx :width 0.5\linewidth :align ccc
|
|
#+attr_latex: :center t :booktabs t
|
|
#+RESULTS:
|
|
| 4.4407e-09 | 2.7656e-12 | 1.0132e-12 |
|
|
| 2.7656e-12 | 4.4407e-09 | 1.0132e-12 |
|
|
| 1.0109e-12 | 1.0109e-12 | 4.4424e-09 |
|
|
|
|
The bode plot of $\bm{G}_{\text{fj}}(s)$ is shown in Figure [[fig:bode_plot_plant_fj]].
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_pz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
|
ylim([1e-13, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(3,3), 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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/bode_plot_plant_fj.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:bode_plot_plant_fj
|
|
#+caption: Bode plot of the diagonal and off-diagonal elements of the plant in the frame of the fast jacks
|
|
#+RESULTS:
|
|
[[file:figs/bode_plot_plant_fj.png]]
|
|
|
|
#+begin_important
|
|
Computing the system in the frame of the fastjack gives good decoupling at low frequency (until the first resonance of the system).
|
|
#+end_important
|
|
|
|
** Plant in the frame of the crystal
|
|
#+begin_src latex :file schematic_jacobian_frame_crystal.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block] (G) {$\bm{G}(s)$};
|
|
\node[block, left=1.5 of G] (Ja) {$\bm{J}_{a}^{-T}$};
|
|
\node[block, right=1.5 of G] (Js) {$\bm{J}_{s}^{-1}$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1.5,0)$) node[above right]{$\begin{bmatrix} \mathcal{F}_{z} \\ \mathcal{M}_{y} \\ \mathcal{M}_{x} \end{bmatrix}$} -- (Ja.west);
|
|
\draw[->] (Ja.east) -- node[midway, above]{$\begin{bmatrix} u_{u_r} \\ u_{u_h} \\ u_d \end{bmatrix}$} (G.west);
|
|
\draw[->] (G.east) -- node[midway, above]{$\begin{bmatrix} x_1 \\ x_2 \\ x_3 \end{bmatrix}$} (Js.west);
|
|
\draw[->] (Js.east) -- ++(1.5, 0) node[above left]{$\begin{bmatrix} d_z \\ r_y \\ r_x \end{bmatrix}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(Ja.south west) ($(Js.east)+(0, 1.4)$)}, fill=black!20!white, draw, inner sep=6pt] (system) {};
|
|
\node[above] at (system.north) {$\bm{G}_{\text{cr}}(s)$};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:schematic_jacobian_frame_crystal
|
|
#+caption: Use of Jacobian matrices to obtain the system in the frame of the crystal
|
|
#+RESULTS:
|
|
[[file:figs/schematic_jacobian_frame_crystal.png]]
|
|
|
|
#+begin_src matlab
|
|
G_mr = inv(J_s_111)*G*inv(J_a_111');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_mr)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| 1.9978e-09 | 3.9657e-09 | 7.7944e-09 |
|
|
| 3.9656e-09 | 8.4979e-08 | -1.5135e-17 |
|
|
| 7.7944e-09 | -3.9252e-17 | 1.834e-07 |
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(3,3), 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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
fig = figure;
|
|
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
for i_out = 1:3
|
|
for i_in = 1:3
|
|
ax = nexttile;
|
|
plot(freqs, abs(squeeze(freqresp(G_mr(i_out, i_in), freqs, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
end
|
|
end
|
|
|
|
linkaxes(findall(fig, 'type', 'axes'),'xy');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
This results in a coupled system.
|
|
The main reason is that, as we map forces to the center of the 111 crystal and not at the center of mass/stiffness of the moving part, vertical forces will induce rotation and torques will induce vertical motion.
|
|
|
|
** Plant at the center of stiffness :noexport:
|
|
|
|
Here, we map the piezo forces at the center of stiffness.
|
|
|
|
Let's first compute the Jacobian:
|
|
|
|
* Active Damping Plant (Strain gauge)
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_active_damping_strain_gauges.m
|
|
:END:
|
|
** Introduction :ignore:
|
|
In this section, we wish to see whether if strain gauges fixed to the piezoelectric actuator can be used for active damping.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_active_damping_strain_gauges.m
|
|
% Active Damping using relative motion sensors (strain gauges)
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Identification
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1;
|
|
% % Stepper Displacement {3x1} [m]
|
|
% io(io_i) = linio([mdl, '/d'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Strain Gauges {3x1} [m]
|
|
io(io_i) = linio([mdl, '/sg'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Extraction of the dynamics
|
|
G_sg = linearize(mdl, io);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
G_sg.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_sg.OutputName = {'sg_ur', 'sg_uh', 'sg_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_sg)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| -1.4113e-13 | 1.0339e-13 | 3.774e-14 |
|
|
| 1.0339e-13 | -1.4113e-13 | 3.774e-14 |
|
|
| 3.7792e-14 | 3.7792e-14 | -7.5585e-14 |
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_sg(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(3,3), 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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
* Active Damping Plant (Force Sensors)
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_active_damping_iff.m
|
|
:END:
|
|
** Introduction :ignore:
|
|
Force sensors are added above the piezoelectric actuators.
|
|
They can consists of a simple piezoelectric ceramic stack.
|
|
See for instance cite:fleming10_integ_strain_force_feedb_high.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_active_damping_iff.m
|
|
% Test of Integral Force Feedback Strategy
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Identification
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 3, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Extraction of the dynamics
|
|
G_fs = linearize(mdl, io);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
G_fs.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_fs.OutputName = {'fs_ur', 'fs_uh', 'fs_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
dcgain(G_fs)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| -1.4113e-13 | 1.0339e-13 | 3.774e-14 |
|
|
| 1.0339e-13 | -1.4113e-13 | 3.774e-14 |
|
|
| 3.7792e-14 | 3.7792e-14 | -7.5585e-14 |
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Bode plot for the plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur');
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', 'off-diag');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_fs(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-13, 1e-7]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(3,3), 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([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/iff_plant_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:iff_plant_bode_plot
|
|
#+caption: Bode plot of IFF Plant
|
|
#+RESULTS:
|
|
[[file:figs/iff_plant_bode_plot.png]]
|
|
|
|
** Controller - Root Locus
|
|
#+begin_src matlab
|
|
Kiff_g1 = eye(3)*1/(1 + s/2/pi/20);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Root Locus for IFF
|
|
gains = logspace(9, 12, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
plot(real(pole(G_fs)), imag(pole(G_fs)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_fs)), imag(tzero(G_fs)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
g = 8e10;
|
|
clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
|
|
plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
|
|
'DisplayName', sprintf('$g=%.0e$', g));
|
|
hold off;
|
|
axis square;
|
|
xlim([-2700, 0]); ylim([0, 2700]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:iff_root_locus
|
|
#+caption: Root Locus plot for the IFF Control strategy
|
|
#+RESULTS:
|
|
[[file:figs/iff_root_locus.png]]
|
|
|
|
#+begin_src matlab
|
|
%% Integral Force Feedback Controller
|
|
Kiff = g*Kiff_g1;
|
|
#+end_src
|
|
|
|
** Damped Plant
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
|
|
%% Inputs
|
|
% Control Input {3x1} [N]
|
|
io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1;
|
|
|
|
%% Outputs
|
|
% Force Sensor {3x1} [m]
|
|
io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% DCM Kinematics
|
|
load('mat/dcm_kinematics.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identification of the Open Loop plant
|
|
controller.type = 0; % Open Loop
|
|
G_ol = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
|
G_ol.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_ol.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identification of the damped plant with IFF
|
|
controller.type = 1; % IFF
|
|
G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io);
|
|
G_dp.InputName = {'u_ur', 'u_uh', 'u_d'};
|
|
G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Comparison of the damped and undamped plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ...
|
|
'DisplayName', 'd - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ...
|
|
'DisplayName', 'uh - OL');
|
|
plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ...
|
|
'DisplayName', 'ur - OL');
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'd - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'uh - IFF');
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ...
|
|
'DisplayName', 'ur - IFF');
|
|
for i = 1:2
|
|
for j = i+1:3
|
|
plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
ylim([1e-12, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))));
|
|
set(gca,'ColorOrderIndex',1)
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--');
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), 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, 0]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/comp_damped_undamped_plant_iff_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:comp_damped_undamped_plant_iff_bode_plot
|
|
#+caption: Bode plot of both the open-loop plant and the damped plant using IFF
|
|
#+RESULTS:
|
|
[[file:figs/comp_damped_undamped_plant_iff_bode_plot.png]]
|
|
|
|
#+begin_important
|
|
The Integral Force Feedback control strategy is very effective in damping the suspension modes of the DCM.
|
|
#+end_important
|
|
|
|
** Save
|
|
#+begin_src matlab :exports none :tangle no
|
|
save('matlab/mat/Kiff.mat', 'Kiff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
save('mat/Kiff.mat', 'Kiff');
|
|
#+end_src
|
|
|
|
* HAC-LAC (IFF) architecture
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/dcm_hac_iff.m
|
|
:END:
|
|
** Introduction :ignore:
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% dcm_hac_iff.m
|
|
% Development of the HAC-IFF control strategy
|
|
#+end_src
|
|
|
|
#+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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
* Helping Functions :noexport:
|
|
** Initialize Path
|
|
#+NAME: m-init-path
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./matlab/mat/'); % Path for data
|
|
addpath('./matlab/'); % Path for scripts
|
|
|
|
%% Simscape Model - Nano Hexapod
|
|
addpath('./matlab/STEPS/')
|
|
#+END_SRC
|
|
|
|
#+NAME: m-init-path-tangle
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for data
|
|
|
|
%% Simscape Model - Nano Hexapod
|
|
addpath('./STEPS/')
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
%% Initialize Parameters for Simscape model
|
|
controller.type = 0; % Open Loop Control
|
|
|
|
%% Options for Linearization
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Open Simulink Model
|
|
mdl = 'simscape_dcm';
|
|
|
|
open(mdl)
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(1, 3, 1000);
|
|
#+END_SRC
|
|
|
|
* Bibliography :ignore:
|
|
#+latex: \printbibliography
|