5416 lines
201 KiB
Org Mode
5416 lines
201 KiB
Org Mode
#+TITLE: Simscape Model - Nano Active Stabilization System
|
|
: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: \input{preamble.tex}
|
|
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
|
|
#+LATEX_HEADER_EXTRA: \bibliography{simscape-nass.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:
|
|
|
|
#+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:
|
|
** Notes
|
|
Prefix is =nass=
|
|
|
|
The goals of this report are:
|
|
- [X] ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]): Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors)
|
|
- [X] ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]): Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...)
|
|
- [ ] Determination of the *optimal stiffness* for the hexapod actuators:
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_optimal_stiffness.org][uncertainty_optimal_stiffness]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/optimal_stiffness_disturbances.org][optimal_stiffness_disturbances]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][state-of-thesis-2020]]
|
|
- [ ] [[file:/home/thomas/Cloud/meetings/group-meetings-me/2020-04-06-NASS-Design/2020-04-06-NASS-Design.org][group-meeting-optimal-stiffness]]
|
|
Should this be in this report? *This should be in chapter 2*
|
|
- [X] Explain why HAC-LAC strategy is nice (*It was already explained in uniaxial model*)
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control.org][different control architectures]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org][hexapod - vibration isolation]]
|
|
- [X] How to apply/optimize IFF on an hexapod? ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]], [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]])
|
|
- [X] ([[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org][decoupling-strategies]]): Decoupling strategies for HAC? (maybe also in previous report)
|
|
*Will be in chapter 2*
|
|
- [X] Validation of the concept using simulations:
|
|
- [X] Find where this simulation in OL/CL is made (maybe for the conference?)
|
|
It was re-made for micro-station validation. Will just have to do the same simulation but with nano-hexapod in closed-loop
|
|
- Tomography experiment (maybe also Ty scans)
|
|
- Open VS Closed loop results
|
|
- *Conclusion*: concept validation
|
|
nano hexapod architecture with APA
|
|
decentralized IFF + centralized HAC
|
|
|
|
- In this section simple control (in the frame of the struts)
|
|
- Justify future used control architecture (control in the frame of the struts? Need to check what was done in ID31 tests)
|
|
- Table that compares different approaches (specify performances in different DoF, same plans on the diagonal, etc...)
|
|
- Literature review about Stewart platform control?
|
|
*In chapter 2: Special section about MIMO control, complementary filters, etc...*
|
|
|
|
** Outline
|
|
*** Control Kinematics
|
|
|
|
- Explain how the position error can be expressed in the frame of the nano-hexapod
|
|
- ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]): Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors)
|
|
- Control architecture, block diagram
|
|
|
|
*** LAC
|
|
|
|
- How to apply/optimize IFF on an hexapod? ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]], [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]])
|
|
- Robustness to payload mass
|
|
- Root Locus
|
|
- Damping optimization
|
|
|
|
*** HAC
|
|
|
|
- ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]): Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...)
|
|
- Determination of the *optimal stiffness* for the hexapod actuators:
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_optimal_stiffness.org][uncertainty_optimal_stiffness]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/optimal_stiffness_disturbances.org][optimal_stiffness_disturbances]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][state-of-thesis-2020]]
|
|
- [ ] [[file:/home/thomas/Cloud/meetings/group-meetings-me/2020-04-06-NASS-Design/2020-04-06-NASS-Design.org][group-meeting-optimal-stiffness]]
|
|
- Effect of micro-station compliance
|
|
- Effect of IFF
|
|
- Effect of payload mass
|
|
- Decoupled plant
|
|
- Controller design
|
|
|
|
*** Simulations
|
|
|
|
- Take into account disturbances, metrology sensor noise. Maybe say here that we don't take in account other noise sources as they will be optimized latter (detail design phase)
|
|
- Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]])
|
|
- Validation of concept
|
|
|
|
** DONE Old Outline
|
|
CLOSED: [2024-11-07 Thu 16:19]
|
|
*** Introduction :ignore:
|
|
|
|
Discussion of:
|
|
- Transformation matrices / control architecture (computation of the position error in the frame of the nano-hexapod)
|
|
- Control of parallel architectures
|
|
- Control in the frame of struts or cartesian?
|
|
- Effect of rotation on IFF? => APA
|
|
- HAC-LAC
|
|
- New noise budgeting?
|
|
|
|
*** Control Kinematics
|
|
|
|
- Explain how the position error can be expressed in the frame of the nano-hexapod
|
|
- block diagram
|
|
- Explain how to go from external metrology to the frame of the nano-hexapod
|
|
|
|
*** High Authority Control - Low Authority Control (HAC-LAC)
|
|
|
|
- general idea
|
|
- case for parallel manipulator: decentralized LAC + centralized HAC
|
|
|
|
*** Decoupling Strategies for parallel manipulators
|
|
[[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org::+TITLE: Diagonal control using the SVD and the Jacobian Matrix][study]]
|
|
|
|
- Jacobian matrices, CoK, CoM, ...
|
|
- Discussion of cubic architecture
|
|
- SVD, Modal, ...
|
|
|
|
*** Decentralized Integral Force Feedback (LAC)
|
|
|
|
- Root Locus
|
|
- Damping optimization
|
|
|
|
*** Decoupled Dynamics
|
|
|
|
- Centralized HAC
|
|
- Control in the frame of the struts
|
|
- Effect of IFF
|
|
|
|
*** Centralized Position Controller (HAC)
|
|
|
|
- Decoupled plant
|
|
- Controller design
|
|
|
|
*** Time domain simulations
|
|
|
|
Goal: validation of the concept
|
|
- Take into account disturbances, sensor noise, etc...
|
|
- Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]])
|
|
|
|
** DONE [#A] Merge the micro-station model with the nano-hexapod model
|
|
CLOSED: [2025-02-12 Wed 12:10] SCHEDULED: <2025-02-12 Wed>
|
|
|
|
- [X] *Start from the Simscape model of the ID31 tests*
|
|
=/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/nass_model_id31.slx=
|
|
- [X] Remove LION metrology to have perfect measurement
|
|
- [X] Remove nano-hexapod model and add simplified model
|
|
- [ ] Add "cylindrical" payloads (configurable in mass)
|
|
|
|
** DONE [#B] Add payload configurable subsystem
|
|
CLOSED: [2025-02-12 Wed 14:17] SCHEDULED: <2025-02-12 Wed>
|
|
|
|
** DONE [#A] Verify formulas to have the errors in the frame of the nano-hexapod and in the frame of the granite
|
|
CLOSED: [2025-02-17 Mon 10:35] SCHEDULED: <2025-02-17 Mon>
|
|
|
|
Errors in the frame of the nano-hexapod:
|
|
\begin{equation}\label{eq:nass_transformation_error}
|
|
\bm{T}_{\text{error}} = \bm{T}_{\mu\text{-station}}^{-1} \cdot \bm{T}_{\text{sample}}
|
|
\end{equation}
|
|
|
|
Errors in the frame of the granite:
|
|
WTe(1:3, 4, i) = WTr(1:3, 4, i) - WTm(1:3, 4, i);
|
|
WTe(1:3, 1:3, i) = WTr(1:3, 1:3, end)*WTm(1:3, 1:3, end)';
|
|
|
|
** DONE [#A] Fix IFF and HAC controllers
|
|
CLOSED: [2025-02-17 Mon 16:00] SCHEDULED: <2025-02-17 Mon>
|
|
|
|
** DONE [#A] Compute all figures
|
|
CLOSED: [2025-02-17 Mon 18:26] SCHEDULED: <2025-02-17 Mon>
|
|
|
|
** DONE [#B] Discuss the necessity of estimated Rz?
|
|
CLOSED: [2025-02-17 Mon 18:26]
|
|
|
|
One big advantage of doing the control in the cartesian plane, is that we don't need the estimation of nano-hexapod Rz, therefore we don't need the encoders anymore!
|
|
|
|
Maybe this should be done *here*.
|
|
Here it can be reminded when doing the control in the cartesian frame.
|
|
|
|
** TODO [#B] Determine which .mat files are used and which are not
|
|
|
|
- [ ] matlab/mat/conf_log.mat
|
|
- [ ] matlab/mat/conf_simscape.mat
|
|
- [ ] matlab/mat/conf_simulink.mat
|
|
- [ ] matlab/mat/nano_hexapod.mat
|
|
- [ ] matlab/mat/nass_disturbances.mat
|
|
- [ ] matlab/mat/nass_model_conf_log.mat
|
|
- [ ] matlab/mat/nass_model_conf_simscape.mat
|
|
- [ ] matlab/mat/nass_model_controller.mat
|
|
- [ ] matlab/mat/nass_model_disturbances.mat
|
|
- [ ] matlab/mat/nass_model_references.mat
|
|
- [ ] matlab/mat/nass_model_stages.mat
|
|
- [ ] matlab/mat/nass_references.mat
|
|
- [ ] matlab/mat/nass_stages.mat
|
|
|
|
** TODO [#B] Check if things are compatible to results of uniaxial model
|
|
|
|
** DONE [#C] Check if it would be interesting to show soft/stiff nano-hexapod plants
|
|
CLOSED: [2025-02-17 Mon 18:26]
|
|
|
|
- [ ] Would we see u-station dynamics with very stiff nano-hexapod?
|
|
- [ ] Would rotation be difficult to handle with soft nano-hexapod?
|
|
|
|
** DONE [#C] Why not plant with very stiff actuators?
|
|
CLOSED: [2025-02-17 Mon 18:26]
|
|
|
|
- [ ] Check if it is confirms that having very stiff actuators is bad
|
|
Not much better decoupling: 10Hz of bandwidth achievable, but may have worst sensitivity to disturbances
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF plant dynamics using the Simscape model
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod();
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N]
|
|
|
|
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e2);
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
G_m1_iff_pz = linearize(mdl, io);
|
|
G_m1_iff_pz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1_iff_pz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% IFF Plant - Without parallel stiffness
|
|
f = logspace(0,4,1000);
|
|
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(squeeze(freqresp(G_m1_iff_pz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(squeeze(freqresp(G_m1_iff_pz(1,1), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$')
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(G_m1_iff_pz(i,i), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(squeeze(freqresp(G_m1_iff_pz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e1]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_iff_pz(i,i), f, 'Hz')))), 'color', colors(1,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-20, 200]);
|
|
yticks([0:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([f(1), f(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF plant dynamics using the Simscape model
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e2);
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m]
|
|
|
|
%% Identify HAC Plant without using IFF
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
G_m1_pz = linearize(mdl, io);
|
|
G_m1_pz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
G_m25_pz = linearize(mdl, io);
|
|
G_m25_pz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
initializeSample('type', 'cylindrical', 'm', 50);
|
|
G_m50_pz = linearize(mdl, io);
|
|
G_m50_pz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m50_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m1_pz(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 1kg')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 25kg')
|
|
plot(freqs, abs(squeeze(freqresp(G_m50_pz(1,1), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 50kg')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1_pz(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m50_pz(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
% ylim([1e-5, 1e1]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_pz(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50_pz(i,i), freqs, 'Hz')))), 'color', colors(3,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
Compare with Hexapod alone:
|
|
#+begin_src matlab
|
|
%% Identify the IFF plant dynamics using the Simscape model
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeGround('type', 'rigid');
|
|
initializeGranite('type', 'rigid');
|
|
initializeTy('type', 'rigid');
|
|
initializeRy('type', 'rigid');
|
|
initializeRz('type', 'rigid');
|
|
initializeMicroHexapod('type', 'rigid');
|
|
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e2);
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m]
|
|
|
|
%% Identify HAC Plant without using IFF
|
|
G_m25_pz_rigid = linearize(mdl, io);
|
|
G_m25_pz_rigid.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 25kg')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 25kg')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
% ylim([1e-5, 1e1]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
** DONE [#A] Add possibility to configure the nano-hexapod to be fully rigid
|
|
CLOSED: [2025-02-12 Wed 14:46]
|
|
|
|
- Use to compare TF without the NASS
|
|
|
|
** CANC [#C] What performance metric can we use? :@christophe:
|
|
CLOSED: [2024-11-12 Tue 09:22]
|
|
- State "CANC" from "QUES" [2024-11-12 Tue 09:22]
|
|
This can be nice to have a (scalar) performance metric that can be used for optimization.
|
|
|
|
In cite:hauge04_sensor_contr_space_based_six, a (scalar) performance metric representing the 6dof transmissibility is used.
|
|
|
|
** DONE [#C] Identify the sensibility to disturbances without the nano-hexapod and save the results
|
|
CLOSED: [2024-11-07 Thu 09:20]
|
|
This can then be used to compare with obtained performance with the nano-hexapod.
|
|
|
|
This should be done in the ustation report (A4).
|
|
|
|
* Introduction :ignore:
|
|
|
|
From last sections:
|
|
- Uniaxial: No stiff nano-hexapod (should also demonstrate that here)
|
|
- Rotating: No soft nano-hexapod, Decentralized IFF can be used robustly by adding parallel stiffness
|
|
- Micro-Station multi body model tuned from a modal analysis
|
|
- Multi-body model of a nano-hexapod that can be merged with the multi-body model of the micro-station
|
|
|
|
In this section:
|
|
- Take the model of the nano-hexapod described in previous section (stiffness 1um/N)
|
|
- Control kinematics: how the external metrology, the nano-hexapod metrology are used to control the sample's position (Section ref:sec:nass_kinematics)
|
|
- Apply decentralized IFF (Section ref:sec:nass_active_damping)
|
|
- Apply HAC-LAC (Section ref:sec:nass_hac)
|
|
- Check robustness to change of payload and to spindle rotation
|
|
- Simulation of experiments
|
|
- Conclusion of the conceptual phase, validation with simulations
|
|
|
|
#+name: fig:nass_simscape_model
|
|
#+caption: 3D view of the NASS multi-body model
|
|
#+attr_latex: :width 0.8\linewidth
|
|
[[file:figs/nass_simscape_model.jpg]]
|
|
|
|
* Control Kinematics
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/nass_1_kinematics.m
|
|
:END:
|
|
<<sec:nass_kinematics>>
|
|
** Introduction :ignore:
|
|
|
|
Figure ref:fig:nass_concept_schematic presents a schematic overview of the NASS.
|
|
This section focuses specifically on the components of the "Instrumentation and Real-Time Control" block.
|
|
|
|
#+name: fig:nass_concept_schematic
|
|
#+caption: Schematic of the Nano Active Stabilization System
|
|
[[file:figs/nass_concept_schematic.png]]
|
|
|
|
As established in the previous section on Stewart platforms, the proposed control strategy combines Decentralized Integral Force Feedback with a High Authority Controller performed in the frame of the struts.
|
|
|
|
For the Nano Active Stabilization System, computing the positioning errors in the frame of the struts involves three key steps.
|
|
First, the system computes the desired sample pose relative to a frame representing the point where the X-ray light is focused using micro-station kinematics, as detailed in Section ref:ssec:nass_ustation_kinematics.
|
|
Second, it measures the actual sample pose relative to the same fix frame, described in Section ref:ssec:nass_sample_pose_error.
|
|
Finally, it determines the sample pose error and maps these errors to the nano-hexapod struts, as explained in Section ref:ssec:nass_error_struts.
|
|
|
|
The complete control architecture is detailed in Section ref:ssec:nass_control_architecture.
|
|
|
|
** 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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Micro Station Kinematics
|
|
<<ssec:nass_ustation_kinematics>>
|
|
|
|
The micro-station kinematics enables the computation of the desired sample pose from the reference signals of each micro-station stage.
|
|
These reference signals consist of the desired lateral position $r_{D_y}$, tilt angle $r_{R_y}$, and spindle angle $r_{R_z}$.
|
|
The micro-hexapod pose is defined by six parameters: three translations ($r_{D_{\mu x}}$, $r_{D_{\mu y}}$, $r_{D_{\mu z}}$) and three rotations ($r_{\theta_{\mu x}}$, $r_{\theta_{\mu y}}$, $r_{\theta_{\mu z}}$).
|
|
|
|
Using these reference signals, the desired sample position relative to the fixed frame is expressed through the homogeneous transformation matrix $\bm{T}_{\mu\text{-station}}$, as defined in equation eqref:eq:nass_sample_ref.
|
|
|
|
\begin{equation}\label{eq:nass_sample_ref}
|
|
\bm{T}_{\mu\text{-station}} = \bm{T}_{D_y} \cdot \bm{T}_{R_y} \cdot \bm{T}_{R_z} \cdot \bm{T}_{\mu\text{-hexapod}}
|
|
\end{equation}
|
|
|
|
\begin{equation}\label{eq:nass_ustation_matrices}
|
|
\begin{align}
|
|
\bm{T}_{D_y} &= \begin{bmatrix}
|
|
1 & 0 & 0 & 0 \\
|
|
0 & 1 & 0 & r_{D_y} \\
|
|
0 & 0 & 1 & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix} \quad
|
|
\bm{T}_{\mu\text{-hexapod}} =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & r_{D_{\mu x}} \\
|
|
& \bm{R}_x(r_{\theta_{\mu x}}) \bm{R}_y(r_{\theta_{\mu y}}) \bm{R}_{z}(r_{\theta_{\mu z}}) & & r_{D_{\mu y}} \\
|
|
& & & r_{D_{\mu z}} \cr
|
|
\hline
|
|
0 & 0 & 0 & 1
|
|
\end{array} \right] \\
|
|
\bm{T}_{R_z} &= \begin{bmatrix}
|
|
\cos(r_{R_z}) & -\sin(r_{R_z}) & 0 & 0 \\
|
|
\sin(r_{R_z}) & \cos(r_{R_z}) & 0 & 0 \\
|
|
0 & 0 & 1 & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix} \quad
|
|
\bm{T}_{R_y} = \begin{bmatrix}
|
|
\cos(r_{R_y}) & 0 & \sin(r_{R_y}) & 0 \\
|
|
0 & 1 & 0 & 0 \\
|
|
-\sin(r_{R_y}) & 0 & \cos(r_{R_y}) & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix}
|
|
\end{align}
|
|
\end{equation}
|
|
|
|
|
|
** Computation of the sample's pose error
|
|
<<ssec:nass_sample_pose_error>>
|
|
|
|
The external metrology system measures the sample position relative to the fixed granite.
|
|
Due to the system's symmetry, this metrology provides measurements for five degrees of freedom: three translations ($D_x$, $D_y$, $D_z$) and two rotations ($R_x$, $R_y$).
|
|
|
|
The sixth degree of freedom ($R_z$) is still required to compute the errors in the frame of the nano-hexapod struts (i.e. to compute the nano-hexapod inverse kinematics).
|
|
This $R_z$ rotation is estimated by combining measurements from the spindle encoder and the nano-hexapod's internal metrology, which consists of relative motion sensors in each strut (note that the micro-hexapod is not used for $R_z$ rotation, and is therefore ignore for $R_z$ estimation).
|
|
|
|
The measured sample pose is represented by the homogeneous transformation matrix $\bm{T}_{\text{sample}}$, as shown in equation eqref:eq:nass_sample_pose.
|
|
|
|
\begin{equation}\label{eq:nass_sample_pose}
|
|
\bm{T}_{\text{sample}} =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & D_{x} \\
|
|
& \bm{R}_x(R_{x}) \bm{R}_y(R_{y}) \bm{R}_{z}(R_{z}) & & D_{y} \\
|
|
& & & D_{z} \cr
|
|
\hline
|
|
0 & 0 & 0 & 1
|
|
\end{array} \right]
|
|
\end{equation}
|
|
|
|
** Position error in the frame of the struts
|
|
<<ssec:nass_error_struts>>
|
|
|
|
|
|
The homogeneous transformation formalism enables straightforward computation of the sample position error.
|
|
This computation involves the previously computed homogeneous $4 \times 4$ matrices: $\bm{T}_{\mu\text{-station}}$ representing the desired pose, and $\bm{T}_{\text{sample}}$ representing the measured pose.
|
|
Their combination yields $\bm{T}_{\text{error}}$, which expresses the position error of the sample in the frame of the rotating nano-hexapod, as shown in equation eqref:eq:nass_transformation_error.
|
|
|
|
\begin{equation}\label{eq:nass_transformation_error}
|
|
\bm{T}_{\text{error}} = \bm{T}_{\mu\text{-station}}^{-1} \cdot \bm{T}_{\text{sample}}
|
|
\end{equation}
|
|
|
|
The known structure of the homogeneous transformation matrix facilitates efficient real-time computation of the inverse.
|
|
From $\bm{T}_{\text{error}}$, the position and orientation errors $\bm{\epsilon}_{\mathcal{X}} = [\epsilon_{D_x},\ \epsilon_{D_y},\ \epsilon_{D_z},\ \epsilon_{R_x},\ \epsilon_{R_y},\ \epsilon_{R_z}]$ of the sample are extracted using equation eqref:eq:nass_compute_errors:
|
|
|
|
\begin{equation}\label{eq:nass_compute_errors}
|
|
\begin{align}
|
|
\epsilon_{D_x} & = \bm{T}_{\text{error}}(1,4) \\
|
|
\epsilon_{D_y} & = \bm{T}_{\text{error}}(2,4) \\
|
|
\epsilon_{D_z} & = \bm{T}_{\text{error}}(3,4) \\
|
|
\epsilon_{R_y} & = \text{atan2}(\bm{T}_{\text{error}}(1,3), \sqrt{\bm{T}_{\text{error}}(1,1)^2 + \bm{T}_{\text{error}}(1,2)^2}) \\
|
|
\epsilon_{R_x} & = \text{atan2}(-\bm{T}_{\text{error}}(2,3)/\cos(\epsilon_{R_y}), \bm{T}_{\text{error}}(3,3)/\cos(\epsilon_{R_y})) \\
|
|
\epsilon_{R_z} & = \text{atan2}(-\bm{T}_{\text{error}}(1,2)/\cos(\epsilon_{R_y}), \bm{T}_{\text{error}}(1,1)/\cos(\epsilon_{R_y})) \\
|
|
\end{align}
|
|
\end{equation}
|
|
|
|
Finally, these errors are mapped to the strut space through the nano-hexapod Jacobian matrix eqref:eq:nass_inverse_kinematics.
|
|
|
|
\begin{equation}\label{eq:nass_inverse_kinematics}
|
|
\bm{\epsilon}_{\mathcal{L}} = \bm{J} \cdot \bm{\epsilon}_{\mathcal{X}}
|
|
\end{equation}
|
|
|
|
** Control Architecture - Summary
|
|
<<ssec:nass_control_architecture>>
|
|
|
|
The complete control architecture is summarized in Figure ref:fig:nass_control_architecture.
|
|
The sample pose is measured using external metrology for five degrees of freedom, while the sixth degree of freedom (Rz) is estimated by combining measurements from the nano-hexapod encoders and spindle encoder.
|
|
|
|
The sample reference pose is determined by the reference signals of the translation stage, tilt stage, spindle, and micro-hexapod.
|
|
Position error computation follows a two-step process: first, homogeneous transformation matrices are used to determine the error in the nano-hexapod frame, then the Jacobian matrix $\bm{J}$ maps these errors to individual strut coordinates.
|
|
|
|
For control purposes, force sensors mounted on each strut are used in a decentralized way for active damping, as detailed in Section ref:sec:nass_active_damping.
|
|
Then, the high authority controller uses the computed errors in the frame of the struts to provides real-time stabilization of the sample position (Section ref:sec:nass_hac).
|
|
|
|
#+begin_src latex :file nass_control_architecture.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{1.0cm}, fill=colorblue!20!white] (metrology) {Metrology};
|
|
\node[block={2.0cm}{2.0cm}, below=0.1 of metrology, align=center, fill=colorblue!20!white] (nhexa) {Nano\\Hexapod};
|
|
\node[block={3.0cm}{1.5cm}, below=0.1 of nhexa, align=center, fill=colorblue!20!white] (ustation) {Micro\\Station};
|
|
|
|
\coordinate[] (inputf) at ($(nhexa.south west)!0.5!(nhexa.north west)$);
|
|
\coordinate[] (outputfn) at ($(nhexa.south east)!0.3!(nhexa.north east)$);
|
|
\coordinate[] (outputde) at ($(nhexa.south east)!0.7!(nhexa.north east)$);
|
|
|
|
\coordinate[] (outputDy) at ($(ustation.south east)!0.1!(ustation.north east)$);
|
|
\coordinate[] (outputRy) at ($(ustation.south east)!0.5!(ustation.north east)$);
|
|
\coordinate[] (outputRz) at ($(ustation.south east)!0.9!(ustation.north east)$);
|
|
|
|
\node[block={1.0cm}{1.0cm}, right=0.5 of outputde, fill=colorred!20!white] (Rz_kinematics) {$\bm{J}_{R_z}^{-1}$};
|
|
\node[block={2.0cm}{2.0cm}, right=2.2 of ustation, align=center, fill=colorred!20!white] (ustation_kinematics) {Compute\\Reference\\Position};
|
|
\node[block={2.0cm}{2.0cm}, right=0.8 of ustation_kinematics, align=center, fill=colorred!20!white] (compute_error) {Compute\\Error\\Position};
|
|
\node[block={2.0cm}{2.0cm}, above=0.8 of compute_error, align=center, fill=colorred!20!white] (compute_pos) {Compute\\Sample\\Position};
|
|
\node[block={1.0cm}{1.0cm}, right=0.8 of compute_error, fill=colorred!20!white] (hexa_jacobian) {$\bm{J}$};
|
|
|
|
\coordinate[] (inputMetrology) at ($(compute_error.north east)!0.3!(compute_error.north west)$);
|
|
\coordinate[] (inputRz) at ($(compute_error.north east)!0.7!(compute_error.north west)$);
|
|
|
|
\node[addb={+}{}{}{}{}, right=0.4 of Rz_kinematics, fill=colorred!20!white] (addRz) {};
|
|
\draw[->] (Rz_kinematics.east) -- (addRz.west);
|
|
\draw[->] (outputRz-|addRz)node[branch]{} -- (addRz.south);
|
|
|
|
\draw[->] (outputDy) node[above right]{$r_{D_y}$} -- (outputDy-|ustation_kinematics.west);
|
|
\draw[->] (outputRy) node[above right]{$r_{R_y}$} -- (outputRy-|ustation_kinematics.west);
|
|
\draw[->] (outputRz) node[above right]{$r_{R_z}$} -- (outputRz-|ustation_kinematics.west);
|
|
|
|
\draw[->] (metrology.east)node[above right]{$[D_x,\,D_y,\,D_z,\,R_x,\,R_y]$} -- (compute_pos.west|-metrology);
|
|
\draw[->] (addRz.east)node[above right]{$R_z$} -- (compute_pos.west|-addRz);
|
|
\draw[->] (compute_pos.south)node -- (compute_error.north)node[above right]{$\bm{y}_{\mathcal{X}}$};
|
|
|
|
\draw[->] (outputde) -- (Rz_kinematics.west) node[above left]{$\bm{\mathcal{L}}$};
|
|
\draw[->] (ustation_kinematics.east) -- (compute_error.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
|
|
\draw[->] (compute_error.east) -- (hexa_jacobian.west) node[above left]{$\bm{\epsilon\mathcal{X}}$};
|
|
\draw[->] (hexa_jacobian.east) -- ++(1.8, 0) node[above left]{$\bm{\epsilon\mathcal{L}}$};
|
|
|
|
\draw[->] (outputfn) -- ($(outputfn-|hexa_jacobian.east) + (1.0, 0)$)coordinate(fn) node[above left]{$\bm{f}_n$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(metrology.north-|ustation.west) (hexa_jacobian.east|-compute_error.south)}, fill=black!10!white, draw, dashed, inner sep=4pt] (plant) {};
|
|
\node[anchor={north east}] at (plant.north east){$\text{Plant}$};
|
|
\end{scope}
|
|
|
|
\node[block, above=0.2 of plant, fill=coloryellow!20!white] (Kiff) {$\bm{K}_{\text{IFF}}$};
|
|
\draw[->] ($(fn)-(0.6,0)$)node[branch]{} |- (Kiff.east);
|
|
|
|
\node[addb={+}{}{}{}{}, left=0.8 of inputf] (addf) {};
|
|
\draw[->] (Kiff.west) -| (addf.north);
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(plant.south-|fn) (addf.west|-Kiff.north)}, fill=black!20!white, draw, dashed, inner sep=4pt] (damped_plant) {};
|
|
\node[anchor={north east}] at (damped_plant.north east){$\text{Damped Plant}$};
|
|
\end{scope}
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(metrology.north-|ustation.west) (hexa_jacobian.east|-compute_error.south)}, fill=black!10!white, draw, dashed, inner sep=4pt] (plant) {};
|
|
\node[anchor={north east}] at (plant.north east){$\text{Plant}$};
|
|
\end{scope}
|
|
|
|
\node[block, left=0.8 of addf, fill=colorgreen!20!white] (Khac) {$\bm{K}_{\text{HAC}}$};
|
|
\draw[->] ($(hexa_jacobian.east)+(1.4,0)$)node[branch]{} |- ($(Khac.west)+(-0.4, -3.4)$) |- (Khac.west);
|
|
\draw[->] (Khac.east) -- node[midway, above]{$\bm{f}^{\prime}$} (addf.west);
|
|
\draw[->] (addf.east) -- (inputf) node[above left]{$\bm{f}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nass_control_architecture
|
|
#+caption: The physical systems are shown in blue, the control kinematics in red, the decentralized Integral Force Feedback in yellow and the centralized High Authority Controller in green.
|
|
#+attr_latex: :width \linewidth
|
|
#+RESULTS:
|
|
[[file:figs/nass_control_architecture.png]]
|
|
|
|
* Decentralized Active Damping
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/nass_2_active_damping.m
|
|
:END:
|
|
<<sec:nass_active_damping>>
|
|
** Introduction :ignore:
|
|
|
|
Building upon the uniaxial model study, this section implements decentralized Integral Force Feedback (IFF) as the first component of the HAC-LAC strategy.
|
|
Springs in parallel to the force sensors are used to guarantee the control robustness as was found using the 3DoF rotating model.
|
|
The objective here is to design a decentralized IFF controller that provides good damping of the nano-hexapod modes across payload masses ranging from $1$ to $50\,\text{kg}$ and rotational velocity up to $360\,\text{deg/s}$.
|
|
Used payloads have a cylindrical shape with 250 mm height and with masses of 1 kg, 25 kg, and 50 kg.
|
|
|
|
** 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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** IFF Plant
|
|
<<ssec:nass_active_damping_plant>>
|
|
|
|
Transfer functions from actuator forces $f_i$ to force sensor measurements $f_{mi}$ are computed using the multi-body model.
|
|
Figure ref:fig:nass_iff_plant_effect_kp examines how parallel stiffness affects the plant dynamics, with identification performed at maximum spindle velocity $\Omega_z = 360\,\text{deg/s}$ and with a payload mass of 25 kg.
|
|
|
|
Without parallel stiffness (Figure ref:fig:nass_iff_plant_no_kp), the dynamics exhibits non-minimum phase zeros at low frequency, confirming predictions from the three-degree-of-freedom rotating model.
|
|
Adding parallel stiffness (Figure ref:fig:nass_iff_plant_kp) transforms these into minimum phase complex conjugate zeros, enabling unconditionally stable decentralized IFF implementation.
|
|
|
|
Though both cases show significant coupling around resonances, stability is guaranteed by the collocated arrangement of actuators and sensors [[cite:&preumont08_trans_zeros_struc_contr_with]].
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF plant dynamics using the Simscape model
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod();
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/NASS'], 3, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N]
|
|
|
|
%% Identify for multi payload masses (no rotation)
|
|
initializeReferences(); % No Spindle Rotation
|
|
% 1kg Sample
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
G_iff_m1 = linearize(mdl, io);
|
|
G_iff_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_iff_m1.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
% 25kg Sample
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
G_iff_m25 = linearize(mdl, io);
|
|
G_iff_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_iff_m25.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
% 50kg Sample
|
|
initializeSample('type', 'cylindrical', 'm', 50);
|
|
G_iff_m50 = linearize(mdl, io);
|
|
G_iff_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_iff_m50.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
%% Effect of Rotation
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 1); % 360 deg/s
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
G_iff_m25_Rz = linearize(mdl, io, 0.1);
|
|
G_iff_m25_Rz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_iff_m25_Rz.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
|
|
%% Effect of Rotation - No added parallel stiffness
|
|
initializeSimplifiedNanoHexapod('actuator_kp', 0);
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 1); % 360 deg/s
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
G_iff_m25_Rz_no_kp = linearize(mdl, io, 0.1);
|
|
G_iff_m25_Rz_no_kp.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_iff_m25_Rz_no_kp.OutputName = {'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% IFF Plant - Without parallel stiffness
|
|
f = logspace(-1,3,1000);
|
|
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(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,1), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $k_p = 0$')
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz_no_kp(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$f_{ni}/f_j$ - $k_p = 0$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e1]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*unwrap(angle(squeeze(freqresp(G_iff_m25_Rz_no_kp(i,i), f, 'Hz')))), 'color', colors(1,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-20, 200]);
|
|
yticks([0:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([f(1), f(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_plant_no_kp.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% IFF Plant - With added parallel stiffness
|
|
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(squeeze(freqresp(G_iff_m25_Rz(i,j), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $k_p = 50N/mm$')
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(f, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), f, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$f_{ni}/f_j$ - $k_p = 50N/mm$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e1]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), f, 'Hz'))), 'color', colors(1,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-20, 200]);
|
|
yticks([0:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([f(1), f(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_plant_kp.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_iff_plant_effect_kp
|
|
#+caption: Effect of stiffness parallel to the force sensor on the IFF plant with $\Omega_z = 360\,\text{deg/s}$ and payload mass of 25kg. The dynamics without parallel stiffness has non-minimum phase zeros at low frequency (\subref{fig:nass_iff_plant_no_kp}). The added parallel stiffness transforms the non-minimum phase zeros to complex conjugate zeros (\subref{fig:nass_iff_plant_kp})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_no_kp}without parallel stiffness}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_iff_plant_no_kp.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_kp}with parallel stiffness}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_iff_plant_kp.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
The effect of rotation, shown in Figure ref:fig:nass_iff_plant_effect_rotation, is negligible as the actuator stiffness ($k_a = 1\,N/\mu m$) is large compared to the negative stiffness induced by gyroscopic effects (estimated from the 3DoF rotating model).
|
|
|
|
Figure ref:fig:nass_iff_plant_effect_payload illustrate the effect of payload mass on the plant dynamics.
|
|
While the poles and zeros are shifting with payload mass, the alternating pattern of poles and zeros is maintained, ensuring that the phase remains bounded between 0 and 180 degrees, and thus good robustness properties.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Effect of spindle's rotation on the 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(freqs, abs(squeeze(freqresp(G_iff_m25(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$ deg/s')
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% plot(freqs, abs(squeeze(freqresp(G_iff_m25_Rz(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
% 'DisplayName', '$f_{ni}/f_j$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e2]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-20, 200]);
|
|
yticks([0:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_plant_effect_rotation.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Effect of the payload's mass on the IFF Plant
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 1kg')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 25kg')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
|
'DisplayName', '$f_{ni}/f_i$ - 50kg')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e2]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-20, 200]);
|
|
yticks([0:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_plant_effect_payload.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_iff_plant_effect_rotation_payload
|
|
#+caption: Effect of the Spindle's rotational velocity on the IFF plant (\subref{fig:nass_iff_plant_effect_rotation}) and effect of the payload's mass on the IFF plant (\subref{fig:nass_iff_plant_effect_payload})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_effect_rotation}Effect of Spindle rotation}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_iff_plant_effect_rotation.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_plant_effect_payload}Effect of payload mass}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_iff_plant_effect_payload.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Controller Design
|
|
<<ssec:nass_active_damping_control>>
|
|
|
|
Previous analysis using the 3DoF rotating model showed that decentralized Integral Force Feedback (IFF) with pure integrators is unstable due to gyroscopic effects caused by spindle rotation.
|
|
This finding is also confirmed with the multi-body model of the NASS: the system is unstable when using pure integrators and without parallel stiffness.
|
|
|
|
This instability can be mitigated by introducing sufficient stiffness in parallel with the force sensors.
|
|
However, as illustrated in Figure ref:fig:nass_iff_plant_kp, adding parallel stiffness increases the low frequency gain.
|
|
If using pure integrators, this would results in high loop gain at low frequencies, adversely affecting the damped plant dynamics, which is undesirable.
|
|
To resolve this issue, a second-order high-pass filter is introduced to limit the low frequency gain, as shown in Equation eqref:eq:nass_kiff.
|
|
|
|
\begin{equation}\label{eq:nass_kiff}
|
|
\bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix}
|
|
K_{\text{IFF}}(s) & & 0 \\
|
|
& \ddots & \\
|
|
0 & & K_{\text{IFF}}(s)
|
|
\end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s} \cdot \frac{\frac{s^2}{\omega_z^2}}{\frac{s^2}{\omega_z^2} + 2 \xi_z \frac{s}{\omega_z} + 1}
|
|
\end{equation}
|
|
|
|
The cut-off frequency of the second-order high-pass filter is tuned to be below the frequency of the complex conjugate zero for the highest mass, which is at $5\,\text{Hz}$.
|
|
The overall gain is then increased to have large loop gain around resonances to be damped, as illustrated in Figure ref:fig:nass_iff_loop_gain.
|
|
|
|
#+begin_src matlab
|
|
%% Verify that parallel stiffness permits to have a stable plant
|
|
Kiff_pure_int = -200/s*eye(6);
|
|
isstable(feedback(G_iff_m25_Rz, Kiff_pure_int, 1))
|
|
isstable(feedback(G_iff_m25_Rz_no_kp, Kiff_pure_int, 1))
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% IFF Controller Design
|
|
% Second order high pass filter
|
|
wz = 2*pi*2;
|
|
xiz = 0.7;
|
|
Ghpf = (s^2/wz^2)/(s^2/wz^2 + 2*xiz*s/wz + 1);
|
|
|
|
Kiff = -200 * ... % Gain
|
|
1/(0.01*2*pi + s) * ... % LPF: provides integral action
|
|
Ghpf * ... % 2nd order HPF (limit low frequency gain)
|
|
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
|
|
|
|
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
|
|
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
% The designed IFF controller is saved
|
|
save('./matlab/mat/nass_K_iff.mat', 'Kiff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
% The designed IFF controller is saved
|
|
save('./mat/nass_K_iff.mat', 'Kiff');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Loop gain for the decentralized IFF
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
|
'DisplayName', '1kg')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
|
'DisplayName', '25kg')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(1,1), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
|
'DisplayName', '50kg')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e2]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m1(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(-Kiff(1,1)*G_iff_m50(i,i), freqs, 'Hz'))), 'color', [colors(3,:), 0.5]);
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-110, 200]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nass_iff_loop_gain.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:nass_iff_loop_gain
|
|
#+caption: Loop gain for the decentralized IFF: $K_{\text{IFF}}(s) \cdot \frac{f_{mi}}{f_i}(s)$
|
|
#+RESULTS:
|
|
[[file:figs/nass_iff_loop_gain.png]]
|
|
|
|
To verify stability, root loci for the three payload configurations are computed and shown in Figure ref:fig:nass_iff_root_locus.
|
|
The results demonstrate that the closed-loop poles remain within the left-half plane, indicating the robust stability properties of the applied decentralized IFF.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Root Locus for the Decentralized IFF controller - 1kg Payload
|
|
figure;
|
|
gains = logspace(-2, 1, 200);
|
|
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
nexttile();
|
|
hold on;
|
|
|
|
plot(real(pole(G_iff_m1)), imag(pole(G_iff_m1)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_iff_m1)), imag(tzero(G_iff_m1)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_iff_m1, g*Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
clpoles = pole(feedback(G_iff_m1, Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), 'kx', ...
|
|
'DisplayName', '$g_{opt}$');
|
|
|
|
xline(0);
|
|
yline(0);
|
|
hold off;
|
|
axis equal;
|
|
xlim([-900, 100]); ylim([-100, 900]);
|
|
xticks([-900:100:0]);
|
|
yticks([0:100:900]);
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
xlabel('Real part'); ylabel('Imaginary part');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_root_locus_1kg.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Root Locus for the Decentralized IFF controller - 25kg Payload
|
|
gains = logspace(-2, 1, 200);
|
|
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
nexttile();
|
|
hold on;
|
|
|
|
plot(real(pole(G_iff_m25)), imag(pole(G_iff_m25)), 'x', 'color', colors(2,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_iff_m25)), imag(tzero(G_iff_m25)), 'o', 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_iff_m25, g*Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
clpoles = pole(feedback(G_iff_m25, Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), 'kx', ...
|
|
'DisplayName', '$g_{opt}$');
|
|
|
|
xline(0);
|
|
yline(0);
|
|
hold off;
|
|
axis equal;
|
|
xlim([-900, 100]); ylim([-100, 900]);
|
|
xticks([-900:100:0]);
|
|
yticks([0:100:900]);
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
xlabel('Real part'); ylabel('Imaginary part');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_root_locus_25kg.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Root Locus for the Decentralized IFF controller - 50kg Payload
|
|
gains = logspace(-2, 1, 200);
|
|
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
nexttile();
|
|
hold on;
|
|
|
|
plot(real(pole(G_iff_m50)), imag(pole(G_iff_m50)), 'x', 'color', colors(3,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_iff_m50)), imag(tzero(G_iff_m50)), 'o', 'color', colors(3,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_iff_m50, g*Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(3,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
clpoles = pole(feedback(G_iff_m50, Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), 'kx', ...
|
|
'DisplayName', '$g_{opt}$');
|
|
|
|
xline(0);
|
|
yline(0);
|
|
hold off;
|
|
axis equal;
|
|
xlim([-900, 100]); ylim([-100, 900]);
|
|
xticks([-900:100:0]);
|
|
yticks([0:100:900]);
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
xlabel('Real part'); ylabel('Imaginary part');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_iff_root_locus_50kg.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:nass_iff_root_locus
|
|
#+caption: Root Loci for Decentralized IFF for three payload masses. Closed-loop poles are shown by the black crosses.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_root_locus_1kg} $1\,\text{kg}$}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/nass_iff_root_locus_1kg.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_root_locus_25kg} $25\,\text{kg}$}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/nass_iff_root_locus_25kg.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_iff_root_locus_50kg} $50\,\text{kg}$}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/nass_iff_root_locus_50kg.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
* Centralized Active Vibration Control
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/nass_3_hac.m
|
|
:END:
|
|
<<sec:nass_hac>>
|
|
** Introduction :ignore:
|
|
|
|
# - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]: Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...)
|
|
|
|
- [ ] Effect of micro-station compliance
|
|
Compare plant with "rigid" u-station and normal u-station
|
|
- Effect of IFF
|
|
- Effect of payload mass
|
|
- Decoupled plant
|
|
- Controller design
|
|
|
|
From control kinematics:
|
|
- Talk about issue of not estimating Rz from external metrology? (maybe could be nice to discuss that during the experiments!)
|
|
- Show what happens is Rz is not estimated (for instance supposed equaled to zero => increased coupling)
|
|
|
|
** 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-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** HAC Plant
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF plant dynamics using the Simscape model
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod();
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m]
|
|
|
|
%% Identify HAC Plant without using IFF
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
G_m1 = linearize(mdl, io);
|
|
G_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
G_m25 = linearize(mdl, io);
|
|
G_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
initializeSample('type', 'cylindrical', 'm', 50);
|
|
G_m50 = linearize(mdl, io);
|
|
G_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
%% Effect of Rotation
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 1); % 360 deg/s
|
|
|
|
G_m1_Rz = linearize(mdl, io, 0.1);
|
|
G_m1_Rz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1_Rz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
#+end_src
|
|
|
|
- Effect of rotation: ref:fig:nass_undamped_plant_effect_Wz
|
|
Add some coupling at low frequency, but still small at the considered velocity.
|
|
This is thanks to the relatively stiff nano-hexapod (CF rotating model)
|
|
- Effect of payload mass:
|
|
Decrease resonance frequencies
|
|
Increase coupling: ref:fig:nass_undamped_plant_effect_mass
|
|
=> control challenge for high payload masses
|
|
- Other effects such as: Ry tilt angle, Rz spindle position, micro-hexapod position are found to have negligible effect on the plant dynamics.
|
|
This is thanks to the fact the the plant dynamics is well decoupled from the micro-station dynamics.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m1(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 0$')
|
|
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, $\Omega = 360$ deg/s')
|
|
plot(freqs, abs(squeeze(freqresp(G_m1(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
|
|
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-11, 2e-5]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1_Rz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_undamped_plant_effect_Wz.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 1 kg')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 25 kg')
|
|
plot(freqs, abs(squeeze(freqresp(G_m50(1,1), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, 50 kg')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m50(i,j), freqs, 'Hz'))), 'color', [colors(3,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-11, 2e-5]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz')))), 'color', colors(3,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_undamped_plant_effect_mass.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_undamped_plant_effect
|
|
#+caption: Effect of the Spindle's rotational velocity on the positioning plant (\subref{fig:nass_undamped_plant_effect_Wz}) and effect of the payload's mass on the positioning plant (\subref{fig:nass_undamped_plant_effect_mass})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_Wz}Effect of rotational velocity $\Omega_z$}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_undamped_plant_effect_Wz.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_undamped_plant_effect_mass}Effect of payload's mass}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_undamped_plant_effect_mass.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
|
|
- Effect of IFF on the plant ref:fig:nass_comp_undamped_damped_plant_m1
|
|
Modes are well damped
|
|
Small coupling increase at low frequency
|
|
- Benefits of using IFF ref:fig:nass_hac_plants
|
|
with added damping, the set of plants to be controlled (with payloads from 1kg to 50kg) is more easily controlled.
|
|
Between 10 and 50Hz, the plant dynamics does not vary a lot with the frequency, whereas without active damping, it would be impossible to design a robust controller with bandwidth above 10Hz that is robust to the change of payload
|
|
|
|
#+begin_src matlab
|
|
%% Identify HAC Plant without using IFF
|
|
initializeReferences(); % No Spindle Rotation
|
|
initializeController('type', 'iff'); % Implemented IFF controller
|
|
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller
|
|
|
|
% 1kg payload
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
G_hac_m1 = linearize(mdl, io);
|
|
G_hac_m1.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_hac_m1.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
% 25kg payload
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
G_hac_m25 = linearize(mdl, io);
|
|
G_hac_m25.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_hac_m25.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
% 50kg payload
|
|
initializeSample('type', 'cylindrical', 'm', 50);
|
|
G_hac_m50 = linearize(mdl, io);
|
|
G_hac_m50.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_hac_m50.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
% Check stability
|
|
if not(isstable(G_hac_m1) && isstable(G_hac_m25) && isstable(G_hac_m50))
|
|
warning('One of HAC plant is not stable')
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, OL')
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, with IFF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-10, 2e-5]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m1(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_hac_m1(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_comp_undamped_damped_plant_m1.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of all the undamped FRF and all the damped FRF
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m1( 1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'DisplayName', 'Undamped - $\epsilon\mathcal{L}_i/f_i$');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m1(1,1), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'DisplayName', 'Damped - $\epsilon\mathcal{L}_i/f_i^\prime$');
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_m50(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/N]'); set(gca, 'XTickLabel',[]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
% ylim([1e-8, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i =1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_m1( i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_m50(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m1( i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m25(i,i), freqs, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_m50(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;
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
% xlim([1, 5e2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_hac_plants.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_hac_plant
|
|
#+caption: Effect of the Spindle's rotational velocity on the positioning plant (\subref{fig:nass_undamped_plant_effect_Wz}) and effect of the payload's mass on the positioning plant (\subref{fig:nass_undamped_plant_effect_mass})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_comp_undamped_damped_plant_m1}Effect of IFF - $m = 1\,\text{kg}$}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_comp_undamped_damped_plant_m1.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_hac_plants}Effect of IFF on the set of plants to control}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_hac_plants.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Effect of micro-station compliance
|
|
|
|
Micro-Station complex dynamics has almost no effect on the plant dynamics (Figure ref:fig:nass_effect_ustation_compliance):
|
|
- adds some alternating poles and zeros above 100Hz, which should not be an issue for control
|
|
|
|
#+begin_src matlab
|
|
%% Identify plant with "rigid" micro-station
|
|
initializeGround('type', 'rigid');
|
|
initializeGranite('type', 'rigid');
|
|
initializeTy('type', 'rigid');
|
|
initializeRy('type', 'rigid');
|
|
initializeRz('type', 'rigid');
|
|
initializeMicroHexapod('type', 'rigid');
|
|
initializeSimplifiedNanoHexapod();
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
|
|
initializeReferences();
|
|
initializeController('type', 'open-loop'); % Implemented IFF controller
|
|
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m]
|
|
|
|
G_m25_rigid = linearize(mdl, io);
|
|
G_m25_rigid.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m25_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Effect of the micro-station limited compliance on the plant dynamics
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_rigid( 1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, OL')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$, with IFF')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_rigid( i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-10, 2e-5]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nass_effect_ustation_compliance.pdf', 'width', 'wide', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_effect_ustation_compliance
|
|
#+caption: Effect of the micro-station limited compliance on the plant dynamics
|
|
#+RESULTS:
|
|
[[file:figs/nass_effect_ustation_compliance.png]]
|
|
|
|
** Higher or lower nano-hexapod stiffness?
|
|
|
|
*Goal*: confirm the analysis with simpler models (uniaxial and 3DoF) that a nano-hexapod stiffness of $\approx 1\,N/\mu m$ should give better performances than a very stiff or very soft nano-hexapod.
|
|
|
|
- *Stiff nano-hexapod*:
|
|
uniaxial model: high nano-hexapod stiffness induce coupling between the nano-hexapod and the micro-station dynamics.
|
|
considering the complex dynamics of the micro-station as shown by the modal analysis, that would result in a complex system to control
|
|
To show that, a nano-hexapod with actuator stiffness equal to 100N/um is initialized, payload of 25kg.
|
|
The dynamics from $\bm{f}$ to $\bm{\epsilon}_{\mathcal{L}}$ is identified and compared to the case where the micro-station is infinitely rigid (figure ref:fig:nass_stiff_nano_hexapod_coupling_ustation):
|
|
- Coupling induced by the micro-station: much more complex and difficult to model / predict
|
|
- Similar to what was predicted using the uniaxial model
|
|
- *Soft nano-hexapod*:
|
|
Nano-hexapod with stiffness of 0.01N/um is initialized, payload of 25kg.
|
|
Dynamics is identified with no spindle rotation, and with spindle rotation of 36deg/s and 360deg/s (Figure ref:fig:nass_soft_nano_hexapod_effect_Wz)
|
|
- Rotation as huge effect on the dynamics: unstable for high rotational velocities, added coupling due to gyroscopic effects, and change of resonance frequencies as a function of the rotational velocity
|
|
- Simple 3DoF rotating model is helpful to understand the complex effect of the rotation => similar conclusion
|
|
- Say that controlling the frame of the struts is not adapted with a soft nano-hexapod, but we should rather control in the frame matching the center of mass of the payload, but we would still obtain large coupling and change of dynamics due to gyroscopic effects.
|
|
|
|
#+begin_src matlab
|
|
%% Identify Dynamics with a Stiff nano-hexapod (100N/um)
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod('actuator_k', 1e8, 'actuator_kp', 0, 'actuator_c', 1e3);
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m]
|
|
|
|
% Identify Plant
|
|
G_m25_pz = linearize(mdl, io);
|
|
G_m25_pz.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m25_pz.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
%% Compare with Nano-Hexapod alone (rigid micro-station)
|
|
initializeGround('type', 'rigid');
|
|
initializeGranite('type', 'rigid');
|
|
initializeTy('type', 'rigid');
|
|
initializeRy('type', 'rigid');
|
|
initializeRz('type', 'rigid');
|
|
initializeMicroHexapod('type', 'rigid');
|
|
|
|
% Identify Plant
|
|
G_m25_pz_rigid = linearize(mdl, io);
|
|
G_m25_pz_rigid.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m25_pz_rigid.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Stiff nano-hexapod - Coupling with the micro-station
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - Rigid')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_i$ - $\mu$-station')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(1,2), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - Rigid')
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(1,2), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'DisplayName', '$\epsilon_{\mathcal{L}i}/f_j$ - $\mu$-station')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,j), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,j), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-12, 3e-7]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz_rigid(i,i), freqs, 'Hz')))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_m25_pz(i,i), freqs, 'Hz')))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-200, 20]);
|
|
yticks([-180:45:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_stiff_nano_hexapod_coupling_ustation.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Identify Dynamics with a Soft nano-hexapod (0.01N/um)
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod('actuator_k', 1e4, 'actuator_kp', 0, 'actuator_c', 1);
|
|
|
|
% Initialize each Simscape model elements
|
|
initializeSample('type', 'cylindrical', 'm', 25); % 25kg payload
|
|
initializeController('type', 'open-loop');
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Strut errors [m]
|
|
|
|
% Identify the dynamics without rotation
|
|
initializeReferences();
|
|
G_m1_vc = linearize(mdl, io);
|
|
G_m1_vc.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1_vc.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
% Identify the dynamics with 36 deg/s rotation
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 10); % 36 deg/s
|
|
|
|
G_m1_vc_Rz_slow = linearize(mdl, io, 0.1);
|
|
G_m1_vc_Rz_slow.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1_vc_Rz_slow.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
|
|
% Identify the dynamics with 360 deg/s rotation
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 1); % 360 deg/s
|
|
|
|
G_m1_vc_Rz_fast = linearize(mdl, io, 0.1);
|
|
G_m1_vc_Rz_fast.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_m1_vc_Rz_fast.OutputName = {'l1', 'l2', 'l3', 'l4', 'l5', 'l6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Soft Nano-Hexapod - effect of rotational velocity on the dynamics
|
|
f = logspace(-1,2,200);
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc(1,1), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 0$')
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(1,1), f, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 36$ deg/s')
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(1,1), f, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ - $\Omega_z = 360$ deg/s')
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc(i,j), f, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,j), f, 'Hz'))), 'color', [colors(2,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,j), f, 'Hz'))), 'color', [colors(3,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-2]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc(i,i), f, 'Hz'))), 'color', colors(1,:));
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_slow(i,i), f, 'Hz'))), 'color', colors(2,:));
|
|
plot(f, 180/pi*angle(squeeze(freqresp(G_m1_vc_Rz_fast(i,i), f, 'Hz'))), 'color', colors(3,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180:90:180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([f(1), f(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_soft_nano_hexapod_effect_Wz.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_soft_stiff_hexapod
|
|
#+caption: Plant dynamics of a stiff ($k_a = 100\,N/\mu m$) nano-hexapod (\subref{fig:nass_stiff_nano_hexapod_coupling_ustation}) and of a soft ($k_a = 0.01\,N/\mu m$) nano-hexapod (\subref{fig:nass_soft_nano_hexapod_effect_Wz})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_stiff_nano_hexapod_coupling_ustation}Stiff nano-hexapod - Coupling with the micro-station}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_stiff_nano_hexapod_coupling_ustation.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_soft_nano_hexapod_effect_Wz}Soft nano-hexapod - Effect of Spindle rotational velocity}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_soft_nano_hexapod_effect_Wz.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Controller design
|
|
|
|
In this section, a high authority controller is design such that:
|
|
- it is robust to the change of payload mass (i.e. is should be stable for all the damped plants of Figure ref:fig:nass_hac_plants)
|
|
- it has reasonably high bandwidth to give good performances (here 10Hz)
|
|
|
|
eqref:eq:nass_robust_hac
|
|
|
|
\begin{equation}\label{eq:nass_robust_hac}
|
|
K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}, \quad \left( \omega_c = 2\pi10\,\text{rad/s},\ \alpha = 2,\ \omega_0 = 2\pi80\,\text{rad/s} \right)
|
|
\end{equation}
|
|
|
|
#+begin_src matlab
|
|
%% HAC Design
|
|
% Wanted crossover
|
|
wc = 2*pi*10; % [rad/s]
|
|
|
|
% Integrator
|
|
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/80);
|
|
|
|
% Gain to have unitary crossover at wc
|
|
H_gain = 1./abs(evalfr(G_hac_m50(1,1), 1j*wc));
|
|
|
|
% Decentralized HAC
|
|
Khac = -H_gain * ... % Gain
|
|
H_int * ... % Integrator
|
|
H_lead * ... % Low Pass filter
|
|
H_lpf * ... % Low Pass filter
|
|
eye(6); % 6x6 Diagonal
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
% The designed HAC controller is saved
|
|
save('./matlab/mat/nass_K_hac.mat', 'Khac');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
% The designed HAC controller is saved
|
|
save('./mat/nass_K_hac.mat', 'Khac');
|
|
#+end_src
|
|
|
|
- "Decentralized" Loop Gain:
|
|
Bandwidth around 10Hz
|
|
- Characteristic Loci:
|
|
Stable for all payloads with acceptable stability margins
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% "Diagonal" loop gain for the High Authority Controller
|
|
f = logspace(-1, 2, 1000);
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), ...
|
|
'color', [colors(1,:), 0.5], 'DisplayName', '1kg');
|
|
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), ...
|
|
'color', [colors(2,:), 0.5], 'DisplayName', '25kg');
|
|
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), ...
|
|
'color', [colors(3,:), 0.5], 'DisplayName', '50kg');
|
|
for i = 2:6
|
|
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5], 'HandleVisibility', 'off');
|
|
plot(f, abs(squeeze(freqresp(Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 0.5], 'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-2, 1e2]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m1( i,i), f, 'Hz'))), 'color', [colors(1,:), 0.5]);
|
|
plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m25(i,i), f, 'Hz'))), 'color', [colors(2,:), 0.5]);
|
|
plot(f, 180/pi*angle(squeeze(freqresp(-Khac(i,i)*G_hac_m50(i,i), f, 'Hz'))), 'color', [colors(3,:), 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([0.1, 100]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_hac_loop_gain.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Characteristic Loci for the High Authority Controller
|
|
Ldet_m1 = zeros(6, length(freqs));
|
|
Lmimo_m1 = squeeze(freqresp(-G_hac_m1*Khac, freqs, 'Hz'));
|
|
for i_f = 2:length(freqs)
|
|
Ldet_m1(:, i_f) = eig(squeeze(Lmimo_m1(:,:,i_f)));
|
|
end
|
|
Ldet_m25 = zeros(6, length(freqs));
|
|
Lmimo_m25 = squeeze(freqresp(-G_hac_m25*Khac, freqs, 'Hz'));
|
|
for i_f = 2:length(freqs)
|
|
Ldet_m25(:, i_f) = eig(squeeze(Lmimo_m25(:,:,i_f)));
|
|
end
|
|
Ldet_m50 = zeros(6, length(freqs));
|
|
Lmimo_m50 = squeeze(freqresp(-G_hac_m50*Khac, freqs, 'Hz'));
|
|
for i_f = 2:length(freqs)
|
|
Ldet_m50(:, i_f) = eig(squeeze(Lmimo_m50(:,:,i_f)));
|
|
end
|
|
|
|
figure;
|
|
hold on;
|
|
plot(real(squeeze(Ldet_m1(1,:))), imag(squeeze(Ldet_m1(1,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'DisplayName', '1kg');
|
|
plot(real(squeeze(Ldet_m1(1,:))),-imag(squeeze(Ldet_m1(1,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet_m25(1,:))), imag(squeeze(Ldet_m25(1,:))), ...
|
|
'.', 'color', colors(2, :), ...
|
|
'DisplayName', '25kg');
|
|
plot(real(squeeze(Ldet_m25(1,:))),-imag(squeeze(Ldet_m25(1,:))), ...
|
|
'.', 'color', colors(2, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet_m50(1,:))), imag(squeeze(Ldet_m50(1,:))), ...
|
|
'.', 'color', colors(3, :), ...
|
|
'DisplayName', '50kg');
|
|
plot(real(squeeze(Ldet_m50(1,:))),-imag(squeeze(Ldet_m50(1,:))), ...
|
|
'.', 'color', colors(3, :), ...
|
|
'HandleVisibility', 'off');
|
|
for i = 2:6
|
|
plot(real(squeeze(Ldet_m1(i,:))), imag(squeeze(Ldet_m1(i,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet_m1(i,:))), -imag(squeeze(Ldet_m1(i,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
plot(real(squeeze(Ldet_m25(i,:))), imag(squeeze(Ldet_m25(i,:))), ...
|
|
'.', 'color', colors(2, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet_m25(i,:))), -imag(squeeze(Ldet_m25(i,:))), ...
|
|
'.', 'color', colors(2, :), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
plot(real(squeeze(Ldet_m50(i,:))), imag(squeeze(Ldet_m50(i,:))), ...
|
|
'.', 'color', colors(3, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet_m50(i,:))), -imag(squeeze(Ldet_m50(i,:))), ...
|
|
'.', 'color', colors(3, :), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
axis square
|
|
xlim([-1.8, 0.2]); ylim([-1, 1]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_hac_loci.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nass_hac_controller
|
|
#+caption: High Authority Controller - "Diagonal Loop Gain" (\subref{fig:nass_hac_loop_gain}) and Characteristic Loci (\subref{fig:nass_hac_loci})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_hac_loop_gain}Loop Gain}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_hac_loop_gain.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_hac_loci}Characteristic Loci}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nass_hac_loci.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** TODO Sensitivity to disturbances :noexport:
|
|
|
|
- Compute transfer functions from spindle vertical error to sample vertical error with HAC-IFF
|
|
Compare without the NASS, and with just IFF
|
|
- Same for horizontal
|
|
|
|
#+begin_src matlab
|
|
% Initialize each Simscape model elements
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
initializeSimplifiedNanoHexapod();
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
|
|
% Initial Simscape Configuration
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeDisturbances('enable', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
initializeReferences();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_y'); io_i = io_i + 1; % Spindle Lateral Vibration [N]
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Spindle Vertical Vibration [N]
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fdy_z'); io_i = io_i + 1; % Vertical Ground Motion [m]
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwy'); io_i = io_i + 1; % Vertical Ground Motion [m]
|
|
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Vertical Ground Motion [m]
|
|
io(io_i) = linio([mdl, '/NASS'], 2, 'output', [], 'y'); io_i = io_i + 1; % Lateral Displacement [m]
|
|
io(io_i) = linio([mdl, '/NASS'], 2, 'output', [], 'z'); io_i = io_i + 1; % Vertical Displacement [m]
|
|
|
|
Gd_ol = linearize(mdl, io);
|
|
Gd_ol.InputName = {'Frz_y', 'Frz_z', 'Fdy_z', 'Dwy', 'Dwz'};
|
|
Gd_ol.OutputName = {'Dy', 'Dz'};
|
|
|
|
initializeController('type', 'iff'); % Implemented IFF controller
|
|
load('nass_K_iff.mat', 'Kiff'); % Load designed IFF controller
|
|
|
|
Gd_iff = linearize(mdl, io);
|
|
Gd_iff.InputName = {'Frz_y', 'Frz_z', 'Fdy_z', 'Dwy', 'Dwz'};
|
|
Gd_iff.OutputName = {'Dy', 'Dz'};
|
|
|
|
initializeController('type', 'hac-iff'); % Implemented IFF controller
|
|
load('nass_K_hac.mat', 'Khac'); % Load designed HAC controller
|
|
|
|
Gd_hac_iff = linearize(mdl, io);
|
|
Gd_hac_iff.InputName = {'Frz_y', 'Frz_z', 'Fdy_z', 'Dwy', 'Dwz'};
|
|
Gd_hac_iff.OutputName = {'Dy', 'Dz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
dist = load('ustation_disturbance_psd.mat');
|
|
#+end_src
|
|
|
|
Spindle, lateral:
|
|
#+begin_src matlab
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dy', 'Frz_y'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff('Dy', 'Frz_y'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dy', 'Frz_y'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $D_z/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
xlim([1, 500]);
|
|
#+end_src
|
|
|
|
Spindle, vertical:
|
|
#+begin_src matlab
|
|
freqs = logspace(-1,3,1000);
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dz', 'Frz_z'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff('Dz', 'Frz_z'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dz', 'Frz_z'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $D_z/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]');
|
|
#+end_src
|
|
|
|
Ground motion, vertical:
|
|
#+begin_src matlab
|
|
freqs = logspace(-1,3,1000);
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dz', 'Dwz'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff('Dz', 'Dwz'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dz', 'Dwz'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $D_z/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
% xlim([1, 500]);
|
|
#+end_src
|
|
|
|
Ground motion, lateral:
|
|
#+begin_src matlab
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd_ol( 'Dy', 'Dwy'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_iff('Dy', 'Dwy'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gd_hac_iff('Dy', 'Dwy'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude $D_y/F_{R_z,z}$ [m/N]'); xlabel('Frequency [Hz]');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
xlim([1, 500]);
|
|
#+end_src
|
|
|
|
Noise Budget:
|
|
#+begin_src matlab
|
|
figure;
|
|
hold on;
|
|
plot(dist.gm_dist.f, sqrt(flip(-cumtrapz(flip(dist.gm_dist.f), flip(dist.gm_dist.pxx_y.*abs(squeeze(freqresp(Gd_ol( 'Dy', 'Dwy'), dist.gm_dist.f, 'Hz'))).^2)))));
|
|
plot(dist.gm_dist.f, sqrt(flip(-cumtrapz(flip(dist.gm_dist.f), flip(dist.gm_dist.pxx_y.*abs(squeeze(freqresp(Gd_iff( 'Dy', 'Dwy'), dist.gm_dist.f, 'Hz'))).^2)))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('ASD [m/sqrt(Hz)]'); xlabel('Frequency [Hz]');
|
|
xticks([1e0, 1e1, 1e2]);
|
|
xlim([1, 500]);
|
|
#+end_src
|
|
|
|
** Tomography experiment
|
|
|
|
- Validation of concept with tomography scans at the highest rotational velocity of $\Omega_z = 360\,\text{deg/s}$
|
|
- Compare obtained results with the smallest beam size that is expected with future beamline upgrade: 200nm (horizontal size) x 100nm (vertical size)
|
|
- Take into account the two main sources of disturbances: ground motion, spindle vibrations
|
|
Other noise sources are not taken into account here as they will be optimized latter (detail design phase): measurement noise, electrical noise for DAC and voltage amplifiers, ...
|
|
|
|
The open-loop errors and the closed-loop errors for the tomography scan with the light sample $1\,kg$ are shown in Figure ref:fig:nass_tomo_1kg_60rpm.
|
|
|
|
#+begin_src matlab
|
|
% Sample is not centered with the rotation axis
|
|
% This is done by offsetfing the micro-hexapod by 0.9um
|
|
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]
|
|
|
|
open(mdl);
|
|
set_param(mdl, 'StopTime', '2');
|
|
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod('AP', P_micro_hexapod);
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'all', 'Ts', 1e-3);
|
|
|
|
initializeDisturbances(...
|
|
'Dw_x', true, ... % Ground Motion - X direction
|
|
'Dw_y', true, ... % Ground Motion - Y direction
|
|
'Dw_z', true, ... % Ground Motion - Z direction
|
|
'Fdy_x', false, ... % Translation Stage - X direction
|
|
'Fdy_z', false, ... % Translation Stage - Z direction
|
|
'Frz_x', true, ... % Spindle - X direction
|
|
'Frz_y', true, ... % Spindle - Y direction
|
|
'Frz_z', true); % Spindle - Z direction
|
|
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 1, ...
|
|
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
|
|
|
|
% Open-Loop Simulation without Nano-Hexapod - 1kg payload
|
|
initializeSimplifiedNanoHexapod('type', 'none');
|
|
initializeController('type', 'open-loop');
|
|
sim(mdl);
|
|
exp_tomo_ol_m1 = simout;
|
|
|
|
% Closed-Loop Simulation with NASS
|
|
initializeSimplifiedNanoHexapod();
|
|
initializeController('type', 'hac-iff');
|
|
load('nass_K_iff.mat', 'Kiff');
|
|
load('nass_K_hac.mat', 'Khac');
|
|
|
|
% 1kg payload
|
|
initializeSample('type', 'cylindrical', 'm', 1);
|
|
sim(mdl);
|
|
exp_tomo_cl_m1 = simout;
|
|
|
|
% 25kg payload
|
|
initializeSample('type', 'cylindrical', 'm', 25);
|
|
sim(mdl);
|
|
exp_tomo_cl_m25 = simout;
|
|
|
|
% 50kg payload
|
|
initializeSample('type', 'cylindrical', 'm', 50);
|
|
sim(mdl);
|
|
exp_tomo_cl_m50 = simout;
|
|
|
|
% Slower tomography for high payload mass
|
|
% initializeReferences(...
|
|
% 'Rz_type', 'rotating', ...
|
|
% 'Rz_period', 10, ... % 36deg/s
|
|
% 'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
|
|
% initializeSample('type', 'cylindrical', 'm', 50);
|
|
% set_param(mdl, 'StopTime', '5');
|
|
% sim(mdl);
|
|
% exp_tomo_cl_m50_slow = simout;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Simulation of tomography experiment - 1kg payload - 360deg/s - XY errors
|
|
figure;
|
|
hold on;
|
|
plot(1e6*exp_tomo_ol_m1.y.x.Data, 1e6*exp_tomo_ol_m1.y.y.Data, 'DisplayName', 'OL')
|
|
plot(1e6*exp_tomo_cl_m1.y.x.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
|
|
hold off;
|
|
xlabel('$D_x$ [$\mu$m]'); ylabel('$D_y$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-2, 2]); ylim([-2, 2]);
|
|
xticks([-2:1:2]);
|
|
yticks([-2:1:2]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_tomo_1kg_60rpm_xy.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
|
|
figure;
|
|
tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(1e6*exp_tomo_ol_m1.y.y.Data, 1e6*exp_tomo_ol_m1.y.z.Data, 'DisplayName', 'OL')
|
|
plot(1e6*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e6*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
|
|
hold off;
|
|
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-2, 2]); ylim([-0.4, 0.4]);
|
|
xticks([-2:1:2]);
|
|
yticks([-2:0.2:2]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile();
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', 'CL')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
|
|
hold off;
|
|
xlabel('$D_y$ [nm]'); ylabel('$D_z$ [nm]');
|
|
axis equal
|
|
xlim([-500, 500]); ylim([-100, 100]);
|
|
xticks([-500:100:500]);
|
|
yticks([-100:50:100]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_tomo_1kg_60rpm_yz.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:nass_tomo_1kg_60rpm
|
|
#+caption: Position error of the sample in the XY (\subref{fig:nass_tomo_1kg_60rpm_xy}) and YZ (\subref{fig:nass_tomo_1kg_60rpm_yz}) planes during a simulation of a tomography experiment at $360\,\text{deg/s}$. 1kg payload is placed on top of the nano-hexapod.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_xy}XY plane}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.9
|
|
[[file:figs/nass_tomo_1kg_60rpm_xy.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_tomo_1kg_60rpm_yz}YZ plane}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.9
|
|
[[file:figs/nass_tomo_1kg_60rpm_yz.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
- Effect of payload mass (Figure ref:fig:nass_tomography_hac_iff):
|
|
Worse performance for high masses, as expected from the control analysis, but still acceptable considering that the rotational velocity of 360deg/s is only used for light payloads.
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m1.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m1.y.z.Data(1e3:end), 'color', colors(1,:), 'DisplayName', '$m = 1$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
|
|
hold off;
|
|
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-200, 200]); ylim([-100, 100]);
|
|
xticks([-200:50:200]); yticks([-100:50:100]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_tomography_hac_iff_m1.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m25.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m25.y.z.Data(1e3:end), 'color', colors(2,:), 'DisplayName', '$m = 25$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
|
|
hold off;
|
|
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-200, 200]); ylim([-100, 100]);
|
|
xticks([-200:50:200]); yticks([-100:50:100]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_tomography_hac_iff_m25.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Simulation of tomography experiment - no payload, 30rpm - YZ errors
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile();
|
|
hold on;
|
|
plot(1e9*exp_tomo_cl_m50.y.y.Data(1e3:end), 1e9*exp_tomo_cl_m50.y.z.Data(1e3:end), 'color', colors(3,:), 'DisplayName', '$m = 50$ kg')
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(100*cos(theta), 50*sin(theta), 'k--', 'DisplayName', 'Beam size')
|
|
hold off;
|
|
xlabel('$D_y$ [$\mu$m]'); ylabel('$D_z$ [$\mu$m]');
|
|
axis equal
|
|
xlim([-200, 200]); ylim([-100, 100]);
|
|
xticks([-200:50:200]); yticks([-100:50:100]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nass_tomography_hac_iff_m50.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:nass_tomography_hac_iff
|
|
#+caption: Simulation of tomography experiments - 360deg/s. Beam size shown by dashed black
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m1} $m = 1\,kg$}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nass_tomography_hac_iff_m1.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m25} $m = 25\,kg$}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nass_tomography_hac_iff_m25.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nass_tomography_hac_iff_m50} $m = 50\,kg$}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nass_tomography_hac_iff_m50.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
* Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
<<sec:nass_conclusion>>
|
|
|
|
|
|
* Bibliography :ignore:
|
|
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
|
|
|
|
* Helping Functions :noexport:
|
|
** Initialize Path
|
|
#+NAME: m-init-path
|
|
#+BEGIN_SRC matlab
|
|
addpath('./matlab/'); % Path for scripts
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./matlab/mat/'); % Path for Computed FRF
|
|
addpath('./matlab/src/'); % Path for functions
|
|
addpath('./matlab/STEPS/'); % Path for STEPS
|
|
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Data directory
|
|
data_dir = './matlab/mat/'
|
|
#+END_SRC
|
|
|
|
#+NAME: m-init-path-tangle
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for Data
|
|
addpath('./src/'); % Path for functions
|
|
addpath('./STEPS/'); % Path for STEPS
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Data directory
|
|
data_dir = './mat/';
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
% Simulink Model name
|
|
mdl = 'nass_model';
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector [Hz]
|
|
freqs = logspace(0, 3, 1000);
|
|
#+END_SRC
|
|
|
|
* Matlab Functions :noexport:
|
|
** =initializeSimscapeConfiguration=: Simscape Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeSimscapeConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
#+begin_src matlab
|
|
function [] = initializeSimscapeConfiguration(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
args.gravity logical {mustBeNumericOrLogical} = true
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
#+begin_src matlab
|
|
conf_simscape = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
#+begin_src matlab
|
|
if args.gravity
|
|
conf_simscape.type = 1;
|
|
else
|
|
conf_simscape.type = 2;
|
|
end
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_conf_simscape.mat', 'file')
|
|
save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
|
|
else
|
|
save('mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file')
|
|
save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeLoggingConfiguration=: Logging Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
#+begin_src matlab
|
|
function [] = initializeLoggingConfiguration(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+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
|
|
#+begin_src matlab
|
|
conf_log = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
#+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
|
|
#+begin_src matlab
|
|
conf_log.Ts = args.Ts;
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_conf_log.mat', 'file')
|
|
save('mat/nass_model_conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('mat/nass_model_conf_log.mat', 'conf_log');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_conf_log.mat', 'file')
|
|
save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_conf_log.mat', 'conf_log');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeReferences=: Generate Reference Signals
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeReferences.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function Declaration and Documentation
|
|
#+begin_src matlab
|
|
function [ref] = initializeReferences(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+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
|
|
#+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
|
|
#+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
|
|
#+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
|
|
#+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
|
|
#+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('nass_model_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 the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_references.mat', 'file')
|
|
save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
|
else
|
|
save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_references.mat', 'file')
|
|
save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeDisturbances=: 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:
|
|
|
|
*** Function Declaration and Documentation
|
|
#+begin_src matlab
|
|
function [] = initializeDisturbances(args)
|
|
% initializeDisturbances - Initialize the disturbances
|
|
%
|
|
% Syntax: [] = initializeDisturbances(args)
|
|
%
|
|
% Inputs:
|
|
% - args -
|
|
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
% Global parameter to enable or disable the disturbances
|
|
args.enable logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - X direction
|
|
args.Dw_x logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - Y direction
|
|
args.Dw_y logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - Z direction
|
|
args.Dw_z logical {mustBeNumericOrLogical} = true
|
|
% Translation Stage - X direction
|
|
args.Fdy_x logical {mustBeNumericOrLogical} = true
|
|
% Translation Stage - Z direction
|
|
args.Fdy_z logical {mustBeNumericOrLogical} = true
|
|
% Spindle - X direction
|
|
args.Frz_x logical {mustBeNumericOrLogical} = true
|
|
% Spindle - Y direction
|
|
args.Frz_y logical {mustBeNumericOrLogical} = true
|
|
% Spindle - Z direction
|
|
args.Frz_z logical {mustBeNumericOrLogical} = true
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
% Initialization of random numbers
|
|
rng("shuffle");
|
|
#+end_src
|
|
|
|
*** Ground Motion
|
|
#+begin_src matlab
|
|
%% Ground Motion
|
|
if args.enable
|
|
% Load the PSD of disturbance
|
|
load('ustation_disturbance_psd.mat', 'gm_dist')
|
|
|
|
% Frequency Data
|
|
Dw.f = gm_dist.f;
|
|
Dw.psd_x = gm_dist.pxx_x;
|
|
Dw.psd_y = gm_dist.pxx_y;
|
|
Dw.psd_z = gm_dist.pxx_z;
|
|
|
|
% Time data
|
|
Fs = 2*Dw.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(Dw.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
Dw.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
|
|
% ASD representation of the ground motion
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(Dw.psd_x(i)/T0);
|
|
end
|
|
|
|
if args.Dw_x
|
|
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)))];;
|
|
Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
|
else
|
|
Dw.x = zeros(length(Dw.t), 1);
|
|
end
|
|
|
|
if args.Dw_y
|
|
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)))];;
|
|
Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
|
else
|
|
Dw.y = zeros(length(Dw.t), 1);
|
|
end
|
|
|
|
if args.Dw_y
|
|
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)))];;
|
|
Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
|
else
|
|
Dw.z = zeros(length(Dw.t), 1);
|
|
end
|
|
|
|
else
|
|
Dw.t = [0,1]; % Time Vector [s]
|
|
Dw.x = [0,0]; % Ground Motion - X [m]
|
|
Dw.y = [0,0]; % Ground Motion - Y [m]
|
|
Dw.z = [0,0]; % Ground Motion - Z [m]
|
|
end
|
|
#+end_src
|
|
|
|
*** Translation stage
|
|
#+begin_src matlab
|
|
%% Translation stage
|
|
if args.enable
|
|
% Load the PSD of disturbance
|
|
load('ustation_disturbance_psd.mat', 'dy_dist')
|
|
|
|
% Frequency Data
|
|
Dy.f = dy_dist.f;
|
|
Dy.psd_x = dy_dist.pxx_fx;
|
|
Dy.psd_z = dy_dist.pxx_fz;
|
|
|
|
% Time data
|
|
Fs = 2*Dy.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(Dy.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
Dy.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
|
|
% ASD representation of the disturbance voice
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(Dy.psd_x(i)/T0);
|
|
end
|
|
|
|
if args.Fdy_x
|
|
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)))];;
|
|
Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N]
|
|
else
|
|
Dy.x = zeros(length(Dy.t), 1);
|
|
end
|
|
|
|
if args.Fdy_z
|
|
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)))];;
|
|
Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N]
|
|
else
|
|
Dy.z = zeros(length(Dy.t), 1);
|
|
end
|
|
|
|
else
|
|
Dy.t = [0,1]; % Time Vector [s]
|
|
Dy.x = [0,0]; % Translation Stage disturbances - X [N]
|
|
Dy.z = [0,0]; % Translation Stage disturbances - Z [N]
|
|
end
|
|
#+end_src
|
|
|
|
*** Spindle
|
|
#+begin_src matlab
|
|
%% Spindle
|
|
if args.enable
|
|
% Load the PSD of disturbance
|
|
load('ustation_disturbance_psd.mat', 'rz_dist')
|
|
|
|
% Frequency Data
|
|
Rz.f = rz_dist.f;
|
|
Rz.psd_x = rz_dist.pxx_fx;
|
|
Rz.psd_y = rz_dist.pxx_fy;
|
|
Rz.psd_z = rz_dist.pxx_fz;
|
|
|
|
% Time data
|
|
Fs = 2*Rz.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(Rz.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
Rz.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
|
|
% ASD representation of the disturbance voice
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(Rz.psd_x(i)/T0);
|
|
end
|
|
|
|
if args.Frz_x
|
|
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)))];;
|
|
Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N]
|
|
else
|
|
Rz.x = zeros(length(Rz.t), 1);
|
|
end
|
|
|
|
if args.Frz_y
|
|
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)))];;
|
|
Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N]
|
|
else
|
|
Rz.y = zeros(length(Rz.t), 1);
|
|
end
|
|
|
|
if args.Frz_z
|
|
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)))];;
|
|
Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N]
|
|
else
|
|
Rz.z = zeros(length(Rz.t), 1);
|
|
end
|
|
|
|
else
|
|
Rz.t = [0,1]; % Time Vector [s]
|
|
Rz.x = [0,0]; % Spindle disturbances - X [N]
|
|
Rz.y = [0,0]; % Spindle disturbances - X [N]
|
|
Rz.z = [0,0]; % Spindle disturbances - Z [N]
|
|
end
|
|
#+end_src
|
|
|
|
*** Direct Forces
|
|
#+begin_src matlab
|
|
u = zeros(100, 6);
|
|
Fd = u;
|
|
#+end_src
|
|
|
|
*** Set initial value to zero
|
|
#+begin_src matlab
|
|
Dw.x = Dw.x - Dw.x(1);
|
|
Dw.y = Dw.y - Dw.y(1);
|
|
Dw.z = Dw.z - Dw.z(1);
|
|
|
|
Dy.x = Dy.x - Dy.x(1);
|
|
Dy.z = Dy.z - Dy.z(1);
|
|
|
|
Rz.x = Rz.x - Rz.x(1);
|
|
Rz.y = Rz.y - Rz.y(1);
|
|
Rz.z = Rz.z - Rz.z(1);
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
|
|
elseif exist('./matlab', 'dir')
|
|
save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeController=: Initialize Controller
|
|
#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no
|
|
function [] = initializeController(args)
|
|
|
|
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
|
|
|
|
controller = struct();
|
|
|
|
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
|
|
|
|
if exist('./mat', 'dir')
|
|
save('mat/nass_model_controller.mat', 'controller');
|
|
elseif exist('./matlab', 'dir')
|
|
save('matlab/mat/nass_model_controller.mat', 'controller');
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
** =describeMicroStationSetup=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/describeMicroStationSetup.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
#+begin_src matlab
|
|
function [] = describeMicroStationSetup()
|
|
% describeMicroStationSetup -
|
|
%
|
|
% Syntax: [] = describeMicroStationSetup()
|
|
%
|
|
% Inputs:
|
|
% - -
|
|
%
|
|
% Outputs:
|
|
% - -
|
|
#+end_src
|
|
|
|
*** Simscape Configuration
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Simscape Configuration:\n');
|
|
|
|
if conf_simscape.type == 1
|
|
fprintf('- Gravity is included\n');
|
|
else
|
|
fprintf('- Gravity is not included\n');
|
|
end
|
|
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** Disturbances
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_disturbances.mat', 'args');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Disturbances:\n');
|
|
if ~args.enable
|
|
fprintf('- No disturbance is included\n');
|
|
else
|
|
if args.Dwx && args.Dwy && args.Dwz
|
|
fprintf('- Ground motion\n');
|
|
end
|
|
if args.Fdy_x && args.Fdy_z
|
|
fprintf('- Vibrations of the Translation Stage\n');
|
|
end
|
|
if args.Frz_z
|
|
fprintf('- Vibrations of the Spindle\n');
|
|
end
|
|
end
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** References
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_references.mat', 'args');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Reference Tracking:\n');
|
|
fprintf('- Translation Stage:\n');
|
|
switch args.Dy_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
case 'triangular'
|
|
fprintf(' - Triangular Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
|
case 'sinusoidal'
|
|
fprintf(' - Sinusoidal Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
|
end
|
|
|
|
fprintf('- Tilt Stage:\n');
|
|
switch args.Ry_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
case 'triangular'
|
|
fprintf(' - Triangular Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
|
case 'sinusoidal'
|
|
fprintf(' - Sinusoidal Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
|
end
|
|
|
|
fprintf('- Spindle:\n');
|
|
switch args.Rz_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
|
|
case { 'rotating', 'rotating-not-filtered' }
|
|
fprintf(' - Rotating\n');
|
|
fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period);
|
|
end
|
|
|
|
|
|
fprintf('- Micro Hexapod:\n');
|
|
switch args.Dh_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
|
|
fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
|
|
end
|
|
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** Micro-Station
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Micro Station:\n');
|
|
|
|
if granite.type == 1 && ...
|
|
ty.type == 1 && ...
|
|
ry.type == 1 && ...
|
|
rz.type == 1 && ...
|
|
micro_hexapod.type == 1;
|
|
fprintf('- All stages are rigid\n');
|
|
elseif granite.type == 2 && ...
|
|
ty.type == 2 && ...
|
|
ry.type == 2 && ...
|
|
rz.type == 2 && ...
|
|
micro_hexapod.type == 2;
|
|
fprintf('- All stages are flexible\n');
|
|
else
|
|
if granite.type == 1 || granite.type == 4
|
|
fprintf('- Granite is rigid\n');
|
|
else
|
|
fprintf('- Granite is flexible\n');
|
|
end
|
|
if ty.type == 1 || ty.type == 4
|
|
fprintf('- Translation Stage is rigid\n');
|
|
else
|
|
fprintf('- Translation Stage is flexible\n');
|
|
end
|
|
if ry.type == 1 || ry.type == 4
|
|
fprintf('- Tilt Stage is rigid\n');
|
|
else
|
|
fprintf('- Tilt Stage is flexible\n');
|
|
end
|
|
if rz.type == 1 || rz.type == 4
|
|
fprintf('- Spindle is rigid\n');
|
|
else
|
|
fprintf('- Spindle is flexible\n');
|
|
end
|
|
if micro_hexapod.type == 1 || micro_hexapod.type == 4
|
|
fprintf('- Micro Hexapod is rigid\n');
|
|
else
|
|
fprintf('- Micro Hexapod is flexible\n');
|
|
end
|
|
|
|
end
|
|
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
** =computeReferencePose=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/computeReferencePose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
|
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
|
%
|
|
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
|
%
|
|
% Inputs:
|
|
% - Dy - Reference of the Translation Stage [m]
|
|
% - Ry - Reference of the Tilt Stage [rad]
|
|
% - Rz - Reference of the Spindle [rad]
|
|
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
|
|
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
|
|
%
|
|
% Outputs:
|
|
% - WTr -
|
|
|
|
%% Translation Stage
|
|
Rty = [1 0 0 0;
|
|
0 1 0 Dy;
|
|
0 0 1 0;
|
|
0 0 0 1];
|
|
|
|
%% Tilt Stage - Pure rotating aligned with Ob
|
|
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
|
0 1 0 0;
|
|
-sin(Ry) 0 cos(Ry) 0;
|
|
0 0 0 1];
|
|
|
|
%% Spindle - Rotation along the Z axis
|
|
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
|
sin(Rz) cos(Rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
|
|
%% Micro-Hexapod
|
|
Rhx = [1 0 0;
|
|
0 cos(Dh(4)) -sin(Dh(4));
|
|
0 sin(Dh(4)) cos(Dh(4))];
|
|
|
|
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
|
0 1 0;
|
|
-sin(Dh(5)) 0 cos(Dh(5))];
|
|
|
|
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
|
sin(Dh(6)) cos(Dh(6)) 0;
|
|
0 0 1];
|
|
|
|
Rh = [1 0 0 Dh(1) ;
|
|
0 1 0 Dh(2) ;
|
|
0 0 1 Dh(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
|
|
|
|
%% Nano-Hexapod
|
|
Rnx = [1 0 0;
|
|
0 cos(Dn(4)) -sin(Dn(4));
|
|
0 sin(Dn(4)) cos(Dn(4))];
|
|
|
|
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
|
0 1 0;
|
|
-sin(Dn(5)) 0 cos(Dn(5))];
|
|
|
|
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
|
|
sin(Dn(6)) cos(Dn(6)) 0;
|
|
0 0 1];
|
|
|
|
Rn = [1 0 0 Dn(1) ;
|
|
0 1 0 Dn(2) ;
|
|
0 0 1 Dn(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
|
|
|
|
%% Total Homogeneous transformation
|
|
WTr = Rty*Rry*Rrz*Rh*Rn;
|
|
end
|
|
#+end_src
|
|
|
|
** =circlefit=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/circlefit.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
#+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
|
|
|
|
** Initialize Micro-Station Stages
|
|
*** =initializeGround=: Ground
|
|
#+begin_src matlab :tangle matlab/src/initializeGround.m :comments none :mkdirp yes :eval no
|
|
function [ground] = initializeGround(args)
|
|
|
|
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
|
|
|
|
ground = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
ground.type = 0;
|
|
case 'rigid'
|
|
ground.type = 1;
|
|
end
|
|
|
|
ground.shape = [2, 2, 0.5]; % [m]
|
|
ground.density = 2800; % [kg/m3]
|
|
|
|
ground.rot_point = args.rot_point;
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'ground', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'ground');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'ground', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'ground');
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeGranite=: Granite
|
|
#+begin_src matlab :tangle matlab/src/initializeGranite.m :comments none :mkdirp yes :eval no
|
|
function [granite] = initializeGranite(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
|
|
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m]
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [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.775 % Height of the measurment point [m]
|
|
end
|
|
|
|
granite = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
granite.type = 0;
|
|
case 'rigid'
|
|
granite.type = 1;
|
|
case 'flexible'
|
|
granite.type = 2;
|
|
end
|
|
|
|
granite.density = args.density; % [kg/m3]
|
|
granite.STEP = 'granite.STEP';
|
|
|
|
% Z-offset for the initial position of the sample with respect to the granite top surface.
|
|
granite.sample_pos = args.sample_pos; % [m]
|
|
|
|
granite.K = args.K; % [N/m]
|
|
granite.C = args.C; % [N/(m/s)]
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'granite', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'granite');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'granite', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'granite');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeTy=: Translation Stage
|
|
#+begin_src matlab :tangle matlab/src/initializeTy.m :comments none :mkdirp yes :eval no
|
|
function [ty] = initializeTy(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
end
|
|
|
|
ty = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
ty.type = 0;
|
|
case 'rigid'
|
|
ty.type = 1;
|
|
case 'flexible'
|
|
ty.type = 2;
|
|
end
|
|
|
|
% Ty Granite frame
|
|
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
|
|
ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP';
|
|
|
|
% Guide Translation Ty
|
|
ty.guide.density = 7800; % [kg/m3] => 76kg
|
|
ty.guide.STEP = 'Ty_Guide.STEP';
|
|
|
|
% Ty - Guide_Translation12
|
|
ty.guide12.density = 7800; % [kg/m3]
|
|
ty.guide12.STEP = 'Ty_Guide_12.STEP';
|
|
|
|
% Ty - Guide_Translation11
|
|
ty.guide11.density = 7800; % [kg/m3]
|
|
ty.guide11.STEP = 'Ty_Guide_11.STEP';
|
|
|
|
% Ty - Guide_Translation22
|
|
ty.guide22.density = 7800; % [kg/m3]
|
|
ty.guide22.STEP = 'Ty_Guide_22.STEP';
|
|
|
|
% Ty - Guide_Translation21
|
|
ty.guide21.density = 7800; % [kg/m3]
|
|
ty.guide21.STEP = 'Ty_Guide_21.STEP';
|
|
|
|
% Ty - Plateau translation
|
|
ty.frame.density = 7800; % [kg/m3]
|
|
ty.frame.STEP = 'Ty_Stage.STEP';
|
|
|
|
% Ty Stator Part
|
|
ty.stator.density = 5400; % [kg/m3]
|
|
ty.stator.STEP = 'Ty_Motor_Stator.STEP';
|
|
|
|
% Ty Rotor Part
|
|
ty.rotor.density = 5400; % [kg/m3]
|
|
ty.rotor.STEP = 'Ty_Motor_Rotor.STEP';
|
|
|
|
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
|
|
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'ty', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'ty');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'ty', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'ty');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeRy=: Tilt Stage
|
|
#+begin_src matlab :tangle matlab/src/initializeRy.m :comments none :mkdirp yes :eval no
|
|
function [ry] = initializeRy(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
args.Ry_init (1,1) double {mustBeNumeric} = 0
|
|
end
|
|
|
|
ry = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
ry.type = 0;
|
|
case 'rigid'
|
|
ry.type = 1;
|
|
case 'flexible'
|
|
ry.type = 2;
|
|
end
|
|
|
|
% Ry - Guide for the tilt stage
|
|
ry.guide.density = 7800; % [kg/m3]
|
|
ry.guide.STEP = 'Tilt_Guide.STEP';
|
|
|
|
% Ry - Rotor of the motor
|
|
ry.rotor.density = 2400; % [kg/m3]
|
|
ry.rotor.STEP = 'Tilt_Motor_Axis.STEP';
|
|
|
|
% Ry - Motor
|
|
ry.motor.density = 3200; % [kg/m3]
|
|
ry.motor.STEP = 'Tilt_Motor.STEP';
|
|
|
|
% Ry - Plateau Tilt
|
|
ry.stage.density = 7800; % [kg/m3]
|
|
ry.stage.STEP = 'Tilt_Stage.STEP';
|
|
|
|
% Z-Offset so that the center of rotation matches the sample center;
|
|
ry.z_offset = 0.58178; % [m]
|
|
|
|
ry.Ry_init = args.Ry_init; % [rad]
|
|
|
|
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
|
|
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'ry', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'ry');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'ry', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'ry');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeRz=: Spindle
|
|
#+begin_src matlab :tangle matlab/src/initializeRz.m :comments none :mkdirp yes :eval no
|
|
function [rz] = initializeRz(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
end
|
|
|
|
rz = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
rz.type = 0;
|
|
case 'rigid'
|
|
rz.type = 1;
|
|
case 'flexible'
|
|
rz.type = 2;
|
|
end
|
|
|
|
% Spindle - Slip Ring
|
|
rz.slipring.density = 7800; % [kg/m3]
|
|
rz.slipring.STEP = 'Spindle_Slip_Ring.STEP';
|
|
|
|
% Spindle - Rotor
|
|
rz.rotor.density = 7800; % [kg/m3]
|
|
rz.rotor.STEP = 'Spindle_Rotor.STEP';
|
|
|
|
% Spindle - Stator
|
|
rz.stator.density = 7800; % [kg/m3]
|
|
rz.stator.STEP = 'Spindle_Stator.STEP';
|
|
|
|
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
|
|
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'rz', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'rz');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'rz', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'rz');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeMicroHexapod=: Micro Hexapod
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeMicroHexapod.m :comments none :mkdirp yes :eval no
|
|
function [micro_hexapod] = initializeMicroHexapod(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = '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 (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e7
|
|
args.Ci (1,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3
|
|
% 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)
|
|
end
|
|
|
|
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);
|
|
|
|
stewart = initializeStrutDynamics(stewart, ...
|
|
'k', args.Ki, ...
|
|
'c', args.Ci);
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
'type_F', '2dof', ...
|
|
'type_M', '3dof');
|
|
|
|
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);
|
|
|
|
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
|
|
|
switch args.type
|
|
case 'none'
|
|
stewart.type = 0;
|
|
case 'rigid'
|
|
stewart.type = 1;
|
|
case 'flexible'
|
|
stewart.type = 2;
|
|
end
|
|
|
|
micro_hexapod = stewart;
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'micro_hexapod');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeSimplifiedNanoHexapod=: Nano Hexapod
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeSimplifiedNanoHexapod.m :comments none :mkdirp yes :eval no
|
|
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'stewart'})} = 'stewart'
|
|
%% initializeFramesPositions
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
|
|
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
|
%% generateGeneralConfiguration
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of fixed joints [m]
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
|
|
args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of mobile joints [m]
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
|
|
args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
|
|
%% Actuators
|
|
args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
|
args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6
|
|
args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e4
|
|
args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605
|
|
args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302
|
|
args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50
|
|
args.actuator_cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100
|
|
args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50
|
|
%% initializeCylindricalPlatforms
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
|
|
%% initializeCylindricalStruts
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
|
%% Bottom and Top Flexible Joints
|
|
args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
|
args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
|
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
%% inverseKinematics
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
stewart = initializeStewartPlatform();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
stewart.type = 0;
|
|
case 'stewart'
|
|
stewart.type = 1;
|
|
end
|
|
|
|
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);
|
|
|
|
stewart = initializeStrutDynamics(stewart, ...
|
|
'type', args.actuator_type, ...
|
|
'k', args.actuator_k, ...
|
|
'kp', args.actuator_kp, ...
|
|
'ke', args.actuator_ke, ...
|
|
'ka', args.actuator_ka, ...
|
|
'c', args.actuator_c, ...
|
|
'cp', args.actuator_cp, ...
|
|
'ce', args.actuator_ce, ...
|
|
'ca', args.actuator_ca);
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
'type_F', args.flex_type_F, ...
|
|
'type_M', args.flex_type_M, ...
|
|
'Kf_M', args.Kf_M, ...
|
|
'Cf_M', args.Cf_M, ...
|
|
'Kt_M', args.Kt_M, ...
|
|
'Ct_M', args.Ct_M, ...
|
|
'Kf_F', args.Kf_F, ...
|
|
'Cf_F', args.Cf_F, ...
|
|
'Kt_F', args.Kt_F, ...
|
|
'Ct_F', args.Ct_F, ...
|
|
'Ka_F', args.Ka_F, ...
|
|
'Ca_F', args.Ca_F, ...
|
|
'Kr_F', args.Kr_F, ...
|
|
'Cr_F', args.Cr_F, ...
|
|
'Ka_M', args.Ka_M, ...
|
|
'Ca_M', args.Ca_M, ...
|
|
'Kr_M', args.Kr_M, ...
|
|
'Cr_M', args.Cr_M);
|
|
|
|
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);
|
|
|
|
nano_hexapod = stewart;
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'nano_hexapod', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'nano_hexapod');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'nano_hexapod');
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeSample=: Sample
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no
|
|
function [sample] = initializeSample(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none'
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 250e-3 % Height [m]
|
|
args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m]
|
|
args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg]
|
|
end
|
|
|
|
sample = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
sample.type = 0;
|
|
sample.m = 0;
|
|
case 'cylindrical'
|
|
sample.type = 1;
|
|
|
|
sample.H = args.H;
|
|
sample.R = args.R;
|
|
sample.m = args.m;
|
|
end
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'sample', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'sample');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'sample', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'sample');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
** Initialize Nano-Hexapod
|
|
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeStewartPlatform.m :comments none :mkdirp yes :eval no
|
|
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 -
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeFramesPositions.m :comments none :mkdirp yes :eval no
|
|
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]
|
|
|
|
arguments
|
|
stewart
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
|
end
|
|
|
|
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]
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =generateGeneralConfiguration=: Generate a Very General Configuration
|
|
|
|
#+begin_src matlab :tangle matlab/src/generateGeneralConfiguration.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
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
|
|
|
|
Fa = zeros(3,6);
|
|
Mb = zeros(3,6);
|
|
|
|
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
|
|
|
|
stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =computeJointsPose=: Compute the Pose of the Joints
|
|
|
|
#+begin_src matlab :tangle matlab/src/computeJointsPose.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
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;
|
|
|
|
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]);
|
|
|
|
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
|
|
|
l = vecnorm(Ab - Aa)';
|
|
|
|
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
|
|
|
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
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeCylindricalPlatforms.m :comments none :mkdirp yes :eval no
|
|
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]
|
|
|
|
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
|
|
|
|
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]);
|
|
|
|
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]);
|
|
|
|
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;
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeCylindricalStruts.m :comments none :mkdirp yes :eval no
|
|
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]
|
|
|
|
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
|
|
|
|
stewart.struts_M.type = 1;
|
|
|
|
%% Compute the properties of the cylindrical struts
|
|
Fsm = args.Fsm;
|
|
Fsh = args.Fsh;
|
|
Fsr = args.Fsr;
|
|
|
|
Msm = args.Msm;
|
|
Msh = args.Msh;
|
|
Msr = args.Msr;
|
|
|
|
I_F = [1/12 * Fsm * (3*Fsr^2 + Fsh^2), ...
|
|
1/12 * Fsm * (3*Fsr^2 + Fsh^2), ...
|
|
1/2 * Fsm * Fsr^2];
|
|
|
|
I_M = [1/12 * Msm * (3*Msr^2 + Msh^2), ...
|
|
1/12 * Msm * (3*Msr^2 + Msh^2), ...
|
|
1/2 * Msm * Msr^2];
|
|
stewart.struts_M.I = I_M;
|
|
stewart.struts_F.I = I_F;
|
|
|
|
stewart.struts_M.M = args.Msm;
|
|
stewart.struts_M.R = args.Msr;
|
|
stewart.struts_M.H = args.Msh;
|
|
|
|
stewart.struts_F.type = 1;
|
|
|
|
stewart.struts_F.M = args.Fsm;
|
|
stewart.struts_F.R = args.Fsr;
|
|
stewart.struts_F.H = args.Fsh;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeStrutDynamics.m :comments none :mkdirp yes :eval no
|
|
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)]
|
|
|
|
arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
|
args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
|
|
args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
|
|
args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
|
|
args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
|
|
args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6
|
|
args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
|
|
|
|
args.F_gain (1,1) double {mustBeNumeric} = 1
|
|
args.me (1,1) double {mustBeNumeric} = 0.01
|
|
args.ma (1,1) double {mustBeNumeric} = 0.01
|
|
end
|
|
|
|
if strcmp(args.type, '1dof')
|
|
stewart.actuators.type = 1;
|
|
elseif strcmp(args.type, '2dof')
|
|
stewart.actuators.type = 2;
|
|
elseif strcmp(args.type, 'flexible')
|
|
stewart.actuators.type = 3;
|
|
end
|
|
|
|
stewart.actuators.k = args.k;
|
|
stewart.actuators.c = args.c;
|
|
|
|
% Parallel stiffness
|
|
stewart.actuators.kp = args.kp;
|
|
stewart.actuators.cp = args.cp;
|
|
|
|
stewart.actuators.ka = args.ka;
|
|
stewart.actuators.ca = args.ca;
|
|
|
|
stewart.actuators.ke = args.ke;
|
|
stewart.actuators.ce = args.ce;
|
|
|
|
stewart.actuators.F_gain = args.F_gain;
|
|
|
|
stewart.actuators.ma = args.ma;
|
|
stewart.actuators.me = args.me;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no
|
|
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)]
|
|
|
|
arguments
|
|
stewart
|
|
args.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
|
args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
|
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
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
|
|
|
|
switch args.type_F
|
|
case '2dof'
|
|
stewart.joints_F.type = 1;
|
|
case '3dof'
|
|
stewart.joints_F.type = 2;
|
|
case '4dof'
|
|
stewart.joints_F.type = 3;
|
|
case '6dof'
|
|
stewart.joints_F.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_F.type = 5;
|
|
otherwise
|
|
error("joints_F are not correctly defined")
|
|
end
|
|
|
|
switch args.type_M
|
|
case '2dof'
|
|
stewart.joints_M.type = 1;
|
|
case '3dof'
|
|
stewart.joints_M.type = 2;
|
|
case '4dof'
|
|
stewart.joints_M.type = 3;
|
|
case '6dof'
|
|
stewart.joints_M.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_M.type = 5;
|
|
otherwise
|
|
error("joints_M are not correctly defined")
|
|
end
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeStewartPose.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
|
|
stewart.actuators.Leq = dLi;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =computeJacobian=: Compute the Jacobian Matrix
|
|
|
|
#+begin_src matlab :tangle matlab/src/computeJacobian.m :comments none :mkdirp yes :eval no
|
|
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:
|
|
% - geometry.J [6x6] - The Jacobian Matrix
|
|
% - geometry.K [6x6] - The Stiffness Matrix
|
|
% - geometry.C [6x6] - The Compliance Matrix
|
|
|
|
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;
|
|
|
|
J = [As' , cross(Ab, As)'];
|
|
|
|
K = J'*diag(Ki)*J;
|
|
|
|
C = inv(K);
|
|
|
|
stewart.geometry.J = J;
|
|
stewart.geometry.K = K;
|
|
stewart.geometry.C = C;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =inverseKinematics=: Compute Inverse Kinematics
|
|
|
|
#+begin_src matlab :tangle matlab/src/inverseKinematics.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
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;
|
|
|
|
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));
|
|
|
|
dLi = Li-l;
|
|
|
|
end
|
|
#+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:../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:../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 modelled as 1DoF.\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))
|
|
if stewart.actuators.kp > 0
|
|
fprintf('\t Added parallel stiffness: kp = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.kp(1))
|
|
end
|
|
elseif stewart.actuators.type == 2
|
|
fprintf('- The actuators are modelled as 2DoF (APA).\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))
|
|
elseif stewart.actuators.type == 3
|
|
fprintf('- The actuators are modelled with a flexible element (FEM).\n')
|
|
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 (2DoF)\n')
|
|
case 2
|
|
fprintf('- The joints on the fixed based are spherical joints (3DoF)\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 (2DoF)\n')
|
|
case 2
|
|
fprintf('- The joints on the mobile based are spherical joints (3DoF)\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
|
|
|