Add Compliance and Transmissibility computation

Change solid to rigid

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)
+  <<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+  <<matlab-init>>
+#+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)
+  <<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+  <<matlab-init>>
+#+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:
+<<sec:computeTransmissibility>>
+
+*** 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:
+<<sec:computeCompliance>>
+
+*** 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

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
+  arguments
-      args.type char {mustBeMember(args.type,{'none', 'solid', 'flexible'})} = 'none'
+    args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'none'
-      args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(3,1)
+    args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1)
-      args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(3,1)
+    args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(3,1)
-    end
+    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

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

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

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;

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;