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

Update the way the micro-hexapod is initialize (pitch-roll-yaw)

Browse Source 
Next verified that the function to compute the wanted sample position
is working => yes
This commit is contained in:
Thomas Dehaeze 2019-12-11 09:33:46 +01:00
parent 9fb86964ef
commit b3e630459a
11 changed files with 380 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

313
metrology/index.html
View File

@ -3,7 +3,7 @@
"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-06 ven. 12:02 -->
<!-- 2019-12-11 mer. 09:33 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Metrology</title>
@ -262,7 +262,10 @@ for the JavaScript code in this tag.
TeX: { equationNumbers: {autoNumber: "AMS"},
MultLineWidth: "85%",
TagSide: "right",
TagIndent: ".8em"
TagIndent: ".8em",
Macros: {
bm: ["{\\boldsymbol #1}",1],
}
}
});
</script>
@ -280,24 +283,25 @@ for the JavaScript code in this tag.
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#org756e44b">1. Verify that the function to compute the reference pose is correct</a>
<li><a href="#orge36f66d">1. How do we measure the position of the sample with respect to the granite</a></li>
<li><a href="#orgcafd665">2. Verify that the function to compute the reference pose is correct</a>
<ul>
<li><a href="#org2c3c43f">1.1. Prepare the Simulation</a></li>
<li><a href="#org85e8e2b">1.2. Verify that the pose of the sample is the same as the computed one</a></li>
<li><a href="#org37d3573">1.3. Conclusion</a></li>
<li><a href="#orgf839195">2.1. Prepare the Simulation</a></li>
<li><a href="#org64c331a">2.2. Verify that the pose of the sample is the same as the computed one</a></li>
<li><a href="#org5dd1a72">2.3. Conclusion</a></li>
</ul>
</li>
<li><a href="#orgf9354ae">2. Verify that the function to convert the position error in the frame fixed to the nano-hexapod is working</a>
<li><a href="#orgc313664">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="#org17b1563">2.1. Prepare the Simulation</a></li>
<li><a href="#orge719d33">2.2. Compute the wanted pose of the sample in the NASS Base from the metrology and the reference</a></li>
<li><a href="#org42e7fa5">2.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="#org4609b8f">2.4. Conclusion</a></li>
<li><a href="#org2e27929">3.1. Prepare the Simulation</a></li>
<li><a href="#org3480bf2">3.2. Compute the wanted pose of the sample in the NASS Base from the metrology and the reference</a></li>
<li><a href="#org028d055">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="#org1a89029">3.4. Conclusion</a></li>
</ul>
</li>
<li><a href="#orgcea5e9e">3. Functions</a>
<li><a href="#org99c9716">4. Functions</a>
<ul>
<li><a href="#org1ce366c">3.1. computeReferencePose</a></li>
<li><a href="#orgf3522b3">4.1. computeReferencePose</a></li>
</ul>
</li>
</ul>
@ -310,29 +314,63 @@ Also, all the stages can be perfectly positioned.
</p>
<p>
In section <a href="#org0e13e1c">1</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).
First, in section <a href="#org96a102f">1</a>, is explained how the measurement of the position of the sample with respect to the granite is performed.
</p>
<p>
In section <a href="#orgeb725b9">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).
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="#orgef14317">2</a>, we introduce some positioning error in the position stages.
Then, in section <a href="#org32bd651">3</a>, we introduce some positioning error in the position 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.
Finally, 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-org756e44b" class="outline-2">
<h2 id="org756e44b"><span class="section-number-2">1</span> Verify that the function to compute the reference pose is correct</h2>
<div id="outline-container-orge36f66d" class="outline-2">
<h2 id="orge36f66d"><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="org0e13e1c"></a>
<a id="org96a102f"></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-orgcafd665" class="outline-2">
<h2 id="orgcafd665"><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="orgeb725b9"></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-org2c3c43f" class="outline-3">
<h3 id="org2c3c43f"><span class="section-number-3">1.1</span> Prepare the Simulation</h3>
<div class="outline-text-3" id="text-1-1">
<div id="outline-container-orgf839195" class="outline-3">
<h3 id="orgf839195"><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>
@ -349,6 +387,23 @@ We set a small <code>StopTime</code>.
</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>
@ -359,17 +414,17 @@ We setup the reference path to be constant.
<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-comment"> % Amplitude [deg]</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-comment"> % Initial angle [deg]</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">0</span>; <span class="org-highlight-numbers-number">0</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-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,deg,deg,deg] of the top platform</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">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,deg,deg,deg] of the top platform</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>;
@ -384,6 +439,7 @@ No position error for now (perfect positioning).
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>
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>
@ -397,9 +453,9 @@ And we run the simulation.
</div>
</div>
<div id="outline-container-org85e8e2b" class="outline-3">
<h3 id="org85e8e2b"><span class="section-number-3">1.2</span> Verify that the pose of the sample is the same as the computed one</h3>
<div class="outline-text-3" id="text-1-2">
<div id="outline-container-org64c331a" class="outline-3">
<h3 id="org64c331a"><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>
@ -413,8 +469,8 @@ Let's denote:
We have then computed:
</p>
<ul class="org-ul">
<li>\({}^W\boldsymbol{T}_R\) which corresponds to the wanted pose of the sample with respect to the granite</li>
<li>\({}^W\boldsymbol{T}_M\) which corresponds to the measured pose of the sample with respect to the granite</li>
<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>
@ -422,10 +478,9 @@ We load the reference and we compute the desired trajectory of the sample in the
</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><span class="org-rainbow-delimiters-depth-1">)</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>
@ -445,13 +500,13 @@ WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlig
<p>
As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\boldsymbol{T}_R = {}^W\boldsymbol{T}_M \]
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]
</p>
<p>
Or are least:
\[ {}^W\boldsymbol{T}_R(1:3, 4) = {}^W\boldsymbol{T}_M(1:3, 4) \]
\[ {}^W\boldsymbol{R}_R^t \cdot {}^W\boldsymbol{R}_M = \boldsymbol{I}_3 \]
\[ {}^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">
@ -463,21 +518,21 @@ WTr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlig
<pre class="example">
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
5.38287405101034e-15
9.42822209193395e-15
-7.25141518012618e-16
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.53210777398272e-14 -1.60173523749974e-14 -7.42461647718073e-16
1.60683098771042e-14 1.53210777398272e-14 -2.33146835171283e-15
-3.95516952522712e-16 -1.72084568816899e-15 9.2370555648813e-14
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-org37d3573" class="outline-3">
<h3 id="org37d3573"><span class="section-number-3">1.3</span> Conclusion</h3>
<div class="outline-text-3" id="text-1-3">
<div id="outline-container-org5dd1a72" class="outline-3">
<h3 id="org5dd1a72"><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.
@ -489,11 +544,11 @@ Both the measurement and the theory gives the same result.
</div>
</div>
<div id="outline-container-orgf9354ae" class="outline-2">
<h2 id="orgf9354ae"><span class="section-number-2">2</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-2">
<div id="outline-container-orgc313664" class="outline-2">
<h2 id="orgc313664"><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="orgef14317"></a>
<a id="org32bd651"></a>
</p>
<p>
We now introduce some positioning error in the stage.
@ -504,9 +559,9 @@ This will induce a global positioning error of the sample with respect to the de
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-org17b1563" class="outline-3">
<h3 id="org17b1563"><span class="section-number-3">2.1</span> Prepare the Simulation</h3>
<div class="outline-text-3" id="text-2-1">
<div id="outline-container-org2e27929" class="outline-3">
<h3 id="org2e27929"><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>
@ -532,15 +587,15 @@ We setup the reference path to be constant.
<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 [deg]</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">180</span>, ...<span class="org-comment"> % Initial angle [deg]</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,deg,deg,deg] of the top platform</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,deg,deg,deg] of the top platform</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>;
@ -554,7 +609,10 @@ Now we introduce some positioning error.
<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 = <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">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">2e</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> ; <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-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m,rad]</span>
Dhe = <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">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">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">3e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m,rad]</span>
% Dhe = zeros(<span class="org-highlight-numbers-number">6</span>,<span class="org-highlight-numbers-number">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]
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>
@ -568,45 +626,44 @@ And we run the simulation.
</div>
</div>
<div id="outline-container-orge719d33" class="outline-3">
<h3 id="orge719d33"><span class="section-number-3">2.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-2-2">
<div id="outline-container-org3480bf2" class="outline-3">
<h3 id="org3480bf2"><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\boldsymbol{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 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\boldsymbol{T}_R &= {}^M\boldsymbol{T}_W \cdot {}^W\boldsymbol{T}_R \\
&= {}^W{\boldsymbol{T}_M}^{-1} \cdot {}^W\boldsymbol{T}_R
{}^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\boldsymbol{T}_R\) corresponds to the pose error of the sample with respect to the nano-hexapod platform.
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\boldsymbol{T}_R\).
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><span class="org-rainbow-delimiters-depth-1">)</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\boldsymbol{T}_M\).
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>;
@ -620,18 +677,19 @@ WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlig
<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\boldsymbol{T}_A = {}^A\boldsymbol{T}_B^{-1} =
\begin{equation}
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
\left[ \begin{array}{ccc|c}
& & & \\
& {}^A\boldsymbol{R}_B^T & & -{}^A \boldsymbol{R}_B^T {}^A\boldsymbol{P}_{O_B} \\
& {}^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\boldsymbol{T}_R\).
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>;
@ -644,6 +702,22 @@ Finally, we compute \({}^M\boldsymbol{T}_R\).
<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">
@ -677,10 +751,10 @@ Verify that the pose error corresponds to the positioning error of the stages.
<tr>
<td class="org-left">Error</td>
<td class="org-right">-1.0e-03</td>
<td class="org-right">-3.0e-03</td>
<td class="org-right">1.0e-06</td>
<td class="org-right">-2.0e-03</td>
<td class="org-right">-1.0e-03</td>
<td class="org-right">-2.0e-03</td>
<td class="org-right">3.0e-06</td>
<td class="org-right">-3.0e-03</td>
</tr>
</tbody>
@ -688,14 +762,14 @@ Verify that the pose error corresponds to the positioning error of the stages.
</div>
</div>
<div id="outline-container-org42e7fa5" class="outline-3">
<h3 id="org42e7fa5"><span class="section-number-3">2.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-2-3">
<div id="outline-container-org028d055" class="outline-3">
<h3 id="org028d055"><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 impose a displacement of the nano hexapod corresponding to the measured position error.
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, <span class="org-highlight-numbers-number">180</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>Erx, <span class="org-highlight-numbers-number">180</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>Ery, <span class="org-highlight-numbers-number">180</span><span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>Erz<span class="org-rainbow-delimiters-depth-1">]</span><span class="org-type">'</span>;
<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>
@ -709,21 +783,12 @@ And we run the simulation.
</div>
<p>
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix \({}^WT_R\).
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>
<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><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 \({}^WT_M\).
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>;
@ -735,7 +800,7 @@ WTm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlig
</div>
<p>
Finally, we compute \({}^MT_R\).
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>;
@ -780,21 +845,21 @@ Verify that the pose error is small.
<tbody>
<tr>
<td class="org-left">Error</td>
<td class="org-right">1.2e-16</td>
<td class="org-right">3.3e-16</td>
<td class="org-right">2.3e-16</td>
<td class="org-right">-6.2e-17</td>
<td class="org-right">1.1e-16</td>
<td class="org-right">2.2e-16</td>
<td class="org-right">-3.0e-09</td>
<td class="org-right">-1.0e-09</td>
<td class="org-right">3.0e-09</td>
<td class="org-right">2.0e-09</td>
<td class="org-right">-1.0e-09</td>
<td class="org-right">3.0e-09</td>
</tr>
</tbody>
</table>
</div>
</div>
<div id="outline-container-org4609b8f" class="outline-3">
<h3 id="org4609b8f"><span class="section-number-3">2.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-4">
<div id="outline-container-org1a89029" class="outline-3">
<h3 id="org1a89029"><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.
@ -805,15 +870,15 @@ Indeed, we are able to convert the position error in the frame of the NASS and t
</div>
</div>
<div id="outline-container-orgcea5e9e" class="outline-2">
<h2 id="orgcea5e9e"><span class="section-number-2">3</span> Functions</h2>
<div class="outline-text-2" id="text-3">
<div id="outline-container-org99c9716" class="outline-2">
<h2 id="org99c9716"><span class="section-number-2">4</span> Functions</h2>
<div class="outline-text-2" id="text-4">
</div>
<div id="outline-container-org1ce366c" class="outline-3">
<h3 id="org1ce366c"><span class="section-number-3">3.1</span> computeReferencePose</h3>
<div class="outline-text-3" id="text-3-1">
<div id="outline-container-orgf3522b3" class="outline-3">
<h3 id="orgf3522b3"><span class="section-number-3">4.1</span> computeReferencePose</h3>
<div class="outline-text-3" id="text-4-1">
<p>
<a id="org89b0447"></a>
<a id="org8505281"></a>
</p>
<p>
@ -821,13 +886,17 @@ 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-rainbow-delimiters-depth-1">)</span>
<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)</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, Ry, Rz, Dh -</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>
@ -869,10 +938,30 @@ This Matlab function is accessible <a href="src/computeReferencePose.m">here</a>
<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> = Rhx<span class="org-type">*</span>Rhy<span class="org-type">*</span>Rhz;
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;
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>
@ -882,7 +971,7 @@ This Matlab function is accessible <a href="src/computeReferencePose.m">here</a>
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-12-06 ven. 12:02</p>
<p class="date">Created: 2019-12-11 mer. 09:33</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
</div>
</body>

