Update the metrology study.
This commit is contained in:
651
tomo-exp/index.html
Normal file
651
tomo-exp/index.html
Normal file
@@ -0,0 +1,651 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2019-12-04 mer. 21:55 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Tomography Experiment</title>
|
||||
<meta name="generator" content="Org mode" />
|
||||
<meta name="author" content="Dehaeze Thomas" />
|
||||
<style type="text/css">
|
||||
<!--/*--><![CDATA[/*><!--*/
|
||||
.title { text-align: center;
|
||||
margin-bottom: .2em; }
|
||||
.subtitle { text-align: center;
|
||||
font-size: medium;
|
||||
font-weight: bold;
|
||||
margin-top:0; }
|
||||
.todo { font-family: monospace; color: red; }
|
||||
.done { font-family: monospace; color: green; }
|
||||
.priority { font-family: monospace; color: orange; }
|
||||
.tag { background-color: #eee; font-family: monospace;
|
||||
padding: 2px; font-size: 80%; font-weight: normal; }
|
||||
.timestamp { color: #bebebe; }
|
||||
.timestamp-kwd { color: #5f9ea0; }
|
||||
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
|
||||
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
|
||||
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
|
||||
.underline { text-decoration: underline; }
|
||||
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
|
||||
p.verse { margin-left: 3%; }
|
||||
pre {
|
||||
border: 1px solid #ccc;
|
||||
box-shadow: 3px 3px 3px #eee;
|
||||
padding: 8pt;
|
||||
font-family: monospace;
|
||||
overflow: auto;
|
||||
margin: 1.2em;
|
||||
}
|
||||
pre.src {
|
||||
position: relative;
|
||||
overflow: visible;
|
||||
padding-top: 1.2em;
|
||||
}
|
||||
pre.src:before {
|
||||
display: none;
|
||||
position: absolute;
|
||||
background-color: white;
|
||||
top: -10px;
|
||||
right: 10px;
|
||||
padding: 3px;
|
||||
border: 1px solid black;
|
||||
}
|
||||
pre.src:hover:before { display: inline;}
|
||||
/* Languages per Org manual */
|
||||
pre.src-asymptote:before { content: 'Asymptote'; }
|
||||
pre.src-awk:before { content: 'Awk'; }
|
||||
pre.src-C:before { content: 'C'; }
|
||||
/* pre.src-C++ doesn't work in CSS */
|
||||
pre.src-clojure:before { content: 'Clojure'; }
|
||||
pre.src-css:before { content: 'CSS'; }
|
||||
pre.src-D:before { content: 'D'; }
|
||||
pre.src-ditaa:before { content: 'ditaa'; }
|
||||
pre.src-dot:before { content: 'Graphviz'; }
|
||||
pre.src-calc:before { content: 'Emacs Calc'; }
|
||||
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
|
||||
pre.src-fortran:before { content: 'Fortran'; }
|
||||
pre.src-gnuplot:before { content: 'gnuplot'; }
|
||||
pre.src-haskell:before { content: 'Haskell'; }
|
||||
pre.src-hledger:before { content: 'hledger'; }
|
||||
pre.src-java:before { content: 'Java'; }
|
||||
pre.src-js:before { content: 'Javascript'; }
|
||||
pre.src-latex:before { content: 'LaTeX'; }
|
||||
pre.src-ledger:before { content: 'Ledger'; }
|
||||
pre.src-lisp:before { content: 'Lisp'; }
|
||||
pre.src-lilypond:before { content: 'Lilypond'; }
|
||||
pre.src-lua:before { content: 'Lua'; }
|
||||
pre.src-matlab:before { content: 'MATLAB'; }
|
||||
pre.src-mscgen:before { content: 'Mscgen'; }
|
||||
pre.src-ocaml:before { content: 'Objective Caml'; }
|
||||
pre.src-octave:before { content: 'Octave'; }
|
||||
pre.src-org:before { content: 'Org mode'; }
|
||||
pre.src-oz:before { content: 'OZ'; }
|
||||
pre.src-plantuml:before { content: 'Plantuml'; }
|
||||
pre.src-processing:before { content: 'Processing.js'; }
|
||||
pre.src-python:before { content: 'Python'; }
|
||||
pre.src-R:before { content: 'R'; }
|
||||
pre.src-ruby:before { content: 'Ruby'; }
|
||||
pre.src-sass:before { content: 'Sass'; }
|
||||
pre.src-scheme:before { content: 'Scheme'; }
|
||||
pre.src-screen:before { content: 'Gnu Screen'; }
|
||||
pre.src-sed:before { content: 'Sed'; }
|
||||
pre.src-sh:before { content: 'shell'; }
|
||||
pre.src-sql:before { content: 'SQL'; }
|
||||
pre.src-sqlite:before { content: 'SQLite'; }
|
||||
/* additional languages in org.el's org-babel-load-languages alist */
|
||||
pre.src-forth:before { content: 'Forth'; }
|
||||
pre.src-io:before { content: 'IO'; }
|
||||
pre.src-J:before { content: 'J'; }
|
||||
pre.src-makefile:before { content: 'Makefile'; }
|
||||
pre.src-maxima:before { content: 'Maxima'; }
|
||||
pre.src-perl:before { content: 'Perl'; }
|
||||
pre.src-picolisp:before { content: 'Pico Lisp'; }
|
||||
pre.src-scala:before { content: 'Scala'; }
|
||||
pre.src-shell:before { content: 'Shell Script'; }
|
||||
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
|
||||
/* additional language identifiers per "defun org-babel-execute"
|
||||
in ob-*.el */
|
||||
pre.src-cpp:before { content: 'C++'; }
|
||||
pre.src-abc:before { content: 'ABC'; }
|
||||
pre.src-coq:before { content: 'Coq'; }
|
||||
pre.src-groovy:before { content: 'Groovy'; }
|
||||
/* additional language identifiers from org-babel-shell-names in
|
||||
ob-shell.el: ob-shell is the only babel language using a lambda to put
|
||||
the execution function name together. */
|
||||
pre.src-bash:before { content: 'bash'; }
|
||||
pre.src-csh:before { content: 'csh'; }
|
||||
pre.src-ash:before { content: 'ash'; }
|
||||
pre.src-dash:before { content: 'dash'; }
|
||||
pre.src-ksh:before { content: 'ksh'; }
|
||||
pre.src-mksh:before { content: 'mksh'; }
|
||||
pre.src-posh:before { content: 'posh'; }
|
||||
/* Additional Emacs modes also supported by the LaTeX listings package */
|
||||
pre.src-ada:before { content: 'Ada'; }
|
||||
pre.src-asm:before { content: 'Assembler'; }
|
||||
pre.src-caml:before { content: 'Caml'; }
|
||||
pre.src-delphi:before { content: 'Delphi'; }
|
||||
pre.src-html:before { content: 'HTML'; }
|
||||
pre.src-idl:before { content: 'IDL'; }
|
||||
pre.src-mercury:before { content: 'Mercury'; }
|
||||
pre.src-metapost:before { content: 'MetaPost'; }
|
||||
pre.src-modula-2:before { content: 'Modula-2'; }
|
||||
pre.src-pascal:before { content: 'Pascal'; }
|
||||
pre.src-ps:before { content: 'PostScript'; }
|
||||
pre.src-prolog:before { content: 'Prolog'; }
|
||||
pre.src-simula:before { content: 'Simula'; }
|
||||
pre.src-tcl:before { content: 'tcl'; }
|
||||
pre.src-tex:before { content: 'TeX'; }
|
||||
pre.src-plain-tex:before { content: 'Plain TeX'; }
|
||||
pre.src-verilog:before { content: 'Verilog'; }
|
||||
pre.src-vhdl:before { content: 'VHDL'; }
|
||||
pre.src-xml:before { content: 'XML'; }
|
||||
pre.src-nxml:before { content: 'XML'; }
|
||||
/* add a generic configuration mode; LaTeX export needs an additional
|
||||
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
|
||||
pre.src-conf:before { content: 'Configuration File'; }
|
||||
|
||||
table { border-collapse:collapse; }
|
||||
caption.t-above { caption-side: top; }
|
||||
caption.t-bottom { caption-side: bottom; }
|
||||
td, th { vertical-align:top; }
|
||||
th.org-right { text-align: center; }
|
||||
th.org-left { text-align: center; }
|
||||
th.org-center { text-align: center; }
|
||||
td.org-right { text-align: right; }
|
||||
td.org-left { text-align: left; }
|
||||
td.org-center { text-align: center; }
|
||||
dt { font-weight: bold; }
|
||||
.footpara { display: inline; }
|
||||
.footdef { margin-bottom: 1em; }
|
||||
.figure { padding: 1em; }
|
||||
.figure p { text-align: center; }
|
||||
.equation-container {
|
||||
display: table;
|
||||
text-align: center;
|
||||
width: 100%;
|
||||
}
|
||||
.equation {
|
||||
vertical-align: middle;
|
||||
}
|
||||
.equation-label {
|
||||
display: table-cell;
|
||||
text-align: right;
|
||||
vertical-align: middle;
|
||||
}
|
||||
.inlinetask {
|
||||
padding: 10px;
|
||||
border: 2px solid gray;
|
||||
margin: 10px;
|
||||
background: #ffffcc;
|
||||
}
|
||||
#org-div-home-and-up
|
||||
{ text-align: right; font-size: 70%; white-space: nowrap; }
|
||||
textarea { overflow-x: auto; }
|
||||
.linenr { font-size: smaller }
|
||||
.code-highlighted { background-color: #ffff00; }
|
||||
.org-info-js_info-navigation { border-style: none; }
|
||||
#org-info-js_console-label
|
||||
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
|
||||
.org-info-js_search-highlight
|
||||
{ background-color: #ffff00; color: #000000; font-weight: bold; }
|
||||
.org-svg { width: 90%; }
|
||||
/*]]>*/-->
|
||||
</style>
|
||||
<script type="text/javascript">
|
||||
/*
|
||||
@licstart The following is the entire license notice for the
|
||||
JavaScript code in this tag.
|
||||
|
||||
Copyright (C) 2012-2019 Free Software Foundation, Inc.
|
||||
|
||||
The JavaScript code in this tag is free software: you can
|
||||
redistribute it and/or modify it under the terms of the GNU
|
||||
General Public License (GNU GPL) as published by the Free Software
|
||||
Foundation, either version 3 of the License, or (at your option)
|
||||
any later version. The code is distributed WITHOUT ANY WARRANTY;
|
||||
without even the implied warranty of MERCHANTABILITY or FITNESS
|
||||
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
|
||||
|
||||
As additional permission under GNU GPL version 3 section 7, you
|
||||
may distribute non-source (e.g., minimized or compacted) forms of
|
||||
that code without the copy of the GNU GPL normally required by
|
||||
section 4, provided you include this license notice and a URL
|
||||
through which recipients can access the Corresponding Source.
|
||||
|
||||
|
||||
@licend The above is the entire license notice
|
||||
for the JavaScript code in this tag.
|
||||
*/
|
||||
<!--/*--><![CDATA[/*><!--*/
|
||||
function CodeHighlightOn(elem, id)
|
||||
{
|
||||
var target = document.getElementById(id);
|
||||
if(null != target) {
|
||||
elem.cacheClassElem = elem.className;
|
||||
elem.cacheClassTarget = target.className;
|
||||
target.className = "code-highlighted";
|
||||
elem.className = "code-highlighted";
|
||||
}
|
||||
}
|
||||
function CodeHighlightOff(elem, id)
|
||||
{
|
||||
var target = document.getElementById(id);
|
||||
if(elem.cacheClassElem)
|
||||
elem.className = elem.cacheClassElem;
|
||||
if(elem.cacheClassTarget)
|
||||
target.className = elem.cacheClassTarget;
|
||||
}
|
||||
/*]]>*///-->
|
||||
</script>
|
||||
<script type="text/x-mathjax-config">
|
||||
MathJax.Hub.Config({
|
||||
displayAlign: "center",
|
||||
displayIndent: "0em",
|
||||
|
||||
"HTML-CSS": { scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
webFont: "TeX"
|
||||
},
|
||||
SVG: {scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
font: "TeX"},
|
||||
NativeMML: {scale: 100},
|
||||
TeX: { equationNumbers: {autoNumber: "AMS"},
|
||||
MultLineWidth: "85%",
|
||||
TagSide: "right",
|
||||
TagIndent: ".8em"
|
||||
}
|
||||
});
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="org-div-home-and-up">
|
||||
<a accesskey="h" href="../index.html"> UP </a>
|
||||
|
|
||||
<a accesskey="H" href="../index.html"> HOME </a>
|
||||
</div><div id="content">
|
||||
<h1 class="title">Tomography Experiment</h1>
|
||||
<div id="table-of-contents">
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#orgab97c4b">1. Initialize Experiment</a></li>
|
||||
<li><a href="#org26c554f">2. Run the Tomography Experiment</a></li>
|
||||
<li><a href="#orgc507e1e">3. <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</a>
|
||||
<ul>
|
||||
<li><a href="#org4537a86">3.1. Wanted Position of the Sample with respect to the Granite</a></li>
|
||||
<li><a href="#org6c67ae5">3.2. Measured Position of the Sample with respect to the Granite</a></li>
|
||||
<li><a href="#org69e38aa">3.3. Positioning Error with respect to the Granite</a></li>
|
||||
<li><a href="#org5246d94">3.4. Position Error Expressed in the Nano-Hexapod Frame</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgab97c4b" class="outline-2">
|
||||
<h2 id="orgab97c4b"><span class="section-number-2">1</span> Initialize Experiment</h2>
|
||||
<div class="outline-text-2" id="text-1">
|
||||
<p>
|
||||
We first initialize all the stages.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code>initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We initialize the reference path for all the stages.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
And we initialize the disturbances.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initDisturbances<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org26c554f" class="outline-2">
|
||||
<h2 id="org26c554f"><span class="section-number-2">2</span> Run the Tomography Experiment</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
We first load the simulation configuration
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, '<span class="org-highlight-numbers-number">1</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'sim_nano_station_tomo'</span>, <span class="org-string">'SimulationCommand'</span>, <span class="org-string">'start'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc507e1e" class="outline-2">
|
||||
<h2 id="orgc507e1e"><span class="section-number-2">3</span> <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<ul class="org-ul">
|
||||
<li class="on"><code>[X]</code> Are the rotation matrix commutable? => no</li>
|
||||
<li class="on"><code>[X]</code> How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book)</li>
|
||||
<li class="off"><code>[ ]</code> Should ask Veijo how he specifies the position of the Symetrie Hexapod</li>
|
||||
<li class="off"><code>[ ]</code> Create functions for all distinct part and then include that in Simulink</li>
|
||||
<li class="off"><code>[ ]</code> How the express the orientation error?</li>
|
||||
<li class="off"><code>[ ]</code> If we use screw coordinate, can we add/subtract them?</li>
|
||||
<li class="off"><code>[ ]</code> Do some simple tests to verify that the algorithm is working fine</li>
|
||||
</ul>
|
||||
|
||||
<blockquote>
|
||||
<p>
|
||||
Rx = [1 0 0;
|
||||
0 cos(t) -sin(t);
|
||||
0 sin(t) cos(t)];
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Ry = [ cos(t) 0 sin(t);
|
||||
0 1 0;
|
||||
-sin(t) 0 cos(t)];
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Rz = [cos(t) -sin(t) 0;
|
||||
sin(t) cos(t) 0;
|
||||
0 0 1];
|
||||
</p>
|
||||
</blockquote>
|
||||
|
||||
<p>
|
||||
Let's define the following frames:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>\(\{W\}\) the frame that is <b>fixed to the granite</b> and its origin at the theoretical meeting point between the X-ray and the spindle axis.</li>
|
||||
<li>\(\{S\}\) the frame <b>attached to the sample</b> (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\).</li>
|
||||
<li>\(\{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.</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
Thus:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>the <b>measurement</b> 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</li>
|
||||
<li>the <b>wanted position</b> 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</li>
|
||||
</ul>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4537a86" class="outline-3">
|
||||
<h3 id="org4537a86"><span class="section-number-3">3.1</span> Wanted Position of the Sample with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-1">
|
||||
<p>
|
||||
Let's define the wanted position of each stage.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Ty = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Ry = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rz = <span class="org-highlight-numbers-number">180</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Hexapod (first consider only translations)</span></code>
|
||||
<code>Thx = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Thy = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Thz = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
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\).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
To do so, we have to define the homogeneous transformation for each stage.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-comment">% Translation Stage</span></code>
|
||||
<code>Rty = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Ty;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Tilt Stage - Pure rotating aligned with Ob</span></code>
|
||||
<code>Rry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Spindle - Rotation along the Z axis</span></code>
|
||||
<code>Rrz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Micro-Hexapod (only rotations first)</span></code>
|
||||
<code>Rh = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> Thx ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Thy ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> Thz ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Ttot = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WOr = Ttot<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>WOr = WOr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>thetar = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>Ttot<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span></code>
|
||||
<code><span class="org-keyword">if</span> thetar <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
|
||||
<code> WSr = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code><span class="org-keyword">else</span></code>
|
||||
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>Ttot<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code> WSr = thetar<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type"><</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code><span class="org-keyword">end</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPr = <span class="org-rainbow-delimiters-depth-1">[</span>WOr ; WSr<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6c67ae5" class="outline-3">
|
||||
<h3 id="org6c67ae5"><span class="section-number-3">3.2</span> Measured Position of the Sample with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-2">
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-comment">% Measurements: Xm, Ym, Zm, Rx, Ry, Rz</span></code>
|
||||
<code>Dxm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Dym = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Dzm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code></code>
|
||||
<code>Rxm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rym = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rzm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Let's compute the corresponding orientation using screw axis.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
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:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>\(\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}\)</li>
|
||||
<li>\({}^W\bm{s}_m\) is the eigen vector of the rotation matrix \(R\) corresponding to the eigen value \(\lambda = 1\)</li>
|
||||
</ul>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>thetam = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>STw<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code><span class="org-keyword">if</span> thetam <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
|
||||
<code> WSm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code><span class="org-keyword">else</span></code>
|
||||
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>STw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code> WSm = thetam<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type"><</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code><span class="org-keyword">end</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPm = <span class="org-rainbow-delimiters-depth-1">[</span>Dxm ; Dym ; Dzm ; WSm<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org69e38aa" class="outline-3">
|
||||
<h3 id="org69e38aa"><span class="section-number-3">3.3</span> Positioning Error with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-3">
|
||||
<p>
|
||||
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 <b>position error</b> 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 \]
|
||||
</p>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPe = WPr <span class="org-type">-</span> WPm;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<blockquote>
|
||||
<p>
|
||||
Now we want to express this error in a frame attached to the <b>base of the nano-hexapod</b> 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).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Or maybe should we want to express this error with respect to the <b>top platform of the nano-hexapod</b>?
|
||||
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:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>from the encoders of each stage</li>
|
||||
<li>from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
</blockquote>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org5246d94" class="outline-3">
|
||||
<h3 id="org5246d94"><span class="section-number-3">3.4</span> Position Error Expressed in the Nano-Hexapod Frame</h3>
|
||||
<div class="outline-text-3" id="text-3-4">
|
||||
<p>
|
||||
We now want the position error to be expressed in \(\{S\}\) (the frame attach to the sample):
|
||||
\[ {}^S E = {}^S T_W \cdot {}^W E \]
|
||||
</p>
|
||||
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Translation Error.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>SEm = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>SEm = SEm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Rotation Error.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>SEr = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>SEr = SEr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Etot = <span class="org-rainbow-delimiters-depth-1">[</span>SEm ; SEr<span class="org-rainbow-delimiters-depth-1">]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2019-12-04 mer. 21:55</p>
|
||||
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
@@ -9,13 +9,13 @@
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
@@ -59,6 +59,7 @@
|
||||
#+end_src
|
||||
|
||||
* Initialize Experiment
|
||||
We first initialize all the stages.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
@@ -72,280 +73,274 @@
|
||||
initializeSample(struct('mass', 1));
|
||||
#+end_src
|
||||
|
||||
We initialize the reference path for all the stages.
|
||||
#+begin_src matlab
|
||||
t = 0:Ts:Tsim;
|
||||
initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));
|
||||
#+end_src
|
||||
|
||||
Generate perturbations
|
||||
And we initialize the disturbances.
|
||||
#+begin_src matlab
|
||||
win = hanning(ceil(1/Ts));
|
||||
[pxx, f] = pwelch(sqrt(1/(2*Ts))*randn(length(t), 1), win, [], [], 1/Ts);
|
||||
initDisturbances();
|
||||
#+end_src
|
||||
|
||||
* Run the Tomography Experiment
|
||||
We first load the simulation configuration
|
||||
#+begin_src matlab
|
||||
load('simscape/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(f, sqrt(pxx));
|
||||
hold off;
|
||||
set(gca, 'xscale', 'log');
|
||||
set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD of the measured velocity $\left[\frac{m/s}{\sqrt{Hz}}\right]$')
|
||||
ylim([0.01, 100]);
|
||||
set_param(conf_simscape, 'StopTime', '1');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
|
||||
Dwx = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
Dwy = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
Dwz = lsim(dist_f.G_gm, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
|
||||
Dw = [Dwx, Dwy, Dwz];
|
||||
set_param('sim_nano_station_tomo', 'SimulationCommand', 'start');
|
||||
#+end_src
|
||||
|
||||
* TODO Tests on the transformation from reference to wanted position
|
||||
- [X] Are the rotation matrix commutable? => no
|
||||
- [X] How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book)
|
||||
- [ ] Should ask Veijo how he specifies the position of the Symetrie Hexapod
|
||||
- [ ] Create functions for all distinct part and then include that in Simulink
|
||||
- [ ] How the express the orientation error?
|
||||
- [ ] If we use screw coordinate, can we add/subtract them?
|
||||
- [ ] Do some simple tests to verify that the algorithm is working fine
|
||||
|
||||
** 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
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, Dwx);
|
||||
plot(t, Dwy);
|
||||
plot(t, Dwz);
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
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
|
||||
Fty_z = lsim(dist_f.G_ty, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
Frz_z = lsim(dist_f.G_rz, sqrt(1/(2*Ts))*randn(length(t), 1), t);
|
||||
% 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
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, Fty_z);
|
||||
plot(t, Frz_z);
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Force [N]');
|
||||
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
|
||||
% Spindle
|
||||
Rz = 2*pi*t;
|
||||
|
||||
% Axisc
|
||||
Rm = zeros(length(t), 2);
|
||||
Rm(:, 2) = pi*ones(length(t), 1);
|
||||
|
||||
inputs = struct( ...
|
||||
'Ts', Ts, ...
|
||||
'Dw', timeseries(Dw, t), ...
|
||||
'Dy', timeseries(zeros(length(t), 1), t), ...
|
||||
'Ry', timeseries(zeros(length(t), 1), t), ...
|
||||
'Rz', timeseries(Rz, t), ...
|
||||
'Dh', timeseries(zeros(length(t), 6), t), ...
|
||||
'Rm', timeseries(Rm, t), ...
|
||||
'Dn', timeseries(zeros(length(t), 6), t), ...
|
||||
'Ds', timeseries(zeros(length(t), 6), t), ...
|
||||
'Fg', timeseries(zeros(length(t), 3), t), ...
|
||||
'Fn', timeseries(zeros(length(t), 6), t), ...
|
||||
'Fnl', timeseries(zeros(length(t), 6), t), ...
|
||||
'Fty_x', timeseries(zeros(length(t), 1), t), ...
|
||||
'Fty_z', timeseries(Fty_z, t), ...
|
||||
'Frz_z', timeseries(Frz_z, t), ...
|
||||
'Fs', timeseries(zeros(length(t), 6), t) ...
|
||||
);
|
||||
#+end_src
|
||||
|
||||
* Test
|
||||
#+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>>
|
||||
WOr = Ttot*[0;0;0;1];
|
||||
WOr = WOr(1:3);
|
||||
#+end_src
|
||||
|
||||
Rotation.
|
||||
#+begin_src matlab
|
||||
cd ..
|
||||
#+end_src
|
||||
|
||||
We define some parameters:
|
||||
- =T0= is the duration in seconds
|
||||
- =N= is the number of samples
|
||||
|
||||
#+begin_src matlab
|
||||
T0 = 10; % Signal Duration [s]
|
||||
N = 10000; % Number of Samples (should be even)
|
||||
#+end_src
|
||||
|
||||
Then, we have:
|
||||
- $f_s = N/T_0$ is the sampling frequency in Hz
|
||||
- $f_c = \frac{1}{2} N/T_0$ is the cut-off frequency in Hz
|
||||
- $f_0 = 1/T_0$ is the frequency resolution of the DFT in Hz
|
||||
#+begin_src matlab
|
||||
Fs = N/T0; % Sampling frequency [Hz]
|
||||
Fc = (1/2)*N/T0; % Sampling frequency [Hz]
|
||||
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
||||
#+end_src
|
||||
|
||||
We then specify the wanted PSD.
|
||||
#+begin_src matlab
|
||||
phi = ones(N/2, 1);
|
||||
% phi = logspace(3, 0, N/2);
|
||||
phi(df:df:Fs/2>10) = 0;
|
||||
#+end_src
|
||||
|
||||
We create $C_x(k)$ such that:
|
||||
\[ C_x(k) = \sqrt{\Phi_{xx}(k\omega_0)\omega_0} \quad k = 1 \dots N/2 \]
|
||||
where $\Phi_{xx}$ is the wanted PSD.
|
||||
#+begin_src matlab
|
||||
C = zeros(N/2, 1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i))*2*Fs; % TODO - Why this normalization?
|
||||
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
|
||||
|
||||
We generate some random phase that will be added to =C=.
|
||||
#+begin_src matlab
|
||||
theta = 2*pi*rand(N/2, 1); % Generate random phase [rad]
|
||||
WPr = [WOr ; WSr];
|
||||
#+end_src
|
||||
|
||||
In order to have
|
||||
\[ C_x(N/2+i) = C_x^*(N/2-i) \quad i = 1 \dots N/2 \]
|
||||
We do the following
|
||||
** 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
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
% 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
|
||||
|
||||
The time domain data is generated by an inverse FFT.
|
||||
Let's compute the corresponding orientation using screw axis.
|
||||
#+begin_src matlab
|
||||
u = ifft(Cx);
|
||||
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
|
||||
A = fft(u);
|
||||
figure; plot(2*A.*conj(A))
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
t = linspace(0, T0, N+1); % Time Vector [s]
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
plot(t, u)
|
||||
xlabel('Time [s]');
|
||||
ylabel('Amplitude');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
nx = length(u);
|
||||
na = 16;
|
||||
win = hanning(floor(nx/na));
|
||||
|
||||
[pxx, f] = pwelch(u, win, 0, [], Fs);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(f,pxx)
|
||||
plot(df:df:Fc,phi)
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Power Spectral Density');
|
||||
set(gca, 'xscale', 'log');
|
||||
set(gca, 'yscale', 'log');
|
||||
#+end_src
|
||||
|
||||
* Test
|
||||
#+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
|
||||
cd ..
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
#+end_src
|
||||
|
||||
We remove the first value with very high PSD.
|
||||
#+begin_src matlab
|
||||
dist_f.f = dist_f.f(3:end);
|
||||
dist_f.psd_gm = dist_f.psd_gm(3:end);
|
||||
#+end_src
|
||||
|
||||
We define some parameters.
|
||||
#+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))
|
||||
#+end_src
|
||||
|
||||
We then specify the wanted PSD.
|
||||
#+begin_src matlab
|
||||
phi = dist_f.psd_gm;
|
||||
#+end_src
|
||||
|
||||
Create amplitudes corresponding to wanted PSD.
|
||||
#+begin_src matlab
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
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
|
||||
|
||||
Add random phase to =C=.
|
||||
#+begin_src matlab
|
||||
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)))];;
|
||||
WPm = [Dxm ; Dym ; Dzm ; WSm];
|
||||
#+end_src
|
||||
|
||||
The time domain data is generated by an inverse FFT.
|
||||
We normalize the =ifft= Matlab command.
|
||||
** 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
|
||||
u = 1/(sqrt(2)*df*1/Fs)*ifft(Cx); % Normalisation of the IFFT
|
||||
t = linspace(0, T0, N+1); % Time Vector [s]
|
||||
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
|
||||
figure;
|
||||
plot(t, u)
|
||||
xlabel('Time [s]');
|
||||
ylabel('Amplitude');
|
||||
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
|
||||
u_rep = [u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u;u];
|
||||
MTr = STw'*Ttot;
|
||||
#+end_src
|
||||
|
||||
Position error:
|
||||
#+begin_src matlab
|
||||
nx = length(u_rep);
|
||||
na = 16;
|
||||
win = hanning(floor(nx/na));
|
||||
|
||||
[pxx, f] = pwelch(u_rep, win, 0, [], Fs);
|
||||
MTr(1:3, 1:4)*[0; 0; 0; 1]
|
||||
#+end_src
|
||||
|
||||
Orientation error:
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(dist_f.f, dist_f.psd_gm, 'DisplayName', 'Original PSD')
|
||||
plot(f, pxx, 'DisplayName', 'Computed')
|
||||
% plot(f, pxx./dist_f.psd_gm, 'k-')
|
||||
hold off;
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Power Spectral Density');
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
legend('location', 'northeast');
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user