tdehaeze/phd-test-bench-id31
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add necessary files for Simscape model

Browse Source
This commit is contained in:
Thomas Dehaeze
2024-11-13 17:16:01 +01:00
parent 6aa8242d3a
commit c43cefe565
116 changed files with 1115152 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
matlab/src/computeJacobian.m Normal file
View File

@@ -0,0 +1,35 @@
function [stewart] = computeJacobian(stewart)
% computeJacobian -
%
% Syntax: [stewart] = computeJacobian(stewart)
%
% Inputs:
% - stewart - With at least the following fields:
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
% - actuators.K [6x1] - Total stiffness of the actuators
%
% Outputs:
% - stewart - With the 3 added field:
% - kinematics.J [6x6] - The Jacobian Matrix
% - kinematics.K [6x6] - The Stiffness Matrix
% - kinematics.C [6x6] - The Compliance Matrix
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
As = stewart.geometry.As;
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
Ab = stewart.geometry.Ab;
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
Ki = stewart.actuators.K;
J = [As' , cross(Ab, As)'];
K = J'*diag(Ki)*J;
C = inv(K);
stewart.kinematics.J = J;
stewart.kinematics.K = K;
stewart.kinematics.C = C;

78
matlab/src/computeJointsPose.m Normal file
View File

@@ -0,0 +1,78 @@
function [stewart] = computeJointsPose(stewart)
% computeJointsPose -
%
% Syntax: [stewart] = computeJointsPose(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
%
% Outputs:
% - stewart - A structure with the following added fields
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
% - geometry.l [6x1] - The i'th element is the initial length of strut i
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
Mb = stewart.platform_M.Mb;
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
FO_A = stewart.platform_F.FO_A;
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
MO_B = stewart.platform_M.MO_B;
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
FO_M = stewart.geometry.FO_M;
Aa = Fa - repmat(FO_A, [1, 6]);
Bb = Mb - repmat(MO_B, [1, 6]);
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
l = vecnorm(Ab - Aa)';
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
FRa = zeros(3,3,6);
MRb = zeros(3,3,6);
for i = 1:6
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
end
stewart.geometry.Aa = Aa;
stewart.geometry.Ab = Ab;
stewart.geometry.Ba = Ba;
stewart.geometry.Bb = Bb;
stewart.geometry.As = As;
stewart.geometry.Bs = Bs;
stewart.geometry.l = l;
stewart.struts_F.l = l/2;
stewart.struts_M.l = l/2;
stewart.platform_F.FRa = FRa;
stewart.platform_M.MRb = MRb;

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

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

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

@@ -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');

90
matlab/src/extractNodes.m Normal file
View File

@@ -0,0 +1,90 @@
function [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes(filename)
% extractNodes -
%
% Syntax: [n_xyz, nodes] = extractNodes(filename)
%
% Inputs:
% - filename - relative or absolute path of the file that contains the Matrix
%
% Outputs:
% - n_xyz -
% - nodes - table containing the node numbers and corresponding dof of the interfaced DoFs
arguments
filename
end
fid = fopen(filename,'rt');
if fid == -1
error('Error opening the file');
end
n_xyz = []; % Contains nodes coordinates
n_i = []; % Contains nodes indices
n_num = []; % Contains node numbers
n_dof = {}; % Contains node directions
while 1
% Read a line
nextline = fgetl(fid);
% End of the file
if ~isstr(nextline), break, end
% Line just before the list of nodes coordinates
if contains(nextline, 'NODE') && ...
contains(nextline, 'X') && ...
contains(nextline, 'Y') && ...
contains(nextline, 'Z')
while 1
nextline = fgetl(fid);
if nextline < 0, break, end
c = sscanf(nextline, ' %f');
if isempty(c), break, end
n_xyz = [n_xyz; c(2:4)'];
n_i = [n_i; c(1)];
end
end
if nextline < 0, break, end
% Line just before the list of node DOF
if contains(nextline, 'NODE LABEL')
while 1
nextline = fgetl(fid);
if nextline < 0, break, end
c = sscanf(nextline, ' %d %s');
if isempty(c), break, end
n_num = [n_num; c(1)];
n_dof{length(n_dof)+1} = char(c(2:end)');
end
nodes = table(n_num, string(n_dof'), 'VariableNames', {'node_i', 'node_dof'});
end
if nextline < 0, break, end
end
fclose(fid);
int_i = unique(nodes.('node_i')); % indices of interface nodes
% Extract XYZ coordinates of only the interface nodes
if length(n_xyz) > 0 && length(n_i) > 0
int_xyz = n_xyz(logical(sum(n_i.*ones(1, length(int_i)) == int_i', 2)), :);
else
int_xyz = n_xyz;
end

39
matlab/src/generateGeneralConfiguration.m Normal file
View File

@@ -0,0 +1,39 @@
function [stewart] = generateGeneralConfiguration(stewart, args)
% generateGeneralConfiguration - Generate a Very General Configuration
%
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
%
% Inputs:
% - args - Can have the following fields:
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
arguments
stewart
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
end
Fa = zeros(3,6);
Mb = zeros(3,6);
for i = 1:6
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
end
stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;

102
matlab/src/h5scan.m Normal file
View File

@@ -0,0 +1,102 @@
function [cntrs,tp] = h5scan(pth,smp,ds,sn,varargin)
i = cellfun(@(x) isa(x,'detector'),varargin);
if any(i), det = varargin{i}; varargin = varargin(~i); else, det = []; end;
if ~isstr(ds), ds = sprintf('%.4d',ds); end;
f = sprintf('%s/%s/%s_%s/%s_%s.h5',pth,smp,smp,ds,smp,ds);
h = h5info(f,sprintf('/%d.1/measurement',sn));
fid = H5F.open(f);
for i = 1:length(h.Links),
nm = h.Links(i).Name;
try,
id = H5D.open(fid,h.Links(i).Value{1});
cntrs.(nm) = H5D.read(id);
H5D.close(id);
if ~isempty(det) & strcmp(nm,det.name), cntrs.(nm) = integrate(det,double(cntrs.(nm))); end;
catch,
warning('solving problem with %s\n',nm);
cntrs.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
end;
[~,tp.(nm)] = fileparts(h.Links(i).Value{1});
end;
try,
h = h5info(f,sprintf('/%d.2/measurement',sn));
catch,
h = [];
end;
if ~isempty(h),
for i = 1:length(h.Links),
nm = h.Links(i).Name;
try,
id = H5D.open(fid,h.Links(i).Value{1});
cntrs.part2.(nm) = H5D.read(id);
H5D.close(id);
catch,
warning('solving problem with %s\n',nm);
cntrs.part2.(nm) = vrtlds(sprintf('%s/%s/%s_%s/scan%.4d/',pth,smp,smp,ds,sn),nm,det);
end;
[~,tp.part2.(nm)] = fileparts(h.Links(i).Value{1});
end;
end;
if length(varargin),
fn = sprintf('/%d.1/instrument/positioners/',sn);
h = h5info(f,fn);
[~,k,m] = intersect({h.Datasets.Name},varargin,'stable');
h.Datasets = h.Datasets(k);
for i = 1:length(h.Datasets),
id = H5D.open(fid,[fn h.Datasets(i).Name]);
cntrs.(h.Datasets(i).Name) = H5D.read(id);
H5D.close(id);
end;
end;
H5F.close(fid);
%%%%%%%%%%%%%%%%%%%%%%%%%%
function A = vrtlds(f,nm,det)
%try,
n = 0; A = [];
fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
while exist(fn) == 2,
fid = H5F.open(fn); n = n+1;
id = H5D.open(fid,sprintf('/entry_0000/ESRF-ID31/%s/data',nm));
if 2 < nargin & strcmp(nm,'p3') & ~isempty(det),
fprintf('integrating %s\n',fn);
if isempty(A),
A = integrate(det,double(H5D.read(id)),1);
else,
tmp = integrate(det,double(H5D.read(id)),1); A.y = cat(2,A.y,tmp.y); A.y0 = cat(2,A.y0,tmp.y0);
end;
else,
fprintf('loading %s\n',fn);
A = cat(3,A,H5D.read(id));
end;
H5D.close(id); H5F.close(fid);
fn = sprintf('%s/%s_%.4d.h5',f,nm,n);
end;
%catch,
% A = [];
%end;
% fid = H5F.open...
% id = H5D.open...
% sid = H5D.get_space(id);
% [ndims,h5_dims]=H5S.get_simple_extent_dims(sid)
% Read a 2x3 hyperslab of data from a dataset, starting in the 4th row and 5th column of the example dataset.
% Create a property list identifier, then open the HDF5 file and the dataset /g1/g1.1/dset1.1.1.
% fid = H5F.open('example.h5');
% id = H5D.open(fid,'/g1/g1.1/dset1.1.1');
% dims = ([500 1679 1475];
% msid = H5S.create_simple(3,dims,[]);
% sid = H5D.get_space(id);
% offset = [n*500 0 0];
% block = dims; % d1: 500 or min(d1tot-n*500,500)
% H5S.select_hyperslab(sid,'H5S_SELECT_SET',offset,[],[],block);
% data = H5D.read(id,'H5ML_DEFAULT',msid,sid,'H5P_DEFAULT');
% H5D.close(id);
% H5F.close(fid);

45
matlab/src/initializeController.m Normal file
View File

@@ -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

59
matlab/src/initializeCylindricalPlatforms.m Normal file
View File

@@ -0,0 +1,59 @@
function [stewart] = initializeCylindricalPlatforms(stewart, args)
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
%
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
%
% Inputs:
% - args - Structure with the following fields:
% - Fpm [1x1] - Fixed Platform Mass [kg]
% - Fph [1x1] - Fixed Platform Height [m]
% - Fpr [1x1] - Fixed Platform Radius [m]
% - Mpm [1x1] - Mobile Platform Mass [kg]
% - Mph [1x1] - Mobile Platform Height [m]
% - Mpr [1x1] - Mobile Platform Radius [m]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - platform_F [struct] - structure with the following fields:
% - type = 1
% - M [1x1] - Fixed Platform Mass [kg]
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
% - H [1x1] - Fixed Platform Height [m]
% - R [1x1] - Fixed Platform Radius [m]
% - platform_M [struct] - structure with the following fields:
% - M [1x1] - Mobile Platform Mass [kg]
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
% - H [1x1] - Mobile Platform Height [m]
% - R [1x1] - Mobile Platform Radius [m]
arguments
stewart
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
end
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/2 *args.Fpm * args.Fpr^2]);
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/2 *args.Mpm * args.Mpr^2]);
stewart.platform_F.type = 1;
stewart.platform_F.I = I_F;
stewart.platform_F.M = args.Fpm;
stewart.platform_F.R = args.Fpr;
stewart.platform_F.H = args.Fph;
stewart.platform_M.type = 1;
stewart.platform_M.I = I_M;
stewart.platform_M.M = args.Mpm;
stewart.platform_M.R = args.Mpr;
stewart.platform_M.H = args.Mph;

71
matlab/src/initializeCylindricalStruts.m Normal file
View File

@@ -0,0 +1,71 @@
function [stewart] = initializeCylindricalStruts(stewart, args)
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
%
% Syntax: [stewart] = initializeCylindricalStruts(args)
%
% Inputs:
% - args - Structure with the following fields:
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - struts_F [struct] - structure with the following fields:
% - M [6x1] - Mass of the Fixed part of the struts [kg]
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
% - struts_M [struct] - structure with the following fields:
% - M [6x1] - Mass of the Mobile part of the struts [kg]
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
arguments
stewart
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
end
Fsm = ones(6,1).*args.Fsm;
Fsh = ones(6,1).*args.Fsh;
Fsr = ones(6,1).*args.Fsr;
Msm = ones(6,1).*args.Msm;
Msh = ones(6,1).*args.Msh;
Msr = ones(6,1).*args.Msr;
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
for i = 1:6
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
1/2 * Fsm(i) * Fsr(i)^2]);
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
1/2 * Msm(i) * Msr(i)^2]);
end
stewart.struts_M.type = 1;
stewart.struts_M.I = I_M;
stewart.struts_M.M = Msm;
stewart.struts_M.R = Msr;
stewart.struts_M.H = Msh;
stewart.struts_F.type = 1;
stewart.struts_F.I = I_F;
stewart.struts_F.M = Fsm;
stewart.struts_F.R = Fsr;
stewart.struts_F.H = Fsh;

214
matlab/src/initializeDisturbances.m Normal file
View File

@@ -0,0 +1,214 @@
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
end

35
matlab/src/initializeFramesPositions.m Normal file
View File

@@ -0,0 +1,35 @@
function [stewart] = initializeFramesPositions(stewart, args)
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
%
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
%
% Inputs:
% - args - Can have the following fields:
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
%
% Outputs:
% - stewart - A structure with the following fields:
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
arguments
stewart
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
end
H = args.H; % Total Height of the Stewart Platform [m]
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
stewart.geometry.H = H;
stewart.geometry.FO_M = FO_M;
stewart.platform_M.MO_B = MO_B;
stewart.platform_F.FO_A = FO_A;

48
matlab/src/initializeGranite.m Normal file
View File

@@ -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

35
matlab/src/initializeGround.m Normal file
View File

@@ -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

48
matlab/src/initializeInertialSensor.m Normal file
View File

@@ -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;

131
matlab/src/initializeJointDynamics.m Normal file
View File

@@ -0,0 +1,131 @@
function [stewart] = initializeJointDynamics(stewart, args)
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
%
% Syntax: [stewart] = initializeJointDynamics(args)
%
% Inputs:
% - args - Structure with the following fields:
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - stewart.joints_F and stewart.joints_M:
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
arguments
stewart
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
args.K_M double {mustBeNumeric} = zeros(6,6)
args.M_M double {mustBeNumeric} = zeros(6,6)
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
args.xi_M double {mustBeNumeric} = 0.1
args.step_file_M char {} = ''
args.K_F double {mustBeNumeric} = zeros(6,6)
args.M_F double {mustBeNumeric} = zeros(6,6)
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
args.xi_F double {mustBeNumeric} = 0.1
args.step_file_F char {} = ''
end
switch args.type_F
case 'universal'
stewart.joints_F.type = 1;
case 'spherical'
stewart.joints_F.type = 2;
case 'universal_p'
stewart.joints_F.type = 3;
case 'spherical_p'
stewart.joints_F.type = 4;
case 'flexible'
stewart.joints_F.type = 5;
case 'universal_3dof'
stewart.joints_F.type = 6;
case 'spherical_3dof'
stewart.joints_F.type = 7;
end
switch args.type_M
case 'universal'
stewart.joints_M.type = 1;
case 'spherical'
stewart.joints_M.type = 2;
case 'universal_p'
stewart.joints_M.type = 3;
case 'spherical_p'
stewart.joints_M.type = 4;
case 'flexible'
stewart.joints_M.type = 5;
case 'universal_3dof'
stewart.joints_M.type = 6;
case 'spherical_3dof'
stewart.joints_M.type = 7;
end
stewart.joints_M.Ka = args.Ka_M;
stewart.joints_M.Kr = args.Kr_M;
stewart.joints_F.Ka = args.Ka_F;
stewart.joints_F.Kr = args.Kr_F;
stewart.joints_M.Ca = args.Ca_M;
stewart.joints_M.Cr = args.Cr_M;
stewart.joints_F.Ca = args.Ca_F;
stewart.joints_F.Cr = args.Cr_F;
stewart.joints_M.Kf = args.Kf_M;
stewart.joints_M.Kt = args.Kt_M;
stewart.joints_F.Kf = args.Kf_F;
stewart.joints_F.Kt = args.Kt_F;
stewart.joints_M.Cf = args.Cf_M;
stewart.joints_M.Ct = args.Ct_M;
stewart.joints_F.Cf = args.Cf_F;
stewart.joints_F.Ct = args.Ct_F;
stewart.joints_F.M = args.M_F;
stewart.joints_F.K = args.K_F;
stewart.joints_F.n_xyz = args.n_xyz_F;
stewart.joints_F.xi = args.xi_F;
stewart.joints_F.xi = args.xi_F;
stewart.joints_F.step_file = args.step_file_F;
stewart.joints_M.M = args.M_M;
stewart.joints_M.K = args.K_M;
stewart.joints_M.n_xyz = args.n_xyz_M;
stewart.joints_M.xi = args.xi_M;
stewart.joints_M.step_file = args.step_file_M;

35
matlab/src/initializeLoggingConfiguration.m Normal file
View File

@@ -0,0 +1,35 @@
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
end

108
matlab/src/initializeMicroHexapod.m Normal file
View File

@@ -0,0 +1,108 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
% generateGeneralConfiguration
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
% initializeStrutDynamics
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
% initializeCylindricalPlatforms
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
% initializeCylindricalStruts
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, ...
'H', args.H, ...
'MO_B', args.MO_B);
stewart = generateGeneralConfiguration(stewart, ...
'FH', args.FH, ...
'FR', args.FR, ...
'FTh', args.FTh, ...
'MH', args.MH, ...
'MR', args.MR, ...
'MTh', args.MTh);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, ...
'K', args.Ki, ...
'C', args.Ci);
stewart = initializeJointDynamics(stewart, ...
'type_F', 'universal_p', ...
'type_M', 'spherical_p');
stewart = initializeCylindricalPlatforms(stewart, ...
'Fpm', args.Fpm, ...
'Fph', args.Fph, ...
'Fpr', args.Fpr, ...
'Mpm', args.Mpm, ...
'Mph', args.Mph, ...
'Mpr', args.Mpr);
stewart = initializeCylindricalStruts(stewart, ...
'Fsm', args.Fsm, ...
'Fsh', args.Fsh, ...
'Fsr', args.Fsr, ...
'Msm', args.Msm, ...
'Msh', args.Msh, ...
'Msr', args.Msr);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart, ...
'AP', args.AP, ...
'ARB', args.ARB);
stewart = initializeInertialSensor(stewart, 'type', 'none');
switch args.type
case 'none'
stewart.type = 0;
case 'rigid'
stewart.type = 1;
case 'flexible'
stewart.type = 2;
end
micro_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nass_model_stages.mat', 'file')
save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
else
save('mat/nass_model_stages.mat', 'micro_hexapod');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nass_model_stages.mat', 'file')
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
else
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
end
end
end

