[REFACTORING] Huge changes, WIP.

This commit is contained in:
Thomas Dehaeze 2018-06-16 22:57:54 +02:00
parent 971a520edd
commit 92b66cb8bb
67 changed files with 3313 additions and 2421 deletions

View File

@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="Control" Type="Relative" />

View File

@ -1,2 +0,0 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="stewart-simscape/src" Type="Relative" />

View File

@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="initialize" Type="Relative" />

View 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')

View File

@ -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')

View File

@ -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')

Binary file not shown.

23
Control/cl_tf.m Normal file
View 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');

View File

@ -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')

View 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');

View File

@ -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'));

View 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')

View 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');

View 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');

View 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');

View File

@ -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');

View File

@ -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');

View File

@ -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')

View File

@ -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

Binary file not shown.

View File

@ -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);

View File

@ -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
View 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');

View File

@ -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]

View File

@ -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

View 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

View 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

View 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

View 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

View 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

View 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
View 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
View 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

View 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

File diff suppressed because it is too large Load Diff

29
initialize/initializeTy.m Normal file
View 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

Binary file not shown.

BIN
mat/G_nass.mat Normal file

Binary file not shown.

BIN
mat/G_xg_to_d.mat Normal file

Binary file not shown.

BIN
mat/T_S.mat Normal file

Binary file not shown.

BIN
mat/axisc.mat Normal file

Binary file not shown.

BIN
mat/control_K_tx.mat Normal file

Binary file not shown.

BIN
mat/control_K_ty.mat Normal file

Binary file not shown.

BIN
mat/control_K_tz.mat Normal file

Binary file not shown.

Binary file not shown.

BIN
mat/granite.mat Normal file

Binary file not shown.

BIN
mat/ground.mat Normal file

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

BIN
mat/micro_hexapod.mat Normal file

Binary file not shown.

BIN
mat/nano_hexapod.mat Normal file

Binary file not shown.

BIN
mat/ry.mat Normal file

Binary file not shown.

BIN
mat/rz.mat Normal file

Binary file not shown.

BIN
mat/sample.mat Normal file

Binary file not shown.

BIN
mat/smiData.mat Normal file

Binary file not shown.

BIN
mat/ty.mat Normal file

Binary file not shown.

BIN
mat/weight_Wxg.mat Normal file

Binary file not shown.

7
readme.org Normal file
View 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
View 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
View 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