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

Use new argument function validation technique

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-01-13 11:42:31 +01:00
parent 994fe2ccc7
commit a2917a50e9
20 changed files with 770 additions and 660 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
src/initDisturbances.m
View File

@@ -1,26 +1,24 @@
function [] = initDisturbances(opts_param)
function [] = initDisturbances(args)
% initDisturbances - Initialize the disturbances
%
% Syntax: [] = initDisturbances(opts_param)
% Syntax: [] = initDisturbances(args)
%
% Inputs:
% - opts_param -
% - args -
%% Default values for opts
opts = struct(...
'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
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
arguments
% Ground Motion - X direction
args.Dwx logical {mustBeNumericOrLogical} = true
% Ground Motion - Y direction
args.Dwy logical {mustBeNumericOrLogical} = true
% Ground Motion - Z direction
args.Dwz logical {mustBeNumericOrLogical} = true
% Translation Stage - X direction
args.Fty_x logical {mustBeNumericOrLogical} = true
% Translation Stage - Z direction
args.Fty_z logical {mustBeNumericOrLogical} = true
% Spindle - Z direction
args.Frz_z logical {mustBeNumericOrLogical} = true
end
load('./disturbances/mat/dist_psd.mat', 'dist_f');
@@ -44,7 +42,7 @@ for i = 1:N/2
C(i) = sqrt(phi(i)*df);
end
if opts.Dwx
if args.Dwx
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)))];;
@@ -53,7 +51,7 @@ else
Dwx = zeros(length(t), 1);
end
if opts.Dwy
if args.Dwy
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)))];;
@@ -62,7 +60,7 @@ else
Dwy = zeros(length(t), 1);
end
if opts.Dwy
if args.Dwy
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)))];;
@@ -71,7 +69,7 @@ else
Dwz = zeros(length(t), 1);
end
if opts.Fty_x
if args.Fty_x
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
@@ -86,7 +84,7 @@ else
Fty_x = zeros(length(t), 1);
end
if opts.Fty_z
if args.Fty_z
phi = dist_f.psd_ty;
C = zeros(N/2,1);
for i = 1:N/2
@@ -101,7 +99,7 @@ else
Fty_z = zeros(length(t), 1);
end
if opts.Frz_z
if args.Frz_z
phi = dist_f.psd_rz;
C = zeros(N/2,1);
for i = 1:N/2

12
src/initializeCedratPiezo.m
View File

@@ -1,14 +1,4 @@
function [cedrat] = initializeCedratPiezo(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
function [cedrat] = initializeCedratPiezo()
%% Stewart Object
cedrat = struct();
cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]

14
src/initializeGranite.m
View File

@@ -1,12 +1,6 @@
function [granite] = initializeGranite(opts_param)
%% Default values for opts
opts = struct('rigid', false);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [granite] = initializeGranite(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
end
%%
@@ -22,7 +16,7 @@ function [granite] = initializeGranite(opts_param)
granite.mass_top = 4000; % [kg] TODO
%% Dynamical Properties
if opts.rigid
if args.rigid
granite.k.x = 1e12; % [N/m]
granite.k.y = 1e12; % [N/m]
granite.k.z = 1e12; % [N/m]

22
src/initializeMicroHexapod.m
View File

@@ -1,16 +1,8 @@
function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Default values for opts
opts = struct(...
'rigid', false, ...
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
%% Stewart Object
@@ -46,7 +38,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
Leg = struct();
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
if opts.rigid
if args.rigid
Leg.k.ax = 1e12; % Stiffness of each leg [N/m]
else
Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
@@ -98,7 +90,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB);
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
%% Save
save('./mat/stages.mat', 'micro_hexapod', '-append');

24
src/initializeMirror.m
View File

@@ -1,15 +1,7 @@
function [] = initializeMirror(opts_param)
%% Default values for opts
opts = struct(...
'shape', 'spherical', ... % spherical or conical
'angle', 45 ...
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [] = initializeMirror(args)
arguments
args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45
end
%%
@@ -24,7 +16,7 @@ function [] = initializeMirror(opts_param)
mirror.density = 2400; % Density of the mirror [kg/m3]
mirror.color = [0.4 1.0 1.0]; % Color of the mirror
mirror.cone_length = mirror.rad*tand(opts.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
%% Shape
mirror.shape = [...
@@ -34,14 +26,14 @@ function [] = initializeMirror(opts_param)
mirror.rad 0 ...
];
if strcmp(opts.shape, 'spherical')
if strcmp(args.shape, 'spherical')
mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]
for z = linspace(0, mirror.h, 101)
mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
end
elseif strcmp(opts.shape, 'conical')
mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.angle) mirror.h];
elseif strcmp(args.shape, 'conical')
mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h];
else
error('Shape should be either conical or spherical');
end

