Simscape Model of the Nano-Active-Stabilization-System
+
+
+
+
+
+Table of Contents
+
+
+-
+
- 1. Simulink project +
- 2. Notes + + +
- 3. Simulink files +
- 4. Simulink Library + + +
- 5. Scripts + + +
- 6. Functions + + +
- 7. Initialize Elements + + +
+
+
+1 Simulink project
+
+
+
+The project can be opened using the simulinkproject function:
+
+
+
+simulinkproject('./'); ++
+When the project opens, a startup script is ran.
+The startup script is defined below and is exported to the project_startup.m script.
+
+
+
+%% +freqs = logspace(-1, 3, 1000); +save_fig = false; +save('./mat/config.mat', 'freqs', 'save_fig'); + +%% +project = simulinkproject; +projectRoot = project.RootFolder; + +myCacheFolder = fullfile(projectRoot, '.SimulinkCache'); +myCodeFolder = fullfile(projectRoot, '.SimulinkCode'); + +Simulink.fileGenControl('set',... + 'CacheFolder', myCacheFolder,... + 'CodeGenFolder', myCodeFolder,... + 'createDir', true); ++
+When the project closes, it runs the project_shutdown.m script defined below.
+
+
+
+Simulink.fileGenControl('reset'); ++
+The project also permits to automatically add defined folder to the path when the project is opened. +
+
+
+
+2 Notes
+
+
+
++Step files are exported from SolidWorks and imported into Simscape. +Each step file is then considered as a solid body. +Inertia and mass matrices are computed from the density of the material and the geometry as defined by the step file. +
+
+
+
+2.1 Simscape files for identification
+
+
+
+
+
+
+| Simscape Name | +Ty | +Ry | +Rz | +Hexa | +NASS | +
|---|---|---|---|---|---|
| id micro station | +F | +F | +F | +F | ++ |
| id nano station stages | +F | +F | +F | +F | +F | +
| id nano station config | +D | +D | +D | +D | +F | +
| control nano station | +D | +D | +D | +D | +F | +
+
+
+2.2 Inputs
+
+
+
+
+
+2.2.1 Perturbations
+
+
+
+
+
+
+| Variable | +Meaning | +Size | +Unit | +
|---|---|---|---|
Dw |
+Ground motion | +3 | +[m] | +
Fg |
+External force applied on granite | +3 | +[N] | +
Fs |
+External force applied on the Sample | +3 | +[N] | +
+
+
+2.2.2 Measurement Noise
+
+
+
+
+
+
+| Variable | +Meaning | +Size | +Unit | +
|---|---|---|---|
| + | + | + | + |
+
+2.2.3 Control Inputs
+
+
+
+
+
+
+| Variable | +Meaning | +Size | +Unit | +
|---|---|---|---|
Fy |
+Actuation force for Ty | +1 | +[N] | +
Dy |
+Imposed displacement for Ty | +1 | +[m] | +
My |
+Actuation torque for Ry | +1 | +[N.m] | +
Ry |
+Imposed rotation for Ry | +1 | +[rad] | +
Mz |
+Actuation torque for Rz | +1 | +[N.m] | +
Rz |
+Imposed rotation for Rz | +1 | +[rad] | +
Fh |
+Actuation force/torque for hexapod (cart) | +6 | +[N, N.m] | +
Fhl |
+Actuation force/torque for hexapod (legs) | +6 | +[N] | +
Dh |
+Imposed position for hexapod (cart) | +6 | +[m, rad] | +
Rm |
+Position of the two masses | +2 | +[rad] | +
Fn |
+Actuation force for the NASS (cart) | +6 | +[N, N.m] | +
Fnl |
+Actuation force for the NASS's legs | +6 | +[N] | +
Dn |
+Imposed position for the NASS (cart) | +6 | +[m, rad] | +
+
+2.3 Outputs
+
+
+
+
+
+
+| Variable | +Meaning | +Size | +Unit | +
|---|---|---|---|
Dgm |
+Absolute displacement of the granite | +3 | +[m] | +
Vgm |
+Absolute Velocity of the granite | +3 | +[m/s] | +
Dym |
+Measured displacement of Ty | +1 | +[m] | +
Rym |
+Measured rotation of Ry | +1 | +[rad] | +
Rzm |
+Measured rotation of Rz | +1 | +[rad] | +
Dhm |
+Measured position of hexapod (cart) | +6 | +[m, rad] | +
Fnlm |
+Measured force of NASS's legs | +6 | +[N] | +
Dnlm |
+Measured elongation of NASS's legs | +6 | +[m] | +
Dnm |
+Measured position of NASS w.r.t NASS's base | +6 | +[m, rad] | +
Vnm |
+Measured absolute velocity of NASS platform | +6 | +[m/s, rad/s] | +
Vnlm |
+Measured absolute velocity of NASS's legs | +6 | +[m/s] | +
Dsm |
+Position of Sample w.r.t. granite frame | +6 | +[m, rad] | +
+
+
+3 Simulink files
+
+
++Few different Simulink files are used: +
+-
+
- kinematics +
- identification - micro station +
- identification - nano station +
- control +
+
+
+4 Simulink Library
+
+
+
++A simulink library is developed in order to share elements between the different simulink files. +
+
+
+
+4.1 inputs
+
+
+
+4.2 nasslibrary
+
+
+
+4.3 poserrorwrtnassbase
+
+
+
+4.4 QuaternionToAngles
+
+
+4.5 RotationMatrixToAngle
+
+
+
+5 Scripts
+
+
+
+
+5.1 Simulation Initialization
+
+
+
+
++This Matlab script is accessible here. +
+ ++This script runs just before the simulation is started. +It is used to load the simulation configuration and the controllers used for the simulation. +
+ +
+
+%% Load all the data used for the simulation +load('./mat/sim_conf.mat'); + +%% Load Controller +load('./mat/controllers.mat'); ++
+
+6 Functions
+
+
+
+
+6.1 computePsdDispl
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [psd_object] = computePsdDispl(sys_data, t_init, n_av) + i_init = find(sys_data.time > t_init, 1); + + han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av)); + Fs = 1/sys_data.time(2); + + [pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs); + [pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs); + [pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs); + + [prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs); + [pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs); + [prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs); + + psd_object = struct(... + 'f', f, ... + 'dx', pdx, ... + 'dy', pdy, ... + 'dz', pdz, ... + 'rx', prx, ... + 'ry', pry, ... + 'rz', prz); +end ++
+
+6.2 computeSetpoint
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function setpoint = computeSetpoint(ty, ry, rz) +%% +setpoint = zeros(6, 1); + +%% Ty +Ty = [1 0 0 0 ; + 0 1 0 ty ; + 0 0 1 0 ; + 0 0 0 1 ]; + +% Tyinv = [1 0 0 0 ; +% 0 1 0 -ty ; +% 0 0 1 0 ; +% 0 0 0 1 ]; + +%% Ry +Ry = [ cos(ry) 0 sin(ry) 0 ; + 0 1 0 0 ; + -sin(ry) 0 cos(ry) 0 ; + 0 0 0 1 ]; + +% TMry = Ty*Ry*Tyinv; + +%% Rz +Rz = [cos(rz) -sin(rz) 0 0 ; + sin(rz) cos(rz) 0 0 ; + 0 0 1 0 ; + 0 0 0 1 ]; + +% TMrz = Ty*TMry*Rz*TMry'*Tyinv; + +%% All stages +% TM = TMrz*TMry*Ty; + +TM = Ty*Ry*Rz; + +[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3)); + +setpoint(1:3) = TM(1:3, 4); +setpoint(4:6) = [thetax, thetay, thetaz]; + +%% Custom Functions +function [thetax, thetay, thetaz] = RM2angle(R) + if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1 + thetay = -asin(R(3, 1)); + thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay)); + thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay)); + else + thetaz = 0; + if abs(R(3, 1)+1) < 1e-6 % R31 = -1 + thetay = pi/2; + thetax = thetaz + atan2(R(1, 2), R(1, 3)); + else + thetay = -pi/2; + thetax = -thetaz + atan2(-R(1, 2), -R(1, 3)); + end + end +end +end ++
+
+6.3 converErrorBasis
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz) +% convertErrorBasis - +% +% Syntax: convertErrorBasis(p_error, ty, ry, rz) +% +% Inputs: +% - p_error - Position error of the sample w.r.t. the granite [m, rad] +% - ty - Measured translation of the Ty stage [m] +% - ry - Measured rotation of the Ry stage [rad] +% - rz - Measured rotation of the Rz stage [rad] +% +% Outputs: +% - P_nass - Position error of the sample w.r.t. the NASS base [m] +% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad] +% +% Example: +% + +%% If line vector => column vector +if size(pos, 2) == 6 + pos = pos'; +end + +if size(setpoint, 2) == 6 + setpoint = setpoint'; +end + +%% Position of the sample in the frame fixed to the Granite +P_granite = [pos(1:3); 1]; % Position [m] +R_granite = [setpoint(1:3); 1]; % Reference [m] + +%% Transformation matrices of the stages +% T-y +TMty = [1 0 0 0 ; + 0 1 0 ty ; + 0 0 1 0 ; + 0 0 0 1 ]; + +% R-y +TMry = [ cos(ry) 0 sin(ry) 0 ; + 0 1 0 0 ; + -sin(ry) 0 cos(ry) 0 ; + 0 0 0 1 ]; + +% R-z +TMrz = [cos(rz) -sin(rz) 0 0 ; + sin(rz) cos(rz) 0 0 ; + 0 0 1 0 ; + 0 0 0 1 ]; + +%% Compute Point coordinates in the new reference fixed to the NASS base +% P_nass = TMrz*TMry*TMty*P_granite; +P_nass = TMrz\TMry\TMty\P_granite; +R_nass = TMrz\TMry\TMty\R_granite; + +dx = R_nass(1)-P_nass(1); +dy = R_nass(2)-P_nass(2); +dz = R_nass(3)-P_nass(3); + +%% Compute new basis vectors linked to the NASS base +% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0]; +% ux_nass = ux_nass(1:3); +% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0]; +% uy_nass = uy_nass(1:3); +% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0]; +% uz_nass = uz_nass(1:3); + +ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0]; +ux_nass = ux_nass(1:3); +uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0]; +uy_nass = uy_nass(1:3); +uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0]; +uz_nass = uz_nass(1:3); + +%% Rotations error w.r.t. granite Frame +% Rotations error w.r.t. granite Frame +rx_nass = pos(4); +ry_nass = pos(5); +rz_nass = pos(6); + +% Rotation matrices of the Sample w.r.t. the Granite +Mrx_error = [1 0 0 ; + 0 cos(-rx_nass) -sin(-rx_nass) ; + 0 sin(-rx_nass) cos(-rx_nass)]; + +Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ; + 0 1 0 ; + -sin(-ry_nass) 0 cos(-ry_nass)]; + +Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ; + sin(-rz_nass) cos(-rz_nass) 0 ; + 0 0 1]; + +% Rotation matrix of the Sample w.r.t. the Granite +Mr_error = Mrz_error*Mry_error*Mrx_error; + +%% Use matrix to solve +R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample + +[thetax, thetay, thetaz] = RM2angle(R); + +error_nass = [dx; dy; dz; thetax; thetay; thetaz]; + +%% Custom Functions +function [thetax, thetay, thetaz] = RM2angle(R) + if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1 + thetay = -asin(R(3, 1)); + % thetaybis = pi-thetay; + thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay)); + % thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis)); + thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay)); + % thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis)); + else + thetaz = 0; + if abs(R(3, 1)+1) < 1e-6 % R31 = -1 + thetay = pi/2; + thetax = thetaz + atan2(R(1, 2), R(1, 3)); + else + thetay = -pi/2; + thetax = -thetaz + atan2(-R(1, 2), -R(1, 3)); + end + end +end + +end ++
+
+6.4 generateDiagPidControl
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [K] = generateDiagPidControl(G, fs) + %% + pid_opts = pidtuneOptions(... + 'PhaseMargin', 50, ... + 'DesignFocus', 'disturbance-rejection'); + + %% + K = tf(zeros(6)); + + for i = 1:6 + input_name = G.InputName(i); + output_name = G.OutputName(i); + K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts)); + end + + K.InputName = G.OutputName; + K.OutputName = G.InputName; +end ++
+
+6.5 identifyPlant
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [sys] = identifyPlant(opts_param) + %% Default values for opts + opts = struct(); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% Options for Linearized + options = linearizeOptions; + options.SampleTime = 0; + + %% Name of the Simulink File + mdl = 'sim_nano_station_id'; + + %% Input/Output definition + io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS + io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion + io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample + io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs + io(5) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample + io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs + io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs + io(8) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base + + %% Run the linearization + G = linearize(mdl, io, 0); + G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ... + 'Dgx', 'Dgy', 'Dgz', ... + 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ... + 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; + G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ... + 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ... + 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ... + 'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}; + + %% Create the sub transfer functions + % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame + sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'})); + % From ground motion to Sample displacement + sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'})); + % From direct forces applied on the sample to displacement of the sample + sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'})); + % From forces applied on NASS's legs to force sensor in each leg + sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})); + % From forces applied on NASS's legs to displacement of each leg + sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})); + % From forces applied on NASS's legs to displacement of each leg + sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'})); +end ++
+
+6.6 runSimulation
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp) + %% Load the controller and save it for the simulation + if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none') + K_obj = load('./mat/K_fb.mat'); + K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok + save('./mat/controllers.mat', 'K'); + elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff') + K_obj = load('./mat/K_fb_iff.mat'); + K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok + save('./mat/controllers.mat', 'K'); + elseif strcmp(ctrl_type, 'ol') + K = tf(zeros(6)); %#ok + save('./mat/controllers.mat', 'K'); + else + error('ctrl_type should be cl or ol'); + end + + %% Active Damping + if strcmp(act_damp, 'iff') + K_iff_crit = load('./mat/K_iff_crit.mat'); + K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok + save('./mat/controllers.mat', 'K_iff', '-append'); + elseif strcmp(act_damp, 'none') + K_iff = tf(zeros(6)); %#ok + save('./mat/controllers.mat', 'K_iff', '-append'); + end + + %% + if strcmp(sys_name, 'pz') + initializeNanoHexapod(struct('actuator', 'piezo')); + elseif strcmp(sys_name, 'vc') + initializeNanoHexapod(struct('actuator', 'lorentz')); + else + error('sys_name should be pz or vc'); + end + + if strcmp(sys_mass, 'light') + initializeSample(struct('mass', 1)); + elseif strcmp(sys_mass, 'heavy') + initializeSample(struct('mass', 50)); + else + error('sys_mass should be light or heavy'); + end + + %% Run the simulation + sim('sim_nano_station_ctrl.slx'); + + %% Split the Dsample matrix into vectors + [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok + time = Dsample.Time; %#ok + + %% Save the result + filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp); + save(sprintf('./mat/%s.mat', filename), ... + 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K'); +end ++
+
7 Initialize Elements
+
+
+
+
+
+7.1 Simulation Configuration
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [] = initializeSimConf(opts_param) + %% Default values for opts + opts = struct('Ts', 1e-4, ... % Sampling time [s] + 'Tsim', 10, ... % Simulation time [s] + 'cl_time', 0, ... % Close Loop time [s] + 'gravity', false ... % Gravity along the z axis + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% + sim_conf = struct(); + + %% + sim_conf.Ts = opts.Ts; + sim_conf.Tsim = opts.Tsim; + sim_conf.cl_time = opts.cl_time; + + %% Gravity + if opts.gravity + sim_conf.g = -9.8; %#ok + else + sim_conf.g = 0; %#ok + end + + %% Save + save('./mat/sim_conf.mat', 'sim_conf'); +end ++
+
+
+7.2 Experiment
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [] = initializeExperiment(exp_name, sys_mass) + if strcmp(exp_name, 'tomography') + opts_sim = struct(... + 'Tsim', 5, ... + 'cl_time', 5 ... + ); + initializeSimConf(opts_sim); + + if strcmp(sys_mass, 'light') + opts_inputs = struct(... + 'Dw', true, ... + 'Rz', 60 ... % rpm + ); + elseif strcpm(sys_mass, 'heavy') + opts_inputs = struct(... + 'Dw', true, ... + 'Rz', 1 ... % rpm + ); + else + error('sys_mass should be light or heavy'); + end + + initializeInputs(opts_inputs); + else + error('exp_name is only configured for tomography'); + end +end ++
+
+
+7.3 Inputs
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [inputs] = initializeInputs(opts_param) + %% Default values for opts + opts = struct( ... + 'Dw', false, ... + 'Dy', false, ... + 'Ry', false, ... + 'Rz', false, ... + 'Dh', false, ... + 'Rm', false, ... + 'Dn', false ... + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% Load Sampling Time and Simulation Time + load('./mat/sim_conf.mat', 'sim_conf'); + + %% Define the time vector + t = 0:sim_conf.Ts:sim_conf.Tsim; + + %% Ground motion - Dw + if islogical(opts.Dw) && opts.Dw == true + load('./mat/perturbations.mat', 'Wxg'); + Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3); + Dw(:, 1) = lsim(Wxg, Dw(:, 1), t); + Dw(:, 2) = lsim(Wxg, Dw(:, 2), t); + Dw(:, 3) = lsim(Wxg, Dw(:, 3), t); + elseif islogical(opts.Dw) && opts.Dw == false + Dw = zeros(length(t), 3); + else + Dw = opts.Dw; + end + + %% Translation stage - Dy + if islogical(opts.Dy) && opts.Dy == true + Dy = zeros(length(t), 1); + elseif islogical(opts.Dy) && opts.Dy == false + Dy = zeros(length(t), 1); + else + Dy = opts.Dy; + end + + %% Tilt Stage - Ry + if islogical(opts.Ry) && opts.Ry == true + Ry = 3*(2*pi/360)*sin(2*pi*0.2*t); + elseif islogical(opts.Ry) && opts.Ry == false + Ry = zeros(length(t), 1); + elseif isnumeric(opts.Ry) && length(opts.Ry) == 1 + Ry = opts.Ry*(2*pi/360)*ones(length(t), 1); + else + Ry = opts.Ry; + end + + %% Spindle - Rz + if islogical(opts.Rz) && opts.Rz == true + Rz = 2*pi*0.5*t; + elseif islogical(opts.Rz) && opts.Rz == false + Rz = zeros(length(t), 1); + elseif isnumeric(opts.Rz) && length(opts.Rz) == 1 + Rz = opts.Rz*(2*pi/60)*t; + else + Rz = opts.Rz; + end + + %% Micro Hexapod - Dh + if islogical(opts.Dh) && opts.Dh == true + Dh = zeros(length(t), 6); + elseif islogical(opts.Dh) && opts.Dh == false + Dh = zeros(length(t), 6); + else + Dh = opts.Dh; + end + + %% Axis Compensation - Rm + if islogical(opts.Rm) + Rm = zeros(length(t), 2); + Rm(:, 2) = pi*ones(length(t), 1); + else + Rm = opts.Rm; + end + + %% Nano Hexapod - Dn + if islogical(opts.Dn) && opts.Dn == true + Dn = zeros(length(t), 6); + elseif islogical(opts.Dn) && opts.Dn == false + Dn = zeros(length(t), 6); + else + Dn = opts.Dn; + end + + %% Setpoint - Ds + Ds = zeros(length(t), 6); + for i = 1:length(t) + Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i)); + end + + %% External Forces applied on the Granite + Fg = zeros(length(t), 3); + + %% External Forces applied on the Sample + Fs = zeros(length(t), 6); + + %% Create the input Structure that will contain all the inputs + inputs = struct( ... + 'Ts', sim_conf.Ts, ... + 'Dw', timeseries(Dw, t), ... + 'Dy', timeseries(Dy, t), ... + 'Ry', timeseries(Ry, t), ... + 'Rz', timeseries(Rz, t), ... + 'Dh', timeseries(Dh, t), ... + 'Rm', timeseries(Rm, t), ... + 'Dn', timeseries(Dn, t), ... + 'Ds', timeseries(Ds, t), ... + 'Fg', timeseries(Fg, t), ... + 'Fs', timeseries(Fs, t) ... + ); + + %% Save + save('./mat/inputs.mat', 'inputs'); + + %% Custom Functions + function setpoint = computeSetpoint(ty, ry, rz) + %% + setpoint = zeros(6, 1); + + %% Ty + TMTy = [1 0 0 0 ; + 0 1 0 ty ; + 0 0 1 0 ; + 0 0 0 1 ]; + + %% Ry + TMRy = [ cos(ry) 0 sin(ry) 0 ; + 0 1 0 0 ; + -sin(ry) 0 cos(ry) 0 ; + 0 0 0 1 ]; + + %% Rz + TMRz = [cos(rz) -sin(rz) 0 0 ; + sin(rz) cos(rz) 0 0 ; + 0 0 1 0 ; + 0 0 0 1 ]; + + %% All stages + TM = TMTy*TMRy*TMRz; + + [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3)); + + setpoint(1:3) = TM(1:3, 4); + setpoint(4:6) = [thetax, thetay, thetaz]; + + %% Custom Functions + function [thetax, thetay, thetaz] = RM2angle(R) + if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1 + thetay = -asin(R(3, 1)); + thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay)); + thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay)); + else + thetaz = 0; + if abs(R(3, 1)+1) < 1e-6 % R31 = -1 + thetay = pi/2; + thetax = thetaz + atan2(R(1, 2), R(1, 3)); + else + thetay = -pi/2; + thetax = -thetaz + atan2(-R(1, 2), -R(1, 3)); + end + end + end + end +end ++
+
+
+7.4 Ground
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+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 ++
+
+
+7.5 Granite
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [granite] = initializeGranite() + %% + granite = struct(); + + %% Static Properties + granite.density = 2800; % [kg/m3] + granite.volume = 0.72; % [m3] TODO - should + granite.mass = granite.density*granite.volume; % [kg] + granite.color = [1 1 1]; + granite.STEP = './STEPS/granite/granite.STEP'; + + %% Dynamical Properties + granite.k.x = 1e8; % [N/m] + granite.c.x = 1e4; % [N/(m/s)] + + granite.k.y = 1e8; % [N/m] + granite.c.y = 1e4; % [N/(m/s)] + + granite.k.z = 1e8; % [N/m] + granite.c.z = 1e4; % [N/(m/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 ++
+
+
+7.6 Translation Stage
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [ty] = initializeTy(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 = 250; % TODO [kg] + + %% Y-Translation - Dynamicals Properties + if opts.rigid + ty.k.ax = 1e10; % Axial Stiffness for each of the 4 guidance (y) [N/m] + else + ty.k.ax = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m] + end + ty.k.rad = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m] + + ty.c.ax = 100*(1/5)*sqrt(ty.k.ax/ty.m); + ty.c.rad = 100*(1/5)*sqrt(ty.k.rad/ty.m); + + %% Save + save('./mat/stages.mat', 'ty', '-append'); +end ++
+
+
+7.7 Tilt Stage
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [ry] = initializeRy(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 = 200; % TODO [kg] + + %% Tilt Stage - Dynamical Properties + if opts.rigid + ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg] + else + ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg] + end + + ry.k.h = 357e6/4; % Stiffness in the direction of the guidance [N/m] + ry.k.rad = 555e6/4; % Stiffness in the top direction [N/m] + ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m] + + ry.c.h = 10*(1/5)*sqrt(ry.k.h/ry.m); + ry.c.rad = 10*(1/5)*sqrt(ry.k.rad/ry.m); + ry.c.rrad = 10*(1/5)*sqrt(ry.k.rrad/ry.m); + ry.c.tilt = 10*(1/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 ++
+
+
+7.8 Spindle
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [rz] = initializeRz(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 + % Estimated stiffnesses + rz.k.ax = 2e9; % Axial Stiffness [N/m] + rz.k.rad = 7e8; % Radial Stiffness [N/m] + rz.k.rot = 100e6*2*pi/360; % Rotational Stiffness [N*m/deg] + + if opts.rigid + rz.k.tilt = 1e10; % Vertical Rotational Stiffness [N*m/deg] + else + rz.k.tilt = 1e2; % TODO what value should I put? [N*m/deg] + end + + % TODO + rz.c.ax = 2*sqrt(rz.k.ax/rz.m); + rz.c.rad = 2*sqrt(rz.k.rad/rz.m); + rz.c.tilt = 100*sqrt(rz.k.tilt/rz.m); + rz.c.rot = 100*sqrt(rz.k.rot/rz.m); + + %% Save + save('./mat/stages.mat', 'rz', '-append'); +end ++
+
+
+7.9 Micro Hexapod
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [micro_hexapod] = initializeMicroHexapod(opts_param) + %% Default values for opts + opts = struct(); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% Stewart Object + micro_hexapod = struct(); + micro_hexapod.h = 350; % Total height of the platform [mm] +% micro_hexapod.jacobian = 269.26; % Distance from the top platform to the Jacobian point [mm] + micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm] + + %% Bottom Plate - Mechanical Design + BP = struct(); + + 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] + Leg.k.ax = 5e7; % Stiffness of each leg [N/m] + Leg.ksi.ax = 3; % Maximum amplification at resonance [] + 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); + + + %% Sphere + SP = struct(); + + SP.height.bottom = 27; % [mm] + SP.height.top = 27; % [mm] + SP.density.bottom = 8000; % [kg/m^3] + SP.density.top = 8000; % [kg/m^3] + SP.color.bottom = [0.6 0.6 0.6]; % [rgb] + SP.color.top = [0.6 0.6 0.6]; % [rgb] + SP.k.ax = 0; % [N*m/deg] + SP.ksi.ax = 10; + + 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]; + + %% + micro_hexapod.BP = BP; + micro_hexapod.TP = TP; + micro_hexapod.Leg = Leg; + micro_hexapod.SP = SP; + + %% + micro_hexapod = initializeParameters(micro_hexapod); + + %% Save + save('./mat/stages.mat', 'micro_hexapod', '-append'); + + %% + function [element] = updateDamping(element) + field = fieldnames(element.k); + for i = 1:length(field) + element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m); + 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 +end ++
+
+7.10 Center of gravity compensation
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+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 ++
+
+
+7.11 Mirror
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [] = initializeMirror(opts_param) + %% Default values for opts + opts = struct(... + 'shape', 'spherical', ... % spherical or conical + 'angle', 45 ... + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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(opts.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(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 + + mirror.shape = [mirror.shape; 0 mirror.h]; + + %% Save + save('./mat/stages.mat', 'mirror', '-append'); +end ++
+
+
+7.12 Nano Hexapod
+
+
+
+
++This Matlab function is accessible here. +
+ +
+
+function [nano_hexapod] = initializeNanoHexapod(opts_param) + %% Default values for opts + opts = struct('actuator', 'piezo'); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% 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] + + %% Bottom Plate + BP = struct(); + + 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]; + + %% Top Plate + TP = struct(); + + 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]; + + %% 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 = 3; + + 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) + element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m); + 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 +end ++
+
+7.13 Sample
+
+
+
+
++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 + end + + %% + sample.k.x = 1e8; + sample.c.x = sqrt(sample.k.x*sample.mass)/10; + + sample.k.y = 1e8; + sample.c.y = sqrt(sample.k.y*sample.mass)/10; + + sample.k.z = 1e8; + sample.c.z = sqrt(sample.k.y*sample.mass)/10; + + %% Save + save('./mat/stages.mat', 'sample', '-append'); +end ++