tdehaeze/phd-simscape-micro-station
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Re-tuned micro-station model

Browse Source
This commit is contained in:
Thomas Dehaeze 2024-10-31 10:37:01 +01:00
parent 151ae07072
commit 04c8b3c9dc
12 changed files with 563 additions and 554 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
matlab/src/computeReferencePose.m Normal file
View File

@ -0,0 +1,77 @@
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
%
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
%
% Inputs:
% - Dy - Reference of the Translation Stage [m]
% - Ry - Reference of the Tilt Stage [rad]
% - Rz - Reference of the Spindle [rad]
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
%
% Outputs:
% - WTr -
%% Translation Stage
Rty = [1 0 0 0;
0 1 0 Dy;
0 0 1 0;
0 0 0 1];
%% Tilt Stage - Pure rotating aligned with Ob
Rry = [ cos(Ry) 0 sin(Ry) 0;
0 1 0 0;
-sin(Ry) 0 cos(Ry) 0;
0 0 0 1];
%% Spindle - Rotation along the Z axis
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
sin(Rz) cos(Rz) 0 0 ;
0 0 1 0 ;
0 0 0 1 ];
%% Micro-Hexapod
Rhx = [1 0 0;
0 cos(Dh(4)) -sin(Dh(4));
0 sin(Dh(4)) cos(Dh(4))];
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
0 1 0;
-sin(Dh(5)) 0 cos(Dh(5))];
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
sin(Dh(6)) cos(Dh(6)) 0;
0 0 1];
Rh = [1 0 0 Dh(1) ;
0 1 0 Dh(2) ;
0 0 1 Dh(3) ;
0 0 0 1 ];
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
%% Nano-Hexapod
Rnx = [1 0 0;
0 cos(Dn(4)) -sin(Dn(4));
0 sin(Dn(4)) cos(Dn(4))];
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
0 1 0;
-sin(Dn(5)) 0 cos(Dn(5))];
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
sin(Dn(6)) cos(Dn(6)) 0;
0 0 1];
Rn = [1 0 0 Dn(1) ;
0 1 0 Dn(2) ;
0 0 1 Dn(3) ;
0 0 0 1 ];
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
%% Total Homogeneous transformation
WTr = Rty*Rry*Rrz*Rh*Rn;
end

147
matlab/src/describeMicroStationSetup.m Normal file
View File

