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

Huge Update

Browse Source 
- Modify the joints for Ty, Ry, Rz to have only one bushing joint.
- Compensation of gravity
This commit is contained in:
Thomas Dehaeze 2020-02-25 17:49:08 +01:00
parent dfdfcff4db
commit 12c2e4a508
36 changed files with 798 additions and 545 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
active_damping/index.org
View File

@ -41,7 +41,7 @@
#+PROPERTY: header-args:latex+ :output-dir figs
:END:
* Introduction :ignore:
* Introduction :ignore:
The goal of this file is to study the use of active damping for the control of the NASS.
In general, three sensors can be used for Active Damping:
@ -276,7 +276,7 @@ We identify the dynamics of the system using the =linearize= function.
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'En'); io_i = io_i + 1; % Metrology Outputs
#+end_src
@ -4105,3 +4105,150 @@ The obtained sensitivity to disturbances is shown in figure [[fig:ine_1dof_sensi
#+name: fig:ine_1dof_sensitivitiy
#+caption: Sensitivity to disturbance when INE is applied on the 1dof system ([[./figs/ine_1dof_sensitivitiy.png][png]], [[./figs/ine_1dof_sensitivitiy.pdf][pdf]])
[[file:figs/ine_1dof_sensitivitiy.png]]
* Test :noexport:
#+begin_src matlab
AP = [0; 0; 0];
#+end_src
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
initializeSimscapeConfiguration('gravity', true);
initializeLoggingConfiguration('log', 'forces');
initializeGround('type', 'none');
initializeGranite('type', 'none');
initializeTy('type', 'rigid');
initializeRy('type', 'init');
initializeRz('type', 'none');
initializeMicroHexapod('type', 'rigid', 'AP', AP, 'Foffset', false);
initializeAxisc('type', 'none');
initializeMirror('type', 'none');
initializeNanoHexapod('type', 'none');
initializeSample('type', 'init', 'Foffset', false);
sim('nass_model');
#+end_src
#+begin_src matlab
save('./mat/Foffset.mat', 'Foffset');
#+end_src
#+begin_src matlab
initializeRz('type', 'flexible');
initializeMicroHexapod('type', 'flexible', 'AP', AP);
initializeSample('type', 'flexible');
sim('nass_model');
#+end_src
* Test bis :noexport:
#+begin_src matlab
AP = [0; 0; 0];
#+end_src
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
initializeSimscapeConfiguration('gravity', true);
initializeLoggingConfiguration('log', 'forces');
initializeGround('type', 'none');
initializeGranite('type', 'none');
initializeTy('type', 'rigid');
initializeRy('type', 'flexible');
initializeRz('type', 'none');
initializeMicroHexapod('type', 'none');
initializeAxisc('type', 'none');
initializeMirror('type', 'none');
initializeNanoHexapod('type', 'none');
initializeSample('type', 'none');
sim('nass_model');
#+end_src
#+begin_src matlab
save('./mat/Foffset.mat', 'Foffset');
#+end_src
#+begin_src matlab
initializeRz('type', 'flexible');
initializeMicroHexapod('type', 'flexible', 'AP', AP);
initializeSample('type', 'flexible');
sim('nass_model');
#+end_src
* Use only one joint for the tilt stage
#+begin_src matlab
K = [2.8e4; 2.8e4; 2.8e4];
%K = [1e8; 1e8; 1e8];
ty = 7*(2*pi)/180;
Ry = [ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)];
K11 = abs(Ry*K);
K12 = abs(Ry*K);
ty = -7*(2*pi)/180;
Ry = [ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)];
K21 = abs(Ry*K);
K22 = abs(Ry*K);
Ktot = zeros(6,1);
Ktot(1) = K11(1) + K12(1) + K21(1) + K22(1);
Ktot(2) = K11(2) + K12(2) + K21(2) + K22(2);
Ktot(3) = K11(3) + K12(3) + K21(3) + K22(3);
%% Stiffness in rotation
% Position of the joint from the next wanted joint position (in the rotated frame)
P11 = [0; 0.334; 0];
P12 = [0; 0.334; 0];
P21 = [0; 0.334; 0];
P22 = [0; 0.334; 0];
Ktot(4:6) = abs(cross(P11, K11)) + abs(cross(P12, K12)) + abs(cross(P21, K21)) + abs(cross(P22, K22))
#+end_src
#+begin_src matlab
Ry_init = 0;
#+end_src
#+begin_src matlab
initializeSimscapeConfiguration('gravity', true);
initializeLoggingConfiguration('log', 'none');
initializeGround('type', 'none');
initializeGranite('type', 'none');
initializeTy('type', 'none');
initializeRy('type', 'init', 'Ry_init', Ry_init);
initializeRz('type', 'init');
initializeMicroHexapod('type', 'init');
initializeAxisc('type', 'none');
initializeMirror('type', 'none');
initializeNanoHexapod('type', 'none');
initializeSample('type', 'init');
sim('nass_model');
#+end_src
#+begin_src matlab
save('./mat/Foffset.mat', 'Fym');
#+end_src
#+begin_src matlab
initializeReferences('Ry_amplitude', 3*(2*pi)/180)
initializeRy('type', 'flexible', 'Ry_init', 3*(2*pi)/180);
sim('nass_model');
#+end_src

241
disturbances/index.html
View File

