4444 lines
142 KiB
Org Mode
4444 lines
142 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', 'flexible');
|
|
initializeTy( 'type', 'flexible');
|
|
initializeRy( 'type', 'flexible');
|
|
initializeRz( 'type', 'flexible');
|
|
initializeMicroHexapod('type', 'flexible');
|
|
|
|
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
|
|
|
|
** DONE [#B] Just keep smallest number of variants for each stage
|
|
CLOSED: [2024-10-30 Wed 16:15]
|
|
|
|
- [ ] none
|
|
- [ ] rigid
|
|
- [ ] flexible
|
|
- [X] Init => Removed
|
|
- [X] modal analysis => Removed
|
|
|
|
** TODO [#A] Verify that we get "correct" compliance
|
|
SCHEDULED: <2024-10-30 Wed>
|
|
|
|
- [ ] Find the compliance measurements
|
|
- The one from modal analysis
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/2018-01-12 - Marc]] : Compliance measurement in X,Y,Z with geophone, marble not glued
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/2018-10-12 - Marc]]: same but in the hutch with glued marble
|
|
- *08/2020*: [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/micro-station-compliance/index.org::+TITLE: Compliance Measurement of the Micro Station]]
|
|
- [ ] See if it matches somehow the current model
|
|
- [ ] If not, see if model parameters can be tuned to have better match
|
|
For instance from values here: file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
|
|
|
|
** TODO [#B] Make good "init" for the Simscape model
|
|
|
|
** TODO [#C] Could see the effect of each stage on the compliance
|
|
|
|
- [ ] Put =rigid= mode one by one to see the effect
|
|
|
|
** TODO [#C] Make a comparison of the measured vibrations of the micro-station with the vibrations of the simscape model of the micro-station
|
|
Do we have a correlation? At least in the frequency domain?
|
|
|
|
Procedure:
|
|
- [ ] Take the time domain measurement of the vibrations due to spindle or translation stage
|
|
- [ ] Take the estimated PSD of the estimated disturbance force
|
|
- [ ] Simulate the Simscape model without the nano-hexapod (same conditions as when measuring the disturbances) and add only the considered disturbance
|
|
- [ ] Save the position errors due to the disturbance
|
|
- [ ] Compare with the measured vibrations
|
|
|
|
|
|
** TODO [#C] Add two perturbations: =Frz_x= and =Frz_y=
|
|
Maybe I can estimate them from the measurements that was made on the spindle?
|
|
|
|
** TODO [#C] Determine which Degree-Of-Freedom to keep and which to constrain
|
|
|
|
For instance, for now the granite can not rotate, but in reality, the modes may be linked to the granite's rotation.
|
|
By constraining more DoF, the simulation will be faster and the obtain state space will have a lower order.
|
|
|
|
** TODO [#B] Compute eigenvalues of the model to see if we have similar frequencies than the modal analysis?
|
|
|
|
* 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', 'flexible');
|
|
initializeTy( 'type', 'flexible');
|
|
initializeRy( 'type', 'flexible');
|
|
initializeRz( 'type', 'flexible');
|
|
initializeMicroHexapod('type', 'flexible');
|
|
|
|
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/Flexible/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Granite/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/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
|
|
|
|
** Measured micro-station compliance
|
|
*** Introduction :ignore:
|
|
|
|
The most important dynamical characteristic of the micro-station that should be well modeled is its compliance.
|
|
This is what can impact the nano-hexapod dynamics.
|
|
|
|
- [ ] Add schematic of the experiment with Micro-Hexapod top platform, location of accelerometers, of impacts, etc...
|
|
|
|
- 4 3-axis accelerometers
|
|
- 10 hammer impacts on the micro-hexapod top plaftorm
|
|
- *Was the rotation compensation axis present?* (I don't think so)
|
|
|
|
*** Position of inertial sensors on top of the micro-hexapod
|
|
Orientation is relative to the frame determined by the X-ray
|
|
|
|
| *Num* | *Position* | *Orientation* | *Sensibility* | *Channels* |
|
|
|-------+------------+---------------+---------------+------------|
|
|
| 1 | [0, +A, 0] | [x, y, z] | 1V/g | 1-3 |
|
|
| 2 | [-B, 0, 0] | [x, y, z] | 1V/g | 4-6 |
|
|
| 3 | [0, -A, 0] | [x, y, z] | 0.1V/g | 7-9 |
|
|
| 4 | [+B, 0, 0] | [x, y, z] | 1V/g | 10-12 |
|
|
|
|
Instrumented Hammer:
|
|
- Channel 13
|
|
- Sensibility: 230 uV/N
|
|
|
|
| Acc Number | Dir | Channel Number |
|
|
|------------+-----+----------------|
|
|
| 1 | x | 1 |
|
|
| 1 | y | 2 |
|
|
| 1 | z | 3 |
|
|
| 2 | x | 4 |
|
|
| 2 | y | 5 |
|
|
| 2 | z | 6 |
|
|
| 3 | x | 7 |
|
|
| 3 | y | 8 |
|
|
| 3 | z | 9 |
|
|
| 4 | x | 10 |
|
|
| 4 | y | 11 |
|
|
| 4 | z | 12 |
|
|
| Hammer | | 13 |
|
|
|
|
From the acceleration measurement of the 4 accelerometers, we can compute the translations and rotations:
|
|
| | *Formula* | *Formula (numbers)* |
|
|
|-------+--------------------------+-----------------------|
|
|
| $D_x$ | (1x + 2x + 3x + 4x)/4 | (1 + 4 + 7 + 10)/4 |
|
|
| $D_y$ | (1y + 2y + 3y + 4y)/4 | (2 + 5 + 8 + 11)/4 |
|
|
| $D_z$ | (1z + 2z + 3z + 4z)/4 | (3 + 6 + 9 + 12)/4 |
|
|
| $R_x$ | (1z - 3z)/A | (1 - 9)/A |
|
|
| $R_y$ | (2z - 4z)/B | (6 - 12)/B |
|
|
| $R_z$ | (3x - 1x)/A, (4y - 2y)/B | (7 - 1)/A, (11 - 5)/B |
|
|
|
|
dL = J X
|
|
#+begin_src matlab
|
|
d = 0.14;
|
|
J = [1 0 0 0 0 -d;
|
|
0 1 0 0 0 0;
|
|
0 0 1 d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 1 0 0 0 -d;
|
|
0 0 1 0 d 0;
|
|
1 0 0 0 0 d;
|
|
0 1 0 0 0 0;
|
|
0 0 1 -d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 1 0 0 0 d;
|
|
0 0 1 0 -d 0];
|
|
|
|
J_inv = pinv(J);
|
|
#+end_src
|
|
|
|
| | Dx | Dy | Dz | Rx | Ry | Rz |
|
|
|-----+----+----+----+----+----+----|
|
|
| a1x | 1 | 0 | 0 | 0 | 0 | -d |
|
|
| a1y | 0 | 1 | 0 | 0 | 0 | 0 |
|
|
| a1z | 0 | 0 | 1 | d | 0 | 0 |
|
|
| a2x | 1 | 0 | 0 | 0 | 0 | 0 |
|
|
| a2y | 0 | 1 | 0 | 0 | 0 | -d |
|
|
| a2z | 0 | 0 | 1 | 0 | d | 0 |
|
|
| a3x | 1 | 0 | 0 | 0 | 0 | d |
|
|
| a3y | 0 | 1 | 0 | 0 | 0 | 0 |
|
|
| a3z | 0 | 0 | 1 | -d | 0 | 0 |
|
|
| a4x | 1 | 0 | 0 | 0 | 0 | 0 |
|
|
| a4y | 0 | 1 | 0 | 0 | 0 | d |
|
|
| a4z | 0 | 0 | 1 | 0 | -d | 0 |
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
|
data2orgtable(J_inv, {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'d1x', 'd1y', 'd1z', 'd2x', 'd2y', 'd2z', 'd3x', 'd3y', 'd3z', 'd4x', 'd4y', 'd4z'}, ' %.5f ');
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| | d1x | d1y | d1z | d2x | d2y | d2z | d3x | d3y | d3z | d4x | d4y | d4z |
|
|
|----+----------+------+---------+------+----------+---------+---------+------+----------+------+---------+----------|
|
|
| Dx | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 |
|
|
| Dy | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 |
|
|
| Dz | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 | 0.0 | 0.0 | 0.25 |
|
|
| Rx | 0.0 | 0.0 | 3.57143 | 0.0 | 0.0 | -0.0 | 0.0 | 0.0 | -3.57143 | 0.0 | 0.0 | -0.0 |
|
|
| Ry | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 3.57143 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | -3.57143 |
|
|
| Rz | -1.78571 | 0.0 | -0.0 | 0.0 | -1.78571 | 0.0 | 1.78571 | 0.0 | -0.0 | 0.0 | 1.78571 | -0.0 |
|
|
|
|
*** Hammer blow position/orientation
|
|
|
|
| *Num* | *Direction* | *Position* | Accelerometer position | Jacobian number |
|
|
|-------+-------------+------------+------------------------+-----------------|
|
|
| 1 | -Y | [0, +A, 0] | 1 | -2 |
|
|
| 2 | -Z | [0, +A, 0] | 1 | -3 |
|
|
| 3 | X | [-B, 0, 0] | 2 | 4 |
|
|
| 4 | -Z | [-B, 0, 0] | 2 | -6 |
|
|
| 5 | Y | [0, -A, 0] | 3 | 8 |
|
|
| 6 | -Z | [0, -A, 0] | 3 | -9 |
|
|
| 7 | -X | [+B, 0, 0] | 4 | -10 |
|
|
| 8 | -Z | [+B, 0, 0] | 4 | -12 |
|
|
| 9 | -X | [0, -A, 0] | 3 | -7 |
|
|
| 10 | -X | [0, +A, 0] | 1 | -1 |
|
|
|
|
From hammer blows to pure forces / torques:
|
|
| | *Formula* | Alternative |
|
|
|-------+--------------+-------------|
|
|
| $F_x$ | +3 | -7 |
|
|
| $F_y$ | -1 | +5 |
|
|
| $F_z$ | -(2 + 6)/2 | -(4 + 8)/2 |
|
|
| $M_x$ | A/2*(2 - 6) | |
|
|
| $M_y$ | B/2*(8 - 4) | |
|
|
| $M_z$ | A/2*(10 - 9) | |
|
|
|
|
F = J' tau
|
|
#+begin_src matlab
|
|
Jf = [0 -1 0 0 0 0;
|
|
0 0 -1 -d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 0 -1 0 -d 0;
|
|
0 1 0 0 0 0;
|
|
0 0 -1 d 0 0;
|
|
-1 0 0 0 0 0;
|
|
0 0 -1 0 d 0;
|
|
-1 0 0 0 0 -d;
|
|
-1 0 0 0 0 d]';
|
|
Jf_inv = pinv(Jf);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
|
data2orgtable(Jf, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}, {'-F1y', '-F1z', '+F2x', '-F2z', '+F3y', '-F3z', '-F4x', '-F4z', '-F3x', '-F1x'}, ' %.2f ');
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| | -F1y | -F1z | +F2x | -F2z | +F3y | -F3z | -F4x | -F4z | -F3x | -F1x |
|
|
|----+------+-------+------+-------+------+------+------+------+-------+------|
|
|
| Fx | 0.0 | 0.0 | 1.0 | 0.0 | 0.0 | 0.0 | -1.0 | 0.0 | -1.0 | -1.0 |
|
|
| Fy | -1.0 | 0.0 | 0.0 | 0.0 | 1.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 |
|
|
| Fz | 0.0 | -1.0 | 0.0 | -1.0 | 0.0 | -1.0 | 0.0 | -1.0 | 0.0 | 0.0 |
|
|
| Mx | 0.0 | -0.14 | 0.0 | 0.0 | 0.0 | 0.14 | 0.0 | 0.0 | 0.0 | 0.0 |
|
|
| My | 0.0 | 0.0 | 0.0 | -0.14 | 0.0 | 0.0 | 0.0 | 0.14 | 0.0 | 0.0 |
|
|
| Mz | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | -0.14 | 0.14 |
|
|
|
|
*** Compute FRF
|
|
Raw measurements are in file:/home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/micro-station-compliance/data/record
|
|
|
|
For each measurement (10):
|
|
- Find the location of the 10 impacts based on "track13"
|
|
- Then for the 12 accelerometer data, compute the FRF, and average them for the 10 impacts
|
|
- Maybe have to take into account the sensitivity, etc...
|
|
|
|
#+begin_src matlab
|
|
raw_data_path = '/home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/micro-station-compliance/data/record/';
|
|
data = [
|
|
load(sprintf('%s/Measurement1.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement2.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement3.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement4.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement5.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement6.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement7.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement8.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement9.mat', raw_data_path)), ...
|
|
load(sprintf('%s/Measurement10.mat', raw_data_path))];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Ts = 1e-3; % Sampling Time [s]
|
|
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
|
% win = hanning(Nfft); % Hanning window
|
|
win = ones(Nfft, 1); % Rectangular window
|
|
% Get the frequency vector
|
|
[~, f] = tfestimate(data(1).Track13, data(1).Track1, win, [], Nfft, 1/Ts);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
pre_n = floor(0.1/Ts);
|
|
post_n = Nfft - pre_n - 1;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
G_raw = zeros(12,10,length(f));
|
|
for i = 1:10
|
|
% Find the impacts
|
|
[~, impacts_i] = find(diff(data(i).Track13 > 50)==1);
|
|
% Only keep the first 10 impacts if there are more than 10 impacts
|
|
if length(impacts_i)>10
|
|
impacts_i(11:end) = [];
|
|
end
|
|
% Initialize the FRF for the current experiment
|
|
G_impact = zeros(12,length(f));
|
|
for impact_i = impacts_i
|
|
i_start = impacts_i - pre_n;
|
|
i_end = impacts_i + post_n;
|
|
data_in = data(i).Track13(i_start:i_end); % [N]
|
|
% Remove hammer DC offset
|
|
data_in = data_in - mean(data_in(end-pre_n:end));
|
|
% Combine all outputs [m/s^2]
|
|
data_out = [data(i).Track1( i_start:i_end); ...
|
|
data(i).Track2( i_start:i_end); ...
|
|
data(i).Track3( i_start:i_end); ...
|
|
data(i).Track4( i_start:i_end); ...
|
|
data(i).Track5( i_start:i_end); ...
|
|
data(i).Track6( i_start:i_end); ...
|
|
data(i).Track7( i_start:i_end); ...
|
|
data(i).Track8( i_start:i_end); ...
|
|
data(i).Track9( i_start:i_end); ...
|
|
data(i).Track10(i_start:i_end); ...
|
|
data(i).Track11(i_start:i_end); ...
|
|
data(i).Track12(i_start:i_end)];
|
|
|
|
[frf, ~] = tfestimate(data_in, data_out', win, [], Nfft, 1/Ts);
|
|
G_raw(:,i,:) = frf'./(-(2*pi*f').^2);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Compute transfer function in cartesian frame
|
|
G_compl_b = pagemtimes(J_inv, pagemtimes(G_raw, Jf_inv));
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
colors = colororder;
|
|
figure;
|
|
hold on;
|
|
% plot(freqs, abs(squeeze(G_compl(1,1,:))), 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
|
|
% plot(freqs, abs(squeeze(G_compl(2,2,:))), 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
|
|
% plot(freqs, abs(squeeze(G_compl(3,3,:))), 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
|
|
plot(f, abs(squeeze(G_compl_b(1,1,:))), '-', 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
|
|
plot(f, abs(squeeze(G_compl_b(2,2,:))), '-', 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
|
|
plot(f, abs(squeeze(G_compl_b(3,3,:))), '-', 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
|
xlim([30, 300]); ylim([1e-9, 2e-6]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
*** Load Data
|
|
#+begin_src matlab
|
|
% Load data
|
|
data = [load('data/Measurement1.mat'), ...
|
|
load('data/Measurement2.mat'), ...
|
|
load('data/Measurement3.mat'), ...
|
|
load('data/Measurement4.mat'), ...
|
|
load('data/Measurement5.mat'), ...
|
|
load('data/Measurement6.mat'), ...
|
|
load('data/Measurement7.mat'), ...
|
|
load('data/Measurement8.mat'), ...
|
|
load('data/Measurement9.mat'), ...
|
|
load('data/Measurement10.mat')];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
% Frequency Vector
|
|
freqs = m3.FFT1_H1_1_13_X_Val;
|
|
w = 2*pi*freqs;
|
|
|
|
% 12 outputs, 10 inputs
|
|
G_raw = zeros(12, 10, length(freqs));
|
|
|
|
for j = 1:10
|
|
for i = 1:12
|
|
G_raw(i,j,:) = data(j).(sprintf("FFT1_H1_%i_13_Y_ReIm", i))./(-w.^2);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Compute transfer function in cartesian frame
|
|
G_compl = pagemtimes(J_inv, pagemtimes(G_raw, Jf_inv));
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
colors = colororder;
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G_compl(1,1,:))), 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
|
|
plot(freqs, abs(squeeze(G_compl(2,2,:))), 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
|
|
plot(freqs, abs(squeeze(G_compl(3,3,:))), 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
|
|
plot(f, abs(squeeze(G_compl_b(1,1,:))), '.', 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
|
|
plot(f, abs(squeeze(G_compl_b(2,2,:))), '.', 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
|
|
plot(f, abs(squeeze(G_compl_b(3,3,:))), '.', 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
|
xlim([30, 300]); ylim([1e-9, 2e-6]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
colors = colororder;
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G_compl(4,4,:))), 'color', colors(1,:), 'DisplayName', '$R_x/M_x$')
|
|
plot(freqs, abs(squeeze(G_compl(5,5,:))), 'color', colors(2,:), 'DisplayName', '$R_y/M_y$')
|
|
plot(freqs, abs(squeeze(G_compl(6,6,:))), 'color', colors(3,:), 'DisplayName', '$R_z/M_z$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
|
xlim([30, 300]); ylim([5e-7, 5e-5]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
*** Diagonal Dynamics
|
|
#+begin_src matlab
|
|
figure;
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G(1,1,:)), '-', 'DisplayName', '$D_x/F_x$')
|
|
plot(freqs, abs(squeeze(G(2,2,:)), '-', 'DisplayName', '$D_y/F_y$')
|
|
plot(freqs, abs(squeeze(G(3,3,:)), '-', 'DisplayName', '$D_z/F_z$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
|
ylim([1e-9, 2e-6]);
|
|
legend('location', 'southwest');
|
|
xlim([30, 300]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G(4,4,:)), '-', 'DisplayName', '$R_x/M_x$')
|
|
plot(freqs, abs(squeeze(G(5,5,:)), '-', 'DisplayName', '$R_y/M_y$')
|
|
plot(freqs, abs(squeeze(G(6,6,:)), '-', 'DisplayName', '$R_z/M_z$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [rad/Nm]');
|
|
ylim([1e-7, 2e-4]);
|
|
legend('location', 'southwest');
|
|
xlim([30, 300]);
|
|
#+end_src
|
|
|
|
*** Equivalent Stiffness and Mass Estimation
|
|
|
|
#+begin_src matlab
|
|
K = [1e7, 1e7, 2e8, 5e7, 3e7, 2e7];
|
|
f_res = [125, 135, 390, 335, 335, 160];
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
M = [20, 20, 20, 11, 7, 20];
|
|
f_res_est = sqrt(K./M)./(2*pi);
|
|
#+end_src
|
|
|
|
Here is the inertia / stiffness to the granite that can represent the micro-station compliance dynamics:
|
|
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
|
data2orgtable([K'], {'x', 'y', 'z', 'Rx', 'Ry', 'Rz'}, {'Stiffness', 'Inertia'}, ' %.1g ');
|
|
#+end_src
|
|
|
|
|
|
#+RESULTS:
|
|
| Stiffness | Inertia |
|
|
|-----------+-------------|
|
|
| x | 10000000.0 |
|
|
| y | 10000000.0 |
|
|
| z | 200000000.0 |
|
|
| Rx | 50000000.0 |
|
|
| Ry | 30000000.0 |
|
|
| Rz | 20000000.0 |
|
|
|
|
** Compare with the Model
|
|
#+begin_src matlab
|
|
%% Initialize simulation with default parameters (flexible elements)
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
|
|
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/Flexible/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/Flexible/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
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G(1,1,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(2,2,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(3,3,:))./(-w.^2)), '.')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(2,2,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(3,3,:), freqs, 'Hz'))), '-')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 2e-6]);
|
|
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(G(1,1,:))./(-w.^2)), '.', 'DisplayName', '$D_x/F_x$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(2,2,:))./(-w.^2)), '.', 'DisplayName', '$D_y/F_y$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(3,3,:))./(-w.^2)), '.', 'DisplayName', '$D_z/F_z$')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(2,2,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(3,3,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
legend('location', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([30, 300]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/compliance_diagonal_translations_comp_model.pdf', 'width', 'full', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+name: fig:compliance_diagonal_translations_comp_model
|
|
#+caption: Dynamics from Forces to Translations
|
|
#+RESULTS:
|
|
[[file:figs/compliance_diagonal_translations_comp_model.png]]
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G(4,4,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(5,5,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(6,6,:))./(-w.^2)), '.')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gm(4,4,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(5,5,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(6,6,:), freqs, 'Hz'))), '-')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude [rad/Nm]'); set(gca, 'XTickLabel',[]);
|
|
% ylim([1e-9, 2e-6]);
|
|
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(G(4,4,:))./(-w.^2)), '.', 'DisplayName', '$R_x/M_x$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(5,5,:))./(-w.^2)), '.', 'DisplayName', '$R_y/M_y$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(6,6,:))./(-w.^2)), '.', 'DisplayName', '$R_z/M_z$')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(4,4,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(5,5,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(6,6,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
legend('location', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([30, 300]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/compliance_diagonal_rotations_comp_model.pdf', 'width', 'full', 'height', 'full');
|
|
#+end_src
|
|
|
|
#+name: fig:compliance_diagonal_rotations_comp_model
|
|
#+caption: Dynamics from Torques to Rotations
|
|
#+RESULTS:
|
|
[[file:figs/compliance_diagonal_rotations_comp_model.png]]
|
|
|
|
| | Stiffness | Unit |
|
|
|-----------+-----------+----------|
|
|
| $K_x$ | 1e7 | [N/m] |
|
|
| $K_y$ | 1e7 | [N/m] |
|
|
| $K_z$ | 2e8 | [N/m] |
|
|
| $K_{R_x}$ | 5e7 | [Nm/rad] |
|
|
| $K_{R_y}$ | 3e7 | [Nm/rad] |
|
|
| $K_{R_z}$ | 2e7 | [Nm/rad] |
|
|
|
|
*** Coupling Dynamics
|
|
#+begin_src matlab
|
|
figure;
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G(1,1,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(2,1,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(3,1,:))./(-w.^2)), '.')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(2,1,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(3,1,:), freqs, 'Hz'))), '-')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 2e-6]);
|
|
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(G(1,1,:))./(-w.^2)), '.', 'DisplayName', '$D_x/F_x$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(2,1,:))./(-w.^2)), '.', 'DisplayName', '$D_y/F_x$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(3,1,:))./(-w.^2)), '.', 'DisplayName', '$D_z/F_x$')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(2,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(3,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
legend('location', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([30, 300]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
figure;
|
|
ax1 = subplot(2,1,1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(G(5,1,:))./(-w.^2)), '.')
|
|
plot(freqs, abs(squeeze(G(4,2,:))./(-w.^2)), '.')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gm(5,1,:), freqs, 'Hz'))), '-')
|
|
plot(freqs, abs(squeeze(freqresp(Gm(4,2,:), freqs, 'Hz'))), '-')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 2e-6]);
|
|
|
|
ax2 = subplot(2,1,2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(G(5,1,:))./(-w.^2)), '.', 'DisplayName', '$R_y/F_x$')
|
|
plot(freqs, 180/pi*angle(squeeze(G(4,2,:))./(-w.^2)), '.', 'DisplayName', '$R_x/F_y$')
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(5,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(4,2,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
legend('location', 'southwest');
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([30, 300]);
|
|
#+end_src
|
|
|
|
|
|
|
|
** 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'})} = '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;
|
|
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'})} = '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;
|
|
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'})} = '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;
|
|
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'})} = '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;
|
|
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'})} = '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;
|
|
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
|