@ -0,0 +1,147 @@
function [] = describeMicroStationSetup()
% describeMicroStationSetup -
%
% Syntax: [] = describeMicroStationSetup()
%
% Inputs:
% - -
%
% Outputs:
% - -
load('./mat/conf_simscape.mat', 'conf_simscape');
fprintf('Simscape Configuration:\n');
if conf_simscape.type == 1
fprintf('- Gravity is included\n');
else
fprintf('- Gravity is not included\n');
end
fprintf('\n');
load('./mat/nass_disturbances.mat', 'args');
fprintf('Disturbances:\n');
if ~args.enable
fprintf('- No disturbance is included\n');
else
if args.Dwx && args.Dwy && args.Dwz
fprintf('- Ground motion\n');
end
if args.Fty_x && args.Fty_z
fprintf('- Vibrations of the Translation Stage\n');
end
if args.Frz_z
fprintf('- Vibrations of the Spindle\n');
end
end
fprintf('\n');
load('./mat/nass_references.mat', 'args');
fprintf('Reference Tracking:\n');
fprintf('- Translation Stage:\n');
switch args.Dy_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
case 'triangular'
fprintf(' - Triangular Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
case 'sinusoidal'
fprintf(' - Sinusoidal Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
end
fprintf('- Tilt Stage:\n');
switch args.Ry_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
case 'triangular'
fprintf(' - Triangular Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
case 'sinusoidal'
fprintf(' - Sinusoidal Path\n');
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
end
fprintf('- Spindle:\n');
switch args.Rz_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
case { 'rotating', 'rotating-not-filtered' }
fprintf(' - Rotating\n');
fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period);
end
fprintf('- Micro Hexapod:\n');
switch args.Dh_type
case 'constant'
fprintf(' - Constant Position\n');
fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
end
fprintf('\n');
load('./mat/controller.mat', 'controller');
fprintf('Controller:\n');
fprintf('- %s\n', controller.name);
fprintf('\n');
load('./mat/stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
fprintf('Micro Station:\n');
if granite.type == 1 && ...
ty.type == 1 && ...
ry.type == 1 && ...
rz.type == 1 && ...
micro_hexapod.type == 1;
fprintf('- All stages are rigid\n');
elseif granite.type == 2 && ...
ty.type == 2 && ...
ry.type == 2 && ...
rz.type == 2 && ...
micro_hexapod.type == 2;
fprintf('- All stages are flexible\n');
else
if granite.type == 1 || granite.type == 4
fprintf('- Granite is rigid\n');
else
fprintf('- Granite is flexible\n');
end
if ty.type == 1 || ty.type == 4
fprintf('- Translation Stage is rigid\n');
else
fprintf('- Translation Stage is flexible\n');
end
if ry.type == 1 || ry.type == 4
fprintf('- Tilt Stage is rigid\n');
else
fprintf('- Tilt Stage is flexible\n');
end
if rz.type == 1 || rz.type == 4
fprintf('- Spindle is rigid\n');
else
fprintf('- Spindle is flexible\n');
end
if micro_hexapod.type == 1 || micro_hexapod.type == 4
fprintf('- Micro Hexapod is rigid\n');
else
fprintf('- Micro Hexapod is flexible\n');
end
end
fprintf('\n');

18
matlab/src/initializeGranite.m
View File

@ -1,11 +1,10 @@
function [granite] = initializeGranite(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m]
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [N/(m/s)]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
@ -21,10 +20,6 @@
granite.type = 1;
case 'flexible'
granite.type = 2;
case 'modal-analysis'
granite.type = 3;
case 'init'
granite.type = 4;
end
granite.density = args.density; % [kg/m3]
@ -35,13 +30,6 @@
granite.K = args.K; % [N/m]
granite.C = args.C; % [N/(m/s)]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fgm');
granite.Deq = -Fgm'./granite.K;
else
granite.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'granite', '-append');

17
matlab/src/initializeMicroHexapod.m
View File

@ -1,7 +1,7 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
@ -32,8 +32,6 @@
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
% Force that stiffness of each joint should apply at t=0
args.Foffset logical {mustBeNumericOrLogical} = false
end
stewart = initializeStewartPlatform();
@ -84,13 +82,6 @@
stewart = initializeInertialSensor(stewart, 'type', 'none');
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fhm');
stewart.actuators.dLeq = -Fhm'./args.Ki;
else
stewart.actuators.dLeq = zeros(6,1);
end
switch args.type
case 'none'
stewart.type = 0;
@ -98,12 +89,6 @@
stewart.type = 1;
case 'flexible'
stewart.type = 2;
case 'modal-analysis'
stewart.type = 3;
case 'init'
stewart.type = 4;
case 'compliance'
stewart.type = 5;
end
micro_hexapod = stewart;

14
matlab/src/initializeRy.m
View File

@ -1,8 +1,7 @@
function [ry] = initializeRy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
@ -15,10 +14,6 @@
ry.type = 1;
case 'flexible'
ry.type = 2;
case 'modal-analysis'
ry.type = 3;
case 'init'
ry.type = 4;
end
% Ry - Guide for the tilt stage
@ -44,13 +39,6 @@
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fym');
ry.Deq = -Fym'./ry.K;
else
ry.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'ry', '-append');

14
matlab/src/initializeRz.m
View File

@ -1,8 +1,7 @@
function [rz] = initializeRz(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
end
rz = struct();
@ -14,10 +13,6 @@
rz.type = 1;
case 'flexible'
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
% Spindle - Slip Ring
@ -35,13 +30,6 @@
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Fzm');
rz.Deq = -Fzm'./rz.K;
else
rz.Deq = zeros(6,1);
end
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')
save('mat/nass_stages.mat', 'rz', '-append');

16
matlab/src/initializeTy.m
View File

@ -1,8 +1,7 @@
function [ty] = initializeTy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = false
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
end
ty = struct();
@ -14,10 +13,6 @@
ty.type = 1;
case 'flexible'
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
% Ty Granite frame
@ -57,14 +52,7 @@
ty.rotor.STEP = 'Ty_Motor_Rotor.STEP';
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('Foffset.mat', 'Ftym');
ty.Deq = -Ftym'./ty.K;
else
ty.Deq = zeros(6,1);
end
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]
if exist('./mat', 'dir')
if exist('./mat/nass_stages.mat', 'file')

BIN
matlab/subsystems/micro_hexapod_bot_plate.slx
View File

Binary file not shown.

BIN
matlab/subsystems/micro_hexapod_strut.slx
View File

Binary file not shown.

BIN
matlab/subsystems/micro_hexapod_top_plate.slx
View File

Binary file not shown.

BIN
matlab/ustation_simscape.slx
View File

Binary file not shown.

814
simscape-micro-station.org
View File

@ -108,23 +108,6 @@ From modal analysis: validation of the multi-body model.
- Do not talk about external metrology
- Therefore, do not talk about computation of the sample position / error
Based on:
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org][kinematics]]: compute sample's motion from each stage motion
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/simscape_subsystems.org][simscape_subsystems]]: general presentation of the micro-station. Used model: solid body + joints. Presentation of each stage.
- [X] [[file:~/Cloud/work-projects/ID31-NASS/documents/work-package-1/work-package-1.org::*Specification of requirements][Specification of requirements]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/identification.org][identification]]: comparison of measurements and simscape model (not so good?)
- [ ] file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/experiments.org][experiments]]: simulation of experiments with the Simscape model
- [ ] Disturbances: Similar to what was done for the uniaxial model (the same?)
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/disturbances.org::+TITLE: Identification of the disturbances]]
- [ ] Measurement of disturbances / things that will have to be corrected using the NASS:
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-to-dynamic/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-control-system/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-sr-rz/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/ground-motion/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-spindle/index.org]]
- [ ] Check [[file:~/Cloud/work-projects/ID31-NASS/specifications/id-31-spindle-meas][this directory]] and [[file:~/Cloud/work-projects/ID31-NASS/specifications/stage-by-stage][this directory]]
** DONE [#B] Put colors for each different stage
CLOSED: [2024-10-30 Wed 14:07]
@ -173,8 +156,8 @@ CLOSED: [2024-10-30 Wed 16:15]
- [X] Init => Removed
- [X] modal analysis => Removed
** TODO [#A] Verify that we get "correct" compliance
SCHEDULED: <2024-10-30 Wed>
** DONE [#A] Verify that we get "correct" compliance
CLOSED: [2024-10-30 Wed 21:53] SCHEDULED: <2024-10-30 Wed>
- [ ] Find the compliance measurements
- The one from modal analysis
@ -185,9 +168,42 @@ SCHEDULED: <2024-10-30 Wed>
- [ ] If not, see if model parameters can be tuned to have better match
For instance from values here: file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
** DONE [#B] Make sure to well model the micro-hexapod
CLOSED: [2024-10-31 Thu 10:36] SCHEDULED: <2024-10-31 Thu>
- [ ] Find total mass (should be 37 kg)
- [ ] Find mass of top platform, bottom platform, struts
- [ ] Estimation of the strut stiffness from the manufacturer stiffness?
X,Y: 5N/um
Z: 50N/um
From the geometry, compute the strut stiffness: should be 10N/um (currently configured at 20N/um)
** TODO [#A] Import all relevant report to this file
Based on:
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org][kinematics]]: compute sample's motion from each stage motion
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/simscape_subsystems.org][simscape_subsystems]]: general presentation of the micro-station. Used model: solid body + joints. Presentation of each stage.
- [X] [[file:~/Cloud/work-projects/ID31-NASS/documents/work-package-1/work-package-1.org::*Specification of requirements][Specification of requirements]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/identification.org][identification]]: comparison of measurements and simscape model (not so good?)
- [ ] file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/experiments.org][experiments]]: simulation of experiments with the Simscape model
- [ ] Disturbances: Similar to what was done for the uniaxial model (the same?)
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/disturbances.org::+TITLE: Identification of the disturbances]]
- [ ] Measurement of disturbances / things that will have to be corrected using the NASS:
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-to-dynamic/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-control-system/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-sr-rz/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/ground-motion/index.org]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-spindle/index.org]]
- [ ] Check [[file:~/Cloud/work-projects/ID31-NASS/specifications/id-31-spindle-meas][this directory]] and [[file:~/Cloud/work-projects/ID31-NASS/specifications/stage-by-stage][this directory]]
** TODO [#B] Make good "init" for the Simscape model
** TODO [#C] Could see the effect of each stage on the compliance
In model properties => Callbacks => Init Fct
There are some loading of .mat files.
Is it the best option?
** TODO [#C] Check the effect of each stage on the compliance
- [ ] Put =rigid= mode one by one to see the effect
@ -215,13 +231,13 @@ By constraining more DoF, the simulation will be faster and the obtain state spa
* Introduction :ignore:
#+name: tab:ustation_section_matlab_code
#+caption: Report sections and corresponding Matlab files
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
#+attr_latex: :center t :booktabs t
| *Sections* | *Matlab File* |
|---------------------------------------------+-------------------------------------|
| Section ref:sec: | =ustation_1_.m= |
# #+name: tab:ustation_section_matlab_code
# #+caption: Report sections and corresponding Matlab files
# #+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
# #+attr_latex: :center t :booktabs t
# | *Sections* | *Matlab File* |
# |------------------+-----------------|
# | Section ref:sec: | =ustation_1_.m= |
* Micro-Station Kinematics
@ -272,7 +288,9 @@ By constraining more DoF, the simulation will be faster and the obtain state spa
*** Spindle
*** Micro-Hexapod
** Specification for each stage
<<ssec:spec_stage>>
<<ssec:spec_stage>>
Figure here: file:/home/thomas/Cloud/work-projects/ID31-NASS/documents/work-package-1/figures
For each stage, after a quick presentation, the specifications (stroke, precision, ...) of the stage, the metrology used with that stage and the parasitic motions associated to it are given.
@ -770,13 +788,7 @@ Some of the springs and dampers values can be estimated from the joints/stages s
** Compare with measurements at the CoM of each element
#+begin_src matlab
%% We load the configuration.
load('conf_simulink.mat');
% We set a small =StopTime=.
set_param(conf_simulink, 'StopTime', '0.5');
%% We initialize all the stages.
%% All stages are initialized
initializeGround( 'type', 'rigid');
initializeGranite( 'type', 'flexible');
initializeTy( 'type', 'flexible');
@ -937,81 +949,86 @@ This is what can impact the nano-hexapod dynamics.
- *Was the rotation compensation axis present?* (I don't think so)
*** Position of inertial sensors on top of the micro-hexapod
Orientation is relative to the frame determined by the X-ray
| *Num* | *Position* | *Orientation* | *Sensibility* | *Channels* |
|-------+------------+---------------+---------------+------------|
| 1 | [0, +A, 0] | [x, y, z] | 1V/g | 1-3 |
| 2 | [-B, 0, 0] | [x, y, z] | 1V/g | 4-6 |
| 3 | [0, -A, 0] | [x, y, z] | 0.1V/g | 7-9 |
| 4 | [+B, 0, 0] | [x, y, z] | 1V/g | 10-12 |
4 accelerometers are fixed to the micro-hexapod top platform.
Instrumented Hammer:
- Channel 13
- Sensibility: 230 uV/N
# | *Num* | *Position* | *Orientation* | *Sensibility* | *Channels* |
# |-------+------------+---------------+---------------+------------|
# | 1 | [0, +A, 0] | [x, y, z] | 1V/g | 1-3 |
# | 2 | [-B, 0, 0] | [x, y, z] | 1V/g | 4-6 |
# | 3 | [0, -A, 0] | [x, y, z] | 0.1V/g | 7-9 |
# | 4 | [+B, 0, 0] | [x, y, z] | 1V/g | 10-12 |
| Acc Number | Dir | Channel Number |
|------------+-----+----------------|
| 1 | x | 1 |
| 1 | y | 2 |
| 1 | z | 3 |
| 2 | x | 4 |
| 2 | y | 5 |
| 2 | z | 6 |
| 3 | x | 7 |
| 3 | y | 8 |
| 3 | z | 9 |
| 4 | x | 10 |
| 4 | y | 11 |
| 4 | z | 12 |
| Hammer | | 13 |
# Instrumented Hammer:
# - Channel 13
# - Sensibility: 230 uV/N
From the acceleration measurement of the 4 accelerometers, we can compute the translations and rotations:
| | *Formula* | *Formula (numbers)* |
|-------+--------------------------+-----------------------|
| $D_x$ | (1x + 2x + 3x + 4x)/4 | (1 + 4 + 7 + 10)/4 |
| $D_y$ | (1y + 2y + 3y + 4y)/4 | (2 + 5 + 8 + 11)/4 |
| $D_z$ | (1z + 2z + 3z + 4z)/4 | (3 + 6 + 9 + 12)/4 |
| $R_x$ | (1z - 3z)/A | (1 - 9)/A |
| $R_y$ | (2z - 4z)/B | (6 - 12)/B |
| $R_z$ | (3x - 1x)/A, (4y - 2y)/B | (7 - 1)/A, (11 - 5)/B |
# | Acc Number | Dir | Channel Number |
# |------------+-----+----------------|
# | 1 | x | 1 |
# | 1 | y | 2 |
# | 1 | z | 3 |
# | 2 | x | 4 |
# | 2 | y | 5 |
# | 2 | z | 6 |
# | 3 | x | 7 |
# | 3 | y | 8 |
# | 3 | z | 9 |
# | 4 | x | 10 |
# | 4 | y | 11 |
# | 4 | z | 12 |
# | Hammer | | 13 |
To convert the 12 acceleration signals $\mathbf{a}_{\mathcal{L}} = [a_{1x}\ a_{1y}\ a_{1z}\ a_{2x}\ \dots\ a_{4z}]$ to the acceleration expressed in cartesian coordinate $\mathbf{a}_{\mathcal{X}} [a_{dx}\ a_{dy}\ a_{dz}\ a_{rx}\ a_{ry}\ a_{rz}]$, a Jacobian matrix can be written based on the positions and orientations of the accelerometers.
\begin{equation}
\mathbf{a}_{\mathcal{L}} = J_a \cdot \mathbf{a}_{\mathcal{X}}
\end{equation}
\begin{equation}
J_a = \begin{bmatrix}
1 & 0 & 0 & 0 & 0 &-d \\
0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 1 & d & 0 & 0 \\
1 & 0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 &-d \\
0 & 0 & 1 & 0 & d & 0 \\
1 & 0 & 0 & 0 & 0 & d \\
0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 1 &-d & 0 & 0 \\
1 & 0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 & d \\
0 & 0 & 1 & 0 &-d & 0
\end{bmatrix}
\end{equation}
Then, the acceleration in the cartesian frame can be computed
\begin{equation}
\mathbf{a}_{\mathcal{X}} = J_a^\dagger \cdot \mathbf{a}_{\mathcal{L}}
\end{equation}
dL = J X
#+begin_src matlab
d = 0.14;
J = [1 0 0 0 0 -d;
0 1 0 0 0 0;
0 0 1 d 0 0;
1 0 0 0 0 0;
0 1 0 0 0 -d;
0 0 1 0 d 0;
1 0 0 0 0 d;
0 1 0 0 0 0;
0 0 1 -d 0 0;
1 0 0 0 0 0;
0 1 0 0 0 d;
0 0 1 0 -d 0];
%% Jacobian for accelerometers
% L = Ja X
d = 0.14; % [m]
Ja = [1 0 0 0 0 -d;
0 1 0 0 0 0;
0 0 1 d 0 0;
1 0 0 0 0 0;
0 1 0 0 0 -d;
0 0 1 0 d 0;
1 0 0 0 0 d;
0 1 0 0 0 0;
0 0 1 -d 0 0;
1 0 0 0 0 0;
0 1 0 0 0 d;
0 0 1 0 -d 0];
J_inv = pinv(J);
Ja_inv = pinv(Ja);
#+end_src
| | Dx | Dy | Dz | Rx | Ry | Rz |
|-----+----+----+----+----+----+----|
| a1x | 1 | 0 | 0 | 0 | 0 | -d |
| a1y | 0 | 1 | 0 | 0 | 0 | 0 |
| a1z | 0 | 0 | 1 | d | 0 | 0 |
| a2x | 1 | 0 | 0 | 0 | 0 | 0 |
| a2y | 0 | 1 | 0 | 0 | 0 | -d |
| a2z | 0 | 0 | 1 | 0 | d | 0 |
| a3x | 1 | 0 | 0 | 0 | 0 | d |
| a3y | 0 | 1 | 0 | 0 | 0 | 0 |
| a3z | 0 | 0 | 1 | -d | 0 | 0 |
| a4x | 1 | 0 | 0 | 0 | 0 | 0 |
| a4y | 0 | 1 | 0 | 0 | 0 | d |
| a4z | 0 | 0 | 1 | 0 | -d | 0 |
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable(J_inv, {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'d1x', 'd1y', 'd1z', 'd2x', 'd2y', 'd2z', 'd3x', 'd3y', 'd3z', 'd4x', 'd4y', 'd4z'}, ' %.5f ');
data2orgtable(Ja_inv, {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'d1x', 'd1y', 'd1z', 'd2x', 'd2y', 'd2z', 'd3x', 'd3y', 'd3z', 'd4x', 'd4y', 'd4z'}, ' %.5f ');
#+end_src
#+RESULTS:
@ -1026,6 +1043,8 @@ data2orgtable(J_inv, {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'d1x', 'd1y', 'd1z',
*** Hammer blow position/orientation
10 hammer hits are performed as shown in Figure ...
| *Num* | *Direction* | *Position* | Accelerometer position | Jacobian number |
|-------+-------------+------------+------------------------+-----------------|
| 1 | -Y | [0, +A, 0] | 1 | -2 |
@ -1039,18 +1058,30 @@ data2orgtable(J_inv, {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'d1x', 'd1y', 'd1z',
| 9 | -X | [0, -A, 0] | 3 | -7 |
| 10 | -X | [0, +A, 0] | 1 | -1 |
From hammer blows to pure forces / torques:
| | *Formula* | Alternative |
|-------+--------------+-------------|
| $F_x$ | +3 | -7 |
| $F_y$ | -1 | +5 |
| $F_z$ | -(2 + 6)/2 | -(4 + 8)/2 |
| $M_x$ | A/2*(2 - 6) | |
| $M_y$ | B/2*(8 - 4) | |
| $M_z$ | A/2*(10 - 9) | |
Similar to what is done for the accelerometers, a Jacobian matrix can be computed to convert the individual hammer forces to force and torques applied at the center of the micro-hexapod top plate.
\begin{equation}
\mathbf{F}_{\mathcal{X}} = J_F^t \cdot \mathbf{F}_{\mathcal{L}}
\end{equation}
\begin{equation}
J_F = \begin{bmatrix}
0 & -1 & 0 & 0 & 0 & 0\\
0 & 0 & -1 & -d & 0 & 0\\
1 & 0 & 0 & 0 & 0 & 0\\
0 & 0 & -1 & 0 & -d & 0\\
0 & 1 & 0 & 0 & 0 & 0\\
0 & 0 & -1 & d & 0 & 0\\
-1 & 0 & 0 & 0 & 0 & 0\\
0 & 0 & -1 & 0 & d & 0\\
-1 & 0 & 0 & 0 & 0 & -d\\
-1 & 0 & 0 & 0 & 0 & d
\end{bmatrix}
\end{equation}
F = J' tau
#+begin_src matlab
%% Jacobian for hammer impacts
% F = Jf' tau
Jf = [0 -1 0 0 0 0;
0 0 -1 -d 0 0;
1 0 0 0 0 0;
@ -1079,56 +1110,50 @@ data2orgtable(Jf, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}, {'-F1y', '-F1z', '+F2x',
| Mz | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | 0.0 | -0.14 | 0.14 |
*** Compute FRF
Raw measurements are in file:/home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/micro-station-compliance/data/record
For each measurement (10):
- Find the location of the 10 impacts based on "track13"
- Then for the 12 accelerometer data, compute the FRF, and average them for the 10 impacts
- Maybe have to take into account the sensitivity, etc...
#+begin_src matlab
raw_data_path = '/home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/micro-station-compliance/data/record/';
%% Load raw measurement data
% "Track1" to "Track12" are the 12 accelerometers
% "Track13" is the instrumented hammer force sensor
data = [
load(sprintf('%s/Measurement1.mat', raw_data_path)), ...
load(sprintf('%s/Measurement2.mat', raw_data_path)), ...
load(sprintf('%s/Measurement3.mat', raw_data_path)), ...
load(sprintf('%s/Measurement4.mat', raw_data_path)), ...
load(sprintf('%s/Measurement5.mat', raw_data_path)), ...
load(sprintf('%s/Measurement6.mat', raw_data_path)), ...
load(sprintf('%s/Measurement7.mat', raw_data_path)), ...
load(sprintf('%s/Measurement8.mat', raw_data_path)), ...
load(sprintf('%s/Measurement9.mat', raw_data_path)), ...
load(sprintf('%s/Measurement10.mat', raw_data_path))];
#+end_src
load('ustation_compliance_hammer_1.mat'), ...
load('ustation_compliance_hammer_2.mat'), ...
load('ustation_compliance_hammer_3.mat'), ...
load('ustation_compliance_hammer_4.mat'), ...
load('ustation_compliance_hammer_5.mat'), ...
load('ustation_compliance_hammer_6.mat'), ...
load('ustation_compliance_hammer_7.mat'), ...
load('ustation_compliance_hammer_8.mat'), ...
load('ustation_compliance_hammer_9.mat'), ...
load('ustation_compliance_hammer_10.mat')];
#+begin_src matlab
%% Prepare the FRF computation
Ts = 1e-3; % Sampling Time [s]
Nfft = floor(1/Ts); % Number of points for the FFT computation
% win = hanning(Nfft); % Hanning window
win = ones(Nfft, 1); % Rectangular window
% Get the frequency vector
[~, f] = tfestimate(data(1).Track13, data(1).Track1, win, [], Nfft, 1/Ts);
#+end_src
#+begin_src matlab
% Samples taken before and after the hammer "impact"
pre_n = floor(0.1/Ts);
post_n = Nfft - pre_n - 1;
#+end_src
#+begin_src matlab
%% Compute the FRFs for the 10 hammer impact locations.
% The FRF from hammer force the 12 accelerometers are computed
% for each of the 10 hammer impacts and averaged
G_raw = zeros(12,10,length(f));
for i = 1:10
% Find the impacts
for i = 1:10 % For each impact location
% Find the 10 impacts
[~, impacts_i] = find(diff(data(i).Track13 > 50)==1);
% Only keep the first 10 impacts if there are more than 10 impacts
if length(impacts_i)>10
impacts_i(11:end) = [];
end
% Initialize the FRF for the current experiment
G_impact = zeros(12,length(f));
% Average the FRF for the 10 impacts
for impact_i = impacts_i
i_start = impacts_i - pre_n;
i_end = impacts_i + post_n;
i_start = impact_i - pre_n;
i_end = impact_i + post_n;
data_in = data(i).Track13(i_start:i_end); % [N]
% Remove hammer DC offset
data_in = data_in - mean(data_in(end-pre_n:end));
@ -1147,162 +1172,56 @@ for i = 1:10
data(i).Track12(i_start:i_end)];
[frf, ~] = tfestimate(data_in, data_out', win, [], Nfft, 1/Ts);
G_raw(:,i,:) = frf'./(-(2*pi*f').^2);
G_raw(:,i,:) = squeeze(G_raw(:,i,:)) + 0.1*frf'./(-(2*pi*f').^2);
end
end
%% Compute transfer function in cartesian frame using Jacobian matrices
% FRF_cartesian = inv(Ja) * FRF * inv(Jf)
FRF_cartesian = pagemtimes(Ja_inv, pagemtimes(G_raw, Jf_inv));
#+end_src
#+begin_src matlab
%% Compute transfer function in cartesian frame
G_compl_b = pagemtimes(J_inv, pagemtimes(G_raw, Jf_inv));
#+end_src
#+begin_src matlab
colors = colororder;
#+begin_src matlab :exports none :results none
%% Measured FRF of the compliance of the micro-station in the Cartesian frame
figure;
hold on;
% plot(freqs, abs(squeeze(G_compl(1,1,:))), 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
% plot(freqs, abs(squeeze(G_compl(2,2,:))), 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
% plot(freqs, abs(squeeze(G_compl(3,3,:))), 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
plot(f, abs(squeeze(G_compl_b(1,1,:))), '-', 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
plot(f, abs(squeeze(G_compl_b(2,2,:))), '-', 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
plot(f, abs(squeeze(G_compl_b(3,3,:))), '-', 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
plot(f, abs(squeeze(FRF_cartesian(1,1,:))), '.', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$')
plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '.', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$')
plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '.', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([30, 300]); ylim([1e-9, 2e-6]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src
*** Load Data
#+begin_src matlab
% Load data
data = [load('data/Measurement1.mat'), ...
load('data/Measurement2.mat'), ...
load('data/Measurement3.mat'), ...
load('data/Measurement4.mat'), ...
load('data/Measurement5.mat'), ...
load('data/Measurement6.mat'), ...
load('data/Measurement7.mat'), ...
load('data/Measurement8.mat'), ...
load('data/Measurement9.mat'), ...
load('data/Measurement10.mat')];
#+end_src
#+begin_src matlab
% Frequency Vector
freqs = m3.FFT1_H1_1_13_X_Val;
w = 2*pi*freqs;
% 12 outputs, 10 inputs
G_raw = zeros(12, 10, length(freqs));
for j = 1:10
for i = 1:12
G_raw(i,j,:) = data(j).(sprintf("FFT1_H1_%i_13_Y_ReIm", i))./(-w.^2);
end
end
#+end_src
#+begin_src matlab
%% Compute transfer function in cartesian frame
G_compl = pagemtimes(J_inv, pagemtimes(G_raw, Jf_inv));
#+end_src
#+begin_src matlab
colors = colororder;
figure;
hold on;
plot(freqs, abs(squeeze(G_compl(1,1,:))), 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
plot(freqs, abs(squeeze(G_compl(2,2,:))), 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
plot(freqs, abs(squeeze(G_compl(3,3,:))), 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
plot(f, abs(squeeze(G_compl_b(1,1,:))), '.', 'color', colors(1,:), 'DisplayName', '$C_x/F_x$')
plot(f, abs(squeeze(G_compl_b(2,2,:))), '.', 'color', colors(2,:), 'DisplayName', '$C_y/F_y$')
plot(f, abs(squeeze(G_compl_b(3,3,:))), '.', 'color', colors(3,:), 'DisplayName', '$C_z/F_z$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([30, 300]); ylim([1e-9, 2e-6]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([20, 500]); ylim([1e-9, 2e-6]);
xticks([20, 50, 100, 200, 500])
#+end_src
#+begin_src matlab
colors = colororder;
figure;
hold on;
plot(freqs, abs(squeeze(G_compl(4,4,:))), 'color', colors(1,:), 'DisplayName', '$R_x/M_x$')
plot(freqs, abs(squeeze(G_compl(5,5,:))), 'color', colors(2,:), 'DisplayName', '$R_y/M_y$')
plot(freqs, abs(squeeze(G_compl(6,6,:))), 'color', colors(3,:), 'DisplayName', '$R_z/M_z$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([30, 300]); ylim([5e-7, 5e-5]);
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/ustation_frf_compliance_xyz.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
*** Diagonal Dynamics
#+begin_src matlab
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(G(1,1,:)), '-', 'DisplayName', '$D_x/F_x$')
plot(freqs, abs(squeeze(G(2,2,:)), '-', 'DisplayName', '$D_y/F_y$')
plot(freqs, abs(squeeze(G(3,3,:)), '-', 'DisplayName', '$D_z/F_z$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
ylim([1e-9, 2e-6]);
legend('location', 'southwest');
xlim([30, 300]);
#+end_src
#+begin_src matlab
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(G(4,4,:)), '-', 'DisplayName', '$R_x/M_x$')
plot(freqs, abs(squeeze(G(5,5,:)), '-', 'DisplayName', '$R_y/M_y$')
plot(freqs, abs(squeeze(G(6,6,:)), '-', 'DisplayName', '$R_z/M_z$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [rad/Nm]');
ylim([1e-7, 2e-4]);
legend('location', 'southwest');
xlim([30, 300]);
#+end_src
*** Equivalent Stiffness and Mass Estimation
#+begin_src matlab
K = [1e7, 1e7, 2e8, 5e7, 3e7, 2e7];
f_res = [125, 135, 390, 335, 335, 160];
#+end_src
#+begin_src matlab
M = [20, 20, 20, 11, 7, 20];
f_res_est = sqrt(K./M)./(2*pi);
#+end_src
Here is the inertia / stiffness to the granite that can represent the micro-station compliance dynamics:
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([K'], {'x', 'y', 'z', 'Rx', 'Ry', 'Rz'}, {'Stiffness', 'Inertia'}, ' %.1g ');
#+end_src
#+name: fig:ustation_frf_compliance_xyz
#+caption: Measured FRF of the compliance of the micro-station in the Cartesian frame
#+RESULTS:
| Stiffness | Inertia |
|-----------+-------------|
| x | 10000000.0 |
| y | 10000000.0 |
| z | 200000000.0 |
| Rx | 50000000.0 |
| Ry | 30000000.0 |
| Rz | 20000000.0 |
[[file:figs/ustation_frf_compliance_xyz.png]]
#+begin_src matlab
figure;
hold on;
plot(f, abs(squeeze(FRF_cartesian(4,4,:))), '-', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$')
plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$')
plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([30, 300]); ylim([1e-9, 2e-6]);
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
#+end_src
** Compare with the Model
#+begin_src matlab
%% Initialize simulation with default parameters (flexible elements)
initializeGround();
@ -1333,196 +1252,121 @@ Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
#+end_src
#+begin_src matlab :exports none
%% Plot of the compliance curves
labels = {'$D_x/F_{x}$', '$D_y/F_{y}$', '$D_z/F_{z}$', '$R_{x}/M_{x}$', '$R_{y}/M_{y}$', '$R_{R}/M_{z}$'};
freqs = logspace(1, 3, 1000);
#+begin_src matlab :exports none :results none
%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
figure;
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gm(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
plot(f, abs(squeeze(FRF_cartesian(1,1,:))), '.', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$ - Measured')
plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '.', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$ - Measured')
plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '.', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$ - Measured')
plot(freqs, abs(squeeze(freqresp(Gm(1,1), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$ - Model')
plot(freqs, abs(squeeze(freqresp(Gm(2,2), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$ - Model')
plot(freqs, abs(squeeze(freqresp(Gm(3,3), freqs, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$ - Model')
hold off;
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance');
legend('location', 'northwest');
#+end_src
- [ ] Comparison with the estimated (or measured) compliance
#+begin_src matlab
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(G(1,1,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(2,2,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(3,3,:))./(-w.^2)), '.')
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(2,2,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(3,3,:), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, 2e-6]);
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(G(1,1,:))./(-w.^2)), '.', 'DisplayName', '$D_x/F_x$')
plot(freqs, 180/pi*angle(squeeze(G(2,2,:))./(-w.^2)), '.', 'DisplayName', '$D_y/F_y$')
plot(freqs, 180/pi*angle(squeeze(G(3,3,:))./(-w.^2)), '.', 'DisplayName', '$D_z/F_z$')
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(2,2,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(3,3,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
xlim([30, 300]);
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([20, 500]); ylim([2e-10, 1e-6]);
xticks([20, 50, 100, 200, 500])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/compliance_diagonal_translations_comp_model.pdf', 'width', 'full', 'height', 'full');
exportFig('figs/ustation_frf_compliance_xyz_model.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:compliance_diagonal_translations_comp_model
#+caption: Dynamics from Forces to Translations
#+name: fig:ustation_frf_compliance_xyz_model
#+caption: Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
#+RESULTS:
[[file:figs/compliance_diagonal_translations_comp_model.png]]
[[file:figs/ustation_frf_compliance_xyz_model.png]]
#+begin_src matlab
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(G(4,4,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(5,5,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(6,6,:))./(-w.^2)), '.')
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gm(4,4,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(5,5,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(6,6,:), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [rad/Nm]'); set(gca, 'XTickLabel',[]);
% ylim([1e-9, 2e-6]);
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(G(4,4,:))./(-w.^2)), '.', 'DisplayName', '$R_x/M_x$')
plot(freqs, 180/pi*angle(squeeze(G(5,5,:))./(-w.^2)), '.', 'DisplayName', '$R_y/M_y$')
plot(freqs, 180/pi*angle(squeeze(G(6,6,:))./(-w.^2)), '.', 'DisplayName', '$R_z/M_z$')
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(4,4,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(5,5,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(6,6,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
xlim([30, 300]);
#+begin_src matlab :exports none :results none
%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
figure;
hold on;
plot(f, abs(squeeze(FRF_cartesian(4,4,:))), '.', 'color', colors(1,:), 'DisplayName', '$R_x/M_x$ - Measured')
plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '.', 'color', colors(2,:), 'DisplayName', '$R_y/M_y$ - Measured')
plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '.', 'color', colors(3,:), 'DisplayName', '$R_z/M_z$ - Measured')
plot(freqs, abs(squeeze(freqresp(Gm(4,4), freqs, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$R_x/M_x$ - Model')
plot(freqs, abs(squeeze(freqresp(Gm(5,5), freqs, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$R_y/M_y$ - Model')
plot(freqs, abs(squeeze(freqresp(Gm(6,6), freqs, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$R_z/M_z$ - Model')
hold off;
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
xlim([20, 500]); ylim([2e-7, 1e-4]);
xticks([20, 50, 100, 200, 500])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/compliance_diagonal_rotations_comp_model.pdf', 'width', 'full', 'height', 'full');
exportFig('figs/ustation_frf_compliance_Rxyz_model.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:compliance_diagonal_rotations_comp_model
#+caption: Dynamics from Torques to Rotations
#+name: fig:ustation_frf_compliance_Rxyz_model
#+caption: Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
#+RESULTS:
[[file:figs/compliance_diagonal_rotations_comp_model.png]]
[[file:figs/ustation_frf_compliance_Rxyz_model.png]]
| | Stiffness | Unit |
|-----------+-----------+----------|
| $K_x$ | 1e7 | [N/m] |
| $K_y$ | 1e7 | [N/m] |
| $K_z$ | 2e8 | [N/m] |
| $K_{R_x}$ | 5e7 | [Nm/rad] |
| $K_{R_y}$ | 3e7 | [Nm/rad] |
| $K_{R_z}$ | 2e7 | [Nm/rad] |
*** Coupling Dynamics
** Get resonance frequencies
#+begin_src matlab
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(G(1,1,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(2,1,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(3,1,:))./(-w.^2)), '.')
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(2,1,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(3,1,:), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, 2e-6]);
%% Initialize simulation with default parameters (flexible elements)
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(G(1,1,:))./(-w.^2)), '.', 'DisplayName', '$D_x/F_x$')
plot(freqs, 180/pi*angle(squeeze(G(2,1,:))./(-w.^2)), '.', 'DisplayName', '$D_y/F_x$')
plot(freqs, 180/pi*angle(squeeze(G(3,1,:))./(-w.^2)), '.', 'DisplayName', '$D_z/F_x$')
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(1,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(2,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(3,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');
initializeReferences();
initializeDisturbances();
initializeSimscapeConfiguration();
initializeLoggingConfiguration();
#+end_src
linkaxes([ax1,ax2],'x');
xlim([30, 300]);
And we identify the dynamics from forces/torques applied on the micro-hexapod top platform to the motion of the micro-hexapod top platform at the same point.
#+begin_src matlab
%% Identification of the compliance
% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform
% Run the linearization
Gm = linearize(mdl, io, 0);
Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
#+end_src
#+begin_src matlab
figure;
ax1 = subplot(2,1,1);
hold on;
plot(freqs, abs(squeeze(G(5,1,:))./(-w.^2)), '.')
plot(freqs, abs(squeeze(G(4,2,:))./(-w.^2)), '.')
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gm(5,1,:), freqs, 'Hz'))), '-')
plot(freqs, abs(squeeze(freqresp(Gm(4,2,:), freqs, 'Hz'))), '-')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude [m/N]'); set(gca, 'XTickLabel',[]);
ylim([1e-9, 2e-6]);
ax2 = subplot(2,1,2);
hold on;
plot(freqs, 180/pi*angle(squeeze(G(5,1,:))./(-w.^2)), '.', 'DisplayName', '$R_y/F_x$')
plot(freqs, 180/pi*angle(squeeze(G(4,2,:))./(-w.^2)), '.', 'DisplayName', '$R_x/F_y$')
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(5,1,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gm(4,2,:), freqs, 'Hz'))), '-', 'HandleVisibility', 'off')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Freqency [Hz]'); ylabel('Phase [deg]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
xlim([30, 300]);
modes_freq = imag(eig(Gm))/2/pi;
modes_freq = sort(modes_freq(modes_freq>0));
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
data2orgtable([modes_freq(1:16), [11.9, 18.6, 37.8, 39.1, 56.3, 69.8, 72.5, 84.8, 91.3, 105.5, 106.6, 112.7, 124.2, 145.3, 150.5, 165.4]'], {'1', '2', '3', '4', '5', '6', '7', '8', '9', '10', '11', '12', '13', '14', '15', '16'}, {'Mode', 'Simscape', 'Modal analysis'}, ' %.1f ');
#+end_src
#+RESULTS:
| Mode | Simscape | Modal analysis |
|------+----------+----------------|
| 1 | 11.7 | 11.9 |
| 2 | 21.3 | 18.6 |
| 3 | 26.1 | 37.8 |
| 4 | 57.5 | 39.1 |
| 5 | 60.6 | 56.3 |
| 6 | 73.0 | 69.8 |
| 7 | 97.9 | 72.5 |
| 8 | 120.2 | 84.8 |
| 9 | 126.2 | 91.3 |
| 10 | 142.4 | 105.5 |
| 11 | 155.9 | 106.6 |
| 12 | 178.5 | 112.7 |
| 13 | 179.3 | 124.2 |
| 14 | 182.6 | 145.3 |
| 15 | 223.6 | 150.5 |
| 16 | 226.4 | 165.4 |
** Conclusion
For such a complex system, we believe that the Simscape Model represents the dynamics of the system with enough fidelity.
@ -1589,6 +1433,8 @@ For such a complex system, we believe that the Simscape Model represents the dyn
* Estimation of disturbances
This was already done in uni-axial model.
* Conclusion
<<sec:uniaxial_conclusion>>
@ -1596,6 +1442,46 @@ For such a complex system, we believe that the Simscape Model represents the dyn
* Bibliography :ignore:
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
* Helping Functions :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/'); % Path for scripts
%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for Computed FRF
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/STEPS/'); % Path for STEPS
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
#+END_SRC
#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files
#+END_SRC
** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
% Simulink Model name
mdl = 'ustation_simscape';
load('conf_simulink.mat');
#+end_src
** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);
#+END_SRC
* Matlab Functions :noexport:
** Simscape Configuration
:PROPERTIES:
@ -1836,8 +1722,8 @@ end
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m]
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [N/(m/s)]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
@ -2010,7 +1896,7 @@ Define the density of the materials as well as the geometry (STEP files).
#+begin_src matlab
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]
#+end_src
*** Save the Structure
@ -4403,41 +4289,3 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
end
#+end_src
* Helping Functions :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/'); % Path for scripts
%% Path for functions, data and scripts
addpath('./matlab/mat/'); % Path for Computed FRF
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/STEPS/'); % Path for STEPS
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
#+END_SRC
#+NAME: m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./mat/'); % Path for Data
addpath('./src/'); % Path for functions
addpath('./STEPS/'); % Path for STEPS
addpath('./subsystems/'); % Path for Subsystems Simulink files
#+END_SRC
** Initialize Simscape Model
#+NAME: m-init-simscape
#+begin_src matlab
% Simulink Model name
mdl = 'ustation_simscape';
#+end_src
** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
%% Frequency Vector
freqs = logspace(log10(10), log10(2e3), 1000);
#+END_SRC