26
src/initializeNanoHexapod.m
View File

@@ -1,16 +1,8 @@
function [nano_hexapod] = initializeNanoHexapod(opts_param)
%% Default values for opts
opts = struct(...
'actuator', 'piezo', ...
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [nano_hexapod] = initializeNanoHexapod(args)
arguments
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
%% Stewart Object
@@ -46,12 +38,12 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
Leg = struct();
Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
if strcmp(opts.actuator, 'piezo')
if strcmp(args.actuator, 'piezo')
Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
elseif strcmp(opts.actuator, 'lorentz')
elseif strcmp(args.actuator, 'lorentz')
Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
else
error('opts.actuator should be piezo or lorentz');
error('args.actuator should be piezo or lorentz');
end
Leg.ksi.ax = 10; % Maximum amplification at resonance []
Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
@@ -101,7 +93,7 @@ function [nano_hexapod] = initializeNanoHexapod(opts_param)
nano_hexapod = initializeParameters(nano_hexapod);
%% Setup equilibrium position of each leg
nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, opts.AP, opts.ARB);
nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, args.AP, args.ARB);
%% Save
save('./mat/stages.mat', 'nano_hexapod', '-append');

119
src/initializeReferences.m
View File

@@ -1,36 +1,45 @@
function [ref] = initializeReferences(opts_param)
function [ref] = initializeReferences(args)
%% Default values for opts
opts = struct( ...
'Ts', 1e-3, ... % Sampling Frequency [s]
'Tmax', 100, ... % Maximum simulation time [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 [rad]
'Ry_period', 1, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', zeros(6, 1), ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
'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', zeros(6,1) ... % 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
arguments
% Sampling Frequency [s]
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
% Maximum simulation time [s]
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
% Either "constant" / "triangular" / "sinusoidal"
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude of the displacement [m]
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "triangular" / "sinusoidal"
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
% Amplitude [rad]
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the displacement [s]
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% Either "constant" / "rotating"
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant'
% Initial angle [rad]
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
% Period of the rotating [s]
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
% For now, only constant is implemented
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
% For now, only constant is implemented
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
% Initial position of the two masses
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
% For now, only constant is implemented
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
% Initial position [m,m,m,rad,rad,rad] of the top platform
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
end
%% Set Sampling Time
Ts = opts.Ts;
Tmax = opts.Tmax;
Ts = args.Ts;
Tmax = args.Tmax;
%% Low Pass Filter to filter out the references
s = zpk('s');
@@ -43,15 +52,15 @@ t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1);
switch opts.Dy_type
switch args.Dy_type
case 'constant'
Dy(:) = opts.Dy_amplitude;
Dy(:) = args.Dy_amplitude;
Dyd(:) = 0;
Dydd(:) = 0;
case 'triangular'
% This is done to unsure that we start with no displacement
Dy_raw = opts.Dy_amplitude*sawtooth(2*pi*t/opts.Dy_period,1/2);
i0 = find(t>=opts.Dy_period/4,1);
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
i0 = find(t>=args.Dy_period/4,1);
Dy(1:end-i0+1) = Dy_raw(i0:end);
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
@@ -60,9 +69,9 @@ switch opts.Dy_type
Dyd = lsim(H_lpf*s, Dy, t);
Dydd = lsim(H_lpf*s^2, Dy, t);
case 'sinusoidal'
Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
Dyd = opts.Dy_amplitude*2*pi/opts.Dy_period*cos(2*pi/opts.Dy_period*t);
Dydd = -opts.Dy_amplitude*(2*pi/opts.Dy_period)^2*sin(2*pi/opts.Dy_period*t);
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
otherwise
warning('Dy_type is not set correctly');
end
@@ -75,14 +84,14 @@ Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1);
switch opts.Ry_type
switch args.Ry_type
case 'constant'
Ry(:) = opts.Ry_amplitude;
Ry(:) = args.Ry_amplitude;
Ryd(:) = 0;
Rydd(:) = 0;
case 'triangular'
Ry_raw = opts.Ry_amplitude*sawtooth(2*pi*t/opts.Ry_period,1/2);
i0 = find(t>=opts.Ry_period/4,1);
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
i0 = find(t>=args.Ry_period/4,1);
Ry(1:end-i0+1) = Ry_raw(i0:end);
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
@@ -91,10 +100,10 @@ switch opts.Ry_type
Ryd = lsim(H_lpf*s, Ry, t);
Rydd = lsim(H_lpf*s^2, Ry, t);
case 'sinusoidal'
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
Ryd = opts.Ry_amplitude*2*pi/opts.Ry_period*cos(2*pi/opts.Ry_period*t);
Rydd = -opts.Ry_amplitude*(2*pi/opts.Ry_period)^2*sin(2*pi/opts.Ry_period*t);
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
otherwise
warning('Ry_type is not set correctly');
end
@@ -107,13 +116,13 @@ Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1);
switch opts.Rz_type
switch args.Rz_type
case 'constant'
Rz(:) = opts.Rz_amplitude;
Rz(:) = args.Rz_amplitude;
Rzd(:) = 0;
Rzdd(:) = 0;
case 'rotating'
Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
% The signal is filtered out
Rz = lsim(H_lpf, Rz, t);
@@ -130,17 +139,17 @@ t = [0, Ts];
Dh = zeros(length(t), 6);
Dhl = zeros(length(t), 6);
switch opts.Dh_type
switch args.Dh_type
case 'constant'
Dh = [opts.Dh_pos, opts.Dh_pos];
Dh = [args.Dh_pos, args.Dh_pos];
load('mat/stages.mat', 'micro_hexapod');
AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
tx = opts.Dh_pos(4);
ty = opts.Dh_pos(5);
tz = opts.Dh_pos(6);
tx = args.Dh_pos(4);
ty = args.Dh_pos(5);
tz = args.Dh_pos(6);
ARB = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
@@ -164,16 +173,16 @@ Dhl = struct('time', t, 'signals', struct('values', Dhl));
%% Axis Compensation - Rm
t = [0, Ts];
Rm = [opts.Rm_pos, opts.Rm_pos];
Rm = [args.Rm_pos, args.Rm_pos];
Rm = struct('time', t, 'signals', struct('values', Rm));
%% Nano-Hexapod
t = [0, Ts];
Dn = zeros(length(t), 6);
switch opts.Dn_type
switch args.Dn_type
case 'constant'
Dn = [opts.Dn_pos, opts.Dn_pos];
Dn = [args.Dn_pos, args.Dn_pos];
otherwise
warning('Dn_type is not set correctly');
end

