diff --git a/matlab/stewart_platform_model.slx b/matlab/stewart_platform_model.slx index 660b1ce..88f2566 100644 Binary files a/matlab/stewart_platform_model.slx and b/matlab/stewart_platform_model.slx differ diff --git a/org/control-study.org b/org/control-study.org index df904e9..11947cc 100644 --- a/org/control-study.org +++ b/org/control-study.org @@ -259,3 +259,458 @@ A lead is added around the crossover frequency which is set to be around 500Hz. linkaxes([ax1,ax2],'x'); #+end_src +* Transmissibility Analysis +** Matlab Init :noexport: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + simulinkproject('../'); +#+end_src + +#+begin_src matlab + open('stewart_platform_model.slx') +#+end_src + +** Initialize the Stewart platform +#+begin_src matlab + stewart = initializeStewartPlatform(); + stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3); + stewart = generateGeneralConfiguration(stewart); + stewart = computeJointsPose(stewart); + stewart = initializeStrutDynamics(stewart); + stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p'); + stewart = initializeCylindricalPlatforms(stewart); + stewart = initializeCylindricalStruts(stewart); + stewart = computeJacobian(stewart); + stewart = initializeStewartPose(stewart); + stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3); +#+end_src + +We set the rotation point of the ground to be at the same point at frames $\{A\}$ and $\{B\}$. +#+begin_src matlab + ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A); + payload = initializePayload('type', 'rigid'); +#+end_src + +** Transmissibility +#+begin_src matlab + %% Options for Linearized + options = linearizeOptions; + options.SampleTime = 0; + + %% Name of the Simulink File + mdl = 'stewart_platform_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad] + io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad] + + %% Run the linearization + T = linearize(mdl, io, options); + T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'}; + T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}; +#+end_src + +#+begin_src matlab + freqs = logspace(1, 4, 1000); + + figure; + for ix = 1:6 + for iy = 1:6 + subplot(6, 6, (ix-1)*6 + iy); + hold on; + plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-5, 10]); + xlim([freqs(1), freqs(end)]); + if ix < 6 + xticklabels({}); + end + if iy > 1 + yticklabels({}); + end + end + end +#+end_src + +From cite:preumont07_six_axis_singl_stage_activ, one can use the Frobenius norm of the transmissibility matrix to obtain a scalar indicator of the transmissibility performance of the system: +\begin{align*} + \| \bm{T}(\omega) \| &= \sqrt{\text{Trace}[\bm{T}(\omega) \bm{T}(\omega)^H]}\\ + &= \sqrt{\Sigma_{i=1}^6 \Sigma_{j=1}^6 |T_{ij}|^2} +\end{align*} + +#+begin_src matlab + freqs = logspace(1, 4, 1000); + + T_norm = zeros(length(freqs), 1); + + for i = 1:length(freqs) + T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')')); + end +#+end_src + +And we normalize by a factor $\sqrt{6}$ to obtain a performance metric comparable to the transmissibility of a one-axis isolator: +\[ \Gamma(\omega) = \|\bm{T}(\omega)\| / \sqrt{6} \] + +#+begin_src matlab + Gamma = T_norm/sqrt(6); +#+end_src + +#+begin_src matlab + figure; + plot(freqs, Gamma) + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +#+end_src + +* Compliance Analysis +** Matlab Init :noexport: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + simulinkproject('../'); +#+end_src + +#+begin_src matlab + open('stewart_platform_model.slx') +#+end_src + +** Initialize the Stewart platform +#+begin_src matlab + stewart = initializeStewartPlatform(); + stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3); + stewart = generateGeneralConfiguration(stewart); + stewart = computeJointsPose(stewart); + stewart = initializeStrutDynamics(stewart); + stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p'); + stewart = initializeCylindricalPlatforms(stewart); + stewart = initializeCylindricalStruts(stewart); + stewart = computeJacobian(stewart); + stewart = initializeStewartPose(stewart); + stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3); +#+end_src + +We set the rotation point of the ground to be at the same point at frames $\{A\}$ and $\{B\}$. +#+begin_src matlab + ground = initializeGround('type', 'none'); + payload = initializePayload('type', 'rigid'); +#+end_src + +** Compliance +#+begin_src matlab + %% Options for Linearized + options = linearizeOptions; + options.SampleTime = 0; + + %% Name of the Simulink File + mdl = 'stewart_platform_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad] + io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad] + + %% Run the linearization + C = linearize(mdl, io, options); + C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; + C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}; +#+end_src + +#+begin_src matlab + freqs = logspace(1, 4, 1000); + + figure; + for ix = 1:6 + for iy = 1:6 + subplot(6, 6, (ix-1)*6 + iy); + hold on; + plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-10, 1e-3]); + xlim([freqs(1), freqs(end)]); + if ix < 6 + xticklabels({}); + end + if iy > 1 + yticklabels({}); + end + end + end +#+end_src + +We can try to use the Frobenius norm to obtain a scalar value representing the 6-dof compliance of the Stewart platform. + +#+begin_src matlab + freqs = logspace(1, 4, 1000); + + C_norm = zeros(length(freqs), 1); + + for i = 1:length(freqs) + C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')')); + end +#+end_src + +#+begin_src matlab + figure; + plot(freqs, C_norm) + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +#+end_src +* Functions +** Compute the Transmissibility +:PROPERTIES: +:header-args:matlab+: :tangle ../src/computeTransmissibility.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + function [T, T_norm, freqs] = computeTransmissibility(args) + % computeTransmissibility - + % + % Syntax: [T, T_norm, freqs] = computeTransmissibility(args) + % + % Inputs: + % - args - Structure with the following fields: + % - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm + % - freqs [] - Frequency vector to estimate the Frobenius norm + % + % Outputs: + % - T [6x6 ss] - Transmissibility matrix + % - T_norm [length(freqs)x1] - Frobenius norm of the Transmissibility matrix + % - freqs [length(freqs)x1] - Frequency vector in [Hz] +#+end_src + +*** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + arguments + args.plots logical {mustBeNumericOrLogical} = false + args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000) + end +#+end_src + +#+begin_src matlab + freqs = args.freqs; +#+end_src + +*** Identification of the Transmissibility Matrix +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + %% Options for Linearized + options = linearizeOptions; + options.SampleTime = 0; + + %% Name of the Simulink File + mdl = 'stewart_platform_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad] + io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad] + + %% Run the linearization + T = linearize(mdl, io, options); + T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'}; + T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}; +#+end_src + +If wanted, the 6x6 transmissibility matrix is plotted. +#+begin_src matlab + p_handle = zeros(6*6,1); + + if args.plots + fig = figure; + for ix = 1:6 + for iy = 1:6 + p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy); + hold on; + plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + if ix < 6 + xticklabels({}); + end + if iy > 1 + yticklabels({}); + end + end + end + + linkaxes(p_handle, 'xy') + xlim([freqs(1), freqs(end)]); + ylim([1e-5, 1e2]); + + han = axes(fig, 'visible', 'off'); + han.XLabel.Visible = 'on'; + han.YLabel.Visible = 'on'; + ylabel(han, 'Frequency [Hz]'); + xlabel(han, 'Transmissibility [m/m]'); + end +#+end_src + +*** Computation of the Frobenius norm +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + T_norm = zeros(length(freqs), 1); + + for i = 1:length(freqs) + T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')')); + end +#+end_src + +#+begin_src matlab + T_norm = T_norm/sqrt(6); +#+end_src + +#+begin_src matlab + if args.plots + figure; + plot(freqs, T_norm) + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); + ylabel('Transmissibility - Frobenius Norm'); + end +#+end_src + +** Compute the Compliance +:PROPERTIES: +:header-args:matlab+: :tangle ../src/computeCompliance.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + function [C, C_norm, freqs] = computeCompliance(args) + % computeCompliance - + % + % Syntax: [C, C_norm, freqs] = computeCompliance(args) + % + % Inputs: + % - args - Structure with the following fields: + % - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm + % - freqs [] - Frequency vector to estimate the Frobenius norm + % + % Outputs: + % - C [6x6 ss] - Compliance matrix + % - C_norm [length(freqs)x1] - Frobenius norm of the Compliance matrix + % - freqs [length(freqs)x1] - Frequency vector in [Hz] +#+end_src + +*** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + arguments + args.plots logical {mustBeNumericOrLogical} = false + args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000) + end +#+end_src + +#+begin_src matlab + freqs = args.freqs; +#+end_src + +*** Identification of the Compliance Matrix +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + %% Options for Linearized + options = linearizeOptions; + options.SampleTime = 0; + + %% Name of the Simulink File + mdl = 'stewart_platform_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % External forces [N, N*m] + io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad] + + %% Run the linearization + C = linearize(mdl, io, options); + C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; + C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}; +#+end_src + +If wanted, the 6x6 transmissibility matrix is plotted. +#+begin_src matlab + p_handle = zeros(6*6,1); + + if args.plots + fig = figure; + for ix = 1:6 + for iy = 1:6 + p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy); + hold on; + plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + if ix < 6 + xticklabels({}); + end + if iy > 1 + yticklabels({}); + end + end + end + + linkaxes(p_handle, 'xy') + xlim([freqs(1), freqs(end)]); + + han = axes(fig, 'visible', 'off'); + han.XLabel.Visible = 'on'; + han.YLabel.Visible = 'on'; + xlabel(han, 'Frequency [Hz]'); + ylabel(han, 'Compliance [m/N, rad/(N*m)]'); + end +#+end_src + +*** Computation of the Frobenius norm +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + freqs = args.freqs; + + C_norm = zeros(length(freqs), 1); + + for i = 1:length(freqs) + C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')')); + end +#+end_src + +#+begin_src matlab + if args.plots + figure; + plot(freqs, C_norm) + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); + ylabel('Compliance - Frobenius Norm'); + end +#+end_src diff --git a/org/simscape-model.org b/org/simscape-model.org index caef737..5f2268a 100644 --- a/org/simscape-model.org +++ b/org/simscape-model.org @@ -170,7 +170,7 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]]. % % Inputs: % - args - Structure with the following fields: - % - type - 'none', 'solid', 'flexible', 'cartesian' + % - type - 'none', 'rigid', 'flexible', 'cartesian' % - h [1x1] - Height of the CoM of the payload w.r.t {M} [m] % This also the position where K and C are defined % - K [6x1] - Stiffness of the Payload [N/m, N/rad] @@ -180,7 +180,7 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]]. % % Outputs: % - payload - Struture with the following properties: - % - type - 1 (none), 2 (solid), 3 (flexible) + % - type - 1 (none), 2 (rigid), 3 (flexible) % - h [1x1] - Height of the CoM of the payload w.r.t {M} [m] % - K [6x1] - Stiffness of the Payload [N/m, N/rad] % - C [6x1] - Stiffness of the Payload [N/(m/s), N/(rad/s)] @@ -194,7 +194,7 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]]. :END: #+begin_src matlab arguments - args.type char {mustBeMember(args.type,{'none', 'solid', 'flexible', 'cartesian'})} = 'none' + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'cartesian'})} = 'none' args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(6,1) args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) args.h (1,1) double {mustBeNumeric, mustBeNonnegative} = 100e-3 @@ -211,7 +211,7 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]]. switch args.type case 'none' payload.type = 1; - case 'solid' + case 'rigid' payload.type = 2; case 'flexible' payload.type = 3; @@ -255,12 +255,13 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]]. % Inputs: % - args - Structure with the following fields: % - type - 'none', 'solid', 'flexible' + % - rot_point [3x1] - Rotation point for the ground motion [m] % - K [3x1] - Translation Stiffness of the Ground [N/m] % - C [3x1] - Translation Damping of the Ground [N/(m/s)] % % Outputs: % - ground - Struture with the following properties: - % - type - 1 (none), 2 (solid), 3 (flexible) + % - type - 1 (none), 2 (rigid), 3 (flexible) % - K [3x1] - Translation Stiffness of the Ground [N/m] % - C [3x1] - Translation Damping of the Ground [N/(m/s)] #+end_src @@ -270,11 +271,12 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]]. :UNNUMBERED: t :END: #+begin_src matlab - arguments - args.type char {mustBeMember(args.type,{'none', 'solid', 'flexible'})} = 'none' - args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(3,1) - args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(3,1) - end + arguments + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'none' + args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) + args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(3,1) + args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(3,1) + end #+end_src *** Add Ground Type @@ -285,7 +287,7 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]]. switch args.type case 'none' ground.type = 1; - case 'solid' + case 'rigid' ground.type = 2; case 'flexible' ground.type = 3; @@ -301,3 +303,10 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]]. ground.C = args.C; #+end_src +*** Rotation Point +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + ground.rot_point = args.rot_point; +#+end_src diff --git a/src/computeCompliance.m b/src/computeCompliance.m new file mode 100644 index 0000000..191cfdd --- /dev/null +++ b/src/computeCompliance.m @@ -0,0 +1,83 @@ +function [C, C_norm, freqs] = computeCompliance(args) +% computeCompliance - +% +% Syntax: [C, C_norm, freqs] = computeCompliance(args) +% +% Inputs: +% - args - Structure with the following fields: +% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm +% - freqs [] - Frequency vector to estimate the Frobenius norm +% +% Outputs: +% - C [6x6 ss] - Compliance matrix +% - C_norm [length(freqs)x1] - Frobenius norm of the Compliance matrix +% - freqs [length(freqs)x1] - Frequency vector in [Hz] + +arguments + args.plots logical {mustBeNumericOrLogical} = false + args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000) +end + +freqs = args.freqs; + +%% Options for Linearized +options = linearizeOptions; +options.SampleTime = 0; + +%% Name of the Simulink File +mdl = 'stewart_platform_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % External forces [N, N*m] +io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad] + +%% Run the linearization +C = linearize(mdl, io, options); +C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}; +C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}; + +p_handle = zeros(6*6,1); + +if args.plots + fig = figure; + for ix = 1:6 + for iy = 1:6 + p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy); + hold on; + plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + if ix < 6 + xticklabels({}); + end + if iy > 1 + yticklabels({}); + end + end + end + + linkaxes(p_handle, 'xy') + xlim([freqs(1), freqs(end)]); + + han = axes(fig, 'visible', 'off'); + han.XLabel.Visible = 'on'; + han.YLabel.Visible = 'on'; + xlabel(han, 'Frequency [Hz]'); + ylabel(han, 'Compliance [m/N, rad/(N*m)]'); +end + +freqs = args.freqs; + +C_norm = zeros(length(freqs), 1); + +for i = 1:length(freqs) + C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')')); +end + +if args.plots + figure; + plot(freqs, C_norm) + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); + ylabel('Compliance - Frobenius Norm'); +end diff --git a/src/computeTransmissibility.m b/src/computeTransmissibility.m new file mode 100644 index 0000000..263e806 --- /dev/null +++ b/src/computeTransmissibility.m @@ -0,0 +1,84 @@ +function [T, T_norm, freqs] = computeTransmissibility(args) +% computeTransmissibility - +% +% Syntax: [T, T_norm, freqs] = computeTransmissibility(args) +% +% Inputs: +% - args - Structure with the following fields: +% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm +% - freqs [] - Frequency vector to estimate the Frobenius norm +% +% Outputs: +% - T [6x6 ss] - Transmissibility matrix +% - T_norm [length(freqs)x1] - Frobenius norm of the Transmissibility matrix +% - freqs [length(freqs)x1] - Frequency vector in [Hz] + +arguments + args.plots logical {mustBeNumericOrLogical} = false + args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000) +end + +freqs = args.freqs; + +%% Options for Linearized +options = linearizeOptions; +options.SampleTime = 0; + +%% Name of the Simulink File +mdl = 'stewart_platform_model'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad] +io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad] + +%% Run the linearization +T = linearize(mdl, io, options); +T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'}; +T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}; + +p_handle = zeros(6*6,1); + +if args.plots + fig = figure; + for ix = 1:6 + for iy = 1:6 + p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy); + hold on; + plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-'); + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + if ix < 6 + xticklabels({}); + end + if iy > 1 + yticklabels({}); + end + end + end + + linkaxes(p_handle, 'xy') + xlim([freqs(1), freqs(end)]); + ylim([1e-5, 1e2]); + + han = axes(fig, 'visible', 'off'); + han.XLabel.Visible = 'on'; + han.YLabel.Visible = 'on'; + ylabel(han, 'Frequency [Hz]'); + xlabel(han, 'Transmissibility [m/m]'); +end + +T_norm = zeros(length(freqs), 1); + +for i = 1:length(freqs) + T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')')); +end + +T_norm = T_norm/sqrt(6); + +if args.plots + figure; + plot(freqs, T_norm) + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + xlabel('Frequency [Hz]'); + ylabel('Transmissibility - Frobenius Norm'); +end diff --git a/src/initializeGround.m b/src/initializeGround.m index decf394..ec0024b 100644 --- a/src/initializeGround.m +++ b/src/initializeGround.m @@ -6,17 +6,19 @@ function [ground] = initializeGround(args) % Inputs: % - args - Structure with the following fields: % - type - 'none', 'solid', 'flexible' +% - rot_point [3x1] - Rotation point for the ground motion [m] % - K [3x1] - Translation Stiffness of the Ground [N/m] % - C [3x1] - Translation Damping of the Ground [N/(m/s)] % % Outputs: % - ground - Struture with the following properties: -% - type - 1 (none), 2 (solid), 3 (flexible) +% - type - 1 (none), 2 (rigid), 3 (flexible) % - K [3x1] - Translation Stiffness of the Ground [N/m] % - C [3x1] - Translation Damping of the Ground [N/(m/s)] arguments - args.type char {mustBeMember(args.type,{'none', 'solid', 'flexible'})} = 'none' + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'none' + args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(3,1) args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(3,1) end @@ -24,7 +26,7 @@ end switch args.type case 'none' ground.type = 1; - case 'solid' + case 'rigid' ground.type = 2; case 'flexible' ground.type = 3; @@ -32,3 +34,5 @@ end ground.K = args.K; ground.C = args.C; + +ground.rot_point = args.rot_point; diff --git a/src/initializePayload.m b/src/initializePayload.m index 171766c..a8d5f76 100644 --- a/src/initializePayload.m +++ b/src/initializePayload.m @@ -5,7 +5,7 @@ function [payload] = initializePayload(args) % % Inputs: % - args - Structure with the following fields: -% - type - 'none', 'solid', 'flexible', 'cartesian' +% - type - 'none', 'rigid', 'flexible', 'cartesian' % - h [1x1] - Height of the CoM of the payload w.r.t {M} [m] % This also the position where K and C are defined % - K [6x1] - Stiffness of the Payload [N/m, N/rad] @@ -15,7 +15,7 @@ function [payload] = initializePayload(args) % % Outputs: % - payload - Struture with the following properties: -% - type - 1 (none), 2 (solid), 3 (flexible) +% - type - 1 (none), 2 (rigid), 3 (flexible) % - h [1x1] - Height of the CoM of the payload w.r.t {M} [m] % - K [6x1] - Stiffness of the Payload [N/m, N/rad] % - C [6x1] - Stiffness of the Payload [N/(m/s), N/(rad/s)] @@ -23,7 +23,7 @@ function [payload] = initializePayload(args) % - I [3x3] - Inertia matrix for the Payload [kg*m2] arguments - args.type char {mustBeMember(args.type,{'none', 'solid', 'flexible', 'cartesian'})} = 'none' + args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'cartesian'})} = 'none' args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(6,1) args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) args.h (1,1) double {mustBeNumeric, mustBeNonnegative} = 100e-3 @@ -34,7 +34,7 @@ end switch args.type case 'none' payload.type = 1; - case 'solid' + case 'rigid' payload.type = 2; case 'flexible' payload.type = 3;