From b82ad45fad5783f2fb05392fa8697611e2c6bc50 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Tue, 29 Oct 2024 15:52:15 +0100 Subject: [PATCH] Only keep necessary Matlab scripts --- matlab/src/computeJacobian.m | 35 -- matlab/src/computeJointsPose.m | 78 --- matlab/src/computeReferencePose.m | 77 --- matlab/src/computeSampleError.m | 20 - matlab/src/describeNassSetup.m | 196 ------- matlab/src/describeStewartPlatform.m | 83 --- matlab/src/displayArchitecture.m | 240 -------- matlab/src/extractNodes.m | 2 +- matlab/src/forwardKinematicsApprox.m | 34 -- matlab/src/generateCubicConfiguration.m | 50 -- matlab/src/generateDiagPidControl.m | 18 - matlab/src/generateGeneralConfiguration.m | 39 -- matlab/src/generateShapedNoise.m | 53 -- matlab/src/generateSinIncreasingAmpl.m | 54 -- matlab/src/generateSpiralAngleTrajectory.m | 35 -- matlab/src/generateSweepExc.m | 76 --- matlab/src/generateXYZTrajectory.m | 48 -- matlab/src/generateYZScanTrajectory.m | 83 --- matlab/src/getJacobianNanoHexapod.m | 34 -- matlab/src/getTransformationMatrixAcc.m | 23 - matlab/src/initializeAmplifiedStrutDynamics.m | 43 -- matlab/src/initializeAxisc.m | 34 -- matlab/src/initializeCedratPiezo.m | 17 - matlab/src/initializeController.m | 39 -- matlab/src/initializeCylindricalPlatforms.m | 59 -- matlab/src/initializeCylindricalStruts.m | 71 --- matlab/src/initializeDisturbances.m | 135 ----- matlab/src/initializeFramesPositions.m | 35 -- matlab/src/initializeGranite.m | 45 -- matlab/src/initializeGround.m | 22 - matlab/src/initializeInertialSensor.m | 48 -- matlab/src/initializeJointDynamics.m | 131 ----- matlab/src/initializeLoggingConfiguration.m | 21 - matlab/src/initializeMicroHexapod.m | 110 ---- matlab/src/initializeMicroHexapodOld.m | 196 ------- matlab/src/initializeMirror.m | 76 --- matlab/src/initializeNanoHexapod.m | 412 +++++++++----- matlab/src/initializeNanoHexapodFinal.m | 305 ---------- matlab/src/initializePosError.m | 28 - matlab/src/initializeReferences.m | 230 -------- matlab/src/initializeRy.m | 54 -- matlab/src/initializeRz.m | 45 -- matlab/src/initializeSample.m | 51 -- matlab/src/initializeSimscapeConfiguration.m | 15 - matlab/src/initializeStewartPlatform.m | 31 -- matlab/src/initializeStewartPose.m | 27 - matlab/src/initializeStrutDynamics.m | 49 -- matlab/src/initializeTy.m | 69 --- matlab/src/initializeZAxisAccelerometer.m | 21 - matlab/src/initializeZAxisGeophone.m | 18 - matlab/src/inverseKinematics.m | 36 -- matlab/src/prepareLinearizeIdentification.m | 27 - matlab/src/prepareTomographyExperiment.m | 29 - matlab/src/project_shutdown.m | 5 - matlab/src/project_startup.m | 18 - matlab/test_nhexa_1_suspended_table.m | 2 +- matlab/test_nhexa_3_model.m | 117 ++-- ...no_hexapod.slx => test_nhexa_simscape.slx} | Bin test-bench-nano-hexapod.org | 527 +++++++++++++++--- 59 files changed, 807 insertions(+), 3669 deletions(-) delete mode 100644 matlab/src/computeJacobian.m delete mode 100644 matlab/src/computeJointsPose.m delete mode 100644 matlab/src/computeReferencePose.m delete mode 100644 matlab/src/computeSampleError.m delete mode 100644 matlab/src/describeNassSetup.m delete mode 100644 matlab/src/describeStewartPlatform.m delete mode 100644 matlab/src/displayArchitecture.m delete mode 100644 matlab/src/forwardKinematicsApprox.m delete mode 100644 matlab/src/generateCubicConfiguration.m delete mode 100644 matlab/src/generateDiagPidControl.m delete mode 100644 matlab/src/generateGeneralConfiguration.m delete mode 100644 matlab/src/generateShapedNoise.m delete mode 100644 matlab/src/generateSinIncreasingAmpl.m delete mode 100644 matlab/src/generateSpiralAngleTrajectory.m delete mode 100644 matlab/src/generateSweepExc.m delete mode 100644 matlab/src/generateXYZTrajectory.m delete mode 100644 matlab/src/generateYZScanTrajectory.m delete mode 100644 matlab/src/getJacobianNanoHexapod.m delete mode 100644 matlab/src/getTransformationMatrixAcc.m delete mode 100644 matlab/src/initializeAmplifiedStrutDynamics.m delete mode 100644 matlab/src/initializeAxisc.m delete mode 100644 matlab/src/initializeCedratPiezo.m delete mode 100644 matlab/src/initializeController.m delete mode 100644 matlab/src/initializeCylindricalPlatforms.m delete mode 100644 matlab/src/initializeCylindricalStruts.m delete mode 100644 matlab/src/initializeDisturbances.m delete mode 100644 matlab/src/initializeFramesPositions.m delete mode 100644 matlab/src/initializeGranite.m delete mode 100644 matlab/src/initializeGround.m delete mode 100644 matlab/src/initializeInertialSensor.m delete mode 100644 matlab/src/initializeJointDynamics.m delete mode 100644 matlab/src/initializeLoggingConfiguration.m delete mode 100644 matlab/src/initializeMicroHexapod.m delete mode 100644 matlab/src/initializeMicroHexapodOld.m delete mode 100644 matlab/src/initializeMirror.m delete mode 100644 matlab/src/initializeNanoHexapodFinal.m delete mode 100644 matlab/src/initializePosError.m delete mode 100644 matlab/src/initializeReferences.m delete mode 100644 matlab/src/initializeRy.m delete mode 100644 matlab/src/initializeRz.m delete mode 100644 matlab/src/initializeSample.m delete mode 100644 matlab/src/initializeSimscapeConfiguration.m delete mode 100644 matlab/src/initializeStewartPlatform.m delete mode 100644 matlab/src/initializeStewartPose.m delete mode 100644 matlab/src/initializeStrutDynamics.m delete mode 100644 matlab/src/initializeTy.m delete mode 100644 matlab/src/initializeZAxisAccelerometer.m delete mode 100644 matlab/src/initializeZAxisGeophone.m delete mode 100644 matlab/src/inverseKinematics.m delete mode 100644 matlab/src/prepareLinearizeIdentification.m delete mode 100644 matlab/src/prepareTomographyExperiment.m delete mode 100644 matlab/src/project_shutdown.m delete mode 100644 matlab/src/project_startup.m rename matlab/{test_bench_nano_hexapod.slx => test_nhexa_simscape.slx} (100%) diff --git a/matlab/src/computeJacobian.m b/matlab/src/computeJacobian.m deleted file mode 100644 index e4a37a8..0000000 --- a/matlab/src/computeJacobian.m +++ /dev/null @@ -1,35 +0,0 @@ -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; diff --git a/matlab/src/computeJointsPose.m b/matlab/src/computeJointsPose.m deleted file mode 100644 index 3e31760..0000000 --- a/matlab/src/computeJointsPose.m +++ /dev/null @@ -1,78 +0,0 @@ -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; diff --git a/matlab/src/computeReferencePose.m b/matlab/src/computeReferencePose.m deleted file mode 100644 index ea8de43..0000000 --- a/matlab/src/computeReferencePose.m +++ /dev/null @@ -1,77 +0,0 @@ -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 diff --git a/matlab/src/computeSampleError.m b/matlab/src/computeSampleError.m deleted file mode 100644 index 5c42696..0000000 --- a/matlab/src/computeSampleError.m +++ /dev/null @@ -1,20 +0,0 @@ -function [MTr] = computeSampleError(WTm, WTr) -% computeSampleError - -% -% Syntax: [MTr] = computeSampleError(WTm, WTr) -% -% Inputs: -% - WTm - Homoegeneous transformation that represent the -% wanted pose of the sample with respect to the granite -% - WTr - Homoegeneous transformation that represent the -% measured pose of the sample with respect to the granite -% -% Outputs: -% - MTr - Homoegeneous transformation that represent the -% wanted pose of the sample expressed in a frame -% attached to the top platform of the nano-hexapod - -MTr = zeros(4,4); - -MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; -end diff --git a/matlab/src/describeNassSetup.m b/matlab/src/describeNassSetup.m deleted file mode 100644 index a5f9b26..0000000 --- a/matlab/src/describeNassSetup.m +++ /dev/null @@ -1,196 +0,0 @@ -function [] = describeNassSetup() -% describeNassSetup - -% -% Syntax: [] = describeNassSetup() -% -% Inputs: -% - - -% -% Outputs: -% - - - -load('./mat/conf_simscape.mat', 'conf_simscape'); - -fprintf('Simscape Configuration:\n'); - -if conf_simscape.type == 1 - fprintf('- Gravity is included\n'); -else - fprintf('- Gravity is not included\n'); -end - -fprintf('\n'); - -load('./mat/nass_disturbances.mat', 'args'); - -fprintf('Disturbances:\n'); -if ~args.enable - fprintf('- No disturbance is included\n'); -else - if args.Dwx && args.Dwy && args.Dwz - fprintf('- Ground motion\n'); - end - if args.Fty_x && args.Fty_z - fprintf('- Vibrations of the Translation Stage\n'); - end - if args.Frz_z - fprintf('- Vibrations of the Spindle\n'); - end -end -fprintf('\n'); - -load('./mat/nass_references.mat', 'args'); - -fprintf('Reference Tracking:\n'); -fprintf('- Translation Stage:\n'); -switch args.Dy_type - case 'constant' - fprintf(' - Constant Position\n'); - fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3); - case 'triangular' - fprintf(' - Triangular Path\n'); - fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3); - fprintf(' - Period = %.0f [s]\n', args.Dy_period); - case 'sinusoidal' - fprintf(' - Sinusoidal Path\n'); - fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3); - fprintf(' - Period = %.0f [s]\n', args.Dy_period); -end - -fprintf('- Tilt Stage:\n'); -switch args.Ry_type - case 'constant' - fprintf(' - Constant Position\n'); - fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3); - case 'triangular' - fprintf(' - Triangular Path\n'); - fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3); - fprintf(' - Period = %.0f [s]\n', args.Ry_period); - case 'sinusoidal' - fprintf(' - Sinusoidal Path\n'); - fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3); - fprintf(' - Period = %.0f [s]\n', args.Ry_period); -end - -fprintf('- Spindle:\n'); -switch args.Rz_type - case 'constant' - fprintf(' - Constant Position\n'); - fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude); - case { 'rotating', 'rotating-not-filtered' } - fprintf(' - Rotating\n'); - fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period); -end - - -fprintf('- Micro Hexapod:\n'); -switch args.Dh_type - case 'constant' - fprintf(' - Constant Position\n'); - fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3)); - fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6)); -end - -fprintf('\n'); - -load('./mat/controller.mat', 'controller'); - -fprintf('Controller:\n'); -fprintf('- %s\n', controller.name); -fprintf('\n'); - -load('./mat/stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc'); - -fprintf('Micro Station:\n'); - -if granite.type == 1 && ... - ty.type == 1 && ... - ry.type == 1 && ... - rz.type == 1 && ... - micro_hexapod.type == 1; - fprintf('- All stages are rigid\n'); -elseif granite.type == 2 && ... - ty.type == 2 && ... - ry.type == 2 && ... - rz.type == 2 && ... - micro_hexapod.type == 2; - fprintf('- All stages are flexible\n'); -else - if granite.type == 1 || granite.type == 4 - fprintf('- Granite is rigid\n'); - else - fprintf('- Granite is flexible\n'); - end - if ty.type == 1 || ty.type == 4 - fprintf('- Translation Stage is rigid\n'); - else - fprintf('- Translation Stage is flexible\n'); - end - if ry.type == 1 || ry.type == 4 - fprintf('- Tilt Stage is rigid\n'); - else - fprintf('- Tilt Stage is flexible\n'); - end - if rz.type == 1 || rz.type == 4 - fprintf('- Spindle is rigid\n'); - else - fprintf('- Spindle is flexible\n'); - end - if micro_hexapod.type == 1 || micro_hexapod.type == 4 - fprintf('- Micro Hexapod is rigid\n'); - else - fprintf('- Micro Hexapod is flexible\n'); - end - -end - -fprintf('\n'); - -load('./mat/stages.mat', 'mirror'); - -fprintf('Reference Mirror:\n'); - -if mirror.type == 2; - fprintf('- flexible fixation\n'); - fprintf('- w = %.0f [Hz]\n', mirror.freq(1)); -else - fprintf('- rigidly attached to the nano-hexapod\n'); -end -fprintf('- m = %.0f [kg]\n', mirror.mass); -fprintf('\n'); - -load('./mat/stages.mat', 'nano_hexapod'); - -fprintf('Nano Hexapod:\n'); - -if nano_hexapod.type == 0; - fprintf('- no included\n'); -elseif nano_hexapod.type == 1 || nano_hexapod.type == 3; - fprintf('- rigid\n'); -elseif nano_hexapod.type == 2; - fprintf('- flexible\n'); - fprintf('- Ki = %.0g [N/m]\n', nano_hexapod.actuators.K(1)); -end - -fprintf('\n'); - -load('./mat/stages.mat', 'sample'); - -fprintf('Sample:\n'); - -if sample.type == 0; - fprintf('- no included\n'); -elseif sample.type == 1 || sample.type == 3; - fprintf('- rigid\n'); - fprintf('- mass = %.0f [kg]\n', sample.mass); - fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3)); -elseif sample.type == 2; - fprintf('- flexible\n'); - fprintf('- mass = %.0f [kg]\n', sample.mass); - fprintf('- moment of inertia = %.2f, %.2f, %.2f [kg m2]\n', sample.inertia(1), sample.inertia(2), sample.inertia(3)); - % fprintf('- Kt = %.0g, %.0g, %.0g [N/m]\n', sample.K(1), sample.K(2), sample.K(3)); - % fprintf('- Kr = %.0g, %.0g, %.0g [Nm/rad]\n', sample.K(4), sample.K(5), sample.K(6)); - fprintf('- wt(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass), 1/2/pi*sqrt(sample.K(1)/sample.mass)); - fprintf('- wr(x,y,z) = %.0f, %.0f, %.0f [Hz]\n', 1/2/pi*sqrt(sample.K(4)/sample.inertia(1)), 1/2/pi*sqrt(sample.K(5)/sample.inertia(2)), 1/2/pi*sqrt(sample.K(6)/sample.inertia(3))); -end -fprintf('\n'); diff --git a/matlab/src/describeStewartPlatform.m b/matlab/src/describeStewartPlatform.m deleted file mode 100644 index 256fc60..0000000 --- a/matlab/src/describeStewartPlatform.m +++ /dev/null @@ -1,83 +0,0 @@ -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 classical.\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)) -elseif stewart.actuators.type == 2 - fprintf('- The actuators are mechanicaly amplified.\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)) -end -fprintf('\n') - -fprintf('JOINTS:\n') - -switch stewart.joints_F.type - case 1 - fprintf('- The joints on the fixed based are universal joints\n') - case 2 - fprintf('- The joints on the fixed based are spherical joints\n') - case 3 - fprintf('- The joints on the fixed based are perfect universal joints\n') - case 4 - fprintf('- The joints on the fixed based are perfect spherical joints\n') -end - -switch stewart.joints_M.type - case 1 - fprintf('- The joints on the mobile based are universal joints\n') - case 2 - fprintf('- The joints on the mobile based are spherical joints\n') - case 3 - fprintf('- The joints on the mobile based are perfect universal joints\n') - case 4 - fprintf('- The joints on the mobile based are perfect spherical joints\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 diff --git a/matlab/src/displayArchitecture.m b/matlab/src/displayArchitecture.m deleted file mode 100644 index 2513217..0000000 --- a/matlab/src/displayArchitecture.m +++ /dev/null @@ -1,240 +0,0 @@ -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 diff --git a/matlab/src/extractNodes.m b/matlab/src/extractNodes.m index 8b0a09f..5c7e679 100644 --- a/matlab/src/extractNodes.m +++ b/matlab/src/extractNodes.m @@ -87,4 +87,4 @@ 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 \ No newline at end of file +end diff --git a/matlab/src/forwardKinematicsApprox.m b/matlab/src/forwardKinematicsApprox.m deleted file mode 100644 index a6f3c23..0000000 --- a/matlab/src/forwardKinematicsApprox.m +++ /dev/null @@ -1,34 +0,0 @@ -function [P, R] = forwardKinematicsApprox(stewart, args) -% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using -% the Jacobian Matrix -% -% Syntax: [P, R] = forwardKinematicsApprox(stewart, args) -% -% Inputs: -% - stewart - A structure with the following fields -% - kinematics.J [6x6] - The Jacobian Matrix -% - args - Can have the following fields: -% - dL [6x1] - Displacement of each strut [m] -% -% Outputs: -% - P [3x1] - The estimated position of {B} with respect to {A} -% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A} - -arguments - stewart - args.dL (6,1) double {mustBeNumeric} = zeros(6,1) -end - -assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') -J = stewart.kinematics.J; - -X = J\args.dL; - -P = X(1:3); - -theta = norm(X(4:6)); -s = X(4:6)/theta; - -R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta); - s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta); - s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)]; diff --git a/matlab/src/generateCubicConfiguration.m b/matlab/src/generateCubicConfiguration.m deleted file mode 100644 index 596787b..0000000 --- a/matlab/src/generateCubicConfiguration.m +++ /dev/null @@ -1,50 +0,0 @@ -function [stewart] = generateCubicConfiguration(stewart, args) -% generateCubicConfiguration - Generate a Cubic Configuration -% -% Syntax: [stewart] = generateCubicConfiguration(stewart, args) -% -% Inputs: -% - stewart - A structure with the following fields -% - geometry.H [1x1] - Total height of the platform [m] -% - args - Can have the following fields: -% - Hc [1x1] - Height of the "useful" part of the cube [m] -% - FOc [1x1] - Height of the center of the cube with respect to {F} [m] -% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m] -% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m] -% -% 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.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 - args.FOc (1,1) double {mustBeNumeric} = 50e-3 - args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 - args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 -end - -assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') -H = stewart.geometry.H; - -sx = [ 2; -1; -1]; -sy = [ 0; 1; -1]; -sz = [ 1; 1; 1]; - -R = [sx, sy, sz]./vecnorm([sx, sy, sz]); - -L = args.Hc*sqrt(3); - -Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc]; - -CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg -CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg - -CSi = (CCm - CCf)./vecnorm(CCm - CCf); - -Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; -Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; - -stewart.platform_F.Fa = Fa; -stewart.platform_M.Mb = Mb; diff --git a/matlab/src/generateDiagPidControl.m b/matlab/src/generateDiagPidControl.m deleted file mode 100644 index 47a777a..0000000 --- a/matlab/src/generateDiagPidControl.m +++ /dev/null @@ -1,18 +0,0 @@ -function [K] = generateDiagPidControl(G, fs) - %% - pid_opts = pidtuneOptions(... - 'PhaseMargin', 50, ... - 'DesignFocus', 'disturbance-rejection'); - - %% - K = tf(zeros(6)); - - for i = 1:6 - input_name = G.InputName(i); - output_name = G.OutputName(i); - K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts)); - end - - K.InputName = G.OutputName; - K.OutputName = G.InputName; -end diff --git a/matlab/src/generateGeneralConfiguration.m b/matlab/src/generateGeneralConfiguration.m deleted file mode 100644 index fbb570d..0000000 --- a/matlab/src/generateGeneralConfiguration.m +++ /dev/null @@ -1,39 +0,0 @@ -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; diff --git a/matlab/src/generateShapedNoise.m b/matlab/src/generateShapedNoise.m deleted file mode 100644 index 51748c2..0000000 --- a/matlab/src/generateShapedNoise.m +++ /dev/null @@ -1,53 +0,0 @@ -function [U_exc] = generateShapedNoise(args) -% generateShapedNoise - Generate a Shaped Noise excitation signal -% -% Syntax: [U_exc] = generateShapedNoise(args) -% -% Inputs: -% - args - Optinal arguments: -% - Ts - Sampling Time - [s] -% - V_mean - Mean value of the excitation voltage - [V] -% - V_exc - Excitation Amplitude, could be numeric or TF - [V rms] -% - t_start - Time at which the noise begins - [s] -% - exc_duration - Duration of the noise - [s] -% - smooth_ends - 'true' or 'false': smooth transition between 0 and V_mean - [-] - -arguments - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 - args.V_mean (1,1) double {mustBeNumeric} = 0 - args.V_exc = 1 - args.t_start (1,1) double {mustBeNumeric, mustBePositive} = 5 - args.exc_duration (1,1) double {mustBeNumeric, mustBePositive} = 10 - args.smooth_ends logical {mustBeNumericOrLogical} = true -end - -t_noise = 0:args.Ts:args.exc_duration; - -if isnumeric(args.V_exc) - V_noise = args.V_mean + args.V_exc*sqrt(1/args.Ts/2)*randn(length(t_noise), 1)'; -elseif isct(args.V_exc) - V_noise = args.V_mean + lsim(args.V_exc, sqrt(1/args.Ts/2)*randn(length(t_noise), 1), t_noise)'; -end - -t_smooth_start = args.Ts:args.Ts:args.t_start; - -V_smooth_start = zeros(size(t_smooth_start)); -V_smooth_end = zeros(size(t_smooth_start)); - -if args.smooth_ends - Vd_max = args.V_mean/(0.7*args.t_start); - - V_d = zeros(size(t_smooth_start)); - V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start); - V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max; - V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start); - - V_smooth_start = cumtrapz(V_d)*args.Ts; - - V_smooth_end = args.V_mean - V_smooth_start; -end - -V_exc = [V_smooth_start, V_noise, V_smooth_end]; -t_exc = args.Ts*[0:1:length(V_exc)-1]; - -U_exc = [t_exc; V_exc]; diff --git a/matlab/src/generateSinIncreasingAmpl.m b/matlab/src/generateSinIncreasingAmpl.m deleted file mode 100644 index 4ce9170..0000000 --- a/matlab/src/generateSinIncreasingAmpl.m +++ /dev/null @@ -1,54 +0,0 @@ -function [U_exc] = generateSinIncreasingAmpl(args) -% generateSinIncreasingAmpl - Generate Sinus with increasing amplitude -% -% Syntax: [U_exc] = generateSinIncreasingAmpl(args) -% -% Inputs: -% - args - Optinal arguments: -% - Ts - Sampling Time - [s] -% - V_mean - Mean value of the excitation voltage - [V] -% - sin_ampls - Excitation Amplitudes - [V] -% - sin_freq - Excitation Frequency - [Hz] -% - sin_num - Number of period for each amplitude - [-] -% - t_start - Time at which the excitation begins - [s] -% - smooth_ends - 'true' or 'false': smooth transition between 0 and V_mean - [-] - -arguments - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 - args.V_mean (1,1) double {mustBeNumeric} = 0 - args.sin_ampls double {mustBeNumeric, mustBePositive} = [0.1, 0.2, 0.3] - args.sin_period (1,1) double {mustBeNumeric, mustBePositive} = 1 - args.sin_num (1,1) double {mustBeNumeric, mustBePositive, mustBeInteger} = 3 - args.t_start (1,1) double {mustBeNumeric, mustBePositive} = 5 - args.smooth_ends logical {mustBeNumericOrLogical} = true -end - -t_noise = 0:args.Ts:args.sin_period*args.sin_num; -sin_exc = []; - -for sin_ampl = args.sin_ampls - sin_exc = [sin_exc, args.V_mean + sin_ampl*sin(2*pi/args.sin_period*t_noise)]; -end - -t_smooth_start = args.Ts:args.Ts:args.t_start; - -V_smooth_start = zeros(size(t_smooth_start)); -V_smooth_end = zeros(size(t_smooth_start)); - -if args.smooth_ends - Vd_max = args.V_mean/(0.7*args.t_start); - - V_d = zeros(size(t_smooth_start)); - V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start); - V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max; - V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start); - - V_smooth_start = cumtrapz(V_d)*args.Ts; - - V_smooth_end = args.V_mean - V_smooth_start; -end - -V_exc = [V_smooth_start, sin_exc, V_smooth_end]; -t_exc = args.Ts*[0:1:length(V_exc)-1]; - -U_exc = [t_exc; V_exc]; diff --git a/matlab/src/generateSpiralAngleTrajectory.m b/matlab/src/generateSpiralAngleTrajectory.m deleted file mode 100644 index ac2d33c..0000000 --- a/matlab/src/generateSpiralAngleTrajectory.m +++ /dev/null @@ -1,35 +0,0 @@ -function [ref] = generateSpiralAngleTrajectory(args) -% generateSpiralAngleTrajectory - -% -% Syntax: [ref] = generateSpiralAngleTrajectory(args) -% -% Inputs: -% - args -% -% Outputs: -% - ref - Reference Signal - -arguments - args.R_tot (1,1) double {mustBeNumeric, mustBePositive} = 10e-6 % [rad] - args.n_turn (1,1) double {mustBeInteger, mustBePositive} = 5 % [-] - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [s] - args.t_turn (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] - args.t_end (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] -end - -time_s = 0:args.Ts:args.n_turn*args.t_turn; -time_e = 0:args.Ts:args.t_end; - -Rx = sin(2*pi*time_s/args.t_turn).*(args.R_tot*time_s/(args.n_turn*args.t_turn)); -Ry = cos(2*pi*time_s/args.t_turn).*(args.R_tot*time_s/(args.n_turn*args.t_turn)); - -Rx = [Rx, 0*time_e]; -Ry = [Ry, Ry(end) - Ry(end)*time_e/args.t_end]; - -t = 0:args.Ts:args.Ts*(length(Rx) - 1); - -ref = zeros(length(Rx), 7); - -ref(:, 1) = t; -ref(:, 5) = Rx; -ref(:, 6) = Ry; diff --git a/matlab/src/generateSweepExc.m b/matlab/src/generateSweepExc.m deleted file mode 100644 index 252b8c5..0000000 --- a/matlab/src/generateSweepExc.m +++ /dev/null @@ -1,76 +0,0 @@ -function [U_exc] = generateSweepExc(args) -% generateSweepExc - Generate a Sweep Sine excitation signal -% -% Syntax: [U_exc] = generateSweepExc(args) -% -% Inputs: -% - args - Optinal arguments: -% - Ts - Sampling Time - [s] -% - f_start - Start frequency of the sweep - [Hz] -% - f_end - End frequency of the sweep - [Hz] -% - V_mean - Mean value of the excitation voltage - [V] -% - V_exc - Excitation Amplitude for the Sweep, could be numeric or TF - [V] -% - t_start - Time at which the sweep begins - [s] -% - exc_duration - Duration of the sweep - [s] -% - sweep_type - 'logarithmic' or 'linear' - [-] -% - smooth_ends - 'true' or 'false': smooth transition between 0 and V_mean - [-] - -arguments - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 - args.f_start (1,1) double {mustBeNumeric, mustBePositive} = 1 - args.f_end (1,1) double {mustBeNumeric, mustBePositive} = 1e3 - args.V_mean (1,1) double {mustBeNumeric} = 0 - args.V_exc = 1 - args.t_start (1,1) double {mustBeNumeric, mustBeNonnegative} = 5 - args.exc_duration (1,1) double {mustBeNumeric, mustBePositive} = 10 - args.sweep_type char {mustBeMember(args.sweep_type,{'log', 'lin'})} = 'lin' - args.smooth_ends logical {mustBeNumericOrLogical} = true -end - -t_sweep = 0:args.Ts:args.exc_duration; - -if strcmp(args.sweep_type, 'log') - V_exc = sin(2*pi*args.f_start * args.exc_duration/log(args.f_end/args.f_start) * (exp(log(args.f_end/args.f_start)*t_sweep/args.exc_duration) - 1)); -elseif strcmp(args.sweep_type, 'lin') - V_exc = sin(2*pi*(args.f_start + (args.f_end - args.f_start)/2/args.exc_duration*t_sweep).*t_sweep); -else - error('sweep_type should either be equal to "log" or to "lin"'); -end - -if isnumeric(args.V_exc) - V_sweep = args.V_mean + args.V_exc*V_exc; -elseif isct(args.V_exc) - if strcmp(args.sweep_type, 'log') - V_sweep = args.V_mean + abs(squeeze(freqresp(args.V_exc, args.f_start*(args.f_end/args.f_start).^(t_sweep/args.exc_duration), 'Hz')))'.*V_exc; - elseif strcmp(args.sweep_type, 'lin') - V_sweep = args.V_mean + abs(squeeze(freqresp(args.V_exc, args.f_start+(args.f_end-args.f_start)/args.exc_duration*t_sweep, 'Hz')))'.*V_exc; - end -end - -if args.t_start > 0 - t_smooth_start = args.Ts:args.Ts:args.t_start; - - V_smooth_start = zeros(size(t_smooth_start)); - V_smooth_end = zeros(size(t_smooth_start)); - - if args.smooth_ends - Vd_max = args.V_mean/(0.7*args.t_start); - - V_d = zeros(size(t_smooth_start)); - V_d(t_smooth_start < 0.2*args.t_start) = t_smooth_start(t_smooth_start < 0.2*args.t_start)*Vd_max/(0.2*args.t_start); - V_d(t_smooth_start > 0.2*args.t_start & t_smooth_start < 0.7*args.t_start) = Vd_max; - V_d(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) = Vd_max - (t_smooth_start(t_smooth_start > 0.7*args.t_start & t_smooth_start < 0.9*args.t_start) - 0.7*args.t_start)*Vd_max/(0.2*args.t_start); - - V_smooth_start = cumtrapz(V_d)*args.Ts; - - V_smooth_end = args.V_mean - V_smooth_start; - end -else - V_smooth_start = []; - V_smooth_end = []; -end - -V_exc = [V_smooth_start, V_sweep, V_smooth_end]; -t_exc = args.Ts*[0:1:length(V_exc)-1]; - -U_exc = [t_exc; V_exc]; diff --git a/matlab/src/generateXYZTrajectory.m b/matlab/src/generateXYZTrajectory.m deleted file mode 100644 index 3c2e69f..0000000 --- a/matlab/src/generateXYZTrajectory.m +++ /dev/null @@ -1,48 +0,0 @@ -function [ref] = generateXYZTrajectory(args) -% generateXYZTrajectory - -% -% Syntax: [ref] = generateXYZTrajectory(args) -% -% Inputs: -% - args -% -% Outputs: -% - ref - Reference Signal - -arguments - args.points double {mustBeNumeric} = zeros(2, 3) % [m] - - args.ti (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % Time to go to first point and after last point [s] - args.tw (1,1) double {mustBeNumeric, mustBeNonnegative} = 0.5 % Time wait between each point [s] - args.tm (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % Motion time between points [s] - - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s] -end - -time_i = 0:args.Ts:args.ti; -time_w = 0:args.Ts:args.tw; -time_m = 0:args.Ts:args.tm; - -% Go to initial position -xyz = (args.points(1,:))'*(time_i/args.ti); - -% Wait -xyz = [xyz, xyz(:,end).*ones(size(time_w))]; - -% Scans -for i = 2:size(args.points, 1) - % Go to next point - xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)]; - % Wait a litle bit - xyz = [xyz, xyz(:,end).*ones(size(time_w))]; -end - -% End motion -xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)]; - -t = 0:args.Ts:args.Ts*(length(xyz) - 1); - -ref = zeros(length(xyz), 7); - -ref(:, 1) = t; -ref(:, 2:4) = xyz'; diff --git a/matlab/src/generateYZScanTrajectory.m b/matlab/src/generateYZScanTrajectory.m deleted file mode 100644 index e23b168..0000000 --- a/matlab/src/generateYZScanTrajectory.m +++ /dev/null @@ -1,83 +0,0 @@ -function [ref] = generateYZScanTrajectory(args) -% generateYZScanTrajectory - -% -% Syntax: [ref] = generateYZScanTrajectory(args) -% -% Inputs: -% - args -% -% Outputs: -% - ref - Reference Signal - -arguments - args.y_tot (1,1) double {mustBeNumeric, mustBePositive} = 10e-6 % [m] - args.z_tot (1,1) double {mustBeNumeric, mustBePositive} = 10e-6 % [m] - - args.n (1,1) double {mustBeInteger, mustBePositive} = 10 % [-] - - args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s] - - args.ti (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s] - args.tw (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s] - args.ty (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s] - args.tz (1,1) double {mustBeNumeric, mustBeNonnegative} = 1 % [s] -end - -time_i = 0:args.Ts:args.ti; -time_w = 0:args.Ts:args.tw; -time_y = 0:args.Ts:args.ty; -time_z = 0:args.Ts:args.tz; - -% Go to initial position -y = (time_i/args.ti)*(args.y_tot/2); - -% Wait -y = [y, y(end)*ones(size(time_w))]; - -% Scans -for i = 1:args.n - if mod(i,2) == 0 - y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot]; - else - y = [y, (args.y_tot/2) - (time_y/args.ty)*args.y_tot]; - end - - if i < args.n - y = [y, y(end)*ones(size(time_z))]; - end -end - -% Wait a litle bit -y = [y, y(end)*ones(size(time_w))]; - -% End motion -y = [y, y(end) - y(end)*time_i/args.ti]; - -% Go to initial position -z = (time_i/args.ti)*(args.z_tot/2); - -% Wait -z = [z, z(end)*ones(size(time_w))]; - -% Scans -for i = 1:args.n - z = [z, z(end)*ones(size(time_y))]; - - if i < args.n - z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)]; - end -end - -% Wait a litle bit -z = [z, z(end)*ones(size(time_w))]; - -% End motion -z = [z, z(end) - z(end)*time_i/args.ti]; - -t = 0:args.Ts:args.Ts*(length(y) - 1); - -ref = zeros(length(y), 7); - -ref(:, 1) = t; -ref(:, 3) = y; -ref(:, 4) = z; diff --git a/matlab/src/getJacobianNanoHexapod.m b/matlab/src/getJacobianNanoHexapod.m deleted file mode 100644 index f660c78..0000000 --- a/matlab/src/getJacobianNanoHexapod.m +++ /dev/null @@ -1,34 +0,0 @@ -function [J] = getJacobianNanoHexapod(Hbm) -% getJacobianNanoHexapod - -% -% Syntax: [J] = getJacobianNanoHexapod(Hbm) -% -% Inputs: -% - Hbm - Height of {B} w.r.t. {M} [m] -% -% Outputs: -% - J - Jacobian Matrix - -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] - -H = 95e-3; % Stewart platform height [m] -Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m] - -si = Fb - Fa; -si = si./vecnorm(si); % Normalize - -Bb = Mb - [0; 0; Hbm]; - -J = [si', cross(Bb, si)']; diff --git a/matlab/src/getTransformationMatrixAcc.m b/matlab/src/getTransformationMatrixAcc.m deleted file mode 100644 index cc4dd44..0000000 --- a/matlab/src/getTransformationMatrixAcc.m +++ /dev/null @@ -1,23 +0,0 @@ -function [M] = getTransformationMatrixAcc(Opm, Osm) -% getTransformationMatrixAcc - -% -% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm) -% -% Inputs: -% - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers -% - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation -% -% Outputs: -% - M - Transformation Matrix - -M = zeros(length(Opm), 6); - -for i = 1:length(Opm) - Ri = [0, Opm(3,i), -Opm(2,i); - -Opm(3,i), 0, Opm(1,i); - Opm(2,i), -Opm(1,i), 0]; - M(i, 1:3) = Osm(:,i)'; - M(i, 4:6) = Osm(:,i)'*Ri; -end - -end diff --git a/matlab/src/initializeAmplifiedStrutDynamics.m b/matlab/src/initializeAmplifiedStrutDynamics.m deleted file mode 100644 index a22bdb0..0000000 --- a/matlab/src/initializeAmplifiedStrutDynamics.m +++ /dev/null @@ -1,43 +0,0 @@ -function [stewart] = initializeAmplifiedStrutDynamics(stewart, args) -% initializeAmplifiedStrutDynamics - Add Stiffness and Damping properties of each strut -% -% Syntax: [stewart] = initializeAmplifiedStrutDynamics(args) -% -% Inputs: -% - args - Structure with the following fields: -% - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] -% - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] -% - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m] -% - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)] -% -% Outputs: -% - stewart - updated Stewart structure with the added fields: -% - actuators.type = 2 -% - actuators.K [6x1] - Total Stiffness of each strut [N/m] -% - actuators.C [6x1] - Total Damping of each strut [N/(m/s)] -% - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] -% - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] -% - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] -% - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)] - -arguments - stewart - args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1) - args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) - args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1) - args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) -end - -K = args.Ka + args.Kr; -C = args.Ca + args.Cr; - -stewart.actuators.type = 2; - -stewart.actuators.Ka = args.Ka; -stewart.actuators.Ca = args.Ca; - -stewart.actuators.Kr = args.Kr; -stewart.actuators.Cr = args.Cr; - -stewart.actuators.K = K; -stewart.actuators.C = K; diff --git a/matlab/src/initializeAxisc.m b/matlab/src/initializeAxisc.m deleted file mode 100644 index 9358a59..0000000 --- a/matlab/src/initializeAxisc.m +++ /dev/null @@ -1,34 +0,0 @@ -function [axisc] = initializeAxisc(args) - -arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' -end - -axisc = struct(); - -switch args.type - case 'none' - axisc.type = 0; - case 'rigid' - axisc.type = 1; - case 'flexible' - axisc.type = 2; -end - -% Structure -axisc.structure.density = 3400; % [kg/m3] -axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP'; - -% Wheel -axisc.wheel.density = 2700; % [kg/m3] -axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP'; - -% Mass -axisc.mass.density = 7800; % [kg/m3] -axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP'; - -% Gear -axisc.gear.density = 7800; % [kg/m3] -axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP'; - -save('./mat/stages.mat', 'axisc', '-append'); diff --git a/matlab/src/initializeCedratPiezo.m b/matlab/src/initializeCedratPiezo.m deleted file mode 100644 index a91dc8e..0000000 --- a/matlab/src/initializeCedratPiezo.m +++ /dev/null @@ -1,17 +0,0 @@ -function [cedrat] = initializeCedratPiezo() - %% Stewart Object - cedrat = struct(); - cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m] - cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m] - - cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)] - cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)] - - cedrat.L = 80; % Total Width of the Actuator[mm] - cedrat.H = 45; % Total Height of the Actuator [mm] - cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm] - cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg] - - %% Save - save('./mat/stages.mat', 'cedrat', '-append'); -end diff --git a/matlab/src/initializeController.m b/matlab/src/initializeController.m deleted file mode 100644 index 76571d8..0000000 --- a/matlab/src/initializeController.m +++ /dev/null @@ -1,39 +0,0 @@ -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 - -save('./mat/controller.mat', 'controller'); diff --git a/matlab/src/initializeCylindricalPlatforms.m b/matlab/src/initializeCylindricalPlatforms.m deleted file mode 100644 index 0b86fc1..0000000 --- a/matlab/src/initializeCylindricalPlatforms.m +++ /dev/null @@ -1,59 +0,0 @@ -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; diff --git a/matlab/src/initializeCylindricalStruts.m b/matlab/src/initializeCylindricalStruts.m deleted file mode 100644 index 2bb7257..0000000 --- a/matlab/src/initializeCylindricalStruts.m +++ /dev/null @@ -1,71 +0,0 @@ -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; diff --git a/matlab/src/initializeDisturbances.m b/matlab/src/initializeDisturbances.m deleted file mode 100644 index 70d6f87..0000000 --- a/matlab/src/initializeDisturbances.m +++ /dev/null @@ -1,135 +0,0 @@ -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.Dwx logical {mustBeNumericOrLogical} = true - % Ground Motion - Y direction - args.Dwy logical {mustBeNumericOrLogical} = true - % Ground Motion - Z direction - args.Dwz logical {mustBeNumericOrLogical} = true - % Translation Stage - X direction - args.Fty_x logical {mustBeNumericOrLogical} = true - % Translation Stage - Z direction - args.Fty_z logical {mustBeNumericOrLogical} = true - % Spindle - Z direction - args.Frz_z logical {mustBeNumericOrLogical} = true -end - -load('./mat/dist_psd.mat', 'dist_f'); - -dist_f.f = dist_f.f(2:end); -dist_f.psd_gm = dist_f.psd_gm(2:end); -dist_f.psd_ty = dist_f.psd_ty(2:end); -dist_f.psd_rz = dist_f.psd_rz(2:end); - -Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] -N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD -T0 = N/Fs; % Signal Duration [s] -df = 1/T0; % Frequency resolution of the DFT [Hz] - % Also equal to (dist_f.f(2)-dist_f.f(1)) -t = linspace(0, T0, N+1)'; % Time Vector [s] -Ts = 1/Fs; % Sampling Time [s] - -phi = dist_f.psd_gm; -C = zeros(N/2,1); -for i = 1:N/2 - C(i) = sqrt(phi(i)*df); -end - -if args.Dwx && args.enable - rng(111); - 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)))];; - Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] -else - Dwx = zeros(length(t), 1); -end - -if args.Dwy && args.enable - rng(112); - 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)))];; - Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] -else - Dwy = zeros(length(t), 1); -end - -if args.Dwy && args.enable - rng(113); - 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)))];; - Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] -else - Dwz = zeros(length(t), 1); -end - -if args.Fty_x && args.enable - phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - rng(121); - 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)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N] - Fty_x = u; -else - Fty_x = zeros(length(t), 1); -end - -if args.Fty_z && args.enable - phi = dist_f.psd_ty; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - rng(122); - 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)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N] - Fty_z = u; -else - Fty_z = zeros(length(t), 1); -end - -if args.Frz_z && args.enable - phi = dist_f.psd_rz; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - rng(131); - 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)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N] - Frz_z = u; -else - Frz_z = zeros(length(t), 1); -end - -u = zeros(length(t), 6); -Fd = u; - -Dwx = Dwx - Dwx(1); -Dwy = Dwy - Dwy(1); -Dwz = Dwz - Dwz(1); -Fty_x = Fty_x - Fty_x(1); -Fty_z = Fty_z - Fty_z(1); -Frz_z = Frz_z - Frz_z(1); - -save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args'); diff --git a/matlab/src/initializeFramesPositions.m b/matlab/src/initializeFramesPositions.m deleted file mode 100644 index f68b97d..0000000 --- a/matlab/src/initializeFramesPositions.m +++ /dev/null @@ -1,35 +0,0 @@ -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; diff --git a/matlab/src/initializeGranite.m b/matlab/src/initializeGranite.m deleted file mode 100644 index 67c1e75..0000000 --- a/matlab/src/initializeGranite.m +++ /dev/null @@ -1,45 +0,0 @@ -function [granite] = initializeGranite(args) - -arguments - args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false - args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3] - args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m] - args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)] - args.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.8 % 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; - case 'modal-analysis' - granite.type = 3; - case 'init' - granite.type = 4; -end - -granite.density = args.density; % [kg/m3] -granite.STEP = './STEPS/granite/granite.STEP'; - -granite.sample_pos = args.sample_pos; % [m] - -granite.K = args.K; % [N/m] -granite.C = args.C; % [N/(m/s)] - -if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fgm'); - granite.Deq = -Fgm'./granite.K; -else - granite.Deq = zeros(6,1); -end - -save('./mat/stages.mat', 'granite', '-append'); diff --git a/matlab/src/initializeGround.m b/matlab/src/initializeGround.m deleted file mode 100644 index d9c4f2a..0000000 --- a/matlab/src/initializeGround.m +++ /dev/null @@ -1,22 +0,0 @@ -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; - -save('./mat/stages.mat', 'ground', '-append'); diff --git a/matlab/src/initializeInertialSensor.m b/matlab/src/initializeInertialSensor.m deleted file mode 100644 index a9509e4..0000000 --- a/matlab/src/initializeInertialSensor.m +++ /dev/null @@ -1,48 +0,0 @@ -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; diff --git a/matlab/src/initializeJointDynamics.m b/matlab/src/initializeJointDynamics.m deleted file mode 100644 index cc78b3a..0000000 --- a/matlab/src/initializeJointDynamics.m +++ /dev/null @@ -1,131 +0,0 @@ -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; diff --git a/matlab/src/initializeLoggingConfiguration.m b/matlab/src/initializeLoggingConfiguration.m deleted file mode 100644 index 5d46e58..0000000 --- a/matlab/src/initializeLoggingConfiguration.m +++ /dev/null @@ -1,21 +0,0 @@ -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; - -save('./mat/conf_log.mat', 'conf_log'); diff --git a/matlab/src/initializeMicroHexapod.m b/matlab/src/initializeMicroHexapod.m deleted file mode 100644 index 643d81e..0000000 --- a/matlab/src/initializeMicroHexapod.m +++ /dev/null @@ -1,110 +0,0 @@ -function [micro_hexapod] = initializeMicroHexapod(args) - -arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = '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) - % Force that stiffness of each joint should apply at t=0 - args.Foffset logical {mustBeNumericOrLogical} = false -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'); - -if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fhm'); - stewart.actuators.dLeq = -Fhm'./args.Ki; -else - stewart.actuators.dLeq = zeros(6,1); -end - -switch args.type - case 'none' - stewart.type = 0; - case 'rigid' - stewart.type = 1; - case 'flexible' - stewart.type = 2; - case 'modal-analysis' - stewart.type = 3; - case 'init' - stewart.type = 4; - case 'compliance' - stewart.type = 5; -end - -micro_hexapod = stewart; -save('./mat/stages.mat', 'micro_hexapod', '-append'); diff --git a/matlab/src/initializeMicroHexapodOld.m b/matlab/src/initializeMicroHexapodOld.m deleted file mode 100644 index 41da308..0000000 --- a/matlab/src/initializeMicroHexapodOld.m +++ /dev/null @@ -1,196 +0,0 @@ -function [micro_hexapod] = initializeMicroHexapod(args) - arguments - args.rigid logical {mustBeNumericOrLogical} = false - args.AP (3,1) double {mustBeNumeric} = zeros(3,1) - args.ARB (3,3) double {mustBeNumeric} = eye(3) - end - - %% Stewart Object - micro_hexapod = struct(); - micro_hexapod.h = 350; % Total height of the platform [mm] - micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm] - - %% Bottom Plate - Mechanical Design - BP = struct(); - - BP.rad.int = 110; % Internal Radius [mm] - BP.rad.ext = 207.5; % External Radius [mm] - BP.thickness = 26; % Thickness [mm] - BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm] - BP.leg.ang = 9.5; % Angle Offset [deg] - BP.density = 8000; % Density of the material [kg/m^3] - BP.color = [0.6 0.6 0.6]; % Color [rgb] - BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness]; - - %% Top Plate - Mechanical Design - TP = struct(); - - TP.rad.int = 82; % Internal Radius [mm] - TP.rad.ext = 150; % Internal Radius [mm] - TP.thickness = 26; % Thickness [mm] - TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm] - TP.leg.ang = 12.1; % Angle Offset [deg] - TP.density = 8000; % Density of the material [kg/m^3] - TP.color = [0.6 0.6 0.6]; % Color [rgb] - TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness]; - - %% Struts - Leg = struct(); - - Leg.stroke = 10e-3; % Maximum Stroke of each leg [m] - if args.rigid - Leg.k.ax = 1e12; % Stiffness of each leg [N/m] - else - Leg.k.ax = 2e7; % Stiffness of each leg [N/m] - end - Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) [] - Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm] - Leg.rad.top = 17; % Radius of the cylinder of the top part [mm] - Leg.density = 8000; % Density of the material [kg/m^3] - Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb] - Leg.color.top = [0.5 0.5 0.5]; % Color [rgb] - - Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm] - Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm] - Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg] - Leg = updateDamping(Leg); - - - %% Sphere - SP = struct(); - - SP.height.bottom = 27; % [mm] - SP.height.top = 27; % [mm] - SP.density.bottom = 8000; % [kg/m^3] - SP.density.top = 8000; % [kg/m^3] - SP.color.bottom = [0.6 0.6 0.6]; % [rgb] - SP.color.top = [0.6 0.6 0.6]; % [rgb] - SP.k.ax = 0; % [N*m/deg] - SP.ksi.ax = 10; - - SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm] - SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm] - SP.rad.bottom = Leg.sphere.bottom; % [mm] - SP.rad.top = Leg.sphere.top; % [mm] - SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg] - - SP = updateDamping(SP); - - %% - Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom]; - Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top]; - - %% - micro_hexapod.BP = BP; - micro_hexapod.TP = TP; - micro_hexapod.Leg = Leg; - micro_hexapod.SP = SP; - - %% - micro_hexapod = initializeParameters(micro_hexapod); - - %% Setup equilibrium position of each leg - micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB); - - %% Save - save('./mat/stages.mat', 'micro_hexapod', '-append'); - - %% - function [element] = updateDamping(element) - field = fieldnames(element.k); - for i = 1:length(field) - element.c.(field{i}) = 2*element.ksi.(field{i})*sqrt(element.k.(field{i})*element.m); - end - end - - %% - function [stewart] = initializeParameters(stewart) - %% Connection points on base and top plate w.r.t. World frame at the center of the base plate - stewart.pos_base = zeros(6, 3); - stewart.pos_top = zeros(6, 3); - - alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases) - alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top - - height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO - - radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base - radius_t = stewart.TP.leg.rad*0.001; % top radius in meters - - for i = 1:3 - % base points - angle_m_b = (2*pi/3)* (i-1) - alpha_b; - angle_p_b = (2*pi/3)* (i-1) + alpha_b; - stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0]; - stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0]; - - % top points - % Top points are 60 degrees offset - angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6; - angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6; - stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height]; - stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height]; - end - - % permute pos_top points so that legs are end points of base and top points - stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom - stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)]; - - %% leg vectors - legs = stewart.pos_top - stewart.pos_base; - leg_length = zeros(6, 1); - leg_vectors = zeros(6, 3); - for i = 1:6 - leg_length(i) = norm(legs(i,:)); - leg_vectors(i,:) = legs(i,:) / leg_length(i); - end - - stewart.Leg.lenght = 1000*leg_length(1)/1.5; - stewart.Leg.shape.bot = [0 0; ... - stewart.Leg.rad.bottom 0; ... - stewart.Leg.rad.bottom stewart.Leg.lenght; ... - stewart.Leg.rad.top stewart.Leg.lenght; ... - stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ... - 0 0.2*stewart.Leg.lenght]; - - %% Calculate revolute and cylindrical axes - rev1 = zeros(6, 3); - rev2 = zeros(6, 3); - cyl1 = zeros(6, 3); - for i = 1:6 - rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]); - rev1(i,:) = rev1(i,:) / norm(rev1(i,:)); - - rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:)); - rev2(i,:) = rev2(i,:) / norm(rev2(i,:)); - - cyl1(i,:) = leg_vectors(i,:); - end - - - %% Coordinate systems - stewart.lower_leg = struct('rotation', eye(3)); - stewart.upper_leg = struct('rotation', eye(3)); - - for i = 1:6 - stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)']; - stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)']; - end - - %% Position Matrix - stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)]; - - %% Compute Jacobian Matrix - aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)]; - stewart.J = getJacobianMatrix(leg_vectors', aa'); - end - - %% - function J = getJacobianMatrix(RM, M_pos_base) - % RM: [3x6] unit vector of each leg in the fixed frame - % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame - J = zeros(6); - J(:, 1:3) = RM'; - J(:, 4:6) = cross(M_pos_base, RM)'; - end -end diff --git a/matlab/src/initializeMirror.m b/matlab/src/initializeMirror.m deleted file mode 100644 index ca98db1..0000000 --- a/matlab/src/initializeMirror.m +++ /dev/null @@ -1,76 +0,0 @@ -function [] = initializeMirror(args) - -arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'rigid' - args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical' - args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg] - args.mass (1,1) double {mustBeNumeric, mustBePositive} = 10 % [kg] - args.freq (6,1) double {mustBeNumeric, mustBeNonnegative} = 200*ones(6,1) % [Hz] -end - -mirror = struct(); - -switch args.type - case 'none' - mirror.type = 0; - case 'rigid' - mirror.type = 1; - case 'flexible' - mirror.type = 2; -end - -mirror.mass = args.mass; -mirror.freq = args.freq; - -mirror.K = zeros(6,1); -mirror.K(1:3) = mirror.mass * (2*pi*mirror.freq(1:3)).^2; - -mirror.C = zeros(6,1); -mirror.C(1:3) = 0.2 * sqrt(mirror.K(1:3).*mirror.mass); - -mirror.Deq = zeros(6,1); - -mirror.h = 0.05; % Height of the mirror [m] - -mirror.thickness = 0.02; % Thickness of the plate supporting the sample [m] - -mirror.hole_rad = 0.125; % radius of the hole in the mirror [m] - -mirror.support_rad = 0.1; % radius of the support plate [m] - -% point of interest offset in z (above the top surfave) [m] -switch args.type - case 'none' - mirror.jacobian = 0.205; - case 'rigid' - mirror.jacobian = 0.205 - mirror.h; - case 'flexible' - mirror.jacobian = 0.205 - mirror.h; -end - -mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m] - -mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point - -mirror.shape = [... - mirror.support_rad+5e-3 mirror.h-mirror.thickness - mirror.hole_rad mirror.h-mirror.thickness; ... - mirror.hole_rad 0; ... - mirror.rad 0 ... -]; - -if strcmp(args.shape, 'spherical') - mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm] - - for z = linspace(0, mirror.h, 101) - mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z]; - end -elseif strcmp(args.shape, 'conical') - mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h]; -else - error('Shape should be either conical or spherical'); -end - -mirror.shape = [mirror.shape; mirror.support_rad+5e-3 mirror.h]; - -save('./mat/stages.mat', 'mirror', '-append'); diff --git a/matlab/src/initializeNanoHexapod.m b/matlab/src/initializeNanoHexapod.m index 4a97fe4..71717b0 100644 --- a/matlab/src/initializeNanoHexapod.m +++ b/matlab/src/initializeNanoHexapod.m @@ -1,142 +1,290 @@ function [nano_hexapod] = initializeNanoHexapod(args) arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'init'})} = 'flexible' - % initializeFramesPositions - args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 - args.MO_B (1,1) double {mustBeNumeric} = 170e-3 - % generateGeneralConfiguration - args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 - args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-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) - % initializeStrutDynamics - args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz', 'amplified'})} = 'piezo' - args.k1 (1,1) double {mustBeNumeric} = 1e6 - args.ke (1,1) double {mustBeNumeric} = 5e6 - args.ka (1,1) double {mustBeNumeric} = 60e6 - args.c1 (1,1) double {mustBeNumeric} = 10 - args.F_gain (1,1) double {mustBeNumeric} = 1 - args.k (1,1) double {mustBeNumeric} = -1 - args.c (1,1) double {mustBeNumeric} = -1 - % initializeJointDynamics - 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.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.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) - % initializeCylindricalPlatforms - args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 - args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 - args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-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} = 120e-3 - % initializeCylindricalStruts - 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 - % inverseKinematics - args.AP (3,1) double {mustBeNumeric} = zeros(3,1) - args.ARB (3,3) double {mustBeNumeric} = eye(3) - % Equilibrium position of each leg - args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1) - % Force that stiffness of each joint should apply at t=0 - args.Foffset logical {mustBeNumericOrLogical} = false + %% 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 -stewart = initializeStewartPlatform(); +nano_hexapod = struct(); -stewart = initializeFramesPositions(stewart, 'H', args.H, 'MO_B', args.MO_B); +nano_hexapod.flex_bot = struct(); -stewart = generateGeneralConfiguration(stewart, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh); - -stewart = computeJointsPose(stewart); - -if args.k > 0 && args.c > 0 - stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', args.k*ones(6,1), 'C', args.c*ones(6,1)); -elseif args.k > 0 - stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', args.k*ones(6,1), 'C', 1.5*sqrt(args.k)*ones(6,1)); -elseif strcmp(args.actuator, 'piezo') - stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', 1e7*ones(6,1), 'C', 1e2*ones(6,1)); -elseif strcmp(args.actuator, 'lorentz') - stewart = initializeStrutDynamics(stewart, 'type', 'classical', 'K', 1e4*ones(6,1), 'C', 1e2*ones(6,1)); -elseif strcmp(args.actuator, 'amplified') - stewart = initializeStrutDynamics(stewart, 'type', 'amplified', ... - 'k1', args.k1*ones(6,1), ... - 'c1', args.c1*ones(6,1), ... - 'ka', args.ka*ones(6,1), ... - 'ke', args.ke*ones(6,1), ... - 'F_gain', args.F_gain*ones(6,1)); -else - error('args.actuator should be piezo, lorentz or amplified'); -end - -stewart = initializeJointDynamics(stewart, ... - 'type_F', args.type_F, ... - 'type_M', args.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); - -stewart = initializeInertialSensor(stewart, 'type', 'accelerometer'); - -if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fnm'); - stewart.actuators.dLeq = -Fnm'./stewart.Ki; -else - stewart.actuators.dLeq = args.dLeq; -end - -switch args.type - case 'none' - stewart.type = 0; - case 'rigid' - stewart.type = 1; +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' - stewart.type = 2; - case 'init' - stewart.type = 4; + nano_hexapod.flex_bot.type = 4; end -nano_hexapod = stewart; -save('./mat/stages.mat', 'nano_hexapod', '-append'); +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 diff --git a/matlab/src/initializeNanoHexapodFinal.m b/matlab/src/initializeNanoHexapodFinal.m deleted file mode 100644 index 6904c2e..0000000 --- a/matlab/src/initializeNanoHexapodFinal.m +++ /dev/null @@ -1,305 +0,0 @@ -function [nano_hexapod] = initializeNanoHexapodFinal(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 - -switch args.controller_type - case 'none' - nano_hexapod.controller.type = 1; - case 'iff' - nano_hexapod.controller.type = 2; - case 'dvf' - nano_hexapod.controller.type = 3; - case 'hac-iff-struts' - nano_hexapod.controller.type = 4; -end - -if nargout == 0 - save('./mat/stages.mat', 'nano_hexapod', '-append'); -end diff --git a/matlab/src/initializePosError.m b/matlab/src/initializePosError.m deleted file mode 100644 index e942c6f..0000000 --- a/matlab/src/initializePosError.m +++ /dev/null @@ -1,28 +0,0 @@ -function [] = initializePosError(args) -% initializePosError - Initialize the position errors -% -% Syntax: [] = initializePosError(args) -% -% Inputs: -% - args - - -arguments - args.error logical {mustBeNumericOrLogical} = false - args.Dy (1,1) double {mustBeNumeric} = 0 % [m] - args.Ry (1,1) double {mustBeNumeric} = 0 % [m] - args.Rz (1,1) double {mustBeNumeric} = 0 % [m] -end - -pos_error = struct(); - -if args.error - pos_error.type = 1; -else - pos_error.type = 0; -end - -pos_error.Dy = args.Dy; -pos_error.Ry = args.Ry; -pos_error.Rz = args.Rz; - -save('./mat/pos_error.mat', 'pos_error'); diff --git a/matlab/src/initializeReferences.m b/matlab/src/initializeReferences.m deleted file mode 100644 index c0471c1..0000000 --- a/matlab/src/initializeReferences.m +++ /dev/null @@ -1,230 +0,0 @@ -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('mat/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)); - -%% Axis Compensation - Rm -t = [0, Ts]; - -Rm = [args.Rm_pos, args.Rm_pos]; -Rm = struct('time', t, 'signals', struct('values', Rm)); - -%% Nano-Hexapod -t = [0, Ts]; -Dn = zeros(length(t), 6); - -switch args.Dn_type - case 'constant' - Dn = [args.Dn_pos, args.Dn_pos]; - - load('mat/stages.mat', 'nano_hexapod'); - - AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)]; - - tx = args.Dn_pos(4); - ty = args.Dn_pos(5); - tz = args.Dn_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)]; - - [~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB); - Dnl = [Dnl, Dnl]; - otherwise - warning('Dn_type is not set correctly'); -end - -Dn = struct('time', t, 'signals', struct('values', Dn)); -Dnl = struct('time', t, 'signals', struct('values', Dnl)); - -%% Save - save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts'); -end diff --git a/matlab/src/initializeRy.m b/matlab/src/initializeRy.m deleted file mode 100644 index b156199..0000000 --- a/matlab/src/initializeRy.m +++ /dev/null @@ -1,54 +0,0 @@ -function [ry] = initializeRy(args) - -arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false - 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; - case 'modal-analysis' - ry.type = 3; - case 'init' - ry.type = 4; -end - -% Ry - Guide for the tilt stage -ry.guide.density = 7800; % [kg/m3] -ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP'; - -% Ry - Rotor of the motor -ry.rotor.density = 2400; % [kg/m3] -ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP'; - -% Ry - Motor -ry.motor.density = 3200; % [kg/m3] -ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP'; - -% Ry - Plateau Tilt -ry.stage.density = 7800; % [kg/m3] -ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP'; - -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 args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fym'); - ry.Deq = -Fym'./ry.K; -else - ry.Deq = zeros(6,1); -end - -save('./mat/stages.mat', 'ry', '-append'); diff --git a/matlab/src/initializeRz.m b/matlab/src/initializeRz.m deleted file mode 100644 index 164e92f..0000000 --- a/matlab/src/initializeRz.m +++ /dev/null @@ -1,45 +0,0 @@ -function [rz] = initializeRz(args) - -arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false -end - -rz = struct(); - -switch args.type - case 'none' - rz.type = 0; - case 'rigid' - rz.type = 1; - case 'flexible' - rz.type = 2; - case 'modal-analysis' - rz.type = 3; - case 'init' - rz.type = 4; -end - -% Spindle - Slip Ring -rz.slipring.density = 7800; % [kg/m3] -rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP'; - -% Spindle - Rotor -rz.rotor.density = 7800; % [kg/m3] -rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP'; - -% Spindle - Stator -rz.stator.density = 7800; % [kg/m3] -rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP'; - -rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7]; -rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4]; - -if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fzm'); - rz.Deq = -Fzm'./rz.K; -else - rz.Deq = zeros(6,1); -end - -save('./mat/stages.mat', 'rz', '-append'); diff --git a/matlab/src/initializeSample.m b/matlab/src/initializeSample.m deleted file mode 100644 index bc4a757..0000000 --- a/matlab/src/initializeSample.m +++ /dev/null @@ -1,51 +0,0 @@ -function [sample] = initializeSample(args) - -arguments - args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible' - args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m] - args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m] - args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg] - args.freq (6,1) double {mustBeNumeric, mustBePositive} = 100*ones(6,1) % [Hz] - args.offset (1,1) double {mustBeNumeric} = 0 % [m] - args.Foffset logical {mustBeNumericOrLogical} = false -end - -sample = struct(); - -switch args.type - case 'none' - sample.type = 0; - case 'rigid' - sample.type = 1; - case 'flexible' - sample.type = 2; - case 'init' - sample.type = 3; -end - -sample.radius = args.radius; % [m] -sample.height = args.height; % [m] -sample.mass = args.mass; % [kg] -sample.offset = args.offset; % [m] - -sample.inertia = [1/12 * sample.mass * (3*sample.radius^2 + sample.height^2); ... - 1/12 * sample.mass * (3*sample.radius^2 + sample.height^2); ... - 1/2 * sample.mass * sample.radius^2]; - -sample.K = zeros(6, 1); -sample.C = zeros(6, 1); - -sample.K(1:3) = sample.mass .* (2*pi * args.freq(1:3)).^2; % [N/m] -sample.C(1:3) = 0.1 * sqrt(sample.K(1:3)*sample.mass); % [N/(m/s)] - -sample.K(4:6) = sample.inertia .* (2*pi * args.freq(4:6)).^2; % [N/m] -sample.C(4:6) = 0.1 * sqrt(sample.K(4:6).*sample.inertia); % [N/(m/s)] - -if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Fsm'); - sample.Deq = -Fsm'./sample.K; -else - sample.Deq = zeros(6,1); -end - -save('./mat/stages.mat', 'sample', '-append'); diff --git a/matlab/src/initializeSimscapeConfiguration.m b/matlab/src/initializeSimscapeConfiguration.m deleted file mode 100644 index ba93c9f..0000000 --- a/matlab/src/initializeSimscapeConfiguration.m +++ /dev/null @@ -1,15 +0,0 @@ -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 - -save('./mat/conf_simscape.mat', 'conf_simscape'); diff --git a/matlab/src/initializeStewartPlatform.m b/matlab/src/initializeStewartPlatform.m deleted file mode 100644 index 0545b4d..0000000 --- a/matlab/src/initializeStewartPlatform.m +++ /dev/null @@ -1,31 +0,0 @@ -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(); diff --git a/matlab/src/initializeStewartPose.m b/matlab/src/initializeStewartPose.m deleted file mode 100644 index 325b501..0000000 --- a/matlab/src/initializeStewartPose.m +++ /dev/null @@ -1,27 +0,0 @@ -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; diff --git a/matlab/src/initializeStrutDynamics.m b/matlab/src/initializeStrutDynamics.m deleted file mode 100644 index 69dc518..0000000 --- a/matlab/src/initializeStrutDynamics.m +++ /dev/null @@ -1,49 +0,0 @@ -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; diff --git a/matlab/src/initializeTy.m b/matlab/src/initializeTy.m deleted file mode 100644 index 19bc37d..0000000 --- a/matlab/src/initializeTy.m +++ /dev/null @@ -1,69 +0,0 @@ -function [ty] = initializeTy(args) - -arguments - args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible' - args.Foffset logical {mustBeNumericOrLogical} = false -end - -ty = struct(); - -switch args.type - case 'none' - ty.type = 0; - case 'rigid' - ty.type = 1; - case 'flexible' - ty.type = 2; - case 'modal-analysis' - ty.type = 3; - case 'init' - ty.type = 4; -end - -% Ty Granite frame -ty.granite_frame.density = 7800; % [kg/m3] => 43kg -ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP'; - -% Guide Translation Ty -ty.guide.density = 7800; % [kg/m3] => 76kg -ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP'; - -% Ty - Guide_Translation12 -ty.guide12.density = 7800; % [kg/m3] -ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP'; - -% Ty - Guide_Translation11 -ty.guide11.density = 7800; % [kg/m3] -ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP'; - -% Ty - Guide_Translation22 -ty.guide22.density = 7800; % [kg/m3] -ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP'; - -% Ty - Guide_Translation21 -ty.guide21.density = 7800; % [kg/m3] -ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP'; - -% Ty - Plateau translation -ty.frame.density = 7800; % [kg/m3] -ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP'; - -% Ty Stator Part -ty.stator.density = 5400; % [kg/m3] -ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP'; - -% Ty Rotor Part -ty.rotor.density = 5400; % [kg/m3] -ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP'; - -ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad] -ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)] - -if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init') - load('mat/Foffset.mat', 'Ftym'); - ty.Deq = -Ftym'./ty.K; -else - ty.Deq = zeros(6,1); -end - -save('./mat/stages.mat', 'ty', '-append'); diff --git a/matlab/src/initializeZAxisAccelerometer.m b/matlab/src/initializeZAxisAccelerometer.m deleted file mode 100644 index 9d17056..0000000 --- a/matlab/src/initializeZAxisAccelerometer.m +++ /dev/null @@ -1,21 +0,0 @@ -function [accelerometer] = initializeZAxisAccelerometer(args) - arguments - args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] - args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz] - end - - %% - accelerometer.m = args.mass; - - %% The Stiffness is set to have the damping resonance frequency - accelerometer.k = accelerometer.m * (2*pi*args.freq)^2; - - %% We set the damping value to have critical damping - accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k); - - %% Gain correction of the accelerometer to have a unity gain until the resonance - accelerometer.gain = -accelerometer.k/accelerometer.m; - - %% Save - save('./mat/accelerometer_z_axis.mat', 'accelerometer'); -end diff --git a/matlab/src/initializeZAxisGeophone.m b/matlab/src/initializeZAxisGeophone.m deleted file mode 100644 index 8612793..0000000 --- a/matlab/src/initializeZAxisGeophone.m +++ /dev/null @@ -1,18 +0,0 @@ -function [geophone] = initializeZAxisGeophone(args) - arguments - args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] - args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz] - end - - %% - geophone.m = args.mass; - - %% The Stiffness is set to have the damping resonance frequency - geophone.k = geophone.m * (2*pi*args.freq)^2; - - %% We set the damping value to have critical damping - geophone.c = 2*sqrt(geophone.m * geophone.k); - - %% Save - save('./mat/geophone_z_axis.mat', 'geophone'); -end diff --git a/matlab/src/inverseKinematics.m b/matlab/src/inverseKinematics.m deleted file mode 100644 index c1c0618..0000000 --- a/matlab/src/inverseKinematics.m +++ /dev/null @@ -1,36 +0,0 @@ -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; diff --git a/matlab/src/prepareLinearizeIdentification.m b/matlab/src/prepareLinearizeIdentification.m deleted file mode 100644 index 3488e7f..0000000 --- a/matlab/src/prepareLinearizeIdentification.m +++ /dev/null @@ -1,27 +0,0 @@ -function [] = prepareLinearizeIdentification(args) - -arguments - args.nass_actuator char {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo' - args.sample_mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg] -end - -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); - -initializeNanoHexapod('actuator', args.nass_actuator); -initializeSample('mass', args.sample_mass); - -initializeReferences(); -initializeDisturbances('enable', false); - -initializeController('type', 'open-loop'); - -initializeSimscapeConfiguration('gravity', true); - -initializeLoggingConfiguration('log', 'none'); diff --git a/matlab/src/prepareTomographyExperiment.m b/matlab/src/prepareTomographyExperiment.m deleted file mode 100644 index 441044d..0000000 --- a/matlab/src/prepareTomographyExperiment.m +++ /dev/null @@ -1,29 +0,0 @@ -function [] = prepareTomographyExperiment(args) - -arguments - args.nass_actuator char {mustBeMember(args.nass_actuator,{'piezo', 'lorentz'})} = 'piezo' - args.sample_mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg] - args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s] -end - -initializeGround(); -initializeGranite(); -initializeTy(); -initializeRy(); -initializeRz(); -initializeMicroHexapod(); -initializeAxisc(); -initializeMirror(); - -initializeNanoHexapod('actuator', args.nass_actuator); -initializeSample('mass', args.sample_mass); - -initializeReferences('Rz_type', 'rotating', 'Rz_period', args.Rz_period); - -initializeDisturbances(); - -initializeController('type', 'open-loop'); - -initializeSimscapeConfiguration('gravity', true); - -initializeLoggingConfiguration('log', 'all'); diff --git a/matlab/src/project_shutdown.m b/matlab/src/project_shutdown.m deleted file mode 100644 index 5bb8d70..0000000 --- a/matlab/src/project_shutdown.m +++ /dev/null @@ -1,5 +0,0 @@ - - -% When the project closes, it runs the =project_shutdown.m= script defined below. - -Simulink.fileGenControl('reset'); diff --git a/matlab/src/project_startup.m b/matlab/src/project_startup.m deleted file mode 100644 index c635e75..0000000 --- a/matlab/src/project_startup.m +++ /dev/null @@ -1,18 +0,0 @@ - - -% When the project opens, a startup script is ran. -% The startup script is defined below and is exported to the =project_startup.m= script. - -project = simulinkproject; -projectRoot = project.RootFolder; - -myCacheFolder = fullfile(projectRoot, '.SimulinkCache'); -myCodeFolder = fullfile(projectRoot, '.SimulinkCode'); - -Simulink.fileGenControl('set',... - 'CacheFolder', myCacheFolder,... - 'CodeGenFolder', myCodeFolder,... - 'createDir', true); - -%% Load the Simscape Configuration -load('mat/conf_simulink.mat'); diff --git a/matlab/test_nhexa_1_suspended_table.m b/matlab/test_nhexa_1_suspended_table.m index cf189e0..f87e3bb 100644 --- a/matlab/test_nhexa_1_suspended_table.m +++ b/matlab/test_nhexa_1_suspended_table.m @@ -20,7 +20,7 @@ device_type = 'None'; % On top of vibration table payload_num = 0; % No Payload % Simulink Model name -mdl = 'test_bench_nano_hexapod'; +mdl = 'test_nhexa_simscape'; %% Colors for the figures colors = colororder; diff --git a/matlab/test_nhexa_3_model.m b/matlab/test_nhexa_3_model.m index 098b086..3cad41a 100644 --- a/matlab/test_nhexa_3_model.m +++ b/matlab/test_nhexa_3_model.m @@ -21,7 +21,7 @@ device_type = 'None'; % On top of vibration table payload_num = 0; % No Payload % Simulink Model name -mdl = 'test_bench_nano_hexapod'; +mdl = 'test_nhexa_simscape'; %% Colors for the figures colors = colororder; @@ -31,66 +31,73 @@ freqs = logspace(log10(10), log10(2e3), 1000); % Extract transfer function matrices from the Simscape Model :noexport: -%% Extract the transfer function matrix from the Simscape model -% Initialization of the Simscape model -table_type = 'Suspended'; % On top of vibration table -device_type = 'Hexapod'; % Nano-Hexapod -payload_num = 0; % No Payload +%% Set to true only if all the dynamics should again computed +% from the simscape model +extract_simscape_dynamics = false +if extract_simscape_dynamics -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); + %% Extract the transfer function matrix from the Simscape model + % Initialization of the Simscape model + table_type = 'Suspended'; % On top of vibration table + device_type = 'Hexapod'; % Nano-Hexapod + payload_num = 0; % No Payload -% Identify the FRF matrix from u to [de,Vs] -clear io; io_i = 1; -io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders -io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders + n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof'); -G_de = {}; -G_Vs = {}; + % Identify the FRF matrix from u to [de,Vs] + clear io; io_i = 1; + io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs + io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders + io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders + + G_de = {}; + G_Vs = {}; + + for i = [0:3] + payload_num = i; % Change the payload on the nano-hexapod + G = exp(-s*1e-4)*linearize(mdl, io, 0.0); + G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; + G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... + 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; + G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; + G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; + end + + % Save the identified plants + save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de') + + %% The same identification is performed, but this time with + % "flexible" model of the APA + table_type = 'Suspended'; % On top of vibration table + device_type = 'Hexapod'; % Nano-Hexapod + payload_num = 0; % No Payload + + n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible'); + + G_de = {}; + G_Vs = {}; + + for i = [0:3] + payload_num = i; % Change the payload on the nano-hexapod + G = exp(-s*1e-4)*linearize(mdl, io, 0.0); + G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; + G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... + 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; + G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; + G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; + end + + % Save the identified plants + save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de') -for i = [0:3] - payload_num = i; % Change the payload on the nano-hexapod - G = exp(-s*1e-4)*linearize(mdl, io, 0.0); - G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; - G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... - 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; - G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; - G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; end -% Save the identified plants -save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de') - -%% The same identification is performed, but this time with -% "flexible" model of the APA -table_type = 'Suspended'; % On top of vibration table -device_type = 'Hexapod'; % Nano-Hexapod -payload_num = 0; % No Payload - -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', 'flexible'); - -G_de = {}; -G_Vs = {}; - -for i = [0:3] - payload_num = i; % Change the payload on the nano-hexapod - G = exp(-s*1e-4)*linearize(mdl, io, 0.0); - G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; - G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... - 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; - G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; - G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; -end - -% Save the identified plants -save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de') - % Nano-Hexapod model dynamics % <> diff --git a/matlab/test_bench_nano_hexapod.slx b/matlab/test_nhexa_simscape.slx similarity index 100% rename from matlab/test_bench_nano_hexapod.slx rename to matlab/test_nhexa_simscape.slx diff --git a/test-bench-nano-hexapod.org b/test-bench-nano-hexapod.org index d5790cb..5a5dd7a 100644 --- a/test-bench-nano-hexapod.org +++ b/test-bench-nano-hexapod.org @@ -1109,82 +1109,93 @@ This is checked in Section ref:ssec:test_nhexa_comp_model_masses. #+end_src ** Extract transfer function matrices from the Simscape Model :noexport: -#+begin_src matlab -%% Extract the transfer function matrix from the Simscape model -% Initialization of the Simscape model -table_type = 'Suspended'; % On top of vibration table -device_type = 'Hexapod'; % Nano-Hexapod -payload_num = 0; % No Payload - -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -% Identify the FRF matrix from u to [de,Vs] -clear io; io_i = 1; -io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders -io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders - -G_de = {}; -G_Vs = {}; - -for i = [0:3] - payload_num = i; % Change the payload on the nano-hexapod - G = exp(-s*1e-4)*linearize(mdl, io, 0.0); - G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; - G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... - 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; - G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; - G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; -end -#+end_src - -#+begin_src matlab :exports none :tangle no -% Save the identified plants -save('matlab/mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de') -#+end_src - #+begin_src matlab :eval no -% Save the identified plants -save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de') +%% Set to true only if all the dynamics should again computed +% from the simscape model +extract_simscape_dynamics = false; +if extract_simscape_dynamics #+end_src #+begin_src matlab -%% The same identification is performed, but this time with -% "flexible" model of the APA -table_type = 'Suspended'; % On top of vibration table -device_type = 'Hexapod'; % Nano-Hexapod -payload_num = 0; % No Payload + %% Extract the transfer function matrix from the Simscape model + % Initialization of the Simscape model + table_type = 'Suspended'; % On top of vibration table + device_type = 'Hexapod'; % Nano-Hexapod + payload_num = 0; % No Payload -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', 'flexible'); + n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof'); -G_de = {}; -G_Vs = {}; + % Identify the FRF matrix from u to [de,Vs] + clear io; io_i = 1; + io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs + io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoders + io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Encoders -for i = [0:3] - payload_num = i; % Change the payload on the nano-hexapod - G = exp(-s*1e-4)*linearize(mdl, io, 0.0); - G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; - G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... - 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; - G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; - G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; -end + G_de = {}; + G_Vs = {}; + + for i = [0:3] + payload_num = i; % Change the payload on the nano-hexapod + G = exp(-s*1e-4)*linearize(mdl, io, 0.0); + G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; + G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... + 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; + G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; + G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; + end #+end_src #+begin_src matlab :exports none :tangle no -% Save the identified plants -save('matlab/mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de') + % Save the identified plants + save('matlab/mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de') #+end_src #+begin_src matlab :eval no -% Save the identified plants -save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de') + % Save the identified plants + save('./mat/test_nhexa_simscape_masses.mat', 'G_Vs', 'G_de') +#+end_src + +#+begin_src matlab + %% The same identification is performed, but this time with + % "flexible" model of the APA + table_type = 'Suspended'; % On top of vibration table + device_type = 'Hexapod'; % Nano-Hexapod + payload_num = 0; % No Payload + + n_hexapod = initializeNanoHexapod('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible'); + + G_de = {}; + G_Vs = {}; + + for i = [0:3] + payload_num = i; % Change the payload on the nano-hexapod + G = exp(-s*1e-4)*linearize(mdl, io, 0.0); + G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; + G.OutputName = {'de1', 'de2', 'de3', 'de4', 'de5', 'de6', ... + 'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'}; + G_de(i+1) = {G({'de1', 'de2', 'de3', 'de4', 'de5', 'de6'},:)}; + G_Vs(i+1) = {G({'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6'},:)}; + end +#+end_src + +#+begin_src matlab :exports none :tangle no + % Save the identified plants + save('matlab/mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de') +#+end_src + +#+begin_src matlab :eval no + % Save the identified plants + save('./mat/test_nhexa_simscape_flexible_masses.mat', 'G_Vs', 'G_de') +#+end_src + +#+begin_src matlab :eval no +end #+end_src ** Nano-Hexapod model dynamics @@ -1773,7 +1784,7 @@ device_type = 'None'; % On top of vibration table payload_num = 0; % No Payload % Simulink Model name -mdl = 'test_bench_nano_hexapod'; +mdl = 'test_nhexa_simscape'; #+end_src ** Initialize other elements @@ -1786,6 +1797,394 @@ colors = colororder; freqs = logspace(log10(10), log10(2e3), 1000); #+END_SRC +** =initializeNanoHexapod= +#+begin_src matlab :tangle matlab/src/initializeNanoHexapod.m :comments none :mkdirp yes :eval no +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 +#+end_src + +** =extractNodes= +#+begin_src matlab :tangle matlab/src/extractNodes.m :comments none :mkdirp yes :eval no +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 +#+end_src + * Footnotes [fn:7]PCB 356B18. Sensitivity is $1\,V/g$, measurement range is $\pm 5\,g$ and bandwidth is $0.5$ to $5\,\text{kHz}$.