tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[REFACTORING] Huge changes, WIP.

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
.SimulinkProject/Root.type.ProjectPath/2d80ad84-a2e6-469a-be37-c276ae1af074.type.Reference.xml
+2
View File
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="Control" Type="Relative" />
.SimulinkProject/Root.type.ProjectPath/394b5507-3405-4742-9ff1-cb1569440644.type.Reference.xml
-2
View File
@@ -1,2 +0,0 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="stewart-simscape/src" Type="Relative" />
.SimulinkProject/Root.type.ProjectPath/a3b2491b-7b63-4e77-b2a4-97d4753c1df9.type.Reference.xml
+2
View File
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="initialize" Type="Relative" />
Analysis/analysis_perturbations.m
+28
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')
Analysis/ground_motion_experiment.m
+23 -19
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')
Analysis/ground_motion_ol_cl.m
+81 -8
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')
Assemblage.slx
BIN
View File
Binary file not shown.
Control/cl_tf.m
+23
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');
Control/control_y.m
-34
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')
Control/generate_control.m
+34
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');
Identification/compare_measurements.m
+66 -14
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'));
Identification/compare_nass_micro_station.m
+41
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')
Identification/identification_G.m
+56
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');
Identification/identification_Gd.m
+38
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');
Identification/identification_NASS.m
+55
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');
Identification/identification_control.m
-36
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');
Identification/identification_marc.m
+34 -41
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');
Identification/identification_stages_plot.m
+35 -27
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')
Identification/identification_stages_run.m
+14 -59
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
Micro_Station_Identification.slx
BIN
View File
Binary file not shown.
init_data.m
+27 -86
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);
init_inputs.m
-67
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');
init_perturbations.m
+12
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');
init_sim_configuration.m
+2 -2
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]
init_simulation.m
+18 -7
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');
init_solidworks_data.m
-2018
View File
File diff suppressed because it is too large Load Diff
initialize/initializeAxisc.m
+17
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
initialize/initializeGranite.m
+17
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
initialize/initializeGround.m
+10
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
initialize/initializeInputs.m
+96
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
initialize/initializeMicroHexapod.m
+194
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
initialize/initializeNanoHexapod.m
+195
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
initialize/initializeRy.m
+25
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
initialize/initializeRz.m
+25
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
initialize/initializeSample.m
+21
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
initialize/initializeSmiData.m
+2017
View File
File diff suppressed because it is too large Load Diff
initialize/initializeTy.m
+29
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
mat/G_f_to_d.mat
BIN
View File
Binary file not shown.
mat/G_nass.mat
BIN
View File
Binary file not shown.
mat/G_xg_to_d.mat
BIN
View File
Binary file not shown.
mat/T_S.mat
BIN
View File
Binary file not shown.
mat/axisc.mat
BIN
View File
Binary file not shown.
mat/control_K_tx.mat
BIN
View File
Binary file not shown.
mat/control_K_ty.mat
BIN
View File
Binary file not shown.
mat/control_K_tz.mat
BIN
View File
Binary file not shown.
mat/controller.mat
BIN
View File
Binary file not shown.
mat/granite.mat
BIN
View File
Binary file not shown.
mat/ground.mat
BIN
View File
Binary file not shown.
mat/id_G_g_g.mat
BIN
View File
Binary file not shown.
mat/id_G_h_g.mat
BIN
View File
Binary file not shown.
mat/id_G_h_h.mat
BIN
View File
Binary file not shown.
mat/inputs.mat
BIN
View File
Binary file not shown.
mat/inputs_ground_motion.mat
BIN
View File
Binary file not shown.
mat/inputs_setpoint.mat
BIN
View File
Binary file not shown.
mat/inputs_spindle.mat
BIN
View File
Binary file not shown.
mat/micro_hexapod.mat
BIN
View File
Binary file not shown.
mat/nano_hexapod.mat
BIN
View File
Binary file not shown.
mat/ry.mat
BIN
View File
Binary file not shown.
mat/rz.mat
BIN
View File
Binary file not shown.
mat/sample.mat
BIN
View File
Binary file not shown.
mat/smiData.mat
BIN
View File
Binary file not shown.
mat/ty.mat
BIN
View File
Binary file not shown.
mat/weight_Wxg.mat
BIN
View File
Binary file not shown.
readme.org
+7
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.
src/identifyG.m
+34
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
src/identifyNass.m
+34
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
stewart-simscape
+1 -1
Submodule stewart-simscape updated: 6fe96032fd...bef54b5bf7