Tomography Experiment with and without disturbances
This commit is contained in:
parent
27055d8f24
commit
b4180feded
@ -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-04 mer. 21:55 -->
|
||||
<!-- 2019-12-13 ven. 19:06 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Tomography Experiment</title>
|
||||
@ -193,6 +193,13 @@
|
||||
.org-svg { width: 90%; }
|
||||
/*]]>*/-->
|
||||
</style>
|
||||
<link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
<link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
<link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
<script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
<script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
<script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
<script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
<script type="text/javascript">
|
||||
/*
|
||||
@licstart The following is the entire license notice for the
|
||||
@ -239,28 +246,6 @@ for the JavaScript code in this tag.
|
||||
}
|
||||
/*]]>*///-->
|
||||
</script>
|
||||
<script type="text/x-mathjax-config">
|
||||
MathJax.Hub.Config({
|
||||
displayAlign: "center",
|
||||
displayIndent: "0em",
|
||||
|
||||
"HTML-CSS": { scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
webFont: "TeX"
|
||||
},
|
||||
SVG: {scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
font: "TeX"},
|
||||
NativeMML: {scale: 100},
|
||||
TeX: { equationNumbers: {autoNumber: "AMS"},
|
||||
MultLineWidth: "85%",
|
||||
TagSide: "right",
|
||||
TagIndent: ".8em"
|
||||
}
|
||||
});
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="org-div-home-and-up">
|
||||
@ -273,378 +258,162 @@ for the JavaScript code in this tag.
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#orgab97c4b">1. Initialize Experiment</a></li>
|
||||
<li><a href="#org26c554f">2. Run the Tomography Experiment</a></li>
|
||||
<li><a href="#orgc507e1e">3. <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</a>
|
||||
<ul>
|
||||
<li><a href="#org4537a86">3.1. Wanted Position of the Sample with respect to the Granite</a></li>
|
||||
<li><a href="#org6c67ae5">3.2. Measured Position of the Sample with respect to the Granite</a></li>
|
||||
<li><a href="#org69e38aa">3.3. Positioning Error with respect to the Granite</a></li>
|
||||
<li><a href="#org5246d94">3.4. Position Error Expressed in the Nano-Hexapod Frame</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org84a12d1">1. Simscape Model</a></li>
|
||||
<li><a href="#orgcb251b3">2. Tomography Experiment with no disturbances</a></li>
|
||||
<li><a href="#orgcc1bc22">3. With Perturbations</a></li>
|
||||
</ul>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgab97c4b" class="outline-2">
|
||||
<h2 id="orgab97c4b"><span class="section-number-2">1</span> Initialize Experiment</h2>
|
||||
<div id="outline-container-org84a12d1" class="outline-2">
|
||||
<h2 id="org84a12d1"><span class="section-number-2">1</span> Simscape Model</h2>
|
||||
<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>
|
||||
We first initialize all the stages.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<code>initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code>initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<pre 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">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
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>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initializeReferences<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
And we initialize the disturbances.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>initDisturbances<span class="org-rainbow-delimiters-depth-1">()</span>;</code>
|
||||
<pre 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>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org26c554f" class="outline-2">
|
||||
<h2 id="org26c554f"><span class="section-number-2">2</span> Run the Tomography Experiment</h2>
|
||||
<div id="outline-container-orgcb251b3" class="outline-2">
|
||||
<h2 id="orgcb251b3"><span class="section-number-2">2</span> Tomography Experiment with no disturbances</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
We first load the simulation configuration
|
||||
And we initialize the disturbances to zero.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<pre 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>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, '<span class="org-highlight-numbers-number">1</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<pre 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>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'sim_nano_station_tomo'</span>, <span class="org-string">'SimulationCommand'</span>, <span class="org-string">'start'</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<pre class="src src-matlab">Dsm_without_dist = Dsm;
|
||||
</pre>
|
||||
</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 id="outline-container-orgc507e1e" class="outline-2">
|
||||
<h2 id="orgc507e1e"><span class="section-number-2">3</span> <span class="todo TODO">TODO</span> Tests on the transformation from reference to wanted position</h2>
|
||||
<div id="outline-container-orgcc1bc22" class="outline-2">
|
||||
<h2 id="orgcc1bc22"><span class="section-number-2">3</span> With Perturbations</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<ul class="org-ul">
|
||||
<li class="on"><code>[X]</code> Are the rotation matrix commutable? => no</li>
|
||||
<li class="on"><code>[X]</code> How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book)</li>
|
||||
<li class="off"><code>[ ]</code> Should ask Veijo how he specifies the position of the Symetrie Hexapod</li>
|
||||
<li class="off"><code>[ ]</code> Create functions for all distinct part and then include that in Simulink</li>
|
||||
<li class="off"><code>[ ]</code> How the express the orientation error?</li>
|
||||
<li class="off"><code>[ ]</code> If we use screw coordinate, can we add/subtract them?</li>
|
||||
<li class="off"><code>[ ]</code> Do some simple tests to verify that the algorithm is working fine</li>
|
||||
</ul>
|
||||
|
||||
<blockquote>
|
||||
<p>
|
||||
Rx = [1 0 0;
|
||||
0 cos(t) -sin(t);
|
||||
0 sin(t) cos(t)];
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Ry = [ cos(t) 0 sin(t);
|
||||
0 1 0;
|
||||
-sin(t) 0 cos(t)];
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Rz = [cos(t) -sin(t) 0;
|
||||
sin(t) cos(t) 0;
|
||||
0 0 1];
|
||||
</p>
|
||||
</blockquote>
|
||||
|
||||
<p>
|
||||
Let's define the following frames:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>\(\{W\}\) the frame that is <b>fixed to the granite</b> and its origin at the theoretical meeting point between the X-ray and the spindle axis.</li>
|
||||
<li>\(\{S\}\) the frame <b>attached to the sample</b> (in reality attached to the top platform of the nano-hexapod) with its origin at 175mm above the top platform of the nano-hexapod.
|
||||
Its origin is \(O_S\).</li>
|
||||
<li>\(\{T\}\) the theoretical wanted frame that correspond to the wanted pose of the frame \(\{S\}\).
|
||||
\(\{T\}\) is computed from the wanted position of each stage. It is thus theoretical and does not correspond to a real position.
|
||||
The origin of \(T\) is \(O_T\) and is the wanted position of the sample.</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
Thus:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>the <b>measurement</b> of the position of the sample corresponds to \({}^W O_S = \begin{bmatrix} {}^WP_{x,m} & {}^WP_{y,m} & {}^WP_{z,m} \end{bmatrix}^T\) in translation and to \(\theta_m {}^W\bm{s}_m = \theta_m \cdot \begin{bmatrix} {}^Ws_{x,m} & {}^Ws_{y,m} & {}^Ws_{z,m} \end{bmatrix}^T\) in rotations</li>
|
||||
<li>the <b>wanted position</b> of the sample expressed w.r.t. the granite is \({}^W O_T = \begin{bmatrix} {}^WP_{x,r} & {}^WP_{y,r} & {}^WP_{z,r} \end{bmatrix}^T\) in translation and to \(\theta_r {}^W\bm{s}_r = \theta_r \cdot \begin{bmatrix} {}^Ws_{x,r} & {}^Ws_{y,r} & {}^Ws_{z,r} \end{bmatrix}^T\) in rotations</li>
|
||||
</ul>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4537a86" class="outline-3">
|
||||
<h3 id="org4537a86"><span class="section-number-3">3.1</span> Wanted Position of the Sample with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-1">
|
||||
<p>
|
||||
Let's define the wanted position of each stage.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Ty = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Ry = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rz = <span class="org-highlight-numbers-number">180</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Hexapod (first consider only translations)</span></code>
|
||||
<code>Thx = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Thy = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Thz = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Now, we compute the corresponding wanted translation and rotation of the sample with respect to the granite frame \(\{W\}\).
|
||||
This corresponds to \({}^WO_T\) and \(\theta_m {}^Ws_m\).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
To do so, we have to define the homogeneous transformation for each stage.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-comment">% Translation Stage</span></code>
|
||||
<code>Rty = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Ty;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Tilt Stage - Pure rotating aligned with Ob</span></code>
|
||||
<code>Rry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Ry<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Spindle - Rotation along the Z axis</span></code>
|
||||
<code>Rrz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code><span class="org-comment">% Micro-Hexapod (only rotations first)</span></code>
|
||||
<code>Rh = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> Thx ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Thy ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> Thz ;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We combine the individual homogeneous transformations into one homogeneous transformation for all the station.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Ttot = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Using this homogeneous transformation, we can compute the wanted position and orientation of the sample with respect to the granite.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WOr = Ttot<span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">0</span>;<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>WOr = WOr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<span class="org-string">'Dwx'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Ground Motion - X direction</span>
|
||||
<span class="org-string">'Dwy'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Ground Motion - Y direction</span>
|
||||
<span class="org-string">'Dwz'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Ground Motion - Z direction</span>
|
||||
<span class="org-string">'Fty_x'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Translation Stage - X direction</span>
|
||||
<span class="org-string">'Fty_z'</span>, <span class="org-constant">true</span>, ...<span class="org-comment"> % Translation Stage - Z direction</span>
|
||||
<span class="org-string">'Frz_z'</span>, <span class="org-constant">true</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>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>thetar = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>Ttot<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span></code>
|
||||
<code><span class="org-keyword">if</span> thetar <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
|
||||
<code> WSr = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code><span class="org-keyword">else</span></code>
|
||||
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>Ttot<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code> WSr = thetar<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type"><</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code><span class="org-keyword">end</span></code>
|
||||
<pre 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>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPr = <span class="org-rainbow-delimiters-depth-1">[</span>WOr ; WSr<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6c67ae5" class="outline-3">
|
||||
<h3 id="org6c67ae5"><span class="section-number-3">3.2</span> Measured Position of the Sample with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-2">
|
||||
<p>
|
||||
The measurement of the position of the sample using the metrology system gives the position and orientation of the sample with respect to the granite.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code><span class="org-comment">% Measurements: Xm, Ym, Zm, Rx, Ry, Rz</span></code>
|
||||
<code>Dxm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Dym = <span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code>Dzm = <span class="org-highlight-numbers-number">0</span>; <span class="org-comment">% [m]</span></code>
|
||||
<code></code>
|
||||
<code>Rxm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rym = <span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code>Rzm = <span class="org-highlight-numbers-number">0</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
||||
hold on;
|
||||
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>
|
||||
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>
|
||||
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;
|
||||
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>
|
||||
|
||||
<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>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We then obtain the orientation measurement in the form of screw coordinate \(\theta_m ({}^Ws_{x,m},\ {}^Ws_{y,m},\ {}^Ws_{z,m})^T\) where:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>\(\theta_m = \cos^{-1} \frac{\text{Tr}(R) - 1}{2}\)</li>
|
||||
<li>\({}^W\bm{s}_m\) is the eigen vector of the rotation matrix \(R\) corresponding to the eigen value \(\lambda = 1\)</li>
|
||||
</ul>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>thetam = acos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">(</span>trace<span class="org-rainbow-delimiters-depth-3">(</span>STw<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-4">)</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% [rad]</span></code>
|
||||
<code><span class="org-keyword">if</span> thetam <span class="org-type">==</span> <span class="org-highlight-numbers-number">0</span></code>
|
||||
<code> WSm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code><span class="org-keyword">else</span></code>
|
||||
<code> <span class="org-rainbow-delimiters-depth-1">[</span>V, D<span class="org-rainbow-delimiters-depth-1">]</span> = eig<span class="org-rainbow-delimiters-depth-1">(</span>STw<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code> WSm = thetam<span class="org-type">*</span>V<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, abs<span class="org-rainbow-delimiters-depth-2">(</span>diag<span class="org-rainbow-delimiters-depth-3">(</span>D<span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type"><</span> <span class="org-constant">eps</span><span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
<code><span class="org-keyword">end</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPm = <span class="org-rainbow-delimiters-depth-1">[</span>Dxm ; Dym ; Dzm ; WSm<span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org69e38aa" class="outline-3">
|
||||
<h3 id="org69e38aa"><span class="section-number-3">3.3</span> Positioning Error with respect to the Granite</h3>
|
||||
<div class="outline-text-3" id="text-3-3">
|
||||
<p>
|
||||
The wanted position expressed with respect to the granite is \({}^WO_T\) and the measured position with respect to the granite is \({}^WO_S\), thus the <b>position error</b> expressed in \(\{W\}\) is
|
||||
\[ {}^W E = {}^W O_T - {}^W O_S \]
|
||||
The same is true for rotations:
|
||||
\[ \theta_\epsilon {}^W\bm{s}_\epsilon = \theta_r {}^W\bm{s}_r - \theta_m {}^W\bm{s}_m \]
|
||||
</p>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>WPe = WPr <span class="org-type">-</span> WPm;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<blockquote>
|
||||
<p>
|
||||
Now we want to express this error in a frame attached to the <b>base of the nano-hexapod</b> with its origin at the same point where the Jacobian of the nano-hexapod is computed (175mm above the top platform + 90mm of total height of the nano-hexapod).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Or maybe should we want to express this error with respect to the <b>top platform of the nano-hexapod</b>?
|
||||
We are measuring the position of the top-platform, and we don't know exactly the position of the bottom platform.
|
||||
We could compute the position of the bottom platform in two ways:
|
||||
</p>
|
||||
<ul class="org-ul">
|
||||
<li>from the encoders of each stage</li>
|
||||
<li>from the measurement of the nano-hexapod top platform + the internal metrology in the nano-hexapod (capacitive sensors e.g)</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
A third option is to say that the maximum stroke of the nano-hexapod is so small that the error should no change to much by the change of base.
|
||||
</p>
|
||||
</blockquote>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org5246d94" class="outline-3">
|
||||
<h3 id="org5246d94"><span class="section-number-3">3.4</span> Position Error Expressed in the Nano-Hexapod Frame</h3>
|
||||
<div class="outline-text-3" id="text-3-4">
|
||||
<p>
|
||||
We now want the position error to be expressed in \(\{S\}\) (the frame attach to the sample):
|
||||
\[ {}^S E = {}^S T_W \cdot {}^W E \]
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Thus we need to compute the homogeneous transformation \({}^ST_W\).
|
||||
Fortunately, this homogeneous transformation can be computed from the measurement of the sample position and orientation with respect to the granite.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Trxm = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rxm<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trym = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rym<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>Trzm = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> sin<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Rzm<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;</code>
|
||||
<code> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code></code>
|
||||
<code>STw = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-rainbow-delimiters-depth-2">[</span> Trym<span class="org-type">*</span>Trxm<span class="org-type">*</span>Trzm , <span class="org-rainbow-delimiters-depth-3">[</span>Dxm; Dym; Dzm<span class="org-rainbow-delimiters-depth-3">]</span><span class="org-rainbow-delimiters-depth-2">]</span>; <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Translation Error.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>SEm = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>SEm = SEm<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Rotation Error.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>SEr = STw <span class="org-type">*</span> <span class="org-rainbow-delimiters-depth-1">[</span>WPe<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-2">)</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">]</span>;</code>
|
||||
<code>SEr = SEr<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>;</code>
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><code>Etot = <span class="org-rainbow-delimiters-depth-1">[</span>SEm ; SEr<span class="org-rainbow-delimiters-depth-1">]</span></code>
|
||||
</pre>
|
||||
</div>
|
||||
<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>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2019-12-04 mer. 21:55</p>
|
||||
<p class="date">Created: 2019-12-13 ven. 19:06</p>
|
||||
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
||||
</div>
|
||||
</body>
|
||||
|
||||
@ -9,13 +9,13 @@
|
||||
#+HTML_LINK_HOME: ../index.html
|
||||
#+HTML_LINK_UP: ../index.html
|
||||
|
||||
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
# #+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
# #+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
* Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
@ -54,11 +55,18 @@
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
* Simscape Model
|
||||
The simulink file to do tomography experiments is =sim_nano_station_tomo.slx=.
|
||||
#+begin_src matlab
|
||||
open('experiment_tomography/matlab/sim_nano_station_tomo.slx')
|
||||
#+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.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
@ -74,38 +82,98 @@ We first initialize all the stages.
|
||||
#+end_src
|
||||
|
||||
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
|
||||
initializeReferences(struct('Rz_type', 'rotating', 'Rz_period', 1));
|
||||
#+end_src
|
||||
|
||||
And we initialize the disturbances.
|
||||
* Tomography Experiment with no disturbances
|
||||
And we initialize the disturbances to zero.
|
||||
#+begin_src matlab
|
||||
initDisturbances();
|
||||
#+end_src
|
||||
|
||||
* Run the Tomography Experiment
|
||||
We first load the simulation configuration
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
opts = struct(...
|
||||
'Dwx', false, ... % Ground Motion - X direction
|
||||
'Dwy', false, ... % Ground Motion - Y direction
|
||||
'Dwz', false, ... % Ground Motion - Z direction
|
||||
'Fty_x', false, ... % Translation Stage - X direction
|
||||
'Fty_z', false, ... % Translation Stage - Z direction
|
||||
'Frz_z', false ... % Spindle - Z direction
|
||||
);
|
||||
initDisturbances(opts);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
set_param(conf_simscape, 'StopTime', '1');
|
||||
sim('sim_nano_station_tomo')
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
set_param('sim_nano_station_tomo', 'SimulationCommand', 'start');
|
||||
Dsm_without_dist = Dsm;
|
||||
#+end_src
|
||||
|
||||
* TODO Tests on the transformation from reference to wanted position
|
||||
- [X] Are the rotation matrix commutable? => no
|
||||
- [X] How to express the measured rotation errors? => screw axis coordinate seems nice (used in Taghirad's book)
|
||||
- [ ] Should ask Veijo how he specifies the position of the Symetrie Hexapod
|
||||
- [ ] Create functions for all distinct part and then include that in Simulink
|
||||
- [ ] How the express the orientation error?
|
||||
- [ ] If we use screw coordinate, can we add/subtract them?
|
||||
- [ ] Do some simple tests to verify that the algorithm is working fine
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
hold on;
|
||||
plot(Dsm_without_dist.x.Time, Dsm_without_dist.x.Data, 'DisplayName', 'x')
|
||||
plot(Dsm_without_dist.y.Time, Dsm_without_dist.y.Data, 'DisplayName', 'y')
|
||||
plot(Dsm_without_dist.z.Time, Dsm_without_dist.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_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:
|
||||
#+begin_quote
|
||||
Rx = [1 0 0;
|
||||
|
||||
Binary file not shown.
BIN
figs/exp_tomo_dist_trans.png
Normal file
BIN
figs/exp_tomo_dist_trans.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 80 KiB |
BIN
figs/exp_tomo_without_dist_trans.png
Normal file
BIN
figs/exp_tomo_without_dist_trans.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 56 KiB |
@ -43,7 +43,7 @@
|
||||
|
||||
* Functions
|
||||
<<sec:functions>>
|
||||
** computePsdDispl
|
||||
** TODO computePsdDispl
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computePsdDispl.m
|
||||
: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_src
|
||||
|
||||
** computeSetpoint
|
||||
** TODO computeSetpoint
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeSetpoint.m
|
||||
: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_src
|
||||
|
||||
** converErrorBasis
|
||||
** TODO converErrorBasis
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/converErrorBasis.m
|
||||
: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_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
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
|
||||
|
||||
74
index.html
74
index.html
@ -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-12 jeu. 11:35 -->
|
||||
<!-- 2019-12-13 ven. 17:55 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<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>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#orgb2c859c">1. Simulink Project (link)</a></li>
|
||||
<li><a href="#org9727aa1">2. Simscape Model (link)</a></li>
|
||||
<li><a href="#org203d0d1">3. Simscape Subsystems (link)</a></li>
|
||||
<li><a href="#orgcd6fbe6">4. Kinematics of the Station (link)</a></li>
|
||||
<li><a href="#orgcd6e706">5. Metrology (link)</a></li>
|
||||
<li><a href="#orgbadb5c3">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="#org3905335">8. Disturbances (link)</a></li>
|
||||
<li><a href="#org0bc662a">9. Tomography Experiment (link)</a></li>
|
||||
<li><a href="#org3e93696">10. Useful Matlab Functions (link)</a></li>
|
||||
<li><a href="#org09e6df5">1. Simulink Project (link)</a></li>
|
||||
<li><a href="#org26385ed">2. Simscape Model (link)</a></li>
|
||||
<li><a href="#org6bd604c">3. Simscape Subsystems (link)</a></li>
|
||||
<li><a href="#org90bceda">4. Kinematics of the Station (link)</a></li>
|
||||
<li><a href="#org476f6e1">5. Metrology (link)</a></li>
|
||||
<li><a href="#org0d1436b">6. Computation of the positioning error of the Sample (link)</a></li>
|
||||
<li><a href="#orgd15f91c">7. Tuning of the Dynamics of the Simscape model (link)</a></li>
|
||||
<li><a href="#orgd1fe7f0">8. Disturbances (link)</a></li>
|
||||
<li><a href="#orgee1f6e3">9. Tomography Experiment (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>
|
||||
</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.
|
||||
</p>
|
||||
|
||||
<div id="outline-container-orgb2c859c" class="outline-2">
|
||||
<h2 id="orgb2c859c"><span class="section-number-2">1</span> Simulink Project (<a href="./simulink_project/index.html">link</a>)</h2>
|
||||
<div id="outline-container-org09e6df5" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-org9727aa1" class="outline-2">
|
||||
<h2 id="org9727aa1"><span class="section-number-2">2</span> Simscape Model (<a href="./simscape/index.html">link</a>)</h2>
|
||||
<div id="outline-container-org26385ed" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-org203d0d1" class="outline-2">
|
||||
<h2 id="org203d0d1"><span class="section-number-2">3</span> Simscape Subsystems (<a href="./simscape_subsystems/index.html">link</a>)</h2>
|
||||
<div id="outline-container-org6bd604c" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-orgcd6fbe6" 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>
|
||||
<div id="outline-container-org90bceda" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-orgcd6e706" class="outline-2">
|
||||
<h2 id="orgcd6e706"><span class="section-number-2">5</span> Metrology (<a href="./metrology/index.html">link</a>)</h2>
|
||||
<div id="outline-container-org476f6e1" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-orgbadb5c3" 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>
|
||||
<div id="outline-container-org0d1436b" class="outline-2">
|
||||
<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">
|
||||
<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.
|
||||
@ -341,8 +342,8 @@ This is done <a href="./positioning_error/index.html">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc5a914e" 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>
|
||||
<div id="outline-container-orgd15f91c" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-org3905335" class="outline-2">
|
||||
<h2 id="org3905335"><span class="section-number-2">8</span> Disturbances (<a href="./disturbances/index.html">link</a>)</h2>
|
||||
<div id="outline-container-orgd1fe7f0" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-org0bc662a" class="outline-2">
|
||||
<h2 id="org0bc662a"><span class="section-number-2">9</span> Tomography Experiment (<a href="./experiment_tomography/index.html">link</a>)</h2>
|
||||
<div id="outline-container-orgee1f6e3" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-org3e93696" class="outline-2">
|
||||
<h2 id="org3e93696"><span class="section-number-2">10</span> Useful Matlab Functions (<a href="./functions/index.html">link</a>)</h2>
|
||||
<div id="outline-container-orgbbb9da8" class="outline-2">
|
||||
<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">
|
||||
<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.
|
||||
</p>
|
||||
|
||||
@ -400,7 +410,7 @@ These functions are all defined <a href="./functions/index.html">here</a>.
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<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>
|
||||
</div>
|
||||
</body>
|
||||
|
||||
@ -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]].
|
||||
|
||||
* Active Damping Techniques ([[./active_damping/index.org][link]])
|
||||
Active damping techniques are applied to the Simscape model.
|
||||
|
||||
* Useful Matlab Functions ([[./functions/index.org][link]])
|
||||
Many matlab functions are shared among all the files of the projects.
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
BIN
mat/stages.mat
BIN
mat/stages.mat
Binary file not shown.
@ -122,7 +122,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
Ry = struct('time', t, 'signals', struct('values', Ry));
|
||||
|
||||
%% Spindle - Rz
|
||||
t = 0:Ts:opts.Rz_period-Ts;
|
||||
t = 0:Ts:100*opts.Rz_period-Ts;
|
||||
Rz = zeros(length(t), 1);
|
||||
|
||||
switch opts.Rz_type
|
||||
@ -192,7 +192,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Function that initialize the disturbances
|
||||
** Initialize Disturbances
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initDisturbances.m
|
||||
: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]].
|
||||
|
||||
*** Function Declaration and Documentation
|
||||
#+begin_src matlab
|
||||
function [] = initDisturbances(opts_param)
|
||||
% initDisturbances - Initialize the disturbances
|
||||
@ -216,7 +217,14 @@ This Matlab function is accessible [[file:src/initDisturbances.m][here]].
|
||||
*** Default values for the Options
|
||||
#+begin_src matlab
|
||||
%% 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
|
||||
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
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
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)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
||||
% Dwx = struct('time', t, 'signals', struct('values', u));
|
||||
Dwx = u;
|
||||
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
||||
else
|
||||
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]
|
||||
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;
|
||||
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
||||
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]
|
||||
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;
|
||||
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
||||
else
|
||||
Dwz = zeros(length(t), 1);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Translation Stage - X direction
|
||||
#+begin_src matlab
|
||||
if opts.Fty_x
|
||||
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
@ -288,10 +314,14 @@ We define some parameters that will be used in the algorithm.
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
||||
Fty_x = u;
|
||||
else
|
||||
Fty_x = zeros(length(t), 1);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Translation Stage - Z direction
|
||||
#+begin_src matlab
|
||||
if opts.Fty_z
|
||||
phi = dist_f.psd_ty;
|
||||
C = zeros(N/2,1);
|
||||
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)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
||||
Fty_z = u;
|
||||
else
|
||||
Fty_z = zeros(length(t), 1);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Spindle - Z direction
|
||||
#+begin_src matlab
|
||||
if opts.Frz_z
|
||||
phi = dist_f.psd_rz;
|
||||
C = zeros(N/2,1);
|
||||
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)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
||||
Frz_z = u;
|
||||
else
|
||||
Frz_z = zeros(length(t), 1);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Direct Forces
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -7,7 +7,14 @@ function [] = initDisturbances(opts_param)
|
||||
% - opts_param -
|
||||
|
||||
%% 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
|
||||
if exist('opts_param','var')
|
||||
@ -36,23 +43,35 @@ C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
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
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
@ -63,7 +82,11 @@ Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
||||
Fty_x = u;
|
||||
else
|
||||
Fty_x = zeros(length(t), 1);
|
||||
end
|
||||
|
||||
if opts.Fty_z
|
||||
phi = dist_f.psd_ty;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
@ -74,7 +97,11 @@ Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
||||
Fty_z = u;
|
||||
else
|
||||
Fty_z = zeros(length(t), 1);
|
||||
end
|
||||
|
||||
if opts.Frz_z
|
||||
phi = dist_f.psd_rz;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
@ -85,6 +112,9 @@ Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
||||
Frz_z = u;
|
||||
else
|
||||
Frz_z = zeros(length(t), 1);
|
||||
end
|
||||
|
||||
u = zeros(length(t), 6);
|
||||
Fd = u;
|
||||
|
||||
@ -66,7 +66,7 @@ function [ref] = initializeReferences(opts_param)
|
||||
Ry = struct('time', t, 'signals', struct('values', Ry));
|
||||
|
||||
%% Spindle - Rz
|
||||
t = 0:Ts:opts.Rz_period-Ts;
|
||||
t = 0:Ts:100*opts.Rz_period-Ts;
|
||||
Rz = zeros(length(t), 1);
|
||||
|
||||
switch opts.Rz_type
|
||||
|
||||
Loading…
Reference in New Issue
Block a user