Simscape Model
Table of Contents
- 1. Simulink Project - Startup and Shutdown scripts
- 2. Simscape Multibody - Presentation
- 3. Simulink files and signal names
- 4. Simulink Library
- 5. Functions
- 6. Initialize Elements
- 6.1. Experiment
- 6.2. Generate Reference Signals
- 6.3. TODO Inputs
- 6.4. Ground
- 6.5. Granite
- 6.6. Translation Stage
- 6.7. Tilt Stage
- 6.8. Spindle
- 6.9. Initialize Hexapod legs' length
- 6.10. Micro Hexapod
- 6.11. Center of gravity compensation
- 6.12. Mirror
- 6.13. Nano Hexapod
- 6.14. Cedrat Actuator
- 6.15. Sample
This file is used to explain how this Simscape Model works.
- In section 1, the simulink project with the associated scripts are presented
- In section 2, an introduction to Simscape Multibody is done
- In section 3, each simscape files are presented with the associated signal names and joint architectures
- In section 4, the list of the Simulink library elements are described
- In section 5, a list of Matlab function that will be used are defined here
- In section 6, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.
1 Simulink Project - Startup and Shutdown scripts
From the Simulink project mathworks page:
Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place.
With Simulink Projects, you can:
- Collaborate: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.
- Automate: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.
- Integrate with source control: Enable easy integration with source control and configuration management tools.
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.
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 Simscape Multibody - Presentation
A simscape model permits to model multi-physics systems.
Simscape Multibody permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors.
2.1 Solid bodies
Each solid body is represented by a solid block.
The geometry of the solid body can be imported using a step file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the step file. They also can be set by hand.
2.2 Frames
Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.
They can be defined from the solid body geometry, or using the rigid transform block.
2.3 Joints
Solid Bodies are connected with joints (between frames of the two solid bodies).
There are various types of joints that are all described here.
| Joint Block | Translational DOFs | Rotational DOFs |
|---|---|---|
| 6-DOF | 3 | 3 |
| Bearing | 1 | 3 |
| Bushing | 3 | 3 |
| Cartesian | 3 | 0 |
| Constant Velocity | 0 | 2 |
| Cylindrical | 1 | 1 |
| Gimbal | 0 | 3 |
| Leadscrew | 1 (coupled) | 1 (coupled) |
| Pin Slot | 1 | 1 |
| Planar | 2 | 1 |
| Prismatic | 1 | 0 |
| Rectangular | 2 | 0 |
| Revolute | 0 | 1 |
| Spherical | 1 | 3 |
| Telescoping | 1 | 3 |
| Universal | 0 | 2 |
| Weld | 0 | 0 |
Joint blocks are assortments of joint primitives:
- Prismatic: allows translation along a single standard axis:
Px,Py,Pz - Revolute: allows rotation about a single standard axis:
Rx,Ry,Rz - Spherical: allow rotation about any 3D axis:
S - Lead Screw: allows coupled rotation and translation on a standard axis:
LSz - Constant Velocity: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts:
CV
| Joint Block | Px | Py | Pz | Rx | Ry | Rz | S | CV | LSz |
|---|---|---|---|---|---|---|---|---|---|
| 6-DOF | x | x | x | x | |||||
| Bearing | x | x | x | x | |||||
| Bushing | x | x | x | x | x | x | |||
| Cartesian | x | x | x | ||||||
| Constant Velocity | x | ||||||||
| Cylindrical | x | x | |||||||
| Gimbal | x | x | x | ||||||
| Leadscrew | x | ||||||||
| Pin Slot | x | x | |||||||
| Planar | x | x | x | ||||||
| Prismatic | x | ||||||||
| Rectangular | x | x | |||||||
| Revolute | x | ||||||||
| Spherical | x | ||||||||
| Telescoping | x | x | |||||||
| Universal | x | x | |||||||
| Weld |
For each of the joint primitive, we can specify the dynamical properties:
- The spring stiffness: either linear or rotational one
- The damping coefficient
For the actuation, we can either specify the motion or the force:
- the force applied in the corresponding DOF is provided by the input
- the motion is provided by the input
A sensor can be added to measure either the position, velocity or acceleration of the joint DOF.
Composite Force/Torque sensing:
- Constraint force
- Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers.
2.4 Measurements
A transform sensor block measures the spatial relationship between two frames: the base B and the follower F.
It can give the rotational and translational position, velocity and acceleration.
The measurement frame should be specified: it corresponds to the frame in which to resolve the selected spatial measurements. The default is world.
If we want to simulate an inertial sensor, we just have to choose B to be the world frame.
Force sensors are included in the joints blocks.
2.5 Excitation
We can apply external forces to the model by using an external force and torque block.
Internal force, acting reciprocally between base and following origins is implemented using the internal force block even though it is usually included in one joint block.
3 Simulink files and signal names
3.1 List of Simscape files
Few different Simulink files are used:
- kinematics
- identification - micro station
- identification - nano station
- control
| 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 |
3.2 List of Inputs
3.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] |
3.2.2 Measurement Noise
| Variable | Meaning | Size | Unit |
|---|---|---|---|
3.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] |
3.3 List of 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] |
4 Simulink Library
4.1 inputs
4.2 nass_library
4.3 pos_error_wrt_nass_base
4.4 QuaternionToAngles
4.5 RotationMatrixToAngle
5 Functions
5.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
5.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
5.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
5.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
5.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, '/Fd'], 1, 'input'); % Disturbance Forces io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs %% Run the linearization G = linearize(mdl, io, options); G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ... 'Dgx', 'Dgy', 'Dgz', ... 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ... 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ... 'Frzz', 'Ftyx', 'Ftyz'}; G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ... 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ... 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ... 'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ... 'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}; %% Create the sub transfer functions minreal_tol = sqrt(eps); % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false); % From ground motion to Sample displacement sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false); % From direct forces applied on the sample to displacement of the sample sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false); % From forces applied on NASS's legs to force sensor in each leg sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); % From forces applied on NASS's legs to displacement of each leg sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); % From forces/torques applied by the NASS to position error sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false); % From forces/torques applied by the NASS to velocity of the actuator sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); % From various disturbance forces to position error sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false); %% We remove low frequency and high frequency dynamics that are usually unstable % using =freqsep= is risky as it may change the shape of the transfer functions % f_min = 0.1; % [Hz] % f_max = 1e4; % [Hz] % [~, sys.G_cart] = freqsep(freqsep(sys.G_cart, 2*pi*f_max), 2*pi*f_min); % [~, sys.G_gm] = freqsep(freqsep(sys.G_gm, 2*pi*f_max), 2*pi*f_min); % [~, sys.G_fs] = freqsep(freqsep(sys.G_fs, 2*pi*f_max), 2*pi*f_min); % [~, sys.G_iff] = freqsep(freqsep(sys.G_iff, 2*pi*f_max), 2*pi*f_min); % [~, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, 2*pi*f_max), 2*pi*f_min); % [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min); %% We finally verify that the system is stable if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant) warning('One of the identified system is unstable'); end end
5.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
6 Initialize Elements
6.1 Experiment
This Matlab function is accessible here.
function [] = initializeExperiment(exp_name, sys_mass) if strcmp(exp_name, 'tomography') 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
6.2 Generate Reference Signals
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 '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); switch opts.Dh_type case 'constant' Dh = [opts.Dh_pos, opts.Dh_pos]; otherwise warning('Dh_type is not set correctly'); end Dh = struct('time', t, 'signals', struct('values', Dh)); %% Axis Compensation - Rm t = [0, Ts]; Rm = [opts.Rm_pos, opts.Rm_pos]; Rm = struct('time', t, 'signals', struct('values', Rm)); %% Nano-Hexapod t = [0, Ts]; Dn = zeros(length(t), 6); switch opts.Dn_type case 'constant' Dn = [opts.Dn_pos, opts.Dn_pos]; otherwise warning('Dn_type is not set correctly'); end Dn = struct('time', t, 'signals', struct('values', Dn)); %% Save save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts'); end
6.3 TODO Inputs
[ ]This function should not be used anymore. Now there are two functions to initialize disturbances and references.
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
6.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
6.5 Granite
This Matlab function is accessible here.
function [granite] = initializeGranite(opts_param) %% Default values for opts opts = struct('rigid', false); %% Populate opts with input parameters if exist('opts_param','var') for opt = fieldnames(opts_param)' opts.(opt{1}) = opts_param.(opt{1}); end 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 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.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
6.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 = 1000; % TODO [kg] %% 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.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
6.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 = 800; % TODO [kg] %% 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.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
6.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 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 % 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
6.9 Initialize Hexapod legs' length
This Matlab function is accessible here.
function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB) % initializeHexapodPosition - % % Syntax: initializeHexapodPosition(hexapod, AP, ARB) % % Inputs: % - hexapod - Hexapod object containing the geometry of the hexapod % - AP - Position vector of point OB expressed in frame {A} % - ARB - Rotation Matrix expressed in frame {A} L0 = zeros(6, 1); for i = 1:length(L) Bbi = hexapod.pos_top_tranform(i, :)'; Aai = hexapod.pos_base(i, :)'; L0(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai); end hexapod.L0 = L0; end
6.10 Micro Hexapod
This Matlab function is accessible here.
function [micro_hexapod] = initializeMicroHexapod(opts_param) %% Default values for opts opts = struct(... 'rigid', false, ... 'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} 'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A} ); %% Populate opts with input parameters if exist('opts_param','var') for opt = fieldnames(opts_param)' opts.(opt{1}) = opts_param.(opt{1}); end end %% 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] %% 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] 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); %% 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); %% Setup equilibrium position of each leg micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB); %% 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}) = 2*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 %% function [L] = initializeHexapodPosition(hexapod, AP, ARB) % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position % % Syntax: initializeHexapodPosition(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} % 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-micro_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 end
6.11 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
6.12 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
6.13 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 = 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 end
6.14 Cedrat Actuator
This Matlab function is accessible here.
function [cedrat] = initializeCedratPiezo(opts_param) %% Default values for opts opts = struct(); %% Populate opts with input parameters if exist('opts_param','var') for opt = fieldnames(opts_param)' opts.(opt{1}) = opts_param.(opt{1}); end end %% 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'); end
6.15 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 = 0.1*sqrt(sample.k.x*sample.mass); sample.k.y = 1e8; sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); sample.k.z = 1e8; sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); %% Save save('./mat/stages.mat', 'sample', '-append'); end