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

Tomography Experiment with and without disturbances

Browse Source
This commit is contained in:
Thomas Dehaeze 2019-12-13 19:07:54 +01:00
parent 27055d8f24
commit b4180feded
16 changed files with 406 additions and 610 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

443
experiment_tomography/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-04 mer. 21:55 --> <!-- 2019-12-13 ven. 19:06 -->
<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>Tomography Experiment</title> <title>Tomography Experiment</title>
@ -193,6 +193,13 @@
.org-svg { width: 90%; } .org-svg { width: 90%; }
/*]]>*/--> /*]]>*/-->
</style> </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"> <script type="text/javascript">
/* /*
@licstart The following is the entire license notice for the @licstart The following is the entire license notice for the
@ -239,28 +246,6 @@ for the JavaScript code in this tag.
} }
/*]]>*///--> /*]]>*///-->
</script> </script>
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
displayAlign: "center",
displayIndent: "0em",
"HTML-CSS": { scale: 100,
linebreaks: { automatic: "false" },
webFont: "TeX"
},
SVG: {scale: 100,
linebreaks: { automatic: "false" },
font: "TeX"},
NativeMML: {scale: 100},
TeX: { equationNumbers: {autoNumber: "AMS"},
MultLineWidth: "85%",
TagSide: "right",
TagIndent: ".8em"
}
});
</script>
<script type="text/javascript"
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
</head> </head>
<body> <body>
<div id="org-div-home-and-up"> <div id="org-div-home-and-up">
@ -273,378 +258,162 @@ 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="#orgab97c4b">1. Initialize Experiment</a></li> <li><a href="#org84a12d1">1. Simscape Model</a></li>
<li><a href="#org26c554f">2. Run the Tomography Experiment</a></li> <li><a href="#orgcb251b3">2. Tomography Experiment with no disturbances</a></li>
<li><a href="#orgc507e1e">3. <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</a> <li><a href="#orgcc1bc22">3. With Perturbations</a></li>
<ul>
<li><a href="#org4537a86">3.1. Wanted Position of the Sample with respect to the Granite</a></li>
<li><a href="#org6c67ae5">3.2. Measured Position of the Sample with respect to the Granite</a></li>
<li><a href="#org69e38aa">3.3. Positioning Error with respect to the Granite</a></li>
<li><a href="#org5246d94">3.4. Position Error Expressed in the Nano-Hexapod Frame</a></li>
</ul>
</li>
</ul> </ul>
</div> </div>
</div> </div>
<div id="outline-container-orgab97c4b" class="outline-2"> <div id="outline-container-org84a12d1" class="outline-2">
<h2 id="orgab97c4b"><span class="section-number-2">1</span> Initialize Experiment</h2> <h2 id="org84a12d1"><span class="section-number-2">1</span> Simscape Model</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p>
The simulink file to do tomography experiments is <code>sim_nano_station_tomo.slx</code>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">open<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'experiment_tomography/matlab/sim_nano_station_tomo.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>
</pre>
</div>
<p>
We load the shared simulink configuration and we set a small <code>StopTime</code>.
</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>;
<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">10</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p> <p>
We first initialize all the stages. We first initialize all the stages.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code>initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;</code> <pre class="src src-matlab">initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;</code> initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
<code>initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code> initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<code>initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code> initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre> </pre>
</div> </div>
<p> <p>
We initialize the reference path for all the stages. We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code>initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code> <pre class="src src-matlab">initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
And we initialize the disturbances.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>initDisturbances<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org26c554f" class="outline-2"> <div id="outline-container-orgcb251b3" class="outline-2">
<h2 id="org26c554f"><span class="section-number-2">2</span> Run the Tomography Experiment</h2> <h2 id="orgcb251b3"><span class="section-number-2">2</span> Tomography Experiment with no disturbances</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
<p> <p>
We first load the simulation configuration And we initialize the disturbances to zero.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code>load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code> <pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
<span class="org-string">'Dwx'</span>, <span class="org-constant">false</span>, ...<span class="org-comment"> % Ground Motion - X direction</span>
<span class="org-string">'Dwy'</span>, <span class="org-constant">false</span>, ...<span class="org-comment"> % Ground Motion - Y direction</span>
<span class="org-string">'Dwz'</span>, <span class="org-constant">false</span>, ...<span class="org-comment"> % Ground Motion - Z direction</span>
<span class="org-string">'Fty_x'</span>, <span class="org-constant">false</span>, ...<span class="org-comment"> % Translation Stage - X direction</span>
<span class="org-string">'Fty_z'</span>, <span class="org-constant">false</span>, ...<span class="org-comment"> % Translation Stage - Z direction</span>
<span class="org-string">'Frz_z'</span>, <span class="org-constant">false</span> ...<span class="org-comment"> % Spindle - Z direction</span>
<span class="org-rainbow-delimiters-depth-1">)</span>;
initDisturbances<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, '<span class="org-highlight-numbers-number">1</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code> <pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'sim_nano_station_tomo'</span><span class="org-rainbow-delimiters-depth-1">)</span>
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'sim_nano_station_tomo'</span>, <span class="org-string">'SimulationCommand'</span>, <span class="org-string">'start'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code> <pre class="src src-matlab">Dsm_without_dist = Dsm;
</pre> </pre>
</div> </div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>;
hold on;
plot<span class="org-rainbow-delimiters-depth-1">(</span>Dsm_without_dist.x.Time, Dsm_without_dist.x.Data, <span class="org-string">'DisplayName'</span>, <span class="org-string">'x'</span><span class="org-rainbow-delimiters-depth-1">)</span>
plot<span class="org-rainbow-delimiters-depth-1">(</span>Dsm_without_dist.y.Time, Dsm_without_dist.y.Data, <span class="org-string">'DisplayName'</span>, <span class="org-string">'y'</span><span class="org-rainbow-delimiters-depth-1">)</span>
plot<span class="org-rainbow-delimiters-depth-1">(</span>Dsm_without_dist.z.Time, Dsm_without_dist.z.Data, <span class="org-string">'DisplayName'</span>, <span class="org-string">'z'</span><span class="org-rainbow-delimiters-depth-1">)</span>
hold off;
xlim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">2</span>, <span class="org-constant">inf</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>;
legend<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'location'</span>, <span class="org-string">'northeast'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<div id="org8e9717d" class="figure">
<p><img src="figs/exp_tomo_without_dist_trans.png" alt="exp_tomo_without_dist_trans.png" />
</p>
<p><span class="figure-number">Figure 1: </span>X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances (<a href="./figs/exp_tomo_without_dist_trans.png">png</a>, <a href="./figs/exp_tomo_without_dist_trans.pdf">pdf</a>)</p>
</div>
<p>
Rotations.
Think of the good way to plot these rotations with respect to time.
</p>
</div> </div>
</div> </div>
<div id="outline-container-orgc507e1e" class="outline-2"> <div id="outline-container-orgcc1bc22" class="outline-2">
<h2 id="orgc507e1e"><span class="section-number-2">3</span> <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</h2> <h2 id="orgcc1bc22"><span class="section-number-2">3</span> With Perturbations</h2>
<div class="outline-text-2" id="text-3"> <div class="outline-text-2" id="text-3">
<ul class="org-ul">
<li class="on"><code>[X]</code> Are the rotation matrix commutable? =&gt; no</li>
<li class="on"><code>[X]</code> How to express the measured rotation errors? =&gt; screw axis coordinate seems nice (used in Taghirad's book)</li>
<li class="off"><code>[&#xa0;]</code> Should ask Veijo how he specifies the position of the Symetrie Hexapod</li>
<li class="off"><code>[&#xa0;]</code> Create functions for all distinct part and then include that in Simulink</li>
<li class="off"><code>[&#xa0;]</code> How the express the orientation error?</li>
<li class="off"><code>[&#xa0;]</code> If we use screw coordinate, can we add/subtract them?</li>
<li class="off"><code>[&#xa0;]</code> Do some simple tests to verify that the algorithm is working fine</li>
</ul>
<blockquote>
<p>
Rx = [1 0 0;
0 cos(t) -sin(t);
0 sin(t) cos(t)];
</p>
<p>
Ry = [ cos(t) 0 sin(t);
0 1 0;
-sin(t) 0 cos(t)];
</p>
<p>
Rz = [cos(t) -sin(t) 0;
sin(t) cos(t) 0;
0 0 1];
</p>
</blockquote>
<p>
Let's define the following frames:
</p>
<ul class="org-ul">
<li>\(\{W\}\) the frame that is <b>fixed to the granite</b> and its origin at the theoretical meeting point between the X-ray and the spindle axis.</li>
<li>\(\{S\}\) the frame <b>attached to the sample</b> (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
Its origin is \(O_S\).</li>
<li>\(\{T\}\) the theoretical wanted frame that correspond to the wanted pose of the frame \(\{S\}\).
\(\{T\}\) is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
The origin of \(T\) is \(O_T\) and is the wanted position of the sample.</li>
</ul>
<p>
Thus:
</p>
<ul class="org-ul">
<li>the <b>measurement</b> of the position of the sample corresponds to \({}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T\) in translation and to \(\theta_m {}^W\bm{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T\) in rotations</li>
<li>the <b>wanted position</b> of the sample expressed w.r.t. the granite is \({}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T\) in translation and to \(\theta_r {}^W\bm{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T\) in rotations</li>
</ul>
</div>
<div id="outline-container-org4537a86" class="outline-3">
<h3 id="org4537a86"><span class="section-number-3">3.1</span> Wanted Position of the Sample with respect to the Granite</h3>
<div class="outline-text-3" id="text-3-1">
<p>
Let's define the wanted position of each stage.
</p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code>Ty = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code> <pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
<code>Ry = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code> <span class="org-string">'Dwx'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Ground Motion - X direction</span>
<code>Rz = <span class="org-highlight-numbers-number">180</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code> <span class="org-string">'Dwy'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Ground Motion - Y direction</span>
<code></code> <span class="org-string">'Dwz'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Ground Motion - Z direction</span>
<code><span class="org-comment">% Hexapod (first consider only translations)</span></code> <span class="org-string">'Fty_x'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Translation Stage - X direction</span>
<code>Thx = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code> <span class="org-string">'Fty_z'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Translation Stage - Z direction</span>
<code>Thy = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code> <span class="org-string">'Frz_z'</span>, <span class="org-constant">true</span> ...<span class="org-comment"> % Spindle - Z direction</span>
<code>Thz = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code> <span class="org-rainbow-delimiters-depth-1">)</span>;
</pre> initDisturbances<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
</div>
<p>
Now, we compute the corresponding wanted translation and rotation of the sample with respect to the granite frame \(\{W\}\).
This corresponds to \({}^WO_T\) and \(\theta_m {}^Ws_m\).
</p>
<p>
To do so, we have to define the homogeneous transformation for each stage.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code><span class="org-comment">% Translation Stage</span></code>
<code>Rty = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Ty;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code></code>
<code><span class="org-comment">% Tilt Stage - Pure rotating aligned with Ob</span></code>
<code>Rry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code></code>
<code><span class="org-comment">% Spindle - Rotation along the Z axis</span></code>
<code>Rrz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code></code>
<code><span class="org-comment">% Micro-Hexapod (only rotations first)</span></code>
<code>Rh = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> Thx ;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Thy ;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> Thz ;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
</pre>
</div>
<p>
We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>Ttot = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh;</code>
</pre>
</div>
<p>
Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>WOr = Ttot<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>WOr = WOr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code>thetar = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>Ttot<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span></code> <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">'sim_nano_station_tomo'</span><span class="org-rainbow-delimiters-depth-1">)</span>
<code><span class="org-keyword">if</span> thetar <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
<code> WSr = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code><span class="org-keyword">else</span></code>
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>Ttot<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
<code> WSr = thetar<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">&lt;</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
<code><span class="org-keyword">end</span></code>
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><code>WPr = <span class="org-rainbow-delimiters-depth-1">[</span>WOr ; WSr<span class="org-rainbow-delimiters-depth-1">]</span>;</code> <pre class="src src-matlab"><span class="org-type">figure</span>;
</pre> hold on;
</div> plot<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.x.Time, Dsm.x.Data, <span class="org-string">'DisplayName'</span>, <span class="org-string">'x'</span><span class="org-rainbow-delimiters-depth-1">)</span>
</div> plot<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.y.Time, Dsm.y.Data, <span class="org-string">'DisplayName'</span>, <span class="org-string">'y'</span><span class="org-rainbow-delimiters-depth-1">)</span>
</div> plot<span class="org-rainbow-delimiters-depth-1">(</span>Dsm.z.Time, Dsm.z.Data, <span class="org-string">'DisplayName'</span>, <span class="org-string">'z'</span><span class="org-rainbow-delimiters-depth-1">)</span>
hold off;
<div id="outline-container-org6c67ae5" class="outline-3"> xlim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">2</span>, <span class="org-constant">inf</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<h3 id="org6c67ae5"><span class="section-number-3">3.2</span> Measured Position of the Sample with respect to the Granite</h3> legend<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'location'</span>, <span class="org-string">'northeast'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<div class="outline-text-3" id="text-3-2">
<p>
The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code><span class="org-comment">% Measurements: Xm, Ym, Zm, Rx, Ry, Rz</span></code>
<code>Dxm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
<code>Dym = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
<code>Dzm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
<code></code>
<code>Rxm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
<code>Rym = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
<code>Rzm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
</pre> </pre>
</div> </div>
<p>
Let's compute the corresponding orientation using screw axis. <div id="org230990b" class="figure">
<p><img src="figs/exp_tomo_dist_trans.png" alt="exp_tomo_dist_trans.png" />
</p> </p>
<div class="org-src-container"> <p><span class="figure-number">Figure 2: </span>X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances (<a href="./figs/exp_tomo_dist_trans.png">png</a>, <a href="./figs/exp_tomo_dist_trans.pdf">pdf</a>)</p>
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code></code>
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
</pre>
</div>
<p>
We then obtain the orientation measurement in the form of screw coordinate \(\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T\) where:
</p>
<ul class="org-ul">
<li>\(\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}\)</li>
<li>\({}^W\bm{s}_m\) is the eigen vector of the rotation matrix \(R\) corresponding to the eigen value \(\lambda = 1\)</li>
</ul>
<div class="org-src-container">
<pre class="src src-matlab"><code>thetam = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>STw<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [rad]</span></code>
<code><span class="org-keyword">if</span> thetam <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
<code> WSm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code><span class="org-keyword">else</span></code>
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>STw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
<code> WSm = thetam<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">&lt;</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
<code><span class="org-keyword">end</span></code>
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><code>WPm = <span class="org-rainbow-delimiters-depth-1">[</span>Dxm ; Dym ; Dzm ; WSm<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
</pre>
</div>
</div>
</div>
<div id="outline-container-org69e38aa" class="outline-3">
<h3 id="org69e38aa"><span class="section-number-3">3.3</span> Positioning Error with respect to the Granite</h3>
<div class="outline-text-3" id="text-3-3">
<p>
The wanted position expressed with respect to the granite is \({}^WO_T\) and the measured position with respect to the granite is \({}^WO_S\), thus the <b>position error</b> expressed in \(\{W\}\) is
\[ {}^W E = {}^W O_T - {}^W O_S \]
The same is true for rotations:
\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>WPe = WPr <span class="org-type">-</span> WPm;</code>
</pre>
</div>
<blockquote>
<p>
Now we want to express this error in a frame attached to the <b>base of the nano-hexapod</b> with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).
</p>
<p>
Or maybe should we want to express this error with respect to the <b>top platform of the nano-hexapod</b>?
We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
We could compute the position of the bottom platform in two ways:
</p>
<ul class="org-ul">
<li>from the encoders of each stage</li>
<li>from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)</li>
</ul>
<p>
A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
</p>
</blockquote>
</div>
</div>
<div id="outline-container-org5246d94" class="outline-3">
<h3 id="org5246d94"><span class="section-number-3">3.4</span> Position Error Expressed in the Nano-Hexapod Frame</h3>
<div class="outline-text-3" id="text-3-4">
<p>
We now want the position error to be expressed in \(\{S\}\) (the frame attach to the sample):
\[ {}^S E = {}^S T_W \cdot {}^W E \]
</p>
<p>
Thus we need to compute the homogeneous transformation \({}^ST_W\).
Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code></code>
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
</pre>
</div>
<p>
Translation Error.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>SEm = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>SEm = SEm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
</pre>
</div>
<p>
Rotation Error.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><code>SEr = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
<code>SEr = SEr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><code>Etot = <span class="org-rainbow-delimiters-depth-1">[</span>SEm ; SEr<span class="org-rainbow-delimiters-depth-1">]</span></code>
</pre>
</div>
</div> </div>
</div> </div>
</div> </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-04 mer. 21:55</p> <p class="date">Created: 2019-12-13 ven. 19:06</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>

120
experiment_tomography/index.org
View File

@ -9,13 +9,13 @@
#+HTML_LINK_HOME: ../index.html #+HTML_LINK_HOME: ../index.html
#+HTML_LINK_UP: ../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/htmlize.css"/>
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.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: <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/jquery.min.js"></script>
# #+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.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/jquery.stickytableheaders.min.js"></script>
# #+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script> #+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
#+HTML_MATHJAX: align: center tagside: right font: TeX #+HTML_MATHJAX: align: center tagside: right font: TeX
@ -41,6 +41,7 @@
#+PROPERTY: header-args:latex+ :output-dir figs #+PROPERTY: header-args:latex+ :output-dir figs
:END: :END:
* Introduction :ignore:
* Matlab Init :noexport:ignore: * Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
@ -54,11 +55,18 @@
simulinkproject('../'); simulinkproject('../');
#+end_src #+end_src
* Simscape Model
The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.
#+begin_src matlab #+begin_src matlab
open('experiment_tomography/matlab/sim_nano_station_tomo.slx') open('experiment_tomography/matlab/sim_nano_station_tomo.slx')
#+end_src #+end_src
* Initialize Experiment We load the shared simulink configuration and we set a small =StopTime=.
#+begin_src matlab
load('mat/conf_simscape.mat');
set_param(conf_simscape, 'StopTime', '10');
#+end_src
We first initialize all the stages. We first initialize all the stages.
#+begin_src matlab #+begin_src matlab
initializeGround(); initializeGround();
@ -74,38 +82,98 @@ We first initialize all the stages.
#+end_src #+end_src
We initialize the reference path for all the stages. We initialize the reference path for all the stages.
All stage is set to its zero position except the Spindle which is rotating at 60rpm.
#+begin_src matlab #+begin_src matlab
initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1)); initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));
#+end_src #+end_src
And we initialize the disturbances. * Tomography Experiment with no disturbances
And we initialize the disturbances to zero.
#+begin_src matlab #+begin_src matlab
initDisturbances(); opts = struct(...
#+end_src 'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
* Run the Tomography Experiment 'Dwz', false, ... % Ground Motion - Z direction
We first load the simulation configuration 'Fty_x', false, ... % Translation Stage - X direction
#+begin_src matlab 'Fty_z', false, ... % Translation Stage - Z direction
load('mat/conf_simscape.mat'); 'Frz_z', false ... % Spindle - Z direction
);
initDisturbances(opts);
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
set_param(conf_simscape, 'StopTime', '1'); sim('sim_nano_station_tomo')
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
set_param('sim_nano_station_tomo', 'SimulationCommand', 'start'); Dsm_without_dist = Dsm;
#+end_src #+end_src
* TODO Tests on the transformation from reference to wanted position #+begin_src matlab
- [X] Are the rotation matrix commutable? => no figure;
- [X] How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book) hold on;
- [ ] Should ask Veijo how he specifies the position of the Symetrie Hexapod plot(Dsm_without_dist.x.Time, Dsm_without_dist.x.Data, 'DisplayName', 'x')
- [ ] Create functions for all distinct part and then include that in Simulink plot(Dsm_without_dist.y.Time, Dsm_without_dist.y.Data, 'DisplayName', 'y')
- [ ] How the express the orientation error? plot(Dsm_without_dist.z.Time, Dsm_without_dist.z.Data, 'DisplayName', 'z')
- [ ] If we use screw coordinate, can we add/subtract them? hold off;
- [ ] Do some simple tests to verify that the algorithm is working fine xlim([2, inf]);
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_without_dist_trans.pdf" :var figsize="wide-normal" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:exp_tomo_without_dist_trans
#+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_trans.png][png]], [[./figs/exp_tomo_without_dist_trans.pdf][pdf]])
[[file:figs/exp_tomo_without_dist_trans.png]]
Rotations.
Think of the good way to plot these rotations with respect to time.
* With Perturbations
#+begin_src matlab
opts = struct(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
initDisturbances(opts);
#+end_src
#+begin_src matlab
sim('sim_nano_station_tomo')
#+end_src
#+begin_src matlab
figure;
hold on;
plot(Dsm.x.Time, Dsm.x.Data, 'DisplayName', 'x')
plot(Dsm.y.Time, Dsm.y.Data, 'DisplayName', 'y')
plot(Dsm.z.Time, Dsm.z.Data, 'DisplayName', 'z')
hold off;
xlim([2, inf]);
legend('location', 'northeast');
#+end_src
#+HEADER: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomo_dist_trans.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+NAME: fig:exp_tomo_dist_trans
#+CAPTION: X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_trans.png][png]], [[./figs/exp_tomo_dist_trans.pdf][pdf]])
[[file:figs/exp_tomo_dist_trans.png]]
* TODO Tests on the transformation from reference to wanted position :noexport:
:PROPERTIES:
:header-args:matlab+: :eval no
:END:
** Introduction :ignore: ** Introduction :ignore:
#+begin_quote #+begin_quote
Rx = [1 0 0; Rx = [1 0 0;

BIN
experiment_tomography/matlab/sim_nano_station_tomo.slx
View File

Binary file not shown.

BIN
figs/exp_tomo_dist_trans.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 80 KiB

BIN
figs/exp_tomo_without_dist_trans.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 56 KiB

127
functions/index.org
View File

@ -43,7 +43,7 @@
* Functions * Functions
<<sec:functions>> <<sec:functions>>
** computePsdDispl ** TODO computePsdDispl
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle ../src/computePsdDispl.m :header-args:matlab+: :tangle ../src/computePsdDispl.m
:header-args:matlab+: :comments none :mkdirp yes :eval no :header-args:matlab+: :comments none :mkdirp yes :eval no
@ -78,7 +78,7 @@ This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].
end end
#+end_src #+end_src
** computeSetpoint ** TODO computeSetpoint
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle ../src/computeSetpoint.m :header-args:matlab+: :tangle ../src/computeSetpoint.m
:header-args:matlab+: :comments none :mkdirp yes :eval no :header-args:matlab+: :comments none :mkdirp yes :eval no
@ -149,7 +149,7 @@ This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].
end end
#+end_src #+end_src
** converErrorBasis ** TODO converErrorBasis
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle ../src/converErrorBasis.m :header-args:matlab+: :tangle ../src/converErrorBasis.m
:header-args:matlab+: :comments none :mkdirp yes :eval no :header-args:matlab+: :comments none :mkdirp yes :eval no
@ -286,127 +286,6 @@ This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
end end
#+end_src #+end_src
** generateDiagPidControl
:PROPERTIES:
:header-args:matlab+: :tangle ../src/generateDiagPidControl.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:generateDiagPidControl>>
This Matlab function is accessible [[file:../src/generateDiagPidControl.m][here]].
#+begin_src matlab
function [K] = generateDiagPidControl(G, fs)
%%
pid_opts = pidtuneOptions(...
'PhaseMargin', 50, ...
'DesignFocus', 'disturbance-rejection');
%%
K = tf(zeros(6));
for i = 1:6
input_name = G.InputName(i);
output_name = G.OutputName(i);
K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
end
K.InputName = G.OutputName;
K.OutputName = G.InputName;
end
#+end_src
** identifyPlant
:PROPERTIES:
:header-args:matlab+: :tangle ../src/identifyPlant.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:identifyPlant>>
This Matlab function is accessible [[file:../src/identifyPlant.m][here]].
#+begin_src matlab
function [sys] = identifyPlant(opts_param)
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_nano_station_id';
%% Input/Output definition
io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces
io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
'Dgx', 'Dgy', 'Dgz', ...
'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
'Frzz', 'Ftyx', 'Ftyz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
%% Create the sub transfer functions
minreal_tol = sqrt(eps);
% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
% From ground motion to Sample displacement
sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
% From direct forces applied on the sample to displacement of the sample
sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
% From forces applied on NASS's legs to force sensor in each leg
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
% From forces applied on NASS's legs to displacement of each leg
sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
% From forces/torques applied by the NASS to position error
sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
% From forces/torques applied by the NASS to velocity of the actuator
sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
% From various disturbance forces to position error
sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
%% We remove low frequency and high frequency dynamics that are usually unstable
% using =freqsep= is risky as it may change the shape of the transfer functions
% f_min = 0.1; % [Hz]
% f_max = 1e4; % [Hz]
% [~, sys.G_cart] = freqsep(freqsep(sys.G_cart, 2*pi*f_max), 2*pi*f_min);
% [~, sys.G_gm] = freqsep(freqsep(sys.G_gm, 2*pi*f_max), 2*pi*f_min);
% [~, sys.G_fs] = freqsep(freqsep(sys.G_fs, 2*pi*f_max), 2*pi*f_min);
% [~, sys.G_iff] = freqsep(freqsep(sys.G_iff, 2*pi*f_max), 2*pi*f_min);
% [~, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, 2*pi*f_max), 2*pi*f_min);
% [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min);
%% We finally verify that the system is stable
if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant)
warning('One of the identified system is unstable');
end
end
#+end_src
** Inverse Kinematics of the Hexapod ** Inverse Kinematics of the Hexapod
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m :header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m

