84 lines
2.3 KiB
Matlab
84 lines
2.3 KiB
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]
|
|
|
|
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
|