[REFACTORING] Huge changes, WIP.
This commit is contained in:
parent
971a520edd
commit
92b66cb8bb
@ -0,0 +1,2 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="Control" Type="Relative" />
|
@ -1,2 +0,0 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="stewart-simscape/src" Type="Relative" />
|
@ -0,0 +1,2 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="initialize" Type="Relative" />
|
28
Analysis/analysis_perturbations.m
Normal file
28
Analysis/analysis_perturbations.m
Normal file
@ -0,0 +1,28 @@
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Load Plant
|
||||
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
|
||||
|
||||
%% Load shape of the perturbation
|
||||
load('./mat/weight_Wxg.mat', 'Wxg');
|
||||
|
||||
%% Effect of the perturbation on the output
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
dx_out = squeeze(abs(freqresp(Wxg*G_xg_to_d(1, 1), freqs, 'Hz')));
|
||||
dy_out = squeeze(abs(freqresp(Wxg*G_xg_to_d(2, 2), freqs, 'Hz')));
|
||||
dz_out = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3), freqs, 'Hz')));
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, dx_out, 'DisplayName', 'Effect of $Dg$ on $D_{x}$');
|
||||
plot(freqs, dy_out, 'DisplayName', 'Effect of $Dg$ on $D_{y}$');
|
||||
plot(freqs, dz_out, 'DisplayName', 'Effect of $Dg$ on $D_{z}$');
|
||||
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
legend('location', 'southwest');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
hold off;
|
||||
|
||||
exportFig('ground_motion_effect', 'normal-normal')
|
@ -1,15 +1,20 @@
|
||||
%% Load open loop data
|
||||
gm_ol = load('../data/ground_motion_001.mat');
|
||||
% gm_ol = load('../data/ground_motion_001.mat');
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%%
|
||||
Dmeas.Data = Dmeas.Data - Dmeas.Data(1, :);
|
||||
sim Assemblage
|
||||
|
||||
%%
|
||||
Dsample.Data = Dsample.Data - Dsample.Data(1, :);
|
||||
|
||||
%% Time domain X-Y-Z
|
||||
figure;
|
||||
hold on;
|
||||
plot(Dmeas.Time, Dmeas.Data(:, 1));
|
||||
plot(Dmeas.Time, Dmeas.Data(:, 2));
|
||||
plot(Dmeas.Time, Dmeas.Data(:, 3));
|
||||
plot(Dsample.Time, Dsample.Data(:, 1));
|
||||
plot(Dsample.Time, Dsample.Data(:, 2));
|
||||
plot(Dsample.Time, Dsample.Data(:, 3));
|
||||
legend({'x', 'y', 'z'})
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
@ -19,21 +24,21 @@ exportFig('tomo_time_translations', 'normal-normal')
|
||||
%% Time domain angles
|
||||
figure;
|
||||
hold on;
|
||||
plot(Dmeas.Time, Dmeas.Data(:, 4));
|
||||
plot(Dmeas.Time, Dmeas.Data(:, 5));
|
||||
plot(Dmeas.Time, Dmeas.Data(:, 6));
|
||||
plot(Dsample.Time, Dsample.Data(:, 4));
|
||||
plot(Dsample.Time, Dsample.Data(:, 5));
|
||||
plot(Dsample.Time, Dsample.Data(:, 6));
|
||||
legend({'$\theta_x$', '$\theta_y$', '$\theta_z$'})
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
xlabel('Time [s]'); ylabel('Angle [rad]');
|
||||
|
||||
exportFig('tomo_time_rotations', 'normal-normal')
|
||||
|
||||
%% PSD X-Y-Z
|
||||
han_windows = hanning(ceil(length(Dmeas.Time)/10));
|
||||
han_windows = hanning(ceil(length(Dsample.Time)/10));
|
||||
|
||||
[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 1), han_windows, 0, [], 1/Ts);
|
||||
[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts);
|
||||
[psd_z, freqs_z] = pwelch(Dmeas.Data(:, 3), han_windows, 0, [], 1/Ts);
|
||||
[psd_x, freqs_x] = pwelch(Dsample.Data(:, 1), han_windows, 0, [], 1/Ts);
|
||||
[psd_y, freqs_y] = pwelch(Dsample.Data(:, 2), han_windows, 0, [], 1/Ts);
|
||||
[psd_z, freqs_z] = pwelch(Dsample.Data(:, 3), han_windows, 0, [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
@ -48,11 +53,11 @@ hold off;
|
||||
exportFig('tomo_psd_translations', 'normal-normal')
|
||||
|
||||
%% PSD X-Y-Z
|
||||
han_windows = hanning(ceil(length(Dmeas.Time)/10));
|
||||
han_windows = hanning(ceil(length(Dsample.Time)/10));
|
||||
|
||||
[psd_x, freqs_x] = pwelch(Dmeas.Data(:, 4), han_windows, 0, [], 1/Ts);
|
||||
[psd_y, freqs_y] = pwelch(Dmeas.Data(:, 5), han_windows, 0, [], 1/Ts);
|
||||
[psd_z, freqs_z] = pwelch(Dmeas.Data(:, 6), han_windows, 0, [], 1/Ts);
|
||||
[psd_x, freqs_x] = pwelch(Dsample.Data(:, 4), han_windows, 0, [], 1/Ts);
|
||||
[psd_y, freqs_y] = pwelch(Dsample.Data(:, 5), han_windows, 0, [], 1/Ts);
|
||||
[psd_z, freqs_z] = pwelch(Dsample.Data(:, 6), han_windows, 0, [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
@ -66,6 +71,5 @@ hold off;
|
||||
|
||||
exportFig('tomo_psd_rotations', 'normal-normal')
|
||||
|
||||
|
||||
%%
|
||||
save('../data/ground_motion.mat', 'Dmeas')
|
||||
save('./data/ground_motion.mat', 'Dsample')
|
@ -1,25 +1,70 @@
|
||||
gm_ol = load('../data/ground_motion_ol.mat', 'Dmeas');
|
||||
gm_cl = load('../data/ground_motion_001.mat', 'Dmeas');
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Used to get Ts
|
||||
run init_sim_configuration;
|
||||
|
||||
%% Load simulation results
|
||||
gm_ol = load('./data/ground_motion_ol.mat', 'Dsample');
|
||||
gm_cl = load('./data/ground_motion.mat', 'Dsample');
|
||||
|
||||
%% Compare OL and CL - Time
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm_ol.Dmeas.Time, gm_ol.Dmeas.Data(:, 2));
|
||||
plot(gm_cl.Dmeas.Time, gm_cl.Dmeas.Data(:, 2));
|
||||
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 1));
|
||||
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 1));
|
||||
legend({'x - OL', 'x - CL'})
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
|
||||
exportFig('gm_control_time_x', 'normal-normal')
|
||||
|
||||
%% Compare OL and CL - Time
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 2));
|
||||
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 2));
|
||||
legend({'y - OL', 'y - CL'})
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
|
||||
exportFig('gm_control_time_y', 'normal-normal')
|
||||
|
||||
%% Compare OL and CL - Time
|
||||
figure;
|
||||
hold on;
|
||||
plot(gm_ol.Dsample.Time, gm_ol.Dsample.Data(:, 3));
|
||||
plot(gm_cl.Dsample.Time, gm_cl.Dsample.Data(:, 3));
|
||||
legend({'z - OL', 'z - CL'})
|
||||
hold off;
|
||||
xlabel('Time [s]'); ylabel('Displacement [m]');
|
||||
|
||||
exportFig('gm_control_time_z', 'normal-normal')
|
||||
|
||||
%% Compare OL and CL - PSD
|
||||
han_windows_ol = hanning(ceil(length(gm_ol.Dmeas.Time)/10));
|
||||
[psd_y_ol, freqs_y_ol] = pwelch(gm_ol.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts);
|
||||
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
|
||||
[psd_x_ol, freqs_x_ol] = pwelch(gm_ol.Dsample.Data(:, 1), han_windows_ol, 0, [], 1/Ts);
|
||||
|
||||
han_windows = hanning(ceil(length(gm_cl.Dmeas.Time)/10));
|
||||
[psd_y, freqs_y] = pwelch(gm_cl.Dmeas.Data(:, 2), han_windows, 0, [], 1/Ts);
|
||||
han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
|
||||
[psd_x, freqs_x] = pwelch(gm_cl.Dsample.Data(:, 1), han_windows, 0, [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs_x_ol, sqrt(psd_x_ol));
|
||||
plot(freqs_x, sqrt(psd_x));
|
||||
set(gca,'xscale','log'); set(gca,'yscale','log');
|
||||
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
|
||||
legend({'y - OL', 'y - CL'})
|
||||
hold off;
|
||||
|
||||
exportFig('gm_control_psd_x', 'normal-normal')
|
||||
|
||||
%% Compare OL and CL - PSD
|
||||
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
|
||||
[psd_y_ol, freqs_y_ol] = pwelch(gm_ol.Dsample.Data(:, 2), han_windows_ol, 0, [], 1/Ts);
|
||||
|
||||
han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
|
||||
[psd_y, freqs_y] = pwelch(gm_cl.Dsample.Data(:, 2), han_windows, 0, [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
@ -31,3 +76,31 @@ legend({'y - OL', 'y - CL'})
|
||||
hold off;
|
||||
|
||||
exportFig('gm_control_psd_y', 'normal-normal')
|
||||
|
||||
%% Compare OL and CL - PSD
|
||||
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
|
||||
load('./mat/weight_Wxg.mat', 'Wxg');
|
||||
load('./mat/T_S.mat', 'S', 'T');
|
||||
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
dz_ol = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3), freqs, 'Hz')));
|
||||
dz_cl = squeeze(abs(freqresp(Wxg*G_xg_to_d(3, 3)*S(3, 3), freqs, 'Hz')));
|
||||
|
||||
han_windows_ol = hanning(ceil(length(gm_ol.Dsample.Time)/10));
|
||||
[psd_z_ol, freqs_z_ol] = pwelch(gm_ol.Dsample.Data(:, 3), han_windows_ol, 0, [], 1/Ts);
|
||||
|
||||
han_windows = hanning(ceil(length(gm_cl.Dsample.Time)/10));
|
||||
[psd_z, freqs_z] = pwelch(gm_cl.Dsample.Data(:, 3), han_windows, 0, [], 1/Ts);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs_z_ol, sqrt(psd_z_ol), '-', 'Color', [0 0.4470 0.7410], 'DisplayName', '$Dg \rightarrow D_x$ - OL (sim)');
|
||||
plot(freqs, dz_ol, '--', 'Color', [0 0.4470 0.7410], 'DisplayName', '$Dg \rightarrow D_x$ - OL (th)');
|
||||
plot(freqs_z, sqrt(psd_z), '-', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$Dg \rightarrow D_x$ - CL (sim)');
|
||||
plot(freqs, dz_cl, '--', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$Dg \rightarrow D_x$ - CL (th)');
|
||||
set(gca,'xscale','log'); set(gca,'yscale','log');
|
||||
xlabel('Frequency [Hz]'); ylabel('PSD [$m/\sqrt{Hz}$]');
|
||||
legend('location', 'southwest');
|
||||
hold off;
|
||||
|
||||
exportFig('gm_control_psd_z', 'normal-normal')
|
||||
|
BIN
Assemblage.slx
BIN
Assemblage.slx
Binary file not shown.
23
Control/cl_tf.m
Normal file
23
Control/cl_tf.m
Normal file
@ -0,0 +1,23 @@
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Load Plant
|
||||
load('./mat/G_xg_to_d.mat', 'G_xg_to_d');
|
||||
load('./mat/G_f_to_d.mat', 'G_f_to_d');
|
||||
load('./mat/controller.mat', 'K');
|
||||
|
||||
%%
|
||||
S = minreal(inv(tf(eye(6))+G_f_to_d*K));
|
||||
T = minreal((tf(eye(6))+G_f_to_d*K)\G_f_to_d*K);
|
||||
|
||||
bodeFig({S(1,1), T(1,1)})
|
||||
legend({'$S_x$', '$T_x$'})
|
||||
|
||||
bodeFig({S(2,2), T(2,2)})
|
||||
legend({'$S_y$', '$T_y$'})
|
||||
|
||||
bodeFig({S(3,3), T(3,3)})
|
||||
legend({'$S_z$', '$T_z$'})
|
||||
|
||||
%%
|
||||
save('./mat/T_S.mat', 'S', 'T');
|
@ -1,34 +0,0 @@
|
||||
|
||||
%%
|
||||
load('./mat/G_f_to_d.mat', 'G_f_to_d');
|
||||
|
||||
%%
|
||||
G = G_f_to_d(2, 2);
|
||||
|
||||
%% Try sisotool
|
||||
sisotool(G)
|
||||
|
||||
%%
|
||||
gain = 1e9;
|
||||
|
||||
%%
|
||||
bodeFig({gain*G}, struct('phase', true))
|
||||
|
||||
%%
|
||||
[~,~,~,Wpm] = margin(gain*G);
|
||||
|
||||
Wpm = 200*2*pi;
|
||||
|
||||
%%
|
||||
s = tf('s');
|
||||
C = gain*(s/(0.2*Wpm)+1)/(s/(10*Wpm)+1)/(1+s/(2*pi*100));%*(s+2*pi*10)/(s+2*pi*0.0001);
|
||||
|
||||
%% Compute Closed loop transfer function
|
||||
bodeFig({C*G}, struct('phase', true))
|
||||
|
||||
%%
|
||||
K = tf(zeros(6));
|
||||
K(2,2) = C;
|
||||
|
||||
%%
|
||||
save('./mat/controller.mat', 'K')
|
34
Control/generate_control.m
Normal file
34
Control/generate_control.m
Normal file
@ -0,0 +1,34 @@
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Load Plant
|
||||
load('./mat/G_f_to_d.mat', 'G_20');
|
||||
|
||||
%% Load previously generated controllers
|
||||
load('./mat/control_K_tx.mat', 'K_tx');
|
||||
load('./mat/control_K_ty.mat', 'K_ty');
|
||||
load('./mat/control_K_tz.mat', 'K_tz');
|
||||
|
||||
%%
|
||||
sisotool('bode', G_20(1, 1), K_tx);
|
||||
K_tx = C;
|
||||
save('./mat/control_K_tx.mat', 'K_tx');
|
||||
|
||||
%%
|
||||
sisotool('bode', G_20(2, 2), K_ty);
|
||||
K_ty = C;
|
||||
save('./mat/control_K_ty.mat', 'K_ty');
|
||||
|
||||
%%
|
||||
sisotool('bode', G_20(3, 3), K_tz);
|
||||
K_tz = C;
|
||||
save('./mat/control_K_tz.mat', 'K_tz');
|
||||
|
||||
%%
|
||||
K = tf(zeros(6));
|
||||
K(1,1) = K_tx;
|
||||
K(2,2) = K_ty;
|
||||
K(3,3) = K_tz;
|
||||
|
||||
%% Save the MIMO control
|
||||
save('./mat/controller.mat', 'K');
|
@ -1,5 +1,6 @@
|
||||
%% Script Description
|
||||
%
|
||||
% Compare identification from the Simscape model
|
||||
% with the identification on the real system.
|
||||
|
||||
%%
|
||||
clear;
|
||||
@ -23,12 +24,12 @@ measure_dirs = {{'tx', 'tx'}, {'ty', 'ty'}, {'tz', 'tz'}};
|
||||
measures = getAllMeasure(m_object, 'marble', 'hexa', measure_dirs, meas_opts);
|
||||
|
||||
%%
|
||||
load('../mat/id_G_h_h.mat', 'G_h_h');
|
||||
load('../mat/id_G_g_g.mat', 'G_g_g');
|
||||
load('../mat/id_G_h_g.mat', 'G_h_g');
|
||||
load('./mat/id_G_h_h.mat', 'G_h_h');
|
||||
load('./mat/id_G_g_g.mat', 'G_g_g');
|
||||
load('./mat/id_G_h_g.mat', 'G_h_g');
|
||||
|
||||
%%
|
||||
freqs = logspace(-1, 3, 2000);
|
||||
freqs = logspace(1, 3, 2000);
|
||||
|
||||
%% Granite to Granite
|
||||
figure;
|
||||
@ -40,7 +41,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(Granite)_x \rightarrow D(granite)_x$');
|
||||
exportFig('comp_model_meas_Fmx_Dmx');
|
||||
exportFig('comp_model_meas_Fmx_Dmx', struct('path', 'Identification'));
|
||||
|
||||
%
|
||||
figure;
|
||||
@ -52,7 +53,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(Granite)_y \rightarrow D(granite)_y$');
|
||||
exportFig('comp_model_meas_Fmy_Dmy');
|
||||
exportFig('comp_model_meas_Fmy_Dmy', struct('path', 'Identification'));
|
||||
|
||||
%
|
||||
figure;
|
||||
@ -64,7 +65,24 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(Granite)_z \rightarrow D(granite)_z$');
|
||||
exportFig('comp_model_meas_Fmz_Dmz');
|
||||
exportFig('comp_model_meas_Fmz_Dmz', struct('path', 'Identification'));
|
||||
|
||||
% All together
|
||||
figure;
|
||||
hold on;
|
||||
plot(measures.Fmx.Dmx.freq_filt, abs(measures.Fmx.Dmx.resp_filt), '-', 'Color', [0 0.4470 0.7410], 'DisplayName', '$F_{m_x} \rightarrow D_{m_x}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_g_g(1, 1), freqs, 'Hz'))), '--', 'Color', [0 0.4470 0.7410], 'DisplayName', '$F_{m_x} \rightarrow D_{m_x}$ - model');
|
||||
|
||||
plot(measures.Fmy.Dmy.freq_filt, abs(measures.Fmy.Dmy.resp_filt), '-', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$F_{m_y} \rightarrow D_{m_y}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_g_g(2, 2), freqs, 'Hz'))), '--', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$F_{m_y} \rightarrow D_{m_y}$ - model');
|
||||
|
||||
plot(measures.Fmz.Dmz.freq_filt, abs(measures.Fmz.Dmz.resp_filt), '-', 'Color', [0.9290 0.6940 0.1250], 'DisplayName', '$F_{m_z} \rightarrow D_{m_z}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_g_g(3, 3), freqs, 'Hz'))), '--', 'Color', [0.9290 0.6940 0.1250], 'DisplayName', '$F_{m_z} \rightarrow D_{m_z}$ - model');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
legend('location', 'northeast');
|
||||
exportFig('comp_model_meas_Fm_Dm', 'wide-tall', struct('path', 'Identification'));
|
||||
|
||||
%% Hexapod to Hexapod
|
||||
figure;
|
||||
@ -76,7 +94,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(\mu Hexapod)_x \rightarrow D(\mu Hexapod)_x$');
|
||||
exportFig('comp_model_meas_Fhx_Dhx');
|
||||
exportFig('comp_model_meas_Fhx_Dhx', struct('path', 'Identification'));
|
||||
|
||||
%
|
||||
figure;
|
||||
@ -88,7 +106,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(\mu Hexapod)_y \rightarrow D(\mu Hexapod)_y$');
|
||||
exportFig('comp_model_meas_Fhy_Dhy');
|
||||
exportFig('comp_model_meas_Fhy_Dhy', struct('path', 'Identification'));
|
||||
|
||||
%
|
||||
figure;
|
||||
@ -100,7 +118,24 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(\mu Hexapod)_z \rightarrow D(\mu Hexapod)_z$');
|
||||
exportFig('comp_model_meas_Fhz_Dhz');
|
||||
exportFig('comp_model_meas_Fhz_Dhz', struct('path', 'Identification'));
|
||||
|
||||
% All together
|
||||
figure;
|
||||
hold on;
|
||||
plot(measures.Fhx.Dhx.freq_filt, abs(measures.Fhx.Dhx.resp_filt), '-', 'Color', [0 0.4470 0.7410], 'DisplayName', '$F_{h_x} \rightarrow D_{h_x}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_h_h(1, 1), freqs, 'Hz'))), '--', 'Color', [0 0.4470 0.7410], 'DisplayName', '$F_{h_x} \rightarrow D_{h_x}$ - model');
|
||||
|
||||
plot(measures.Fhy.Dhy.freq_filt, abs(measures.Fhy.Dhy.resp_filt), '-', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$F_{h_y} \rightarrow D_{h_y}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_h_h(2, 2), freqs, 'Hz'))), '--', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$F_{h_y} \rightarrow D_{h_y}$ - model');
|
||||
|
||||
plot(measures.Fhz.Dhz.freq_filt, abs(measures.Fhz.Dhz.resp_filt), '-', 'Color', [0.9290 0.6940 0.1250], 'DisplayName', '$F_{h_z} \rightarrow D_{h_z}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_h_h(3, 3), freqs, 'Hz'))), '--', 'Color', [0.9290 0.6940 0.1250], 'DisplayName', '$F_{h_z} \rightarrow D_{h_z}$ - model');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
legend('location', 'southwest');
|
||||
exportFig('comp_model_meas_Fh_Dh', 'wide-tall', struct('path', 'Identification'));
|
||||
|
||||
%% Hexapod to Granite
|
||||
figure;
|
||||
@ -112,7 +147,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(\mu Hexapod)_x \rightarrow D(Granite)_x$');
|
||||
exportFig('comp_model_meas_Fhx_Dmx');
|
||||
exportFig('comp_model_meas_Fhx_Dmx', struct('path', 'Identification'));
|
||||
|
||||
%
|
||||
figure;
|
||||
@ -124,7 +159,7 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(\mu Hexapod)_y \rightarrow D(Granite)_y$');
|
||||
exportFig('comp_model_meas_Fhy_Dmy');
|
||||
exportFig('comp_model_meas_Fhy_Dmy', struct('path', 'Identification'));
|
||||
|
||||
%
|
||||
figure;
|
||||
@ -136,4 +171,21 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [$Hz$]'); ylabel('Amplitude [$m/N$]');
|
||||
legend({'meas.', 'id.'}, 'location', 'northwest');
|
||||
title('Transfer function: $F(\mu Hexapod)_z \rightarrow D(Granite)_z$');
|
||||
exportFig('comp_model_meas_Fhz_Dmz');
|
||||
exportFig('comp_model_meas_Fhz_Dmz', struct('path', 'Identification'));
|
||||
|
||||
% All together
|
||||
figure;
|
||||
hold on;
|
||||
plot(measures.Fhx.Dmx.freq_filt, abs(measures.Fhx.Dmx.resp_filt), '-', 'Color', [0 0.4470 0.7410], 'DisplayName', '$F_{h_x} \rightarrow D_{m_x}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_h_g(1, 1), freqs, 'Hz'))), '--', 'Color', [0 0.4470 0.7410], 'DisplayName', '$F_{h_x} \rightarrow D_{m_x}$ - model');
|
||||
|
||||
plot(measures.Fhy.Dmy.freq_filt, abs(measures.Fhy.Dmy.resp_filt), '-', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$F_{h_y} \rightarrow D_{m_y}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_h_g(2, 2), freqs, 'Hz'))), '--', 'Color', [0.8500 0.3250 0.0980], 'DisplayName', '$F_{h_y} \rightarrow D_{m_y}$ - model');
|
||||
|
||||
plot(measures.Fhz.Dmz.freq_filt, abs(measures.Fhz.Dmz.resp_filt), '-', 'Color', [0.9290 0.6940 0.1250], 'DisplayName', '$F_{h_z} \rightarrow D_{m_z}$ - meas.')
|
||||
plot(freqs, abs(squeeze(freqresp(G_h_g(3, 3), freqs, 'Hz'))), '--', 'Color', [0.9290 0.6940 0.1250], 'DisplayName', '$F_{h_z} \rightarrow D_{m_z}$ - model');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
legend('location', 'southwest');
|
||||
exportFig('comp_model_meas_Fh_Dm', 'wide-tall', struct('path', 'Identification'));
|
||||
|
41
Identification/compare_nass_micro_station.m
Normal file
41
Identification/compare_nass_micro_station.m
Normal file
@ -0,0 +1,41 @@
|
||||
%% Script Description
|
||||
% Compare the plan when on top of a rigid support
|
||||
% and on top of the micro-station (flexible support)
|
||||
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Load Transfer Functions
|
||||
load('./mat/G_nass.mat', 'G_nass_1', 'G_nass_20', 'G_nass_50');
|
||||
load('./stewart-simscape/mat/G_cart.mat', 'G_cart_1', 'G_cart_20', 'G_cart_50')
|
||||
load('./mat/G_f_to_d.mat', 'G_1', 'G_20', 'G_50')
|
||||
|
||||
%% Compare NASS alone and NASS on top of the Station
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
bodeFig({G_nass_50(1, 1), G_cart_50(1, 1)}, freqs, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $\mu$-station', '$F_{n_x} \rightarrow D_{n_x}$ - Rigid support'})
|
||||
legend('location', 'southwest')
|
||||
|
||||
bodeFig({G_nass_50(2, 2), G_cart_50(2, 2)}, freqs, struct('phase', true))
|
||||
legend({'$F_{n_y} \rightarrow D_{n_y}$ - $\mu$-station', '$F_{n_y} \rightarrow D_{n_y}$ - rigid support'})
|
||||
legend('location', 'southwest')
|
||||
|
||||
bodeFig({G_nass_50(3, 3), G_cart_50(3, 3)}, freqs, struct('phase', true))
|
||||
legend({'$F_{n_z} \rightarrow D_{n_z}$ - $\mu$-station', '$F_{n_z} \rightarrow D_{n_z}$ - rigid support'})
|
||||
legend('location', 'southwest')
|
||||
|
||||
%% Compare Displacement of the NASS with Displacement of the Sample
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
bodeFig({G_nass_50(1, 1), G_50(1, 1)}, freqs, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_x} \rightarrow D_{s_x}$'})
|
||||
legend('location', 'southwest')
|
||||
|
||||
bodeFig({G_nass_50(2, 2), G_50(2, 2)}, freqs, struct('phase', true))
|
||||
legend({'$F_{n_y} \rightarrow D_{n_y}$', '$F_{n_y} \rightarrow D_{s_y}$'})
|
||||
legend('location', 'southwest')
|
||||
|
||||
bodeFig({G_nass_50(3, 3), G_50(3, 3)}, freqs, struct('phase', true))
|
||||
legend({'$F_{n_z} \rightarrow D_{n_z}$', '$F_{n_z} \rightarrow D_{s_z}$'})
|
||||
legend('location', 'southwest')
|
56
Identification/identification_G.m
Normal file
56
Identification/identification_G.m
Normal file
@ -0,0 +1,56 @@
|
||||
%% Script Description
|
||||
% Identification of a force injected into the NASS (in cartesian
|
||||
% coordinates) to the relative displacement of the sample
|
||||
% and granite.
|
||||
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 1));
|
||||
|
||||
[G_1, G_1_raw] = identifyG();
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 20));
|
||||
|
||||
[G_20, G_20_raw] = identifyG();
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 50));
|
||||
|
||||
[G_50, G_50_raw] = identifyG();
|
||||
|
||||
%%
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
bodeFig({G_1(1, 1), G_1(2, 2), G_1(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{x}$ - $M = 1Kg$', ...
|
||||
'$F_{n_y} \rightarrow D_{y}$ - $M = 1Kg$', ...
|
||||
'$F_{n_z} \rightarrow D_{z}$ - $M = 1Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('G_xyz', 'normal-normal')
|
||||
|
||||
bodeFig({G_1(1, 1), G_20(1, 1), G_50(1, 1)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{x}$ - $M = 1Kg$', ...
|
||||
'$F_{n_x} \rightarrow D_{x}$ - $M = 20Kg$', ...
|
||||
'$F_{n_x} \rightarrow D_{x}$ - $M = 50Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('G_x_mass', 'normal-normal')
|
||||
|
||||
bodeFig({G_1(2, 2), G_20(2, 2), G_50(2, 2)}, struct('phase', true))
|
||||
legend({'$F_{n_y} \rightarrow D_{y}$ - $M = 1Kg$', ...
|
||||
'$F_{n_y} \rightarrow D_{y}$ - $M = 20Kg$', ...
|
||||
'$F_{n_y} \rightarrow D_{y}$ - $M = 50Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('G_y_mass', 'normal-normal')
|
||||
|
||||
bodeFig({G_1(3, 3), G_20(3, 3), G_50(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_z} \rightarrow D_{z}$ - $M = 1Kg$', ...
|
||||
'$F_{n_z} \rightarrow D_{z}$ - $M = 20Kg$', ...
|
||||
'$F_{n_z} \rightarrow D_{z}$ - $M = 50Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('G_z_mass', 'normal-normal')
|
||||
|
||||
%%
|
||||
save('./mat/G_f_to_d.mat', 'G_1', 'G_20', 'G_50');
|
38
Identification/identification_Gd.m
Normal file
38
Identification/identification_Gd.m
Normal file
@ -0,0 +1,38 @@
|
||||
%% Script Description
|
||||
% Identification of the transfer function
|
||||
% from Ground Motion to sample displacement.
|
||||
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Options for preprocessing the identified transfer functions
|
||||
f_low = 10;
|
||||
f_high = 10000;
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'Assemblage';
|
||||
|
||||
%% NASS
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Micro-Station/Gm'],1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Sample'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
Gd_xg_to_d_raw = linearize(mdl,io, 0);
|
||||
|
||||
Gd_xg_to_d = preprocessIdTf(Gd_xg_to_d_raw, f_low, f_high);
|
||||
|
||||
% Input/Output names
|
||||
Gd_xg_to_d.InputName = {'Dgx', 'Dgy', 'Dgz'};
|
||||
Gd_xg_to_d.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({Gd_xg_to_d(1, 1), Gd_xg_to_d(2, 2), Gd_xg_to_d(3, 3)}, struct('phase', true))
|
||||
legend({'$Dg_{x} \rightarrow D_{x}$', '$Dg_{y} \rightarrow D_{y}$', '$Dg_{z} \rightarrow D_{z}$'})
|
||||
|
||||
%%
|
||||
save('./mat/Gd_xg_to_d.mat', 'Gd_xg_to_d');
|
55
Identification/identification_NASS.m
Normal file
55
Identification/identification_NASS.m
Normal file
@ -0,0 +1,55 @@
|
||||
%% Script Description
|
||||
% Identification of the NASS from cartesian actuation
|
||||
% to cartesian displacement.
|
||||
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 0));
|
||||
|
||||
G_nass_1 = identifyNass();
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 10));
|
||||
|
||||
G_nass_20 = identifyNass();
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 50));
|
||||
|
||||
G_nass_50 = identifyNass();
|
||||
|
||||
%%
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
bodeFig({G_nass_1(1, 1), G_nass_1(2, 2), G_nass_1(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $M = 1Kg$', ...
|
||||
'$F_{n_y} \rightarrow D_{n_y}$ - $M = 1Kg$', ...
|
||||
'$F_{n_z} \rightarrow D_{n_z}$ - $M = 1Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('nass_cart_xyz', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass_1(1, 1), G_nass_20(1, 1), G_nass_50(1, 1)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $M = 1Kg$', ...
|
||||
'$F_{n_x} \rightarrow D_{n_x}$ - $M = 20Kg$', ...
|
||||
'$F_{n_x} \rightarrow D_{n_x}$ - $M = 50Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('nass_cart_x_mass', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass_1(2, 2), G_nass_20(2, 2), G_nass_50(2, 2)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$ - $M = 1Kg$', ...
|
||||
'$F_{n_x} \rightarrow D_{n_x}$ - $M = 20Kg$', ...
|
||||
'$F_{n_x} \rightarrow D_{n_x}$ - $M = 50Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('nass_cart_y_mass', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass_1(3, 3), G_nass_20(3, 3), G_nass_50(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_z} \rightarrow D_{n_z}$ - $M = 1Kg$', ...
|
||||
'$F_{n_z} \rightarrow D_{n_z}$ - $M = 20Kg$', ...
|
||||
'$F_{n_z} \rightarrow D_{n_z}$ - $M = 50Kg$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('nass_cart_z_mass', 'normal-normal')
|
||||
|
||||
%% Save Transfer Functions
|
||||
save('./mat/G_nass.mat', 'G_nass_1', 'G_nass_20', 'G_nass_50');
|
@ -1,36 +0,0 @@
|
||||
%% Script Description
|
||||
%
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%% Options for preprocessing the identified transfer functions
|
||||
f_low = 10;
|
||||
f_high = 10000;
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'Assemblage';
|
||||
|
||||
%% NASS
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Micro-Station/Fn'],1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_f_to_d = linearize(mdl,io, 0);
|
||||
|
||||
G_f_to_d = preprocessIdTf(G_f_to_d, 10, 10000);
|
||||
|
||||
% Input/Output names
|
||||
G_f_to_d.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
G_f_to_d.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({G_f_to_d(1, 1), G_f_to_d(2, 2), G_f_to_d(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{x}$', '$F_{n_y} \rightarrow D_{y}$', '$F_{n_z} \rightarrow D_{z}$'})
|
||||
|
||||
%%
|
||||
save('./mat/G_f_to_d.mat', 'G_f_to_d');
|
@ -1,85 +1,78 @@
|
||||
%% Script Description
|
||||
% Run an identification of each stage from input to output
|
||||
% Save all computed transfer functions into one .mat file
|
||||
% Make the same identification as Marc did
|
||||
% Should comment out the nano-hexapod and sample before
|
||||
% runing this script.
|
||||
|
||||
%%
|
||||
clear;
|
||||
close all;
|
||||
clc
|
||||
|
||||
%% Define options for bode plots
|
||||
bode_opts = bodeoptions;
|
||||
|
||||
bode_opts.Title.FontSize = 12;
|
||||
bode_opts.XLabel.FontSize = 12;
|
||||
bode_opts.YLabel.FontSize = 12;
|
||||
bode_opts.FreqUnits = 'Hz';
|
||||
bode_opts.MagUnits = 'abs';
|
||||
bode_opts.MagScale = 'log';
|
||||
bode_opts.PhaseWrapping = 'on';
|
||||
bode_opts.PhaseVisible = 'on';
|
||||
clear; close all; clc;
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'Assemblage';
|
||||
mdl = 'Micro_Station_Identification';
|
||||
|
||||
%% Micro-Hexapod
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fhexa_cart'],1,'input');
|
||||
io(2) = linio([mdl, '/meas_micro_hexapod'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Fm'],1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Micro_Hexapod_Inertial_Sensor'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_h_h = linearize(mdl,io, 0);
|
||||
G_h_h = G_h_h(1:3, 1:3);
|
||||
G_h_h = minreal(G_h_h);
|
||||
G_h_h_raw = linearize(mdl,io, 0);
|
||||
G_h_h_raw = G_h_h_raw(1:3, 1:3);
|
||||
G_h_h = preprocessIdTf(G_h_h_raw, 10, 10000);
|
||||
|
||||
% Input/Output names
|
||||
G_h_h.InputName = {'Fux', 'Fuy', 'Fuz'};
|
||||
G_h_h.OutputName = {'Dux', 'Duy', 'Duz'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
figure;
|
||||
bode(G_h_h(1, 1), bode_opts)
|
||||
bodeFig({G_h_h(1, 1), G_h_h(2, 2), G_h_h(3, 3)})
|
||||
legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_y} \rightarrow D_{h_y}$', '$F_{h_z} \rightarrow D_{h_z}$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('id_marc_h_to_h', 'normal-normal', struct('path', 'Identification'))
|
||||
|
||||
%% Granite
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Granite_F'],1,'input');
|
||||
io(2) = linio([mdl, '/meas_granite'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/F_granite'],1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Granite_Inertial_Sensor'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_g_g = linearize(mdl,io, 0);
|
||||
G_g_g = minreal(G_g_g);
|
||||
G_g_g_raw = linearize(mdl,io, 0);
|
||||
G_g_g = preprocessIdTf(G_g_g_raw, 10, 10000);
|
||||
|
||||
% Input/Output names
|
||||
G_g_g.InputName = {'Fgx', 'Fgy', 'Fgz'};
|
||||
G_g_g.OutputName = {'Dgx', 'Dgy', 'Dgz'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
figure;
|
||||
bode(G_h_h(2, 2), bode_opts)
|
||||
bodeFig({G_g_g(1, 1), G_g_g(2, 2), G_g_g(3, 3)})
|
||||
legend({'$F_{g_x} \rightarrow D_{g_x}$', '$F_{g_y} \rightarrow D_{g_y}$', '$F_{g_z} \rightarrow D_{g_z}$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('id_marc_g_to_g', 'normal-normal', struct('path', 'Identification'))
|
||||
|
||||
%% Micro Hexapod to Granite
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fhexa_cart'],1,'input');
|
||||
io(2) = linio([mdl, '/meas_granite'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Fm'],1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Granite_Inertial_Sensor'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_h_g = linearize(mdl,io, 0);
|
||||
G_h_g = G_h_g(1:3, 1:3);
|
||||
G_h_g = minreal(G_h_g);
|
||||
G_h_g_raw = linearize(mdl,io, 0);
|
||||
G_h_g_raw = G_h_g_raw(1:3, 1:3);
|
||||
G_h_g = preprocessIdTf(G_h_g_raw, 10, 10000);
|
||||
|
||||
% Input/Output names
|
||||
G_h_g.InputName = {'Fhx', 'Fhy', 'Fhz'};
|
||||
G_h_g.OutputName = {'Dgx', 'Dgy', 'Dgz'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
figure;
|
||||
bode(G_h_g(2, 2), bode_opts)
|
||||
bodeFig({G_h_g(1, 1), G_h_g(2, 2), G_h_g(3, 3)})
|
||||
legend({'$F_{h_x} \rightarrow D_{g_x}$', '$F_{h_y} \rightarrow D_{g_y}$', '$F_{h_z} \rightarrow D_{g_z}$'})
|
||||
legend('location', 'southwest')
|
||||
exportFig('id_marc_h_to_g', 'normal-normal', struct('path', 'Identification'))
|
||||
|
||||
%%
|
||||
save('../mat/id_G_h_h.mat', 'G_h_h');
|
||||
save('../mat/id_G_g_g.mat', 'G_g_g');
|
||||
save('../mat/id_G_h_g.mat', 'G_h_g');
|
||||
save('./mat/id_G_h_h.mat', 'G_h_h');
|
||||
save('./mat/id_G_g_g.mat', 'G_g_g');
|
||||
save('./mat/id_G_h_g.mat', 'G_h_g');
|
||||
|
@ -1,42 +1,50 @@
|
||||
%% Script Description
|
||||
%
|
||||
% From the identification, plot all the
|
||||
% transfer funcions.
|
||||
|
||||
%%
|
||||
clear;
|
||||
close all;
|
||||
clc
|
||||
|
||||
%% Define options for bode plots
|
||||
bode_opts = bodeoptions;
|
||||
|
||||
bode_opts.Title.FontSize = 12;
|
||||
bode_opts.XLabel.FontSize = 12;
|
||||
bode_opts.YLabel.FontSize = 12;
|
||||
bode_opts.FreqUnits = 'Hz';
|
||||
bode_opts.MagUnits = 'abs';
|
||||
bode_opts.MagScale = 'log';
|
||||
bode_opts.PhaseWrapping = 'on';
|
||||
bode_opts.PhaseVisible = 'off';
|
||||
clear; close all; clc
|
||||
|
||||
%% Load Data
|
||||
load('../mat/identified_tf.mat');
|
||||
load('./mat/identified_tf.mat');
|
||||
|
||||
%% Y-Translation Stage
|
||||
figure;
|
||||
bode(G_ty, bode_opts)
|
||||
bodeFig({G_ty}, struct('phase', true))
|
||||
legend({'$F_{y} \rightarrow D_{y}$'})
|
||||
exportFig('id_ty', 'normal-normal')
|
||||
|
||||
%% Tilt Stage
|
||||
figure;
|
||||
bode(G_ry, bode_opts)
|
||||
bodeFig({G_ry}, struct('phase', true))
|
||||
legend({'$M_{y} \rightarrow R_{y}$'})
|
||||
exportFig('id_ry', 'normal-normal')
|
||||
|
||||
%% Spindle
|
||||
figure;
|
||||
bode(G_rz, bode_opts)
|
||||
bodeFig({G_rz}, struct('phase', true))
|
||||
legend({'$M_{z} \rightarrow R_{z}$'})
|
||||
exportFig('id_ry', 'normal-normal')
|
||||
|
||||
%% Hexapod Symetrie
|
||||
figure;
|
||||
bode(G_hexa, bode_opts)
|
||||
bodeFig({G_hexa(1, 1), G_hexa(2, 2), G_hexa(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_y} \rightarrow D_{h_y}$', '$F_{h_z} \rightarrow D_{h_z}$'})
|
||||
exportFig('id_hexapod_trans', 'normal-normal')
|
||||
|
||||
bodeFig({G_hexa(4, 4), G_hexa(5, 5), G_hexa(6, 6)}, struct('phase', true))
|
||||
legend({'$M_{h_x} \rightarrow R_{h_x}$', '$M_{h_y} \rightarrow R_{h_y}$', '$M_{h_z} \rightarrow R_{h_z}$'})
|
||||
exportFig('id_hexapod_rot', 'normal-normal')
|
||||
|
||||
bodeFig({G_hexa(1, 1), G_hexa(2, 1), G_hexa(3, 1)}, struct('phase', true))
|
||||
legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_x} \rightarrow D_{h_y}$', '$F_{h_x} \rightarrow D_{h_z}$'})
|
||||
exportFig('id_hexapod_coupling', 'normal-normal')
|
||||
|
||||
%% NASS
|
||||
figure;
|
||||
bode(G_nass(1:3, 1:3), bode_opts)
|
||||
bodeFig({G_nass(1, 1), G_nass(2, 2), G_nass(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_y} \rightarrow D_{n_y}$', '$F_{n_z} \rightarrow D_{n_z}$'})
|
||||
exportFig('id_nass_trans', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass(4, 4), G_nass(5, 5), G_nass(6, 6)}, struct('phase', true))
|
||||
legend({'$M_{n_x} \rightarrow R_{n_x}$', '$M_{n_y} \rightarrow R_{n_y}$', '$M_{n_z} \rightarrow R_{n_z}$'})
|
||||
exportFig('id_nass_rot', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass(1, 1), G_nass(2, 1), G_nass(3, 1)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_x} \rightarrow D_{n_y}$', '$F_{n_x} \rightarrow D_{n_z}$'})
|
||||
exportFig('id_nass_coupling', 'normal-normal')
|
||||
|
@ -5,6 +5,9 @@
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%%
|
||||
initializeSample(struct('mass', 10));
|
||||
|
||||
%% Options for preprocessing the identified transfer functions
|
||||
f_low = 10; % [Hz]
|
||||
f_high = 10000; % [Hz]
|
||||
@ -14,12 +17,12 @@ options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'Assemblage';
|
||||
mdl = 'Micro_Station_Identification';
|
||||
|
||||
%% Y-Translation Stage
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fy'],1,'input');
|
||||
io(2) = linio([mdl, '/Dy_meas'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Fy'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Translation y'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_ty_raw = linearize(mdl,io, 0);
|
||||
@ -31,15 +34,10 @@ G_ty = preprocessIdTf(G_ty_raw, f_low, f_high);
|
||||
G_ty.InputName = {'Fy'};
|
||||
G_ty.OutputName = {'Dy'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({G_ty}, struct('phase', true))
|
||||
legend({'$F_{y} \rightarrow D_{y}$'})
|
||||
exportFig('id_ty', 'normal-normal')
|
||||
|
||||
%% Tilt Stage
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/My'],1,'input');
|
||||
io(2) = linio([mdl, '/Ry_meas'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Ry'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Tilt'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_ry_raw = linearize(mdl,io, 0);
|
||||
@ -51,15 +49,10 @@ G_ry = preprocessIdTf(G_ry_raw, f_low, f_high);
|
||||
G_ry.InputName = {'My'};
|
||||
G_ry.OutputName = {'Ry'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({G_ry}, struct('phase', true))
|
||||
legend({'$M_{y} \rightarrow R_{y}$'})
|
||||
exportFig('id_ry', 'normal-normal')
|
||||
|
||||
%% Spindle
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Mz'],1,'input');
|
||||
io(2) = linio([mdl, '/Rz_meas'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Rz'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Spindle'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_rz_raw = linearize(mdl,io, 0);
|
||||
@ -71,15 +64,10 @@ G_rz = preprocessIdTf(G_rz_raw, f_low, f_high);
|
||||
G_rz.InputName = {'Mz'};
|
||||
G_rz.OutputName = {'Rz'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({G_rz}, struct('phase', true))
|
||||
legend({'$M_{z} \rightarrow R_{z}$'})
|
||||
exportFig('id_ry', 'normal-normal')
|
||||
|
||||
%% Hexapod Symetrie
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fhexa_cart'],1,'input');
|
||||
io(2) = linio([mdl, '/Dm_meas'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Fm'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Micro_Hexapod'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_hexa_raw = linearize(mdl,io, 0);
|
||||
@ -91,23 +79,10 @@ G_hexa = preprocessIdTf(G_hexa_raw, f_low, f_high);
|
||||
G_hexa.InputName = {'Fhexa_x', 'Fhexa_y', 'Fhexa_z', 'Mhexa_x', 'Mhexa_y', 'Mhexa_z'};
|
||||
G_hexa.OutputName = {'Dhexa_x', 'Dhexa_y', 'Dhexa_z', 'Rhexa_x', 'Rhexa_y', 'Rhexa_z'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({G_hexa(1, 1), G_hexa(2, 2), G_hexa(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_y} \rightarrow D_{h_y}$', '$F_{h_z} \rightarrow D_{h_z}$'})
|
||||
exportFig('id_hexapod_trans', 'normal-normal')
|
||||
|
||||
bodeFig({G_hexa(4, 4), G_hexa(5, 5), G_hexa(6, 6)}, struct('phase', true))
|
||||
legend({'$M_{h_x} \rightarrow R_{h_x}$', '$M_{h_y} \rightarrow R_{h_y}$', '$M_{h_z} \rightarrow R_{h_z}$'})
|
||||
exportFig('id_hexapod_rot', 'normal-normal')
|
||||
|
||||
bodeFig({G_hexa(1, 1), G_hexa(2, 1), G_hexa(3, 1)}, struct('phase', true))
|
||||
legend({'$F_{h_x} \rightarrow D_{h_x}$', '$F_{h_x} \rightarrow D_{h_y}$', '$F_{h_x} \rightarrow D_{h_z}$'})
|
||||
exportFig('id_hexapod_coupling', 'normal-normal')
|
||||
|
||||
%% NASS
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Fnass_cart'],1,'input');
|
||||
io(2) = linio([mdl, '/Dn_meas'],1,'output');
|
||||
io(1) = linio([mdl, '/Micro-Station/Fn'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
c = linearize(mdl,io, 0);
|
||||
@ -119,25 +94,5 @@ G_nass = preprocessIdTf(G_nass_raw, f_low, f_high);
|
||||
G_nass.InputName = {'Fnass_x', 'Fnass_y', 'Fnass_z', 'Mnass_x', 'Mnass_y', 'Mnass_z'};
|
||||
G_nass.OutputName = {'Dnass_x', 'Dnass_y', 'Dnass_z', 'Dnass_x', 'Dnass_y', 'Dnass_z'};
|
||||
|
||||
% Bode Plot of the linearized function
|
||||
bodeFig({G_nass(1, 1), G_nass(2, 2), G_nass(3, 3)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_y} \rightarrow D_{n_y}$', '$F_{n_z} \rightarrow D_{n_z}$'})
|
||||
exportFig('id_nass_trans', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass(4, 4), G_nass(5, 5), G_nass(6, 6)}, struct('phase', true))
|
||||
legend({'$M_{n_x} \rightarrow R_{n_x}$', '$M_{n_y} \rightarrow R_{n_y}$', '$M_{n_z} \rightarrow R_{n_z}$'})
|
||||
exportFig('id_nass_rot', 'normal-normal')
|
||||
|
||||
bodeFig({G_nass(1, 1), G_nass(2, 1), G_nass(3, 1)}, struct('phase', true))
|
||||
legend({'$F_{n_x} \rightarrow D_{n_x}$', '$F_{n_x} \rightarrow D_{n_y}$', '$F_{n_x} \rightarrow D_{n_z}$'})
|
||||
exportFig('id_nass_coupling', 'normal-normal')
|
||||
|
||||
%% Save all transfer function
|
||||
save('../mat/identified_tf.mat', 'G_ty', 'G_ry', 'G_rz', 'G_hexa', 'G_nass')
|
||||
|
||||
%% Functions
|
||||
function G = preprocessIdTf(G0, f_low, f_high)
|
||||
[~,G1] = freqsep(G0, 2*pi*f_low);
|
||||
[G2,~] = freqsep(G1, 2*pi*f_high);
|
||||
G = minreal(G2);
|
||||
end
|
||||
|
BIN
Micro_Station_Identification.slx
Normal file
BIN
Micro_Station_Identification.slx
Normal file
Binary file not shown.
113
init_data.m
113
init_data.m
@ -1,100 +1,41 @@
|
||||
%%
|
||||
run init_solidworks_data.m
|
||||
clear; close all; clc;
|
||||
|
||||
%% Ground
|
||||
ground = struct();
|
||||
%% Initialize Inputs
|
||||
opts_inputs = struct(...
|
||||
'ground_motion', false...
|
||||
);
|
||||
|
||||
ground.shape = [2, 2, 0.5]; % m
|
||||
initializeInputs(opts_inputs);
|
||||
|
||||
%% Granite
|
||||
granite = struct();
|
||||
%% Initialize SolidWorks Data
|
||||
initializeSmiData();
|
||||
|
||||
granite.m = smiData.Solid(5).mass;
|
||||
granite.k.ax = 1e8; % x-y-z Stiffness of the granite [N/m]
|
||||
granite.ksi.ax = 1;
|
||||
%% Initialize Ground
|
||||
initializeGround();
|
||||
|
||||
granite = updateDamping(granite);
|
||||
%% Initialize Granite
|
||||
initializeGranite();
|
||||
|
||||
%% Translation stage
|
||||
ty = struct();
|
||||
%% Initialize Translation stage
|
||||
initializeTy();
|
||||
|
||||
ty.m = smiData.Solid(4).mass+smiData.Solid(6).mass+smiData.Solid(7).mass+smiData.Solid(8).mass+smiData.Solid(9).mass+4*smiData.Solid(11).mass+smiData.Solid(24).mass+smiData.Solid(25).mass+smiData.Solid(28).mass;
|
||||
%% Initialize Tilt Stage
|
||||
initializeRy();
|
||||
|
||||
ty.k.ax = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m]
|
||||
ty.k.rad = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
|
||||
%% Initialize Spindle
|
||||
initializeRz();
|
||||
|
||||
ty.ksi.ax = 0.05;
|
||||
ty.ksi.rad = 1;
|
||||
%% Initialize Hexapod Symétrie
|
||||
initializeMicroHexapod();
|
||||
|
||||
ty = updateDamping(ty);
|
||||
%% Initialize Center of Gravity compensation
|
||||
initializeAxisc();
|
||||
|
||||
%% Tilt Stage
|
||||
ry = struct();
|
||||
%% Initialize NASS
|
||||
initializeNanoHexapod();
|
||||
|
||||
ry.m = smiData.Solid(26).mass+smiData.Solid(18).mass+smiData.Solid(10).mass;
|
||||
%% Initialize Sample
|
||||
opts_sample = struct('mass', 20);
|
||||
|
||||
ry.k.h = 357e6/4; % Stiffness in the direction of the guidance [N/m]
|
||||
ry.k.rad = 555e6/4; % Stiffness in the top direction [N/m]
|
||||
ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m]
|
||||
ry.k.tilt = 1e4 ; % Rotation stiffness around y [N*m/deg]
|
||||
|
||||
ry.ksi.h = 1;
|
||||
ry.ksi.rad = 1;
|
||||
ry.ksi.rrad = 1;
|
||||
ry.ksi.tilt = 1;
|
||||
|
||||
ry = updateDamping(ry);
|
||||
|
||||
%% Spindle
|
||||
rz = struct();
|
||||
|
||||
rz.m = smiData.Solid(12).mass+6*smiData.Solid(20).mass+smiData.Solid(19).mass;
|
||||
|
||||
rz.k.ax = 2e9; % Axial Stiffness [N/m]
|
||||
rz.k.rad = 7e8; % Radial Stiffness [N/m]
|
||||
rz.k.tilt = 1e5; % TODO
|
||||
rz.k.rot = 1e5; % Rotational Stiffness [N*m/deg]
|
||||
|
||||
rz.ksi.ax = 1;
|
||||
rz.ksi.rad = 1;
|
||||
rz.ksi.tilt = 1;
|
||||
rz.ksi.rot = 1;
|
||||
|
||||
rz = updateDamping(rz);
|
||||
|
||||
%% Hexapod Symétrie
|
||||
run stewart-simscape\params_micro_hexapod.m;
|
||||
micro_hexapod = stewart;
|
||||
clear stewart;
|
||||
|
||||
%% Center of gravity compensation
|
||||
axisc = struct();
|
||||
|
||||
axisc.m = smiData.Solid(30).mass;
|
||||
axisc.k.ax = 1; % TODO [N*m/deg)]
|
||||
axisc.ksi.ax = 1;
|
||||
|
||||
axisc = updateDamping(axisc);
|
||||
|
||||
%% NASS
|
||||
run stewart-simscape\params_micro_hexapod.m;
|
||||
nano_hexapod = stewart;
|
||||
clear stewart;
|
||||
|
||||
%% Sample
|
||||
sample = struct();
|
||||
|
||||
sample.radius = 100; % Sample radius [mm]
|
||||
sample.height = 300; % Sample height [mm]
|
||||
sample.mass = 50; % Sample mass [kg]
|
||||
sample.offset = 0; % Decentralization offset [mm]
|
||||
sample.color = [0.9 0.1 0.1]; % Sample color
|
||||
|
||||
%%
|
||||
function element = updateDamping(element)
|
||||
field = fieldnames(element.k);
|
||||
for i = 1:length(field)
|
||||
element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
|
||||
% element.c.(field{i}) = element.k.(field{i})/1000;
|
||||
end
|
||||
end
|
||||
initializeSample(opts_sample);
|
||||
|
@ -1,67 +0,0 @@
|
||||
%%
|
||||
run init_sim_configuration.m
|
||||
run init_data.m
|
||||
|
||||
%%
|
||||
time_vector = 0:Ts:Tsim;
|
||||
|
||||
%% Set point [m, rad]
|
||||
setpoint = zeros(length(time_vector), 6);
|
||||
|
||||
% setpoint(ceil(10/Ts):end, 2) = 1e-6; % Step of 1 micro-meter in y direction
|
||||
|
||||
r_setpoint = timeseries(setpoint, time_vector);
|
||||
|
||||
%% Ground motion
|
||||
xg = zeros(length(time_vector), 3);
|
||||
|
||||
% Wxg = 1e-5*(s/(2e2)^(1/3) + 2*pi*0.1)^3/(s + 2*pi*0.1)^3;
|
||||
% Wxg = Wxg*(s/(0.5e6)^(1/3) + 2*pi*10)^3/(s + 2*pi*10)^3;
|
||||
% Wxg = Wxg/(1+s/(2*pi*2000));
|
||||
%
|
||||
% xg = 1/sqrt(2)*100*random('norm', 0, 1, length(time_vector), 3);
|
||||
% xg(:, 1) = lsim(Wxg, xg(:, 1), time_vector);
|
||||
% xg(:, 2) = lsim(Wxg, xg(:, 2), time_vector);
|
||||
% xg(:, 3) = lsim(Wxg, xg(:, 3), time_vector);
|
||||
|
||||
r_Gm = timeseries(xg, time_vector);
|
||||
|
||||
% figure;
|
||||
% plot(r_Gm)
|
||||
|
||||
%% Translation stage [m]
|
||||
ty = zeros(length(time_vector), 1);
|
||||
|
||||
r_Ty = timeseries(ty, time_vector);
|
||||
|
||||
%% Tilt Stage [rad]
|
||||
r_tilt = zeros(length(time_vector), 1);
|
||||
|
||||
% r_tilt = 3*(2*pi/360)*sin(2*pi*0.2*time_vector);
|
||||
|
||||
r_My = timeseries(r_tilt, time_vector);
|
||||
|
||||
%% Spindle [rad]
|
||||
r_spindle = zeros(length(time_vector), 1);
|
||||
|
||||
% r_spindle = 2*pi*0.5*time_vector;
|
||||
|
||||
r_Mz = timeseries(r_spindle, time_vector);
|
||||
|
||||
%% Micro Hexapod
|
||||
u_hexa = zeros(length(time_vector), 6);
|
||||
|
||||
r_u_hexa = timeseries(u_hexa, time_vector);
|
||||
|
||||
%% Center of gravity compensation
|
||||
mass = zeros(length(time_vector), 2);
|
||||
|
||||
r_mass = timeseries(mass, time_vector);
|
||||
|
||||
%% Nano Hexapod
|
||||
n_hexa = zeros(length(time_vector), 6);
|
||||
|
||||
r_n_hexa = timeseries(n_hexa, time_vector);
|
||||
|
||||
%%
|
||||
save('./mat/inputs_setpoint.mat', 'r_setpoint', 'r_Gm', 'r_Ty', 'r_My', 'r_Mz', 'r_u_hexa', 'r_mass', 'r_n_hexa');
|
12
init_perturbations.m
Normal file
12
init_perturbations.m
Normal file
@ -0,0 +1,12 @@
|
||||
%%
|
||||
clear; close all; clc;
|
||||
|
||||
%%
|
||||
s = tf('s');
|
||||
|
||||
%% Ground Motion
|
||||
Wxg = 1e-5*(s/(2e2)^(1/3) + 2*pi*0.1)^3/(s + 2*pi*0.1)^3;
|
||||
Wxg = Wxg*(s/(0.5e6)^(1/3) + 2*pi*10)^3/(s + 2*pi*10)^3;
|
||||
Wxg = Wxg/(1+s/(2*pi*2000));
|
||||
|
||||
save('./mat/weight_Wxg.mat', 'Wxg');
|
@ -1,6 +1,6 @@
|
||||
%% Solver Configuration
|
||||
Ts = 1e-4; % Sampling time [s]
|
||||
Tsim = 10; % Simulation time [s]
|
||||
Ts = 1e-3; % Sampling time [s]
|
||||
Tsim = 1; % Simulation time [s]
|
||||
|
||||
%% Gravity
|
||||
g = 0 ; % Gravity along the z axis [m/s^2]
|
||||
|
@ -1,11 +1,22 @@
|
||||
%%
|
||||
%% Load Simulation configuration
|
||||
run init_sim_configuration.m
|
||||
run init_data.m
|
||||
|
||||
%% Signals Applied to the system
|
||||
% load('./mat/inputs_ground_motion.mat');
|
||||
% load('./mat/inputs_spindle.mat');
|
||||
load('./mat/inputs_setpoint.mat');
|
||||
%% Load SolidWorks Data
|
||||
% load('./mat/smiData.mat', 'smiData');
|
||||
|
||||
%%
|
||||
%% Load data of each stage
|
||||
load('./mat/ground.mat', 'ground');
|
||||
load('./mat/granite.mat', 'granite');
|
||||
load('./mat/ty.mat', 'ty');
|
||||
load('./mat/ry.mat', 'ry');
|
||||
load('./mat/rz.mat', 'rz');
|
||||
load('./mat/micro_hexapod.mat', 'micro_hexapod');
|
||||
load('./mat/axisc.mat', 'axisc');
|
||||
load('./mat/nano_hexapod.mat', 'nano_hexapod');
|
||||
load('./mat/sample.mat', 'sample');
|
||||
|
||||
%% Load Signals Applied to the system
|
||||
load('./mat/inputs.mat', 'inputs');
|
||||
|
||||
%% Load Controller
|
||||
load('./mat/controller.mat', 'K');
|
||||
|
File diff suppressed because it is too large
Load Diff
17
initialize/initializeAxisc.m
Normal file
17
initialize/initializeAxisc.m
Normal file
@ -0,0 +1,17 @@
|
||||
function [axisc] = initializeAxisc()
|
||||
%%
|
||||
load('./mat/smiData.mat', 'smiData');
|
||||
|
||||
%%
|
||||
axisc = struct();
|
||||
|
||||
axisc.m = smiData.Solid(30).mass;
|
||||
axisc.k.ax = 1; % TODO [N*m/deg)]
|
||||
|
||||
axisc.c.ax = axisc.k.ax/1000;
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/axisc.mat', 'axisc');
|
||||
end
|
||||
end
|
17
initialize/initializeGranite.m
Normal file
17
initialize/initializeGranite.m
Normal file
@ -0,0 +1,17 @@
|
||||
function [granite] = initializeGranite()
|
||||
%%
|
||||
load('./mat/smiData.mat', 'smiData');
|
||||
|
||||
%%
|
||||
granite = struct();
|
||||
|
||||
granite.m = smiData.Solid(5).mass;
|
||||
granite.k.ax = 1e8; % x-y-z Stiffness of the granite [N/m]
|
||||
|
||||
granite.c.ax = granite.k.ax/1000;
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/granite.mat', 'granite');
|
||||
end
|
||||
end
|
10
initialize/initializeGround.m
Normal file
10
initialize/initializeGround.m
Normal file
@ -0,0 +1,10 @@
|
||||
function [ground] = initializeGround()
|
||||
ground = struct();
|
||||
|
||||
ground.shape = [2, 2, 0.5]; % m
|
||||
|
||||
if nargout == 0
|
||||
save('./mat/ground.mat', 'ground')
|
||||
end
|
||||
end
|
||||
|
96
initialize/initializeInputs.m
Normal file
96
initialize/initializeInputs.m
Normal file
@ -0,0 +1,96 @@
|
||||
function [inputs] = initializeInputs(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct('setpoint', false, ...
|
||||
'ground_motion', false, ...
|
||||
'ty', false, ...
|
||||
'ry', false, ...
|
||||
'rz', false ...
|
||||
);
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Load Sampling Time and Simulation Time
|
||||
run init_sim_configuration.m
|
||||
|
||||
%% Define the time vector
|
||||
time_vector = 0:Ts:Tsim;
|
||||
|
||||
%% Create the input Structure that will contain all the inputs
|
||||
inputs = struct();
|
||||
|
||||
%% Set point [m, rad]
|
||||
if opts.setpoint
|
||||
setpoint = zeros(length(time_vector), 6);
|
||||
setpoint(ceil(10/Ts):end, 2) = 1e-6; % Step of 1 micro-meter in y direction
|
||||
else
|
||||
setpoint = zeros(length(time_vector), 6);
|
||||
end
|
||||
|
||||
inputs.setpoint = timeseries(setpoint, time_vector);
|
||||
|
||||
%% Ground motion
|
||||
if opts.ground_motion
|
||||
load('./mat/weight_Wxg.mat', 'Wxg');
|
||||
ground_motion = 1/sqrt(2)*100*random('norm', 0, 1, length(time_vector), 3);
|
||||
ground_motion(:, 1) = lsim(Wxg, ground_motion(:, 1), time_vector);
|
||||
ground_motion(:, 2) = lsim(Wxg, ground_motion(:, 2), time_vector);
|
||||
ground_motion(:, 3) = lsim(Wxg, ground_motion(:, 3), time_vector);
|
||||
else
|
||||
ground_motion = zeros(length(time_vector), 3);
|
||||
end
|
||||
|
||||
inputs.ground_motion = timeseries(ground_motion, time_vector);
|
||||
|
||||
%% Translation stage [m]
|
||||
if opts.ty
|
||||
ty = zeros(length(time_vector), 1);
|
||||
else
|
||||
ty = zeros(length(time_vector), 1);
|
||||
end
|
||||
|
||||
inputs.ty = timeseries(ty, time_vector);
|
||||
|
||||
%% Tilt Stage [rad]
|
||||
if opts.ty
|
||||
ry = 3*(2*pi/360)*sin(2*pi*0.2*time_vector);
|
||||
else
|
||||
ry = zeros(length(time_vector), 1);
|
||||
end
|
||||
|
||||
inputs.ry = timeseries(ry, time_vector);
|
||||
|
||||
%% Spindle [rad]
|
||||
if opts.ty
|
||||
rz = 2*pi*0.5*time_vector;
|
||||
else
|
||||
rz = zeros(length(time_vector), 1);
|
||||
end
|
||||
|
||||
inputs.rz = timeseries(rz, time_vector);
|
||||
|
||||
%% Micro Hexapod
|
||||
u_hexa = zeros(length(time_vector), 6);
|
||||
|
||||
inputs.micro_hexapod = timeseries(u_hexa, time_vector);
|
||||
|
||||
%% Center of gravity compensation
|
||||
mass = zeros(length(time_vector), 2);
|
||||
|
||||
inputs.axisc = timeseries(mass, time_vector);
|
||||
|
||||
%% Nano Hexapod
|
||||
n_hexa = zeros(length(time_vector), 6);
|
||||
|
||||
inputs.nano_hexapod = timeseries(n_hexa, time_vector);
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/inputs.mat', 'inputs');
|
||||
end
|
||||
end
|
||||
|
194
initialize/initializeMicroHexapod.m
Normal file
194
initialize/initializeMicroHexapod.m
Normal file
@ -0,0 +1,194 @@
|
||||
function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct();
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Stewart Object
|
||||
micro_hexapod = struct();
|
||||
micro_hexapod.h = 350; % Total height of the platform [mm]
|
||||
micro_hexapod.jacobian = 435; % Point where the Jacobian is computed => Center of rotation [mm]
|
||||
|
||||
%% Bottom Plate
|
||||
BP = struct();
|
||||
|
||||
BP.rad.int = 110; % Internal Radius [mm]
|
||||
BP.rad.ext = 207.5; % External Radius [mm]
|
||||
BP.thickness = 26; % Thickness [mm]
|
||||
BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm]
|
||||
BP.leg.ang = 9.5; % Angle Offset [deg]
|
||||
BP.density = 8000; % Density of the material [kg/m^3]
|
||||
BP.color = [0.6 0.6 0.6]; % Color [rgb]
|
||||
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
|
||||
|
||||
%% Top Plate
|
||||
TP = struct();
|
||||
|
||||
TP.rad.int = 82; % Internal Radius [mm]
|
||||
TP.rad.ext = 150; % Internal Radius [mm]
|
||||
TP.thickness = 26; % Thickness [mm]
|
||||
TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm]
|
||||
TP.leg.ang = 12.1; % Angle Offset [deg]
|
||||
TP.density = 8000; % Density of the material [kg/m^3]
|
||||
TP.color = [0.6 0.6 0.6]; % Color [rgb]
|
||||
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
|
||||
|
||||
%% Leg
|
||||
Leg = struct();
|
||||
|
||||
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
|
||||
Leg.k.ax = 5e7; % Stiffness of each leg [N/m]
|
||||
Leg.ksi.ax = 3; % Maximum amplification at resonance []
|
||||
Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
|
||||
Leg.rad.top = 17; % Radius of the cylinder of the top part [mm]
|
||||
Leg.density = 8000; % Density of the material [kg/m^3]
|
||||
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
|
||||
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
|
||||
|
||||
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
|
||||
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
|
||||
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
|
||||
Leg = updateDamping(Leg);
|
||||
|
||||
|
||||
%% Sphere
|
||||
SP = struct();
|
||||
|
||||
SP.height.bottom = 27; % [mm]
|
||||
SP.height.top = 27; % [mm]
|
||||
SP.density.bottom = 8000; % [kg/m^3]
|
||||
SP.density.top = 8000; % [kg/m^3]
|
||||
SP.color.bottom = [0.6 0.6 0.6]; % [rgb]
|
||||
SP.color.top = [0.6 0.6 0.6]; % [rgb]
|
||||
SP.k.ax = 0; % [N*m/deg]
|
||||
SP.ksi.ax = 10;
|
||||
|
||||
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
|
||||
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
|
||||
SP.rad.bottom = Leg.sphere.bottom; % [mm]
|
||||
SP.rad.top = Leg.sphere.top; % [mm]
|
||||
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
|
||||
|
||||
SP = updateDamping(SP);
|
||||
|
||||
%%
|
||||
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
|
||||
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
|
||||
|
||||
%%
|
||||
micro_hexapod.BP = BP;
|
||||
micro_hexapod.TP = TP;
|
||||
micro_hexapod.Leg = Leg;
|
||||
micro_hexapod.SP = SP;
|
||||
|
||||
%%
|
||||
micro_hexapod = initializeParameters(micro_hexapod);
|
||||
|
||||
%%
|
||||
if nargout == 0
|
||||
save('./mat/micro_hexapod.mat', 'micro_hexapod')
|
||||
end
|
||||
|
||||
%%
|
||||
function [element] = updateDamping(element)
|
||||
field = fieldnames(element.k);
|
||||
for i = 1:length(field)
|
||||
element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
|
||||
end
|
||||
end
|
||||
|
||||
%%
|
||||
function [stewart] = initializeParameters(stewart)
|
||||
%% Connection points on base and top plate w.r.t. World frame at the center of the base plate
|
||||
stewart.pos_base = zeros(6, 3);
|
||||
stewart.pos_top = zeros(6, 3);
|
||||
|
||||
alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
|
||||
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
|
||||
|
||||
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
|
||||
|
||||
radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
|
||||
radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
|
||||
|
||||
for i = 1:3
|
||||
% base points
|
||||
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
|
||||
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
|
||||
stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
|
||||
stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
|
||||
|
||||
% top points
|
||||
% Top points are 60 degrees offset
|
||||
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
|
||||
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
|
||||
stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
|
||||
stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
|
||||
end
|
||||
|
||||
% permute pos_top points so that legs are end points of base and top points
|
||||
stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
|
||||
stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% leg vectors
|
||||
legs = stewart.pos_top - stewart.pos_base;
|
||||
leg_length = zeros(6, 1);
|
||||
leg_vectors = zeros(6, 3);
|
||||
for i = 1:6
|
||||
leg_length(i) = norm(legs(i,:));
|
||||
leg_vectors(i,:) = legs(i,:) / leg_length(i);
|
||||
end
|
||||
|
||||
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
|
||||
stewart.Leg.shape.bot = [0 0; ...
|
||||
stewart.Leg.rad.bottom 0; ...
|
||||
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
|
||||
stewart.Leg.rad.top stewart.Leg.lenght; ...
|
||||
stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
|
||||
0 0.2*stewart.Leg.lenght];
|
||||
|
||||
%% Calculate revolute and cylindrical axes
|
||||
rev1 = zeros(6, 3);
|
||||
rev2 = zeros(6, 3);
|
||||
cyl1 = zeros(6, 3);
|
||||
for i = 1:6
|
||||
rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
|
||||
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
|
||||
|
||||
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
|
||||
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
|
||||
|
||||
cyl1(i,:) = leg_vectors(i,:);
|
||||
end
|
||||
|
||||
|
||||
%% Coordinate systems
|
||||
stewart.lower_leg = struct('rotation', eye(3));
|
||||
stewart.upper_leg = struct('rotation', eye(3));
|
||||
|
||||
for i = 1:6
|
||||
stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
end
|
||||
|
||||
%% Position Matrix
|
||||
stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% Compute Jacobian Matrix
|
||||
aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
|
||||
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
||||
end
|
||||
|
||||
function J = getJacobianMatrix(RM,M_pos_base)
|
||||
% RM: [3x6] unit vector of each leg in the fixed frame
|
||||
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
|
||||
J = zeros(6);
|
||||
J(:, 1:3) = RM';
|
||||
J(:, 4:6) = cross(M_pos_base, RM)';
|
||||
end
|
||||
end
|
195
initialize/initializeNanoHexapod.m
Normal file
195
initialize/initializeNanoHexapod.m
Normal file
@ -0,0 +1,195 @@
|
||||
function [nano_hexapod] = initializeNanoHexapod(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct();
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Stewart Object
|
||||
nano_hexapod = struct();
|
||||
nano_hexapod.h = 90; % Total height of the platform [mm]
|
||||
nano_hexapod.jacobian = 174.5; % Point where the Jacobian is computed => Center of rotation [mm]
|
||||
|
||||
%% Bottom Plate
|
||||
BP = struct();
|
||||
|
||||
BP.rad.int = 0; % Internal Radius [mm]
|
||||
BP.rad.ext = 150; % External Radius [mm]
|
||||
BP.thickness = 10; % Thickness [mm]
|
||||
BP.leg.rad = 100; % Radius where the legs articulations are positionned [mm]
|
||||
BP.leg.ang = 5; % Angle Offset [deg]
|
||||
BP.density = 8000;% Density of the material [kg/m^3]
|
||||
BP.color = [0.7 0.7 0.7]; % Color [rgb]
|
||||
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
|
||||
|
||||
%% Top Plate
|
||||
TP = struct();
|
||||
|
||||
TP.rad.int = 0; % Internal Radius [mm]
|
||||
TP.rad.ext = 100; % Internal Radius [mm]
|
||||
TP.thickness = 10; % Thickness [mm]
|
||||
TP.leg.rad = 90; % Radius where the legs articulations are positionned [mm]
|
||||
TP.leg.ang = 5; % Angle Offset [deg]
|
||||
TP.density = 8000;% Density of the material [kg/m^3]
|
||||
TP.color = [0.7 0.7 0.7]; % Color [rgb]
|
||||
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
|
||||
|
||||
%% Leg
|
||||
Leg = struct();
|
||||
|
||||
Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
|
||||
Leg.k.ax = 5e7; % Stiffness of each leg [N/m]
|
||||
Leg.ksi.ax = 10; % Maximum amplification at resonance []
|
||||
Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
|
||||
Leg.rad.top = 10; % Radius of the cylinder of the top part [mm]
|
||||
Leg.density = 8000; % Density of the material [kg/m^3]
|
||||
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
|
||||
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
|
||||
|
||||
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
|
||||
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
|
||||
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
|
||||
|
||||
Leg = updateDamping(Leg);
|
||||
|
||||
|
||||
%% Sphere
|
||||
SP = struct();
|
||||
|
||||
SP.height.bottom = 15; % [mm]
|
||||
SP.height.top = 15; % [mm]
|
||||
SP.density.bottom = 8000; % [kg/m^3]
|
||||
SP.density.top = 8000; % [kg/m^3]
|
||||
SP.color.bottom = [0.7 0.7 0.7]; % [rgb]
|
||||
SP.color.top = [0.7 0.7 0.7]; % [rgb]
|
||||
SP.k.ax = 0; % [N*m/deg]
|
||||
SP.ksi.ax = 3;
|
||||
|
||||
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
|
||||
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
|
||||
SP.rad.bottom = Leg.sphere.bottom; % [mm]
|
||||
SP.rad.top = Leg.sphere.top; % [mm]
|
||||
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
|
||||
|
||||
SP = updateDamping(SP);
|
||||
|
||||
%%
|
||||
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
|
||||
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
|
||||
|
||||
%%
|
||||
nano_hexapod.BP = BP;
|
||||
nano_hexapod.TP = TP;
|
||||
nano_hexapod.Leg = Leg;
|
||||
nano_hexapod.SP = SP;
|
||||
|
||||
%%
|
||||
nano_hexapod = initializeParameters(nano_hexapod);
|
||||
|
||||
%%
|
||||
if nargout == 0
|
||||
save('./mat/nano_hexapod.mat', 'nano_hexapod')
|
||||
end
|
||||
|
||||
%%
|
||||
function [element] = updateDamping(element)
|
||||
field = fieldnames(element.k);
|
||||
for i = 1:length(field)
|
||||
element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
|
||||
end
|
||||
end
|
||||
|
||||
%%
|
||||
function [stewart] = initializeParameters(stewart)
|
||||
%% Connection points on base and top plate w.r.t. World frame at the center of the base plate
|
||||
stewart.pos_base = zeros(6, 3);
|
||||
stewart.pos_top = zeros(6, 3);
|
||||
|
||||
alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
|
||||
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
|
||||
|
||||
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
|
||||
|
||||
radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
|
||||
radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
|
||||
|
||||
for i = 1:3
|
||||
% base points
|
||||
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
|
||||
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
|
||||
stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
|
||||
stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
|
||||
|
||||
% top points
|
||||
% Top points are 60 degrees offset
|
||||
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
|
||||
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
|
||||
stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
|
||||
stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
|
||||
end
|
||||
|
||||
% permute pos_top points so that legs are end points of base and top points
|
||||
stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
|
||||
stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% leg vectors
|
||||
legs = stewart.pos_top - stewart.pos_base;
|
||||
leg_length = zeros(6, 1);
|
||||
leg_vectors = zeros(6, 3);
|
||||
for i = 1:6
|
||||
leg_length(i) = norm(legs(i,:));
|
||||
leg_vectors(i,:) = legs(i,:) / leg_length(i);
|
||||
end
|
||||
|
||||
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
|
||||
stewart.Leg.shape.bot = [0 0; ...
|
||||
stewart.Leg.rad.bottom 0; ...
|
||||
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
|
||||
stewart.Leg.rad.top stewart.Leg.lenght; ...
|
||||
stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
|
||||
0 0.2*stewart.Leg.lenght];
|
||||
|
||||
%% Calculate revolute and cylindrical axes
|
||||
rev1 = zeros(6, 3);
|
||||
rev2 = zeros(6, 3);
|
||||
cyl1 = zeros(6, 3);
|
||||
for i = 1:6
|
||||
rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
|
||||
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
|
||||
|
||||
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
|
||||
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
|
||||
|
||||
cyl1(i,:) = leg_vectors(i,:);
|
||||
end
|
||||
|
||||
|
||||
%% Coordinate systems
|
||||
stewart.lower_leg = struct('rotation', eye(3));
|
||||
stewart.upper_leg = struct('rotation', eye(3));
|
||||
|
||||
for i = 1:6
|
||||
stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
end
|
||||
|
||||
%% Position Matrix
|
||||
stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% Compute Jacobian Matrix
|
||||
aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
|
||||
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
||||
end
|
||||
|
||||
function J = getJacobianMatrix(RM,M_pos_base)
|
||||
% RM: [3x6] unit vector of each leg in the fixed frame
|
||||
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
|
||||
J = zeros(6);
|
||||
J(:, 1:3) = RM';
|
||||
J(:, 4:6) = cross(M_pos_base, RM)';
|
||||
end
|
||||
end
|
25
initialize/initializeRy.m
Normal file
25
initialize/initializeRy.m
Normal file
@ -0,0 +1,25 @@
|
||||
function [ry] = initializeRy()
|
||||
%%
|
||||
load('./mat/smiData.mat', 'smiData');
|
||||
|
||||
%%
|
||||
ry = struct();
|
||||
|
||||
ry.m = smiData.Solid(26).mass+smiData.Solid(18).mass+smiData.Solid(10).mass;
|
||||
|
||||
ry.k.h = 357e6/4; % Stiffness in the direction of the guidance [N/m]
|
||||
ry.k.rad = 555e6/4; % Stiffness in the top direction [N/m]
|
||||
ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m]
|
||||
ry.k.tilt = 1e4 ; % Rotation stiffness around y [N*m/deg]
|
||||
|
||||
ry.c.h = ry.k.h/1000;
|
||||
ry.c.rad = ry.k.rad/1000;
|
||||
ry.c.rrad = ry.k.rrad/1000;
|
||||
ry.c.tilt = ry.k.tilt/1000;
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/ry.mat', 'ry');
|
||||
end
|
||||
end
|
||||
|
25
initialize/initializeRz.m
Normal file
25
initialize/initializeRz.m
Normal file
@ -0,0 +1,25 @@
|
||||
function [rz] = initializeRz()
|
||||
%%
|
||||
load('./mat/smiData.mat', 'smiData');
|
||||
|
||||
%%
|
||||
rz = struct();
|
||||
|
||||
rz.m = smiData.Solid(12).mass+6*smiData.Solid(20).mass+smiData.Solid(19).mass;
|
||||
|
||||
rz.k.ax = 2e9; % Axial Stiffness [N/m]
|
||||
rz.k.rad = 7e8; % Radial Stiffness [N/m]
|
||||
rz.k.tilt = 1e5; % TODO
|
||||
rz.k.rot = 1e5; % Rotational Stiffness [N*m/deg]
|
||||
|
||||
rz.c.ax = rz.k.ax/1000;
|
||||
rz.c.rad = rz.k.rad/1000;
|
||||
rz.c.tilt = rz.k.tilt/1000;
|
||||
rz.c.rot = rz.k.rot/1000;
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/rz.mat', 'rz');
|
||||
end
|
||||
end
|
||||
|
21
initialize/initializeSample.m
Normal file
21
initialize/initializeSample.m
Normal file
@ -0,0 +1,21 @@
|
||||
function [] = initializeSample(opts_param)
|
||||
%% Default values for opts
|
||||
sample = struct('radius', 100,...
|
||||
'height', 300,...
|
||||
'mass', 50,...
|
||||
'offset', 0,...
|
||||
'color', [0.9 0.1 0.1] ...
|
||||
);
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
sample.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/sample.mat', 'sample');
|
||||
end
|
||||
end
|
2017
initialize/initializeSmiData.m
Normal file
2017
initialize/initializeSmiData.m
Normal file
File diff suppressed because it is too large
Load Diff
29
initialize/initializeTy.m
Normal file
29
initialize/initializeTy.m
Normal file
@ -0,0 +1,29 @@
|
||||
function [ty] = initializeTy()
|
||||
%%
|
||||
load('./mat/smiData.mat', 'smiData');
|
||||
|
||||
%%
|
||||
ty = struct();
|
||||
|
||||
ty.m = smiData.Solid(4).mass+...
|
||||
smiData.Solid(6).mass+...
|
||||
smiData.Solid(7).mass+...
|
||||
smiData.Solid(8).mass+...
|
||||
smiData.Solid(9).mass+...
|
||||
4*smiData.Solid(11).mass+...
|
||||
smiData.Solid(24).mass+...
|
||||
smiData.Solid(25).mass+...
|
||||
smiData.Solid(28).mass;
|
||||
|
||||
ty.k.ax = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m]
|
||||
ty.k.rad = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
|
||||
|
||||
ty.c.ax = ty.k.ax/1000;
|
||||
ty.c.rad = ty.k.rad/1000;
|
||||
|
||||
%% Save if no output argument
|
||||
if nargout == 0
|
||||
save('./mat/ty.mat', 'ty');
|
||||
end
|
||||
end
|
||||
|
BIN
mat/G_f_to_d.mat
BIN
mat/G_f_to_d.mat
Binary file not shown.
BIN
mat/G_nass.mat
Normal file
BIN
mat/G_nass.mat
Normal file
Binary file not shown.
BIN
mat/G_xg_to_d.mat
Normal file
BIN
mat/G_xg_to_d.mat
Normal file
Binary file not shown.
BIN
mat/T_S.mat
Normal file
BIN
mat/T_S.mat
Normal file
Binary file not shown.
BIN
mat/axisc.mat
Normal file
BIN
mat/axisc.mat
Normal file
Binary file not shown.
BIN
mat/control_K_tx.mat
Normal file
BIN
mat/control_K_tx.mat
Normal file
Binary file not shown.
BIN
mat/control_K_ty.mat
Normal file
BIN
mat/control_K_ty.mat
Normal file
Binary file not shown.
BIN
mat/control_K_tz.mat
Normal file
BIN
mat/control_K_tz.mat
Normal file
Binary file not shown.
Binary file not shown.
BIN
mat/granite.mat
Normal file
BIN
mat/granite.mat
Normal file
Binary file not shown.
BIN
mat/ground.mat
Normal file
BIN
mat/ground.mat
Normal file
Binary file not shown.
BIN
mat/id_G_g_g.mat
BIN
mat/id_G_g_g.mat
Binary file not shown.
BIN
mat/id_G_h_g.mat
BIN
mat/id_G_h_g.mat
Binary file not shown.
BIN
mat/id_G_h_h.mat
BIN
mat/id_G_h_h.mat
Binary file not shown.
BIN
mat/inputs.mat
BIN
mat/inputs.mat
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
mat/micro_hexapod.mat
Normal file
BIN
mat/micro_hexapod.mat
Normal file
Binary file not shown.
BIN
mat/nano_hexapod.mat
Normal file
BIN
mat/nano_hexapod.mat
Normal file
Binary file not shown.
BIN
mat/ry.mat
Normal file
BIN
mat/ry.mat
Normal file
Binary file not shown.
BIN
mat/rz.mat
Normal file
BIN
mat/rz.mat
Normal file
Binary file not shown.
BIN
mat/sample.mat
Normal file
BIN
mat/sample.mat
Normal file
Binary file not shown.
BIN
mat/smiData.mat
Normal file
BIN
mat/smiData.mat
Normal file
Binary file not shown.
BIN
mat/ty.mat
Normal file
BIN
mat/ty.mat
Normal file
Binary file not shown.
BIN
mat/weight_Wxg.mat
Normal file
BIN
mat/weight_Wxg.mat
Normal file
Binary file not shown.
7
readme.org
Normal file
7
readme.org
Normal file
@ -0,0 +1,7 @@
|
||||
#+TITLE: Simscape - NASS
|
||||
|
||||
* Description of the files
|
||||
|
||||
* List of things to do
|
||||
** TODO Apply some MIMO control
|
||||
** TODO Create multiple folder inside the ~mat~ folder.
|
34
src/identifyG.m
Normal file
34
src/identifyG.m
Normal file
@ -0,0 +1,34 @@
|
||||
function [G, G_raw] = identifyG(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct('f_low', 0.1, ...
|
||||
'f_high', 10000 ...
|
||||
);
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'Micro_Station_Identification';
|
||||
|
||||
%% Centralized control (Cartesian coordinates)
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Micro-Station/Fn'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Sample'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_raw = linearize(mdl,io, 0);
|
||||
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
||||
|
||||
G = preprocessIdTf(G_raw, opts.f_low, opts.f_high);
|
||||
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
||||
end
|
34
src/identifyNass.m
Normal file
34
src/identifyNass.m
Normal file
@ -0,0 +1,34 @@
|
||||
function [G_cart, G_cart_raw] = identifyNass(opts_param)
|
||||
%% Default values for opts
|
||||
opts = struct('f_low', 1,...
|
||||
'f_high', 10000 ...
|
||||
);
|
||||
|
||||
%% Populate opts with input parameters
|
||||
if exist('opts_param','var')
|
||||
for opt = fieldnames(opts_param)'
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
end
|
||||
end
|
||||
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'Micro_Station_Identification';
|
||||
|
||||
%% Centralized control (Cartesian coordinates)
|
||||
% Input/Output definition
|
||||
io(1) = linio([mdl, '/Micro-Station/Fn'], 1,'input');
|
||||
io(2) = linio([mdl, '/Micro-Station/Nano_Hexapod'],1,'output');
|
||||
|
||||
% Run the linearization
|
||||
G_cart_raw = linearize(mdl,io, 0);
|
||||
|
||||
G_cart = preprocessIdTf(G_cart_raw, opts.f_low, opts.f_high);
|
||||
|
||||
% Input/Output names
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
G_cart.OutputName = {'Dnx', 'Dny', 'Dnz', 'Rnx', 'Rny', 'Rnz'};
|
||||
end
|
@ -1 +1 @@
|
||||
Subproject commit 6fe96032fd93d81cfcb6c78fea8a79bb586dd488
|
||||
Subproject commit bef54b5bf7967d8eb61a2d24d39a729e5d34c76d
|
Loading…
Reference in New Issue
Block a user