add all files
This commit is contained in:
90
C4-test-bench-nano-hexapod/src/extractNodes.m
Normal file
90
C4-test-bench-nano-hexapod/src/extractNodes.m
Normal 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
|
||||
290
C4-test-bench-nano-hexapod/src/initializeNanoHexapod.m
Normal file
290
C4-test-bench-nano-hexapod/src/initializeNanoHexapod.m
Normal file
@@ -0,0 +1,290 @@
|
||||
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
|
||||
Reference in New Issue
Block a user