1 Helping Functions
-
+
+The full Simscape Model is represented in Figure 1. +
+ + +
+
-
+
Figure 1: Screenshot of the Multi-Body Model representation
-
1.1 Generate Reference Signals
+ ++This model is divided into multiple subsystems that are independent. +These subsystems are saved in separate files and imported in the main file using a block balled “subsystem reference”. +
+ ++Each stage is configured (geometry, mass properties, dynamic properties …) using one function. +
+ ++These functions are defined below. +
+ +
+
-
-1 Ground
+ + +
+
+
+Simscape Model
+
+
++The model of the Ground is composed of: +
+-
+
- A Cartesian joint that is used to simulation the ground motion +
- A solid that represents the ground on which the granite is located +
+
+
+
+
+
Figure 2: Simscape model for the Ground
+
+
+
+
Figure 3: Simscape picture for the Ground
+
+
+
+Function description
+
+
+
+
+function [ground] = initializeGround() ++
+
-
-
-1.1 Function content
-
+First, we initialize the granite structure.
-This Matlab function is accessible 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 [rad] - 'Ry_period', 10, ... % 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', [0; 0; 0; 0; 0; 0], ... % 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', [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(:) = opts.Ry_amplitude; - case 'triangular' - Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t; - Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period); - Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period); - case 'sinusoidal' - - 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(:) = opts.Rz_amplitude; - case 'rotating' - Rz(:) = 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); - Dhl = zeros(length(t), 6); - - switch opts.Dh_type - case 'constant' - Dh = [opts.Dh_pos, opts.Dh_pos]; - - load('./mat/stages.mat', 'micro_hexapod'); - - AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)]; - - tx = opts.Dh_pos(4); - ty = opts.Dh_pos(5); - tz = opts.Dh_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)]; - - [Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB); - Dhl = [Dhl, Dhl]; - otherwise - warning('Dh_type is not set correctly'); - end - Dh = struct('time', t, 'signals', struct('values', Dh)); - Dhl = struct('time', t, 'signals', struct('values', Dhl)); - - %% 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); - - 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', 'Dhl', 'Rm', 'Dn', 'Ts'); -end +ground = struct();
-
1.2 Inverse Kinematics of the Hexapod
-
-
-This Matlab function is accessible here. +We set the shape and density of the ground solid element.
-
-
- % 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
+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} +ground.shape = [2, 2, 0.5]; % [m] +ground.density = 2800; % [kg/m3] ++
+The ground structure is saved.
+
+
save('./mat/stages.mat', 'ground', '-append');
-
2 Initialize Elements
+
+
2 Granite
-
-
2.1 Ground
-
+
+
+
+
+
+Simscape Model
+- +The Simscape model of the granite is composed of:
+-
+
- A cartesian joint such that the granite can vibrations along x, y and z axis +
- A solid +
-This Matlab function is accessible here.
+The output sample_pos corresponds to the impact point of the X-ray.
+
+
+
+
+
Figure 4: Simscape model for the Granite
+
+
+
+
Figure 5: Simscape picture for the Granite
+
+
+
- ground.shape = [2, 2, 0.5]; % [m]
- ground.density = 2800; % [kg/m3]
- ground.color = [0.5, 0.5, 0.5];
-
- %% Save
- save('./mat/stages.mat', 'ground', '-append');
+Function description
+
-
+function [ground] = initializeGround() - %% - ground = struct(); +function [granite] = initializeGranite(args) ++
+
-Optional Parameters
+
+
+
arguments
+ 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]
+ args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
end
-
2.2 Granite
-
+
- %% Positioning parameters
- granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m]
+
+
+
+Function content
+
-
+First, we initialize the granite structure.
-This Matlab function is accessible here. -
-
-
- %% 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 [granite] = initializeGranite(opts_param) - %% Default values for opts - opts = struct('rigid', false); +granite = struct(); ++
+Properties of the Material and link to the geometry of the granite. +
+
+
- %%
- granite = struct();
+granite.density = args.density; % [kg/m3] +granite.STEP = './STEPS/granite/granite.STEP'; ++
+Stiffness of the connection with Ground. +
+
+
- %% Static Properties
- granite.density = 2800; % [kg/m3]
- granite.volume = 0.749; % [m3] TODO - should
- granite.mass = granite.density*granite.volume; % [kg]
- granite.color = [1 1 1];
- granite.STEP = './STEPS/granite/granite.STEP';
+granite.k.x = 4e9; % [N/m] +granite.k.y = 3e8; % [N/m] +granite.k.z = 8e8; % [N/m] ++
+Damping of the connection with Ground. +
+
+
- granite.mass_top = 4000; % [kg] TODO
+granite.c.x = 4.0e5; % [N/(m/s)] +granite.c.y = 1.1e5; % [N/(m/s)] +granite.c.z = 9.0e5; % [N/(m/s)] ++
+Equilibrium position of the Cartesian joint. +
+
+
- %% Dynamical Properties
- if opts.rigid
- granite.k.x = 1e12; % [N/m]
- granite.k.y = 1e12; % [N/m]
- granite.k.z = 1e12; % [N/m]
- granite.k.rx = 1e10; % [N*m/deg]
- granite.k.ry = 1e10; % [N*m/deg]
- granite.k.rz = 1e10; % [N*m/deg]
- else
- granite.k.x = 4e9; % [N/m]
- granite.k.y = 3e8; % [N/m]
- granite.k.z = 8e8; % [N/m]
- granite.k.rx = 1e4; % [N*m/deg]
- granite.k.ry = 1e4; % [N*m/deg]
- granite.k.rz = 1e6; % [N*m/deg]
- end
+granite.x0 = args.x0; +granite.y0 = args.y0; +granite.z0 = args.z0; ++
+Z-offset for the initial position of the sample with respect to the granite top surface. +
+
+
- granite.c.x = 0.1*sqrt(granite.mass_top*granite.k.x); % [N/(m/s)]
- granite.c.y = 0.1*sqrt(granite.mass_top*granite.k.y); % [N/(m/s)]
- granite.c.z = 0.5*sqrt(granite.mass_top*granite.k.z); % [N/(m/s)]
- granite.c.rx = 0.1*sqrt(granite.mass_top*granite.k.rx); % [N*m/(deg/s)]
- granite.c.ry = 0.1*sqrt(granite.mass_top*granite.k.ry); % [N*m/(deg/s)]
- granite.c.rz = 0.1*sqrt(granite.mass_top*granite.k.rz); % [N*m/(deg/s)]
+granite.sample_pos = 0.8; % [m]
+
+
+The granite structure is saved.
+
+
+save('./mat/stages.mat', 'granite', '-append'); ++
+
3 Translation Stage
+ - %% Save - save('./mat/stages.mat', 'granite', '-append'); +
+
+
+Simscape Model
+
+
++The Simscape model of the Translation stage consist of: +
+-
+
- One rigid body for the fixed part of the translation stage +
- One rigid body for the moving part of the translation stage +
- Four 6-DOF Joints that only have some rigidity in the X and Z directions. +The rigidity in rotation comes from the fact that we use multiple joints that are located at different points +
- One 6-DOF joint that represent the Actuator. +It is used to impose the motion in the Y direction +
- One 6-DOF joint to inject force disturbance in the X and Z directions +
+
+
+
+
+
Figure 6: Simscape model for the Translation Stage
+
+
+
+
Figure 7: Simscape picture for the Translation Stage
+
+
+
+Function description
+
+
+
+
+function [ty] = initializeTy(args) ++
+
-Optional Parameters
+
+
+
arguments
+ 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]
end
-
2.3 Translation Stage
-
+
+
+
+
+
+Function content
+
-
+First, we initialize the ty structure.
-This Matlab function is accessible here. -
-
-
- %% 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 [ty] = initializeTy(opts_param) - %% Default values for opts - opts = struct('rigid', false); +ty = struct(); ++
+Define the density of the materials as well as the geometry (STEP files). +
+
+
+
+% Ty Granite frame +ty.granite_frame.density = 7800; % [kg/m3] => 43kg +ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP'; - %% - ty = struct(); +% Guide Translation Ty +ty.guide.density = 7800; % [kg/m3] => 76kg +ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP'; - %% Y-Translation - Static Properties - % Ty Granite frame - ty.granite_frame.density = 7800; % [kg/m3] - ty.granite_frame.color = [0.753 1 0.753]; - ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP'; - % Guide Translation Ty - ty.guide.density = 7800; % [kg/m3] - ty.guide.color = [0.792 0.820 0.933]; - ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP'; - % Ty - Guide_Translation12 - ty.guide12.density = 7800; % [kg/m3] - ty.guide12.color = [0.792 0.820 0.933]; - ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP'; - % Ty - Guide_Translation11 - ty.guide11.density = 7800; % [kg/m3] - ty.guide11.color = [0.792 0.820 0.933]; - ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP'; - % Ty - Guide_Translation22 - ty.guide22.density = 7800; % [kg/m3] - ty.guide22.color = [0.792 0.820 0.933]; - ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP'; - % Ty - Guide_Translation21 - ty.guide21.density = 7800; % [kg/m3] - ty.guide21.color = [0.792 0.820 0.933]; - ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP'; - % Ty - Plateau translation - ty.frame.density = 7800; % [kg/m3] - ty.frame.color = [0.792 0.820 0.933]; - ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP'; - % Ty Stator Part - ty.stator.density = 5400; % [kg/m3] - ty.stator.color = [0.792 0.820 0.933]; - ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP'; - % Ty Rotor Part - ty.rotor.density = 5400; % [kg/m3] - ty.rotor.color = [0.792 0.820 0.933]; - ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP'; +% Ty - Guide_Translation12 +ty.guide12.density = 7800; % [kg/m3] +ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP'; - ty.m = 1000; % TODO [kg] +% Ty - Guide_Translation11 +ty.guide11.density = 7800; % [kg/m3] +ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP'; - %% Y-Translation - Dynamicals Properties - if opts.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 - 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] - end +% Ty - Guide_Translation22 +ty.guide22.density = 7800; % [kg/m3] +ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP'; - ty.c.ax = 0.1*sqrt(ty.k.ax*ty.m); - ty.c.rad = 0.1*sqrt(ty.k.rad*ty.m); +% Ty - Guide_Translation21 +ty.guide21.density = 7800; % [kg/m3] +ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP'; - %% Save - save('./mat/stages.mat', 'ty', '-append'); +% Ty - Plateau translation +ty.frame.density = 7800; % [kg/m3] +ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP'; + +% Ty Stator Part +ty.stator.density = 5400; % [kg/m3] +ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP'; + +% Ty Rotor Part +ty.rotor.density = 5400; % [kg/m3] +ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP'; ++
+Stiffness of the stage. +
+
+
+
+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] ++
+Damping of the stage. +
+
+
+
+ty.c.ax = 70710; % [N/(m/s)] +ty.c.rad = 22360; % [N/(m/s)] ++
+Equilibrium position of the joints. +
+
+
+
+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; ++
+The ty structure is saved.
+
+
+save('./mat/stages.mat', 'ty', '-append'); ++
+
4 Tilt Stage
+ + +
+
+
+Simscape Model
+
+
++The Simscape model of the Tilt stage is composed of: +
+-
+
- Two solid bodies for the two part of the stage +
- Four 6-DOF joints to model the flexibility of the stage. +These joints are virtually located along the rotation axis and are connecting the two solid bodies. +These joints have some translation stiffness in the u-v-w directions aligned with the joint. +The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations +
- A Bushing Joint used for the Actuator. +The Ry motion is imposed by the input. +
+
+
+
+
+
Figure 8: Simscape model for the Tilt Stage
+
+
+
+
Figure 9: Simscape picture for the Tilt Stage
+
+
+
+Function description
+
+
+
+
+function [ry] = initializeRy(args) ++
+
-Optional Parameters
+
+
+
arguments
+ 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]
end
-
2.4 Tilt Stage
-
+
+
+
+
+
+Function content
+
-
+First, we initialize the ry structure.
-This Matlab function is accessible here. -
-
-
- %% 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 [ry] = initializeRy(opts_param) - %% Default values for opts - opts = struct('rigid', false); +ry = struct(); ++
+Properties of the Material and link to the geometry of the Tilt stage. +
+
+
- %% Tilt Stage - Dynamical Properties
- if opts.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]
- ry.k.rrad = 1e12; % Stiffness in the side direction [N/m]
- else
- 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
+% Ry - Guide for the tilt stage +ry.guide.density = 7800; % [kg/m3] +ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP'; - %% - ry = struct(); +% Ry - Rotor of the motor +ry.rotor.density = 2400; % [kg/m3] +ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP'; - %% Tilt Stage - Static Properties - % Ry - Guide for the tilt stage - ry.guide.density = 7800; % [kg/m3] - ry.guide.color = [0.792 0.820 0.933]; - ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP'; - % Ry - Rotor of the motor - ry.rotor.density = 2400; % [kg/m3] - ry.rotor.color = [0.792 0.820 0.933]; - ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP'; - % Ry - Motor - ry.motor.density = 3200; % [kg/m3] - ry.motor.color = [0.792 0.820 0.933]; - ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP'; - % Ry - Plateau Tilt - ry.stage.density = 7800; % [kg/m3] - ry.stage.color = [0.792 0.820 0.933]; - ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP'; +% Ry - Motor +ry.motor.density = 3200; % [kg/m3] +ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP'; - ry.m = 800; % TODO [kg] +% Ry - Plateau Tilt +ry.stage.density = 7800; % [kg/m3] +ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP'; ++
+Stiffness of the stage. +
+
+
- ry.c.h = 0.1*sqrt(ry.k.h*ry.m);
- ry.c.rad = 0.1*sqrt(ry.k.rad*ry.m);
- ry.c.rrad = 0.1*sqrt(ry.k.rrad*ry.m);
- ry.c.tilt = 0.1*sqrt(ry.k.tilt*ry.m);
+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] ++
+Damping of the stage. +
+
+
- %% Positioning parameters
- ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]
+ry.c.tilt = 2.8e2; +ry.c.h = 2.8e4; +ry.c.rad = 2.8e4; +ry.c.rrad = 2.8e4; ++
+Equilibrium position of the joints. +
+
+
- %% Save
- save('./mat/stages.mat', 'ry', '-append');
+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; ++
+Z-Offset so that the center of rotation matches the sample center; +
+
+
+
+ry.z_offset = 0.58178; % [m]
+
+
+The ty structure is saved.
+
+
+save('./mat/stages.mat', 'ry', '-append'); ++
+
5 Spindle
+ + +
+
+
+Simscape Model
+
+
++The Simscape model of the Spindle is composed of: +
+-
+
- Two rigid bodies: the stator and the rotor +
- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction. +
- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations +
+
+
+
+
+
Figure 10: Simscape model for the Spindle
+
+
+
+
Figure 11: Simscape picture for the Spindle
+
+
+
+Function description
+
+
+
+
+function [rz] = initializeRz(args) ++
+
-Optional Parameters
+
+
+
arguments
+ args.rigid logical {mustBeNumericOrLogical} = false
+ 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.rx0 (1,1) double {mustBeNumeric} = 0 % [rad]
+ args.ry0 (1,1) double {mustBeNumeric} = 0 % [rad]
end
-
2.5 Spindle
-
+
- %% Save
- save('./mat/stages.mat', 'rz', '-append');
+
+
+
+Function content
+
-
+First, we initialize the rz structure.
-This Matlab function is accessible here. -
-
-
- %% 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 [rz] = initializeRz(opts_param) - %% Default values for opts - opts = struct('rigid', false); +rz = struct(); ++
+Properties of the Material and link to the geometry of the spindle. +
+
+
- % Estimated mass of the mooving part
- rz.m = 250; % [kg]
+% Spindle - Slip Ring +rz.slipring.density = 7800; % [kg/m3] +rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP'; - %% - rz = struct(); +% Spindle - Rotor +rz.rotor.density = 7800; % [kg/m3] +rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP'; - %% Spindle - Static Properties - % Spindle - Slip Ring - rz.slipring.density = 7800; % [kg/m3] - rz.slipring.color = [0.792 0.820 0.933]; - rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP'; - % Spindle - Rotor - rz.rotor.density = 7800; % [kg/m3] - rz.rotor.color = [0.792 0.820 0.933]; - rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP'; - % Spindle - Stator - rz.stator.density = 7800; % [kg/m3] - rz.stator.color = [0.792 0.820 0.933]; - rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP'; +% Spindle - Stator +rz.stator.density = 7800; % [kg/m3] +rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP'; ++
+Stiffness of the stage. +
+
+
- %% Spindle - Dynamical Properties
+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] ++
+Damping of the stage. +
+
+
- if opts.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]
- rz.k.rad = 1e12; % Radial Stiffness (X, Y) [N/m]
- else
- 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]
- end
+rz.c.rot = 1.6e3; +rz.c.tilt = 1.6e3; +rz.c.ax = 7.1e4; +rz.c.rad = 4.2e4; ++
+Equilibrium position of the joints. +
+
+
- % Damping
- rz.c.ax = 0.1*sqrt(rz.k.ax*rz.m);
- rz.c.rad = 0.1*sqrt(rz.k.rad*rz.m);
- rz.c.tilt = 0.1*sqrt(rz.k.tilt*rz.m);
- rz.c.rot = 0.1*sqrt(rz.k.rot*rz.m);
+rz.x0 = args.x0; +rz.y0 = args.y0; +rz.z0 = args.z0; +rz.rx0 = args.rx0; +rz.ry0 = args.ry0; ++
+The rz structure is saved.
+
+
+save('./mat/stages.mat', 'rz', '-append'); ++
+
6 Micro Hexapod
+ + +
+
+
+Simscape Model
+
+
+
+
+
+
+
+
+
Figure 12: Simscape model for the Micro-Hexapod
+
+
+
+
Figure 13: Simscape picture for the Micro-Hexapod
+
+
+
+Function description
+
+
+
+
+function [micro_hexapod] = initializeMicroHexapod(args) ++
+
-Optional Parameters
+
+
+
arguments + % initializeFramesPositions + args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 + args.MO_B (1,1) double {mustBeNumeric} = 270e-3 + % generateGeneralConfiguration + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3 + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180) + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3 + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180) + % initializeStrutDynamics + args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1) + args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1) + % initializeCylindricalPlatforms + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 + % initializeCylindricalStruts + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3 + % 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) end
-
2.6 Micro Hexapod
-
-
-
-
- %% Bottom Plate - Mechanical Design
- BP = struct();
+-This Matlab function is accessible here. -
- +
+
+
+Function content
+
-
- %% 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 [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} - ); +micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B); +micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh); +micro_hexapod = computeJointsPose(micro_hexapod); +micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci); +micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr); +micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr); +micro_hexapod = computeJacobian(micro_hexapod); +[Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB); +micro_hexapod.Li = Li; +micro_hexapod.dLi = dLi; ++
+Equilibrium position of the each joint. +
+
+
- %% Stewart Object
- micro_hexapod = struct();
- micro_hexapod.h = 350; % Total height of the platform [mm]
- micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
+micro_hexapod.dLeq = args.dLeq; ++
+The micro_hexapod structure is saved.
+
+
+save('./mat/stages.mat', 'micro_hexapod', '-append'); ++
+
+
+7 Center of gravity compensation
+ - BP.rad.int = 110; % Internal Radius [mm] - BP.rad.ext = 207.5; % External Radius [mm] - BP.thickness = 26; % Thickness [mm] - BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm] - BP.leg.ang = 9.5; % Angle Offset [deg] - BP.density = 8000; % Density of the material [kg/m^3] - BP.color = [0.6 0.6 0.6]; % Color [rgb] - BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness]; - - %% Top Plate - Mechanical Design - TP = struct(); - - TP.rad.int = 82; % Internal Radius [mm] - TP.rad.ext = 150; % Internal Radius [mm] - TP.thickness = 26; % Thickness [mm] - TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm] - TP.leg.ang = 12.1; % Angle Offset [deg] - TP.density = 8000; % Density of the material [kg/m^3] - TP.color = [0.6 0.6 0.6]; % Color [rgb] - TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness]; - - %% Struts - Leg = struct(); - - Leg.stroke = 10e-3; % Maximum Stroke of each leg [m] - if opts.rigid - Leg.k.ax = 1e12; % Stiffness of each leg [N/m] - else - Leg.k.ax = 2e7; % Stiffness of each leg [N/m] - end - Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) [] - Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm] - Leg.rad.top = 17; % Radius of the cylinder of the top part [mm] - Leg.density = 8000; % Density of the material [kg/m^3] - Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb] - Leg.color.top = [0.5 0.5 0.5]; % Color [rgb] - - Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm] - Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm] - Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg] - Leg = updateDamping(Leg); +
+
- for i = 1:6
- stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
- stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
- end
+Simscape Model
+
+
++The Simscape model of the Center of gravity compensator is composed of: +
+-
+
- One main solid that is connected to two other solids (the masses to position of center of mass) through two revolute joints +
- The angle of both revolute joints is set by the input +
+
- %% Coordinate systems
- stewart.lower_leg = struct('rotation', eye(3));
- stewart.upper_leg = struct('rotation', eye(3));
+
+
Figure 14: Simscape model for the Center of Mass compensation system
+
+
+
+
Figure 15: Simscape picture for the Center of Mass compensation system
+
+
- %% Position Matrix
- stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
+Function description
+
+
+
+
+function [axisc] = initializeAxisc() ++
+
- %% Compute Jacobian Matrix
- aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
- stewart.J = getJacobianMatrix(leg_vectors', aa');
- end
- %%
- function J = getJacobianMatrix(RM, M_pos_base)
- % RM: [3x6] unit vector of each leg in the fixed frame
- % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
- J = zeros(6);
- J(:, 1:3) = RM';
- J(:, 4:6) = cross(M_pos_base, RM)';
- end
+Optional Parameters
+
+
+
+
+Function content
+
+
+
+First, we initialize the axisc structure.
+
+
+
+axisc = struct(); ++
+Properties of the Material and link to the geometry files. +
+
+
+
+% Structure +axisc.structure.density = 3400; % [kg/m3] +axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP'; + +% Wheel +axisc.wheel.density = 2700; % [kg/m3] +axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP'; + +% Mass +axisc.mass.density = 7800; % [kg/m3] +axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP'; + +% Gear +axisc.gear.density = 7800; % [kg/m3] +axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP'; ++
+The axisc structure is saved.
+
+
+save('./mat/stages.mat', 'axisc', '-append'); ++
+
8 Mirror
+ + +
+
+
+Simscape Model
+
+
+
+The Simscape Model of the mirror is just a solid body.
+The output mirror_center corresponds to the center of the Sphere and is the point of measurement for the metrology
+
+
+
+
+
+
Figure 16: Simscape model for the Mirror
+
+
+
+
Figure 17: Simscape picture for the Mirror
+
+
+
+Function description
+
+
+
+
+function [] = initializeMirror(args) ++
+
-Optional Parameters
+
+
+
arguments
+ args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
+ args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
end
-
2.7 Center of gravity compensation
-
+
+
+
+
+
+Function content
+
-
+First, we initialize the mirror structure.
+
mirror = struct(); ++
-This Matlab function is accessible here. +We define the geometrical values.
+
+
mirror.h = 50; % Height of the mirror [mm] +mirror.thickness = 25; % Thickness of the plate supporting the sample [mm] +mirror.hole_rad = 120; % radius of the hole in the mirror [mm] +mirror.support_rad = 100; % radius of the support plate [mm] +mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm] +mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm] ++
-
- %% Axis Compensator - Static Properties
- % Structure
- axisc.structure.density = 3400; % [kg/m3]
- axisc.structure.color = [0.792 0.820 0.933];
- axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP';
- % Wheel
- axisc.wheel.density = 2700; % [kg/m3]
- axisc.wheel.color = [0.753 0.753 0.753];
- axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP';
- % Mass
- axisc.mass.density = 7800; % [kg/m3]
- axisc.mass.color = [0.792 0.820 0.933];
- axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP';
- % Gear
- axisc.gear.density = 7800; % [kg/m3]
- axisc.gear.color = [0.792 0.820 0.933];
- axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP';
+function [axisc] = initializeAxisc()
- %%
- axisc = struct();
+mirror.density = 2400; % Density of the material [kg/m3]
+
+
+
- axisc.m = 40; % TODO [kg]
+mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point ++
+Now we define the Shape of the mirror. +We first start with the internal part. +
+
+
- %% Axis Compensator - Dynamical Properties
- % axisc.k.ax = 1; % TODO [N*m/deg)]
- % axisc.c.ax = (1/1)*sqrt(axisc.k.ax/axisc.m);
+mirror.shape = [... + 0 mirror.h-mirror.thickness + mirror.hole_rad mirror.h-mirror.thickness; ... + mirror.hole_rad 0; ... + mirror.rad 0 ... +]; ++
+Then, we define the reflective used part of the mirror. +
+
+
+
+if strcmp(args.shape, 'spherical') + mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm] - %% Save - save('./mat/stages.mat', 'axisc', '-append'); + 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(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 ++
+Finally, we close the shape. +
+
+
+
+mirror.shape = [mirror.shape; 0 mirror.h]; ++
+The mirror structure is saved.
+
+
+save('./mat/stages.mat', 'mirror', '-append'); ++
+
9 Nano Hexapod
+ + +
+
+
+Simscape Model
+
+
+
+
+
+
+
+
+
Figure 18: Simscape model for the Nano Hexapod
+
+
+
+
Figure 19: Simscape picture for the Nano Hexapod
+
+
+
+Function description
+
+
+
+
+function [nano_hexapod] = initializeNanoHexapod(args) ++
+
-Optional Parameters
+
+
+
arguments + % initializeFramesPositions + args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 + args.MO_B (1,1) double {mustBeNumeric} = 175e-3 + % generateGeneralConfiguration + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180) + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180) + % initializeStrutDynamics + args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo' + % initializeCylindricalPlatforms + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + % initializeCylindricalStruts + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + % 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) end
-
2.8 Mirror
-
-
-
-
- %%
- mirror = struct();
- mirror.h = 50; % height of the mirror [mm]
- mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
- mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
- mirror.support_rad = 100; % radius of the support plate [mm]
- mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
- mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
+-This Matlab function is accessible here. -
+
+
+Function content
+
+
+
+
nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B); +nano_hexapod = generateGeneralConfiguration(nano_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh); +nano_hexapod = computeJointsPose(nano_hexapod); +if strcmp(args.actuator, 'piezo') + nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e7*ones(6,1), 'Ci', 1e2*ones(6,1)); +elseif strcmp(args.actuator, 'lorentz') + nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e4*ones(6,1), 'Ci', 1e2*ones(6,1)); +else + error('args.actuator should be piezo or lorentz'); +end +nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr); +nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr); +nano_hexapod = computeJacobian(nano_hexapod); +[Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB); +nano_hexapod.Li = Li; +nano_hexapod.dLi = dLi; ++
-
- %% 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 [] = initializeMirror(opts_param) - %% Default values for opts - opts = struct(... - 'shape', 'spherical', ... % spherical or conical - 'angle', 45 ... - ); +nano_hexapod.dLeq = args.dLeq; ++
+
+save('./mat/stages.mat', 'nano_hexapod', '-append'); ++
+
10 Sample
+ - mirror.density = 2400; % Density of the mirror [kg/m3] - mirror.color = [0.4 1.0 1.0]; % Color of the mirror +
+
- mirror.shape = [mirror.shape; 0 mirror.h];
+Simscape Model
+
+
++The Simscape model of the sample environment is composed of: +
+-
+
- A rigid transform that can be used to translate the sample (position offset) +
- A cartesian joint to add some flexibility to the sample environment mount +
- A solid that represent the sample +
- An input is added to apply some external forces and torques at the center of the sample environment. +This could be the case for cable forces for instance. +
+
- if strcmp(opts.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];
- else
- error('Shape should be either conical or spherical');
- end
+
+
Figure 20: Simscape model for the Sample
+
+
+
+
Figure 21: Simscape picture for the Sample
+
+
- %% Save
- save('./mat/stages.mat', 'mirror', '-append');
+Function description
+
+
+
+
+function [sample] = initializeSample(args) ++
+
-Optional Parameters
+
+
+
arguments
+ 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]
end
-
2.9 Nano Hexapod
-
+
- TP.rad.int = 0; % Internal Radius [mm]
- TP.rad.ext = 100; % Internal Radius [mm]
- TP.thickness = 10; % Thickness [mm]
- TP.leg.rad = 90; % Radius where the legs articulations are positionned [mm]
- TP.leg.ang = 5; % Angle Offset [deg]
- TP.density = 8000;% Density of the material [kg/m^3]
- TP.color = [0.7 0.7 0.7]; % Color [rgb]
- TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
+
+
+
+Function content
+
-
+First, we initialize the sample structure.
-This Matlab function is accessible here. -
-
-
- %% 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 [nano_hexapod] = initializeNanoHexapod(opts_param) - %% Default values for opts - opts = struct('actuator', 'piezo'); +sample = struct(); ++
+We define the geometrical parameters of the sample as well as its mass and position. +
+
+
- %% Stewart Object
- nano_hexapod = struct();
- nano_hexapod.h = 90; % Total height of the platform [mm]
- nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm]
-% nano_hexapod.jacobian = 174.26; % Point where the Jacobian is computed => Center of rotation [mm]
+sample.radius = args.radius; % [m] +sample.height = args.height; % [m] +sample.mass = args.mass; % [kg] +sample.offset = args.offset; % [m] ++
+Stiffness of the sample fixation. +
+
+
- %% Bottom Plate
- BP = struct();
+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] ++
+Damping of the sample fixation. +
+
+
- BP.rad.int = 0; % Internal Radius [mm]
- BP.rad.ext = 150; % External Radius [mm]
- BP.thickness = 10; % Thickness [mm]
- BP.leg.rad = 100; % Radius where the legs articulations are positionned [mm]
- BP.leg.ang = 5; % Angle Offset [deg]
- BP.density = 8000;% Density of the material [kg/m^3]
- BP.color = [0.7 0.7 0.7]; % Color [rgb]
- BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
+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)] ++
+Equilibrium position of the Cartesian joint corresponding to the sample fixation. +
+
+
- %% Top Plate
- TP = struct();
+sample.x0 = args.x0; % [m] +sample.y0 = args.y0; % [m] +sample.z0 = args.z0; % [m] ++
+The sample structure is saved.
+
+
+save('./mat/stages.mat', 'sample', '-append'); ++
+
11 Generate Reference Signals
+ - %% Leg - Leg = struct(); +
+
- Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
- if strcmp(opts.actuator, 'piezo')
- Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
- elseif strcmp(opts.actuator, 'lorentz')
- Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
- else
- error('opts.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]
- Leg.rad.top = 10; % Radius of the cylinder of the top part [mm]
- Leg.density = 8000; % Density of the material [kg/m^3]
- Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
- Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
-
- Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
- Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
- Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
-
- Leg = updateDamping(Leg);
-
-
- %% Sphere
- SP = struct();
-
- SP.height.bottom = 15; % [mm]
- SP.height.top = 15; % [mm]
- SP.density.bottom = 8000; % [kg/m^3]
- SP.density.top = 8000; % [kg/m^3]
- SP.color.bottom = [0.7 0.7 0.7]; % [rgb]
- SP.color.top = [0.7 0.7 0.7]; % [rgb]
- SP.k.ax = 0; % [N*m/deg]
- SP.ksi.ax = 0;
-
- SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
- SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
- SP.rad.bottom = Leg.sphere.bottom; % [mm]
- SP.rad.top = Leg.sphere.top; % [mm]
- SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
-
- SP = updateDamping(SP);
-
- %%
- Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
- Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
-
- %%
- nano_hexapod.BP = BP;
- nano_hexapod.TP = TP;
- nano_hexapod.Leg = Leg;
- nano_hexapod.SP = SP;
-
- %%
- nano_hexapod = initializeParameters(nano_hexapod);
-
- %% Save
- save('./mat/stages.mat', 'nano_hexapod', '-append');
-
- %%
- function [element] = updateDamping(element)
- field = fieldnames(element.k);
- for i = 1:length(field)
- if element.ksi.(field{i}) > 0
- element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
- else
- element.c.(field{i}) = 0;
- end
- end
- end
-
- %%
- function [stewart] = initializeParameters(stewart)
- %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
- stewart.pos_base = zeros(6, 3);
- stewart.pos_top = zeros(6, 3);
-
- alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
- alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
-
- height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
-
- radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
- radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
-
- for i = 1:3
- % base points
- angle_m_b = (2*pi/3)* (i-1) - alpha_b;
- angle_p_b = (2*pi/3)* (i-1) + alpha_b;
- stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
- stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
-
- % top points
- % Top points are 60 degrees offset
- angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
- angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
- stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
- stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
- end
-
- % permute pos_top points so that legs are end points of base and top points
- stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
- stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
-
- %% leg vectors
- legs = stewart.pos_top - stewart.pos_base;
- leg_length = zeros(6, 1);
- leg_vectors = zeros(6, 3);
- for i = 1:6
- leg_length(i) = norm(legs(i,:));
- leg_vectors(i,:) = legs(i,:) / leg_length(i);
- end
-
- stewart.Leg.lenght = 1000*leg_length(1)/1.5;
- stewart.Leg.shape.bot = [0 0; ...
- stewart.Leg.rad.bottom 0; ...
- stewart.Leg.rad.bottom stewart.Leg.lenght; ...
- stewart.Leg.rad.top stewart.Leg.lenght; ...
- stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
- 0 0.2*stewart.Leg.lenght];
-
- %% Calculate revolute and cylindrical axes
- rev1 = zeros(6, 3);
- rev2 = zeros(6, 3);
- cyl1 = zeros(6, 3);
- for i = 1:6
- rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
- rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
-
- rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
- rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
-
- cyl1(i,:) = leg_vectors(i,:);
- end
-
-
- %% Coordinate systems
- stewart.lower_leg = struct('rotation', eye(3));
- stewart.upper_leg = struct('rotation', eye(3));
-
- for i = 1:6
- stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
- stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
- end
-
- %% Position Matrix
- stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
-
- %% Compute Jacobian Matrix
- aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
- stewart.J = getJacobianMatrix(leg_vectors', aa');
- end
-
- function J = getJacobianMatrix(RM,M_pos_base)
- % RM: [3x6] unit vector of each leg in the fixed frame
- % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
- J = zeros(6);
- J(:, 1:3) = RM';
- J(:, 4:6) = cross(M_pos_base, RM)';
- end
+Function Declaration and Documentation
+
+
+
+
+function [ref] = initializeReferences(args) ++
+
-Optional Parameters
+
+
+
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
-
2.10 Cedrat Actuator
-
+
+
+
+
+
+
+Initialize Parameters
+
+
+
+
+%% Set Sampling Time +Ts = args.Ts; +Tmax = args.Tmax; + +%% Low Pass Filter to filter out the references +s = zpk('s'); +w0 = 2*pi*10; +xi = 1; +H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2); ++
+
+
+Translation Stage
+
+
+
+
+%% Translation stage - Dy +t = 0:Ts:Tmax; % Time Vector [s] +Dy = zeros(length(t), 1); +Dyd = zeros(length(t), 1); +Dydd = zeros(length(t), 1); +switch args.Dy_type + case 'constant' + Dy(:) = args.Dy_amplitude; + Dyd(:) = 0; + Dydd(:) = 0; + case 'triangular' + % This is done to unsure that we start with no displacement + 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 + + % The signal is filtered out + Dy = lsim(H_lpf, Dy, t); + Dyd = lsim(H_lpf*s, Dy, t); + Dydd = lsim(H_lpf*s^2, Dy, t); + case 'sinusoidal' + 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 + +Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd); ++
+
+
+Tilt Stage
+
+
+
+
+%% Tilt Stage - Ry +t = 0:Ts:Tmax; % Time Vector [s] +Ry = zeros(length(t), 1); +Ryd = zeros(length(t), 1); +Rydd = zeros(length(t), 1); + +switch args.Ry_type + case 'constant' + Ry(:) = args.Ry_amplitude; + Ryd(:) = 0; + Rydd(:) = 0; + case 'triangular' + 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 + + % The signal is filtered out + Ry = lsim(H_lpf, Ry, t); + Ryd = lsim(H_lpf*s, Ry, t); + Rydd = lsim(H_lpf*s^2, Ry, t); + case 'sinusoidal' + Ry(:) = args.Ry_amplitude*sin(2*pi/args.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 + +Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd); ++
+
+
+Spindle
+
+
+
+
+%% Spindle - Rz +t = 0:Ts:Tmax; % Time Vector [s] +Rz = zeros(length(t), 1); +Rzd = zeros(length(t), 1); +Rzdd = zeros(length(t), 1); + +switch args.Rz_type + case 'constant' + Rz(:) = args.Rz_amplitude; + Rzd(:) = 0; + Rzdd(:) = 0; + case 'rotating' + Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t; + + % The signal is filtered out + Rz = lsim(H_lpf, Rz, t); + Rzd = lsim(H_lpf*s, Rz, t); + Rzdd = lsim(H_lpf*s^2, Rz, t); + otherwise + warning('Rz_type is not set correctly'); +end + +Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd); ++
+
+
+Micro Hexapod
+
+
+
+
+%% Micro-Hexapod +t = [0, Ts]; +Dh = zeros(length(t), 6); +Dhl = zeros(length(t), 6); + +switch args.Dh_type + case 'constant' + Dh = [args.Dh_pos, args.Dh_pos]; + + load('mat/stages.mat', 'micro_hexapod'); + + AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)]; + + 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; + 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)]; + + [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB); + Dhl = [Dhl, Dhl]; + otherwise + warning('Dh_type is not set correctly'); +end + +Dh = struct('time', t, 'signals', struct('values', Dh)); +Dhl = struct('time', t, 'signals', struct('values', Dhl)); ++
+
+
+Axis Compensation
+
+
+
+
+%% Axis Compensation - Rm +t = [0, Ts]; + +Rm = [args.Rm_pos, args.Rm_pos]; +Rm = struct('time', t, 'signals', struct('values', Rm)); ++
+
+
+Nano Hexapod
+
+
+
+
+%% Nano-Hexapod +t = [0, Ts]; +Dn = zeros(length(t), 6); + +switch args.Dn_type + case 'constant' + Dn = [args.Dn_pos, args.Dn_pos]; + otherwise + warning('Dn_type is not set correctly'); +end + +Dn = struct('time', t, 'signals', struct('values', Dn)); ++
+
+Save
+
+
+
+
+%% Save + save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts'); +end ++
+>
-** Generate Reference Signals
+* Introduction :ignore:
+
+The full Simscape Model is represented in Figure [[fig:simscape_picture]].
+
+#+name: fig:simscape_picture
+#+caption: Screenshot of the Multi-Body Model representation
+[[file:figs/images/simscape_picture.png]]
+
+This model is divided into multiple subsystems that are independent.
+These subsystems are saved in separate files and imported in the main file using a block balled "subsystem reference".
+
+Each stage is configured (geometry, mass properties, dynamic properties ...) using one function.
+
+These functions are defined below.
+
+* Ground
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeGround.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The model of the Ground is composed of:
+- A *Cartesian* joint that is used to simulation the ground motion
+- A solid that represents the ground on which the granite is located
+
+#+name: fig:simscape_model_ground
+#+attr_org: :width 800
+#+caption: Simscape model for the Ground
+[[file:figs/images/simscape_model_ground.png]]
+
+#+name: fig:simscape_picture_ground
+#+attr_org: :width 800
+#+caption: Simscape picture for the Ground
+[[file:figs/images/simscape_picture_ground.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [ground] = initializeGround()
+#+end_src
+
+** Function content
+First, we initialize the =granite= structure.
+#+begin_src matlab
+ ground = struct();
+#+end_src
+
+We set the shape and density of the ground solid element.
+#+begin_src matlab
+ ground.shape = [2, 2, 0.5]; % [m]
+ ground.density = 2800; % [kg/m3]
+#+end_src
+
+The =ground= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'ground', '-append');
+#+end_src
+
+* Granite
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeGranite.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the granite is composed of:
+- A cartesian joint such that the granite can vibrations along x, y and z axis
+- A solid
+
+The output =sample_pos= corresponds to the impact point of the X-ray.
+
+#+name: fig:simscape_model_granite
+#+attr_org: :width 800
+#+caption: Simscape model for the Granite
+[[file:figs/images/simscape_model_granite.png]]
+
+#+name: fig:simscape_picture_granite
+#+attr_org: :width 800
+#+caption: Simscape picture for the Granite
+[[file:figs/images/simscape_picture_granite.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [granite] = initializeGranite(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ 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]
+ args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =granite= structure.
+#+begin_src matlab
+ granite = struct();
+#+end_src
+
+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
+
+The =granite= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'granite', '-append');
+#+end_src
+
+* Translation Stage
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeTy.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Translation stage consist of:
+- One rigid body for the fixed part of the translation stage
+- One rigid body for the moving part of the translation stage
+- Four 6-DOF Joints that only have some rigidity in the X and Z directions.
+ The rigidity in rotation comes from the fact that we use multiple joints that are located at different points
+- One 6-DOF joint that represent the Actuator.
+ It is used to impose the motion in the Y direction
+- One 6-DOF joint to inject force disturbance in the X and Z directions
+
+#+name: fig:simscape_model_ty
+#+ATTR_ORG: :width 800
+#+caption: Simscape model for the Translation Stage
+[[file:figs/images/simscape_model_ty.png]]
+
+#+name: fig:simscape_picture_ty
+#+attr_org: :width 800
+#+caption: Simscape picture for the Translation Stage
+[[file:figs/images/simscape_picture_ty.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [ty] = initializeTy(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ 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]
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =ty= structure.
+#+begin_src matlab
+ ty = struct();
+#+end_src
+
+Define the density of the materials as well as the geometry (STEP files).
+#+begin_src matlab
+ % Ty Granite frame
+ ty.granite_frame.density = 7800; % [kg/m3] => 43kg
+ ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP';
+
+ % Guide Translation Ty
+ ty.guide.density = 7800; % [kg/m3] => 76kg
+ ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP';
+
+ % Ty - Guide_Translation12
+ ty.guide12.density = 7800; % [kg/m3]
+ ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP';
+
+ % Ty - Guide_Translation11
+ ty.guide11.density = 7800; % [kg/m3]
+ ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP';
+
+ % Ty - Guide_Translation22
+ ty.guide22.density = 7800; % [kg/m3]
+ ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP';
+
+ % Ty - Guide_Translation21
+ ty.guide21.density = 7800; % [kg/m3]
+ ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP';
+
+ % Ty - Plateau translation
+ ty.frame.density = 7800; % [kg/m3]
+ ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP';
+
+ % Ty Stator Part
+ ty.stator.density = 5400; % [kg/m3]
+ ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
+
+ % Ty Rotor Part
+ ty.rotor.density = 5400; % [kg/m3]
+ ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
+#+end_src
+
+Stiffness of the stage.
+#+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]
+#+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 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;
+#+end_src
+
+The =ty= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'ty', '-append');
+#+end_src
+
+* Tilt Stage
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeRy.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Tilt stage is composed of:
+- Two solid bodies for the two part of the stage
+- *Four* 6-DOF joints to model the flexibility of the stage.
+ These joints are virtually located along the rotation axis and are connecting the two solid bodies.
+ These joints have some translation stiffness in the u-v-w directions aligned with the joint.
+ The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations
+- A Bushing Joint used for the Actuator.
+ The Ry motion is imposed by the input.
+
+#+name: fig:simscape_model_ry
+#+attr_org: :width 800
+#+caption: Simscape model for the Tilt Stage
+[[file:figs/images/simscape_model_ry.png]]
+
+#+name: fig:simscape_picture_ry
+#+attr_org: :width 800
+#+caption: Simscape picture for the Tilt Stage
+[[file:figs/images/simscape_picture_ry.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [ry] = initializeRy(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ 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]
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =ry= structure.
+#+begin_src matlab
+ ry = struct();
+#+end_src
+
+Properties of the Material and link to the geometry of the Tilt stage.
+#+begin_src matlab
+ % Ry - Guide for the tilt stage
+ ry.guide.density = 7800; % [kg/m3]
+ ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP';
+
+ % Ry - Rotor of the motor
+ ry.rotor.density = 2400; % [kg/m3]
+ ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP';
+
+ % Ry - Motor
+ ry.motor.density = 3200; % [kg/m3]
+ ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
+
+ % Ry - Plateau Tilt
+ ry.stage.density = 7800; % [kg/m3]
+ 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
+
+The =ty= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'ry', '-append');
+#+end_src
+
+* Spindle
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeRz.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Spindle is composed of:
+- Two rigid bodies: the stator and the rotor
+- A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.
+- The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations
+
+#+name: fig:simscape_model_rz
+#+attr_org: :width 800
+#+caption: Simscape model for the Spindle
+[[file:figs/images/simscape_model_rz.png]]
+
+#+name: fig:simscape_picture_rz
+#+attr_org: :width 800
+#+caption: Simscape picture for the Spindle
+[[file:figs/images/simscape_picture_rz.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [rz] = initializeRz(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ args.rigid logical {mustBeNumericOrLogical} = false
+ 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.rx0 (1,1) double {mustBeNumeric} = 0 % [rad]
+ args.ry0 (1,1) double {mustBeNumeric} = 0 % [rad]
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =rz= structure.
+#+begin_src matlab
+ rz = struct();
+#+end_src
+
+Properties of the Material and link to the geometry of the spindle.
+#+begin_src matlab
+ % Spindle - Slip Ring
+ rz.slipring.density = 7800; % [kg/m3]
+ rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP';
+
+ % Spindle - Rotor
+ rz.rotor.density = 7800; % [kg/m3]
+ rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
+
+ % Spindle - Stator
+ rz.stator.density = 7800; % [kg/m3]
+ rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
+#+end_src
+
+Stiffness of the stage.
+#+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]
+#+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 joints.
+#+begin_src matlab
+ rz.x0 = args.x0;
+ rz.y0 = args.y0;
+ rz.z0 = args.z0;
+ rz.rx0 = args.rx0;
+ rz.ry0 = args.ry0;
+#+end_src
+
+The =rz= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'rz', '-append');
+#+end_src
+
+* Micro Hexapod
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+#+name: fig:simscape_model_micro_hexapod
+#+attr_org: :width 800
+#+caption: Simscape model for the Micro-Hexapod
+[[file:figs/images/simscape_model_micro_hexapod.png]]
+
+#+name: fig:simscape_picture_micro_hexapod
+#+attr_org: :width 800
+#+caption: Simscape picture for the Micro-Hexapod
+[[file:figs/images/simscape_picture_micro_hexapod.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [micro_hexapod] = initializeMicroHexapod(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ % initializeFramesPositions
+ args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
+ args.MO_B (1,1) double {mustBeNumeric} = 270e-3
+ % generateGeneralConfiguration
+ args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+ args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
+ args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
+ args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
+ args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
+ args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
+ % initializeStrutDynamics
+ args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
+ args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
+ % initializeCylindricalPlatforms
+ args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
+ args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
+ args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
+ args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
+ args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
+ args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
+ % initializeCylindricalStruts
+ args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
+ args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+ args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
+ args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
+ args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+ args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
+ % 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)
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
+ micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
+ micro_hexapod = computeJointsPose(micro_hexapod);
+ micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
+ micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
+ micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
+ micro_hexapod = computeJacobian(micro_hexapod);
+ [Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
+ micro_hexapod.Li = Li;
+ micro_hexapod.dLi = dLi;
+#+end_src
+
+Equilibrium position of the each joint.
+#+begin_src matlab
+ micro_hexapod.dLeq = args.dLeq;
+#+end_src
+
+The =micro_hexapod= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'micro_hexapod', '-append');
+#+end_src
+
+* Center of gravity compensation
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeAxisc.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the Center of gravity compensator is composed of:
+- One main solid that is connected to two other solids (the masses to position of center of mass) through two revolute joints
+- The angle of both revolute joints is set by the input
+
+#+name: fig:simscape_model_axisc
+#+attr_org: :width 800
+#+caption: Simscape model for the Center of Mass compensation system
+[[file:figs/images/simscape_model_axisc.png]]
+
+#+name: fig:simscape_picture_axisc
+#+attr_org: :width 800
+#+caption: Simscape picture for the Center of Mass compensation system
+[[file:figs/images/simscape_picture_axisc.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [axisc] = initializeAxisc()
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =axisc= structure.
+#+begin_src matlab
+ axisc = struct();
+#+end_src
+
+Properties of the Material and link to the geometry files.
+#+begin_src matlab
+ % Structure
+ axisc.structure.density = 3400; % [kg/m3]
+ axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP';
+
+ % Wheel
+ axisc.wheel.density = 2700; % [kg/m3]
+ axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP';
+
+ % Mass
+ axisc.mass.density = 7800; % [kg/m3]
+ axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP';
+
+ % Gear
+ axisc.gear.density = 7800; % [kg/m3]
+ axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP';
+#+end_src
+
+The =axisc= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'axisc', '-append');
+#+end_src
+
+* Mirror
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeMirror.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape Model of the mirror is just a solid body.
+The output =mirror_center= corresponds to the center of the Sphere and is the point of measurement for the metrology
+
+#+name: fig:simscape_model_mirror
+#+attr_org: :width 800
+#+caption: Simscape model for the Mirror
+[[file:figs/images/simscape_model_mirror.png]]
+
+#+name: fig:simscape_picture_mirror
+#+attr_org: :width 800
+#+caption: Simscape picture for the Mirror
+[[file:figs/images/simscape_picture_mirror.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [] = initializeMirror(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
+ args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =mirror= structure.
+#+begin_src matlab
+ mirror = struct();
+#+end_src
+
+We define the geometrical values.
+#+begin_src matlab
+ mirror.h = 50; % Height of the mirror [mm]
+ mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
+ mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
+ mirror.support_rad = 100; % radius of the support plate [mm]
+ mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
+ mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
+#+end_src
+
+#+begin_src matlab
+ mirror.density = 2400; % Density of the material [kg/m3]
+#+end_src
+
+#+begin_src matlab
+ mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
+#+end_src
+
+Now we define the Shape of the mirror.
+We first start with the internal part.
+#+begin_src matlab
+ mirror.shape = [...
+ 0 mirror.h-mirror.thickness
+ mirror.hole_rad mirror.h-mirror.thickness; ...
+ mirror.hole_rad 0; ...
+ mirror.rad 0 ...
+ ];
+#+end_src
+
+Then, we define the reflective used part of the mirror.
+#+begin_src matlab
+ 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(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
+#+end_src
+
+Finally, we close the shape.
+#+begin_src matlab
+ mirror.shape = [mirror.shape; 0 mirror.h];
+#+end_src
+
+The =mirror= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'mirror', '-append');
+#+end_src
+
+* Nano Hexapod
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeNanoHexapod.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+name: fig:simscape_model_nano_hexapod
+#+attr_org: :width 800
+#+caption: Simscape model for the Nano Hexapod
+[[file:figs/images/simscape_model_nano_hexapod.png]]
+
+#+name: fig:simscape_picture_nano_hexapod
+#+attr_org: :width 800
+#+caption: Simscape picture for the Nano Hexapod
+[[file:figs/images/simscape_picture_nano_hexapod.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [nano_hexapod] = initializeNanoHexapod(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ % initializeFramesPositions
+ args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
+ args.MO_B (1,1) double {mustBeNumeric} = 175e-3
+ % generateGeneralConfiguration
+ args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+ args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+ args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
+ args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+ args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
+ args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
+ % initializeStrutDynamics
+ args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
+ % initializeCylindricalPlatforms
+ args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
+ args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
+ args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
+ args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
+ args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
+ args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+ % initializeCylindricalStruts
+ args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+ args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+ args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
+ args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+ args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+ args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
+ % 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)
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
+ nano_hexapod = generateGeneralConfiguration(nano_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
+ nano_hexapod = computeJointsPose(nano_hexapod);
+ if strcmp(args.actuator, 'piezo')
+ nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e7*ones(6,1), 'Ci', 1e2*ones(6,1));
+ elseif strcmp(args.actuator, 'lorentz')
+ nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e4*ones(6,1), 'Ci', 1e2*ones(6,1));
+ else
+ error('args.actuator should be piezo or lorentz');
+ end
+ nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
+ nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
+ nano_hexapod = computeJacobian(nano_hexapod);
+ [Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
+ nano_hexapod.Li = Li;
+ nano_hexapod.dLi = dLi;
+#+end_src
+
+#+begin_src matlab
+ nano_hexapod.dLeq = args.dLeq;
+#+end_src
+
+#+begin_src matlab
+ save('./mat/stages.mat', 'nano_hexapod', '-append');
+#+end_src
+
+* Sample
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initializeSample.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+** Simscape Model
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The Simscape model of the sample environment is composed of:
+- A rigid transform that can be used to translate the sample (position offset)
+- A cartesian joint to add some flexibility to the sample environment mount
+- A solid that represent the sample
+- An input is added to apply some external forces and torques at the center of the sample environment.
+ This could be the case for cable forces for instance.
+
+#+name: fig:simscape_model_sample
+#+attr_org: :width 800
+#+caption: Simscape model for the Sample
+[[file:figs/images/simscape_model_sample.png]]
+
+#+name: fig:simscape_picture_sample
+#+attr_org: :width 800
+#+caption: Simscape picture for the Sample
+[[file:figs/images/simscape_picture_sample.png]]
+
+** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ function [sample] = initializeSample(args)
+#+end_src
+
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+ arguments
+ 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]
+ end
+#+end_src
+
+** Function content
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+First, we initialize the =sample= structure.
+#+begin_src matlab
+ sample = struct();
+#+end_src
+
+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.offset = args.offset; % [m]
+#+end_src
+
+Stiffness of the sample fixation.
+#+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]
+#+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 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]
+#+end_src
+
+The =sample= structure is saved.
+#+begin_src matlab
+ save('./mat/stages.mat', 'sample', '-append');
+#+end_src
+
+* Generate Reference Signals
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeReferences.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<>
-This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
-
-*** Function Declaration and Documentation
+** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
function [ref] = initializeReferences(args)
#+end_src
-*** Optional Parameters
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
arguments
% Sampling Frequency [s]
@@ -99,7 +1103,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
#+end_src
-*** Initialize Parameters
+** Initialize Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Set Sampling Time
Ts = args.Ts;
@@ -112,7 +1119,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
#+end_src
-*** Translation Stage
+** Translation Stage
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Translation stage - Dy
t = 0:Ts:Tmax; % Time Vector [s]
@@ -146,7 +1156,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
#+end_src
-*** Tilt Stage
+** Tilt Stage
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Tilt Stage - Ry
t = 0:Ts:Tmax; % Time Vector [s]
@@ -181,7 +1194,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
#+end_src
-*** Spindle
+** Spindle
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Spindle - Rz
t = 0:Ts:Tmax; % Time Vector [s]
@@ -208,46 +1224,52 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
#+end_src
-*** Micro Hexapod
+** Micro Hexapod
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
- %% Micro-Hexapod
- t = [0, Ts];
- Dh = zeros(length(t), 6);
- Dhl = zeros(length(t), 6);
+ %% Micro-Hexapod
+ t = [0, Ts];
+ Dh = zeros(length(t), 6);
+ Dhl = zeros(length(t), 6);
- switch args.Dh_type
- case 'constant'
- Dh = [args.Dh_pos, args.Dh_pos];
+ switch args.Dh_type
+ case 'constant'
+ Dh = [args.Dh_pos, args.Dh_pos];
- load('mat/stages.mat', 'micro_hexapod');
+ load('mat/stages.mat', 'micro_hexapod');
- AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
+ AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
- tx = args.Dh_pos(4);
- ty = args.Dh_pos(5);
- tz = args.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;
- 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)];
+ 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)];
- [Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
- Dhl = [Dhl, Dhl];
- otherwise
- warning('Dh_type is not set correctly');
- end
+ [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
+ Dhl = [Dhl, Dhl];
+ otherwise
+ warning('Dh_type is not set correctly');
+ end
- Dh = struct('time', t, 'signals', struct('values', Dh));
- Dhl = struct('time', t, 'signals', struct('values', Dhl));
+ Dh = struct('time', t, 'signals', struct('values', Dh));
+ Dhl = struct('time', t, 'signals', struct('values', Dhl));
#+end_src
-*** Axis Compensation
+** Axis Compensation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Axis Compensation - Rm
t = [0, Ts];
@@ -256,7 +1278,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Rm = struct('time', t, 'signals', struct('values', Rm));
#+end_src
-*** Nano Hexapod
+** Nano Hexapod
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Nano-Hexapod
t = [0, Ts];
@@ -272,14 +1297,17 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
Dn = struct('time', t, 'signals', struct('values', Dn));
#+end_src
-*** Save
+** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
%% Save
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
end
#+end_src
-** Initialize Disturbances
+* Initialize Disturbances
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeDisturbances.m
:header-args:matlab+: :comments none :mkdirp yes
@@ -287,9 +1315,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
:END:
<>
-This Matlab function is accessible [[file:src/initializeDisturbances.m][here]].
-
-*** Function Declaration and Documentation
+** Function Declaration and Documentation
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
function [] = initializeDisturbances(args)
% initializeDisturbances - Initialize the disturbances
@@ -301,7 +1330,10 @@ This Matlab function is accessible [[file:src/initializeDisturbances.m][here]].
#+end_src
-*** Optional Parameters
+** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
arguments
% Global parameter to enable or disable the disturbances
@@ -322,7 +1354,10 @@ This Matlab function is accessible [[file:src/initializeDisturbances.m][here]].
#+end_src
-*** Load Data
+** Load Data
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
load('./disturbances/mat/dist_psd.mat', 'dist_f');
#+end_src
@@ -335,7 +1370,10 @@ We remove the first frequency point that usually is very large.
dist_f.psd_rz = dist_f.psd_rz(2:end);
#+end_src
-*** Parameters
+** Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
We define some parameters that will be used in the algorithm.
#+begin_src matlab
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
@@ -347,7 +1385,10 @@ We define some parameters that will be used in the algorithm.
Ts = 1/Fs; % Sampling Time [s]
#+end_src
-*** Ground Motion
+** Ground Motion
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
phi = dist_f.psd_gm;
C = zeros(N/2,1);
@@ -389,7 +1430,10 @@ We define some parameters that will be used in the algorithm.
end
#+end_src
-*** Translation Stage - X direction
+** Translation Stage - X direction
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
if args.Fty_x && args.enable
phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
@@ -407,7 +1451,10 @@ We define some parameters that will be used in the algorithm.
end
#+end_src
-*** Translation Stage - Z direction
+** Translation Stage - Z direction
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
if args.Fty_z && args.enable
phi = dist_f.psd_ty;
@@ -425,7 +1472,10 @@ We define some parameters that will be used in the algorithm.
end
#+end_src
-*** Spindle - Z direction
+** Spindle - Z direction
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
if args.Frz_z && args.enable
phi = dist_f.psd_rz;
@@ -443,13 +1493,19 @@ We define some parameters that will be used in the algorithm.
end
#+end_src
-*** Direct Forces
+** Direct Forces
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
u = zeros(length(t), 6);
Fd = u;
#+end_src
-*** Set initial value to zero
+** Set initial value to zero
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
Dwx = Dwx - Dwx(1);
Dwy = Dwy - Dwy(1);
@@ -459,20 +1515,21 @@ We define some parameters that will be used in the algorithm.
Frz_z = Frz_z - Frz_z(1);
#+end_src
-*** Save
+** Save
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
#+begin_src matlab
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
#+end_src
-** Z-Axis Geophone
+* Z-Axis Geophone
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeZAxisGeophone.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<>
-This Matlab function is accessible [[file:../src/initializeZAxisGeophone.m][here]].
-
#+begin_src matlab
function [geophone] = initializeZAxisGeophone(args)
arguments
@@ -494,15 +1551,13 @@ This Matlab function is accessible [[file:../src/initializeZAxisGeophone.m][here
end
#+end_src
-** Z-Axis Accelerometer
+* Z-Axis Accelerometer
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeZAxisAccelerometer.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<>
-This Matlab function is accessible [[file:../src/initializeZAxisAccelerometer.m][here]].
-
#+begin_src matlab
function [accelerometer] = initializeZAxisAccelerometer(args)
arguments
@@ -526,302 +1581,14 @@ This Matlab function is accessible [[file:../src/initializeZAxisAccelerometer.m]
save('./mat/accelerometer_z_axis.mat', 'accelerometer');
end
#+end_src
-
-* Initialize Elements
-:PROPERTIES:
-:ID: a0819dea-8d7a-4d55-b961-2b2ca2312344
-:END:
-<>
-** Ground
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeGround.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeGround.m][here]].
-
-#+begin_src matlab
-function [ground] = initializeGround()
- %%
- ground = struct();
-
- ground.shape = [2, 2, 0.5]; % [m]
- ground.density = 2800; % [kg/m3]
- ground.color = [0.5, 0.5, 0.5];
-
- %% Save
- save('./mat/stages.mat', 'ground', '-append');
-end
-#+end_src
-
-** Granite
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeGranite.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeGranite.m][here]].
-
-#+begin_src matlab
- function [granite] = initializeGranite(args)
- arguments
- args.rigid logical {mustBeNumericOrLogical} = false
- end
-
- %%
- granite = struct();
-
- %% Static Properties
- granite.density = 2800; % [kg/m3]
- granite.volume = 0.749; % [m3] TODO - should
- granite.mass = granite.density*granite.volume; % [kg]
- granite.color = [1 1 1];
- granite.STEP = './STEPS/granite/granite.STEP';
-
- granite.mass_top = 4000; % [kg] TODO
-
- %% Dynamical Properties
- if args.rigid
- granite.k.x = 1e12; % [N/m]
- granite.k.y = 1e12; % [N/m]
- granite.k.z = 1e12; % [N/m]
- granite.k.rx = 1e10; % [N*m/deg]
- granite.k.ry = 1e10; % [N*m/deg]
- granite.k.rz = 1e10; % [N*m/deg]
- else
- granite.k.x = 4e9; % [N/m]
- granite.k.y = 3e8; % [N/m]
- granite.k.z = 8e8; % [N/m]
- granite.k.rx = 1e4; % [N*m/deg]
- granite.k.ry = 1e4; % [N*m/deg]
- granite.k.rz = 1e6; % [N*m/deg]
- end
-
- granite.c.x = 0.1*sqrt(granite.mass_top*granite.k.x); % [N/(m/s)]
- granite.c.y = 0.1*sqrt(granite.mass_top*granite.k.y); % [N/(m/s)]
- granite.c.z = 0.5*sqrt(granite.mass_top*granite.k.z); % [N/(m/s)]
- granite.c.rx = 0.1*sqrt(granite.mass_top*granite.k.rx); % [N*m/(deg/s)]
- granite.c.ry = 0.1*sqrt(granite.mass_top*granite.k.ry); % [N*m/(deg/s)]
- granite.c.rz = 0.1*sqrt(granite.mass_top*granite.k.rz); % [N*m/(deg/s)]
-
- %% Positioning parameters
- granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m]
-
- %% Save
- save('./mat/stages.mat', 'granite', '-append');
- end
-#+end_src
-
-** Translation Stage
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeTy.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeTy.m][here]].
-
-#+begin_src matlab
- function [ty] = initializeTy(args)
- arguments
- args.rigid logical {mustBeNumericOrLogical} = false
- end
-
- %%
- ty = struct();
-
- %% Y-Translation - Static Properties
- % Ty Granite frame
- ty.granite_frame.density = 7800; % [kg/m3]
- ty.granite_frame.color = [0.753 1 0.753];
- ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP';
- % Guide Translation Ty
- ty.guide.density = 7800; % [kg/m3]
- ty.guide.color = [0.792 0.820 0.933];
- ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP';
- % Ty - Guide_Translation12
- ty.guide12.density = 7800; % [kg/m3]
- ty.guide12.color = [0.792 0.820 0.933];
- ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP';
- % Ty - Guide_Translation11
- ty.guide11.density = 7800; % [kg/m3]
- ty.guide11.color = [0.792 0.820 0.933];
- ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP';
- % Ty - Guide_Translation22
- ty.guide22.density = 7800; % [kg/m3]
- ty.guide22.color = [0.792 0.820 0.933];
- ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP';
- % Ty - Guide_Translation21
- ty.guide21.density = 7800; % [kg/m3]
- ty.guide21.color = [0.792 0.820 0.933];
- ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP';
- % Ty - Plateau translation
- ty.frame.density = 7800; % [kg/m3]
- ty.frame.color = [0.792 0.820 0.933];
- ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP';
- % Ty Stator Part
- ty.stator.density = 5400; % [kg/m3]
- ty.stator.color = [0.792 0.820 0.933];
- ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
- % Ty Rotor Part
- ty.rotor.density = 5400; % [kg/m3]
- ty.rotor.color = [0.792 0.820 0.933];
- ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
-
- ty.m = 1000; % TODO [kg]
-
- %% Y-Translation - Dynamicals Properties
- 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
- 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]
- end
-
- ty.c.ax = 0.1*sqrt(ty.k.ax*ty.m);
- ty.c.rad = 0.1*sqrt(ty.k.rad*ty.m);
-
- %% Save
- save('./mat/stages.mat', 'ty', '-append');
- end
-#+end_src
-
-** Tilt Stage
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeRy.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeRy.m][here]].
-
-#+begin_src matlab
- function [ry] = initializeRy(args)
- arguments
- args.rigid logical {mustBeNumericOrLogical} = false
- end
-
- %%
- ry = struct();
-
- %% Tilt Stage - Static Properties
- % Ry - Guide for the tilt stage
- ry.guide.density = 7800; % [kg/m3]
- ry.guide.color = [0.792 0.820 0.933];
- ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP';
- % Ry - Rotor of the motor
- ry.rotor.density = 2400; % [kg/m3]
- ry.rotor.color = [0.792 0.820 0.933];
- ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP';
- % Ry - Motor
- ry.motor.density = 3200; % [kg/m3]
- ry.motor.color = [0.792 0.820 0.933];
- ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
- % Ry - Plateau Tilt
- ry.stage.density = 7800; % [kg/m3]
- ry.stage.color = [0.792 0.820 0.933];
- ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
-
- ry.m = 800; % TODO [kg]
-
- %% Tilt Stage - Dynamical Properties
- 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]
- ry.k.rrad = 1e12; % Stiffness in the side direction [N/m]
- else
- 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
-
- ry.c.h = 0.1*sqrt(ry.k.h*ry.m);
- ry.c.rad = 0.1*sqrt(ry.k.rad*ry.m);
- ry.c.rrad = 0.1*sqrt(ry.k.rrad*ry.m);
- ry.c.tilt = 0.1*sqrt(ry.k.tilt*ry.m);
-
- %% Positioning parameters
- ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]
-
- %% Save
- save('./mat/stages.mat', 'ry', '-append');
- end
-#+end_src
-
-** Spindle
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeRz.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeRz.m][here]].
-
-#+begin_src matlab
- function [rz] = initializeRz(args)
- arguments
- args.rigid logical {mustBeNumericOrLogical} = false
- end
-
- %%
- rz = struct();
-
- %% Spindle - Static Properties
- % Spindle - Slip Ring
- rz.slipring.density = 7800; % [kg/m3]
- rz.slipring.color = [0.792 0.820 0.933];
- rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP';
- % Spindle - Rotor
- rz.rotor.density = 7800; % [kg/m3]
- rz.rotor.color = [0.792 0.820 0.933];
- rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
- % Spindle - Stator
- rz.stator.density = 7800; % [kg/m3]
- rz.stator.color = [0.792 0.820 0.933];
- rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
-
- % Estimated mass of the mooving part
- rz.m = 250; % [kg]
-
- %% Spindle - Dynamical Properties
-
- 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]
- rz.k.rad = 1e12; % Radial Stiffness (X, Y) [N/m]
- else
- 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]
- end
-
- % Damping
- rz.c.ax = 0.1*sqrt(rz.k.ax*rz.m);
- rz.c.rad = 0.1*sqrt(rz.k.rad*rz.m);
- rz.c.tilt = 0.1*sqrt(rz.k.tilt*rz.m);
- rz.c.rot = 0.1*sqrt(rz.k.rot*rz.m);
-
- %% Save
- save('./mat/stages.mat', 'rz', '-append');
- end
-#+end_src
-
+* Old functions :noexport:
** Micro Hexapod
:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
+:header-args:matlab+: :tangle ../src/initializeMicroHexapodOld.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<>
-This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]].
-
#+begin_src matlab
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
@@ -1020,181 +1787,6 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
end
end
#+end_src
-
-** Center of gravity compensation
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeAxisc.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeAxisc.m][here]].
-
-#+begin_src matlab
- function [axisc] = initializeAxisc()
- %%
- axisc = struct();
-
- %% Axis Compensator - Static Properties
- % Structure
- axisc.structure.density = 3400; % [kg/m3]
- axisc.structure.color = [0.792 0.820 0.933];
- axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP';
- % Wheel
- axisc.wheel.density = 2700; % [kg/m3]
- axisc.wheel.color = [0.753 0.753 0.753];
- axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP';
- % Mass
- axisc.mass.density = 7800; % [kg/m3]
- axisc.mass.color = [0.792 0.820 0.933];
- axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP';
- % Gear
- axisc.gear.density = 7800; % [kg/m3]
- axisc.gear.color = [0.792 0.820 0.933];
- axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP';
-
- axisc.m = 40; % TODO [kg]
-
- %% Axis Compensator - Dynamical Properties
- % axisc.k.ax = 1; % TODO [N*m/deg)]
- % axisc.c.ax = (1/1)*sqrt(axisc.k.ax/axisc.m);
-
- %% Save
- save('./mat/stages.mat', 'axisc', '-append');
- end
-#+end_src
-
-** Mirror
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeMirror.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
-
-#+begin_src matlab
- function [] = initializeMirror(args)
- arguments
- args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
- args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45
- end
-
- %%
- mirror = struct();
- mirror.h = 50; % height of the mirror [mm]
- mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
- mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
- mirror.support_rad = 100; % radius of the support plate [mm]
- mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
- mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
-
- 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(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
-
- %% Shape
- mirror.shape = [...
- 0 mirror.h-mirror.thickness
- mirror.hole_rad mirror.h-mirror.thickness; ...
- mirror.hole_rad 0; ...
- mirror.rad 0 ...
- ];
-
- 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(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
-
- mirror.shape = [mirror.shape; 0 mirror.h];
-
- %% Save
- save('./mat/stages.mat', 'mirror', '-append');
- end
-#+end_src
-
-** Nano Hexapod
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeNanoHexapod.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]].
-
-#+begin_src matlab
- function [nano_hexapod] = initializeNanoHexapod(args)
- arguments
- % initializeFramesPositions
- args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
- args.MO_B (1,1) double {mustBeNumeric} = 175e-3
- % generateGeneralConfiguration
- args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
- args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-3;
- args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
- args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
- args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
- args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
- % initializeStrutDynamics
- args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
- % initializeCylindricalPlatforms
- args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
- args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
- args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
- args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
- args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
- args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
- % initializeCylindricalStruts
- args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
- args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
- args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
- args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
- args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
- args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
- % inverseKinematics
- args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
- args.ARB (3,3) double {mustBeNumeric} = eye(3)
- end
-
- stewart = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
-
- stewart = generateGeneralConfiguration(stewart, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
-
- stewart = computeJointsPose(stewart);
-
- if strcmp(args.actuator, 'piezo')
- stewart = initializeStrutDynamics(stewart, 'Ki', 1e7*ones(6,1), ...
- 'Ci', 1e2*ones(6,1));
- elseif strcmp(args.actuator, 'lorentz')
- stewart = initializeStrutDynamics(stewart, 'Ki', 1e4*ones(6,1), ...
- 'Ci', 1e2*ones(6,1));
- else
- error('args.actuator should be piezo or lorentz');
- end
-
- stewart = initializeCylindricalPlatforms(stewart, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
-
- stewart = initializeCylindricalStruts(stewart, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
-
- stewart = computeJacobian(stewart);
-
- [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
-
- nano_hexapod = stewart;
-
- %% Save
- save('./mat/stages.mat', 'nano_hexapod', '-append');
- end
-#+end_src
-
** Cedrat Actuator
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeCedratPiezo.m
@@ -1202,8 +1794,6 @@ This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]
:END:
<>
-This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]].
-
#+begin_src matlab
function [cedrat] = initializeCedratPiezo()
%% Stewart Object
@@ -1223,38 +1813,3 @@ This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]]
save('./mat/stages.mat', 'cedrat', '-append');
end
#+end_src
-
-** Sample
-:PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeSample.m
-:header-args:matlab+: :comments none :mkdirp yes :eval no
-:END:
-<>
-
-This Matlab function is accessible [[file:../src/initializeSample.m][here]].
-
-#+begin_src matlab
- function [sample] = initializeSample(sample)
- arguments
- sample.radius (1,1) double {mustBeNumeric, mustBePositive} = 100 % [mm]
- sample.height (1,1) double {mustBeNumeric, mustBePositive} = 300 % [mm]
- sample.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
- sample.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
- sample.offset (1,1) double {mustBeNumeric} = 0 % [mm]
- sample.color (1,3) double {mustBeNumeric} = [0.45, 0.45, 0.45]
- end
-
- %%
- sample.k.x = sample.mass * (2*pi * sample.freq)^2;
- sample.c.x = 0.1*sqrt(sample.k.x*sample.mass);
-
- sample.k.y = sample.mass * (2*pi * sample.freq)^2;
- sample.c.y = 0.1*sqrt(sample.k.y*sample.mass);
-
- sample.k.z = sample.mass * (2*pi * sample.freq)^2;
- sample.c.z = 0.1*sqrt(sample.k.z*sample.mass);
-
- %% Save
- save('./mat/stages.mat', 'sample', '-append');
- end
-#+end_src
12 Initialize Disturbances
+ + +
+
+
+Function Declaration and Documentation
+
+
+
+
+function [] = initializeDisturbances(args) +% initializeDisturbances - Initialize the disturbances +% +% Syntax: [] = initializeDisturbances(args) +% +% Inputs: +% - args - + ++
+
+
+
+Optional Parameters
+
+
+
+
+arguments + % Global parameter to enable or disable the disturbances + args.enable logical {mustBeNumericOrLogical} = true + % 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 Data
+
+
+
+
load('./disturbances/mat/dist_psd.mat', 'dist_f'); ++
-This Matlab function is accessible here. +We remove the first frequency point that usually is very large.
+
+
+
+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 ++
-
- %% Populate opts with input parameters
- if exist('opts_param','var')
- for opt = fieldnames(opts_param)'
- opts.(opt{1}) = opts_param.(opt{1});
- end
+function [cedrat] = initializeCedratPiezo(opts_param) - %% Default values for opts - opts = struct(); +if args.Dwx && args.enable + 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)))];; + Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] +else + Dwx = zeros(length(t), 1); +end ++
+
+
+if args.Dwy && args.enable + 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)))];; + Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] +else + Dwy = zeros(length(t), 1); +end ++
+
+if args.Dwy && args.enable + 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)))];; + Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] +else + Dwz = zeros(length(t), 1); +end ++
+
-Translation Stage - X direction
+
+
+
if args.Fty_x && args.enable + 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 - - %% Stewart Object - cedrat = struct(); - cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m] - cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m] - - cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)] - cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)] - - cedrat.L = 80; % Total Width of the Actuator[mm] - cedrat.H = 45; % Total Height of the Actuator [mm] - cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm] - cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg] - - %% Save - save('./mat/stages.mat', 'cedrat', '-append'); + 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; +else + Fty_x = zeros(length(t), 1); end
-
diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org
index 6caef29..192d338 100644
--- a/simscape_subsystems/index.org
+++ b/simscape_subsystems/index.org
@@ -41,24 +41,1028 @@
#+PROPERTY: header-args:latex+ :output-dir figs
:END:
-* Introduction :ignore:
-* General Subsystems
-<2.11 Sample
-
-
+
+
+
+
+Translation Stage - Z direction
+
+
+
+
+if args.Fty_z && args.enable + 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; +else + Fty_z = zeros(length(t), 1); +end ++
+
+
+Spindle - Z direction
+
+
+
+
+if args.Frz_z && args.enable + 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; +else + Frz_z = zeros(length(t), 1); +end ++
+
+
+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'); ++
+
+
+13 Z-Axis Geophone
+-This Matlab function is accessible here. +
-
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 [geophone] = initializeZAxisGeophone(args) + arguments + args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] + args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz] end %% - sample.k.x = 1e8; - sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); + geophone.m = args.mass; - sample.k.y = 1e8; - sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); + %% The Stiffness is set to have the damping resonance frequency + geophone.k = geophone.m * (2*pi*args.freq)^2; - sample.k.z = 1e8; - sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); + %% We set the damping value to have critical damping + geophone.c = 2*sqrt(geophone.m * geophone.k); %% Save - save('./mat/stages.mat', 'sample', '-append'); + save('./mat/geophone_z_axis.mat', 'geophone'); end
+
14 Z-Axis Accelerometer
+
+
+
+
+
+function [accelerometer] = initializeZAxisAccelerometer(args) + arguments + args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] + args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz] + end + + %% + accelerometer.m = args.mass; + + %% The Stiffness is set to have the damping resonance frequency + accelerometer.k = accelerometer.m * (2*pi*args.freq)^2; + + %% We set the damping value to have critical damping + accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k); + + %% Gain correction of the accelerometer to have a unity gain until the resonance + accelerometer.gain = -accelerometer.k/accelerometer.m; + + %% Save + save('./mat/accelerometer_z_axis.mat', 'accelerometer'); +end ++