Figure 1: Schematic of the Micro Station and the sources of disturbance
@@ -324,19 +310,19 @@ Because we cannot measure directly the perturbation forces, we have the measure This file is divided in the following sections:-
-
- Section 1: the simscape model used here is presented -
- Section 2: 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 -
- Section 3: the bode plot of those transfer functions are shown -
- Section 4: the measured PSD of the effect of the disturbances are shown -
- Section 5: from the model and the measured PSD, the PSD of the disturbance forces are computed -
- Section 6: with the computed PSD, the noise budget of the system is done +
- Section 1: the simscape model used here is presented +
- Section 2: 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 +
- Section 3: the bode plot of those transfer functions are shown +
- Section 4: the measured PSD of the effect of the disturbances are shown +
- Section 5: from the model and the measured PSD, the PSD of the disturbance forces are computed +
- Section 6: with the computed PSD, the noise budget of the system is done
-
1 Simscape Model
-
+
+
-1 Simscape Model
+@@ -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.
-
-
-
open('disturbances/matlab/sim_micro_station_disturbances.slx'); --
We load the configuration and we set a small StopTime.
-
@@ -367,72 +348,83 @@ We load the configuration and we set a small load('mat/conf_simscape.mat'); -set_param(conf_simscape, 'StopTime', '0.5'); +load('mat/conf_simulink.mat'); +set_param(conf_simulink, 'StopTime', '0.5');
StopTime.
We initialize all the stages.
-
initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); -initializeNanoHexapod(struct('actuator', 'piezo')); -initializeSample(struct('mass', 50)); +initializeGround(); +initializeGranite('type', 'modal-analysis'); +initializeTy(); +initializeRy(); +initializeRz(); +initializeMicroHexapod('type', 'modal-analysis'); +initializeAxisc('type', 'none'); +initializeMirror('type', 'none'); +initializeNanoHexapod('type', 'none'); +initializeSample('type', 'none');
-
2 Identification
-
+
+
-2 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.
%% Options for Linearized options = linearizeOptions; -options.SampleTime = 0; +options.SampleTime = 0; %% Name of the Simulink File -mdl = 'sim_micro_station_disturbances'; +mdl = 'nass_model';
%% Micro-Hexapod -% Input/Output definition -io(1) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion -io(2) = linio([mdl, '/Fty'], 1, 'input'); % Parasitic force Ty -io(3) = linio([mdl, '/Frz'], 1, 'input'); % Parasitic force Rz -io(4) = linio([mdl, '/Dgm'], 1, 'output'); % Absolute motion - Granite -io(5) = linio([mdl, '/Dhm'], 1, 'output'); % Absolute Motion - Hexapod -io(6) = linio([mdl, '/Vm'], 1, 'output'); % Relative Velocity hexapod/granite + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Dwz'); io_i = io_i + 1; % Vertical Ground Motion + io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Fty_z'); io_i = io_i + 1; % Parasitic force Ty + io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'Frz_z'); io_i = io_i + 1; % Parasitic force Rz + io(io_i) = linio([mdl, '/Micro-Station/Granite/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1; % Absolute motion - Granite + io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion - Hexapod + % io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Velocity hexapod/granite
% Run the linearization -G = linearize(mdl, io, 0); +G = linearize(mdl, io, 0); + +% We Take only the outputs corresponding to the vertical acceleration +G = G([3,9], :); % Input/Output names -G.InputName = {'Dw', 'Fty', 'Frz'}; -G.OutputName = {'Dgm', 'Dhm', 'Vm'}; +G.InputName = {'Dw', 'Fty', 'Frz'}; +G.OutputName = {'Agm', 'Ahm'}; + +% We integrate 1 time the output to have the velocity and we +% substract the absolute velocities to have the relative velocity +G = (1/s)*tf([-1, 1])*G; + +% Input/Output names +G.InputName = {'Dw', 'Fty', 'Frz'}; +G.OutputName = {'Vm'};
-
3 Sensitivity to Disturbances
-
+
+
3 Sensitivity to Disturbances
+
-
+
-
Figure 2: Sensitivity to Ground Motion (png, pdf)
@@ -440,7 +432,7 @@ G.OutputName = { +
Figure 3: Sensitivity to vertical forces applied by the Ty stage (png, pdf)
@@ -448,7 +440,7 @@ G.OutputName = { +
Figure 4: Sensitivity to vertical forces applied by the Rz stage (png, pdf)
@@ -456,11 +448,11 @@ G.OutputName = {
-
4 Power Spectral Density of the effect of the disturbances
-
+
+
4 Power Spectral Density of the effect of the disturbances
+- + 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:
-
@@ -473,10 +465,10 @@ Also, the Ground Motion is measured.
-
@@ -484,20 +476,20 @@ tyx = load(
-gm = load('./disturbances/mat/psd_gm.mat', 'f', 'psd_gm', 'psd_gv'); -rz = load('./disturbances/mat/pxsp_r.mat', 'f', 'pxsp_r'); -tyz = load('./disturbances/mat/pxz_ty_r.mat', 'f', 'pxz_ty_r'); -tyx = load('./disturbances/mat/pxe_ty_r.mat', 'f', 'pxe_ty_r'); +gm = load('./disturbances/mat/psd_gm.mat', 'f', 'psd_gm', 'psd_gv'); +rz = load('./disturbances/mat/pxsp_r.mat', 'f', 'pxsp_r'); +tyz = load('./disturbances/mat/pxz_ty_r.mat', 'f', 'pxz_ty_r'); +tyx = load('./disturbances/mat/pxe_ty_r.mat', 'f', 'pxe_ty_r');
gm.psd_rv = gm.psd_gm.*abs(squeeze(freqresp(G('Vm', 'Dw'), gm.f, 'Hz'))).^2; +gm.psd_rv = gm.psd_gm.*abs(squeeze(freqresp(G('Vm', 'Dw'), gm.f, 'Hz'))).^2;
-The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures 5 and 6. +The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures 5 and 6.
-The Cumulative Amplitude Spectrum of the relative motion is shown in figure 7. +The Cumulative Amplitude Spectrum of the relative motion is shown in figure 7.
-
+
-
Figure 5: Amplitude Spectral Density of the relative velocity of the hexapod with respect to the granite due to different sources of perturbation (png, pdf)
@@ -505,14 +497,14 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure +
Figure 6: Amplitude Spectral Density of the relative displacement of the hexapod with respect to the granite due to different sources of perturbation (png, pdf)
+
-
Figure 7: Cumulative Amplitude Spectrum of the relative motion due to different sources of perturbation (png, pdf)
@@ -520,25 +512,25 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure
-
5 Compute the Power Spectral Density of the disturbance force
-
+
+
5 Compute the Power Spectral Density 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 3) and from the measured PSD of the relative motion (section 4), 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 3) and from the measured PSD of the relative motion (section 4), we can compute the PSD of the disturbance force.
-
-rz.psd_f = rz.pxsp_r./abs(squeeze(freqresp(G('Vm', 'Frz'), rz.f, 'Hz'))).^2; -tyz.psd_f = tyz.pxz_ty_r./abs(squeeze(freqresp(G('Vm', 'Fty'), tyz.f, 'Hz'))).^2; +rz.psd_f = rz.pxsp_r./abs(squeeze(freqresp(G('Vm', 'Frz'), rz.f, 'Hz'))).^2; +tyz.psd_f = tyz.pxz_ty_r./abs(squeeze(freqresp(G('Vm', 'Fty'), tyz.f, 'Hz'))).^2;
+
Figure 8: Amplitude Spectral Density of the disturbance force (png, pdf)
@@ -546,11 +538,11 @@ tyz.psd_f = tyz.pxz_ty_r./abs -6 Noise Budget
-
+
+
6 Noise Budget
+@@ -559,7 +551,7 @@ We should verify that this is coherent with the measurements.
-
+
Figure 9: Computed Effect of the disturbances on the relative displacement hexapod/granite (png, pdf)
@@ -567,7 +559,7 @@ We should verify that this is coherent with the measurements. -
+
-
-
7 Save
-
+>
#+end_src
@@ -189,6 +195,9 @@ This file is divided in the following sections:
#+end_src
* Simscape Model
+:PROPERTIES:
+:CUSTOM_ID: Simscape-Model
+:END:
<>
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:
<>
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:
<>
#+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:
<>
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:
<>
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:
<>
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
diff --git a/disturbances/mat/dist_psd.mat b/disturbances/mat/dist_psd.mat
index 56b78fa..4ab206e 100644
Binary files a/disturbances/mat/dist_psd.mat and b/disturbances/mat/dist_psd.mat differ
diff --git a/functions/index.org b/functions/index.org
index 82e3e21..024e30a 100644
--- a/functions/index.org
+++ b/functions/index.org
@@ -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:
-<>
-
-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;
diff --git a/kinematics/index.org b/kinematics/index.org
index 760f62f..86775a5 100644
--- a/kinematics/index.org
+++ b/kinematics/index.org
@@ -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:
diff --git a/mat/Foffset.mat b/mat/Foffset.mat
new file mode 100644
index 0000000..9712f2b
Binary files /dev/null and b/mat/Foffset.mat differ
diff --git a/mat/conf_log.mat b/mat/conf_log.mat
index f67282c..0ecffe7 100644
Binary files a/mat/conf_log.mat and b/mat/conf_log.mat differ
diff --git a/mat/conf_simscape.mat b/mat/conf_simscape.mat
index 47068e1..ee26a2e 100644
Binary files a/mat/conf_simscape.mat and b/mat/conf_simscape.mat differ
diff --git a/mat/controller.mat b/mat/controller.mat
index dfdcf66..bb6fbd2 100644
Binary files a/mat/controller.mat and b/mat/controller.mat differ
diff --git a/mat/controllers.mat b/mat/controllers.mat
index b814c47..7dfdb8d 100644
Binary files a/mat/controllers.mat and b/mat/controllers.mat differ
diff --git a/mat/nass_disturbances.mat b/mat/nass_disturbances.mat
index 3940d35..8876c22 100644
Binary files a/mat/nass_disturbances.mat and b/mat/nass_disturbances.mat differ
diff --git a/mat/nass_references.mat b/mat/nass_references.mat
index aae6b59..3227b24 100644
Binary files a/mat/nass_references.mat and b/mat/nass_references.mat differ
diff --git a/mat/pos_error.mat b/mat/pos_error.mat
new file mode 100644
index 0000000..3f92923
Binary files /dev/null and b/mat/pos_error.mat differ
diff --git a/mat/stages.mat b/mat/stages.mat
index 87ca2a4..ec280fc 100644
Binary files a/mat/stages.mat and b/mat/stages.mat differ
diff --git a/positioning_error/index.org b/positioning_error/index.org
index bf4f0ff..81ae369 100644
--- a/positioning_error/index.org
+++ b/positioning_error/index.org
@@ -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
diff --git a/positioning_error/matlab/sim_nano_station_metrology.slx b/positioning_error/matlab/sim_nano_station_metrology.slx
deleted file mode 100644
index db202f1..0000000
Binary files a/positioning_error/matlab/sim_nano_station_metrology.slx and /dev/null differ
diff --git a/simscape/nass_model.slx b/simscape/nass_model.slx
index cae3d08..f4a0d7b 100644
Binary files a/simscape/nass_model.slx and b/simscape/nass_model.slx differ
diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org
index 42b6668..cb7a064 100644
--- a/simscape_subsystems/index.org
+++ b/simscape_subsystems/index.org
@@ -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:
+<>
+
+** 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
diff --git a/simscape_subsystems/micro_hexapod_leg.slx b/simscape_subsystems/micro_hexapod_leg.slx
index 4d88953..2cd6419 100644
Binary files a/simscape_subsystems/micro_hexapod_leg.slx and b/simscape_subsystems/micro_hexapod_leg.slx differ
diff --git a/simscape_subsystems/micro_hexapod_leg_rigid.slx b/simscape_subsystems/micro_hexapod_leg_rigid.slx
new file mode 100644
index 0000000..b1769e1
Binary files /dev/null and b/simscape_subsystems/micro_hexapod_leg_rigid.slx differ
diff --git a/simscape_subsystems/nano_hexapod_leg_rigid.slx b/simscape_subsystems/nano_hexapod_leg_rigid.slx
new file mode 100644
index 0000000..6534e2d
Binary files /dev/null and b/simscape_subsystems/nano_hexapod_leg_rigid.slx differ
diff --git a/simscape_subsystems/nass_disturbances.slx b/simscape_subsystems/nass_disturbances.slx
index 796636e..87804bd 100644
Binary files a/simscape_subsystems/nass_disturbances.slx and b/simscape_subsystems/nass_disturbances.slx differ
diff --git a/simscape_subsystems/nass_references.slx b/simscape_subsystems/nass_references.slx
index 81e1995..055b9cf 100644
Binary files a/simscape_subsystems/nass_references.slx and b/simscape_subsystems/nass_references.slx differ
diff --git a/src/computeReferencePose.m b/src/computeReferencePose.m
index 948a3a4..ea8de43 100644
--- a/src/computeReferencePose.m
+++ b/src/computeReferencePose.m
@@ -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;
diff --git a/src/initializeGranite.m b/src/initializeGranite.m
index a4623d4..21c21fa 100644
--- a/src/initializeGranite.m
+++ b/src/initializeGranite.m
@@ -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');
diff --git a/src/initializeLoggingConfiguration.m b/src/initializeLoggingConfiguration.m
index 76b6c06..5d46e58 100644
--- a/src/initializeLoggingConfiguration.m
+++ b/src/initializeLoggingConfiguration.m
@@ -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;
diff --git a/src/initializeMicroHexapod.m b/src/initializeMicroHexapod.m
index a0b3a25..9204995 100644
--- a/src/initializeMicroHexapod.m
+++ b/src/initializeMicroHexapod.m
@@ -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');
diff --git a/src/initializeMirror.m b/src/initializeMirror.m
index 5c3ae2c..3ff845c 100644
--- a/src/initializeMirror.m
+++ b/src/initializeMirror.m
@@ -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]
diff --git a/src/initializePosError.m b/src/initializePosError.m
new file mode 100644
index 0000000..b0b12ec
--- /dev/null
+++ b/src/initializePosError.m
@@ -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');
diff --git a/src/initializeReferences.m b/src/initializeReferences.m
index efc9e30..ea313e4 100644
--- a/src/initializeReferences.m
+++ b/src/initializeReferences.m
@@ -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
diff --git a/src/initializeRy.m b/src/initializeRy.m
index 8da87f1..34aa7e9 100644
--- a/src/initializeRy.m
+++ b/src/initializeRy.m
@@ -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');
diff --git a/src/initializeRz.m b/src/initializeRz.m
index 6b8922a..6e89a17 100644
--- a/src/initializeRz.m
+++ b/src/initializeRz.m
@@ -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');
diff --git a/src/initializeSample.m b/src/initializeSample.m
index 6dd558a..e6ddb74 100644
--- a/src/initializeSample.m
+++ b/src/initializeSample.m
@@ -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');
diff --git a/src/initializeTy.m b/src/initializeTy.m
index 8209837..5dd580f 100644
--- a/src/initializeTy.m
+++ b/src/initializeTy.m
@@ -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');
diff --git a/src/inverseKinematicsHexapod.m b/src/inverseKinematicsHexapod.m
deleted file mode 100644
index d8b1a90..0000000
--- a/src/inverseKinematicsHexapod.m
+++ /dev/null
@@ -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
+
@@ -599,8 +591,7 @@ save(
-
diff --git a/disturbances/index.org b/disturbances/index.org
index 1c04d3c..51ff49d 100644
--- a/disturbances/index.org
+++ b/disturbances/index.org
@@ -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)
<7 Save
+The PSD of the disturbance force are now saved for further analysis (the mat file is accessible here).
-
dist_f = struct(); +dist_f = struct(); dist_f.f = gm.f; % Frequency Vector [Hz] -dist_f.psd_gm = gm.psd_gm; % Power Spectral Density of the Ground Motion [m^2/Hz] -dist_f.psd_ty = tyz.psd_f; % Power Spectral Density of the force induced by the Ty stage in the Z direction [N^2/Hz] -dist_f.psd_rz = rz.psd_f; % Power Spectral Density of the force induced by the Rz stage in the Z direction [N^2/Hz] +dist_f.psd_gm = gm.psd_gm; % Power Spectral Density of the Ground Motion [m^2/Hz] +dist_f.psd_ty = tyz.psd_f; % Power Spectral Density of the force induced by the Ty stage in the Z direction [N^2/Hz] +dist_f.psd_rz = rz.psd_f; % Power Spectral Density of the force induced by the Rz stage in the Z direction [N^2/Hz] -save('./disturbances/mat/dist_psd.mat', 'dist_f'); +save('./disturbances/mat/dist_psd.mat', 'dist_f');
Created: 2019-12-13 ven. 16:39
- +Created: 2020-02-18 mar. 17:44