@ -1,9 +1,10 @@
<?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>
<!-- 2019-12-13 ven. 16:39 -->
<!-- 2020-02-18 mar. 17:44 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Identification of the disturbances</title>
@ -205,7 +206,7 @@
@licstart The following is the entire license notice for the
JavaScript code in this tag.
Copyright (C) 2012-2019 Free Software Foundation, Inc.
Copyright (C) 2012-2020 Free Software Foundation, Inc.
The JavaScript code in this tag is free software: you can
redistribute it and/or modify it under the terms of the GNU
@ -246,31 +247,16 @@ 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",
Macros: {
bm: ["{\\boldsymbol #1}",1],
}
}
});
</script>
<script type="text/javascript"
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
<script>
MathJax = {
tex: { macros: {
bm: ["\\boldsymbol{#1}",1],
}
}
};
</script>
<script type="text/javascript"
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
</head>
<body>
<div id="org-div-home-and-up">
@ -283,13 +269,13 @@ for the JavaScript code in this tag.
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#org19fda9d">1. Simscape Model</a></li>
<li><a href="#org9e4d547">2. Identification</a></li>
<li><a href="#orgef77399">3. Sensitivity to Disturbances</a></li>
<li><a href="#org27e1b01">4. Power Spectral Density of the effect of the disturbances</a></li>
<li><a href="#orge82082e">5. Compute the Power Spectral Density of the disturbance force</a></li>
<li><a href="#org56c79b2">6. Noise Budget</a></li>
<li><a href="#org5d50476">7. Save</a></li>
<li><a href="#Simscape-Model">1. Simscape Model</a></li>
<li><a href="#Identification">2. Identification</a></li>
<li><a href="#Sensitivity-to-Disturbances">3. Sensitivity to Disturbances</a></li>
<li><a href="#Power-Spectral-Density-of-the-effect-of-the-disturbances">4. Power Spectral Density of the effect of the disturbances</a></li>
<li><a href="#Compute-the-Power-Spectral-Density-of-the-disturbance-force">5. Compute the Power Spectral Density of the disturbance force</a></li>
<li><a href="#Noise-Budget">6. Noise Budget</a></li>
<li><a href="#Save">7. Save</a></li>
</ul>
</div>
</div>
@ -299,7 +285,7 @@ The goal here is to extract the Power Spectral Density of the sources of perturb
</p>
<p>
The sources of perturbations are (schematically shown in figure <a href="#orgf25aa60">1</a>):
The sources of perturbations are (schematically shown in figure <a href="#org0dbac72">1</a>):
</p>
<ul class="org-ul">
<li>\(D_w\): Ground Motion</li>
@ -308,12 +294,12 @@ These forces can be due to imperfect guiding for instance.</li>
</ul>
<p>
Because we cannot measure directly the perturbation forces, we have the measure the effect of those perturbations on the system (in terms of velocity for instance using geophones, \(D\) on figure <a href="#orgf25aa60">1</a>) and then, using a model, compute the forces that induced such velocity.
Because we cannot measure directly the perturbation forces, we have the measure the effect of those perturbations on the system (in terms of velocity for instance using geophones, \(D\) on figure <a href="#org0dbac72">1</a>) and then, using a model, compute the forces that induced such velocity.
</p>
<div id="orgf25aa60" class="figure">
<div id="org0dbac72" class="figure">
<p><img src="figs/uniaxial-model-micro-station.png" alt="uniaxial-model-micro-station.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Schematic of the Micro Station and the sources of disturbance</p>
@ -324,19 +310,19 @@ Because we cannot measure directly the perturbation forces, we have the measure
This file is divided in the following sections:
</p>
<ul class="org-ul">
<li>Section <a href="#org9123adf">1</a>: the simscape model used here is presented</li>
<li>Section <a href="#orgdd2d5a8">2</a>: transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup</li>
<li>Section <a href="#org35da589">3</a>: the bode plot of those transfer functions are shown</li>
<li>Section <a href="#orgec11822">4</a>: the measured PSD of the effect of the disturbances are shown</li>
<li>Section <a href="#org235ee96">5</a>: from the model and the measured PSD, the PSD of the disturbance forces are computed</li>
<li>Section <a href="#org44895f0">6</a>: with the computed PSD, the noise budget of the system is done</li>
<li>Section <a href="#org2c7a8eb">1</a>: the simscape model used here is presented</li>
<li>Section <a href="#orgdef3420">2</a>: transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup</li>
<li>Section <a href="#org1eb00b2">3</a>: the bode plot of those transfer functions are shown</li>
<li>Section <a href="#orgd955c86">4</a>: the measured PSD of the effect of the disturbances are shown</li>
<li>Section <a href="#org6914212">5</a>: from the model and the measured PSD, the PSD of the disturbance forces are computed</li>
<li>Section <a href="#orgc51ba08">6</a>: with the computed PSD, the noise budget of the system is done</li>
</ul>
<div id="outline-container-org19fda9d" class="outline-2">
<h2 id="org19fda9d"><span class="section-number-2">1</span> Simscape Model</h2>
<div class="outline-text-2" id="text-1">
<div id="outline-container-Simscape%20Model" class="outline-2">
<h2 id="Simscape-Model"><span class="section-number-2">1</span> Simscape Model</h2>
<div class="outline-text-2" id="text-Simscape-Model">
<p>
<a id="org9123adf"></a>
<a id="org2c7a8eb"></a>
</p>
<p>
@ -349,17 +335,12 @@ We add disturbances forces in the vertical direction for the Translation Stage a
Also, we measure the absolute displacement of the granite and of the top platform of the Hexapod.
</p>
<div class="org-src-container">
<pre class="src src-matlab">open<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'disturbances/matlab/sim_micro_station_disturbances.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<p>
We load the 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">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<pre class="src src-matlab">load(<span class="org-string">'mat/conf_simulink.mat'</span>);
<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">conf_simulink</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'0.5'</span>);
</pre>
</div>
@ -367,72 +348,83 @@ We load the configuration and we set a small <code>StopTime</code>.
We initialize all the stages.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeGround<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeGranite<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeTy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRy<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeRz<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeAxisc<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeMirror<span class="org-rainbow-delimiters-depth-1">()</span>;
initializeNanoHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
initializeSample<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'mass'</span>, <span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<pre class="src src-matlab">initializeGround();
initializeGranite(<span class="org-string">'type'</span>, <span class="org-string">'modal-analysis'</span>);
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod(<span class="org-string">'type'</span>, <span class="org-string">'modal-analysis'</span>);
initializeAxisc(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
initializeMirror(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
initializeNanoHexapod(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
initializeSample(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
</pre>
</div>
</div>
</div>
<div id="outline-container-org9e4d547" class="outline-2">
<h2 id="org9e4d547"><span class="section-number-2">2</span> Identification</h2>
<div class="outline-text-2" id="text-2">
<div id="outline-container-Identification" class="outline-2">
<h2 id="Identification"><span class="section-number-2">2</span> Identification</h2>
<div class="outline-text-2" id="text-Identification">
<p>
<a id="orgdd2d5a8"></a>
<a id="orgdef3420"></a>
The transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup with the code below.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span>
options = linearizeOptions;
options.SampleTime = <span class="org-highlight-numbers-number">0</span>;
options.SampleTime = 0;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'sim_micro_station_disturbances'</span>;
mdl = <span class="org-string">'nass_model'</span>;
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Micro-Hexapod</span></span>
<span class="org-comment">% Input/Output definition</span>
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-string">'/Dw'</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-string">'input'</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Ground Motion</span>
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-string">'/Fty'</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-string">'input'</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Parasitic force Ty</span>
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-string">'/Frz'</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-string">'input'</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Parasitic force Rz</span>
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-string">'/Dgm'</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-string">'output'</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Absolute motion - Granite</span>
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-string">'/Dhm'</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-string">'output'</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Absolute Motion - Hexapod</span>
io<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = linio<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>mdl, <span class="org-string">'/Vm'</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-string">'output'</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Relative Velocity hexapod/granite</span>
clear io; io_i = 1;
io(io_i) = linio([mdl, <span class="org-string">'/Disturbances'</span>], 1, <span class="org-string">'openinput'</span>, [], <span class="org-string">'Dwz'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Vertical Ground Motion</span>
io(io_i) = linio([mdl, <span class="org-string">'/Disturbances'</span>], 1, <span class="org-string">'openinput'</span>, [], <span class="org-string">'Fty_z'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Parasitic force Ty</span>
io(io_i) = linio([mdl, <span class="org-string">'/Disturbances'</span>], 1, <span class="org-string">'openinput'</span>, [], <span class="org-string">'Frz_z'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Parasitic force Rz</span>
io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station/Granite/Modal Analysis/accelerometer'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute motion - Granite</span>
io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Motion - Hexapod</span>
<span class="org-comment">% io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Velocity hexapod/granite</span>
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-comment">% Run the linearization</span>
G = linearize<span class="org-rainbow-delimiters-depth-1">(</span>mdl, io, <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-1">)</span>;
G = linearize(mdl, io, 0);
<span class="org-comment">% We Take only the outputs corresponding to the vertical acceleration</span>
G = G([3,9], <span class="org-type">:</span>);
<span class="org-comment">% Input/Output names</span>
G.InputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'Dw'</span>, <span class="org-string">'Fty'</span>, <span class="org-string">'Frz'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
G.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class="org-string">'Dgm'</span>, <span class="org-string">'Dhm'</span>, <span class="org-string">'Vm'</span><span class="org-rainbow-delimiters-depth-1">}</span>;
G.InputName = {<span class="org-string">'Dw'</span>, <span class="org-string">'Fty'</span>, <span class="org-string">'Frz'</span>};
G.OutputName = {<span class="org-string">'Agm'</span>, <span class="org-string">'Ahm'</span>};
<span class="org-comment">% We integrate 1 time the output to have the velocity and we</span>
<span class="org-comment">% substract the absolute velocities to have the relative velocity</span>
G = (1<span class="org-type">/</span>s)<span class="org-type">*</span>tf([<span class="org-type">-</span>1, 1])<span class="org-type">*</span>G;
<span class="org-comment">% Input/Output names</span>
G.InputName = {<span class="org-string">'Dw'</span>, <span class="org-string">'Fty'</span>, <span class="org-string">'Frz'</span>};
G.OutputName = {<span class="org-string">'Vm'</span>};
</pre>
</div>
</div>
</div>
<div id="outline-container-orgef77399" class="outline-2">
<h2 id="orgef77399"><span class="section-number-2">3</span> Sensitivity to Disturbances</h2>
<div class="outline-text-2" id="text-3">
<div id="outline-container-Sensitivity%20to%20Disturbances" class="outline-2">
<h2 id="Sensitivity-to-Disturbances"><span class="section-number-2">3</span> Sensitivity to Disturbances</h2>
<div class="outline-text-2" id="text-Sensitivity-to-Disturbances">
<p>
<a id="org35da589"></a>
<a id="org1eb00b2"></a>
</p>
<div id="orgab7828d" class="figure">
<div id="org40ebd12" class="figure">
<p><img src="figs/sensitivity_dist_gm.png" alt="sensitivity_dist_gm.png" />
</p>
<p><span class="figure-number">Figure 2: </span>Sensitivity to Ground Motion (<a href="./figs/sensitivity_dist_gm.png">png</a>, <a href="./figs/sensitivity_dist_gm.pdf">pdf</a>)</p>
@ -440,7 +432,7 @@ G.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class=
<div id="org1351421" class="figure">
<div id="org488da2a" class="figure">
<p><img src="figs/sensitivity_dist_fty.png" alt="sensitivity_dist_fty.png" />
</p>
<p><span class="figure-number">Figure 3: </span>Sensitivity to vertical forces applied by the Ty stage (<a href="./figs/sensitivity_dist_fty.png">png</a>, <a href="./figs/sensitivity_dist_fty.pdf">pdf</a>)</p>
@ -448,7 +440,7 @@ G.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class=
<div id="org667306c" class="figure">
<div id="org8152b2c" class="figure">
<p><img src="figs/sensitivity_dist_frz.png" alt="sensitivity_dist_frz.png" />
</p>
<p><span class="figure-number">Figure 4: </span>Sensitivity to vertical forces applied by the Rz stage (<a href="./figs/sensitivity_dist_frz.png">png</a>, <a href="./figs/sensitivity_dist_frz.pdf">pdf</a>)</p>
@ -456,11 +448,11 @@ G.OutputName = <span class="org-rainbow-delimiters-depth-1">{</span><span class=
</div>
</div>
<div id="outline-container-org27e1b01" class="outline-2">
<h2 id="org27e1b01"><span class="section-number-2">4</span> Power Spectral Density of the effect of the disturbances</h2>
<div class="outline-text-2" id="text-4">
<div id="outline-container-Power%20Spectral%20Density%20of%20the%20effect%20of%20the%20disturbances" class="outline-2">
<h2 id="Power-Spectral-Density-of-the-effect-of-the-disturbances"><span class="section-number-2">4</span> Power Spectral Density of the effect of the disturbances</h2>
<div class="outline-text-2" id="text-Power-Spectral-Density-of-the-effect-of-the-disturbances">
<p>
<a id="orgec11822"></a>
<a id="orgd955c86"></a>
The PSD of the relative velocity between the hexapod and the marble in \([(m/s)^2/Hz]\) are loaded for the following sources of disturbance:
</p>
<ul class="org-ul">
@ -473,10 +465,10 @@ Also, the Ground Motion is measured.
</p>
<div class="org-src-container">
<pre class="src src-matlab">gm = load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./disturbances/mat/psd_gm.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'psd_gm'</span>, <span class="org-string">'psd_gv'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
rz = load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./disturbances/mat/pxsp_r.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'pxsp_r'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
tyz = load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./disturbances/mat/pxz_ty_r.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'pxz_ty_r'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
tyx = load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./disturbances/mat/pxe_ty_r.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'pxe_ty_r'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<pre class="src src-matlab">gm = load(<span class="org-string">'./disturbances/mat/psd_gm.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'psd_gm'</span>, <span class="org-string">'psd_gv'</span>);
rz = load(<span class="org-string">'./disturbances/mat/pxsp_r.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'pxsp_r'</span>);
tyz = load(<span class="org-string">'./disturbances/mat/pxz_ty_r.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'pxz_ty_r'</span>);
tyx = load(<span class="org-string">'./disturbances/mat/pxe_ty_r.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'pxe_ty_r'</span>);
</pre>
</div>
@ -484,20 +476,20 @@ tyx = load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-
We now compute the relative velocity between the hexapod and the granite due to ground motion.
</p>
<div class="org-src-container">
<pre class="src src-matlab">gm.psd_rv = gm.psd_gm<span class="org-type">.*</span>abs<span class="org-rainbow-delimiters-depth-1">(</span>squeeze<span class="org-rainbow-delimiters-depth-2">(</span>freqresp<span class="org-rainbow-delimiters-depth-3">(</span>G<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-string">'Vm'</span>, <span class="org-string">'Dw'</span><span class="org-rainbow-delimiters-depth-4">)</span>, gm.f, <span class="org-string">'Hz'</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">.^</span><span class="org-highlight-numbers-number">2</span>;
<pre class="src src-matlab">gm.psd_rv = gm.psd_gm<span class="org-type">.*</span>abs(squeeze(freqresp(G(<span class="org-string">'Vm'</span>, <span class="org-string">'Dw'</span>), gm.f, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
</pre>
</div>
<p>
The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures <a href="#orge500b6c">5</a> and <a href="#orgd78766d">6</a>.
The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures <a href="#orga80b694">5</a> and <a href="#org337df44">6</a>.
</p>
<p>
The Cumulative Amplitude Spectrum of the relative motion is shown in figure <a href="#org8fa6982">7</a>.
The Cumulative Amplitude Spectrum of the relative motion is shown in figure <a href="#org2f4ee86">7</a>.
</p>
<div id="orge500b6c" class="figure">
<div id="orga80b694" class="figure">
<p><img src="figs/dist_effect_relative_velocity.png" alt="dist_effect_relative_velocity.png" />
</p>
<p><span class="figure-number">Figure 5: </span>Amplitude Spectral Density of the relative velocity of the hexapod with respect to the granite due to different sources of perturbation (<a href="./figs/dist_effect_relative_velocity.png">png</a>, <a href="./figs/dist_effect_relative_velocity.pdf">pdf</a>)</p>
@ -505,14 +497,14 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure <a h
<div id="orgd78766d" class="figure">
<div id="org337df44" class="figure">
<p><img src="figs/dist_effect_relative_motion.png" alt="dist_effect_relative_motion.png" />
</p>
<p><span class="figure-number">Figure 6: </span>Amplitude Spectral Density of the relative displacement of the hexapod with respect to the granite due to different sources of perturbation (<a href="./figs/dist_effect_relative_motion.png">png</a>, <a href="./figs/dist_effect_relative_motion.pdf">pdf</a>)</p>
</div>
<div id="org8fa6982" class="figure">
<div id="org2f4ee86" class="figure">
<p><img src="figs/dist_effect_relative_motion_cas.png" alt="dist_effect_relative_motion_cas.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Cumulative Amplitude Spectrum of the relative motion due to different sources of perturbation (<a href="./figs/dist_effect_relative_motion_cas.png">png</a>, <a href="./figs/dist_effect_relative_motion_cas.pdf">pdf</a>)</p>
@ -520,25 +512,25 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure <a h
</div>
</div>
<div id="outline-container-orge82082e" class="outline-2">
<h2 id="orge82082e"><span class="section-number-2">5</span> Compute the Power Spectral Density of the disturbance force</h2>
<div class="outline-text-2" id="text-5">
<div id="outline-container-Compute%20the%20Power%20Spectral%20Density%20of%20the%20disturbance%20force" class="outline-2">
<h2 id="Compute-the-Power-Spectral-Density-of-the-disturbance-force"><span class="section-number-2">5</span> Compute the Power Spectral Density of the disturbance force</h2>
<div class="outline-text-2" id="text-Compute-the-Power-Spectral-Density-of-the-disturbance-force">
<p>
<a id="org235ee96"></a>
<a id="org6914212"></a>
</p>
<p>
Now, from the extracted transfer functions from the disturbance force to the relative motion of the hexapod with respect to the granite (section <a href="#org35da589">3</a>) and from the measured PSD of the relative motion (section <a href="#orgec11822">4</a>), we can compute the PSD of the disturbance force.
Now, from the extracted transfer functions from the disturbance force to the relative motion of the hexapod with respect to the granite (section <a href="#org1eb00b2">3</a>) and from the measured PSD of the relative motion (section <a href="#orgd955c86">4</a>), we can compute the PSD of the disturbance force.
</p>
<div class="org-src-container">
<pre class="src src-matlab">rz.psd_f = rz.pxsp_r<span class="org-type">./</span>abs<span class="org-rainbow-delimiters-depth-1">(</span>squeeze<span class="org-rainbow-delimiters-depth-2">(</span>freqresp<span class="org-rainbow-delimiters-depth-3">(</span>G<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-string">'Vm'</span>, <span class="org-string">'Frz'</span><span class="org-rainbow-delimiters-depth-4">)</span>, rz.f, <span class="org-string">'Hz'</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">.^</span><span class="org-highlight-numbers-number">2</span>;
tyz.psd_f = tyz.pxz_ty_r<span class="org-type">./</span>abs<span class="org-rainbow-delimiters-depth-1">(</span>squeeze<span class="org-rainbow-delimiters-depth-2">(</span>freqresp<span class="org-rainbow-delimiters-depth-3">(</span>G<span class="org-rainbow-delimiters-depth-4">(</span><span class="org-string">'Vm'</span>, <span class="org-string">'Fty'</span><span class="org-rainbow-delimiters-depth-4">)</span>, tyz.f, <span class="org-string">'Hz'</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">.^</span><span class="org-highlight-numbers-number">2</span>;
<pre class="src src-matlab">rz.psd_f = rz.pxsp_r<span class="org-type">./</span>abs(squeeze(freqresp(G(<span class="org-string">'Vm'</span>, <span class="org-string">'Frz'</span>), rz.f, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
tyz.psd_f = tyz.pxz_ty_r<span class="org-type">./</span>abs(squeeze(freqresp(G(<span class="org-string">'Vm'</span>, <span class="org-string">'Fty'</span>), tyz.f, <span class="org-string">'Hz'</span>)))<span class="org-type">.^</span>2;
</pre>
</div>
<div id="org39f9337" class="figure">
<div id="org9a8bb49" class="figure">
<p><img src="figs/dist_force_psd.png" alt="dist_force_psd.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Amplitude Spectral Density of the disturbance force (<a href="./figs/dist_force_psd.png">png</a>, <a href="./figs/dist_force_psd.pdf">pdf</a>)</p>
@ -546,11 +538,11 @@ tyz.psd_f = tyz.pxz_ty_r<span class="org-type">./</span>abs<span class="org-rain
</div>
</div>
<div id="outline-container-org56c79b2" class="outline-2">
<h2 id="org56c79b2"><span class="section-number-2">6</span> Noise Budget</h2>
<div class="outline-text-2" id="text-6">
<div id="outline-container-Noise%20Budget" class="outline-2">
<h2 id="Noise-Budget"><span class="section-number-2">6</span> Noise Budget</h2>
<div class="outline-text-2" id="text-Noise-Budget">
<p>
<a id="org44895f0"></a>
<a id="orgc51ba08"></a>
</p>
<p>
@ -559,7 +551,7 @@ We should verify that this is coherent with the measurements.
</p>
<div id="orga5608a0" class="figure">
<div id="orgede2c8c" class="figure">
<p><img src="figs/psd_effect_dist_verif.png" alt="psd_effect_dist_verif.png" />
</p>
<p><span class="figure-number">Figure 9: </span>Computed Effect of the disturbances on the relative displacement hexapod/granite (<a href="./figs/psd_effect_dist_verif.png">png</a>, <a href="./figs/psd_effect_dist_verif.pdf">pdf</a>)</p>
@ -567,7 +559,7 @@ We should verify that this is coherent with the measurements.
<div id="org96f239c" class="figure">
<div id="org4aeb9e5" class="figure">
<p><img src="figs/cas_computed_relative_displacement.png" alt="cas_computed_relative_displacement.png" />
</p>
<p><span class="figure-number">Figure 10: </span>CAS of the total Relative Displacement due to all considered sources of perturbation (<a href="./figs/cas_computed_relative_displacement.png">png</a>, <a href="./figs/cas_computed_relative_displacement.pdf">pdf</a>)</p>
@ -575,23 +567,23 @@ We should verify that this is coherent with the measurements.
</div>
</div>
<div id="outline-container-org5d50476" class="outline-2">
<h2 id="org5d50476"><span class="section-number-2">7</span> Save</h2>
<div class="outline-text-2" id="text-7">
<div id="outline-container-Save" class="outline-2">
<h2 id="Save"><span class="section-number-2">7</span> Save</h2>
<div class="outline-text-2" id="text-Save">
<p>
The PSD of the disturbance force are now saved for further analysis (the mat file is accessible <a href="mat/dist_psd.mat">here</a>).
</p>
<div class="org-src-container">
<pre class="src src-matlab">dist_f = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
<pre class="src src-matlab">dist_f = struct();
dist_f.f = gm.f; <span class="org-comment">% Frequency Vector [Hz]</span>
dist_f.psd_gm = gm.psd_gm; % Power Spectral Density of the Ground Motion [m<span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span>Hz]
dist_f.psd_ty = tyz.psd_f; % Power Spectral Density of the force induced by the Ty stage in the Z direction [N<span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span>Hz]
dist_f.psd_rz = rz.psd_f; % Power Spectral Density of the force induced by the Rz stage in the Z direction [N<span class="org-type">^</span><span class="org-highlight-numbers-number">2</span><span class="org-type">/</span>Hz]
dist_f.psd_gm = gm.psd_gm; <span class="org-comment">% Power Spectral Density of the Ground Motion [m^2/Hz]</span>
dist_f.psd_ty = tyz.psd_f; <span class="org-comment">% Power Spectral Density of the force induced by the Ty stage in the Z direction [N^2/Hz]</span>
dist_f.psd_rz = rz.psd_f; <span class="org-comment">% Power Spectral Density of the force induced by the Rz stage in the Z direction [N^2/Hz]</span>
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./disturbances/mat/dist_psd.mat'</span>, <span class="org-string">'dist_f'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
save(<span class="org-string">'./disturbances/mat/dist_psd.mat'</span>, <span class="org-string">'dist_f'</span>);
</pre>
</div>
</div>
@ -599,8 +591,7 @@ save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-12-13 ven. 16:39</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
<p class="date">Created: 2020-02-18 mar. 17:44</p>
</div>
</body>
</html>

27
disturbances/index.org
View File

@ -42,6 +42,9 @@
:END:
* Introduction :ignore:
:PROPERTIES:
:CUSTOM_ID: Introduction
:END:
The goal here is to extract the Power Spectral Density of the sources of perturbation.
The sources of perturbations are (schematically shown in figure [[fig:uniaxial-model-micro-station]]):
@ -172,6 +175,9 @@ This file is divided in the following sections:
- Section [[sec:noise_budget]]: with the computed PSD, the noise budget of the system is done
* Matlab Init :noexport:ignore:
:PROPERTIES:
:CUSTOM_ID: Matlab-Init
:END:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
@ -189,6 +195,9 @@ This file is divided in the following sections:
#+end_src
* Simscape Model
:PROPERTIES:
:CUSTOM_ID: Simscape-Model
:END:
<<sec:simscape_model>>
The following Simscape model of the micro-station is the same model used for the dynamical analysis.
@ -218,6 +227,9 @@ We initialize all the stages.
#+end_src
* Identification
:PROPERTIES:
:CUSTOM_ID: Identification
:END:
<<sec:identification>>
The transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup with the code below.
@ -262,6 +274,9 @@ The transfer functions from the disturbance forces to the relative velocity of t
#+end_src
* Sensitivity to Disturbances
:PROPERTIES:
:CUSTOM_ID: Sensitivity-to-Disturbances
:END:
<<sec:sensitivity_disturbances>>
#+begin_src matlab :exports none
@ -330,6 +345,9 @@ The transfer functions from the disturbance forces to the relative velocity of t
[[file:figs/sensitivity_dist_frz.png]]
* Power Spectral Density of the effect of the disturbances
:PROPERTIES:
:CUSTOM_ID: Power-Spectral-Density-of-the-effect-of-the-disturbances
:END:
<<sec:psd_dist>>
The PSD of the relative velocity between the hexapod and the marble in $[(m/s)^2/Hz]$ are loaded for the following sources of disturbance:
- Slip Ring Rotation
@ -434,6 +452,9 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure [[fi
[[file:figs/dist_effect_relative_motion_cas.png]]
* Compute the Power Spectral Density of the disturbance force
:PROPERTIES:
:CUSTOM_ID: Compute-the-Power-Spectral-Density-of-the-disturbance-force
:END:
<<sec:psd_force_dist>>
Now, from the extracted transfer functions from the disturbance force to the relative motion of the hexapod with respect to the granite (section [[sec:sensitivity_disturbances]]) and from the measured PSD of the relative motion (section [[sec:psd_dist]]), we can compute the PSD of the disturbance force.
@ -466,6 +487,9 @@ Now, from the extracted transfer functions from the disturbance force to the rel
[[file:figs/dist_force_psd.png]]
* Noise Budget
:PROPERTIES:
:CUSTOM_ID: Noise-Budget
:END:
<<sec:noise_budget>>
Now, from the compute spectral density of the disturbance sources, we can compute the resulting relative motion of the Hexapod with respect to the granite using the model.
@ -526,6 +550,9 @@ We should verify that this is coherent with the measurements.
[[file:figs/cas_computed_relative_displacement.png]]
* Save
:PROPERTIES:
:CUSTOM_ID: Save
:END:
The PSD of the disturbance force are now saved for further analysis (the mat file is accessible [[file:mat/dist_psd.mat][here]]).
#+begin_src matlab

BIN
disturbances/mat/dist_psd.mat
View File

Binary file not shown.

34
functions/index.org
View File

@ -284,38 +284,6 @@ This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
end
#+end_src
* Inverse Kinematics of the Hexapod
:PROPERTIES:
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:inverseKinematicsHexapod>>
This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].
#+begin_src matlab
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
#+end_src
* computeReferencePose
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeReferencePose.m
@ -398,7 +366,7 @@ This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
0 0 1 Dn(3) ;
0 0 0 1 ];
Rn(1:3, 1:3) = Rnx*Rny*Rnz;
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
%% Total Homogeneous transformation
WTr = Rty*Rry*Rrz*Rh*Rn;

47
kinematics/index.org
View File

@ -97,7 +97,7 @@ The three rotations that we define thus corresponds to the Euler U-V-W angles.
We should have the *same behavior* for the Micro-Hexapod on Simscape (same inputs at least).
However, the Bushing Joint makes rotations around mobiles axes (X, Y' and then Z'') and not fixed axes (X, Y and Z).
*** TODO Using Inverse Kinematics and Leg Actuators
*** Using Inverse Kinematics and Leg Actuators
Here, we can use the Inverse Kinematic of the Hexapod to determine the length of each leg in order to obtain some defined translation and rotation of the mobile platform.
The advantages are:
@ -147,20 +147,20 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
**** Matlab Implementation
We open the Simulink file.
#+begin_src matlab
open('nass_model.slx')
#+end_src
We load the configuration and set a small =StopTime=.
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
set_param(conf_simulink, 'StopTime', '0.1');
#+end_src
We define the wanted position/orientation of the Hexapod under study.
#+begin_src matlab
tx = 0.1; % [rad]
ty = 0.2; % [rad]
tz = 0.05; % [rad]
tx = 0.05; % [rad]
ty = 0.1; % [rad]
tz = 0.02; % [rad]
Rx = [1 0 0;
0 cos(tx) -sin(tx);
@ -175,34 +175,47 @@ We define the wanted position/orientation of the Hexapod under study.
0 0 1];
ARB = Rz*Ry*Rx;
AP = [0.01; 0.02; 0.03]; % [m]
AP = [0.1; 0.005; 0.01]; % [m]
#+end_src
hexapod = initializeMicroHexapod('AP', AP, 'ARB', ARB);
#+begin_src matlab
initializeSimscapeConfiguration('gravity', false);
initializeGround('type', 'none');
initializeGranite('type', 'none');
initializeTy('type', 'none');
initializeRy('type', 'none');
initializeRz('type', 'none');
initializeMicroHexapod('type', 'rigid', 'AP', AP, 'ARB', ARB);
initializeAxisc('type', 'none');
initializeMirror('type', 'none');
initializeNanoHexapod('type', 'none');
initializeSample('type', 'none');
initializeLoggingConfiguration('log', 'all');
#+end_src
We run the simulation.
#+begin_src matlab
sim()
sim('nass_model');
#+end_src
And we verify that we indeed succeed to go to the wanted position.
#+begin_src matlab :results table replace
[simout.x.Data(end) ; simout.y.Data(end) ; simout.z.Data(end)] - AP
[simout.Dhm.x.Data(end) ; simout.Dhm.y.Data(end) ; simout.Dhm.z.Data(end)] - AP
#+end_src
#+RESULTS:
| 1.611e-10 |
| -1.3748e-10 |
| 8.4879e-11 |
| 8.4655e-16 |
| 1.5586e-15 |
| -2.1337e-16 |
#+begin_src matlab :results table replace
simout.R.Data(:, :, end)-ARB
simout.Dhm.R.Data(:, :, end)-ARB
#+end_src
#+RESULTS:
| -1.2659e-10 | 6.5603e-11 | 6.2183e-10 |
| 1.0354e-10 | -5.2439e-11 | -5.2425e-10 |
| -5.9816e-10 | 5.532e-10 | -1.7737e-10 |
| -1.1102e-16 | -1.36e-15 | 4.2744e-15 |
| 1.0651e-15 | 6.6613e-16 | 5.1278e-15 |
| -4.2882e-15 | -4.9336e-15 | 1.1102e-16 |
* TODO Tests on the transformation from reference to wanted position :noexport:
:PROPERTIES:

BIN
mat/Foffset.mat Normal file
View File

Binary file not shown.

BIN
mat/conf_log.mat
View File

Binary file not shown.

BIN
mat/conf_simscape.mat
View File

Binary file not shown.

BIN
mat/controller.mat
View File

Binary file not shown.

BIN
mat/controllers.mat
View File

Binary file not shown.

BIN
mat/nass_disturbances.mat
View File

Binary file not shown.

BIN
mat/nass_references.mat
View File

Binary file not shown.

BIN
mat/pos_error.mat Normal file
View File

Binary file not shown.

BIN
mat/stages.mat
View File

Binary file not shown.

118
positioning_error/index.org
View File

@ -99,27 +99,34 @@ The goal here is to perfectly move the station and verify that there is no misma
#+end_src
#+begin_src matlab
open('positioning_error/matlab/sim_nano_station_metrology.slx')
open('nass_model.slx')
#+end_src
** Prepare the Simulation
We set a small =StopTime=.
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeAxisc('type', 'rigid');
initializeMirror('type', 'rigid');
initializeNanoHexapod('type', 'rigid', 'actuator', 'piezo');
initializeSample('type', 'rigid', 'mass', 50);
#+end_src
No disturbance and no gravity.
#+begin_src matlab
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
#+end_src
We setup the reference path to be constant.
@ -154,9 +161,14 @@ No position error for now (perfect positioning).
Dne = zeros(6,1); % [m,m,m,rad,rad,rad]
#+end_src
We want to log the signals
#+begin_src matlab
initializeLoggingConfiguration('log', 'all');
#+end_src
And we run the simulation.
#+begin_src matlab
sim('sim_nano_station_metrology');
sim('nass_model');
#+end_src
** Verify that the pose of the sample is the same as the computed one
@ -171,20 +183,20 @@ We have then computed:
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
n = length(Dref.Dy.Time);
n = length(simout.r.Dy.Time);
WTr = zeros(4, 4, n);
for i = 1:n
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:), Dref.Dn.Data(i,:));
WTr(:, :, i) = computeReferencePose(simout.r.Dy.Data(i), simout.r.Ry.Data(i), simout.r.Rz.Data(i), simout.r.Dh.Data(i,:), simout.r.Dn.Data(i,:));
end
#+end_src
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
n = length(simout.y.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(1:3, 1:3, :) = simout.y.R.Data;
WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
@ -242,32 +254,34 @@ We want to verify that we are able to measure this positioning error and convert
#+end_src
#+begin_src matlab
open('positioning_error/matlab/sim_nano_station_metrology.slx')
open('nass_model.slx')
#+end_src
** Prepare the Simulation
We load the configuration.
#+begin_src matlab
load('mat/conf_simulink.mat');
#+end_src
We set a small =StopTime=.
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
#+end_src
We initialize all the stages.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);
initializeGround('type', 'rigid');
initializeGranite('type', 'rigid');
initializeTy('type', 'rigid');
initializeRy('type', 'rigid');
initializeRz('type', 'rigid');
initializeMicroHexapod('type', 'rigid');
initializeAxisc('type', 'rigid');
initializeMirror('type', 'rigid');
initializeNanoHexapod('type', 'rigid', 'actuator', 'piezo');
initializeSample('type', 'rigid', 'mass', 50);
#+end_src
No disturbance and no gravity.
#+begin_src matlab
initializeSimscapeConfiguration('gravity', false);
initializeDisturbances('enable', false);
#+end_src
We setup the reference path to be constant.
@ -294,14 +308,12 @@ Now we introduce some positioning error.
Dye = 1e-6; % [m]
Rye = 2e-4; % [rad]
Rze = 1e-5; % [rad]
Dhe = zeros(6,1);
Dhle = [1e-6 ; 2e-6 ; 3e-6 ; -2e-6 ; 1e-6 ; 2e-6]; % [m]
Dne = zeros(6,1);
initializePosError('error', true, 'Dy', Dye, 'Ry', Rye, 'Rz', Rze);
#+end_src
And we run the simulation.
#+begin_src matlab
sim('sim_nano_station_metrology');
sim('nass_model');
#+end_src
** Compute the wanted pose of the sample in the NASS Base from the metrology and the reference
@ -319,20 +331,20 @@ The top platform of the nano-hexapod is considered to be rigidly connected to th
We load the reference and we compute the desired trajectory of the sample in the form of an homogeneous transformation matrix ${}^W\bm{T}_R$.
#+begin_src matlab
n = length(Dref.Dy.Time);
n = length(simout.r.Dy.Time);
WTr = zeros(4, 4, n);
for i = 1:n
WTr(:, :, i) = computeReferencePose(Dref.Dy.Data(i), Dref.Ry.Data(i), Dref.Rz.Data(i), Dref.Dh.Data(i,:), Dref.Dn.Data(i,:));
WTr(:, :, i) = computeReferencePose(simout.r.Dy.Data(i), simout.r.Ry.Data(i), simout.r.Rz.Data(i), simout.r.Dh.Data(i,:), simout.r.Dn.Data(i,:));
end
#+end_src
We also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
n = length(simout.y.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(1:3, 1:3, :) = simout.y.R.Data;
WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
@ -387,9 +399,9 @@ Verify that the pose error corresponds to the positioning error of the stages.
#+end_src
#+RESULTS:
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+----------+-----------+-----------+-----------|
| Error | 2.8e-06 | -2.0e-06 | -1.3e-06 | -5.1e-06 | -1.8e-04 | 4.2e-07 |
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+----------+----------+----------+-----------+-----------+-----------|
| Error | -1.0e-11 | -1.0e-06 | -6.2e-16 | -2.0e-09 | -2.0e-04 | -1.0e-05 |
** Verify that be imposing the error motion on the nano-hexapod, we indeed have zero error at the end
We now keep the wanted pose but we impose a displacement of the nano hexapod corresponding to the measured position error.
@ -407,13 +419,13 @@ We now keep the wanted pose but we impose a displacement of the nano hexapod cor
'Rm_type', 'constant', ... % For now, only constant is implemented
'Rm_pos', [0, pi]', ... % Initial position of the two masses
'Dn_type', 'constant', ... % For now, only constant is implemented
'Dn_pos', [Edx, Edy, Edz, Erx, Ery, Erz]' ... % Initial position [m,m,m,rad,rad,rad] of the top platform
'Dn_pos', [Edx; Edy; Edz; Erx; Ery; Erz] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
);
#+end_src
And we run the simulation.
#+begin_src matlab
sim('sim_nano_station_metrology');
sim('nass_model');
#+end_src
We keep the old computed computed reference pose ${}^W\bm{T}_r$ even though we have change the nano hexapod reference, but this is not a real wanted reference but rather a adaptation to reject the positioning errors.
@ -421,10 +433,10 @@ We keep the old computed computed reference pose ${}^W\bm{T}_r$ even though we h
As the displacement is perfect, we also measure in simulation the pose of the sample with respect to the granite.
From that we can compute the homogeneous transformation matrix ${}^W\bm{T}_M$.
#+begin_src matlab
n = length(Dsm.R.Time);
n = length(simout.y.R.Time);
WTm = zeros(4, 4, n);
WTm(1:3, 1:3, :) = Dsm.R.Data;
WTm(1:3, 4, :) = [Dsm.x.Data' ; Dsm.y.Data' ; Dsm.z.Data'];
WTm(1:3, 1:3, :) = simout.y.R.Data;
WTm(1:3, 4, :) = [simout.y.x.Data' ; simout.y.y.Data' ; simout.y.z.Data'];
WTm(4, 4, :) = 1;
#+end_src
@ -451,9 +463,9 @@ Verify that the pose error is small.
#+end_src
#+RESULTS:
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+---------+---------+-----------+-----------+-----------|
| Error | 2.0e-16 | 1.1e-16 | 3.2e-18 | -1.1e-17 | 1.0e-17 | -9.5e-16 |
| | Edx [m] | Edy [m] | Edz [m] | Erx [rad] | Ery [rad] | Erz [rad] |
|-------+---------+----------+---------+-----------+-----------+-----------|
| Error | 2.4e-16 | -9.9e-17 | 4.4e-16 | 2.0e-09 | -8.9e-15 | 2.0e-13 |
** Conclusion
#+begin_important

BIN
positioning_error/matlab/sim_nano_station_metrology.slx
View File

Binary file not shown.

BIN
simscape/nass_model.slx
View File

Binary file not shown.

432
simscape_subsystems/index.org
View File

@ -129,7 +129,7 @@ These functions are defined below.
:END:
#+begin_src matlab
arguments
args.log char {mustBeMember(args.log,{'none', 'all'})} = 'none'
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
end
#+end_src
@ -152,6 +152,8 @@ These functions are defined below.
conf_log.type = 0;
case 'all'
conf_log.type = 1;
case 'forces'
conf_log.type = 2;
end
#+end_src
@ -298,7 +300,8 @@ The output =sample_pos= corresponds to the impact point of the X-ray.
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis'})} = 'flexible'
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
@ -330,45 +333,51 @@ First, we initialize the =granite= structure.
granite.type = 2;
case 'modal-analysis'
granite.type = 3;
case 'init'
granite.type = 4;
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry of the granite.
#+begin_src matlab
granite.density = args.density; % [kg/m3]
granite.STEP = './STEPS/granite/granite.STEP';
#+end_src
Stiffness of the connection with Ground.
#+begin_src matlab
granite.k.x = 4e9; % [N/m]
granite.k.y = 3e8; % [N/m]
granite.k.z = 8e8; % [N/m]
#+end_src
Damping of the connection with Ground.
#+begin_src matlab
granite.c.x = 4.0e5; % [N/(m/s)]
granite.c.y = 1.1e5; % [N/(m/s)]
granite.c.z = 9.0e5; % [N/(m/s)]
#+end_src
Equilibrium position of the Cartesian joint.
#+begin_src matlab
granite.x0 = args.x0;
granite.y0 = args.y0;
granite.z0 = args.z0;
#+end_src
Z-offset for the initial position of the sample with respect to the granite top surface.
#+begin_src matlab
granite.sample_pos = 0.8; % [m]
#+end_src
** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
granite.K = [4e9; 3e8; 8e8]; % [N/m]
granite.C = [4.0e5; 1.1e5; 9.0e5]; % [N/(m/s)]
#+end_src
** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fgm');
granite.Deq = -Fgm'./granite.K;
else
granite.Deq = zeros(6,1);
end
#+end_src
** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
@ -423,15 +432,8 @@ The Simscape model of the Translation stage consist of:
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
end
#+end_src
@ -444,7 +446,6 @@ First, we initialize the =ty= structure.
ty = struct();
#+end_src
** Add Translation Stage Type
:PROPERTIES:
:UNNUMBERED: t
@ -459,11 +460,12 @@ First, we initialize the =ty= structure.
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
@ -506,28 +508,28 @@ Define the density of the materials as well as the geometry (STEP files).
ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
#+end_src
Stiffness of the stage.
** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
ty.k.ax = 5e8; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 5e7; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
#+end_src
Damping of the stage.
#+begin_src matlab
ty.c.ax = 70710; % [N/(m/s)]
ty.c.rad = 22360; % [N/(m/s)]
#+end_src
** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
Equilibrium position of the joints.
#+begin_src matlab
ty.x0_11 = args.x11;
ty.z0_11 = args.z11;
ty.x0_12 = args.x12;
ty.z0_12 = args.z12;
ty.x0_21 = args.x21;
ty.z0_21 = args.z21;
ty.x0_22 = args.x22;
ty.z0_22 = args.z22;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Ftym');
ty.Deq = -Ftym'./ty.K;
else
ty.Deq = zeros(6,1);
end
#+end_src
** Save the Structure
@ -584,19 +586,9 @@ The Simscape model of the Tilt stage is composed of:
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
#+end_src
@ -624,12 +616,12 @@ First, we initialize the =ry= structure.
ry.type = 2;
case 'modal-analysis'
ry.type = 3;
case 'init'
ry.type = 4;
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
@ -652,48 +644,44 @@ Properties of the Material and link to the geometry of the Tilt stage.
ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
#+end_src
Stiffness of the stage.
#+begin_src matlab
ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
ry.k.h = 1e8; % Stiffness in the direction of the guidance [N/m]
ry.k.rad = 1e8; % Stiffness in the top direction [N/m]
ry.k.rrad = 1e8; % Stiffness in the side direction [N/m]
#+end_src
Damping of the stage.
#+begin_src matlab
ry.c.tilt = 2.8e2;
ry.c.h = 2.8e4;
ry.c.rad = 2.8e4;
ry.c.rrad = 2.8e4;
#+end_src
Equilibrium position of the joints.
#+begin_src matlab
ry.x0_11 = args.x11;
ry.y0_11 = args.y11;
ry.z0_11 = args.z11;
ry.x0_12 = args.x12;
ry.y0_12 = args.y12;
ry.z0_12 = args.z12;
ry.x0_21 = args.x21;
ry.y0_21 = args.y21;
ry.z0_21 = args.z21;
ry.x0_22 = args.x22;
ry.y0_22 = args.y22;
ry.z0_22 = args.z22;
#+end_src
Z-Offset so that the center of rotation matches the sample center;
#+begin_src matlab
ry.z_offset = 0.58178; % [m]
#+end_src
#+begin_src matlab
ry.Ry_init = args.Ry_init; % [rad]
#+end_src
** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
#+end_src
** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fym');
ry.Deq = -Fym'./ry.K;
else
ry.Deq = zeros(6,1);
end
#+end_src
** Save the Structure
:PROPERTIES:
:UNNUMBERED: t
:END:
The =ty= structure is saved.
The =ry= structure is saved.
#+begin_src matlab
save('./mat/stages.mat', 'ry', '-append');
#+end_src
@ -739,12 +727,8 @@ The Simscape model of the Spindle is composed of:
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
end
#+end_src
@ -757,7 +741,6 @@ First, we initialize the =rz= structure.
rz = struct();
#+end_src
** Add Spindle Type
:PROPERTIES:
:UNNUMBERED: t
@ -772,13 +755,16 @@ First, we initialize the =rz= structure.
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry of the spindle.
#+begin_src matlab
% Spindle - Slip Ring
@ -794,29 +780,28 @@ Properties of the Material and link to the geometry of the spindle.
rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
#+end_src
Stiffness of the stage.
** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
rz.k.rot = 1e6; % TODO - Rotational Stiffness (Rz) [N*m/deg]
rz.k.tilt = 1e6; % Rotational Stiffness (Rx, Ry) [N*m/deg]
rz.k.ax = 2e9; % Axial Stiffness (Z) [N/m]
rz.k.rad = 7e8; % Radial Stiffness (X, Y) [N/m]
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
#+end_src
Damping of the stage.
#+begin_src matlab
rz.c.rot = 1.6e3;
rz.c.tilt = 1.6e3;
rz.c.ax = 7.1e4;
rz.c.rad = 4.2e4;
#+end_src
** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
Equilibrium position of the joints.
#+begin_src matlab
rz.x0 = args.x0;
rz.y0 = args.y0;
rz.z0 = args.z0;
rz.rx0 = args.rx0;
rz.ry0 = args.ry0;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fzm');
rz.Deq = -Fzm'./rz.K;
else
rz.Deq = zeros(6,1);
end
#+end_src
** Save the Structure
@ -864,7 +849,7 @@ The =rz= structure is saved.
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
@ -895,8 +880,8 @@ The =rz= structure is saved.
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
% Equilibrium position of each leg
args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
% Force that stiffness of each joint should apply at t=0
args.Foffset logical {mustBeNumericOrLogical} = true
end
#+end_src
@ -919,10 +904,14 @@ The =rz= structure is saved.
Equilibrium position of the each joint.
#+begin_src matlab
micro_hexapod.dLeq = args.dLeq;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fhm');
micro_hexapod.dLeq = -Fhm'./args.Ki;
else
micro_hexapod.dLeq = zeros(6,1);
end
#+end_src
** Add Type
:PROPERTIES:
:UNNUMBERED: t
@ -937,6 +926,8 @@ Equilibrium position of the each joint.
micro_hexapod.type = 2;
case 'modal-analysis'
micro_hexapod.type = 3;
case 'init'
micro_hexapod.type = 4;
end
#+end_src
@ -1017,10 +1008,11 @@ First, we initialize the =axisc= structure.
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
Properties of the Material and link to the geometry files.
#+begin_src matlab
% Structure
@ -1116,29 +1108,30 @@ First, we initialize the =mirror= structure.
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
We define the geometrical values.
#+begin_src matlab
mirror.h = 50; % Height of the mirror [mm]
mirror.h = 0.05; % Height of the mirror [m]
mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
mirror.thickness = 0.025; % Thickness of the plate supporting the sample [m]
mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
mirror.hole_rad = 0.120; % radius of the hole in the mirror [m]
mirror.support_rad = 100; % radius of the support plate [mm]
mirror.support_rad = 0.1; % radius of the support plate [m]
% point of interest offset in z (above the top surfave) [mm]
% point of interest offset in z (above the top surfave) [m]
switch args.type
case 'none'
mirror.jacobian = 200;
mirror.jacobian = 0.20;
case 'rigid'
mirror.jacobian = 200 - mirror.h;
mirror.jacobian = 0.20 - mirror.h;
end
mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m]
#+end_src
#+begin_src matlab
@ -1352,15 +1345,13 @@ The Simscape model of the sample environment is composed of:
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m]
args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m]
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible'
args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m]
args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m]
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
args.Foffset logical {mustBeNumericOrLogical} = true
end
#+end_src
@ -1385,40 +1376,46 @@ First, we initialize the =sample= structure.
sample.type = 1;
case 'flexible'
sample.type = 2;
case 'init'
sample.type = 3;
end
#+end_src
** Function content
** Material and Geometry
:PROPERTIES:
:UNNUMBERED: t
:END:
We define the geometrical parameters of the sample as well as its mass and position.
#+begin_src matlab
sample.radius = args.radius; % [m]
sample.height = args.height; % [m]
sample.mass = args.mass; % [kg]
sample.mass = args.mass; % [kg]
sample.offset = args.offset; % [m]
#+end_src
Stiffness of the sample fixation.
** Stiffness and Damping properties
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
sample.k.x = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.k.y = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.k.z = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.K = ones(3,1) * sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.C = 0.1 * sqrt(sample.K*sample.mass); % [N/(m/s)]
#+end_src
Damping of the sample fixation.
#+begin_src matlab
sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); % [N/(m/s)]
sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); % [N/(m/s)]
sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); % [N/(m/s)]
#+end_src
** Equilibrium position of the each joint.
:PROPERTIES:
:UNNUMBERED: t
:END:
Equilibrium position of the Cartesian joint corresponding to the sample fixation.
#+begin_src matlab
sample.x0 = args.x0; % [m]
sample.y0 = args.y0; % [m]
sample.z0 = args.z0; % [m]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fsm');
sample.Deq = -Fsm'./sample.K;
else
sample.Deq = zeros(3,1);
end
#+end_src
** Save the Structure
@ -1746,11 +1743,33 @@ The =controller= structure is saved.
switch args.Dn_type
case 'constant'
Dn = [args.Dn_pos, args.Dn_pos];
load('mat/stages.mat', 'nano_hexapod');
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
tx = args.Dn_pos(4);
ty = args.Dn_pos(5);
tz = args.Dn_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
Dnl = [Dnl, Dnl];
otherwise
warning('Dn_type is not set correctly');
end
Dn = struct('time', t, 'signals', struct('values', Dn));
Dnl = struct('time', t, 'signals', struct('values', Dnl));
#+end_src
** Save
@ -1759,7 +1778,7 @@ The =controller= structure is saved.
:END:
#+begin_src matlab
%% Save
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'Ts');
end
#+end_src
@ -1985,6 +2004,81 @@ We define some parameters that will be used in the algorithm.
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
#+end_src
* Initialize Position Errors
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializePosError.m
:header-args:matlab+: :comments none :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:initializePosError>>
** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = initializePosError(args)
% initializePosError - Initialize the position errors
%
% Syntax: [] = initializePosError(args)
%
% Inputs:
% - args -
#+end_src
** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.error logical {mustBeNumericOrLogical} = false
args.Dy (1,1) double {mustBeNumeric} = 0 % [m]
args.Ry (1,1) double {mustBeNumeric} = 0 % [m]
args.Rz (1,1) double {mustBeNumeric} = 0 % [m]
end
#+end_src
** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
First, we initialize the =pos_error= structure.
#+begin_src matlab
pos_error = struct();
#+end_src
** Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
if args.error
pos_error.type = 1;
else
pos_error.type = 0;
end
#+end_src
** Position Errors
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
pos_error.Dy = args.Dy;
pos_error.Ry = args.Ry;
pos_error.Rz = args.Rz;
#+end_src
** Save
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
save('mat/pos_error.mat', 'pos_error');
#+end_src
* Z-Axis Geophone
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeZAxisGeophone.m

BIN
simscape_subsystems/micro_hexapod_leg.slx
View File

Binary file not shown.

BIN
simscape_subsystems/micro_hexapod_leg_rigid.slx Normal file
View File

Binary file not shown.

BIN
simscape_subsystems/nano_hexapod_leg_rigid.slx Normal file
View File

Binary file not shown.

BIN
simscape_subsystems/nass_disturbances.slx
View File

Binary file not shown.

BIN
simscape_subsystems/nass_references.slx
View File

Binary file not shown.

2
src/computeReferencePose.m
View File

@ -70,7 +70,7 @@ function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
0 0 1 Dn(3) ;
0 0 0 1 ];
Rn(1:3, 1:3) = Rnx*Rny*Rnz;
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
%% Total Homogeneous transformation
WTr = Rty*Rry*Rrz*Rh*Rn;

27
src/initializeGranite.m
View File

@ -1,7 +1,8 @@
function [granite] = initializeGranite(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis'})} = 'flexible'
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
@ -19,23 +20,23 @@ switch args.type
granite.type = 2;
case 'modal-analysis'
granite.type = 3;
case 'init'
granite.type = 4;
end
granite.density = args.density; % [kg/m3]
granite.STEP = './STEPS/granite/granite.STEP';
granite.k.x = 4e9; % [N/m]
granite.k.y = 3e8; % [N/m]
granite.k.z = 8e8; % [N/m]
granite.c.x = 4.0e5; % [N/(m/s)]
granite.c.y = 1.1e5; % [N/(m/s)]
granite.c.z = 9.0e5; % [N/(m/s)]
granite.x0 = args.x0;
granite.y0 = args.y0;
granite.z0 = args.z0;
granite.sample_pos = 0.8; % [m]
granite.K = [4e9; 3e8; 8e8]; % [N/m]
granite.C = [4.0e5; 1.1e5; 9.0e5]; % [N/(m/s)]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fgm');
granite.Deq = -Fgm'./granite.K;
else
granite.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'granite', '-append');

4
src/initializeLoggingConfiguration.m
View File

@ -1,7 +1,7 @@
function [] = initializeLoggingConfiguration(args)
arguments
args.log char {mustBeMember(args.log,{'none', 'all'})} = 'none'
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
end
@ -12,6 +12,8 @@ switch args.log
conf_log.type = 0;
case 'all'
conf_log.type = 1;
case 'forces'
conf_log.type = 2;
end
conf_log.Ts = args.Ts;

15
src/initializeMicroHexapod.m
View File

@ -1,7 +1,7 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
@ -32,8 +32,8 @@ arguments
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
% Equilibrium position of each leg
args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
% Force that stiffness of each joint should apply at t=0
args.Foffset logical {mustBeNumericOrLogical} = true
end
micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
@ -47,7 +47,12 @@ micro_hexapod = computeJacobian(micro_hexapod);
micro_hexapod.Li = Li;
micro_hexapod.dLi = dLi;
micro_hexapod.dLeq = args.dLeq;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fhm');
micro_hexapod.dLeq = -Fhm'./args.Ki;
else
micro_hexapod.dLeq = zeros(6,1);
end
switch args.type
case 'none'
@ -58,6 +63,8 @@ switch args.type
micro_hexapod.type = 2;
case 'modal-analysis'
micro_hexapod.type = 3;
case 'init'
micro_hexapod.type = 4;
end
save('./mat/stages.mat', 'micro_hexapod', '-append');

16
src/initializeMirror.m
View File

@ -15,23 +15,23 @@ switch args.type
mirror.type = 1;
end
mirror.h = 50; % Height of the mirror [mm]
mirror.h = 0.05; % Height of the mirror [m]
mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
mirror.thickness = 0.025; % Thickness of the plate supporting the sample [m]
mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
mirror.hole_rad = 0.120; % radius of the hole in the mirror [m]
mirror.support_rad = 100; % radius of the support plate [mm]
mirror.support_rad = 0.1; % radius of the support plate [m]
% point of interest offset in z (above the top surfave) [mm]
% point of interest offset in z (above the top surfave) [m]
switch args.type
case 'none'
mirror.jacobian = 200;
mirror.jacobian = 0.20;
case 'rigid'
mirror.jacobian = 200 - mirror.h;
mirror.jacobian = 0.20 - mirror.h;
end
mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m]
mirror.density = 2400; % Density of the material [kg/m3]

28
src/initializePosError.m Normal file
View File

@ -0,0 +1,28 @@
function [] = initializePosError(args)
% initializePosError - Initialize the position errors
%
% Syntax: [] = initializePosError(args)
%
% Inputs:
% - args -
arguments
args.error logical {mustBeNumericOrLogical} = false
args.Dy (1,1) double {mustBeNumeric} = 0 % [m]
args.Ry (1,1) double {mustBeNumeric} = 0 % [m]
args.Rz (1,1) double {mustBeNumeric} = 0 % [m]
end
pos_error = struct();
if args.error
pos_error.type = 1;
else
pos_error.type = 0;
end
pos_error.Dy = args.Dy;
pos_error.Ry = args.Ry;
pos_error.Rz = args.Rz;
save('mat/pos_error.mat', 'pos_error');

24
src/initializeReferences.m
View File

@ -186,12 +186,34 @@ Dn = zeros(length(t), 6);
switch args.Dn_type
case 'constant'
Dn = [args.Dn_pos, args.Dn_pos];
load('mat/stages.mat', 'nano_hexapod');
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
tx = args.Dn_pos(4);
ty = args.Dn_pos(5);
tz = args.Dn_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1]*...
[ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)]*...
[1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
Dnl = [Dnl, Dnl];
otherwise
warning('Dn_type is not set correctly');
end
Dn = struct('time', t, 'signals', struct('values', Dn));
Dnl = struct('time', t, 'signals', struct('values', Dnl));
%% Save
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'Ts');
end

53
src/initializeRy.m
View File

@ -1,19 +1,9 @@
function [ry] = initializeRy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
ry = struct();
@ -27,6 +17,8 @@ switch args.type
ry.type = 2;
case 'modal-analysis'
ry.type = 3;
case 'init'
ry.type = 4;
end
% Ry - Guide for the tilt stage
@ -45,29 +37,18 @@ ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
ry.stage.density = 7800; % [kg/m3]
ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
ry.k.h = 1e8; % Stiffness in the direction of the guidance [N/m]
ry.k.rad = 1e8; % Stiffness in the top direction [N/m]
ry.k.rrad = 1e8; % Stiffness in the side direction [N/m]
ry.c.tilt = 2.8e2;
ry.c.h = 2.8e4;
ry.c.rad = 2.8e4;
ry.c.rrad = 2.8e4;
ry.x0_11 = args.x11;
ry.y0_11 = args.y11;
ry.z0_11 = args.z11;
ry.x0_12 = args.x12;
ry.y0_12 = args.y12;
ry.z0_12 = args.z12;
ry.x0_21 = args.x21;
ry.y0_21 = args.y21;
ry.z0_21 = args.z21;
ry.x0_22 = args.x22;
ry.y0_22 = args.y22;
ry.z0_22 = args.z22;
ry.z_offset = 0.58178; % [m]
ry.Ry_init = args.Ry_init; % [rad]
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fym');
ry.Deq = -Fym'./ry.K;
else
ry.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'ry', '-append');

32
src/initializeRz.m
View File

@ -1,12 +1,8 @@
function [rz] = initializeRz(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
end
rz = struct();
@ -20,6 +16,8 @@ switch args.type
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
% Spindle - Slip Ring
@ -34,20 +32,14 @@ rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
rz.stator.density = 7800; % [kg/m3]
rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
rz.k.rot = 1e6; % TODO - Rotational Stiffness (Rz) [N*m/deg]
rz.k.tilt = 1e6; % Rotational Stiffness (Rx, Ry) [N*m/deg]
rz.k.ax = 2e9; % Axial Stiffness (Z) [N/m]
rz.k.rad = 7e8; % Radial Stiffness (X, Y) [N/m]
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
rz.c.rot = 1.6e3;
rz.c.tilt = 1.6e3;
rz.c.ax = 7.1e4;
rz.c.rad = 4.2e4;
rz.x0 = args.x0;
rz.y0 = args.y0;
rz.z0 = args.z0;
rz.rx0 = args.rx0;
rz.ry0 = args.ry0;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fzm');
rz.Deq = -Fzm'./rz.K;
else
rz.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'rz', '-append');

38
src/initializeSample.m
View File

@ -1,15 +1,13 @@
function [sample] = initializeSample(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m]
args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m]
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible'
args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m]
args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m]
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
args.Foffset logical {mustBeNumericOrLogical} = true
end
sample = struct();
@ -21,23 +19,23 @@ switch args.type
sample.type = 1;
case 'flexible'
sample.type = 2;
case 'init'
sample.type = 3;
end
sample.radius = args.radius; % [m]
sample.height = args.height; % [m]
sample.mass = args.mass; % [kg]
sample.mass = args.mass; % [kg]
sample.offset = args.offset; % [m]
sample.k.x = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.k.y = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.k.z = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.K = ones(3,1) * sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.C = 0.1 * sqrt(sample.K*sample.mass); % [N/(m/s)]
sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); % [N/(m/s)]
sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); % [N/(m/s)]
sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); % [N/(m/s)]
sample.x0 = args.x0; % [m]
sample.y0 = args.y0; % [m]
sample.z0 = args.z0; % [m]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fsm');
sample.Deq = -Fsm'./sample.K;
else
sample.Deq = zeros(3,1);
end
save('./mat/stages.mat', 'sample', '-append');

34
src/initializeTy.m
View File

@ -1,15 +1,8 @@
function [ty] = initializeTy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
end
ty = struct();
@ -23,6 +16,8 @@ switch args.type
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
% Ty Granite frame
@ -61,19 +56,14 @@ ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
ty.rotor.density = 5400; % [kg/m3]
ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
ty.k.ax = 5e8; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 5e7; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
ty.c.ax = 70710; % [N/(m/s)]
ty.c.rad = 22360; % [N/(m/s)]
ty.x0_11 = args.x11;
ty.z0_11 = args.z11;
ty.x0_12 = args.x12;
ty.z0_12 = args.z12;
ty.x0_21 = args.x21;
ty.z0_21 = args.z21;
ty.x0_22 = args.x22;
ty.z0_22 = args.z22;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Ftym');
ty.Deq = -Ftym'./ty.K;
else
ty.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'ty', '-append');

20
src/inverseKinematicsHexapod.m
View File

@ -1,20 +0,0 @@
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end