489 lines
14 KiB
HTML
489 lines
14 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<?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>
|
|
<!-- 2020-05-05 mar. 10:33 -->
|
|
<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="./css/htmlize.css"/>
|
|
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
<script src="./js/jquery.min.js"></script>
|
|
<script src="./js/bootstrap.min.js"></script>
|
|
<script src="./js/jquery.stickytableheaders.min.js"></script>
|
|
<script src="./js/readtheorg.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="#org3fa0f20">1. describeNassSetup</a>
|
|
<ul>
|
|
<li><a href="#org3615302">Function description</a></li>
|
|
<li><a href="#org3b8c4f7">Simscape Configuration</a></li>
|
|
<li><a href="#org5b65749">Disturbances</a></li>
|
|
<li><a href="#org59f7825">References</a></li>
|
|
<li><a href="#org90b1ac8">Controller</a></li>
|
|
<li><a href="#orgdd5b7a5">Micro-Station</a></li>
|
|
<li><a href="#org1687c05">Metrology</a></li>
|
|
<li><a href="#orgee5944e">Nano Hexapod</a></li>
|
|
<li><a href="#orga499c17">Sample</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgdc168b9">2. computeReferencePose</a></li>
|
|
<li><a href="#org493ab7f">3. Compute the Sample Position Error w.r.t. the NASS</a></li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3fa0f20" class="outline-2">
|
|
<h2 id="org3fa0f20"><span class="section-number-2">1</span> describeNassSetup</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org0b9cc9d"></a>
|
|
</p>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org3615302" class="outline-3">
|
|
<h3 id="org3615302">Function description</h3>
|
|
<div class="outline-text-3" id="text-org3615302">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">function [] = describeNassSetup()
|
|
% describeNassSetup -
|
|
%
|
|
% Syntax: [] = describeNassSetup()
|
|
%
|
|
% Inputs:
|
|
% - -
|
|
%
|
|
% Outputs:
|
|
% - -
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3b8c4f7" class="outline-3">
|
|
<h3 id="org3b8c4f7">Simscape Configuration</h3>
|
|
<div class="outline-text-3" id="text-org3b8c4f7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/conf_simscape.mat', 'conf_simscape');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Simscape Configuration:\n');
|
|
|
|
if conf_simscape.type == 1
|
|
fprintf('- Gravity is included\n');
|
|
else
|
|
fprintf('- Gravity is not included\n');
|
|
end
|
|
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5b65749" class="outline-3">
|
|
<h3 id="org5b65749">Disturbances</h3>
|
|
<div class="outline-text-3" id="text-org5b65749">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/nass_disturbances.mat', 'args');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Disturbances:\n');
|
|
if ~args.enable
|
|
fprintf('- No disturbance is included\n');
|
|
else
|
|
if args.Dwx && args.Dwy && args.Dwz
|
|
fprintf('- Ground motion\n');
|
|
end
|
|
if args.Fty_x && args.Fty_z
|
|
fprintf('- Vibrations of the Translation Stage\n');
|
|
end
|
|
if args.Frz_z
|
|
fprintf('- Vibrations of the Spindle\n');
|
|
end
|
|
end
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org59f7825" class="outline-3">
|
|
<h3 id="org59f7825">References</h3>
|
|
<div class="outline-text-3" id="text-org59f7825">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/nass_references.mat', 'args');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Reference Tracking:\n');
|
|
fprintf('- Translation Stage:\n');
|
|
switch args.Dy_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
case 'triangular'
|
|
fprintf(' - Triangular Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
|
case 'sinusoidal'
|
|
fprintf(' - Sinusoidal Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
|
end
|
|
|
|
fprintf('- Tilt Stage:\n');
|
|
switch args.Ry_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
case 'triangular'
|
|
fprintf(' - Triangular Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
|
case 'sinusoidal'
|
|
fprintf(' - Sinusoidal Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
|
end
|
|
|
|
fprintf('- Spindle:\n');
|
|
switch args.Rz_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
|
|
case { 'rotating', 'rotating-not-filtered' }
|
|
fprintf(' - Rotating\n');
|
|
fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period);
|
|
end
|
|
|
|
|
|
fprintf('- Micro Hexapod:\n');
|
|
switch args.Dh_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
|
|
fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
|
|
end
|
|
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org90b1ac8" class="outline-3">
|
|
<h3 id="org90b1ac8">Controller</h3>
|
|
<div class="outline-text-3" id="text-org90b1ac8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/controller.mat', 'controller');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Controller:\n');
|
|
fprintf('- %s\n', controller.name);
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdd5b7a5" class="outline-3">
|
|
<h3 id="orgdd5b7a5">Micro-Station</h3>
|
|
<div class="outline-text-3" id="text-orgdd5b7a5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Micro Station:\n');
|
|
|
|
if granite.type == 1 && ...
|
|
ty.type == 1 && ...
|
|
ry.type == 1 && ...
|
|
rz.type == 1 && ...
|
|
micro_hexapod.type == 1;
|
|
fprintf('- All stages are rigid\n');
|
|
elseif granite.type == 2 && ...
|
|
ty.type == 2 && ...
|
|
ry.type == 2 && ...
|
|
rz.type == 2 && ...
|
|
micro_hexapod.type == 2;
|
|
fprintf('- All stages are flexible\n');
|
|
else
|
|
if granite.type == 1 || granite.type == 4
|
|
fprintf('- Granite is rigid\n');
|
|
else
|
|
fprintf('- Granite is flexible\n');
|
|
end
|
|
if ty.type == 1 || ty.type == 4
|
|
fprintf('- Translation Stage is rigid\n');
|
|
else
|
|
fprintf('- Translation Stage is flexible\n');
|
|
end
|
|
if ry.type == 1 || ry.type == 4
|
|
fprintf('- Tilt Stage is rigid\n');
|
|
else
|
|
fprintf('- Tilt Stage is flexible\n');
|
|
end
|
|
if rz.type == 1 || rz.type == 4
|
|
fprintf('- Spindle is rigid\n');
|
|
else
|
|
fprintf('- Spindle is flexible\n');
|
|
end
|
|
if micro_hexapod.type == 1 || micro_hexapod.type == 4
|
|
fprintf('- Micro Hexapod is rigid\n');
|
|
else
|
|
fprintf('- Micro Hexapod is flexible\n');
|
|
end
|
|
|
|
end
|
|
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1687c05" class="outline-3">
|
|
<h3 id="org1687c05">Metrology</h3>
|
|
<div class="outline-text-3" id="text-org1687c05">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/stages.mat', 'mirror');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Reference Mirror:\n');
|
|
|
|
if mirror.type == 2;
|
|
fprintf('- flexible fixation\n');
|
|
fprintf('- w = %.0f [Hz]\n', mirror.freq(1));
|
|
else
|
|
fprintf('- rigidly attached to the nano-hexapod\n');
|
|
end
|
|
fprintf('- m = %.0f [kg]\n', mirror.mass);
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgee5944e" class="outline-3">
|
|
<h3 id="orgee5944e">Nano Hexapod</h3>
|
|
<div class="outline-text-3" id="text-orgee5944e">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/stages.mat', 'nano_hexapod');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Nano Hexapod:\n');
|
|
|
|
if nano_hexapod.type == 0;
|
|
fprintf('- no included\n');
|
|
elseif nano_hexapod.type == 1 || nano_hexapod.type == 3;
|
|
fprintf('- rigid\n');
|
|
elseif nano_hexapod.type == 2;
|
|
fprintf('- flexible\n');
|
|
fprintf('- Ki = %.0g [N/m]\n', nano_hexapod.actuators.K(1));
|
|
end
|
|
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga499c17" class="outline-3">
|
|
<h3 id="orga499c17">Sample</h3>
|
|
<div class="outline-text-3" id="text-orga499c17">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/stages.mat', 'sample');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">fprintf('Sample:\n');
|
|
|
|
if sample.type == 0;
|
|
fprintf('- no included\n');
|
|
elseif sample.type == 1 || sample.type == 3;
|
|
fprintf('- rigid\n');
|
|
fprintf('- mass = %.0f [kg]\n', sample.mass);
|
|
fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3));
|
|
elseif sample.type == 2;
|
|
fprintf('- flexible\n');
|
|
fprintf('- mass = %.0f [kg]\n', sample.mass);
|
|
fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3));
|
|
% fprintf('- Kt = %.0g, %.0g, %.0g [N/m]\n', sample.K(1), sample.K(2), sample.K(3));
|
|
% fprintf('- Kr = %.0g, %.0g, %.0g [Nm/rad]\n', sample.K(4), sample.K(5), sample.K(6));
|
|
fprintf('- wt(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass));
|
|
fprintf('- wr(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(4)/sample.inertia(1)), 1/2/pi*sqrt(sample.K(5)/sample.inertia(2)), 1/2/pi*sqrt(sample.K(6)/sample.inertia(3)));
|
|
end
|
|
fprintf('\n');
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdc168b9" class="outline-2">
|
|
<h2 id="orgdc168b9"><span class="section-number-2">2</span> computeReferencePose</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="org98cbe6e"></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">function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
|
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
|
%
|
|
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
|
%
|
|
% Inputs:
|
|
% - Dy - Reference of the Translation Stage [m]
|
|
% - Ry - Reference of the Tilt Stage [rad]
|
|
% - Rz - Reference of the Spindle [rad]
|
|
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
|
|
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
|
|
%
|
|
% Outputs:
|
|
% - WTr -
|
|
|
|
%% Translation Stage
|
|
Rty = [1 0 0 0;
|
|
0 1 0 Dy;
|
|
0 0 1 0;
|
|
0 0 0 1];
|
|
|
|
%% Tilt Stage - Pure rotating aligned with Ob
|
|
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
|
0 1 0 0;
|
|
-sin(Ry) 0 cos(Ry) 0;
|
|
0 0 0 1];
|
|
|
|
%% Spindle - Rotation along the Z axis
|
|
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
|
sin(Rz) cos(Rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
|
|
%% Micro-Hexapod
|
|
Rhx = [1 0 0;
|
|
0 cos(Dh(4)) -sin(Dh(4));
|
|
0 sin(Dh(4)) cos(Dh(4))];
|
|
|
|
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
|
0 1 0;
|
|
-sin(Dh(5)) 0 cos(Dh(5))];
|
|
|
|
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
|
sin(Dh(6)) cos(Dh(6)) 0;
|
|
0 0 1];
|
|
|
|
Rh = [1 0 0 Dh(1) ;
|
|
0 1 0 Dh(2) ;
|
|
0 0 1 Dh(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
|
|
|
|
%% Nano-Hexapod
|
|
Rnx = [1 0 0;
|
|
0 cos(Dn(4)) -sin(Dn(4));
|
|
0 sin(Dn(4)) cos(Dn(4))];
|
|
|
|
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
|
0 1 0;
|
|
-sin(Dn(5)) 0 cos(Dn(5))];
|
|
|
|
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
|
|
sin(Dn(6)) cos(Dn(6)) 0;
|
|
0 0 1];
|
|
|
|
Rn = [1 0 0 Dn(1) ;
|
|
0 1 0 Dn(2) ;
|
|
0 0 1 Dn(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
|
|
|
|
%% Total Homogeneous transformation
|
|
WTr = Rty*Rry*Rrz*Rh*Rn;
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org493ab7f" class="outline-2">
|
|
<h2 id="org493ab7f"><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="org6dcd4fb"></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">function [MTr] = computeSampleError(WTm, WTr)
|
|
% computeSampleError -
|
|
%
|
|
% Syntax: [MTr] = computeSampleError(WTm, WTr)
|
|
%
|
|
% Inputs:
|
|
% - WTm - Homoegeneous transformation that represent the
|
|
% wanted pose of the sample with respect to the granite
|
|
% - WTr - Homoegeneous transformation that represent the
|
|
% measured pose of the sample with respect to the granite
|
|
%
|
|
% Outputs:
|
|
% - MTr - Homoegeneous transformation that represent the
|
|
% wanted pose of the sample expressed in a frame
|
|
% attached to the top platform of the nano-hexapod
|
|
|
|
MTr = zeros(4,4);
|
|
|
|
MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
|
|
end
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2020-05-05 mar. 10:33</p>
|
|
</div>
|
|
</body>
|
|
</html>
|