305
matlab/src/initializeNanoHexapod.m Normal file
View File

@@ -0,0 +1,305 @@
function [nano_hexapod] = initializeNanoHexapod(args)
arguments
%% Bottom Flexible Joints
args.flex_bot_type char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
args.flex_bot_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
args.flex_bot_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
%% Top Flexible Joints
args.flex_top_type char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
args.flex_top_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
args.flex_top_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
%% Jacobian - Location of frame {A} and {B}
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
%% Relative Motion Sensor
args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
%% Top Plate
args.top_plate_type char {mustBeMember(args.top_plate_type,{'rigid', 'flexible'})} = 'rigid'
args.top_plate_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
%% Actuators
args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
args.actuator_Ga (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
args.actuator_Gs (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
% For 2DoF
args.actuator_k (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*380000
args.actuator_ke (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*4952605
args.actuator_ka (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*2476302
args.actuator_c (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*5
args.actuator_ce (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*100
args.actuator_ca (6,1) double {mustBeNumeric, mustBePositive} = ones(6,1)*50
args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
% For Flexible Frame
args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
% Misalignment
args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
args.actuator_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
%% Controller
args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
end
nano_hexapod = struct();
nano_hexapod.flex_bot = struct();
switch args.flex_bot_type
case '2dof'
nano_hexapod.flex_bot.type = 1;
case '3dof'
nano_hexapod.flex_bot.type = 2;
case '4dof'
nano_hexapod.flex_bot.type = 3;
case 'flexible'
nano_hexapod.flex_bot.type = 4;
end
nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
nano_hexapod.flex_bot.kz = args.flex_bot_kz; % Axial stiffness [N/m]
nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cz = args.flex_bot_cz; %[N/(m/s)]
nano_hexapod.flex_top = struct();
switch args.flex_top_type
case '2dof'
nano_hexapod.flex_top.type = 1;
case '3dof'
nano_hexapod.flex_top.type = 2;
case '4dof'
nano_hexapod.flex_top.type = 3;
case 'flexible'
nano_hexapod.flex_top.type = 4;
end
nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
nano_hexapod.flex_top.kz = args.flex_top_kz; % Axial stiffness [N/m]
nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
nano_hexapod.flex_top.cz = args.flex_top_cz; %[N/(m/s)]
nano_hexapod.motion_sensor = struct();
switch args.motion_sensor_type
case 'struts'
nano_hexapod.motion_sensor.type = 1;
case 'plates'
nano_hexapod.motion_sensor.type = 2;
end
nano_hexapod.actuator = struct();
switch args.actuator_type
case '2dof'
nano_hexapod.actuator.type = 1;
case 'flexible frame'
nano_hexapod.actuator.type = 2;
case 'flexible'
nano_hexapod.actuator.type = 3;
end
%% Actuator gain [N/V]
if all(args.actuator_Ga == 0)
switch args.actuator_type
case '2dof'
nano_hexapod.actuator.Ga = ones(6,1)*(-2.5796);
case 'flexible frame'
nano_hexapod.actuator.Ga = ones(6,1); % TODO
case 'flexible'
nano_hexapod.actuator.Ga = ones(6,1)*23.2;
end
else
nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
end
%% Sensor gain [V/m]
if all(args.actuator_Gs == 0)
switch args.actuator_type
case '2dof'
nano_hexapod.actuator.Gs = ones(6,1)*466664;
case 'flexible frame'
nano_hexapod.actuator.Gs = ones(6,1); % TODO
case 'flexible'
nano_hexapod.actuator.Gs = ones(6,1)*(-4898341);
end
else
nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
end
switch args.actuator_type
case '2dof'
nano_hexapod.actuator.k = args.actuator_k; % [N/m]
nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
nano_hexapod.actuator.c = args.actuator_c; % [N/(m/s)]
nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
case 'flexible frame'
nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
case 'flexible'
nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
end
nano_hexapod.geometry = struct();
Fa = [[-86.05, -74.78, 22.49],
[ 86.05, -74.78, 22.49],
[ 107.79, -37.13, 22.49],
[ 21.74, 111.91, 22.49],
[-21.74, 111.91, 22.49],
[-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
Mb = [[-28.47, -106.25, -22.50],
[ 28.47, -106.25, -22.50],
[ 106.25, 28.47, -22.50],
[ 77.78, 77.78, -22.50],
[-77.78, 77.78, -22.50],
[-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
si = Fb - Fa;
si = si./vecnorm(si); % Normalize
Fc = [[-29.362, -105.765, 52.605]
[ 29.362, -105.765, 52.605]
[ 106.276, 27.454, 52.605]
[ 76.914, 78.31, 52.605]
[-76.914, 78.31, 52.605]
[-106.276, 27.454, 52.605]]'*1e-3; % Meas pos w.r.t. {F}
Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
nano_hexapod.geometry.Fa = Fa;
nano_hexapod.geometry.Fb = Fb;
nano_hexapod.geometry.Fc = Fc;
nano_hexapod.geometry.Mb = Mb;
nano_hexapod.geometry.Mc = Mc;
nano_hexapod.geometry.si = si;
nano_hexapod.geometry.MO_B = args.MO_B;
Bb = Mb - [0; 0; args.MO_B];
nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
switch args.motion_sensor_type
case 'struts'
nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
case 'plates'
Bc = Mc - [0; 0; args.MO_B];
nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
end
nano_hexapod.top_plate = struct();
switch args.top_plate_type
case 'rigid'
nano_hexapod.top_plate.type = 1;
case 'flexible'
nano_hexapod.top_plate.type = 2;
nano_hexapod.top_plate.R_flex = ...
{[ 0.53191886726305 0.4795690716524 0.69790817745892
-0.29070157897799 0.8775041341865 -0.38141720787774
-0.79533320729697 0 0.60617249143351 ],
[ 0.53191886726305 -0.4795690716524 -0.69790817745892
0.29070157897799 0.8775041341865 -0.38141720787774
0.79533320729697 0 0.60617249143351 ],
[-0.01420448131633 -0.9997254079576 -0.01863709726680
0.60600604129104 -0.0234330681729 0.79511481512719
-0.79533320729697 0 0.60617249143351 ],
[-0.51771438594672 -0.5201563363051 0.67927108019212
0.31530446231304 -0.8540710660135 -0.41369760724945
0.79533320729697 0 0.60617249143351 ],
[-0.51771438594671 0.5201563363052 -0.67927108019211
-0.31530446231304 -0.8540710660135 -0.41369760724945
-0.79533320729697 0 0.60617249143351 ],
[-0.01420448131632 0.9997254079576 0.01863709726679
-0.60600604129104 -0.0234330681729 0.79511481512719
0.79533320729697 0 0.60617249143351 ] };
nano_hexapod.top_plate.R_enc = ...
{ [-0.877504134186525 -0.479569071652412 0
0.479569071652412 -0.877504134186525 0
0 0 1 ],
[ 0.877504134186525 -0.479569071652413 0
0.479569071652413 0.877504134186525 0
0 0 1 ],
[ 0.023433068172945 0.999725407957606 0
-0.999725407957606 0.023433068172945 0
0 0 1 ],
[-0.854071066013566 -0.520156336305202 0
0.520156336305202 -0.854071066013566 0
0 0 1 ],
[ 0.854071066013574 -0.520156336305191 0
0.520156336305191 0.854071066013574 0
0 0 1 ],
[-0.023433068172958 0.999725407957606 0
-0.999725407957606 -0.023433068172958 0
0 0 1 ] };
nano_hexapod.top_plate.K = readmatrix('top_plate_K_6.CSV'); % Stiffness Matrix
nano_hexapod.top_plate.M = readmatrix('top_plate_M_6.CSV'); % Mass Matrix
nano_hexapod.top_plate.P = extractNodes('top_plate_out_nodes_3D_qua.txt'); % Node coordiantes [m]
nano_hexapod.top_plate.xi = args.top_plate_xi; % Damping ratio
end
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

201
matlab/src/initializeReferences.m Normal file
View File

@@ -0,0 +1,201 @@
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
end

57
matlab/src/initializeRy.m Normal file
View File

@@ -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

47
matlab/src/initializeRz.m Normal file
View File

@@ -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

34
matlab/src/initializeSample.m Normal file
View File

@@ -0,0 +1,34 @@
function [sample] = initializeSample(args)
arguments
args.type char {mustBeMember(args.type,{'0', '1', '2', '3'})} = '0'
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

29
matlab/src/initializeSimscapeConfiguration.m Normal file
View File

@@ -0,0 +1,29 @@
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
end

31
matlab/src/initializeStewartPlatform.m Normal file
View File

@@ -0,0 +1,31 @@
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();

27
matlab/src/initializeStewartPose.m Normal file
View File

@@ -0,0 +1,27 @@
function [stewart] = initializeStewartPose(stewart, args)
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
% It uses the inverse kinematic
%
% Syntax: [stewart] = initializeStewartPose(stewart, args)
%
% Inputs:
% - stewart - A structure with the following fields
% - Aa [3x6] - The positions ai expressed in {A}
% - Bb [3x6] - The positions bi expressed in {B}
% - args - Can have the following fields:
% - AP [3x1] - The wanted position of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
stewart.actuators.Leq = dLi;

49
matlab/src/initializeStrutDynamics.m Normal file
View File

@@ -0,0 +1,49 @@
function [stewart] = initializeStrutDynamics(stewart, args)
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
%
% Syntax: [stewart] = initializeStrutDynamics(args)
%
% Inputs:
% - args - Structure with the following fields:
% - K [6x1] - Stiffness of each strut [N/m]
% - C [6x1] - Damping of each strut [N/(m/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - actuators.type = 1
% - actuators.K [6x1] - Stiffness of each strut [N/m]
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
arguments
stewart
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
end
if strcmp(args.type, 'classical')
stewart.actuators.type = 1;
elseif strcmp(args.type, 'amplified')
stewart.actuators.type = 2;
end
stewart.actuators.K = args.K;
stewart.actuators.C = args.C;
stewart.actuators.k1 = args.k1;
stewart.actuators.c1 = args.c1;
stewart.actuators.ka = args.ka;
stewart.actuators.ke = args.ke;
stewart.actuators.F_gain = args.F_gain;
stewart.actuators.ma = args.ma;
stewart.actuators.me = args.me;

71
matlab/src/initializeTy.m Normal file
View File

@@ -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

36
matlab/src/inverseKinematics.m Normal file
View File

@@ -0,0 +1,36 @@
function [Li, dLi] = inverseKinematics(stewart, args)
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
%
% Syntax: [stewart] = inverseKinematics(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - geometry.Aa [3x6] - The positions ai expressed in {A}
% - geometry.Bb [3x6] - The positions bi expressed in {B}
% - geometry.l [6x1] - Length of each strut
% - args - Can have the following fields:
% - AP [3x1] - The wanted position of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
Aa = stewart.geometry.Aa;
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
Bb = stewart.geometry.Bb;
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
l = stewart.geometry.l;
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
dLi = Li-l;

31
matlab/src/sphereFit.m Normal file
View File

@@ -0,0 +1,31 @@
function [Center,Radius] = sphereFit(X)
% this fits a sphere to a collection of data using a closed form for the
% solution (opposed to using an array the size of the data set).
% Minimizes Sum((x-xc)^2+(y-yc)^2+(z-zc)^2-r^2)^2
% x,y,z are the data, xc,yc,zc are the sphere's center, and r is the radius
% Assumes that points are not in a singular configuration, real numbers, ...
% if you have coplanar data, use a circle fit with svd for determining the
% plane, recommended Circle Fit (Pratt method), by Nikolai Chernov
% http://www.mathworks.com/matlabcentral/fileexchange/22643
% Input:
% X: n x 3 matrix of cartesian data
% Outputs:
% Center: Center of sphere
% Radius: Radius of sphere
% Author:
% Alan Jennings, University of Dayton
A=[mean(X(:,1).*(X(:,1)-mean(X(:,1)))), ...
2*mean(X(:,1).*(X(:,2)-mean(X(:,2)))), ...
2*mean(X(:,1).*(X(:,3)-mean(X(:,3)))); ...
0, ...
mean(X(:,2).*(X(:,2)-mean(X(:,2)))), ...
2*mean(X(:,2).*(X(:,3)-mean(X(:,3)))); ...
0, ...
0, ...
mean(X(:,3).*(X(:,3)-mean(X(:,3))))];
A=A+A.';
B=[mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,1)-mean(X(:,1))));...
mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,2)-mean(X(:,2))));...
mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,3)-mean(X(:,3))))];
Center=(A\B).';
Radius=sqrt(mean(sum([X(:,1)-Center(1),X(:,2)-Center(2),X(:,3)-Center(3)].^2,2)));