78
metrology/index.org
View File

@ -64,7 +64,7 @@ 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 $\boldsymbol{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.
@ -101,6 +101,20 @@ We set a small =StopTime=.
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( ...
@ -115,11 +129,11 @@ We setup the reference path to be constant.
'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; 0; 0; -3*pi/180; 1*pi/180; 3*pi/180], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'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; 0; 0; 1*pi/180; 0; 1*pi/180] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'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);
@ -146,8 +160,8 @@ Let's denote:
- $\{M\}$ the frame corresponding to the measured pose of the sample
We have then computed:
- ${}^W\boldsymbol{T}_R$ which corresponds to the wanted pose of the sample with respect to the granite
- ${}^W\boldsymbol{T}_M$ which corresponds to the measured pose of the sample with respect to the granite
- ${}^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 ${}^WT_R$.
#+begin_src matlab
@ -169,11 +183,11 @@ From that we can compute the homogeneous transformation matrix ${}^WT_M$.
#+end_src
As the simulation is perfect (no measurement error and no motion error), we should have that
\[ {}^W\boldsymbol{T}_R = {}^W\boldsymbol{T}_M \]
\[ {}^W\bm{T}_R = {}^W\bm{T}_M \]
Or are least:
\[ {}^W\boldsymbol{T}_R(1:3, 4) = {}^W\boldsymbol{T}_M(1:3, 4) \]
\[ {}^W\boldsymbol{R}_R^t \cdot {}^W\boldsymbol{R}_M = \boldsymbol{I}_3 \]
\[ {}^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)
@ -184,14 +198,14 @@ Or are least:
#+begin_example
WTr(1:3, 4, end)-WTm(1:3, 4, end)
ans =
-8.47173893536723e-15
-1.38430933382949e-15
-8.88361324636402e-16
1.8027246362351e-14
1.40408518145563e-14
6.93889390390723e-17
WTr(1:3, 1:3, end)'*WTm(1:3, 1:3, end)-eye(3)
ans =
2.66453525910038e-15 1.19459143341844e-15 -1.07098845850834e-17
-1.185456383777e-15 2.66453525910038e-15 2.9392720896082e-16
1.07732002978906e-17 -2.9392720896082e-16 2.88657986402541e-15
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
@ -276,17 +290,17 @@ And we run the simulation.
** 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\boldsymbol{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 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\boldsymbol{T}_R &= {}^M\boldsymbol{T}_W \cdot {}^W\boldsymbol{T}_R \\
&= {}^W{\boldsymbol{T}_M}^{-1} \cdot {}^W\boldsymbol{T}_R
{}^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\boldsymbol{T}_R$ corresponds to the pose error of the sample with respect to the nano-hexapod platform.
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\boldsymbol{T}_R$.
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);
@ -296,7 +310,7 @@ We load the reference and we compute the desired trajectory of the sample in the
#+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\boldsymbol{T}_M$.
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);
@ -307,17 +321,17 @@ From that we can compute the homogeneous transformation matrix ${}^W\boldsymbol{
The *inverse of the transformation matrix* can be obtain by (it is less computation intensive than doing a full inverse)
\begin{equation}
{}^B\boldsymbol{T}_A = {}^A\boldsymbol{T}_B^{-1} =
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
\left[ \begin{array}{ccc|c}
& & & \\
& {}^A\boldsymbol{R}_B^T & & -{}^A \boldsymbol{R}_B^T {}^A\boldsymbol{P}_{O_B} \\
& {}^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\boldsymbol{T}_R$.
Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
MTr = zeros(4, 4, n);
for i = 1:n
@ -372,10 +386,10 @@ And we run the simulation.
sim('simscape/sim_nano_station_metrology.slx');
#+end_src
We keep the old computed computed reference pose ${}^W\boldsymbol{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.
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\boldsymbol{T}_M$.
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);
@ -384,7 +398,7 @@ From that we can compute the homogeneous transformation matrix ${}^W\boldsymbol{
WTm(4, 4, :) = 1;
#+end_src
Finally, we compute ${}^M\boldsymbol{T}_R$.
Finally, we compute ${}^M\bm{T}_R$.
#+begin_src matlab
MTr = zeros(4, 4, n);
for i = 1:n
@ -437,7 +451,7 @@ This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
% - 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 [m, m, m, rad, rad, 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:
@ -480,7 +494,7 @@ This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
0 0 1 Dh(3) ;
0 0 0 1 ];
Rh(1:3, 1:3) = Rhx*Rhy*Rhz;
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
%% Nano-Hexapod
Rnx = [1 0 0;
@ -534,8 +548,8 @@ Let's define the following frames:
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\boldsymbol{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\boldsymbol{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T$ in rotations
- 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)
@ -655,7 +669,7 @@ Let's compute the corresponding orientation using screw axis.
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\boldsymbol{s}_m$ is the eigen vector of the rotation matrix $R$ corresponding to the eigen value $\lambda = 1$
- ${}^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]
@ -675,7 +689,7 @@ We then obtain the orientation measurement in the form of screw coordinate $\the
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\boldsymbol{s}_\epsilon = \theta_r {}^W\boldsymbol{s}_r - \theta_m {}^W\boldsymbol{s}_m \]
\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]
#+begin_src matlab
WPe = WPr - WPm;

103
simscape/index.org
View File

@ -755,8 +755,7 @@ This Matlab function is accessible [[file:../src/runSimulation.m][here]].
'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
end
#+end_src
* Initialize Elements
<<sec:initialize_elements>>
* Helping Functions
** Experiment
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeExperiment.m
@ -797,7 +796,6 @@ This Matlab function is accessible [[file:../src/initializeExperiment.m][here]].
:header-args:matlab+: :comments org :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:initializeReferences>>
This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
@ -817,7 +815,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'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
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
'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
@ -887,14 +885,37 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
%% Micro-Hexapod
t = [0, Ts];
Dh = zeros(length(t), 6);
Dhl = zeros(length(t), 6);
switch opts.Dh_type
case 'constant'
Dh = [opts.Dh_pos, opts.Dh_pos];
load('./mat/stages.mat', 'micro_hexapod');
AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];
tx = opts.Dh_pos(4);
ty = opts.Dh_pos(5);
tz = opts.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
Dhl = [Dhl, Dhl];
otherwise
warning('Dh_type is not set correctly');
end
Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl));
%% Axis Compensation - Rm
t = [0, Ts];
@ -914,7 +935,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Dn = struct('time', t, 'signals', struct('values', Dn));
%% Save
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
end
#+end_src
@ -1108,6 +1129,42 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
end
#+end_src
** Inverse Kinematics of the Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
:header-args:matlab+: :comments org :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:inverseKinematicsHexapod>>
This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].
#+begin_src matlab
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
#+end_src
* Initialize Elements
<<sec:initialize_elements>>
** Ground
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeGround.m
@ -1420,19 +1477,19 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]].
** Initialize Hexapod legs' length
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeHexapodPosition.m
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
:header-args:matlab+: :comments org :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:initializeHexapodPosition>>
<<sec:inverseKinematicsHexapod>>
This Matlab function is accessible [[file:../src/initializeHexapodPosition.m][here]].
This Matlab function is accessible [[file:../src/inverseKinematicsHexapod.m][here]].
#+begin_src matlab
function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition -
function [hexapod] = inverseKinematicsHexapod(hexapod, AP, ARB)
% inverseKinematicsHexapod -
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
@ -1563,7 +1620,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB);
%% Save
save('./mat/stages.mat', 'micro_hexapod', '-append');
@ -1666,28 +1723,6 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
J(:, 1:3) = RM';
J(:, 4:6) = cross(M_pos_base, RM)';
end
%%
function [L] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
end
#+end_src

