484 lines
26 KiB
HTML
484 lines
26 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
|
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
|
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
|
<head>
|
|
<!-- 2021-02-20 sam. 23:08 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Matlab Functions used for the NASS Project</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
<script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
</head>
|
|
<body>
|
|
<div id="org-div-home-and-up">
|
|
<a accesskey="h" href="./index.html"> UP </a>
|
|
|
|
|
<a accesskey="H" href="../../index.html"> HOME </a>
|
|
</div><div id="content">
|
|
<h1 class="title">Matlab Functions used for the NASS Project</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#org4210488">1. describeNassSetup</a>
|
|
<ul>
|
|
<li><a href="#org048d120">Function description</a></li>
|
|
<li><a href="#orgf678c44">Simscape Configuration</a></li>
|
|
<li><a href="#org09d336f">Disturbances</a></li>
|
|
<li><a href="#orgf4f5d8b">References</a></li>
|
|
<li><a href="#orga131750">Controller</a></li>
|
|
<li><a href="#org58ad578">Micro-Station</a></li>
|
|
<li><a href="#org0d8c061">Metrology</a></li>
|
|
<li><a href="#org61bb891">Nano Hexapod</a></li>
|
|
<li><a href="#org4eba4ef">Sample</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org6008c77">2. computeReferencePose</a></li>
|
|
<li><a href="#org5693f09">3. Compute the Sample Position Error w.r.t. the NASS</a></li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4210488" class="outline-2">
|
|
<h2 id="org4210488"><span class="section-number-2">1</span> describeNassSetup</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org0b45d8b"></a>
|
|
</p>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org048d120" class="outline-3">
|
|
<h3 id="org048d120">Function description</h3>
|
|
<div class="outline-text-3" id="text-org048d120">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[]</span> = <span class="org-function-name">describeNassSetup</span>()
|
|
<span class="org-comment">% describeNassSetup -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [] = describeNassSetup()</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - -</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf678c44" class="outline-3">
|
|
<h3 id="orgf678c44">Simscape Configuration</h3>
|
|
<div class="outline-text-3" id="text-orgf678c44">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/conf_simscape.mat'</span>, <span class="org-string">'conf_simscape'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Simscape Configuration:\n'</span>);
|
|
|
|
<span class="org-keyword">if</span> conf_simscape.type <span class="org-type">==</span> 1
|
|
fprintf(<span class="org-string">'- Gravity is included\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Gravity is not included\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org09d336f" class="outline-3">
|
|
<h3 id="org09d336f">Disturbances</h3>
|
|
<div class="outline-text-3" id="text-org09d336f">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/nass_disturbances.mat'</span>, <span class="org-string">'args'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Disturbances:\n'</span>);
|
|
<span class="org-keyword">if</span> <span class="org-type">~</span>args.enable
|
|
fprintf(<span class="org-string">'- No disturbance is included\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
<span class="org-keyword">if</span> args.Dwx <span class="org-type">&&</span> args.Dwy <span class="org-type">&&</span> args.Dwz
|
|
fprintf(<span class="org-string">'- Ground motion\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">if</span> args.Fty_x <span class="org-type">&&</span> args.Fty_z
|
|
fprintf(<span class="org-string">'- Vibrations of the Translation Stage\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">if</span> args.Frz_z
|
|
fprintf(<span class="org-string">'- Vibrations of the Spindle\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf4f5d8b" class="outline-3">
|
|
<h3 id="orgf4f5d8b">References</h3>
|
|
<div class="outline-text-3" id="text-orgf4f5d8b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/nass_references.mat'</span>, <span class="org-string">'args'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Reference Tracking:\n'</span>);
|
|
fprintf(<span class="org-string">'- Translation Stage:\n'</span>);
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Dy_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
fprintf(<span class="org-string">' - Constant Position\n'</span>);
|
|
fprintf(<span class="org-string">' - Dy = %.0f [mm]\n'</span>, args.Dy_amplitude<span class="org-type">*</span>1e3);
|
|
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
|
|
fprintf(<span class="org-string">' - Triangular Path\n'</span>);
|
|
fprintf(<span class="org-string">' - Amplitude = %.0f [mm]\n'</span>, args.Dy_amplitude<span class="org-type">*</span>1e3);
|
|
fprintf(<span class="org-string">' - Period = %.0f [s]\n'</span>, args.Dy_period);
|
|
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
|
|
fprintf(<span class="org-string">' - Sinusoidal Path\n'</span>);
|
|
fprintf(<span class="org-string">' - Amplitude = %.0f [mm]\n'</span>, args.Dy_amplitude<span class="org-type">*</span>1e3);
|
|
fprintf(<span class="org-string">' - Period = %.0f [s]\n'</span>, args.Dy_period);
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'- Tilt Stage:\n'</span>);
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Ry_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
fprintf(<span class="org-string">' - Constant Position\n'</span>);
|
|
fprintf(<span class="org-string">' - Ry = %.0f [mm]\n'</span>, args.Ry_amplitude<span class="org-type">*</span>1e3);
|
|
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
|
|
fprintf(<span class="org-string">' - Triangular Path\n'</span>);
|
|
fprintf(<span class="org-string">' - Amplitude = %.0f [mm]\n'</span>, args.Ry_amplitude<span class="org-type">*</span>1e3);
|
|
fprintf(<span class="org-string">' - Period = %.0f [s]\n'</span>, args.Ry_period);
|
|
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
|
|
fprintf(<span class="org-string">' - Sinusoidal Path\n'</span>);
|
|
fprintf(<span class="org-string">' - Amplitude = %.0f [mm]\n'</span>, args.Ry_amplitude<span class="org-type">*</span>1e3);
|
|
fprintf(<span class="org-string">' - Period = %.0f [s]\n'</span>, args.Ry_period);
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'- Spindle:\n'</span>);
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Rz_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
fprintf(<span class="org-string">' - Constant Position\n'</span>);
|
|
fprintf(<span class="org-string">' - Rz = %.0f [deg]\n'</span>, 180<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>args.Rz_amplitude);
|
|
<span class="org-keyword">case</span> { <span class="org-string">'rotating'</span>, <span class="org-string">'rotating-not-filtered'</span> }
|
|
fprintf(<span class="org-string">' - Rotating\n'</span>);
|
|
fprintf(<span class="org-string">' - Speed = %.0f [rpm]\n'</span>, 60<span class="org-type">/</span>args.Rz_period);
|
|
<span class="org-keyword">end</span>
|
|
|
|
|
|
fprintf(<span class="org-string">'- Micro Hexapod:\n'</span>);
|
|
<span class="org-keyword">switch</span> <span class="org-constant">args.Dh_type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'constant'</span>
|
|
fprintf(<span class="org-string">' - Constant Position\n'</span>);
|
|
fprintf(<span class="org-string">' - Dh = %.0f, %.0f, %.0f [mm]\n'</span>, args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
|
|
fprintf(<span class="org-string">' - Rh = %.0f, %.0f, %.0f [deg]\n'</span>, args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga131750" class="outline-3">
|
|
<h3 id="orga131750">Controller</h3>
|
|
<div class="outline-text-3" id="text-orga131750">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/controller.mat'</span>, <span class="org-string">'controller'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Controller:\n'</span>);
|
|
fprintf(<span class="org-string">'- %s\n'</span>, controller.name);
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org58ad578" class="outline-3">
|
|
<h3 id="org58ad578">Micro-Station</h3>
|
|
<div class="outline-text-3" id="text-org58ad578">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'ground'</span>, <span class="org-string">'granite'</span>, <span class="org-string">'ty'</span>, <span class="org-string">'ry'</span>, <span class="org-string">'rz'</span>, <span class="org-string">'micro_hexapod'</span>, <span class="org-string">'axisc'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Micro Station:\n'</span>);
|
|
|
|
<span class="org-keyword">if</span> granite.type <span class="org-type">==</span> 1 <span class="org-type">&&</span> ...
|
|
ty.type <span class="org-type">==</span> 1 <span class="org-type">&&</span> ...
|
|
ry.type <span class="org-type">==</span> 1 <span class="org-type">&&</span> ...
|
|
rz.type <span class="org-type">==</span> 1 <span class="org-type">&&</span> ...
|
|
micro_hexapod.type <span class="org-type">==</span> 1;
|
|
fprintf(<span class="org-string">'- All stages are rigid\n'</span>);
|
|
<span class="org-keyword">elseif</span> granite.type <span class="org-type">==</span> 2 <span class="org-type">&&</span> ...
|
|
ty.type <span class="org-type">==</span> 2 <span class="org-type">&&</span> ...
|
|
ry.type <span class="org-type">==</span> 2 <span class="org-type">&&</span> ...
|
|
rz.type <span class="org-type">==</span> 2 <span class="org-type">&&</span> ...
|
|
micro_hexapod.type <span class="org-type">==</span> 2;
|
|
fprintf(<span class="org-string">'- All stages are flexible\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
<span class="org-keyword">if</span> granite.type <span class="org-type">==</span> 1 <span class="org-type">||</span> granite.type <span class="org-type">==</span> 4
|
|
fprintf(<span class="org-string">'- Granite is rigid\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Granite is flexible\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">if</span> ty.type <span class="org-type">==</span> 1 <span class="org-type">||</span> ty.type <span class="org-type">==</span> 4
|
|
fprintf(<span class="org-string">'- Translation Stage is rigid\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Translation Stage is flexible\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">if</span> ry.type <span class="org-type">==</span> 1 <span class="org-type">||</span> ry.type <span class="org-type">==</span> 4
|
|
fprintf(<span class="org-string">'- Tilt Stage is rigid\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Tilt Stage is flexible\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">if</span> rz.type <span class="org-type">==</span> 1 <span class="org-type">||</span> rz.type <span class="org-type">==</span> 4
|
|
fprintf(<span class="org-string">'- Spindle is rigid\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Spindle is flexible\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">if</span> micro_hexapod.type <span class="org-type">==</span> 1 <span class="org-type">||</span> micro_hexapod.type <span class="org-type">==</span> 4
|
|
fprintf(<span class="org-string">'- Micro Hexapod is rigid\n'</span>);
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- Micro Hexapod is flexible\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0d8c061" class="outline-3">
|
|
<h3 id="org0d8c061">Metrology</h3>
|
|
<div class="outline-text-3" id="text-org0d8c061">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'mirror'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Reference Mirror:\n'</span>);
|
|
|
|
<span class="org-keyword">if</span> mirror.type <span class="org-type">==</span> 2;
|
|
fprintf(<span class="org-string">'- flexible fixation\n'</span>);
|
|
fprintf(<span class="org-string">'- w = %.0f [Hz]\n'</span>, mirror.freq(1));
|
|
<span class="org-keyword">else</span>
|
|
fprintf(<span class="org-string">'- rigidly attached to the nano-hexapod\n'</span>);
|
|
<span class="org-keyword">end</span>
|
|
fprintf(<span class="org-string">'- m = %.0f [kg]\n'</span>, mirror.mass);
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org61bb891" class="outline-3">
|
|
<h3 id="org61bb891">Nano Hexapod</h3>
|
|
<div class="outline-text-3" id="text-org61bb891">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'nano_hexapod'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Nano Hexapod:\n'</span>);
|
|
|
|
<span class="org-keyword">if</span> nano_hexapod.type <span class="org-type">==</span> 0;
|
|
fprintf(<span class="org-string">'- no included\n'</span>);
|
|
<span class="org-keyword">elseif</span> nano_hexapod.type <span class="org-type">==</span> 1 <span class="org-type">||</span> nano_hexapod.type <span class="org-type">==</span> 3;
|
|
fprintf(<span class="org-string">'- rigid\n'</span>);
|
|
<span class="org-keyword">elseif</span> nano_hexapod.type <span class="org-type">==</span> 2;
|
|
fprintf(<span class="org-string">'- flexible\n'</span>);
|
|
fprintf(<span class="org-string">'- Ki = %.0g [N/m]\n'</span>, nano_hexapod.actuators.K(1));
|
|
<span class="org-keyword">end</span>
|
|
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4eba4ef" class="outline-3">
|
|
<h3 id="org4eba4ef">Sample</h3>
|
|
<div class="outline-text-3" id="text-org4eba4ef">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> load(<span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'sample'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> fprintf(<span class="org-string">'Sample:\n'</span>);
|
|
|
|
<span class="org-keyword">if</span> sample.type <span class="org-type">==</span> 0;
|
|
fprintf(<span class="org-string">'- no included\n'</span>);
|
|
<span class="org-keyword">elseif</span> sample.type <span class="org-type">==</span> 1 <span class="org-type">||</span> sample.type <span class="org-type">==</span> 3;
|
|
fprintf(<span class="org-string">'- rigid\n'</span>);
|
|
fprintf(<span class="org-string">'- mass = %.0f [kg]\n'</span>, sample.mass);
|
|
fprintf(<span class="org-string">'- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n'</span>, sample.inertia(1), sample.inertia(2), sample.inertia(3));
|
|
<span class="org-keyword">elseif</span> sample.type <span class="org-type">==</span> 2;
|
|
fprintf(<span class="org-string">'- flexible\n'</span>);
|
|
fprintf(<span class="org-string">'- mass = %.0f [kg]\n'</span>, sample.mass);
|
|
fprintf(<span class="org-string">'- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n'</span>, sample.inertia(1), sample.inertia(2), sample.inertia(3));
|
|
<span class="org-comment">% fprintf('- Kt = %.0g, %.0g, %.0g [N/m]\n', sample.K(1), sample.K(2), sample.K(3));</span>
|
|
<span class="org-comment">% fprintf('- Kr = %.0g, %.0g, %.0g [Nm/rad]\n', sample.K(4), sample.K(5), sample.K(6));</span>
|
|
fprintf(<span class="org-string">'- wt(x,y,z) = %.0f, %.0f, %.0f [Hz]\n'</span>, 1<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>sqrt(sample.K(1)<span class="org-type">/</span>sample.mass), 1<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>sqrt(sample.K(1)<span class="org-type">/</span>sample.mass), 1<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>sqrt(sample.K(1)<span class="org-type">/</span>sample.mass));
|
|
fprintf(<span class="org-string">'- wr(x,y,z) = %.0f, %.0f, %.0f [Hz]\n'</span>, 1<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>sqrt(sample.K(4)<span class="org-type">/</span>sample.inertia(1)), 1<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>sqrt(sample.K(5)<span class="org-type">/</span>sample.inertia(2)), 1<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">*</span>sqrt(sample.K(6)<span class="org-type">/</span>sample.inertia(3)));
|
|
<span class="org-keyword">end</span>
|
|
fprintf(<span class="org-string">'\n'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6008c77" class="outline-2">
|
|
<h2 id="org6008c77"><span class="section-number-2">2</span> computeReferencePose</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orgdaeb489"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="..//src/computeReferencePose.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[WTr]</span> = <span class="org-function-name">computeReferencePose</span>(<span class="org-variable-name">Dy</span>, <span class="org-variable-name">Ry</span>, <span class="org-variable-name">Rz</span>, <span class="org-variable-name">Dh</span>, <span class="org-variable-name">Dn</span>)
|
|
<span class="org-comment">% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - Dy - Reference of the Translation Stage [m]</span>
|
|
<span class="org-comment">% - Ry - Reference of the Tilt Stage [rad]</span>
|
|
<span class="org-comment">% - Rz - Reference of the Spindle [rad]</span>
|
|
<span class="org-comment">% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]</span>
|
|
<span class="org-comment">% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - WTr -</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Translation Stage</span></span>
|
|
Rty = [1 0 0 0;
|
|
0 1 0 Dy;
|
|
0 0 1 0;
|
|
0 0 0 1];
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Tilt Stage - Pure rotating aligned with Ob</span></span>
|
|
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
|
0 1 0 0;
|
|
<span class="org-type">-</span>sin(Ry) 0 cos(Ry) 0;
|
|
0 0 0 1];
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Rotation along the Z axis</span></span>
|
|
Rrz = [cos(Rz) <span class="org-type">-</span>sin(Rz) 0 0 ;
|
|
sin(Rz) cos(Rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Micro-Hexapod</span></span>
|
|
Rhx = [1 0 0;
|
|
0 cos(Dh(4)) <span class="org-type">-</span>sin(Dh(4));
|
|
0 sin(Dh(4)) cos(Dh(4))];
|
|
|
|
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
|
0 1 0;
|
|
<span class="org-type">-</span>sin(Dh(5)) 0 cos(Dh(5))];
|
|
|
|
Rhz = [cos(Dh(6)) <span class="org-type">-</span>sin(Dh(6)) 0;
|
|
sin(Dh(6)) cos(Dh(6)) 0;
|
|
0 0 1];
|
|
|
|
Rh = [1 0 0 Dh(1) ;
|
|
0 1 0 Dh(2) ;
|
|
0 0 1 Dh(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rh(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = Rhz<span class="org-type">*</span>Rhy<span class="org-type">*</span>Rhx;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Nano-Hexapod</span></span>
|
|
Rnx = [1 0 0;
|
|
0 cos(Dn(4)) <span class="org-type">-</span>sin(Dn(4));
|
|
0 sin(Dn(4)) cos(Dn(4))];
|
|
|
|
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
|
0 1 0;
|
|
<span class="org-type">-</span>sin(Dn(5)) 0 cos(Dn(5))];
|
|
|
|
Rnz = [cos(Dn(6)) <span class="org-type">-</span>sin(Dn(6)) 0;
|
|
sin(Dn(6)) cos(Dn(6)) 0;
|
|
0 0 1];
|
|
|
|
Rn = [1 0 0 Dn(1) ;
|
|
0 1 0 Dn(2) ;
|
|
0 0 1 Dn(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rn(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = Rnz<span class="org-type">*</span>Rny<span class="org-type">*</span>Rnx;
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Total Homogeneous transformation</span></span>
|
|
WTr = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh<span class="org-type">*</span>Rn;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5693f09" class="outline-2">
|
|
<h2 id="org5693f09"><span class="section-number-2">3</span> Compute the Sample Position Error w.r.t. the NASS</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
<a id="org7e4ab10"></a>
|
|
</p>
|
|
|
|
<p>
|
|
This Matlab function is accessible <a href="..//src/computeSampleError.m">here</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"> <span class="org-keyword">function</span> <span class="org-variable-name">[MTr]</span> = <span class="org-function-name">computeSampleError</span>(<span class="org-variable-name">WTm</span>, <span class="org-variable-name">WTr</span>)
|
|
<span class="org-comment">% computeSampleError -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [MTr] = computeSampleError(WTm, WTr)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - WTm - Homoegeneous transformation that represent the</span>
|
|
<span class="org-comment">% wanted pose of the sample with respect to the granite</span>
|
|
<span class="org-comment">% - WTr - Homoegeneous transformation that represent the</span>
|
|
<span class="org-comment">% measured pose of the sample with respect to the granite</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - MTr - Homoegeneous transformation that represent the</span>
|
|
<span class="org-comment">% wanted pose of the sample expressed in a frame</span>
|
|
<span class="org-comment">% attached to the top platform of the nano-hexapod</span>
|
|
|
|
MTr = zeros(4,4);
|
|
|
|
MTr = [WTm(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">'</span>, <span class="org-type">-</span>WTm(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">'*</span>WTm(1<span class="org-type">:</span>3,4) ; 0 0 0 1]<span class="org-type">*</span>WTr;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2021-02-20 sam. 23:08</p>
|
|
</div>
|
|
</body>
|
|
</html>
|