598 lines
17 KiB
Mathematica
598 lines
17 KiB
Mathematica
|
%% Clear Workspace and Close figures
|
||
|
|
clear; close all; clc;
|
||
|
|
|
||
|
|
%% Intialize Laplace variable
|
||
|
|
s = zpk('s');
|
||
|
|
|
||
|
|
%% Add useful folders to the path
|
||
|
|
addpath('test_bench_struts/');
|
||
|
|
addpath('png/');
|
||
|
|
addpath('mat/');
|
||
|
|
addpath('src/');
|
||
|
|
|
||
|
|
%% Frequency vector used for many plots
|
||
|
|
freqs = 2*logspace(0, 3, 1000);
|
||
|
|
|
||
|
|
%% Options for Linearized
|
||
|
|
options = linearizeOptions;
|
||
|
|
options.SampleTime = 0;
|
||
|
|
|
||
|
|
%% Name of the Simulink File
|
||
|
|
mdl = 'test_bench_struts';
|
||
|
|
|
||
|
|
%% Open the Simulink File
|
||
|
|
open(mdl)
|
||
|
|
|
||
|
|
%% Initialize structure containing data for the Simscape model
|
||
|
|
n_hexapod = struct();
|
||
|
|
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
|
||
|
|
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
|
||
|
|
n_hexapod.actuator = initializeAPA('type', '2dof');
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
|
||
|
|
io(io_i) = linio([mdl, '/Vs'], 1, 'openoutput'); io_i = io_i + 1; % Sensor Voltage
|
||
|
|
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||
|
|
io(io_i) = linio([mdl, '/da'], 1, 'openoutput'); io_i = io_i + 1; % Interferometer
|
||
|
|
|
||
|
|
%% Run the linearization
|
||
|
|
Gs = linearize(mdl, io, 0.0, options);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'Vs', 'de', 'da'};
|
||
|
|
|
||
|
|
%% Bode plot of the transfer functions
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Encoder')
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), 'DisplayName', 'Interferometer')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
legend('location', 'southwest');
|
||
|
|
|
||
|
|
ax1b = nexttile([2,1]);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), 'k-')
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))))
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))))
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:45:360);
|
||
|
|
ylim([-180, 180])
|
||
|
|
|
||
|
|
ax2b = nexttile;
|
||
|
|
hold on;
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), 'k-')
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:45:360);
|
||
|
|
ylim([0, 180])
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax1b,ax2b],'x');
|
||
|
|
xlim([10, 2e3]);
|
||
|
|
|
||
|
|
%% Load measured FRF
|
||
|
|
load('meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
|
||
|
|
|
||
|
|
%% Add time delay to the Simscape model
|
||
|
|
Gs = exp(-s*Ts)*Gs;
|
||
|
|
|
||
|
|
%% Compare the FRF and identified dynamics from Va to Vs and da
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(int_frf(:, 1)), 'color', [0,0,0,0.2], ...
|
||
|
|
'DisplayName', 'Meas. FRF');
|
||
|
|
for i = 2:length(leg_nums)
|
||
|
|
plot(f, abs(int_frf(:, i)), 'color', [0,0,0,0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), '-', ...
|
||
|
|
'DisplayName', 'Model')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_a/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax1b = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
|
||
|
|
'DisplayName', 'Meas. FRF');
|
||
|
|
for i = 1:length(leg_nums)
|
||
|
|
plot(f, abs(iff_frf(:, i)), 'color', [0,0,0,0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-', ...
|
||
|
|
'DisplayName', 'Model')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-2, 1e2]);
|
||
|
|
legend('location', 'southeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(leg_nums)
|
||
|
|
plot(f, 180/pi*angle(int_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('da', 'Va'), freqs, 'Hz'))), '-')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
ax2b = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(leg_nums)
|
||
|
|
plot(f, 180/pi*angle(iff_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('Vs', 'Va'), freqs, 'Hz'))), '-')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax1b,ax2b],'x');
|
||
|
|
xlim([10, 2e3]);
|
||
|
|
|
||
|
|
%% Compare the FRF and identified dynamics from Va to de
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, 1)), 'color', [0,0,0,0.2], ...
|
||
|
|
'DisplayName', 'Meas. FRF');
|
||
|
|
for i = 2:length(leg_nums)
|
||
|
|
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-', ...
|
||
|
|
'DisplayName', 'Model')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(leg_nums)
|
||
|
|
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([20, 2e3]);
|
||
|
|
|
||
|
|
%% Load measured FRF of the struts
|
||
|
|
load('meas_struts_frf.mat', 'f', 'Ts', 'enc_frf', 'int_frf', 'iff_frf', 'leg_nums');
|
||
|
|
|
||
|
|
%% Initialize Simscape data
|
||
|
|
n_hexapod.flex_bot = initializeBotFlexibleJoint('type', '4dof');
|
||
|
|
n_hexapod.flex_top = initializeTopFlexibleJoint('type', '4dof');
|
||
|
|
n_hexapod.actuator = initializeAPA('type', 'flexible');
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
|
||
|
|
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||
|
|
|
||
|
|
%% Identification
|
||
|
|
Gs = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
Gs.InputName = {'Va'};
|
||
|
|
Gs.OutputName = {'de'};
|
||
|
|
|
||
|
|
%% Measured FRF from Vs to de and identified dynamics using the flexible APA
|
||
|
|
freqs = 2*logspace(0, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
|
||
|
|
'DisplayName', 'Meas. FRF');
|
||
|
|
for i = 2:length(leg_nums)
|
||
|
|
plot(f, abs(enc_frf(:, i)), 'color', [0,0,0,0.2], ...
|
||
|
|
'HandleVisibility', 'off');
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-', ...
|
||
|
|
'DisplayName', 'Model')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(leg_nums)
|
||
|
|
plot(f, 180/pi*angle(enc_frf(:, i)), 'color', [0,0,0,0.2]);
|
||
|
|
end
|
||
|
|
set(gca,'ColorOrderIndex',1);
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs('de', 'Va'), freqs, 'Hz'))), '-')
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([10, 2e3]);
|
||
|
|
|
||
|
|
%% Considered misalignments
|
||
|
|
dy_aligns = [-0.5, -0.1, 0, 0.1, 0.5]*1e-3; % [m]
|
||
|
|
|
||
|
|
%% Transfer functions from u to de for all the misalignment in y direction
|
||
|
|
Gs_align = {zeros(length(dy_aligns), 1)};
|
||
|
|
|
||
|
|
for i = 1:length(dy_aligns)
|
||
|
|
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [0; dy_aligns(i); 0]);
|
||
|
|
|
||
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
G.InputName = {'Va'};
|
||
|
|
G.OutputName = {'de'};
|
||
|
|
|
||
|
|
Gs_align(i) = {G};
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Transfer function from Vs to de - effect of x-misalignment
|
||
|
|
freqs = 2*logspace(0, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(dy_aligns)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))), ...
|
||
|
|
'DisplayName', sprintf('$d_y = %.1f$ [mm]', 1e3*dy_aligns(i)));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(dy_aligns)
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([10, 2e3]);
|
||
|
|
|
||
|
|
%% Considered misalignments
|
||
|
|
dx_aligns = [-0.1, -0.05, 0, 0.05, 0.1]*1e-3; % [m]
|
||
|
|
|
||
|
|
%% Transfer functions from u to de for all the misalignment in x direction
|
||
|
|
Gs_align = {zeros(length(dx_aligns), 1)};
|
||
|
|
|
||
|
|
for i = 1:length(dx_aligns)
|
||
|
|
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [dx_aligns(i); 0; 0]);
|
||
|
|
|
||
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
G.InputName = {'Va'};
|
||
|
|
G.OutputName = {'de'};
|
||
|
|
|
||
|
|
Gs_align(i) = {G};
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Transfer function from Vs to de - effect of x-misalignment
|
||
|
|
freqs = 2*logspace(0, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(dx_aligns)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))), ...
|
||
|
|
'DisplayName', sprintf('$d_x = %.2f$ [mm]', 1e3*dx_aligns(i)));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(dx_aligns)
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs_align{i}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([10, 2e3]);
|
||
|
|
|
||
|
|
%% Tuned misalignment [m]
|
||
|
|
d_aligns = [[-0.05, -0.3, 0];
|
||
|
|
[ 0, 0.5, 0];
|
||
|
|
[-0.1, -0.3, 0];
|
||
|
|
[ 0, 0.3, 0];
|
||
|
|
[-0.05, 0.05, 0]]'*1e-3;
|
||
|
|
|
||
|
|
%% Idenfity the transfer function from actuator to encoder for all cases
|
||
|
|
Gs_align = {zeros(size(d_aligns,2), 1)};
|
||
|
|
|
||
|
|
for i = 1:size(d_aligns,2)
|
||
|
|
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', d_aligns(:,i));
|
||
|
|
|
||
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
G.InputName = {'Va'};
|
||
|
|
G.OutputName = {'de'};
|
||
|
|
|
||
|
|
Gs_align(i) = {G};
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Comparison of the plants (encoder output) when tuning the misalignment
|
||
|
|
freqs = 2*logspace(0, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
||
|
|
ax1 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, 1)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{1}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]');
|
||
|
|
|
||
|
|
ax2 = nexttile();
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, 2)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{2}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
ax3 = nexttile(4);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, 3)), 'DisplayName', 'Meas.');
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{3}('de', 'Va'), freqs, 'Hz'))), ...
|
||
|
|
'DisplayName', 'Model');
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
|
||
|
|
legend('location', 'southwest', 'FontSize', 8);
|
||
|
|
|
||
|
|
ax4 = nexttile(5);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, 4)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{4}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
|
||
|
|
ax5 = nexttile(6);
|
||
|
|
hold on;
|
||
|
|
plot(f, abs(enc_frf(:, 5)));
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs_align{5}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2,ax3,ax4,ax5],'xy');
|
||
|
|
xlim([20, 2e3]); ylim([1e-8, 1e-3]);
|
||
|
|
|
||
|
|
%% Input/Output definition
|
||
|
|
clear io; io_i = 1;
|
||
|
|
io(io_i) = linio([mdl, '/Va'], 1, 'openinput'); io_i = io_i + 1; % Actuator Voltage
|
||
|
|
io(io_i) = linio([mdl, '/de'], 1, 'openoutput'); io_i = io_i + 1; % Encoder
|
||
|
|
|
||
|
|
%% APA Initialization
|
||
|
|
n_hexapod.actuator = initializeAPA('type', 'flexible', 'd_align', [0.1e-3; 0.5e-3; 0]);
|
||
|
|
|
||
|
|
%% Tested bending stiffnesses [Nm/rad]
|
||
|
|
kRs = [3, 4, 5, 6, 7];
|
||
|
|
|
||
|
|
%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
|
||
|
|
Gs = {zeros(length(kRs), 1)};
|
||
|
|
|
||
|
|
for i = 1:length(kRs)
|
||
|
|
n_hexapod.flex_bot = initializeBotFlexibleJoint(...
|
||
|
|
'type', '4dof', ...
|
||
|
|
'kRx', kRs(i), ...
|
||
|
|
'kRy', kRs(i));
|
||
|
|
n_hexapod.flex_top = initializeTopFlexibleJoint(...
|
||
|
|
'type', '4dof', ...
|
||
|
|
'kRx', kRs(i), ...
|
||
|
|
'kRy', kRs(i));
|
||
|
|
|
||
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
G.InputName = {'Va'};
|
||
|
|
G.OutputName = {'de'};
|
||
|
|
|
||
|
|
Gs(i) = {G};
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Plot the obtained transfer functions for all the bending stiffnesses
|
||
|
|
freqs = 2*logspace(1, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(kRs)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))), ...
|
||
|
|
'DisplayName', sprintf('$k_R = %.0f$ [Nm/rad]', kRs(i)));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(kRs)
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([20, 2e3]);
|
||
|
|
|
||
|
|
%% Tested axial stiffnesses [N/m]
|
||
|
|
kzs = [5e7 7.5e7 1e8 2.5e8];
|
||
|
|
|
||
|
|
%% Idenfity the transfer function from actuator to encoder for all bending stiffnesses
|
||
|
|
Gs = {zeros(length(kzs), 1)};
|
||
|
|
|
||
|
|
for i = 1:length(kzs)
|
||
|
|
n_hexapod.flex_bot = initializeBotFlexibleJoint(...
|
||
|
|
'type', '4dof', ...
|
||
|
|
'kz', kzs(i));
|
||
|
|
n_hexapod.flex_top = initializeTopFlexibleJoint(...
|
||
|
|
'type', '4dof', ...
|
||
|
|
'kz', kzs(i));
|
||
|
|
|
||
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
G.InputName = {'Va'};
|
||
|
|
G.OutputName = {'de'};
|
||
|
|
|
||
|
|
Gs(i) = {G};
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Plot the obtained transfer functions for all the axial stiffnesses
|
||
|
|
freqs = 2*logspace(1, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(kzs)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))), ...
|
||
|
|
'DisplayName', sprintf('$k_z = %.1e$ [N/m]', kzs(i)));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'northeast');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(kzs)
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([20, 2e3]);
|
||
|
|
|
||
|
|
%% Tested bending dampings [Nm/(rad/s)]
|
||
|
|
cRs = [1e-3, 5e-3, 1e-2, 5e-2, 1e-1];
|
||
|
|
|
||
|
|
%% Idenfity the transfer function from actuator to encoder for all bending dampins
|
||
|
|
Gs = {zeros(length(kRs), 1)};
|
||
|
|
|
||
|
|
for i = 1:length(kRs)
|
||
|
|
n_hexapod.flex_bot = initializeBotFlexibleJoint(...
|
||
|
|
'type', '4dof', ...
|
||
|
|
'cRx', cRs(i), ...
|
||
|
|
'cRy', cRs(i));
|
||
|
|
n_hexapod.flex_top = initializeTopFlexibleJoint(...
|
||
|
|
'type', '4dof', ...
|
||
|
|
'cRx', cRs(i), ...
|
||
|
|
'cRy', cRs(i));
|
||
|
|
|
||
|
|
G = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||
|
|
G.InputName = {'Va'};
|
||
|
|
G.OutputName = {'de'};
|
||
|
|
|
||
|
|
Gs(i) = {G};
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Plot the obtained transfer functions for all the bending stiffnesses
|
||
|
|
freqs = 2*logspace(1, 3, 1000);
|
||
|
|
|
||
|
|
figure;
|
||
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||
|
|
|
||
|
|
ax1 = nexttile([2,1]);
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(kRs)
|
||
|
|
plot(freqs, abs(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))), ...
|
||
|
|
'DisplayName', sprintf('$c_R = %.3f\\,[\\frac{Nm}{rad/s}]$', cRs(i)));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||
|
|
ylabel('Amplitude $d_e/V_a$ [m/V]'); set(gca, 'XTickLabel',[]);
|
||
|
|
hold off;
|
||
|
|
ylim([1e-8, 1e-3]);
|
||
|
|
legend('location', 'southwest');
|
||
|
|
|
||
|
|
ax2 = nexttile;
|
||
|
|
hold on;
|
||
|
|
for i = 1:length(kRs)
|
||
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{i}('de', 'Va'), freqs, 'Hz'))));
|
||
|
|
end
|
||
|
|
hold off;
|
||
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||
|
|
hold off;
|
||
|
|
yticks(-360:90:360); ylim([-180, 180]);
|
||
|
|
|
||
|
|
linkaxes([ax1,ax2],'x');
|
||
|
|
xlim([20, 2e3]);
|