Add Compliance and Transmissibility computation
Change solid to rigid
This commit is contained in:
parent
a83598d7dd
commit
1321d12e4d
Binary file not shown.
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
83
src/computeCompliance.m
Normal file
83
src/computeCompliance.m
Normal file
@ -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
|
||||
84
src/computeTransmissibility.m
Normal file
84
src/computeTransmissibility.m
Normal file
@ -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
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user