Remove badly tangled files

This commit is contained in:
Thomas Dehaeze 2020-02-25 18:31:33 +01:00
parent e3e6043810
commit f1dd11c3a4
9 changed files with 2 additions and 1313 deletions

View File

@ -3383,7 +3383,7 @@ Window used for =pwelch= function.
* Useful Functions
** prepareLinearizeIdentification
:PROPERTIES:
:header-args:matlab+: :tangle src/prepareLinearizeIdentification.m
:header-args:matlab+: :tangle ../src/prepareLinearizeIdentification.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:prepareLinearizeIdentification>>
@ -3455,7 +3455,7 @@ We do not need to log any signal.
** prepareTomographyExperiment
:PROPERTIES:
:header-args:matlab+: :tangle src/prepareTomographyExperiment.m
:header-args:matlab+: :tangle ../src/prepareTomographyExperiment.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:prepareTomographyExperiment>>

View File

@ -41,249 +41,6 @@
#+PROPERTY: header-args:latex+ :output-dir figs
:END:
* TODO computePsdDispl
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computePsdDispl.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computePsdDispl>>
This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].
#+begin_src matlab
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
i_init = find(sys_data.time > t_init, 1);
han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
Fs = 1/sys_data.time(2);
[pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
[pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
[pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
[prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
[pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
[prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
psd_object = struct(...
'f', f, ...
'dx', pdx, ...
'dy', pdy, ...
'dz', pdz, ...
'rx', prx, ...
'ry', pry, ...
'rz', prz);
end
#+end_src
* TODO computeSetpoint
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeSetpoint.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computeSetpoint>>
This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].
#+begin_src matlab
function setpoint = computeSetpoint(ty, ry, rz)
%%
setpoint = zeros(6, 1);
%% Ty
Ty = [1 0 0 0 ;
0 1 0 ty ;
0 0 1 0 ;
0 0 0 1 ];
% Tyinv = [1 0 0 0 ;
% 0 1 0 -ty ;
% 0 0 1 0 ;
% 0 0 0 1 ];
%% Ry
Ry = [ cos(ry) 0 sin(ry) 0 ;
0 1 0 0 ;
-sin(ry) 0 cos(ry) 0 ;
0 0 0 1 ];
% TMry = Ty*Ry*Tyinv;
%% Rz
Rz = [cos(rz) -sin(rz) 0 0 ;
sin(rz) cos(rz) 0 0 ;
0 0 1 0 ;
0 0 0 1 ];
% TMrz = Ty*TMry*Rz*TMry'*Tyinv;
%% All stages
% TM = TMrz*TMry*Ty;
TM = Ty*Ry*Rz;
[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
setpoint(1:3) = TM(1:3, 4);
setpoint(4:6) = [thetax, thetay, thetaz];
%% Custom Functions
function [thetax, thetay, thetaz] = RM2angle(R)
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
thetay = -asin(R(3, 1));
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
else
thetaz = 0;
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
thetay = pi/2;
thetax = thetaz + atan2(R(1, 2), R(1, 3));
else
thetay = -pi/2;
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
end
end
end
end
#+end_src
* TODO converErrorBasis
:PROPERTIES:
:header-args:matlab+: :tangle ../src/converErrorBasis.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:converErrorBasis>>
This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
#+begin_src matlab
function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
% convertErrorBasis -
%
% Syntax: convertErrorBasis(p_error, ty, ry, rz)
%
% Inputs:
% - p_error - Position error of the sample w.r.t. the granite [m, rad]
% - ty - Measured translation of the Ty stage [m]
% - ry - Measured rotation of the Ry stage [rad]
% - rz - Measured rotation of the Rz stage [rad]
%
% Outputs:
% - P_nass - Position error of the sample w.r.t. the NASS base [m]
% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
%
% Example:
%
%% If line vector => column vector
if size(pos, 2) == 6
pos = pos';
end
if size(setpoint, 2) == 6
setpoint = setpoint';
end
%% Position of the sample in the frame fixed to the Granite
P_granite = [pos(1:3); 1]; % Position [m]
R_granite = [setpoint(1:3); 1]; % Reference [m]
%% Transformation matrices of the stages
% T-y
TMty = [1 0 0 0 ;
0 1 0 ty ;
0 0 1 0 ;
0 0 0 1 ];
% R-y
TMry = [ cos(ry) 0 sin(ry) 0 ;
0 1 0 0 ;
-sin(ry) 0 cos(ry) 0 ;
0 0 0 1 ];
% R-z
TMrz = [cos(rz) -sin(rz) 0 0 ;
sin(rz) cos(rz) 0 0 ;
0 0 1 0 ;
0 0 0 1 ];
%% Compute Point coordinates in the new reference fixed to the NASS base
% P_nass = TMrz*TMry*TMty*P_granite;
P_nass = TMrz\TMry\TMty\P_granite;
R_nass = TMrz\TMry\TMty\R_granite;
dx = R_nass(1)-P_nass(1);
dy = R_nass(2)-P_nass(2);
dz = R_nass(3)-P_nass(3);
%% Compute new basis vectors linked to the NASS base
% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
% ux_nass = ux_nass(1:3);
% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
% uy_nass = uy_nass(1:3);
% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
% uz_nass = uz_nass(1:3);
ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
ux_nass = ux_nass(1:3);
uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
uy_nass = uy_nass(1:3);
uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
uz_nass = uz_nass(1:3);
%% Rotations error w.r.t. granite Frame
% Rotations error w.r.t. granite Frame
rx_nass = pos(4);
ry_nass = pos(5);
rz_nass = pos(6);
% Rotation matrices of the Sample w.r.t. the Granite
Mrx_error = [1 0 0 ;
0 cos(-rx_nass) -sin(-rx_nass) ;
0 sin(-rx_nass) cos(-rx_nass)];
Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
0 1 0 ;
-sin(-ry_nass) 0 cos(-ry_nass)];
Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
sin(-rz_nass) cos(-rz_nass) 0 ;
0 0 1];
% Rotation matrix of the Sample w.r.t. the Granite
Mr_error = Mrz_error*Mry_error*Mrx_error;
%% Use matrix to solve
R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
[thetax, thetay, thetaz] = RM2angle(R);
error_nass = [dx; dy; dz; thetax; thetay; thetaz];
%% Custom Functions
function [thetax, thetay, thetaz] = RM2angle(R)
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
thetay = -asin(R(3, 1));
% thetaybis = pi-thetay;
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
% thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
% thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
else
thetaz = 0;
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
thetay = pi/2;
thetax = thetaz + atan2(R(1, 2), R(1, 3));
else
thetay = -pi/2;
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
end
end
end
end
#+end_src
* computeReferencePose
:PROPERTIES:
:header-args:matlab+: :tangle ../src/computeReferencePose.m

View File

@ -1,398 +0,0 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Simscape Model
% The simulink file for the identification is =sim_micro_station_id.slx=.
open('identification/matlab/sim_micro_station_id.slx')
% We load the configuration and we set a small =StopTime=.
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
% We initialize all the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);
% Compute the transfer functions
% We first define some parameters for the identification.
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_micro_station_id';
%% Micro-Hexapod
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/Fm_ext'],1,'openinput');
io(2) = linio([mdl, '/Micro-Station/Fg_ext'],1,'openinput');
io(3) = linio([mdl, '/Micro-Station/Dm_inertial'],1,'output');
io(4) = linio([mdl, '/Micro-Station/Ty_inertial'],1,'output');
io(5) = linio([mdl, '/Micro-Station/Ry_inertial'],1,'output');
io(6) = linio([mdl, '/Micro-Station/Dg_inertial'],1,'output');
% Run the linearization
G_ms = linearize(mdl, io, 0);
% Input/Output names
G_ms.InputName = {'Fmx', 'Fmy', 'Fmz',...
'Fgx', 'Fgy', 'Fgz'};
G_ms.OutputName = {'Dmx', 'Dmy', 'Dmz', ...
'Tyx', 'Tyy', 'Tyz', ...
'Ryx', 'Ryy', 'Ryz', ...
'Dgx', 'Dgy', 'Dgz'};
%% Save the obtained transfer functions
save('./mat/id_micro_station.mat', 'G_ms');
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Simscape Model
% The simulink file for the analysis is =sim_micro_station_modal_analysis.slx=.
open('identification/matlab/sim_micro_station_modal_analysis.slx')
% We load the configuration and we set a small =StopTime=.
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5');
% We initialize all the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 50);
% Identification
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'sim_micro_station_modal_analysis';
%% Micro-Hexapod
% Input/Output definition
io(1) = linio([mdl, '/Micro-Station/F_hammer'],1,'openinput');
io(2) = linio([mdl, '/Micro-Station/acc9'],1,'output');
io(3) = linio([mdl, '/Micro-Station/acc10'],1,'output');
io(4) = linio([mdl, '/Micro-Station/acc11'],1,'output');
io(5) = linio([mdl, '/Micro-Station/acc12'],1,'output');
% Run the linearization
G_ms = linearize(mdl, io, 0);
% Input/Output names
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
G_ms.OutputName = {'x9', 'y9', 'z9', ...
'x10', 'y10', 'z10', ...
'x11', 'y11', 'z11', ...
'x12', 'y12', 'z12'};
% Plot Results
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(G_ms('x9', 'Fx'), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
% Compare with measurements
load('../meas/modal-analysis/mat/frf_coh_matrices.mat', 'FRFs', 'COHs', 'freqs');
dirs = {'x', 'y', 'z'};
n_acc = 9;
n_dir = 1; % x, y, z
n_exc = 1; % x, y, z
figure;
hold on;
plot(freqs, abs(squeeze(FRFs(3*(n_acc-1) + n_dir, n_exc, :)))./((2*pi*freqs).^2)');
plot(freqs, abs(squeeze(freqresp(G_ms([dirs{n_dir}, num2str(n_acc)], ['F', dirs{n_dir}]), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Prepare the Simulation
open('nass_model.slx')
% We load the configuration.
load('mat/conf_simulink.mat');
% We set a small =StopTime=.
set_param(conf_simulink, 'StopTime', '0.5');
% We initialize all the stages.
initializeGround( 'type', 'rigid');
initializeGranite( 'type', 'modal-analysis');
initializeTy( 'type', 'modal-analysis');
initializeRy( 'type', 'modal-analysis');
initializeRz( 'type', 'modal-analysis');
initializeMicroHexapod('type', 'modal-analysis');
initializeAxisc( 'type', 'flexible');
initializeMirror( 'type', 'none');
initializeNanoHexapod( 'type', 'none');
initializeSample( 'type', 'none');
initializeController( 'type', 'open-loop');
initializeLoggingConfiguration('log', 'none');
initializeReferences();
initializeDisturbances('enable', false);
% Estimate the position of the CoM of each solid and compare with the one took for the Measurement Analysis
% Thanks to the [[https://fr.mathworks.com/help/physmod/sm/ref/inertiasensor.html][Inertia Sensor]] simscape block, it is possible to estimate the position of the Center of Mass of a solid body with respect to a defined frame.
sim('nass_model')
% Create a frame at the CoM of each solid body
% Now we use one =inertiasensor= block connected on each solid body that measured the center of mass of this solid with respect to the same connected frame.
% We do that in order to position an accelerometer on the Simscape model at this particular point.
open('identification/matlab/sim_micro_station_com_estimation.slx')
sim('sim_micro_station_com_estimation')
% #+RESULTS:
% | | granite bot | granite top | ty | ry | rz | hexa |
% |--------+-------------+-------------+-------+--------+-------+-------|
% | X [mm] | 0.0 | 51.7 | 0.9 | -0.1 | 0.0 | -0.0 |
% | Y [mm] | 0.0 | 753.2 | 0.7 | 5.2 | -0.0 | 0.1 |
% | Z [mm] | -250.0 | 22.9 | -17.1 | -146.5 | -23.2 | -47.1 |
% We now same this for further use:
granite_bot_com = granite_bot_com.Data(end, :)';
granite_top_com = granite_top_com.Data(end, :)';
ty_com = ty_com.Data(end, :)';
ry_com = ry_com.Data(end, :)';
rz_com = rz_com.Data(end, :)';
hexa_com = hexa_com.Data(end, :)';
save('mat/solids_com.mat', 'granite_bot_com', 'granite_top_com', 'ty_com', 'ry_com', 'rz_com', 'hexa_com');
% Identification of the dynamics of the Simscape Model
% We now use a new Simscape Model where 6DoF inertial sensors are located at the Center of Mass of each solid body.
% load('mat/solids_com.mat', 'granite_bot_com', 'granite_top_com', 'ty_com', 'ry_com', 'rz_com', 'hexa_com');
open('nass_model.slx')
% We use the =linearize= function in order to estimate the dynamics from forces applied on the Translation stage at the same position used for the real modal analysis to the inertial sensors.
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Granite/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Modal Analysis/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
% Run the linearization
G_ms = linearize(mdl, io, 0);
%% Input/Output definition
clear io; io_i = 1;
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
% The output of =G_ms= is the acceleration of each solid body.
% In order to obtain a displacement, we divide the obtained transfer function by $1/s^{2}$;
G_ms = G_ms/s^2;
% Compare with measurements
% We now load the Frequency Response Functions measurements during the Modal Analysis (accessible [[file:../../meas/modal-analysis/index.org][here]]).
load('../meas/modal-analysis/mat/frf_coh_matrices.mat', 'freqs');
load('../meas/modal-analysis/mat/frf_com.mat', 'FRFs_CoM');
% We then compare the measurements with the identified transfer functions using the Simscape Model.
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'}
n_stg = 3;
n_dir = 6; % x, y, z, Rx, Ry, Rz
n_exc = 2; % x, y, z
f = logspace(0, 3, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg-1) + n_dir, n_exc, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
hold off;
xlim([1, 200]);
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'gtop', 'ty', 'ry', 'rz', 'hexa'}
f = logspace(1, 3, 1000);
figure;
for n_stg = 1:2
for n_dir = 1:3
subplot(3, 2, (n_dir-1)*2 + n_stg);
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
if n_dir == 3
xlabel('Frequency [Hz]');
end
hold off;
xlim([10, 1000]);
ylim([1e-12, 1e-6]);
end
end
% #+NAME: fig:identification_comp_bot_stages
% #+CAPTION: caption ([[./figs/identification_comp_bot_stages.png][png]], [[./figs/identification_comp_bot_stages.pdf][pdf]])
% [[file:figs/identification_comp_bot_stages.png]]
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'ry', 'rz', 'hexa'}
f = logspace(1, 3, 1000);
figure;
for n_stg = 1:2
for n_dir = 1:3
subplot(3, 2, (n_dir-1)*2 + n_stg);
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg+2) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
if n_dir == 3
xlabel('Frequency [Hz]');
end
hold off;
xlim([10, 1000]);
ylim([1e-12, 1e-6]);
end
end
% #+NAME: fig:identification_comp_mid_stages
% #+CAPTION: caption ([[./figs/identification_comp_mid_stages.png][png]], [[./figs/identification_comp_mid_stages.pdf][pdf]])
% [[file:figs/identification_comp_mid_stages.png]]
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
stages = {'hexa'}
f = logspace(1, 3, 1000);
figure;
for n_stg = 1
for n_dir = 1:3
subplot(3, 1, (n_dir-1) + n_stg);
title(['F ', dirs{n_dir}, ' to ', stages{n_stg}, ' ', dirs{n_dir}]);
hold on;
plot(freqs, abs(squeeze(FRFs_CoM(6*(n_stg+4) + n_dir, n_dir, :)))./((2*pi*freqs).^2)');
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_dir}]), f, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]');
if n_dir == 3
xlabel('Frequency [Hz]');
end
hold off;
xlim([10, 1000]);
ylim([1e-12, 1e-6]);
end
end

View File

@ -1,407 +0,0 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Simscape Model
% <<sec:simscape_model>>
open('nass_model.slx');
% We load the shared simulink configuration and we set the =StopTime=.
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '5');
% We first initialize all the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 1);
% We initialize the reference path for all the stages.
% All stage is set to its zero position except the Spindle which is rotating at 60rpm.
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
% Simulation Setup
% And we initialize the disturbances to be equal to zero.
initializeDisturbances(...
'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
tomo_align_no_dist = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'tomo_align_no_dist');
t = tomo_align_no_dist.t;
MTr = tomo_align_no_dist.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_tomo_without_dist_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_without_dist_trans.png][png]], [[./figs/exp_tomo_without_dist_trans.pdf][pdf]])
% [[file:figs/exp_tomo_without_dist_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% Simulation Setup
% We now activate the disturbances.
initializeDisturbances(...
'Dwx', true, ... % Ground Motion - X direction
'Dwy', true, ... % Ground Motion - Y direction
'Dwz', true, ... % Ground Motion - Z direction
'Fty_x', true, ... % Translation Stage - X direction
'Fty_z', true, ... % Translation Stage - Z direction
'Frz_z', true ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
tomo_align_dist = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_align_dist', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'tomo_align_dist');
t = tomo_align_dist.t;
MTr = tomo_align_dist.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_tomo_dist_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. the granite when performing tomography experiment with disturbances ([[./figs/exp_tomo_dist_trans.png][png]], [[./figs/exp_tomo_dist_trans.pdf][pdf]])
% [[file:figs/exp_tomo_dist_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% Simulation Setup
% We first set the wanted translation of the Micro Hexapod.
P_micro_hexapod = [0.01; 0; 0]; % [m]
% We initialize the reference path.
initializeReferences('Dh_pos', [P_micro_hexapod; 0; 0; 0], 'Rz_type', 'rotating', 'Rz_period', 1);
% We initialize the stages.
initializeMicroHexapod('AP', P_micro_hexapod);
% And we initialize the disturbances to zero.
initializeDisturbances(...
'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
tomo_not_align = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'tomo_not_align', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'tomo_not_align');
t = tomo_not_align.t;
MTr = tomo_not_align.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_tomo_offset_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_tomo_offset_trans.png][png]], [[./figs/exp_tomo_offset_trans.pdf][pdf]])
% [[file:figs/exp_tomo_offset_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% Simulation Setup
% We set the reference path.
initializeReferences('Dy_type', 'triangular', 'Dy_amplitude', 10e-3, 'Dy_period', 1);
% We initialize the stages.
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', 'piezo');
initializeSample('mass', 1);
% And we initialize the disturbances to zero.
initializeDisturbances(...
'Dwx', false, ... % Ground Motion - X direction
'Dwy', false, ... % Ground Motion - Y direction
'Dwz', false, ... % Ground Motion - Z direction
'Fty_x', false, ... % Translation Stage - X direction
'Fty_z', false, ... % Translation Stage - Z direction
'Frz_z', false ... % Spindle - Z direction
);
% We simulate the model.
sim('nass_model');
% And we save the obtained data.
ty_scan = struct('t', t, 'MTr', MTr);
save('experiment_tomography/mat/experiment.mat', 'ty_scan', '-append');
% Analysis
load('experiment_tomography/mat/experiment.mat', 'ty_scan');
t = ty_scan.t;
MTr = ty_scan.MTr;
Edx = squeeze(MTr(1, 4, :));
Edy = squeeze(MTr(2, 4, :));
Edz = squeeze(MTr(3, 4, :));
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
Ery = atan2( squeeze(MTr(1, 3, :)), squeeze(sqrt(MTr(1, 1, :).^2 + MTr(1, 2, :).^2)));
Erx = atan2(-squeeze(MTr(2, 3, :))./cos(Ery), squeeze(MTr(3, 3, :))./cos(Ery));
Erz = atan2(-squeeze(MTr(1, 2, :))./cos(Ery), squeeze(MTr(1, 1, :))./cos(Ery));
figure;
ax1 = subplot(1, 3, 1);
plot(t, Edx, 'DisplayName', '$\epsilon_{x}$')
ylabel('Displacement [m]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Edy, 'DisplayName', '$\epsilon_{y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Edz, 'DisplayName', '$\epsilon_{z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);
% #+NAME: fig:exp_ty_scan_trans
% #+CAPTION: X-Y-Z translation of the sample w.r.t. granite when performing tomography experiment with no disturbances ([[./figs/exp_ty_scan_trans.png][png]], [[./figs/exp_ty_scan_trans.pdf][pdf]])
% [[file:figs/exp_ty_scan_trans.png]]
figure;
ax1 = subplot(1, 3, 1);
plot(t, Erx, 'DisplayName', '$\epsilon_{\theta x}$')
ylabel('Rotation [rad]');
legend('location', 'northeast');
ax2 = subplot(1, 3, 2);
plot(t, Ery, 'DisplayName', '$\epsilon_{\theta y}$')
xlabel('Time [s]');
legend('location', 'northeast');
ax3 = subplot(1, 3, 3);
plot(t, Erz, 'DisplayName', '$\epsilon_{\theta z}$')
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3],'x');
xlim([2, inf]);

View File

@ -1,27 +0,0 @@
function [] = prepareLinearizeIdentification(args)
arguments
args.nass_actuator char {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
args.sample_mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
end
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', args.nass_actuator);
initializeSample('mass', args.sample_mass);
initializeReferences();
initializeDisturbances('enable', false);
initializeController('type', 'open-loop');
initializeSimscapeConfiguration('gravity', true);
initializeLoggingConfiguration('log', 'none');

View File

@ -1,29 +0,0 @@
function [] = prepareTomographyExperiment(args)
arguments
args.nass_actuator char {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo'
args.sample_mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
end
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
initializeNanoHexapod('actuator', args.nass_actuator);
initializeSample('mass', args.sample_mass);
initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Rz_period);
initializeDisturbances();
initializeController('type', 'open-loop');
initializeSimscapeConfiguration('gravity', true);
initializeLoggingConfiguration('log', 'all');

View File

@ -1,23 +0,0 @@
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
i_init = find(sys_data.time > t_init, 1);
han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
Fs = 1/sys_data.time(2);
[pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
[pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
[pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
[prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
[pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
[prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
psd_object = struct(...
'f', f, ...
'dx', pdx, ...
'dy', pdy, ...
'dz', pdz, ...
'rx', prx, ...
'ry', pry, ...
'rz', prz);
end

View File

@ -1,59 +0,0 @@
function setpoint = computeSetpoint(ty, ry, rz)
%%
setpoint = zeros(6, 1);
%% Ty
Ty = [1 0 0 0 ;
0 1 0 ty ;
0 0 1 0 ;
0 0 0 1 ];
% Tyinv = [1 0 0 0 ;
% 0 1 0 -ty ;
% 0 0 1 0 ;
% 0 0 0 1 ];
%% Ry
Ry = [ cos(ry) 0 sin(ry) 0 ;
0 1 0 0 ;
-sin(ry) 0 cos(ry) 0 ;
0 0 0 1 ];
% TMry = Ty*Ry*Tyinv;
%% Rz
Rz = [cos(rz) -sin(rz) 0 0 ;
sin(rz) cos(rz) 0 0 ;
0 0 1 0 ;
0 0 0 1 ];
% TMrz = Ty*TMry*Rz*TMry'*Tyinv;
%% All stages
% TM = TMrz*TMry*Ty;
TM = Ty*Ry*Rz;
[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
setpoint(1:3) = TM(1:3, 4);
setpoint(4:6) = [thetax, thetay, thetaz];
%% Custom Functions
function [thetax, thetay, thetaz] = RM2angle(R)
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
thetay = -asin(R(3, 1));
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
else
thetaz = 0;
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
thetay = pi/2;
thetax = thetaz + atan2(R(1, 2), R(1, 3));
else
thetay = -pi/2;
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
end
end
end
end

View File

@ -1,125 +0,0 @@
function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
% convertErrorBasis -
%
% Syntax: convertErrorBasis(p_error, ty, ry, rz)
%
% Inputs:
% - p_error - Position error of the sample w.r.t. the granite [m, rad]
% - ty - Measured translation of the Ty stage [m]
% - ry - Measured rotation of the Ry stage [rad]
% - rz - Measured rotation of the Rz stage [rad]
%
% Outputs:
% - P_nass - Position error of the sample w.r.t. the NASS base [m]
% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
%
% Example:
%
%% If line vector => column vector
if size(pos, 2) == 6
pos = pos';
end
if size(setpoint, 2) == 6
setpoint = setpoint';
end
%% Position of the sample in the frame fixed to the Granite
P_granite = [pos(1:3); 1]; % Position [m]
R_granite = [setpoint(1:3); 1]; % Reference [m]
%% Transformation matrices of the stages
% T-y
TMty = [1 0 0 0 ;
0 1 0 ty ;
0 0 1 0 ;
0 0 0 1 ];
% R-y
TMry = [ cos(ry) 0 sin(ry) 0 ;
0 1 0 0 ;
-sin(ry) 0 cos(ry) 0 ;
0 0 0 1 ];
% R-z
TMrz = [cos(rz) -sin(rz) 0 0 ;
sin(rz) cos(rz) 0 0 ;
0 0 1 0 ;
0 0 0 1 ];
%% Compute Point coordinates in the new reference fixed to the NASS base
% P_nass = TMrz*TMry*TMty*P_granite;
P_nass = TMrz\TMry\TMty\P_granite;
R_nass = TMrz\TMry\TMty\R_granite;
dx = R_nass(1)-P_nass(1);
dy = R_nass(2)-P_nass(2);
dz = R_nass(3)-P_nass(3);
%% Compute new basis vectors linked to the NASS base
% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
% ux_nass = ux_nass(1:3);
% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
% uy_nass = uy_nass(1:3);
% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
% uz_nass = uz_nass(1:3);
ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
ux_nass = ux_nass(1:3);
uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
uy_nass = uy_nass(1:3);
uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
uz_nass = uz_nass(1:3);
%% Rotations error w.r.t. granite Frame
% Rotations error w.r.t. granite Frame
rx_nass = pos(4);
ry_nass = pos(5);
rz_nass = pos(6);
% Rotation matrices of the Sample w.r.t. the Granite
Mrx_error = [1 0 0 ;
0 cos(-rx_nass) -sin(-rx_nass) ;
0 sin(-rx_nass) cos(-rx_nass)];
Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
0 1 0 ;
-sin(-ry_nass) 0 cos(-ry_nass)];
Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
sin(-rz_nass) cos(-rz_nass) 0 ;
0 0 1];
% Rotation matrix of the Sample w.r.t. the Granite
Mr_error = Mrz_error*Mry_error*Mrx_error;
%% Use matrix to solve
R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
[thetax, thetay, thetaz] = RM2angle(R);
error_nass = [dx; dy; dz; thetax; thetay; thetaz];
%% Custom Functions
function [thetax, thetay, thetaz] = RM2angle(R)
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
thetay = -asin(R(3, 1));
% thetaybis = pi-thetay;
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
% thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
% thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
else
thetaz = 0;
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
thetay = pi/2;
thetax = thetaz + atan2(R(1, 2), R(1, 3));
else
thetay = -pi/2;
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
end
end
end
end