Merged micro-station and nano-hexapod models
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.
@@ -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,37 @@
|
||||
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:
|
||||
% - geometry.J [6x6] - The Jacobian Matrix
|
||||
% - geometry.K [6x6] - The Stiffness Matrix
|
||||
% - geometry.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.geometry.J = J;
|
||||
stewart.geometry.K = K;
|
||||
stewart.geometry.C = C;
|
||||
|
||||
end
|
||||
@@ -0,0 +1,80 @@
|
||||
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;
|
||||
|
||||
end
|
||||
@@ -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,80 @@
|
||||
function [] = describeStewartPlatform(stewart)
|
||||
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
|
||||
%
|
||||
% Syntax: [] = describeStewartPlatform(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart
|
||||
%
|
||||
% Outputs:
|
||||
|
||||
arguments
|
||||
stewart
|
||||
end
|
||||
|
||||
fprintf('GEOMETRY:\n')
|
||||
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
|
||||
|
||||
if stewart.platform_M.MO_B(3) > 0
|
||||
fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3))
|
||||
else
|
||||
fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
|
||||
end
|
||||
|
||||
fprintf('- The initial length of the struts are:\n')
|
||||
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('ACTUATORS:\n')
|
||||
if stewart.actuators.type == 1
|
||||
fprintf('- The actuators are modelled as 1DoF.\n')
|
||||
fprintf('- The Stiffness and Damping of each actuators is:\n')
|
||||
fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.k(1), stewart.actuators.c(1))
|
||||
if stewart.actuators.kp > 0
|
||||
fprintf('\t Added parallel stiffness: kp = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.kp(1))
|
||||
end
|
||||
elseif stewart.actuators.type == 2
|
||||
fprintf('- The actuators are modelled as 2DoF (APA).\n')
|
||||
fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
|
||||
fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.ka(1), stewart.actuators.ca(1))
|
||||
fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
|
||||
fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.kr(1), stewart.actuators.cr(1))
|
||||
elseif stewart.actuators.type == 3
|
||||
fprintf('- The actuators are modelled with a flexible element (FEM).\n')
|
||||
end
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('JOINTS:\n')
|
||||
|
||||
switch stewart.joints_F.type
|
||||
case 1
|
||||
fprintf('- The joints on the fixed based are universal joints (2DoF)\n')
|
||||
case 2
|
||||
fprintf('- The joints on the fixed based are spherical joints (3DoF)\n')
|
||||
end
|
||||
|
||||
switch stewart.joints_M.type
|
||||
case 1
|
||||
fprintf('- The joints on the mobile based are universal joints (2DoF)\n')
|
||||
case 2
|
||||
fprintf('- The joints on the mobile based are spherical joints (3DoF)\n')
|
||||
end
|
||||
|
||||
fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
|
||||
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
|
||||
|
||||
fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
|
||||
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
|
||||
fprintf('\n')
|
||||
|
||||
fprintf('KINEMATICS:\n')
|
||||
|
||||
if isfield(stewart.kinematics, 'K')
|
||||
fprintf('- The Stiffness matrix K is (in [N/m]):\n')
|
||||
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
|
||||
end
|
||||
|
||||
if isfield(stewart.kinematics, 'C')
|
||||
fprintf('- The Damping matrix C is (in [m/N]):\n')
|
||||
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
|
||||
end
|
||||
@@ -0,0 +1,240 @@
|
||||
function [] = displayArchitecture(stewart, args)
|
||||
% displayArchitecture - 3D plot of the Stewart platform architecture
|
||||
%
|
||||
% Syntax: [] = displayArchitecture(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart
|
||||
% - args - Structure with 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}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
% - F_color [color] - Color used for the Fixed elements
|
||||
% - M_color [color] - Color used for the Mobile elements
|
||||
% - L_color [color] - Color used for the Legs elements
|
||||
% - frames [true/false] - Display the Frames
|
||||
% - legs [true/false] - Display the Legs
|
||||
% - joints [true/false] - Display the Joints
|
||||
% - labels [true/false] - Display the Labels
|
||||
% - platforms [true/false] - Display the Platforms
|
||||
% - views ['all', 'xy', 'yz', 'xz', 'default'] -
|
||||
%
|
||||
% Outputs:
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
args.F_color = [0 0.4470 0.7410]
|
||||
args.M_color = [0.8500 0.3250 0.0980]
|
||||
args.L_color = [0 0 0]
|
||||
args.frames logical {mustBeNumericOrLogical} = true
|
||||
args.legs logical {mustBeNumericOrLogical} = true
|
||||
args.joints logical {mustBeNumericOrLogical} = true
|
||||
args.labels logical {mustBeNumericOrLogical} = true
|
||||
args.platforms logical {mustBeNumericOrLogical} = true
|
||||
args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
|
||||
end
|
||||
|
||||
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, 'H'), 'stewart.geometry should have attribute H')
|
||||
H = stewart.geometry.H;
|
||||
|
||||
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;
|
||||
|
||||
if ~strcmp(args.views, 'all')
|
||||
figure;
|
||||
else
|
||||
f = figure('visible', 'off');
|
||||
end
|
||||
|
||||
hold on;
|
||||
|
||||
FTa = [eye(3), FO_A; ...
|
||||
zeros(1,3), 1];
|
||||
ATb = [args.ARB, args.AP; ...
|
||||
zeros(1,3), 1];
|
||||
BTm = [eye(3), -MO_B; ...
|
||||
zeros(1,3), 1];
|
||||
|
||||
FTm = FTa*ATb*BTm;
|
||||
|
||||
d_unit_vector = H/4;
|
||||
|
||||
d_label = H/20;
|
||||
|
||||
Ff = [0, 0, 0];
|
||||
if args.frames
|
||||
quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
|
||||
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
||||
|
||||
if args.labels
|
||||
text(Ff(1) + d_label, ...
|
||||
Ff(2) + d_label, ...
|
||||
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
|
||||
end
|
||||
end
|
||||
|
||||
if args.frames
|
||||
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
|
||||
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
||||
|
||||
if args.labels
|
||||
text(FO_A(1) + d_label, ...
|
||||
FO_A(2) + d_label, ...
|
||||
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
|
||||
end
|
||||
end
|
||||
|
||||
if args.platforms && stewart.platform_F.type == 1
|
||||
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
||||
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
|
||||
center = [0; 0; 0]; % Center of the circle
|
||||
radius = stewart.platform_F.R; % Radius of the circle [m]
|
||||
|
||||
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||||
|
||||
plot3(points(1,:), ...
|
||||
points(2,:), ...
|
||||
points(3,:), '-', 'Color', args.F_color);
|
||||
end
|
||||
|
||||
if args.joints
|
||||
scatter3(Fa(1,:), ...
|
||||
Fa(2,:), ...
|
||||
Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
||||
if args.labels
|
||||
for i = 1:size(Fa,2)
|
||||
text(Fa(1,i) + d_label, ...
|
||||
Fa(2,i), ...
|
||||
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
|
||||
|
||||
if args.frames
|
||||
FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||||
quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
|
||||
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
|
||||
|
||||
if args.labels
|
||||
text(Fm(1) + d_label, ...
|
||||
Fm(2) + d_label, ...
|
||||
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
|
||||
end
|
||||
end
|
||||
|
||||
FB = FO_A + args.AP;
|
||||
|
||||
if args.frames
|
||||
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
||||
quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
|
||||
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
|
||||
|
||||
if args.labels
|
||||
text(FB(1) - d_label, ...
|
||||
FB(2) + d_label, ...
|
||||
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
|
||||
end
|
||||
end
|
||||
|
||||
if args.platforms && stewart.platform_M.type == 1
|
||||
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
||||
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
|
||||
center = Fm(1:3); % Center of the circle
|
||||
radius = stewart.platform_M.R; % Radius of the circle [m]
|
||||
|
||||
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
||||
|
||||
plot3(points(1,:), ...
|
||||
points(2,:), ...
|
||||
points(3,:), '-', 'Color', args.M_color);
|
||||
end
|
||||
|
||||
if args.joints
|
||||
Fb = FTm*[Mb;ones(1,6)];
|
||||
|
||||
scatter3(Fb(1,:), ...
|
||||
Fb(2,:), ...
|
||||
Fb(3,:), 'MarkerEdgeColor', args.M_color);
|
||||
|
||||
if args.labels
|
||||
for i = 1:size(Fb,2)
|
||||
text(Fb(1,i) + d_label, ...
|
||||
Fb(2,i), ...
|
||||
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
if args.legs
|
||||
for i = 1:6
|
||||
plot3([Fa(1,i), Fb(1,i)], ...
|
||||
[Fa(2,i), Fb(2,i)], ...
|
||||
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
|
||||
|
||||
if args.labels
|
||||
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
|
||||
(Fa(2,i)+Fb(2,i))/2, ...
|
||||
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
switch args.views
|
||||
case 'default'
|
||||
view([1 -0.6 0.4]);
|
||||
case 'xy'
|
||||
view([0 0 1]);
|
||||
case 'xz'
|
||||
view([0 -1 0]);
|
||||
case 'yz'
|
||||
view([1 0 0]);
|
||||
end
|
||||
axis equal;
|
||||
axis off;
|
||||
|
||||
if strcmp(args.views, 'all')
|
||||
hAx = findobj('type', 'axes');
|
||||
|
||||
figure;
|
||||
s1 = subplot(2,2,1);
|
||||
copyobj(get(hAx(1), 'Children'), s1);
|
||||
view([0 0 1]);
|
||||
axis equal;
|
||||
axis off;
|
||||
title('Top')
|
||||
|
||||
s2 = subplot(2,2,2);
|
||||
copyobj(get(hAx(1), 'Children'), s2);
|
||||
view([1 -0.6 0.4]);
|
||||
axis equal;
|
||||
axis off;
|
||||
|
||||
s3 = subplot(2,2,3);
|
||||
copyobj(get(hAx(1), 'Children'), s3);
|
||||
view([1 0 0]);
|
||||
axis equal;
|
||||
axis off;
|
||||
title('Front')
|
||||
|
||||
s4 = subplot(2,2,4);
|
||||
copyobj(get(hAx(1), 'Children'), s4);
|
||||
view([0 -1 0]);
|
||||
axis equal;
|
||||
axis off;
|
||||
title('Side')
|
||||
|
||||
close(f);
|
||||
end
|
||||
@@ -0,0 +1,41 @@
|
||||
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;
|
||||
|
||||
end
|
||||
@@ -0,0 +1,45 @@
|
||||
function [] = initializeController(args)
|
||||
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
|
||||
end
|
||||
|
||||
controller = struct();
|
||||
|
||||
switch args.type
|
||||
case 'open-loop'
|
||||
controller.type = 1;
|
||||
controller.name = 'Open-Loop';
|
||||
case 'dvf'
|
||||
controller.type = 2;
|
||||
controller.name = 'Decentralized Direct Velocity Feedback';
|
||||
case 'iff'
|
||||
controller.type = 3;
|
||||
controller.name = 'Decentralized Integral Force Feedback';
|
||||
case 'hac-dvf'
|
||||
controller.type = 4;
|
||||
controller.name = 'HAC-DVF';
|
||||
case 'ref-track-L'
|
||||
controller.type = 5;
|
||||
controller.name = 'Reference Tracking in the frame of the legs';
|
||||
case 'ref-track-iff-L'
|
||||
controller.type = 6;
|
||||
controller.name = 'Reference Tracking in the frame of the legs + IFF';
|
||||
case 'cascade-hac-lac'
|
||||
controller.type = 7;
|
||||
controller.name = 'Cascade Control + HAC-LAC';
|
||||
case 'hac-iff'
|
||||
controller.type = 8;
|
||||
controller.name = 'HAC-IFF';
|
||||
case 'stabilizing'
|
||||
controller.type = 9;
|
||||
controller.name = 'Stabilizing Controller';
|
||||
end
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
save('mat/nass_model_controller.mat', 'controller');
|
||||
elseif exist('./matlab', 'dir')
|
||||
save('matlab/mat/nass_model_controller.mat', 'controller');
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,61 @@
|
||||
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;
|
||||
|
||||
end
|
||||
@@ -0,0 +1,69 @@
|
||||
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
|
||||
|
||||
stewart.struts_M.type = 1;
|
||||
|
||||
%% Compute the properties of the cylindrical struts
|
||||
Fsm = args.Fsm;
|
||||
Fsh = args.Fsh;
|
||||
Fsr = args.Fsr;
|
||||
|
||||
Msm = args.Msm;
|
||||
Msh = args.Msh;
|
||||
Msr = args.Msr;
|
||||
|
||||
I_F = [1/12 * Fsm * (3*Fsr^2 + Fsh^2), ...
|
||||
1/12 * Fsm * (3*Fsr^2 + Fsh^2), ...
|
||||
1/2 * Fsm * Fsr^2];
|
||||
|
||||
I_M = [1/12 * Msm * (3*Msr^2 + Msh^2), ...
|
||||
1/12 * Msm * (3*Msr^2 + Msh^2), ...
|
||||
1/2 * Msm * Msr^2];
|
||||
stewart.struts_M.I = I_M;
|
||||
stewart.struts_F.I = I_F;
|
||||
|
||||
stewart.struts_M.M = args.Msm;
|
||||
stewart.struts_M.R = args.Msr;
|
||||
stewart.struts_M.H = args.Msh;
|
||||
|
||||
stewart.struts_F.type = 1;
|
||||
|
||||
stewart.struts_F.M = args.Fsm;
|
||||
stewart.struts_F.R = args.Fsr;
|
||||
stewart.struts_F.H = args.Fsh;
|
||||
|
||||
end
|
||||
@@ -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,37 @@
|
||||
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;
|
||||
|
||||
end
|
||||
@@ -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,129 @@
|
||||
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,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
||||
args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
||||
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
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 '2dof'
|
||||
stewart.joints_F.type = 1;
|
||||
case '3dof'
|
||||
stewart.joints_F.type = 2;
|
||||
case '4dof'
|
||||
stewart.joints_F.type = 3;
|
||||
case '6dof'
|
||||
stewart.joints_F.type = 4;
|
||||
case 'flexible'
|
||||
stewart.joints_F.type = 5;
|
||||
otherwise
|
||||
error("joints_F are not correctly defined")
|
||||
end
|
||||
|
||||
switch args.type_M
|
||||
case '2dof'
|
||||
stewart.joints_M.type = 1;
|
||||
case '3dof'
|
||||
stewart.joints_M.type = 2;
|
||||
case '4dof'
|
||||
stewart.joints_M.type = 3;
|
||||
case '6dof'
|
||||
stewart.joints_M.type = 4;
|
||||
case 'flexible'
|
||||
stewart.joints_M.type = 5;
|
||||
otherwise
|
||||
error("joints_M are not correctly defined")
|
||||
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;
|
||||
|
||||
end
|
||||
@@ -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 (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e7
|
||||
args.Ci (1,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3
|
||||
% 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', '2dof', ...
|
||||
'type_M', '3dof');
|
||||
|
||||
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,36 @@
|
||||
function [sample] = initializeSample(args)
|
||||
|
||||
arguments
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 % Height [m]
|
||||
args.R (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 % Radius [m]
|
||||
args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg]
|
||||
end
|
||||
|
||||
sample = struct();
|
||||
|
||||
switch args.type
|
||||
case '0'
|
||||
sample.type = 0;
|
||||
case '1'
|
||||
sample.type = 1;
|
||||
case '2'
|
||||
sample.type = 2;
|
||||
case '3'
|
||||
sample.type = 3;
|
||||
end
|
||||
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'sample', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'sample');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'sample', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'sample');
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,145 @@
|
||||
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)
|
||||
|
||||
arguments
|
||||
%% initializeFramesPositions
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
||||
%% generateGeneralConfiguration
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of fixed joints [m]
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
|
||||
args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of mobile joints [m]
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
|
||||
args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
|
||||
%% Actuators
|
||||
args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
||||
args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6
|
||||
args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e4
|
||||
args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605
|
||||
args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302
|
||||
args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50
|
||||
args.actuator_cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100
|
||||
args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50
|
||||
%% initializeCylindricalPlatforms
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
|
||||
%% initializeCylindricalStruts
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
||||
%% Bottom and Top Flexible Joints
|
||||
args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
||||
args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
||||
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
%% 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, ...
|
||||
'type', args.actuator_type, ...
|
||||
'k', args.actuator_k, ...
|
||||
'kp', args.actuator_kp, ...
|
||||
'ke', args.actuator_ke, ...
|
||||
'ka', args.actuator_ka, ...
|
||||
'c', args.actuator_c, ...
|
||||
'cp', args.actuator_cp, ...
|
||||
'ce', args.actuator_ce, ...
|
||||
'ca', args.actuator_ca);
|
||||
|
||||
stewart = initializeJointDynamics(stewart, ...
|
||||
'type_F', args.flex_type_F, ...
|
||||
'type_M', args.flex_type_M, ...
|
||||
'Kf_M', args.Kf_M, ...
|
||||
'Cf_M', args.Cf_M, ...
|
||||
'Kt_M', args.Kt_M, ...
|
||||
'Ct_M', args.Ct_M, ...
|
||||
'Kf_F', args.Kf_F, ...
|
||||
'Cf_F', args.Cf_F, ...
|
||||
'Kt_F', args.Kt_F, ...
|
||||
'Ct_F', args.Ct_F, ...
|
||||
'Ka_F', args.Ka_F, ...
|
||||
'Ca_F', args.Ca_F, ...
|
||||
'Kr_F', args.Kr_F, ...
|
||||
'Cr_F', args.Cr_F, ...
|
||||
'Ka_M', args.Ka_M, ...
|
||||
'Ca_M', args.Ca_M, ...
|
||||
'Kr_M', args.Kr_M, ...
|
||||
'Cr_M', args.Cr_M);
|
||||
|
||||
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);
|
||||
|
||||
nano_hexapod = stewart;
|
||||
if exist('./mat', 'dir')
|
||||
if exist('./mat/nass_model_stages.mat', 'file')
|
||||
save('mat/nass_model_stages.mat', 'nano_hexapod', '-append');
|
||||
else
|
||||
save('mat/nass_model_stages.mat', 'nano_hexapod');
|
||||
end
|
||||
elseif exist('./matlab', 'dir')
|
||||
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
||||
save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append');
|
||||
else
|
||||
save('matlab/mat/nass_model_stages.mat', 'nano_hexapod');
|
||||
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,33 @@
|
||||
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();
|
||||
|
||||
end
|
||||
@@ -0,0 +1,29 @@
|
||||
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;
|
||||
|
||||
end
|
||||
@@ -0,0 +1,60 @@
|
||||
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,{'1dof', '2dof', 'flexible'})} = '1dof'
|
||||
args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
|
||||
args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
|
||||
args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
|
||||
args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
|
||||
args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
||||
args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6
|
||||
args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
|
||||
|
||||
args.F_gain (1,1) double {mustBeNumeric} = 1
|
||||
args.me (1,1) double {mustBeNumeric} = 0.01
|
||||
args.ma (1,1) double {mustBeNumeric} = 0.01
|
||||
end
|
||||
|
||||
if strcmp(args.type, '1dof')
|
||||
stewart.actuators.type = 1;
|
||||
elseif strcmp(args.type, '2dof')
|
||||
stewart.actuators.type = 2;
|
||||
elseif strcmp(args.type, 'flexible')
|
||||
stewart.actuators.type = 3;
|
||||
end
|
||||
|
||||
stewart.actuators.k = args.k;
|
||||
stewart.actuators.c = args.c;
|
||||
|
||||
% Parallel stiffness
|
||||
stewart.actuators.kp = args.kp;
|
||||
stewart.actuators.cp = args.cp;
|
||||
|
||||
stewart.actuators.ka = args.ka;
|
||||
stewart.actuators.ca = args.ca;
|
||||
|
||||
stewart.actuators.ke = args.ke;
|
||||
stewart.actuators.ce = args.ce;
|
||||
|
||||
stewart.actuators.F_gain = args.F_gain;
|
||||
|
||||
stewart.actuators.ma = args.ma;
|
||||
stewart.actuators.me = args.me;
|
||||
|
||||
end
|
||||
@@ -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,38 @@
|
||||
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;
|
||||
|
||||
end
|
||||
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.
+2751
-32
File diff suppressed because it is too large
Load Diff
Binary file not shown.
+146
-7
@@ -1,8 +1,9 @@
|
||||
% Created 2024-03-19 Tue 11:06
|
||||
% Created 2025-02-12 Wed 11:34
|
||||
% Intended LaTeX compiler: pdflatex
|
||||
\documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt}
|
||||
|
||||
\input{preamble.tex}
|
||||
\input{preamble_extra.tex}
|
||||
\bibliography{simscape-nass.bib}
|
||||
\author{Dehaeze Thomas}
|
||||
\date{\today}
|
||||
@@ -12,7 +13,7 @@
|
||||
pdftitle={Simscape Model - Nano Active Stabilization System},
|
||||
pdfkeywords={},
|
||||
pdfsubject={},
|
||||
pdfcreator={Emacs 29.2 (Org mode 9.7)},
|
||||
pdfcreator={Emacs 29.4 (Org mode 9.6)},
|
||||
pdflang={English}}
|
||||
\usepackage{biblatex}
|
||||
|
||||
@@ -22,20 +23,158 @@
|
||||
\tableofcontents
|
||||
|
||||
\clearpage
|
||||
|
||||
From last sections:
|
||||
\begin{itemize}
|
||||
\item Uniaxial: No stiff nano-hexapod (should also demonstrate that here)
|
||||
\item Rotating: No soft nano-hexapod, Decentralized IFF can be used robustly by adding parallel stiffness
|
||||
\end{itemize}
|
||||
|
||||
In this section:
|
||||
\begin{itemize}
|
||||
\item Take the model of the nano-hexapod with stiffness 1um/N
|
||||
\item Apply decentralized IFF
|
||||
\item Apply HAC-LAC
|
||||
\item Check robustness to payload change
|
||||
\item Simulation of experiments
|
||||
\end{itemize}
|
||||
|
||||
\begin{table}[htbp]
|
||||
\caption{\label{tab:simscape_nass_section_matlab_code}Report sections and corresponding Matlab files}
|
||||
\centering
|
||||
\begin{tabularx}{0.6\linewidth}{lX}
|
||||
\toprule
|
||||
\textbf{Sections} & \textbf{Matlab File}\\
|
||||
\midrule
|
||||
Section \ref{sec:simscape_nass_1_a} & \texttt{simscape\_nass\_1\_.m}\\
|
||||
Section \ref{sec:nass_1_a} & \texttt{nass\_1\_.m}\\
|
||||
\bottomrule
|
||||
\end{tabularx}
|
||||
\caption{\label{tab:nass_section_matlab_code}Report sections and corresponding Matlab files}
|
||||
|
||||
\end{table}
|
||||
\chapter{Section 1}
|
||||
\label{sec:simscape_nass_1_a}
|
||||
|
||||
\chapter{Control Kinematics}
|
||||
\label{sec:nass_kinematics}
|
||||
\begin{itemize}
|
||||
\item Explain how the position error can be expressed in the frame of the nano-hexapod
|
||||
\item[{$\square$}] \href{file:///home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning\_error.org}{positioning\_error}: Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors)
|
||||
\item Control architecture, block diagram
|
||||
|
||||
\item Schematic with micro-station + nass + metrology + control system
|
||||
\item Zoom in the control system with blocs
|
||||
\item Then explain all the blocs
|
||||
\item Say that there are many control strategies.
|
||||
It will be the topic of chapter 2.3.
|
||||
Here, we start with something simple: control in the frame of the struts
|
||||
\end{itemize}
|
||||
\section{Micro Station Kinematics}
|
||||
|
||||
\begin{itemize}
|
||||
\item from \ref{ssec:ustation_kinematics}, computation of the wanted sample pose from the setpoint of each stage.
|
||||
\end{itemize}
|
||||
|
||||
\section{Computation of the sample's pose error}
|
||||
|
||||
From metrology (here supposed to be perfect 6-DoF), compute the sample's pose error.
|
||||
Has to invert the homogeneous transformation.
|
||||
|
||||
\section{Position error in the frame of the nano-hexapod}
|
||||
|
||||
Explain how to compute the errors in the frame of the struts (rotating)
|
||||
|
||||
\chapter{Decentralized Active Damping}
|
||||
\label{sec:nass_active_damping}
|
||||
\begin{itemize}
|
||||
\item How to apply/optimize IFF on an hexapod? ()
|
||||
\item Robustness to payload mass
|
||||
\item Root Locus
|
||||
\item Damping optimization
|
||||
|
||||
\item\relax [ ]\href{file:///home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control\_active\_damping.org}{control\_active\_damping}
|
||||
\item\relax [ ]\href{file:///home/thomas/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org}{active damping for stewart platforms}
|
||||
\item\relax [ ]\href{file:///home/thomas/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/bibliography.org}{Vibration Control and Active Damping}
|
||||
\end{itemize}
|
||||
\section{IFF Plant}
|
||||
|
||||
\begin{itemize}
|
||||
\item Show how it changes with the payload mass (1, 25, 50)
|
||||
\item Effect of rotation (1rpm, 60rpm)
|
||||
\end{itemize}
|
||||
|
||||
\section{Controller Design}
|
||||
|
||||
\begin{itemize}
|
||||
\item Apply IFF
|
||||
\item Show Root Locus
|
||||
\item Choose optimal gain.
|
||||
Here in MIMO, cannot have optimal damping for all modes. (there is a paper that tries to optimize that)
|
||||
\item Show robustness to change of payload (loci?)
|
||||
\item Reference to paper showing stability in MIMO for decentralized IFF
|
||||
\end{itemize}
|
||||
|
||||
\section{Sensitivity to disturbances}
|
||||
|
||||
\begin{itemize}
|
||||
\item Compute transfer functions from spindle vertical error to sample vertical error with IFF (and compare without the NASS)
|
||||
\item Same for horizontal
|
||||
\item Maybe noise budgeting, but may be complex in MIMO\ldots{}
|
||||
\end{itemize}
|
||||
|
||||
\chapter{Centralized Active Vibration Control}
|
||||
\label{sec:nass_hac}
|
||||
\begin{itemize}
|
||||
\item[{$\square$}] \href{file:///home/thomas/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty\_experiment.org}{uncertainty\_experiment}: Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc\ldots{})
|
||||
\item Effect of micro-station compliance
|
||||
\item Effect of IFF
|
||||
\item Effect of payload mass
|
||||
\item Decoupled plant
|
||||
\item Controller design
|
||||
\end{itemize}
|
||||
|
||||
From control kinematics:
|
||||
\begin{itemize}
|
||||
\item Talk about issue of not estimating Rz from external metrology? (maybe could be nice to discuss that during the experiments!)
|
||||
\item Show what happens is Rz is not estimated (for instance supposed equaled to zero => increased coupling)
|
||||
\end{itemize}
|
||||
\section{HAC Plant}
|
||||
|
||||
\begin{itemize}
|
||||
\item Compute transfer function from u to dL (with IFF applied)
|
||||
\end{itemize}
|
||||
|
||||
\section{Effect of Payload mass}
|
||||
|
||||
\begin{itemize}
|
||||
\item Show effect of payload mass + rotation
|
||||
\end{itemize}
|
||||
|
||||
\section{Controller design}
|
||||
|
||||
\begin{itemize}
|
||||
\item Show robustness with Loci
|
||||
\end{itemize}
|
||||
|
||||
\section{Sensitivity to disturbances}
|
||||
|
||||
\begin{itemize}
|
||||
\item Compute transfer functions from spindle vertical error to sample vertical error with HAC-IFF
|
||||
Compare without the NASS, and with just IFF
|
||||
\item Same for horizontal
|
||||
\item Maybe noise budgeting, but may be complex in MIMO\ldots{}
|
||||
\end{itemize}
|
||||
|
||||
\section{Tomography experiment}
|
||||
|
||||
\begin{itemize}
|
||||
\item With HAC-IFF, perform tomography experiment, and compare with open-loop
|
||||
|
||||
\item Take into account disturbances, metrology sensor noise. Maybe say here that we don't take in account other noise sources as they will be optimized latter (detail design phase)
|
||||
\item Tomography + lateral scans (same as what was done in open loop \href{file:///home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org}{here})
|
||||
\item Validation of concept
|
||||
\end{itemize}
|
||||
|
||||
\chapter{Conclusion}
|
||||
\label{sec:simscape_nass_conclusion}
|
||||
\label{sec:nass_conclusion}
|
||||
|
||||
|
||||
\printbibliography[heading=bibintoc,title={Bibliography}]
|
||||
\end{document}
|
||||
|
||||
Reference in New Issue
Block a user