add all files
This commit is contained in:
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,21 @@
|
||||
function [xc,yc,R,a] = circlefit(x,y)
|
||||
%
|
||||
% [xc yx R] = circfit(x,y)
|
||||
%
|
||||
% fits a circle in x,y plane in a more accurate
|
||||
% (less prone to ill condition )
|
||||
% procedure than circfit2 but using more memory
|
||||
% x,y are column vector where (x(i),y(i)) is a measured point
|
||||
%
|
||||
% result is center point (yc,xc) and radius R
|
||||
% an optional output is the vector of coeficient a
|
||||
% describing the circle's equation
|
||||
%
|
||||
% x^2+y^2+a(1)*x+a(2)*y+a(3)=0
|
||||
%
|
||||
% By: Izhak bucher 25/oct /1991,
|
||||
x=x(:); y=y(:);
|
||||
a=[x y ones(size(x))]\[-(x.^2+y.^2)];
|
||||
xc = -.5*a(1);
|
||||
yc = -.5*a(2);
|
||||
R = sqrt((a(1)^2+a(2)^2)/4-a(3));
|
||||
@@ -0,0 +1,35 @@
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
|
||||
J = [As' , cross(Ab, As)'];
|
||||
|
||||
K = J'*diag(Ki)*J;
|
||||
|
||||
C = inv(K);
|
||||
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
@@ -0,0 +1,78 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
||||
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
||||
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
||||
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
|
||||
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
||||
Fa = stewart.platform_F.Fa;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
||||
Mb = stewart.platform_M.Mb;
|
||||
|
||||
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
||||
FO_A = stewart.platform_F.FO_A;
|
||||
|
||||
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
||||
MO_B = stewart.platform_M.MO_B;
|
||||
|
||||
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
||||
FO_M = stewart.geometry.FO_M;
|
||||
|
||||
Aa = Fa - repmat(FO_A, [1, 6]);
|
||||
Bb = Mb - repmat(MO_B, [1, 6]);
|
||||
|
||||
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
||||
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
||||
|
||||
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
||||
|
||||
l = vecnorm(Ab - Aa)';
|
||||
|
||||
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
||||
|
||||
FRa = zeros(3,3,6);
|
||||
MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
||||
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
||||
|
||||
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
||||
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
||||
end
|
||||
|
||||
stewart.geometry.Aa = Aa;
|
||||
stewart.geometry.Ab = Ab;
|
||||
stewart.geometry.Ba = Ba;
|
||||
stewart.geometry.Bb = Bb;
|
||||
stewart.geometry.As = As;
|
||||
stewart.geometry.Bs = Bs;
|
||||
stewart.geometry.l = l;
|
||||
|
||||
stewart.struts_F.l = l/2;
|
||||
stewart.struts_M.l = l/2;
|
||||
|
||||
stewart.platform_F.FRa = FRa;
|
||||
stewart.platform_M.MRb = MRb;
|
||||
@@ -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
|
||||
@@ -0,0 +1,141 @@
|
||||
function [] = describeMicroStationSetup()
|
||||
% describeMicroStationSetup -
|
||||
%
|
||||
% Syntax: [] = describeMicroStationSetup()
|
||||
%
|
||||
% Inputs:
|
||||
% - -
|
||||
%
|
||||
% Outputs:
|
||||
% - -
|
||||
|
||||
load('./mat/nass_model_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_model_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.Fdy_x && args.Fdy_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_model_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/nass_model_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');
|
||||
@@ -0,0 +1,39 @@
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
Fa = zeros(3,6);
|
||||
Mb = zeros(3,6);
|
||||
|
||||
for i = 1:6
|
||||
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
|
||||
stewart.platform_F.Fa = Fa;
|
||||
stewart.platform_M.Mb = Mb;
|
||||
@@ -0,0 +1,59 @@
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F [struct] - structure with the following fields:
|
||||
% - type = 1
|
||||
% - M [1x1] - Fixed Platform Mass [kg]
|
||||
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Fixed Platform Height [m]
|
||||
% - R [1x1] - Fixed Platform Radius [m]
|
||||
% - platform_M [struct] - structure with the following fields:
|
||||
% - M [1x1] - Mobile Platform Mass [kg]
|
||||
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - H [1x1] - Mobile Platform Height [m]
|
||||
% - R [1x1] - Mobile Platform Radius [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
end
|
||||
|
||||
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
||||
1/2 *args.Fpm * args.Fpr^2]);
|
||||
|
||||
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
||||
1/2 *args.Mpm * args.Mpr^2]);
|
||||
|
||||
stewart.platform_F.type = 1;
|
||||
|
||||
stewart.platform_F.I = I_F;
|
||||
stewart.platform_F.M = args.Fpm;
|
||||
stewart.platform_F.R = args.Fpr;
|
||||
stewart.platform_F.H = args.Fph;
|
||||
|
||||
stewart.platform_M.type = 1;
|
||||
|
||||
stewart.platform_M.I = I_M;
|
||||
stewart.platform_M.M = args.Mpm;
|
||||
stewart.platform_M.R = args.Mpr;
|
||||
stewart.platform_M.H = args.Mph;
|
||||
@@ -0,0 +1,71 @@
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts_F [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - struts_M [struct] - structure with the following fields:
|
||||
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
end
|
||||
|
||||
Fsm = ones(6,1).*args.Fsm;
|
||||
Fsh = ones(6,1).*args.Fsh;
|
||||
Fsr = ones(6,1).*args.Fsr;
|
||||
|
||||
Msm = ones(6,1).*args.Msm;
|
||||
Msh = ones(6,1).*args.Msh;
|
||||
Msr = ones(6,1).*args.Msr;
|
||||
|
||||
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
||||
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
||||
|
||||
for i = 1:6
|
||||
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
||||
1/2 * Fsm(i) * Fsr(i)^2]);
|
||||
|
||||
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
||||
1/2 * Msm(i) * Msr(i)^2]);
|
||||
end
|
||||
|
||||
stewart.struts_M.type = 1;
|
||||
|
||||
stewart.struts_M.I = I_M;
|
||||
stewart.struts_M.M = Msm;
|
||||
stewart.struts_M.R = Msr;
|
||||
stewart.struts_M.H = Msh;
|
||||
|
||||
stewart.struts_F.type = 1;
|
||||
|
||||
stewart.struts_F.I = I_F;
|
||||
stewart.struts_F.M = Fsm;
|
||||
stewart.struts_F.R = Fsr;
|
||||
stewart.struts_F.H = Fsh;
|
||||
@@ -0,0 +1,211 @@
|
||||
function [] = initializeDisturbances(args)
|
||||
% initializeDisturbances - Initialize the disturbances
|
||||
%
|
||||
% Syntax: [] = initializeDisturbances(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args -
|
||||
|
||||
arguments
|
||||
% Global parameter to enable or disable the disturbances
|
||||
args.enable logical {mustBeNumericOrLogical} = true
|
||||
% Ground Motion - X direction
|
||||
args.Dw_x logical {mustBeNumericOrLogical} = true
|
||||
% Ground Motion - Y direction
|
||||
args.Dw_y logical {mustBeNumericOrLogical} = true
|
||||
% Ground Motion - Z direction
|
||||
args.Dw_z logical {mustBeNumericOrLogical} = true
|
||||
% Translation Stage - X direction
|
||||
args.Fdy_x logical {mustBeNumericOrLogical} = true
|
||||
% Translation Stage - Z direction
|
||||
args.Fdy_z logical {mustBeNumericOrLogical} = true
|
||||
% Spindle - X direction
|
||||
args.Frz_x logical {mustBeNumericOrLogical} = true
|
||||
% Spindle - Y direction
|
||||
args.Frz_y logical {mustBeNumericOrLogical} = true
|
||||
% Spindle - Z direction
|
||||
args.Frz_z logical {mustBeNumericOrLogical} = true
|
||||
end
|
||||
|
||||
% Initialization of random numbers
|
||||
rng("shuffle");
|
||||
|
||||
%% Ground Motion
|
||||
if args.enable
|
||||
% Load the PSD of disturbance
|
||||
load('ustation_disturbance_psd.mat', 'gm_dist')
|
||||
|
||||
% Frequency Data
|
||||
Dw.f = gm_dist.f;
|
||||
Dw.psd_x = gm_dist.pxx_x;
|
||||
Dw.psd_y = gm_dist.pxx_y;
|
||||
Dw.psd_z = gm_dist.pxx_z;
|
||||
|
||||
% Time data
|
||||
Fs = 2*Dw.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||||
N = 2*length(Dw.f); % Number of Samples match the one of the wanted PSD
|
||||
T0 = N/Fs; % Signal Duration [s]
|
||||
Dw.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
||||
|
||||
% ASD representation of the ground motion
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(Dw.psd_x(i)/T0);
|
||||
end
|
||||
|
||||
if args.Dw_x
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
||||
else
|
||||
Dw.x = zeros(length(Dw.t), 1);
|
||||
end
|
||||
|
||||
if args.Dw_y
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
||||
else
|
||||
Dw.y = zeros(length(Dw.t), 1);
|
||||
end
|
||||
|
||||
if args.Dw_y
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
||||
else
|
||||
Dw.z = zeros(length(Dw.t), 1);
|
||||
end
|
||||
|
||||
else
|
||||
Dw.t = [0,1]; % Time Vector [s]
|
||||
Dw.x = [0,0]; % Ground Motion - X [m]
|
||||
Dw.y = [0,0]; % Ground Motion - Y [m]
|
||||
Dw.z = [0,0]; % Ground Motion - Z [m]
|
||||
end
|
||||
|
||||
%% Translation stage
|
||||
if args.enable
|
||||
% Load the PSD of disturbance
|
||||
load('ustation_disturbance_psd.mat', 'dy_dist')
|
||||
|
||||
% Frequency Data
|
||||
Dy.f = dy_dist.f;
|
||||
Dy.psd_x = dy_dist.pxx_fx;
|
||||
Dy.psd_z = dy_dist.pxx_fz;
|
||||
|
||||
% Time data
|
||||
Fs = 2*Dy.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||||
N = 2*length(Dy.f); % Number of Samples match the one of the wanted PSD
|
||||
T0 = N/Fs; % Signal Duration [s]
|
||||
Dy.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
||||
|
||||
% ASD representation of the disturbance voice
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(Dy.psd_x(i)/T0);
|
||||
end
|
||||
|
||||
if args.Fdy_x
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N]
|
||||
else
|
||||
Dy.x = zeros(length(Dy.t), 1);
|
||||
end
|
||||
|
||||
if args.Fdy_z
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N]
|
||||
else
|
||||
Dy.z = zeros(length(Dy.t), 1);
|
||||
end
|
||||
|
||||
else
|
||||
Dy.t = [0,1]; % Time Vector [s]
|
||||
Dy.x = [0,0]; % Translation Stage disturbances - X [N]
|
||||
Dy.z = [0,0]; % Translation Stage disturbances - Z [N]
|
||||
end
|
||||
|
||||
%% Spindle
|
||||
if args.enable
|
||||
% Load the PSD of disturbance
|
||||
load('ustation_disturbance_psd.mat', 'rz_dist')
|
||||
|
||||
% Frequency Data
|
||||
Rz.f = rz_dist.f;
|
||||
Rz.psd_x = rz_dist.pxx_fx;
|
||||
Rz.psd_y = rz_dist.pxx_fy;
|
||||
Rz.psd_z = rz_dist.pxx_fz;
|
||||
|
||||
% Time data
|
||||
Fs = 2*Rz.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
||||
N = 2*length(Rz.f); % Number of Samples match the one of the wanted PSD
|
||||
T0 = N/Fs; % Signal Duration [s]
|
||||
Rz.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
||||
|
||||
% ASD representation of the disturbance voice
|
||||
C = zeros(N/2,1);
|
||||
for i = 1:N/2
|
||||
C(i) = sqrt(Rz.psd_x(i)/T0);
|
||||
end
|
||||
|
||||
if args.Frz_x
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N]
|
||||
else
|
||||
Rz.x = zeros(length(Rz.t), 1);
|
||||
end
|
||||
|
||||
if args.Frz_y
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N]
|
||||
else
|
||||
Rz.y = zeros(length(Rz.t), 1);
|
||||
end
|
||||
|
||||
if args.Frz_z
|
||||
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
||||
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
||||
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
||||
Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N]
|
||||
else
|
||||
Rz.z = zeros(length(Rz.t), 1);
|
||||
end
|
||||
|
||||
else
|
||||
Rz.t = [0,1]; % Time Vector [s]
|
||||
Rz.x = [0,0]; % Spindle disturbances - X [N]
|
||||
Rz.y = [0,0]; % Spindle disturbances - X [N]
|
||||
Rz.z = [0,0]; % Spindle disturbances - Z [N]
|
||||
end
|
||||
|
||||
u = zeros(100, 6);
|
||||
Fd = u;
|
||||
|
||||
Dw.x = Dw.x - Dw.x(1);
|
||||
Dw.y = Dw.y - Dw.y(1);
|
||||
Dw.z = Dw.z - Dw.z(1);
|
||||
|
||||
Dy.x = Dy.x - Dy.x(1);
|
||||
Dy.z = Dy.z - Dy.z(1);
|
||||
|
||||
Rz.x = Rz.x - Rz.x(1);
|
||||
Rz.y = Rz.y - Rz.y(1);
|
||||
Rz.z = Rz.z - Rz.z(1);
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
|
||||
elseif exist('./matlab', 'dir')
|
||||
save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
|
||||
end
|
||||
@@ -0,0 +1,35 @@
|
||||
function [stewart] = initializeFramesPositions(stewart, args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||
end
|
||||
|
||||
H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
||||
|
||||
stewart.geometry.H = H;
|
||||
stewart.geometry.FO_M = FO_M;
|
||||
stewart.platform_M.MO_B = MO_B;
|
||||
stewart.platform_F.FO_A = FO_A;
|
||||
@@ -0,0 +1,48 @@
|
||||
function [granite] = initializeGranite(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
|
||||
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
|
||||
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]
|
||||
args.sample_pos (1,1) double {mustBeNumeric} = 0.775 % Height of the measurment point [m]
|
||||
end
|
||||
|
||||
granite = struct();
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
granite.type = 0;
|
||||
case 'rigid'
|
||||
granite.type = 1;
|
||||
case 'flexible'
|
||||
granite.type = 2;
|
||||
end
|
||||
|
||||
granite.density = args.density; % [kg/m3]
|
||||
granite.STEP = 'granite.STEP';
|
||||
|
||||
% Z-offset for the initial position of the sample with respect to the granite top surface.
|
||||
granite.sample_pos = args.sample_pos; % [m]
|
||||
|
||||
granite.K = args.K; % [N/m]
|
||||
granite.C = args.C; % [N/(m/s)]
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'granite', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'granite');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'granite', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'granite');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,35 @@
|
||||
function [ground] = initializeGround(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
|
||||
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
|
||||
end
|
||||
|
||||
ground = struct();
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
ground.type = 0;
|
||||
case 'rigid'
|
||||
ground.type = 1;
|
||||
end
|
||||
|
||||
ground.shape = [2, 2, 0.5]; % [m]
|
||||
ground.density = 2800; % [kg/m3]
|
||||
|
||||
ground.rot_point = args.rot_point;
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'ground', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'ground');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'ground', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'ground');
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,48 @@
|
||||
function [stewart] = initializeInertialSensor(stewart, args)
|
||||
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeInertialSensor(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type - 'geophone', 'accelerometer', 'none'
|
||||
% - mass [1x1] - Weight of the inertial mass [kg]
|
||||
% - freq [1x1] - Cutoff frequency [Hz]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - stewart.sensors.inertial
|
||||
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
||||
% - K [1x1] - Stiffness [N/m]
|
||||
% - C [1x1] - Damping [N/(m/s)]
|
||||
% - M [1x1] - Inertial Mass [kg]
|
||||
% - G [1x1] - Gain
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
||||
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
||||
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
||||
end
|
||||
|
||||
sensor = struct();
|
||||
|
||||
switch args.type
|
||||
case 'geophone'
|
||||
sensor.type = 1;
|
||||
|
||||
sensor.M = args.mass;
|
||||
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||||
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||||
case 'accelerometer'
|
||||
sensor.type = 2;
|
||||
|
||||
sensor.M = args.mass;
|
||||
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
||||
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
||||
sensor.G = -sensor.K/sensor.M;
|
||||
case 'none'
|
||||
sensor.type = 3;
|
||||
end
|
||||
|
||||
stewart.sensors.inertial = sensor;
|
||||
@@ -0,0 +1,131 @@
|
||||
function [stewart] = initializeJointDynamics(stewart, args)
|
||||
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
|
||||
%
|
||||
% Syntax: [stewart] = initializeJointDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
||||
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
||||
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
|
||||
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
|
||||
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
|
||||
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
|
||||
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
|
||||
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
|
||||
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
|
||||
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - stewart.joints_F and stewart.joints_M:
|
||||
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
|
||||
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
|
||||
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
|
||||
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
|
||||
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
|
||||
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
|
||||
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
|
||||
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
|
||||
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
||||
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
||||
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
||||
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
||||
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
||||
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
||||
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
||||
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
||||
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
||||
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
||||
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.K_M double {mustBeNumeric} = zeros(6,6)
|
||||
args.M_M double {mustBeNumeric} = zeros(6,6)
|
||||
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
||||
args.xi_M double {mustBeNumeric} = 0.1
|
||||
args.step_file_M char {} = ''
|
||||
args.K_F double {mustBeNumeric} = zeros(6,6)
|
||||
args.M_F double {mustBeNumeric} = zeros(6,6)
|
||||
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
||||
args.xi_F double {mustBeNumeric} = 0.1
|
||||
args.step_file_F char {} = ''
|
||||
end
|
||||
|
||||
switch args.type_F
|
||||
case 'universal'
|
||||
stewart.joints_F.type = 1;
|
||||
case 'spherical'
|
||||
stewart.joints_F.type = 2;
|
||||
case 'universal_p'
|
||||
stewart.joints_F.type = 3;
|
||||
case 'spherical_p'
|
||||
stewart.joints_F.type = 4;
|
||||
case 'flexible'
|
||||
stewart.joints_F.type = 5;
|
||||
case 'universal_3dof'
|
||||
stewart.joints_F.type = 6;
|
||||
case 'spherical_3dof'
|
||||
stewart.joints_F.type = 7;
|
||||
end
|
||||
|
||||
switch args.type_M
|
||||
case 'universal'
|
||||
stewart.joints_M.type = 1;
|
||||
case 'spherical'
|
||||
stewart.joints_M.type = 2;
|
||||
case 'universal_p'
|
||||
stewart.joints_M.type = 3;
|
||||
case 'spherical_p'
|
||||
stewart.joints_M.type = 4;
|
||||
case 'flexible'
|
||||
stewart.joints_M.type = 5;
|
||||
case 'universal_3dof'
|
||||
stewart.joints_M.type = 6;
|
||||
case 'spherical_3dof'
|
||||
stewart.joints_M.type = 7;
|
||||
end
|
||||
|
||||
stewart.joints_M.Ka = args.Ka_M;
|
||||
stewart.joints_M.Kr = args.Kr_M;
|
||||
|
||||
stewart.joints_F.Ka = args.Ka_F;
|
||||
stewart.joints_F.Kr = args.Kr_F;
|
||||
|
||||
stewart.joints_M.Ca = args.Ca_M;
|
||||
stewart.joints_M.Cr = args.Cr_M;
|
||||
|
||||
stewart.joints_F.Ca = args.Ca_F;
|
||||
stewart.joints_F.Cr = args.Cr_F;
|
||||
|
||||
stewart.joints_M.Kf = args.Kf_M;
|
||||
stewart.joints_M.Kt = args.Kt_M;
|
||||
|
||||
stewart.joints_F.Kf = args.Kf_F;
|
||||
stewart.joints_F.Kt = args.Kt_F;
|
||||
|
||||
stewart.joints_M.Cf = args.Cf_M;
|
||||
stewart.joints_M.Ct = args.Ct_M;
|
||||
|
||||
stewart.joints_F.Cf = args.Cf_F;
|
||||
stewart.joints_F.Ct = args.Ct_F;
|
||||
|
||||
stewart.joints_F.M = args.M_F;
|
||||
stewart.joints_F.K = args.K_F;
|
||||
stewart.joints_F.n_xyz = args.n_xyz_F;
|
||||
stewart.joints_F.xi = args.xi_F;
|
||||
stewart.joints_F.xi = args.xi_F;
|
||||
stewart.joints_F.step_file = args.step_file_F;
|
||||
|
||||
stewart.joints_M.M = args.M_M;
|
||||
stewart.joints_M.K = args.K_M;
|
||||
stewart.joints_M.n_xyz = args.n_xyz_M;
|
||||
stewart.joints_M.xi = args.xi_M;
|
||||
stewart.joints_M.step_file = args.step_file_M;
|
||||
@@ -0,0 +1,33 @@
|
||||
function [] = initializeLoggingConfiguration(args)
|
||||
|
||||
arguments
|
||||
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
|
||||
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||||
end
|
||||
|
||||
conf_log = struct();
|
||||
|
||||
switch args.log
|
||||
case 'none'
|
||||
conf_log.type = 0;
|
||||
case 'all'
|
||||
conf_log.type = 1;
|
||||
case 'forces'
|
||||
conf_log.type = 2;
|
||||
end
|
||||
|
||||
conf_log.Ts = args.Ts;
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_conf_log.mat', 'file')
|
||||
save('mat/nass_model_conf_log.mat', 'conf_log', '-append');
|
||||
else
|
||||
save('mat/nass_model_conf_log.mat', 'conf_log');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_conf_log.mat', 'file')
|
||||
save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_conf_log.mat', 'conf_log');
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,108 @@
|
||||
function [micro_hexapod] = initializeMicroHexapod(args)
|
||||
|
||||
arguments
|
||||
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
|
||||
% generateGeneralConfiguration
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
|
||||
% initializeStrutDynamics
|
||||
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
|
||||
% initializeCylindricalPlatforms
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
||||
% initializeCylindricalStruts
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
||||
% inverseKinematics
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
stewart = initializeStewartPlatform();
|
||||
|
||||
stewart = initializeFramesPositions(stewart, ...
|
||||
'H', args.H, ...
|
||||
'MO_B', args.MO_B);
|
||||
|
||||
stewart = generateGeneralConfiguration(stewart, ...
|
||||
'FH', args.FH, ...
|
||||
'FR', args.FR, ...
|
||||
'FTh', args.FTh, ...
|
||||
'MH', args.MH, ...
|
||||
'MR', args.MR, ...
|
||||
'MTh', args.MTh);
|
||||
|
||||
stewart = computeJointsPose(stewart);
|
||||
|
||||
stewart = initializeStrutDynamics(stewart, ...
|
||||
'K', args.Ki, ...
|
||||
'C', args.Ci);
|
||||
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', 'universal_p', ...
|
||||
'type_M', 'spherical_p');
|
||||
|
||||
stewart = initializeCylindricalPlatforms(stewart, ...
|
||||
'Fpm', args.Fpm, ...
|
||||
'Fph', args.Fph, ...
|
||||
'Fpr', args.Fpr, ...
|
||||
'Mpm', args.Mpm, ...
|
||||
'Mph', args.Mph, ...
|
||||
'Mpr', args.Mpr);
|
||||
|
||||
stewart = initializeCylindricalStruts(stewart, ...
|
||||
'Fsm', args.Fsm, ...
|
||||
'Fsh', args.Fsh, ...
|
||||
'Fsr', args.Fsr, ...
|
||||
'Msm', args.Msm, ...
|
||||
'Msh', args.Msh, ...
|
||||
'Msr', args.Msr);
|
||||
|
||||
stewart = computeJacobian(stewart);
|
||||
|
||||
stewart = initializeStewartPose(stewart, ...
|
||||
'AP', args.AP, ...
|
||||
'ARB', args.ARB);
|
||||
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
stewart.type = 0;
|
||||
case 'rigid'
|
||||
stewart.type = 1;
|
||||
case 'flexible'
|
||||
stewart.type = 2;
|
||||
end
|
||||
|
||||
micro_hexapod = stewart;
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'micro_hexapod');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,199 @@
|
||||
function [ref] = initializeReferences(args)
|
||||
|
||||
arguments
|
||||
% Sampling Frequency [s]
|
||||
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
||||
% Maximum simulation time [s]
|
||||
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
||||
% Either "constant" / "triangular" / "sinusoidal"
|
||||
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
||||
% Amplitude of the displacement [m]
|
||||
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
||||
% Period of the displacement [s]
|
||||
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
% Either "constant" / "triangular" / "sinusoidal"
|
||||
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
||||
% Amplitude [rad]
|
||||
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
||||
% Period of the displacement [s]
|
||||
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
% Either "constant" / "rotating"
|
||||
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
|
||||
% Initial angle [rad]
|
||||
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
||||
% Period of the rotating [s]
|
||||
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
% For now, only constant is implemented
|
||||
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
|
||||
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
||||
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
||||
% For now, only constant is implemented
|
||||
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
|
||||
% Initial position of the two masses
|
||||
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
|
||||
% For now, only constant is implemented
|
||||
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
|
||||
% Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
|
||||
%% Set Sampling Time
|
||||
Ts = args.Ts;
|
||||
Tmax = args.Tmax;
|
||||
|
||||
%% Low Pass Filter to filter out the references
|
||||
s = zpk('s');
|
||||
w0 = 2*pi*10;
|
||||
xi = 1;
|
||||
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
||||
|
||||
%% Translation stage - Dy
|
||||
t = 0:Ts:Tmax; % Time Vector [s]
|
||||
Dy = zeros(length(t), 1);
|
||||
Dyd = zeros(length(t), 1);
|
||||
Dydd = zeros(length(t), 1);
|
||||
switch args.Dy_type
|
||||
case 'constant'
|
||||
Dy(:) = args.Dy_amplitude;
|
||||
Dyd(:) = 0;
|
||||
Dydd(:) = 0;
|
||||
case 'triangular'
|
||||
% This is done to unsure that we start with no displacement
|
||||
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
|
||||
i0 = find(t>=args.Dy_period/4,1);
|
||||
Dy(1:end-i0+1) = Dy_raw(i0:end);
|
||||
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
|
||||
|
||||
% The signal is filtered out
|
||||
Dy = lsim(H_lpf, Dy, t);
|
||||
Dyd = lsim(H_lpf*s, Dy, t);
|
||||
Dydd = lsim(H_lpf*s^2, Dy, t);
|
||||
case 'sinusoidal'
|
||||
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
|
||||
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
|
||||
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
|
||||
otherwise
|
||||
warning('Dy_type is not set correctly');
|
||||
end
|
||||
|
||||
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
|
||||
|
||||
%% Tilt Stage - Ry
|
||||
t = 0:Ts:Tmax; % Time Vector [s]
|
||||
Ry = zeros(length(t), 1);
|
||||
Ryd = zeros(length(t), 1);
|
||||
Rydd = zeros(length(t), 1);
|
||||
|
||||
switch args.Ry_type
|
||||
case 'constant'
|
||||
Ry(:) = args.Ry_amplitude;
|
||||
Ryd(:) = 0;
|
||||
Rydd(:) = 0;
|
||||
case 'triangular'
|
||||
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
|
||||
i0 = find(t>=args.Ry_period/4,1);
|
||||
Ry(1:end-i0+1) = Ry_raw(i0:end);
|
||||
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
|
||||
|
||||
% The signal is filtered out
|
||||
Ry = lsim(H_lpf, Ry, t);
|
||||
Ryd = lsim(H_lpf*s, Ry, t);
|
||||
Rydd = lsim(H_lpf*s^2, Ry, t);
|
||||
case 'sinusoidal'
|
||||
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
|
||||
|
||||
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
|
||||
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
|
||||
otherwise
|
||||
warning('Ry_type is not set correctly');
|
||||
end
|
||||
|
||||
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
|
||||
|
||||
%% Spindle - Rz
|
||||
t = 0:Ts:Tmax; % Time Vector [s]
|
||||
Rz = zeros(length(t), 1);
|
||||
Rzd = zeros(length(t), 1);
|
||||
Rzdd = zeros(length(t), 1);
|
||||
|
||||
switch args.Rz_type
|
||||
case 'constant'
|
||||
Rz(:) = args.Rz_amplitude;
|
||||
Rzd(:) = 0;
|
||||
Rzdd(:) = 0;
|
||||
case 'rotating-not-filtered'
|
||||
Rz(:) = 2*pi/args.Rz_period*t;
|
||||
|
||||
% The signal is filtered out
|
||||
Rz(:) = 2*pi/args.Rz_period*t;
|
||||
Rzd(:) = 2*pi/args.Rz_period;
|
||||
Rzdd(:) = 0;
|
||||
|
||||
% We add the angle offset
|
||||
Rz = Rz + args.Rz_amplitude;
|
||||
|
||||
case 'rotating'
|
||||
Rz(:) = 2*pi/args.Rz_period*t;
|
||||
|
||||
% The signal is filtered out
|
||||
Rz = lsim(H_lpf, Rz, t);
|
||||
Rzd = lsim(H_lpf*s, Rz, t);
|
||||
Rzdd = lsim(H_lpf*s^2, Rz, t);
|
||||
|
||||
% We add the angle offset
|
||||
Rz = Rz + args.Rz_amplitude;
|
||||
otherwise
|
||||
warning('Rz_type is not set correctly');
|
||||
end
|
||||
|
||||
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
|
||||
|
||||
%% Micro-Hexapod
|
||||
t = [0, Ts];
|
||||
Dh = zeros(length(t), 6);
|
||||
Dhl = zeros(length(t), 6);
|
||||
|
||||
switch args.Dh_type
|
||||
case 'constant'
|
||||
Dh = [args.Dh_pos, args.Dh_pos];
|
||||
|
||||
load('nass_model_stages.mat', 'micro_hexapod');
|
||||
|
||||
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
|
||||
|
||||
tx = args.Dh_pos(4);
|
||||
ty = args.Dh_pos(5);
|
||||
tz = args.Dh_pos(6);
|
||||
|
||||
ARB = [cos(tz) -sin(tz) 0;
|
||||
sin(tz) cos(tz) 0;
|
||||
0 0 1]*...
|
||||
[ cos(ty) 0 sin(ty);
|
||||
0 1 0;
|
||||
-sin(ty) 0 cos(ty)]*...
|
||||
[1 0 0;
|
||||
0 cos(tx) -sin(tx);
|
||||
0 sin(tx) cos(tx)];
|
||||
|
||||
[~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
|
||||
Dhl = [Dhl, Dhl];
|
||||
otherwise
|
||||
warning('Dh_type is not set correctly');
|
||||
end
|
||||
|
||||
Dh = struct('time', t, 'signals', struct('values', Dh));
|
||||
Dhl = struct('time', t, 'signals', struct('values', Dhl));
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_references.mat', 'file')
|
||||
save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
||||
else
|
||||
save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_references.mat', 'file')
|
||||
save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,57 @@
|
||||
function [ry] = initializeRy(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
||||
args.Ry_init (1,1) double {mustBeNumeric} = 0
|
||||
end
|
||||
|
||||
ry = struct();
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
ry.type = 0;
|
||||
case 'rigid'
|
||||
ry.type = 1;
|
||||
case 'flexible'
|
||||
ry.type = 2;
|
||||
end
|
||||
|
||||
% Ry - Guide for the tilt stage
|
||||
ry.guide.density = 7800; % [kg/m3]
|
||||
ry.guide.STEP = 'Tilt_Guide.STEP';
|
||||
|
||||
% Ry - Rotor of the motor
|
||||
ry.rotor.density = 2400; % [kg/m3]
|
||||
ry.rotor.STEP = 'Tilt_Motor_Axis.STEP';
|
||||
|
||||
% Ry - Motor
|
||||
ry.motor.density = 3200; % [kg/m3]
|
||||
ry.motor.STEP = 'Tilt_Motor.STEP';
|
||||
|
||||
% Ry - Plateau Tilt
|
||||
ry.stage.density = 7800; % [kg/m3]
|
||||
ry.stage.STEP = 'Tilt_Stage.STEP';
|
||||
|
||||
% Z-Offset so that the center of rotation matches the sample center;
|
||||
ry.z_offset = 0.58178; % [m]
|
||||
|
||||
ry.Ry_init = args.Ry_init; % [rad]
|
||||
|
||||
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
|
||||
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'ry', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'ry');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'ry', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'ry');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,47 @@
|
||||
function [rz] = initializeRz(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
||||
end
|
||||
|
||||
rz = struct();
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
rz.type = 0;
|
||||
case 'rigid'
|
||||
rz.type = 1;
|
||||
case 'flexible'
|
||||
rz.type = 2;
|
||||
end
|
||||
|
||||
% Spindle - Slip Ring
|
||||
rz.slipring.density = 7800; % [kg/m3]
|
||||
rz.slipring.STEP = 'Spindle_Slip_Ring.STEP';
|
||||
|
||||
% Spindle - Rotor
|
||||
rz.rotor.density = 7800; % [kg/m3]
|
||||
rz.rotor.STEP = 'Spindle_Rotor.STEP';
|
||||
|
||||
% Spindle - Stator
|
||||
rz.stator.density = 7800; % [kg/m3]
|
||||
rz.stator.STEP = 'Spindle_Stator.STEP';
|
||||
|
||||
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
|
||||
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'rz', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'rz');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'rz', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'rz');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,27 @@
|
||||
function [] = initializeSimscapeConfiguration(args)
|
||||
|
||||
arguments
|
||||
args.gravity logical {mustBeNumericOrLogical} = true
|
||||
end
|
||||
|
||||
conf_simscape = struct();
|
||||
|
||||
if args.gravity
|
||||
conf_simscape.type = 1;
|
||||
else
|
||||
conf_simscape.type = 2;
|
||||
end
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_conf_simscape.mat', 'file')
|
||||
save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
|
||||
else
|
||||
save('mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file')
|
||||
save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,31 @@
|
||||
function [stewart] = initializeStewartPlatform()
|
||||
% initializeStewartPlatform - Initialize the stewart structure
|
||||
%
|
||||
% Syntax: [stewart] = initializeStewartPlatform(args)
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following sub-structures:
|
||||
% - platform_F -
|
||||
% - platform_M -
|
||||
% - joints_F -
|
||||
% - joints_M -
|
||||
% - struts_F -
|
||||
% - struts_M -
|
||||
% - actuators -
|
||||
% - geometry -
|
||||
% - properties -
|
||||
|
||||
stewart = struct();
|
||||
stewart.platform_F = struct();
|
||||
stewart.platform_M = struct();
|
||||
stewart.joints_F = struct();
|
||||
stewart.joints_M = struct();
|
||||
stewart.struts_F = struct();
|
||||
stewart.struts_M = struct();
|
||||
stewart.actuators = struct();
|
||||
stewart.sensors = struct();
|
||||
stewart.sensors.inertial = struct();
|
||||
stewart.sensors.force = struct();
|
||||
stewart.sensors.relative = struct();
|
||||
stewart.geometry = struct();
|
||||
stewart.kinematics = struct();
|
||||
@@ -0,0 +1,27 @@
|
||||
function [stewart] = initializeStewartPose(stewart, args)
|
||||
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
|
||||
% It uses the inverse kinematic
|
||||
%
|
||||
% Syntax: [stewart] = initializeStewartPose(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Aa [3x6] - The positions ai expressed in {A}
|
||||
% - Bb [3x6] - The positions bi expressed in {B}
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
||||
|
||||
stewart.actuators.Leq = dLi;
|
||||
@@ -0,0 +1,49 @@
|
||||
function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - K [6x1] - Stiffness of each strut [N/m]
|
||||
% - C [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - actuators.type = 1
|
||||
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
||||
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
|
||||
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
||||
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
||||
args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
|
||||
args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
|
||||
args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
|
||||
args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
|
||||
args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
|
||||
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
||||
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
||||
end
|
||||
|
||||
if strcmp(args.type, 'classical')
|
||||
stewart.actuators.type = 1;
|
||||
elseif strcmp(args.type, 'amplified')
|
||||
stewart.actuators.type = 2;
|
||||
end
|
||||
|
||||
stewart.actuators.K = args.K;
|
||||
stewart.actuators.C = args.C;
|
||||
|
||||
stewart.actuators.k1 = args.k1;
|
||||
stewart.actuators.c1 = args.c1;
|
||||
|
||||
stewart.actuators.ka = args.ka;
|
||||
stewart.actuators.ke = args.ke;
|
||||
|
||||
stewart.actuators.F_gain = args.F_gain;
|
||||
|
||||
stewart.actuators.ma = args.ma;
|
||||
stewart.actuators.me = args.me;
|
||||
@@ -0,0 +1,71 @@
|
||||
function [ty] = initializeTy(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
||||
end
|
||||
|
||||
ty = struct();
|
||||
|
||||
switch args.type
|
||||
case 'none'
|
||||
ty.type = 0;
|
||||
case 'rigid'
|
||||
ty.type = 1;
|
||||
case 'flexible'
|
||||
ty.type = 2;
|
||||
end
|
||||
|
||||
% Ty Granite frame
|
||||
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
|
||||
ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP';
|
||||
|
||||
% Guide Translation Ty
|
||||
ty.guide.density = 7800; % [kg/m3] => 76kg
|
||||
ty.guide.STEP = 'Ty_Guide.STEP';
|
||||
|
||||
% Ty - Guide_Translation12
|
||||
ty.guide12.density = 7800; % [kg/m3]
|
||||
ty.guide12.STEP = 'Ty_Guide_12.STEP';
|
||||
|
||||
% Ty - Guide_Translation11
|
||||
ty.guide11.density = 7800; % [kg/m3]
|
||||
ty.guide11.STEP = 'Ty_Guide_11.STEP';
|
||||
|
||||
% Ty - Guide_Translation22
|
||||
ty.guide22.density = 7800; % [kg/m3]
|
||||
ty.guide22.STEP = 'Ty_Guide_22.STEP';
|
||||
|
||||
% Ty - Guide_Translation21
|
||||
ty.guide21.density = 7800; % [kg/m3]
|
||||
ty.guide21.STEP = 'Ty_Guide_21.STEP';
|
||||
|
||||
% Ty - Plateau translation
|
||||
ty.frame.density = 7800; % [kg/m3]
|
||||
ty.frame.STEP = 'Ty_Stage.STEP';
|
||||
|
||||
% Ty Stator Part
|
||||
ty.stator.density = 5400; % [kg/m3]
|
||||
ty.stator.STEP = 'Ty_Motor_Stator.STEP';
|
||||
|
||||
% Ty Rotor Part
|
||||
ty.rotor.density = 5400; % [kg/m3]
|
||||
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; 1e4]; % [N/(m/s), N*m/(rad/s)]
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'ty', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'ty');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'ty', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'ty');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,31 @@
|
||||
function [accelerometer] = initializeZAxisAccelerometer(args)
|
||||
arguments
|
||||
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
|
||||
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz]
|
||||
end
|
||||
|
||||
%%
|
||||
accelerometer.m = args.mass;
|
||||
|
||||
%% The Stiffness is set to have the damping resonance frequency
|
||||
accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
|
||||
|
||||
%% We set the damping value to have critical damping
|
||||
accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
|
||||
|
||||
%% Gain correction of the accelerometer to have a unity gain until the resonance
|
||||
accelerometer.gain = -accelerometer.k/accelerometer.m;
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/accelerometer_z_axis.mat', 'file')
|
||||
save('mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
|
||||
else
|
||||
save('mat/accelerometer_z_axis.mat', 'accelerometer');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/accelerometer_z_axis.mat', 'file')
|
||||
save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer', '-append');
|
||||
else
|
||||
save('matlab/mat/accelerometer_z_axis.mat', 'accelerometer');
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,28 @@
|
||||
function [geophone] = initializeZAxisGeophone(args)
|
||||
arguments
|
||||
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
|
||||
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz]
|
||||
end
|
||||
|
||||
%%
|
||||
geophone.m = args.mass;
|
||||
|
||||
%% The Stiffness is set to have the damping resonance frequency
|
||||
geophone.k = geophone.m * (2*pi*args.freq)^2;
|
||||
|
||||
%% We set the damping value to have critical damping
|
||||
geophone.c = 2*sqrt(geophone.m * geophone.k);
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/geophone_z_axis.mat', 'file')
|
||||
save('mat/geophone_z_axis.mat', 'geophone', '-append');
|
||||
else
|
||||
save('mat/geophone_z_axis.mat', 'geophone');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/geophone_z_axis.mat', 'file')
|
||||
save('matlab/mat/geophone_z_axis.mat', 'geophone', '-append');
|
||||
else
|
||||
save('matlab/mat/geophone_z_axis.mat', 'geophone');
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,36 @@
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
|
||||
dLi = Li-l;
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,90 @@
|
||||
%% ustation_1_kinematics.m
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% 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
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'ustation_simscape';
|
||||
|
||||
load('nass_model_conf_simulink.mat');
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(log10(10), log10(2e3), 1000);
|
||||
|
||||
%% Stage setpoints
|
||||
Dy = 1e-3; % Translation Stage [m]
|
||||
Ry = 3*pi/180; % Tilt Stage [rad]
|
||||
Rz = 180*pi/180; % Spindle [rad]
|
||||
|
||||
%% Stage individual Homogeneous transformations
|
||||
% 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-Station homogeneous transformation
|
||||
Ttot = Rty*Rry*Rrz;
|
||||
|
||||
%% Compute translations and rotations (Euler angles) induced by the micro-station
|
||||
ustation_dx = Ttot(1,4);
|
||||
ustation_dy = Ttot(2,4);
|
||||
ustation_dz = Ttot(3,4);
|
||||
ustation_ry = atan2( Ttot(1, 3), sqrt(Ttot(1, 1)^2 + Ttot(1, 2)^2));
|
||||
ustation_rx = atan2(-Ttot(2, 3)/cos(ustation_ry), Ttot(3, 3)/cos(ustation_ry));
|
||||
ustation_rz = atan2(-Ttot(1, 2)/cos(ustation_ry), Ttot(1, 1)/cos(ustation_ry));
|
||||
|
||||
%% Verification using the Simscape model
|
||||
% All stages are initialized as rigid bodies to avoid any guiding error
|
||||
initializeGround( 'type', 'rigid');
|
||||
initializeGranite( 'type', 'rigid');
|
||||
initializeTy( 'type', 'rigid');
|
||||
initializeRy( 'type', 'rigid');
|
||||
initializeRz( 'type', 'rigid');
|
||||
initializeMicroHexapod('type', 'rigid');
|
||||
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
|
||||
initializeReferences('Dy_amplitude', Dy, ...
|
||||
'Ry_amplitude', Ry, ...
|
||||
'Rz_amplitude', Rz);
|
||||
|
||||
initializeDisturbances('enable', false);
|
||||
|
||||
set_param(conf_simulink, 'StopTime', '0.5');
|
||||
|
||||
% Simulation is performed
|
||||
sim(mdl);
|
||||
|
||||
% Sample's motion is computed from "external metrology"
|
||||
T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end); simout.y.z.Data(end)]; [0,0,0,1]];
|
||||
sim_dx = T_sim(1,4);
|
||||
sim_dy = T_sim(2,4);
|
||||
sim_dz = T_sim(3,4);
|
||||
sim_ry = atan2( T_sim(1, 3), sqrt(T_sim(1, 1)^2 + T_sim(1, 2)^2));
|
||||
sim_rx = atan2(-T_sim(2, 3)/cos(sim_ry), T_sim(3, 3)/cos(sim_ry));
|
||||
sim_rz = atan2(-T_sim(1, 2)/cos(sim_ry), T_sim(1, 1)/cos(sim_ry));
|
||||
@@ -0,0 +1,321 @@
|
||||
%% ustation_2_modeling.m
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% 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
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'ustation_simscape';
|
||||
|
||||
load('nass_model_conf_simulink.mat');
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(log10(10), log10(2e3), 1000);
|
||||
|
||||
%% Indentify the model dynamics from the 3 hammer imapcts
|
||||
% To the motion of each solid body at their CoM
|
||||
|
||||
% All stages are initialized
|
||||
initializeGround( 'type', 'rigid');
|
||||
initializeGranite( 'type', 'flexible');
|
||||
initializeTy( 'type', 'flexible');
|
||||
initializeRy( 'type', 'flexible');
|
||||
initializeRz( 'type', 'flexible');
|
||||
initializeMicroHexapod('type', 'flexible');
|
||||
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
|
||||
initializeReferences();
|
||||
initializeDisturbances('enable', false);
|
||||
|
||||
% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Granite/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
% Run the linearization
|
||||
G_ms = linearize(mdl, io, 0);
|
||||
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
|
||||
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
|
||||
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
|
||||
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
|
||||
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
|
||||
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
|
||||
|
||||
% Load estimated FRF at CoM
|
||||
load('ustation_frf_matrix.mat', 'freqs');
|
||||
load('ustation_frf_com.mat', 'frfs_CoM');
|
||||
|
||||
% Initialization of some variables to the figures
|
||||
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
||||
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'};
|
||||
f = logspace(log10(10), log10(500), 1000);
|
||||
|
||||
%% Spindle - X response
|
||||
n_stg = 5; % Measured Stage
|
||||
n_dir = 1; % Measured DoF: x, y, z, Rx, Ry, Rz
|
||||
n_exc = 1; % Hammer impact: x, y, z
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
||||
text(50, 2e-2,{'$D_x/F_x$ - Spindle'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
xlim([10, 200]);
|
||||
ylim([1e-6, 1e-1])
|
||||
|
||||
%% Hexapod - Y response
|
||||
n_stg = 6; % Measured Stage
|
||||
n_dir = 2; % Measured DoF: x, y, z, Rx, Ry, Rz
|
||||
n_exc = 2; % Hammer impact: x, y, z
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
||||
text(50, 2e-2,{'$D_y/F_y$ - Hexapod'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
xlim([10, 200]);
|
||||
ylim([1e-6, 1e-1])
|
||||
|
||||
%% Tilt stage - Z response
|
||||
n_stg = 4; % Measured Stage
|
||||
n_dir = 3; % Measured DoF: x, y, z, Rx, Ry, Rz
|
||||
n_exc = 3; % Hammer impact: x, y, z
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
||||
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
||||
text(50, 2e-2,{'$D_z/F_z$ - Tilt'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
xlim([10, 200]);
|
||||
ylim([1e-6, 1e-1])
|
||||
|
||||
% Positions and orientation of accelerometers
|
||||
% | *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 |
|
||||
%
|
||||
|
||||
% Measuerment channels
|
||||
%
|
||||
% | 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 |
|
||||
|
||||
% 10 measurements corresponding to 10 different Hammer impact locations
|
||||
% | *Num* | *Direction* | *Position* | Accelerometer position | Jacobian number |
|
||||
% |-------+-------------+------------+------------------------+-----------------|
|
||||
% | 1 | -Y | [0, +A, 0] | 1 | -2 |
|
||||
% | 2 | -Z | [0, +A, 0] | 1 | -3 |
|
||||
% | 3 | X | [-B, 0, 0] | 2 | 4 |
|
||||
% | 4 | -Z | [-B, 0, 0] | 2 | -6 |
|
||||
% | 5 | Y | [0, -A, 0] | 3 | 8 |
|
||||
% | 6 | -Z | [0, -A, 0] | 3 | -9 |
|
||||
% | 7 | -X | [+B, 0, 0] | 4 | -10 |
|
||||
% | 8 | -Z | [+B, 0, 0] | 4 | -12 |
|
||||
% | 9 | -X | [0, -A, 0] | 3 | -7 |
|
||||
% | 10 | -X | [0, +A, 0] | 1 | -1 |
|
||||
|
||||
|
||||
%% Jacobian for accelerometers
|
||||
% aX = Ja . aL
|
||||
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];
|
||||
|
||||
Ja_inv = pinv(Ja);
|
||||
|
||||
%% 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;
|
||||
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]';
|
||||
Jf_inv = pinv(Jf);
|
||||
|
||||
%% Load raw measurement data
|
||||
% "Track1" to "Track12" are the 12 accelerometers
|
||||
% "Track13" is the instrumented hammer force sensor
|
||||
data = [
|
||||
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')];
|
||||
|
||||
%% Prepare the FRF computation
|
||||
Ts = 1e-3; % Sampling Time [s]
|
||||
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
||||
win = ones(Nfft, 1); % Rectangular window
|
||||
[~, f] = tfestimate(data(1).Track13, data(1).Track1, win, [], Nfft, 1/Ts);
|
||||
|
||||
% Samples taken before and after the hammer "impact"
|
||||
pre_n = floor(0.1/Ts);
|
||||
post_n = Nfft - pre_n - 1;
|
||||
|
||||
%% 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 % 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
|
||||
|
||||
% Average the FRF for the 10 impacts
|
||||
for impact_i = impacts_i
|
||||
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));
|
||||
% Combine all outputs [m/s^2]
|
||||
data_out = [data(i).Track1( i_start:i_end); ...
|
||||
data(i).Track2( i_start:i_end); ...
|
||||
data(i).Track3( i_start:i_end); ...
|
||||
data(i).Track4( i_start:i_end); ...
|
||||
data(i).Track5( i_start:i_end); ...
|
||||
data(i).Track6( i_start:i_end); ...
|
||||
data(i).Track7( i_start:i_end); ...
|
||||
data(i).Track8( i_start:i_end); ...
|
||||
data(i).Track9( i_start:i_end); ...
|
||||
data(i).Track10(i_start:i_end); ...
|
||||
data(i).Track11(i_start:i_end); ...
|
||||
data(i).Track12(i_start:i_end)];
|
||||
|
||||
[frf, ~] = tfestimate(data_in, data_out', win, [], Nfft, 1/Ts);
|
||||
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));
|
||||
|
||||
%% Identification of the compliance of the micro-station
|
||||
|
||||
% Initialize simulation with default parameters (flexible elements)
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
|
||||
initializeReferences();
|
||||
initializeSimscapeConfiguration();
|
||||
initializeLoggingConfiguration();
|
||||
|
||||
% 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'};
|
||||
|
||||
%% 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(1,1,:))), '-', 'color', [colors(1,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_x/F_x$ - Measured')
|
||||
plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_y/F_y$ - Measured')
|
||||
plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$D_z/F_z$ - Measured')
|
||||
plot(f, abs(squeeze(freqresp(Gm(1,1), f, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$ - Model')
|
||||
plot(f, abs(squeeze(freqresp(Gm(2,2), f, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$ - Model')
|
||||
plot(f, abs(squeeze(freqresp(Gm(3,3), f, '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('Magnitude [m/N]');
|
||||
xlim([20, 500]); ylim([2e-10, 1e-6]);
|
||||
xticks([20, 50, 100, 200, 500])
|
||||
|
||||
%% 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,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_x/M_x$ - Measured')
|
||||
plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', [colors(2,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_y/M_y$ - Measured')
|
||||
plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', [colors(3,:), 0.5], 'linewidth', 2.5, 'DisplayName', '$R_z/M_z$ - Measured')
|
||||
plot(f, abs(squeeze(freqresp(Gm(4,4), f, 'Hz'))), '-', 'color', colors(1,:), 'DisplayName', '$R_x/M_x$ - Model')
|
||||
plot(f, abs(squeeze(freqresp(Gm(5,5), f, 'Hz'))), '-', 'color', colors(2,:), 'DisplayName', '$R_y/M_y$ - Model')
|
||||
plot(f, abs(squeeze(freqresp(Gm(6,6), f, '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 [rad/Nm]');
|
||||
xlim([20, 500]); ylim([2e-7, 1e-4]);
|
||||
xticks([20, 50, 100, 200, 500])
|
||||
@@ -0,0 +1,345 @@
|
||||
%% ustation_3_disturbances.m
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% 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
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'ustation_simscape';
|
||||
|
||||
load('nass_model_conf_simulink.mat');
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(log10(10), log10(2e3), 1000);
|
||||
|
||||
%% Compute Floor Motion Spectral Density
|
||||
% Load floor motion data
|
||||
% velocity in [m/s] is measured in X, Y and Z directions
|
||||
load('ustation_ground_motion.mat', 'Ts', 'Fs', 'vel_x', 'vel_y', 'vel_z', 't');
|
||||
|
||||
% Estimate ground displacement from measured velocity
|
||||
% This is done by integrating the motion
|
||||
gm_x = lsim(1/(s+0.1*2*pi), vel_x, t);
|
||||
gm_y = lsim(1/(s+0.1*2*pi), vel_y, t);
|
||||
gm_z = lsim(1/(s+0.1*2*pi), vel_z, t);
|
||||
|
||||
Nfft = floor(2/Ts);
|
||||
win = hanning(Nfft);
|
||||
Noverlap = floor(Nfft/2);
|
||||
|
||||
[pxx_gm_vx, f_gm] = pwelch(vel_x, win, Noverlap, Nfft, 1/Ts);
|
||||
[pxx_gm_vy, ~] = pwelch(vel_y, win, Noverlap, Nfft, 1/Ts);
|
||||
[pxx_gm_vz, ~] = pwelch(vel_z, win, Noverlap, Nfft, 1/Ts);
|
||||
|
||||
% Convert PSD in velocity to PSD in displacement
|
||||
pxx_gm_x = pxx_gm_vx./((2*pi*f_gm).^2);
|
||||
pxx_gm_y = pxx_gm_vy./((2*pi*f_gm).^2);
|
||||
pxx_gm_z = pxx_gm_vz./((2*pi*f_gm).^2);
|
||||
|
||||
%% Measured ground motion
|
||||
figure;
|
||||
hold on;
|
||||
plot(t, 1e6*gm_x, 'DisplayName', '$D_{xf}$')
|
||||
plot(t, 1e6*gm_y, 'DisplayName', '$D_{yf}$')
|
||||
plot(t, 1e6*gm_z, 'DisplayName', '$D_{zf}$')
|
||||
hold off;
|
||||
xlabel('Time [s]');
|
||||
ylabel('Ground motion [$\mu$m]')
|
||||
xlim([0, 5]); ylim([-2, 2])
|
||||
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Ty errors
|
||||
% Setpoint is in [mm]
|
||||
% Vertical error is in [um]
|
||||
ty_errors = load('ustation_errors_ty.mat');
|
||||
|
||||
% Compute best straight line to remove it from data
|
||||
average_error = mean(ty_errors.ty_z')';
|
||||
straight_line = average_error - detrend(average_error, 1);
|
||||
|
||||
%% Measurement of the linear (vertical) deviation of the Translation stage
|
||||
figure;
|
||||
hold on;
|
||||
plot(ty_errors.setpoint, ty_errors.ty_z(:,1), '-', 'color', colors(1,:), 'DisplayName', 'Raw data')
|
||||
plot(ty_errors.setpoint, ty_errors.ty_z(:,2:end), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(ty_errors.setpoint, straight_line, '--', 'color', colors(2,:), 'DisplayName', 'Linear fit')
|
||||
hold off;
|
||||
xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
|
||||
xlim([-5, 5]); ylim([-8, 8]);
|
||||
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
|
||||
%% Measurement of the linear (vertical) deviation of the Translation stage - Remove best linear fit
|
||||
figure;
|
||||
plot(ty_errors.setpoint, ty_errors.ty_z - straight_line, 'k-')
|
||||
xlabel('$D_y$ position [mm]'); ylabel('Residual vertical error [$\mu$m]');
|
||||
xlim([-5, 5]); ylim([-0.4, 0.4]);
|
||||
|
||||
%% Convert the data to frequency domain
|
||||
% Suppose a certain constant velocity scan
|
||||
delta_ty = (ty_errors.setpoint(end) - ty_errors.setpoint(1))/(length(ty_errors.setpoint) - 1); % [mm]
|
||||
ty_vel = 8*1.125; % [mm/s]
|
||||
Ts = delta_ty/ty_vel;
|
||||
Fs = 1/Ts;
|
||||
|
||||
% Frequency Analysis
|
||||
Nfft = floor(length(ty_errors.setpoint)); % Number of frequency points
|
||||
win = hanning(Nfft); % Windowing
|
||||
Noverlap = floor(Nfft/2); % Overlap for frequency analysis
|
||||
|
||||
% Comnpute the power spectral density
|
||||
[pxx_dy_dz, f_dy] = pwelch(1e-6*detrend(ty_errors.ty_z, 1), win, Noverlap, Nfft, Fs);
|
||||
pxx_dy_dz = mean(pxx_dy_dz')';
|
||||
|
||||
% Having the PSD of the lateral error equal to the PSD of the vertical error
|
||||
% is a reasonable assumption (and verified in practice)
|
||||
pxx_dy_dx = pxx_dy_dz;
|
||||
|
||||
%% Spindle Errors
|
||||
% Errors are already converted to x,y,z,Rx,Ry
|
||||
% Units are in [m] and [rad]
|
||||
spindle_errors = load('ustation_errors_spindle.mat');
|
||||
|
||||
%% Measured radial errors of the Spindle
|
||||
figure;
|
||||
hold on;
|
||||
plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Raw data')
|
||||
% Compute best fitting circle
|
||||
[x0, y0, R] = circlefit(spindle_errors.Dx, spindle_errors.Dy);
|
||||
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
||||
plot(1e6*(x0 + R*cos(theta)), 1e6*(y0 + R*sin(theta)), '--', 'DisplayName', 'Best Fit')
|
||||
hold off;
|
||||
xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
|
||||
axis equal
|
||||
xlim([-1, 1]); ylim([-1, 1]);
|
||||
xticks([-1, -0.5, 0, 0.5, 1]);
|
||||
yticks([-1, -0.5, 0, 0.5, 1]);
|
||||
leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Measured axial errors of the Spindle
|
||||
figure;
|
||||
plot(spindle_errors.deg(1:100:end)/360, 1e9*spindle_errors.Dz(1:100:end))
|
||||
xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
|
||||
axis square
|
||||
xlim([0,2]); ylim([-40, 40]);
|
||||
|
||||
%% Measured tilt errors of the Spindle
|
||||
figure;
|
||||
plot(1e6*spindle_errors.Rx(1:100:end), 1e6*spindle_errors.Ry(1:100:end))
|
||||
xlabel('X angle [$\mu$rad]'); ylabel('Y angle [$\mu$rad]');
|
||||
axis equal
|
||||
xlim([-35, 35]); ylim([-35, 35]);
|
||||
xticks([-30, -15, 0, 15, 30]);
|
||||
yticks([-30, -15, 0, 15, 30]);
|
||||
|
||||
%% Remove the circular fit from the data
|
||||
[x0, y0, R] = circlefit(spindle_errors.Dx, spindle_errors.Dy);
|
||||
|
||||
% Search the best angular match
|
||||
fun = @(theta)rms((spindle_errors.Dx - (x0 + R*cos(pi/180*spindle_errors.deg+theta(1)))).^2 + (spindle_errors.Dy - (y0 - R*sin(pi/180*spindle_errors.deg+theta(1)))).^2);
|
||||
x0 = [0];
|
||||
delta_theta = fminsearch(fun, 0);
|
||||
|
||||
% Compute the remaining error after removing the best circular fit
|
||||
spindle_errors.Dx_err = spindle_errors.Dx - (x0 + R*cos(pi/180*spindle_errors.deg+delta_theta));
|
||||
spindle_errors.Dy_err = spindle_errors.Dy - (y0 - R*sin(pi/180*spindle_errors.deg+delta_theta));
|
||||
|
||||
%% Convert the data to frequency domain
|
||||
Ts = (spindle_errors.t(end) - spindle_errors.t(1))/(length(spindle_errors.t)-1); % [s]
|
||||
Fs = 1/Ts; % [Hz]
|
||||
|
||||
% Frequency Analysis
|
||||
Nfft = floor(2.0*Fs); % Number of frequency points
|
||||
win = hanning(Nfft); % Windowing
|
||||
Noverlap = floor(Nfft/2); % Overlap for frequency analysis
|
||||
|
||||
% Comnpute the power spectral density
|
||||
[pxx_rz_dz, f_rz] = pwelch(spindle_errors.Dz, win, Noverlap, Nfft, Fs);
|
||||
[pxx_rz_dx, ~ ] = pwelch(spindle_errors.Dx_err, win, Noverlap, Nfft, Fs);
|
||||
[pxx_rz_dy, ~ ] = pwelch(spindle_errors.Dy_err, win, Noverlap, Nfft, Fs);
|
||||
|
||||
%% Extract sensitivity to disturbances from the Simscape model
|
||||
% Initialize stages
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
|
||||
initializeDisturbances('enable', false);
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeLoggingConfiguration();
|
||||
|
||||
% Configure inputs and outputs
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/Dwx'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
|
||||
io(io_i) = linio([mdl, '/Disturbances/Dwy'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
|
||||
io(io_i) = linio([mdl, '/Disturbances/Dwz'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
|
||||
io(io_i) = linio([mdl, '/Disturbances/Fdy_x'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Ty
|
||||
io(io_i) = linio([mdl, '/Disturbances/Fdy_z'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Ty
|
||||
io(io_i) = linio([mdl, '/Disturbances/Frz_x'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
|
||||
io(io_i) = linio([mdl, '/Disturbances/Frz_y'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
|
||||
io(io_i) = linio([mdl, '/Disturbances/Frz_z'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
|
||||
io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/x'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
|
||||
io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/y'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
|
||||
io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/z'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
|
||||
|
||||
% Run the linearization
|
||||
Gd = linearize(mdl, io, 0);
|
||||
Gd.InputName = {'Dwx', 'Dwy', 'Dwz', 'Fdy_x', 'Fdy_z', 'Frz_x', 'Frz_y', 'Frz_z'};
|
||||
Gd.OutputName = {'Dx', 'Dy', 'Dz'};
|
||||
|
||||
% The identified dynamics are then saved for further use.
|
||||
save('./mat/ustation_disturbance_sensitivity.mat', 'Gd')
|
||||
|
||||
%% Sensitivity to Ground motion
|
||||
freqs = logspace(log10(10), log10(2e2), 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Dwx'), freqs, 'Hz'))), 'DisplayName', '$D_x/D_{xf}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dy', 'Dwy'), freqs, 'Hz'))), 'DisplayName', '$D_y/D_{yf}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Dwz'), freqs, 'Hz'))), 'DisplayName', '$D_z/D_{zf}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Sensitivity to Translation stage disturbance forces
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Fdy_x'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_x/F_{xD_y}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Fdy_z'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$D_z/F_{zD_y}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
ylim([1e-10, 1e-7]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Sensitivity to Spindle disturbance forces
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Frz_x'), freqs, 'Hz'))), 'DisplayName', '$D_x/F_{xR_z}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dy', 'Frz_y'), freqs, 'Hz'))), 'DisplayName', '$D_y/F_{yR_z}$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Frz_z'), freqs, 'Hz'))), 'DisplayName', '$D_z/F_{zR_z}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
||||
ylim([1e-10, 1e-7]);
|
||||
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Compute the PSD of the equivalent disturbance sources
|
||||
pxx_rz_fx = pxx_rz_dx./abs(squeeze(freqresp(Gd('Dx', 'Frz_x'), f_rz, 'Hz'))).^2;
|
||||
pxx_rz_fy = pxx_rz_dy./abs(squeeze(freqresp(Gd('Dy', 'Frz_y'), f_rz, 'Hz'))).^2;
|
||||
pxx_rz_fz = pxx_rz_dz./abs(squeeze(freqresp(Gd('Dz', 'Frz_z'), f_rz, 'Hz'))).^2;
|
||||
|
||||
pxx_dy_fx = pxx_dy_dx./abs(squeeze(freqresp(Gd('Dx', 'Fdy_x'), f_dy, 'Hz'))).^2;
|
||||
pxx_dy_fz = pxx_dy_dz./abs(squeeze(freqresp(Gd('Dz', 'Fdy_z'), f_dy, 'Hz'))).^2;
|
||||
|
||||
%% Save the PSD of the disturbance sources for further used
|
||||
% in the Simscape model
|
||||
|
||||
% Ground motion
|
||||
min_f = 1; max_f = 100;
|
||||
gm_dist.f = f_gm(f_gm < max_f & f_gm > min_f);
|
||||
gm_dist.pxx_x = pxx_gm_x(f_gm < max_f & f_gm > min_f);
|
||||
gm_dist.pxx_y = pxx_gm_y(f_gm < max_f & f_gm > min_f);
|
||||
gm_dist.pxx_z = pxx_gm_z(f_gm < max_f & f_gm > min_f);
|
||||
|
||||
% Translation stage
|
||||
min_f = 0.5; max_f = 500;
|
||||
dy_dist.f = f_dy(f_dy < max_f & f_dy > min_f);
|
||||
dy_dist.pxx_fx = pxx_dy_fx(f_dy < max_f & f_dy > min_f);
|
||||
dy_dist.pxx_fz = pxx_dy_fz(f_dy < max_f & f_dy > min_f);
|
||||
|
||||
% Spindle
|
||||
min_f = 1; max_f = 500;
|
||||
rz_dist.f = f_rz(f_rz < max_f & f_rz > min_f);
|
||||
rz_dist.pxx_fx = pxx_rz_fx(f_rz < max_f & f_rz > min_f);
|
||||
rz_dist.pxx_fy = pxx_rz_fy(f_rz < max_f & f_rz > min_f);
|
||||
rz_dist.pxx_fz = pxx_rz_fz(f_rz < max_f & f_rz > min_f);
|
||||
|
||||
% The identified dynamics are then saved for further use.
|
||||
save('./mat/ustation_disturbance_psd.mat', 'rz_dist', 'dy_dist', 'gm_dist')
|
||||
|
||||
%% Ground Motion
|
||||
figure;
|
||||
hold on;
|
||||
plot(f_gm, sqrt(pxx_gm_x), 'DisplayName', '$D_{wx}$');
|
||||
plot(f_gm, sqrt(pxx_gm_y), 'DisplayName', '$D_{wy}$');
|
||||
plot(f_gm, sqrt(pxx_gm_z), 'DisplayName', '$D_{wz}$');
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
|
||||
xlim([1, 200]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f_dy, sqrt(pxx_dy_fx), 'DisplayName', '$F_{xD_y}$');
|
||||
plot(f_dy, sqrt(pxx_dy_fz), 'DisplayName', '$F_{zD_y}$');
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
|
||||
xlim([1, 200]); ylim([1e-3, 1e3]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(f_rz, sqrt(pxx_rz_fx), 'DisplayName', '$F_{xR_z}$');
|
||||
plot(f_rz, sqrt(pxx_rz_fy), 'DisplayName', '$F_{yR_z}$');
|
||||
plot(f_rz, sqrt(pxx_rz_fz), 'DisplayName', '$F_{zR_z}$');
|
||||
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
|
||||
xlim([1, 200]); ylim([1e-3, 1e3]);
|
||||
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Compute time domain disturbance signals
|
||||
initializeDisturbances();
|
||||
load('nass_model_disturbances.mat');
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(Rz.t, Rz.x, 'DisplayName', '$F_{xR_z}$');
|
||||
plot(Rz.t, Rz.y, 'DisplayName', '$F_{yR_z}$');
|
||||
plot(Rz.t, Rz.z, 'DisplayName', '$F_{zR_z}$');
|
||||
xlabel('Time [s]'); ylabel('Amplitude [N]')
|
||||
xlim([0, 1]); ylim([-100, 100]);
|
||||
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(Dy.t, Dy.x, 'DisplayName', '$F_{xD_y}$');
|
||||
plot(Dy.t, Dy.z, 'DisplayName', '$F_{zD_y}$');
|
||||
xlabel('Time [s]'); ylabel('Amplitude [N]')
|
||||
xlim([0, 1]); ylim([-60, 60])
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(Dw.t, 1e6*Dw.x, 'DisplayName', '$D_{xf}$');
|
||||
plot(Dw.t, 1e6*Dw.y, 'DisplayName', '$D_{yf}$');
|
||||
plot(Dw.t, 1e6*Dw.z, 'DisplayName', '$D_{zf}$');
|
||||
xlabel('Time [s]'); ylabel('Amplitude [$\mu$m]')
|
||||
xlim([0, 1]); ylim([-0.6, 0.6])
|
||||
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
@@ -0,0 +1,141 @@
|
||||
%% ustation_4_experiments.m
|
||||
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
%% 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
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'ustation_simscape';
|
||||
|
||||
load('nass_model_conf_simulink.mat');
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
|
||||
%% Frequency Vector
|
||||
freqs = logspace(log10(10), log10(2e3), 1000);
|
||||
|
||||
%% Tomography experiment
|
||||
% Sample is not centered with the rotation axis
|
||||
% This is done by offsetfing the micro-hexapod by 0.9um
|
||||
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]
|
||||
|
||||
set_param(conf_simulink, 'StopTime', '10');
|
||||
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod('AP', P_micro_hexapod);
|
||||
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
|
||||
initializeDisturbances(...
|
||||
'Dw_x', true, ... % Ground Motion - X direction
|
||||
'Dw_y', true, ... % Ground Motion - Y direction
|
||||
'Dw_z', true, ... % Ground Motion - Z direction
|
||||
'Fdy_x', false, ... % Translation Stage - X direction
|
||||
'Fdy_z', false, ... % Translation Stage - Z direction
|
||||
'Frz_x', true, ... % Spindle - X direction
|
||||
'Frz_y', true, ... % Spindle - Y direction
|
||||
'Frz_z', true); % Spindle - Z direction
|
||||
|
||||
initializeReferences(...
|
||||
'Rz_type', 'rotating', ...
|
||||
'Rz_period', 1, ...
|
||||
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
|
||||
|
||||
sim(mdl);
|
||||
exp_tomography = simout;
|
||||
|
||||
%% Compare with the measurements
|
||||
spindle_errors = load('ustation_errors_spindle.mat');
|
||||
|
||||
%% Measured radial errors of the Spindle
|
||||
figure;
|
||||
hold on;
|
||||
plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Measurements')
|
||||
plot(1e6*exp_tomography.y.x.Data, 1e6*exp_tomography.y.y.Data, 'DisplayName', 'Simulation')
|
||||
hold off;
|
||||
xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
|
||||
axis equal
|
||||
xlim([-1, 1]); ylim([-1, 1]);
|
||||
xticks([-1, -0.5, 0, 0.5, 1]);
|
||||
yticks([-1, -0.5, 0, 0.5, 1]);
|
||||
leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Measured axial errors of the Spindle
|
||||
figure;
|
||||
hold on;
|
||||
plot(spindle_errors.deg(1:100:end)/360, detrend(1e9*spindle_errors.Dz(1:100:end), 0), 'DisplayName', 'Measurements')
|
||||
plot(exp_tomography.y.z.Time, detrend(1e9*exp_tomography.y.z.Data, 0), 'DisplayName', 'Simulation')
|
||||
hold off;
|
||||
xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
|
||||
axis square
|
||||
xlim([0,2]); ylim([-40, 40]);
|
||||
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
|
||||
%% Translation stage latteral scans
|
||||
set_param(conf_simulink, 'StopTime', '2');
|
||||
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
|
||||
initializeSimscapeConfiguration('gravity', false);
|
||||
initializeLoggingConfiguration('log', 'all');
|
||||
|
||||
initializeDisturbances(...
|
||||
'Dw_x', true, ... % Ground Motion - X direction
|
||||
'Dw_y', true, ... % Ground Motion - Y direction
|
||||
'Dw_z', true, ... % Ground Motion - Z direction
|
||||
'Fdy_x', true, ... % Translation Stage - X direction
|
||||
'Fdy_z', true, ... % Translation Stage - Z direction
|
||||
'Frz_x', false, ... % Spindle - X direction
|
||||
'Frz_y', false, ... % Spindle - Y direction
|
||||
'Frz_z', false); % Spindle - Z direction
|
||||
|
||||
initializeReferences(...
|
||||
'Dy_type', 'triangular', ...
|
||||
'Dy_amplitude', 4.5e-3, ...
|
||||
'Dy_period', 2);
|
||||
|
||||
sim(mdl);
|
||||
exp_latteral_scans = simout;
|
||||
|
||||
% Load the experimentally measured errors
|
||||
ty_errors = load('ustation_errors_ty.mat');
|
||||
|
||||
% Compute best straight line to remove it from data
|
||||
average_error = mean(ty_errors.ty_z')';
|
||||
straight_line = average_error - detrend(average_error, 1);
|
||||
|
||||
% Only plot data for the scan from -4.5mm to 4.5mm
|
||||
dy_setpoint = 1e3*exp_latteral_scans.y.y.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5);
|
||||
dz_error = detrend(1e6*exp_latteral_scans.y.z.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5), 0);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,1) - straight_line, 0), 'color', colors(1,:), 'DisplayName', 'Measurement')
|
||||
% plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,[4,6]) - straight_line, 0), 'color', colors(1,:), 'HandleVisibility', 'off')
|
||||
plot(dy_setpoint, dz_error, 'color', colors(2,:), 'DisplayName', 'Simulation')
|
||||
hold off;
|
||||
xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
|
||||
xlim([-5, 5]); ylim([-0.4, 0.4]);
|
||||
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
||||
leg.ItemTokenSize(1) = 15;
|
||||
Binary file not shown.
Reference in New Issue
Block a user