Update the metrology study.
This commit is contained in:
64
src/computeReferencePose.m
Normal file
64
src/computeReferencePose.m
Normal file
@@ -0,0 +1,64 @@
|
||||
% computeReferencePose
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/computeReferencePose.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:computeReferencePose>>
|
||||
|
||||
% This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
|
||||
|
||||
|
||||
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
||||
%
|
||||
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
%
|
||||
% Inputs:
|
||||
% - Dy, Ry, Rz, Dh -
|
||||
%
|
||||
% Outputs:
|
||||
% - WTr -
|
||||
|
||||
%% Translation Stage
|
||||
Rty = [1 0 0 0;
|
||||
0 1 0 Dy;
|
||||
0 0 1 0;
|
||||
0 0 0 1];
|
||||
|
||||
%% Tilt Stage - Pure rotating aligned with Ob
|
||||
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
||||
0 1 0 0;
|
||||
-sin(Ry) 0 cos(Ry) 0;
|
||||
0 0 0 1];
|
||||
|
||||
%% Spindle - Rotation along the Z axis
|
||||
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
||||
sin(Rz) cos(Rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
|
||||
%% Micro-Hexapod
|
||||
Rhx = [1 0 0;
|
||||
0 cos(Dh(4)) -sin(Dh(4));
|
||||
0 sin(Dh(4)) cos(Dh(4))];
|
||||
|
||||
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
||||
0 1 0;
|
||||
-sin(Dh(5)) 0 cos(Dh(5))];
|
||||
|
||||
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
||||
sin(Dh(6)) cos(Dh(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rh = [1 0 0 Dh(1) ;
|
||||
0 1 0 Dh(2) ;
|
||||
0 0 1 Dh(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rh(1:3, 1:3) = Rhx*Rhy*Rhz;
|
||||
|
||||
%% Total Homogeneous transformation
|
||||
WTr = Rty*Rry*Rrz*Rh;
|
||||
end
|
||||
72
src/identifyPlantFRF.m
Normal file
72
src/identifyPlantFRF.m
Normal file
@@ -0,0 +1,72 @@
|
||||
% identifyPlantFRF
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/identifyPlantFRF.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:identifyPlant>>
|
||||
|
||||
% This Matlab function is accessible [[file:../src/identifyPlant.m][here]].
|
||||
|
||||
|
||||
function [sys] = identifyPlantFRF(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct();
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'sim_nano_station_id';
|
||||
|
||||
%% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
|
||||
io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
|
||||
io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
|
||||
io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
|
||||
io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces
|
||||
io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
|
||||
io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
|
||||
io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
|
||||
io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
|
||||
io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs
|
||||
|
||||
input = frest.Random('Ts',1e-4);
|
||||
|
||||
%% Run the linearization
|
||||
G = frestimate(mdl, io, input);
|
||||
% G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
|
||||
% 'Dgx', 'Dgy', 'Dgz', ...
|
||||
% 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
|
||||
% 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
|
||||
% 'Frzz', 'Ftyx', 'Ftyz'};
|
||||
% G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
|
||||
% 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
|
||||
% 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
|
||||
% 'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
|
||||
% 'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
||||
|
||||
% %% Create the sub transfer functions
|
||||
% minreal_tol = sqrt(eps);
|
||||
% % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
|
||||
% sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
||||
% % From ground motion to Sample displacement
|
||||
% sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
|
||||
% % From direct forces applied on the sample to displacement of the sample
|
||||
% sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
|
||||
% % From forces applied on NASS's legs to force sensor in each leg
|
||||
% sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
||||
% % From forces applied on NASS's legs to displacement of each leg
|
||||
% sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
||||
% % From forces/torques applied by the NASS to position error
|
||||
% sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
||||
% % From forces/torques applied by the NASS to velocity of the actuator
|
||||
% sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
||||
% % From various disturbance forces to position error
|
||||
% sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
|
||||
|
||||
end
|
||||
135
src/initDisturbances.m
Normal file
135
src/initDisturbances.m
Normal file
@@ -0,0 +1,135 @@
|
||||
% Function that initialize the disturbances
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/initDisturbances.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:initDisturbances>>
|
||||
|
||||
% This Matlab function is accessible [[file:src/initDisturbances.m][here]].
|
||||
|
||||
|
||||
function [] = initDisturbances(opts_param)
|
||||
% initDisturbances - Initialize the disturbances
|
||||
%
|
||||
% Syntax: [] = initDisturbances(opts_param)
|
||||
%
|
||||
% Inputs:
|
||||
% - opts_param -
|
||||
|
||||
% Default values for the Options
|
||||
|
||||
%% Default values for opts
|
||||
opts = struct();
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
% Load Data
|
||||
|
||||
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
||||
|
||||
|
||||
|
||||
% We remove the first frequency point that usually is very large.
|
||||
|
||||
dist_f.f = dist_f.f(2:end);
|
||||
dist_f.psd_gm = dist_f.psd_gm(2:end);
|
||||
dist_f.psd_ty = dist_f.psd_ty(2:end);
|
||||
dist_f.psd_rz = dist_f.psd_rz(2:end);
|
||||
|
||||
% Parameters
|
||||
% We define some parameters that will be used in the algorithm.
|
||||
|
||||
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||||
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
|
||||
T0 = N/Fs; % Signal Duration [s]
|
||||
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
||||
% Also equal to (dist_f.f(2)-dist_f.f(1))
|
||||
t = linspace(0, T0, N+1)'; % Time Vector [s]
|
||||
Ts = 1/Fs; % Sampling Time [s]
|
||||
|
||||
% Ground Motion
|
||||
|
||||
phi = dist_f.psd_gm;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
||||
% Dwx = struct('time', t, 'signals', struct('values', u));
|
||||
Dwx = u;
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
||||
Dwy = u;
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
||||
Dwz = u;
|
||||
|
||||
% Translation Stage - X direction
|
||||
|
||||
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
||||
Fty_x = u;
|
||||
|
||||
% Translation Stage - Z direction
|
||||
|
||||
phi = dist_f.psd_ty;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
||||
Fty_z = u;
|
||||
|
||||
% Spindle - Z direction
|
||||
|
||||
phi = dist_f.psd_rz;
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(phi(i)*df);
|
||||
end
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
||||
Frz_z = u;
|
||||
|
||||
% Direct Forces
|
||||
|
||||
u = zeros(length(t), 6);
|
||||
Fd = u;
|
||||
|
||||
% Set initial value to zero
|
||||
|
||||
Dwx = Dwx - Dwx(1);
|
||||
Dwy = Dwy - Dwy(1);
|
||||
Dwz = Dwz - Dwz(1);
|
||||
Fty_x = Fty_x - Fty_x(1);
|
||||
Fty_z = Fty_z - Fty_z(1);
|
||||
Frz_z = Frz_z - Frz_z(1);
|
||||
|
||||
% Save
|
||||
|
||||
save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
|
||||
128
src/initializeReferences.m
Normal file
128
src/initializeReferences.m
Normal file
@@ -0,0 +1,128 @@
|
||||
% Generate Reference Signals
|
||||
% :PROPERTIES:
|
||||
% :header-args:matlab+: :tangle ../src/initializeReferences.m
|
||||
% :header-args:matlab+: :comments org :mkdirp yes
|
||||
% :header-args:matlab+: :eval no :results none
|
||||
% :END:
|
||||
% <<sec:initializeReferences>>
|
||||
|
||||
% This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
|
||||
|
||||
function [ref] = initializeReferences(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct( ...
|
||||
'Ts', 1e-3, ... % Sampling Frequency [s]
|
||||
'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
|
||||
'Dy_period', 1, ... % Period of the displacement [s]
|
||||
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Ry_amplitude', 0, ... % Amplitude [deg]
|
||||
'Ry_period', 10, ... % Period of the displacement [s]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', 0, ... % Initial angle [deg]
|
||||
'Rz_period', 1, ... % Period of the rotating [s]
|
||||
'Dh_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
'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', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
);
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Set Sampling Time
|
||||
Ts = opts.Ts;
|
||||
|
||||
%% Translation stage - Dy
|
||||
t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
|
||||
Dy = zeros(length(t), 1);
|
||||
switch opts.Dy_type
|
||||
case 'constant'
|
||||
Dy(:) = opts.Dy_amplitude;
|
||||
case 'triangular'
|
||||
Dy(:) = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
|
||||
Dy(t<0.75*opts.Dy_period) = 2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
|
||||
Dy(t<0.25*opts.Dy_period) = 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
|
||||
case 'sinusoidal'
|
||||
Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
|
||||
otherwise
|
||||
warning('Dy_type is not set correctly');
|
||||
end
|
||||
Dy = struct('time', t, 'signals', struct('values', Dy));
|
||||
|
||||
|
||||
%% Tilt Stage - Ry
|
||||
t = 0:Ts:opts.Ry_period-Ts;
|
||||
Ry = zeros(length(t), 1);
|
||||
|
||||
switch opts.Ry_type
|
||||
case 'constant'
|
||||
Ry(:) = pi/180*opts.Ry_amplitude;
|
||||
case 'triangular'
|
||||
Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t;
|
||||
Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period);
|
||||
Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period);
|
||||
case 'sinusoidal'
|
||||
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
|
||||
otherwise
|
||||
warning('Ry_type is not set correctly');
|
||||
end
|
||||
Ry = struct('time', t, 'signals', struct('values', Ry));
|
||||
|
||||
%% Spindle - Rz
|
||||
t = 0:Ts:opts.Rz_period-Ts;
|
||||
Rz = zeros(length(t), 1);
|
||||
|
||||
switch opts.Rz_type
|
||||
case 'constant'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude;
|
||||
case 'rotating'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t;
|
||||
otherwise
|
||||
warning('Rz_type is not set correctly');
|
||||
end
|
||||
Rz = struct('time', t, 'signals', struct('values', Rz));
|
||||
|
||||
%% Micro-Hexapod
|
||||
t = [0, Ts];
|
||||
Dh = zeros(length(t), 6);
|
||||
|
||||
opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad]
|
||||
|
||||
switch opts.Dh_type
|
||||
case 'constant'
|
||||
Dh = [opts.Dh_pos, opts.Dh_pos];
|
||||
otherwise
|
||||
warning('Dh_type is not set correctly');
|
||||
end
|
||||
Dh = struct('time', t, 'signals', struct('values', Dh));
|
||||
|
||||
%% Axis Compensation - Rm
|
||||
t = [0, Ts];
|
||||
Rm = [opts.Rm_pos, opts.Rm_pos];
|
||||
Rm = struct('time', t, 'signals', struct('values', Rm));
|
||||
|
||||
%% Nano-Hexapod
|
||||
t = [0, Ts];
|
||||
Dn = zeros(length(t), 6);
|
||||
|
||||
opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad]
|
||||
|
||||
switch opts.Dn_type
|
||||
case 'constant'
|
||||
Dn = [opts.Dn_pos, opts.Dn_pos];
|
||||
otherwise
|
||||
warning('Dn_type is not set correctly');
|
||||
end
|
||||
Dn = struct('time', t, 'signals', struct('values', Dn));
|
||||
|
||||
%% Save
|
||||
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
|
||||
end
|
||||
Reference in New Issue
Block a user