74
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-12 jeu. 11:35 --> <!-- 2019-12-13 ven. 17:55 -->
<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>Simscape Model of the Nano-Active-Stabilization-System</title> <title>Simscape Model of the Nano-Active-Stabilization-System</title>
@ -258,16 +258,17 @@ 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="#orgb2c859c">1. Simulink Project (link)</a></li> <li><a href="#org09e6df5">1. Simulink Project (link)</a></li>
<li><a href="#org9727aa1">2. Simscape Model (link)</a></li> <li><a href="#org26385ed">2. Simscape Model (link)</a></li>
<li><a href="#org203d0d1">3. Simscape Subsystems (link)</a></li> <li><a href="#org6bd604c">3. Simscape Subsystems (link)</a></li>
<li><a href="#orgcd6fbe6">4. Kinematics of the Station (link)</a></li> <li><a href="#org90bceda">4. Kinematics of the Station (link)</a></li>
<li><a href="#orgcd6e706">5. Metrology (link)</a></li> <li><a href="#org476f6e1">5. Metrology (link)</a></li>
<li><a href="#orgbadb5c3">6. Computation of the positioning error of the Sample (link)</a></li> <li><a href="#org0d1436b">6. Computation of the positioning error of the Sample (link)</a></li>
<li><a href="#orgc5a914e">7. Tuning of the Dynamics of the Simscape model (link)</a></li> <li><a href="#orgd15f91c">7. Tuning of the Dynamics of the Simscape model (link)</a></li>
<li><a href="#org3905335">8. Disturbances (link)</a></li> <li><a href="#orgd1fe7f0">8. Disturbances (link)</a></li>
<li><a href="#org0bc662a">9. Tomography Experiment (link)</a></li> <li><a href="#orgee1f6e3">9. Tomography Experiment (link)</a></li>
<li><a href="#org3e93696">10. Useful Matlab Functions (link)</a></li> <li><a href="#orgbbb9da8">10. Active Damping Techniques (link)</a></li>
<li><a href="#org7bec1df">11. Useful Matlab Functions (link)</a></li>
</ul> </ul>
</div> </div>
</div> </div>
@ -276,8 +277,8 @@ for the JavaScript code in this tag.
Here are links to the documents related to the Simscape model of the Nano-Active-Stabilization-System. Here are links to the documents related to the Simscape model of the Nano-Active-Stabilization-System.
</p> </p>
<div id="outline-container-orgb2c859c" class="outline-2"> <div id="outline-container-org09e6df5" class="outline-2">
<h2 id="orgb2c859c"><span class="section-number-2">1</span> Simulink Project (<a href="./simulink_project/index.html">link</a>)</h2> <h2 id="org09e6df5"><span class="section-number-2">1</span> Simulink Project (<a href="./simulink_project/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p> <p>
The project is managed with a Simulink Project. The project is managed with a Simulink Project.
@ -286,8 +287,8 @@ Such project is briefly presented <a href="./simulink_project/index.html">here</
</div> </div>
</div> </div>
<div id="outline-container-org9727aa1" class="outline-2"> <div id="outline-container-org26385ed" class="outline-2">
<h2 id="org9727aa1"><span class="section-number-2">2</span> Simscape Model (<a href="./simscape/index.html">link</a>)</h2> <h2 id="org26385ed"><span class="section-number-2">2</span> Simscape Model (<a href="./simscape/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
<p> <p>
The model of the NASS is based on Simulink and Simscape Multi-Body. The model of the NASS is based on Simulink and Simscape Multi-Body.
@ -296,8 +297,8 @@ Such toolbox is presented <a href="./simscape/index.html">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org203d0d1" class="outline-2"> <div id="outline-container-org6bd604c" class="outline-2">
<h2 id="org203d0d1"><span class="section-number-2">3</span> Simscape Subsystems (<a href="./simscape_subsystems/index.html">link</a>)</h2> <h2 id="org6bd604c"><span class="section-number-2">3</span> Simscape Subsystems (<a href="./simscape_subsystems/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-3"> <div class="outline-text-2" id="text-3">
<p> <p>
The model is decomposed of multiple subsystems. The model is decomposed of multiple subsystems.
@ -311,8 +312,8 @@ All these subsystems are described <a href="./simscape_subsystems/index.html">he
</div> </div>
</div> </div>
<div id="outline-container-orgcd6fbe6" class="outline-2"> <div id="outline-container-org90bceda" class="outline-2">
<h2 id="orgcd6fbe6"><span class="section-number-2">4</span> Kinematics of the Station (<a href="./kinematics/index.html">link</a>)</h2> <h2 id="org90bceda"><span class="section-number-2">4</span> Kinematics of the Station (<a href="./kinematics/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-4"> <div class="outline-text-2" id="text-4">
<p> <p>
First, we consider perfectly rigid elements and joints and we just study the kinematic of the station. First, we consider perfectly rigid elements and joints and we just study the kinematic of the station.
@ -322,8 +323,8 @@ This is detailed <a href="./kinematics/index.html">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orgcd6e706" class="outline-2"> <div id="outline-container-org476f6e1" class="outline-2">
<h2 id="orgcd6e706"><span class="section-number-2">5</span> Metrology (<a href="./metrology/index.html">link</a>)</h2> <h2 id="org476f6e1"><span class="section-number-2">5</span> Metrology (<a href="./metrology/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-5"> <div class="outline-text-2" id="text-5">
<p> <p>
In this document (accessible <a href="./metrology/index.html">here</a>), we discuss the measurement of the sample with respect to the granite. In this document (accessible <a href="./metrology/index.html">here</a>), we discuss the measurement of the sample with respect to the granite.
@ -331,8 +332,8 @@ In this document (accessible <a href="./metrology/index.html">here</a>), we disc
</div> </div>
</div> </div>
<div id="outline-container-orgbadb5c3" class="outline-2"> <div id="outline-container-org0d1436b" class="outline-2">
<h2 id="orgbadb5c3"><span class="section-number-2">6</span> Computation of the positioning error of the Sample (<a href="./positioning_error/index.html">link</a>)</h2> <h2 id="org0d1436b"><span class="section-number-2">6</span> Computation of the positioning error of the Sample (<a href="./positioning_error/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-6"> <div class="outline-text-2" id="text-6">
<p> <p>
From the measurement of the position of the sample with respect to the granite and from the wanted position of each stage, we can compute the positioning error of the sample with respect to the nano-hexapod. From the measurement of the position of the sample with respect to the granite and from the wanted position of each stage, we can compute the positioning error of the sample with respect to the nano-hexapod.
@ -341,8 +342,8 @@ This is done <a href="./positioning_error/index.html">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orgc5a914e" class="outline-2"> <div id="outline-container-orgd15f91c" class="outline-2">
<h2 id="orgc5a914e"><span class="section-number-2">7</span> Tuning of the Dynamics of the Simscape model (<a href="./identification/index.html">link</a>)</h2> <h2 id="orgd15f91c"><span class="section-number-2">7</span> Tuning of the Dynamics of the Simscape model (<a href="./identification/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-7"> <div class="outline-text-2" id="text-7">
<p> <p>
From dynamical measurements perform on the real positioning station, we tune the parameters of the simscape model to have similar dynamics. From dynamical measurements perform on the real positioning station, we tune the parameters of the simscape model to have similar dynamics.
@ -354,8 +355,8 @@ This is explained <a href="./identification/index.html">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org3905335" class="outline-2"> <div id="outline-container-orgd1fe7f0" class="outline-2">
<h2 id="org3905335"><span class="section-number-2">8</span> Disturbances (<a href="./disturbances/index.html">link</a>)</h2> <h2 id="orgd1fe7f0"><span class="section-number-2">8</span> Disturbances (<a href="./disturbances/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-8"> <div class="outline-text-2" id="text-8">
<p> <p>
The effect of disturbances on the position of the micro-station have been measured. The effect of disturbances on the position of the micro-station have been measured.
@ -372,8 +373,8 @@ We also discuss how the disturbances are implemented in the model.
</div> </div>
</div> </div>
<div id="outline-container-org0bc662a" class="outline-2"> <div id="outline-container-orgee1f6e3" class="outline-2">
<h2 id="org0bc662a"><span class="section-number-2">9</span> Tomography Experiment (<a href="./experiment_tomography/index.html">link</a>)</h2> <h2 id="orgee1f6e3"><span class="section-number-2">9</span> Tomography Experiment (<a href="./experiment_tomography/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-9"> <div class="outline-text-2" id="text-9">
<p> <p>
Now that the dynamics of the Model have been tuned and the Disturbances have included, we can simulate experiments. Now that the dynamics of the Model have been tuned and the Disturbances have included, we can simulate experiments.
@ -385,10 +386,19 @@ Tomography experiments are simulated and the results are presented <a href="./ex
</div> </div>
</div> </div>
<div id="outline-container-org3e93696" class="outline-2"> <div id="outline-container-orgbbb9da8" class="outline-2">
<h2 id="org3e93696"><span class="section-number-2">10</span> Useful Matlab Functions (<a href="./functions/index.html">link</a>)</h2> <h2 id="orgbbb9da8"><span class="section-number-2">10</span> Active Damping Techniques (<a href="./active_damping/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-10"> <div class="outline-text-2" id="text-10">
<p> <p>
Active damping techniques are applied to the Simscape model.
</p>
</div>
</div>
<div id="outline-container-org7bec1df" class="outline-2">
<h2 id="org7bec1df"><span class="section-number-2">11</span> Useful Matlab Functions (<a href="./functions/index.html">link</a>)</h2>
<div class="outline-text-2" id="text-11">
<p>
Many matlab functions are shared among all the files of the projects. Many matlab functions are shared among all the files of the projects.
</p> </p>
@ -400,7 +410,7 @@ These functions are all defined <a href="./functions/index.html">here</a>.
</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-12 jeu. 11:35</p> <p class="date">Created: 2019-12-13 ven. 17:55</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>

3
index.org
View File

@ -88,6 +88,9 @@ Now that the dynamics of the Model have been tuned and the Disturbances have inc
Tomography experiments are simulated and the results are presented [[./experiment_tomography/index.org][here]]. Tomography experiments are simulated and the results are presented [[./experiment_tomography/index.org][here]].
* Active Damping Techniques ([[./active_damping/index.org][link]])
Active damping techniques are applied to the Simscape model.
* Useful Matlab Functions ([[./functions/index.org][link]]) * Useful Matlab Functions ([[./functions/index.org][link]])
Many matlab functions are shared among all the files of the projects. Many matlab functions are shared among all the files of the projects.

BIN
mat/nass_disturbances.mat
View File

Binary file not shown.

BIN
mat/nass_references.mat
View File

Binary file not shown.

BIN
mat/stages.mat
View File

Binary file not shown.

57
simscape_subsystems/index.org
View File

@ -122,7 +122,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Ry = struct('time', t, 'signals', struct('values', Ry)); Ry = struct('time', t, 'signals', struct('values', Ry));
%% Spindle - Rz %% Spindle - Rz
t = 0:Ts:opts.Rz_period-Ts; t = 0:Ts:100*opts.Rz_period-Ts;
Rz = zeros(length(t), 1); Rz = zeros(length(t), 1);
switch opts.Rz_type switch opts.Rz_type
@ -192,7 +192,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
end end
#+end_src #+end_src
** Function that initialize the disturbances ** Initialize Disturbances
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle ../src/initDisturbances.m :header-args:matlab+: :tangle ../src/initDisturbances.m
:header-args:matlab+: :comments none :mkdirp yes :header-args:matlab+: :comments none :mkdirp yes
@ -202,6 +202,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
This Matlab function is accessible [[file:src/initDisturbances.m][here]]. This Matlab function is accessible [[file:src/initDisturbances.m][here]].
*** Function Declaration and Documentation
#+begin_src matlab #+begin_src matlab
function [] = initDisturbances(opts_param) function [] = initDisturbances(opts_param)
% initDisturbances - Initialize the disturbances % initDisturbances - Initialize the disturbances
@ -216,7 +217,14 @@ This Matlab function is accessible [[file:src/initDisturbances.m][here]].
*** Default values for the Options *** Default values for the Options
#+begin_src matlab #+begin_src matlab
%% Default values for opts %% Default values for opts
opts = struct(); opts = struct(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
%% Populate opts with input parameters %% Populate opts with input parameters
if exist('opts_param','var') if exist('opts_param','var')
@ -258,26 +266,44 @@ We define some parameters that will be used in the algorithm.
for i = 1:N/2 for i = 1:N/2
C(i) = sqrt(phi(i)*df); C(i) = sqrt(phi(i)*df);
end end
#+end_src
#+begin_src matlab
if opts.Dwx
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
% Dwx = struct('time', t, 'signals', struct('values', u)); else
Dwx = u; Dwx = zeros(length(t), 1);
end
#+end_src
#+begin_src matlab
if opts.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
Dwy = u; else
Dwy = zeros(length(t), 1);
end
#+end_src
#+begin_src matlab
if opts.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad] theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
Dwz = u; else
Dwz = zeros(length(t), 1);
end
#+end_src #+end_src
*** Translation Stage - X direction *** Translation Stage - X direction
#+begin_src matlab #+begin_src matlab
if opts.Fty_x
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -288,10 +314,14 @@ We define some parameters that will be used in the algorithm.
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N] u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
Fty_x = u; Fty_x = u;
else
Fty_x = zeros(length(t), 1);
end
#+end_src #+end_src
*** Translation Stage - Z direction *** Translation Stage - Z direction
#+begin_src matlab #+begin_src matlab
if opts.Fty_z
phi = dist_f.psd_ty; phi = dist_f.psd_ty;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -302,10 +332,14 @@ We define some parameters that will be used in the algorithm.
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N] u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
Fty_z = u; Fty_z = u;
else
Fty_z = zeros(length(t), 1);
end
#+end_src #+end_src
*** Spindle - Z direction *** Spindle - Z direction
#+begin_src matlab #+begin_src matlab
if opts.Frz_z
phi = dist_f.psd_rz; phi = dist_f.psd_rz;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -316,6 +350,9 @@ We define some parameters that will be used in the algorithm.
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N] u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
Frz_z = u; Frz_z = u;
else
Frz_z = zeros(length(t), 1);
end
#+end_src #+end_src
*** Direct Forces *** Direct Forces

BIN
simscape_subsystems/nass_disturbances.slx
View File

Binary file not shown.

BIN
simscape_subsystems/tilt_stage_D.slx
View File

Binary file not shown.

64
src/initDisturbances.m
View File

@ -7,7 +7,14 @@ function [] = initDisturbances(opts_param)
% - opts_param - % - opts_param -
%% Default values for opts %% Default values for opts
opts = struct(); opts = struct(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
%% Populate opts with input parameters %% Populate opts with input parameters
if exist('opts_param','var') if exist('opts_param','var')
@ -36,23 +43,35 @@ C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
C(i) = sqrt(phi(i)*df); C(i) = sqrt(phi(i)*df);
end end
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
% Dwx = struct('time', t, 'signals', struct('values', u));
Dwx = u;
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
Dwy = u;
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
Dwz = u;
if opts.Dwx
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
else
Dwx = zeros(length(t), 1);
end
if opts.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
else
Dwy = zeros(length(t), 1);
end
if opts.Dwy
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];;
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
else
Dwz = zeros(length(t), 1);
end
if opts.Fty_x
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -63,7 +82,11 @@ Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N] u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
Fty_x = u; Fty_x = u;
else
Fty_x = zeros(length(t), 1);
end
if opts.Fty_z
phi = dist_f.psd_ty; phi = dist_f.psd_ty;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -74,7 +97,11 @@ Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N] u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
Fty_z = u; Fty_z = u;
else
Fty_z = zeros(length(t), 1);
end
if opts.Frz_z
phi = dist_f.psd_rz; phi = dist_f.psd_rz;
C = zeros(N/2,1); C = zeros(N/2,1);
for i = 1:N/2 for i = 1:N/2
@ -85,6 +112,9 @@ Cx = [0 ; C.*complex(cos(theta),sin(theta))];
Cx = [Cx; flipud(conj(Cx(2:end)))];; Cx = [Cx; flipud(conj(Cx(2:end)))];;
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N] u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
Frz_z = u; Frz_z = u;
else
Frz_z = zeros(length(t), 1);
end
u = zeros(length(t), 6); u = zeros(length(t), 6);
Fd = u; Fd = u;

2
src/initializeReferences.m
View File

@ -66,7 +66,7 @@ function [ref] = initializeReferences(opts_param)
Ry = struct('time', t, 'signals', struct('values', Ry)); Ry = struct('time', t, 'signals', struct('values', Ry));
%% Spindle - Rz %% Spindle - Rz
t = 0:Ts:opts.Rz_period-Ts; t = 0:Ts:100*opts.Rz_period-Ts;
Rz = zeros(length(t), 1); Rz = zeros(length(t), 1);
switch opts.Rz_type switch opts.Rz_type