BIN
simscape/sim_nano_station_disp.slx
View File

Binary file not shown.

BIN
simscape/sim_nano_station_metrology.slx
View File

Binary file not shown.

BIN
simscape_subsystems/hexapod_leg.slx
View File

Binary file not shown.

BIN
simscape_subsystems/micro_hexapod_rigid_simple.slx
View File

Binary file not shown.

BIN
simscape_subsystems/nass_references.slx
View File

Binary file not shown.

34
src/computeReferencePose.m
View File

@ -9,13 +9,17 @@
% This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
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)
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
%
% Inputs:
% - Dy, Ry, Rz, Dh -
% - 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 -
@ -57,8 +61,28 @@ function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
0 0 1 Dh(3) ;
0 0 0 1 ];
Rh(1:3, 1:3) = Rhx*Rhy*Rhz;
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;
WTr = Rty*Rry*Rrz*Rh*Rn;
end

24
src/initializeMicroHexapod.m
View File

@ -109,7 +109,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB);
%% Save
save('./mat/stages.mat', 'micro_hexapod', '-append');
@ -212,26 +212,4 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
J(:, 1:3) = RM';
J(:, 4:6) = cross(M_pos_base, RM)';
end
%%
function [L] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
end

49
src/initializeReferences.m
View File

@ -17,13 +17,13 @@ function [ref] = initializeReferences(opts_param)
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [deg]
'Ry_amplitude', 0, ... % Amplitude [rad]
'Ry_period', 10, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0, ... % Initial angle [deg]
'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'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
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
'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
@ -64,13 +64,13 @@ function [ref] = initializeReferences(opts_param)
switch opts.Ry_type
case 'constant'
Ry(:) = pi/180*opts.Ry_amplitude;
Ry(:) = opts.Ry_amplitude;
case 'triangular'
Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t;
Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period);
Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period);
Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
case 'sinusoidal'
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
otherwise
warning('Ry_type is not set correctly');
end
@ -82,9 +82,9 @@ function [ref] = initializeReferences(opts_param)
switch opts.Rz_type
case 'constant'
Rz(:) = pi/180*opts.Rz_amplitude;
Rz(:) = opts.Rz_amplitude;
case 'rotating'
Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t;
Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
otherwise
warning('Rz_type is not set correctly');
end
@ -93,16 +93,37 @@ function [ref] = initializeReferences(opts_param)
%% Micro-Hexapod
t = [0, Ts];
Dh = zeros(length(t), 6);
opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad]
Dhl = zeros(length(t), 6);
switch opts.Dh_type
case 'constant'
Dh = [opts.Dh_pos, opts.Dh_pos];
load('./mat/stages.mat', 'micro_hexapod');
AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];
tx = opts.Dh_pos(4);
ty = opts.Dh_pos(5);
tz = opts.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
Dhl = [Dhl, Dhl];
otherwise
warning('Dh_type is not set correctly');
end
Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl));
%% Axis Compensation - Rm
t = [0, Ts];
@ -113,8 +134,6 @@ function [ref] = initializeReferences(opts_param)
t = [0, Ts];
Dn = zeros(length(t), 6);
opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad]
switch opts.Dn_type
case 'constant'
Dn = [opts.Dn_pos, opts.Dn_pos];
@ -124,5 +143,5 @@ function [ref] = initializeReferences(opts_param)
Dn = struct('time', t, 'signals', struct('values', Dn));
%% Save
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
end