tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Split the metrology folder into two folders

Browse Source
This commit is contained in:
Thomas Dehaeze 2019-12-11 17:33:16 +01:00
parent 69f9348d6a
commit 021965a2b4
5 changed files with 1638 additions and 1423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

690
metrology/index.html
View File

@ -3,7 +3,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2019-12-11 mer. 10:28 --> <!-- 2019-12-11 mer. 17:33 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Metrology</title> <title>Metrology</title>
@ -283,76 +283,35 @@ for the JavaScript code in this tag.
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#orgca47132">1. How do we measure the position of the sample with respect to the granite</a></li> <li><a href="#orgdf449a5">1. How do we measure the position of the sample with respect to the granite</a></li>
<li><a href="#orgb0ca1ab">2. Verify that the function to compute the reference pose is correct</a>
<ul>
<li><a href="#org824aee5">2.1. Prepare the Simulation</a></li>
<li><a href="#org3530c48">2.2. Verify that the pose of the sample is the same as the computed one</a></li>
<li><a href="#org40ffd7b">2.3. Conclusion</a></li>
</ul>
</li>
<li><a href="#org3fda22b">3. Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working</a>
<ul>
<li><a href="#orgd4dab89">3.1. Prepare the Simulation</a></li>
<li><a href="#orge43864b">3.2. Compute the wanted pose of the sample in the NASS Base from the metrology and the reference</a></li>
<li><a href="#orgfc9d145">3.3. Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end</a></li>
<li><a href="#org4293675">3.4. Conclusion</a></li>
</ul>
</li>
<li><a href="#org1449831">4. Functions</a>
<ul>
<li><a href="#org3a6492a">4.1. computeReferencePose</a></li>
</ul>
</li>
</ul> </ul>
</div> </div>
</div> </div>
<p> <p>
The global measurement and control schematic is shown in figure <a href="#orgf9367e8">1</a>. The global measurement and control schematic is shown in figure <a href="#org1be8cfd">1</a>.
</p> </p>
<div id="orgf9367e8" class="figure"> <div id="org1be8cfd" class="figure">
<p><img src="figs/control-schematic-nass.png" alt="control-schematic-nass.png" /> <p><img src="figs/control-schematic-nass.png" alt="control-schematic-nass.png" />
</p> </p>
<p><span class="figure-number">Figure 1: </span>Global Control Schematic for the Station</p> <p><span class="figure-number">Figure 1: </span>Global Control Schematic for the Station</p>
</div> </div>
<p> <p>
In this document, we develop and verify that the two green blocs are working. In this document, are interesting by the "compute Sample Position w.r.t. Granite" bloc.
</p> </p>
<p> <p>
We suppose that we are able to measure perfectly the position of the sample with respect to the granite. First, in section <a href="#org09b824d">1</a>, is explained how the measurement of the position of the sample with respect to the granite is performed (using Simscape blocs).
This means that we do not care about the bloc "Compute Sample Position w.r.t. Granite" that makes the transformation from the interferometer measurements to the position of the sample.
We suppose that we can directly measure perfectly the position of the sample with respect to the granite.
</p> </p>
<p> <div id="outline-container-orgdf449a5" class="outline-2">
Also, all the stages can be perfectly positioned. <h2 id="orgdf449a5"><span class="section-number-2">1</span> How do we measure the position of the sample with respect to the granite</h2>
</p>
<p>
First, in section <a href="#org485d3de">1</a>, is explained how the measurement of the position of the sample with respect to the granite is performed (using Simscape blocs).
</p>
<p>
In section <a href="#orgbbdea18">2</a>, we verify that the function developed to compute the wanted pose (translation and orientation) of the sample with respect to the granite can be determined from the wanted position of each stage (translation stage, tilt stage, spindle and micro-hexapod). This corresponds to the bloc "Compute Wanted Sample Position w.r.t. Granite" in figure <a href="#orgf9367e8">1</a>.
To do so, we impose a perfect displacement and all the stage, we perfectly measure the position of the sample with respect to the granite, and we verify that this measured position corresponds to the computed wanted pose of the sample.
</p>
<p>
Then, in section <a href="#orga5b1edf">3</a>, we introduce some positioning error in the micro-station's stages.
The positioning error of the sample expressed with respect to the granite frame (the one measured) is expressed in a frame connected to the NASS top platform (corresponding to the green bloc "Compute Sample Position Error w.r.t. NASS" in figure <a href="#orgf9367e8">1</a>).
Then, we move the NASS such that it compensate for the positioning error that are expressed in the frame of the NASS, and we verify that the positioning error of the sample is well compensated.
</p>
<div id="outline-container-orgca47132" class="outline-2">
<h2 id="orgca47132"><span class="section-number-2">1</span> How do we measure the position of the sample with respect to the granite</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p> <p>
<a id="org485d3de"></a> <a id="org09b824d"></a>
A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame. A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame.
</p> </p>
@ -377,639 +336,10 @@ We can then determine extract other orientation conventions such that Euler angl
</p> </p>
</div> </div>
</div> </div>
<div id="outline-container-orgb0ca1ab" class="outline-2">
<h2 id="orgb0ca1ab"><span class="section-number-2">2</span> Verify that the function to compute the reference pose is correct</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="orgbbdea18"></a>
</p>
<p>
The goal here is to perfectly move the station and verify that there is no mismatch between the metrology measurement and the computation of the reference pose.
</p>
</div>
<div id="outline-container-org824aee5" class="outline-3">
<h3 id="org824aee5"><span class="section-number-3">2.1</span> Prepare the Simulation</h3>
<div class="outline-text-3" id="text-2-1">
<p>
We load the configuration.
</p>
<div class="org-src-container">
<pre class="src src-matlab">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>;
</pre>
</div>
<p>
We set a small <code>StopTime</code>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><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">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We initialize all the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
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>;
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">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We setup the reference path to be constant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
<span class="org-string">'Ts'</span>, <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Sampling Frequency [s]</span>
<span class="org-string">'Dy_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">5e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span>
<span class="org-string">'Dy_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
<span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Ry_amplitude'</span>, <span class="org-type">-</span><span class="org-highlight-numbers-number">1</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"> % Amplitude [rad]</span>
<span class="org-string">'Ry_period'</span>, <span class="org-highlight-numbers-number">10</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
<span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span>
<span class="org-string">'Rz_amplitude'</span>, <span class="org-type">-</span><span class="org-highlight-numbers-number">135</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"> % Initial angle [rad]</span>
<span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the rotating [s]</span>
<span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dh_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">01</span>; <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">02</span>; <span class="org-type">-</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">03</span>; <span class="org-type">-</span><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-highlight-numbers-number">1</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-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-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-string">'Rm_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Rm_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>, <span class="org-constant">pi</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-type">'</span>, ...<span class="org-comment"> % Initial position of the two masses</span>
<span class="org-string">'Dn_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dn_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>; <span class="org-highlight-numbers-number">3e</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-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">1</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-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-rainbow-delimiters-depth-1">)</span>;
initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
No position error for now (perfect positioning).
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dye = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span>
Rye = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [rad]</span>
Rze = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [rad]</span>
Dhe = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [m,rad]</span>
Dhle = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [m]</span>
Dne = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [m,rad]</span>
</pre>
</div>
<p>
And we run the simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/sim_nano_station_metrology.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
</div>
</div>
<div id="outline-container-org3530c48" class="outline-3">
<h3 id="org3530c48"><span class="section-number-3">2.2</span> Verify that the pose of the sample is the same as the computed one</h3>
<div class="outline-text-3" id="text-2-2">
<p>
Let's denote:
</p>
<ul class="org-ul">
<li>\(\{W\}\) the initial fixed frame (base in which the interferometric measurement is done)</li>
<li>\(\{R\}\) the reference frame corresponding to the wanted pose of the sample</li>
<li>\(\{M\}\) the frame corresponding to the measured pose of the sample</li>
</ul>
<p>
We have then computed:
</p>
<ul class="org-ul">
<li>\({}^W\bm{T}_R\) which corresponds to the wanted pose of the sample with respect to the granite</li>
<li>\({}^W\bm{T}_M\) which corresponds to the measured pose of the sample with respect to the granite</li>
</ul>
<p>
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix \({}^W\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = computeReferencePose<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Ry.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Rz.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dh.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dn.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix \({}^W\bm{T}_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.R.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = Dsm.R.Data;
WTm<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-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>Dsm.x.Data<span class="org-type">'</span> ; Dsm.y.Data<span class="org-type">'</span> ; Dsm.z.Data<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span>;
WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span>;
</pre>
</div>
<p>
As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]
</p>
<p>
Or are least:
\[ {}^W\bm{T}_R(1:3, 4) = {}^W\bm{T}_M(1:3, 4) \]
\[ {}^W\bm{R}_R^t \cdot {}^W\bm{R}_M = \bm{I}_3 \]
</p>
<div class="org-src-container">
<pre class="src src-matlab">WTr<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-highlight-numbers-number">4</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>WTm<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-highlight-numbers-number">4</span>, end<span class="org-rainbow-delimiters-depth-1">)</span>
WTr<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'*</span>WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>eye<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>
</pre>
</div>
<pre class="example">
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
1.8027246362351e-14
1.40408518145563e-14
6.93889390390723e-17
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
ans =
1.59872115546023e-14 -1.56629266848118e-14 -3.71230823859037e-16
1.56742023874057e-14 1.59872115546023e-14 -2.12330153459561e-15
-1.14144804719274e-15 -5.51642065360625e-16 9.28146448586631e-14
</pre>
</div>
</div>
<div id="outline-container-org40ffd7b" class="outline-3">
<h3 id="org40ffd7b"><span class="section-number-3">2.3</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-3">
<div class="important">
<p>
We are able to compute the wanted position and orientation of the sample.
Both the measurement and the theory gives the same result.
</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org3fda22b" class="outline-2">
<h2 id="org3fda22b"><span class="section-number-2">3</span> Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="orga5b1edf"></a>
</p>
<p>
We now introduce some positioning error in the stage.
This will induce a global positioning error of the sample with respect to the desired pose that we can compute.
</p>
<p>
We want to verify that we are able to measure this positioning error and convert it in the frame attached to the Nano-hexapod.
</p>
</div>
<div id="outline-container-orgd4dab89" class="outline-3">
<h3 id="orgd4dab89"><span class="section-number-3">3.1</span> Prepare the Simulation</h3>
<div class="outline-text-3" id="text-3-1">
<p>
We load the configuration.
</p>
<div class="org-src-container">
<pre class="src src-matlab">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>;
</pre>
</div>
<p>
We set a small <code>StopTime</code>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><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">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We initialize all the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
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>;
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">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We setup the reference path to be constant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
<span class="org-string">'Ts'</span>, <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Sampling Frequency [s]</span>
<span class="org-string">'Dy_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span>
<span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Ry_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude [rad]</span>
<span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span>
<span class="org-string">'Rz_amplitude'</span>, <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"> % Initial angle [rad]</span>
<span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dh_pos'</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">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-string">'Rm_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Rm_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>, <span class="org-constant">pi</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-type">'</span>, ...<span class="org-comment"> % Initial position of the two masses</span>
<span class="org-string">'Dn_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dn_pos'</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">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-rainbow-delimiters-depth-1">)</span>;
initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
Now we introduce some positioning error.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dye = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span>; <span class="org-comment">% [m]</span>
Rye = <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">4</span>; <span class="org-comment">% [rad]</span>
Rze = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">5</span>; <span class="org-comment">% [rad]</span>
Dhe = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
% Dne = [<span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span> ; <span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span> ; <span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">3e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span> ; <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>]; % [m,rad]
Dhle = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">3e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-type">-</span><span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
Dne = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
And we run the simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/sim_nano_station_metrology.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
</div>
</div>
<div id="outline-container-orge43864b" class="outline-3">
<h3 id="orge43864b"><span class="section-number-3">3.2</span> Compute the wanted pose of the sample in the NASS Base from the metrology and the reference</h3>
<div class="outline-text-3" id="text-3-2">
<p>
Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.
</p>
<p>
We would like to compute \({}^M\bm{T}_R\) which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame \(\{M\}\)).
</p>
<p>
We have:
</p>
\begin{align}
{}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
&= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
\end{align}
<p>
The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, \({}^M\bm{T}_R\) corresponds to the pose error of the sample with respect to the nano-hexapod platform.
</p>
<p>
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix \({}^W\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = computeReferencePose<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Ry.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Rz.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dh.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dn.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
We also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix \({}^W\bm{T}_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.R.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = Dsm.R.Data;
WTm<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-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>Dsm.x.Data<span class="org-type">'</span> ; Dsm.y.Data<span class="org-type">'</span> ; Dsm.z.Data<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span>;
WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span>;
</pre>
</div>
<p>
The <b>inverse of the transformation matrix</b> can be obtain by (it is less computation intensive than doing a full inverse)
</p>
\begin{equation}
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
\left[ \begin{array}{ccc|c}
& & & \\
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
& & & \\
\hline
0 & 0 & 0 & 1 \\
\end{array} \right]
\end{equation}
<p>
Finally, we compute \({}^M\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">MTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
MTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'</span>, <span class="org-type">-</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>WTm<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">4</span>,<span class="org-constant">i</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><span class="org-type">*</span>WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
Verify that the pose error corresponds to the positioning error of the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">MTr<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, end<span class="org-rainbow-delimiters-depth-1">)</span>
Rx = <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> cos<span class="org-rainbow-delimiters-depth-2">(</span>Erx<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Erx<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>Erx<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Erx<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Ry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ery<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>Ery<span class="org-rainbow-delimiters-depth-2">)</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>;
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Ery<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>Ery<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Erz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Erz<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>Erz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Erz<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>;
</pre>
</div>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-left" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
</colgroup>
<thead>
<tr>
<th scope="col" class="org-left">&#xa0;</th>
<th scope="col" class="org-right">Edx [m]</th>
<th scope="col" class="org-right">Edy [m]</th>
<th scope="col" class="org-right">Edz [m]</th>
<th scope="col" class="org-right">Erx [rad]</th>
<th scope="col" class="org-right">Ery [rad]</th>
<th scope="col" class="org-right">Erz [rad]</th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">Error</td>
<td class="org-right">2.8e-06</td>
<td class="org-right">-2.0e-06</td>
<td class="org-right">-1.3e-06</td>
<td class="org-right">-5.1e-06</td>
<td class="org-right">-1.8e-04</td>
<td class="org-right">4.2e-07</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-orgfc9d145" class="outline-3">
<h3 id="orgfc9d145"><span class="section-number-3">3.3</span> Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end</h3>
<div class="outline-text-3" id="text-3-3">
<p>
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
</p>
<div class="org-src-container">
<pre class="src src-matlab">opts.Dn_pos = <span class="org-rainbow-delimiters-depth-1">[</span>Edx, Edy, Edz, Erx, Ery, Erz<span class="org-rainbow-delimiters-depth-1">]</span><span class="org-type">'</span>;
initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
And we run the simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/sim_nano_station_metrology.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We keep the old computed computed reference pose \({}^W\bm{T}_r\) even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.
</p>
<p>
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix \({}^W\bm{T}_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.R.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = Dsm.R.Data;
WTm<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-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>Dsm.x.Data<span class="org-type">'</span> ; Dsm.y.Data<span class="org-type">'</span> ; Dsm.z.Data<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span>;
WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span>;
</pre>
</div>
<p>
Finally, we compute \({}^M\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">MTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
MTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'</span>, <span class="org-type">-</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>WTm<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">4</span>,<span class="org-constant">i</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><span class="org-type">*</span>WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
Verify that the pose error is small.
</p>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-left" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
</colgroup>
<thead>
<tr>
<th scope="col" class="org-left">&#xa0;</th>
<th scope="col" class="org-right">Edx [m]</th>
<th scope="col" class="org-right">Edy [m]</th>
<th scope="col" class="org-right">Edz [m]</th>
<th scope="col" class="org-right">Erx [rad]</th>
<th scope="col" class="org-right">Ery [rad]</th>
<th scope="col" class="org-right">Erz [rad]</th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">Error</td>
<td class="org-right">1.0e-16</td>
<td class="org-right">-1.1e-18</td>
<td class="org-right">3.3e-20</td>
<td class="org-right">1.5e-16</td>
<td class="org-right">5.8e-17</td>
<td class="org-right">-6.1e-16</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-org4293675" class="outline-3">
<h3 id="org4293675"><span class="section-number-3">3.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-3-4">
<div class="important">
<p>
Indeed, we are able to convert the position error in the frame of the NASS and then compensate these errors with the NASS.
</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org1449831" class="outline-2">
<h2 id="org1449831"><span class="section-number-2">4</span> Functions</h2>
<div class="outline-text-2" id="text-4">
</div>
<div id="outline-container-org3a6492a" class="outline-3">
<h3 id="org3a6492a"><span class="section-number-3">4.1</span> computeReferencePose</h3>
<div class="outline-text-3" id="text-4-1">
<p>
<a id="org81bf206"></a>
</p>
<p>
This Matlab function is accessible <a href="src/computeReferencePose.m">here</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">WTr</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">computeReferencePose</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">Dy</span>, <span class="org-variable-name">Ry</span>, <span class="org-variable-name">Rz</span>, <span class="org-variable-name">Dh</span>, <span class="org-variable-name">Dn</span><span class="org-rainbow-delimiters-depth-1">)</span>
<span class="org-comment">% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - Dy - Reference of the Translation Stage [m]</span>
<span class="org-comment">% - Ry - Reference of the Tilt Stage [rad]</span>
<span class="org-comment">% - Rz - Reference of the Spindle [rad]</span>
<span class="org-comment">% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]</span>
<span class="org-comment">% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - WTr -</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Translation Stage</span></span>
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>;
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Dy;
<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>;
<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>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Tilt Stage - Pure rotating aligned with Ob</span></span>
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>;
<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>;
<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>;
<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>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Rotation along the Z axis</span></span>
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> ;
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> ;
<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> ;
<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>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Micro-Hexapod</span></span>
Rhx = <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> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rhy = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><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">1</span> <span class="org-highlight-numbers-number">0</span>;
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rhz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><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>;
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> Dh<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-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Dh<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">2</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">1</span> Dh<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">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>;
Rh<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-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> = Rhz<span class="org-type">*</span>Rhy<span class="org-type">*</span>Rhx;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Nano-Hexapod</span></span>
Rnx = <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> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rny = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><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">1</span> <span class="org-highlight-numbers-number">0</span>;
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rnz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><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>;
Rn = <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> Dn<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-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Dn<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">2</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">1</span> Dn<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">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>;
Rn<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-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> = Rnx<span class="org-type">*</span>Rny<span class="org-type">*</span>Rnz;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Total Homogeneous transformation</span></span>
WTr = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh<span class="org-type">*</span>Rn;
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
</div>
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-12-11 mer. 10:28</p> <p class="date">Created: 2019-12-11 mer. 17:33</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p> <p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
</div> </div>
</body> </body>

744
metrology/index.org
View File

@ -48,23 +48,10 @@ The global measurement and control schematic is shown in figure [[fig:control-sc
#+caption: Global Control Schematic for the Station #+caption: Global Control Schematic for the Station
[[file:figs/control-schematic-nass.png]] [[file:figs/control-schematic-nass.png]]
In this document, we develop and verify that the two green blocs are working. In this document, are interesting by the "compute Sample Position w.r.t. Granite" bloc.
We suppose that we are able to measure perfectly the position of the sample with respect to the granite.
This means that we do not care about the bloc "Compute Sample Position w.r.t. Granite" that makes the transformation from the interferometer measurements to the position of the sample.
We suppose that we can directly measure perfectly the position of the sample with respect to the granite.
Also, all the stages can be perfectly positioned.
First, in section [[sec:measurement_principle]], is explained how the measurement of the position of the sample with respect to the granite is performed (using Simscape blocs). First, in section [[sec:measurement_principle]], is explained how the measurement of the position of the sample with respect to the granite is performed (using Simscape blocs).
In section [[sec:compute_reference]], we verify that the function developed to compute the wanted pose (translation and orientation) of the sample with respect to the granite can be determined from the wanted position of each stage (translation stage, tilt stage, spindle and micro-hexapod). This corresponds to the bloc "Compute Wanted Sample Position w.r.t. Granite" in figure [[fig:control-schematic-nass]].
To do so, we impose a perfect displacement and all the stage, we perfectly measure the position of the sample with respect to the granite, and we verify that this measured position corresponds to the computed wanted pose of the sample.
Then, in section [[sec:compute_pos_error]], we introduce some positioning error in the micro-station's stages.
The positioning error of the sample expressed with respect to the granite frame (the one measured) is expressed in a frame connected to the NASS top platform (corresponding to the green bloc "Compute Sample Position Error w.r.t. NASS" in figure [[fig:control-schematic-nass]]).
Then, we move the NASS such that it compensate for the positioning error that are expressed in the frame of the NASS, and we verify that the positioning error of the sample is well compensated.
* How do we measure the position of the sample with respect to the granite * How do we measure the position of the sample with respect to the granite
<<sec:measurement_principle>> <<sec:measurement_principle>>
A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame. A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame.
@ -78,732 +65,3 @@ The outputs of the transform sensor are:
- the *rotation matrix* $\bm{R}$ that permits to rotate the base frame into the follower frame. - the *rotation matrix* $\bm{R}$ that permits to rotate the base frame into the follower frame.
We can then determine extract other orientation conventions such that Euler angles or screw axis. We can then determine extract other orientation conventions such that Euler angles or screw axis.
* Verify that the function to compute the reference pose is correct
<<sec:compute_reference>>
** Introduction :ignore:
The goal here is to perfectly move the station and verify that there is no mismatch between the metrology measurement and the computation of the reference pose.
** 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
simulinkproject('../');
#+end_src
#+begin_src matlab
open 'simscape/sim_nano_station_metrology.slx'
#+end_src
** Prepare the Simulation
We load the configuration.
#+begin_src matlab
load('mat/conf_simscape.mat');
#+end_src
We set a small =StopTime=.
#+begin_src matlab
set_param(conf_simscape, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
#+end_src
We setup the reference path to be constant.
#+begin_src matlab
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 5e-3, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', -1*pi/180, ... % Amplitude [rad]
'Ry_period', 10, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', -135*pi/180, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0.01; 0.02; -0.03; -3*pi/180; 1*pi/180; 3*pi/180], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0, pi]', ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [1e-3; 2e-3; 3e-3; 1*pi/180; 0; 1*pi/180] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
initializeReferences(opts);
#+end_src
No position error for now (perfect positioning).
#+begin_src matlab
Dye = 0; % [m]
Rye = 0; % [rad]
Rze = 0; % [rad]
Dhe = zeros(6,1); % [m,rad]
Dhle = zeros(6,1); % [m]
Dne = zeros(6,1); % [m,rad]
#+end_src
And we run the simulation.
#+begin_src matlab
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
** Verify that the pose of the sample is the same as the computed one
Let's denote:
- $\{W\}$ the initial fixed frame (base in which the interferometric measurement is done)
- $\{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:
- ${}^W\bm{T}_R$ which corresponds to the wanted pose of the sample with respect to the granite
- ${}^W\bm{T}_M$ which corresponds to the measured pose of the sample with respect to the granite
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
n = length(Dref.Dy.Time);
WTr = zeros(4, 4, n);
for i = 1:n
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:), Dref.Dn.Data(i,:));
end
#+end_src
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]
Or are least:
\[ {}^W\bm{T}_R(1:3, 4) = {}^W\bm{T}_M(1:3, 4) \]
\[ {}^W\bm{R}_R^t \cdot {}^W\bm{R}_M = \bm{I}_3 \]
#+begin_src matlab :results output replace
WTr(1:3, 4, end)-WTm(1:3, 4, end)
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
#+end_src
#+RESULTS:
#+begin_example
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
1.8027246362351e-14
1.40408518145563e-14
6.93889390390723e-17
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
ans =
1.59872115546023e-14 -1.56629266848118e-14 -3.71230823859037e-16
1.56742023874057e-14 1.59872115546023e-14 -2.12330153459561e-15
-1.14144804719274e-15 -5.51642065360625e-16 9.28146448586631e-14
#+end_example
** Conclusion
#+begin_important
We are able to compute the wanted position and orientation of the sample.
Both the measurement and the theory gives the same result.
#+end_important
* Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working
<<sec:compute_pos_error>>
** Introduction :ignore:
We now introduce some positioning error in the stage.
This will induce a global positioning error of the sample with respect to the desired pose that we can compute.
We want to verify that we are able to measure this positioning error and convert it in the frame attached to the Nano-hexapod.
** 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
simulinkproject('../');
#+end_src
#+begin_src matlab
open 'simscape/sim_nano_station_metrology.slx'
#+end_src
** Prepare the Simulation
We load the configuration.
#+begin_src matlab
load('mat/conf_simscape.mat');
#+end_src
We set a small =StopTime=.
#+begin_src matlab
set_param(conf_simscape, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
#+end_src
We setup the reference path to be constant.
#+begin_src matlab
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [rad]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0*pi/180, ... % Initial angle [rad]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0, pi]', ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
initializeReferences(opts);
#+end_src
Now we introduce some positioning error.
#+begin_src matlab
Dye = 1e-6; % [m]
Rye = 2e-4; % [rad]
Rze = 1e-5; % [rad]
Dhe = zeros(6,1);
% Dne = [1e-3 ; 0 ; 2e-3 ; 0 ; 3e-3 ; 1e-3]; % [m,rad]
Dhle = [1e-6 ; 2e-6 ; 3e-6 ; -2e-6 ; 1e-6 ; 2e-6]; % [m]
Dne = zeros(6,1);
#+end_src
And we run the simulation.
#+begin_src matlab
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
** Compute the wanted pose of the sample in the NASS Base from the metrology and the reference
Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.
We would like to compute ${}^M\bm{T}_R$ which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame $\{M\}$).
We have:
\begin{align}
{}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
&= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
\end{align}
The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, ${}^M\bm{T}_R$ corresponds to the pose error of the sample with respect to the nano-hexapod platform.
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
n = length(Dref.Dy.Time);
WTr = zeros(4, 4, n);
for i = 1:n
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:), Dref.Dn.Data(i,:));
end
#+end_src
We also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
The *inverse of the transformation matrix* can be obtain by (it is less computation intensive than doing a full inverse)
\begin{equation}
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
\left[ \begin{array}{ccc|c}
& & & \\
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
& & & \\
\hline
0 & 0 & 0 & 1 \\
\end{array} \right]
\end{equation}
Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
MTr = zeros(4, 4, n);
for i = 1:n
MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
end
#+end_src
Verify that the pose error corresponds to the positioning error of the stages.
#+begin_src matlab :exports none
Edx = MTr(1, 4, end);
Edy = MTr(2, 4, end);
Edz = MTr(3, 4, end);
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
#+end_src
#+begin_src matlab
MTr(1:3, 1:3, end)
Rx = [1 0 0;
0 cos(Erx) -sin(Erx);
0 sin(Erx) cos(Erx)];
Ry = [ cos(Ery) 0 sin(Ery);
0 1 0;
-sin(Ery) 0 cos(Ery)];
Rz = [cos(Erz) -sin(Erz) 0;
sin(Erz) cos(Erz) 0;
0 0 1];
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
#+end_src
#+RESULTS:
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+----------+-----------+-----------+-----------|
| Error | 2.8e-06 | -2.0e-06 | -1.3e-06 | -5.1e-06 | -1.8e-04 | 4.2e-07 |
** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
#+begin_src matlab
opts.Dn_pos = [Edx, Edy, Edz, Erx, Ery, Erz]';
initializeReferences(opts);
#+end_src
And we run the simulation.
#+begin_src matlab
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
We keep the old computed computed reference pose ${}^W\bm{T}_r$ even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
MTr = zeros(4, 4, n);
for i = 1:n
MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
end
#+end_src
Verify that the pose error is small.
#+begin_src matlab :exports none
Edx = MTr(1, 4, end);
Edy = MTr(2, 4, end);
Edz = MTr(3, 4, end);
Ery = atan2(MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
#+end_src
#+RESULTS:
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+---------+-----------+-----------+-----------|
| Error | 1.0e-16 | -1.1e-18 | 3.3e-20 | 1.5e-16 | 5.8e-17 | -6.1e-16 |
** Conclusion
#+begin_important
Indeed, we are able to convert the position error in the frame of the NASS and then compensate these errors with the NASS.
#+end_important
* Verify that we are able to compensate the errors using the nano-hexapod
* Functions
** computeReferencePose
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeReferencePose.m
:header-args:matlab+: :comments none :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:computeReferencePose>>
This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
#+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) = Rnx*Rny*Rnz;
%% Total Homogeneous transformation
WTr = Rty*Rry*Rrz*Rh*Rn;
end
#+end_src
* Tests on the transformation from reference to wanted position :noexport:
** 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
** 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
simulinkproject('../');
#+end_src
#+begin_src matlab
open 'simscape/sim_nano_station_metrology.slx'
#+end_src
** 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 => seems better
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 \\
&= {}^W{T_M}^{-1} {}^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

1
positioning_error/figs Symbolic link
View File

@ -0,0 +1 @@
../figs

913
positioning_error/index.html Normal file
View File

@ -0,0 +1,913 @@
<?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-11 mer. 17:33 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Computation of the Positioning Error with respect to the nano-hexapod</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>
<link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
<link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
<link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
<script type="text/javascript" src="../js/jquery.min.js"></script>
<script type="text/javascript" src="../js/bootstrap.min.js"></script>
<script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
<script type="text/javascript" src="../js/readtheorg.js"></script>
<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",
Macros: {
bm: ["{\\boldsymbol #1}",1],
}
}
});
</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">Computation of the Positioning Error with respect to the nano-hexapod</h1>
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#orge221809">1. How do we measure the position of the sample with respect to the granite</a></li>
<li><a href="#org1148842">2. Verify that the function to compute the reference pose is correct</a>
<ul>
<li><a href="#orgb069429">2.1. Prepare the Simulation</a></li>
<li><a href="#org79597b7">2.2. Verify that the pose of the sample is the same as the computed one</a></li>
<li><a href="#org9dd1626">2.3. Conclusion</a></li>
</ul>
</li>
<li><a href="#org54d1c35">3. Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working</a>
<ul>
<li><a href="#org07d2e9e">3.1. Prepare the Simulation</a></li>
<li><a href="#orgd5c310c">3.2. Compute the wanted pose of the sample in the NASS Base from the metrology and the reference</a></li>
<li><a href="#org109338f">3.3. Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end</a></li>
<li><a href="#orga75d3ed">3.4. Conclusion</a></li>
</ul>
</li>
<li><a href="#orge995172">4. Verify that we are able to compensate the errors using the nano-hexapod</a></li>
</ul>
</div>
</div>
<p>
The global measurement and control schematic is shown in figure <a href="#orgf0b4232">1</a>.
</p>
<div id="orgf0b4232" class="figure">
<p><img src="figs/control-schematic-nass.png" alt="control-schematic-nass.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Global Control Schematic for the Station</p>
</div>
<p>
In this document, we develop and verify that the two green blocs are working.
</p>
<p>
We suppose that we are able to measure perfectly the position of the sample with respect to the granite.
This means that we do not care about the bloc "Compute Sample Position w.r.t. Granite" that makes the transformation from the interferometer measurements to the position of the sample.
We suppose that we can directly measure perfectly the position of the sample with respect to the granite.
</p>
<p>
Also, all the stages can be perfectly positioned.
</p>
<p>
In section <a href="#org44336a1">2</a>, we verify that the function developed to compute the wanted pose (translation and orientation) of the sample with respect to the granite can be determined from the wanted position of each stage (translation stage, tilt stage, spindle and micro-hexapod). This corresponds to the bloc "Compute Wanted Sample Position w.r.t. Granite" in figure <a href="#orgf0b4232">1</a>.
To do so, we impose a perfect displacement and all the stage, we perfectly measure the position of the sample with respect to the granite, and we verify that this measured position corresponds to the computed wanted pose of the sample.
</p>
<p>
Then, in section <a href="#org9f1c3ad">3</a>, we introduce some positioning error in the micro-station's stages.
The positioning error of the sample expressed with respect to the granite frame (the one measured) is expressed in a frame connected to the NASS top platform (corresponding to the green bloc "Compute Sample Position Error w.r.t. NASS" in figure <a href="#orgf0b4232">1</a>).
Then, we move the NASS such that it compensate for the positioning error that are expressed in the frame of the NASS, and we verify that the positioning error of the sample is well compensated.
</p>
<div id="outline-container-orge221809" class="outline-2">
<h2 id="orge221809"><span class="section-number-2">1</span> How do we measure the position of the sample with respect to the granite</h2>
<div class="outline-text-2" id="text-1">
<p>
<a id="org38f1c05"></a>
A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame.
</p>
<p>
The base frame is fixed to the granite and located at the initial sample location that defines the zero position.
</p>
<p>
The follower frame is attached to the sample (or more precisely to the reflector).
</p>
<p>
The outputs of the transform sensor are:
</p>
<ul class="org-ul">
<li>the 3 translations x, y and z in meter</li>
<li>the <b>rotation matrix</b> \(\bm{R}\) that permits to rotate the base frame into the follower frame.</li>
</ul>
<p>
We can then determine extract other orientation conventions such that Euler angles or screw axis.
</p>
</div>
</div>
<div id="outline-container-org1148842" class="outline-2">
<h2 id="org1148842"><span class="section-number-2">2</span> Verify that the function to compute the reference pose is correct</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="org44336a1"></a>
</p>
<p>
The goal here is to perfectly move the station and verify that there is no mismatch between the metrology measurement and the computation of the reference pose.
</p>
</div>
<div id="outline-container-orgb069429" class="outline-3">
<h3 id="orgb069429"><span class="section-number-3">2.1</span> Prepare the Simulation</h3>
<div class="outline-text-3" id="text-2-1">
<p>
We load the configuration.
</p>
<div class="org-src-container">
<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'mat/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We set a small <code>StopTime</code>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><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">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We initialize all the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
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>;
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">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We setup the reference path to be constant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
<span class="org-string">'Ts'</span>, <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Sampling Frequency [s]</span>
<span class="org-string">'Dy_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">5e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span>
<span class="org-string">'Dy_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
<span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Ry_amplitude'</span>, <span class="org-type">-</span><span class="org-highlight-numbers-number">1</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"> % Amplitude [rad]</span>
<span class="org-string">'Ry_period'</span>, <span class="org-highlight-numbers-number">10</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
<span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span>
<span class="org-string">'Rz_amplitude'</span>, <span class="org-type">-</span><span class="org-highlight-numbers-number">135</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"> % Initial angle [rad]</span>
<span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the rotating [s]</span>
<span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dh_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">01</span>; <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">02</span>; <span class="org-type">-</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">03</span>; <span class="org-type">-</span><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-highlight-numbers-number">1</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-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-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-string">'Rm_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Rm_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>, <span class="org-constant">pi</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-type">'</span>, ...<span class="org-comment"> % Initial position of the two masses</span>
<span class="org-string">'Dn_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dn_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>; <span class="org-highlight-numbers-number">3e</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-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">1</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-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-rainbow-delimiters-depth-1">)</span>;
initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
No position error for now (perfect positioning).
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dye = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span>
Rye = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [rad]</span>
Rze = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [rad]</span>
Dhe = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [m,rad]</span>
Dhle = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [m]</span>
Dne = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [m,rad]</span>
</pre>
</div>
<p>
And we run the simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/sim_nano_station_metrology.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
</div>
</div>
<div id="outline-container-org79597b7" class="outline-3">
<h3 id="org79597b7"><span class="section-number-3">2.2</span> Verify that the pose of the sample is the same as the computed one</h3>
<div class="outline-text-3" id="text-2-2">
<p>
Let's denote:
</p>
<ul class="org-ul">
<li>\(\{W\}\) the initial fixed frame (base in which the interferometric measurement is done)</li>
<li>\(\{R\}\) the reference frame corresponding to the wanted pose of the sample</li>
<li>\(\{M\}\) the frame corresponding to the measured pose of the sample</li>
</ul>
<p>
We have then computed:
</p>
<ul class="org-ul">
<li>\({}^W\bm{T}_R\) which corresponds to the wanted pose of the sample with respect to the granite</li>
<li>\({}^W\bm{T}_M\) which corresponds to the measured pose of the sample with respect to the granite</li>
</ul>
<p>
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix \({}^W\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = computeReferencePose<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Ry.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Rz.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dh.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dn.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix \({}^W\bm{T}_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.R.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = Dsm.R.Data;
WTm<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-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>Dsm.x.Data<span class="org-type">'</span> ; Dsm.y.Data<span class="org-type">'</span> ; Dsm.z.Data<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span>;
WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span>;
</pre>
</div>
<p>
As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]
</p>
<p>
Or are least:
\[ {}^W\bm{T}_R(1:3, 4) = {}^W\bm{T}_M(1:3, 4) \]
\[ {}^W\bm{R}_R^t \cdot {}^W\bm{R}_M = \bm{I}_3 \]
</p>
<div class="org-src-container">
<pre class="src src-matlab">WTr<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-highlight-numbers-number">4</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>WTm<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-highlight-numbers-number">4</span>, end<span class="org-rainbow-delimiters-depth-1">)</span>
WTr<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'*</span>WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>eye<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>
</pre>
</div>
<pre class="example">
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
1.8027246362351e-14
1.40408518145563e-14
6.93889390390723e-17
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
ans =
1.59872115546023e-14 -1.56629266848118e-14 -3.71230823859037e-16
1.56742023874057e-14 1.59872115546023e-14 -2.12330153459561e-15
-1.14144804719274e-15 -5.51642065360625e-16 9.28146448586631e-14
</pre>
</div>
</div>
<div id="outline-container-org9dd1626" class="outline-3">
<h3 id="org9dd1626"><span class="section-number-3">2.3</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-3">
<div class="important">
<p>
We are able to compute the wanted position and orientation of the sample.
Both the measurement and the theory gives the same result.
</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org54d1c35" class="outline-2">
<h2 id="org54d1c35"><span class="section-number-2">3</span> Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="org9f1c3ad"></a>
</p>
<p>
We now introduce some positioning error in the stage.
This will induce a global positioning error of the sample with respect to the desired pose that we can compute.
</p>
<p>
We want to verify that we are able to measure this positioning error and convert it in the frame attached to the Nano-hexapod.
</p>
</div>
<div id="outline-container-org07d2e9e" class="outline-3">
<h3 id="org07d2e9e"><span class="section-number-3">3.1</span> Prepare the Simulation</h3>
<div class="outline-text-3" id="text-3-1">
<p>
We load the configuration.
</p>
<div class="org-src-container">
<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'mat/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We set a small <code>StopTime</code>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><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">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We initialize all the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
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>;
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">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We setup the reference path to be constant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span> ...
<span class="org-string">'Ts'</span>, <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>, ...<span class="org-comment"> % Sampling Frequency [s]</span>
<span class="org-string">'Dy_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span>
<span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Ry_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude [rad]</span>
<span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span>
<span class="org-string">'Rz_amplitude'</span>, <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"> % Initial angle [rad]</span>
<span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dh_pos'</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">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-string">'Rm_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Rm_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>, <span class="org-constant">pi</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-type">'</span>, ...<span class="org-comment"> % Initial position of the two masses</span>
<span class="org-string">'Dn_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dn_pos'</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">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
<span class="org-rainbow-delimiters-depth-1">)</span>;
initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
Now we introduce some positioning error.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dye = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span>; <span class="org-comment">% [m]</span>
Rye = <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">4</span>; <span class="org-comment">% [rad]</span>
Rze = <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">5</span>; <span class="org-comment">% [rad]</span>
Dhe = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
% Dne = [<span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span> ; <span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span> ; <span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">3e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span> ; <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span>]; % [m,rad]
Dhle = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">3e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-type">-</span><span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span> ; <span class="org-highlight-numbers-number">2e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
Dne = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
And we run the simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/sim_nano_station_metrology.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
</div>
</div>
<div id="outline-container-orgd5c310c" class="outline-3">
<h3 id="orgd5c310c"><span class="section-number-3">3.2</span> Compute the wanted pose of the sample in the NASS Base from the metrology and the reference</h3>
<div class="outline-text-3" id="text-3-2">
<p>
Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.
</p>
<p>
We would like to compute \({}^M\bm{T}_R\) which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame \(\{M\}\)).
</p>
<p>
We have:
</p>
\begin{align}
{}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
&= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
\end{align}
<p>
The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, \({}^M\bm{T}_R\) corresponds to the pose error of the sample with respect to the nano-hexapod platform.
</p>
<p>
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix \({}^W\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = computeReferencePose<span class="org-rainbow-delimiters-depth-1">(</span>Dref.Dy.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Ry.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Rz.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dh.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span>, Dref.Dn.Data<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>,<span class="org-type">:</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
We also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix \({}^W\bm{T}_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.R.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = Dsm.R.Data;
WTm<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-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>Dsm.x.Data<span class="org-type">'</span> ; Dsm.y.Data<span class="org-type">'</span> ; Dsm.z.Data<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span>;
WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span>;
</pre>
</div>
<p>
The <b>inverse of the transformation matrix</b> can be obtain by (it is less computation intensive than doing a full inverse)
</p>
\begin{equation}
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
\left[ \begin{array}{ccc|c}
& & & \\
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
& & & \\
\hline
0 & 0 & 0 & 1 \\
\end{array} \right]
\end{equation}
<p>
Finally, we compute \({}^M\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">MTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
MTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'</span>, <span class="org-type">-</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>WTm<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">4</span>,<span class="org-constant">i</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><span class="org-type">*</span>WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
Verify that the pose error corresponds to the positioning error of the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">MTr<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, end<span class="org-rainbow-delimiters-depth-1">)</span>
Rx = <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> cos<span class="org-rainbow-delimiters-depth-2">(</span>Erx<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Erx<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>Erx<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Erx<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Ry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ery<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>Ery<span class="org-rainbow-delimiters-depth-2">)</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>;
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Ery<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>Ery<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Erz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Erz<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>Erz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Erz<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>;
</pre>
</div>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-left" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
</colgroup>
<thead>
<tr>
<th scope="col" class="org-left">&#xa0;</th>
<th scope="col" class="org-right">Edx [m]</th>
<th scope="col" class="org-right">Edy [m]</th>
<th scope="col" class="org-right">Edz [m]</th>
<th scope="col" class="org-right">Erx [rad]</th>
<th scope="col" class="org-right">Ery [rad]</th>
<th scope="col" class="org-right">Erz [rad]</th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">Error</td>
<td class="org-right">2.8e-06</td>
<td class="org-right">-2.0e-06</td>
<td class="org-right">-1.3e-06</td>
<td class="org-right">-5.1e-06</td>
<td class="org-right">-1.8e-04</td>
<td class="org-right">4.2e-07</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-org109338f" class="outline-3">
<h3 id="org109338f"><span class="section-number-3">3.3</span> Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end</h3>
<div class="outline-text-3" id="text-3-3">
<p>
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
</p>
<div class="org-src-container">
<pre class="src src-matlab">opts.Dn_pos = <span class="org-rainbow-delimiters-depth-1">[</span>Edx, Edy, Edz, Erx, Ery, Erz<span class="org-rainbow-delimiters-depth-1">]</span><span class="org-type">'</span>;
initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
And we run the simulation.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/sim_nano_station_metrology.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We keep the old computed computed reference pose \({}^W\bm{T}_r\) even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.
</p>
<p>
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix \({}^W\bm{T}_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">n = length<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.R.Time<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
WTm<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-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = Dsm.R.Data;
WTm<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-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>Dsm.x.Data<span class="org-type">'</span> ; Dsm.y.Data<span class="org-type">'</span> ; Dsm.z.Data<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span>;
WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">1</span>;
</pre>
</div>
<p>
Finally, we compute \({}^M\bm{T}_R\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">MTr = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, n<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:n</span>
MTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">[</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'</span>, <span class="org-type">-</span>WTm<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-constant">i</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>WTm<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">4</span>,<span class="org-constant">i</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><span class="org-type">*</span>WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
</pre>
</div>
<p>
Verify that the pose error is small.
</p>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-left" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
</colgroup>
<thead>
<tr>
<th scope="col" class="org-left">&#xa0;</th>
<th scope="col" class="org-right">Edx [m]</th>
<th scope="col" class="org-right">Edy [m]</th>
<th scope="col" class="org-right">Edz [m]</th>
<th scope="col" class="org-right">Erx [rad]</th>
<th scope="col" class="org-right">Ery [rad]</th>
<th scope="col" class="org-right">Erz [rad]</th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left">Error</td>
<td class="org-right">1.0e-16</td>
<td class="org-right">-1.1e-18</td>
<td class="org-right">3.3e-20</td>
<td class="org-right">1.5e-16</td>
<td class="org-right">5.8e-17</td>
<td class="org-right">-6.1e-16</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-orga75d3ed" class="outline-3">
<h3 id="orga75d3ed"><span class="section-number-3">3.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-3-4">
<div class="important">
<p>
Indeed, we are able to convert the position error in the frame of the NASS and then compensate these errors with the NASS.
</p>
</div>
</div>
</div>
</div>
<div id="outline-container-orge995172" class="outline-2">
<h2 id="orge995172"><span class="section-number-2">4</span> Verify that we are able to compensate the errors using the nano-hexapod</h2>
</div>
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-12-11 mer. 17:33</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
</div>
</body>
</html>

713
positioning_error/index.org Normal file
View File

@ -0,0 +1,713 @@
#+TITLE: Computation of the Positioning Error with respect to the nano-hexapod
:DRAWER:
#+STARTUP: overview
#+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="../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
#+PROPERTY: header-args:matlab :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:shell :eval no-export
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{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 raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
:END:
* Introduction :ignore:
The global measurement and control schematic is shown in figure [[fig:control-schematic-nass]].
#+name: fig:control-schematic-nass
#+caption: Global Control Schematic for the Station
[[file:figs/control-schematic-nass.png]]
In this document, we develop and verify that the two green blocs are working.
We suppose that we are able to measure perfectly the position of the sample with respect to the granite.
This means that we do not care about the bloc "Compute Sample Position w.r.t. Granite" that makes the transformation from the interferometer measurements to the position of the sample.
We suppose that we can directly measure perfectly the position of the sample with respect to the granite.
Also, all the stages can be perfectly positioned.
In section [[sec:compute_reference]], we verify that the function developed to compute the wanted pose (translation and orientation) of the sample with respect to the granite can be determined from the wanted position of each stage (translation stage, tilt stage, spindle and micro-hexapod). This corresponds to the bloc "Compute Wanted Sample Position w.r.t. Granite" in figure [[fig:control-schematic-nass]].
To do so, we impose a perfect displacement and all the stage, we perfectly measure the position of the sample with respect to the granite, and we verify that this measured position corresponds to the computed wanted pose of the sample.
Then, in section [[sec:compute_pos_error]], we introduce some positioning error in the micro-station's stages.
The positioning error of the sample expressed with respect to the granite frame (the one measured) is expressed in a frame connected to the NASS top platform (corresponding to the green bloc "Compute Sample Position Error w.r.t. NASS" in figure [[fig:control-schematic-nass]]).
Then, we move the NASS such that it compensate for the positioning error that are expressed in the frame of the NASS, and we verify that the positioning error of the sample is well compensated.
* How do we measure the position of the sample with respect to the granite
<<sec:measurement_principle>>
A transform sensor block gives the translation and orientation of the follower frame with respect to the base frame.
The base frame is fixed to the granite and located at the initial sample location that defines the zero position.
The follower frame is attached to the sample (or more precisely to the reflector).
The outputs of the transform sensor are:
- the 3 translations x, y and z in meter
- the *rotation matrix* $\bm{R}$ that permits to rotate the base frame into the follower frame.
We can then determine extract other orientation conventions such that Euler angles or screw axis.
* Verify that the function to compute the reference pose is correct
<<sec:compute_reference>>
** Introduction :ignore:
The goal here is to perfectly move the station and verify that there is no mismatch between the metrology measurement and the computation of the reference pose.
** 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
simulinkproject('../');
#+end_src
#+begin_src matlab
open 'simscape/sim_nano_station_metrology.slx'
#+end_src
** Prepare the Simulation
We load the configuration.
#+begin_src matlab
load('mat/conf_simscape.mat');
#+end_src
We set a small =StopTime=.
#+begin_src matlab
set_param(conf_simscape, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
#+end_src
We setup the reference path to be constant.
#+begin_src matlab
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 5e-3, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', -1*pi/180, ... % Amplitude [rad]
'Ry_period', 10, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', -135*pi/180, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0.01; 0.02; -0.03; -3*pi/180; 1*pi/180; 3*pi/180], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0, pi]', ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [1e-3; 2e-3; 3e-3; 1*pi/180; 0; 1*pi/180] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
initializeReferences(opts);
#+end_src
No position error for now (perfect positioning).
#+begin_src matlab
Dye = 0; % [m]
Rye = 0; % [rad]
Rze = 0; % [rad]
Dhe = zeros(6,1); % [m,rad]
Dhle = zeros(6,1); % [m]
Dne = zeros(6,1); % [m,rad]
#+end_src
And we run the simulation.
#+begin_src matlab
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
** Verify that the pose of the sample is the same as the computed one
Let's denote:
- $\{W\}$ the initial fixed frame (base in which the interferometric measurement is done)
- $\{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:
- ${}^W\bm{T}_R$ which corresponds to the wanted pose of the sample with respect to the granite
- ${}^W\bm{T}_M$ which corresponds to the measured pose of the sample with respect to the granite
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
n = length(Dref.Dy.Time);
WTr = zeros(4, 4, n);
for i = 1:n
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:), Dref.Dn.Data(i,:));
end
#+end_src
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]
Or are least:
\[ {}^W\bm{T}_R(1:3, 4) = {}^W\bm{T}_M(1:3, 4) \]
\[ {}^W\bm{R}_R^t \cdot {}^W\bm{R}_M = \bm{I}_3 \]
#+begin_src matlab :results output replace
WTr(1:3, 4, end)-WTm(1:3, 4, end)
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
#+end_src
#+RESULTS:
#+begin_example
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
1.8027246362351e-14
1.40408518145563e-14
6.93889390390723e-17
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
ans =
1.59872115546023e-14 -1.56629266848118e-14 -3.71230823859037e-16
1.56742023874057e-14 1.59872115546023e-14 -2.12330153459561e-15
-1.14144804719274e-15 -5.51642065360625e-16 9.28146448586631e-14
#+end_example
** Conclusion
#+begin_important
We are able to compute the wanted position and orientation of the sample.
Both the measurement and the theory gives the same result.
#+end_important
* Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working
<<sec:compute_pos_error>>
** Introduction :ignore:
We now introduce some positioning error in the stage.
This will induce a global positioning error of the sample with respect to the desired pose that we can compute.
We want to verify that we are able to measure this positioning error and convert it in the frame attached to the Nano-hexapod.
** 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
simulinkproject('../');
#+end_src
#+begin_src matlab
open 'simscape/sim_nano_station_metrology.slx'
#+end_src
** Prepare the Simulation
We load the configuration.
#+begin_src matlab
load('mat/conf_simscape.mat');
#+end_src
We set a small =StopTime=.
#+begin_src matlab
set_param(conf_simscape, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod(struct('actuator', 'piezo'));
initializeSample(struct('mass', 50));
#+end_src
We setup the reference path to be constant.
#+begin_src matlab
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [rad]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0*pi/180, ... % Initial angle [rad]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0, pi]', ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
initializeReferences(opts);
#+end_src
Now we introduce some positioning error.
#+begin_src matlab
Dye = 1e-6; % [m]
Rye = 2e-4; % [rad]
Rze = 1e-5; % [rad]
Dhe = zeros(6,1);
% Dne = [1e-3 ; 0 ; 2e-3 ; 0 ; 3e-3 ; 1e-3]; % [m,rad]
Dhle = [1e-6 ; 2e-6 ; 3e-6 ; -2e-6 ; 1e-6 ; 2e-6]; % [m]
Dne = zeros(6,1);
#+end_src
And we run the simulation.
#+begin_src matlab
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
** Compute the wanted pose of the sample in the NASS Base from the metrology and the reference
Now that we have introduced some positioning error, the computed wanted pose and the measured pose will not be the same.
We would like to compute ${}^M\bm{T}_R$ which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame $\{M\}$).
We have:
\begin{align}
{}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
&= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
\end{align}
The top platform of the nano-hexapod is considered to be rigidly connected to the sample, thus, ${}^M\bm{T}_R$ corresponds to the pose error of the sample with respect to the nano-hexapod platform.
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
n = length(Dref.Dy.Time);
WTr = zeros(4, 4, n);
for i = 1:n
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:), Dref.Dn.Data(i,:));
end
#+end_src
We also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
The *inverse of the transformation matrix* can be obtain by (it is less computation intensive than doing a full inverse)
\begin{equation}
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
\left[ \begin{array}{ccc|c}
& & & \\
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^A\bm{P}_{O_B} \\
& & & \\
\hline
0 & 0 & 0 & 1 \\
\end{array} \right]
\end{equation}
Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
MTr = zeros(4, 4, n);
for i = 1:n
MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
end
#+end_src
Verify that the pose error corresponds to the positioning error of the stages.
#+begin_src matlab :exports none
Edx = MTr(1, 4, end);
Edy = MTr(2, 4, end);
Edz = MTr(3, 4, end);
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
#+end_src
#+begin_src matlab
MTr(1:3, 1:3, end)
Rx = [1 0 0;
0 cos(Erx) -sin(Erx);
0 sin(Erx) cos(Erx)];
Ry = [ cos(Ery) 0 sin(Ery);
0 1 0;
-sin(Ery) 0 cos(Ery)];
Rz = [cos(Erz) -sin(Erz) 0;
sin(Erz) cos(Erz) 0;
0 0 1];
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
#+end_src
#+RESULTS:
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+----------+-----------+-----------+-----------|
| Error | 2.8e-06 | -2.0e-06 | -1.3e-06 | -5.1e-06 | -1.8e-04 | 4.2e-07 |
** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
#+begin_src matlab
opts.Dn_pos = [Edx, Edy, Edz, Erx, Ery, Erz]';
initializeReferences(opts);
#+end_src
And we run the simulation.
#+begin_src matlab
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
We keep the old computed computed reference pose ${}^W\bm{T}_r$ even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
MTr = zeros(4, 4, n);
for i = 1:n
MTr(:, :, i) = [WTm(1:3,1:3,i)', -WTm(1:3,1:3,i)'*WTm(1:3,4,i) ; 0 0 0 1]*WTr(:,:,i);
end
#+end_src
Verify that the pose error is small.
#+begin_src matlab :exports none
Edx = MTr(1, 4, end);
Edy = MTr(2, 4, end);
Edz = MTr(3, 4, end);
Ery = atan2(MTr(1, 3, end), sqrt(MTr(1, 1, end)^2 + MTr(1, 2, end)^2));
Erx = atan2(-MTr(2, 3, end)/cos(Ery), MTr(3, 3, end)/cos(Ery));
Erz = atan2(-MTr(1, 2, end)/cos(Ery), MTr(1, 1, end)/cos(Ery));
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([Edx, Edy, Edz, Erx, Ery, Erz], {'Error'}, {'Edx [m]', 'Edy [m]', 'Edz [m]', 'Erx [rad]', 'Ery [rad]', 'Erz [rad]'}, ' %.1e ');
#+end_src
#+RESULTS:
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+---------+-----------+-----------+-----------|
| Error | 1.0e-16 | -1.1e-18 | 3.3e-20 | 1.5e-16 | 5.8e-17 | -6.1e-16 |
** Conclusion
#+begin_important
Indeed, we are able to convert the position error in the frame of the NASS and then compensate these errors with the NASS.
#+end_important
* Verify that we are able to compensate the errors using the nano-hexapod
* Tests on the transformation from reference to wanted position :noexport:
** 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
** 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
simulinkproject('../');
#+end_src
#+begin_src matlab
open 'simscape/sim_nano_station_metrology.slx'
#+end_src
** 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 => seems better
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 \\
&= {}^W{T_M}^{-1} {}^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