14
src/initializeRy.m
View File

@@ -1,12 +1,6 @@
function [ry] = initializeRy(opts_param)
%% Default values for opts
opts = struct('rigid', false);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [ry] = initializeRy(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
end
%%
@@ -33,7 +27,7 @@ function [ry] = initializeRy(opts_param)
ry.m = 800; % TODO [kg]
%% Tilt Stage - Dynamical Properties
if opts.rigid
if args.rigid
ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m]
ry.k.rad = 1e12; % Stiffness in the top direction [N/m]

14
src/initializeRz.m
View File

@@ -1,12 +1,6 @@
function [rz] = initializeRz(opts_param)
%% Default values for opts
opts = struct('rigid', false);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [rz] = initializeRz(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
end
%%
@@ -31,7 +25,7 @@ function [rz] = initializeRz(opts_param)
%% Spindle - Dynamical Properties
if opts.rigid
if args.rigid
rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg]
rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg]
rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m]

21
src/initializeSample.m
View File

@@ -1,17 +1,10 @@
function [sample] = initializeSample(opts_param)
%% Default values for opts
sample = struct('radius', 100, ...
'height', 300, ...
'mass', 50, ...
'offset', 0, ...
'color', [0.45, 0.45, 0.45] ...
);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
sample.(opt{1}) = opts_param.(opt{1});
end
function [sample] = initializeSample(sample)
arguments
sample.radius (1,1) double {mustBeNumeric, mustBePositive} = 100
sample.height (1,1) double {mustBeNumeric, mustBePositive} = 300
sample.mass (1,1) double {mustBeNumeric, mustBePositive} = 50
sample.offset (1,1) double {mustBeNumeric} = 0
sample.color (1,3) double {mustBeNumeric} = [0.45, 0.45, 0.45]
end
%%

14
src/initializeTy.m
View File

@@ -1,12 +1,6 @@
function [ty] = initializeTy(opts_param)
%% Default values for opts
opts = struct('rigid', false);
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
function [ty] = initializeTy(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
end
%%
@@ -53,7 +47,7 @@ function [ty] = initializeTy(opts_param)
ty.m = 1000; % TODO [kg]
%% Y-Translation - Dynamicals Properties
if opts.rigid
if args.rigid
ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
else