3871 lines
119 KiB
Org Mode
3871 lines
119 KiB
Org Mode
#+TITLE: Simscape Model - Micro Station
|
|
:DRAWER:
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ../index.html
|
|
#+HTML_LINK_UP: ../index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
|
|
#+BIND: org-latex-image-default-option "scale=1"
|
|
#+BIND: org-latex-image-default-width ""
|
|
|
|
#+LaTeX_CLASS: scrreprt
|
|
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
|
|
#+LATEX_HEADER: \input{preamble.tex}
|
|
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
|
|
#+LATEX_HEADER_EXTRA: \bibliography{simscape-micro-station.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")
|
|
|
|
#+latex: \clearpage
|
|
:END:
|
|
|
|
* 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 =ustation=
|
|
|
|
From modal analysis: validation of the multi-body model.
|
|
*Goals*:
|
|
- *Modelling of the micro-station*: Kinematics + Dynamics + Disturbances
|
|
- Kinematics of each stage
|
|
- Modelling: solid bodies + joints. Show what is used for each stage
|
|
- Correlation with the dynamical measurements
|
|
- Inclusion of disturbances (correlation with measurements)
|
|
*Notes*:
|
|
- Do not talk about Nano-Hexapod yet
|
|
- Do not talk about external metrology
|
|
- Therefore, do not talk about computation of the sample position / error
|
|
|
|
Based on:
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org][kinematics]]: compute sample's motion from each stage motion
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/simscape_subsystems.org][simscape_subsystems]]: general presentation of the micro-station. Used model: solid body + joints. Presentation of each stage.
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/documents/work-package-1/work-package-1.org::*Specification of requirements][Specification of requirements]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/identification.org][identification]]: comparison of measurements and simscape model (not so good?)
|
|
- [ ] file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/experiments.org][experiments]]: simulation of experiments with the Simscape model
|
|
- [ ] Disturbances: Similar to what was done for the uniaxial model (the same?)
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/disturbances.org::+TITLE: Identification of the disturbances]]
|
|
- [ ] Measurement of disturbances / things that will have to be corrected using the NASS:
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-to-dynamic/index.org]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-control-system/index.org]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-sr-rz/index.org]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/ground-motion/index.org]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-spindle/index.org]]
|
|
- [ ] Check [[file:~/Cloud/work-projects/ID31-NASS/specifications/id-31-spindle-meas][this directory]] and [[file:~/Cloud/work-projects/ID31-NASS/specifications/stage-by-stage][this directory]]
|
|
|
|
** DONE [#B] Put colors for each different stage
|
|
CLOSED: [2024-10-30 Wed 14:07]
|
|
|
|
** DONE [#B] Delete Gravity compensation Stage
|
|
CLOSED: [2024-10-30 Wed 13:51]
|
|
|
|
** DONE [#B] Maybe make a simpler Simscape model for this report
|
|
CLOSED: [2024-10-30 Wed 13:51]
|
|
|
|
** DONE [#A] Open the Simscape model and verify it all works
|
|
CLOSED: [2024-10-30 Wed 13:33] SCHEDULED: <2024-10-30 Wed>
|
|
|
|
#+begin_src matlab
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
initializeGround( 'type', 'rigid');
|
|
initializeGranite( 'type', 'modal-analysis');
|
|
initializeTy( 'type', 'modal-analysis');
|
|
initializeRy( 'type', 'modal-analysis');
|
|
initializeRz( 'type', 'modal-analysis');
|
|
initializeMicroHexapod('type', 'modal-analysis');
|
|
|
|
initializeReferences();
|
|
initializeDisturbances('enable', false);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
|
|
% initializeAxisc( 'type', 'flexible');
|
|
|
|
% initializeMirror( 'type', 'none');
|
|
% initializeNanoHexapod( 'type', 'none');
|
|
% initializeSample( 'type', 'none');
|
|
|
|
initializeController( 'type', 'open-loop');
|
|
|
|
#+end_src
|
|
|
|
** TODO [#B] Make good "init" for the Simscape model
|
|
|
|
** TODO [#A] Verify that we get "correct" compliance
|
|
SCHEDULED: <2024-10-30 Wed>
|
|
|
|
** TODO [#B] Just keep smallest number of variants for each stage
|
|
|
|
- [ ] rigid
|
|
- [ ] flexible
|
|
- [X] Init => Removed
|
|
|
|
* Introduction :ignore:
|
|
|
|
|
|
#+name: tab:ustation_section_matlab_code
|
|
#+caption: Report sections and corresponding Matlab files
|
|
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Sections* | *Matlab File* |
|
|
|---------------------------------------------+-------------------------------------|
|
|
| Section ref:sec: | =ustation_1_.m= |
|
|
|
|
|
|
* Micro-Station Kinematics
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/.m
|
|
:END:
|
|
<<sec:ustation_kinematics>>
|
|
** Introduction :ignore:
|
|
|
|
[[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org]]
|
|
|
|
# - Small overview of each stage and associated stiffnesses / inertia
|
|
# - schematic that shows to considered DoF
|
|
# - import from CAD
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% .m
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Motion Stages
|
|
*** Translation Stage
|
|
*** Tilt Stage
|
|
*** Spindle
|
|
*** Micro-Hexapod
|
|
** Specification for each stage
|
|
<<ssec:spec_stage>>
|
|
|
|
For each stage, after a quick presentation, the specifications (stroke, precision, ...) of the stage, the metrology used with that stage and the parasitic motions associated to it are given.
|
|
|
|
*** Translation along Y
|
|
**** Presentation
|
|
This is the first stage, it is fixed directly on the granite.
|
|
In order to minimize the parasitic motions, two cylinders are used to guide the stage.
|
|
The motor used to drive the stage is one linear motor.
|
|
The 3D model of this translation stage is shown figure ref:fig:stage_translation.
|
|
|
|
#+name: fig:stage_translation
|
|
#+caption: Translation stage
|
|
[[./figures/stages/stage_translation.pdf]]
|
|
|
|
**** Specifications
|
|
#+attr_latex: :align |c|c|c|c|c| :placement [!htpb]
|
|
#+CAPTION: Specifications for the translation stage along Y
|
|
#+NAME: fig:spec-t-y
|
|
|--------+------------------------+--------------------------+--------------+--------------|
|
|
| Motion | Mode | Stroke | Repetability | MIM[fn:3] |
|
|
|--------+------------------------+--------------------------+--------------+--------------|
|
|
| $T_y$ | P[fn:1]/S[fn:2] | $\pm5 mm$ | $0.04 \mu m$ | $0.02 \mu m$ |
|
|
| | rotation stage along Y | ($\pm10 mm$ if possible) | | |
|
|
|--------+------------------------+--------------------------+--------------+--------------|
|
|
|
|
**** Metrology
|
|
A linear digital encoder is used to measure the displacement of the translation stage along the y axis.
|
|
|
|
**** Parasitic motions
|
|
#+attr_latex: :align |c|c|c| :placement [!htpb]
|
|
#+CAPTION: Parasitic motions for the translation stage along Y
|
|
#+NAME: fig:parasitic-motions-t-y
|
|
|--------------------+-----------------------+------------------------------|
|
|
| Axial Motion ($y$) | Radial Motion ($y-z$) | Rotation motion ($\theta-z$) |
|
|
|--------------------+-----------------------+------------------------------|
|
|
| $10nm$ | $20nm$ | $< 1.7 \mu rad$ |
|
|
|--------------------+-----------------------+------------------------------|
|
|
|
|
*** Rotation around Y
|
|
**** Presentation
|
|
This stage is mainly use for the reflectivity experiment. This permit to make the sample rotates around the y axis.
|
|
The rotation axis should be parallel to the y-axis and cross the X-ray.
|
|
|
|
As it is located below the Spindle, it make the axis of rotation of the spindle to also tilt with respect to the X-ray.
|
|
|
|
4 joints are used in order to pre-stress the system.
|
|
|
|
The model of the stage is shown figure ref:fig:stage_tilt.
|
|
|
|
#+name: fig:stage_tilt
|
|
#+caption: Tilt Stage
|
|
[[./figures/stages/stage_tilt.pdf]]
|
|
|
|
**** Specifications
|
|
#+attr_latex: :align |c|c|c|c|c| :placement [!htpb]
|
|
#+CAPTION: Specifications for the rotation stage along Y
|
|
#+NAME: fig:spec-r-y
|
|
|------------+------+----------------------+--------------+-------------|
|
|
| Motion | Mode | Stroke | Repetability | MIM |
|
|
|------------+------+----------------------+--------------+-------------|
|
|
| $\theta_y$ | S | $\pm51 mrad$ | $5 \mu rad$ | $2 \mu rad$ |
|
|
| | | Speed: $\pm 3^o/min$ | | |
|
|
|------------+------+----------------------+--------------+-------------|
|
|
|
|
**** Metrology
|
|
Two linear digital encoders are used (one on each side).
|
|
|
|
**** Parasitic motions
|
|
#+attr_latex: :align |c|c|c| :placement [!htpb]
|
|
#+CAPTION: Parasitic motions for the rotation stage along Y
|
|
#+NAME: fig:parasitic-motions-r-y
|
|
|-------------------+--------------------+---------------|
|
|
| Axial Error ($y$) | Radial Error ($z$) | Tilt error () |
|
|
|-------------------+--------------------+---------------|
|
|
| $0.5\mu m$ | $10nm$ | $1.5 \mu rad$ |
|
|
|-------------------+--------------------+---------------|
|
|
|
|
*** Spindle
|
|
**** Presentation
|
|
The Spindle consist of one stator (attached to the tilt stage) and one rotor.
|
|
|
|
The rotor is separated from the stator thanks to an air bearing.
|
|
|
|
The spindle is represented figure ref:fig:stage_spindle.
|
|
|
|
It has been developed by Lab-Leuven[fn:5].
|
|
|
|
#+name: fig:stage_spindle
|
|
#+caption: Spindle
|
|
[[./figures/stages/stage_spindle.pdf]]
|
|
|
|
**** Specifications
|
|
#+attr_latex: :align |c|c|c|c|c| :placement [!htpb]
|
|
#+CAPTION: Specifications for the Spindle
|
|
#+NAME: fig:spec-r-y
|
|
|------------+------+----------------------+--------------+-------------|
|
|
| Motion | Mode | Stroke | Repetability | MIM |
|
|
|------------+------+----------------------+--------------+-------------|
|
|
| $\theta_z$ | S | $\pm51 mrad$ | $5 \mu rad$ | $2 \mu rad$ |
|
|
| | | Speed: $\pm 3^o/min$ | | |
|
|
|------------+------+----------------------+--------------+-------------|
|
|
|
|
**** Metrology
|
|
There is an incremental rotation encoder.
|
|
|
|
**** Parasitic motions
|
|
#+attr_latex: :align |c|c| :placement [!htpb]
|
|
#+CAPTION: Parasitic motions for the Spindle
|
|
#+NAME: fig:parasitic-motions-r-z
|
|
|------------------------+----------------------|
|
|
| Radial Error ($x$-$y$) | Vertical Error ($z$) |
|
|
|------------------------+----------------------|
|
|
| $0.33\mu m$ | $0.07\mu m$ |
|
|
|------------------------+----------------------|
|
|
|
|
More complete measurements have been conducted at the PEL[fn:7].
|
|
|
|
*** Long Stroke Hexapod
|
|
**** Presentation
|
|
The long stroke hexapod consists of 6 legs, each composed of:
|
|
- one linear actuators
|
|
- one limit switch
|
|
- one absolute linear encoder
|
|
- one ball joint at each side, one is fixed on the fixed platform, the other on the mobile platform
|
|
|
|
This long stroke hexapod has been developed by Symetrie[fn:4] and is shown figure ref:fig:stage_hexapod.
|
|
|
|
#+name: fig:stage_hexapod
|
|
#+caption: Hexapod Symetrie
|
|
[[./figures/stages/stage_hexapod.pdf]]
|
|
|
|
**** Specifications
|
|
#+attr_latex: :align |c|c|c|c|c| :placement [!htpb]
|
|
#+CAPTION: Specifications for the long stroke hexapod
|
|
#+NAME: fig:spec-hexa
|
|
|------------------+--------------+----------------+--------------+---------------|
|
|
| Motion | Stroke | Repetability | MIM | Stiffness |
|
|
|------------------+--------------+----------------+--------------+---------------|
|
|
| $T_{\mu_x}$ | $\pm10mm$ | $\pm1\mu m$ | $0.5\mu m$ | $>12N/\mu m$ |
|
|
| $T_{\mu_y}$ | $\pm10mm$ | $\pm1\mu m$ | $0.5\mu m$ | $>12N/\mu m$ |
|
|
| $T_{\mu_z}$ | $\pm10mm$ | $\pm1\mu m$ | $0.5\mu m$ | $>135N/\mu m$ |
|
|
| $\theta_{\mu_x}$ | $\pm3 deg$ | $\pm5 \mu rad$ | $2.5\mu rad$ | |
|
|
| $\theta_{\mu_y}$ | $\pm3 deg$ | $\pm5 \mu rad$ | $2.5\mu rad$ | |
|
|
| $\theta_{\mu_z}$ | $\pm0.5 deg$ | $\pm5 \mu rad$ | $2.5\mu rad$ | |
|
|
|------------------+--------------+----------------+--------------+---------------|
|
|
|
|
**** Metrology
|
|
The metrology consist of one absolute linear encoder in each leg.
|
|
|
|
**** Parasitic motions
|
|
#+attr_latex: :align |c|c|c| :placement [!htpb]
|
|
#+CAPTION: Parasitic motions for the Spindle
|
|
#+NAME: fig:parasitic-motions-r-z
|
|
|------------------+--------------+----------------|
|
|
| Movement | Resolution | Repeatability |
|
|
|------------------+--------------+----------------|
|
|
| $T_{\mu_x}$ | $0.5\mu m$ | $\pm 1\mu m$ |
|
|
| $T_{\mu_y}$ | $0.5\mu m$ | $\pm 1\mu m$ |
|
|
| $T_{\mu_z}$ | $0.1\mu m$ | $\pm 1\mu m$ |
|
|
| $\theta_{\mu_x}$ | $2.5\mu rad$ | $\pm 4\mu rad$ |
|
|
| $\theta_{\mu_y}$ | $2.5\mu rad$ | $\pm 4\mu rad$ |
|
|
|------------------+--------------+----------------|
|
|
*** Short Stroke Hexapod
|
|
**** Presentation
|
|
This last stage is located just below the sample to study.
|
|
This is the only stage that is not yet build nor designed.
|
|
|
|
**** Specifications
|
|
#+attr_latex: :align |c|c|c|c|c| :placement [!htpb]
|
|
#+CAPTION: Specifications for the nano-stage
|
|
#+NAME: fig:spec-nano
|
|
|------------------+----------------+--------------+-------|
|
|
| Motion | Stroke | Repetability | MIM |
|
|
|------------------+----------------+--------------+-------|
|
|
| $T_{\nu_x}$ | $\pm10\mu m$ | $10nm$ | $3nm$ |
|
|
| $T_{\nu_y}$ | $\pm10\mu m$ | $10nm$ | $3nm$ |
|
|
| $T_{\nu_z}$ | $\pm10\mu m$ | $10nm$ | $3nm$ |
|
|
| $\theta_{\nu_x}$ | $\pm10\mu rad$ | $1.7\mu rad$ | |
|
|
| $\theta_{\nu_y}$ | $\pm10\mu rad$ | $1.7\mu rad$ | |
|
|
| $\theta_{\nu_z}$ | $\pm10\mu rad$ | | |
|
|
|------------------+----------------+--------------+-------|
|
|
|
|
**** Dimensions
|
|
- Bottom plate :
|
|
- Inner diameter: $>150mm$
|
|
- External diameter: $<305mm$
|
|
- Top plate
|
|
- External diameter: $<300mm$
|
|
- Height: $90mm$
|
|
- Location of the rotation point: Centered, at $175mm$ above the top platform
|
|
|
|
** Compute sample's motion from stage motion
|
|
*** Introduction :ignore:
|
|
#+begin_quote
|
|
Rx = [1 0 0;
|
|
0 cos(t) -sin(t);
|
|
0 sin(t) cos(t)];
|
|
|
|
Ry = [ cos(t) 0 sin(t);
|
|
0 1 0;
|
|
-sin(t) 0 cos(t)];
|
|
|
|
Rz = [cos(t) -sin(t) 0;
|
|
sin(t) cos(t) 0;
|
|
0 0 1];
|
|
#+end_quote
|
|
|
|
Let's define the following frames:
|
|
- $\{W\}$ the frame that is *fixed to the granite* and its origin at the theoretical meeting point between the X-ray and the spindle axis.
|
|
- $\{S\}$ the frame *attached to the sample* (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
|
|
Its origin is $O_S$.
|
|
- $\{T\}$ the theoretical wanted frame that correspond to the wanted pose of the frame $\{S\}$.
|
|
$\{T\}$ is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
|
|
The origin of $T$ is $O_T$ and is the wanted position of the sample.
|
|
|
|
Thus:
|
|
- the *measurement* of the position of the sample corresponds to ${}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T$ in translation and to $\theta_m {}^W\bm{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T$ in rotations
|
|
- the *wanted position* of the sample expressed w.r.t. the granite is ${}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T$ in translation and to $\theta_r {}^W\bm{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T$ in rotations
|
|
|
|
*** Wanted Position of the Sample with respect to the Granite
|
|
Let's define the wanted position of each stage.
|
|
#+begin_src matlab
|
|
Ty = 0; % [m]
|
|
Ry = 3*pi/180; % [rad]
|
|
Rz = 180*pi/180; % [rad]
|
|
|
|
% Hexapod (first consider only translations)
|
|
Thx = 0; % [m]
|
|
Thy = 0; % [m]
|
|
Thz = 0; % [m]
|
|
#+end_src
|
|
|
|
Now, we compute the corresponding wanted translation and rotation of the sample with respect to the granite frame $\{W\}$.
|
|
This corresponds to ${}^WO_T$ and $\theta_m {}^Ws_m$.
|
|
|
|
To do so, we have to define the homogeneous transformation for each stage.
|
|
#+begin_src matlab
|
|
% Translation Stage
|
|
Rty = [1 0 0 0;
|
|
0 1 0 Ty;
|
|
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 (only rotations first)
|
|
Rh = [1 0 0 Thx ;
|
|
0 1 0 Thy ;
|
|
0 0 1 Thz ;
|
|
0 0 0 1 ];
|
|
#+end_src
|
|
|
|
We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
|
|
#+begin_src matlab
|
|
Ttot = Rty*Rry*Rrz*Rh;
|
|
#+end_src
|
|
|
|
Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
|
|
|
|
Translation.
|
|
#+begin_src matlab
|
|
WOr = Ttot*[0;0;0;1];
|
|
WOr = WOr(1:3);
|
|
#+end_src
|
|
|
|
Rotation.
|
|
#+begin_src matlab
|
|
thetar = acos((trace(Ttot(1:3, 1:3))-1)/2)
|
|
if thetar == 0
|
|
WSr = [0; 0; 0];
|
|
else
|
|
[V, D] = eig(Ttot(1:3, 1:3));
|
|
WSr = thetar*V(:, abs(diag(D) - 1) < eps(1));
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
WPr = [WOr ; WSr];
|
|
#+end_src
|
|
|
|
*** Measured Position of the Sample with respect to the Granite
|
|
The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
|
|
#+begin_src matlab
|
|
% Measurements: Xm, Ym, Zm, Rx, Ry, Rz
|
|
Dxm = 0; % [m]
|
|
Dym = 0; % [m]
|
|
Dzm = 0; % [m]
|
|
|
|
Rxm = 0*pi/180; % [rad]
|
|
Rym = 0*pi/180; % [rad]
|
|
Rzm = 180*pi/180; % [rad]
|
|
#+end_src
|
|
|
|
Let's compute the corresponding orientation using screw axis.
|
|
#+begin_src matlab
|
|
Trxm = [1 0 0;
|
|
0 cos(Rxm) -sin(Rxm);
|
|
0 sin(Rxm) cos(Rxm)];
|
|
Trym = [ cos(Rym) 0 sin(Rym);
|
|
0 1 0;
|
|
-sin(Rym) 0 cos(Rym)];
|
|
Trzm = [cos(Rzm) -sin(Rzm) 0;
|
|
sin(Rzm) cos(Rzm) 0;
|
|
0 0 1];
|
|
|
|
STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
|
|
#+end_src
|
|
|
|
We then obtain the orientation measurement in the form of screw coordinate $\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T$ where:
|
|
- $\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}$
|
|
- ${}^W\bm{s}_m$ is the eigen vector of the rotation matrix $R$ corresponding to the eigen value $\lambda = 1$
|
|
|
|
#+begin_src matlab
|
|
thetam = acos((trace(STw(1:3, 1:3))-1)/2); % [rad]
|
|
if thetam == 0
|
|
WSm = [0; 0; 0];
|
|
else
|
|
[V, D] = eig(STw(1:3, 1:3));
|
|
WSm = thetam*V(:, abs(diag(D) - 1) < eps(1));
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
WPm = [Dxm ; Dym ; Dzm ; WSm];
|
|
#+end_src
|
|
|
|
*** Positioning Error with respect to the Granite
|
|
The wanted position expressed with respect to the granite is ${}^WO_T$ and the measured position with respect to the granite is ${}^WO_S$, thus the *position error* expressed in $\{W\}$ is
|
|
\[ {}^W E = {}^W O_T - {}^W O_S \]
|
|
The same is true for rotations:
|
|
\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]
|
|
|
|
#+begin_src matlab
|
|
WPe = WPr - WPm;
|
|
#+end_src
|
|
|
|
#+begin_quote
|
|
Now we want to express this error in a frame attached to the *base of the nano-hexapod* with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).
|
|
|
|
Or maybe should we want to express this error with respect to the *top platform of the nano-hexapod*?
|
|
We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
|
|
We could compute the position of the bottom platform in two ways:
|
|
- from the encoders of each stage
|
|
- from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)
|
|
|
|
A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
|
|
#+end_quote
|
|
|
|
*** Position Error Expressed in the Nano-Hexapod Frame
|
|
We now want the position error to be expressed in $\{S\}$ (the frame attach to the sample) for control:
|
|
\[ {}^S E = {}^S T_W \cdot {}^W E \]
|
|
|
|
Thus we need to compute the homogeneous transformation ${}^ST_W$.
|
|
Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
|
|
#+begin_src matlab
|
|
Trxm = [1 0 0;
|
|
0 cos(Rxm) -sin(Rxm);
|
|
0 sin(Rxm) cos(Rxm)];
|
|
Trym = [ cos(Rym) 0 sin(Rym);
|
|
0 1 0;
|
|
-sin(Rym) 0 cos(Rym)];
|
|
Trzm = [cos(Rzm) -sin(Rzm) 0;
|
|
sin(Rzm) cos(Rzm) 0;
|
|
0 0 1];
|
|
|
|
STw = [[ Trym*Trxm*Trzm , [Dxm; Dym; Dzm]]; 0 0 0 1];
|
|
#+end_src
|
|
|
|
Translation Error.
|
|
#+begin_src matlab
|
|
SEm = STw * [WPe(1:3); 0];
|
|
SEm = SEm(1:3);
|
|
#+end_src
|
|
|
|
Rotation Error.
|
|
#+begin_src matlab
|
|
SEr = STw * [WPe(4:6); 0];
|
|
SEr = SEr(1:3);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Etot = [SEm ; SEr]
|
|
#+end_src
|
|
*** Another try
|
|
Let's denote:
|
|
- $\{W\}$ the initial fixed frame
|
|
- $\{R\}$ the reference frame corresponding to the wanted pose of the sample
|
|
- $\{M\}$ the frame corresponding to the measured pose of the sample
|
|
|
|
We have then computed:
|
|
- ${}^WT_R$
|
|
- ${}^WT_M$
|
|
|
|
We have:
|
|
\begin{align}
|
|
{}^MT_R &= {}^MT_W {}^WT_R \\
|
|
&= {}^WT_M^t {}^WT_R
|
|
\end{align}
|
|
|
|
#+begin_src matlab
|
|
MTr = STw'*Ttot;
|
|
#+end_src
|
|
|
|
Position error:
|
|
#+begin_src matlab
|
|
MTr(1:3, 1:4)*[0; 0; 0; 1]
|
|
#+end_src
|
|
|
|
Orientation error:
|
|
#+begin_src matlab
|
|
MTr(1:3, 1:3)
|
|
#+end_src
|
|
|
|
*** Verification
|
|
How can we verify that the computation is correct?
|
|
Options:
|
|
- Test with simscape multi-body
|
|
- Impose motion on each stage
|
|
- Measure the position error w.r.t. the NASS
|
|
- Compare with the computation
|
|
|
|
* Stage Modeling
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/.m
|
|
:END:
|
|
<<sec:ustation_kinematics>>
|
|
** Introduction :ignore:
|
|
|
|
The goal here is to tune the Simscape model of the station in order to have a good dynamical representation of the real system.
|
|
|
|
In order to do so, we reproduce the Modal Analysis done on the station using the Simscape model.
|
|
|
|
We can then compare the measured Frequency Response Functions with the identified dynamics of the model.
|
|
|
|
Finally, this should help to tune the parameters of the model such that the dynamics is closer to the measured FRF.
|
|
|
|
# Validation of the Model
|
|
# - Most important metric: support compliance
|
|
# - Compare model and measurement
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% .m
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Some notes about the Simscape Model
|
|
|
|
The Simscape Model of the micro-station consists of several solid bodies:
|
|
- Bottom Granite
|
|
- Top Granite
|
|
- Translation Stage
|
|
- Tilt Stage
|
|
- Spindle
|
|
- Hexapod
|
|
|
|
Each solid body has some characteristics: Center of Mass, mass, moment of inertia, etc...
|
|
These parameters are automatically computed from the geometry and from the density of the materials.
|
|
|
|
Then, the solid bodies are connected with springs and dampers.
|
|
Some of the springs and dampers values can be estimated from the joints/stages specifications, however, we here prefer to tune these values based on the measurements.
|
|
|
|
** Compare with measurements at the CoM of each element
|
|
#+begin_src matlab
|
|
%% We load the configuration.
|
|
load('conf_simulink.mat');
|
|
|
|
% We set a small =StopTime=.
|
|
set_param(conf_simulink, 'StopTime', '0.5');
|
|
|
|
%% We initialize all the stages.
|
|
initializeGround( 'type', 'rigid');
|
|
initializeGranite( 'type', 'modal-analysis');
|
|
initializeTy( 'type', 'modal-analysis');
|
|
initializeRy( 'type', 'modal-analysis');
|
|
initializeRz( 'type', 'modal-analysis');
|
|
initializeMicroHexapod('type', 'modal-analysis');
|
|
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
initializeReferences();
|
|
initializeDisturbances('enable', false);
|
|
#+end_src
|
|
|
|
We now use a new Simscape Model where 6DoF inertial sensors are located at the Center of Mass of each solid body.
|
|
|
|
We use the =linearize= function in order to estimate the dynamics from forces applied on the Translation stage at the same position used for the real modal analysis to the inertial sensors.
|
|
#+begin_src matlab
|
|
%% Identification
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Granite/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
% Run the linearization
|
|
G_ms = linearize(mdl, io, 0);
|
|
|
|
% Input/Output definition
|
|
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
|
|
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
|
|
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
|
|
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
|
|
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
|
|
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
|
|
|
|
% Put the output of G in displacements instead of acceleration
|
|
G_ms = G_ms/s^2;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Load estimated FRF at CoM
|
|
load('/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A3-micro-station-modal-analysis/matlab/mat/frf_matrix.mat', 'freqs');
|
|
load('/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A3-micro-station-modal-analysis/matlab/mat/frf_com.mat', 'frfs_CoM');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Compare the two
|
|
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
|
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'}
|
|
|
|
n_stg = 3;
|
|
n_dir = 6; % x, y, z, Rx, Ry, Rz
|
|
n_exc = 2; % x, y, z
|
|
|
|
f = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :)))./((2*pi*freqs).^2)');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]');
|
|
hold off;
|
|
xlim([1, 200]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
|
stages = {'gtop', 'ty', 'ry', 'rz', 'hexa'}
|
|
|
|
f = logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
for n_stg = 1:2
|
|
for n_dir = 1:3
|
|
subplot(3, 2, (n_dir-1)*2 + n_stg);
|
|
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]');
|
|
if n_dir == 3
|
|
xlabel('Frequency [Hz]');
|
|
end
|
|
hold off;
|
|
xlim([10, 1000]);
|
|
ylim([1e-12, 1e-6]);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
|
stages = {'ry', 'rz', 'hexa'}
|
|
|
|
f = logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
for n_stg = 1:2
|
|
for n_dir = 1:3
|
|
subplot(3, 2, (n_dir-1)*2 + n_stg);
|
|
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg+2) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]');
|
|
if n_dir == 3
|
|
xlabel('Frequency [Hz]');
|
|
end
|
|
hold off;
|
|
xlim([10, 1000]);
|
|
ylim([1e-12, 1e-6]);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
|
stages = {'hexa'}
|
|
|
|
f = logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
for n_stg = 1
|
|
for n_dir = 1:3
|
|
subplot(3, 1, (n_dir-1) + n_stg);
|
|
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg+4) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]');
|
|
if n_dir == 3
|
|
xlabel('Frequency [Hz]');
|
|
end
|
|
hold off;
|
|
xlim([10, 1000]);
|
|
ylim([1e-12, 1e-6]);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Obtained Compliance of the Micro-Station
|
|
#+begin_src matlab
|
|
%% Initialize simulation with default parameters (flexible elements)
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod('type', 'compliance');
|
|
|
|
initializeReferences();
|
|
initializeDisturbances();
|
|
initializeSimscapeConfiguration();
|
|
initializeLoggingConfiguration();
|
|
#+end_src
|
|
|
|
And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point.
|
|
|
|
#+begin_src matlab
|
|
%% Identification of the compliance
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Compliance/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform
|
|
|
|
% Run the linearization
|
|
Gm = linearize(mdl, io, 0);
|
|
Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
|
|
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot of the compliance curves
|
|
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
|
|
|
|
freqs = logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gm(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]');
|
|
ylabel('Compliance');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
- [ ] Comparison with the estimated (or measured) compliance
|
|
|
|
** Conclusion
|
|
For such a complex system, we believe that the Simscape Model represents the dynamics of the system with enough fidelity.
|
|
|
|
* Measurement of Positioning Errors
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/ustation_2_kinematics.m
|
|
:END:
|
|
<<sec:ustation_kinematics>>
|
|
** Introduction :ignore:
|
|
|
|
[[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org]]
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
|
|
|
|
* Simulation of Scientific Experiments
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/.m
|
|
:END:
|
|
<<sec:ustation_kinematics>>
|
|
** Introduction :ignore:
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
|
|
* Estimation of disturbances
|
|
* Conclusion
|
|
<<sec:uniaxial_conclusion>>
|
|
|
|
|
|
* Bibliography :ignore:
|
|
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
|
|
|
|
* Matlab Functions :noexport:
|
|
** Simscape Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeSimscapeConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = initializeSimscapeConfiguration(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.gravity logical {mustBeNumericOrLogical} = true
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
conf_simscape = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
if args.gravity
|
|
conf_simscape.type = 1;
|
|
else
|
|
conf_simscape.type = 2;
|
|
end
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/conf_simscape.mat', 'file')
|
|
save('mat/conf_simscape.mat', 'conf_simscape', '-append');
|
|
else
|
|
save('mat/conf_simscape.mat', 'conf_simscape');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/conf_simscape.mat', 'file')
|
|
save('matlab/mat/conf_simscape.mat', 'conf_simscape', '-append');
|
|
else
|
|
save('matlab/mat/conf_simscape.mat', 'conf_simscape');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Logging Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = initializeLoggingConfiguration(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
conf_log = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.log
|
|
case 'none'
|
|
conf_log.type = 0;
|
|
case 'all'
|
|
conf_log.type = 1;
|
|
case 'forces'
|
|
conf_log.type = 2;
|
|
end
|
|
#+end_src
|
|
|
|
*** Sampling Time
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
conf_log.Ts = args.Ts;
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/conf_log.mat', 'file')
|
|
save('mat/conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('mat/conf_log.mat', 'conf_log');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/conf_log.mat', 'file')
|
|
save('matlab/mat/conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('matlab/mat/conf_log.mat', 'conf_log');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Ground
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeGround.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ground] = initializeGround(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
|
|
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =granite= structure.
|
|
#+begin_src matlab
|
|
ground = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type
|
|
case 'none'
|
|
ground.type = 0;
|
|
case 'rigid'
|
|
ground.type = 1;
|
|
end
|
|
#+end_src
|
|
|
|
*** Ground Solid properties
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
We set the shape and density of the ground solid element.
|
|
#+begin_src matlab
|
|
ground.shape = [2, 2, 0.5]; % [m]
|
|
ground.density = 2800; % [kg/m3]
|
|
#+end_src
|
|
|
|
*** Rotation Point
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
ground.rot_point = args.rot_point;
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_stages.mat', 'file')
|
|
save('mat/nass_stages.mat', 'ground', '-append');
|
|
else
|
|
save('mat/nass_stages.mat', 'ground');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_stages.mat', 'file')
|
|
save('matlab/mat/nass_stages.mat', 'ground', '-append');
|
|
else
|
|
save('matlab/mat/nass_stages.mat', 'ground');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Granite
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeGranite.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [granite] = initializeGranite(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis'})} = 'flexible'
|
|
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
|
|
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
|
|
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
|
|
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
|
|
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
|
|
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
|
|
args.sample_pos (1,1) double {mustBeNumeric} = 0.8 % Height of the measurment point [m]
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =granite= structure.
|
|
#+begin_src matlab
|
|
granite = struct();
|
|
#+end_src
|
|
|
|
*** Add Granite Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type
|
|
case 'none'
|
|
granite.type = 0;
|
|
case 'rigid'
|
|
granite.type = 1;
|
|
case 'flexible'
|
|
granite.type = 2;
|
|
case 'modal-analysis'
|
|
granite.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
*** Material and Geometry
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
Properties of the Material and link to the geometry of the granite.
|
|
#+begin_src matlab
|
|
granite.density = args.density; % [kg/m3]
|
|
granite.STEP = 'granite.STEP';
|
|
#+end_src
|
|
|
|
Z-offset for the initial position of the sample with respect to the granite top surface.
|
|
#+begin_src matlab
|
|
granite.sample_pos = args.sample_pos; % [m]
|
|
#+end_src
|
|
|
|
*** Stiffness and Damping properties
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
granite.K = args.K; % [N/m]
|
|
granite.C = args.C; % [N/(m/s)]
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_stages.mat', 'file')
|
|
save('mat/nass_stages.mat', 'granite', '-append');
|
|
else
|
|
save('mat/nass_stages.mat', 'granite');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_stages.mat', 'file')
|
|
save('matlab/mat/nass_stages.mat', 'granite', '-append');
|
|
else
|
|
save('matlab/mat/nass_stages.mat', 'granite');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Translation Stage
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeTy.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ty] = initializeTy(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =ty= structure.
|
|
#+begin_src matlab
|
|
ty = struct();
|
|
#+end_src
|
|
|
|
*** Add Translation Stage Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type
|
|
case 'none'
|
|
ty.type = 0;
|
|
case 'rigid'
|
|
ty.type = 1;
|
|
case 'flexible'
|
|
ty.type = 2;
|
|
case 'modal-analysis'
|
|
ty.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
*** Material and Geometry
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Define the density of the materials as well as the geometry (STEP files).
|
|
#+begin_src matlab
|
|
% Ty Granite frame
|
|
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
|
|
ty.granite_frame.STEP = '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';
|
|
#+end_src
|
|
|
|
*** Stiffness and Damping properties
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
|
|
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_stages.mat', 'file')
|
|
save('mat/nass_stages.mat', 'ty', '-append');
|
|
else
|
|
save('mat/nass_stages.mat', 'ty');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_stages.mat', 'file')
|
|
save('matlab/mat/nass_stages.mat', 'ty', '-append');
|
|
else
|
|
save('matlab/mat/nass_stages.mat', 'ty');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Tilt Stage
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeRy.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ry] = initializeRy(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
|
|
args.Ry_init (1,1) double {mustBeNumeric} = 0
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =ry= structure.
|
|
#+begin_src matlab
|
|
ry = struct();
|
|
#+end_src
|
|
|
|
|
|
*** Add Tilt Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type
|
|
case 'none'
|
|
ry.type = 0;
|
|
case 'rigid'
|
|
ry.type = 1;
|
|
case 'flexible'
|
|
ry.type = 2;
|
|
case 'modal-analysis'
|
|
ry.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
*** Material and Geometry
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Properties of the Material and link to the geometry of the Tilt stage.
|
|
#+begin_src matlab
|
|
% Ry - Guide for the tilt stage
|
|
ry.guide.density = 7800; % [kg/m3]
|
|
ry.guide.STEP = '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';
|
|
#+end_src
|
|
|
|
Z-Offset so that the center of rotation matches the sample center;
|
|
#+begin_src matlab
|
|
ry.z_offset = 0.58178; % [m]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
ry.Ry_init = args.Ry_init; % [rad]
|
|
#+end_src
|
|
|
|
*** Stiffness and Damping properties
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
|
|
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_stages.mat', 'file')
|
|
save('mat/nass_stages.mat', 'ry', '-append');
|
|
else
|
|
save('mat/nass_stages.mat', 'ry');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_stages.mat', 'file')
|
|
save('matlab/mat/nass_stages.mat', 'ry', '-append');
|
|
else
|
|
save('matlab/mat/nass_stages.mat', 'ry');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Spindle
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeRz.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [rz] = initializeRz(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =rz= structure.
|
|
#+begin_src matlab
|
|
rz = struct();
|
|
#+end_src
|
|
|
|
*** Add Spindle Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type
|
|
case 'none'
|
|
rz.type = 0;
|
|
case 'rigid'
|
|
rz.type = 1;
|
|
case 'flexible'
|
|
rz.type = 2;
|
|
case 'modal-analysis'
|
|
rz.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
*** Material and Geometry
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
Properties of the Material and link to the geometry of the spindle.
|
|
#+begin_src matlab
|
|
% Spindle - Slip Ring
|
|
rz.slipring.density = 7800; % [kg/m3]
|
|
rz.slipring.STEP = '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';
|
|
#+end_src
|
|
|
|
*** Stiffness and Damping properties
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
|
|
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_stages.mat', 'file')
|
|
save('mat/nass_stages.mat', 'rz', '-append');
|
|
else
|
|
save('mat/nass_stages.mat', 'rz');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_stages.mat', 'file')
|
|
save('matlab/mat/nass_stages.mat', 'rz', '-append');
|
|
else
|
|
save('matlab/mat/nass_stages.mat', 'rz');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Micro Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeMicroHexapod.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [micro_hexapod] = initializeMicroHexapod(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'compliance'})} = 'flexible'
|
|
% initializeFramesPositions
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
|
|
% generateGeneralConfiguration
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
|
|
% initializeStrutDynamics
|
|
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
|
|
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
|
|
% initializeCylindricalPlatforms
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
|
% initializeCylindricalStruts
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
|
% inverseKinematics
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
#+end_src
|
|
|
|
*** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart = initializeStewartPlatform();
|
|
|
|
stewart = initializeFramesPositions(stewart, ...
|
|
'H', args.H, ...
|
|
'MO_B', args.MO_B);
|
|
|
|
stewart = generateGeneralConfiguration(stewart, ...
|
|
'FH', args.FH, ...
|
|
'FR', args.FR, ...
|
|
'FTh', args.FTh, ...
|
|
'MH', args.MH, ...
|
|
'MR', args.MR, ...
|
|
'MTh', args.MTh);
|
|
|
|
stewart = computeJointsPose(stewart);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart = initializeStrutDynamics(stewart, ...
|
|
'K', args.Ki, ...
|
|
'C', args.Ci);
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
'type_F', 'universal_p', ...
|
|
'type_M', 'spherical_p');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Fpm', args.Fpm, ...
|
|
'Fph', args.Fph, ...
|
|
'Fpr', args.Fpr, ...
|
|
'Mpm', args.Mpm, ...
|
|
'Mph', args.Mph, ...
|
|
'Mpr', args.Mpr);
|
|
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', args.Fsm, ...
|
|
'Fsh', args.Fsh, ...
|
|
'Fsr', args.Fsr, ...
|
|
'Msm', args.Msm, ...
|
|
'Msh', args.Msh, ...
|
|
'Msr', args.Msr);
|
|
|
|
stewart = computeJacobian(stewart);
|
|
|
|
stewart = initializeStewartPose(stewart, ...
|
|
'AP', args.AP, ...
|
|
'ARB', args.ARB);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type
|
|
case 'none'
|
|
stewart.type = 0;
|
|
case 'rigid'
|
|
stewart.type = 1;
|
|
case 'flexible'
|
|
stewart.type = 2;
|
|
case 'modal-analysis'
|
|
stewart.type = 3;
|
|
case 'compliance'
|
|
stewart.type = 5;
|
|
end
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
micro_hexapod = stewart;
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_stages.mat', 'file')
|
|
save('mat/nass_stages.mat', 'micro_hexapod', '-append');
|
|
else
|
|
save('mat/nass_stages.mat', 'micro_hexapod');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_stages.mat', 'file')
|
|
save('matlab/mat/nass_stages.mat', 'micro_hexapod', '-append');
|
|
else
|
|
save('matlab/mat/nass_stages.mat', 'micro_hexapod');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** 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
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ref] = initializeReferences(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
% Sampling Frequency [s]
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
|
% Maximum simulation time [s]
|
|
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
|
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
|
% Amplitude of the displacement [m]
|
|
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the displacement [s]
|
|
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
|
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
|
% Amplitude [rad]
|
|
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the displacement [s]
|
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% Either "constant" / "rotating"
|
|
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
|
|
% Initial angle [rad]
|
|
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the rotating [s]
|
|
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% For now, only constant is implemented
|
|
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
|
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
|
% For now, only constant is implemented
|
|
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
|
|
% Initial position of the two masses
|
|
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
|
|
% For now, only constant is implemented
|
|
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform
|
|
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
end
|
|
#+end_src
|
|
|
|
|
|
*** Initialize Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Set Sampling Time
|
|
Ts = args.Ts;
|
|
Tmax = args.Tmax;
|
|
|
|
%% Low Pass Filter to filter out the references
|
|
s = zpk('s');
|
|
w0 = 2*pi*10;
|
|
xi = 1;
|
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
|
#+end_src
|
|
|
|
*** Translation Stage
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Translation stage - Dy
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Dy = zeros(length(t), 1);
|
|
Dyd = zeros(length(t), 1);
|
|
Dydd = zeros(length(t), 1);
|
|
switch args.Dy_type
|
|
case 'constant'
|
|
Dy(:) = args.Dy_amplitude;
|
|
Dyd(:) = 0;
|
|
Dydd(:) = 0;
|
|
case 'triangular'
|
|
% This is done to unsure that we start with no displacement
|
|
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
|
|
i0 = find(t>=args.Dy_period/4,1);
|
|
Dy(1:end-i0+1) = Dy_raw(i0:end);
|
|
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
|
|
|
|
% The signal is filtered out
|
|
Dy = lsim(H_lpf, Dy, t);
|
|
Dyd = lsim(H_lpf*s, Dy, t);
|
|
Dydd = lsim(H_lpf*s^2, Dy, t);
|
|
case 'sinusoidal'
|
|
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
|
|
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
|
|
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
|
|
otherwise
|
|
warning('Dy_type is not set correctly');
|
|
end
|
|
|
|
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
|
|
#+end_src
|
|
|
|
*** Tilt Stage
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Tilt Stage - Ry
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Ry = zeros(length(t), 1);
|
|
Ryd = zeros(length(t), 1);
|
|
Rydd = zeros(length(t), 1);
|
|
|
|
switch args.Ry_type
|
|
case 'constant'
|
|
Ry(:) = args.Ry_amplitude;
|
|
Ryd(:) = 0;
|
|
Rydd(:) = 0;
|
|
case 'triangular'
|
|
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
|
|
i0 = find(t>=args.Ry_period/4,1);
|
|
Ry(1:end-i0+1) = Ry_raw(i0:end);
|
|
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
|
|
|
|
% The signal is filtered out
|
|
Ry = lsim(H_lpf, Ry, t);
|
|
Ryd = lsim(H_lpf*s, Ry, t);
|
|
Rydd = lsim(H_lpf*s^2, Ry, t);
|
|
case 'sinusoidal'
|
|
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
|
|
|
|
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
|
|
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
|
|
otherwise
|
|
warning('Ry_type is not set correctly');
|
|
end
|
|
|
|
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
|
|
#+end_src
|
|
|
|
*** Spindle
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Spindle - Rz
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Rz = zeros(length(t), 1);
|
|
Rzd = zeros(length(t), 1);
|
|
Rzdd = zeros(length(t), 1);
|
|
|
|
switch args.Rz_type
|
|
case 'constant'
|
|
Rz(:) = args.Rz_amplitude;
|
|
Rzd(:) = 0;
|
|
Rzdd(:) = 0;
|
|
case 'rotating-not-filtered'
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
|
|
|
% The signal is filtered out
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
|
Rzd(:) = 2*pi/args.Rz_period;
|
|
Rzdd(:) = 0;
|
|
|
|
% We add the angle offset
|
|
Rz = Rz + args.Rz_amplitude;
|
|
|
|
case 'rotating'
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
|
|
|
% The signal is filtered out
|
|
Rz = lsim(H_lpf, Rz, t);
|
|
Rzd = lsim(H_lpf*s, Rz, t);
|
|
Rzdd = lsim(H_lpf*s^2, Rz, t);
|
|
|
|
% We add the angle offset
|
|
Rz = Rz + args.Rz_amplitude;
|
|
otherwise
|
|
warning('Rz_type is not set correctly');
|
|
end
|
|
|
|
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
|
|
#+end_src
|
|
|
|
*** Micro Hexapod
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Micro-Hexapod
|
|
t = [0, Ts];
|
|
Dh = zeros(length(t), 6);
|
|
Dhl = zeros(length(t), 6);
|
|
|
|
switch args.Dh_type
|
|
case 'constant'
|
|
Dh = [args.Dh_pos, args.Dh_pos];
|
|
|
|
load('nass_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_references.mat', 'file')
|
|
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
|
else
|
|
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_references.mat', 'file')
|
|
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
|
else
|
|
save('matlab/mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
|
|
** Initialize Disturbances
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeDisturbances.m
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
|
|
*** Function Declaration and Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = initializeDisturbances(args)
|
|
% initializeDisturbances - Initialize the disturbances
|
|
%
|
|
% Syntax: [] = initializeDisturbances(args)
|
|
%
|
|
% Inputs:
|
|
% - args -
|
|
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
% Global parameter to enable or disable the disturbances
|
|
args.enable logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - X direction
|
|
args.Dwx logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - Y direction
|
|
args.Dwy logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - Z direction
|
|
args.Dwz logical {mustBeNumericOrLogical} = true
|
|
% Translation Stage - X direction
|
|
args.Fty_x logical {mustBeNumericOrLogical} = true
|
|
% Translation Stage - Z direction
|
|
args.Fty_z logical {mustBeNumericOrLogical} = true
|
|
% Spindle - Z direction
|
|
args.Frz_z logical {mustBeNumericOrLogical} = true
|
|
end
|
|
#+end_src
|
|
|
|
|
|
*** Load Data
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('dist_psd.mat', 'dist_f');
|
|
#+end_src
|
|
|
|
We remove the first frequency point that usually is very large.
|
|
#+begin_src matlab :exports none
|
|
dist_f.f = dist_f.f(2:end);
|
|
dist_f.psd_gm = dist_f.psd_gm(2:end);
|
|
dist_f.psd_ty = dist_f.psd_ty(2:end);
|
|
dist_f.psd_rz = dist_f.psd_rz(2:end);
|
|
#+end_src
|
|
|
|
*** Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
We define some parameters that will be used in the algorithm.
|
|
#+begin_src matlab
|
|
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
|
% Also equal to (dist_f.f(2)-dist_f.f(1))
|
|
t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
Ts = 1/Fs; % Sampling Time [s]
|
|
#+end_src
|
|
|
|
*** Ground Motion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
phi = dist_f.psd_gm;
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(phi(i)*df);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
if args.Dwx && args.enable
|
|
rng(111);
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
|
else
|
|
Dwx = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
if args.Dwy && args.enable
|
|
rng(112);
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
|
else
|
|
Dwy = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
if args.Dwy && args.enable
|
|
rng(113);
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
|
else
|
|
Dwz = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
*** Translation Stage - X direction
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
if args.Fty_x && args.enable
|
|
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(phi(i)*df);
|
|
end
|
|
rng(121);
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
|
Fty_x = u;
|
|
else
|
|
Fty_x = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
*** Translation Stage - Z direction
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
if args.Fty_z && args.enable
|
|
phi = dist_f.psd_ty;
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(phi(i)*df);
|
|
end
|
|
rng(122);
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
|
Fty_z = u;
|
|
else
|
|
Fty_z = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
*** Spindle - Z direction
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
if args.Frz_z && args.enable
|
|
phi = dist_f.psd_rz;
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(phi(i)*df);
|
|
end
|
|
rng(131);
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
|
Frz_z = u;
|
|
else
|
|
Frz_z = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
*** Direct Forces
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
u = zeros(length(t), 6);
|
|
Fd = u;
|
|
#+end_src
|
|
|
|
*** Set initial value to zero
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
Dwx = Dwx - Dwx(1);
|
|
Dwy = Dwy - Dwy(1);
|
|
Dwz = Dwz - Dwz(1);
|
|
Fty_x = Fty_x - Fty_x(1);
|
|
Fty_z = Fty_z - Fty_z(1);
|
|
Frz_z = Frz_z - Frz_z(1);
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_disturbances.mat', 'file')
|
|
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
|
|
else
|
|
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_disturbances.mat', 'file')
|
|
save('matlab/mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args', '-append');
|
|
else
|
|
save('matlab/mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeStewartPlatform=: Initialize the Stewart Platform structure
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeStewartPlatform.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+name: fig:stewart-frames-position
|
|
#+caption: Definition of the position of the frames
|
|
[[file:figs/stewart-frames-position.png]]
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeStewartPlatform()
|
|
% initializeStewartPlatform - Initialize the stewart structure
|
|
%
|
|
% Syntax: [stewart] = initializeStewartPlatform(args)
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following sub-structures:
|
|
% - platform_F -
|
|
% - platform_M -
|
|
% - joints_F -
|
|
% - joints_M -
|
|
% - struts_F -
|
|
% - struts_M -
|
|
% - actuators -
|
|
% - geometry -
|
|
% - properties -
|
|
#+end_src
|
|
|
|
*** Initialize the Stewart structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart = struct();
|
|
stewart.platform_F = struct();
|
|
stewart.platform_M = struct();
|
|
stewart.joints_F = struct();
|
|
stewart.joints_M = struct();
|
|
stewart.struts_F = struct();
|
|
stewart.struts_M = struct();
|
|
stewart.actuators = struct();
|
|
stewart.sensors = struct();
|
|
stewart.sensors.inertial = struct();
|
|
stewart.sensors.force = struct();
|
|
stewart.sensors.relative = struct();
|
|
stewart.geometry = struct();
|
|
stewart.kinematics = struct();
|
|
#+end_src
|
|
|
|
** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeFramesPositions.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+name: fig:stewart-frames-position
|
|
#+caption: Definition of the position of the frames
|
|
[[file:figs/stewart-frames-position.png]]
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeFramesPositions(stewart, args)
|
|
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
%
|
|
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
|
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following fields:
|
|
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
|
end
|
|
#+end_src
|
|
|
|
*** Compute the position of each frame
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
H = args.H; % Total Height of the Stewart Platform [m]
|
|
|
|
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
|
|
|
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
|
|
|
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.geometry.H = H;
|
|
stewart.geometry.FO_M = FO_M;
|
|
stewart.platform_M.MO_B = MO_B;
|
|
stewart.platform_F.FO_A = FO_A;
|
|
#+end_src
|
|
|
|
** =generateGeneralConfiguration=: Generate a Very General Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/generateGeneralConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
|
|
The radius of the circles can be chosen as well as the angles where the joints are located (see Figure [[fig:joint_position_general]]).
|
|
|
|
#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
|
|
\begin{tikzpicture}
|
|
% Internal and external limit
|
|
\draw[fill=white!80!black] (0, 0) circle [radius=3];
|
|
% Circle where the joints are located
|
|
\draw[dashed] (0, 0) circle [radius=2.5];
|
|
|
|
% Bullets for the positions of the joints
|
|
\node[] (J1) at ( 80:2.5){$\bullet$};
|
|
\node[] (J2) at (100:2.5){$\bullet$};
|
|
\node[] (J3) at (200:2.5){$\bullet$};
|
|
\node[] (J4) at (220:2.5){$\bullet$};
|
|
\node[] (J5) at (320:2.5){$\bullet$};
|
|
\node[] (J6) at (340:2.5){$\bullet$};
|
|
|
|
% Name of the points
|
|
\node[above right] at (J1) {$a_{1}$};
|
|
\node[above left] at (J2) {$a_{2}$};
|
|
\node[above left] at (J3) {$a_{3}$};
|
|
\node[right ] at (J4) {$a_{4}$};
|
|
\node[left ] at (J5) {$a_{5}$};
|
|
\node[above right] at (J6) {$a_{6}$};
|
|
|
|
% First 2 angles
|
|
\draw[dashed, ->] (0:1) arc [start angle=0, end angle=80, radius=1] node[below right]{$\theta_{1}$};
|
|
\draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left ]{$\theta_{2}$};
|
|
|
|
% Division of 360 degrees by 3
|
|
\draw[dashed] (0, 0) -- ( 80:3.2);
|
|
\draw[dashed] (0, 0) -- (100:3.2);
|
|
\draw[dashed] (0, 0) -- (200:3.2);
|
|
\draw[dashed] (0, 0) -- (220:3.2);
|
|
\draw[dashed] (0, 0) -- (320:3.2);
|
|
\draw[dashed] (0, 0) -- (340:3.2);
|
|
|
|
% Radius for the position of the joints
|
|
\draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);
|
|
|
|
\draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:joint_position_general
|
|
#+caption: Position of the joints
|
|
#+RESULTS:
|
|
[[file:figs/stewart_bottom_plate.png]]
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = generateGeneralConfiguration(stewart, args)
|
|
% generateGeneralConfiguration - Generate a Very General Configuration
|
|
%
|
|
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
|
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
|
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
|
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
|
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
|
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
|
end
|
|
#+end_src
|
|
|
|
*** Compute the pose
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
Fa = zeros(3,6);
|
|
Mb = zeros(3,6);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
for i = 1:6
|
|
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
|
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
|
end
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
#+end_src
|
|
|
|
** =computeJointsPose=: Compute the Pose of the Joints
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/computeJointsPose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+name: fig:stewart-struts
|
|
#+caption: Position and orientation of the struts
|
|
[[file:figs/stewart-struts.png]]
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = computeJointsPose(stewart)
|
|
% computeJointsPose -
|
|
%
|
|
% Syntax: [stewart] = computeJointsPose(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following added fields
|
|
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
|
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
|
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
|
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
|
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
|
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
|
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
|
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
|
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
|
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
|
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
|
#+end_src
|
|
|
|
*** Check the =stewart= structure elements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
|
Fa = stewart.platform_F.Fa;
|
|
|
|
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
|
Mb = stewart.platform_M.Mb;
|
|
|
|
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
|
FO_A = stewart.platform_F.FO_A;
|
|
|
|
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
|
MO_B = stewart.platform_M.MO_B;
|
|
|
|
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
|
FO_M = stewart.geometry.FO_M;
|
|
#+end_src
|
|
|
|
*** Compute the position of the Joints
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
Aa = Fa - repmat(FO_A, [1, 6]);
|
|
Bb = Mb - repmat(MO_B, [1, 6]);
|
|
|
|
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
|
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
|
#+end_src
|
|
|
|
*** Compute the strut length and orientation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
|
|
|
l = vecnorm(Ab - Aa)';
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
|
#+end_src
|
|
|
|
*** Compute the orientation of the Joints
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
FRa = zeros(3,3,6);
|
|
MRb = zeros(3,3,6);
|
|
|
|
for i = 1:6
|
|
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
|
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
|
|
|
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
|
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
|
end
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.geometry.Aa = Aa;
|
|
stewart.geometry.Ab = Ab;
|
|
stewart.geometry.Ba = Ba;
|
|
stewart.geometry.Bb = Bb;
|
|
stewart.geometry.As = As;
|
|
stewart.geometry.Bs = Bs;
|
|
stewart.geometry.l = l;
|
|
|
|
stewart.struts_F.l = l/2;
|
|
stewart.struts_M.l = l/2;
|
|
|
|
stewart.platform_F.FRa = FRa;
|
|
stewart.platform_M.MRb = MRb;
|
|
#+end_src
|
|
|
|
** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeCylindricalPlatforms.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
|
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
|
%
|
|
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
|
% - Fph [1x1] - Fixed Platform Height [m]
|
|
% - Fpr [1x1] - Fixed Platform Radius [m]
|
|
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
|
% - Mph [1x1] - Mobile Platform Height [m]
|
|
% - Mpr [1x1] - Mobile Platform Radius [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F [struct] - structure with the following fields:
|
|
% - type = 1
|
|
% - M [1x1] - Fixed Platform Mass [kg]
|
|
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
|
% - H [1x1] - Fixed Platform Height [m]
|
|
% - R [1x1] - Fixed Platform Radius [m]
|
|
% - platform_M [struct] - structure with the following fields:
|
|
% - M [1x1] - Mobile Platform Mass [kg]
|
|
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
|
% - H [1x1] - Mobile Platform Height [m]
|
|
% - R [1x1] - Mobile Platform Radius [m]
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
end
|
|
#+end_src
|
|
|
|
*** Compute the Inertia matrices of platforms
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
|
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
|
1/2 *args.Fpm * args.Fpr^2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
|
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
|
1/2 *args.Mpm * args.Mpr^2]);
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.platform_F.type = 1;
|
|
|
|
stewart.platform_F.I = I_F;
|
|
stewart.platform_F.M = args.Fpm;
|
|
stewart.platform_F.R = args.Fpr;
|
|
stewart.platform_F.H = args.Fph;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart.platform_M.type = 1;
|
|
|
|
stewart.platform_M.I = I_M;
|
|
stewart.platform_M.M = args.Mpm;
|
|
stewart.platform_M.R = args.Mpr;
|
|
stewart.platform_M.H = args.Mph;
|
|
#+end_src
|
|
|
|
** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeCylindricalStruts.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeCylindricalStruts(stewart, args)
|
|
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
|
%
|
|
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
|
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
|
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
|
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
|
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
|
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - struts_F [struct] - structure with the following fields:
|
|
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
|
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
|
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
|
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
|
% - struts_M [struct] - structure with the following fields:
|
|
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
|
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
|
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
|
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
end
|
|
#+end_src
|
|
|
|
*** Compute the properties of the cylindrical struts
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
Fsm = ones(6,1).*args.Fsm;
|
|
Fsh = ones(6,1).*args.Fsh;
|
|
Fsr = ones(6,1).*args.Fsr;
|
|
|
|
Msm = ones(6,1).*args.Msm;
|
|
Msh = ones(6,1).*args.Msh;
|
|
Msr = ones(6,1).*args.Msr;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
|
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
|
|
|
for i = 1:6
|
|
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
|
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
|
1/2 * Fsm(i) * Fsr(i)^2]);
|
|
|
|
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
|
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
|
1/2 * Msm(i) * Msr(i)^2]);
|
|
end
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.struts_M.type = 1;
|
|
|
|
stewart.struts_M.I = I_M;
|
|
stewart.struts_M.M = Msm;
|
|
stewart.struts_M.R = Msr;
|
|
stewart.struts_M.H = Msh;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart.struts_F.type = 1;
|
|
|
|
stewart.struts_F.I = I_F;
|
|
stewart.struts_F.M = Fsm;
|
|
stewart.struts_F.R = Fsr;
|
|
stewart.struts_F.H = Fsh;
|
|
#+end_src
|
|
|
|
** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeStrutDynamics.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+name: fig:piezoelectric_stack
|
|
#+attr_html: :width 500px
|
|
#+caption: Example of a piezoelectric stach actuator (PI)
|
|
[[file:figs/piezoelectric_stack.jpg]]
|
|
|
|
A simplistic model of such amplified actuator is shown in Figure [[fig:actuator_model_simple]] where:
|
|
- $K$ represent the vertical stiffness of the actuator
|
|
- $C$ represent the vertical damping of the actuator
|
|
- $F$ represents the force applied by the actuator
|
|
- $F_{m}$ represents the total measured force
|
|
- $v_{m}$ represents the absolute velocity of the top part of the actuator
|
|
- $d_{m}$ represents the total relative displacement of the actuator
|
|
|
|
#+begin_src latex :file actuator_model_simple.pdf :tangle no
|
|
\begin{tikzpicture}
|
|
\draw (-1, 0) -- (1, 0);
|
|
|
|
% Spring, Damper, and Actuator
|
|
\draw[spring] (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
|
|
\draw[damper] ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
|
|
\draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};
|
|
|
|
\node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};
|
|
|
|
\node[left] at (fsens.west) {$F_{m}$};
|
|
|
|
\draw[dashed] (1, 0) -- ++(0.4, 0);
|
|
\draw[dashed] (1, 1.7) -- ++(0.4, 0);
|
|
|
|
\draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};
|
|
|
|
\draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:actuator_model_simple
|
|
#+caption: Simple model of an Actuator
|
|
#+RESULTS:
|
|
[[file:figs/actuator_model_simple.png]]
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeStrutDynamics(stewart, args)
|
|
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
|
%
|
|
% Syntax: [stewart] = initializeStrutDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - K [6x1] - Stiffness of each strut [N/m]
|
|
% - C [6x1] - Damping of each strut [N/(m/s)]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - actuators.type = 1
|
|
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
|
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
|
args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
|
|
args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
|
|
args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
|
|
args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
|
|
args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
|
|
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
|
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
|
end
|
|
#+end_src
|
|
|
|
*** Add Stiffness and Damping properties of each strut
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
if strcmp(args.type, 'classical')
|
|
stewart.actuators.type = 1;
|
|
elseif strcmp(args.type, 'amplified')
|
|
stewart.actuators.type = 2;
|
|
end
|
|
|
|
stewart.actuators.K = args.K;
|
|
stewart.actuators.C = args.C;
|
|
|
|
stewart.actuators.k1 = args.k1;
|
|
stewart.actuators.c1 = args.c1;
|
|
|
|
stewart.actuators.ka = args.ka;
|
|
stewart.actuators.ke = args.ke;
|
|
|
|
stewart.actuators.F_gain = args.F_gain;
|
|
|
|
stewart.actuators.ma = args.ma;
|
|
stewart.actuators.me = args.me;
|
|
#+end_src
|
|
|
|
** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeJointDynamics.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeJointDynamics(stewart, args)
|
|
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
|
|
%
|
|
% Syntax: [stewart] = initializeJointDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
|
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
|
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
|
|
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
|
|
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
|
|
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
|
|
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
|
|
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
|
|
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
|
|
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - stewart.joints_F and stewart.joints_M:
|
|
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
|
|
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
|
|
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
|
|
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
|
|
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
|
|
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
|
|
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
|
|
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
|
|
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
|
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
|
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
|
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
|
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
|
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
|
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
|
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
|
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.K_M double {mustBeNumeric} = zeros(6,6)
|
|
args.M_M double {mustBeNumeric} = zeros(6,6)
|
|
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
|
args.xi_M double {mustBeNumeric} = 0.1
|
|
args.step_file_M char {} = ''
|
|
args.K_F double {mustBeNumeric} = zeros(6,6)
|
|
args.M_F double {mustBeNumeric} = zeros(6,6)
|
|
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
|
args.xi_F double {mustBeNumeric} = 0.1
|
|
args.step_file_F char {} = ''
|
|
end
|
|
#+end_src
|
|
|
|
*** Add Actuator Type
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.type_F
|
|
case 'universal'
|
|
stewart.joints_F.type = 1;
|
|
case 'spherical'
|
|
stewart.joints_F.type = 2;
|
|
case 'universal_p'
|
|
stewart.joints_F.type = 3;
|
|
case 'spherical_p'
|
|
stewart.joints_F.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_F.type = 5;
|
|
case 'universal_3dof'
|
|
stewart.joints_F.type = 6;
|
|
case 'spherical_3dof'
|
|
stewart.joints_F.type = 7;
|
|
end
|
|
|
|
switch args.type_M
|
|
case 'universal'
|
|
stewart.joints_M.type = 1;
|
|
case 'spherical'
|
|
stewart.joints_M.type = 2;
|
|
case 'universal_p'
|
|
stewart.joints_M.type = 3;
|
|
case 'spherical_p'
|
|
stewart.joints_M.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_M.type = 5;
|
|
case 'universal_3dof'
|
|
stewart.joints_M.type = 6;
|
|
case 'spherical_3dof'
|
|
stewart.joints_M.type = 7;
|
|
end
|
|
#+end_src
|
|
|
|
*** Add Stiffness and Damping in Translation of each strut
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Axial and Radial (shear) Stiffness
|
|
#+begin_src matlab
|
|
stewart.joints_M.Ka = args.Ka_M;
|
|
stewart.joints_M.Kr = args.Kr_M;
|
|
|
|
stewart.joints_F.Ka = args.Ka_F;
|
|
stewart.joints_F.Kr = args.Kr_F;
|
|
#+end_src
|
|
|
|
Translation Damping
|
|
#+begin_src matlab
|
|
stewart.joints_M.Ca = args.Ca_M;
|
|
stewart.joints_M.Cr = args.Cr_M;
|
|
|
|
stewart.joints_F.Ca = args.Ca_F;
|
|
stewart.joints_F.Cr = args.Cr_F;
|
|
#+end_src
|
|
|
|
*** Add Stiffness and Damping in Rotation of each strut
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Rotational Stiffness
|
|
#+begin_src matlab
|
|
stewart.joints_M.Kf = args.Kf_M;
|
|
stewart.joints_M.Kt = args.Kt_M;
|
|
|
|
stewart.joints_F.Kf = args.Kf_F;
|
|
stewart.joints_F.Kt = args.Kt_F;
|
|
#+end_src
|
|
|
|
Rotational Damping
|
|
#+begin_src matlab
|
|
stewart.joints_M.Cf = args.Cf_M;
|
|
stewart.joints_M.Ct = args.Ct_M;
|
|
|
|
stewart.joints_F.Cf = args.Cf_F;
|
|
stewart.joints_F.Ct = args.Ct_F;
|
|
#+end_src
|
|
|
|
*** Stiffness and Mass matrices for flexible joint
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
stewart.joints_F.M = args.M_F;
|
|
stewart.joints_F.K = args.K_F;
|
|
stewart.joints_F.n_xyz = args.n_xyz_F;
|
|
stewart.joints_F.xi = args.xi_F;
|
|
stewart.joints_F.xi = args.xi_F;
|
|
stewart.joints_F.step_file = args.step_file_F;
|
|
|
|
stewart.joints_M.M = args.M_M;
|
|
stewart.joints_M.K = args.K_M;
|
|
stewart.joints_M.n_xyz = args.n_xyz_M;
|
|
stewart.joints_M.xi = args.xi_M;
|
|
stewart.joints_M.step_file = args.step_file_M;
|
|
#+end_src
|
|
|
|
** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeStewartPose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeStewartPose(stewart, args)
|
|
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
|
|
% It uses the inverse kinematic
|
|
%
|
|
% Syntax: [stewart] = initializeStewartPose(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - Aa [3x6] - The positions ai expressed in {A}
|
|
% - Bb [3x6] - The positions bi expressed in {B}
|
|
% - args - Can have the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
#+end_src
|
|
|
|
*** Use the Inverse Kinematic function
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.actuators.Leq = dLi;
|
|
#+end_src
|
|
|
|
** =computeJacobian=: Compute the Jacobian Matrix
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/computeJacobian.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = computeJacobian(stewart)
|
|
% computeJacobian -
|
|
%
|
|
% Syntax: [stewart] = computeJacobian(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - With at least the following fields:
|
|
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
|
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
|
% - actuators.K [6x1] - Total stiffness of the actuators
|
|
%
|
|
% Outputs:
|
|
% - stewart - With the 3 added field:
|
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
|
% - kinematics.K [6x6] - The Stiffness Matrix
|
|
% - kinematics.C [6x6] - The Compliance Matrix
|
|
#+end_src
|
|
|
|
*** Check the =stewart= structure elements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
|
As = stewart.geometry.As;
|
|
|
|
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
|
Ab = stewart.geometry.Ab;
|
|
|
|
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
|
Ki = stewart.actuators.K;
|
|
#+end_src
|
|
|
|
|
|
*** Compute Jacobian Matrix
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
J = [As' , cross(Ab, As)'];
|
|
#+end_src
|
|
|
|
*** Compute Stiffness Matrix
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
K = J'*diag(Ki)*J;
|
|
#+end_src
|
|
|
|
*** Compute Compliance Matrix
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
C = inv(K);
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.kinematics.J = J;
|
|
stewart.kinematics.K = K;
|
|
stewart.kinematics.C = C;
|
|
#+end_src
|
|
|
|
** =initializeInertialSensor=: Initialize the inertial sensor in each strut
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeInertialSensor.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Geophone - Working Principle
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
From the schematic of the Z-axis geophone shown in Figure [[fig:z_axis_geophone]], we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
|
|
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
|
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
|
|
|
#+name: fig:z_axis_geophone
|
|
#+caption: Schematic of a Z-Axis geophone
|
|
[[file:figs/inertial_sensor.png]]
|
|
|
|
We see that at frequencies above $\omega_0$:
|
|
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
|
|
|
|
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
|
|
|
|
We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
|
|
|
|
*** Accelerometer - Working Principle
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
From the schematic of the Z-axis accelerometer shown in Figure [[fig:z_axis_accelerometer]], we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
|
|
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
|
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
|
|
|
#+name: fig:z_axis_accelerometer
|
|
#+caption: Schematic of a Z-Axis geophone
|
|
[[file:figs/inertial_sensor.png]]
|
|
|
|
We see that at frequencies below $\omega_0$:
|
|
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
|
|
|
|
And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
|
|
|
|
Note that there is trade-off between:
|
|
- the highest measurable acceleration $\omega_0$
|
|
- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [stewart] = initializeInertialSensor(stewart, args)
|
|
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
|
%
|
|
% Syntax: [stewart] = initializeInertialSensor(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - type - 'geophone', 'accelerometer', 'none'
|
|
% - mass [1x1] - Weight of the inertial mass [kg]
|
|
% - freq [1x1] - Cutoff frequency [Hz]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - stewart.sensors.inertial
|
|
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
|
% - K [1x1] - Stiffness [N/m]
|
|
% - C [1x1] - Damping [N/(m/s)]
|
|
% - M [1x1] - Inertial Mass [kg]
|
|
% - G [1x1] - Gain
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
|
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
|
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
|
end
|
|
#+end_src
|
|
|
|
*** Compute the properties of the sensor
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
sensor = struct();
|
|
|
|
switch args.type
|
|
case 'geophone'
|
|
sensor.type = 1;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
|
case 'accelerometer'
|
|
sensor.type = 2;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
|
sensor.G = -sensor.K/sensor.M;
|
|
case 'none'
|
|
sensor.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
*** Populate the =stewart= structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
stewart.sensors.inertial = sensor;
|
|
#+end_src
|
|
|
|
|
|
|
|
** =inverseKinematics=: Compute Inverse Kinematics
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/inverseKinematics.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Theory
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
|
|
|
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
|
\begin{align*}
|
|
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
|
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
|
\end{align*}
|
|
|
|
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
|
\begin{equation}
|
|
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
|
\end{equation}
|
|
|
|
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
|
\begin{equation}
|
|
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
|
\end{equation}
|
|
|
|
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
|
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [Li, dLi] = inverseKinematics(stewart, args)
|
|
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
|
%
|
|
% Syntax: [stewart] = inverseKinematics(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
|
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
|
% - geometry.l [6x1] - Length of each strut
|
|
% - args - Can have the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
%
|
|
% Outputs:
|
|
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
#+end_src
|
|
|
|
*** Check the =stewart= structure elements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
|
Aa = stewart.geometry.Aa;
|
|
|
|
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
|
Bb = stewart.geometry.Bb;
|
|
|
|
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
|
l = stewart.geometry.l;
|
|
#+end_src
|
|
|
|
|
|
*** Compute
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
dLi = Li-l;
|
|
#+end_src
|
|
|
|
** =describeMicroStationSetup=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/describeMicroStationSetup.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = describeMicroStationSetup()
|
|
% describeMicroStationSetup -
|
|
%
|
|
% Syntax: [] = describeMicroStationSetup()
|
|
%
|
|
% Inputs:
|
|
% - -
|
|
%
|
|
% Outputs:
|
|
% - -
|
|
#+end_src
|
|
|
|
*** Simscape Configuration
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('./mat/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
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('./mat/nass_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.Fty_x && args.Fty_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
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('./mat/nass_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
|
|
|
|
*** Controller
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('./mat/controller.mat', 'controller');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Controller:\n');
|
|
fprintf('- %s\n', controller.name);
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** Micro-Station
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('./mat/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
|
|
|
|
* 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
|
|
#+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
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
% Simulink Model name
|
|
mdl = 'ustation_simscape';
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(log10(10), log10(2e3), 1000);
|
|
#+END_SRC
|